当前位置：首页 > news >正文

从零开始学习Slam|ICP原理与应用

news 来源：原创 2025/5/9 4:50:09

ICP全称Iterative Closest Point，翻译过来就是迭代最近点。ICP在点云配准（registration）领域应用的非常广泛.

ICP算法流程

首先对于一幅点云中的每个点，在另一幅点云中计算匹配点（最近点）
极小化匹配点间的匹配误差，计算位姿
然后将计算的位姿作用于点云
再重新计算匹配点
如此迭代，直到迭代次数达到阈值，或者极小化的能量函数变化量小于设定阈值

求解ICP的三个步骤

计算两组点的质心位置 (平均位置)p,p’，然后计算每个点的去质心坐标： $q_i= p_i - p; q_i' = p_i'-p'$
根据以下优化问题计算旋转矩阵：
$这一步的目标是找到一个旋转矩阵R^*$ ，使得 $旋转后的点集{Rq_i'}与点集 {q_i }之间的距离最小。$ 这里的距离是通过计算每个点对之间的欧氏距离的平方和来衡量的：
$R^* = argmin_R \frac{1}{2}\sum_{i=1}^{n}\left\| q_i - Rq_i' \right\|^2$ ,n是点的数量。
根据第二步的 R，计算 t：
一旦找到了最优的旋转矩阵 R* ，就可以计算平移向量 t* 。这个向量表示在旋转和平移之后，两个点集的质心之间的相对位置。
平移向量 t* 的计算公式是： $t^*= p−R^* p'$ 。这里，p 和 p’ 是原始点集的质心，R* 是找到的最优旋转矩阵。

缺陷

上面介绍是最简单的点和点匹配的ICP算法，实际情况中由于噪声，我们仍需其他方法去使算法更加鲁棒。例如，极小化的误差项包括对应点的点到点的欧式距离，和对应点的点到平面距离,以及极小化对应点的颜色值误差等。2003年的时候，pottman 和Hofer两位大牛的论文中证明了当两幅点云比较接近时，极小化对应点的点到平面距离比点到点距离更接近两个平面之间的真实距离，也就是说计算点到平面的距离更靠谱！
在这里插入图片描述

题目

1.

在这里插入图片描述
证明：

展开等式左边， $q_i^T Rq_i'$ 是一个标量（向量乘以矩阵再乘以向量的结果是标量）
展开等式右边：
由于 $Rq_i'q_i^T$ 是一个矩阵，其第i个对角线元素可以表示为： $\sum_{j=1}^{n}R_{ij}(q_{i}'q_{i}^T)_{ji}$
由于 $q_i'q_i^T$ 是一个外积,其元素 $q_i'q_i^T)_{ji}$
可以表示为：
$q_i'q_i^T)_{ji}=q_{i'j}q_{i,j}$
因此我们可以将 $Rq_i'q_i^T)_{ii}$ 重写为 $(Rq_i'q_i^T)_{ii} = \sum_{j=1}^n R_{ij} q_{i',j} q_{i,j}$ 将这个表达式代入迹的计算中，我们得到：
$\text{tr}(R q_i' q_i^T) = \sum_{i=1}^n \sum_{j=1}^n R_{ij} q_{i'j}q_{i,j}$ 这实际上是 $q_i^TRq_i'$ 的展开形式，因为：
$q_i^T R q_i' = \sum_{i=1}^n \sum_{j=1}^n q_{i,i} R_{ij} q_{i'j}$
由于矩阵乘法的交换律，我们可以将 $q _{i'i}和 q_{i'j }$ 的顺序交换，得到：
$q_i^T R q_i' = \sum_{i=1}^n \sum_{j=1}^n R_{ij} q_{i'j} q_{i,j}$

2.

2、精心设计的ICP编程实践

给定一个轨迹１，数据格式：timestamp tx ty tz qx qy qz qw，自定义一个任意的旋转矩阵和平移向量（可以尝试不同的值，甚至加一些噪声看看结果有什么变化），对轨迹１进行变换，得到一个新的轨迹２，使用ICP算法（提示：取平移作为三维空间点）估计轨迹１，２之间的位姿，然后将该位姿作用在轨迹２。

验证：ICP算法估计的旋转矩阵和平移向量是否准确；轨迹１，２是否重合。

如下是我加了一个旋转平移量后的两个轨迹，经过ICP计算好位姿后再反作用在变换后的轨迹，最终两个轨迹是重合滴！

