CPose3D.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2017, 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 |
   +------------------------------------------------------------------------+ */

#include "base-precomp.h"  // Precompiled headers

#include <mrpt/config.h>  // for HAVE_SINCOS
#include <mrpt/utils/types_math.h>  // for CVectorD...
#include <mrpt/math/CMatrix.h>  // for CMatrix
#include <mrpt/math/geometry.h>  // for skew_sym...
#include <mrpt/math/matrix_serialization.h>  // for operator>>
#include <mrpt/math/wrap2pi.h>  // for wrapToPi
#include <mrpt/poses/CPoint2D.h>  // for CPoint2D
#include <mrpt/poses/CPoint3D.h>  // for CPoint3D
#include <mrpt/poses/CPose2D.h>  // for CPose2D
#include <mrpt/poses/CPose3D.h>  // for CPose3D
#include <mrpt/poses/CPose3DQuat.h>  // for CPose3DQuat
#include <mrpt/poses/CPose3DRotVec.h>  // for CPose3DR...
#include <mrpt/utils/CStream.h>  // for CStream
#include <algorithm>  // for move
#include <cmath>  // for fabs
#include <iomanip>  // for operator<<
#include <limits>  // for numeric_...
#include <ostream>  // for operator<<
#include <string>  // for allocator
#include <mrpt/math/CArrayNumeric.h>  // for CArrayDo...
#include <mrpt/math/CMatrixFixedNumeric.h>  // for CMatrixF...
#include <mrpt/math/CMatrixTemplateNumeric.h>  // for CMatrixD...
#include <mrpt/math/CQuaternion.h>  // for CQuatern...
#include <mrpt/math/homog_matrices.h>  // for homogene...
#include <mrpt/math/lightweight_geom_data.h>  // for TPoint3D
#include <mrpt/math/ops_containers.h>  // for dotProduct
#include <mrpt/utils/CSerializable.h>  // for CSeriali...
#include <mrpt/utils/bits.h>  // for square
#include <mrpt/math/utils_matlab.h>
#include <mrpt/otherlibs/sophus/so3.hpp>
#include <mrpt/otherlibs/sophus/se3.hpp>

using namespace mrpt;
using namespace mrpt::math;
using namespace mrpt::utils;
using namespace mrpt::poses;

IMPLEMENTS_SERIALIZABLE(CPose3D, CSerializable, mrpt::poses)

/*---------------------------------------------------------------
        Constructors
  ---------------------------------------------------------------*/
CPose3D::CPose3D() : m_ypr_uptodate(true), m_yaw(0), m_pitch(0), m_roll(0)
{
        m_coords[0] = m_coords[1] = m_coords[2] = 0;
        m_ROT.unit(3, 1.0);
}

CPose3D::CPose3D(
        const double x, const double y, const double z, const double yaw,
        const double pitch, const double roll)
        : m_ROT(UNINITIALIZED_MATRIX), m_ypr_uptodate(false)
{
        setFromValues(x, y, z, yaw, pitch, roll);
}

CPose3D::CPose3D(const mrpt::math::TPose3D& o) : m_ypr_uptodate(false)
{
        setFromValues(o.x, o.y, o.z, o.yaw, o.pitch, o.roll);
}

CPose3D::CPose3D(const CPose2D& p) : m_ypr_uptodate(false)
{
        setFromValues(p.x(), p.y(), 0, p.phi(), 0, 0);
}

CPose3D::CPose3D(const CPoint3D& p)
        : m_ypr_uptodate(false), m_yaw(), m_pitch(), m_roll()
{
        setFromValues(p.x(), p.y(), p.z());
}

CPose3D::CPose3D(const math::CMatrixDouble& m)
        : m_ROT(UNINITIALIZED_MATRIX), m_ypr_uptodate(false)
{
        ASSERT_ABOVEEQ_(mrpt::math::size(m, 1), 3);
        ASSERT_ABOVEEQ_(mrpt::math::size(m, 2), 4);
        for (int r = 0; r < 3; r++)
                for (int c = 0; c < 3; c++) m_ROT(r, c) = m.get_unsafe(r, c);
        for (int r = 0; r < 3; r++) m_coords[r] = m.get_unsafe(r, 3);
}

CPose3D::CPose3D(const math::CMatrixDouble44& m)
        : m_ROT(UNINITIALIZED_MATRIX), m_ypr_uptodate(false)
{
        for (int r = 0; r < 3; r++)
                for (int c = 0; c < 3; c++) m_ROT(r, c) = m.get_unsafe(r, c);
        for (int r = 0; r < 3; r++) m_coords[r] = m.get_unsafe(r, 3);
}

/** Constructor from a quaternion (which only represents the 3D rotation part)
 * and a 3D displacement. */
CPose3D::CPose3D(
        const mrpt::math::CQuaternionDouble& q, const double _x, const double _y,
        const double _z)
        : m_ROT(UNINITIALIZED_MATRIX), m_ypr_uptodate(false)
{
        double yaw, pitch, roll;
        q.rpy(roll, pitch, yaw);
        this->setFromValues(_x, _y, _z, yaw, pitch, roll);
}

/** Constructor from a quaternion-based full pose. */
CPose3D::CPose3D(const CPose3DQuat& p)
        : m_ROT(UNINITIALIZED_MATRIX), m_ypr_uptodate(false)
{
        // Extract XYZ + ROT from quaternion:
        m_coords[0] = p.x();
        m_coords[1] = p.y();
        m_coords[2] = p.z();
        p.quat().rotationMatrixNoResize(m_ROT);
}

