Mat_<double> Find2DRigidTransform(const vector<Point_<V> >& a, const vector<Point_<V> >& b,
                              Point_<V>* diff = 0, V* angle = 0, V* scale = 0) {
//use PCA to find relational scale
Mat_<V> P; Mat(a).reshape(1,a.size()).copyTo(P);
Mat_<V> Q; Mat(b).reshape(1,b.size()).copyTo(Q);
PCA a_pca(P,Mat(),CV_PCA_DATA_AS_ROW), b_pca(Q,Mat(),CV_PCA_DATA_AS_ROW);
double s = sqrt(b_pca.eigenvalues.at<V>(0)) / sqrt(a_pca.eigenvalues.at<V>(0));
//  cout << a_pca.eigenvectors << endl << a_pca.eigenvalues << endl << a_pca.mean << endl;
//  cout << b_pca.eigenvectors << endl << b_pca.eigenvalues << endl << b_pca.mean << endl;

//convert to matrices and subtract mean

Scalar a_m = Scalar(a_pca.mean.at<V>(0),a_pca.mean.at<V>(1));
Scalar b_m = Scalar(b_pca.mean.at<V>(0),b_pca.mean.at<V>(1));

P -= repeat((Mat_<V>(1,2) << a_m[0],a_m[1]), P.rows, 1);
Q -= repeat((Mat_<V>(1,2) << b_m[0],b_m[1]), Q.rows, 1);

//from http://en.wikipedia.org/wiki/Kabsch_algorithm
Mat_<double> A = P.t() * Q;
SVD svd(A);
Mat_<double> C = svd.vt.t() * svd.u.t();
double d = (determinant(C) > 0) ? 1 : -1;
Mat_<double> R = svd.vt.t() * (Mat_<double>(2,2) << 1,0, 0,d) * svd.u.t();
Mat_<double> T = (Mat_<double>(3,3) << 1,0,b_m[0]/s, 0,1,b_m[1]/s, 0,0,1) *
                (Mat_<double>(3,3) << s,0,0, 0,s,0, 0,0,s) *
                (Mat_<double>(3,3) << R(0,0),R(0,1),0, R(1,0),R(1,1),0, 0,0,1) *
                (Mat_<double>(3,3) << 1,0,-a_m[0], 0,1,-a_m[1], 0,0,1)
                ;
if (diff!=NULL) {
    diff->x = b_m[0]-a_m[0];
    diff->y = b_m[1]-a_m[1];
}
if (angle!=NULL) {
    *angle = atan2(R(1,0),R(0,0));
}
if (scale!=NULL) {
    *scale = s;
}
return T(Range(0,2),Range::all());