当前位置：首页 > news >正文

双目系统求坐标系A到坐标系B的旋转矩阵和平移向量——C++代码实现

news 2025/3/13 0:12:47

双目系统求解一个坐标系到另外一个坐标系的 R 和 T 最常用的方法是基于奇异值分解（Singular Value Decomposition, SVD），这个方法在计算机视觉和 3D 配准中被广泛使用，也称为 Kabsch 算法。

需要有两个坐标系测量的点，至少有3组对应点（能够组成一个平面的点，即不是一条直线上的点），通过这几组对应点来求解旋转矩阵和平移矩阵。

需要注意的是，这几组点需要一一对应，并且个数一致。

具体的：

步骤 1：计算质心

首先，计算 A 坐标系 和 B 坐标系 中点集的质心（中心点）：

其中：

CA 是所有 PiA 点的平均值（质心）。
CB 是所有 PiB 点的平均值（质心）。

然后，将点集转换为去中心化坐标：

步骤 2：计算协方差矩阵

计算 A 方向到 B 方向的协方差矩阵：

其中：

H 是大小 3×3 矩阵，描述了 A 和 B 点集之间的空间关系。

步骤 3：对协方差矩阵进行SVD分解

其中：

U,S,V 是 SVD 分解结果。
U,V 是正交矩阵，S 是奇异值对角矩阵。

步骤 4：计算旋转矩阵R

旋转矩阵由 SVD 分解的 U 和 V 计算得到：

如果 det⁡(R)=−1，说明发生了反射（而不是纯旋转），需要修正：

步骤 5：计算平移向量T

平移向量 T 由质心关系计算：

SVD本质上是在寻找最佳的旋转对齐方式：
- 通过 H=USV^T分解，找到最佳的旋转 R=UV^T，使 A 和 B 点集尽可能对齐。
- 旋转矩阵 R 需要满足正交性约束 RR^T=I，保证变换不会引入缩放或变形。
平移向量 T 由质心差计算：
- 计算两个点集的质心，确保变换后的点集仍然围绕新的中心。
整个过程最小化点对之间的平方误差：
- 通过 SVD 计算出的 R 和 T 可以使转换后的点与目标点的误差最小（利用最小二乘解）。

代码实现如下：

