CPose3DRotVec.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/poses/CPose3D.h>
#include <mrpt/poses/CPoint2D.h>
#include <mrpt/poses/CPose3DQuat.h>
#include <mrpt/poses/CPose3DRotVec.h>
#include <mrpt/poses/CPoint3D.h>
#include <mrpt/math/geometry.h>  // skew_symmetric3()
#include <mrpt/utils/CStream.h>
#include <iomanip>
#include <limits>

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

IMPLEMENTS_SERIALIZABLE(CPose3DRotVec, CSerializable, mrpt::poses)

MRPT_TODO("Complete missing methods")

/*---------------------------------------------------------------
        Constructors
  ---------------------------------------------------------------*/
CPose3DRotVec::CPose3DRotVec(const math::CMatrixDouble44& m)
{
        m_coords[0] = m(0, 3);
        m_coords[1] = m(1, 3);
        m_coords[2] = m(2, 3);

        m_rotvec = rotVecFromRotMat(m);
}

CPose3DRotVec::CPose3DRotVec(const CPose3D& m)
{
        m_coords[0] = m.x();
        m_coords[1] = m.y();
        m_coords[2] = m.z();
        CMatrixDouble44 R;
        m.getHomogeneousMatrix(R);
        m_rotvec = rotVecFromRotMat(R);
}

/** Constructor from a quaternion (which only represents the 3D rotation part)
 * and a 3D displacement. */
CPose3DRotVec::CPose3DRotVec(
        const mrpt::math::CQuaternionDouble& q, const double _x, const double _y,
        const double _z)
{
        m_coords[0] = _x;
        m_coords[1] = _y;
        m_coords[2] = _z;
        const double a = sqrt(q.x() * q.x() + q.y() * q.y() + q.z() * q.z());
        const double TH = 0.001;
        const double k = a < TH ? 2 : 2 * acos(q.r()) / sqrt(1 - q.r() * q.r());
        m_rotvec[0] = k * q.x();
        m_rotvec[1] = k * q.y();
        m_rotvec[2] = k * q.z();
}

/*---------------------------------------------------------------
   Implements the writing to a CStream capability of
         CSerializable objects
  ---------------------------------------------------------------*/
void CPose3DRotVec::writeToStream(mrpt::utils::CStream& out, int* version) const
{
        if (version)
                *version = 0;
        else
        {
                out << m_coords[0] << m_coords[1] << m_coords[2] << m_rotvec[0]
                        << m_rotvec[1] << m_rotvec[2];
        }
}

/*---------------------------------------------------------------
        Implements the reading from a CStream capability of
                CSerializable objects
  ---------------------------------------------------------------*/
void CPose3DRotVec::readFromStream(mrpt::utils::CStream& in, int version)
{
        switch (version)
        {
                case 0:
                {
                        in >> m_coords[0] >> m_coords[1] >> m_coords[2] >> m_rotvec[0] >>
                                m_rotvec[1] >> m_rotvec[2];
                }
                break;
                default:
                        MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version)
        };
}

void CPose3DRotVec::setFromXYZAndAngles(
        const double x, const double y, const double z, const double yaw,
        const double pitch, const double roll)
{
        CPose3D aux(x, y, z, yaw, pitch, roll);
        this->m_coords[0] = aux.m_coords[0];
        this->m_coords[1] = aux.m_coords[1];
        this->m_coords[2] = aux.m_coords[2];
        this->m_rotvec = aux.ln_rotation();
}

CArrayDouble<3> CPose3DRotVec::rotVecFromRotMat(const math::CMatrixDouble44& m)
{
        // go through cpose3d
        CArrayDouble<3> out;
        CPose3D aux(m);
        out = aux.ln_rotation();
        return out;
}  // end-rotVecFromRotMat

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

/** Get the 3x3 rotation matrix \sa getHomogeneousMatrix  */
void CPose3DRotVec::getRotationMatrix(mrpt::math::CMatrixDouble33& ROT) const
{
        // through cpose3D
        ROT = CPose3D().exp_rotation(this->m_rotvec);
        //    cout << "CPOSE_3D: " << ROT;
}

