Go to the documentation of this file.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 CPOSE2D_H
00029 #define CPOSE2D_H
00030
00031 #include <mrpt/poses/CPose.h>
00032
00033 namespace mrpt
00034 {
00035 namespace poses
00036 {
00037 DEFINE_SERIALIZABLE_PRE( CPose2D )
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052 class BASE_IMPEXP CPose2D : public CPose<CPose2D>, public mrpt::utils::CSerializable
00053 {
00054 public:
00055
00056 DEFINE_SERIALIZABLE( CPose2D )
00057
00058 public:
00059 mrpt::math::CArrayDouble<2> m_coords;
00060
00061 protected:
00062 double m_phi;
00063
00064 public:
00065
00066 CPose2D();
00067
00068
00069 CPose2D(const double x,const double y,const double phi);
00070
00071
00072 CPose2D(const CPoint2D &);
00073
00074
00075 explicit CPose2D(const CPose3D &);
00076
00077
00078 CPose2D(const mrpt::math::TPose2D &);
00079
00080
00081 explicit CPose2D(const CPoint3D &);
00082
00083
00084 inline CPose2D(TConstructorFlags_Poses constructor_dummy_param) { }
00085
00086
00087 inline const double &phi() const { return m_phi; }
00088
00089 inline double &phi() { return m_phi; }
00090
00091
00092 inline void phi(double angle) { m_phi=angle; }
00093
00094 inline void phi_incr(const double Aphi) { m_phi+=Aphi; }
00095
00096
00097 void getAsVector(vector_double &v) const;
00098
00099
00100
00101
00102 void getHomogeneousMatrix(CMatrixDouble44 & out_HM ) const;
00103
00104
00105
00106 CPose2D operator + (const CPose2D& D) const ;
00107
00108
00109
00110
00111 void composeFrom(const CPose2D &A, const CPose2D &B);
00112
00113
00114
00115 CPose3D operator + (const CPose3D& D) const ;
00116
00117
00118
00119 CPoint2D operator + (const CPoint2D& u) const ;
00120
00121
00122 void composePoint(double lx,double ly,double &gx, double &gy) const;
00123
00124
00125
00126 CPoint3D operator + (const CPoint3D& u) const ;
00127
00128
00129
00130
00131
00132 void inverseComposeFrom(const CPose2D& A, const CPose2D& B );
00133
00134
00135 inline CPose2D operator - (const CPose2D& b) const
00136 {
00137 CPose2D ret(UNINITIALIZED_POSE);
00138 ret.inverseComposeFrom(*this,b);
00139 return ret;
00140 }
00141
00142
00143
00144
00145 void AddComponents(CPose2D &p);
00146
00147
00148
00149 void operator *=(const double s);
00150
00151
00152 inline CPose2D& operator += (const CPose2D& b)
00153 {
00154 composeFrom(*this,b);
00155 return *this;
00156 }
00157
00158
00159
00160 void normalizePhi();
00161
00162
00163
00164
00165 void asString(std::string &s) const { s = mrpt::format("[%f %f %f]",x(),y(),RAD2DEG(m_phi)); }
00166 inline std::string asString() const { std::string s; asString(s); return s; }
00167
00168
00169
00170
00171
00172 void fromString(const std::string &s) {
00173 CMatrixDouble m;
00174 if (!m.fromMatlabStringFormat(s)) THROW_EXCEPTION("Malformed expression in ::fromString");
00175 ASSERTMSG_(mrpt::math::size(m,1)==1 && mrpt::math::size(m,2)==3, "Wrong size of vector in ::fromString");
00176 x( m.get_unsafe(0,0) );
00177 y( m.get_unsafe(0,1) );
00178 phi( DEG2RAD(m.get_unsafe(0,2)) );
00179 }
00180
00181 inline const double &operator[](unsigned int i)const
00182 {
00183 switch(i)
00184 {
00185 case 0:return m_coords[0];
00186 case 1:return m_coords[1];
00187 case 2:return m_phi;
00188 default:
00189 throw std::runtime_error("CPose2D::operator[]: Index of bounds.");
00190 }
00191 }
00192 inline double &operator[](unsigned int i)
00193 {
00194 switch(i)
00195 {
00196 case 0:return m_coords[0];
00197 case 1:return m_coords[1];
00198 case 2:return m_phi;
00199 default:
00200 throw std::runtime_error("CPose2D::operator[]: Index of bounds.");
00201 }
00202 }
00203
00204
00205 inline void changeCoordinatesReference( const CPose2D & p ) { composeFrom(p,CPose2D(*this)); }
00206
00207 typedef CPose2D type_value;
00208 enum { is_3D_val = 0 };
00209 static inline bool is_3D() { return is_3D_val!=0; }
00210 enum { rotation_dimensions = 2 };
00211 enum { is_PDF_val = 0 };
00212 static inline bool is_PDF() { return is_PDF_val!=0; }
00213
00214
00215
00216 typedef double value_type;
00217 typedef double& reference;
00218 typedef const double& const_reference;
00219 typedef std::size_t size_type;
00220 typedef std::ptrdiff_t difference_type;
00221
00222
00223 enum { static_size = 3 };
00224 static inline size_type size() { return static_size; }
00225 static inline bool empty() { return false; }
00226 static inline size_type max_size() { return static_size; }
00227 static inline void resize(const size_t n) { if (n!=static_size) throw std::logic_error(format("Try to change the size of CPose2D to %u.",static_cast<unsigned>(n))); }
00228
00229
00230
00231 };
00232
00233
00234 std::ostream BASE_IMPEXP & operator << (std::ostream& o, const CPose2D& p);
00235
00236
00237 CPose2D BASE_IMPEXP operator -(const CPose2D &p);
00238
00239 mrpt::math::TPoint2D BASE_IMPEXP operator +(const CPose2D &pose, const mrpt::math::TPoint2D &pnt);
00240
00241 bool BASE_IMPEXP operator==(const CPose2D &p1,const CPose2D &p2);
00242 bool BASE_IMPEXP operator!=(const CPose2D &p1,const CPose2D &p2);
00243
00244 typedef mrpt::aligned_containers<CPose2D>::vector_t StdVector_CPose2D;
00245 typedef mrpt::aligned_containers<CPose2D>::deque_t StdDeque_CPose2D;
00246
00247 }
00248 }
00249
00250 #endif