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/MatrixBaseAddons.h>
00034 #include <hector_pose_estimation/Eigen/QuaternionBaseAddons.h>
00035
00036 #include <Eigen/Core>
00037 #include <Eigen/Geometry>
00038 #include <stdexcept>
00039
00040 namespace hector_pose_estimation {
00041 using Eigen::Dynamic;
00042 using Eigen::Lower;
00043 using Eigen::Upper;
00044 using Eigen::DenseBase;
00045
00046 typedef double ScalarType;
00047 typedef Eigen::DenseIndex IndexType;
00048
00049 typedef Eigen::Quaternion<ScalarType> Quaternion;
00050
00051 template <int Rows>
00052 struct ColumnVector_ {
00053 typedef Eigen::Matrix<ScalarType, Rows, 1,
00054 Eigen::ColMajor,
00055 (Rows != Dynamic ? Rows : MaxVectorSize), 1
00056 > type;
00057 };
00058 typedef typename ColumnVector_<Dynamic>::type ColumnVector;
00059 typedef typename ColumnVector_<3>::type ColumnVector3;
00060
00061 template <int Cols>
00062 struct RowVector_ {
00063 typedef Eigen::Matrix<ScalarType, 1, Cols,
00064 Eigen::RowMajor,
00065 1, (Cols != Dynamic ? Cols : MaxVectorSize)
00066 > type;
00067 };
00068 typedef typename RowVector_<Dynamic>::type RowVector;
00069 typedef typename RowVector_<3>::type RowVector3;
00070
00071 template <int Rows, int Cols>
00072 struct Matrix_ {
00073 typedef Eigen::Matrix<ScalarType, Rows, Cols,
00074 (Rows == 1 && Cols != 1 ? Eigen::RowMajor : Eigen::ColMajor),
00075 (Rows != Dynamic ? Rows : MaxMatrixRowsCols),
00076 (Cols != Dynamic ? Cols : MaxMatrixRowsCols)
00077 > type;
00078 };
00079 typedef typename Matrix_<Dynamic,Dynamic>::type Matrix;
00080 typedef typename Matrix_<3,3>::type Matrix3;
00081
00082 template <int RowsCols>
00083 struct SymmetricMatrix_ {
00084 typedef typename Matrix_<RowsCols,RowsCols>::type type;
00085 };
00086 typedef typename SymmetricMatrix_<3>::type SymmetricMatrix3;
00087 typedef typename SymmetricMatrix_<6>::type SymmetricMatrix6;
00088 typedef typename SymmetricMatrix_<Dynamic>::type SymmetricMatrix;
00089
00090 template <typename OtherDerived>
00091 static inline Matrix3 SkewSymmetricMatrix(const Eigen::MatrixBase<OtherDerived>& other) {
00092 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Eigen::MatrixBase<OtherDerived>, 3);
00093 Matrix3 result;
00094 result << 0.0, -other.z(), other.y(),
00095 other.z(), 0.0, -other.x(),
00096 -other.y(), other.x(), 0.0;
00097 return result;
00098 }
00099
00100 }
00101
00102 namespace Eigen {
00103
00104
00105
00106
00107
00108 #if !EIGEN_VERSION_AT_LEAST(3,0,91)
00109
00110 template<typename T, int _Options> class DenseStorage<T, 0, Dynamic, Dynamic, _Options>
00111 : public DenseStorage<T, 0, 0, 0, _Options> { };
00112
00113 template<typename T, int _Rows, int _Options> class DenseStorage<T, 0, _Rows, Dynamic, _Options>
00114 : public DenseStorage<T, 0, 0, 0, _Options> { };
00115
00116 template<typename T, int _Cols, int _Options> class DenseStorage<T, 0, Dynamic, _Cols, _Options>
00117 : public DenseStorage<T, 0, 0, 0, _Options> { };
00118 #endif
00119
00120 }
00121
00122 namespace hector_pose_estimation {
00123
00124 using Eigen::VectorBlock;
00125 typedef VectorBlock<ColumnVector,3> VectorBlock3;
00126 typedef VectorBlock<ColumnVector,4> VectorBlock4;
00127 typedef VectorBlock<const ColumnVector,3> ConstVectorBlock3;
00128 typedef VectorBlock<const ColumnVector,4> ConstVectorBlock4;
00129
00130 using Eigen::Block;
00131 typedef Eigen::Block<Matrix,Dynamic,Dynamic> MatrixBlock;
00132
00133 }
00134
00135 #endif // HECTOR_POSE_ESTIMATION_MATRIX_H