已知两组三维点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的点的数量、最大迭代次数、阈值。