void PointCloudProcessing::getRTGeometricLinearSystem(PointCloudC::Ptr corrRef, PointCloudC::Ptr corrNew, Eigen::Matrix4f &transMat) { // build linear system Ax = b; int rows = 3*corrRef->points.size(); int cols = 6; Eigen::MatrixXf A(rows, cols); A.setZero(); Eigen::MatrixXf b(rows, 1); b.setZero(); for(int i=0; i<corrRef->points.size(); i++) { float X1 = corrRef->points.at(i).x; float Y1 = corrRef->points.at(i).y; float Z1 = corrRef->points.at(i).z; float X0 = corrNew->points.at(i).x; float Y0 = corrNew->points.at(i).y; float Z0 = corrNew->points.at(i).z; A(3*i, 0) = 0; A(3*i, 1) = Z0+Z1; A(3*i, 2) = -Y0-Y1; A(3*i, 3) = 1; A(3*i+1, 0) = -Z0-Z1; A(3*i+1, 1) = 0; A(3*i+1, 2) = X0+X1; A(3*i+1, 4) = 1; A(3*i+2, 0) = Y0+Y1; A(3*i+1, 1) = -X0-X1; A(3*i+2, 2) = 0; A(3*i+2, 5) = 1; b(3*i, 0) = X1-X0; b(3*i+1, 0) = Y1-Y0; b(3*i+2, 0) = Z1-Z0; } // std::cout<<"A = "<<A<<std::endl; // std::cout<<"b = "<<b<<std::endl; Eigen::MatrixXf solveX(rows, 1); solveX.setZero(); solveX = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b); // std::cout<<"solveX: "<<solveX<<"\n"; // solveX = [Rx, Ry, Rz, T'x, T'y, T'z]; Eigen::Matrix3f I_Vx; I_Vx.setOnes(); I_Vx(0,0) = 1; I_Vx(0,1) = solveX(2); I_Vx(0,2) = -solveX(1); I_Vx(1,0) = -solveX(2); I_Vx(1,1) = 1; I_Vx(1,2) = solveX(0); I_Vx(2,0) = solveX(1); I_Vx(2,1) = -solveX(0); I_Vx(2,2) = 1; Eigen::Vector3f Tx; Tx.setZero(); Tx(0) = solveX(3); Tx(1) = solveX(4); Tx(2) = solveX(5); Eigen::Vector3f tx; tx.setZero(); tx = I_Vx.inverse()*Tx; transMat(0,3) = tx(0); transMat(1,3) = tx(1); transMat(2,3) = tx(2); Eigen::Matrix3f IplusVx; IplusVx.setOnes(); IplusVx(0,0) = 1; IplusVx(0,1) = -solveX(2); IplusVx(0,2) = solveX(1); IplusVx(1,0) = solveX(2); IplusVx(1,1) = 1; IplusVx(1,2) = -solveX(0); IplusVx(2,0) = -solveX(1); IplusVx(2,1) = solveX(0); IplusVx(2,2) = 1; Eigen::Matrix3f Ro; Ro.setZero(); Ro = I_Vx.inverse()*IplusVx; transMat(0,0) = Ro(0,0); transMat(0,1) = Ro(0,1); transMat(0,2) = Ro(0,2); transMat(0,3) = tx(0); transMat(1,0) = Ro(1,0); transMat(1,1) = Ro(1,1); transMat(1,2) = Ro(1,2); transMat(1,3) = tx(1); transMat(2,0) = Ro(2,0); transMat(2,1) = Ro(2,1); transMat(2,2) = Ro(2,2); transMat(2,3) = tx(2); // std::cout<<"transMat Geometric: "<<transMat<<"\n"; }