CPose3DQuat.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

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

#include <mrpt/poses/CPose3D.h>
#include <mrpt/poses/CPose3DQuat.h>
#include <mrpt/serialization/CArchive.h>
#include <mrpt/serialization/CSchemeArchiveBase.h>
#include <mrpt/serialization/CSerializable.h>
#include <Eigen/Dense>
#include <iomanip>
#include <limits>

using namespace std;
using namespace mrpt;
using namespace mrpt::math;
using namespace mrpt::poses;

IMPLEMENTS_SERIALIZABLE(CPose3DQuat, CSerializable, mrpt::poses)

/** Constructor from a CPose3D */
CPose3DQuat::CPose3DQuat(const CPose3D& p)
{
    x() = p.x();
    y() = p.y();
    z() = p.z();
    p.getAsQuaternion(m_quat);
}

/** Constructor from a 4x4 homogeneous transformation matrix.
 */
CPose3DQuat::CPose3DQuat(const CMatrixDouble44& M)
    : m_quat(UNINITIALIZED_QUATERNION)
{
    m_coords[0] = M(0, 3);
    m_coords[1] = M(1, 3);
    m_coords[2] = M(2, 3);
    CPose3D p(M);
    p.getAsQuaternion(m_quat);
}

/** Returns the corresponding 4x4 homogeneous transformation matrix for the
 * point(translation) or pose (translation+orientation).
 * \sa getInverseHomogeneousMatrix
 */
void CPose3DQuat::getHomogeneousMatrix(CMatrixDouble44& out_HM) const
{
    m_quat.rotationMatrixNoResize(out_HM);
    out_HM(0, 3) = m_coords[0];
    out_HM(1, 3) = m_coords[1];
    out_HM(2, 3) = m_coords[2];
    out_HM(3, 0) = out_HM(3, 1) = out_HM(3, 2) = 0;
    out_HM(3, 3) = 1;
}

/** Returns a 1x7 vector with [x y z qr qx qy qz] */
void CPose3DQuat::asVector(vector_t& v) const
{
    v[0] = m_coords[0];
    v[1] = m_coords[1];
    v[2] = m_coords[2];
    v[3] = m_quat[0];
    v[4] = m_quat[1];
    v[5] = m_quat[2];
    v[6] = m_quat[3];
}

/**  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 CPose3DQuat::composeFrom(const CPose3DQuat& A, const CPose3DQuat& B)
{
    // The 3D point:
    double gx, gy, gz;
    A.m_quat.rotatePoint(
        B.m_coords[0], B.m_coords[1], B.m_coords[2], gx, gy, gz);
    this->m_coords[0] = A.m_coords[0] + gx;
    this->m_coords[1] = A.m_coords[1] + gy;
    this->m_coords[2] = A.m_coords[2] + gz;

    // The 3D rotation:
    this->m_quat.crossProduct(A.m_quat, B.m_quat);
}

/**  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
 */
void CPose3DQuat::inverseComposeFrom(const CPose3DQuat& A, const CPose3DQuat& B)
{
    // The 3D point:
    const CQuaternionDouble B_conj(
        B.m_quat.r(), -B.m_quat.x(), -B.m_quat.y(), -B.m_quat.z());
    B_conj.rotatePoint(
        A.m_coords[0] - B.m_coords[0], A.m_coords[1] - B.m_coords[1],
        A.m_coords[2] - B.m_coords[2], this->m_coords[0], this->m_coords[1],
        this->m_coords[2]);
    // The 3D rotation:
    this->m_quat.crossProduct(B_conj, A.m_quat);
}

/**  Computes the 3D point G such as \f$ G = this \oplus L \f$.
 * \sa inverseComposeFrom
 */
