小余儿

V1

2022/03/14阅读:27主题:全栈蓝

ICP算法

前言

ICP的英文全称为Iterative Closest Point,即为迭代最近点。它在激光雷达应用频率很高,主要是在点云配准领域。ICP算法在是是视觉SLAM中应用也非常多,这个算法还是很重要。我们下面的讨论还是基于视觉SLAM,好了我们开始吧!

ICP算法流程

ICP算法顾名思义,就是找最近点。算法流程如下:

  • step1:预处理点云
  • step2:寻找对应点(最近点)
  • step3:根据对应点,计算R和t
  • step4:对点云进行转换,计算误差
  • step5:不断迭代,直至误差小于某一个值

3D-3D: ICP

假设我们有一组配对好的3D点如下:

现在我们想要找到一个欧式变换,使得

上述的这个问题就可以使用ICP算法求解。这里主要需要注意下,如果仅考虑3D点之间的变换,此时和相机并没有关系。在RGB-D SLAM中用ICP问题指代匹配好的两组点间的运动估计问题(跟激光SLAM有区别,激光SLAM通常是未知的情况)。ICP求解主要是两种方式,一种是SVD,另一种是非线性优化方式求解。

SVD方法

根据前面描述的ICP问题,令第 对点的误差为:

这里解释一下,我们用一个点 (第二幅图中数据)来估算另一个点 时(在第一幅中数据,即为观测坐标),必然会产生误差。

我们构建最小二乘问题,求解使用平方和打到极小的 ,则有

定义图像1和图像2中两组点的之心为:

在误差函数做如下处理(去质心处理):

最终优化目标函数简化为

我们可以先求左边的第一项,我们记去质心坐标为:

由于左边第一项只和 有关,因此我们先计算旋转矩阵 ,则

我们将 进行展开,则为

由于 ,继续化简优化目标函数变为

为了解 ,定义矩阵:

使用SVD分解,使得 ,当 满秩时,则

解得 以后, 即为

非线性优化方法

李代数表达位姿,迭代方式找到最优值。目标函数为:

实践:求解ICP

SVD方法

  • 原理和前面介绍的一致,代码如下:
void pose_estimation_3d3d(const vector<Point3f> &pts1, const vector<Point3f> &pts2, Mat &R, Mat &t) 
{
  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());
  if (R_.determinant() < 0) {
    R_ = -R_;
  }
  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));
}

分类:

后端

标签:

C++

作者介绍

小余儿
V1