Main MRPT website > C++ reference for MRPT 1.4.0
CPose3DQuat.h
Go to the documentation of this file.
1/* +---------------------------------------------------------------------------+
2 | Mobile Robot Programming Toolkit (MRPT) |
3 | http://www.mrpt.org/ |
4 | |
5 | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file |
6 | See: http://www.mrpt.org/Authors - All rights reserved. |
7 | Released under BSD License. See details in http://www.mrpt.org/License |
8 +---------------------------------------------------------------------------+ */
9#ifndef CPose3DQuat_H
10#define CPose3DQuat_H
11
12#include <mrpt/poses/CPose.h>
15#include <mrpt/poses/CPoint3D.h>
18
19namespace mrpt
20{
21namespace poses
22{
24
25 /** A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
26 *
27 * For a complete description of Points/Poses, see mrpt::poses::CPoseOrPoint, or refer
28 * to the <a href="http://www.mrpt.org/2D_3D_Geometry"> 2D/3D Geometry tutorial</a> in the wiki.
29 *
30 * To access the translation use x(), y() and z(). To access the rotation, use CPose3DQuat::quat().
31 *
32 * This class also behaves like a STL container, since it has begin(), end(), iterators, and can be accessed with the [] operator
33 * with indices running from 0 to 6 to access the [x y z qr qx qy qz] as if they were a vector. Thus, a CPose3DQuat can be used
34 * as a 7-vector anywhere the MRPT math functions expect any kind of vector.
35 *
36 * This class and CPose3D are very similar, and they can be converted to the each other automatically via transformation constructors.
37 *
38 * \sa CPose3D (for a class based on a 4x4 matrix instead of a quaternion), mrpt::math::TPose3DQuat, mrpt::poses::CPose3DQuatPDF for a probabilistic version of this class, mrpt::math::CQuaternion, CPoseOrPoint
39 * \ingroup poses_grp
40 */
41 class BASE_IMPEXP CPose3DQuat : public CPose<CPose3DQuat>, public mrpt::utils::CSerializable
42 {
43 // This must be added to any CSerializable derived class:
45
46 public:
47 mrpt::math::CArrayDouble<3> m_coords; //!< The translation vector [x,y,z]
49
50 public:
51 /** Read/Write access to the quaternion representing the 3D rotation. */
52 inline mrpt::math::CQuaternionDouble & quat() { return m_quat; }
53 /** Read-only access to the quaternion representing the 3D rotation. */
54 inline const mrpt::math::CQuaternionDouble & quat() const { return m_quat; }
55
56 /** Read/Write access to the translation vector in R^3. */
57 inline mrpt::math::CArrayDouble<3> & xyz() { return m_coords; }
58 /** Read-only access to the translation vector in R^3. */
59 inline const mrpt::math::CArrayDouble<3> & xyz() const { return m_coords; }
60
61
62 /** Default constructor, initialize translation to zeros and quaternion to no rotation. */
63 inline CPose3DQuat() : m_quat() { m_coords[0]=m_coords[1]=m_coords[2]=0.; }
64
65 /** Constructor which left all the quaternion members un-initialized, for use when speed is critical; Use UNINITIALIZED_POSE as argument to this constructor. */
66 inline CPose3DQuat(mrpt::math::TConstructorFlags_Quaternions ) : m_quat(mrpt::math::UNINITIALIZED_QUATERNION) { }
67 /** \overload */
68 inline CPose3DQuat(TConstructorFlags_Poses ) : m_quat(mrpt::math::UNINITIALIZED_QUATERNION) { }
69
70 /** Constructor with initilization of the pose - the quaternion is normalized to make sure it's unitary */
71 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(); }
72
73 /** Constructor from a CPose3D */
74 explicit CPose3DQuat(const CPose3D &p);
75
76 /** Constructor from lightweight object. */
77 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; }
78
79 /** Constructor from a 4x4 homogeneous transformation matrix.
80 */
82
83 /** Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (translation+orientation).
84 * \sa getInverseHomogeneousMatrix
85 */
87
88 /** Returns a 1x7 vector with [x y z qr qx qy qz] */
90 /// \overload
92 v[0] = m_coords[0]; v[1] = m_coords[1]; v[2] = m_coords[2];
93 v[3] = m_quat[0]; v[4] = m_quat[1]; v[5] = m_quat[2]; v[6] = m_quat[3];
94 }
95
96 /** Makes \f$ this = A \oplus B \f$ this method is slightly more efficient than "this= A + B;" since it avoids the temporary object.
97 * \note A or B can be "this" without problems.
98 * \sa inverseComposeFrom, composePoint
99 */
100 void composeFrom(const CPose3DQuat& A, const CPose3DQuat& B );
101
102 /** Makes \f$ this = A \ominus B \f$ this method is slightly more efficient than "this= A - B;" since it avoids the temporary object.
103 * \note A or B can be "this" without problems.
104 * \sa composeFrom, composePoint
105 */
106 void inverseComposeFrom(const CPose3DQuat& A, const CPose3DQuat& B );
107
108 /** Computes the 3D point G such as \f$ G = this \oplus L \f$.
109 * \sa composeFrom, inverseComposePoint
110 */
111 void composePoint(const double lx,const double ly,const double lz,double &gx,double &gy,double &gz,
112 mrpt::math::CMatrixFixedNumeric<double,3,3> *out_jacobian_df_dpoint = NULL,
113 mrpt::math::CMatrixFixedNumeric<double,3,7> *out_jacobian_df_dpose = NULL ) const;
114
115 /** Computes the 3D point L such as \f$ L = G \ominus this \f$.
116 * \sa composePoint, composeFrom
117 */
118 void inverseComposePoint(const double gx,const double gy,const double gz,double &lx,double &ly,double &lz,
119 mrpt::math::CMatrixFixedNumeric<double,3,3> *out_jacobian_df_dpoint = NULL,
120 mrpt::math::CMatrixFixedNumeric<double,3,7> *out_jacobian_df_dpose = NULL ) const;
121
122 /** Computes the 3D point G such as \f$ G = this \oplus L \f$.
123 * POINT1 and POINT1 can be anything supporing [0],[1],[2].
124 * \sa composePoint */
125 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]); }
126
127 /** Computes the 3D point L such as \f$ L = G \ominus this \f$. \sa inverseComposePoint */
128 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]); }
129
130 /** Computes the 3D point G such as \f$ G = this \oplus L \f$. \sa composePoint */
131 inline CPoint3D operator +( const CPoint3D &L) const { CPoint3D G; composePoint(L[0],L[1],L[2], G[0],G[1],G[2]); return G; }
132
133 /** Computes the 3D point G such as \f$ G = this \oplus L \f$. \sa composePoint */
134 inline mrpt::math::TPoint3D operator +( const mrpt::math::TPoint3D &L) const { mrpt::math::TPoint3D G; composePoint(L[0],L[1],L[2], G[0],G[1],G[2]); return G; }
135
136 /** Computes the 3D point L such as \f$ L = G \ominus this \f$. \sa inverseComposePoint */
137 inline CPoint3D operator -( const CPoint3D &G) const { CPoint3D L; inverseComposePoint(G[0],G[1],G[2], L[0],L[1],L[2]); return L; }
138
139 /** Computes the 3D point L such as \f$ L = G \ominus this \f$. \sa inverseComposePoint */
140 inline mrpt::math::TPoint3D operator -( const mrpt::math::TPoint3D &G) const { mrpt::math::TPoint3D L; inverseComposePoint(G[0],G[1],G[2], L[0],L[1],L[2]); return L; }
141
142 /** Scalar multiplication (all x y z qr qx qy qz elements are multiplied by the scalar).
143 */
144 virtual void operator *=(const double s);
145
146 /** Make \f$ this = this \oplus b \f$ */
147 inline CPose3DQuat& operator += (const CPose3DQuat& b)
148 {
149 composeFrom(*this,b);
150 return *this;
151 }
152
153 /** Return the composed pose \f$ ret = this \oplus p \f$ */
154 inline CPose3DQuat operator + (const CPose3DQuat& p) const
155 {
156 CPose3DQuat ret;
157 ret.composeFrom(*this,p);
158 return ret;
159 }
160
161 /** Make \f$ this = this \ominus b \f$ */
162 inline CPose3DQuat& operator -= (const CPose3DQuat& b)
163 {
164 inverseComposeFrom(*this,b);
165 return *this;
166 }
167
168 /** Return the composed pose \f$ ret = this \ominus p \f$ */
169 inline CPose3DQuat operator - (const CPose3DQuat& p) const
170 {
171 CPose3DQuat ret;
172 ret.inverseComposeFrom(*this,p);
173 return ret;
174 }
175
176 /** Convert this pose into its inverse, saving the result in itself. \sa operator- */
177 void inverse();
178
179 /** Returns a human-readable textual representation of the object (eg: "[x y z qr qx qy qz]", angles in degrees.)
180 * \sa fromString
181 */
182 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]); }
183 inline std::string asString() const { std::string s; asString(s); return s; }
184
185 /** Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04 -0.8 1 0 0 0]" )
186 * \sa asString
187 * \exception std::exception On invalid format
188 */
189 void fromString(const std::string &s) {
191 if (!m.fromMatlabStringFormat(s)) THROW_EXCEPTION("Malformed expression in ::fromString");
192 ASSERTMSG_(mrpt::math::size(m,1)==1 && mrpt::math::size(m,2)==7, "Wrong size of vector in ::fromString");
193 m_coords[0] = m.get_unsafe(0,0); m_coords[1] = m.get_unsafe(0,1); m_coords[2] = m.get_unsafe(0,2);
194 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);
195 }
196
197 /** Read only [] operator */
198 inline const double &operator[](unsigned int i) const
199 {
200 switch(i)
201 {
202 case 0:return m_coords[0];
203 case 1:return m_coords[1];
204 case 2:return m_coords[2];
205 case 3:return m_quat[0];
206 case 4:return m_quat[1];
207 case 5:return m_quat[2];
208 case 6:return m_quat[3];
209 default:
210 throw std::runtime_error("CPose3DQuat::operator[]: Index of bounds.");
211 }
212 }
213 /** Read/write [] operator */
214 inline double &operator[](unsigned int i)
215 {
216 switch(i)
217 {
218 case 0:return m_coords[0];
219 case 1:return m_coords[1];
220 case 2:return m_coords[2];
221 case 3:return m_quat[0];
222 case 4:return m_quat[1];
223 case 5:return m_quat[2];
224 case 6:return m_quat[3];
225 default:
226 throw std::runtime_error("CPose3DQuat::operator[]: Index of bounds.");
227 }
228 }
229
230 /** Computes the spherical coordinates of a 3D point as seen from the 6D pose specified by this object.
231 * For the coordinate system see the top of this page.
232 * If the matrix pointers are not NULL, the Jacobians will be also computed for the range-yaw-pitch variables wrt the passed 3D point and this 7D pose.
233 */
235 const mrpt::math::TPoint3D &point,
236 double &out_range,
237 double &out_yaw,
238 double &out_pitch,
239 mrpt::math::CMatrixFixedNumeric<double,3,3> *out_jacob_dryp_dpoint = NULL,
240 mrpt::math::CMatrixFixedNumeric<double,3,7> *out_jacob_dryp_dpose = NULL
241 ) const;
242
243 public:
244 typedef CPose3DQuat type_value; //!< Used to emulate CPosePDF types, for example, in mrpt::graphs::CNetworkOfPoses
245 enum { is_3D_val = 1 };
246 static inline bool is_3D() { return is_3D_val!=0; }
247 enum { rotation_dimensions = 3 };
248 enum { is_PDF_val = 1 };
249 static inline bool is_PDF() { return is_PDF_val!=0; }
250
251 inline const type_value & getPoseMean() const { return *this; }
252 inline type_value & getPoseMean() { return *this; }
253
254 /** @name STL-like methods and typedefs
255 @{ */
256 typedef double value_type; //!< The type of the elements
257 typedef double& reference;
258 typedef const double& const_reference;
259 typedef std::size_t size_type;
260 typedef std::ptrdiff_t difference_type;
261
262 // size is constant
263 enum { static_size = 7 };
264 static inline size_type size() { return static_size; }
265 static inline bool empty() { return false; }
266 static inline size_type max_size() { return static_size; }
267 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))); }
268
269 inline void assign(const size_t N, const double val)
270 {
271 if (N!=7) throw std::runtime_error("CPose3DQuat::assign: Try to resize to length!=7.");
272 m_coords.fill(val);
273 m_quat.fill(val);
274 }
275
276 struct iterator : public std::iterator<std::random_access_iterator_tag,value_type>
277 {
278 private:
279 typedef std::iterator<std::random_access_iterator_tag,value_type> iterator_base;
280 CPose3DQuat *m_obj; //!< A reference to the source of this iterator
281 size_t m_cur_idx; //!< The iterator points to this element.
282 typedef value_type T; //!< The type of the matrix elements
283
284 inline void check_limits(bool allow_end = false) const
285 {
286 #ifdef _DEBUG
287 ASSERTMSG_(m_obj!=NULL,"non initialized iterator");
288 if (m_cur_idx> (allow_end ? 7u : 6u) ) THROW_EXCEPTION("Index out of range in iterator.")
289 #else
290 MRPT_UNUSED_PARAM(allow_end);
291 #endif
292 }
293 public:
294 inline bool operator <(const iterator &it2) const { return m_cur_idx < it2.m_cur_idx; }
295 inline bool operator >(const iterator &it2) const { return m_cur_idx > it2.m_cur_idx; }
296 inline iterator() : m_obj(NULL),m_cur_idx(0) { }
297 inline iterator(CPose3DQuat &obj, size_t start_idx) : m_obj(&obj),m_cur_idx(start_idx) { check_limits(true); /*Dont report as error an iterator to end()*/ }
298 inline CPose3DQuat::reference operator*() const { check_limits(); return (*m_obj)[m_cur_idx]; }
300 check_limits();
301 ++m_cur_idx;
302 return *this;
303 }
304 inline iterator operator++(int) {
305 iterator it=*this;
306 ++*this;
307 return it;
308 }
310 --m_cur_idx;
311 check_limits();
312 return *this;
313 }
314 inline iterator operator--(int) {
315 iterator it=*this;
316 --*this;
317 return it;
318 }
319 inline iterator &operator+=(iterator_base::difference_type off) {
320 m_cur_idx+=off;
321 check_limits(true);
322 return *this;
323 }
324 inline iterator operator+(iterator_base::difference_type off) const {
325 iterator it=*this;
326 it+=off;
327 return it;
328 }
329 inline iterator &operator-=(iterator_base::difference_type off) {
330 return (*this)+=(-off);
331 }
332 inline iterator operator-(iterator_base::difference_type off) const {
333 iterator it=*this;
334 it-=off;
335 return it;
336 }
337 inline iterator_base::difference_type operator-(const iterator &it) const { return m_cur_idx - it.m_cur_idx; }
338 inline CPose3DQuat::reference operator[](iterator_base::difference_type off) const { return (*m_obj)[m_cur_idx+off]; }
339 inline bool operator==(const iterator &it) const { return m_obj==it.m_obj && m_cur_idx==it.m_cur_idx; }
340 inline bool operator!=(const iterator &it) const { return !(operator==(it)); }
341 }; // end iterator
342
343 struct const_iterator : public std::iterator<std::random_access_iterator_tag,value_type>
344 {
345 private:
346 typedef std::iterator<std::random_access_iterator_tag,value_type> iterator_base;
347 const CPose3DQuat *m_obj; //!< A reference to the source of this iterator
348 size_t m_cur_idx; //!< The iterator points to this element.
349 typedef value_type T; //!< The type of the matrix elements
350
351 inline void check_limits(bool allow_end = false) const
352 {
353 #ifdef _DEBUG
354 ASSERTMSG_(m_obj!=NULL,"non initialized iterator");
355 if (m_cur_idx> (allow_end ? 7u : 6u) ) THROW_EXCEPTION("Index out of range in iterator.")
356 #else
357 MRPT_UNUSED_PARAM(allow_end);
358 #endif
359 }
360 public:
361 inline bool operator <(const const_iterator &it2) const { return m_cur_idx < it2.m_cur_idx; }
362 inline bool operator >(const const_iterator &it2) const { return m_cur_idx > it2.m_cur_idx; }
363 inline const_iterator() : m_obj(NULL),m_cur_idx(0) { }
364 inline const_iterator(const CPose3DQuat &obj, size_t start_idx) : m_obj(&obj),m_cur_idx(start_idx) { check_limits(true); /*Dont report as error an iterator to end()*/ }
365 inline CPose3DQuat::const_reference operator*() const { check_limits(); return (*m_obj)[m_cur_idx]; }
367 check_limits();
368 ++m_cur_idx;
369 return *this;
370 }
372 const_iterator it=*this;
373 ++*this;
374 return it;
375 }
377 --m_cur_idx;
378 check_limits();
379 return *this;
380 }
382 const_iterator it=*this;
383 --*this;
384 return it;
385 }
386 inline const_iterator &operator+=(iterator_base::difference_type off) {
387 m_cur_idx+=off;
388 check_limits(true);
389 return *this;
390 }
391 inline const_iterator operator+(iterator_base::difference_type off) const {
392 const_iterator it=*this;
393 it+=off;
394 return it;
395 }
396 inline const_iterator &operator-=(iterator_base::difference_type off) {
397 return (*this)+=(-off);
398 }
399 inline const_iterator operator-(iterator_base::difference_type off) const {
400 const_iterator it=*this;
401 it-=off;
402 return it;
403 }
404 inline iterator_base::difference_type operator-(const const_iterator &it) const { return m_cur_idx - it.m_cur_idx; }
405 inline CPose3DQuat::const_reference operator[](iterator_base::difference_type off) const { return (*m_obj)[m_cur_idx+off]; }
406 inline bool operator==(const const_iterator &it) const { return m_obj==it.m_obj && m_cur_idx==it.m_cur_idx; }
407 inline bool operator!=(const const_iterator &it) const { return !(operator==(it)); }
408 }; // end const_iterator
409
410 typedef std::reverse_iterator<iterator> reverse_iterator;
411 typedef std::reverse_iterator<const_iterator> const_reverse_iterator;
412 inline iterator begin() { return iterator(*this,0); }
413 inline iterator end() { return iterator(*this,static_size); }
414 inline const_iterator begin() const { return const_iterator(*this,0); }
415 inline const_iterator end() const { return const_iterator(*this,static_size); }
420
421
423 {
424 std::swap(o.m_coords, m_coords);
425 o.m_quat.swap(m_quat);
426 }
427
428 /** @} */
429 //! See ops_containers.h
431 //DECLARE_MRPT_CONTAINER_TYPES
432
434
435 }; // End of class def.
437
438 std::ostream BASE_IMPEXP & operator << (std::ostream& o, const CPose3DQuat& p);
439
440 /** Unary - operator: return the inverse pose "-p" (Note that is NOT the same than a pose with all its arguments multiplied by "-1") */
441 CPose3DQuat BASE_IMPEXP operator -(const CPose3DQuat &p);
442
443
444
445 } // End of namespace
446} // End of namespace
447
448#endif
#define DEFINE_SERIALIZABLE_PRE(class_name)
This declaration must be inserted in all CSerializable classes definition, before the class declarati...
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
#define DEFINE_SERIALIZABLE_POST(class_name)
A partial specialization of CArrayNumeric for double numbers.
Definition: CArrayNumeric.h:75
A numeric matrix of compile-time fixed size.
A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ,...
Definition: CQuaternion.h:43
void normalize()
Normalize this quaternion, so its norm becomes the unitity.
Definition: CQuaternion.h:220
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction.
Definition: types_math.h:65
A class used to store a 3D point.
Definition: CPoint3D.h:33
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:73
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,...
Definition: CPose3DQuat.h:42
std::string asString() const
Definition: CPose3DQuat.h:183
void getAsVector(mrpt::math::CArrayDouble< 7 > &v) const
This is an overloaded member function, provided for convenience. It differs from the above function o...
Definition: CPose3DQuat.h:91
void setToNaN() MRPT_OVERRIDE
Set all data fields to quiet NaN.
mrpt::math::CArrayDouble< 3 > & xyz()
Read/Write access to the translation vector in R^3.
Definition: CPose3DQuat.h:57
CPose3DQuat(mrpt::math::TConstructorFlags_Quaternions)
Constructor which left all the quaternion members un-initialized, for use when speed is critical; Use...
Definition: CPose3DQuat.h:66
void assign(const size_t N, const double val)
Definition: CPose3DQuat.h:269
CPose3DQuat(const CPose3D &p)
Constructor from a CPose3D.
double value_type
The type of the elements.
Definition: CPose3DQuat.h:256
void fromString(const std::string &s)
Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04 -0....
Definition: CPose3DQuat.h:189
void getAsVector(mrpt::math::CVectorDouble &v) const
Returns a 1x7 vector with [x y z qr qx qy qz].
mrpt::math::CArrayDouble< 3 > m_coords
The translation vector [x,y,z].
Definition: CPose3DQuat.h:47
reverse_iterator rend()
Definition: CPose3DQuat.h:418
CPose3DQuat(const mrpt::math::CMatrixDouble44 &M)
Constructor from a 4x4 homogeneous transformation matrix.
CPose3DQuat(const mrpt::math::TPose3DQuat &p)
Constructor from lightweight object.
Definition: CPose3DQuat.h:77
std::ptrdiff_t difference_type
Definition: CPose3DQuat.h:260
void asString(std::string &s) const
Returns a human-readable textual representation of the object (eg: "[x y z qr qx qy qz]",...
Definition: CPose3DQuat.h:182
const_reverse_iterator rbegin() const
Definition: CPose3DQuat.h:417
const mrpt::math::CQuaternionDouble & quat() const
Read-only access to the quaternion representing the 3D rotation.
Definition: CPose3DQuat.h:54
const_reverse_iterator rend() const
Definition: CPose3DQuat.h:419
const_iterator begin() const
Definition: CPose3DQuat.h:414
void swap(CPose3DQuat &o)
Definition: CPose3DQuat.h:422
const double & const_reference
Definition: CPose3DQuat.h:258
mrpt::math::CQuaternionDouble m_quat
The quaternion.
Definition: CPose3DQuat.h:48
static void resize(const size_t n)
Definition: CPose3DQuat.h:267
const_iterator end() const
Definition: CPose3DQuat.h:415
static size_type max_size()
Definition: CPose3DQuat.h:266
std::reverse_iterator< iterator > reverse_iterator
Definition: CPose3DQuat.h:410
void inverse()
Convert this pose into its inverse, saving the result in itself.
void inverseComposePoint(const double gx, const double gy, const double gz, double &lx, double &ly, double &lz, mrpt::math::CMatrixFixedNumeric< double, 3, 3 > *out_jacobian_df_dpoint=NULL, mrpt::math::CMatrixFixedNumeric< double, 3, 7 > *out_jacobian_df_dpose=NULL) const
Computes the 3D point L such as .
CPose3DQuat mrpt_autotype
See ops_containers.h.
Definition: CPose3DQuat.h:430
void inverseComposeFrom(const CPose3DQuat &A, const CPose3DQuat &B)
Makes this method is slightly more efficient than "this= A - B;" since it avoids the temporary objec...
void inverseComposePoint(const POINT1 &G, POINT2 &L) const
Computes the 3D point L such as .
Definition: CPose3DQuat.h:128
reverse_iterator rbegin()
Definition: CPose3DQuat.h:416
const type_value & getPoseMean() const
Definition: CPose3DQuat.h:251
double & operator[](unsigned int i)
Read/write [] operator.
Definition: CPose3DQuat.h:214
type_value & getPoseMean()
Definition: CPose3DQuat.h:252
CPose3DQuat()
Default constructor, initialize translation to zeros and quaternion to no rotation.
Definition: CPose3DQuat.h:63
void composePoint(const POINT1 &L, POINT2 &G) const
Computes the 3D point G such as .
Definition: CPose3DQuat.h:125
void composeFrom(const CPose3DQuat &A, const CPose3DQuat &B)
Makes this method is slightly more efficient than "this= A + B;" since it avoids the temporary objec...
void getHomogeneousMatrix(mrpt::math::CMatrixDouble44 &out_HM) const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
std::reverse_iterator< const_iterator > const_reverse_iterator
Definition: CPose3DQuat.h:411
CPose3DQuat(const double x, const double y, const double z, const mrpt::math::CQuaternionDouble &q)
Constructor with initilization of the pose - the quaternion is normalized to make sure it's unitary.
Definition: CPose3DQuat.h:71
CPose3DQuat type_value
Used to emulate CPosePDF types, for example, in mrpt::graphs::CNetworkOfPoses.
Definition: CPose3DQuat.h:244
void sphericalCoordinates(const mrpt::math::TPoint3D &point, double &out_range, double &out_yaw, double &out_pitch, mrpt::math::CMatrixFixedNumeric< double, 3, 3 > *out_jacob_dryp_dpoint=NULL, mrpt::math::CMatrixFixedNumeric< double, 3, 7 > *out_jacob_dryp_dpose=NULL) const
Computes the spherical coordinates of a 3D point as seen from the 6D pose specified by this object.
mrpt::math::CQuaternionDouble & quat()
Read/Write access to the quaternion representing the 3D rotation.
Definition: CPose3DQuat.h:52
CPose3DQuat(TConstructorFlags_Poses)
This is an overloaded member function, provided for convenience. It differs from the above function o...
Definition: CPose3DQuat.h:68
static size_type size()
Definition: CPose3DQuat.h:264
const double & operator[](unsigned int i) const
Read only [] operator.
Definition: CPose3DQuat.h:198
const mrpt::math::CArrayDouble< 3 > & xyz() const
Read-only access to the translation vector in R^3.
Definition: CPose3DQuat.h:59
void composePoint(const double lx, const double ly, const double lz, double &gx, double &gy, double &gz, mrpt::math::CMatrixFixedNumeric< double, 3, 3 > *out_jacobian_df_dpoint=NULL, mrpt::math::CMatrixFixedNumeric< double, 3, 7 > *out_jacobian_df_dpose=NULL) const
Computes the 3D point G such as .
A base class for representing a pose in 2D or 3D.
Definition: CPose.h:26
Scalar * iterator
Definition: eigen_plugins.h:23
const Scalar * const_iterator
Definition: eigen_plugins.h:24
@ static_size
Definition: eigen_plugins.h:17
EIGEN_STRONG_INLINE iterator begin()
Definition: eigen_plugins.h:26
EIGEN_STRONG_INLINE iterator end()
Definition: eigen_plugins.h:27
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
Definition: mrpt_macros.h:28
#define THROW_EXCEPTION(msg)
Definition: mrpt_macros.h:110
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
Definition: mrpt_macros.h:290
#define ASSERTMSG_(f, __ERROR_MSG)
Definition: mrpt_macros.h:260
size_t size(const MATRIXLIKE &m, int dim)
Definition: bits.h:38
TConstructorFlags_Quaternions
Definition: CQuaternion.h:22
CPose2D BASE_IMPEXP operator-(const CPose2D &p)
Unary - operator: return the inverse pose "-p" (Note that is NOT the same than a pose with negative x...
mrpt::math::TPoint2D BASE_IMPEXP operator+(const CPose2D &pose, const mrpt::math::TPoint2D &pnt)
Compose a 2D point from a new coordinate base given by a 2D pose.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
STL namespace.
Lightweight 3D point.
Lightweight 3D pose (three spatial coordinates, plus a quaternion ).
double z
Translation in x,y,z.
const CPose3DQuat * m_obj
A reference to the source of this iterator.
Definition: CPose3DQuat.h:347
size_t m_cur_idx
The iterator points to this element.
Definition: CPose3DQuat.h:348
const_iterator operator+(iterator_base::difference_type off) const
Definition: CPose3DQuat.h:391
std::iterator< std::random_access_iterator_tag, value_type > iterator_base
Definition: CPose3DQuat.h:346
CPose3DQuat::const_reference operator[](iterator_base::difference_type off) const
Definition: CPose3DQuat.h:405
void check_limits(bool allow_end=false) const
Definition: CPose3DQuat.h:351
const_iterator & operator+=(iterator_base::difference_type off)
Definition: CPose3DQuat.h:386
value_type T
The type of the matrix elements.
Definition: CPose3DQuat.h:349
bool operator!=(const const_iterator &it) const
Definition: CPose3DQuat.h:407
bool operator==(const const_iterator &it) const
Definition: CPose3DQuat.h:406
CPose3DQuat::const_reference operator*() const
Definition: CPose3DQuat.h:365
const_iterator operator-(iterator_base::difference_type off) const
Definition: CPose3DQuat.h:399
const_iterator(const CPose3DQuat &obj, size_t start_idx)
Definition: CPose3DQuat.h:364
const_iterator & operator-=(iterator_base::difference_type off)
Definition: CPose3DQuat.h:396
iterator_base::difference_type operator-(const const_iterator &it) const
Definition: CPose3DQuat.h:404
bool operator==(const iterator &it) const
Definition: CPose3DQuat.h:339
iterator operator-(iterator_base::difference_type off) const
Definition: CPose3DQuat.h:332
iterator & operator-=(iterator_base::difference_type off)
Definition: CPose3DQuat.h:329
CPose3DQuat::reference operator[](iterator_base::difference_type off) const
Definition: CPose3DQuat.h:338
void check_limits(bool allow_end=false) const
Definition: CPose3DQuat.h:284
bool operator!=(const iterator &it) const
Definition: CPose3DQuat.h:340
iterator(CPose3DQuat &obj, size_t start_idx)
Definition: CPose3DQuat.h:297
CPose3DQuat::reference operator*() const
Definition: CPose3DQuat.h:298
CPose3DQuat * m_obj
A reference to the source of this iterator.
Definition: CPose3DQuat.h:280
iterator_base::difference_type operator-(const iterator &it) const
Definition: CPose3DQuat.h:337
std::iterator< std::random_access_iterator_tag, value_type > iterator_base
Definition: CPose3DQuat.h:279
size_t m_cur_idx
The iterator points to this element.
Definition: CPose3DQuat.h:281
value_type T
The type of the matrix elements.
Definition: CPose3DQuat.h:282
iterator operator+(iterator_base::difference_type off) const
Definition: CPose3DQuat.h:324
iterator & operator+=(iterator_base::difference_type off)
Definition: CPose3DQuat.h:319



Page generated by Doxygen 1.9.6 for MRPT 1.4.0 SVN: at Wed Mar 22 06:08:57 UTC 2023