Implementation of the iterative closest point algorithm. Algorithm is based on the work outlined in [1]. A point cloud is transformed such that it best "matches" a reference point cloud.
For each point in the dynamic point cloud, we search for its closest point in the static point cloud. We solve for an affine transform which minimized the distance between these point pairs. The dynamic point are updated with this affine transform and the process is repeated until a stopping condition is met.
Transforms are solved using SVD decomposition and searching is performed using kd-tree NN search.
class Point
{
float pos[3]; // x,y,z coordinates of a point in space.
}
ICP.h:
void icp(std::vector<Point*> &dynamicPointCloud, std::vector<Point*> &staticPointCloud);
build the following located in /src :
- ICP.cpp
- ICP.h
- KdTree.cpp
- KdTree.h
- svd.cpp
- svd.h
- defs_and_types.h
iteration 0:
iteration 20:
iteration 40:
iteration 100:
iteration 400:
[1] Arun, K. Somani, Thomas S. Huang, and Steven D. Blostein. "Least-squares fitting of two 3-D point sets." IEEE Transactions on pattern analysis and machine intelligence 5 (1987): 698-700.