迭代最近点ICP推导（C++实现）

 

C++实现：

#include 
#include 
#include 

using namespace cv;
using namespace std;

void ICP(const vector& pts1, const vector& pts2);

// ICP仿真
void main()
{
	Eigen::Vector3f p1_c1(0, 0, 20);
	Eigen::Vector3f p2_c1(2, 4, 30);
	Eigen::Vector3f p3_c1(5, 9, 40);
	Eigen::Vector3f p4_c1(6, 8, 25);
	
	Eigen::AngleAxisf rotation_vector(30 * CV_PI / 180, Eigen::Vector3f(0, 1, 0));//绕y轴逆时针旋转30度(转yaw)
	Eigen::Matrix R21 = rotation_vector.toRotationMatrix();
	cout << "R21: " << endl << R21 << endl;
	Eigen::Vector3f t21(5, 3, 1);

	Eigen::Vector3f p1_c2 = R21*p1_c1 + t21;
	Eigen::Vector3f p2_c2 = R21*p2_c1 + t21;
	Eigen::Vector3f p3_c2 = R21*p3_c1 + t21;
	Eigen::Vector3f p4_c2 = R21*p4_c1 + t21;

	vector p_c1, p_c2;
	p_c1.push_back(Point3f(p1_c1[0], p1_c1[1], p1_c1[2]));
	p_c1.push_back(Point3f(p2_c1[0], p2_c1[1], p2_c1[2]));
	p_c1.push_back(Point3f(p3_c1[0], p3_c1[1], p3_c1[2]));
	p_c1.push_back(Point3f(p4_c1[0], p4_c1[1], p4_c1[2]));

	p_c2.push_back(Point3f(p1_c2[0], p1_c2[1], p1_c2[2]));
	p_c2.push_back(Point3f(p2_c2[0], p2_c2[1], p2_c2[2]));
	p_c2.push_back(Point3f(p3_c2[0], p3_c2[1], p3_c2[2]));
	p_c2.push_back(Point3f(p4_c2[0], p4_c2[1], p4_c2[2]));

	ICP(p_c1, p_c2);

	getchar();
}

void ICP(const vector& pts1,const vector& pts2)
{
	Point3f p1, p2;     // center of mass
	int N = pts1.size();
	for (int i = 0; i q1(N), q2(N); // remove the center
	for (int i = 0; i svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV);
	Eigen::Matrix3f U = svd.matrixU();
	Eigen::Matrix3f V = svd.matrixV();

	Eigen::Matrix3f R_12 = U* (V.transpose());
	Eigen::Vector3f t_12 = Eigen::Vector3f(p1.x, p1.y, p1.z) - R_12 * Eigen::Vector3f(p2.x, p2.y, p2.z);

	// 验证
	Eigen::AngleAxisf R_21;
	R_21.fromRotationMatrix(R_12.transpose());
	cout << "aix: " << R_21.axis().transpose() << endl;
	cout << "angle: " << R_21.angle() * 180 / CV_PI << endl;
	cout << "t: " << (-R_12.transpose()*t_12).transpose() << endl;
}