/** Constructor from a rotation vector-based full pose. */
CPose3D::CPose3D(const CPose3DRotVec& p)
        : m_ROT(UNINITIALIZED_MATRIX), m_ypr_uptodate(false)
{
        m_coords[0] = p.m_coords[0];
        m_coords[1] = p.m_coords[1];
        m_coords[2] = p.m_coords[2];

        this->setRotationMatrix(this->exp_rotation(p.m_rotvec));
}

/*---------------------------------------------------------------
   Implements the writing to a CStream capability of
         CSerializable objects
  ---------------------------------------------------------------*/
void CPose3D::writeToStream(mrpt::utils::CStream& out, int* version) const
{
        if (version)
                *version = 2;
        else
        {
                const CPose3DQuat q(*this);
                // The coordinates:
                out << q[0] << q[1] << q[2] << q[3] << q[4] << q[5] << q[6];
        }
}

/*---------------------------------------------------------------
        Implements the reading from a CStream capability of
                CSerializable objects
  ---------------------------------------------------------------*/
void CPose3D::readFromStream(mrpt::utils::CStream& in, int version)
{
        switch (version)
        {
                case 0:
                {
                        // The coordinates:
                        CMatrix HM2;
                        in >> HM2;
                        ASSERT_(mrpt::math::size(HM2, 1) == 4 && HM2.isSquare())

                        m_ROT = HM2.block(0, 0, 3, 3).cast<double>();

                        m_coords[0] = HM2.get_unsafe(0, 3);
                        m_coords[1] = HM2.get_unsafe(1, 3);
                        m_coords[2] = HM2.get_unsafe(2, 3);
                        m_ypr_uptodate = false;
                }
                break;
                case 1:
                {
                        // The coordinates:
                        CMatrixDouble44 HM;
                        in >> HM;

                        m_ROT = HM.block(0, 0, 3, 3);

                        m_coords[0] = HM.get_unsafe(0, 3);
                        m_coords[1] = HM.get_unsafe(1, 3);
                        m_coords[2] = HM.get_unsafe(2, 3);
                        m_ypr_uptodate = false;
                }
                break;
                case 2:
                {
                        // An equivalent CPose3DQuat
                        CPose3DQuat p(UNINITIALIZED_QUATERNION);
                        in >> p[0] >> p[1] >> p[2] >> p[3] >> p[4] >> p[5] >> p[6];

                        // Extract XYZ + ROT from quaternion:
                        m_ypr_uptodate = false;
                        m_coords[0] = p.x();
                        m_coords[1] = p.y();
                        m_coords[2] = p.z();
                        p.quat().rotationMatrixNoResize(m_ROT);
                }
                break;
                default:
                        MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version)
        };
}

/**  Textual output stream function.
 */
std::ostream& mrpt::poses::operator<<(std::ostream& o, const CPose3D& p)
{
        const std::streamsize old_pre = o.precision();
        const std::ios_base::fmtflags old_flags = o.flags();
        o << "(x,y,z,yaw,pitch,roll)=(" << std::fixed << std::setprecision(4)
          << p.m_coords[0] << "," << p.m_coords[1] << "," << p.m_coords[2] << ","
          << std::setprecision(2) << RAD2DEG(p.yaw()) << "deg,"
          << RAD2DEG(p.pitch()) << "deg," << RAD2DEG(p.roll()) << "deg)";
        o.flags(old_flags);
        o.precision(old_pre);
        return o;
}

/*---------------------------------------------------------------
  Implements the writing to a mxArray for Matlab
 ---------------------------------------------------------------*/
#if MRPT_HAS_MATLAB
// Add to implement mexplus::from template specialization
IMPLEMENTS_MEXPLUS_FROM(mrpt::poses::CPose3D)

mxArray* CPose3D::writeToMatlab() const
{
        const char* fields[] = {"R", "t"};
        mexplus::MxArray pose_struct(
                mexplus::MxArray::Struct(sizeof(fields) / sizeof(fields[0]), fields));
        pose_struct.set("R", mrpt::math::convertToMatlab(this->m_ROT));
        pose_struct.set("t", mrpt::math::convertToMatlab(this->m_coords));
        return pose_struct.release();
}
#endif

/*---------------------------------------------------------------
                                normalizeAngles
---------------------------------------------------------------*/
void CPose3D::normalizeAngles() { updateYawPitchRoll(); }
/*---------------------------------------------------------------
 Set the pose from 3D point and yaw/pitch/roll angles, in radians.
---------------------------------------------------------------*/
void CPose3D::setFromValues(
        const double x0, const double y0, const double z0, const double yaw,
        const double pitch, const double roll)
{
        m_coords[0] = x0;
        m_coords[1] = y0;
        m_coords[2] = z0;
        this->m_yaw = mrpt::math::wrapToPi(yaw);
        this->m_pitch = mrpt::math::wrapToPi(pitch);
        this->m_roll = mrpt::math::wrapToPi(roll);

        m_ypr_uptodate = true;

        rebuildRotationMatrix();
}

/*---------------------------------------------------------------
 Set the pose from 3D point and yaw/pitch/roll angles, in radians.
---------------------------------------------------------------*/
void CPose3D::rebuildRotationMatrix()
{
#ifdef HAVE_SINCOS
        double cy, sy;
        ::sincos(m_yaw, &sy, &cy);
        double cp, sp;
        ::sincos(m_pitch, &sp, &cp);
        double cr, sr;
        ::sincos(m_roll, &sr, &cr);
#else
        const double cy = cos(m_yaw);
        const double sy = sin(m_yaw);
        const double cp = cos(m_pitch);
        const double sp = sin(m_pitch);
        const double cr = cos(m_roll);
        const double sr = sin(m_roll);
#endif

        alignas(16) const double rot_vals[] = {cy * cp,
                                                                                   cy * sp * sr - sy * cr,
                                                                                   cy * sp * cr + sy * sr,
                                                                                   sy * cp,
                                                                                   sy * sp * sr + cy * cr,
                                                                                   sy * sp * cr - cy * sr,
                                                                                   -sp,
                                                                                   cp * sr,
                                                                                   cp * cr};
        m_ROT.loadFromArray(rot_vals);
}