void CPose3DQuat::composePoint(
    const double lx, const double ly, const double lz, double& gx, double& gy,
    double& gz, mrpt::math::CMatrixFixed<double, 3, 3>* out_jacobian_df_dpoint,
    mrpt::math::CMatrixFixed<double, 3, 7>* out_jacobian_df_dpose) const
{
    if (out_jacobian_df_dpoint || out_jacobian_df_dpose)
    {
        const double qx2 = square(m_quat.x());
        const double qy2 = square(m_quat.y());
        const double qz2 = square(m_quat.z());

        // Jacob: df/dpoint
        if (out_jacobian_df_dpoint)
        {
            // 3x3:  df_{qr} / da

            alignas(MRPT_MAX_STATIC_ALIGN_BYTES) const double vals[3 * 3] = {
                1 - 2 * (qy2 + qz2),
                2 * (m_quat.x() * m_quat.y() - m_quat.r() * m_quat.z()),
                2 * (m_quat.r() * m_quat.y() + m_quat.x() * m_quat.z()),

                2 * (m_quat.r() * m_quat.z() + m_quat.x() * m_quat.y()),
                1 - 2 * (qx2 + qz2),
                2 * (m_quat.y() * m_quat.z() - m_quat.r() * m_quat.x()),

                2 * (m_quat.x() * m_quat.z() - m_quat.r() * m_quat.y()),
                2 * (m_quat.r() * m_quat.x() + m_quat.y() * m_quat.z()),
                1 - 2 * (qx2 + qy2)};
            out_jacobian_df_dpoint->loadFromArray(vals);
        }

        // Jacob: df/dpose
        if (out_jacobian_df_dpose)
        {
            // 3x7:  df_{qr} / dp
            alignas(MRPT_MAX_STATIC_ALIGN_BYTES) const double vals1[3 * 7] = {
                1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0};
            out_jacobian_df_dpose->loadFromArray(vals1);

            alignas(MRPT_MAX_STATIC_ALIGN_BYTES) const double vals[3 * 4] = {
                2 * (-m_quat.z() * ly + m_quat.y() * lz),
                2 * (m_quat.y() * ly + m_quat.z() * lz),
                2 * (-2 * m_quat.y() * lx + m_quat.x() * ly + m_quat.r() * lz),
                2 * (-2 * m_quat.z() * lx - m_quat.r() * ly + m_quat.x() * lz),

                2 * (m_quat.z() * lx - m_quat.x() * lz),
                2 * (m_quat.y() * lx - 2 * m_quat.x() * ly - m_quat.r() * lz),
                2 * (m_quat.x() * lx + m_quat.z() * lz),
                2 * (m_quat.r() * lx - 2 * m_quat.z() * ly + m_quat.y() * lz),

                2 * (-m_quat.y() * lx + m_quat.x() * ly),
                2 * (m_quat.z() * lx + m_quat.r() * ly - 2 * m_quat.x() * lz),
                2 * (-m_quat.r() * lx + m_quat.z() * ly - 2 * m_quat.y() * lz),
                2 * (m_quat.x() * lx + m_quat.y() * ly)};

            CMatrixDouble44 norm_jacob(UNINITIALIZED_MATRIX);
            this->quat().normalizationJacobian(norm_jacob);

            out_jacobian_df_dpose->asEigen().block<3, 4>(0, 3) =
                (CMatrixFixed<double, 3, 4>(vals) * norm_jacob).eval();
        }
    }

    // function itself:
    m_quat.rotatePoint(lx, ly, lz, gx, gy, gz);
    gx += m_coords[0];
    gy += m_coords[1];
    gz += m_coords[2];
}

/**  Computes the 3D point G such as \f$ L = G \ominus this \f$.
 * \sa composeFrom
 */
