FindAffineTransform.c

https://github.com/opencv/opencv/blob/a66f61748fc4861374b8dc458866a9614925350a/modules/calib3d/src/ptsetreg.cpp#L1018

// From dlib 
template <typename T>
    point_transform_affine find_similarity_transform (
        const std::vector<dlib::vector<T,2> >& from_points,
        const std::vector<dlib::vector<T,2> >& to_points
    )
    {
        // make sure requires clause is not broken
        DLIB_ASSERT(from_points.size() == to_points.size() &&
                    from_points.size() >= 2,
            "\t point_transform_affine find_similarity_transform(from_points, to_points)"
            << "\n\t Invalid inputs were given to this function."
            << "\n\t from_points.size(): " << from_points.size()
            << "\n\t to_points.size():   " << to_points.size()
            );

        // We use the formulas from the paper: Least-squares estimation of transformation
        // parameters between two point patterns by Umeyama.  They are equations 34 through
        // 43.

        dlib::vector<double,2> mean_from, mean_to;
        double sigma_from = 0, sigma_to = 0;
        matrix<double,2,2> cov;
        cov = 0;

        for (unsigned long i = 0; i < from_points.size(); ++i)
        {
            mean_from += from_points[i];
            mean_to += to_points[i];
        }
        mean_from /= from_points.size();
        mean_to   /= from_points.size();

        for (unsigned long i = 0; i < from_points.size(); ++i)
        {
            sigma_from += length_squared(from_points[i] - mean_from);
            sigma_to += length_squared(to_points[i] - mean_to);
            cov += (to_points[i] - mean_to)*trans(from_points[i] - mean_from);
        }

        sigma_from /= from_points.size();
        sigma_to   /= from_points.size();
        cov        /= from_points.size();

        matrix<double,2,2> u, v, s, d;
        svd(cov, u,d,v);
        s = identity_matrix(cov);
        if (det(cov) < 0 || (det(cov) == 0 && det(u)*det(v)<0))
        {
            if (d(1,1) < d(0,0))
                s(1,1) = -1;
            else
                s(0,0) = -1;
        }

        matrix<double,2,2> r = u*s*trans(v);
        double c = 1; 
        if (sigma_from != 0)
            c = 1.0/sigma_from * trace(d*s);
        vector<double,2> t = mean_to - c*r*mean_from;

        return point_transform_affine(c*r, t);
    }
    
    





// find affine transformation between two pointsets (use least square matching)
static bool computeAffine(const vector<Point2d> &srcPoints, const vector<Point2d> &dstPoints, Mat &transf)
{
    // sanity check
    if ((srcPoints.size() < 3) || (srcPoints.size() != dstPoints.size()))
        return false;

    // container for output
    transf.create(2, 3, CV_64F);
    /* The output is a 2x3 matrix 
     * a    b    c
     * d    e    f 
     */

    // fill the matrices
    const int n = (int)srcPoints.size(),
    const int m = 3;
    
    Mat A(n,m,CV_64F); /* matrix of source points 
                        * x0 y0 1
                          x1 y1 1
                          x2 y2 1
                          x3 y3 1 
                           ... 
                           */ 
    Mat xc(n,1,CV_64F); /* Vectors of target points 
                            x0,
                            x1,
                            x2,
                            x3
                            ...*/
    Mat yc(n,1,CV_64F);
    
    for(int i=0; i<n; i++)
    {
        double x = srcPoints[i].x, 
        double y = srcPoints[i].y;
        
        double rowI[m] = {x, y, 1};
        Mat(1,m,CV_64F,rowI).copyTo(A.row(i));
        
        xc.at<double>(i,0) = dstPoints[i].x;
        yc.at<double>(i,0) = dstPoints[i].y;
    }

    // solve linear equations (for x and for y)
    Mat aTa, resX, resY;
    mulTransposed(A, aTa, true);
    solve(aTa, A.t()*xc, resX, DECOMP_CHOLESKY);
    solve(aTa, A.t()*yc, resY, DECOMP_CHOLESKY);

    // store result
    memcpy(transf.ptr<double>(0), resX.data, m*sizeof(double));
    memcpy(transf.ptr<double>(1), resY.data, m*sizeof(double));

    return true