CPose3D.h

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file     |
   | See: http://www.mrpt.org/Authors - All rights reserved.                |
   | Released under BSD License. See details in http://www.mrpt.org/License |
   +------------------------------------------------------------------------+ */
#ifndef CPOSE3D_H
#define CPOSE3D_H

#include <mrpt/poses/CPose.h>
#include <mrpt/math/CMatrixFixedNumeric.h>
#include <mrpt/math/CQuaternion.h>
#include <mrpt/system/string_utils.h>

// Add for declaration of mexplus::from template specialization
DECLARE_MEXPLUS_FROM(mrpt::poses::CPose3D)

namespace mrpt::poses
{
class CPose3DQuat;
class CPose3DRotVec;

/** A class used to store a 3D pose (a 3D translation + a rotation in 3D).
 *   The 6D transformation in SE(3) stored in this class is kept in two
 *   separate containers: a 3-array for the translation, and a 3x3 rotation
 * matrix.
 *
 *  This class allows parameterizing 6D poses as a 6-vector: [x y z yaw pitch
 * roll] (read below
 *   for the angles convention). Note however,
 *   that the yaw/pitch/roll angles are only computed (on-demand and
 * transparently)
 *   when the user requests them. Normally, rotations and transformations are
 * always handled
 *   via the 3x3 rotation matrix.
 *
 *  Yaw/Pitch/Roll angles are defined as successive rotations around *local*
 * (dynamic) axes in the Z/Y/X order:
 *
 *  <div align=center>
 *   <img src="CPose3D.gif">
 *  </div>
 *
 * It may be extremely confusing and annoying to find a different criterion also
 * involving
 * the names "yaw, pitch, roll" but regarding rotations around *global* (static)
 * axes.
 * Fortunately, it's very easy to see (by writing down the product of the three
 * rotation matrices) that both conventions lead to exactly the same numbers.
 * Only, that it's conventional to write the numbers in reverse order.
 * That is, the same rotation can be described equivalently with any of these
 * two
 * parameterizations:
 *
 * - In local axes Z/Y/X convention: [yaw pitch roll]   (This is the convention
 * used in mrpt::poses::CPose3D)
 * - In global axes X/Y/Z convention: [roll pitch yaw] (One of the Euler angles
 * conventions)
 *
 * For further descriptions of point & pose classes, see
 * mrpt::poses::CPoseOrPoint or refer
 * to the [2D/3D Geometry tutorial](http://www.mrpt.org/2D_3D_Geometry) online.
 *
 * To change the individual components of the pose, use CPose3D::setFromValues.
 * This class assures that the internal
 * 3x3 rotation matrix is always up-to-date with the "yaw pitch roll" members.
 *
 * Rotations in 3D can be also represented by quaternions. See
 * mrpt::math::CQuaternion, and method CPose3D::getAsQuaternion.
 *
 * This class and CPose3DQuat are very similar, and they can be converted to the
 * each other automatically via transformation constructors.
 *
 * There are Lie algebra methods: \a exp and \a ln (see the methods for
 * documentation).
 *
 * \note Read also: "A tutorial on SE(3) transformation parameterizations and
 * on-manifold optimization", (Technical report), 2010-2014.
 * [PDF](http://ingmec.ual.es/~jlblanco/papers/jlblanco2010geometry3D_techrep.pdf)
 *
 * \ingroup poses_grp
 * \sa CPoseOrPoint,CPoint3D, mrpt::math::CQuaternion
 */
class CPose3D : public CPose<CPose3D>, public mrpt::serialization::CSerializable
{
    DEFINE_SERIALIZABLE(CPose3D)

    // This must be added for declaration of MEX-related functions
    DECLARE_MEX_CONVERSION

   public:
    /** The translation vector [x,y,z] access directly or with x(), y(), z()
     * setter/getter methods. */
    mrpt::math::CArrayDouble<3> m_coords;

