Go to the documentation of this file.
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];
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);
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;
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;
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.")
397 return (*
this)+=(-off);
size_t m_cur_idx
The iterator points to this element.
const type_value & getPoseMean() const
mrpt::math::CQuaternionDouble m_quat
The quaternion.
iterator(CPose3DQuat &obj, size_t start_idx)
#define THROW_EXCEPTION(msg)
const mrpt::math::CArrayDouble< 3 > & xyz() const
Read-only access to the translation vector in R^3.
std::iterator< std::random_access_iterator_tag, value_type > iterator_base
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...
#define ASSERTMSG_(f, __ERROR_MSG)
const mrpt::math::CQuaternionDouble & quat() const
Read-only access to the quaternion representing the 3D rotation.
EIGEN_STRONG_INLINE iterator begin()
const_iterator operator++(int)
const_iterator & operator++()
void normalize()
Normalize this quaternion, so its norm becomes the unitity.
value_type T
The type of the matrix elements.
void assign(const size_t N, const double val)
CPose3DQuat(mrpt::math::TConstructorFlags_Quaternions)
Constructor which left all the quaternion members un-initialized, for use when speed is critical; Use...
CPose3DQuat(const mrpt::math::TPose3DQuat &p)
Constructor from lightweight object.
CPose3DQuat::const_reference operator*() const
reverse_iterator rbegin()
CPose3DQuat::const_reference operator[](iterator_base::difference_type off) const
double value_type
The type of the elements.
const CPose3DQuat * m_obj
A reference to the source of this iterator.
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction.
void composePoint(const POINT1 &L, POINT2 &G) const
Computes the 3D point G such as .
value_type T
The type of the matrix elements.
iterator_base::difference_type operator-(const iterator &it) const
void check_limits(bool allow_end=false) const
bool operator!=(const iterator &it) const
CPose3DQuat mrpt_autotype
See ops_containers.h.
A base class for representing a pose in 2D or 3D.
size_t size(const MATRIXLIKE &m, int dim)
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
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 swap(CPose3DQuat &o)
EIGEN_STRONG_INLINE iterator end()
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
bool operator!=(const const_iterator &it) const
std::ptrdiff_t difference_type
const_iterator end() const
bool operator==(const iterator &it) const
const typedef Scalar * const_iterator
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,...
Eigen::Matrix< dataType, 4, 4 > inverse(Eigen::Matrix< dataType, 4, 4 > &pose)
bool operator==(const const_iterator &it) const
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.
CPose3DQuat::reference operator*() const
std::iterator< std::random_access_iterator_tag, value_type > iterator_base
const_iterator begin() const
const_iterator & operator--()
const_reverse_iterator rend() const
size_t m_cur_idx
The iterator points to this element.
mrpt::math::CQuaternionDouble & quat()
Read/Write access to the quaternion representing the 3D rotation.
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.
type_value & getPoseMean()
std::vector< T1 > & operator+=(std::vector< T1 > &a, const std::vector< T2 > &b)
a+=b (element-wise sum)
#define DEFINE_SERIALIZABLE_POST(class_name)
CPose3DQuat(TConstructorFlags_Poses)
This is an overloaded member function, provided for convenience. It differs from the above function o...
CPose3DQuat type_value
Used to emulate CPosePDF types, for example, in mrpt::graphs::CNetworkOfPoses.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
const_iterator(const CPose3DQuat &obj, size_t start_idx)
const double & operator[](unsigned int i) const
Read only [] operator.
const_iterator operator+(iterator_base::difference_type off) const
TConstructorFlags_Quaternions
void inverseComposePoint(const POINT1 &G, POINT2 &L) const
Computes the 3D point L such as .
iterator & operator-=(iterator_base::difference_type off)
#define DEFINE_SERIALIZABLE_PRE(class_name)
This declaration must be inserted in all CSerializable classes definition, before the class declarati...
iterator & operator+=(iterator_base::difference_type off)
CPose3DQuat * m_obj
A reference to the source of this iterator.
double z
Translation in x,y,z.
A numeric matrix of compile-time fixed size.
bool operator<(const CPoint< DERIVEDCLASS > &a, const CPoint< DERIVEDCLASS > &b)
Used by STL algorithms.
static void resize(const size_t n)
const_reverse_iterator rbegin() const
iterator operator+(iterator_base::difference_type off) const
CPose3DQuat::reference operator[](iterator_base::difference_type off) const
A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ,...
void fromString(const std::string &s)
Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04 -0....
void getAsVector(mrpt::math::CArrayDouble< 7 > &v) const
This is an overloaded member function, provided for convenience. It differs from the above function o...
Lightweight 3D pose (three spatial coordinates, plus a quaternion ).
bool operator==(const CPoint< DERIVEDCLASS > &p1, const CPoint< DERIVEDCLASS > &p2)
static size_type max_size()
std::string asString() const
CPose3DQuat()
Default constructor, initialize translation to zeros and quaternion to no rotation.
const typedef double & const_reference
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
bool operator>(const CArray< T, N > &x, const CArray< T, N > &y)
const_iterator operator-(iterator_base::difference_type off) const
iterator operator-(iterator_base::difference_type off) const
std::vector< T1 > & operator*=(std::vector< T1 > &a, const std::vector< T2 > &b)
a*=b (element-wise multiplication)
mrpt::math::CArrayDouble< 3 > & xyz()
Read/Write access to the translation vector in R^3.
const_iterator & operator-=(iterator_base::difference_type off)
double & operator[](unsigned int i)
Read/write [] operator.
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 check_limits(bool allow_end=false) const
std::reverse_iterator< const_iterator > const_reverse_iterator
A class used to store a 3D point.
void asString(std::string &s) const
Returns a human-readable textual representation of the object (eg: "[x y z qr qx qy qz]",...
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)
iterator_base::difference_type operator-(const const_iterator &it) const
mrpt::math::CArrayDouble< 3 > m_coords
The translation vector [x,y,z].
std::reverse_iterator< iterator > reverse_iterator
const_iterator operator--(int)
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
Page generated by Doxygen 1.8.16 for MRPT 1.4.0 SVN: at Mon Oct 14 23:08:25 UTC 2019 | | |