/*---------------------------------------------------------------
                sphericalCoordinates
---------------------------------------------------------------*/
void CPose3DRotVec::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;
}

/*---------------------------------------------------------------
                composePoint
---------------------------------------------------------------*/
void CPose3DRotVec::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) const
{
        const double angle = this->m_rotvec.norm();
        const double K1 = sin(angle) / angle;
        const double K2 = (1 - cos(angle)) / (angle * angle);

        const double tx = this->m_coords[0];
        const double ty = this->m_coords[1];
        const double tz = this->m_coords[2];

        const double w1 = this->m_rotvec[0];
        const double w2 = this->m_rotvec[1];
        const double w3 = this->m_rotvec[2];

        const double w1_2 = w1 * w1;
        const double w2_2 = w2 * w2;
        const double w3_2 = w3 * w3;

        gx = lx * (1 - K2 * (w2_2 + w3_2)) + ly * (K2 * w1 * w2 - K1 * w3) +
                 lz * (K1 * w2 + K2 * w1 * w3) + tx;
        gy = lx * (K1 * w3 + K2 * w1 * w2) + ly * (1 - K2 * (w1_2 + w3_2)) +
                 lz * (K2 * w2 * w3 - K1 * w1) + ty;
        gz = lx * (K2 * w1 * w3 - K1 * w2) + ly * (K1 * w1 + K2 * w2 * w3) +
                 lz * (1 - K2 * (w1_2 + w2_2)) + tz;

        if (out_jacobian_df_dpoint || out_jacobian_df_dpose)
        {
                MRPT_TODO("Jacobians")
                THROW_EXCEPTION("Jacobians not implemented yet")
        }
}

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

bool mrpt::poses::operator==(const CPose3DRotVec& p1, const CPose3DRotVec& p2)
{
        return (p1.m_coords == p2.m_coords) && (p1.m_rotvec == p2.m_rotvec);
}

bool mrpt::poses::operator!=(const CPose3DRotVec& p1, const CPose3DRotVec& p2)
{
        return (p1.m_coords != p2.m_coords) || (p1.m_rotvec != p2.m_rotvec);
}

/*---------------------------------------------------------------
                                point3D = pose3D + point3D
  ---------------------------------------------------------------*/
CPoint3D CPose3DRotVec::operator+(const CPoint3D& b) const
{
        CPoint3D outPoint;

        this->composePoint(
                b.m_coords[0], b.m_coords[1], b.m_coords[2], outPoint.m_coords[0],
                outPoint.m_coords[1], outPoint.m_coords[2]);

        return outPoint;
}

/*---------------------------------------------------------------
                                point3D = pose3D + point2D
  ---------------------------------------------------------------*/
CPoint3D CPose3DRotVec::operator+(const CPoint2D& b) const
{
        CPoint3D outPoint;

        this->composePoint(
                b.m_coords[0], b.m_coords[1], 0, outPoint.m_coords[0],
                outPoint.m_coords[1], outPoint.m_coords[2]);

        return outPoint;
}

void CPose3DRotVec::toQuatXYZ(CPose3DQuat& q) const
{
        q.m_coords[0] = this->m_coords[0];
        q.m_coords[1] = this->m_coords[1];
        q.m_coords[2] = this->m_coords[2];

        const double a = sqrt(
                this->m_rotvec[0] * this->m_rotvec[0] +
                this->m_rotvec[1] * this->m_rotvec[1] +
                this->m_rotvec[2] * this->m_rotvec[2]);
        if (a < 0.001)
        {
                q.m_quat.r(1);
                q.m_quat.x(0.5 * this->m_rotvec[0]);
                q.m_quat.y(0.5 * this->m_rotvec[1]);
                q.m_quat.z(0.5 * this->m_rotvec[2]);
                // TO DO: output of the jacobian
                // df_dr   = 0.25*[-r';2*eye(3)];
        }
        else
        {
                q.m_quat.fromRodriguesVector(this->m_rotvec);

                // TO DO: output of the jacobian
                //        a2      = a*a;
                //        a3      = a2*a;
                //
                //        r1      = r(1);
                //        r2      = r(2);
                //        r3      = r(3);
                //        s       = sin(a/2);
                //        c       = cos(a/2);
                //
                //        A       = a*c-2*s;
                //
                //        df_dr   = 1/a3*[-r1*a2*s -r2*a2*s -r3*a2*s; ...
                //            2*a2*s+r1*r1*A r1*r2*A r1*r3*A; ...
                //            r1*r2*A 2*a2*s+r2*r2*A r2*r3*A; ...
                //            r1*r3*A r2*r3*A 2*a2*s+r3*r3*A];        % jacobian of
                //            transf.
                //
        }
}

