Icp is a project mainly written in Scheme, it's free.
Iterated closest points algorithm
h2. Implementation of an Iterative Closest Point (ICP) algorithm
This is an implementation of the ICP algorithm given in Lu & Milios, "Robot Pose Estimation in Unknown Environments by Matching 2D Range Scans". It can be used to find the rotation and transform that gives a locally optimal alignment of one set of points with another. The actual algorithm consists of three subalgorithms: