Building From Scratch

Software Setup

We will use ROS noetic with C++ language. Detailed setup tutorial can be found in my previous posts.

Dataset

To keep things straightforward, we will utilize a basic handmade dataset for this post. We will create two point clouds, each with a small number of points.

In our setup, the red dots depict the initial 2D point cloud, while the green dots indicate the second point cloud. Our goal is to align these two sets: we'll use the red dots as our reference and carefully adjust the position of the green dots so they align perfectly with the red ones.

Computing Transformation

We will implement the basic ICP algorithm in C++. The function prototype is as follows:

void FindAlignment(Eigen::MatrixXd &X, Eigen::MatrixXd &Y) {

Here, X and Y are 2 x N matrices, where N is the number of points.

$X = \begin{bmatrix} x_{1x} & x_{2x} & \cdots & x_{Nx} \\ x_{1y} & x_{2y} & \cdots & x_{Ny} \end{bmatrix}$

$Y = \begin{bmatrix} y_{1x} & y_{2x} & \cdots & y_{Nx} \\ y_{1y} & y_{2y} & \cdots & y_{Ny} \end{bmatrix}$

Remember, here we assume that we know all the correspondences, e.g. $x_1$ corresponds to $y_1$, $x_2$ to $y_2$, and so on.

Step 1: Center both point clouds to the origin

$x_0 = \frac{1}{n} \sum_{i=1}^{n} x_i$

$y_0 = \frac{1}{n} \sum_{i=1}^{n} y_i$

$x_i = x_i - x_0$

$y_i = y_i - y_0$

  Eigen::Vector2d centroidX = X.rowwise().mean();
  Eigen::Vector2d centroidY = Y.rowwise().mean();
  X.colwise() -= centroidX;
  Y.colwise() -= centroidY;

You can see that the point clouds are now centered at the origin. The red dot is the centroid of the reference point cloud, while the green dot is the centroid of the second point cloud. However, in the picture below, the center points overlap each other and seems like a single orange dot.

Step 2: Compute $\mathbf{R}$ and $\mathbf{t}$ using Singular Value Decomposition (SVD)

$\mathbf{H} = \sum_{i=1}^{n} (x_i - x_0)(y_i - y_0)$

$\mathbf{H} = \mathbf{U} \mathbf{D} \mathbf{V}^T$

$\mathbf{R} = \mathbf{V} \mathbf{U}^T$

$\mathbf{t} = \frac{1}{n} \sum_{i=1}^{n} (\bar{x}_i - x_i) = \frac{1}{n} \sum_{i=1}^{n} (\mathbf{R} x_i - x_i)$

  Eigen::Matrix2d H = X * Y.transpose();
  Eigen::JacobiSVD<Eigen::MatrixXd> svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV);
  Eigen::Matrix2d U = svd.matrixU();
  Eigen::Matrix2d V = svd.matrixV();
  Eigen::Matrix2d R = V * U.transpose();
  Eigen::Vector2d t = centroidY - R * centroidX;

Step 3: Apply the transformation to the incoming point cloud

  X = R * X;
  X.colwise() += t;
}

Result

The resulting rotation matrix $\mathbf{R}$ and translation vector $\mathbf{t}$ are:

R:
-5.55112e-17            1
          -1 -5.55112e-17

t:
  3
  4

Conclusion

This is a very basic implementation of the ICP algorithm. But still, we could perfectly align the two point clouds using this algorithm. In the next post, we will implement the ICP algorithm without knowing the correspondences between the two point clouds using iterative optimization techniques. Stay tuned!