行业知识
5.【自动驾驶与机器人中的SLAM技术】2D点云的scan matching算法 和 检测退化场景的思路
目录
- 1. 基于优化的点到点/线的配准
- 2. 对似然场图像进行插值，提高匹配精度
- 3. 对二维激光点云中会对SLAM功能产生退化场景的检测
- 4. 在诸如扫地机器人等这样基于2D激光雷达导航的机器人，如何处理悬空/低矮物体
- 5. 也���迎大家来我的读书号--过千帆，学习交流。
1. 基于优化的点到点/线的配准
这里实现了基于g2o优化器的优化方法。
图优化中涉及两个概念-顶点和边。我们的优化变量认为是顶点，误差项就是边。我们通过g2o声明一个图模型，然后往图模型中添加顶点和与顶点相关联的边，再选定优化算法（比如LM）就可以进行优化了。想熟悉g2o的小伙伴们感兴趣的话，可以到这个链接看一下。
g2o的基本框架和编程套路如下图：
基于g2o的点对点的配准算法代码实现如下：
bool Icp2d::AlignWithG2oP2P(SE2& init_pose){ double rk_delta = 0.8; // 核函数阈值 float max_dis2 = 0.01; // 最近邻时的最远距离（平方） int min_effect_pts = 20; // 最小有效点数 // 构建图优化，先设定g2o typedef g2o::BlockSolver BlockSolverType; // 每个误差项优化变量维度为3， 误差值维度是2 typedef g2o::LinearSolverDense LinearSolverType; // 线性求解器类型 // 梯度下降方法，可以从GN, LM, DogLeg 中选 auto solver = new g2o::OptimizationAlgorithmLevenberg( g2o::make_unique(g2o::make_unique())); g2o::SparseOptimizer optimizer; // 图模型 optimizer.setAlgorithm(solver); // 设置求解器 optimizer.setVerbose(false); // 打开调试输出 // 往图中添加顶点 VertexSE2 *v = new VertexSE2(); // 新建SE2位姿顶点 v->setId(0); // 设置顶点的id v->setEstimate(init_pose); // 设置顶点的估计值为初始位姿 optimizer.addVertex(v); // 将顶点添加到优化器中 // 往图中添加边 int effective_num = 0; // 有效点数 // 遍历source for (size_t i = 0; i ranges.size(); ++i) { float range = source_scan_->ranges[i]; if (range range_min || range > source_scan_->range_max) { continue; } float angle = source_scan_->angle_min + i * source_scan_->angle_increment; float theta = v->estimate().so2().log(); Vec2d pw = v->estimate() * Vec2d(range * std::cos(angle), range * std::sin(angle)); Point2d pt; pt.x = pw.x(); pt.y = pw.y(); // 最近邻 std::vector nn_idx; std::vector dis; kdtree_.nearestKSearch(pt, 1, nn_idx, dis); if (nn_idx.size() > 0 && dis[0] points[nn_idx[0]].x, target_cloud_->points[nn_idx[0]].y); // 当前激光点在目标点云中的最近邻点坐标 auto *edge = new EdgeIcpP2P(range, angle, theta); // 构建约束边 edge->setVertex(0, v); // 设置连接的顶点 edge->setMeasurement(qw); // 观测，即最近邻 edge->setInformation(Mat2d::Identity()); // 观测为2维点坐标，因此信息矩阵需设为2x2单位矩阵 auto rk = new g2o::RobustKernelHuber; // Huber鲁棒核函数 rk->setDelta(rk_delta); // 设置阈值 edge->setRobustKernel(rk); // 为边设置鲁棒核函数 optimizer.addEdge(edge); // 将约束边添加到优化器中 } } if (effective_num estimate(); LOG(INFO) double rk_delta = 0.8; // 核函数阈值 float max_dis2 = 0.01; // 最近邻时的最远距离（平方） int min_effect_pts = 20; // 最小有效点数 // 构建图优化，先设定g2o typedef g2o::BlockSolversetEstimate(init_pose); // 设置顶点的估计值为初始位姿 optimizer.addVertex(v); // 将顶点添加到优化器中 // 往图中添加边 int effective_num = 0; // 有效点数 // 遍历source for (size_t i = 0; i ranges.size(); ++i) { float range = source_scan_->ranges[i]; if (range range_min || range > source_scan_->range_max) { continue; } float angle = source_scan_->angle_min + i * source_scan_->angle_increment; float theta = v->estimate().so2().log(); Vec2d pw = v->estimate() * Vec2d(range * std::cos(angle), range * std::sin(angle)); Point2d pt; pt.x = pw.x(); pt.y = pw.y(); // 查找5个最近邻 std::vector nn_idx; std::vector dis; kdtree_.nearestKSearch(pt, 5, nn_idx, dis); std::vector effective_pts; // 有效点 for (int j = 0; j points[nn_idx[j]].x, target_cloud_->points[nn_idx[j]].y)); } } if (effective_pts.size() setVertex(0, v); // 设置连接的顶点 edge->setInformation(Eigen::Matrix::Identity()); // 误差为1维 auto rk = new g2o::RobustKernelHuber; // Huber鲁棒核函数 rk->setDelta(rk_delta); // 设置阈值 edge->setRobustKernel(rk); // 为边设置鲁棒核函数 optimizer.addEdge(edge); // 将约束边添加到优化器中 } } if (effective_num estimate(); LOG(INFO) if (data.size()双线性插值的过程如图：
对应到程序中：
a = y y ; a=yy; a=yy;
b = x x ; b=xx; b=xx;
v 1 = d a t a [ 0 ] ; v1=data[0]; v1=data[0];
v 2 = d a t a [ 1 ] ; v2=data[1]; v2=data[1];
v 3 = d a t a [ i m g . s t e p / s i z e o f ( T ) ] ; v3=data[img.step / sizeof(T)]; v3=data[img.step/sizeof(T)];
v 4 = d a t a [ i m g . s t e p / s i z e o f ( T ) + 1 ] ; v4=data[img.step / sizeof(T) + 1]; v4=data[img.step/sizeof(T)+1];
===================================>可得：
v 7 = f l o a t ( ( 1 − x x ) ∗ ( 1 − y y ) ∗ d a t a [ 0 ] + x x ∗ ( 1 − y y ) ∗ d a t a [ 1 ] + ( 1 − x x ) ∗ y y ∗ d a t a [ i m g . s t e p / s i z e o f ( T ) ] + x x ∗ y y ∗ d a t a [ i m g . s t e p / s i z e o f ( T ) + 1 ] ) ; v7=float((1 - xx) * (1 - yy) * data[0] + xx * (1 - yy) * data[1] + (1 - xx) * yy * data[img.step / sizeof(T)] + xx * yy * data[img.step / sizeof(T) + 1]); v7=float((1−xx)∗(1−yy)∗data[0]+xx∗(1−yy)∗data[1]+(1−xx)∗yy∗data[img.step/sizeof(T)]+xx∗yy∗data[img.step/sizeof(T)+1]);
效果展示：
3. 对二维激光点云中会对SLAM功能产生退化场景的检测
视频中高博讲到对于2DSLAM来说，可以对点云进行直线拟合，比较场景中直线方向是否“大体一致”。
具体思路：可以对场景中的点云进行聚类，然后对每个类簇中的点进行直线拟合，保存所有直线的系数。参考提示中的条件判断实现该功能。
实现代码如下：
bool Icp2d::IsDegeneration(){ if (target_cloud_->empty()) { LOG(ERROR) size(); if(point_size range_min || target_scan_->ranges[i] > target_scan_->range_max) { continue; } double real_angle = target_scan_->angle_min + i * target_scan_->angle_increment; PointType p; p.x = target_scan_->ranges[i] * std::cos(real_angle); p.y = target_scan_->ranges[i] * std::sin(real_angle); p.z = 1.0; target_cloud->points.push_back(p); } pcl::search::KdTree::Ptr kdtree; // (new pcl::search::KdTree) kdtree = boost::make_shared(); // 构建kdtree kdtree->setInputCloud(target_cloud); pcl::EuclideanClusterExtraction clusterExtractor; // 创建一个向量来存储聚类的结果 std::vector cluster_indices; clusterExtractor.setClusterTolerance(0.02); // 设置聚类的距离阈值 clusterExtractor.setMinClusterSize(10); // 设置聚类的最小点数 clusterExtractor.setMaxClusterSize(1000); // 设置聚类的最大点数 clusterExtractor.setSearchMethod(kdtree); // 使用kdtree树进行加速 clusterExtractor.setInputCloud(target_cloud); // 设置点云聚类对象的输入点云数据 clusterExtractor.extract(cluster_indices); // 执行点云聚类 LOG(INFO) LOG(INFO) pts.emplace_back(Eigen::Vector2d(pt.x, pt.y)); } // 拟合直线，组装J、H和误差 Eigen::Vector3d line_coeffs; // 利用当前点附近的几个有效近邻点，基于SVD奇异值分解，拟合出ax+by+c=0 中的最小直线系数 a,b,c，对应公式（6.11） if (math::FitLine2D(pts, line_coeffs)) { line_coeffs_vector.push_back(line_coeffs); } // 为聚类分配不同的颜色 double r = static_cast if(clusterNumber == 1) return true; // 如果仅有一条直线，就是退化场景 float max_score = 0; for(int i = 0; i
下一篇：服务器raid的读写怎么测试?
