在三维空间中，若刚体发生移动，其变换可通过一个4x4的变换矩阵来描述，该矩阵涵盖了旋转、平移等变换。当我们掌握了刚体上四个点在移动前后的坐标时，便可利用这些点来求解变换矩阵。SVD（奇异值分解）是求解此类问题的一种有效手段。
在C++中，若希望使用std::vector与Eigen库结合，而不采用Eigen::MatrixXd来存储和处理点的坐标，可以这样做。以下是一个示例代码，它展示了如何仅使用std::vector<Eigen::Vector3d>来存储点坐标，并通过SVD技术求解刚体的变换矩阵：
#include <iostream>
#include <vector>
#include <Eigen/Dense>
#include <Eigen/SVD>using namespace Eigen;
using namespace std;// 定义一个函数，用于根据原始点和变换后的点计算变换矩阵
Matrix4d computeTransformMatrix(const vector<Vector3d>& points, const vector<Vector3d>& points_prime) {// 确保点的数量相同assert(points.size() == points_prime.size() && points.size() >= 3);// 计算质心Vector3d centroid = Vector3d::Zero();Vector3d centroid_prime = Vector3d::Zero();for (size_t i = 0; i < points.size(); ++i) {centroid += points[i];centroid_prime += points_prime[i];}centroid /= static_cast<double>(points.size());centroid_prime /= static_cast<double>(points_prime.size());// 减去质心，得到相对于质心的坐标vector<Vector3d> centered_points(points.size());vector<Vector3d> centered_points_prime(points_prime.size());for (size_t i = 0; i < points.size(); ++i) {centered_points[i] = points[i] - centroid;centered_points_prime[i] = points_prime[i] - centroid_prime;}// 构建协方差矩阵HMatrix3d H = Matrix3d::Zero();for (size_t i = 0; i < points.size(); ++i) {H += centered_points[i] * centered_points_prime[i].transpose();}// 使用SVD求解旋转矩阵RJacobiSVD<Matrix3d> svd(H, ComputeFullU | ComputeFullV);Matrix3d U = svd.matrixU();Matrix3d V = svd.matrixV();Matrix3d R = U * V.transpose();// 如有必要，通过反射确保旋转方向正确if (R.determinant() < 0) {R.col(2) *= -1;}// 计算平移向量tVector3d t = centroid_prime - R * centroid;// 构建变换矩阵TMatrix4d T = Matrix4d::Identity();T.block<3, 3>(0, 0) = R;T.block<3, 1>(0, 3) = t;return T;
}int main() {// 原始点和变换后的点vector<Vector3d> points = {{1, 2, 3},{5, 6, 7},{9, 10, 11},{13, 14, 15}};vector<Vector3d> points_prime = {{16, 17, 18},{20, 21, 22},{24, 25, 26},{28, 29, 30}};// 计算变换矩阵Matrix4d transform_matrix = computeTransformMatrix(points, points_prime);// 输出变换矩阵cout << "变换矩阵 T:\n" << transform_matrix << endl;return 0;
}
在这个示例中，computeTransformMatrix函数接收两个std::vector<Eigen::Vector3d>类型的参数，它们分别代表原始点和变换后的点。函数内部首先计算质心，然后构建协方差矩阵，接着使用SVD求解旋转矩阵，最后计算平移向量并构建变换矩阵。在main函数中，我们提供了四个点在移动前后的坐标，并调用computeTransformMatrix函数来计算变换矩阵，然后将其输出。
