读自动驾驶激光雷达物体检测技术（Lidar Obstacle Detection）（3）：Segmentation
发布时间
阅读量：
阅读量
分割任务的目标是将道路相关点与场景相关点区分开来，在该细分领域中研究者开发了两种方法。
第一种：
下图中的12分别是第一种程序保存的点云结果。
下面是保存的点云的可视化结果：
obstCloud可视化结果
planeCloud可视化结果
针对本项目, 开发并实现了两个关键函数SegmentPlane和SeparateClouds. 其中, SegmentPlane用于识别位于道路平面内的内联点(inliers), 而SeparateClouds则用于分离出非道路物体(outliers)的空间点云数据.
在SegmentPlane函数中, 我们采用参数列表的形式接收输入, 其中包含点云数据、最大迭代步数以及距离阈值等关键参数. 该函数通过std::pair对象来组织物体与地面的点云信息. 非inliers点通常代表障碍物, 因此SeparateClouds函数的作用就是提取这些障碍物信息. 粒子分割算法是一种循环处理的过程, 在提升分割精度的同时也增加了计算复杂度. 如果设置的距离阈值过大, 可能会导致同一物体被错误识别为多个分离物体; 相反, 如果阈值设置过小, 则可能导致长物体被误判为多个独立物体（例如卡车）.
头文件：
//segmentation.h
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/crop_box.h>
#include <pcl/visualization/cloud_viewer.h>
#include<vector>
#include <iostream>
#include <string>
#include<Eigen/Core>
#include<Eigen/Dense>
#include <pcl/common/common.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/common/transforms.h>
#include <unordered_set>
#include <ctime>
#include <chrono>
using namespace std;
using namespace pcl;
struct Color //结构体中使用构造函数初始化列表
{
float r, g, b;
//Color(float setR, float setG, float setB): r(setR), g(setG), b(setB){} //构造函数初始化列表
Color(float setR, float setG, float setB) //含有参数的构造函数，以便创建Color变量时不向其传递参数时，提供默认值
{
r = setR;
g = setG;
b = setB;
}
};
std::pair<pcl::PointCloud<pcl::PointXYZ>::Ptr, pcl::PointCloud<pcl::PointXYZ>::Ptr> SegmentPlane(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, int maxIterations, float distanceTol);
std::pair<pcl::PointCloud<pcl::PointXYZ>::Ptr, pcl::PointCloud<pcl::PointXYZ>::Ptr> SeparateClouds(pcl::PointIndices::Ptr inliers, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud);
void renderPointCloud(pcl::visualization::PCLVisualizer::Ptr& viewer, const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, std::string name, Color color = Color(1, 1, 1));
void renderPointCloud(pcl::visualization::PCLVisualizer::Ptr& viewer, const pcl::PointCloud<pcl::PointXYZI>::Ptr& cloud, std::string name, Color color = Color(-1, -1, -1));
AI助手源文件：
#include "segmentation.h"
//——————————————(2.1)————————————此处使用了pcl点云库中的模型分割 程序中貌似没有用到 ok
std::pair<pcl::PointCloud<pcl::PointXYZ>::Ptr, pcl::PointCloud<pcl::PointXYZ>::Ptr> SegmentPlane(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, int maxIterations, float distanceThreshold)
{
// Time segmentation process
auto startTime = std::chrono::steady_clock::now();
// pcl::PointIndices::Ptr inliers; // Build on the stack
pcl::PointIndices::Ptr inliers(new pcl::PointIndices); // Build on the heap
// TODO:: Fill in this function to find inliers for the cloud.
pcl::ModelCoefficients::Ptr coefficient(new pcl::ModelCoefficients);
pcl::SACSegmentation<pcl::PointXYZ> seg;//创建分割对象
//可选
seg.setOptimizeCoefficients(true);//设置对估计的模型参数进行优化处理
//必选
seg.setModelType(pcl::SACMODEL_PLANE);设置分割模型类别
seg.setMethodType(pcl::SAC_RANSAC);//设置用哪个随机参数估计方法
seg.setMaxIterations(maxIterations);设置最大迭代次数
seg.setDistanceThreshold(distanceThreshold);//设置判断是否为模型内点的距离阈值
// Segment the largest planar component from the remaining cloud
seg.setInputCloud(cloud);
seg.segment(*inliers, *coefficient);
if (inliers->indices.size() == 0)
{
std::cerr << "Could not estimate a planar model for the given dataset" << std::endl;
}
auto endTime = std::chrono::steady_clock::now();
auto elapsedTime = std::chrono::duration_cast<std::chrono::milliseconds>(endTime - startTime);
std::cout << "plane segmentation took " << elapsedTime.count() << " milliseconds" << std::endl;
std::pair<pcl::PointCloud<pcl::PointXYZ>::Ptr, pcl::PointCloud<pcl::PointXYZ>::Ptr> segResult = SeparateClouds(inliers, cloud);
return segResult;
}
//———————————————(2.2)———————————对点云库中的模型分割，将点云保存为两个文件 主程序中依然好像没有用到 ok
std::pair<pcl::PointCloud<pcl::PointXYZ>::Ptr, pcl::PointCloud<pcl::PointXYZ>::Ptr> SeparateClouds(pcl::PointIndices::Ptr inliers, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
//创建了两个点云块，一个存放障碍物，一个存放地面
pcl::PointCloud<pcl::PointXYZ>::Ptr obstCloud(new pcl::PointCloud<pcl::PointXYZ>());
pcl::PointCloud<pcl::PointXYZ>::Ptr planeCloud(new pcl::PointCloud<pcl::PointXYZ>());
for (int index : inliers->indices)
{
planeCloud->points.push_back(cloud->points[index]);
}
// create extraction object
pcl::ExtractIndices<pcl::PointXYZ> extract;//创建点云提取对象
extract.setInputCloud(cloud);//设置输入点云
extract.setIndices(inliers);//设置分割后的内点为需要提取的点集
extract.setNegative(true);//true是保留外点
extract.filter(*obstCloud);//提取输出存储到obstCloud
std::pair<pcl::PointCloud<pcl::PointXYZ>::Ptr, pcl::PointCloud<pcl::PointXYZ>::Ptr> segResult(obstCloud,
planeCloud);
pcl::PCDWriter writer; //只会写入obstCloud
writer.write("F:\ SFND_Lidar_Obstacle_Detection\ SFND_Lidar_Obstacle_Detection\ data\ pcd\ pcd\ obstCloud.pcd", *obstCloud);
extract.setNegative(false);//设置提取内点而非外点
extract.filter(*planeCloud);//提取输出存储到planeCloud
writer.write("F:\ SFND_Lidar_Obstacle_Detection\ SFND_Lidar_Obstacle_Detection\ data\ pcd\ pcd\ planeCloud.pcd", *planeCloud);
// std::pair<PtCdtr<PointT>, PtCdtr<PointT>> segResult(cloud, cloud);
return segResult;
}
void renderPointCloud(pcl::visualization::PCLVisualizer::Ptr& viewer, const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, std::string name, Color color)
{
viewer->addPointCloud<pcl::PointXYZ>(cloud, name);
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, name);
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, color.r, color.g, color.b, name);
}
void renderPointCloud(pcl::visualization::PCLVisualizer::Ptr& viewer, const pcl::PointCloud<pcl::PointXYZI>::Ptr& cloud, std::string name, Color color)
{
if (color.r == -1)
{
// Select color based off of cloud intensity
pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZI> intensity_distribution(cloud, "intensity");
viewer->addPointCloud<pcl::PointXYZI>(cloud, intensity_distribution, name);
}
else
{
// Select color based off input value
viewer->addPointCloud<pcl::PointXYZI>(cloud, name);
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, color.r, color.g, color.b, name);
}
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, name);
}
AI助手主函数：
#include "filter.h"
int main(int argc, char **argv)
{
bool render_obst = false;
bool render_plane = false;
// 1、输入的是 滤波后存入的点云filter.pcd
pcl::PointCloud<pcl::PointXYZ>::Ptr inputCloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PCDReader reader;
reader.read("F:\ SFND_Lidar_Obstacle_Detection\ SFND_Lidar_Obstacle_Detection\ data\ pcd\ pcd\ filter.pcd", *inputCloud);
pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("显示窗口")); //窗口显示点云
std::pair<pcl::PointCloud<pcl::PointXYZ>::Ptr, pcl::PointCloud<pcl::PointXYZ>::Ptr> segmentCloud = SegmentPlane(
inputCloud, 100, 0.2);//-------------------------(2.1)
if (render_obst)
{
renderPointCloud(viewer, segmentCloud.first, "obstCloud", Color(1, 0, 0));
}
if (render_plane) {
renderPointCloud(viewer, segmentCloud.second, "planeCloud", Color(0, 1, 0));
}
viewer->addPointCloud(segmentCloud.first, "*cloud");
viewer->resetCamera();
system("pause");
return (0);
}
AI助手第二种：基于Manual RANSAC Segmentation实现的分割效果展示：
代码涉及到的知识：
C++ unordered_set定义及初始化详解：http://c.biancheng.net/view/546.html
容器中的count()函数：<>
count()函数的作用如下：它用于计算指定值在整个序列中的出现次数。调用count()方法时需传递三个参数：起始位置first、结束位置last以及目标值value。其中first和last分别代表序列的首和尾迭代器（或指针），而value则为要查询的目标元素。
下图代表的是输入，是在上个博客处理过的滤波后的点云：
输入滤波后的点云数据
下图表示的是分割的效果。
提取的点云数据
保存的out_plane结果：
out_plane
保存的in_plane结果：
in_plane
代码展示：
目前粒子分割主要采用RANSAC算法进行处理。该算法全称为Random Sample Consensus（随机样本一致），其主要用于检测数据中的异常值并构建模型。经过多次迭代后返回最佳模型：每次迭代均随机选取数据的一个子集并生成一个模型拟合该子样本（如一条直线或一个平面）。其中一种优化方法是基于最少内联点（inliers）的选择：对于直线采用两点确定一条直线的方式；对于平面则需要三点即可确定一个平面方程。随后计算每个剩余点与模型之间的距离以确定内联点数量：距离小于设定阈值的距离点被视为内联点。具有最多内联点的一次迭代即被认定为最佳模型。这正是本项目中所采用的方法：通过不断寻找拥有最多内联点的最优模型从而排除离群点的影响。另外一种策略是基于一定比例采样：例如从总样本中选取20%的数据点进行建模并计算误差；误差最小的一次迭代即被选作最佳模型此方法的优势在于无需逐个计算每一点与当前模型的距离从而提高效率以下配图展示了基于RANSAC算法的一条直线拟合情况真实激光雷达数据下则是对地面平面进行拟合从而分离出地面物体与障碍物以下将单独实现基于RANSAC算法的平面拟合模块
以下为平面RANSAC的主体代码。（但在实际中PCL已经内置了RANSAC函数, 而且比写的这个计算速度更快, 所以直接用内置的就行了.）
#include <iostream>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/cloud_viewer.h>
#include <unordered_set>
#include<cstdlib>
using namespace std;
int main()
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PCDReader reader;
reader.read("F:\ SFND_Lidar_Obstacle_Detection\ SFND_Lidar_Obstacle_Detection\ data\ pcd\ pcd\ filter.pcd", *cloud);
int num_points = cloud->points.size();
cout <<"输入点云的总点数："<< num_points << endl; //2063
std::unordered_set<int> inliersResult;//是个容器
int maxIterations = 40; //迭代次数
while (maxIterations--) //
{
std::unordered_set<int> inliers; //存放平面的内点，平面上的点也属于平面的内点
//因为一开始定义inliers，内点并没有存在，所以随机在原始点云中随机选取了三个点作为点云的求取平面系数的三个点
while (inliers.size() < 3) //当内点小于3 就随机选取一个点 放入内点中 也就是需要利用到三个内点
{
inliers.insert(rand() % num_points); //产生 0~num_points 中的一个随机数
}//是为了随机的选取三个点
// 需要至少三个点 才能找到地面
float x1, y1, z1, x2, y2, z2, x3, y3, z3;
auto itr = inliers.begin(); //auto 自动类型 //itr是个迭代器
x1 = cloud->points[*itr].x;
y1 = cloud->points[*itr].y;
z1 = cloud->points[*itr].z;
itr++;
x2 = cloud->points[*itr].x;
y2 = cloud->points[*itr].y;
z2 = cloud->points[*itr].z;
itr++;
x3 = cloud->points[*itr].x;
y3 = cloud->points[*itr].y;
z3 = cloud->points[*itr].z;
//计算平面系数
float a, b, c, d, sqrt_abc;
a = (y2 - y1) * (z3 - z1) - (z2 - z1) * (y3 - y1);
b = (z2 - z1) * (x3 - x1) - (x2 - x1) * (z3 - z1);
c = (x2 - x1) * (y3 - y1) - (y2 - y1) * (x3 - x1);
d = -(a * x1 + b * y1 + c * z1);
sqrt_abc = sqrt(a * a + b * b + c * c);
//分别计算这些点到平面的距离
for (int i = 0; i < num_points; i++)//遍历输入点云内的所有点，判断该点距离找到平面的距离是否在阈值范围内
{
if (inliers.count(i) > 0) //判断一下有没有内点
{ // that means: if the inlier in already exist, we dont need do anymore
continue;
}
pcl::PointXYZ point = cloud->points[i];
float x = point.x;
float y = point.y;
float z = point.z;
float dist = fabs(a * x + b * y + c * z + d) / sqrt_abc; // calculate the distance between other points and plane
float distanceTol = 0.3;
if (dist < distanceTol)//如果距离平面的距离小于设定的阈值就把他归为平面上的点
{
inliers.insert(i); //如果点云中的点 距离平面距离的点比较远 那么该点则视为内点
}
//将inliersResult 中的内容不断更新，因为地面的点一定是最多的，所以迭代40次 找到的inliersResult最大时（即找到最大的那个平面），也就相当于找到了地面
//inliersResult 中存储的也就是地面上的点云
if (inliers.size() > inliersResult.size())
{
inliersResult = inliers;
}
}
//cout <<"每次迭代提取的平面点数："<< inliers.size() << endl;
}
//迭代结束后，所有属于平面上的内点都存放在inliersResult中
//std::unordered_set<int> inliersResult;
cout << "总迭代完成后找到的最大平面点数："<< inliersResult.size() << endl; //1633
//创建两片点云，一片存放地面，一片存放非地面的其他点
-----------111111111111111
/*pcl::PointCloud<pcl::PointXYZ>::Ptr out_plane(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr in_plane(new pcl::PointCloud<pcl::PointXYZ>);*/
pcl::PointCloud<pcl::PointXYZ> out_plane;//此处是反着定义的，out_plane代表的是平面
pcl::PointCloud<pcl::PointXYZ> in_plane;//此处是反着定义的，in_plane代表的是非平面
for (int i = 0; i < num_points; i++)//遍历点云中所有的点
{
pcl::PointXYZ pt = cloud->points[i];
if (inliersResult.count(i))//if非0为真；容器中的count函数是统计容器中等于value元素的个数；count(first,last,value); first是容器的首迭代器，last是容器的末迭代器，value是询问的元素。
{
//-----------111111111111111
//out_plane->points.push_back(pt);
out_plane.push_back(pt);//if为真表明遍历到的该点是存在于查找到的平面上的点，则将该点保存到创建的平面点云中。
}
else
{
//-----------111111111111111
//in_plane->points.push_back(pt);
in_plane.push_back(pt);
}
}
使用非ptr得点云定义在使用write函数时会很容易保存成功。如果定义ptr形式得out_cloud,in_cloud，在写入时有时间会不成功但是也不保存只是调试得时候可以看见异常
pcl::PCDWriter writer;//保存提取的点云文件
writer.write("F:\ SFND_Lidar_Obstacle_Detection\ SFND_Lidar_Obstacle_Detection\ data\ pcd\ pcd\ out_plane.pcd", out_plane);
writer.write("F:\ SFND_Lidar_Obstacle_Detection\ SFND_Lidar_Obstacle_Detection\ data\ pcd\ pcd\ in_plane.pcd", in_plane);
使用非指针的点云结构就是不是ptr定义的cloud的话，就不能用viewer,至于为什么个人觉得viewer->addPointCloud()这个函数只接受指针点云传入
//pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("显示窗口")); //窗口显示点云
//viewer->addPointCloud(in_plane, "*cloud");
//viewer->resetCamera(); //相机点重置
//viewer->spin();
system("pause");
return (0);
}
AI助手全部评论 (0)
还没有任何评论哟~