/***************************** 题目：给定一个轨迹１，数据格式：timestamp tx ty tz qx qy qz qw* 自定义一个任意的旋转矩阵和平移向量（可以尝试不同的值看看结果有什么变化），对轨迹１进行变换，得到一个新的轨迹２* 使用ICP算法（提示：取平移作为三维空间点）估计轨迹１，２之间的位姿，然后将该位姿作用在轨迹２* 验证：ICP算法估计的旋转矩阵和平移向量是否准确；轨迹１，２是否重合* 
* 本程序学习目标：* 熟悉ICP算法的原理及应用。** 公众号：计算机视觉life。发布于公众号旗下知识星球：从零开始学习SLAM，参考答案请到星球内查看。* 时间：2019.05* 作者：小六
****************************/
#include <iostream>
#include "sophus/se3.h"
#include <fstream>
#include <pangolin/pangolin.h>
#include <opencv2/core/core.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/SVD>using namespace std;
using namespace cv;void DrawTrajectory(vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> pose1,vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> pose2);int main()
{// path to trajectory filestring trajectory_file = "../trajectory.txt";vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> pose_groundtruth;vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> pose_new;vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> pose_estimate;vector<Point3f> pts_new, pts_groundtruth;Eigen::Quaterniond q;Eigen::Vector3d t;Sophus::SE3 T;string timestamp;ifstream textFile;// 自定义一个变换矩阵/**********************　开始你的代码，参考星球里作业５代码　****************************/// 旋转向量（轴角）：沿Z轴旋转45°Eigen::Vector3d rotation_vector(0, 0, M_PI / 4); // 沿Z轴旋转45°// 平移向量，可以自己自定义，我这里是 x=3, y=-1, z=0.8，可以多尝试其他值Eigen::Vector3d translation_vector(3, -1, 0.8); // 平移向量/**********************　结束你的代码　****************************/Sophus::SE3 myTransform(rotate,tranlation);cout<<"rotation matrix =\n"<<rotation_vector.matrix() <<endl;cout<<"translation vector =\n"<<tranlation <<endl;cout<<"myTransform =\n"<<myTransform.matrix() <<endl;textFile.open(trajectory_file.c_str());//　读取轨迹数据/**********************　开始你的代码，参考星球里作业８代码　****************************/// 提示：取平移作为三维空间点，平移向量 t 直接表示了刚体变换后的坐标系原点在世界坐标系中的位置。因此，它可以被看作是一个三维空间中的点的坐标。while (textFile >> timestamp >> t.x() >> t.y() >> t.z() >> q.x() >> q.y() >> q.z() >> q.w()) {Sophus::SE3 pose(q, t);pose_groundtruth.push_back(pose);                                     // pose 位姿 包含位置和方向pts_groundtruth.push_back(Point3f(t.x(), t.y(), t.z()));             //pts是points的缩写，表示三维空间的一个点// 应用变换生成新的轨迹Eigen::Vector3d new_t = myTransform * t;// 表示将三维向量 t 通过刚体变换 myTransform 进行变换，得到新的三维向量 new_t// myTransform.so3() 是 Sophus::SE3 类的一个方法，它提取出旋转部分，并将其表示为 Sophus::SO3 类型。// Sophus::SO3 是一个专门用于表示三维旋转的类，可以使用旋转矩阵或四元数来表示旋转。// unit_quaternion() 是 Sophus::SO3 类的一个方法，它将旋转表示为单位四元数。Eigen::Quaterniond new_q = q * myTransform.so3().unit_quaternion();//表示将原始旋转 q 与 myTransform 的旋转部分进行复合，生成新的旋转四元数 new_qpose_new.push_back(Sophus::SE3(new_q, new_t));pts_new.push_back(Point3f(new_t.x(), new_t.y(), new_t.z()));}/**********************　结束你的代码　****************************/textFile.close();// 使用ICP算法估计轨迹１，２之间的位姿，然后将该位姿作用在轨迹２/**********************　开始你的代码，参考十四讲中第７章ICP代码　****************************/Mat pts1, pts2;pts1 = Mat(pts_groundtruth).reshape(0, pts_groundtruth.size());pts2 = Mat(pts_new).reshape(0, pts_new.size());Mat R, t;Mat outliers;             // outliers 是指那些在匹配过程中未能正确配对的点int method = cv::SOLVEPNP_ITERATIVE;double reprojThreshold = 3.0;bool useExtrinsicGuess = false;// 使用solvePnP估计位姿solvePnP(pts1, pts2, Mat::eye(3, 3, CV_64F), Mat::zeros(4, 1, CV_64F), R, t, useExtrinsicGuess, method, noArray(), &outliers);// 将旋转矩阵从Rodrigues形式转换为3x3矩阵Rodrigues(R, R);// 输出估计的位姿cout << "Estimated rotation matrix:\n" << R << endl;cout << "Estimated translation vector:\n" << t.t() << endl;// 将估计的位姿作用于轨迹2for (size_t i = 0; i < pose_new.size(); i++) {Eigen::Vector3d new_t = R * pose_new[i].translation() + t.t();Eigen::Quaterniond new_q = pose_new[i].so3().unit_quaternion() * Eigen::Quaterniond(R);pose_estimate.push_back(Sophus::SE3(new_q, new_t));}/**********************　结束你的代码　****************************///    DrawTrajectory(pose_groundtruth, pose_new);  // 变换前的两个轨迹DrawTrajectory(pose_groundtruth, pose_estimate);  // 轨迹应该是重合的return 0;
}void DrawTrajectory(vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> pose1,vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> pose2) {if (pose1.empty()||pose2.empty()) {cerr << "Trajectory is empty!" << endl;return;}// create pangolin window and plot the trajectorypangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);glEnable(GL_DEPTH_TEST);glEnable(GL_BLEND);glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);pangolin::OpenGlRenderState s_cam(pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0));pangolin::View &d_cam = pangolin::CreateDisplay().SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f).SetHandler(new pangolin::Handler3D(s_cam));while (pangolin::ShouldQuit() == false) {glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);d_cam.Activate(s_cam);glClearColor(1.0f, 1.0f, 1.0f, 1.0f);glLineWidth(2);for (size_t i = 0; i < pose1.size() - 1; i++) {glColor3f(1 - (float) i / pose1.size(), 0.0f, (float) i / pose1.size());glBegin(GL_LINES);auto p1 = pose1[i], p2 = pose1[i + 1];glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);glEnd();}for (size_t i = 0; i < pose2.size() - 1; i++) {glColor3f(1 - (float) i / pose2.size(), 0.0f, (float) i / pose2.size());glBegin(GL_LINES);auto p1 = pose2[i], p2 = pose2[i + 1];glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);glEnd();}pangolin::FinishFrame();usleep(5000);   // sleep 5 ms}
}

ICP算法流程

求解ICP的三个步骤

缺陷

题目

1.

2.

相关文章：