se3_l2.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 "tfest-precomp.h"  // Precompiled headers

#include <mrpt/tfest/se3.h>
#include <mrpt/poses/CPose3D.h>
#include <mrpt/poses/CPose3DQuat.h>

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

// "Closed-form solution of absolute orientation using unit quaternions", BKP
// Horn, Journal of the Optical Society of America, 1987.
// Algorithm:
// 0. Preliminary
//      pLi = { pLix, pLiy, pLiz }
//      pRi = { pRix, pRiy, pRiz }
// -------------------------------------------------------
// 1. Find the centroids of the two sets of measurements:
//      ct_others = (1/n)*sum{i}( pLi )     ct_others = { cLx, cLy, cLz }
//      ct_this = (1/n)*sum{i}( pRi )       ct_this = { cRx, cRy, cRz }
//
// 2. Substract centroids from the point coordinates:
//      pLi' = pLi - ct_others                  pLi' = { pLix', pLiy', pLiz' }
//      pRi' = pRi - ct_this                    pRi' = { pRix', pRiy', pRiz' }
//
// 3. For each pair of coordinates (correspondences) compute the nine possible
// products:
//      pi1 = pLix'*pRix'       pi2 = pLix'*pRiy'       pi3 = pLix'*pRiz'
//      pi4 = pLiy'*pRix'       pi5 = pLiy'*pRiy'       pi6 = pLiy'*pRiz'
//      pi7 = pLiz'*pRix'       pi8 = pLiz'*pRiy'       pi9 = pLiz'*pRiz'
//
// 4. Compute S components:
//      Sxx = sum{i}( pi1 )     Sxy = sum{i}( pi2 )     Sxz = sum{i}( pi3 )
//      Syx = sum{i}( pi4 )     Syy = sum{i}( pi5 )     Syz = sum{i}( pi6 )
//      Szx = sum{i}( pi7 )     Szy = sum{i}( pi8 )     Szz = sum{i}( pi9 )
//
// 5. Compute N components:
//          [ Sxx+Syy+Szz   Syz-Szy         Szx-Sxz         Sxy-Syx      ]
//          [ Syz-Szy       Sxx-Syy-Szz     Sxy+Syx         Szx+Sxz      ]
//      N = [ Szx-Sxz       Sxy+Syx         -Sxx+Syy-Szz    Syz+Szy      ]
//          [ Sxy-Syx       Szx+Sxz         Syz+Szy         -Sxx-Syy+Szz ]
//
// 6. Rotation represented by the quaternion eigenvector correspondent to the
// higher eigenvalue of N
//
// 7. Scale computation (symmetric expression)
//      s = sqrt( sum{i}( square(abs(pRi')) / sum{i}( square(abs(pLi')) ) )
//
// 8. Translation computation (distance between the Right centroid and the
// scaled and rotated Left centroid)
//      t = ct_this-sR(ct_others)

/*---------------------------------------------------------------
                    se3_l2  (old "HornMethod()")
  ---------------------------------------------------------------*/
