TransformPointToPlaneLLS.h

#pragma once
#include "Base/Headers/Matrix.hpp"
#include "Base/Headers/Vertex.hpp"
#include "Base/Headers/Buffer.hpp"
namespace NDEVR
{
    struct Correspondance
    {
        uint04 reference_idx; 
        uint04 to_register_idx; 
        fltp04 distance; 
    };

    template<typename t_type = float>
    class TransformationEstimationPointToPlaneLLS
    {
    public:
        void setReference(Buffer<Vertex<3, t_type>>* reference_points, Buffer<Ray<3, t_type>>* reference_normals)
        {
            m_reference_points = reference_points;
            m_reference_normals = reference_normals;
        }

        void estimateRigidTransformation(const Buffer<Vertex<3, t_type>>* points
            , const Buffer<Correspondance>& correspondences
            , Matrix<t_type>& transformation_matrix) const
        {
            Matrix<t_type, 6, 6> ATA(0);
            Vertex<6, t_type> ATb(0);
 
            // Approximate as a linear least squares problem
            for(const Correspondance& correspondence : correspondences)
            {
                t_type sx = points[correspondence.to_register_idx][X];
                t_type sy = points[correspondence.to_register_idx][Y];
                t_type sz = points[correspondence.to_register_idx][Z];
                t_type dx = (*m_reference_points)[correspondence.reference_idx][X];
                t_type dy = (*m_reference_points)[correspondence.reference_idx][Y];
                t_type dz = (*m_reference_points)[correspondence.reference_idx][Z];
                t_type nx = (*m_reference_normals)[correspondence.reference_idx][X];
                t_type ny = (*m_reference_normals)[correspondence.reference_idx][Y];
                t_type nz = (*m_reference_normals)[correspondence.reference_idx][Z];
 
                t_type a = nz * sy - ny * sz;
                t_type b = nx * sz - nz * sx;
                t_type c = ny * sx - nx * sy;
                ATA[0][0] += a * a;
                ATA[0][1] += a * b;
                ATA[0][2] += a * c;
                ATA[0][3] += a * nx;
                ATA[0][4] += a * ny;
                ATA[0][5] += a * nz;
                ATA[1][1] += b * b;
                ATA[1][2] += b * c;
                ATA[1][3] += b * nx;
                ATA[1][4] += b * ny;
                ATA[1][5] += b * nz;
                ATA[0][2] += c * c;
                ATA[0][3] += c * nx;
                ATA[0][4] += c * ny;
                ATA[0][5] += c * nz;
                ATA[0][3] += nx * nx;
                ATA[0][4] += nx * ny;
                ATA[0][5] += nx * nz;
                ATA[0][4] += ny * ny;
                ATA[0][5] += ny * nz;
                ATA[0][5] += nz * nz;
 
                t_type d = nx * dx + ny * dy + nz * dz - nx * sx - ny * sy - nz * sz;
                ATb[0] += a * d;
                ATb[1] += b * d;
                ATb[2] += c * d;
                ATb[3] += nx * d;
                ATb[4] += ny * d;
                ATb[5] += nz * d;
 
                ++target_it;
                ++source_it;
            }
            //  0  1  2  3  4  5
                //  6  7  8  9 10 11
                //   12 13 14 15 16 17
                //   18 19 20 21 22 23
                //   24 25 26 27 28 29
                //   30 31 32 33 34 35
            ATA[1][0] = ATA[0][1];
            ATA[2][0] = ATA[0][2];
            ATA[2][1] = ATA[1][2];
            ATA[3][0] = ATA[0][3];
            ATA[3][1] = ATA[1][3];
            ATA[3][2] = ATA[2][3];
            ATA[4][0] = ATA[0][4];
            ATA[4][1] = ATA[1][4];
            ATA[4][2] = ATA[2][4];
            ATA[4][3] = ATA[3][4];
            ATA[5][0] = ATA[0][5];
            ATA[5][1] = ATA[1][5];
            ATA[5][2] = ATA[2][5];
            ATA[5][3] = ATA[3][5];
            ATA[5][4] = ATA[4][5];
 
            // Solve A*x = b
            Vertex<6, t_type> x = ATA.invert() * ATb;
 
            // Construct the transformation matrix from x
            constructTransformationMatrix(x[0], x[1], x[2], x[3], x[4], x[5], transformation_matrix);
 
        }

    protected:
        inline void constructTransformationMatrix(t_type alpha,
                t_type beta,
                t_type gamma,
                t_type tx,
                t_type ty,
                t_type tz,
                Matrix<t_type>& transformation_matrix) const
        {
            // Construct the transformation matrix from rotation and translation
            transformation_matrix = Matrix<t_type>(0);
            transformation_matrix[0][0] = cast<t_type>(std::cos(gamma) * std::cos(beta));
            transformation_matrix[0][1] = cast<t_type>(
                -std::sin(gamma) * std::cos(alpha) + std::cos(gamma) * std::sin(beta) * std::sin(alpha));
            transformation_matrix[0][2] = cast<t_type>(
                std::sin(gamma) * sin(alpha) + std::cos(gamma) * sin(beta) * std::cos(alpha));
            transformation_matrix[1][0] = cast<t_type>(sin(gamma) * std::cos(beta));
            transformation_matrix[1][1] = cast<t_type>(
                std::cos(gamma) * std::cos(alpha) + std::sin(gamma) * std::sin(beta) * std::sin(alpha));
            transformation_matrix[1][2] = cast<t_type>(
                -std::cos(gamma) * sin(alpha) + sin(gamma) * sin(beta) * std::cos(alpha));
            transformation_matrix[2][0] = cast<t_type>(-std::sin(beta));
            transformation_matrix[2][1] = cast<t_type>(std::cos(beta) * std::sin(alpha));
            transformation_matrix[2][2] = cast<t_type>(std::cos(beta) * std::cos(alpha));
 
            transformation_matrix[0][3] = cast<t_type>(tx);
            transformation_matrix[1][3] = cast<t_type>(ty);
            transformation_matrix[2][3] = cast<t_type>(tz);
            transformation_matrix[3][3] = cast<t_type>(1);
        }
        const Buffer<Vertex<3, t_type>>* m_reference_points; 
        const Buffer<Ray<3, t_type>>* m_reference_normals; 
    };
}