/*---------------------------------------------------------------
                Scalar multiplication.
---------------------------------------------------------------*/
void CPose3D::operator*=(const double s)
{
        updateYawPitchRoll();
        m_coords[0] *= s;
        m_coords[1] *= s;
        m_coords[2] *= s;
        m_yaw *= s;
        m_pitch *= s;
        m_roll *= s;
        rebuildRotationMatrix();
}

/*---------------------------------------------------------------
                getYawPitchRoll
---------------------------------------------------------------*/
void CPose3D::getYawPitchRoll(double& yaw, double& pitch, double& roll) const
{
        ASSERTDEBMSG_(
                std::abs(
                        sqrt(
                                square(m_ROT(0, 0)) + square(m_ROT(1, 0)) +
                                square(m_ROT(2, 0))) -
                        1) < 3e-3,
                "Homogeneous matrix is not orthogonal & normalized!: " +
                        m_ROT.inMatlabFormat())
        ASSERTDEBMSG_(
                std::abs(
                        sqrt(
                                square(m_ROT(0, 1)) + square(m_ROT(1, 1)) +
                                square(m_ROT(2, 1))) -
                        1) < 3e-3,
                "Homogeneous matrix is not orthogonal & normalized!: " +
                        m_ROT.inMatlabFormat())
        ASSERTDEBMSG_(
                std::abs(
                        sqrt(
                                square(m_ROT(0, 2)) + square(m_ROT(1, 2)) +
                                square(m_ROT(2, 2))) -
                        1) < 3e-3,
                "Homogeneous matrix is not orthogonal & normalized!: " +
                        m_ROT.inMatlabFormat())

        // Pitch is in the range [-pi/2, pi/2 ], so this calculation is enough:
        pitch = atan2(
                -m_ROT(2, 0),
                hypot(m_ROT(0, 0), m_ROT(1, 0)));  // asin( - m_ROT(2,0) );

        // Roll:
        if ((fabs(m_ROT(2, 1)) + fabs(m_ROT(2, 2))) <
                10 * std::numeric_limits<double>::epsilon())
        {
                // Gimbal lock between yaw and roll. This one is arbitrarily forced to
                // be zero.
                // Check
                // http://reference.mrpt.org/devel/classmrpt_1_1poses_1_1_c_pose3_d.html.
                // If cos(pitch)==0, the homogeneous matrix is:
                // When sin(pitch)==1:
                //  /0  cysr-sycr cycr+sysr x\   /0  sin(r-y) cos(r-y)  x\.
                //  |0  sysr+cycr sycr-cysr y| = |0  cos(r-y) -sin(r-y) y|
                //  |-1     0         0     z|   |-1    0         0     z|
                //  \0      0         0     1/   \0     0         0     1/
                //
                // And when sin(pitch)=-1:
                //  /0 -cysr-sycr -cycr+sysr x\   /0 -sin(r+y) -cos(r+y) x\.
                //  |0 -sysr+cycr -sycr-cysr y| = |0 cos(r+y)  -sin(r+y) y|
                //  |1      0          0     z|   |1    0          0     z|
                //  \0      0          0     1/   \0    0          0     1/
                //
                // Both cases are in a "gimbal lock" status. This happens because pitch
                // is vertical.

                roll = 0.0;
                if (pitch > 0)
                        yaw = atan2(m_ROT(1, 2), m_ROT(0, 2));
                else
                        yaw = atan2(-m_ROT(1, 2), -m_ROT(0, 2));
        }
        else
        {
                roll = atan2(m_ROT(2, 1), m_ROT(2, 2));
                // Yaw:
                yaw = atan2(m_ROT(1, 0), m_ROT(0, 0));
        }
}

/*---------------------------------------------------------------
                sphericalCoordinates
---------------------------------------------------------------*/
void CPose3D::sphericalCoordinates(
        const TPoint3D& point, double& out_range, double& out_yaw,
        double& out_pitch) const
{
        // Pass to coordinates as seen from this 6D pose:
        TPoint3D local;
        this->inverseComposePoint(
                point.x, point.y, point.z, local.x, local.y, local.z);

        // Range:
        out_range = local.norm();

        // Yaw:
        if (local.y != 0 || local.x != 0)
                out_yaw = atan2(local.y, local.x);
        else
                out_yaw = 0;

        // Pitch:
        if (out_range != 0)
                out_pitch = -asin(local.z / out_range);
        else
                out_pitch = 0;
}

CPose3D CPose3D::getOppositeScalar() const
{
        return CPose3D(
                -m_coords[0], -m_coords[1], -m_coords[2], -m_yaw, -m_pitch, -m_roll);
}

/*---------------------------------------------------------------
                addComponents
---------------------------------------------------------------*/
void CPose3D::addComponents(const CPose3D& p)
{
        updateYawPitchRoll();
        m_coords[0] += p.m_coords[0];
        m_coords[1] += p.m_coords[1];
        m_coords[2] += p.m_coords[2];
        m_yaw += p.m_yaw;
        m_pitch += p.m_pitch;
        m_roll += p.m_roll;
        rebuildRotationMatrix();
}

