Is there an easy way / algorithm to map two clouds of 2D points?

Question

I am wondering if there is an easy way to map (register) 2 clouds from 2d points.

Say I have an object represented by dots and a cluttered second image with object dots and noise (noise in the form of dots that are useless).

In principle, an object can be rotated 2d, and also translated and scaled.

I know there is an ICP algorithm, but I think this is not a good approach due to the high noise.

I hope you understand what I mean. please ask (if this is true), something is unclear.

amuses

Answer 1

Here is a function that finds translation and rotation. Scaling generalization, weighted points and RANSAC are straightforward. I used the openCV library for visualization and SVD. The function below combines data generation, Unit Test and the actual solution.

// rotation and translation in 2D from point correspondences void rigidTransform2D(const int N) { // Algorithm: http://igl.ethz.ch/projects/ARAP/svd_rot.pdf const bool debug = false; // print more debug info const bool add_noise = true; // add noise to imput and output srand(time(NULL)); // randomize each time /********************************* * Creat data with some noise **********************************/ // Simulated transformation Point2f T(1.0f, -2.0f); float a = 30.0; // [-180, 180], see atan2(y, x) float noise_level = 0.1f; cout<<"True parameters: rot = "<<a<<"deg., T = "<<T<< "; noise level = "<<noise_level<<endl; // noise vector<Point2f> noise_src(N), noise_dst(N); for (int i=0; i<N; i++) { noise_src[i] = Point2f(randf(noise_level), randf(noise_level)); noise_dst[i] = Point2f(randf(noise_level), randf(noise_level)); } // create data with noise vector<Point2f> src(N), dst(N); float Rdata = 10.0f; // radius of data float cosa = cos(a*DEG2RAD); float sina = sin(a*DEG2RAD); for (int i=0; i<N; i++) { // src float x1 = randf(Rdata); float y1 = randf(Rdata); src[i] = Point2f(x1,y1); if (add_noise) src[i] += noise_src[i]; // dst float x2 = x1*cosa - y1*sina; float y2 = x1*sina + y1*cosa; dst[i] = Point2f(x2,y2) + T; if (add_noise) dst[i] += noise_dst[i]; if (debug) cout<<i<<": "<<src[i]<<"---"<<dst[i]<<endl; } // Calculate data centroids Scalar centroid_src = mean(src); Scalar centroid_dst = mean(dst); Point2f center_src(centroid_src[0], centroid_src[1]); Point2f center_dst(centroid_dst[0], centroid_dst[1]); if (debug) cout<<"Centers: "<<center_src<<", "<<center_dst<<endl; /********************************* * Visualize data **********************************/ // Visualization namedWindow("data", 1); float w = 400, h = 400; Mat Mdata(w, h, CV_8UC3); Mdata = Scalar(0); Point2f center_img(w/2, h/2); float scl = 0.4*min(w/Rdata, h/Rdata); // compensate for noise scl/=sqrt(2); // compensate for rotation effect Point2f dT = (center_src+center_dst)*0.5; // compensate for translation for (int i=0; i<N; i++) { Point2f p1(scl*(src[i] - dT)); Point2f p2(scl*(dst[i] - dT)); // invert Y axis p1.y = -p1.y; p2.y = -p2.y; // add image center p1+=center_img; p2+=center_img; circle(Mdata, p1, 1, Scalar(0, 255, 0)); circle(Mdata, p2, 1, Scalar(0, 0, 255)); line(Mdata, p1, p2, Scalar(100, 100, 100)); } /********************************* * Get 2D rotation and translation **********************************/ markTime(); // subtract centroids from data for (int i=0; i<N; i++) { src[i] -= center_src; dst[i] -= center_dst; } // compute a covariance matrix float Cxx = 0.0, Cxy = 0.0, Cyx = 0.0, Cyy = 0.0; for (int i=0; i<N; i++) { Cxx += src[i].x*dst[i].x; Cxy += src[i].x*dst[i].y; Cyx += src[i].y*dst[i].x; Cyy += src[i].y*dst[i].y; } Mat Mcov = (Mat_<float>(2, 2)<<Cxx, Cxy, Cyx, Cyy); if (debug) cout<<"Covariance Matrix "<<Mcov<<endl; // SVD cv::SVD svd; svd = SVD(Mcov, SVD::FULL_UV); if (debug) { cout<<"U = "<<svd.u<<endl; cout<<"W = "<<svd.w<<endl; cout<<"V transposed = "<<svd.vt<<endl; } // rotation = V*Ut Mat V = svd.vt.t(); Mat Ut = svd.ut(); float det_VUt = determinant(V*Ut); Mat W = (Mat_<float>(2, 2)<<1.0, 0.0, 0.0, det_VUt); float rot[4]; Mat R_est(2, 2, CV_32F, rot); R_est = V*W*Ut; if (debug) cout<<"Rotation matrix: "<<R_est<<endl; float cos_est = rot[0]; float sin_est = rot[2]; float ang = atan2(sin_est, cos_est); // translation = mean_dst - R*mean_src Point2f center_srcRot = Point2f( cos_est*center_src.x - sin_est*center_src.y, sin_est*center_src.x + cos_est*center_src.y); Point2f T_est = center_dst - center_srcRot; // RMSE double RMSE = 0.0; for (int i=0; i<N; i++) { Point2f dst_est( cos_est*src[i].x - sin_est*src[i].y, sin_est*src[i].x + cos_est*src[i].y); RMSE += SQR(dst[i].x - dst_est.x) + SQR(dst[i].y - dst_est.y); } if (N>0) RMSE = sqrt(RMSE/N); // Final estimate msg cout<<"Estimate = "<<ang*RAD2DEG<<"deg., T = "<<T_est<<"; RMSE = "<<RMSE<<endl; // show image printTime(1); imshow("data", Mdata); waitKey(-1); return; } // rigidTransform2D() // --------------------------- 3DOF // calculates squared error from two point mapping; assumes rotation around Origin. inline float sqErr_3Dof(Point2f p1, Point2f p2, float cos_alpha, float sin_alpha, Point2f T) { float x2_est = Tx + cos_alpha * p1.x - sin_alpha * p1.y; float y2_est = Ty + sin_alpha * p1.x + cos_alpha * p1.y; Point2f p2_est(x2_est, y2_est); Point2f dp = p2_est-p2; float sq_er = dp.dot(dp); // squared distance //cout<<dp<<endl; return sq_er; } // calculate RMSE for point-to-point metrics float RMSE_3Dof(const vector<Point2f>& src, const vector<Point2f>& dst, const float* param, const bool* inliers, const Point2f center) { const bool all_inliers = (inliers==NULL); // handy when we run QUADRTATIC will all inliers unsigned int n = src.size(); assert(n>0 && n==dst.size()); float ang_rad = param[0]; Point2f T(param[1], param[2]); float cos_alpha = cos(ang_rad); float sin_alpha = sin(ang_rad); double RMSE = 0.0; int ninliers = 0; for (unsigned int i=0; i<n; i++) { if (all_inliers || inliers[i]) { RMSE += sqErr_3Dof(src[i]-center, dst[i]-center, cos_alpha, sin_alpha, T); ninliers++; } } //cout<<"RMSE = "<<RMSE<<endl; if (ninliers>0) return sqrt(RMSE/ninliers); else return LARGE_NUMBER; } // Sets inliers and returns their count inline int setInliers3Dof(const vector<Point2f>& src, const vector <Point2f>& dst, bool* inliers, const float* param, const float max_er, const Point2f center) { float ang_rad = param[0]; Point2f T(param[1], param[2]); // set inliers unsigned int ninliers = 0; unsigned int n = src.size(); assert(n>0 && n==dst.size()); float cos_ang = cos(ang_rad); float sin_ang = sin(ang_rad); float max_sqErr = max_er*max_er; // comparing squared values if (inliers==NULL) { // just get the number of inliers (eg after QUADRATIC fit only) for (unsigned int i=0; i<n; i++) { float sqErr = sqErr_3Dof(src[i]-center, dst[i]-center, cos_ang, sin_ang, T); if ( sqErr < max_sqErr) ninliers++; } } else { // get the number of inliers and set them (eg for RANSAC) for (unsigned int i=0; i<n; i++) { float sqErr = sqErr_3Dof(src[i]-center, dst[i]-center, cos_ang, sin_ang, T); if ( sqErr < max_sqErr) { inliers[i] = 1; ninliers++; } else { inliers[i] = 0; } } } return ninliers; } // fits 3DOF (rotation and translation in 2D) with least squares. float fit3DofQUADRATICold(const vector<Point2f>& src, const vector<Point2f>& dst, float* param, const bool* inliers, const Point2f center) { const bool all_inliers = (inliers==NULL); // handy when we run QUADRTATIC will all inliers unsigned int n = src.size(); assert(dst.size() == n); // count inliers int ninliers; if (all_inliers) { ninliers = n; } else { ninliers = 0; for (unsigned int i=0; i<n; i++){ if (inliers[i]) ninliers++; } } // under-dermined system if (ninliers<2) { // param[0] = 0.0f; // ? // param[1] = 0.0f; // param[2] = 0.0f; return LARGE_NUMBER; } /* * x1*cosx(a)-y1*sin(a) + Tx = X1 * x1*sin(a)+y1*cos(a) + Ty = Y1 * * approximation for small angle a (radians) sin(a)=a, cos(a)=1; * * x1*1 - y1*a + Tx = X1 * x1*a + y1*1 + Ty = Y1 * * in matrix form M1*h=M2 * * 2n x 4 4 x 1 2n x 1 * * -y1 1 0 x1 * a = X1 * x1 0 1 y1 Tx Y1 * Ty * 1=Z * ---------------------------- * src1 res src2 */ // 4 x 1 float res_ar[4]; // alpha, Tx, Ty, 1 Mat res(4, 1, CV_32F, res_ar); // 4 x 1 // 2n x 4 Mat src1(2*ninliers, 4, CV_32F); // 2n x 4 // 2n x 1 Mat src2(2*ninliers, 1, CV_32F); // 2n x 1: [X1, Y1, X2, Y2, X3, Y3]' for (unsigned int i=0, row_cnt = 0; i<n; i++) { // use inliers only if (all_inliers || inliers[i]) { float x = src[i].x - center.x; float y = src[i].y - center.y; // first row // src1 float* rowPtr = src1.ptr<float>(row_cnt); rowPtr[0] = -y; rowPtr[1] = 1.0f; rowPtr[2] = 0.0f; rowPtr[3] = x; // src2 src2.at<float> (0, row_cnt) = dst[i].x - center.x; // second row row_cnt++; // src1 rowPtr = src1.ptr<float>(row_cnt); rowPtr[0] = x; rowPtr[1] = 0.0f; rowPtr[2] = 1.0f; rowPtr[3] = y; // src2 src2.at<float> (0, row_cnt) = dst[i].y - center.y; } } cv::solve(src1, src2, res, DECOMP_SVD); // estimators float alpha_est; Point2f T_est; // original alpha_est = res.at<float>(0, 0); T_est = Point2f(res.at<float>(1, 0), res.at<float>(2, 0)); float Z = res.at<float>(3, 0); if (abs(Z-1.0) > 0.1) { //cout<<"Bad Z in fit3DOF(), Z should be close to 1.0 = "<<Z<<endl; //return LARGE_NUMBER; } param[0] = alpha_est; // rad param[1] = T_est.x; param[2] = T_est.y; // calculate RMSE float RMSE = RMSE_3Dof(src, dst, param, inliers, center); return RMSE; } // fit3DofQUADRATICOLd() // fits 3DOF (rotation and translation in 2D) with least squares. float fit3DofQUADRATIC(const vector<Point2f>& src_, const vector<Point2f>& dst_, float* param, const bool* inliers, const Point2f center) { const bool debug = false; // print more debug info const bool all_inliers = (inliers==NULL); // handy when we run QUADRTATIC will all inliers assert(dst_.size() == src_.size()); int N = src_.size(); // collect inliers vector<Point2f> src, dst; int ninliers; if (all_inliers) { ninliers = N; src = src_; // copy constructor dst = dst_; } else { ninliers = 0; for (int i=0; i<N; i++){ if (inliers[i]) { ninliers++; src.push_back(src_[i]); dst.push_back(dst_[i]); } } } if (ninliers<2) { param[0] = 0.0f; // default return when there is not enough points param[1] = 0.0f; param[2] = 0.0f; return LARGE_NUMBER; } /* Algorithm: Least-Square Rigid Motion Using SVD by Olga Sorkine * http://igl.ethz.ch/projects/ARAP/svd_rot.pdf * * Subtract centroids, calculate SVD(cov), * R = V[1, det(VU')]'U', T = mean_q-R*mean_p */ // Calculate data centroids Scalar centroid_src = mean(src); Scalar centroid_dst = mean(dst); Point2f center_src(centroid_src[0], centroid_src[1]); Point2f center_dst(centroid_dst[0], centroid_dst[1]); if (debug) cout<<"Centers: "<<center_src<<", "<<center_dst<<endl; // subtract centroids from data for (int i=0; i<ninliers; i++) { src[i] -= center_src; dst[i] -= center_dst; } // compute a covariance matrix float Cxx = 0.0, Cxy = 0.0, Cyx = 0.0, Cyy = 0.0; for (int i=0; i<ninliers; i++) { Cxx += src[i].x*dst[i].x; Cxy += src[i].x*dst[i].y; Cyx += src[i].y*dst[i].x; Cyy += src[i].y*dst[i].y; } Mat Mcov = (Mat_<float>(2, 2)<<Cxx, Cxy, Cyx, Cyy); Mcov /= (ninliers-1); if (debug) cout<<"Covariance-like Matrix "<<Mcov<<endl; // SVD of covariance cv::SVD svd; svd = SVD(Mcov, SVD::FULL_UV); if (debug) { cout<<"U = "<<svd.u<<endl; cout<<"W = "<<svd.w<<endl; cout<<"V transposed = "<<svd.vt<<endl; } // rotation (V*Ut) Mat V = svd.vt.t(); Mat Ut = svd.ut(); float det_VUt = determinant(V*Ut); Mat W = (Mat_<float>(2, 2)<<1.0, 0.0, 0.0, det_VUt); float rot[4]; Mat R_est(2, 2, CV_32F, rot); R_est = V*W*Ut; if (debug) cout<<"Rotation matrix: "<<R_est<<endl; float cos_est = rot[0]; float sin_est = rot[2]; float ang = atan2(sin_est, cos_est); // translation (mean_dst - R*mean_src) Point2f center_srcRot = Point2f( cos_est*center_src.x - sin_est*center_src.y, sin_est*center_src.x + cos_est*center_src.y); Point2f T_est = center_dst - center_srcRot; // Final estimate msg if (debug) cout<<"Estimate = "<<ang*RAD2DEG<<"deg., T = "<<T_est<<endl; param[0] = ang; // rad param[1] = T_est.x; param[2] = T_est.y; // calculate RMSE float RMSE = RMSE_3Dof(src_, dst_, param, inliers, center); return RMSE; } // fit3DofQUADRATIC() // RANSAC fit in 3DOF: 1D rot and 2D translation (maximizes the number of inliers) // NOTE: no data normalization is currently performed float fit3DofRANSAC(const vector<Point2f>& src, const vector<Point2f>& dst, float* best_param, bool* inliers, const Point2f center , const float inlierMaxEr, const int niter) { const int ITERATION_TO_SETTLE = 2; // iterations to settle inliers and param const float INLIERS_RATIO_OK = 0.95f; // stopping criterion // size of data vector unsigned int N = src.size(); assert(N==dst.size()); // unrealistic case if(N<2) { best_param[0] = 0.0f; // ? best_param[1] = 0.0f; best_param[2] = 0.0f; return LARGE_NUMBER; } unsigned int ninliers; // current number of inliers unsigned int best_ninliers = 0; // number of inliers float best_rmse = LARGE_NUMBER; // error float cur_rmse; // current distance error float param[3]; // rad, Tx, Ty vector <Point2f> src_2pt(2), dst_2pt(2);// min set of 2 points (1 correspondence generates 2 equations) srand (time(NULL)); // iterations for (int iter = 0; iter<niter; iter++) { #ifdef DEBUG_RANSAC cout<<"iteration "<<iter<<": "; #endif // 1. Select a random set of 2 points (not obligatory inliers but valid) int i1, i2; i1 = rand() % N; // [0, N[ i2 = i1; while (i2==i1) { i2 = rand() % N; } src_2pt[0] = src[i1]; // corresponding points src_2pt[1] = src[i2]; dst_2pt[0] = dst[i1]; dst_2pt[1] = dst[i2]; bool two_inliers[] = {true, true}; // 2. Quadratic fit for 2 points cur_rmse = fit3DofQUADRATIC(src_2pt, dst_2pt, param, two_inliers, center); // 3. Recalculate to settle params and inliers using a larger set for (int iter2=0; iter2<ITERATION_TO_SETTLE; iter2++) { ninliers = setInliers3Dof(src, dst, inliers, param, inlierMaxEr, center); // changes inliers cur_rmse = fit3DofQUADRATIC(src, dst, param, inliers, center); // changes cur_param } // potential ill-condition or large error if (ninliers<2) { #ifdef DEBUG_RANSAC cout<<" !!! less than 2 inliers "<<endl; #endif continue; } else { #ifdef DEBUG_RANSAC cout<<" "<<ninliers<<" inliers; "; #endif } #ifdef DEBUG_RANSAC cout<<"; recalculate: RMSE = "<<cur_rmse<<", "<<ninliers <<" inliers"; #endif // 4. found a better solution? if (ninliers > best_ninliers) { best_ninliers = ninliers; best_param[0] = param[0]; best_param[1] = param[1]; best_param[2] = param[2]; best_rmse = cur_rmse; #ifdef DEBUG_RANSAC cout<<" --- Solution improved: "<< best_param[0]<<", "<<best_param[1]<<", "<<param[2]<<endl; #endif // exit condition float inlier_ratio = (float)best_ninliers/N; if (inlier_ratio > INLIERS_RATIO_OK) { #ifdef DEBUG_RANSAC cout<<"Breaking early after "<< iter+1<< " iterations; inlier ratio = "<<inlier_ratio<<endl; #endif break; } } else { #ifdef DEBUG_RANSAC cout<<endl; #endif } } // iterations // 5. recreate inliers for the best parameters ninliers = setInliers3Dof(src, dst, inliers, best_param, inlierMaxEr, center); return best_rmse; } // fit3DofRANSAC() 

