两组3D点结算两坐标系的R和T

     每组最少三个点解两坐标系的R和T，采用论文《Least-Squares Fitting of Two 3-D Point Sets》的方法，参照nghiaho的博客：http://nghiaho.com/?page_id=671，及gihub的MATLAB代码，github地址为：https://github.com/nghiaho12/rigid_transform_3D，用C++及Eigen库实现了此功能（考虑了两组点尺度不同问题），记录一下。

#include <pangolin/pangolin.h>
#include <Eigen/Core>
#include <unistd.h>
#include<iostream>
#include<fstream>
#include<algorithm>
#include<chrono>
#include<opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <math.h>


using namespace std;
using namespace cv;
using namespace Eigen;

  

int main(int argc, char **argv) {

  //vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses;
   //Eigen::Quaterniond q;
    //MatrixXd t = MatrixXd::Random(3,1);
    Vector3d t = Vector3d::Random(),error = Vector3d::Random();
    Matrix3d R, R_estimate,V,U;


    R << -0.6208, 0.7763,-0.1091,
         -0.2820,-0.3509,-0.8929,
         -0.7315,-0.5236, 0.4368;
    //cout<<R.determinant()<<endl;
         
    int n = 10;
    //A: 10*3,  B: 3*10
    MatrixXd A = 100*MatrixXd::Random(n,3),A_move,B_move,A_norm,B_norm,lam2;
    MatrixXd B = 100*R*A.transpose()+(100*t).replicate(1,n)+error.replicate(1,n);
    
    Vector3d centroid_A = A.colwise().sum()/n;
    Vector3d centroid_B = B.rowwise().sum()/n;
    //cout<<"centroid_A = \n"<<centroid_A<<endl;
    //cout<<"centroid_B = \n"<<centroid_B<<endl;

    
    A_move = A.transpose() - centroid_A.replicate(1,n);
    B_move = B - centroid_B.replicate(1,n);
    A_norm = A_move.cwiseProduct(A_move).colwise().sum();
    B_norm = B_move.cwiseProduct(B_move).colwise().sum();
    //cout<<"A_norm.array():\n"<<A_norm.array()<<endl;
    lam2 = A_norm.cwiseQuotient(B_norm);
    double lam_2 = sqrt(lam2.mean());
    cout<<"lam_2:\n"<<1/lam_2<<endl;
    
    

    Matrix<double,3,3> H = (A.transpose()-centroid_A.replicate(1,n))*((lam_2*B-(lam_2*centroid_B).replicate(1,n)).transpose());
    //cout<<"H = "<<H<<endl;
    //SVD decomposition
    JacobiSVD<MatrixXd> svd(H, ComputeThinU | ComputeThinV);
    //cout<<"svd.matrixU():\n"<<svd.matrixU()<<endl;
    //cout<<"svd.matrixV():\n"<<svd.matrixV()<<endl;
    //cout<<"svd.singularValues():\n"<<svd.singularValues()<<endl;

    U = svd.matrixU();
    V = svd.matrixV();
    R_estimate = V*U.transpose();
    if(R_estimate.determinant()<0)
    {
        cout<<"Reflection detected\n"<<endl;
        V.col(2) = -1*V.col(2);
        R_estimate = V*U.transpose();
    }
    t = -R_estimate*centroid_A + lam_2 * centroid_B;
    
    Matrix<double,3,10> B2,err;
    double err_sum,rmse;
    
    B2 = R_estimate*A.transpose()+t.replicate(1,n);
    err = B2 -lam_2 *B;
    err =  err.cwiseProduct(err);
    err_sum = err.sum();
    rmse = sqrt(err_sum);
    cout<<"rmse:\n"<<rmse<<endl;
    return 0;
}

运行结果：