/*---------------------------------------------------------------
                distanceEuclidean6D
---------------------------------------------------------------*/
double CPose3D::distanceEuclidean6D(const CPose3D& o) const
{
        updateYawPitchRoll();
        o.updateYawPitchRoll();
        return sqrt(
                square(o.m_coords[0] - m_coords[0]) +
                square(o.m_coords[1] - m_coords[1]) +
                square(o.m_coords[2] - m_coords[2]) +
                square(wrapToPi(o.m_yaw - m_yaw)) +
                square(wrapToPi(o.m_pitch - m_pitch)) +
                square(wrapToPi(o.m_roll - m_roll)));
}

/*---------------------------------------------------------------
                composePoint
---------------------------------------------------------------*/
void CPose3D::composePoint(
        double lx, double ly, double lz, double& gx, double& gy, double& gz,
        mrpt::math::CMatrixFixedNumeric<double, 3, 3>* out_jacobian_df_dpoint,
        mrpt::math::CMatrixFixedNumeric<double, 3, 6>* out_jacobian_df_dpose,
        mrpt::math::CMatrixFixedNumeric<double, 3, 6>* out_jacobian_df_dse3,
        bool use_small_rot_approx) const
{
        // Jacob: df/dpoint
        if (out_jacobian_df_dpoint) *out_jacobian_df_dpoint = m_ROT;

        // Jacob: df/dpose
        if (out_jacobian_df_dpose)
        {
                if (use_small_rot_approx)
                {
                        // Linearized Jacobians around (yaw,pitch,roll)=(0,0,0):
                        alignas(16) const double nums[3 * 6] = {
                                1, 0, 0, -ly, lz, 0, 0, 1, 0, lx, 0, -lz, 0, 0, 1, 0, -lx, ly};
                        out_jacobian_df_dpose->loadFromArray(nums);
                }
                else
                {
                        // Exact Jacobians:
                        updateYawPitchRoll();
#ifdef HAVE_SINCOS
                        double cy, sy;
                        ::sincos(m_yaw, &sy, &cy);
                        double cp, sp;
                        ::sincos(m_pitch, &sp, &cp);
                        double cr, sr;
                        ::sincos(m_roll, &sr, &cr);
#else
                        const double cy = cos(m_yaw);
                        const double sy = sin(m_yaw);
                        const double cp = cos(m_pitch);
                        const double sp = sin(m_pitch);
                        const double cr = cos(m_roll);
                        const double sr = sin(m_roll);
#endif

                        alignas(16) const double nums[3 * 6] = {
                                1, 0, 0,
                                -lx * sy * cp + ly * (-sy * sp * sr - cy * cr) +
                                        lz * (-sy * sp * cr + cy * sr),  // d_x'/d_yaw
                                -lx * cy * sp + ly * (cy * cp * sr) +
                                        lz * (cy * cp * cr),  // d_x'/d_pitch
                                ly * (cy * sp * cr + sy * sr) +
                                        lz * (-cy * sp * sr + sy * cr),  // d_x'/d_roll
                                0,
                                1, 0,
                                lx * cy * cp + ly * (cy * sp * sr - sy * cr) +
                                        lz * (cy * sp * cr + sy * sr),  // d_y'/d_yaw
                                -lx * sy * sp + ly * (sy * cp * sr) +
                                        lz * (sy * cp * cr),  // d_y'/d_pitch
                                ly * (sy * sp * cr - cy * sr) +
                                        lz * (-sy * sp * sr - cy * cr),  // d_y'/d_roll
                                0,
                                0, 1,
                                0,  // d_z' / d_yaw
                                -lx * cp - ly * sp * sr - lz * sp * cr,  // d_z' / d_pitch
                                ly * cp * cr - lz * cp * sr  // d_z' / d_roll
                        };
                        out_jacobian_df_dpose->loadFromArray(nums);
                }
        }

        gx = m_ROT(0, 0) * lx + m_ROT(0, 1) * ly + m_ROT(0, 2) * lz + m_coords[0];
        gy = m_ROT(1, 0) * lx + m_ROT(1, 1) * ly + m_ROT(1, 2) * lz + m_coords[1];
        gz = m_ROT(2, 0) * lx + m_ROT(2, 1) * ly + m_ROT(2, 2) * lz + m_coords[2];

        // Jacob: df/dse3
        if (out_jacobian_df_dse3)
        {
                alignas(16) const double nums[3 * 6] = {
                        1, 0, 0, 0, gz, -gy, 0, 1, 0, -gz, 0, gx, 0, 0, 1, gy, -gx, 0};
                out_jacobian_df_dse3->loadFromArray(nums);
        }
}

// TODO: Use SSE2? OTOH, this forces mem align...
#if MRPT_HAS_SSE2 && defined(MRPT_USE_SSE2)
/*static inline __m128 transformSSE(const __m128* matrix, const __m128& in)
{
        ASSERT_(((size_t)matrix & 15) == 0);
        __m128 a0 = _mm_mul_ps(_mm_load_ps((float*)(matrix+0)),
_mm_shuffle_ps(in,in,_MM_SHUFFLE(0,0,0,0)));
        __m128 a1 = _mm_mul_ps(_mm_load_ps((float*)(matrix+1)),
_mm_shuffle_ps(in,in,_MM_SHUFFLE(1,1,1,1)));
        __m128 a2 = _mm_mul_ps(_mm_load_ps((float*)(matrix+2)),
_mm_shuffle_ps(in,in,_MM_SHUFFLE(2,2,2,2)));

        return _mm_add_ps(_mm_add_ps(a0,a1),a2);
}*/
#endif  // SSE2

/*---------------------------------------------------------------
                getAsVector
---------------------------------------------------------------*/
void CPose3D::getAsVector(CVectorDouble& r) const
{
        updateYawPitchRoll();
        r.resize(6);
        r[0] = m_coords[0];
        r[1] = m_coords[1];
        r[2] = m_coords[2];
        r[3] = m_yaw;
        r[4] = m_pitch;
        r[5] = m_roll;
}

