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 CPOSE3D_H
00029 #define CPOSE3D_H
00030
00031 #include <mrpt/poses/CPose.h>
00032 #include <mrpt/math/CMatrixFixedNumeric.h>
00033 #include <mrpt/math/CQuaternion.h>
00034
00035 namespace mrpt
00036 {
00037 namespace poses
00038 {
00039 using namespace mrpt::math;
00040
00041 class BASE_IMPEXP CPose3DQuat;
00042
00043 DEFINE_SERIALIZABLE_PRE( CPose3D )
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071 class BASE_IMPEXP CPose3D : public CPose<CPose3D>, public mrpt::utils::CSerializable
00072 {
00073
00074 DEFINE_SERIALIZABLE( CPose3D )
00075
00076 public:
00077 CArrayDouble<3> m_coords;
00078 CMatrixDouble33 m_ROT;
00079
00080 protected:
00081 mutable bool m_ypr_uptodate;
00082 mutable double m_yaw, m_pitch, m_roll;
00083
00084
00085 void rebuildRotationMatrix();
00086
00087
00088 inline void updateYawPitchRoll() const { if (!m_ypr_uptodate) { m_ypr_uptodate=true; getYawPitchRoll( m_yaw, m_pitch, m_roll ); } }
00089
00090 public:
00091
00092
00093
00094
00095 CPose3D();
00096
00097
00098 CPose3D(const double x,const double y,const double z,const double yaw=0, const double pitch=0, const double roll=0);
00099
00100
00101 explicit CPose3D(const math::CMatrixDouble &m);
00102
00103
00104 explicit CPose3D(const math::CMatrixDouble44 &m);
00105
00106
00107 template <class MATRIX33,class VECTOR3>
00108 inline CPose3D(const MATRIX33 &rot, const VECTOR3& xyz) : m_ROT(UNINITIALIZED_MATRIX), m_ypr_uptodate(false)
00109 {
00110 ASSERT_EQUAL_(size(rot,1),3); ASSERT_EQUAL_(size(rot,2),3);ASSERT_EQUAL_(xyz.size(),3)
00111 for (int r=0;r<3;r++)
00112 for (int c=0;c<3;c++)
00113 m_ROT(r,c)=rot.get_unsafe(r,c);
00114 for (int r=0;r<3;r++) m_coords[r]=xyz[r];
00115 }
00116
00117 inline CPose3D(const CMatrixDouble33 &rot, const CArrayDouble<3>& xyz) : m_coords(xyz),m_ROT(rot), m_ypr_uptodate(false)
00118 { }
00119
00120
00121
00122 CPose3D(const CPose2D &);
00123
00124
00125
00126 CPose3D(const CPoint3D &);
00127
00128
00129
00130 CPose3D(const mrpt::math::TPose3D &);
00131
00132
00133 CPose3D(const mrpt::math::CQuaternionDouble &q, const double x, const double y, const double z );
00134
00135
00136 CPose3D(const CPose3DQuat &);
00137
00138
00139 inline CPose3D(TConstructorFlags_Poses constructor_dummy_param) : m_ROT(UNINITIALIZED_MATRIX), m_ypr_uptodate(false) { }
00140
00141
00142
00143
00144
00145 inline explicit CPose3D(const CArrayDouble<12> &vec12) : m_ROT( UNINITIALIZED_MATRIX ), m_ypr_uptodate(false) {
00146 setFrom12Vector(vec12);
00147 }
00148
00149
00150
00151
00152
00153
00154
00155
00156
00157
00158
00159 inline void getHomogeneousMatrix(CMatrixDouble44 & out_HM ) const
00160 {
00161 out_HM.insertMatrix(0,0,m_ROT);
00162 for (int i=0;i<3;i++) out_HM(i,3)=m_coords[i];
00163 out_HM(3,0)=out_HM(3,1)=out_HM(3,2)=0.; out_HM(3,3)=1.;
00164 }
00165
00166 inline CMatrixDouble44 getHomogeneousMatrixVal() const { CMatrixDouble44 M; getHomogeneousMatrix(M); return M;}
00167
00168
00169 inline void getRotationMatrix( mrpt::math::CMatrixDouble33 & ROT ) const { ROT = m_ROT; }
00170
00171 inline const mrpt::math::CMatrixDouble33 & getRotationMatrix() const { return m_ROT; }
00172
00173
00174
00175
00176
00177
00178
00179
00180 inline CPose3D operator + (const CPose3D& b) const
00181 {
00182 CPose3D ret(UNINITIALIZED_POSE);
00183 ret.composeFrom(*this,b);
00184 return ret;
00185 }
00186
00187
00188 CPoint3D operator + (const CPoint3D& b) const;
00189
00190
00191 CPoint3D operator + (const CPoint2D& b) const;
00192
00193
00194 void sphericalCoordinates(
00195 const TPoint3D &point,
00196 double &out_range,
00197 double &out_yaw,
00198 double &out_pitch ) const;
00199
00200
00201
00202
00203
00204
00205
00206 void composePoint(double lx,double ly,double lz, double &gx, double &gy, double &gz,
00207 mrpt::math::CMatrixFixedNumeric<double,3,3> *out_jacobian_df_dpoint=NULL,
00208 mrpt::math::CMatrixFixedNumeric<double,3,6> *out_jacobian_df_dpose=NULL,
00209 mrpt::math::CMatrixFixedNumeric<double,3,6> *out_jacobian_df_dse3=NULL,
00210 bool use_small_rot_approx = false) const;
00211
00212
00213
00214
00215 inline void composePoint(const TPoint3D local_point, TPoint3D &global_point) const {
00216 composePoint(local_point.x,local_point.y,local_point.z, global_point.x,global_point.y,global_point.z );
00217 }
00218
00219
00220 inline void composePoint(double lx,double ly,double lz, float &gx, float &gy, float &gz ) const {
00221 double ggx, ggy,ggz;
00222 composePoint(lx,ly,lz,ggx,ggy,ggz);
00223 gx = ggx; gy = ggy; gz = ggz;
00224 }
00225
00226
00227
00228
00229
00230
00231
00232 void inverseComposePoint(const double gx,const double gy,const double gz,double &lx,double &ly,double &lz,
00233 mrpt::math::CMatrixFixedNumeric<double,3,3> *out_jacobian_df_dpoint=NULL,
00234 mrpt::math::CMatrixFixedNumeric<double,3,6> *out_jacobian_df_dpose=NULL,
00235 mrpt::math::CMatrixFixedNumeric<double,3,6> *out_jacobian_df_dse3=NULL ) const;
00236
00237
00238
00239
00240 void composeFrom(const CPose3D& A, const CPose3D& B );
00241
00242
00243 inline CPose3D& operator += (const CPose3D& b)
00244 {
00245 composeFrom(*this,b);
00246 return *this;
00247 }
00248
00249
00250
00251
00252
00253 void inverseComposeFrom(const CPose3D& A, const CPose3D& B );
00254
00255
00256 inline CPose3D operator - (const CPose3D& b) const
00257 {
00258 CPose3D ret(UNINITIALIZED_POSE);
00259 ret.inverseComposeFrom(*this,b);
00260 return ret;
00261 }
00262
00263
00264 void inverse();
00265
00266
00267 inline void changeCoordinatesReference( const CPose3D & p ) { composeFrom(p,CPose3D(*this)); }
00268
00269
00270
00271
00272
00273
00274
00275
00276
00277
00278 void addComponents(const CPose3D &p);
00279
00280
00281
00282
00283 void normalizeAngles();
00284
00285
00286 void operator *=(const double s);
00287
00288
00289
00290
00291 void setFromValues(
00292 const double x0,
00293 const double y0,
00294 const double z0,
00295 const double yaw=0,
00296 const double pitch=0,
00297 const double roll=0);
00298
00299
00300
00301
00302 template <typename VECTORLIKE>
00303 inline void setFromXYZQ(
00304 const VECTORLIKE &v,
00305 const size_t index_offset = 0)
00306 {
00307 ASSERT_ABOVEEQ_(v.size(), 7+index_offset)
00308
00309 mrpt::math::CQuaternion<typename VECTORLIKE::value_type> q( v[index_offset+3],v[index_offset+4],v[index_offset+5],v[index_offset+6] );
00310 q.rotationMatrixNoResize(m_ROT);
00311 m_ypr_uptodate=false;
00312 m_coords[0] = v[index_offset+0];
00313 m_coords[1] = v[index_offset+1];
00314 m_coords[2] = v[index_offset+2];
00315 }
00316
00317
00318
00319
00320 inline void setYawPitchRoll(
00321 const double yaw_,
00322 const double pitch_,
00323 const double roll_)
00324 {
00325 setFromValues(x(),y(),z(),yaw_,pitch_,roll_);
00326 }
00327
00328
00329
00330
00331
00332 template <class ARRAYORVECTOR>
00333 inline void setFrom12Vector(const ARRAYORVECTOR &vec12)
00334 {
00335 m_ROT.set_unsafe(0,0, vec12[0]); m_ROT.set_unsafe(0,1, vec12[3]); m_ROT.set_unsafe(0,2, vec12[6]);
00336 m_ROT.set_unsafe(1,0, vec12[1]); m_ROT.set_unsafe(1,1, vec12[4]); m_ROT.set_unsafe(1,2, vec12[7]);
00337 m_ROT.set_unsafe(2,0, vec12[2]); m_ROT.set_unsafe(2,1, vec12[5]); m_ROT.set_unsafe(2,2, vec12[8]);
00338 m_coords[0] = vec12[ 9];
00339 m_coords[1] = vec12[10];
00340 m_coords[2] = vec12[11];
00341 }
00342
00343
00344
00345
00346
00347 template <class ARRAYORVECTOR>
00348 inline void getAs12Vector(ARRAYORVECTOR &vec12) const
00349 {
00350 vec12[0] = m_ROT.get_unsafe(0,0); vec12[3] = m_ROT.get_unsafe(0,1); vec12[6] = m_ROT.get_unsafe(0,2);
00351 vec12[1] = m_ROT.get_unsafe(1,0); vec12[4] = m_ROT.get_unsafe(1,1); vec12[7] = m_ROT.get_unsafe(1,2);
00352 vec12[2] = m_ROT.get_unsafe(2,0); vec12[5] = m_ROT.get_unsafe(2,1); vec12[8] = m_ROT.get_unsafe(2,2);
00353 vec12[ 9] = m_coords[0];
00354 vec12[10] = m_coords[1];
00355 vec12[11] = m_coords[2];
00356 }
00357
00358
00359
00360
00361 void getYawPitchRoll( double &yaw, double &pitch, double &roll ) const;
00362
00363 inline double yaw() const { updateYawPitchRoll(); return m_yaw; }
00364 inline double pitch() const { updateYawPitchRoll(); return m_pitch; }
00365 inline double roll() const { updateYawPitchRoll(); return m_roll; }
00366
00367
00368 void getAsVector(vector_double &v) const;
00369
00370
00371
00372
00373
00374
00375 void getAsQuaternion(
00376 mrpt::math::CQuaternionDouble &q,
00377 mrpt::math::CMatrixFixedNumeric<double,4,3> *out_dq_dr = NULL
00378 ) const;
00379
00380 inline const double &operator[](unsigned int i) const
00381 {
00382 updateYawPitchRoll();
00383 switch(i)
00384 {
00385 case 0:return m_coords[0];
00386 case 1:return m_coords[1];
00387 case 2:return m_coords[2];
00388 case 3:return m_yaw;
00389 case 4:return m_pitch;
00390 case 5:return m_roll;
00391 default:
00392 throw std::runtime_error("CPose3D::operator[]: Index of bounds.");
00393 }
00394 }
00395
00396
00397
00398
00399
00400
00401
00402 void asString(std::string &s) const { updateYawPitchRoll(); s = mrpt::format("[%f %f %f %f %f %f]",m_coords[0],m_coords[1],m_coords[2],RAD2DEG(m_yaw),RAD2DEG(m_pitch),RAD2DEG(m_roll)); }
00403 inline std::string asString() const { std::string s; asString(s); return s; }
00404
00405
00406
00407
00408
00409 void fromString(const std::string &s) {
00410 CMatrixDouble m;
00411 if (!m.fromMatlabStringFormat(s)) THROW_EXCEPTION("Malformed expression in ::fromString");
00412 ASSERTMSG_(mrpt::math::size(m,1)==1 && mrpt::math::size(m,2)==6, "Wrong size of vector in ::fromString");
00413 this->setFromValues(m.get_unsafe(0,0),m.get_unsafe(0,1),m.get_unsafe(0,2),DEG2RAD(m.get_unsafe(0,3)),DEG2RAD(m.get_unsafe(0,4)),DEG2RAD(m.get_unsafe(0,5)));
00414 }
00415
00416
00417 bool isHorizontal( const double tolerance=0) const;
00418
00419
00420 double distanceEuclidean6D( const CPose3D &o ) const;
00421
00422
00423
00424
00425
00426
00427
00428
00429
00430
00431 static CPose3D exp(const mrpt::math::CArrayNumeric<double,6> & vect);
00432
00433
00434
00435 static CMatrixDouble33 exp_rotation(const mrpt::math::CArrayNumeric<double,3> & vect);
00436
00437
00438
00439
00440
00441
00442 void ln(mrpt::math::CArrayDouble<6> &out_ln) const;
00443
00444
00445 inline mrpt::math::CArrayDouble<6> ln() const { mrpt::math::CArrayDouble<6> ret; ln(ret); return ret; }
00446
00447
00448
00449
00450
00451 void ln_jacob(mrpt::math::CMatrixFixedNumeric<double,6,12> &J) const;
00452
00453
00454
00455
00456 static void ln_rot_jacob(const CMatrixDouble33 &R, CMatrixFixedNumeric<double,3,9> &M);
00457
00458
00459
00460 CArrayDouble<3> ln_rotation() const;
00461
00462
00463
00464 typedef CPose3D type_value;
00465 enum { is_3D_val = 1 };
00466 static inline bool is_3D() { return is_3D_val!=0; }
00467 enum { rotation_dimensions = 3 };
00468 enum { is_PDF_val = 0 };
00469 static inline bool is_PDF() { return is_PDF_val!=0; }
00470
00471
00472
00473 typedef double value_type;
00474 typedef double& reference;
00475 typedef const double& const_reference;
00476 typedef std::size_t size_type;
00477 typedef std::ptrdiff_t difference_type;
00478
00479
00480
00481 enum { static_size = 6 };
00482 static inline size_type size() { return static_size; }
00483 static inline bool empty() { return false; }
00484 static inline size_type max_size() { return static_size; }
00485 static inline void resize(const size_t n) { if (n!=static_size) throw std::logic_error(format("Try to change the size of CPose3D to %u.",static_cast<unsigned>(n))); }
00486
00487
00488 };
00489
00490
00491 std::ostream BASE_IMPEXP & operator << (std::ostream& o, const CPose3D& p);
00492
00493
00494 CPose3D BASE_IMPEXP operator -(const CPose3D &p);
00495
00496 bool BASE_IMPEXP operator==(const CPose3D &p1,const CPose3D &p2);
00497 bool BASE_IMPEXP operator!=(const CPose3D &p1,const CPose3D &p2);
00498
00499
00500
00501
00502 template <class DERIVEDCLASS> CPose3D CPose<DERIVEDCLASS>::operator -(const CPose3D& b) const
00503 {
00504 CMatrixDouble44 B_INV(UNINITIALIZED_MATRIX);
00505 b.getInverseHomogeneousMatrix( B_INV );
00506 CMatrixDouble44 HM(UNINITIALIZED_MATRIX);
00507 static_cast<const DERIVEDCLASS*>(this)->getHomogeneousMatrix(HM);
00508 CMatrixDouble44 RES(UNINITIALIZED_MATRIX);
00509 RES.multiply(B_INV,HM);
00510 return CPose3D( RES );
00511 }
00512
00513
00514 }
00515 }
00516
00517 #endif