estimate_points_eigen.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/estimate_points_eigen.h>
#include <mrpt/core/exceptions.h>
 
#include <stdexcept>
 
mp2p_icp::PointCloudEigen mp2p_icp::estimate_points_eigen(
    const float* xs, const float* ys, const float* zs,
    mrpt::optional_ref<const std::vector<size_t>> indices, std::optional<size_t> totalCount)
{
    MRPT_START
 
    mrpt::math::TPoint3Df                  mean{0, 0, 0};
    mrpt::math::CMatrixFixed<double, 3, 3> mat_a;
    mat_a.setZero();
 
    // sanity checks:
    if (totalCount)
    {
        if (*totalCount < 3)
        {
            THROW_EXCEPTION("totalCount: at least 3 points required.");
        }
 
        const float inv_n = 1.0f / static_cast<float>(*totalCount);
        for (size_t i = 0; i < *totalCount; i++)
        {
            mean.x += xs[i];
            mean.y += ys[i];
            mean.z += zs[i];
        }
        mean *= inv_n;
        for (size_t i = 0; i < *totalCount; i++)
        {
            const auto a = mrpt::math::TPoint3Df(xs[i], ys[i], zs[i]) - mean;
            mat_a(0, 0) += a.x * a.x;
            mat_a(1, 0) += a.x * a.y;
            mat_a(2, 0) += a.x * a.z;
            mat_a(1, 1) += a.y * a.y;
            mat_a(2, 1) += a.y * a.z;
            mat_a(2, 2) += a.z * a.z;
        }
        mat_a *= inv_n;
    }
    else
    {
        ASSERTMSG_(indices, "Provide either optional<> indices or totalCount.");
        const auto& idxs = indices->get();
 
        if (idxs.size() < 3)
        {
            THROW_EXCEPTION("indices: at least 3 points required.");
        }
 
        const float inv_n = 1.0f / static_cast<float>(idxs.size());
        for (size_t i = 0; i < idxs.size(); i++)
        {
            const auto pt_idx = idxs[i];
            mean.x += xs[pt_idx];
            mean.y += ys[pt_idx];
            mean.z += zs[pt_idx];
        }
        mean *= inv_n;
        for (size_t i = 0; i < idxs.size(); i++)
        {
            const auto pt_idx = idxs[i];
            const auto a      = mrpt::math::TPoint3Df(xs[pt_idx], ys[pt_idx], zs[pt_idx]) - mean;
            mat_a(0, 0) += a.x * a.x;
            mat_a(1, 0) += a.x * a.y;
            mat_a(2, 0) += a.x * a.z;
            mat_a(1, 1) += a.y * a.y;
            mat_a(2, 1) += a.y * a.z;
            mat_a(2, 2) += a.z * a.z;
        }
        mat_a *= inv_n;
    }
 
    // Complete the upper-half symmetric part of the matrix:
    mat_a(0, 1) = mat_a(1, 0);
    mat_a(0, 2) = mat_a(2, 0);
    mat_a(1, 2) = mat_a(2, 1);
 
    mp2p_icp::PointCloudEigen ret;
    ret.meanCov = {mrpt::poses::CPoint3D(mean), mat_a};
 
    // Find eigenvalues & eigenvectors:
    // This only looks at the lower-triangular part of the cov matrix.
    mrpt::math::CMatrixFixed<double, 3, 3> eigVectors;
    std::vector<double>                    eigVals;
 
    mat_a.eig_symmetric(eigVectors, eigVals);
 
    for (int i = 0; i < 3; i++)
    {
        ret.eigVals[i]    = eigVals[i];
        const auto ev     = eigVectors.extractColumn<mrpt::math::TVector3D>(i);
        ret.eigVectors[i] = {ev.x, ev.y, ev.z};
    }
 
    return ret;
 
    MRPT_END
}
 
void mp2p_icp::vector_of_points_to_xyz(
    const std::vector<mrpt::math::TPoint3Df>& pts, std::vector<float>& xs, std::vector<float>& ys,
    std::vector<float>& zs)
{
    const auto N = pts.size();
    xs.resize(N);
    ys.resize(N);
    zs.resize(N);
    for (size_t i = 0; i < N; i++)
    {
        xs[i] = pts[i].x;
        ys[i] = pts[i].y;
        zs[i] = pts[i].z;
    }
}