c++ - Is there an easy way/algorithm to match 2 clouds of 2D points?

Question

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

Let's say I have an object represented by points and an cluttered 2nd image with the object points and noise (noise in a way of points that are useless).

Basically the object can be 2d rotated as well as translated and scaled.

I know there is the ICP - Algorithm but I think that this is not a good approach due to high noise.

I hope that you understand what i mean. please ask if (im sure it is) anything is unclear.

cheers

Answer 1

Here is the function that finds translation and rotation. Generalization to scaling, weighted points, and RANSAC are straight forward. I used openCV library for visualization and SVD. The function below combines data generation, Unit Test , and 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.u.t();
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 = T.x + cos_alpha * p1.x - sin_alpha * p1.y;
    float y2_est = T.y + 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 (e.g. 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 (e.g. 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]