00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029 #ifndef HECTOR_POSE_ESTIMATION_MATRIX_H
00030 #define HECTOR_POSE_ESTIMATION_MATRIX_H
00031
00032 #include <Eigen/Geometry>
00033 #include <stdexcept>
00034
00035 #define ASSERT_SYMMETRIC_MATRIX_TO_BE_SYMMETRIC
00036 #define ASSERT_SYMMETRIC_MATRIX_TO_BE_SYMMETRIC_PRECISION 1e-5
00037 #define FORCE_SYMMETRIC_MATRIX_TO_BE_SYMMETRIC
00038
00039 namespace hector_pose_estimation {
00040 using Eigen::Dynamic;
00041 using Eigen::Lower;
00042 using Eigen::Upper;
00043
00044 typedef double ScalarType;
00045 typedef Eigen::DenseIndex IndexType;
00046 typedef Eigen::Matrix<ScalarType,Dynamic,1> ColumnVector;
00047 typedef Eigen::Matrix<ScalarType,1,Dynamic> RowVector;
00048 typedef Eigen::Matrix<ScalarType,Dynamic,Dynamic> Matrix;
00049 typedef Eigen::Quaternion<ScalarType> Quaternion;
00050
00051 using Eigen::VectorBlock;
00052 typedef VectorBlock<ColumnVector,3> VectorBlock3;
00053 typedef VectorBlock<ColumnVector,4> VectorBlock4;
00054 typedef VectorBlock<const ColumnVector,3> ConstVectorBlock3;
00055 typedef VectorBlock<const ColumnVector,4> ConstVectorBlock4;
00056
00057 using Eigen::Block;
00058 typedef Eigen::Block<Matrix,Dynamic,Dynamic> MatrixBlock;
00059
00060 template <int Rows>
00061 class ColumnVector_ : public Eigen::Matrix<ScalarType,Rows,1> {
00062 public:
00063 EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(Rows != Dynamic)
00064
00065 typedef ColumnVector_<Rows> Derived;
00066 typedef Eigen::Matrix<ScalarType,Rows,1> Base;
00067 typedef typename Eigen::internal::traits<Base>::Scalar Scalar;
00068 typedef typename Eigen::internal::traits<Base>::Index Index;
00069 typedef Eigen::Map<Base> Map;
00070 typedef Eigen::Map<const Base> ConstMap;
00071
00072 ColumnVector_() { this->setZero(); }
00073 ColumnVector_(Scalar value) { this->setConstant(value); }
00074 ColumnVector_(Scalar x, Scalar y, Scalar z) : Eigen::Matrix<ScalarType,Rows,1>(x, y, z) {}
00075 template <typename OtherDerived> ColumnVector_(const Eigen::MatrixBase<OtherDerived>& other) : Base(other) {}
00076
00077 EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Derived)
00078 };
00079 typedef ColumnVector_<3> ColumnVector3;
00080
00081 template <int Cols>
00082 class RowVector_ : public Eigen::Matrix<ScalarType,1,Cols> {
00083 public:
00084 EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(Cols != Dynamic)
00085
00086 typedef RowVector_<Cols> Derived;
00087 typedef Eigen::Matrix<ScalarType,1,Cols> Base;
00088 typedef typename Eigen::internal::traits<Base>::Scalar Scalar;
00089 typedef typename Eigen::internal::traits<Base>::Index Index;
00090 typedef Eigen::Map<Base> Map;
00091 typedef Eigen::Map<const Base> ConstMap;
00092
00093 RowVector_() { this->setZero(); }
00094 RowVector_(Scalar value) { this->setConstant(value); }
00095 RowVector_(Scalar x, Scalar y, Scalar z) : Eigen::Matrix<ScalarType,1,Cols>(x, y, z) {}
00096 template <typename OtherDerived> RowVector_(const Eigen::MatrixBase<OtherDerived>& other) : Base(other) {}
00097
00098 EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Derived)
00099 };
00100 typedef RowVector_<3> RowVector3;
00101
00102 template <int Rows, int Cols>
00103 class Matrix_ : public Eigen::Matrix<ScalarType,Rows,Cols> {
00104 public:
00105 EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(Rows != Dynamic || Cols != Dynamic)
00106
00107 typedef Matrix_<Rows,Cols> Derived;
00108 typedef Eigen::Matrix<ScalarType,Rows,Cols> Base;
00109 typedef typename Eigen::internal::traits<Base>::Scalar Scalar;
00110 typedef typename Eigen::internal::traits<Base>::Index Index;
00111 typedef Eigen::Map<Base> Map;
00112 typedef Eigen::Map<const Base> ConstMap;
00113
00114 Matrix_() { this->setZero(); }
00115 Matrix_(Scalar value) { this->setConstant(value); }
00116 template <typename OtherDerived> Matrix_(const Eigen::MatrixBase<OtherDerived>& other) : Base(other) {}
00117
00118 EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Derived)
00119
00120 protected:
00121 explicit Matrix_(Index rows, Index cols) : Base(rows, cols) {}
00122 };
00123 typedef Matrix_<3,3> Matrix3;
00124
00125
00126
00127
00128
00129
00130
00131
00132
00133
00134
00135
00136
00137
00138
00139
00140
00141
00142
00143
00144
00145
00146
00147
00148
00150
00151
00152
00153
00154
00155
00156
00157
00158
00159
00160
00161
00162
00163
00164
00165
00166
00167
00168
00169
00170
00171
00172
00173
00174
00175
00176
00177
00178
00179
00180
00181
00182
00183
00184
00185
00186
00187
00188
00189
00190
00191
00192
00193
00194
00195
00196
00197
00198
00199
00200
00201
00202
00203
00204
00205
00206
00207
00208
00209
00210
00211
00212
00213
00214
00215
00216
00217
00218
00219
00220
00221
00222
00223
00224
00225
00226
00227
00228
00229
00230
00231
00232 template <int RowsCols>
00233 class SymmetricMatrix_ : public Matrix_<RowsCols,RowsCols> {
00234 public:
00235 EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(RowsCols != Dynamic)
00236
00237 typedef SymmetricMatrix_<RowsCols> Derived;
00238 typedef Matrix_<RowsCols,RowsCols> Storage;
00239 typedef typename Storage::Base Base;
00240 typedef Eigen::SelfAdjointView<typename Storage::Base,Upper> SelfAdjointView;
00241 typedef typename Eigen::internal::traits<Base>::Scalar Scalar;
00242 typedef typename Eigen::internal::traits<Base>::Index Index;
00243 typedef Eigen::Map<Base> Map;
00244 typedef Eigen::Map<const Base> ConstMap;
00245
00246
00247 SymmetricMatrix_() {}
00248 SymmetricMatrix_(Scalar value) { this->setConstant(value); }
00249 template <typename OtherDerived> SymmetricMatrix_(const Eigen::MatrixBase<OtherDerived>& other) : Storage(other) { symmetric(); }
00250
00251 template <typename OtherDerived> Derived& operator=(const Eigen::MatrixBase<OtherDerived>& other) {
00252 this->Base::operator=(other);
00253 return symmetric();
00254 }
00255
00256 Derived& symmetric() {
00257 #if defined(ASSERT_SYMMETRIC_MATRIX_TO_BE_SYMMETRIC)
00258 eigen_assert(this->isApprox(this->transpose(), ASSERT_SYMMETRIC_MATRIX_TO_BE_SYMMETRIC_PRECISION));
00259 #endif
00260 #if defined(FORCE_SYMMETRIC_MATRIX_TO_BE_SYMMETRIC)
00261 this->template triangularView<Eigen::Lower>() = this->transpose();
00262 #endif
00263 return *this;
00264 }
00265
00266
00267
00268
00269
00270 Derived inverse() const {
00271
00272 return this->Storage::inverse().eval();
00273 }
00274
00275 protected:
00276 explicit SymmetricMatrix_(Index dim) : Storage(dim, dim) {}
00277 };
00278 typedef SymmetricMatrix_<3> SymmetricMatrix3;
00279
00280 class SymmetricMatrix : public SymmetricMatrix_<Dynamic> {
00281 public:
00282 typedef SymmetricMatrix Derived;
00283 typedef SymmetricMatrix_<Dynamic> Storage;
00284 typedef typename Storage::Base Base;
00285 typedef Eigen::SelfAdjointView<typename Storage::Base,Upper> SelfAdjointView;
00286 typedef typename Eigen::internal::traits<Base>::Scalar Scalar;
00287 typedef typename Eigen::internal::traits<Base>::Index Index;
00288 typedef Eigen::Map<Base> Map;
00289 typedef Eigen::Map<const Base> ConstMap;
00290
00291
00292 SymmetricMatrix() : SymmetricMatrix_<Dynamic>() {}
00293 SymmetricMatrix(Index dim) : SymmetricMatrix_<Dynamic>(dim) { this->setZero(); }
00294 SymmetricMatrix(Index dim, Scalar value) : SymmetricMatrix_<Dynamic>(dim) { this->setConstant(value); }
00295 template <int OtherRowsCols> SymmetricMatrix(const SymmetricMatrix_<OtherRowsCols>& other) : SymmetricMatrix_<Dynamic>(other) {}
00296 template <typename OtherDerived> SymmetricMatrix(const Eigen::MatrixBase<OtherDerived>& other) : SymmetricMatrix_<Dynamic>(other) {}
00297
00298 template <typename OtherDerived> Derived& operator=(const Eigen::MatrixBase<OtherDerived>& other) {
00299 this->Base::operator=(other);
00300 return symmetric();
00301 }
00302
00303 Derived& symmetric() {
00304 Storage::symmetric();
00305 return *this;
00306 }
00307
00308 void resize(Index size) {
00309 Base::resize(size, size);
00310 }
00311
00312 void conservativeResize(Index size) {
00313 Base::conservativeResize(size, size);
00314 }
00315 };
00316
00317 }
00318
00319 #endif // HECTOR_POSE_ESTIMATION_MATRIX_H