optimal_tf_olae.cpp

Go to the documentation of this file.

/* -------------------------------------------------------------------------
 *  A repertory of multi primitive-to-primitive (MP2P) ICP algorithms in C++
 * Copyright (C) 2018-2024 Jose Luis Blanco, University of Almeria
 * See LICENSE for license information.
 * ------------------------------------------------------------------------- */
#include <mp2p_icp/optimal_tf_olae.h>
#include <mrpt/core/exceptions.h>
#include <mrpt/poses/Lie/SE.h>
#include <mrpt/tfest/se3.h>
 
#include <Eigen/Dense>
 
#include "visit_correspondences.h"
 
using namespace mp2p_icp;
 
// Convert to quaternion by normalizing q=[1, optim_rot], then to rot. matrix:
static mrpt::poses::CPose3D gibbs2pose(const Eigen::Vector3d& v)
{
    auto       x = v[0], y = v[1], z = v[2];
    const auto r = 1.0 / std::sqrt(1.0 + x * x + y * y + z * z);
    x *= r;
    y *= r;
    z *= r;
    auto q = mrpt::math::CQuaternionDouble(r, -x, -y, -z);
 
    // Quaternion to 3x3 rot matrix:
    return mrpt::poses::CPose3D(q, .0, .0, .0);
}
 
struct OLAE_LinearSystems
{
    Eigen::Matrix3d M, Mx, My, Mz;
    Eigen::Vector3d v, vx, vy, vz;
 
    Eigen::Matrix3d B;
};
 
