The Mobile Robot Programming Toolkit

Results

The main method used in this example is CICP::Align3DPDF. This example first simulates a pair of 3D point clouds by performing ray-tracing from two different poses:

Next, a small error in all six dimensions is introduced in the pose of one point cloud, leading to unaligned point clouds:

After applying the ICP-3D algorithm to these unaligned maps, the algorithm successfully recovers the correct alignment, as can be seen in the figure:

The code

The up-to-date example can be found in: http://mrpt.googlecode.com/svn/trunk/samples/icp3D/