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
00033 namespace mrpt
00034 {
00035 namespace poses
00036 {
00037 DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE( CPose3D, CPose )
00038
00039
00158 class MRPTDLLIMPEXP CPose3D : public CPose
00159 {
00160 friend class CPose;
00161 friend class CPose2D;
00162 friend class CPoint;
00163 friend std::ostream MRPTDLLIMPEXP & operator << (std::ostream& o, const CPose3D& p);
00164
00165
00166 DEFINE_SERIALIZABLE( CPose3D )
00167
00168
00169 public:
00172 CPose3D(const double& x=0,const double& y=0,const double& z=0,const double& yaw=0, const double& pitch=0, const double& roll=0);
00173
00176 CPose3D( const CPose3D &o);
00177
00180 CPose3D & operator=( const CPose3D &o);
00181
00184 CPose3D(const math::CMatrixDouble &m);
00185
00188 CPose3D(const CPose2D &);
00189
00192 CPose3D(const CPoint3D &);
00193
00199 const math::CMatrixDouble& getHomogeneousMatrix() const;
00200
00206 void getHomogeneousMatrix(const math::CMatrixDouble *& ptrHM ) const;
00207
00208
00213 void addComponents(const CPose3D &p);
00214
00218 void normalizeAngles();
00219
00222 void operator *=(const double & s);
00223
00227 void sphericalCoordinates(
00228 const CPoint3D &point,
00229 double &out_range,
00230 double &out_yaw,
00231 double &out_pitch ) const;
00232
00236 void setFromValues(
00237 const double &x0,
00238 const double &y0,
00239 const double &z0,
00240 const double &yaw=0,
00241 const double &pitch=0,
00242 const double &roll=0);
00243
00247 void getYawPitchRoll( double &yaw, double &pitch, double &roll );
00248
00251 double yaw, pitch, roll;
00252
00255 double distanceEuclidean6D( const CPose3D &o ) const;
00256
00257 protected:
00258
00260 void rebuildHomogeneousMatrix();
00261
00264 math::CMatrixD HM;
00265
00266 };
00267
00268
00269 std::ostream MRPTDLLIMPEXP & operator << (std::ostream& o, const CPose3D& p);
00270
00271 }
00272 }
00273
00274 #endif