void CPose3D::getAsVector(mrpt::math::CArrayDouble<6>& r) const
{
        updateYawPitchRoll();
        r[0] = m_coords[0];
        r[1] = m_coords[1];
        r[2] = m_coords[2];
        r[3] = m_yaw;
        r[4] = m_pitch;
        r[5] = m_roll;
}

/*---------------------------------------------------------------
                unary -
---------------------------------------------------------------*/
CPose3D mrpt::poses::operator-(const CPose3D& b)
{
        CMatrixDouble44 B_INV(UNINITIALIZED_MATRIX);
        b.getInverseHomogeneousMatrix(B_INV);
        return CPose3D(B_INV);
}

void CPose3D::getAsQuaternion(
        mrpt::math::CQuaternionDouble& q,
        mrpt::math::CMatrixFixedNumeric<double, 4, 3>* out_dq_dr) const
{
        updateYawPitchRoll();
        mrpt::math::TPose3D(0, 0, 0, m_yaw, m_pitch, m_roll)
                .getAsQuaternion(q, out_dq_dr);
}

bool mrpt::poses::operator==(const CPose3D& p1, const CPose3D& p2)
{
        return (p1.m_coords == p2.m_coords) &&
                   ((p1.getRotationMatrix() - p2.getRotationMatrix())
                                .array()
                                .abs()
                                .maxCoeff() < 1e-6);
}

bool mrpt::poses::operator!=(const CPose3D& p1, const CPose3D& p2)
{
        return (p1.m_coords != p2.m_coords) ||
                   ((p1.getRotationMatrix() - p2.getRotationMatrix())
                                .array()
                                .abs()
                                .maxCoeff() >= 1e-6);
}

/*---------------------------------------------------------------
                                point3D = pose3D + point3D
  ---------------------------------------------------------------*/
CPoint3D CPose3D::operator+(const CPoint3D& b) const
{
        return CPoint3D(
                m_coords[0] + m_ROT(0, 0) * b.x() + m_ROT(0, 1) * b.y() +
                        m_ROT(0, 2) * b.z(),
                m_coords[1] + m_ROT(1, 0) * b.x() + m_ROT(1, 1) * b.y() +
                        m_ROT(1, 2) * b.z(),
                m_coords[2] + m_ROT(2, 0) * b.x() + m_ROT(2, 1) * b.y() +
                        m_ROT(2, 2) * b.z());
}

/*---------------------------------------------------------------
                                point3D = pose3D + point2D
  ---------------------------------------------------------------*/
CPoint3D CPose3D::operator+(const CPoint2D& b) const
{
        return CPoint3D(
                m_coords[0] + m_ROT(0, 0) * b.x() + m_ROT(0, 1) * b.y(),
                m_coords[1] + m_ROT(1, 0) * b.x() + m_ROT(1, 1) * b.y(),
                m_coords[2] + m_ROT(2, 0) * b.x() + m_ROT(2, 1) * b.y());
}

/*---------------------------------------------------------------
                                this = A + B
  ---------------------------------------------------------------*/
void CPose3D::composeFrom(const CPose3D& A, const CPose3D& B)
{
        // The translation part HM(0:3,3)
        if (this == &B)
        {
                // we need to make a temporary copy of the vector:
                const CArrayDouble<3> B_coords = B.m_coords;
                for (int r = 0; r < 3; r++)
                        m_coords[r] = A.m_coords[r] + A.m_ROT(r, 0) * B_coords[0] +
                                                  A.m_ROT(r, 1) * B_coords[1] +
                                                  A.m_ROT(r, 2) * B_coords[2];
        }
        else
        {
                for (int r = 0; r < 3; r++)
                        m_coords[r] = A.m_coords[r] + A.m_ROT(r, 0) * B.m_coords[0] +
                                                  A.m_ROT(r, 1) * B.m_coords[1] +
                                                  A.m_ROT(r, 2) * B.m_coords[2];
        }

        // Important: Make this multiplication AFTER the translational part, to cope
        // with the case when A==this
        m_ROT.multiply_AB(A.m_ROT, B.m_ROT);

        m_ypr_uptodate = false;
}

/** Convert this pose into its inverse, saving the result in itself. */
void CPose3D::inverse()
{
        CMatrixDouble33 inv_rot(UNINITIALIZED_MATRIX);
        CArrayDouble<3> inv_xyz;

        mrpt::math::homogeneousMatrixInverse(m_ROT, m_coords, inv_rot, inv_xyz);

        m_ROT = inv_rot;
        m_coords = inv_xyz;
        m_ypr_uptodate = false;
}

/*---------------------------------------------------------------
                                                isHorizontal
 ---------------------------------------------------------------*/
bool CPose3D::isHorizontal(const double tolerance) const
{
        updateYawPitchRoll();
        return (fabs(m_pitch) <= tolerance || M_PI - fabs(m_pitch) <= tolerance) &&
                   (fabs(m_roll) <= tolerance ||
                        fabs(mrpt::math::wrapToPi(m_roll - M_PI)) <= tolerance);
}