static OLAE_LinearSystems olae_build_linear_system(
    const Pairings& in, const WeightParameters& wp, const mrpt::math::TPoint3D& ct_local,
    const mrpt::math::TPoint3D& ct_global, OutlierIndices& in_out_outliers)
{
    MRPT_START
 
    using mrpt::math::TPoint3D;
    using mrpt::math::TVector3D;
 
    OLAE_LinearSystems res;
 
    // Build the linear system: M g = v
    res.M = Eigen::Matrix3d::Zero();
    res.v = Eigen::Vector3d::Zero();
 
    // Attitude profile matrix:
    res.B = Eigen::Matrix3d::Zero();
 
    // Lambda: process each pairing:
    auto lambda_each_pair =
        [&](const mrpt::math::TVector3D& bi, const mrpt::math::TVector3D& ri, const double wi)
    {
// We will evaluate M from an alternative expression below from the
// attitude profile matrix B instead, since it seems to be slightly more
// stable, numerically. The original code for M is left here for
// reference, though.
#if 0
    // M+=(1/2)* ([s_i]_{x})^2
    // with: s_i = b_i + r_i
    const double sx = bi.x + ri.x, sy = bi.y + ri.y, sz = bi.z + ri.z;
 
    /* ([s_i]_{x})^2 is:
     *
     *  ⎡    2     2                          ⎤
     *  ⎢- sy  - sz      sx⋅sy        sx⋅sz   ⎥
     *  ⎢                                     ⎥
     *  ⎢                 2     2             ⎥
     *  ⎢   sx⋅sy     - sx  - sz      sy⋅sz   ⎥
     *  ⎢                                     ⎥
     *  ⎢                              2     2⎥
     *  ⎣   sx⋅sz        sy⋅sz     - sx  - sy ⎦
     */
    const double c00 = -sy * sy - sz * sz;
    const double c11 = -sx * sx - sz * sz;
    const double c22 = -sx * sx - sy * sy;
    const double c01 = sx * sy;
    const double c02 = sx * sz;
    const double c12 = sy * sz;
 
    // clang-format off
    const auto dM = (Eigen::Matrix3d() <<
       c00, c01, c02,
       c01, c11, c12,
       c02, c12, c22 ).finished();
    // clang-format on
 
    // res.M += wi * dM;
 
    // The missing (1/2) from the formulas above:
    res.M *= 0.5;
#endif
        /* v-= weight *  [b_i]_{x}  r_i
         *  Each term is:
         *  ⎡by⋅rz - bz⋅ry ⎤   ⎡ B23 - B32 ⎤
         *  ⎢              ⎥   |           ⎥
         *  ⎢-bx⋅rz + bz⋅rx⎥ = | B31 - B13 ⎥
         *  ⎢              ⎥   |           ⎥
         *  ⎣bx⋅ry - by⋅rx ⎦   ⎣ B12 - B21 ⎦
         *
         * B (attitude profile matrix):
         *
         * B+= weight * (b_i * r_i')
         *
         */
 
        // clang-format off
    const auto dV = (Eigen::Vector3d() <<
       (bi.y * ri.z - bi.z * ri.y),
       (-bi.x * ri.z + bi.z * ri.x),
       (bi.x * ri.y - bi.y * ri.x) ).finished();
        // clang-format on
 
        res.v -= wi * dV;
 
        // clang-format off
    const auto dB = (Eigen::Matrix3d() <<
       bi.x * ri.x, bi.x * ri.y, bi.x * ri.z,
       bi.y * ri.x, bi.y * ri.y, bi.y * ri.z,
       bi.z * ri.x, bi.z * ri.y, bi.z * ri.z).finished();
        // clang-format on
        res.B += wi * dB;
    };  // end lambda for visit_correspondences()
 
    // Lambda for the final stage after visiting all corres:
    auto lambda_final = [&](const double w_sum)
    {
        // Normalize weights. OLAE assumes \sum(w_i) = 1.0
        if (w_sum > .0)
        {
            const auto f = (1.0 / w_sum);
            // res.M *= f;
            res.v *= f;
            res.B *= f;
        }
        else
        {
            // We either had NO input correspondences, or ALL were detected
            // as outliers... What to do in this case?
        }
    };
 
    visit_correspondences(
        in, wp, ct_local, ct_global, in_out_outliers, lambda_each_pair, lambda_final,
        true /* DO make unit point vectors for OLAE */);
 
    // Now, compute the other three sets of linear systems, corresponding
    // to the "sequential rotation method" [shuster1981attitude], so we can
    // later keep the best one (i.e. the one with the largest |M|).
    {
        const Eigen::Matrix3d S = res.B + res.B.transpose();
        const double          p = res.B.trace() + 1;
        const double          m = res.B.trace() - 1;
        // Short cut:
        const auto& v = res.v;
 
        // Set #0: M g=v, without further rotations (the system built above).
        // clang-format off
        res.M = (Eigen::Matrix3d() <<
           S(0,0)-p,  S(0,1), S(0,2),
           S(0,1),   S(1,1)-p, S(1,2),
           S(0,2),   S(1,2),  S(2,2)-p ).finished();
        // clang-format on
 
        const auto&  M0 = res.M;  // shortcut
        const double z1 = v[0], z2 = v[1], z3 = v[2];
 
        // Set #1: rotating 180 deg around "x":
        // clang-format off
        res.Mx = (Eigen::Matrix3d() <<
           m     ,      -z3  ,     z2,
           -z3   ,  M0(2,2),     -S(1,2),
           z2    ,  -S(1,2),    M0(1,1)).finished();
        res.vx = (Eigen::Vector3d() <<
            -z1, S(0,2), -S(0,1)
            ).finished();
        // clang-format on
 
        // Set #2: rotating 180 deg around "y":
        // clang-format off
        res.My = (Eigen::Matrix3d() <<
           M0(2,2),     z3  ,     -S(0,2),
           z3     ,       m ,     -z1,
         -S(0,2)  ,     -z1 ,   M0(0,0)).finished();
        res.vy = (Eigen::Vector3d() <<
            -S(1,2), -z2, S(0,1)
            ).finished();
        // clang-format on
 
        // Set #3: rotating 180 deg around "z":
        // clang-format off
        res.Mz = (Eigen::Matrix3d() <<
           M0(1,1),  -S(0,1),     -z2,
          -S(0,1) ,  M0(0,0),      z1,
             -z2  ,      z1 ,      m).finished();
        res.vz = (Eigen::Vector3d() <<
            S(1,2), -S(0,2), -z3
            ).finished();
        // clang-format on
    }
 
    return res;
 
    MRPT_END
}
 
