通过两组匹配好的3D点，计算相对位姿变换

通过两组匹配好的3D点，计算相对位姿变换

采用的方法为ICP，求解方式分为两种：SVD或者非线性优化方法
本文采用SVD的方法求解

构建最小二乘的代价函数，求得使误差平方和达到最小的Rt

首先定义两组点的质心

代入代价函数

上面三项中最后一项求和为零，因此代价函数变为

式中第一项只与R有关，因此可以先求得一个R使得第一项最小然后再求t
对第一项展开得到

第一项和第二项都与R无关，因此最后优化目标函数变为：

最后通过SVD方法求得使得上述代价函数最小的R，先定义矩阵

对其进行SVD分解

当W满秩时，R为：

得到R后即可求得t

/*给两组已经匹配好的3D点，来计算相对位姿变换，写代码*/
#include
#include 
#include
#include 
#include 
#include "Eigen/Geometry"
using namespace std;
#define PI 3.1415926
int main()
{
	/*生成旋转矩阵*/
	Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
	Eigen::AngleAxisd rotationVector(PI / 4, Eigen::Vector3d(0, 0, 1));
	R = rotationVector.toRotationMatrix();
	cout << R << endl;

	//生成数据点
	Eigen::Vector3d p1 = Eigen::Vector3d(1, 2, 3);
	Eigen::Vector3d p2 = Eigen::Vector3d(6, 5, 4);
	Eigen::Vector3d p3 = Eigen::Vector3d(8, 7, 9);
	vector<Eigen::Vector3d> pA = { p1, p2, p3 };

	//生成旋转后的点
	vector<Eigen::Vector3d> pB;
	for (auto p:pA)
	{
		pB.emplace_back(R*p);
	}


	/*求两个点云的中心*/
	Eigen::Vector3d qA = Eigen::Vector3d(0, 0, 0), qB = Eigen::Vector3d(0, 0, 0);
	for (int i = 0; i < pA.size(); i++)
	{
		for (int j = 0; j < 3; j++)
		{
			qA[j] += pA[i][j];
			qB[j] += pB[i][j];
		}
	}
	qA = qA / pA.size();
	qB = qB / pB.size();

	/*求去中心的点云*/
	for (int i = 0; i < pA.size(); i++)
	{
		pA[i] = pA[i] - qA;
		pB[i] = pB[i] - qB;
	}

	/*求矩阵W*/
	Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
	for (int i = 0; i < pA.size(); i++)
	{
		W += pA[i] * pB[i].transpose();
	}

	/*SVD分解*/
	Eigen::JacobiSVD<Eigen::Matrix3d> svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV);
	Eigen::Matrix3d U = svd.matrixU();
	Eigen::Matrix3d V = svd.matrixV();

	Eigen::Matrix3d Rr = U * V.transpose();
	cout << Rr << endl;

}