Implementation of Iterative Closest Point in C++

Question

Here, is my implementation of Iterative Closest Point algorithm in C++. The code is written using the Eigen library. I have tried to implement an efficient coding methodology best to my knowledge however, I believe there are still improvements that can be made in terms of vectorizing the code and getting rid of for loops.

For further information: GitHub

#include<iostream>
#include<eigen3/Eigen/Dense>
#include<eigen3/Eigen/Core>
#include<eigen3/Eigen/SVD>
#include<fstream>
#include<vector>

// Run using: g++ -std=c++17 -I/usr/include/eigen3 ICP.cpp -o ICP -O2 -DNDEBUG

using namespace Eigen;

struct Tyx
{
    MatrixXd Rot;
    VectorXd trans;
};

// Rigid Registration.
Tyx ComputeOptimalRigidRegistration(MatrixXd X, MatrixXd Y, MatrixXi C)
{
    Tyx T;
    // Calculate the point cloud centroids.
    MatrixXd x_subset = X(C.col(0), Eigen::placeholders::all);
    MatrixXd y_subset = Y(C.col(1), Eigen::placeholders::all);
    VectorXd x_centroid = x_subset.colwise().mean();
    VectorXd y_centroid = y_subset.colwise().mean();

    // Calculate the deviation of X and Y.
    MatrixXd x_deviation = x_subset.rowwise() - x_centroid.transpose();
    MatrixXd y_deviation = y_subset.rowwise() - y_centroid.transpose();

    // Calculate covariance.
    MatrixXd W = x_deviation.transpose() * y_deviation;
    //std::cout << "W covarieance matrix: " << W << std::endl;

    //Calculate the SVD.
    JacobiSVD<MatrixXd> svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV);
    MatrixXd U = svd.matrixU();
    MatrixXd V = svd.matrixV();
    //std::cout << "U = " << U << std::endl;
    //std::cout << "V = " << V << std::endl;

    // Construct optimal rotation
    T.Rot = U * V.transpose();
    //std::cout << "T.Rot = " << T.Rot << std::endl;

    // Construct Optimal translation
    T.trans = y_centroid - (x_centroid.transpose()*T.Rot).transpose();
    //std::cout << "T.trans = " << T.trans << std::endl;

    return T;
}


// Get the correspondences. 
MatrixXi EstimateCorrespondences(MatrixXd X, MatrixXd Y, Vector3d t, Matrix3d R, double d_max)
{
    //Initialize C as empty.
    MatrixXi C(0,2);
    int N = X.rows();
    MatrixXd transposed_x(N,3);
    int index;
    VectorXd norm(N);
    for(int i = 0; i < X.rows(); i++)
    {
        transposed_x.row(i) = X.row(i) * R + t.transpose();
    }
    // Find the indexes in the least square sense. 
    for(int i =0; i < transposed_x.rows(); i++)
    {
        // diff = Y.rowwise() - transposed_x.row(i);
        // norm = diff.rowwise().norm();
        norm = (Y.rowwise() - transposed_x.row(i)).rowwise().norm();
        // Print the first 10 elements of the norm vector. Debugging purposes.
        // if (i == 0)
        // {
        //     std::cout << "X = " << X.row(0) << std::endl;
        //     std::cout << "Y = " << Y.row(0) << std::endl;
        //     std::cout << "transposed_x = " << transposed_x.row(0) << std::endl;
        //     std::cout << norm.head(10) << std::endl;            
        // }
        norm.minCoeff(&index);
        if (norm.coeff(index) < d_max)
        {
            C.conservativeResize(C.rows()+1, Eigen::NoChange);
            C.row(C.rows()-1) << i,index;
        }
    }
    return C;
}


Tyx ICP_algo(MatrixXd X, MatrixXd Y, Vector3d t0, Matrix3d R0, double d_max, int max_iter)
{
    int iter = 0;
    Tyx T;
    MatrixXi C;
    while(true)
    {
        C = EstimateCorrespondences(X , Y , t0 , R0 , d_max);
        T = ComputeOptimalRigidRegistration(X, Y, C);
        t0 = T.trans;
        R0 = T.Rot;
        iter = iter +1;
        if (iter == max_iter)
        {
            std::cout << "Max iterations reached " << iter << std::endl;
            break;
        }
    }
    T.Corresp = C;

    return T;
}



int main()
{

    // Create a vector of vector
    std::vector<std::vector<double>> data;
    double value;
    //Open the text file.
    std::ifstream file("pclX.txt");
    //Read the data from the file to the matrix
    while(file >> value)
    {
        data.push_back({value});
        while (file.peek() == ' ')
        {
            file >> value;
            data.back().push_back(value);
        }
    }
    file.close();
    int n_rows = data.size();
    int n_cols = data[0].size();
    MatrixXd pcl_X(n_rows,n_cols);

    for (int i = 0; i < n_rows; ++i)
    {
        for (int j =0; j < n_cols; ++j)
        {
            pcl_X(i,j) = data[i][j];
        }
    }
    file.close();
    // Similarly for pcl_Y
    std::ifstream file_y("pclY.txt");
    //Read the data from the file to the matrix
    // Clear the data vector
    data.clear();
    while(file_y >> value)
    {
        data.push_back({value});
        while (file_y.peek() == ' ')
        {
            file_y >> value;
            data.back().push_back(value);
        }
    }
    file_y.close();
    n_rows = data.size();
    n_cols = data[0].size();
    MatrixXd pcl_Y(n_rows,n_cols);

    for (int i = 0; i < n_rows; i++)
    {
        for (int j =0; j < n_cols; j++)
        {
            pcl_Y(i,j) = data[i][j];
        }
    }

    // Initial translational vector and Rotational matrix
    Vector3d t = Vector3d::Zero();
    Matrix3d R = Matrix3d::Identity();  // Create a identity matrix.
    double d_max = 0.25;
    int iter = 30;
    Tyx result;

    // std::cout << "pcl_X = " << pcl_X.row(0) << std::endl;
    // std::cout << "pcl_Y = " << pcl_Y.row(0) << std::endl;
    clock_t begin = clock();
    result = ICP_algo(pcl_X, pcl_Y, t, R, d_max, iter);
    clock_t end  = clock();
    double elapsed_secs = double(end - begin) / CLOCKS_PER_SEC;
    std::cout << "Time taken : " << elapsed_secs << std::endl;

    std::cout << "\n Translation vector: " << result.trans << std::endl;
    std::cout << "-----------------------------" << std::endl;
    std::cout << "Rotation matrix: " << result.Rot << std::endl;

    MatrixXd result_pt_cloud(pcl_X.rows(),3);
    for(int i = 0; i < pcl_X.rows(); i++)
    {
        result_pt_cloud.row(i) = pcl_X.row(i) * result.Rot + result.trans.transpose();
    }
    // Write the result point cloud to a text file.
    std::ofstream file_result("result.txt");
    for(int i = 0; i < result_pt_cloud.rows(); i++)
    {
        file_result << result_pt_cloud(i,0) << " " << result_pt_cloud(i,1) << " " << result_pt_cloud(i,2) << std::endl;
    }

    return 0;

}

Answer 1

General Observations

Code that contains commented out executable code (debug code) such as

    // std::cout << "pcl_X = " << pcl_X.row(0) << std::endl;
    // std::cout << "pcl_Y = " << pcl_Y.row(0) << std::endl;

is not ready to be reviewed. Please keep this in mind for future code reviews.

DRY Code

There is a programming principle called the Don't Repeat Yourself Principle sometimes referred to as DRY code. If you find yourself repeating the same code multiple times it is better to encapsulate it in a function. If it is possible to loop through the code that can reduce repetition as well.

The code to read the data from the files into the matrices is repeated, the code can be turned into a function which takes the file name as in input parameter and returns the data vector. There are 2 possible functions here, one returns the data vector and the other returns the matrix.

Data Only

static std::vector<std::vector<double>> getFileData(std::string fileName)
{
    std::vector<std::vector<double>> data;
    double value;
    //Open the text file.
    std::ifstream file(fileName);
    //Read the data from the file to the matrix
    while (file >> value)
    {
        data.push_back({ value });
        while (file.peek() == ' ')
        {
            file >> value;
            data.back().push_back(value);
        }
    }
    file.close();

    return data;
}

Matrix

static MatrixXd getMatrixFromFile(std::string fileName)
{
    std::vector<std::vector<double>> data;
    double value;
    //Open the text file.
    std::ifstream file(fileName);
    //Read the data from the file to the matrix
    while (file >> value)
    {
        data.push_back({ value });
        while (file.peek() == ' ')
        {
            file >> value;
            data.back().push_back(value);
        }
    }
    file.close();

    int n_rows = data.size();
    int n_cols = data[0].size();
    MatrixXd pcl_X(n_rows, n_cols);

    for (int i = 0; i < n_rows; ++i)
    {
        for (int j = 0; j < n_cols; ++j)
        {
            pcl_X(i, j) = data[i][j];
        }
    }

    return pcl_X;
}

Complexity

The function main() is too complex (does too much). As programs grow in size the use of main() should be limited to calling functions that parse the command line, calling functions that set up for processing, calling functions that execute the desired function of the program, and calling functions to clean up after the main portion of the program.

There is also a programming principle called the Single Responsibility Principle that applies here. The Single Responsibility Principle states:

that every module, class, or function should have responsibility over a single part of the functionality provided by the software, and that responsibility should be entirely encapsulated by that module, class or function.

In part this complexity can be reduced with DRY Code above.

Something else to keep in mind is that a general rule of thumb in programming or software engineering is that no function should be larger than one screen in an IDE or editor, if it gets larger than this it is difficult to read, write and maintain.

The main() function is currently over 90 lines of code, most IDEs present 55 lines or less in the screen.

Timing Excution

The clock() function that the code currently uses is a wall clock function which returns the current time of day. There are more accurate ways to time a function() using std::chrono functions. This is a class I put together for this purpose, you can find it in one of my questions. Creating an instance starts the timer, calling stopTimerAndReport() reports the result.

#ifndef CC_UTILITY_TIMER_H
#define CC_UTILITY_TIMER_H

#include <chrono>
#include <ctime>
#include <iomanip>
#include <iostream>
#include <string_view>

class UtilityTimer
{
public:
    using clock = std::chrono::steady_clock;

    void startTimer() noexcept
    {
        start = clock::now();
    }
    void stopTimerAndReport(std::string_view whatIsBeingTimed) noexcept
    {
        clock::time_point end = clock::now();

        std::chrono::duration<double> elapsed_seconds = end - start;
        double ElapsedTimeForOutPut = elapsed_seconds.count();

        using std::chrono::system_clock;
        auto const now = system_clock::to_time_t(system_clock::now());
        std::clog << "finished " << whatIsBeingTimed << std::put_time(std::localtime(&now), "%c")
              << "\nelapsed time in seconds: " << ElapsedTimeForOutPut << "\n\n\n";
    }

private:
    clock::time_point start = clock::now();
};

#endif // CC_UTILITY_TIMER_H