/**  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 CPose3D::inverseComposeFrom(const CPose3D& A, const CPose3D& B)
{
        // this    =    A  (-)  B
        // HM_this = inv(HM_B) * HM_A
        //
        // [  R_b  | t_b ] -1   [  R_a  | t_a ]    [ R_b^t * Ra |    ..    ]
        // [ ------+-----]    * [ ------+-----]  = [ ---------- +----------]
        // [ 0 0 0 |  1  ]      [ 0 0 0 |  1  ]    [  0  0   0  |      1   ]
        //

        // XYZ part:
        CMatrixDouble33 R_b_inv(UNINITIALIZED_MATRIX);
        CArrayDouble<3> t_b_inv;
        mrpt::math::homogeneousMatrixInverse(B.m_ROT, B.m_coords, R_b_inv, t_b_inv);

        for (int i = 0; i < 3; i++)
                m_coords[i] = t_b_inv[i] + R_b_inv(i, 0) * A.m_coords[0] +
                                          R_b_inv(i, 1) * A.m_coords[1] +
                                          R_b_inv(i, 2) * A.m_coords[2];

        // Rot part:
        m_ROT.multiply_AB(R_b_inv, A.m_ROT);
        m_ypr_uptodate = false;
}

/**  Computes the 3D point L such as \f$ L = G \ominus this \f$.
  * \sa composePoint, composeFrom
  */
void CPose3D::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,
        mrpt::math::CMatrixFixedNumeric<double, 3, 6>* out_jacobian_df_dpose,
        mrpt::math::CMatrixFixedNumeric<double, 3, 6>* out_jacobian_df_dse3) const
{
        CMatrixDouble33 R_inv(UNINITIALIZED_MATRIX);
        CArrayDouble<3> t_inv;
        mrpt::math::homogeneousMatrixInverse(m_ROT, m_coords, R_inv, t_inv);

        // Jacob: df/dpoint
        if (out_jacobian_df_dpoint) *out_jacobian_df_dpoint = R_inv;

        // Jacob: df/dpose
        if (out_jacobian_df_dpose)
        {
                // TODO: Perhaps this and the sin/cos's can be avoided if all needed
                // terms are already in m_ROT ???
                updateYawPitchRoll();

#ifdef HAVE_SINCOS
                double cy, sy;
                ::sincos(m_yaw, &sy, &cy);
                double cp, sp;
                ::sincos(m_pitch, &sp, &cp);
                double cr, sr;
                ::sincos(m_roll, &sr, &cr);
#else
                const double cy = cos(m_yaw);
                const double sy = sin(m_yaw);
                const double cp = cos(m_pitch);
                const double sp = sin(m_pitch);
                const double cr = cos(m_roll);
                const double sr = sin(m_roll);
#endif

                const double m11_dy = -sy * cp;
                const double m12_dy = cy * cp;
                const double m13_dy = 0;
                const double m11_dp = -cy * sp;
                const double m12_dp = -sy * sp;
                const double m13_dp = -cp;
                const double m11_dr = 0;
                const double m12_dr = 0;
                const double m13_dr = 0;

                const double m21_dy = (-sy * sp * sr - cy * cr);
                const double m22_dy = (cy * sp * sr - sy * cr);
                const double m23_dy = 0;
                const double m21_dp = (cy * cp * sr);
                const double m22_dp = (sy * cp * sr);
                const double m23_dp = -sp * sr;
                const double m21_dr = (cy * sp * cr + sy * sr);
                const double m22_dr = (sy * sp * cr - cy * sr);
                const double m23_dr = cp * cr;

                const double m31_dy = (-sy * sp * cr + cy * sr);
                const double m32_dy = (cy * sp * cr + sy * sr);
                const double m33_dy = 0;
                const double m31_dp = (cy * cp * cr);
                const double m32_dp = (sy * cp * cr);
                const double m33_dp = -sp * cr;
                const double m31_dr = (-cy * sp * sr + sy * cr);
                const double m32_dr = (-sy * sp * sr - cy * cr);
                const double m33_dr = -cp * sr;

                const double Ax = gx - m_coords[0];
                const double Ay = gy - m_coords[1];
                const double Az = gz - m_coords[2];

                alignas(16) const double nums[3 * 6] = {
                        -m_ROT(0, 0),
                        -m_ROT(1, 0),
                        -m_ROT(2, 0),
                        Ax * m11_dy + Ay * m12_dy + Az * m13_dy,  // d_x'/d_yaw
                        Ax * m11_dp + Ay * m12_dp + Az * m13_dp,  // d_x'/d_pitch
                        Ax * m11_dr + Ay * m12_dr + Az * m13_dr,  // d_x'/d_roll

                        -m_ROT(0, 1),
                        -m_ROT(1, 1),
                        -m_ROT(2, 1),
                        Ax * m21_dy + Ay * m22_dy + Az * m23_dy,  // d_x'/d_yaw
                        Ax * m21_dp + Ay * m22_dp + Az * m23_dp,  // d_x'/d_pitch
                        Ax * m21_dr + Ay * m22_dr + Az * m23_dr,  // d_x'/d_roll

                        -m_ROT(0, 2),
                        -m_ROT(1, 2),
                        -m_ROT(2, 2),
                        Ax * m31_dy + Ay * m32_dy + Az * m33_dy,  // d_x'/d_yaw
                        Ax * m31_dp + Ay * m32_dp + Az * m33_dp,  // d_x'/d_pitch
                        Ax * m31_dr + Ay * m32_dr + Az * m33_dr,  // d_x'/d_roll
                };
                out_jacobian_df_dpose->loadFromArray(nums);
        }

        lx = t_inv[0] + R_inv(0, 0) * gx + R_inv(0, 1) * gy + R_inv(0, 2) * gz;
        ly = t_inv[1] + R_inv(1, 0) * gx + R_inv(1, 1) * gy + R_inv(1, 2) * gz;
        lz = t_inv[2] + R_inv(2, 0) * gx + R_inv(2, 1) * gy + R_inv(2, 2) * gz;

        // Jacob: df/dse3
        if (out_jacobian_df_dse3)
        {
                alignas(16) const double nums[3 * 6] = {
                        -1, 0, 0, 0, -lz, ly, 0, -1, 0, lz, 0, -lx, 0, 0, -1, -ly, lx, 0};
                out_jacobian_df_dse3->loadFromArray(nums);
        }
}