   protected:
    /** The 3x3 rotation matrix, access with getRotationMatrix(),
     * setRotationMatrix() (It's not safe to set this field as public) */
    mrpt::math::CMatrixDouble33 m_ROT;

    /** Whether yaw/pitch/roll members are up-to-date since the last rotation
     * matrix update. */
    mutable bool m_ypr_uptodate;
    /** These variables are updated every time that the object rotation matrix
     * is modified (construction, loading from values, pose composition, etc )
     */
    mutable double m_yaw, m_pitch, m_roll;

    /** Rebuild the homog matrix from the angles. */
    void rebuildRotationMatrix();

    /** Updates Yaw/pitch/roll members from the m_ROT  */
    inline void updateYawPitchRoll() const
    {
        if (!m_ypr_uptodate)
        {
            m_ypr_uptodate = true;
            getYawPitchRoll(m_yaw, m_pitch, m_roll);
        }
    }

   public:
    /** @name Constructors
        @{ */

    /** Default constructor, with all the coordinates set to zero. */
    CPose3D();

    /** Constructor with initilization of the pose; (remember that angles are
     * always given in radians!)  */
    CPose3D(
        const double x, const double y, const double z, const double yaw = 0,
        const double pitch = 0, const double roll = 0);

    /** Constructor from a 4x4 homogeneous matrix - the passed matrix can be
     * actually of any size larger than or equal 3x4, since only those first
     * values are used (the last row of a homogeneous 4x4 matrix are always
     * fixed). */
    explicit CPose3D(const math::CMatrixDouble& m);

    /** Constructor from a 4x4 homogeneous matrix: */
    explicit CPose3D(const math::CMatrixDouble44& m);

    /** Constructor from a 3x3 rotation matrix and a the translation given as a
     * 3-vector, a 3-array, a CPoint3D or a mrpt::math::TPoint3D */
    template <class MATRIX33, class VECTOR3>
    inline CPose3D(const MATRIX33& rot, const VECTOR3& xyz)
        : m_ROT(mrpt::math::UNINITIALIZED_MATRIX), m_ypr_uptodate(false)
    {
        ASSERT_EQUAL_(rot.rows(), 3);
        ASSERT_EQUAL_(rot.cols(), 3);
        ASSERT_EQUAL_(xyz.size(), 3);
        for (int r = 0; r < 3; r++)
            for (int c = 0; c < 3; c++) m_ROT(r, c) = rot(r, c);
        for (int r = 0; r < 3; r++) m_coords[r] = xyz[r];
    }
    //! \overload
    inline CPose3D(
        const mrpt::math::CMatrixDouble33& rot,
        const mrpt::math::CArrayDouble<3>& xyz)
        : m_coords(xyz), m_ROT(rot), m_ypr_uptodate(false)
    {
    }

    /** Constructor from a CPose2D object.
     */
    explicit CPose3D(const CPose2D&);

    /** Constructor from a CPoint3D object.
     */
    explicit CPose3D(const CPoint3D&);

    /** Constructor from lightweight object.
     */
    explicit CPose3D(const mrpt::math::TPose3D&);

    mrpt::math::TPose3D asTPose() const;

    /** Constructor from a quaternion (which only represents the 3D rotation
     * part) and a 3D displacement. */
    CPose3D(
        const mrpt::math::CQuaternionDouble& q, const double x, const double y,
        const double z);

    /** Constructor from a CPose3DQuat. */
    explicit CPose3D(const CPose3DQuat&);

    /** Constructor from a CPose3DRotVec. */
    explicit CPose3D(const CPose3DRotVec& p);

    /** Fast constructor that leaves all the data uninitialized - call with
     * UNINITIALIZED_POSE as argument */
    inline CPose3D(TConstructorFlags_Poses)
        : m_ROT(mrpt::math::UNINITIALIZED_MATRIX), m_ypr_uptodate(false)
    {
    }

