ICP算法的python实现 icp 算法

   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讲