CPose3DRotVec.cpp

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 |
   +------------------------------------------------------------------------+ */

#include "poses-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/serialization/CArchive.h>
#include <iomanip>
#include <limits>

using namespace mrpt;
using namespace mrpt::math;

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();
}

uint8_t CPose3DRotVec::serializeGetVersion() const { return 0; }
void CPose3DRotVec::serializeTo(mrpt::serialization::CArchive& out) const
{
    out << m_coords[0] << m_coords[1] << m_coords[2] << m_rotvec[0]
        << m_rotvec[1] << m_rotvec[2];
}
void CPose3DRotVec::serializeFrom(
    mrpt::serialization::CArchive& in, uint8_t 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
    const auto coords = RA * B.m_coords + A.m_coords;
    m_coords = coords;

// 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(MRPT_MAX_ALIGN_BYTES) 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(MRPT_MAX_ALIGN_BYTES) 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(MRPT_MAX_ALIGN_BYTES) 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(MRPT_MAX_ALIGN_BYTES) 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();
    }
}