已知一个物体在不同坐标系下的三维点，使用SVD分解求解两个坐标系的变换矩阵

已知两组三维点A、B，它们分别是同一个物体在不同坐标系下的三维坐标，通过这两组三维点求解两个坐标系的变换矩阵。

void solve_rigid_transform_3d(std::vector<cv::Point3d> A, std::vector<cv::Point3d> B, Eigen::Matrix3d& R, Eigen::Vector3d& t) {
	
	if (A.size() != B.size())
	{
		std::cout << "两个点集中点的个数需要相等" << std::endl;
		exit(1);
	}
	
	int N = A.size();
	Eigen::MatrixXd source(N, 3), target(N, 3);
	for (int i = 0; i < N; i++) {
		source(i, 0) = A[i].x;
		source(i, 1) = A[i].y;
		source(i, 2) = A[i].z;
		target(i, 0) = B[i].x;
		target(i, 1) = B[i].y;
		target(i, 2) = B[i].z;
	}

	/*std::cout << "source :" << std::endl << source << std::endl;
	std::cout << "target :" << std::endl << target << std::endl;*/

	// 1、计算质心
	Eigen::RowVector3d meanVector1 = source.colwise().mean();  // 每列求均值
	Eigen::RowVector3d meanVector2 = target.colwise().mean();

	// 2、去质心
	source.rowwise() -= meanVector1;                           // 去质心坐标
	target.rowwise() -= meanVector2;

	// 3、构建协方差矩阵
	Eigen::Matrix3d covMat;
	covMat = (source.transpose() * target) / int(source.rows());

	// 4、SVD分解求旋转矩阵
	Eigen::JacobiSVD<Eigen::MatrixXd> svd(covMat, Eigen::ComputeFullU | Eigen::ComputeFullV);
	Eigen::Matrix3d V = svd.matrixV();
	Eigen::Matrix3d U = svd.matrixU();

	// 5、求旋转平移向量
	R = V * U.transpose(); 
	if (R.determinant() < 0)
	{
		R *= -1;
	};
	t = meanVector2 - (R * meanVector1.transpose()).transpose();
}

在实际应用中，测得的点的坐标可能会有误差，可以通过加入RANSAC算法来增加精度。
先将输入的两个点集A和B随机选择一定数量的点作为样本集，然后调用solve_rigid_transform_3d函数得到对应的旋转和平移矩阵，将这个变换作用于剩余的点，计算它们与对应点的距离是否小于一定的阈值，如果满足条件，则将这个点加入到内点集中。重复这个过程多次，选择内点最多的一组R和t再来计算最后的R和t

void solve_rigid_transform_3d_with_ransac(std::vector<cv::Point3d> A, std::vector<cv::Point3d> B, Eigen::Matrix3d& R, Eigen::Vector3d& t, int n, int max_iterations, double threshold) {
	
	int num_points = A.size();
	int best_num_inliers = 0;
	Eigen::Matrix3d best_R;
	Eigen::Vector3d best_t;

	// RANSAC
	for (int i = 0; i < max_iterations; i++) {

		// 从A和B中随机选择n个点
		std::vector<int> indices(num_points);
		std::iota(indices.begin(), indices.end(), 0);
		std::random_shuffle(indices.begin(), indices.end());
		std::vector<cv::Point3d> sample_A(n);	
		std::vector<cv::Point3d> sample_B(n);	
		for (int j = 0; j < n; j++) {
			sample_A[j] = A[indices[j]];		//从A随机选择n个点
			sample_B[j] = B[indices[j]];		//从B随机选择n个点
		}


		// 使用n个随机点求解刚性变换
		Eigen::Matrix3d sample_R;
		Eigen::Vector3d sample_t;
		solve_rigid_transform_3d(sample_A, sample_B, sample_R, sample_t);


		// 更新内点并更新最好的R,t
		int num_inliers = 0;
		for (int j = 0; j < num_points; j++) {
			Eigen::Vector3d vec_A(3, 1);
			Eigen::Vector3d vec_B(3, 1);
			vec_A(0, 0) = A[j].x;
			vec_A(1, 0) = A[j].y;
			vec_A(2, 0) = A[j].z;
			vec_B(0, 0) = B[j].x;
			vec_B(1, 0) = B[j].y;
			vec_B(2, 0) = B[j].z;
			Eigen::Vector3d transformed_A = sample_R * vec_A + sample_t;
			if ((transformed_A - vec_B).norm() < threshold) {
				num_inliers++;
			}
		}

		if (num_inliers > best_num_inliers) {
			best_num_inliers = num_inliers;
			best_R = sample_R;
			best_t = sample_t;
		}
	}// RANSAC结束
	

	// 用最好的R，t来计算满足阈值的内点，用这些内点计算最后的R，t
	std::vector<cv::Point3d> inliers_A(best_num_inliers);
	std::vector<cv::Point3d> inliers_B(best_num_inliers);
	int inlier_idx = 0;
	for (int i = 0; i < num_points; i++) {
		Eigen::Vector3d vec_A(3, 1);
		Eigen::Vector3d vec_B(3, 1);
		vec_A(0, 0) = A[i].x;
		vec_A(1, 0) = A[i].y;
		vec_A(2, 0) = A[i].z;
		vec_B(0, 0) = B[i].x;
		vec_B(1, 0) = B[i].y;
		vec_B(2, 0) = B[i].z;
		Eigen::Vector3d transformed_A = best_R * vec_A + best_t;
		if ((transformed_A - vec_B).norm() < threshold) {
			inliers_A[inlier_idx] = A[i];
			inliers_B[inlier_idx] = B[i];
			inlier_idx++;
		}
	}

	solve_rigid_transform_3d(inliers_A, inliers_B, R, t);
}

增加的三个参数n, max_iterations, threshold分别表示随机选择用来计算R和t的点的数量、最大迭代次数、阈值。