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 <hector_pose_estimation/matrix_config.h>
00033 #include <hector_pose_estimation/Eigen/QuaternionBaseAddons.h>
00034
00035 #include <Eigen/Core>
00036 #include <Eigen/Geometry>
00037 #include <stdexcept>
00038
00039 namespace hector_pose_estimation {
00040 using Eigen::Dynamic;
00041 using Eigen::Lower;
00042 using Eigen::Upper;
00043 using Eigen::DenseBase;
00044
00045 typedef double ScalarType;
00046 typedef Eigen::DenseIndex IndexType;
00047
00048 typedef Eigen::Quaternion<ScalarType> Quaternion;
00049
00050 template <int Rows>
00051 class ColumnVector_ : public Eigen::Matrix<ScalarType,Rows,1,0,(Rows != Dynamic ? Rows : MaxVectorSize),1> {
00052 public:
00053 typedef ColumnVector_<Rows> Derived;
00054 typedef Eigen::Matrix<ScalarType,Rows,1,0,(Rows != Dynamic ? Rows : MaxVectorSize),1> Base;
00055 typedef Eigen::Map<Base> Map;
00056 typedef Eigen::Map<const Base> ConstMap;
00057
00058 ColumnVector_() { this->setZero(); }
00059 explicit ColumnVector_(IndexType size) : Base(size) { assert(Rows == Dynamic || size == Rows); this->setZero(); }
00060
00061 ColumnVector_(ScalarType x, ScalarType y, ScalarType z) : Base(x, y, z) {}
00062 template <typename OtherDerived> ColumnVector_(const Eigen::MatrixBase<OtherDerived>& other) : Base(other) {}
00063
00064 EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Derived)
00065 Derived& operator=(ScalarType scalar) { this->setConstant(scalar); return *this; }
00066 };
00067 typedef ColumnVector_<Dynamic> ColumnVector;
00068 typedef ColumnVector_<3> ColumnVector3;
00069
00070 template <int Cols>
00071 class RowVector_ : public Eigen::Matrix<ScalarType,1,Cols,Eigen::RowMajor,1,MaxVectorSize> {
00072 public:
00073 typedef RowVector_<Cols> Derived;
00074 typedef Eigen::Matrix<ScalarType,1,Cols,0,1,MaxVectorSize> Base;
00075 typedef Eigen::Map<Base> Map;
00076 typedef Eigen::Map<const Base> ConstMap;
00077
00078 RowVector_() { this->setZero(); }
00079 explicit RowVector_(IndexType size) : Base(size) { assert(Cols == Dynamic || size == Cols); this->setZero(); }
00080
00081 RowVector_(ScalarType x, ScalarType y, ScalarType z) : Base(x, y, z) {}
00082 template <typename OtherDerived> RowVector_(const Eigen::MatrixBase<OtherDerived>& other) : Base(other) {}
00083
00084 EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Derived)
00085 Derived& operator=(ScalarType scalar) { this->setConstant(scalar); return *this; }
00086 };
00087 typedef RowVector_<Dynamic> RowVector;
00088 typedef RowVector_<3> RowVector3;
00089
00090 template <int Rows, int Cols>
00091 class Matrix_ : public Eigen::Matrix<ScalarType,Rows,Cols,(Rows==1 && Cols!=1 ? Eigen::RowMajor : Eigen::ColMajor),(Rows != Dynamic ? Rows : MaxMatrixRowsCols),(Cols != Dynamic ? Cols : MaxMatrixRowsCols)> {
00092 public:
00093 typedef Matrix_<Rows,Cols> Derived;
00094 typedef Eigen::Matrix<ScalarType,Rows,Cols,(Rows==1 && Cols!=1 ? Eigen::RowMajor : Eigen::ColMajor),(Rows != Dynamic ? Rows : MaxMatrixRowsCols),(Cols != Dynamic ? Cols : MaxMatrixRowsCols)> Base;
00095 typedef Eigen::Map<Base> Map;
00096 typedef Eigen::Map<const Base> ConstMap;
00097
00098 Matrix_() { this->setZero(); }
00099 explicit Matrix_(IndexType rows, IndexType cols) : Base(rows, cols) { assert(Rows == Dynamic || rows == Rows); assert(Cols == Dynamic || cols == Cols); this->setZero(); }
00100
00101 template <typename OtherDerived> Matrix_(const Eigen::MatrixBase<OtherDerived>& other) : Base(other) {}
00102
00103 EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Derived)
00104 Derived& operator=(ScalarType scalar) { this->setConstant(scalar); return *this; }
00105 };
00106 typedef Matrix_<Dynamic,Dynamic> Matrix;
00107 typedef Matrix_<3,3> Matrix3;
00108
00109 template <int RowsCols>
00110 class SymmetricMatrix_ : public Matrix_<RowsCols,RowsCols> {
00111 public:
00112 typedef SymmetricMatrix_<RowsCols> Derived;
00113 typedef Matrix_<RowsCols,RowsCols> Storage;
00114 using typename Storage::Base;
00115
00116 using typename Storage::Map;
00117 using typename Storage::ConstMap;
00118
00119
00120 SymmetricMatrix_() {}
00121
00122 explicit SymmetricMatrix_(IndexType dim) : Storage(dim, dim) { assert(RowsCols == Dynamic || dim == RowsCols); }
00123 template <typename OtherDerived> SymmetricMatrix_(const Eigen::MatrixBase<OtherDerived>& other) : Storage(other) {
00124 #if defined(FORCE_SYMMETRIC_MATRIX_TO_BE_SYMMETRIC)
00125 symmetric();
00126 #endif
00127 }
00128 SymmetricMatrix_(const Derived& other) : Storage(other) {}
00129
00130 template <typename OtherDerived> Derived& operator=(const Eigen::MatrixBase<OtherDerived>& other) {
00131 this->Storage::operator=(other);
00132 #if defined(ASSERT_SYMMETRIC_MATRIX_TO_BE_SYMMETRIC)
00133 eigen_assert(this->isApprox(this->transpose(), ASSERT_SYMMETRIC_MATRIX_TO_BE_SYMMETRIC_PRECISION));
00134 #endif
00135 #if defined(FORCE_SYMMETRIC_MATRIX_TO_BE_SYMMETRIC)
00136 return symmetric();
00137 #else
00138 return *this;
00139 #endif
00140 }
00141
00142 Derived& operator=(const Derived& other) {
00143 this->Storage::operator=(other);
00144 return *this;
00145 }
00146
00147 Derived& symmetric() {
00148 this->Storage::operator=((*this + this->transpose()) / 2);
00149 return *this;
00150 }
00151
00152
00153
00154
00155
00156
00158
00159
00160 };
00161 typedef SymmetricMatrix_<3> SymmetricMatrix3;
00162
00163 class SymmetricMatrix : public SymmetricMatrix_<Dynamic> {
00164 public:
00165 typedef SymmetricMatrix Derived;
00166 typedef SymmetricMatrix_<Dynamic> Storage;
00167 using typename Storage::Base;
00168
00169 using typename Storage::Map;
00170 using typename Storage::ConstMap;
00171
00172
00173 SymmetricMatrix() : Storage() {}
00174 SymmetricMatrix(IndexType dim) : Storage(dim) {}
00175
00176 template <int OtherRowsCols> SymmetricMatrix(const SymmetricMatrix_<OtherRowsCols>& other) : Storage(other) {}
00177 template <typename OtherDerived> SymmetricMatrix(const Eigen::MatrixBase<OtherDerived>& other) : Storage(other) {}
00178
00179 template <typename OtherDerived> Derived& operator=(const Eigen::MatrixBase<OtherDerived>& other) {
00180 this->Storage::operator=(other);
00181 return symmetric();
00182 }
00183
00184 Derived& symmetric() {
00185 this->Storage::symmetric();
00186 return *this;
00187 }
00188
00189 void resize(IndexType size) {
00190 this->Storage::resize(size, size);
00191 }
00192
00193 void conservativeResize(IndexType size) {
00194 this->Storage::conservativeResize(size, size);
00195 }
00196 };
00197
00198 class SkewSymmetricMatrix : public Matrix3 {
00199 public:
00200 template <typename OtherDerived> SkewSymmetricMatrix(const Eigen::MatrixBase<OtherDerived>& other) : Matrix3()
00201 {
00202 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Eigen::MatrixBase<OtherDerived>, 3);
00203 *this << 0.0, -other.z(), other.y(),
00204 other.z(), 0.0, -other.x(),
00205 -other.y(), other.x(), 0.0;
00206 }
00207 };
00208
00209 }
00210
00211 namespace Eigen {
00212
00213
00214
00215
00216
00217 #if !EIGEN_VERSION_AT_LEAST(3,0,91)
00218
00219 template<typename T, int _Options> class DenseStorage<T, 0, Dynamic, Dynamic, _Options>
00220 : public DenseStorage<T, 0, 0, 0, _Options> { };
00221
00222 template<typename T, int _Rows, int _Options> class DenseStorage<T, 0, _Rows, Dynamic, _Options>
00223 : public DenseStorage<T, 0, 0, 0, _Options> { };
00224
00225 template<typename T, int _Cols, int _Options> class DenseStorage<T, 0, Dynamic, _Cols, _Options>
00226 : public DenseStorage<T, 0, 0, 0, _Options> { };
00227 #endif
00228
00229 namespace internal {
00230
00231 #define EIGEN_FORWARD_TRAITS_FOR_TYPE(Base) \
00232 typedef typename traits<Base>::Scalar Scalar; \
00233 typedef typename traits<Base>::StorageKind StorageKind; \
00234 typedef typename traits<Base>::Index Index; \
00235 typedef typename traits<Base>::XprKind XprKind; \
00236 enum { \
00237 RowsAtCompileTime = traits<Base>::RowsAtCompileTime, \
00238 ColsAtCompileTime = traits<Base>::ColsAtCompileTime, \
00239 MaxRowsAtCompileTime = traits<Base>::MaxRowsAtCompileTime, \
00240 MaxColsAtCompileTime = traits<Base>::MaxColsAtCompileTime, \
00241 Flags = traits<Base>::Flags, \
00242 CoeffReadCost = traits<Base>::CoeffReadCost, \
00243 Options = traits<Base>::Options, \
00244 InnerStrideAtCompileTime = traits<Base>::InnerStrideAtCompileTime, \
00245 OuterStrideAtCompileTime = traits<Base>::OuterStrideAtCompileTime \
00246 }
00247
00248 template <int Rows> struct traits< hector_pose_estimation::ColumnVector_<Rows> > {
00249 typedef typename hector_pose_estimation::ColumnVector_<Rows>::Base EigenType;
00250 EIGEN_FORWARD_TRAITS_FOR_TYPE(EigenType);
00251 };
00252
00253 template <int Cols> struct traits< hector_pose_estimation::RowVector_<Cols> > {
00254 typedef typename hector_pose_estimation::RowVector_<Cols>::Base EigenType;
00255 EIGEN_FORWARD_TRAITS_FOR_TYPE(EigenType);
00256 };
00257
00258 template <int Rows, int Cols> struct traits< hector_pose_estimation::Matrix_<Rows,Cols> > {
00259 typedef typename hector_pose_estimation::Matrix_<Rows,Cols>::Base EigenType;
00260 EIGEN_FORWARD_TRAITS_FOR_TYPE(EigenType);
00261 };
00262
00263 template <int RowsCols> struct traits< hector_pose_estimation::SymmetricMatrix_<RowsCols> > {
00264 typedef typename hector_pose_estimation::SymmetricMatrix_<RowsCols>::Base EigenType;
00265 EIGEN_FORWARD_TRAITS_FOR_TYPE(EigenType);
00266 };
00267
00268 template <> struct traits< hector_pose_estimation::SymmetricMatrix > {
00269 typedef hector_pose_estimation::SymmetricMatrix::Base EigenType;
00270 EIGEN_FORWARD_TRAITS_FOR_TYPE(EigenType);
00271 };
00272
00273 }
00274 }
00275
00276 namespace hector_pose_estimation {
00277
00278 using Eigen::VectorBlock;
00279 typedef VectorBlock<ColumnVector,3> VectorBlock3;
00280 typedef VectorBlock<ColumnVector,4> VectorBlock4;
00281 typedef VectorBlock<const ColumnVector,3> ConstVectorBlock3;
00282 typedef VectorBlock<const ColumnVector,4> ConstVectorBlock4;
00283
00284 using Eigen::Block;
00285 typedef Eigen::Block<Matrix,Dynamic,Dynamic> MatrixBlock;
00286
00287 }
00288
00289 #endif // HECTOR_POSE_ESTIMATION_MATRIX_H