C++實現：

#include <iostream>
#include <opencv2/opencv.hpp>
#include <Eigen/eigen>

using namespace cv;
using namespace std;

void ICP(const vector<Point3f>& pts1, const vector<Point3f>& 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<float, 3, 3> 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<Point3f> 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<Point3f>& pts1,const vector<Point3f>& pts2)
{
	Point3f p1, p2;     // center of mass
	int N = pts1.size();
	for (int i = 0; i<N; i++)
	{
		p1 += pts1[i];
		p2 += pts2[i];
	}
	p1 = Point3f(Vec3f(p1) / N);
	p2 = Point3f(Vec3f(p2) / N);
	vector<Point3f> q1(N), q2(N); // remove the center
	for (int i = 0; i<N; i++)
	{
		q1[i] = pts1[i] - p1;
		q2[i] = pts2[i] - p2;
	}

	// compute q1*q2^T
	Eigen::Matrix3f W = Eigen::Matrix3f::Zero();
	for (int i = 0; i<N; i++)
	{
		W += Eigen::Vector3f(q1[i].x, q1[i].y, q1[i].z) * Eigen::Vector3f(q2[i].x, q2[i].y, q2[i].z).transpose();
	}

	// SVD on W
	Eigen::JacobiSVD<Eigen::Matrix3f> 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;
}