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 CPose3DQuat_H
00029 #define CPose3DQuat_H
00030
00031 #include <mrpt/poses/CPose.h>
00032 #include <mrpt/math/CMatrixFixedNumeric.h>
00033 #include <mrpt/math/CQuaternion.h>
00034 #include <mrpt/poses/CPoint3D.h>
00035 #include <mrpt/math/lightweight_geom_data.h>
00036
00037 namespace mrpt
00038 {
00039 namespace poses
00040 {
00041 using namespace mrpt::math;
00042
00043 class CPose3D;
00044
00045 DEFINE_SERIALIZABLE_PRE( CPose3DQuat )
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062 class BASE_IMPEXP CPose3DQuat : public CPose<CPose3DQuat>, public mrpt::utils::CSerializable
00063 {
00064
00065 DEFINE_SERIALIZABLE( CPose3DQuat )
00066
00067 public:
00068 CArrayDouble<3> m_coords;
00069 mrpt::math::CQuaternionDouble m_quat;
00070
00071 public:
00072
00073 inline mrpt::math::CQuaternionDouble & quat() { return m_quat; }
00074
00075 inline const mrpt::math::CQuaternionDouble & quat() const { return m_quat; }
00076
00077
00078 inline mrpt::math::CArrayDouble<3> & xyz() { return m_coords; }
00079
00080 inline const mrpt::math::CArrayDouble<3> & xyz() const { return m_coords; }
00081
00082
00083
00084 inline CPose3DQuat() : m_quat() { m_coords[0]=m_coords[1]=m_coords[2]=0.; }
00085
00086
00087 inline CPose3DQuat(TConstructorFlags_Quaternions constructor_dummy_param) : m_quat(UNINITIALIZED_QUATERNION) { }
00088
00089
00090 inline CPose3DQuat(const double x,const double y,const double z,const mrpt::math::CQuaternionDouble &q ) : m_quat(q) { m_coords[0]=x; m_coords[1]=y; m_coords[2]=z; m_quat.normalize(); }
00091
00092
00093 CPose3DQuat(const CPose3D &p);
00094
00095
00096 CPose3DQuat(const mrpt::math::TPose3DQuat &p) : m_quat(p.qr,p.qx,p.qy,p.qz) { x()=p.x; y()=p.y; z()=p.z; }
00097
00098
00099
00100 explicit CPose3DQuat(const CMatrixDouble44 &M);
00101
00102
00103
00104
00105 void getHomogeneousMatrix(CMatrixDouble44 & out_HM ) const;
00106
00107
00108 void getAsVector(vector_double &v) const;
00109
00110
00111
00112
00113
00114 void composeFrom(const CPose3DQuat& A, const CPose3DQuat& B );
00115
00116
00117
00118
00119
00120 void inverseComposeFrom(const CPose3DQuat& A, const CPose3DQuat& B );
00121
00122
00123
00124
00125 void composePoint(const double lx,const double ly,const double lz,double &gx,double &gy,double &gz,
00126 mrpt::math::CMatrixFixedNumeric<double,3,3> *out_jacobian_df_dpoint = NULL,
00127 mrpt::math::CMatrixFixedNumeric<double,3,7> *out_jacobian_df_dpose = NULL ) const;
00128
00129
00130
00131
00132 void inverseComposePoint(const double gx,const double gy,const double gz,double &lx,double &ly,double &lz,
00133 mrpt::math::CMatrixFixedNumeric<double,3,3> *out_jacobian_df_dpoint = NULL,
00134 mrpt::math::CMatrixFixedNumeric<double,3,7> *out_jacobian_df_dpose = NULL ) const;
00135
00136
00137
00138
00139 template <class POINT1,class POINT2> inline void composePoint( const POINT1 &L, POINT2 &G) const { composePoint(L[0],L[1],L[2], G[0],G[1],G[2]); }
00140
00141
00142 template <class POINT1,class POINT2> inline void inverseComposePoint( const POINT1 &G, POINT2 &L) const { inverseComposePoint(G[0],G[1],G[2],L[0],L[1],L[2]); }
00143
00144
00145 inline CPoint3D operator +( const CPoint3D &L) const { CPoint3D G; composePoint(L[0],L[1],L[2], G[0],G[1],G[2]); return G; }
00146
00147
00148 inline TPoint3D operator +( const TPoint3D &L) const { TPoint3D G; composePoint(L[0],L[1],L[2], G[0],G[1],G[2]); return G; }
00149
00150
00151 inline CPoint3D operator -( const CPoint3D &G) const { CPoint3D L; inverseComposePoint(G[0],G[1],G[2], L[0],L[1],L[2]); return L; }
00152
00153
00154 inline TPoint3D operator -( const TPoint3D &G) const { TPoint3D L; inverseComposePoint(G[0],G[1],G[2], L[0],L[1],L[2]); return L; }
00155
00156
00157
00158 virtual void operator *=(const double s);
00159
00160
00161 inline CPose3DQuat& operator += (const CPose3DQuat& b)
00162 {
00163 composeFrom(*this,b);
00164 return *this;
00165 }
00166
00167
00168 inline CPose3DQuat operator + (const CPose3DQuat& p) const
00169 {
00170 CPose3DQuat ret;
00171 ret.composeFrom(*this,p);
00172 return ret;
00173 }
00174
00175
00176 inline CPose3DQuat& operator -= (const CPose3DQuat& b)
00177 {
00178 inverseComposeFrom(*this,b);
00179 return *this;
00180 }
00181
00182
00183 inline CPose3DQuat operator - (const CPose3DQuat& p) const
00184 {
00185 CPose3DQuat ret;
00186 ret.inverseComposeFrom(*this,p);
00187 return ret;
00188 }
00189
00190
00191
00192
00193 void asString(std::string &s) const { s = mrpt::format("[%f %f %f %f %f %f %f]",m_coords[0],m_coords[1],m_coords[2],m_quat[0],m_quat[1],m_quat[2],m_quat[3]); }
00194 inline std::string asString() const { std::string s; asString(s); return s; }
00195
00196
00197
00198
00199
00200 void fromString(const std::string &s) {
00201 CMatrixDouble m;
00202 if (!m.fromMatlabStringFormat(s)) THROW_EXCEPTION("Malformed expression in ::fromString");
00203 ASSERTMSG_(mrpt::math::size(m,1)==1 && mrpt::math::size(m,2)==7, "Wrong size of vector in ::fromString");
00204 m_coords[0] = m.get_unsafe(0,0); m_coords[1] = m.get_unsafe(0,1); m_coords[2] = m.get_unsafe(0,2);
00205 m_quat[0] = m.get_unsafe(0,3); m_quat[1] = m.get_unsafe(0,4); m_quat[2] = m.get_unsafe(0,5); m_quat[3] = m.get_unsafe(0,6);
00206 }
00207
00208
00209 inline const double &operator[](unsigned int i) const
00210 {
00211 switch(i)
00212 {
00213 case 0:return m_coords[0];
00214 case 1:return m_coords[1];
00215 case 2:return m_coords[2];
00216 case 3:return m_quat[0];
00217 case 4:return m_quat[1];
00218 case 5:return m_quat[2];
00219 case 6:return m_quat[3];
00220 default:
00221 throw std::runtime_error("CPose3DQuat::operator[]: Index of bounds.");
00222 }
00223 }
00224
00225 inline double &operator[](unsigned int i)
00226 {
00227 switch(i)
00228 {
00229 case 0:return m_coords[0];
00230 case 1:return m_coords[1];
00231 case 2:return m_coords[2];
00232 case 3:return m_quat[0];
00233 case 4:return m_quat[1];
00234 case 5:return m_quat[2];
00235 case 6:return m_quat[3];
00236 default:
00237 throw std::runtime_error("CPose3DQuat::operator[]: Index of bounds.");
00238 }
00239 }
00240
00241
00242
00243
00244
00245 void sphericalCoordinates(
00246 const TPoint3D &point,
00247 double &out_range,
00248 double &out_yaw,
00249 double &out_pitch,
00250 mrpt::math::CMatrixFixedNumeric<double,3,3> *out_jacob_dryp_dpoint = NULL,
00251 mrpt::math::CMatrixFixedNumeric<double,3,7> *out_jacob_dryp_dpose = NULL
00252 ) const;
00253
00254 public:
00255 typedef CPose3DQuat type_value;
00256 enum { is_3D_val = 1 };
00257 static inline bool is_3D() { return is_3D_val!=0; }
00258 enum { rotation_dimensions = 3 };
00259 enum { is_PDF_val = 1 };
00260 static inline bool is_PDF() { return is_PDF_val!=0; }
00261
00262
00263
00264
00265 typedef double value_type;
00266 typedef double& reference;
00267 typedef const double& const_reference;
00268 typedef std::size_t size_type;
00269 typedef std::ptrdiff_t difference_type;
00270
00271
00272 enum { static_size = 7 };
00273 static inline size_type size() { return static_size; }
00274 static inline bool empty() { return false; }
00275 static inline size_type max_size() { return static_size; }
00276 static inline void resize(const size_t n) { if (n!=static_size) throw std::logic_error(format("Try to change the size of CPose3DQuat to %u.",static_cast<unsigned>(n))); }
00277
00278 inline void assign(const size_t N, const double val)
00279 {
00280 if (N!=7) throw std::runtime_error("CPose3DQuat::assign: Try to resize to length!=7.");
00281 m_coords.fill(val);
00282 m_quat.fill(val);
00283 }
00284
00285 struct iterator : public std::iterator<std::random_access_iterator_tag,value_type>
00286 {
00287 private:
00288 typedef std::iterator<std::random_access_iterator_tag,value_type> iterator_base;
00289 CPose3DQuat *m_obj;
00290 size_t m_cur_idx;
00291 typedef value_type T;
00292
00293 inline void check_limits(bool allow_end = false) const
00294 {
00295 #ifdef _DEBUG
00296 ASSERTMSG_(m_obj!=NULL,"non initialized iterator");
00297 if (m_cur_idx> (allow_end ? 7u : 6u) ) THROW_EXCEPTION("Index out of range in iterator.")
00298 #endif
00299 }
00300 public:
00301 inline bool operator <(const iterator &it2) const { return m_cur_idx < it2.m_cur_idx; }
00302 inline bool operator >(const iterator &it2) const { return m_cur_idx > it2.m_cur_idx; }
00303 inline iterator() : m_obj(NULL),m_cur_idx(0) { }
00304 inline iterator(CPose3DQuat &obj, size_t start_idx) : m_obj(&obj),m_cur_idx(start_idx) { check_limits(true); }
00305 inline CPose3DQuat::reference operator*() const { check_limits(); return (*m_obj)[m_cur_idx]; }
00306 inline iterator &operator++() {
00307 check_limits();
00308 ++m_cur_idx;
00309 return *this;
00310 }
00311 inline iterator operator++(int) {
00312 iterator it=*this;
00313 ++*this;
00314 return it;
00315 }
00316 inline iterator &operator--() {
00317 --m_cur_idx;
00318 check_limits();
00319 return *this;
00320 }
00321 inline iterator operator--(int) {
00322 iterator it=*this;
00323 --*this;
00324 return it;
00325 }
00326 inline iterator &operator+=(iterator_base::difference_type off) {
00327 m_cur_idx+=off;
00328 check_limits(true);
00329 return *this;
00330 }
00331 inline iterator operator+(iterator_base::difference_type off) const {
00332 iterator it=*this;
00333 it+=off;
00334 return it;
00335 }
00336 inline iterator &operator-=(iterator_base::difference_type off) {
00337 return (*this)+=(-off);
00338 }
00339 inline iterator operator-(iterator_base::difference_type off) const {
00340 iterator it=*this;
00341 it-=off;
00342 return it;
00343 }
00344 inline iterator_base::difference_type operator-(const iterator &it) const { return m_cur_idx - it.m_cur_idx; }
00345 inline CPose3DQuat::reference operator[](iterator_base::difference_type off) const { return (*m_obj)[m_cur_idx+off]; }
00346 inline bool operator==(const iterator &it) const { return m_obj==it.m_obj && m_cur_idx==it.m_cur_idx; }
00347 inline bool operator!=(const iterator &it) const { return !(operator==(it)); }
00348 };
00349
00350 struct const_iterator : public std::iterator<std::random_access_iterator_tag,value_type>
00351 {
00352 private:
00353 typedef std::iterator<std::random_access_iterator_tag,value_type> iterator_base;
00354 const CPose3DQuat *m_obj;
00355 size_t m_cur_idx;
00356 typedef value_type T;
00357
00358 inline void check_limits(bool allow_end = false) const
00359 {
00360 #ifdef _DEBUG
00361 ASSERTMSG_(m_obj!=NULL,"non initialized iterator");
00362 if (m_cur_idx> (allow_end ? 7u : 6u) ) THROW_EXCEPTION("Index out of range in iterator.")
00363 #endif
00364 }
00365 public:
00366 inline bool operator <(const const_iterator &it2) const { return m_cur_idx < it2.m_cur_idx; }
00367 inline bool operator >(const const_iterator &it2) const { return m_cur_idx > it2.m_cur_idx; }
00368 inline const_iterator() : m_obj(NULL),m_cur_idx(0) { }
00369 inline const_iterator(const CPose3DQuat &obj, size_t start_idx) : m_obj(&obj),m_cur_idx(start_idx) { check_limits(true); }
00370 inline CPose3DQuat::const_reference operator*() const { check_limits(); return (*m_obj)[m_cur_idx]; }
00371 inline const_iterator &operator++() {
00372 check_limits();
00373 ++m_cur_idx;
00374 return *this;
00375 }
00376 inline const_iterator operator++(int) {
00377 const_iterator it=*this;
00378 ++*this;
00379 return it;
00380 }
00381 inline const_iterator &operator--() {
00382 --m_cur_idx;
00383 check_limits();
00384 return *this;
00385 }
00386 inline const_iterator operator--(int) {
00387 const_iterator it=*this;
00388 --*this;
00389 return it;
00390 }
00391 inline const_iterator &operator+=(iterator_base::difference_type off) {
00392 m_cur_idx+=off;
00393 check_limits(true);
00394 return *this;
00395 }
00396 inline const_iterator operator+(iterator_base::difference_type off) const {
00397 const_iterator it=*this;
00398 it+=off;
00399 return it;
00400 }
00401 inline const_iterator &operator-=(iterator_base::difference_type off) {
00402 return (*this)+=(-off);
00403 }
00404 inline const_iterator operator-(iterator_base::difference_type off) const {
00405 const_iterator it=*this;
00406 it-=off;
00407 return it;
00408 }
00409 inline iterator_base::difference_type operator-(const const_iterator &it) const { return m_cur_idx - it.m_cur_idx; }
00410 inline CPose3DQuat::const_reference operator[](iterator_base::difference_type off) const { return (*m_obj)[m_cur_idx+off]; }
00411 inline bool operator==(const const_iterator &it) const { return m_obj==it.m_obj && m_cur_idx==it.m_cur_idx; }
00412 inline bool operator!=(const const_iterator &it) const { return !(operator==(it)); }
00413 };
00414
00415 typedef std::reverse_iterator<iterator> reverse_iterator;
00416 typedef std::reverse_iterator<const_iterator> const_reverse_iterator;
00417 inline iterator begin() { return iterator(*this,0); }
00418 inline iterator end() { return iterator(*this,static_size); }
00419 inline const_iterator begin() const { return const_iterator(*this,0); }
00420 inline const_iterator end() const { return const_iterator(*this,static_size); }
00421 inline reverse_iterator rbegin() { return reverse_iterator(end()); }
00422 inline const_reverse_iterator rbegin() const { return const_reverse_iterator(end()); }
00423 inline reverse_iterator rend() { return reverse_iterator(begin()); }
00424 inline const_reverse_iterator rend() const { return const_reverse_iterator(begin()); }
00425
00426
00427 void swap (CPose3DQuat& o)
00428 {
00429 std::swap(o.m_coords, m_coords);
00430 o.m_quat.swap(m_quat);
00431 }
00432
00433
00434
00435 typedef CPose3DQuat mrpt_autotype;
00436
00437
00438 };
00439
00440 std::ostream BASE_IMPEXP & operator << (std::ostream& o, const CPose3DQuat& p);
00441
00442
00443 }
00444 }
00445
00446 #endif