Categories
code graphics opencv programming Solutions video vision

Iterative Closest Point (ICP) for 2D curves with OpenCV [w/ code]

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

  1. Find the points which are closest to our curve
  2. Compute the best rotation and translation between our points and the closest points
  3. Move our points according to the found transformation, and check the error
  4. 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.

30 replies on “Iterative Closest Point (ICP) for 2D curves with OpenCV [w/ code]”

Hello Roy, i just check your source code but it seems like there are som error. For example, “using namespace cv”, there is not such namespace does exist. I suppose you used other way to define that things. If in that case, can you show me how to do that? Thanks

Hi Duy
The problem could be that you use an old version of OpenCV (before 2.X) which doesn’t have namespacing, or that you don’t include the correct files from the correct directory.
Since the #include directives are probably right, I’ll bet you just have to set up the project to take include files from the OpenCV include directory (e.g. “D:\opencv-2.1\include\”).
Roy.

Hi Roy
I download and set up OpenCV 2.0 in my desktop and run your code again. Here are those errors i got.
Error 15 error C2064: term does not evaluate to a function taking 0 arguments 15
Error 16 error C2064: term does not evaluate to a function taking 0 arguments 15
Error 17 error C2039: ‘at’ : is not a member of ‘CvMat’ 16
Error 18 error C2064: term does not evaluate to a function taking 0 arguments 17
Error 19 error C2064: term does not evaluate to a function taking 0 arguments 17
Error 20 error C2039: ‘at’ : is not a member of ‘CvMat’ 18
Error 23 error C2664: ‘cv::circle’ : cannot convert parameter 1 from ‘CvMat’ to ‘cv::Mat &’ 21
Error 28 error C2664: ‘ShowPoints’ : cannot convert parameter 1 from ‘cv::Mat’ to ‘CvMat &’ 61
Error 29 error C2678: binary ‘-‘ : no operator found which takes a left-hand operand of type ‘cv::Mat’ (or there is no acceptable conversion) 77
Error 30 error C2678: binary ‘-‘ : no operator found which takes a left-hand operand of type ‘cv::Mat’ (or there is no acceptable conversion) 78
Error 32 error C2678: binary ‘+’ : no operator found which takes a left-hand operand of type ‘cv::Mat’ (or there is no acceptable conversion) 107
Error 33 error C2660: ‘cv::namedWindow’ : function does not take 1 arguments 120
Error 34 error C2678: binary ‘-‘ : no operator found which takes a left-hand operand of type ‘cv::Mat’ (or there is no acceptable conversion) 131
Error 35 error C2678: binary ‘-‘ : no operator found which takes a left-hand operand of type ‘cv::Mat’ (or there is no acceptable conversion) 132
Error 36 error C2678: binary ‘+’ : no operator found which takes a left-hand operand of type ‘cv::Mat’ (or there is no acceptable conversion) 173
Error 37 error C2678: binary ‘+’ : no operator found which takes a left-hand operand of type ‘cv::Mat’ (or there is no acceptable conversion) 174
Error 40 error C2589: ‘(‘ : illegal token on right side of ‘::’ 212
Error 41 error C2059: syntax error : ‘::’ 212
Do you have any idea to solve these problem? ( I’m kind of newbie with OpenCV )

Hi Duy
It looks like your are mixing old OpenCV C API (CvMat) with new C++ API (cv::Mat), and the function definitions are not compatible…
See that you are using all cv::Mat and not CvMat.
Re the binary operator errors, please show the lines, else I can’t help much.
Roy.

Hi Roy.
You’re right about the CvMat stuff. Regarding the binary operator errors, the lines was shown at the end of each errors
error C2678: binary ‘-‘ : no operator found which takes a left-hand operand of type ‘cv::Mat’ 77
error C2678: binary ‘-‘ : no operator found which takes a left-hand operand of type ‘cv::Mat’ (or there is no acceptable conversion) 78
error C2678: binary ‘+’ : no operator found which takes a left-hand operand of type ‘cv::Mat’ (or there is no acceptable conversion) 107
error C2660: ‘cv::namedWindow’ : function does not take 1 arguments 120
error C2678: binary ‘-‘ : no operator found which takes a left-hand operand of type ‘cv::Mat’ (or there is no acceptable conversion) 131
error C2678: binary ‘-‘ : no operator found which takes a left-hand operand of type ‘cv::Mat’ (or there is no acceptable conversion) 132
error C2678: binary ‘+’ : no operator found which takes a left-hand operand of type ‘cv::Mat’ (or there is no acceptable conversion) 173
error C2678: binary ‘+’ : no operator found which takes a left-hand operand of type ‘cv::Mat’ (or there is no acceptable conversion) 174
error C2589: ‘(‘ : illegal token on right side of ‘::’ 212
error C2059: syntax error : ‘::’ 212

By the way, i’m using Microsof Visual Studio 2005. I also keep the same order of your source

This is very strange as there are ‘+’ and ‘-‘ binary operator for cv::Mat…
Maybe you should use OpenCV v2.1, which is the latest.
This sounds like an includes problem. See that you have “#include “, “#include ” and “using namespace cv;”
And your project is searching in the right directories for the files
Roy.

hi,
thanks you for the code,
what is the dimensions of the matrices you are working with?
how many rows, how many columns?
and what do each row/column represent?
thanks,

Hi
I guess you mean the findTheBestTransform part…
Anyway, Mat m and Mat d are the 2D points matrices of size Nx2 (N rows, 2 columns for the x and y coordinates) and Mx2.
The H matrix is NxM, and is a “correlation matrix”.
The R matrix is a 2×2 rotation matrix, and T is a 1×2 translation vector.
Later, X is again the working (moving) points matrix and “destination” are the blue points – the points you wish the curve to snap to.
Hope it helps
Roy

Hi Roy.
When i tried to compile your source code. I got many errors as following:
error C2678: binary ‘-‘ : no operator found which takes a left-hand operand of type ‘cv::Mat’ (or there is no acceptable conversion)
It appears in the code line
Mat mc = m – m_bar;// in the compute transform function
It seems like the complier cannot convert from cvScalar to cvMat.
Do you know how to solve this problem ?. I’m using VS 2005 and openCV 2.1

Hi Roy,
Nice work, thanks!
What if I have a set of selected points in the point cloud and I want to find exactly the same points even if the cloud has moved (and not a set of points with similar shape at the original pixel coordinates)?
Say,
– select four points in the cloud above
– move the cloud including the selected points (affine transformation)
– find the same points above
I tried to apply your software to this problem, but I always get another point set which has similar shape at its original pixel location. I assume this is because the nearest neighbour distances are minimal for this set.
Do you know any solution to this problem?
Thanks,
Zo

You might want to use something like “Mat H = zeros(2,2,CV_32FC1);”. Else there’s no guarantee for it being filled with zeros. At least I don’t know of any default initialization for opencv matrices.

Hi I am trying to implement this from first principles at the moment, just about got to grips with SVD, could you explain a little more about how you generated the correlation matrix? I am trying to do this from a set of 2D Cartesian points.
Thanks =]

How to transform 2D -> 3D IPC ?
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] ) );
Only this line will be problem ?

