C++ implementation of 3-dimensinal ICP (Iterative Closet Point) algorithm.
input : two sets of points in three-dimensional space A and B.
output : rotation and translation of B to fit in A.
- For each point in A, calculate the matching point (closest point) in B.
- Minimize the matching error between matching points and calculate the pose.
- Apply the calculated pose to the point cloud.
- Recalculate matching points.
- Iterate until the number of iterations > threshold or the value of the minimized energy function < threshold.
- SVD-based least squares best-fitting algorithm is used for the corresponding point set. (Point-to-Point)
- Eigen library is used for matrix operstions.
- Knn search method in nanoflann is used to find the nearest neighbor of each point.
- Eigen library is used for matrices operations.
- Use The Stanford Bunny Models as dataset.
- Use PCL 1.7 APIs to load datasets and visualize the PointClouds.
mkdir build
cd build
cmake ..
make
./icp_simple ../data/bun{000,045}.ply
The following are results after 1, 6 and 12 iterations.
- Import the real-world datasets.
- Visualize the datasets.
- Visualize the iterative process.
- Random Sampling
- Voxel Grid Sampling
- NSS (Normal Space Sampling)
- Feature Detaction
- Use KD-tree/OCtree to find the nearst neighbor of each point.
- Normal Shooting
- Feature descriptor matching
- Remove correspondence with high distance for outlier rejection.
- Remove worst x% of correspondences for outlier rejection.
- Point-to-Plane
- CUDA : Data Parallelism