/*---------------------------------------------------------------
                                this = A + B
  ---------------------------------------------------------------*/
void CPose3DRotVec::composeFrom(
        const CPose3DRotVec& A, const CPose3DRotVec& B,
        mrpt::math::CMatrixFixedNumeric<double, 6, 6>* out_jacobian_drvtC_drvtA,
        mrpt::math::CMatrixFixedNumeric<double, 6, 6>* out_jacobian_drvtC_drvtB)

{
        const double a1 = A.m_rotvec.norm();  // angles
        const double a2 = B.m_rotvec.norm();

        // if the angles are small, we can compute the approximate composition
        // easily
        if (a1 < 0.01 && a2 < 0.01)
        {
                const CMatrixDouble33 Ra = Eigen::MatrixXd::Identity(3, 3) +
                                                                   mrpt::math::skew_symmetric3(A.m_rotvec);

                this->m_rotvec = A.m_rotvec + B.m_rotvec;
                this->m_coords = A.m_coords + Ra * B.m_coords;

                if (out_jacobian_drvtC_drvtA || out_jacobian_drvtC_drvtB)
                {
                        if (out_jacobian_drvtC_drvtA)  // jacobian wrt A
                        {
                                out_jacobian_drvtC_drvtA->setIdentity(6, 6);
                                out_jacobian_drvtC_drvtA->insertMatrix(
                                        3, 0, mrpt::math::skew_symmetric3_neg(B.m_coords));
                        }

                        if (out_jacobian_drvtC_drvtB)  // jacobian wrt B
                        {
                                out_jacobian_drvtC_drvtB->setIdentity(6, 6);
                                out_jacobian_drvtC_drvtB->insertMatrix(
                                        3, 3, mrpt::math::skew_symmetric3(A.m_rotvec));
                                (*out_jacobian_drvtC_drvtB)(3, 3) = (*out_jacobian_drvtC_drvtB)(
                                        4, 4) = (*out_jacobian_drvtC_drvtB)(5, 5) = 1;
                        }
                }
                return;
        }

        CMatrixDouble33 RA, RB;
        A.getRotationMatrix(RA);
        B.getRotationMatrix(RB);

        // Translation part
        CArrayDouble<3> coords = RA * B.m_coords + A.m_coords;
        this->m_coords[0] = coords[0];
        this->m_coords[1] = coords[1];
        this->m_coords[2] = coords[2];

// Rotation part:
#if 0
    else if (A_is_small)            this->m_rotvec = B.m_rotvec;
    else if (B_is_small)            this->m_rotvec = A.m_rotvec;
    else
    {
        const double a1_inv = 1/a1;
        const double a2_inv = 1/a2;

        const double sin_a1_2 = sin(0.5*a1);
        const double cos_a1_2 = cos(0.5*a1);
        const double sin_a2_2 = sin(0.5*a2);
        const double cos_a2_2 = cos(0.5*a2);

        const double KA = sin_a1_2*sin_a2_2;
        const double KB = sin_a1_2*cos_a2_2;
        const double KC = cos_a1_2*sin_a2_2;
        const double KD = cos_a1_2*cos_a2_2;

        const double r11 = a1_inv*A.m_rotvec[0];
        const double r12 = a1_inv*A.m_rotvec[1];
        const double r13 = a1_inv*A.m_rotvec[2];

        const double r21 = a2_inv*B.m_rotvec[0];
        const double r22 = a2_inv*B.m_rotvec[1];
        const double r23 = a2_inv*B.m_rotvec[2];

        const double q3[] = {
            KD - KA*(r11*r21+r12*r22+r13*r23),
            KC*r21 + KB*r11 + KA*(r22*r13-r23*r12),
            KC*r22 + KB*r12 + KA*(r23*r11-r21*r13),
            KC*r23 + KB*r13 + KA*(r21*r12-r22*r11)
        };

        const double param = 2*acos(q3[0])/sqrt(1-q3[0]*q3[0]);
        this->m_rotvec[0] = param*q3[1];
        this->m_rotvec[1] = param*q3[2];
        this->m_rotvec[2] = param*q3[3];
    }
#endif

        /* */

        // Rotation part
        CPose3D aux(UNINITIALIZED_POSE);
        aux.setRotationMatrix(RA * RB);
        this->m_rotvec = aux.ln_rotation();

        //    cout << "WO Approx: " << *this << endl;

        /* */

        // Output Jacobians (if desired)
        if (out_jacobian_drvtC_drvtA || out_jacobian_drvtC_drvtB)
        {
                CPose3DQuat qA, qB, qC;
                this->toQuatXYZ(qC);

                const double& qCr = qC.m_quat[0];
                const double& qCx = qC.m_quat[1];
                const double& qCy = qC.m_quat[2];
                const double& qCz = qC.m_quat[3];

                const double& r1 = this->m_rotvec[0];
                const double& r2 = this->m_rotvec[1];
                const double& r3 = this->m_rotvec[2];

                const double C = 1 / (1 - qCr * qCr);
                const double D = acos(qCr) / sqrt(1 - qCr * qCr);

                alignas(16) const double aux_valsH[] = {
                        2 * C * qCx * (D * qCr - 1), 2 * D, 0,   0,
                        2 * C * qCy * (D * qCr - 1), 0,         2 * D, 0,
                        2 * C * qCz * (D * qCr - 1), 0,         0,       2 * D};

                CMatrixFixedNumeric<double, 3, 4> H(aux_valsH);

                const double alpha = sqrt(r1 * r1 + r2 * r2 + r3 * r3);
                const double alpha2 = alpha * alpha;
                const double KA = alpha * cos(alpha / 2) - 2 * sin(alpha / 2);

                alignas(16) const double aux_valsG[] = {
                        -r1 * alpha2 * sin(alpha / 2),
                        -r2 * alpha2 * sin(alpha / 2),
                        -r3 * alpha2 * sin(alpha / 2),
                        2 * alpha2 * sin(alpha / 2) + r1 * r1 * KA,
                        r1 * r2 * KA,
                        r1 * r3 * KA,
                        r1 * r2 * KA,
                        2 * alpha2 * sin(alpha / 2) + r2 * r2 * KA,
                        r2 * r3 * KA,
                        r1 * r3 * KA,
                        r2 * r3 * KA,
                        2 * alpha2 * sin(alpha / 2) + r3 * r3 * KA};

                CMatrixFixedNumeric<double, 4, 3> G(aux_valsG);

                if (out_jacobian_drvtC_drvtB)
                {
                        A.toQuatXYZ(qA);

                        const double& qAr = qA.m_quat[0];
                        const double& qAx = qA.m_quat[1];
                        const double& qAy = qA.m_quat[2];
                        const double& qAz = qA.m_quat[3];

                        alignas(16) const double aux_valsQA[] = {
                                qAr, -qAx, -qAy, -qAz, qAx, qAr, qAz,  -qAy,
                                qAy, -qAz, qAr,  qAx,  qAz, qAy, -qAx, qAr};
                        CMatrixDouble44 QA(aux_valsQA);

                        CMatrixDouble33 jac_rot_B;
                        jac_rot_B.multiply_ABC(H, QA, G);

                        out_jacobian_drvtC_drvtB->fill(0);
                        out_jacobian_drvtC_drvtB->insertMatrix(0, 0, jac_rot_B);
                        out_jacobian_drvtC_drvtB->insertMatrix(3, 3, RA);
                }
                if (out_jacobian_drvtC_drvtA)
                {
                        B.toQuatXYZ(qB);

                        const double& qBr = qB.m_quat[0];
                        const double& qBx = qB.m_quat[1];
                        const double& qBy = qB.m_quat[2];
                        const double& qBz = qB.m_quat[3];

                        alignas(16) const double aux_valsQB[] = {
                                qBr, -qBx, -qBy, -qBz, qBx, qBr,  -qBz, qBy,
                                qBy, qBz,  qBr,  -qBx, qBz, -qBy, qBx,  qBr};
                        CMatrixDouble44 QB(aux_valsQB);

                        CMatrixDouble33 jac_rot_A, id3;
                        jac_rot_A.multiply_ABC(H, QB, G);

                        id3.eye();
                        out_jacobian_drvtC_drvtA->fill(0);
                        out_jacobian_drvtC_drvtA->insertMatrix(0, 0, jac_rot_A);
                        out_jacobian_drvtC_drvtB->insertMatrix(3, 3, id3);
                }
        }
}  // end composeFrom

