4
\$\begingroup\$

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; }
\$\endgroup\$
3
  • \$\begingroup\$ I am confused. Whar are you solving? Closest point can be anything. \$\endgroup\$ Commented Jan 7, 2023 at 20:05
  • \$\begingroup\$ Oh. I have 2 point cloud data taken from 2 text files. I am taking those and computing the transformations (Rot and Trans) for converting one to the other and saving the resultant point cloud in a text file. For more info \$\endgroup\$ Commented Jan 7, 2023 at 20:09
  • 2
    \$\begingroup\$ That's not a closest point algo. You should at least explain what are you trying to solve/optimize. IIRC computing transformations (affine or orthogonal) from an array of points to an array of points can be computed directly - inversion and/or square root of a matrix at most and/or some decomposition like SVD. No need for general optimization methods. \$\endgroup\$ Commented Jan 7, 2023 at 21:25

1 Answer 1

Reset to default
2
\$\begingroup\$

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
\$\endgroup\$

You must log in to answer this question.

Start asking to get answers

Find the answer to your question by asking.

Ask question

Explore related questions

See similar questions with these tags.