47 mrpt::math::CArrayDouble<3> m_coords;
63 inline CPose3DQuat() : m_quat() { m_coords[0]=m_coords[1]=m_coords[2]=0.; }
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];
100 void composeFrom(
const CPose3DQuat& A,
const CPose3DQuat& B );
106 void inverseComposeFrom(
const CPose3DQuat& A,
const CPose3DQuat& B );
111 void composePoint(
const double lx,
const double ly,
const double lz,
double &gx,
double &gy,
double &gz,
118 void inverseComposePoint(
const double gx,
const double gy,
const double gz,
double &lx,
double &ly,
double &lz,
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]); }
149 composeFrom(*
this,b);
162 inline CPose3DQuat& operator -= (
const CPose3DQuat& b)
164 inverseComposeFrom(*
this,b);
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]); }
191 if (!m.fromMatlabStringFormat(s))
THROW_EXCEPTION(
"Malformed expression 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);
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];
210 throw std::runtime_error(
"CPose3DQuat::operator[]: Index of bounds.");
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];
226 throw std::runtime_error(
"CPose3DQuat::operator[]: Index of bounds.");
234 void sphericalCoordinates(
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; }
265 static inline bool empty() {
return false; }
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))); }
269 inline void assign(
const size_t N,
const double val)
271 if (N!=7)
throw std::runtime_error(
"CPose3DQuat::assign: Try to resize to length!=7.");
279 typedef std::iterator<std::random_access_iterator_tag,value_type>
iterator_base;
282 typedef value_type
T;
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.")
297 inline iterator(CPose3DQuat &obj,
size_t start_idx) : m_obj(&obj),m_cur_idx(start_idx) { check_limits(
true); }
330 return (*
this)+=(-off);
346 typedef std::iterator<std::random_access_iterator_tag,value_type>
iterator_base;
349 typedef value_type
T;
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.")
364 inline const_iterator(
const CPose3DQuat &obj,
size_t start_idx) : m_obj(&obj),m_cur_idx(start_idx) { check_limits(
true); }
397 return (*
this)+=(-off);
416 inline reverse_iterator
rbegin() {
return reverse_iterator(
end()); }
417 inline const_reverse_iterator
rbegin()
const {
return const_reverse_iterator(
end()); }
418 inline reverse_iterator
rend() {
return reverse_iterator(
begin()); }
419 inline const_reverse_iterator
rend()
const {
return const_reverse_iterator(
begin()); }
CPose3DQuat * m_obj
A reference to the source of this iterator.
value_type T
The type of the matrix elements.
CPose3DQuat::reference operator*() const
const double & const_reference
const_reverse_iterator rbegin() const
static size_type max_size()
size_t m_cur_idx
The iterator points to this element.
CPose3DQuat::reference operator[](iterator_base::difference_type off) const
EIGEN_STRONG_INLINE iterator end()
const type_value & getPoseMean() const
std::string asString() const
CPose3DQuat(mrpt::math::TConstructorFlags_Quaternions)
Constructor which left all the quaternion members un-initialized, for use when speed is critical; Use...
void assign(const size_t N, const double val)
mrpt::math::CArrayDouble< 3 > & xyz()
Read/Write access to the translation vector in R^3.
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...
reverse_iterator rbegin()
const_iterator operator-(iterator_base::difference_type off) const
#define DEFINE_SERIALIZABLE_PRE(class_name)
This declaration must be inserted in all CSerializable classes definition, before the class declarati...
bool operator!=(const const_iterator &it) const
TConstructorFlags_Quaternions
const_iterator begin() const
iterator(CPose3DQuat &obj, size_t start_idx)
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
const_iterator operator+(iterator_base::difference_type off) const
#define THROW_EXCEPTION(msg)
void fromString(const std::string &s)
Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04 -0...
CPose3DQuat(const mrpt::math::TPose3DQuat &p)
Constructor from lightweight object.
size_t size(const MATRIXLIKE &m, int dim)
std::iterator< std::random_access_iterator_tag, value_type > iterator_base
EIGEN_STRONG_INLINE iterator begin()
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
CQuaternion< double > CQuaternionDouble
A quaternion of data type "double".
bool operator>(const CArray< T, N > &x, const CArray< T, N > &y)
const Scalar * const_iterator
void getAsVector(mrpt::math::CArrayDouble< 7 > &v) const
bool operator==(const const_iterator &it) const
const_iterator operator++(int)
iterator & operator+=(iterator_base::difference_type off)
void check_limits(bool allow_end=false) const
bool operator!=(const iterator &it) const
iterator operator+(iterator_base::difference_type off) const
A numeric matrix of compile-time fixed size.
const_iterator & operator-=(iterator_base::difference_type off)
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
const mrpt::math::CQuaternionDouble & quat() const
Read-only access to the quaternion representing the 3D rotation.
class BASE_IMPEXP CSerializable
std::vector< T1 > & operator+=(std::vector< T1 > &a, const std::vector< T2 > &b)
a+=b (element-wise sum)
double z
Translation in x,y,z.
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.
iterator_base::difference_type operator-(const const_iterator &it) const
const CPose3DQuat * m_obj
A reference to the source of this iterator.
A base class for representing a pose in 2D or 3D.
Eigen::Matrix< dataType, 4, 4 > inverse(Eigen::Matrix< dataType, 4, 4 > &pose)
double value_type
The type of the elements.
double & operator[](unsigned int i)
Read/write [] operator.
void asString(std::string &s) const
Returns a human-readable textual representation of the object (eg: "[x y z qr qx qy qz]"...
const double & operator[](unsigned int i) const
Read only [] operator.
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
class BASE_IMPEXP CPose3DQuat
A class used to store a 3D point.
type_value & getPoseMean()
const_iterator operator--(int)
Lightweight 3D pose (three spatial coordinates, plus a quaternion ).
mrpt::math::CArrayDouble< 3 > m_coords
The translation vector [x,y,z].
const_iterator & operator++()
CPose3DQuat(TConstructorFlags_Poses)
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
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 check_limits(bool allow_end=false) const
CPose3DQuat mrpt_autotype
See ops_containers.h.
static void resize(const size_t n)
bool operator==(const CPoint< DERIVEDCLASS > &p1, const CPoint< DERIVEDCLASS > &p2)
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...
std::iterator< std::random_access_iterator_tag, value_type > iterator_base
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
std::ptrdiff_t difference_type
iterator & operator-=(iterator_base::difference_type off)
const_iterator & operator+=(iterator_base::difference_type off)
CPose3DQuat::const_reference operator[](iterator_base::difference_type off) const
CPose3DQuat()
Default constructor, initialize translation to zeros and quaternion to no rotation.
const_iterator end() const
bool operator<(const CPoint< DERIVEDCLASS > &a, const CPoint< DERIVEDCLASS > &b)
Used by STL algorithms.
iterator_base::difference_type operator-(const iterator &it) const
bool operator==(const iterator &it) const
void inverseComposePoint(const POINT1 &G, POINT2 &L) const
Computes the 3D point L such as .
const_reverse_iterator rend() const
#define DEFINE_SERIALIZABLE_POST(class_name)
mrpt::math::CQuaternionDouble m_quat
The quaternion.
std::reverse_iterator< iterator > reverse_iterator
A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ...
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...
CPose3DQuat type_value
Used to emulate CPosePDF types, for example, in mrpt::graphs::CNetworkOfPoses.
std::reverse_iterator< const_iterator > const_reverse_iterator
iterator operator-(iterator_base::difference_type off) const
size_t m_cur_idx
The iterator points to this element.
const mrpt::math::CArrayDouble< 3 > & xyz() const
Read-only access to the translation vector in R^3.
value_type T
The type of the matrix elements.
#define ASSERTMSG_(f, __ERROR_MSG)
const_iterator & operator--()
const_iterator(const CPose3DQuat &obj, size_t start_idx)
std::vector< T1 > & operator*=(std::vector< T1 > &a, const std::vector< T2 > &b)
a*=b (element-wise multiplication)
CPose3DQuat::const_reference operator*() const
void swap(CPose3DQuat &o)
void composePoint(const POINT1 &L, POINT2 &G) const
Computes the 3D point G such as .