Replacement for estimateRigidTransform using GSL instead of OpenCV

getTransform.cpp

// call in place of something like: Mat warp_mat = estimateRigidTransform( f, c, false);
// like this: Mat warp_mat = getSimilarityTransform(f,c);
// then you can use it with warpAffine 

// needs GSL http://www.gnu.org/software/gsl/
// #include <gsl/gsl_linalg.h>
// #include <gsl/gsl_blas.h>

Mat getTransform(vector<Point2f> src, vector<Point2f> dst){
    // use GSL to do the actual transformations.... but put it back into opencv-happy format
    
    Mat warp_mat( 2, 3, CV_64FC1, Scalar::all(0));

    if (src.size() != dst.size()){
        printf("Error: vectors not the same size\n");
        return warp_mat;
    }

    int num_pairs = src.size();
    printf("finding similarity based on [%d] pairs of points\n", num_pairs);

    gsl_matrix *m_gsl_src   = gsl_matrix_calloc(3, num_pairs);
    gsl_matrix *m_gsl_dst  = gsl_matrix_calloc(3, num_pairs);

    for (int i = 0; i < num_pairs; i++){
        gsl_matrix_set(m_gsl_src, 0, i, src[i].x);
        gsl_matrix_set(m_gsl_src, 1, i, src[i].y);
        gsl_matrix_set(m_gsl_src, 2, i, 1);
        gsl_matrix_set(m_gsl_dst, 0, i, dst[i].x);
        gsl_matrix_set(m_gsl_dst, 1, i, dst[i].y);
        gsl_matrix_set(m_gsl_dst, 2, i, 1);
    }

    gsl_matrix *A = gsl_matrix_alloc(3,3);
    gsl_matrix *B = gsl_matrix_alloc(3,3);
    
    // goal: solve for transform
    // transform * src = dst
    // transform = dst * src' * inv(src * src')
    

    //part 1: A = dst * src'
    gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, m_gsl_dst, m_gsl_src, 0.0, A);

    //part 2: B = src * src'
    gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1, m_gsl_src, m_gsl_src, 0.0, B);

    //part 3: invert B...
    gsl_matrix *inv = gsl_matrix_alloc(3,3);
    gsl_permutation *p = gsl_permutation_alloc(3);
    int s;
    gsl_linalg_LU_decomp(B, p, &s);
    gsl_linalg_LU_invert(B, p, inv);

    //part 4: multiply A and B^(-1)
    gsl_matrix *m_gsl_transform = gsl_matrix_calloc(3,3);
    gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, A, inv, 0.0, m_gsl_transform);

    //set warp_map 
    printf("The transform is...\n");
    for (int i = 0; i < 2; i++){
        for (int j = 0; j < 3; j++){
            printf("%.10f ", gsl_matrix_get(m_gsl_transform, i, j));
            warp_mat.at<double>(i, j) =  gsl_matrix_get(m_gsl_transform, i, j);
        }
        printf("\n");
    }

    return warp_mat;
}