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.