    /** Constructor from an array with these 12 elements: [r11 r21 r31 r12 r22
     * r32 r13 r23 r33 tx ty tz]
     *  where r{ij} are the entries of the 3x3 rotation matrix and t{x,y,z} are
     * the 3D translation of the pose
     *  \sa setFrom12Vector, getAs12Vector
     */
    inline explicit CPose3D(const mrpt::math::CArrayDouble<12>& vec12)
        : m_ROT(mrpt::math::UNINITIALIZED_MATRIX), m_ypr_uptodate(false)
    {
        setFrom12Vector(vec12);
    }

    /** @} */  // end Constructors

    /** @name Access 3x3 rotation and 4x4 homogeneous matrices
        @{ */

    /** Returns the corresponding 4x4 homogeneous transformation matrix for the
     * point(translation) or pose (translation+orientation).
     * \sa getInverseHomogeneousMatrix, getRotationMatrix
     */
    inline void getHomogeneousMatrix(mrpt::math::CMatrixDouble44& out_HM) const
    {
        out_HM.insertMatrix(0, 0, m_ROT);
        for (int i = 0; i < 3; i++) out_HM(i, 3) = m_coords[i];
        out_HM(3, 0) = out_HM(3, 1) = out_HM(3, 2) = 0.;
        out_HM(3, 3) = 1.;
    }

    /** Get the 3x3 rotation matrix \sa getHomogeneousMatrix  */
    inline void getRotationMatrix(mrpt::math::CMatrixDouble33& ROT) const
    {
        ROT = m_ROT;
    }
    //! \overload
    inline const mrpt::math::CMatrixDouble33& getRotationMatrix() const
    {
        return m_ROT;
    }

    /** Sets the 3x3 rotation matrix \sa getRotationMatrix, getHomogeneousMatrix
     */
    inline void setRotationMatrix(const mrpt::math::CMatrixDouble33& ROT)
    {
        m_ROT = ROT;
        m_ypr_uptodate = false;
    }

    /** @} */  // end rot and HM

    /** @name Pose-pose and pose-point compositions and operators
        @{ */

    /** The operator \f$ a \oplus b \f$ is the pose compounding operator. */
    inline CPose3D operator+(const CPose3D& b) const
    {
        CPose3D ret(UNINITIALIZED_POSE);
        ret.composeFrom(*this, b);
        return ret;
    }

    /** The operator \f$ a \oplus b \f$ is the pose compounding operator. */
    CPoint3D operator+(const CPoint3D& b) const;

    /** The operator \f$ a \oplus b \f$ is the pose compounding operator. */
    CPoint3D operator+(const CPoint2D& b) const;

    /** Computes the spherical coordinates of a 3D point as seen from the 6D
     * pose specified by this object. For the coordinate system see the top of
     * this page. */
    void sphericalCoordinates(
        const mrpt::math::TPoint3D& point, double& out_range, double& out_yaw,
        double& out_pitch) const;

    /** An alternative, slightly more efficient way of doing \f$ G = P \oplus L
     * \f$ with G and L being 3D points and P this 6D pose.
     *  If pointers are provided, the corresponding Jacobians are returned.
     *  "out_jacobian_df_dse3" stands for the Jacobian with respect to the 6D
     * locally Euclidean vector in the tangent space of SE(3).
     *  See [this
     * report](http://ingmec.ual.es/~jlblanco/papers/jlblanco2010geometry3D_techrep.pdf)
     * for mathematical details.
     *  \param  If set to true, the Jacobian "out_jacobian_df_dpose" uses a
     * fastest linearized appoximation (valid only for small rotations!).
     */
    void composePoint(
        double lx, double ly, double lz, double& gx, double& gy, double& gz,
        mrpt::math::CMatrixFixedNumeric<double, 3, 3>* out_jacobian_df_dpoint =
            nullptr,
        mrpt::math::CMatrixFixedNumeric<double, 3, 6>* out_jacobian_df_dpose =
            nullptr,
        mrpt::math::CMatrixFixedNumeric<double, 3, 6>* out_jacobian_df_dse3 =
            nullptr,
        bool use_small_rot_approx = false) const;

    /** An alternative, slightly more efficient way of doing \f$ G = P \oplus L
     * \f$ with G and L being 3D points and P this 6D pose.
     * \note local_point is passed by value to allow global and local point to
     * be the same variable
     */
    inline void composePoint(
        const mrpt::math::TPoint3D& local_point,
        mrpt::math::TPoint3D& global_point) const
    {
        composePoint(
            local_point.x, local_point.y, local_point.z, global_point.x,
            global_point.y, global_point.z);
    }
    /** This version of the method assumes that the resulting point has no Z
     * component (use with caution!) */
    inline void composePoint(
        const mrpt::math::TPoint3D& local_point,
        mrpt::math::TPoint2D& global_point) const
    {
        double dummy_z;
        composePoint(
            local_point.x, local_point.y, local_point.z, global_point.x,
            global_point.y, dummy_z);
    }

    /** An alternative, slightly more efficient way of doing \f$ G = P \oplus L
     * \f$ with G and L being 3D points and P this 6D pose.  */
    inline void composePoint(
        double lx, double ly, double lz, float& gx, float& gy, float& gz) const
    {
        double ggx, ggy, ggz;
        composePoint(lx, ly, lz, ggx, ggy, ggz);
        gx = static_cast<float>(ggx);
        gy = static_cast<float>(ggy);
        gz = static_cast<float>(ggz);
    }

    /**  Computes the 3D point L such as \f$ L = G \ominus this \f$.
     *  If pointers are provided, the corresponding Jacobians are returned.
     *  "out_jacobian_df_dse3" stands for the Jacobian with respect to the 6D
     * locally Euclidean vector in the tangent space of SE(3).
     *  See [this
     * report](http://ingmec.ual.es/~jlblanco/papers/jlblanco2010geometry3D_techrep.pdf)
     * for mathematical details.
     * \sa composePoint, composeFrom
     */
    void inverseComposePoint(
        const double gx, const double gy, const double gz, double& lx,
        double& ly, double& lz,
        mrpt::math::CMatrixFixedNumeric<double, 3, 3>* out_jacobian_df_dpoint =
            nullptr,
        mrpt::math::CMatrixFixedNumeric<double, 3, 6>* out_jacobian_df_dpose =
            nullptr,
        mrpt::math::CMatrixFixedNumeric<double, 3, 6>* out_jacobian_df_dse3 =
            nullptr) const;

    /** \overload */
    inline void inverseComposePoint(
        const mrpt::math::TPoint3D& g, mrpt::math::TPoint3D& l) const
    {
        inverseComposePoint(g.x, g.y, g.z, l.x, l.y, l.z);
    }

    /** overload for 2D points \exception If the z component of the result is
     * greater than some epsilon */
    inline void inverseComposePoint(
        const mrpt::math::TPoint2D& g, mrpt::math::TPoint2D& l,
        const double eps = 1e-6) const
    {
        double lz;
        inverseComposePoint(g.x, g.y, 0, l.x, l.y, lz);
        ASSERT_BELOW_(std::abs(lz), eps);
    }

    /**  Makes "this = A (+) B"; this method is slightly more efficient than
     * "this= A + B;" since it avoids the temporary object.
     *  \note A or B can be "this" without problems.
     */
    void composeFrom(const CPose3D& A, const CPose3D& B);

    /** Make \f$ this = this \oplus b \f$  (\a b can be "this" without problems)
     */
    inline CPose3D& operator+=(const CPose3D& b)
    {
        composeFrom(*this, b);
        return *this;
    }

    /**  Makes \f$ this = A \ominus B \f$ this method is slightly more efficient
     * than "this= A - B;" since it avoids the temporary object.
     *  \note A or B can be "this" without problems.
     * \sa composeFrom, composePoint
     */
    void inverseComposeFrom(const CPose3D& A, const CPose3D& B);

    /** Compute \f$ RET = this \oplus b \f$  */
    inline CPose3D operator-(const CPose3D& b) const
    {
        CPose3D ret(UNINITIALIZED_POSE);
        ret.inverseComposeFrom(*this, b);
        return ret;
    }

    /** Convert this pose into its inverse, saving the result in itself. \sa
     * operator- */
    void inverse();

    /** makes: this = p (+) this */
    inline void changeCoordinatesReference(const CPose3D& p)
    {
        composeFrom(p, CPose3D(*this));
    }

    /** @} */  // compositions

    /** Return the opposite of the current pose instance by taking the negative
     * of all its components \a individually
     */
    CPose3D getOppositeScalar() const;

    /** @name Access and modify contents
        @{ */

    /** Scalar sum of all 6 components: This is diferent from poses composition,
     * which is implemented as "+" operators.
     * \sa normalizeAngles
     */
    void addComponents(const CPose3D& p);

    /** Rebuild the internal matrix & update the yaw/pitch/roll angles within
     * the ]-PI,PI] range (Must be called after using addComponents)
     * \sa addComponents
     */
    void normalizeAngles();

    /** Scalar multiplication of x,y,z,yaw,pitch & roll (angles will be wrapped
     * to the ]-pi,pi] interval). */
    void operator*=(const double s);

    /** Set the pose from a 3D position (meters) and yaw/pitch/roll angles
     * (radians) - This method recomputes the internal rotation matrix.
     * \sa getYawPitchRoll, setYawPitchRoll
     */
    void setFromValues(
        const double x0, const double y0, const double z0, const double yaw = 0,
        const double pitch = 0, const double roll = 0);

    /** Set the pose from a 3D position (meters) and a quaternion, stored as [x
     * y z qr qx qy qz] in a 7-element vector.
     * \sa setFromValues, getYawPitchRoll, setYawPitchRoll, CQuaternion,
     * getAsQuaternion
     */
    template <typename VECTORLIKE>
    inline void setFromXYZQ(const VECTORLIKE& v, const size_t index_offset = 0)
    {
        ASSERT_ABOVEEQ_(v.size(), 7 + index_offset);
        // The 3x3 rotation part:
        mrpt::math::CQuaternion<typename VECTORLIKE::value_type> q(
            v[index_offset + 3], v[index_offset + 4], v[index_offset + 5],
            v[index_offset + 6]);
        q.rotationMatrixNoResize(m_ROT);
        m_ypr_uptodate = false;
        m_coords[0] = v[index_offset + 0];
        m_coords[1] = v[index_offset + 1];
        m_coords[2] = v[index_offset + 2];
    }

    /** Set the 3 angles of the 3D pose (in radians) - This method recomputes
     * the internal rotation coordinates matrix.
     * \sa getYawPitchRoll, setFromValues
     */
    inline void setYawPitchRoll(
        const double yaw_, const double pitch_, const double roll_)
    {
        setFromValues(x(), y(), z(), yaw_, pitch_, roll_);
    }

    /** Set pose from an array with these 12 elements: [r11 r21 r31 r12 r22 r32
     * r13 r23 r33 tx ty tz]
     *  where r{ij} are the entries of the 3x3 rotation matrix and t{x,y,z} are
     * the 3D translation of the pose
     *  \sa getAs12Vector
     */
    template <class ARRAYORVECTOR>
    inline void setFrom12Vector(const ARRAYORVECTOR& vec12)
    {
        m_ROT.set_unsafe(0, 0, vec12[0]);
        m_ROT.set_unsafe(0, 1, vec12[3]);
        m_ROT.set_unsafe(0, 2, vec12[6]);
        m_ROT.set_unsafe(1, 0, vec12[1]);
        m_ROT.set_unsafe(1, 1, vec12[4]);
        m_ROT.set_unsafe(1, 2, vec12[7]);
        m_ROT.set_unsafe(2, 0, vec12[2]);
        m_ROT.set_unsafe(2, 1, vec12[5]);
        m_ROT.set_unsafe(2, 2, vec12[8]);
        m_ypr_uptodate = false;
        m_coords[0] = vec12[9];
        m_coords[1] = vec12[10];
        m_coords[2] = vec12[11];
    }

