两个点集间的变换矩阵(或相似矩阵)计算（应用于点云配准、图像匹配）

@（笔记）两个点集间的变换矩阵(或相似矩阵)计算（应用于点云配准、图像匹配）

同尺度点云

问题描述：两个相同尺度的点云，仅发生旋转平移(R、T)变换，已知相互匹配点集的情况下，计算R、T。

void pose_estimation_3d3d(
	const vector<Point3f>& pts1,
	const vector<Point3f>& pts2,
	Eigen::Matrix4f & TransMatrix
)
{
	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::Matrix3d W = Eigen::Matrix3d::Zero();
	for (int i = 0; i < N; i++)
	{
		W += Eigen::Vector3d(q1[i].x, q1[i].y, q1[i].z) * Eigen::Vector3d(q2[i].x, q2[i].y, q2[i].z).transpose();
	}
	cout << "W=" << W << endl;

	// SVD on W
	Eigen::JacobiSVD<Eigen::Matrix3d> svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV);
	Eigen::Matrix3d U = svd.matrixU();
	Eigen::Matrix3d V = svd.matrixV();
	cout << "U=" << U << endl;
	cout << "V=" << V << endl;

	Eigen::Matrix3d R_ = U* (V.transpose());
	Eigen::Vector3d t_ = Eigen::Vector3d(p1.x, p1.y, p1.z) - R_ * Eigen::Vector3d(p2.x, p2.y, p2.z);

	// convert to cv::Mat
	
	/*R = (Mat_<double>(3, 3) <<
		R_(0, 0), R_(0, 1), R_(0, 2),
		R_(1, 0), R_(1, 1), R_(1, 2),
		R_(2, 0), R_(2, 1), R_(2, 2)
		);
	t = (Mat_<double>(3, 1) << t_(0, 0), t_(1, 0), t_(2, 0));*/
	TransMatrix << R_(0, 0), R_(0, 1), R_(0, 2), t_(0, 0),
		R_(1, 0), R_(1, 1), R_(1, 2), t_(1, 0),
		R_(2, 0), R_(2, 1), R_(2, 2), t_(2, 0),
		0, 0, 0, 1;
}

reference :

高翔视觉SLAM十四讲第一版,7.9 3D-3D：ICP;

不同尺度点云(同样支持其他维度点集)

问题描述：两个不同尺度的点云，发生相似(cR、T)变换，已知相互匹配点集的情况下，计算相似矩阵。

void matrix_estimation_3d3d(
	const vector<Point3f>& pts1,
	const vector<Point3f>& pts2,
	Eigen::Matrix4f & TransMatrix
)
{
	Eigen::Matrix<float, 3, Eigen::Dynamic> cloud_tgt(3, pts1.size());
	Eigen::Matrix<float, 3, Eigen::Dynamic> cloud_src(3, pts1.size());
	for (int i = 0; i < pointcapture.pts1.size(); i++) {
		cloud_src(0, i) = pts1[i].x;
		cloud_src(1, i) = pts1[i].y;
		cloud_src(2, i) = pts1[i].z;
		cloud_tgt(0, i) = pts2[i].x;
		cloud_tgt(1, i) = pts2[i].y;
		cloud_tgt(2, i) = pts2[i].z;
	}
	TransMatrix = Eigen::umeyama(cloud_src, cloud_tgt, true);//置为true,则可估计不同尺度点集的配准，包含cR、T,c为尺度因子;
}

关于Eigen::umeyama ( const MatrixBase< Derived > & src, const
MatrixBase< OtherDerived > & dst, bool with_scaling = true ) 函数的说明

reference :

https://eigen.tuxfamily.org/dox-3.2/group__Geometry__Module.html#gab3f5a82a24490b936f8694cf8fef8e60（Eigen官方文档）