<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN"> <html><head><meta http-equiv="Content-Type" content="text/html;charset=iso-8859-1"> <title>CPose3DQuat.h Source File</title> <link href="doxygen.css" rel="stylesheet" type="text/css"> <link href="tabs.css" rel="stylesheet" type="text/css"> </head><body> <div align="left"><a href="http://www.mrpt.org/">Main MRPT website</a> > <b>C++ reference</b> </div> <div align="right"> <a href="index.html"><img border="0" src="mrpt_logo.png" alt="MRPT logo"></a> </div> <!-- Generated by Doxygen 1.7.5 --> <script type="text/javascript"> var searchBox = new SearchBox("searchBox", "search",false,'Search'); </script> <div id="navrow1" class="tabs"> <ul class="tablist"> <li><a href="index.html"><span>Main Page</span></a></li> <li><a href="pages.html"><span>Related Pages</span></a></li> <li><a href="modules.html"><span>Modules</span></a></li> <li><a href="namespaces.html"><span>Namespaces</span></a></li> <li><a href="annotated.html"><span>Classes</span></a></li> <li class="current"><a href="files.html"><span>Files</span></a></li> <li> <div id="MSearchBox" class="MSearchBoxInactive"> <div class="left"> <form id="FSearchBox" action="search.php" method="get"> <img id="MSearchSelect" src="search/mag.png" alt=""/> <input type="text" id="MSearchField" name="query" value="Search" size="20" accesskey="S" onfocus="searchBox.OnSearchFieldFocus(true)" onblur="searchBox.OnSearchFieldFocus(false)"/> </form> </div><div class="right"></div> </div> </li> </ul> </div> <div id="navrow2" class="tabs2"> <ul class="tablist"> <li><a href="files.html"><span>File List</span></a></li> <li><a href="globals.html"><span>File Members</span></a></li> </ul> </div> <div class="header"> <div class="headertitle"> <div class="title">CPose3DQuat.h</div> </div> </div> <div class="contents"> <a href="_c_pose3_d_quat_8h.html">Go to the documentation of this file.</a><div class="fragment"><pre class="fragment"><a name="l00001"></a>00001 <span class="comment">/* +---------------------------------------------------------------------------+</span> <a name="l00002"></a>00002 <span class="comment"> | The Mobile Robot Programming Toolkit (MRPT) C++ library |</span> <a name="l00003"></a>00003 <span class="comment"> | |</span> <a name="l00004"></a>00004 <span class="comment"> | http://www.mrpt.org/ |</span> <a name="l00005"></a>00005 <span class="comment"> | |</span> <a name="l00006"></a>00006 <span class="comment"> | Copyright (C) 2005-2011 University of Malaga |</span> <a name="l00007"></a>00007 <span class="comment"> | |</span> <a name="l00008"></a>00008 <span class="comment"> | This software was written by the Machine Perception and Intelligent |</span> <a name="l00009"></a>00009 <span class="comment"> | Robotics Lab, University of Malaga (Spain). |</span> <a name="l00010"></a>00010 <span class="comment"> | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> |</span> <a name="l00011"></a>00011 <span class="comment"> | |</span> <a name="l00012"></a>00012 <span class="comment"> | This file is part of the MRPT project. |</span> <a name="l00013"></a>00013 <span class="comment"> | |</span> <a name="l00014"></a>00014 <span class="comment"> | MRPT is free software: you can redistribute it and/or modify |</span> <a name="l00015"></a>00015 <span class="comment"> | it under the terms of the GNU General Public License as published by |</span> <a name="l00016"></a>00016 <span class="comment"> | the Free Software Foundation, either version 3 of the License, or |</span> <a name="l00017"></a>00017 <span class="comment"> | (at your option) any later version. |</span> <a name="l00018"></a>00018 <span class="comment"> | |</span> <a name="l00019"></a>00019 <span class="comment"> | MRPT is distributed in the hope that it will be useful, |</span> <a name="l00020"></a>00020 <span class="comment"> | but WITHOUT ANY WARRANTY; without even the implied warranty of |</span> <a name="l00021"></a>00021 <span class="comment"> | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |</span> <a name="l00022"></a>00022 <span class="comment"> | GNU General Public License for more details. |</span> <a name="l00023"></a>00023 <span class="comment"> | |</span> <a name="l00024"></a>00024 <span class="comment"> | You should have received a copy of the GNU General Public License |</span> <a name="l00025"></a>00025 <span class="comment"> | along with MRPT. If not, see <http://www.gnu.org/licenses/>. |</span> <a name="l00026"></a>00026 <span class="comment"> | |</span> <a name="l00027"></a>00027 <span class="comment"> +---------------------------------------------------------------------------+ */</span> <a name="l00028"></a>00028 <span class="preprocessor">#ifndef CPose3DQuat_H</span> <a name="l00029"></a>00029 <span class="preprocessor"></span><span class="preprocessor">#define CPose3DQuat_H</span> <a name="l00030"></a>00030 <span class="preprocessor"></span> <a name="l00031"></a>00031 <span class="preprocessor">#include <<a class="code" href="_c_pose_8h.html">mrpt/poses/CPose.h</a>></span> <a name="l00032"></a>00032 <span class="preprocessor">#include <<a class="code" href="_c_matrix_fixed_numeric_8h.html">mrpt/math/CMatrixFixedNumeric.h</a>></span> <a name="l00033"></a>00033 <span class="preprocessor">#include <<a class="code" href="_c_quaternion_8h.html">mrpt/math/CQuaternion.h</a>></span> <a name="l00034"></a>00034 <span class="preprocessor">#include <<a class="code" href="_c_point3_d_8h.html">mrpt/poses/CPoint3D.h</a>></span> <a name="l00035"></a>00035 <span class="preprocessor">#include <<a class="code" href="lightweight__geom__data_8h.html">mrpt/math/lightweight_geom_data.h</a>></span> <a name="l00036"></a>00036 <a name="l00037"></a>00037 <span class="keyword">namespace </span>mrpt <a name="l00038"></a>00038 { <a name="l00039"></a>00039 <span class="keyword">namespace </span>poses <a name="l00040"></a>00040 { <a name="l00041"></a>00041 <span class="keyword">using namespace </span>mrpt::math; <a name="l00042"></a>00042 <a name="l00043"></a>00043 <span class="keyword">class </span>CPose3D; <a name="l00044"></a>00044 <a name="l00045"></a><a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_ptr.html#a4b86b6a5a2ee1912b3f6bf01fbeb7343">00045</a> <a class="code" href="_c_serializable_8h.html#a5155da97198db5986e6369e2fe7a3539" title="This declaration must be inserted in all CSerializable classes definition, before the class declarati...">DEFINE_SERIALIZABLE_PRE</a>( <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html" title="A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).">CPose3DQuat</a> ) <a name="l00046"></a>00046 <a name="l00047"></a>00047 <span class="comment">/** A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).</span> <a name="l00048"></a>00048 <span class="comment"> *</span> <a name="l00049"></a>00049 <span class="comment"> * For a complete description of Points/Poses, see mrpt::poses::CPoseOrPoint, or refer</span> <a name="l00050"></a>00050 <span class="comment"> * to the <a href="http://www.mrpt.org/2D_3D_Geometry"> 2D/3D Geometry tutorial</a> in the wiki.</span> <a name="l00051"></a>00051 <span class="comment"> *</span> <a name="l00052"></a>00052 <span class="comment"> * To access the translation use x(), y() and z(). To access the rotation, use CPose3DQuat::quat().</span> <a name="l00053"></a>00053 <span class="comment"> *</span> <a name="l00054"></a>00054 <span class="comment"> * This class also behaves like a STL container, since it has begin(), end(), iterators, and can be accessed with the [] operator</span> <a name="l00055"></a>00055 <span class="comment"> * 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</span> <a name="l00056"></a>00056 <span class="comment"> * as a 7-vector anywhere the MRPT math functions expect any kind of vector.</span> <a name="l00057"></a>00057 <span class="comment"> *</span> <a name="l00058"></a>00058 <span class="comment"> * This class and CPose3D are very similar, and they can be converted to the each other automatically via transformation constructors.</span> <a name="l00059"></a>00059 <span class="comment"> *</span> <a name="l00060"></a>00060 <span class="comment"> * \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</span> <a name="l00061"></a>00061 <span class="comment"> * \ingroup poses_grp</span> <a name="l00062"></a>00062 <span class="comment"> */</span> <a name="l00063"></a>00063 class <a class="code" href="base_2include_2mrpt_2base_2link__pragmas_8h.html#a6045fa0129b1a3d6c8bf895470e66574">BASE_IMPEXP</a> <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html" title="A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).">CPose3DQuat</a> : public <a class="code" href="classmrpt_1_1poses_1_1_c_pose.html" title="A base class for representing a pose in 2D or 3D.">CPose</a><<a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html" title="A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).">CPose3DQuat</a>>, public mrpt::utils::<a class="code" href="classmrpt_1_1utils_1_1_c_serializable.html" title="The virtual base class which provides a unified interface for all persistent objects in MRPT...">CSerializable</a> <a name="l00064"></a>00064 { <a name="l00065"></a>00065 <span class="comment">// This must be added to any CSerializable derived class:</span> <a name="l00066"></a><a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a1e12fdc42ff03da2337e9eff5d3d33cf">00066</a> <a class="code" href="_c_serializable_8h.html#a72ab55bf7ae009c89b75715cfa21e84d" title="This declaration must be inserted in all CSerializable classes definition, within the class declarati...">DEFINE_SERIALIZABLE</a>( CPose3DQuat ) <a name="l00067"></a>00067 <a name="l00068"></a>00068 public: <a name="l00069"></a><a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a8bb129994081aabac12002650db15da7">00069</a> <a class="code" href="classmrpt_1_1math_1_1_c_array_double.html" title="A partial specialization of CArrayNumeric for double numbers.">CArrayDouble</a><3> m_coords; <span class="comment">//!< The translation vector [x,y,z]</span> <a name="l00070"></a><a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a6eaa571d3cda372a2a90df451b8573fb">00070</a> <span class="comment"></span> mrpt::math::<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html">CQuaternionDouble</a> m_quat; <span class="comment">//!< The quaternion.</span> <a name="l00071"></a>00071 <span class="comment"></span> <a name="l00072"></a>00072 public:<span class="comment"></span> <a name="l00073"></a>00073 <span class="comment"> /** Read/Write access to the quaternion representing the 3D rotation. */</span> <a name="l00074"></a><a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a4024588ba95bfe06ea40e05e91a6369c">00074</a> inline mrpt::math::<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html">CQuaternionDouble</a> & quat() { <span class="keywordflow">return</span> m_quat; }<span class="comment"></span> <a name="l00075"></a>00075 <span class="comment"> /** Read-only access to the quaternion representing the 3D rotation. */</span> <a name="l00076"></a><a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a99c8eead8ebe2942acb354a8d18e9937">00076</a> <span class="keyword">inline</span> <span class="keyword">const</span> mrpt<a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html">::math::CQuaternionDouble</a> & <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a99c8eead8ebe2942acb354a8d18e9937" title="Read-only access to the quaternion representing the 3D rotation.">quat</a>()<span class="keyword"> const </span>{ <span class="keywordflow">return</span> m_quat; } <a name="l00077"></a>00077 <span class="comment"></span> <a name="l00078"></a>00078 <span class="comment"> /** Read/Write access to the translation vector in R^3. */</span> <a name="l00079"></a><a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#ad447075c52b44f5a7b062168c2778a4b">00079</a> <span class="keyword">inline</span> mrpt<a class="code" href="classmrpt_1_1math_1_1_c_array_double.html">::math::CArrayDouble<3></a> & <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#ad447075c52b44f5a7b062168c2778a4b" title="Read/Write access to the translation vector in R^3.">xyz</a>() { <span class="keywordflow">return</span> m_coords; }<span class="comment"></span> <a name="l00080"></a>00080 <span class="comment"> /** Read-only access to the translation vector in R^3. */</span> <a name="l00081"></a><a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#ab59b46f0d7abb09a591ba4820e8905cb">00081</a> <span class="keyword">inline</span> <span class="keyword">const</span> mrpt<a class="code" href="classmrpt_1_1math_1_1_c_array_double.html">::math::CArrayDouble<3></a> & <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#ab59b46f0d7abb09a591ba4820e8905cb" title="Read-only access to the translation vector in R^3.">xyz</a>()<span class="keyword"> const </span>{ <span class="keywordflow">return</span> m_coords; } <a name="l00082"></a>00082 <a name="l00083"></a>00083 <span class="comment"></span> <a name="l00084"></a>00084 <span class="comment"> /** Default constructor, initialize translation to zeros and quaternion to no rotation. */</span> <a name="l00085"></a><a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#ab668a07c7ee3f04d3478338dfd4f9c19">00085</a> <span class="keyword">inline</span> <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#ab668a07c7ee3f04d3478338dfd4f9c19" title="Default constructor, initialize translation to zeros and quaternion to no rotation.">CPose3DQuat</a>() : m_quat() { m_coords[0]=m_coords[1]=m_coords[2]=0.; } <a name="l00086"></a>00086 <span class="comment"></span> <a name="l00087"></a>00087 <span class="comment"> /** Constructor which left all the quaternion members un-initialized, for use when speed is critical; Use UNINITIALIZED_POSE as argument to this constructor. */</span> <a name="l00088"></a><a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#adf76b4fded26fa42fd05840ddccd7f68">00088</a> <span class="keyword">inline</span> <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#adf76b4fded26fa42fd05840ddccd7f68" title="Constructor which left all the quaternion members un-initialized, for use when speed is critical; Use...">CPose3DQuat</a>(<a class="code" href="namespacemrpt_1_1math.html#a8ab289d85828809b390d477f824a908a">TConstructorFlags_Quaternions</a> constructor_dummy_param) : m_quat(<a class="code" href="namespacemrpt_1_1math.html#a8ab289d85828809b390d477f824a908aa845e9a46f0b0e9601057449d65849fb5">UNINITIALIZED_QUATERNION</a>) { } <a name="l00089"></a>00089 <span class="comment"></span> <a name="l00090"></a>00090 <span class="comment"> /** Constructor with initilization of the pose - the quaternion is normalized to make sure it's unitary */</span> <a name="l00091"></a><a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#ad786230cc9f3196bee6783e93a39ed9f">00091</a> <span class="keyword">inline</span> <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#ad786230cc9f3196bee6783e93a39ed9f" title="Constructor with initilization of the pose - the quaternion is normalized to make sure it's unitary...">CPose3DQuat</a>(<span class="keyword">const</span> <span class="keywordtype">double</span> x,<span class="keyword">const</span> <span class="keywordtype">double</span> <a class="code" href="namespace_eigen_1_1internal.html#a3d7a581aeb951248dc6fe114e9e05f07">y</a>,<span class="keyword">const</span> <span class="keywordtype">double</span> z,<span class="keyword">const</span> <a class="code" href="classmrpt_1_1math_1_1_c_quaternion.html">mrpt::math::CQuaternionDouble</a> &q ) : m_quat(q) { m_coords[0]=x; m_coords[1]=<a class="code" href="namespace_eigen_1_1internal.html#a3d7a581aeb951248dc6fe114e9e05f07">y</a>; m_coords[2]=z; m_quat.normalize(); } <a name="l00092"></a>00092 <span class="comment"></span> <a name="l00093"></a>00093 <span class="comment"> /** Constructor from a CPose3D */</span> <a name="l00094"></a>00094 <span class="keyword">explicit</span> <a class="code" href="namespacemrpt_1_1poses.html#a1d4ae634d5f0417a36ca15a4d928300d">CPose3DQuat</a>(<span class="keyword">const</span> <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d.html" title="A class used to store a 3D pose (a 3D translation + a rotation in 3D).">CPose3D</a> &p); <a name="l00095"></a>00095 <span class="comment"></span> <a name="l00096"></a>00096 <span class="comment"> /** Constructor from lightweight object. */</span> <a name="l00097"></a><a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a45d339e4f96a9a38517a86b19a735948">00097</a> <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a45d339e4f96a9a38517a86b19a735948" title="Constructor from lightweight object.">CPose3DQuat</a>(<span class="keyword">const</span> <a class="code" href="structmrpt_1_1math_1_1_t_pose3_d_quat.html" title="Lightweight 3D pose (three spatial coordinates, plus a quaternion ).">mrpt::math::TPose3DQuat</a> &p) : m_quat(p.qr,p.qx,p.qy,p.qz) { x()=p.<a class="code" href="structmrpt_1_1math_1_1_t_pose3_d_quat.html#a4d5a55048bb3b66ee87cbe7c6328d49b" title="Translation in x.">x</a>; <a class="code" href="namespace_eigen_1_1internal.html#a3d7a581aeb951248dc6fe114e9e05f07">y</a>()=p.<a class="code" href="structmrpt_1_1math_1_1_t_pose3_d_quat.html#a62fd68871a749de38ebbe29287997681" title="Translation in y.">y</a>; z()=p.<a class="code" href="structmrpt_1_1math_1_1_t_pose3_d_quat.html#a0bbd0b4bedd6f143a44a0fde5c8c2d8d" title="Translation in z.">z</a>; } <a name="l00098"></a>00098 <span class="comment"></span> <a name="l00099"></a>00099 <span class="comment"> /** Constructor from a 4x4 homogeneous transformation matrix.</span> <a name="l00100"></a>00100 <span class="comment"> */</span> <a name="l00101"></a>00101 <span class="keyword">explicit</span> <a class="code" href="namespacemrpt_1_1poses.html#a1d4ae634d5f0417a36ca15a4d928300d">CPose3DQuat</a>(<span class="keyword">const</span> <a class="code" href="classmrpt_1_1math_1_1_c_matrix_fixed_numeric.html">CMatrixDouble44</a> &M); <a name="l00102"></a>00102 <span class="comment"></span> <a name="l00103"></a>00103 <span class="comment"> /** Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (translation+orientation).</span> <a name="l00104"></a>00104 <span class="comment"> * \sa getInverseHomogeneousMatrix</span> <a name="l00105"></a>00105 <span class="comment"> */</span> <a name="l00106"></a>00106 <span class="keywordtype">void</span> getHomogeneousMatrix(<a class="code" href="classmrpt_1_1math_1_1_c_matrix_fixed_numeric.html">CMatrixDouble44</a> & out_HM ) <span class="keyword">const</span>; <a name="l00107"></a>00107 <span class="comment"></span> <a name="l00108"></a>00108 <span class="comment"> /** Returns a 1x7 vector with [x y z qr qx qy qz] */</span> <a name="l00109"></a>00109 <span class="keywordtype">void</span> getAsVector(<a class="code" href="structmrpt_1_1dynamicsize__vector.html" title="The base class of MRPT vectors, actually, Eigen column matrices of dynamic size with specialized cons...">vector_double</a> &v) <span class="keyword">const</span>; <a name="l00110"></a>00110 <span class="comment"></span> <a name="l00111"></a>00111 <span class="comment"> /** Makes \f$ this = A \oplus B \f$ this method is slightly more efficient than "this= A + B;" since it avoids the temporary object.</span> <a name="l00112"></a>00112 <span class="comment"> * \note A or B can be "this" without problems.</span> <a name="l00113"></a>00113 <span class="comment"> * \sa inverseComposeFrom, composePoint</span> <a name="l00114"></a>00114 <span class="comment"> */</span> <a name="l00115"></a>00115 <span class="keywordtype">void</span> composeFrom(<span class="keyword">const</span> CPose3DQuat& A, <span class="keyword">const</span> CPose3DQuat& B ); <a name="l00116"></a>00116 <span class="comment"></span> <a name="l00117"></a>00117 <span class="comment"> /** Makes \f$ this = A \ominus B \f$ this method is slightly more efficient than "this= A - B;" since it avoids the temporary object.</span> <a name="l00118"></a>00118 <span class="comment"> * \note A or B can be "this" without problems.</span> <a name="l00119"></a>00119 <span class="comment"> * \sa composeFrom, composePoint</span> <a name="l00120"></a>00120 <span class="comment"> */</span> <a name="l00121"></a>00121 <span class="keywordtype">void</span> inverseComposeFrom(<span class="keyword">const</span> CPose3DQuat& A, <span class="keyword">const</span> CPose3DQuat& B ); <a name="l00122"></a>00122 <span class="comment"></span> <a name="l00123"></a>00123 <span class="comment"> /** Computes the 3D point G such as \f$ G = this \oplus L \f$.</span> <a name="l00124"></a>00124 <span class="comment"> * \sa composeFrom, inverseComposePoint</span> <a name="l00125"></a>00125 <span class="comment"> */</span> <a name="l00126"></a>00126 <span class="keywordtype">void</span> composePoint(<span class="keyword">const</span> <span class="keywordtype">double</span> lx,<span class="keyword">const</span> <span class="keywordtype">double</span> ly,<span class="keyword">const</span> <span class="keywordtype">double</span> lz,<span class="keywordtype">double</span> &gx,<span class="keywordtype">double</span> &gy,<span class="keywordtype">double</span> &gz, <a name="l00127"></a>00127 <a class="code" href="classmrpt_1_1math_1_1_c_matrix_fixed_numeric.html">mrpt::math::CMatrixFixedNumeric<double,3,3></a> *out_jacobian_df_dpoint = NULL, <a name="l00128"></a>00128 <a class="code" href="classmrpt_1_1math_1_1_c_matrix_fixed_numeric.html" title="A numeric matrix of compile-time fixed size.">mrpt::math::CMatrixFixedNumeric<double,3,7></a> *out_jacobian_df_dpose = NULL ) <span class="keyword">const</span>; <a name="l00129"></a>00129 <span class="comment"></span> <a name="l00130"></a>00130 <span class="comment"> /** Computes the 3D point L such as \f$ L = G \ominus this \f$.</span> <a name="l00131"></a>00131 <span class="comment"> * \sa composePoint, composeFrom</span> <a name="l00132"></a>00132 <span class="comment"> */</span> <a name="l00133"></a>00133 <span class="keywordtype">void</span> inverseComposePoint(<span class="keyword">const</span> <span class="keywordtype">double</span> gx,<span class="keyword">const</span> <span class="keywordtype">double</span> gy,<span class="keyword">const</span> <span class="keywordtype">double</span> gz,<span class="keywordtype">double</span> &lx,<span class="keywordtype">double</span> &ly,<span class="keywordtype">double</span> &lz, <a name="l00134"></a>00134 <a class="code" href="classmrpt_1_1math_1_1_c_matrix_fixed_numeric.html">mrpt::math::CMatrixFixedNumeric<double,3,3></a> *out_jacobian_df_dpoint = NULL, <a name="l00135"></a>00135 <a class="code" href="classmrpt_1_1math_1_1_c_matrix_fixed_numeric.html" title="A numeric matrix of compile-time fixed size.">mrpt::math::CMatrixFixedNumeric<double,3,7></a> *out_jacobian_df_dpose = NULL ) <span class="keyword">const</span>; <a name="l00136"></a>00136 <span class="comment"></span> <a name="l00137"></a>00137 <span class="comment"> /** Computes the 3D point G such as \f$ G = this \oplus L \f$.</span> <a name="l00138"></a>00138 <span class="comment"> * POINT1 and POINT1 can be anything supporing [0],[1],[2].</span> <a name="l00139"></a>00139 <span class="comment"> * \sa composePoint */</span> <a name="l00140"></a><a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a671c260a7d24f0ddbb4ad08e25d6611a">00140</a> <span class="keyword">template</span> <<span class="keyword">class</span> POINT1,<span class="keyword">class</span> POINT2> <span class="keyword">inline</span> <span class="keywordtype">void</span> <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a671c260a7d24f0ddbb4ad08e25d6611a" title="Computes the 3D point G such as .">composePoint</a>( <span class="keyword">const</span> POINT1 &L, POINT2 &G)<span class="keyword"> const </span>{ <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a671c260a7d24f0ddbb4ad08e25d6611a" title="Computes the 3D point G such as .">composePoint</a>(L[0],L[1],L[2], G[0],G[1],G[2]); } <a name="l00141"></a>00141 <span class="comment"></span> <a name="l00142"></a>00142 <span class="comment"> /** Computes the 3D point L such as \f$ L = G \ominus this \f$. \sa inverseComposePoint */</span> <a name="l00143"></a><a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#ad6ef488978e988252550e1098271750c">00143</a> <span class="keyword">template</span> <<span class="keyword">class</span> POINT1,<span class="keyword">class</span> POINT2> <span class="keyword">inline</span> <span class="keywordtype">void</span> <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#ad6ef488978e988252550e1098271750c" title="Computes the 3D point L such as .">inverseComposePoint</a>( <span class="keyword">const</span> POINT1 &G, POINT2 &L)<span class="keyword"> const </span>{ <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#ad6ef488978e988252550e1098271750c" title="Computes the 3D point L such as .">inverseComposePoint</a>(G[0],G[1],G[2],L[0],L[1],L[2]); } <a name="l00144"></a>00144 <span class="comment"></span> <a name="l00145"></a>00145 <span class="comment"> /** Computes the 3D point G such as \f$ G = this \oplus L \f$. \sa composePoint */</span> <a name="l00146"></a><a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a19532f44b26904b158464d2e594bea10">00146</a> <span class="keyword">inline</span> <a class="code" href="classmrpt_1_1poses_1_1_c_point3_d.html" title="A class used to store a 3D point.">CPoint3D</a> <a class="code" href="namespacemrpt_1_1poses.html#afb287beee02202a14b09f1011b648e6f" title="Compose a 2D point from a new coordinate base given by a 2D pose.">operator +</a>( <span class="keyword">const</span> <a class="code" href="classmrpt_1_1poses_1_1_c_point3_d.html" title="A class used to store a 3D point.">CPoint3D</a> &L)<span class="keyword"> const </span>{ <a class="code" href="classmrpt_1_1poses_1_1_c_point3_d.html" title="A class used to store a 3D point.">CPoint3D</a> G; composePoint(L[0],L[1],L[2], G[0],G[1],G[2]); <span class="keywordflow">return</span> G; } <a name="l00147"></a>00147 <span class="comment"></span> <a name="l00148"></a>00148 <span class="comment"> /** Computes the 3D point G such as \f$ G = this \oplus L \f$. \sa composePoint */</span> <a name="l00149"></a><a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a2660ffad95ebbe3adaaba9fd04b52e09">00149</a> <span class="keyword">inline</span> <a class="code" href="structmrpt_1_1math_1_1_t_point3_d.html" title="Lightweight 3D point.">TPoint3D</a> <a class="code" href="namespacemrpt_1_1poses.html#afb287beee02202a14b09f1011b648e6f" title="Compose a 2D point from a new coordinate base given by a 2D pose.">operator +</a>( <span class="keyword">const</span> <a class="code" href="structmrpt_1_1math_1_1_t_point3_d.html" title="Lightweight 3D point.">TPoint3D</a> &L)<span class="keyword"> const </span>{ <a class="code" href="structmrpt_1_1math_1_1_t_point3_d.html" title="Lightweight 3D point.">TPoint3D</a> G; composePoint(L[0],L[1],L[2], G[0],G[1],G[2]); <span class="keywordflow">return</span> G; } <a name="l00150"></a>00150 <span class="comment"></span> <a name="l00151"></a>00151 <span class="comment"> /** Computes the 3D point L such as \f$ L = G \ominus this \f$. \sa inverseComposePoint */</span> <a name="l00152"></a><a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#afbb042875c7c2dac27cd10c31affeced">00152</a> <span class="keyword">inline</span> <a class="code" href="classmrpt_1_1poses_1_1_c_point3_d.html" title="A class used to store a 3D point.">CPoint3D</a> <a class="code" href="namespacemrpt_1_1poses.html#a678c1606b35876eae036755599a22d84" title="Unary - operator: return the inverse pose "-p" (Note that is NOT the same than a pose with negative x...">operator -</a>( <span class="keyword">const</span> <a class="code" href="classmrpt_1_1poses_1_1_c_point3_d.html" title="A class used to store a 3D point.">CPoint3D</a> &G)<span class="keyword"> const </span>{ <a class="code" href="classmrpt_1_1poses_1_1_c_point3_d.html" title="A class used to store a 3D point.">CPoint3D</a> L; inverseComposePoint(G[0],G[1],G[2], L[0],L[1],L[2]); <span class="keywordflow">return</span> L; } <a name="l00153"></a>00153 <span class="comment"></span> <a name="l00154"></a>00154 <span class="comment"> /** Computes the 3D point L such as \f$ L = G \ominus this \f$. \sa inverseComposePoint */</span> <a name="l00155"></a><a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#af88c791d886c74f36d7693f12a39381f">00155</a> <span class="keyword">inline</span> <a class="code" href="structmrpt_1_1math_1_1_t_point3_d.html" title="Lightweight 3D point.">TPoint3D</a> <a class="code" href="namespacemrpt_1_1poses.html#a678c1606b35876eae036755599a22d84" title="Unary - operator: return the inverse pose "-p" (Note that is NOT the same than a pose with negative x...">operator -</a>( <span class="keyword">const</span> <a class="code" href="structmrpt_1_1math_1_1_t_point3_d.html" title="Lightweight 3D point.">TPoint3D</a> &G)<span class="keyword"> const </span>{ <a class="code" href="structmrpt_1_1math_1_1_t_point3_d.html" title="Lightweight 3D point.">TPoint3D</a> L; inverseComposePoint(G[0],G[1],G[2], L[0],L[1],L[2]); <span class="keywordflow">return</span> L; } <a name="l00156"></a>00156 <span class="comment"></span> <a name="l00157"></a>00157 <span class="comment"> /** Scalar multiplication (all x y z qr qx qy qz elements are multiplied by the scalar).</span> <a name="l00158"></a>00158 <span class="comment"> */</span> <a name="l00159"></a>00159 <span class="keyword">virtual</span> <span class="keywordtype">void</span> <a class="code" href="group__container__ops__grp.html#gabc43a80aa18fe8ffdfe69fed3ad115a9" title="a*=b (element-wise multiplication)">operator *=</a>(<span class="keyword">const</span> <span class="keywordtype">double</span> s); <a name="l00160"></a>00160 <span class="comment"></span> <a name="l00161"></a>00161 <span class="comment"> /** Make \f$ this = this \oplus b \f$ */</span> <a name="l00162"></a><a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a3a419654a8aeed0bb379589cc4a95d7a">00162</a> <span class="keyword">inline</span> CPose3DQuat& <a class="code" href="group__container__ops__grp.html#ga28127b8dfe78fea7644c4f2a3517cdef" title="a+=b (element-wise sum)">operator += </a>(<span class="keyword">const</span> CPose3DQuat& b) <a name="l00163"></a>00163 { <a name="l00164"></a>00164 composeFrom(*<span class="keyword">this</span>,b); <a name="l00165"></a>00165 <span class="keywordflow">return</span> *<span class="keyword">this</span>; <a name="l00166"></a>00166 } <a name="l00167"></a>00167 <span class="comment"></span> <a name="l00168"></a>00168 <span class="comment"> /** Return the composed pose \f$ ret = this \oplus p \f$ */</span> <a name="l00169"></a><a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#aaca16f0bc169f26c17d5e97fd00bab21">00169</a> <span class="keyword">inline</span> CPose3DQuat <a class="code" href="namespacemrpt_1_1poses.html#afb287beee02202a14b09f1011b648e6f" title="Compose a 2D point from a new coordinate base given by a 2D pose.">operator + </a>(<span class="keyword">const</span> CPose3DQuat& p)<span class="keyword"> const</span> <a name="l00170"></a>00170 <span class="keyword"> </span>{ <a name="l00171"></a>00171 CPose3DQuat ret; <a name="l00172"></a>00172 ret.<a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#ac7481c7936f7a07f89dcfa55b298db57" title="Makes this method is slightly more efficient than "this= A + B;" since it avoids the temporary objec...">composeFrom</a>(*<span class="keyword">this</span>,p); <a name="l00173"></a>00173 <span class="keywordflow">return</span> ret; <a name="l00174"></a>00174 } <a name="l00175"></a>00175 <span class="comment"></span> <a name="l00176"></a>00176 <span class="comment"> /** Make \f$ this = this \ominus b \f$ */</span> <a name="l00177"></a><a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a29557dcae7eedd233bd41e9138471459">00177</a> <span class="keyword">inline</span> CPose3DQuat& operator -= (<span class="keyword">const</span> CPose3DQuat& b) <a name="l00178"></a>00178 { <a name="l00179"></a>00179 inverseComposeFrom(*<span class="keyword">this</span>,b); <a name="l00180"></a>00180 <span class="keywordflow">return</span> *<span class="keyword">this</span>; <a name="l00181"></a>00181 } <a name="l00182"></a>00182 <span class="comment"></span> <a name="l00183"></a>00183 <span class="comment"> /** Return the composed pose \f$ ret = this \ominus p \f$ */</span> <a name="l00184"></a><a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a276c598fb7f1eeae4063f91a1383438f">00184</a> <span class="keyword">inline</span> CPose3DQuat <a class="code" href="namespacemrpt_1_1poses.html#a678c1606b35876eae036755599a22d84" title="Unary - operator: return the inverse pose "-p" (Note that is NOT the same than a pose with negative x...">operator - </a>(<span class="keyword">const</span> CPose3DQuat& p)<span class="keyword"> const</span> <a name="l00185"></a>00185 <span class="keyword"> </span>{ <a name="l00186"></a>00186 CPose3DQuat ret; <a name="l00187"></a>00187 ret.<a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a99b5dd88f31392236ef8d4d6a56ff892" title="Makes this method is slightly more efficient than "this= A - B;" since it avoids the temporary objec...">inverseComposeFrom</a>(*<span class="keyword">this</span>,p); <a name="l00188"></a>00188 <span class="keywordflow">return</span> ret; <a name="l00189"></a>00189 } <a name="l00190"></a>00190 <span class="comment"></span> <a name="l00191"></a>00191 <span class="comment"> /** Returns a human-readable textual representation of the object (eg: "[x y z qr qx qy qz]", angles in degrees.)</span> <a name="l00192"></a>00192 <span class="comment"> * \sa fromString</span> <a name="l00193"></a>00193 <span class="comment"> */</span> <a name="l00194"></a><a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#af347b2956b8562ae837f340d029db2e3">00194</a> <span class="keywordtype">void</span> <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#af347b2956b8562ae837f340d029db2e3" title="Returns a human-readable textual representation of the object (eg: "[x y z qr qx qy qz]"...">asString</a>(<a class="code" href="classstd_1_1string.html" title="STL class.">std::string</a> &s)<span class="keyword"> const </span>{ s = <a class="code" href="namespacemrpt.html#a3a27af794b658df5491e2b7678f8ccb8" title="A std::string version of C sprintf.">mrpt::format</a>(<span class="stringliteral">"[%f %f %f %f %f %f %f]"</span>,m_coords[0],m_coords[1],m_coords[2],m_quat[0],m_quat[1],m_quat[2],m_quat[3]); } <a name="l00195"></a><a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a803457063280e981aef31bff7850e0d5">00195</a> <span class="keyword">inline</span> std<a class="code" href="classstd_1_1string.html" title="STL class.">::string</a> <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a803457063280e981aef31bff7850e0d5">asString</a>()<span class="keyword"> const </span>{ std<a class="code" href="classstd_1_1string.html" title="STL class.">::string</a> s; <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a803457063280e981aef31bff7850e0d5">asString</a>(s); <span class="keywordflow">return</span> s; } <a name="l00196"></a>00196 <span class="comment"></span> <a name="l00197"></a>00197 <span class="comment"> /** Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04 -0.8 1 0 0 0]" )</span> <a name="l00198"></a>00198 <span class="comment"> * \sa asString</span> <a name="l00199"></a>00199 <span class="comment"> * \exception std::exception On invalid format</span> <a name="l00200"></a>00200 <span class="comment"> */</span> <a name="l00201"></a><a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a2655839d702b28c62717b8b01ef078db">00201</a> <span class="keywordtype">void</span> <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a2655839d702b28c62717b8b01ef078db" title="Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04 -0...">fromString</a>(<span class="keyword">const</span> <a class="code" href="classstd_1_1string.html" title="STL class.">std::string</a> &s) { <a name="l00202"></a>00202 <a class="code" href="classmrpt_1_1math_1_1_c_matrix_template_numeric.html" title="A matrix of dynamic size.">CMatrixDouble</a> m; <a name="l00203"></a>00203 <span class="keywordflow">if</span> (!m.fromMatlabStringFormat(s)) <a class="code" href="mrpt__macros_8h.html#aaa3f404ea85a6575a7139f8d101370ba">THROW_EXCEPTION</a>(<span class="stringliteral">"Malformed expression in ::fromString"</span>); <a name="l00204"></a>00204 <a class="code" href="mrpt__macros_8h.html#ad30ea0382c594c0e2efe88212e9352b0">ASSERTMSG_</a>(<a class="code" href="namespacemrpt_1_1math.html#a632ae0aecf78103f87f18f9ac33f7170">mrpt::math::size</a>(m,1)==1 && <a class="code" href="namespacemrpt_1_1math.html#a632ae0aecf78103f87f18f9ac33f7170">mrpt::math::size</a>(m,2)==7, <span class="stringliteral">"Wrong size of vector in ::fromString"</span>); <a name="l00205"></a>00205 m_coords[0] = m.get_unsafe(0,0); m_coords[1] = m.get_unsafe(0,1); m_coords[2] = m.get_unsafe(0,2); <a name="l00206"></a>00206 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); <a name="l00207"></a>00207 } <a name="l00208"></a>00208 <span class="comment"></span> <a name="l00209"></a>00209 <span class="comment"> /** Read only [] operator */</span> <a name="l00210"></a><a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a19b065b16d8213458c337c03af687211">00210</a> <span class="keyword">inline</span> <span class="keyword">const</span> <span class="keywordtype">double</span> &operator[](<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i)<span class="keyword"> const</span> <a name="l00211"></a>00211 <span class="keyword"> </span>{ <a name="l00212"></a>00212 <span class="keywordflow">switch</span>(i) <a name="l00213"></a>00213 { <a name="l00214"></a>00214 <span class="keywordflow">case</span> 0:<span class="keywordflow">return</span> m_coords[0]; <a name="l00215"></a>00215 <span class="keywordflow">case</span> 1:<span class="keywordflow">return</span> m_coords[1]; <a name="l00216"></a>00216 <span class="keywordflow">case</span> 2:<span class="keywordflow">return</span> m_coords[2]; <a name="l00217"></a>00217 <span class="keywordflow">case</span> 3:<span class="keywordflow">return</span> m_quat[0]; <a name="l00218"></a>00218 <span class="keywordflow">case</span> 4:<span class="keywordflow">return</span> m_quat[1]; <a name="l00219"></a>00219 <span class="keywordflow">case</span> 5:<span class="keywordflow">return</span> m_quat[2]; <a name="l00220"></a>00220 <span class="keywordflow">case</span> 6:<span class="keywordflow">return</span> m_quat[3]; <a name="l00221"></a>00221 <span class="keywordflow">default</span>: <a name="l00222"></a>00222 <span class="keywordflow">throw</span> <a class="code" href="classstd_1_1runtime__error.html" title="STL class.">std::runtime_error</a>(<span class="stringliteral">"CPose3DQuat::operator[]: Index of bounds."</span>); <a name="l00223"></a>00223 } <a name="l00224"></a>00224 }<span class="comment"></span> <a name="l00225"></a>00225 <span class="comment"> /** Read/write [] operator */</span> <a name="l00226"></a><a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a8893a327f88d6fb93e6f4d4d2227a492">00226</a> <span class="keyword">inline</span> <span class="keywordtype">double</span> &operator[](<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i) <a name="l00227"></a>00227 { <a name="l00228"></a>00228 <span class="keywordflow">switch</span>(i) <a name="l00229"></a>00229 { <a name="l00230"></a>00230 <span class="keywordflow">case</span> 0:<span class="keywordflow">return</span> m_coords[0]; <a name="l00231"></a>00231 <span class="keywordflow">case</span> 1:<span class="keywordflow">return</span> m_coords[1]; <a name="l00232"></a>00232 <span class="keywordflow">case</span> 2:<span class="keywordflow">return</span> m_coords[2]; <a name="l00233"></a>00233 <span class="keywordflow">case</span> 3:<span class="keywordflow">return</span> m_quat[0]; <a name="l00234"></a>00234 <span class="keywordflow">case</span> 4:<span class="keywordflow">return</span> m_quat[1]; <a name="l00235"></a>00235 <span class="keywordflow">case</span> 5:<span class="keywordflow">return</span> m_quat[2]; <a name="l00236"></a>00236 <span class="keywordflow">case</span> 6:<span class="keywordflow">return</span> m_quat[3]; <a name="l00237"></a>00237 <span class="keywordflow">default</span>: <a name="l00238"></a>00238 <span class="keywordflow">throw</span> <a class="code" href="classstd_1_1runtime__error.html" title="STL class.">std::runtime_error</a>(<span class="stringliteral">"CPose3DQuat::operator[]: Index of bounds."</span>); <a name="l00239"></a>00239 } <a name="l00240"></a>00240 } <a name="l00241"></a>00241 <span class="comment"></span> <a name="l00242"></a>00242 <span class="comment"> /** Computes the spherical coordinates of a 3D point as seen from the 6D pose specified by this object.</span> <a name="l00243"></a>00243 <span class="comment"> * For the coordinate system see the top of this page.</span> <a name="l00244"></a>00244 <span class="comment"> * 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.</span> <a name="l00245"></a>00245 <span class="comment"> */</span> <a name="l00246"></a>00246 <span class="keywordtype">void</span> sphericalCoordinates( <a name="l00247"></a>00247 <span class="keyword">const</span> <a class="code" href="structmrpt_1_1math_1_1_t_point3_d.html" title="Lightweight 3D point.">TPoint3D</a> &point, <a name="l00248"></a>00248 <span class="keywordtype">double</span> &out_range, <a name="l00249"></a>00249 <span class="keywordtype">double</span> &out_yaw, <a name="l00250"></a>00250 <span class="keywordtype">double</span> &out_pitch, <a name="l00251"></a>00251 <a class="code" href="classmrpt_1_1math_1_1_c_matrix_fixed_numeric.html">mrpt::math::CMatrixFixedNumeric<double,3,3></a> *out_jacob_dryp_dpoint = NULL, <a name="l00252"></a>00252 <a class="code" href="classmrpt_1_1math_1_1_c_matrix_fixed_numeric.html" title="A numeric matrix of compile-time fixed size.">mrpt::math::CMatrixFixedNumeric<double,3,7></a> *out_jacob_dryp_dpose = NULL <a name="l00253"></a>00253 ) <span class="keyword">const</span>; <a name="l00254"></a>00254 <a name="l00255"></a>00255 <span class="keyword">public</span>: <a name="l00256"></a><a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#ad84b342f8e445bf7496e6cc438122925">00256</a> <span class="keyword">typedef</span> CPose3DQuat <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#ad84b342f8e445bf7496e6cc438122925" title="Used to emulate CPosePDF types, for example, in mrpt::graphs::CNetworkOfPoses.">type_value</a>; <span class="comment">//!< Used to emulate CPosePDF types, for example, in mrpt::graphs::CNetworkOfPoses</span> <a name="l00257"></a><a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a4b2becad4df86bf3b70da392b8e1263aaacd3d2af8c62f8860eb2508af3b54b00">00257</a> <span class="comment"></span> <span class="keyword">enum</span> { is_3D_val = 1 }; <a name="l00258"></a><a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a7f749d4b3aaba7d2b227f02f2faf74ce">00258</a> <span class="keyword">static</span> <span class="keyword">inline</span> <span class="keywordtype">bool</span> <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a7f749d4b3aaba7d2b227f02f2faf74ce">is_3D</a>() { <span class="keywordflow">return</span> is_3D_val!=0; } <a name="l00259"></a><a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a422bcd8b029b901ad5bdf7228657bd69a0fa085184492925055aa1a26d0f17574">00259</a> <span class="keyword">enum</span> { rotation_dimensions = 3 }; <a name="l00260"></a><a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#aa6f21312bf130a59f2e4fc7dc343f02bafdd54aa0360725fd7f14dee64853d662">00260</a> <span class="keyword">enum</span> { is_PDF_val = 1 }; <a name="l00261"></a><a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#aa7dd05db42568870ed3b34941c59fcd0">00261</a> <span class="keyword">static</span> <span class="keyword">inline</span> <span class="keywordtype">bool</span> <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#aa7dd05db42568870ed3b34941c59fcd0">is_PDF</a>() { <span class="keywordflow">return</span> is_PDF_val!=0; } <a name="l00262"></a>00262 <a name="l00263"></a><a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a09a155cb84a4c7c49726024502d4e668">00263</a> <span class="keyword">inline</span> <span class="keyword">const</span> type_value & <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a09a155cb84a4c7c49726024502d4e668">getPoseMean</a>()<span class="keyword"> const </span>{ <span class="keywordflow">return</span> *<span class="keyword">this</span>; } <a name="l00264"></a><a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#afe4a406c5d6db18918487bac33274135">00264</a> <span class="keyword">inline</span> type_value & <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#afe4a406c5d6db18918487bac33274135">getPoseMean</a>() { <span class="keywordflow">return</span> *<span class="keyword">this</span>; } <a name="l00265"></a>00265 <span class="comment"></span> <a name="l00266"></a>00266 <span class="comment"> /** @name STL-like methods and typedefs</span> <a name="l00267"></a>00267 <span class="comment"> @{ */</span> <a name="l00268"></a><a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a24c5199a8179890c364eb19922642094">00268</a> <span class="keyword">typedef</span> <span class="keywordtype">double</span> <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a24c5199a8179890c364eb19922642094" title="The type of the elements.">value_type</a>; <span class="comment">//!< The type of the elements</span> <a name="l00269"></a><a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#ab969ee568877a7da9fd65026c848f7ad">00269</a> <span class="comment"></span> <span class="keyword">typedef</span> <span class="keywordtype">double</span>& <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#ab969ee568877a7da9fd65026c848f7ad">reference</a>; <a name="l00270"></a><a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a6d745ba48ba2fd32586fdb2f9e36a4f4">00270</a> <span class="keyword">typedef</span> <span class="keyword">const</span> <span class="keywordtype">double</span>& <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a6d745ba48ba2fd32586fdb2f9e36a4f4">const_reference</a>; <a name="l00271"></a><a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#aee191140bb457df8eaacd692304b9a4c">00271</a> <span class="keyword">typedef</span> std::size_t <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#aee191140bb457df8eaacd692304b9a4c">size_type</a>; <a name="l00272"></a><a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a4a220aaf707b27c98e1695584b1f0a79">00272</a> <span class="keyword">typedef</span> std::ptrdiff_t <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a4a220aaf707b27c98e1695584b1f0a79">difference_type</a>; <a name="l00273"></a>00273 <a name="l00274"></a>00274 <span class="comment">// size is constant</span> <a name="l00275"></a><a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a2beaf0a47bc87e792c0bfef83acd9861a017769da5f6cddf28bc7670e1c1572f7">00275</a> <span class="keyword">enum</span> { <a class="code" href="eigen__plugins_8h.html#a99fb83031ce9923c84392b4e92f956b5ad4a8f9f8c603d29a20ca6f9c056dd4da">static_size</a> = 7 }; <a name="l00276"></a><a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#aee96109b53011d095fc9d63b5e471745">00276</a> <span class="keyword">static</span> <span class="keyword">inline</span> size_type <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#aee96109b53011d095fc9d63b5e471745">size</a>() { <span class="keywordflow">return</span> <a class="code" href="eigen__plugins_8h.html#a99fb83031ce9923c84392b4e92f956b5ad4a8f9f8c603d29a20ca6f9c056dd4da">static_size</a>; } <a name="l00277"></a><a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a0b065c40246516ad883b5a1f4d334f56">00277</a> <span class="keyword">static</span> <span class="keyword">inline</span> <span class="keywordtype">bool</span> <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a0b065c40246516ad883b5a1f4d334f56">empty</a>() { <span class="keywordflow">return</span> <span class="keyword">false</span>; } <a name="l00278"></a><a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a7e1edf8928c7781865bec639218b557f">00278</a> <span class="keyword">static</span> <span class="keyword">inline</span> size_type <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a7e1edf8928c7781865bec639218b557f">max_size</a>() { <span class="keywordflow">return</span> <a class="code" href="eigen__plugins_8h.html#a99fb83031ce9923c84392b4e92f956b5ad4a8f9f8c603d29a20ca6f9c056dd4da">static_size</a>; } <a name="l00279"></a><a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a7169a6d0dae084cc59c3f6b4378e49f6">00279</a> <span class="keyword">static</span> <span class="keyword">inline</span> <span class="keywordtype">void</span> <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a7169a6d0dae084cc59c3f6b4378e49f6">resize</a>(<span class="keyword">const</span> <span class="keywordtype">size_t</span> n) { <span class="keywordflow">if</span> (n!=<a class="code" href="eigen__plugins_8h.html#a99fb83031ce9923c84392b4e92f956b5ad4a8f9f8c603d29a20ca6f9c056dd4da">static_size</a>) <span class="keywordflow">throw</span> <a class="code" href="classstd_1_1logic__error.html" title="STL class.">std::logic_error</a>(<a class="code" href="namespacemrpt.html#a3a27af794b658df5491e2b7678f8ccb8" title="A std::string version of C sprintf.">format</a>(<span class="stringliteral">"Try to change the size of CPose3DQuat to %u."</span>,static_cast<unsigned>(n))); } <a name="l00280"></a>00280 <a name="l00281"></a><a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a1e511ac60b94fa5e83a42029567607b3">00281</a> <span class="keyword">inline</span> <span class="keywordtype">void</span> <a class="code" href="eigen__plugins_8h.html#a430e227498fc091226ec9d694f07ac8d">assign</a>(<span class="keyword">const</span> <span class="keywordtype">size_t</span> N, <span class="keyword">const</span> <span class="keywordtype">double</span> val) <a name="l00282"></a>00282 { <a name="l00283"></a>00283 <span class="keywordflow">if</span> (N!=7) <span class="keywordflow">throw</span> <a class="code" href="classstd_1_1runtime__error.html" title="STL class.">std::runtime_error</a>(<span class="stringliteral">"CPose3DQuat::assign: Try to resize to length!=7."</span>); <a name="l00284"></a>00284 m_coords.fill(val); <a name="l00285"></a>00285 m_quat.fill(val); <a name="l00286"></a>00286 } <a name="l00287"></a>00287 <a name="l00288"></a>00288 <span class="keyword">struct </span><a class="code" href="eigen__plugins_8h.html#a39c5d6430ea9395ae7ae729dd0c3f18c">iterator</a> : <span class="keyword">public</span> std::<a class="code" href="eigen__plugins_8h.html#a39c5d6430ea9395ae7ae729dd0c3f18c">iterator</a><std::random_access_iterator_tag,value_type> <a name="l00289"></a>00289 { <a name="l00290"></a>00290 <span class="keyword">private</span>: <a name="l00291"></a><a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1iterator.html#ab5e17f4639a3457ace408e1f54938a11">00291</a> <span class="keyword">typedef</span> std<a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1iterator.html">::iterator<std::random_access_iterator_tag,value_type></a> <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1iterator.html#ab5e17f4639a3457ace408e1f54938a11">iterator_base</a>; <a name="l00292"></a><a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1iterator.html#aae596a87642bdac80ba4d263bc366ebd">00292</a> CPose3DQuat *<a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1iterator.html#aae596a87642bdac80ba4d263bc366ebd" title="A reference to the source of this iterator.">m_obj</a>; <span class="comment">//!< A reference to the source of this iterator</span> <a name="l00293"></a><a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1iterator.html#ac7f3f72a2d503de3e0a173e1eeffaf80">00293</a> <span class="comment"></span> <span class="keywordtype">size_t</span> <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1iterator.html#ac7f3f72a2d503de3e0a173e1eeffaf80" title="The iterator points to this element.">m_cur_idx</a>; <span class="comment">//!< The iterator points to this element.</span> <a name="l00294"></a><a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1iterator.html#ad019a7ed85650a424d50756a74ee2377">00294</a> <span class="comment"></span> <span class="keyword">typedef</span> value_type <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1iterator.html#ad019a7ed85650a424d50756a74ee2377" title="The type of the matrix elements.">T</a>; <span class="comment">//!< The type of the matrix elements</span> <a name="l00295"></a>00295 <span class="comment"></span> <a name="l00296"></a><a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1iterator.html#a221038aa7f551a588c355594a49607f5">00296</a> <span class="keyword">inline</span> <span class="keywordtype">void</span> check_limits(<span class="keywordtype">bool</span> allow_end = <span class="keyword">false</span>)<span class="keyword"> const</span> <a name="l00297"></a>00297 <span class="keyword"> </span>{ <a name="l00298"></a>00298 <span class="preprocessor"> #ifdef _DEBUG</span> <a name="l00299"></a>00299 <span class="preprocessor"></span> <a class="code" href="mrpt__macros_8h.html#ad30ea0382c594c0e2efe88212e9352b0">ASSERTMSG_</a>(m_obj!=NULL,<span class="stringliteral">"non initialized iterator"</span>); <a name="l00300"></a>00300 <span class="keywordflow">if</span> (m_cur_idx> (allow_end ? 7u : 6u) ) <a class="code" href="mrpt__macros_8h.html#aaa3f404ea85a6575a7139f8d101370ba">THROW_EXCEPTION</a>(<span class="stringliteral">"Index out of range in iterator."</span>) <a name="l00301"></a>00301 <span class="preprocessor">#endif</span> <a name="l00302"></a>00302 <span class="preprocessor"></span> } <a name="l00303"></a>00303 <span class="keyword">public</span>: <a name="l00304"></a><a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1iterator.html#ab4e92fd0403891bd7bdf77bcb3153951">00304</a> <span class="keyword">inline</span> <span class="keywordtype">bool</span> <a class="code" href="namespacemrpt_1_1poses.html#aff846586035182f99707712cb073a5d2" title="Used by STL algorithms.">operator <</a>(<span class="keyword">const</span> <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1iterator.html">iterator</a> &it2)<span class="keyword"> const </span>{ <span class="keywordflow">return</span> m_cur_idx < it2.m_cur_idx; } <a name="l00305"></a><a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1iterator.html#a0a20a2a4ad8f7b5c1064dbcd33a01b87">00305</a> <span class="keyword">inline</span> <span class="keywordtype">bool</span> <a class="code" href="namespacemrpt_1_1math.html#a98d3ccede4741958b8b770ec3a42ba6a">operator ></a>(<span class="keyword">const</span> <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1iterator.html">iterator</a> &it2)<span class="keyword"> const </span>{ <span class="keywordflow">return</span> m_cur_idx > it2.m_cur_idx; } <a name="l00306"></a><a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1iterator.html#a396efe2050165f1c0080f3920f25904d">00306</a> <span class="keyword">inline</span> <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1iterator.html#a396efe2050165f1c0080f3920f25904d">iterator</a>() : m_obj(NULL),m_cur_idx(0) { } <a name="l00307"></a><a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1iterator.html#a75fa37ed1686105a606beb59ac38e92a">00307</a> <span class="keyword">inline</span> <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1iterator.html#a75fa37ed1686105a606beb59ac38e92a">iterator</a>(CPose3DQuat &obj, <span class="keywordtype">size_t</span> start_idx) : m_obj(&obj),m_cur_idx(start_idx) { check_limits(<span class="keyword">true</span>); <span class="comment">/*Dont report as error an iterator to end()*/</span> } <a name="l00308"></a><a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1iterator.html#ad3d545516b200f4cf6822cc50b246a36">00308</a> <span class="keyword">inline</span> CPose3DQuat<a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#ab969ee568877a7da9fd65026c848f7ad">::reference</a> <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1iterator.html#ad3d545516b200f4cf6822cc50b246a36">operator*</a>()<span class="keyword"> const </span>{ check_limits(); <span class="keywordflow">return</span> (*m_obj)[m_cur_idx]; } <a name="l00309"></a><a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1iterator.html#ae763abab77bd7345a5fa4795de7b96b8">00309</a> <span class="keyword">inline</span> <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1iterator.html">iterator</a> &<a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1iterator.html#ae763abab77bd7345a5fa4795de7b96b8">operator++</a>() { <a name="l00310"></a>00310 check_limits(); <a name="l00311"></a>00311 ++m_cur_idx; <a name="l00312"></a>00312 <span class="keywordflow">return</span> *<span class="keyword">this</span>; <a name="l00313"></a>00313 } <a name="l00314"></a><a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1iterator.html#ad0e4673169b068c4d5dffe62a15e2edf">00314</a> <span class="keyword">inline</span> <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1iterator.html">iterator</a> <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1iterator.html#ad0e4673169b068c4d5dffe62a15e2edf">operator++</a>(<span class="keywordtype">int</span>) { <a name="l00315"></a>00315 <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1iterator.html">iterator</a> it=*<span class="keyword">this</span>; <a name="l00316"></a>00316 ++*<span class="keyword">this</span>; <a name="l00317"></a>00317 <span class="keywordflow">return</span> it; <a name="l00318"></a>00318 } <a name="l00319"></a><a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1iterator.html#a4c31fde9e61894e7151ea0ad844aaa7c">00319</a> <span class="keyword">inline</span> <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1iterator.html">iterator</a> &<a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1iterator.html#a4c31fde9e61894e7151ea0ad844aaa7c">operator--</a>() { <a name="l00320"></a>00320 --m_cur_idx; <a name="l00321"></a>00321 check_limits(); <a name="l00322"></a>00322 <span class="keywordflow">return</span> *<span class="keyword">this</span>; <a name="l00323"></a>00323 } <a name="l00324"></a><a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1iterator.html#a92b35529c712fe2232e5eba366b3dede">00324</a> <span class="keyword">inline</span> <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1iterator.html">iterator</a> <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1iterator.html#a92b35529c712fe2232e5eba366b3dede">operator--</a>(<span class="keywordtype">int</span>) { <a name="l00325"></a>00325 <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1iterator.html">iterator</a> it=*<span class="keyword">this</span>; <a name="l00326"></a>00326 --*<span class="keyword">this</span>; <a name="l00327"></a>00327 <span class="keywordflow">return</span> it; <a name="l00328"></a>00328 } <a name="l00329"></a><a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1iterator.html#ac92e276c62d5e4095f1b9c18ece0220e">00329</a> <span class="keyword">inline</span> <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1iterator.html">iterator</a> &<a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1iterator.html#ac92e276c62d5e4095f1b9c18ece0220e">operator+=</a>(iterator_base::difference_type off) { <a name="l00330"></a>00330 m_cur_idx+=off; <a name="l00331"></a>00331 check_limits(<span class="keyword">true</span>); <a name="l00332"></a>00332 <span class="keywordflow">return</span> *<span class="keyword">this</span>; <a name="l00333"></a>00333 } <a name="l00334"></a><a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1iterator.html#afbfd8703e46ceeb00e1d71ebe403ca87">00334</a> <span class="keyword">inline</span> <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1iterator.html">iterator</a> <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1iterator.html#afbfd8703e46ceeb00e1d71ebe403ca87">operator+</a>(iterator_base::difference_type off)<span class="keyword"> const </span>{ <a name="l00335"></a>00335 <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1iterator.html">iterator</a> it=*<span class="keyword">this</span>; <a name="l00336"></a>00336 it+=off; <a name="l00337"></a>00337 <span class="keywordflow">return</span> it; <a name="l00338"></a>00338 } <a name="l00339"></a><a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1iterator.html#a49eb11dfd284ffa94be910408c1456ab">00339</a> <span class="keyword">inline</span> <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1iterator.html">iterator</a> &<a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1iterator.html#a49eb11dfd284ffa94be910408c1456ab">operator-=</a>(iterator_base::difference_type off) { <a name="l00340"></a>00340 <span class="keywordflow">return</span> (*<span class="keyword">this</span>)+=(-off); <a name="l00341"></a>00341 } <a name="l00342"></a><a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1iterator.html#a37ecde0f6b6811d3ba78d6060005fbe0">00342</a> <span class="keyword">inline</span> <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1iterator.html">iterator</a> <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1iterator.html#a37ecde0f6b6811d3ba78d6060005fbe0">operator-</a>(iterator_base::difference_type off)<span class="keyword"> const </span>{ <a name="l00343"></a>00343 <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1iterator.html">iterator</a> it=*<span class="keyword">this</span>; <a name="l00344"></a>00344 it-=off; <a name="l00345"></a>00345 <span class="keywordflow">return</span> it; <a name="l00346"></a>00346 } <a name="l00347"></a><a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1iterator.html#a9554cd0448e3bc90408497ba47521794">00347</a> <span class="keyword">inline</span> <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1iterator.html#ab5e17f4639a3457ace408e1f54938a11">iterator_base</a><a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a4a220aaf707b27c98e1695584b1f0a79">::difference_type</a> <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1iterator.html#a9554cd0448e3bc90408497ba47521794">operator-</a>(<span class="keyword">const</span> <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1iterator.html">iterator</a> &it)<span class="keyword"> const </span>{ <span class="keywordflow">return</span> m_cur_idx - it.m_cur_idx; } <a name="l00348"></a><a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1iterator.html#adffd201d941afbfd67e2ce27524ded62">00348</a> <span class="keyword">inline</span> CPose3DQuat<a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#ab969ee568877a7da9fd65026c848f7ad">::reference</a> <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1iterator.html#adffd201d941afbfd67e2ce27524ded62">operator[]</a>(iterator_base::difference_type off)<span class="keyword"> const </span>{ <span class="keywordflow">return</span> (*m_obj)[m_cur_idx+off]; } <a name="l00349"></a><a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1iterator.html#a037ddfa23f103e65f0a0d816307e4829">00349</a> <span class="keyword">inline</span> <span class="keywordtype">bool</span> <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1iterator.html#a037ddfa23f103e65f0a0d816307e4829">operator==</a>(<span class="keyword">const</span> <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1iterator.html">iterator</a> &it)<span class="keyword"> const </span>{ <span class="keywordflow">return</span> m_obj==it.m_obj && m_cur_idx==it.m_cur_idx; } <a name="l00350"></a><a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1iterator.html#a0574c433c3a47afcb966cb284ab4930d">00350</a> <span class="keyword">inline</span> <span class="keywordtype">bool</span> <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1iterator.html#a0574c433c3a47afcb966cb284ab4930d">operator!=</a>(<span class="keyword">const</span> <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1iterator.html">iterator</a> &it)<span class="keyword"> const </span>{ <span class="keywordflow">return</span> !(<a class="code" href="namespacemrpt_1_1poses.html#acd8c946fdfab1501027f3a9347181ebf">operator==</a>(it)); } <a name="l00351"></a>00351 }; <span class="comment">// end iterator</span> <a name="l00352"></a>00352 <a name="l00353"></a>00353 <span class="keyword">struct </span><a class="code" href="eigen__plugins_8h.html#a8dbda719917732693c56cee228465ed9">const_iterator</a> : <span class="keyword">public</span> std::<a class="code" href="eigen__plugins_8h.html#a39c5d6430ea9395ae7ae729dd0c3f18c">iterator</a><std::random_access_iterator_tag,value_type> <a name="l00354"></a>00354 { <a name="l00355"></a>00355 <span class="keyword">private</span>: <a name="l00356"></a><a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1const__iterator.html#a78bfe06e8dc85082bde51760c108423c">00356</a> <span class="keyword">typedef</span> std<a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1iterator.html">::iterator<std::random_access_iterator_tag,value_type></a> <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1const__iterator.html#a78bfe06e8dc85082bde51760c108423c">iterator_base</a>; <a name="l00357"></a><a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1const__iterator.html#a31b465e7bc82b36785387ae2af4e1655">00357</a> <span class="keyword">const</span> CPose3DQuat *<a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1const__iterator.html#a31b465e7bc82b36785387ae2af4e1655" title="A reference to the source of this iterator.">m_obj</a>; <span class="comment">//!< A reference to the source of this iterator</span> <a name="l00358"></a><a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1const__iterator.html#a45f8b0a526afffabe94d474034356903">00358</a> <span class="comment"></span> <span class="keywordtype">size_t</span> <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1const__iterator.html#a45f8b0a526afffabe94d474034356903" title="The iterator points to this element.">m_cur_idx</a>; <span class="comment">//!< The iterator points to this element.</span> <a name="l00359"></a><a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1const__iterator.html#aaf9de89040458a8a9ef54af19144ff12">00359</a> <span class="comment"></span> <span class="keyword">typedef</span> value_type <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1const__iterator.html#aaf9de89040458a8a9ef54af19144ff12" title="The type of the matrix elements.">T</a>; <span class="comment">//!< The type of the matrix elements</span> <a name="l00360"></a>00360 <span class="comment"></span> <a name="l00361"></a><a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1const__iterator.html#a0e01ab1cca22ef305c580b151873a818">00361</a> <span class="keyword">inline</span> <span class="keywordtype">void</span> check_limits(<span class="keywordtype">bool</span> allow_end = <span class="keyword">false</span>)<span class="keyword"> const</span> <a name="l00362"></a>00362 <span class="keyword"> </span>{ <a name="l00363"></a>00363 <span class="preprocessor"> #ifdef _DEBUG</span> <a name="l00364"></a>00364 <span class="preprocessor"></span> <a class="code" href="mrpt__macros_8h.html#ad30ea0382c594c0e2efe88212e9352b0">ASSERTMSG_</a>(m_obj!=NULL,<span class="stringliteral">"non initialized iterator"</span>); <a name="l00365"></a>00365 <span class="keywordflow">if</span> (m_cur_idx> (allow_end ? 7u : 6u) ) <a class="code" href="mrpt__macros_8h.html#aaa3f404ea85a6575a7139f8d101370ba">THROW_EXCEPTION</a>(<span class="stringliteral">"Index out of range in iterator."</span>) <a name="l00366"></a>00366 <span class="preprocessor">#endif</span> <a name="l00367"></a>00367 <span class="preprocessor"></span> } <a name="l00368"></a>00368 <span class="keyword">public</span>: <a name="l00369"></a><a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1const__iterator.html#a063d7fcf29eb9a8f393c64882389f882">00369</a> <span class="keyword">inline</span> <span class="keywordtype">bool</span> <a class="code" href="namespacemrpt_1_1poses.html#aff846586035182f99707712cb073a5d2" title="Used by STL algorithms.">operator <</a>(<span class="keyword">const</span> <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1const__iterator.html">const_iterator</a> &it2)<span class="keyword"> const </span>{ <span class="keywordflow">return</span> m_cur_idx < it2.m_cur_idx; } <a name="l00370"></a><a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1const__iterator.html#a78b137f2d3b7d1778ff076e648ca0aa3">00370</a> <span class="keyword">inline</span> <span class="keywordtype">bool</span> <a class="code" href="namespacemrpt_1_1math.html#a98d3ccede4741958b8b770ec3a42ba6a">operator ></a>(<span class="keyword">const</span> <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1const__iterator.html">const_iterator</a> &it2)<span class="keyword"> const </span>{ <span class="keywordflow">return</span> m_cur_idx > it2.m_cur_idx; } <a name="l00371"></a><a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1const__iterator.html#a97ac4978a1f85d865bbb6da37989b0b0">00371</a> <span class="keyword">inline</span> <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1const__iterator.html#a97ac4978a1f85d865bbb6da37989b0b0">const_iterator</a>() : m_obj(NULL),m_cur_idx(0) { } <a name="l00372"></a><a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1const__iterator.html#af115975d3f93bb1619b09d7eeea4ac83">00372</a> <span class="keyword">inline</span> <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1const__iterator.html#af115975d3f93bb1619b09d7eeea4ac83">const_iterator</a>(<span class="keyword">const</span> CPose3DQuat &obj, <span class="keywordtype">size_t</span> start_idx) : m_obj(&obj),m_cur_idx(start_idx) { check_limits(<span class="keyword">true</span>); <span class="comment">/*Dont report as error an iterator to end()*/</span> } <a name="l00373"></a><a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1const__iterator.html#ac92c9ba1c2c29fdd85156bcce90aa537">00373</a> <span class="keyword">inline</span> CPose3DQuat<a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a6d745ba48ba2fd32586fdb2f9e36a4f4">::const_reference</a> <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1const__iterator.html#ac92c9ba1c2c29fdd85156bcce90aa537">operator*</a>()<span class="keyword"> const </span>{ check_limits(); <span class="keywordflow">return</span> (*m_obj)[m_cur_idx]; } <a name="l00374"></a><a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1const__iterator.html#afcf89e93bff7884d3d788e647e13210f">00374</a> <span class="keyword">inline</span> <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1const__iterator.html">const_iterator</a> &<a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1const__iterator.html#afcf89e93bff7884d3d788e647e13210f">operator++</a>() { <a name="l00375"></a>00375 check_limits(); <a name="l00376"></a>00376 ++m_cur_idx; <a name="l00377"></a>00377 <span class="keywordflow">return</span> *<span class="keyword">this</span>; <a name="l00378"></a>00378 } <a name="l00379"></a><a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1const__iterator.html#ad9ed9a711cb3b41d7b836b80ed37e721">00379</a> <span class="keyword">inline</span> <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1const__iterator.html">const_iterator</a> <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1const__iterator.html#ad9ed9a711cb3b41d7b836b80ed37e721">operator++</a>(<span class="keywordtype">int</span>) { <a name="l00380"></a>00380 <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1const__iterator.html">const_iterator</a> it=*<span class="keyword">this</span>; <a name="l00381"></a>00381 ++*<span class="keyword">this</span>; <a name="l00382"></a>00382 <span class="keywordflow">return</span> it; <a name="l00383"></a>00383 } <a name="l00384"></a><a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1const__iterator.html#a097f3cf56ad22c2a234bf10574f288c4">00384</a> <span class="keyword">inline</span> <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1const__iterator.html">const_iterator</a> &<a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1const__iterator.html#a097f3cf56ad22c2a234bf10574f288c4">operator--</a>() { <a name="l00385"></a>00385 --m_cur_idx; <a name="l00386"></a>00386 check_limits(); <a name="l00387"></a>00387 <span class="keywordflow">return</span> *<span class="keyword">this</span>; <a name="l00388"></a>00388 } <a name="l00389"></a><a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1const__iterator.html#a591070968ea6980ab57eb6519fe5296c">00389</a> <span class="keyword">inline</span> <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1const__iterator.html">const_iterator</a> <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1const__iterator.html#a591070968ea6980ab57eb6519fe5296c">operator--</a>(<span class="keywordtype">int</span>) { <a name="l00390"></a>00390 <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1const__iterator.html">const_iterator</a> it=*<span class="keyword">this</span>; <a name="l00391"></a>00391 --*<span class="keyword">this</span>; <a name="l00392"></a>00392 <span class="keywordflow">return</span> it; <a name="l00393"></a>00393 } <a name="l00394"></a><a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1const__iterator.html#a598d0294015510a19ecb72599ee80d91">00394</a> <span class="keyword">inline</span> <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1const__iterator.html">const_iterator</a> &<a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1const__iterator.html#a598d0294015510a19ecb72599ee80d91">operator+=</a>(iterator_base::difference_type off) { <a name="l00395"></a>00395 m_cur_idx+=off; <a name="l00396"></a>00396 check_limits(<span class="keyword">true</span>); <a name="l00397"></a>00397 <span class="keywordflow">return</span> *<span class="keyword">this</span>; <a name="l00398"></a>00398 } <a name="l00399"></a><a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1const__iterator.html#abdbce5f4d3e59a7c08682d3f63cec6cc">00399</a> <span class="keyword">inline</span> <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1const__iterator.html">const_iterator</a> <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1const__iterator.html#abdbce5f4d3e59a7c08682d3f63cec6cc">operator+</a>(iterator_base::difference_type off)<span class="keyword"> const </span>{ <a name="l00400"></a>00400 <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1const__iterator.html">const_iterator</a> it=*<span class="keyword">this</span>; <a name="l00401"></a>00401 it+=off; <a name="l00402"></a>00402 <span class="keywordflow">return</span> it; <a name="l00403"></a>00403 } <a name="l00404"></a><a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1const__iterator.html#a9241929d5239d1d98c9fafbfc3c34112">00404</a> <span class="keyword">inline</span> <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1const__iterator.html">const_iterator</a> &<a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1const__iterator.html#a9241929d5239d1d98c9fafbfc3c34112">operator-=</a>(iterator_base::difference_type off) { <a name="l00405"></a>00405 <span class="keywordflow">return</span> (*<span class="keyword">this</span>)+=(-off); <a name="l00406"></a>00406 } <a name="l00407"></a><a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1const__iterator.html#ab124f75d46441d73f966b92d769b98cb">00407</a> <span class="keyword">inline</span> <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1const__iterator.html">const_iterator</a> <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1const__iterator.html#ab124f75d46441d73f966b92d769b98cb">operator-</a>(iterator_base::difference_type off)<span class="keyword"> const </span>{ <a name="l00408"></a>00408 <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1const__iterator.html">const_iterator</a> it=*<span class="keyword">this</span>; <a name="l00409"></a>00409 it-=off; <a name="l00410"></a>00410 <span class="keywordflow">return</span> it; <a name="l00411"></a>00411 } <a name="l00412"></a><a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1const__iterator.html#a5347239a4c4fff4abb62de1ececb1df9">00412</a> <span class="keyword">inline</span> <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1const__iterator.html#a78bfe06e8dc85082bde51760c108423c">iterator_base</a><a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a4a220aaf707b27c98e1695584b1f0a79">::difference_type</a> <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1const__iterator.html#a5347239a4c4fff4abb62de1ececb1df9">operator-</a>(<span class="keyword">const</span> <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1const__iterator.html">const_iterator</a> &it)<span class="keyword"> const </span>{ <span class="keywordflow">return</span> m_cur_idx - it.m_cur_idx; } <a name="l00413"></a><a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1const__iterator.html#ae5479d942a53b7e5cf37e24171d36704">00413</a> <span class="keyword">inline</span> CPose3DQuat<a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a6d745ba48ba2fd32586fdb2f9e36a4f4">::const_reference</a> <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1const__iterator.html#ae5479d942a53b7e5cf37e24171d36704">operator[]</a>(iterator_base::difference_type off)<span class="keyword"> const </span>{ <span class="keywordflow">return</span> (*m_obj)[m_cur_idx+off]; } <a name="l00414"></a><a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1const__iterator.html#a1f353a65c71d6787d9b39c431849f903">00414</a> <span class="keyword">inline</span> <span class="keywordtype">bool</span> <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1const__iterator.html#a1f353a65c71d6787d9b39c431849f903">operator==</a>(<span class="keyword">const</span> <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1const__iterator.html">const_iterator</a> &it)<span class="keyword"> const </span>{ <span class="keywordflow">return</span> m_obj==it.m_obj && m_cur_idx==it.m_cur_idx; } <a name="l00415"></a><a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1const__iterator.html#aaa27ea7f1d4a8ce6ddf9713027827b03">00415</a> <span class="keyword">inline</span> <span class="keywordtype">bool</span> <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1const__iterator.html#aaa27ea7f1d4a8ce6ddf9713027827b03">operator!=</a>(<span class="keyword">const</span> <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1const__iterator.html">const_iterator</a> &it)<span class="keyword"> const </span>{ <span class="keywordflow">return</span> !(<a class="code" href="namespacemrpt_1_1poses.html#acd8c946fdfab1501027f3a9347181ebf">operator==</a>(it)); } <a name="l00416"></a>00416 }; <span class="comment">// end const_iterator</span> <a name="l00417"></a>00417 <a name="l00418"></a><a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a7f6e9649e9850f9be43624864735f7f9">00418</a> <span class="keyword">typedef</span> std<a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a7f6e9649e9850f9be43624864735f7f9">::reverse_iterator<iterator></a> <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a7f6e9649e9850f9be43624864735f7f9">reverse_iterator</a>; <a name="l00419"></a><a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#ad1d04e49d0f6a28e4edfeea028f0a8d8">00419</a> <span class="keyword">typedef</span> std<a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a7f6e9649e9850f9be43624864735f7f9">::reverse_iterator<const_iterator></a> <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#ad1d04e49d0f6a28e4edfeea028f0a8d8">const_reverse_iterator</a>; <a name="l00420"></a><a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a672f06fba1ba5f8bfbafe0229b00f10b">00420</a> <span class="keyword">inline</span> <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1iterator.html">iterator</a> <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a672f06fba1ba5f8bfbafe0229b00f10b">begin</a>() { <span class="keywordflow">return</span> <a class="code" href="eigen__plugins_8h.html#a39c5d6430ea9395ae7ae729dd0c3f18c">iterator</a>(*<span class="keyword">this</span>,0); } <a name="l00421"></a><a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#ab357d277cc4d44b9c72579f4d4e62abf">00421</a> <span class="keyword">inline</span> <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1iterator.html">iterator</a> <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#ab357d277cc4d44b9c72579f4d4e62abf">end</a>() { <span class="keywordflow">return</span> <a class="code" href="eigen__plugins_8h.html#a39c5d6430ea9395ae7ae729dd0c3f18c">iterator</a>(*<span class="keyword">this</span>,<a class="code" href="eigen__plugins_8h.html#a99fb83031ce9923c84392b4e92f956b5ad4a8f9f8c603d29a20ca6f9c056dd4da">static_size</a>); } <a name="l00422"></a><a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a1e89710c8f8e401481f36f412f1f7b07">00422</a> <span class="keyword">inline</span> <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1const__iterator.html">const_iterator</a> <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a1e89710c8f8e401481f36f412f1f7b07">begin</a>()<span class="keyword"> const </span>{ <span class="keywordflow">return</span> <a class="code" href="eigen__plugins_8h.html#a8dbda719917732693c56cee228465ed9">const_iterator</a>(*<span class="keyword">this</span>,0); } <a name="l00423"></a><a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a755c9b1b5c5452634b06d9a94df1d380">00423</a> <span class="keyword">inline</span> <a class="code" href="structmrpt_1_1poses_1_1_c_pose3_d_quat_1_1const__iterator.html">const_iterator</a> <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a755c9b1b5c5452634b06d9a94df1d380">end</a>()<span class="keyword"> const </span>{ <span class="keywordflow">return</span> <a class="code" href="eigen__plugins_8h.html#a8dbda719917732693c56cee228465ed9">const_iterator</a>(*<span class="keyword">this</span>,<a class="code" href="eigen__plugins_8h.html#a99fb83031ce9923c84392b4e92f956b5ad4a8f9f8c603d29a20ca6f9c056dd4da">static_size</a>); } <a name="l00424"></a><a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#aa08e1547040f8ccd4fe2ffe6b4eed0be">00424</a> <span class="keyword">inline</span> reverse_iterator <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#aa08e1547040f8ccd4fe2ffe6b4eed0be">rbegin</a>() { <span class="keywordflow">return</span> reverse_iterator(<a class="code" href="eigen__plugins_8h.html#ade5b39864c905cbb824d0ff6eb0d888c">end</a>()); } <a name="l00425"></a><a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#adb8857f60b9ca49cf91479a44fed1fe5">00425</a> <span class="keyword">inline</span> const_reverse_iterator <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#adb8857f60b9ca49cf91479a44fed1fe5">rbegin</a>()<span class="keyword"> const </span>{ <span class="keywordflow">return</span> const_reverse_iterator(<a class="code" href="eigen__plugins_8h.html#ade5b39864c905cbb824d0ff6eb0d888c">end</a>()); } <a name="l00426"></a><a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a2cfc23d1f1790f5f0412b1169bf415f5">00426</a> <span class="keyword">inline</span> reverse_iterator <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a2cfc23d1f1790f5f0412b1169bf415f5">rend</a>() { <span class="keywordflow">return</span> reverse_iterator(<a class="code" href="eigen__plugins_8h.html#ab295fd8164bf1b1acecbcb29520d62b7">begin</a>()); } <a name="l00427"></a><a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a0bdcbc0144075e05181c03f8cb9764e8">00427</a> <span class="keyword">inline</span> const_reverse_iterator <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a0bdcbc0144075e05181c03f8cb9764e8">rend</a>()<span class="keyword"> const </span>{ <span class="keywordflow">return</span> const_reverse_iterator(<a class="code" href="eigen__plugins_8h.html#ab295fd8164bf1b1acecbcb29520d62b7">begin</a>()); } <a name="l00428"></a>00428 <a name="l00429"></a>00429 <a name="l00430"></a><a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a5fe7e0813d7025b530a45fa72a2fd40d">00430</a> <span class="keywordtype">void</span> swap (CPose3DQuat& o) <a name="l00431"></a>00431 { <a name="l00432"></a>00432 std::swap(o.<a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a8bb129994081aabac12002650db15da7" title="The translation vector [x,y,z].">m_coords</a>, m_coords); <a name="l00433"></a>00433 o.<a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a6eaa571d3cda372a2a90df451b8573fb" title="The quaternion.">m_quat</a>.swap(m_quat); <a name="l00434"></a>00434 } <a name="l00435"></a>00435 <span class="comment"></span> <a name="l00436"></a>00436 <span class="comment"> /** @} */</span><span class="comment"></span> <a name="l00437"></a>00437 <span class="comment"> //! See ops_containers.h</span> <a name="l00438"></a><a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a9855e79cca93e989bc3a8e20bd485151">00438</a> <span class="comment"></span> <span class="keyword">typedef</span> CPose3DQuat <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html#a9855e79cca93e989bc3a8e20bd485151" title="See ops_containers.h.">mrpt_autotype</a>; <a name="l00439"></a>00439 <span class="comment">//DECLARE_MRPT_CONTAINER_TYPES</span> <a name="l00440"></a>00440 <a name="l00441"></a>00441 }; <span class="comment">// End of class def.</span> <a name="l00442"></a>00442 <a name="l00443"></a>00443 std<a class="code" href="classstd_1_1ostream.html" title="STL class.">::ostream</a> <a class="code" href="base_2include_2mrpt_2base_2link__pragmas_8h.html#a6045fa0129b1a3d6c8bf895470e66574">BASE_IMPEXP</a> & <a class="code" href="namespacemrpt_1_1poses.html#aa4247c4d793d20edb770e56f22b286f1" title="Dumps a point as a string [x,y] or [x,y,z].">operator << </a>(<a class="code" href="classstd_1_1ostream.html" title="STL class.">std::ostream</a>& o, <span class="keyword">const</span> <a class="code" href="classmrpt_1_1poses_1_1_c_pose3_d_quat.html" title="A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).">CPose3DQuat</a>& p); <a name="l00444"></a>00444 <a name="l00445"></a>00445 <a name="l00446"></a>00446 } <span class="comment">// End of namespace</span> <a name="l00447"></a>00447 } <span class="comment">// End of namespace</span> <a name="l00448"></a>00448 <a name="l00449"></a>00449 <span class="preprocessor">#endif</span> </pre></div></div> </div> <br><hr><br> <table border="0" width="100%"> <tr> <td> Page generated by <a href="http://www.doxygen.org" target="_blank">Doxygen 1.7.5</a> for MRPT 0.9.5 SVN: at Sun Sep 25 17:20:18 UTC 2011</td><td></td> <td width="100"> </td> <td width="150"> </td></tr> </table> </body></html>