// See .h docs, and associated technical report.
bool mp2p_icp::optimal_tf_olae(
    const Pairings& in, const WeightParameters& wp, OptimalTF_Result& result)
{
    MRPT_START
 
    using mrpt::math::TPoint3D;
    using mrpt::math::TVector3D;
 
    // Note on notation: we are search the relative transformation of
    // the "other" frame wrt to "this", i.e. "this"="global",
    // "other"="local":
    //   p_this = pose \oplus p_other
    //   p_A    = pose \oplus p_B      --> pB = p_A \ominus pose
 
    // Reset output to defaults:
    result = OptimalTF_Result();
 
    // Normalize weights for each feature type and for each target (attitude
    // / translation):
    ASSERT_(wp.pair_weights.pt2pt >= .0);
    ASSERT_(wp.pair_weights.ln2ln >= .0);
    ASSERT_(wp.pair_weights.pl2pl >= .0);
 
    // Compute the centroids:
    auto [ct_local, ct_global] = eval_centroids_robust(in, result.outliers /* empty for now  */);
 
    // Build the linear system: M g = v
    OLAE_LinearSystems linsys =
        olae_build_linear_system(in, wp, ct_local, ct_global, result.outliers /* empty for now  */);
 
    // Re-evaluate the centroids, now that we have a guess on outliers.
    if (!result.outliers.empty())
    {
        // Re-evaluate the centroids:
        const auto [new_ct_local, new_ct_global] = eval_centroids_robust(in, result.outliers);
 
        ct_local  = new_ct_local;
        ct_global = new_ct_global;
 
        // And rebuild the linear system with the new values:
        linsys = olae_build_linear_system(in, wp, ct_local, ct_global, result.outliers);
    }
 
    // We are finding the optimal rotation "g", as a Gibbs vector.
    // Solve linear system for optimal rotation: M g = v
 
    const double detM_orig = std::abs(linsys.M.determinant()),
                 detMx     = std::abs(linsys.Mx.determinant()),
                 detMy     = std::abs(linsys.My.determinant()),
                 detMz     = std::abs(linsys.Mz.determinant());
 
#if 0
    // clang-format off
    std::cout << " |M_orig|= " << detM_orig << "\n"
                 " |M_x|   = " << detMx << "\n"
                 " |M_t|   = " << detMy << "\n"
                 " |M_z|   = " << detMz << "\n";
    // clang-format on
#endif
 
    if (detM_orig > mrpt::max3(detMx, detMy, detMz))
    {
        // original rotation is the best numerically-determined problem:
        const auto sol0    = gibbs2pose(linsys.M.colPivHouseholderQr().solve(linsys.v));
        result.optimalPose = sol0;
#if 0
        std::cout << "M   : |M|="
                  << mrpt::format("%16.07f", linsys.M.determinant())
                  << " sol: " << sol0.asString() << "\n";
#endif
    }
    else if (detMx > mrpt::max3(detM_orig, detMy, detMz))
    {
        // rotation wrt X is the best choice:
        auto sol1          = gibbs2pose(linsys.Mx.colPivHouseholderQr().solve(linsys.vx));
        sol1               = mrpt::poses::CPose3D(0, 0, 0, 0, 0, M_PI) + sol1;
        result.optimalPose = sol1;
#if 0
        std::cout << "M_x : |M|="
                  << mrpt::format("%16.07f", linsys.Mx.determinant())
                  << " sol: " << sol1.asString() << "\n";
#endif
    }
    else if (detMy > mrpt::max3(detM_orig, detMx, detMz))
    {
        // rotation wrt Y is the best choice:
        auto sol2          = gibbs2pose(linsys.My.colPivHouseholderQr().solve(linsys.vy));
        sol2               = mrpt::poses::CPose3D(0, 0, 0, 0, M_PI, 0) + sol2;
        result.optimalPose = sol2;
#if 0
        std::cout << "M_y : |M|="
                  << mrpt::format("%16.07f", linsys.My.determinant())
                  << " sol: " << sol2.asString() << "\n";
#endif
    }
    else
    {
        // rotation wrt Z is the best choice:
        auto sol3          = gibbs2pose(linsys.Mz.colPivHouseholderQr().solve(linsys.vz));
        sol3               = mrpt::poses::CPose3D(0, 0, 0, M_PI, 0, 0) + sol3;
        result.optimalPose = sol3;
#if 0
        std::cout << "M_z : |M|="
                  << mrpt::format("%16.07f", linsys.Mz.determinant())
                  << " sol: " << sol3.asString() << "\n";
#endif
    }
 
    // Use centroids to solve for optimal translation:
    mrpt::math::TPoint3D pp;
    result.optimalPose.composePoint(ct_local.x, ct_local.y, ct_local.z, pp.x, pp.y, pp.z);
    // Scale, if used, was: pp *= s;
 
    result.optimalPose.x(ct_global.x - pp.x);
    result.optimalPose.y(ct_global.y - pp.y);
    result.optimalPose.z(ct_global.z - pp.z);
 
    return true;
 
    MRPT_END
}