estimate rigid transform from two sets of points, a simplified version for cv::estimateRigidTransfrom

get_affine_transform.cpp

#include <opencv2/core/core.hpp>

static cv::Mat get_affine_transform(const cv::Point2f* a, const cv::Point2f* b, int count)
{
    double sa[4][4]={{0.}};
    double sb[4]={0.};
    double m[4];
    cv::Mat A( 4, 4, CV_64F, sa );
    cv::Mat B( 4, 1, CV_64F, sb );
    cv::Mat MM( 4, 1, CV_64F, m );

    for (int i=0; i<count; i++)
    {
        sa[0][0] += a[i].x*a[i].x + a[i].y*a[i].y;
        sa[0][2] += a[i].x;
        sa[0][3] += a[i].y;

        sb[0] += a[i].x*b[i].x + a[i].y*b[i].y;
        sb[1] += a[i].x*b[i].y - a[i].y*b[i].x;
        sb[2] += b[i].x;
        sb[3] += b[i].y;
    }

    sa[1][1] = sa[0][0];
    sa[2][1] = sa[1][2] = -sa[0][3];
    sa[3][1] = sa[1][3] = sa[2][0] = sa[0][2];
    sa[2][2] = sa[3][3] = count;
    sa[3][0] = sa[0][3];

//     cv::solve(A, B, MM, cv::DECOMP_EIG);
    cv::solve(A, B, MM, cv::DECOMP_SVD);

    cv::Mat M(2, 3, CV_64F);
    double* om = M.ptr<double>();
    om[0] = om[4] = m[0];
    om[1] = -m[1];
    om[3] = m[1];
    om[2] = m[2];
    om[5] = m[3];

    return M;
}