Hey Roy,
first thank u for this Tutorial it was very helpful for me to get into the ICP stuff. However, using the SVD version, I get worse results than with the standart approach. Is this possible, or may there be a bug somewhere in the function?
Cheers,
Matt

In findBestReansformSVD(Mat& _m, Mat& _d), please correct :
Mat H(2,2,CV_32FC1);
to
Mat H(2,2,CV_32FC1, Scalar(0)); : this creates the ‘flipped’ matrix in debug. In both Release and Debug you have undefined behavior if you do not initialize the H values as you can have bogus data to begin with …

hi am working on opencv image processing. Can you say any idea of converting a image file to hex(.dat) format,

Hi Roy,
Thanks for the tuto.
I adapted it a little for my purpose. I have sets of 1000 points to match. The results are repeatable, but :
if I run one set alone, I dont have the same result as if I run all the sets together (one after the other). The first pass is the same, but not the second, when I have new points for the sets.
I tried to replace floats by double, it improved a little the results and decreased the difference between the two type of test conditions, but not entirely and my results are still false.
The error is in flann_knn function:
flann::Index flann_index(dest_64f, flann::KDTreeIndexParams(2)); // using 4 randomized kdtrees
flann_index.knnSearch(obj_32f, m_indices, m_dists, 1, flann::SearchParams(64) );
The m_dists are différents for the same set!
I don’t know what to do now…
Thanks for help!

Hi,
I’m trying to compile your code but unfortunately i get an error.
The following:
icp.cpp:187:113: error: could not convert ‘std::vector()’ from ‘std::vector’ to ‘std::vector&’
float flann_knn(Mat& m_destinations, Mat& m_object, vector& ptpairs, vector& dists = vector()) {
Is this a compiler problem or did i forget something?
David

Leave a Reply

Your email address will not be published. Required fields are marked *