    /** Get the pose representation as an array with these 12 elements: [r11 r21
     * r31 r12 r22 r32 r13 r23 r33 tx ty tz]
     *  where r{ij} are the entries of the 3x3 rotation matrix and t{x,y,z} are
     * the 3D translation of the pose
     *  \sa setFrom12Vector
     */
    template <class ARRAYORVECTOR>
    inline void getAs12Vector(ARRAYORVECTOR& vec12) const
    {
        vec12[0] = m_ROT.get_unsafe(0, 0);
        vec12[3] = m_ROT.get_unsafe(0, 1);
        vec12[6] = m_ROT.get_unsafe(0, 2);
        vec12[1] = m_ROT.get_unsafe(1, 0);
        vec12[4] = m_ROT.get_unsafe(1, 1);
        vec12[7] = m_ROT.get_unsafe(1, 2);
        vec12[2] = m_ROT.get_unsafe(2, 0);
        vec12[5] = m_ROT.get_unsafe(2, 1);
        vec12[8] = m_ROT.get_unsafe(2, 2);
        vec12[9] = m_coords[0];
        vec12[10] = m_coords[1];
        vec12[11] = m_coords[2];
    }

    /** Returns the three angles (yaw, pitch, roll), in radians, from the
     * rotation matrix.
     * \sa setFromValues, yaw, pitch, roll
     */
    void getYawPitchRoll(double& yaw, double& pitch, double& roll) const;

    /** Get the YAW angle (in radians)  \sa setFromValues */
    inline double yaw() const
    {
        updateYawPitchRoll();
        return m_yaw;
    }
    /** Get the PITCH angle (in radians) \sa setFromValues */
    inline double pitch() const
    {
        updateYawPitchRoll();
        return m_pitch;
    }
    /** Get the ROLL angle (in radians) \sa setFromValues */
    inline double roll() const
    {
        updateYawPitchRoll();
        return m_roll;
    }

    /** Returns a 1x6 vector with [x y z yaw pitch roll] */
    void getAsVector(mrpt::math::CVectorDouble& v) const;
    /// \overload
    void getAsVector(mrpt::math::CArrayDouble<6>& v) const;

    /** Returns the quaternion associated to the rotation of this object (NOTE:
     * XYZ translation is ignored)
     * \f[ \mathbf{q} = \left( \begin{array}{c} \cos (\phi /2) \cos (\theta /2)
     * \cos (\psi /2) +  \sin (\phi /2) \sin (\theta /2) \sin (\psi /2) \\ \sin
     * (\phi /2) \cos (\theta /2) \cos (\psi /2) -  \cos (\phi /2) \sin (\theta
     * /2) \sin (\psi /2) \\ \cos (\phi /2) \sin (\theta /2) \cos (\psi /2) +
     * \sin (\phi /2) \cos (\theta /2) \sin (\psi /2) \\ \cos (\phi /2) \cos
     * (\theta /2) \sin (\psi /2) -  \sin (\phi /2) \sin (\theta /2) \cos (\psi
     * /2) \\ \end{array}\right) \f]
     * With : \f$ \phi = roll \f$,  \f$ \theta = pitch \f$ and \f$ \psi = yaw
     * \f$.
     * \param out_dq_dr  If provided, the 4x3 Jacobian of the transformation
     * will be computed and stored here. It's the Jacobian of the transformation
     * from (yaw pitch roll) to (qr qx qy qz).
     */
    void getAsQuaternion(
        mrpt::math::CQuaternionDouble& q,
        mrpt::math::CMatrixFixedNumeric<double, 4, 3>* out_dq_dr =
            nullptr) const;

