2つの対応するポイントセット(対応するポイントセット)があります。
ユークリッド変換 を見つけると、次のように なります。
ICPアルゴリズムは最小二乗法に基づいて繰り返し計算されるため、二乗誤差の合計が最小値に達します。
次の3つのステップで解決できます。
(1)2組の点の重心を定義し、目的関数を簡略化する
合計後のクロスターム部分は ゼロであるため、目的関数は次のように簡略化されます。
第1項は、Rが取得される限り、回転行列Rにのみ関連し、第2項がゼロであれば、tを取得できます。
(2)各点の重心座標を計算し、回転行列Rを計算する
前記第二項及びR、Rのみ3番目の項目に関連する何の関係もありません。したがって、目的関数は次のようになります。
特異値分解(SVD)で解くには、まず行列を定義します。
Wは行列、Wは得るSVDあります。
ここで、 特異値の対角マトリックス組成物、Wフルランク、Rです。
(3)変換行列tを計算する
(4)スラムビジュアル14レクチャー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));
}