Answer 2

Let me first make sure that I am interpreting your question correctly. You have two sets of two-dimensional points, one of which contains all the “good” points corresponding to some object of interest, and one of them contains those points under the affine transformation with added points. Correctly?

If this is correct, then there is a fairly reliable and effective way to both reject the noisy points and determine the transformation between your points of interest. The algorithm that is commonly used to deflect noisy points (“peaks”) is called RANSAC , and the algorithm used to determine the transform can take several forms, but the most recent art is known as the five-point algorithm and can be found here - the MATLAB implementation can be found here .

Unfortunately, I do not know about the mature implementation of both of them; you may have to do some work to implement RANSAC and integrate it with the five-point algorithm.

Edit:

In fact, OpenCV has an implementation that is redundant for your task (which means that it will work, but will take more time than necessary), but it is ready to work out of the box. The function of interest is called cv :: findFundamentalMat .

Answer 3

I believe that you are looking for something like David Lowe's SIFT (Large-Scale Invariant Object Conversion). Another option is SURF (SIFT is patent protected). OpenCV Computer Library Introduces SURF Implementation

Answer 4

I would try using distance geometry (http://en.wikipedia.org/wiki/Distance_geometry) for this

Create a scalar for each point, summing its distances to all neighbors within a certain radius. Although not ideal, it will be a good discriminator for every point.

Then put all the scalars in a map that allows you to extract point (p) from its scalar (s) plus / minus some delta
M (s + delta) = p (e.g. KD tree) (http://en.wikipedia.org/wiki/Kd-tree)

Place the entire reference set of two-dimensional points on the map

On the other (test) set of two-dimensional points:
foreach test scaling (esp if you have a good idea what are the typical scaling values)
... scale each point to S
... recalculate scalars of a test set of points
...... for each point P in the test case (or perhaps a sample for a faster method)
......... search point in the reference scalar map in some delta
......... discard P if the mapping is not found
......... else foreach Point P 'found
............ consider the neighbors P and see if they have the corresponding scalars in the control map in some delta (for example, the reference point has neighbors with approximately the same value)
......... if all the tested points have a comparison in the control set, you find the comparison of the test point P with the reference point P '-> map of the map of the control point to the control point ...... refuse to scale if no recordings

Note that this is trivially parallelized in several different places.

This is not in my head, based on research that I did many years ago. It lacks subtle details, but the general idea is clear: to find points in a noisy (test) graph, the distances to the nearest neighbors are about the same as the control set. Noisy graphs will have to measure distances with a larger margin of error, which is less noisy graphs.

The algorithm works great for noise-free graphs.

Edit : There is a refinement for an algorithm that does not require viewing various scaling. When calculating a scalar for each point, use a relative measure instead. This will be an invariant transformation

Answer 5

From C ++, you can use ITK to register an image. It includes many registration functions that will work in the presence of noise.

Answer 6

KLT (Kanade Lucas Tomasi) Feature Tracker makes Affine Consistency Check for tracked features. The Affin Consistency Check includes translation, rotation, and scaling. I don’t know if this will help you because you cannot directly use the function (which calculates the affine transformation of the rectangular area). But perhaps you can learn from the documentation and the source code how you can calculate the affine transformation and adapt it to your problem (instead of clouds of points in a rectangular area).

Answer 7

You want a Denton-Beveridge dot matching algorithm. The source code at the bottom of the page below, as well as a document that explains the algorithm and why Ransac is a poor choice for this problem.

http://jasondenton.me/pntmatch.html