#include <iostream>
#include <opencv2/opencv.hpp>
#include <vector>// 计算质心
cv::Point3d calculateCentroid(const std::vector<cv::Point3d>& points) {cv::Point3d centroid(0, 0, 0);for (const auto& point : points) {centroid += point;}centroid.x /= points.size();centroid.y /= points.size();centroid.z /= points.size();return centroid;
}// 计算旋转矩阵和平移向量
void calculateTransformation(const std::vector<cv::Point3d>& A, const std::vector<cv::Point3d>& B, cv::Mat& R, cv::Mat& T) {if (A.size() != B.size()) {std::cout << "A和B的点数必须相同！" << std::endl;return;}// 计算质心cv::Point3d centroidA = calculateCentroid(A);cv::Point3d centroidB = calculateCentroid(B);// 计算偏差矩阵 Hcv::Mat H = cv::Mat::zeros(3, 3, CV_64F);for (size_t i = 0; i < A.size(); ++i) {cv::Mat a = (cv::Mat_<double>(3, 1) << A[i].x - centroidA.x, A[i].y - centroidA.y, A[i].z - centroidA.z);cv::Mat b = (cv::Mat_<double>(1, 3) << B[i].x - centroidB.x, B[i].y - centroidB.y, B[i].z - centroidB.z);H += a * b;  // 直接矩阵乘法计算 H}// 进行 SVD 分解cv::Mat U, S, Vt;cv::SVD::compute(H, S, U, Vt);// 计算旋转矩阵 RR = Vt.t() * U.t();// 确保 det(R) = 1if (cv::determinant(R) < 0) {Vt.row(2) *= -1; // 调整 V 矩阵的符号R = Vt.t() * U.t();}// 计算平移向量 T = centroidB - R * centroidAcv::Mat centroidAMat = (cv::Mat_<double>(3, 1) << centroidA.x, centroidA.y, centroidA.z);cv::Mat centroidBMat = (cv::Mat_<double>(3, 1) << centroidB.x, centroidB.y, centroidB.z);T = centroidBMat - R * centroidAMat;std::cout << "旋转矩阵 R: \n" << R << std::endl;std::cout << "平移向量 T: \n" << T << std::endl;
}// 坐标变换
cv::Point3d transformPoint(const cv::Point3d& point, const cv::Mat& R, const cv::Mat& T, bool toB) {cv::Mat pointMat = (cv::Mat_<double>(3, 1) << point.x, point.y, point.z);cv::Mat transformedMat;if (toB) {transformedMat = R * pointMat + T;  // A -> B}else {transformedMat = R.t() * (pointMat - T);  // B -> A}return cv::Point3d(transformedMat.at<double>(0), transformedMat.at<double>(1), transformedMat.at<double>(2));
}int main() {// A 和 B 坐标点std::vector<cv::Point3d> A = {{-1926.29, -1142.51, 9369.27}, {-1925.98, 12.925, 9582.14},{-1929.87, 1515.27, 9856.06}, {-1137.32, 1528.46, 9950.69},{1154.64, 1501.94, 10137.9},{1958.71, -22.3978, 9893.48}, {1964.75, -1175.61, 9682.58}};std::vector<cv::Point3d> B = {{-6184.491, 769.482, 14389.891}, {-6180.158, 1399.036, 15378.021},{-6181.900, 2222.525, 16667.394}, {-5384.299, 2214.519, 16703.414},{-3087.388, 2216.120, 16702.862}, {-2287.239, 1408.678, 15385.726}, {-2285.709, 780.394, 14398.723}};// 计算旋转矩阵 R 和平移向量 Tcv::Mat R, T;calculateTransformation(A, B, R, T);// 选取测试点cv::Point3d testPointA = A[1];cv::Point3d testPointB = B[0];// A -> Bcv::Point3d transformedPointB = transformPoint(testPointA, R, T, true);std::cout << "A坐标系的点转换到B坐标系: " << transformedPointB << std::endl;// B -> Acv::Point3d transformedPointA = transformPoint(testPointB, R, T, false);//std::cout << "B坐标系的点转换到A坐标系: " << transformedPointA << std::endl;return 0;
}

可以通过 transformPoint 函数回代查看反推A坐标系中的点到B坐标系下的三维坐标。

或者使用下面的版本（方法一样，显示方式不同）：

#include <iostream>
#include <opencv2/opencv.hpp>
#include <vector>// 计算质心
cv::Point3d computeCentroid(const std::vector<cv::Point3d>& points) {cv::Point3d centroid(0, 0, 0);for (const auto& p : points) centroid += p;centroid.x /= points.size();centroid.y /= points.size();centroid.z /= points.size();return centroid;
}// 使用 Kabsch 算法计算最优 R 和 T
void computeOptimalRT(const std::vector<cv::Point3d>& A, const std::vector<cv::Point3d>& B, cv::Mat& R, cv::Mat& T) {int N = A.size();if (N != B.size()) {std::cerr << "点数不匹配！" << std::endl;return;}// 计算质心cv::Point3d centroidA = computeCentroid(A);cv::Point3d centroidB = computeCentroid(B);// 构造偏移矩阵 Hcv::Mat H = cv::Mat::zeros(3, 3, CV_64F);for (int i = 0; i < N; i++) {cv::Mat a = (cv::Mat_<double>(3, 1) << A[i].x - centroidA.x, A[i].y - centroidA.y, A[i].z - centroidA.z);cv::Mat b = (cv::Mat_<double>(1, 3) << B[i].x - centroidB.x, B[i].y - centroidB.y, B[i].z - centroidB.z);H += a * b;  // 矩阵乘法}// SVD 分解 Hcv::Mat U, S, Vt;cv::SVD::compute(H, S, U, Vt);// 计算 RR = Vt.t() * U.t();if (cv::determinant(R) < 0) {Vt.row(2) *= -1; // 确保 R 符合右手坐标系R = Vt.t() * U.t();}// 计算 Tcv::Mat centroidA_mat = (cv::Mat_<double>(3, 1) << centroidA.x, centroidA.y, centroidA.z);cv::Mat centroidB_mat = (cv::Mat_<double>(3, 1) << centroidB.x, centroidB.y, centroidB.z);T = centroidB_mat - R * centroidA_mat;
}// 验证转换结果
double verifyTransformation(const std::vector<cv::Point3d>& A, const std::vector<cv::Point3d>& B, const cv::Mat& R, const cv::Mat& T) {double mse = 0.0;std::cout << "验证转换结果:\n";for (size_t i = 0; i < A.size(); ++i) {cv::Mat pointA = (cv::Mat_<double>(3, 1) << A[i].x, A[i].y, A[i].z);cv::Mat transformedPoint = R * pointA + T;cv::Point3d transformed(transformedPoint.at<double>(0), transformedPoint.at<double>(1), transformedPoint.at<double>(2));double errorX = transformed.x - B[i].x;double errorY = transformed.y - B[i].y;double errorZ = transformed.z - B[i].z;mse += (errorX * errorX + errorY * errorY + errorZ * errorZ);std::cout << "A点: " << A[i] << " 转换后: " << transformed << "，实际测量值: " << B[i] << std::endl;}mse /= A.size();//std::cout << "均方误差 (MSE): " << mse << std::endl;return mse;
}int main() {// A 和 B 坐标点std::vector<cv::Point3d> A = {{-1926.29, -1142.51, 9369.27}, {-1925.98, 12.925, 9582.14},{-1929.87, 1515.27, 9856.06}, {-1137.32, 1528.46, 9950.69},{1154.64, 1501.94, 10137.9}, {1958.71, -22.3978, 9893.48},{1964.75, -1175.61, 9682.58}};std::vector<cv::Point3d> B = {{-6184.491, 769.482, 14389.891}, {-6180.158, 1399.036, 15378.021},{-6181.900, 2222.525, 16667.394}, {-5384.299, 2214.519, 16703.414},{-3087.388, 2216.120, 16702.862}, {-2287.239, 1408.678, 15385.726},{-2285.709, 780.394, 14398.723}};cv::Mat R, T;computeOptimalRT(A, B, R, T);std::cout << "旋转矩阵R:\n" << R << std::endl;std::cout << "平移向量T:\n" << T << std::endl;verifyTransformation(A, B, R, T);return 0;
}

运行结果如下：