本节目录如下：
目录
激光SLAM里程计方案有以下几种方式：
其中基于栅格和基于匹配的方法有所了解，基于栅格的方法可以看gmapping的相关博客，这里介绍基于直接匹配ICP和NDT的方案。
激光点云的帧间匹配问题主要由两种方式实现，一种是ICP，另一种是更加常用的NDT（Normal Distributions Transform，正太分布变换）。
1、ICP变换
ICP(Iterative Closest Point，ICP)求解是3D-3D位姿估计问题中并没有出现相机模型，也就是说，仅考虑3D点之间的变换时，和相机并没有关系。
在激光SLAM和视觉SLAM中都会遇到ICP问题，不过由于激光数据特征不够丰富，我们无从知道两个点集之间的帧间匹配关系，只能认为距离最近的两个点为同一个，所以这个方法称为迭代最近点。而在视觉中，特征点为我们提供了较好的匹配关系，所以整个问题就变得更简单了。在RGB-D SLAM中，可以用这种方式估计相机位姿。下文我们用ICP指代匹配好的两组点之间的运动估计问题。
视觉上的ICP求解分为两种方式：利用线性代数的求解（主要是SVD），以及利用非线性优化的方法。
1.1 激光点云的ICP
ICP系列有很多方案，基本组合思路如下：
我们这里记录的方法是基于点到点的ICP：
1.1.1 ICP代码
相关问题汇总参考如下博客：
https://blog.csdn.net/lilynothing/article/details/80170521
1.1.2 ICP的优缺点
1. 在较好的初值情况下，可以得到很好的算法收敛性。但是这也是一个缺点，就是初始值不好的话，结果也会收到影响。
2. 在找匹配点的时候，认为距离最近的点就是对应点，这点比较果断。CPD会在每个距离之前加上权值，所以相比ICP有一定的优化效果。
3. ICP对于形状相似，但是角度偏差比较大的情况，匹配效果比较差。因为是基于最近点找的。CPD的话是全局的考虑，如果是局部和全局进行匹配的话，CPD不一定能取得比ICP更好的结果。
1.2 视觉SLAM中的ICP
给两组已经匹配好的3D点，计算相对位姿变换，写代码。
匹配两组已知坐标的3D点当然是采用ICP，参考《视觉SLAM十四讲》，ICP的解法一共有两种：SVD方法和非线性优化方法，下面过一遍SVD方法的推导过程：
参考：给两组已经匹配好的3D点，计算相对位姿变换，写代码
1.2.1 SVD方法
构建最小二乘的代价函数，求得使误差平方和达到最小的R，t：
定义两组点的质心
对代价函数做如下处理（后面添加的这两步会达到R与t分离的目的）：
上面三项中最后一项求和为零，因此代价函数变为：
第一项只和R有关，因此我们可以先求得一个R使得第一项最小，然后再求t。
我们记去质心的点分别为 和
，我们对第一项展开得：
第一项和第二项都与R无关，因此最后优化目标函数变为：
最后通过SVD方法求得使得上述代价函数最小的R，先定义矩阵：
对其进行SVD分解：
当W满秩时，R为：
解得R之后，就可以进一步求得t。代码如下：具体的应用代码在《SLAM14讲中》pose_estimate_3d3d.cpp文件中，具体的做法是先对匹配好的3D点使用SVD求解，再对求解出的R和t进行BA优化。代码在如下网址：
https://github.com/jiamen/SLAMStudy/blob/main/ch7_ICP/pose_estimate_3d3d.cpp
// 《SLAM14讲》SVD求解 P174
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 求出《SLAM14讲》P174页的 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();
}
cout << "W = " << W << endl;
// SVD on W
Eigen::JacobiSVD<Eigen::Matrix3d> svd(W, Eigen::ComputeFullU | Eigen::ComputeThinV);
Eigen::Matrix3d U = svd.matrixU(); // 得到U矩阵
Eigen::Matrix3d V = svd.matrixV(); // 得到V矩阵
// 第2帧到第1帧的变换，与PnP中第1 determinant行列式
if( U.determinant() * V.determinant() < 0 )
{
for(int x=0; x < 3; x ++)
{
U(x, 2) *= -1;
}
}
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); // P174页 式7.53 t* = p - R p'
// convert to cv::Mat 将矩阵和向量统一转换为转换为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));
}其他代码也是类似：
#include <iostream>
#include <Eigen/Core>
#include <Eigen/Geometry>
using namespace std;
int main()
{
// 生成旋转矩阵
Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
Eigen::AngleAxisd rotationVector(M_PI/4, Eigen::Vector3d(0,0,1));
R = rotationVector.toRotationMatrix();
cout<<R<<endl<<endl;
// 生成一系列原始点
Eigen::Vector3d p1 = Eigen::Vector3d(1,2,3);
Eigen::Vector3d p2 = Eigen::Vector3d(6,5,4);
Eigen::Vector3d p3 = Eigen::Vector3d(8,7,9);
vector<Eigen::Vector3d> pA = {p1, p2, p3};
// 生成旋转后的点
vector<Eigen::Vector3d> pB;
for(auto p:pA)
{
pB.emplace_back(R*p);
}
// 求两个点云的中心
Eigen::Vector3d qA = Eigen::Vector3d(0,0,0), qB = Eigen::Vector3d(0,0,0);
for(int i = 0; i< pA.size(); i++)
{
for(int j = 0; j<3; j++)
{
qA[j] += pA[i][j];// pA[i][j]中每个向量是纵着排列，所以循环一次相当于求每一行的和
qB[j] += pB[i][j];
}
}
qA = qA/pA.size();
qB = qB/pB.size();
// 求去中心的点云
for(int i = 0; i<pA.size(); i++)
{
pA[i] = pA[i]-qA;
pB[i] = pB[i]-qB;
}
// 求矩阵W
Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
for(int i = 0; i<pA.size(); i++)
{
W += pA[i]*pB[i].transpose();
}
// SVD分解
Eigen::JacobiSVD<Eigen::Matrix3d> svd(W,Eigen::ComputeFullU|Eigen::ComputeFullV);
Eigen::Matrix3d U = svd.matrixU();
Eigen::Matrix3d V = svd.matrixV();
Eigen::Matrix3d Rr = U*V.transpose();
cout<<Rr<<endl;
}1.2.2 非线性优化
求解可以看前面PnP部分重投影误差的推导过程，由于这里没有是直接对3D点进行操作，没有相机模型，所以可以直接用下面公式;
可以得到：
在对ICP问题使用非线性优化来计算时，我们依然希望使用李代数来表达相机位姿。与SVD思路不同的地方在于，在优化过程中我们不仅考虑相机的位姿，同时会优化3D点的空间位置。对我们来说，RGB-D相机每次可以观测到路表单的三维位置，从而产生一个观测数据
代码如下：
2、NDT变换
NDT（Normal Distribution Transform）：NDT算法的基本思想是构建多维变量的正太分布，如果变换参数是两个点云的最佳匹配，则变换点的概率密度很大。因此，用优化的方法求出使得概率密度之和最大的变换参数。
NDT与ICP的不同：
1、ICP要求点云之间尽量对应，而NDT是求栅格内点云的概率分布，对单点匹配的依赖程度降低，允许存在小部分差异的匹配；
2、效率上，NDT也高于ICP。原因是ICP需要迭代匹配。
对原分布求最大化，相当于对负对数求最小化。
2.1 NDT的代码
3、点云滤波
3.1 点云数据结构
在RGB-D SLAM中用到的点数据结构和点云数据结构分别是：
// 类定义
typedef pcl::PointXYZRGBA PointT;
typedef pcl::PointCloud<PointT> PointCloud;3.2 点云滤波基本操作
点云滤波属于点云数据预处理操作，点云数据预处理解决的问题有：
- 点云范围、处理对象区域
- 点云遮挡、局部视角
- 点云数据量大
- 点云采集噪声、平滑处理
3.2.1 直通滤波器
具体的代码操作如下：
pcl::PassThroungh<pcl::PointXYZ> pass;
// 创建滤波器对象
pcl::PassThrough<pcl::PointXYZ> pass;
// 设置输入点云
pass.setInputCloud(cloud);
// 滤波字段名被设置为z轴方向
pass.setFilterFieldName("z");
// 可接受的范围为(0.0, 0.5)
pass.setFilterLimits(0.0, 0.5);
// 设置保留范围内，还是过滤掉范围内
// pass.setFilterLimitsNegative(true);
// 执行滤波，保存过滤结果在cloud_filtered
pass.filter(*cloud_filtered);
3.2.2 体素滤波器
体素滤波器算法流程如下：
代码如下：
pcl::VoxelGrid<pcl::PCLPointCloud2> vox;
// 创建滤波器对象
pcl::VoxelGrid<pcl::PointXYZ> vox;
// 设置输入点云
pcl::setInputCloud(cloud);
// 设置滤波时创建的体素体积为1cm的立方体
vox.setLeafSize(0.01f, 0.01f, 0.01f);
// 执行滤波，保存过滤结果过在cloud_filtered
vox.filter(*cloud_filtered);
3.2.3 统计滤波器
统计滤波器算法流程如下：
设置代码如下：
pcl::StatisticalOutLierRemoval<pcl::PointXYZ> sor;
// 创建滤波器对象
pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
// 设置输入点云
sor.setInputCloud(cloud);
// 设置在进行统计时考虑查询点临近点数
sor.setMeanK(50);
// 设置判断是否为离群点的阈值
sor.setStddevMulThresh(1.0);
// 执行滤波，保存过滤结果在cloud_filtered
sor.filter(*cloud_filtered);效果如下：
3.3.4 半径滤波器
算法思路：
1. 为每个点设置搜索半径r，在半径r内统计邻域点个数；
2. 设置邻域点集数阈值min，邻域点数少于min的点会被删除。
代码如下：
效果如下：
3.3.5 均匀采样
代码如下：
效果如下：
3.3.6 条件滤波器
3.3.7 索引提取
3.3.8 其他点云滤波方法
3.3.11 双边滤波
3.3.12 高斯滤波
3.2 RGB-D SLAM中的点云滤波操作
RGB-D SLAM在最后做点云拼接融合时，用到了上述两种滤波操作，分别是直通滤波器pcl::PassThrought<pcl::PointXYZ> pass和体素（网格）滤波器pcl::VoxelGrid<pcl::PCLPointCloud2> vox。原因是使用网格滤波器，用来调整地图分辨率；使用Z方向区间滤波器，由于rgbd有效深度空间有限，把太远的点去掉。
代码如下：
/*
* 拼接点云地图
* */
cout << "saving the point cloud map ..." << endl;
PointCloud::Ptr output(new PointCloud()); // 全局地图
PointCloud::Ptr tmp(new PointCloud());
pcl::VoxelGrid<PointT> voxel; // 网格滤波器，调整地图分辨率
pcl::PassThrough<PointT> pass; // z方向区间滤波器，由于rgbd相机的有效深度区间有限，把太远的去掉
pass.setFilterFieldName("z");
pass.setFilterLimits(0.0, 4.0); // 4m以上的就不要了
// ① 设置滤波时创建的体素体积为1cm的立方体
double gridsize = atof(pd.getData("voxel_grid").c_str()); // 分辨图可以在parameters.txt里调
voxel.setLeafSize(gridsize, gridsize, gridsize);
for (size_t i=0; i<keyframes.size(); i ++)
{
// 从g2o里取出一帧
g2o::VertexSE3* vertex = dynamic_cast<g2o::VertexSE3*>(globalOptimizer.vertex(keyframes[i].frameID));
Eigen::Isometry3d pose = vertex->estimate(); // 该帧优化后的位姿
PointCloud::Ptr newCloud = image2PointCloud(keyframes[i].rgb, keyframes[i].depth, camera); // 转成点云
// 以下是两种滤波操作，分别是网格滤波器和区间滤波
voxel.setInputCloud(newCloud);
voxel.filter(*tmp);
pass.setInputCloud(tmp);
pass.filter(*newCloud);
// 把点云变换后加入到全局地图中
pcl::transformPointCloud(*newCloud, *tmp, pose.matrix());
*output += *tmp;
tmp->clear();
newCloud->clear();
}
// ②设置输入点云
voxel.setInputCloud(output);
// ③执行滤波，保存过滤结果在tmp
voxel.filter(*tmp);
// 存储
pcl::io::savePCDFile("./result.pcd", *tmp);
cout << "Final map is saved." << endl;
return 0;
}
870
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