CPose3D CPose3D::exp(
        const mrpt::math::CArrayNumeric<double, 6>& mu, bool pseudo_exponential)
{
        CPose3D P(UNINITIALIZED_POSE);
        CPose3D::exp(mu, P, pseudo_exponential);
        return P;
}

void CPose3D::exp(
        const mrpt::math::CArrayNumeric<double, 6>& mu, CPose3D& out_pose,
        bool pseudo_exponential)
{
        if (pseudo_exponential)
        {
                auto R = Sophus::SO3<double>::exp(mu.block<3, 1>(3, 0));
                out_pose.setRotationMatrix(R.matrix());
                out_pose.x(mu[0]);
                out_pose.y(mu[1]);
                out_pose.z(mu[2]);
        }
        else
        {
                auto R = Sophus::SE3<double>::exp(mu);
                out_pose = CPose3D(CMatrixDouble44(R.matrix()));
        }
}

CArrayDouble<3> CPose3D::ln_rotation() const
{
        Sophus::SO3<double> R(this->m_ROT);
        return R.log();
}

CMatrixDouble33 CPose3D::exp_rotation(
        const mrpt::math::CArrayNumeric<double, 3>& w)
{
        auto R = Sophus::SO3<double>::exp(w);
        return R.matrix();
}

void CPose3D::ln(CArrayDouble<6>& result) const
{
        const Sophus::SE3<double> RT(m_ROT, m_coords);
        result = RT.log();
}

/* The following code fragments are based on formulas originally reported in the
 * TooN and RobotVision packages */
namespace mrpt
{
namespace poses
{
template <class VEC3, class MAT33>
inline void deltaR(const MAT33& R, VEC3& v)
{
        v[0] = R(2, 1) - R(1, 2);
        v[1] = R(0, 2) - R(2, 0);
        v[2] = R(1, 0) - R(0, 1);
}

template <typename VEC3, typename MAT3x3, typename MAT3x9>
inline void M3x9(const VEC3& a, const MAT3x3& B, MAT3x9& RES)
{
        alignas(16) const double vals[] = {
                a[0],    -B(0, 2), B(0, 1),  B(0, 2),  a[0],    -B(0, 0), -B(0, 1),
                B(0, 0),  a[0],         a[1],    -B(1, 2), B(1, 1), B(1, 2),  a[1],
                -B(1, 0), -B(1, 1), B(1, 0),  a[1],             a[2],   -B(2, 2), B(2, 1),
                B(2, 2),  a[2],         -B(2, 0), -B(2, 1), B(2, 0), a[2]};
        RES.loadFromArray(vals);
}

inline CMatrixDouble33 ddeltaRt_dR(const CPose3D& P)
{
        const CMatrixDouble33& R = P.getRotationMatrix();
        const CArrayDouble<3>& t = P.m_coords;

        CArrayDouble<3> abc;
        deltaR(R, abc);
        double a = abc[0];
        double b = abc[1];
        double c = abc[2];

        alignas(16) const double vals[] = {
                -b * t[1] - c * t[2],    2 * b * t[0] - a * t[1],
                2 * c * t[0] - a * t[2],  -b * t[0] + 2 * a * t[1],
                -a * t[0] - c * t[2],    2 * c * t[1] - b * t[2],
                -c * t[0] + 2 * a * t[2], -c * t[1] + 2 * b * t[2],
                -a * t[0] - b * t[1]};
        return CMatrixDouble33(vals);
}

inline void dVinvt_dR(const CPose3D& P, CMatrixFixedNumeric<double, 3, 9>& J)
{
        CArrayDouble<3> a;
        CMatrixDouble33 B(UNINITIALIZED_MATRIX);

        const CMatrixDouble33& R = P.getRotationMatrix();
        const CArrayDouble<3>& t = P.m_coords;

        const double d = 0.5 * (R(0, 0) + R(1, 1) + R(2, 2) - 1);

        if (d > 0.9999)
        {
                a[0] = a[1] = a[2] = 0;
                B.zeros();
        }
        else
        {
                const double theta = acos(d);
                const double theta2 = square(theta);
                const double oned2 = (1 - square(d));
                const double sq = std::sqrt(oned2);
                const double cot = 1. / tan(0.5 * theta);
                const double csc2 = square(1. / sin(0.5 * theta));

                CMatrixDouble33 skewR(UNINITIALIZED_MATRIX);
                CArrayDouble<3> vr;
                deltaR(R, vr);
                mrpt::math::skew_symmetric3(vr, skewR);

                CArrayDouble<3> skewR_t;
                skewR.multiply_Ab(t, skewR_t);

                skewR_t *= -(d * theta - sq) / (8 * pow(sq, 3));
                a = skewR_t;

                CMatrixDouble33 skewR2(UNINITIALIZED_MATRIX);
                skewR2.multiply_AB(skewR, skewR);

                CArrayDouble<3> skewR2_t;
                skewR2.multiply_Ab(t, skewR2_t);
                skewR2_t *=
                        (((theta * sq - d * theta2) * (0.5 * theta * cot - 1)) -
                         theta * sq * ((0.25 * theta * cot) + 0.125 * theta2 * csc2 - 1)) /
                        (4 * theta2 * square(oned2));
                a += skewR2_t;

                mrpt::math::skew_symmetric3(t, B);
                B *= -0.5 * theta / (2 * sq);

                B += -(theta * cot - 2) / (8 * oned2) * ddeltaRt_dR(P);
        }
        M3x9(a, B, J);
}
}
}

