有两组对应的点集(corresponding point sets):
求欧式变换 使得:
ICP 算法基于最小二乘法进行迭代计算,使得误差平方和达到极小值:
可通过以下三个步骤进行求解:
(1)定义两组点的质心,简化目标函数
交叉项部分 在求和之后零,因此目标函数简化为:
第一项只与旋转矩阵R 有关,只要获得 R ,令第二项为零就能求得 t 。
(2)计算每个点的去质心坐标,计算旋转矩阵R
其中,第二项 与R 无关,只有第三项与 R有关。因而,目标函数变为:
通过 Singular Value Decomposition (SVD) 来进行求解,先定义矩阵:
W是一个 矩阵,对W 进行 SVD 分解,得:
其中, 为奇异值组成的对角矩阵,当 W满秩时,R 为:
(3)计算平移矩阵t
(4)slam视觉十四讲ICP代码:
void pose_estimation_3d3d(const vector<Point3f>& pts1,
const vector<Point3f>& pts2,
Mat& R, Mat& t)
{
// center of mass
Point3f p1, p2;
int N = pts1.size();
for (int i=0; i<N; i++)
{
p1 += pts1[i];
p2 += pts2[i];
}
p1 /= N;
p2 /= N;
// subtract COM
vector<Point3f> q1(N), q2(N);
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));
}