個人博客：http://www.chenjianqu.com/
原文鏈接：http://www.chenjianqu.com/show-96.html
ICP即迭代最近點(Iterative Closest Point，ICP)，用於求解一組匹配好的3D點之間的運動。3D點可由RGB-D或雙目相機得來，然後將關鍵點進行匹配。ICP的求解分爲兩種方式：利用線性代數的求解(SVD)，以及利用非線性優化方式的求解(Bundle Adjustment)。
SVD求解
定義第 i 對點的誤差項：ei = pi - (Rpi’+t)，然後構建最小二乘問題，求使誤差平方和達到極小的R,t：
求解方法
首先定義兩組3D點的質心(平均值)：p=(∑pi)/n，p'=(∑pi')/n。則誤差公式變形如下：
其中後一項 (pi- p - R(pi'-p')) 求和之後爲0，因此優化目標函數簡化爲：
觀察上式，發現左邊項只與旋轉矩陣R相關，右邊既有R也有t，但只和質心有關。只要求得R，令上式中的|p-Rp'-t|=0，就可得到 t。因此ICP的求解步驟爲：
SVD分解
因此我們首先想辦法解出R。根據上式展開R的誤差項：
上式最右邊項和R無關，第二項RTR = I，亦與R無關。因此，實際優化目標函數變爲：
上式的變形是根據跡的性質來的。定義矩陣：
W是一個3x3矩陣，對W進行SVD分解得：W = UΣVT。根據：<K. S. Arun, T. S. Huang, and S. D. Blostein, “Least-squares fitting of two 3-d point sets,” Pattern Analysis and Machine Intelligence, IEEE Transactions on, no. 5, pp. 698–700, 1987.> 和 <F. Pomerleau, F. Colas, and R. Siegwart, “A review of point cloud registration algorithms for mobile robotics,”Foundations and Trends in Robotics (FnTROB), vol. 4, no. 1, pp. 1–104, 2015.>這兩篇論文，當W滿秩時，旋轉矩陣 R=UVT。解出R後，再根據 t=p-Rp' 即可解出平移向量 t。
非線性優化方法
求解 ICP 的另一種方式是使用非線性優化，以迭代的方式去找最優值。以李代數表達位姿時，目標函數可以寫成：
使用李代數擾動模型表示誤差項關於位姿的表示：
於是，在非線性優化中只需不斷迭代，就能找到極小值。在唯一解的情況下，只要能找到極小值解，那麼這個極小值就是全局最優值——因此不會遇到局部極小而非全局最小的情況。這也意味着 ICP 求解可以任意選定初始值，這是已匹配點時求解 ICP 的一大好處。
代碼實現
代碼實現如下，先提取並匹配ORB特徵，然後構建對應的3D點，接着使用SVD方法求解兩幀組3D特徵點之間的運動，並使用高斯牛頓法優化。
#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Eigen/SVD>
#include <sophus/se3.hpp>
using namespace std;
using namespace cv;
void find_feature_matches(
const Mat &img_1, const Mat &img_2,
std::vector<KeyPoint> &keypoints_1,
std::vector<KeyPoint> &keypoints_2,
std::vector<DMatch> &matches);
Point2d pixel2cam(const Point2d &p, const Mat &K);
void pose_estimation_3d3d(
const vector<Point3f> &pts1,
const vector<Point3f> &pts2,
Mat &R, Mat &t
);
void bundleAdjustmentGN(
const vector<Point3f> &points_3d,
const vector<Point3f> &points_2d,
Mat &R, Mat &t
);
int main(int argc, char **argv)
{
//讀取圖像
Mat img_1 = imread("1.png", CV_LOAD_IMAGE_COLOR);
Mat img_2 = imread("2.png", CV_LOAD_IMAGE_COLOR);
vector<KeyPoint> keypoints_1, keypoints_2;
vector<DMatch> matches;
find_feature_matches(img_1, img_2, keypoints_1, keypoints_2, matches);
cout << "一共找到了" << matches.size() << "組匹配點" << endl;
// 建立3D點
Mat depth1 = imread("1_depth.png", CV_LOAD_IMAGE_UNCHANGED); // 深度圖爲16位無符號數，單通道圖像
Mat depth2 = imread("2_depth.png", CV_LOAD_IMAGE_UNCHANGED); // 深度圖爲16位無符號數，單通道圖像
Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
vector<Point3f> pts1, pts2;
//獲得兩張圖片關鍵點的相機座標系3D點
for (DMatch m:matches) {
Point2f pt1=keypoints_1[m.queryIdx].pt;
Point2f pt2=keypoints_2[m.trainIdx].pt;
ushort d1 = depth1.ptr<unsigned short>(int(pt1.y))[int(pt1.x)];
ushort d2 = depth2.ptr<unsigned short>(int(pt2.y))[int(pt2.x)];
if (d1 == 0 || d2 == 0) // bad depth
continue;
Point2d p1 = pixel2cam(pt1, K);
Point2d p2 = pixel2cam(pt2, K);
float dd1 = float(d1) / 5000.0;
float dd2 = float(d2) / 5000.0;
pts1.push_back(Point3f(p1.x * dd1, p1.y * dd1, dd1));
pts2.push_back(Point3f(p2.x * dd2, p2.y * dd2, dd2));
}
//ICP運動估計
Mat R, t;
pose_estimation_3d3d(pts1, pts2, R, t);
cout << "ICP via SVD results: " << endl;
cout << "R = " << R << endl;
cout << "t = " << t << endl;
cout << "R_inv = " << R.t() << endl;
cout << "t_inv = " << -R.t() * t << endl;
//BA優化
bundleAdjustmentGN(pts1, pts2, R, t);
return 0;
}
void find_feature_matches(const Mat &img_1, const Mat &img_2,
std::vector<KeyPoint> &keypoints_1,
std::vector<KeyPoint> &keypoints_2,
std::vector<DMatch> &matches) {
Mat descriptors_1, descriptors_2;
// used in OpenCV3
Ptr<FeatureDetector> detector = ORB::create();
Ptr<DescriptorExtractor> descriptor = ORB::create();
Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
detector->detect(img_1, keypoints_1);//-- 第一步:檢測 Oriented FAST 角點位置
detector->detect(img_2, keypoints_2);
descriptor->compute(img_1, keypoints_1, descriptors_1);//-- 第二步:根據角點位置計算 BRIEF 描述子
descriptor->compute(img_2, keypoints_2, descriptors_2);
vector<DMatch> match;
matcher->match(descriptors_1, descriptors_2, match);//-- 第三步:對兩幅圖像中的BRIEF描述子進行匹配，使用 Hamming 距離
//-- 第四步:匹配點對篩選
double min_dist = 10000, max_dist = 0;
//找出所有匹配之間的最小距離和最大距離, 即是最相似的和最不相似的兩組點之間的距離
for (int i = 0; i < descriptors_1.rows; i++) {
double dist = match[i].distance;
if (dist < min_dist) min_dist = dist;
if (dist > max_dist) max_dist = dist;
}
//當描述子之間的距離大於兩倍的最小距離時,即認爲匹配有誤.但有時候最小距離會非常小,設置一個經驗值30作爲下限.
for (int i = 0; i < descriptors_1.rows; i++)
if (match[i].distance <= max(2 * min_dist, 30.0))
matches.push_back(match[i]);
}
Point2d pixel2cam(const Point2d &p, const Mat &K) {
return Point2d(
(p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),
(p.y - K.at<double>(1, 2)) / K.at<double>(1, 1)
);
}
//ICP求解
void pose_estimation_3d3d(const vector<Point3f> &pts1,
const vector<Point3f> &pts2,
Mat &R, Mat &t) {
//計算兩組點的質心
Point3f p1, p2;
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);
for (int i = 0; i < N; i++) {
q1[i] = pts1[i] - p1;
q2[i] = pts2[i] - p2;
}
//計算W矩陣
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();//計算W=sum（q1*q2^T）
cout << "W=" << W << endl;
// 計算W的SVD分解
Eigen::JacobiSVD<Eigen::Matrix3d> svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::Matrix3d U = svd.matrixU();
Eigen::Matrix3d V = svd.matrixV();
//計算R=U V^T
Eigen::Matrix3d R_ = U * (V.transpose());
if (R_.determinant() < 0) {//如果行列式小於0
R_ = -R_;
}
//計算t=p-R p'
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));
}
//使用高斯牛頓法
void bundleAdjustmentGN(
const vector<Point3f> &pts1,
const vector<Point3f> &pts2,
Mat &R, Mat &t
){
const int iterations=10;
double cost=0,lastCost=10;
Eigen::Matrix3d R_;
Eigen::Vector3d t_;
R_<<R.at<double>(0,0),R.at<double>(0,1),R.at<double>(0,2),
R.at<double>(1,0),R.at<double>(1,1),R.at<double>(1,2),
R.at<double>(2,0),R.at<double>(2,1),R.at<double>(2,2);
t_<<t.at<double>(0,0),
t.at<double>(1,0),
t.at<double>(2,0);
for(int iter=0;iter<iterations;iter++){
cost = 0;
Eigen::Matrix<double, 6, 6> H = Eigen::Matrix<double, 6, 6>::Zero();
Eigen::Matrix<double,6,1> b = Eigen::Matrix<double, 6, 1>::Zero();
for(int i=0;i<pts1.size();i++){
//計算所有特徵點的代價
Eigen::Vector3d p1=Eigen::Vector3d(pts1[i].x, pts1[i].y,pts1[i].z);
Eigen::Vector3d p2=Eigen::Vector3d(pts2[i].x, pts2[i].y,pts2[i].z);
Eigen::Vector3d p1_=R_*p2+t_;
Eigen::Vector3d e=p1-p1_;
cost+=e.squaredNorm();//+=平方和;
//計算雅可比矩陣
Eigen::Matrix<double,4,6> J;
J<<1,0,0,0,p1_(2,0),-p1_(1,0),
0,1,0,-p1_(2,0),0,p1_(0,0),
0,0,1,p1_(1,0),-p1_(0,0),0,
0,0,0,0,0,0;
J=-J;
//變爲齊次座標
Eigen::Vector4d e_=Eigen::Vector4d(e(0,0),e(1,0),e(2,0),0);
//計算H和b矩陣
H += J.transpose() * J;
b += -J.transpose() * e_;
}
//解方程：H*dx=b，其中dx是李代數的增量
Eigen::Matrix<double,6,1> dx;
dx = H.ldlt().solve(b);
Sophus::SE3d T_ =Sophus::SE3d(R_,t_);
Sophus::SE3d newT = Sophus::SE3d::exp(dx) * T_;
Eigen::Matrix<double,4,4> T=newT.matrix();
R_<<T(0,0),T(0,1),T(0,2),
T(1,0),T(1,1),T(1,2),
T(2,0),T(2,1),T(2,2);
t_<<T(0,3),T(1,3),T(2,3);
//超過極小點時，結束迭代
//if (iter > 0 && cost >= lastCost) {
// cout << "cost: " << cost << ", last cost: " << lastCost << endl;
// break;
//}
cout<<"cost="<<cost<<endl;
lastCost=cost;
}
}
結果顯示高斯牛頓法似乎沒有起到優化作用，可能是我哪裏寫錯了。。。。
參考文獻
[0]高翔.視覺SLAM14講