@(笔记)两个点集间的变换矩阵(或相似矩阵)计算(应用于点云配准、图像匹配)
同尺度点云
问题描述:两个相同尺度的点云,仅发生旋转平移(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官方文档)