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 #ifndef mrpt_math_forwddecls_H
00029 #define mrpt_math_forwddecls_H
00030
00031 #include <mrpt/config.h>
00032 #include <mrpt/base/link_pragmas.h>
00033 #include <cmath>
00034 #include <cstdlib>
00035 #include <algorithm>
00036
00037
00038
00039
00040
00041
00042 namespace mrpt
00043 {
00044 namespace utils
00045 {
00046 class BASE_IMPEXP CStream;
00047 template<class T> inline T square(const T x);
00048 }
00049
00050 namespace system
00051 {
00052 std::string BASE_IMPEXP MRPT_getVersion();
00053 }
00054
00055 namespace poses
00056 {
00057 class CPoint2D;
00058 class CPoint3D;
00059 class CPose2D;
00060 class CPose3D;
00061 class CPose3DQuat;
00062 }
00063
00064 namespace math
00065 {
00066 struct TPoint2D;
00067 struct TPoint3D;
00068 struct TPose2D;
00069 struct TPose3D;
00070 struct TPose3DQuat;
00071
00072 namespace detail
00073 {
00074
00075 template <typename MAT,int TypeSizeAtCompileTime>
00076 struct TAuxResizer {
00077 static inline void internal_resize(MAT &obj, size_t row, size_t col) { }
00078 static inline void internal_resize(MAT &obj, size_t nsize) { }
00079 };
00080 template <typename MAT>
00081 struct TAuxResizer<MAT,-1> {
00082 static inline void internal_resize(MAT &obj, size_t row, size_t col) { obj.derived().conservativeResize(row,col); }
00083 static inline void internal_resize(MAT &obj, size_t nsize) { obj.derived().conservativeResize(nsize); }
00084 };
00085 }
00086
00087
00088
00089
00090 enum TMatrixTextFileFormat
00091 {
00092 MATRIX_FORMAT_ENG = 0,
00093 MATRIX_FORMAT_FIXED = 1,
00094 MATRIX_FORMAT_INT = 2
00095 };
00096
00097
00098
00099 enum TConstructorFlags_Matrices
00100 {
00101 UNINITIALIZED_MATRIX = 0
00102 };
00103
00104
00105 template <class T> class CMatrixTemplate;
00106 template <class T> class CMatrixTemplateObjects;
00107
00108 #define MRPT_MATRIX_CONSTRUCTORS_FROM_POSES(_CLASS_) \
00109 explicit inline _CLASS_( const mrpt::math::TPose2D &p) { mrpt::math::containerFromPoseOrPoint(*this,p); } \
00110 explicit inline _CLASS_( const mrpt::math::TPose3D &p) { mrpt::math::containerFromPoseOrPoint(*this,p); } \
00111 explicit inline _CLASS_( const mrpt::math::TPose3DQuat &p) { mrpt::math::containerFromPoseOrPoint(*this,p); } \
00112 explicit inline _CLASS_( const mrpt::math::TPoint2D &p) { mrpt::math::containerFromPoseOrPoint(*this,p); } \
00113 explicit inline _CLASS_( const mrpt::math::TPoint3D &p) { mrpt::math::containerFromPoseOrPoint(*this,p); } \
00114 explicit inline _CLASS_( const mrpt::poses::CPose2D &p) { mrpt::math::containerFromPoseOrPoint(*this,p); } \
00115 explicit inline _CLASS_( const mrpt::poses::CPose3D &p) { mrpt::math::containerFromPoseOrPoint(*this,p); } \
00116 explicit inline _CLASS_( const mrpt::poses::CPose3DQuat &p) { mrpt::math::containerFromPoseOrPoint(*this,p); } \
00117 explicit inline _CLASS_( const mrpt::poses::CPoint2D &p) { mrpt::math::containerFromPoseOrPoint(*this,p); } \
00118 explicit inline _CLASS_( const mrpt::poses::CPoint3D &p) { mrpt::math::containerFromPoseOrPoint(*this,p); }
00119
00120
00121 template <class CONTAINER1,class CONTAINER2> void cumsum(const CONTAINER1 &in_data, CONTAINER2 &out_cumsum);
00122
00123 template <class CONTAINER> inline typename CONTAINER::value_type norm(const CONTAINER &v);
00124 template <class CONTAINER> inline typename CONTAINER::value_type norm_inf(const CONTAINER &v);
00125
00126 template <class MAT_A,class SKEW_3VECTOR,class MAT_OUT> void multiply_A_skew3(const MAT_A &A,const SKEW_3VECTOR &v, MAT_OUT &out);
00127 template <class SKEW_3VECTOR,class MAT_A,class MAT_OUT> void multiply_skew3_A(const SKEW_3VECTOR &v,const MAT_A &A, MAT_OUT &out);
00128
00129 namespace detail
00130 {
00131
00132 TPoint2D BASE_IMPEXP lightFromPose(const mrpt::poses::CPoint2D &p);
00133 TPoint3D BASE_IMPEXP lightFromPose(const mrpt::poses::CPoint3D &p);
00134 TPose2D BASE_IMPEXP lightFromPose(const mrpt::poses::CPose2D &p);
00135 TPose3D BASE_IMPEXP lightFromPose(const mrpt::poses::CPose3D &p);
00136 TPose3DQuat BASE_IMPEXP lightFromPose(const mrpt::poses::CPose3DQuat &p);
00137
00138 template <class MATORG, class MATDEST>
00139 void extractMatrix(
00140 const MATORG &M,
00141 const size_t first_row,
00142 const size_t first_col,
00143 MATDEST &outMat);
00144 }
00145
00146
00147 template <class CONTAINER> CONTAINER & containerFromPoseOrPoint(CONTAINER &C, const TPoint2D &p);
00148 template <class CONTAINER> CONTAINER & containerFromPoseOrPoint(CONTAINER &C, const TPoint3D &p);
00149 template <class CONTAINER> CONTAINER & containerFromPoseOrPoint(CONTAINER &C, const TPose2D &p);
00150 template <class CONTAINER> CONTAINER & containerFromPoseOrPoint(CONTAINER &C, const TPose3D &p);
00151 template <class CONTAINER> CONTAINER & containerFromPoseOrPoint(CONTAINER &C, const TPose3DQuat &p);
00152
00153 template <class CONTAINER> inline CONTAINER & containerFromPoseOrPoint(CONTAINER &C, const mrpt::poses::CPoint2D &p) { return containerFromPoseOrPoint(C, mrpt::math::detail::lightFromPose(p)); }
00154 template <class CONTAINER> inline CONTAINER & containerFromPoseOrPoint(CONTAINER &C, const mrpt::poses::CPoint3D &p) { return containerFromPoseOrPoint(C, mrpt::math::detail::lightFromPose(p)); }
00155 template <class CONTAINER> inline CONTAINER & containerFromPoseOrPoint(CONTAINER &C, const mrpt::poses::CPose2D &p) { return containerFromPoseOrPoint(C, mrpt::math::detail::lightFromPose(p)); }
00156 template <class CONTAINER> inline CONTAINER & containerFromPoseOrPoint(CONTAINER &C, const mrpt::poses::CPose3D &p) { return containerFromPoseOrPoint(C, mrpt::math::detail::lightFromPose(p)); }
00157 template <class CONTAINER> inline CONTAINER & containerFromPoseOrPoint(CONTAINER &C, const mrpt::poses::CPose3DQuat &p) { return containerFromPoseOrPoint(C, mrpt::math::detail::lightFromPose(p)); }
00158
00159
00160
00161 namespace detail {
00162
00163
00164
00165
00166
00167
00168
00169
00170
00171 template<typename T> class VicinityTraits;
00172
00173
00174
00175
00176
00177
00178
00179
00180
00181
00182
00183 template<typename MatrixType,typename T,typename ReturnType,size_t D> struct getVicinity;
00184
00185 }
00186
00187
00188 template <class T> T wrapTo2Pi(T a);
00189
00190
00191 }
00192 }
00193
00194 #endif