/** Convert this pose into its inverse, saving the result in itself. */
void CPose3DRotVec::inverse()
{
        CMatrixDouble44 B_INV(UNINITIALIZED_MATRIX);
        this->getInverseHomogeneousMatrix(B_INV);
        this->setFromTransformationMatrix(B_INV);
}

/** Compute the inverse of this pose and return the result. */
CPose3DRotVec CPose3DRotVec::getInverse() const
{
        CPose3DRotVec inv_rvt(UNINITIALIZED_POSE);
        CMatrixDouble44 B_INV(UNINITIALIZED_MATRIX);
        this->getInverseHomogeneousMatrix(B_INV);
        inv_rvt.setFromTransformationMatrix(B_INV);

        return inv_rvt;
}

/**  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 CPose3DRotVec::inverseComposeFrom(
        const CPose3DRotVec& A, const CPose3DRotVec& 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   ]
        //
        CMatrixDouble44 HM_A(UNINITIALIZED_MATRIX), HM_B_inv(UNINITIALIZED_MATRIX),
                HM_C(UNINITIALIZED_MATRIX);

        A.getHomogeneousMatrix(HM_A);
        B.getInverseHomogeneousMatrix(HM_B_inv);

        HM_C.multiply_AB(HM_B_inv, HM_A);

        this->m_rotvec = this->rotVecFromRotMat(HM_C);

        this->m_coords[0] = HM_C(0, 3);
        this->m_coords[1] = HM_C(1, 3);
        this->m_coords[2] = HM_C(2, 3);
}

/**  Computes the 3D point L such as \f$ L = G \ominus this \f$.
  * \sa composePoint, composeFrom
  */
void CPose3DRotVec::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) const
{
        MRPT_UNUSED_PARAM(out_jacobian_df_dpoint);
        MRPT_UNUSED_PARAM(out_jacobian_df_dpose);
        CPose3DRotVec rvt = this->getInverse();
        rvt.composePoint(gx, gy, gz, lx, ly, lz);
        MRPT_TODO("Jacobians");
}

/** Exponentiate a Vector in the SE3 Lie Algebra to generate a new
 * CPose3DRotVec.
  */
CPose3DRotVec CPose3DRotVec::exp(const mrpt::math::CArrayDouble<6>& mu)
{
        return CPose3DRotVec(mu);
}

/** Take the logarithm of the 3x3 rotation matrix, generating the corresponding
 * vector in the Lie Algebra.
  */
CArrayDouble<3> CPose3DRotVec::ln_rotation() const { return m_rotvec; }
/** Take the logarithm of the 3x4 matrix defined by this pose, generating the
 * corresponding vector in the SE3 Lie Algebra.
  */
void CPose3DRotVec::ln(CArrayDouble<6>& result) const
{
        result.head<3>() = m_coords;
        result.tail<3>() = m_rotvec;
}

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