«

»

Jun 06

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.

Share
  • Duy

    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

  • http://www.morethantechnical.com Roy

    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.

  • Duy

    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 )

  • http://www.morethantechnical.com Roy

    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.

  • Duy

    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

  • Duy

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

  • http://www.morethantechnical.com Roy

    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.

  • jv

    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,

  • http://www.morethantechnical.com Roy

    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 2x2 rotation matrix, and T is a 1x2 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

  • shleebot

    good job Roy thanks for publishing your code.

  • Hung

    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

  • Zo

    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

  • mb

    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.

  • Richard

    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 =]

  • http://pkmital.com Parag

    So incredibly helpful thank you for your time in sharing this.

  • http://robocraft.ru noonv

    Maybe some example of usage? :)

  • Pol

    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 ?

  • http://www.morethantechnical.com Roy

    @Pol
    Actually, what I like to use for 3D ICP these days is libPCL
    It's an OpenCV sister project, and they rock just as hard... :)
    Check out:
    http://pointclouds.org/documentation/tutorials/iterative_closest_point.php#iterative-closest-point

    All the algorithms are built in... just plug into your code.

    R.

  • RonaldPan

    I do not understand why m = m * R;
    According to the paper, it should be R * m

    Thanks.

  • Matt

    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

  • http://www.morethantechnical.com Roy

    @Matt
    I'm referring you to PCL (here)
    they have a bunch of ICP algorithms packed in a very easy API
    although they focus on 3D, you can easily use it for 2D cases
    Check out
    http://docs.pointclouds.org/trunk/group__registration.html
    http://pointclouds.org/documentation/tutorials/iterative_closest_point.php

  • Matt

    Thanks, I will check it out!
    Cheers

  • Guang

    Could this code be applied to two sets of points with different scales?

  • http://www.morethantechnical.com Roy
  • akm sabbir

    u r cool Roy. i hope u will rock the MIT.

  • Temp

    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 ...

  • sharmila

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

  • Heymatite

    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!

  • david

    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

  • Pingback: contour matching | DL-UAT()