    inline const double& operator[](unsigned int i) const
    {
        updateYawPitchRoll();
        switch (i)
        {
            case 0:
                return m_coords[0];
            case 1:
                return m_coords[1];
            case 2:
                return m_coords[2];
            case 3:
                return m_yaw;
            case 4:
                return m_pitch;
            case 5:
                return m_roll;
            default:
                throw std::runtime_error(
                    "CPose3D::operator[]: Index of bounds.");
        }
    }
    // CPose3D CANNOT have a write [] operator, since it'd leave the object in
    // an inconsistent state (outdated rotation matrix).
    // Use setFromValues() instead.
    // inline double &operator[](unsigned int i)

    /** Returns a human-readable textual representation of the object (eg: "[x y
     * z yaw pitch roll]", angles in degrees.)
     * \sa fromString
     */
    void asString(std::string& s) const
    {
        using mrpt::RAD2DEG;
        updateYawPitchRoll();
        s = mrpt::format(
            "[%f %f %f %f %f %f]", m_coords[0], m_coords[1], m_coords[2],
            RAD2DEG(m_yaw), RAD2DEG(m_pitch), RAD2DEG(m_roll));
    }
    inline std::string asString() const
    {
        std::string s;
        asString(s);
        return s;
    }

    /** Set the current object value from a string generated by 'asString' (eg:
     * "[x y z yaw pitch roll]", angles in deg. )
     * \sa asString
     * \exception std::exception On invalid format
     */
    void fromString(const std::string& s)
    {
        using mrpt::DEG2RAD;
        mrpt::math::CMatrixDouble m;
        if (!m.fromMatlabStringFormat(s))
            THROW_EXCEPTION("Malformed expression in ::fromString");
        ASSERTMSG_(m.rows() == 1 && m.cols() == 6, "Expected vector length=6");
        this->setFromValues(
            m.get_unsafe(0, 0), m.get_unsafe(0, 1), m.get_unsafe(0, 2),
            DEG2RAD(m.get_unsafe(0, 3)), DEG2RAD(m.get_unsafe(0, 4)),
            DEG2RAD(m.get_unsafe(0, 5)));
    }
    /** Same as fromString, but without requiring the square brackets in the
     * string */
    void fromStringRaw(const std::string& s)
    {
        this->fromString("[" + s + "]");
    }

    /** Return true if the 6D pose represents a Z axis almost exactly vertical
     * (upwards or downwards), with a given tolerance (if set to 0 exact
     * horizontality is tested). */
    bool isHorizontal(const double tolerance = 0) const;

    /** The euclidean distance between two poses taken as two 6-length vectors
     * (angles in radians). */
    double distanceEuclidean6D(const CPose3D& o) const;

    /** @} */  // modif. components

    /** @name Lie Algebra methods
        @{ */

    /** Exponentiate a Vector in the SE(3) Lie Algebra to generate a new CPose3D
     * (static method).
     * \param pseudo_exponential If set to true, XYZ are copied from the first
     * three elements in the vector instead of using the proper Lie Algebra
     * formulas (this is actually the common practice in robotics literature).
     */
    static CPose3D exp(
        const mrpt::math::CArrayNumeric<double, 6>& vect,
        bool pseudo_exponential = false);

    /** \overload */
    static void exp(
        const mrpt::math::CArrayNumeric<double, 6>& vect, CPose3D& out_pose,
        bool pseudo_exponential = false);

    /** Exponentiate a vector in the Lie algebra to generate a new SO(3) (a 3x3
     * rotation matrix). */
    static mrpt::math::CMatrixDouble33 exp_rotation(
        const mrpt::math::CArrayNumeric<double, 3>& vect);

    /** Take the logarithm of the 3x4 matrix defined by this pose, generating
     * the corresponding vector in the SE(3) Lie Algebra.
     * \sa ln_jacob
     */
    void ln(mrpt::math::CArrayDouble<6>& out_ln) const;

    /// \overload
    inline mrpt::math::CArrayDouble<6> ln() const
    {
        mrpt::math::CArrayDouble<6> ret;
        ln(ret);
        return ret;
    }

    /** Jacobian of the logarithm of the 3x4 matrix defined by this pose.
     * \sa ln
     */
    void ln_jacob(mrpt::math::CMatrixFixedNumeric<double, 6, 12>& J) const;

    /** Static function to compute the Jacobian of the SO(3) Logarithm function,
     * evaluated at a given 3x3 rotation matrix R.
     * \sa ln, ln_jacob
     */
    static void ln_rot_jacob(
        const mrpt::math::CMatrixDouble33& R,
        mrpt::math::CMatrixFixedNumeric<double, 3, 9>& M);

    /** Take the logarithm of the 3x3 rotation matrix, generating the
     * corresponding vector in the Lie Algebra */
    mrpt::math::CArrayDouble<3> ln_rotation() const;

    /** The Jacobian d (e^eps * D) / d eps , with eps=increment in Lie Algebra.
     * \note Eq. 10.3.5 in tech report
     * http://ingmec.ual.es/~jlblanco/papers/jlblanco2010geometry3D_techrep.pdf
     */
    static void jacob_dexpeD_de(
        const CPose3D& D, Eigen::Matrix<double, 12, 6>& jacob);

    /** The Jacobian d (A * e^eps * D) / d eps , with eps=increment in Lie
     * Algebra.
     * \note Eq. 10.3.7 in tech report
     * http://ingmec.ual.es/~jlblanco/papers/jlblanco2010geometry3D_techrep.pdf
     */
    static void jacob_dAexpeD_de(
        const CPose3D& A, const CPose3D& D,
        Eigen::Matrix<double, 12, 6>& jacob);

    /** @} */

    void setToNaN() override;

    /** Used to emulate CPosePDF types, for example, in
     * mrpt::graphs::CNetworkOfPoses */
    using type_value = CPose3D;
    enum
    {
        is_3D_val = 1
    };
    static constexpr bool is_3D() { return is_3D_val != 0; }
    enum
    {
        rotation_dimensions = 3
    };
    enum
    {
        is_PDF_val = 0
    };
    static constexpr bool is_PDF() { return is_PDF_val != 0; }
    inline const type_value& getPoseMean() const { return *this; }
    inline type_value& getPoseMean() { return *this; }
    /** @name STL-like methods and typedefs
       @{   */
    /** The type of the elements */
    using value_type = double;
    using reference = double&;
    using const_reference = const double&;
    using size_type = std::size_t;
    using difference_type = std::ptrdiff_t;

    // size is constant
    enum
    {
        static_size = 6
    };
    static constexpr size_type size() { return static_size; }
    static constexpr bool empty() { return false; }
    static constexpr size_type max_size() { return static_size; }
    static inline void resize(const size_t n)
    {
        if (n != static_size)
            throw std::logic_error(format(
                "Try to change the size of CPose3D to %u.",
                static_cast<unsigned>(n)));
    }
    /** @} */

};  // End of class def.

std::ostream& operator<<(std::ostream& o, const CPose3D& p);

/** Unary - operator: return the inverse pose "-p" (Note that is NOT the same
 * than a pose with negative x y z yaw pitch roll) */
CPose3D operator-(const CPose3D& p);

bool operator==(const CPose3D& p1, const CPose3D& p2);
bool operator!=(const CPose3D& p1, const CPose3D& p2);

}  // namespace mrpt::poses
#endif