void CPose3D::ln_jacob(mrpt::math::CMatrixFixedNumeric<double, 6, 12>& J) const
{
        J.zeros();
        // Jacobian structure 6x12:
        // (3rows, for t)       [       d_Vinvt_dR (3x9)    |  Vinv (3x3)  ]
        //                      [  -------------------------+------------- ]
        // (3rows, for \omega)  [       d_lnR_dR   (3x9)    |    0 (3x3)   ]
        //
        //          derivs wrt:     R_col1 R_col2  R_col3   |       t
        //
        // (Will be explained better in:
        // http://www.mrpt.org/6D_poses:equivalences_compositions_and_uncertainty )
        //
        {
                CMatrixFixedNumeric<double, 3, 9> M(UNINITIALIZED_MATRIX);
                ln_rot_jacob(m_ROT, M);
                J.insertMatrix(3, 0, M);
        }
        {
                CMatrixFixedNumeric<double, 3, 9> M(UNINITIALIZED_MATRIX);
                dVinvt_dR(*this, M);
                J.insertMatrix(0, 0, M);
        }

        const CMatrixDouble33& R = m_ROT;
        CArrayDouble<3> omega;
        CMatrixDouble33 Omega(UNINITIALIZED_MATRIX);

        CMatrixDouble33 V_inv(UNINITIALIZED_MATRIX);
        V_inv.unit(3, 1.0);  // Start with the identity_3

        const double d = 0.5 * (R(0, 0) + R(1, 1) + R(2, 2) - 1);
        if (d > 0.99999)
        {
                mrpt::poses::deltaR(R, omega);
                omega *= 0.5;
                mrpt::math::skew_symmetric3(omega, Omega);
                CMatrixDouble33 Omega2(UNINITIALIZED_MATRIX);
                Omega2.multiply_AAt(Omega);
                Omega2 *= 1.0 / 12.0;

                Omega *= 0.5;

                V_inv -= Omega;
                V_inv -= Omega2;
        }
        else
        {
                mrpt::poses::deltaR(R, omega);

                const double theta = acos(d);
                omega *= theta / (2 * std::sqrt(1 - d * d));

                mrpt::math::skew_symmetric3(omega, Omega);

                CMatrixDouble33 Omega2(UNINITIALIZED_MATRIX);
                Omega2.multiply_AAt(Omega);

                Omega2 *= (1 - theta / (2 * std::tan(theta * 0.5))) / square(theta);
                Omega *= 0.5;

                V_inv -= Omega;
                V_inv += Omega2;
        }
        J.insertMatrix(0, 9, V_inv);
}

void CPose3D::ln_rot_jacob(
        const CMatrixDouble33& R, CMatrixFixedNumeric<double, 3, 9>& M)
{
        const double d = 0.5 * (R(0, 0) + R(1, 1) + R(2, 2) - 1);
        CArrayDouble<3> a;
        CMatrixDouble33 B(UNINITIALIZED_MATRIX);
        if (d > 0.99999)
        {
                a[0] = a[1] = a[2] = 0;
                B.unit(3, -0.5);
        }
        else
        {
                const double theta = acos(d);
                const double d2 = square(d);
                const double sq = std::sqrt(1 - d2);
                deltaR(R, a);
                a *= (d * theta - sq) / (4 * (sq * sq * sq));
                B.unit(3, -theta / (2 * sq));
        }
        M3x9(a, B, M);
}

// Eq. 10.3.5 in tech report
// http://ingmec.ual.es/~jlblanco/papers/jlblanco2010geometry3D_techrep.pdf
void CPose3D::jacob_dexpeD_de(
        const CPose3D& D, Eigen::Matrix<double, 12, 6>& jacob)
{
        jacob.block<9, 3>(0, 0).setZero();
        jacob.block<3, 3>(9, 0).setIdentity();
        for (int i = 0; i < 3; i++)
        {
                Eigen::Block<Eigen::Matrix<double, 12, 6>, 3, 3, false> trg_blc =
                        jacob.block<3, 3>(3 * i, 3);
                mrpt::math::skew_symmetric3_neg(D.m_ROT.block<3, 1>(0, i), trg_blc);
        }
        {
                Eigen::Block<Eigen::Matrix<double, 12, 6>, 3, 3, false> trg_blc =
                        jacob.block<3, 3>(9, 3);
                mrpt::math::skew_symmetric3_neg(D.m_coords, trg_blc);
        }
}

// Eq. 10.3.7 in tech report
// http://ingmec.ual.es/~jlblanco/papers/jlblanco2010geometry3D_techrep.pdf
void CPose3D::jacob_dAexpeD_de(
        const CPose3D& A, const CPose3D& D, Eigen::Matrix<double, 12, 6>& jacob)
{
        jacob.block<9, 3>(0, 0).setZero();
        jacob.block<3, 3>(9, 0) = A.getRotationMatrix();
        Eigen::Matrix<double, 3, 3> aux;
        for (int i = 0; i < 3; i++)
        {
                mrpt::math::skew_symmetric3_neg(
                        D.getRotationMatrix().block<3, 1>(0, i), aux);
                jacob.block<3, 3>(3 * i, 3) = A.m_ROT * aux;
        }
        mrpt::math::skew_symmetric3_neg(D.m_coords, aux);
        jacob.block<3, 3>(9, 3) = A.m_ROT * aux;
}

void CPose3D::setToNaN()
{
        for (int i = 0; i < 3; i++)
                for (int j = 0; j < 3; j++)
                        m_ROT(i, j) = std::numeric_limits<double>::quiet_NaN();

        for (int i = 0; i < 3; i++)
                m_coords[i] = std::numeric_limits<double>::quiet_NaN();
}