void CPose3DQuat::inverseComposePoint(
    const double gx, const double gy, const double gz, double& lx, double& ly,
    double& lz, mrpt::math::CMatrixFixed<double, 3, 3>* out_jacobian_df_dpoint,
    mrpt::math::CMatrixFixed<double, 3, 7>* out_jacobian_df_dpose) const
{
    if (out_jacobian_df_dpoint || out_jacobian_df_dpose)
    {
        const double qx2 = square(m_quat.x());
        const double qy2 = square(m_quat.y());
        const double qz2 = square(m_quat.z());

        // Jacob: df/dpoint
        if (out_jacobian_df_dpoint)
        {
            // 3x3:  df_{m_quat.r()} / da
            //      inv_df_da =
            //      [ - 2*qy^2 - 2*qz^2 + 1,     2*qx*qy - 2*qr*qz,     2*qr*qy
            //+
            // 2*qx*qz]
            //      [     2*qr*qz + 2*qx*qy, - 2*qx^2 - 2*qz^2 + 1,     2*qy*qz
            //-
            // 2*qr*qx]
            //      [     2*qx*qz - 2*qr*qy,     2*qr*qx + 2*qy*qz, - 2*qx^2 -
            // 2*qy^2 + 1]
            //

            alignas(MRPT_MAX_STATIC_ALIGN_BYTES) const double vals[3 * 3] = {
                1 - 2 * (qy2 + qz2),
                2 * (m_quat.x() * m_quat.y() + m_quat.r() * m_quat.z()),
                2 * (-m_quat.r() * m_quat.y() + m_quat.x() * m_quat.z()),

                2 * (-m_quat.r() * m_quat.z() + m_quat.x() * m_quat.y()),
                1 - 2 * (qx2 + qz2),
                2 * (m_quat.y() * m_quat.z() + m_quat.r() * m_quat.x()),

                2 * (m_quat.x() * m_quat.z() + m_quat.r() * m_quat.y()),
                2 * (-m_quat.r() * m_quat.x() + m_quat.y() * m_quat.z()),
                1 - 2 * (qx2 + qy2)};
            out_jacobian_df_dpoint->loadFromArray(vals);
        }

        // Jacob: df/dpose
        if (out_jacobian_df_dpose)
        {
            // 3x7:  df_{m_quat.r()} / dp
            //      inv_df_dp =
            //[ 2*qy^2 + 2*qz^2 - 1, - 2*qr*qz - 2*qx*qy,   2*qr*qy - 2*qx*qz,
            // 2*qz*(ay - y) - 2*qy*(az - z),                 2*qy*(ay - y) +
            // 2*qz*(az - z), 2*qx*(ay - y) - 4*qy*(ax - x) - 2*qr*(az - z),
            // 2*qr*(ay - y) - 4*qz*(ax - x) + 2*qx*(az - z)]
            //[   2*qr*qz - 2*qx*qy, 2*qx^2 + 2*qz^2 - 1, - 2*qr*qx - 2*qy*qz,
            // 2*qx*(az - z) - 2*qz*(ax - x), 2*qy*(ax - x) - 4*qx*(ay - y) +
            // 2*qr*(az - z),                 2*qx*(ax - x) + 2*qz*(az - z),
            // 2*qy*(az - z) - 4*qz*(ay - y) - 2*qr*(ax - x)]
            //[ - 2*qr*qy - 2*qx*qz,   2*qr*qx - 2*qy*qz, 2*qx^2 + 2*qy^2 - 1,
            // 2*qy*(ax - x) - 2*qx*(ay - y), 2*qz*(ax - x) - 2*qr*(ay - y) -
            // 4*qx*(az - z), 2*qr*(ax - x) + 2*qz*(ay - y) - 4*qy*(az - z),
            // 2*qx*(ax - x) + 2*qy*(ay - y)]
            //
            const double qr = m_quat.r();
            const double qx = m_quat.x();
            const double qy = m_quat.y();
            const double qz = m_quat.z();

            alignas(MRPT_MAX_STATIC_ALIGN_BYTES) const double vals1[3 * 7] = {
                2 * qy2 + 2 * qz2 - 1,
                -2 * qr * qz - 2 * qx * qy,
                2 * qr * qy - 2 * qx * qz,
                0,
                0,
                0,
                0,

                2 * qr * qz - 2 * qx * qy,
                2 * qx2 + 2 * qz2 - 1,
                -2 * qr * qx - 2 * qy * qz,
                0,
                0,
                0,
                0,

                -2 * qr * qy - 2 * qx * qz,
                2 * qr * qx - 2 * qy * qz,
                2 * qx2 + 2 * qy2 - 1,
                0,
                0,
                0,
                0,
            };

            out_jacobian_df_dpose->loadFromArray(vals1);

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

            alignas(MRPT_MAX_STATIC_ALIGN_BYTES)
                const double vals[3 * 4] = {-qy * Az + qz * Ay,
                                            qy * Ay + qz * Az,
                                            qx * Ay - 2 * qy * Ax - qr * Az,
                                            qx * Az + qr * Ay - 2 * qz * Ax,

                                            qx * Az - qz * Ax,
                                            qy * Ax - 2 * qx * Ay + qr * Az,
                                            qx * Ax + qz * Az,
                                            qy * Az - 2 * qz * Ay - qr * Ax,

                                            qy * Ax - qx * Ay,
                                            qz * Ax - qr * Ay - 2 * qx * Az,
                                            qr * Ax + qz * Ay - 2 * qy * Az,
                                            qx * Ax + qy * Ay};

            CMatrixDouble44 norm_jacob(UNINITIALIZED_MATRIX);
            this->quat().normalizationJacobian(norm_jacob);

            out_jacobian_df_dpose->insertMatrix(
                0, 3, (CMatrixFixed<double, 3, 4>(vals) * norm_jacob).eval());
        }
    }

    // function itself:
    m_quat.inverseRotatePoint(
        gx - m_coords[0], gy - m_coords[1], gz - m_coords[2], lx, ly, lz);
}