bool se3_l2_internal(
    std::vector<mrpt::math::TPoint3D>&
        points_this,  // IN/OUT: It gets modified!
    std::vector<mrpt::math::TPoint3D>&
        points_other,  // IN/OUT: It gets modified!
    mrpt::poses::CPose3DQuat& out_transform,
    double& out_scale, bool forceScaleToUnity)
{
    MRPT_START

    ASSERT_EQUAL_(points_this.size(), points_other.size());
    // Compute the centroids
    TPoint3D ct_others(0, 0, 0), ct_this(0, 0, 0);
    const size_t nMatches = points_this.size();

    if (nMatches < 3)
        return false;  // Nothing we can estimate without 3 points!!

    for (size_t i = 0; i < nMatches; i++)
    {
        ct_others += points_other[i];
        ct_this += points_this[i];
    }

    const double F = 1.0 / nMatches;
    ct_others *= F;
    ct_this *= F;

    CMatrixDouble33 S;  // Zeroed by default

    // Substract the centroid and compute the S matrix of cross products
    for (size_t i = 0; i < nMatches; i++)
    {
        points_this[i] -= ct_this;
        points_other[i] -= ct_others;

        S.get_unsafe(0, 0) += points_other[i].x * points_this[i].x;
        S.get_unsafe(0, 1) += points_other[i].x * points_this[i].y;
        S.get_unsafe(0, 2) += points_other[i].x * points_this[i].z;

        S.get_unsafe(1, 0) += points_other[i].y * points_this[i].x;
        S.get_unsafe(1, 1) += points_other[i].y * points_this[i].y;
        S.get_unsafe(1, 2) += points_other[i].y * points_this[i].z;

        S.get_unsafe(2, 0) += points_other[i].z * points_this[i].x;
        S.get_unsafe(2, 1) += points_other[i].z * points_this[i].y;
        S.get_unsafe(2, 2) += points_other[i].z * points_this[i].z;
    }

    // Construct the N matrix
    CMatrixDouble44 N;  // Zeroed by default

    N.set_unsafe(
        0, 0, S.get_unsafe(0, 0) + S.get_unsafe(1, 1) + S.get_unsafe(2, 2));
    N.set_unsafe(0, 1, S.get_unsafe(1, 2) - S.get_unsafe(2, 1));
    N.set_unsafe(0, 2, S.get_unsafe(2, 0) - S.get_unsafe(0, 2));
    N.set_unsafe(0, 3, S.get_unsafe(0, 1) - S.get_unsafe(1, 0));

    N.set_unsafe(1, 0, N.get_unsafe(0, 1));
    N.set_unsafe(
        1, 1, S.get_unsafe(0, 0) - S.get_unsafe(1, 1) - S.get_unsafe(2, 2));
    N.set_unsafe(1, 2, S.get_unsafe(0, 1) + S.get_unsafe(1, 0));
    N.set_unsafe(1, 3, S.get_unsafe(2, 0) + S.get_unsafe(0, 2));

    N.set_unsafe(2, 0, N.get_unsafe(0, 2));
    N.set_unsafe(2, 1, N.get_unsafe(1, 2));
    N.set_unsafe(
        2, 2, -S.get_unsafe(0, 0) + S.get_unsafe(1, 1) - S.get_unsafe(2, 2));
    N.set_unsafe(2, 3, S.get_unsafe(1, 2) + S.get_unsafe(2, 1));

    N.set_unsafe(3, 0, N.get_unsafe(0, 3));
    N.set_unsafe(3, 1, N.get_unsafe(1, 3));
    N.set_unsafe(3, 2, N.get_unsafe(2, 3));
    N.set_unsafe(
        3, 3, -S.get_unsafe(0, 0) - S.get_unsafe(1, 1) + S.get_unsafe(2, 2));

    // q is the quaternion correspondent to the greatest eigenvector of the N
    // matrix (last column in Z)
    CMatrixDouble44 Z, D;
    vector<double> v;

    N.eigenVectors(Z, D);
    Z.extractCol(Z.cols() - 1, v);

    ASSERTDEB_(
        fabs(
            sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2] + v[3] * v[3]) - 1.0) <
        0.1);

    // Make q_r > 0
    if (v[0] < 0)
    {
        v[0] *= -1;
        v[1] *= -1;
        v[2] *= -1;
        v[3] *= -1;
    }

    // out_transform: Create a pose rotation with the quaternion
    for (unsigned int i = 0; i < 4; i++)  // insert the quaternion part
        out_transform[i + 3] = v[i];

    // Compute scale
    double s;
    if (forceScaleToUnity)
    {
        s = 1.0;  // Enforce scale to be 1
    }
    else
    {
        double num = 0.0;
        double den = 0.0;
        for (size_t i = 0; i < nMatches; i++)
        {
            num += square(points_other[i].x) + square(points_other[i].y) +
                   square(points_other[i].z);
            den += square(points_this[i].x) + square(points_this[i].y) +
                   square(points_this[i].z);
        }  // end-for

        // The scale:
        s = std::sqrt(num / den);
    }

    TPoint3D pp;
    out_transform.composePoint(
        ct_others.x, ct_others.y, ct_others.z, pp.x, pp.y, pp.z);
    pp *= s;

    out_transform[0] = ct_this.x - pp.x;  // X
    out_transform[1] = ct_this.y - pp.y;  // Y
    out_transform[2] = ct_this.z - pp.z;  // Z

    out_scale = s;  // return scale
    return true;

    MRPT_END
}  // end se3_l2_internal()

bool tfest::se3_l2(
    const std::vector<mrpt::math::TPoint3D>& in_points_this,
    const std::vector<mrpt::math::TPoint3D>& in_points_other,
    mrpt::poses::CPose3DQuat& out_transform, double& out_scale,
    bool forceScaleToUnity)
{
    // make a copy because we need it anyway to substract the centroid and to
    // provide a unified interface to TMatchingList API
    std::vector<mrpt::math::TPoint3D> points_this = in_points_this;
    std::vector<mrpt::math::TPoint3D> points_other = in_points_other;

    return se3_l2_internal(
        points_this, points_other, out_transform, out_scale, forceScaleToUnity);
}

bool tfest::se3_l2(
    const mrpt::tfest::TMatchingPairList& corrs,
    mrpt::poses::CPose3DQuat& out_transform, double& out_scale,
    bool forceScaleToUnity)
{
    // Transform data types:
    const size_t N = corrs.size();
    std::vector<mrpt::math::TPoint3D> points_this(N), points_other(N);
    for (size_t i = 0; i < N; i++)
    {
        points_this[i].x = corrs[i].this_x;
        points_this[i].y = corrs[i].this_y;
        points_this[i].z = corrs[i].this_z;
        points_other[i].x = corrs[i].other_x;
        points_other[i].y = corrs[i].other_y;
        points_other[i].z = corrs[i].other_z;
    }
    return se3_l2_internal(
        points_this, points_other, out_transform, out_scale, forceScaleToUnity);
}