ICP – Iterative closest point, is a very trivial algorithm for matching object templates to noisy data. It’s also super easy to program, so it’s good material for a tutorial. The goal is to take a known set of points (usually defining a curve or object exterior) and register it, as good as possible, to a set of other points, usually a larger and noisy set in which we would like to find the object. The basic algorithm is described very briefly in wikipedia, but there are a ton of papers on the subject.

I’ll take you through the steps of programming it with OpenCV.

So the algorithm is basically

- Find the points which are closest to our curve
- Compute the best rotation and translation between our points and the closest points
- Move our points according to the found transformation, and check the error
- Run another iteration until convergence.

How easy is that?? OpenCV gives us the tools to do everything in there!

## Finding closest points

In v2.x of OpCV they introduced the FLANN framework of range search methods. It’s fairly easy to use, and produces good results, although I did run into some problems with it before. Let’s see how it’s used:

float flann_knn(Mat& m_destinations, Mat& m_object, vector<int>& ptpairs, vector<float>& dists = vector<float>()) { // find nearest neighbors using FLANN cv::Mat m_indices(m_object.rows, 1, CV_32S); cv::Mat m_dists(m_object.rows, 1, CV_32F); Mat dest_32f; m_destinations.convertTo(dest_32f,CV_32FC2); Mat obj_32f; m_object.convertTo(obj_32f,CV_32FC2); assert(dest_32f.type() == CV_32F); cv::flann::Index flann_index(dest_32f, cv::flann::KDTreeIndexParams(2)); // using 2 randomized kdtrees flann_index.knnSearch(obj_32f, m_indices, m_dists, 1, cv::flann::SearchParams(64) ); int* indices_ptr = m_indices.ptr<int>(0); //float* dists_ptr = m_dists.ptr<float>(0); for (int i=0;i<m_indices.rows;++i) { ptpairs.push_back(indices_ptr[i]); } dists.resize(m_dists.rows); m_dists.copyTo(Mat(dists)); return cv::sum(m_dists)[0]; }

This code was practically ripped off OpenCV’s sample code, and worked straight up.. You can see I have 2 input matrices that define the “destination” points – these are the unknown, the “object” points – these are our points, and an output vector to mark the correspondence between the two sets. I also have a vector for distances between points of each pair.

I define a FLANN index object using KD-tree and perform a KNN (K nearest neighbors) search on it, for all the object points. After invoking the search function, it’s all garnish.

## Compute transform

With the point pairs given, computing the transform between the two sets should be easy. And it is. I started by computing it using least-squares to find the parameters of the transformation: rotation angle, translation in X and Y directions. But my solution was not “resistant” to scale, so I looked up another easy solution to the problem, and surely enough a simple solution was found in this paper.

void findBestReansformSVD(Mat& _m, Mat& _d) { Mat m; _m.convertTo(m,CV_32F); Mat d; _d.convertTo(d,CV_32F); Scalar d_bar = mean(d); Scalar m_bar = mean(m); Mat mc = m - m_bar; Mat dc = d - d_bar; mc = mc.reshape(1); dc = dc.reshape(1); Mat H(2,2,CV_32FC1); for(int i=0;i<mc.rows;i++) { Mat mci = mc(Range(i,i+1),Range(0,2)); Mat dci = dc(Range(i,i+1),Range(0,2)); H = H + mci.t() * dci; } cv::SVD svd(H); Mat R = svd.vt.t() * svd.u.t(); double det_R = cv::determinant(R); if(abs(det_R + 1.0) < 0.0001) { float _tmp[4] = {1,0,0,cv::determinant(svd.vt*svd.u)}; R = svd.u * Mat(2,2,CV_32FC1,_tmp) * svd.vt; } #ifdef BTM_DEBUG //for some strange reason the debug version of OpenCV is flipping the matrix R = -R; #endif float* _R = R.ptr<float>(0); Scalar T(d_bar[0] - (m_bar[0]*_R[0] + m_bar[1]*_R[1]),d_bar[1] - (m_bar[0]*_R[2] + m_bar[1]*_R[3])); m = m.reshape(1); m = m * R; m = m.reshape(2); m = m + T;// + m_bar; m.convertTo(_m,CV_32S); }

I really just followed what they said in the paper: take the mean point off the two sets, build a correlation matrix from the distances, do SVD and use U and Vt for computing the rotation and translation. It actually works!

## Putting it together

This is the easy part, just using these two functions and small while loop:

while(true) { pair.clear(); dists.clear(); double dist = flann_knn(destination, X, pair, dists); if(lastDist <= dist) { X = lastGood; break; //converged? } lastDist = dist; X.copyTo(lastGood); cout << "distance: " << dist << endl; Mat X_bar(X.size(),X.type()); for(int i=0;i<X.rows;i++) { Point p = destination.at<Point>(pair[i],0); X_bar.at<Point>(i,0) = p; } ShowQuery(destination,X,X_bar); X = X.reshape(2); X_bar = X_bar.reshape(2); findBestReansformSVD(X,X_bar); X = X.reshape(1); // back to 1-channel }

This will iteratively search for points and move the curve until the curve doesn’t move anymore.

## Some proof

## Code

Available in the SVN repo:

svn checkout https://morethantechnical.googlecode.com/svn/trunk/ICP ICP

Thanks for joining!

Roy.