/*---------------------------------------------------------------
    *=
  ---------------------------------------------------------------*/
void CPose3DQuat::operator*=(const double s)
{
    m_coords[0] *= s;
    m_coords[1] *= s;
    m_coords[2] *= s;
    m_quat[0] *= s;
    m_quat[1] *= s;
    m_quat[2] *= s;
    m_quat[3] *= s;
}

uint8_t CPose3DQuat::serializeGetVersion() const { return 0; }
void CPose3DQuat::serializeTo(mrpt::serialization::CArchive& out) const
{
    out << m_coords[0] << m_coords[1] << m_coords[2] << m_quat[0] << m_quat[1]
        << m_quat[2] << m_quat[3];
}
void CPose3DQuat::serializeFrom(
    mrpt::serialization::CArchive& in, uint8_t version)
{
    switch (version)
    {
        case 0:
        {
            in >> m_coords[0] >> m_coords[1] >> m_coords[2] >> m_quat[0] >>
                m_quat[1] >> m_quat[2] >> m_quat[3];
        }
        break;
        default:
            MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
    };
}
/** Serialize CSerializable Object to CSchemeArchiveBase derived object*/
void CPose3DQuat::serializeTo(
    mrpt::serialization::CSchemeArchiveBase& out) const
{
    SCHEMA_SERIALIZE_DATATYPE_VERSION(1);
    CPoint3D point(m_coords[0], m_coords[1], m_coords[2]);
    out["point"] = point;
    out["orientation"]["r"] = m_quat[0];
    out["orientation"]["x"] = m_quat[1];
    out["orientation"]["y"] = m_quat[2];
    out["orientation"]["z"] = m_quat[3];
}
/** Serialize CSchemeArchiveBase derived object to CSerializable Object*/
void CPose3DQuat::serializeFrom(mrpt::serialization::CSchemeArchiveBase& in)
{
    uint8_t version;
    SCHEMA_DESERIALIZE_DATATYPE_VERSION();
    switch (version)
    {
        case 1:
        {
            CPoint3D point;
            in["point"].readTo(point);
            m_coords[0] = point.x();
            m_coords[1] = point.y();
            m_coords[2] = point.z();
            m_quat[0] = static_cast<double>(in["orientation"]["r"]);
            m_quat[1] = static_cast<double>(in["orientation"]["x"]);
            m_quat[2] = static_cast<double>(in["orientation"]["y"]);
            m_quat[3] = static_cast<double>(in["orientation"]["z"]);
        }
        break;
        default:
            MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
    }
}
/*---------------------------------------------------------------
        sphericalCoordinates
---------------------------------------------------------------*/
void CPose3DQuat::sphericalCoordinates(
    const TPoint3D& point, double& out_range, double& out_yaw,
    double& out_pitch,
    mrpt::math::CMatrixFixed<double, 3, 3>* out_jacob_dryp_dpoint,
    mrpt::math::CMatrixFixed<double, 3, 7>* out_jacob_dryp_dpose) const
{
    const bool comp_jacobs =
        out_jacob_dryp_dpoint != nullptr || out_jacob_dryp_dpose != nullptr;

    // Pass to coordinates as seen from this 6D pose:
    CMatrixFixed<double, 3, 3> jacob_dinv_dpoint,
        *ptr_ja1 = comp_jacobs ? &jacob_dinv_dpoint : nullptr;
    CMatrixFixed<double, 3, 7> jacob_dinv_dpose,
        *ptr_ja2 = comp_jacobs ? &jacob_dinv_dpose : nullptr;

    TPoint3D local;
    this->inverseComposePoint(
        point.x, point.y, point.z, local.x, local.y, local.z, ptr_ja1, ptr_ja2);

    // 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;

    // Jacobians are:
    //  dryp_dpoint = dryp_dlocalpoint  * dinv_dpoint
    //  dryp_dpose  = dryp_dlocalpoint  * dinv_dpose
    if (comp_jacobs)
    {
        if (out_range == 0)
            THROW_EXCEPTION("Jacobians are undefined for range=0");

        /* MATLAB:
            syms H h_range h_yaw h_pitch real;
            syms xi_ yi_ zi_ real;
            h_range = sqrt(xi_^2+yi_^2+zi_^2);
            h_yaw   = atan(yi_/xi_);
            % h_pitch = -asin(zi_/ sqrt( xi_^2 + yi_^2 + zi_^2 ) );
            h_pitch = -atan(zi_, sqrt( xi_^2 + yi_^2) );
            H=[ h_range ; h_yaw ; h_pitch ];
            jacob_fesf_xyz=jacobian(H,[xi_ yi_ zi_])
        */
        const double _r = 1.0 / out_range;
        const double x2 = square(local.x);
        const double y2 = square(local.y);

        const double t2 = std::sqrt(x2 + y2);
        const double _K = 1.0 / (t2 * square(out_range));

        double vals[3 * 3] = {local.x * _r,
                              local.y * _r,
                              local.z * _r,
                              -local.y / (x2 * (y2 / x2 + 1)),
                              1.0 / (local.x * (y2 / x2 + 1)),
                              0,
                              (local.x * local.z) * _K,
                              (local.y * local.z) * _K,
                              -t2 / square(out_range)};

        const CMatrixDouble33 dryp_dlocalpoint(vals);
        if (out_jacob_dryp_dpoint)
            *out_jacob_dryp_dpoint = dryp_dlocalpoint * jacob_dinv_dpoint;
        if (out_jacob_dryp_dpose)
            *out_jacob_dryp_dpose = dryp_dlocalpoint * jacob_dinv_dpose;
    }
}

/**  Textual output stream function.
 */
std::ostream& mrpt::poses::operator<<(std::ostream& o, const CPose3DQuat& p)
{
    const std::streamsize old_pre = o.precision();
    const ios_base::fmtflags old_flags = o.flags();
    o << "(x,y,z,qr,qx,qy,qz)=(" << std::fixed << std::setprecision(4)
      << p.m_coords[0] << "," << p.m_coords[1] << "," << p.m_coords[2] << ","
      << p.quat()[0] << "," << p.quat()[1] << "," << p.quat()[2] << ","
      << p.quat()[3] << ")";
    o.flags(old_flags);
    o.precision(old_pre);
    return o;
}

/** Unary - operator: return the inverse pose "-p" (Note that is NOT the same
 * than a pose with all its arguments multiplied by "-1") */
CPose3DQuat mrpt::poses::operator-(const CPose3DQuat& p)
{
    CPose3DQuat ret = p;
    ret.inverse();
    return ret;
}

/** Convert this pose into its inverse, saving the result in itself. \sa
 * operator- */
void CPose3DQuat::inverse()
{
    // Invert translation:
    this->inverseComposePoint(0, 0, 0, m_coords[0], m_coords[1], m_coords[2]);

    // Invert rotation: [qr qx qy qz] ==> [qr -qx -qy -qz]
    m_quat[1] = -m_quat[1];
    m_quat[2] = -m_quat[2];
    m_quat[3] = -m_quat[3];
}

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

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

bool mrpt::poses::operator==(const CPose3DQuat& p1, const CPose3DQuat& p2)
{
    return p1.quat() == p2.quat() && p1.x() == p2.x() && p1.y() == p2.y() &&
           p1.z() == p2.z();
}

bool mrpt::poses::operator!=(const CPose3DQuat& p1, const CPose3DQuat& p2)
{
    return !(p1 == p2);
}

CPoint3D mrpt::poses::operator-(const CPoint3D& G, const CPose3DQuat& p)
{
    CPoint3D L;
    p.inverseComposePoint(G[0], G[1], G[2], L[0], L[1], L[2]);
    return L;
}

TPoint3D mrpt::poses::operator-(const TPoint3D& G, const CPose3DQuat& p)
{
    mrpt::math::TPoint3D L;
    p.inverseComposePoint(G[0], G[1], G[2], L[0], L[1], L[2]);
    return L;
}

TPose3DQuat CPose3DQuat::asTPose() const
{
    return TPose3DQuat(
        x(), y(), z(), m_quat.r(), m_quat.x(), m_quat.y(), m_quat.z());
}

void CPose3DQuat::fromString(const std::string& s)
{
    mrpt::math::CMatrixDouble m;
    if (!m.fromMatlabStringFormat(s))
        THROW_EXCEPTION("Malformed expression in ::fromString");
    ASSERTMSG_(m.rows() == 1 && m.cols() == 7, "Expected vector length=7");
    m_coords[0] = m(0, 0);
    m_coords[1] = m(0, 1);
    m_coords[2] = m(0, 2);
    m_quat[0] = m(0, 3);
    m_quat[1] = m(0, 4);
    m_quat[2] = m(0, 5);
    m_quat[3] = m(0, 6);
}

void CPose3DQuat::fromStringRaw(const std::string& s)
{
    this->fromString("[" + s + "]");
}