se3_l2.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 "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::utils;
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.getColCount()-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::utils::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);
}