基于点云分割的RANSAC算法实现
RANSAC算法就不叙述了,对于这种常见算法的原理讲解网上挺多的,感兴趣的可以自行百度。
操作系统:Ubuntu18.04
PCL库:1.11
写这个代码的动机,主要是最近在做点云的研究,有幸发现点云分割这个操作,就使用PCL库对点云进行了基本的平面分割处理,使用的是PCL库提供的RANSAC代码,但是发现,这个算法的效果并不是很好,基于PCL库提供的RANSAC算法代码如下:
(代码片段,基于网络上的点云平面分割及简化)
//点云平面分割
pcl::SACSegmentation<PointT> seg;
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointCloud<PointT>::Ptr cloud_plane(new pcl::PointCloud<PointT> ());
pcl::PointCloud<PointT>::Ptr cloud_f(new pcl::PointCloud<PointT>);
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (10000);
seg.setDistanceThreshold (0.01);
int nr_points = (int)cloud_filtered->points.size(), count = 0;
while(cloud_filtered->points.size() > 0.3 * nr_points){
// Segment the largest planar component from the remaining cloud
seg.setInputCloud (cloud_filtered);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0){
cout << "Could not estimate a planar model for the given dataset." << endl;
break;
}
// Extract the planar inliers from the input cloud
pcl::ExtractIndices<PointT> extract;
extract.setInputCloud (cloud_filtered);
extract.setIndices (inliers);
extract.setNegative (false);
// random color
int color_r = random()%256;
int color_g = random()%256;
int color_b = random()%256;
// Get the points associated with the planar surface
extract.filter (*cloud_plane);
cout << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << endl;
for (int i = 0; i <cloud_plane->points.size (); i++){
if (rand() %100 < 30){
pcl::PointXYZRGB data;
data.x = cloud_plane->points[i].x;
data.y = cloud_plane->points[i].y;
data.z = cloud_plane->points[i].z;
data.r = color_r;
data.g = color_g;
data.b = color_b;
Result->points.push_back(data);
}
}
// pcl::io::savePCDFileBinary("Plane_" + to_string(count) + ".pcd", *cloud_plane);
// count++;
// Remove the planar inliers, extract the rest
extract.setNegative (true);
extract.filter (*cloud_f);
*cloud_filtered = *cloud_f;
}
// random color
int color_r = random()%256;
int color_g = random()%256;
int color_b = random()%256;
for (int i = 0; i <cloud_filtered->points.size (); i++){
if (rand() %100 < 70){
pcl::PointXYZRGB data;
data.x = cloud_plane->points[i].x;
data.y = cloud_plane->points[i].y;
data.z = cloud_plane->points[i].z;
data.r = color_r;
data.g = color_g;
data.b = color_b;
Result->points.push_back(data);
}
}
效果并不是很好,主要是因为RANSAC代码会提取出来斜平面(该斜平面其实应该是两个互相垂直的平面),这里就不贴图了,主要是这个代码是之前运行的,当时并没有保存这个代码运行的结果,有兴趣的可以自己运行一下代码。
后来,就想,之所以会出现提取出斜平面的现象,是因为在做平面分割的时候,只使用了点云的空间位置信息,但是,由于我的点云信息来自于RGB-D相机,是具有颜色信息的,那么,我可不可以自己写一个RANSAC算法,添加一个颜色约束呢?这就是下面的代码:
(代码片段,基于网上的RANSAC代码修改,使用了颜色约束,同时也保留了颜色信息)
//函数:RANSAC(随机采样一致性算法),基于pcl实现,与原有的pcl平面分割算法的区别在于:在创建平面的时候考虑到了点云的颜色信息,输入对象为具有颜色信息的点云
pcl::PointCloud<pcl::PointXYZRGB>::Ptr Ransac_Plane_With_Color(pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pc, int iter, double dis_thresh, int color_thresh){
//最终要返回的结果
pcl::PointCloud<pcl::PointXYZRGB>::Ptr res(new pcl::PointCloud<pcl::PointXYZRGB>);
//输入点云的大小
int Size_pc = pc->points.size();
//平面拟合
for(int i = 0; i < iter; i++){
//临时参数初始化
int p1, p2, p3;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr Tmp(new pcl::PointCloud<pcl::PointXYZRGB>);
//随机选取三个点,需满足:1、三点不共线;2、三点颜色近似
while(true){
//随机选取三个点
p1 = random()%Size_pc;
p2 = random()%Size_pc;
p3 = random()%Size_pc;
//是否存在相同的点
if(p1 == p2 || p1 == p3 || p2 == p3)
continue;
//判断三点是否共线
if( (pc->points[p1].x - pc->points[p2].x)*(pc->points[p1].y - pc->points[p3].y) == (pc->points[p1].x - pc->points[p3].x)*(pc->points[p1].y - pc->points[p2].y) )
continue;
//判断三点的颜色是否近似
if(abs(pc->points[p1].r - pc->points[p2].r) > color_thresh || abs(pc->points[p1].g - pc->points[p2].g) > color_thresh || abs(pc->points[p1].b - pc->points[p2].b) > color_thresh ||
abs(pc->points[p1].r - pc->points[p3].r) > color_thresh || abs(pc->points[p1].g - pc->points[p3].g) > color_thresh || abs(pc->points[p1].b - pc->points[p3].b) > color_thresh ||
abs(pc->points[p3].r - pc->points[p2].r) > color_thresh || abs(pc->points[p3].g - pc->points[p2].g) > color_thresh || abs(pc->points[p3].b - pc->points[p2].b) > color_thresh)
continue;
//找到合适的三个点
break;
}
//将三个点存入临时变量点云Tmp中
Tmp->points.push_back(pc->points[p1]);
Tmp->points.push_back(pc->points[p2]);
Tmp->points.push_back(pc->points[p3]);
//使用三个点,拟合一个平面
// # A = (y2 - y1)*(z3 - z1) - (z2 - z1)*(y3 - y1);
// # B = (x3 - x1)*(z2 - z1) - (x2 - x1)*(z3 - z1);
// # C = (x2 - x1)*(y3 - y1) - (x3 - x1)*(y2 - y1);
// 平面方程表达式:A*x + B*y + C*z + D = 0
double A = (pc->points[p2].y - pc->points[p1].y)*(pc->points[p3].z - pc->points[p1].z) - (pc->points[p2].z - pc->points[p1].z)*(pc->points[p3].y - pc->points[p1].y);
double B = (pc->points[p3].x - pc->points[p1].x)*(pc->points[p2].z - pc->points[p1].z) - (pc->points[p3].z - pc->points[p1].z)*(pc->points[p2].x - pc->points[p1].x);
double C = (pc->points[p3].y - pc->points[p1].y)*(pc->points[p2].x - pc->points[p1].x) - (pc->points[p3].x - pc->points[p1].x)*(pc->points[p2].y - pc->points[p1].y);
double D = -A*pc->points[p1].x - B*pc->points[p1].y - C*pc->points[p1].z;
//遍历整个输入点云,计算每个点云与平面的距离,并将内点存入临时变量点云中
double root = sqrt(A*A+B*B+C*C);
for(int i = 0; i < pc->points.size(); i++){
if(i != p1 && i != p2 && i != p3){
double dis = abs(A*pc->points[i].x + B*pc->points[i].y + C*pc->points[i].z + D)/root;
if(dis < dis_thresh)
Tmp->points.push_back(pc->points[i]);
}
}
//比较临时变量点云与res的大小,选取点云数多的点云,并释放new申请的空间
if(Tmp->points.size() > res->points.size())
Tmp->swap(*res);
}
//返回最终结果
return res;
}
上述代码,最终返回的是从点云中分割出来的一个平面,由于在最初选取(三个)点生成平面的时候考虑到了点云之间的颜色差异不应该太大(默认同一个平面的点云颜色应该近似),效果应该会比较好一点(我并没有作对比试验),感兴趣的可以自己做一下对比实验。
上述代码还有一点问题,虽然上述代码使用到了PCL中的一些内容,但是却只能分割出一个平面,不能够将一个点云中的多个平面都分割出来。考虑到PCL库提供的RANSAC算法其实也只能分割出一个面,之所以能将点云中的多个面都分离出来,是因为使用了extract函数,可以根据点云索引向量将输入点云的分成两部分。
因此,又将代码改成了下面这一版(但是这版在提取出平面之后会失去颜色信息,原因是extract的输入点云对象为pcl::PointCloudpcl::PointXYZ,不可以是pcl::PointCloudpcl::PointXYZRGB),代码如下:
(代码片段)
//函数:RANSAC(随机采样一致性算法),基于pcl实现,与原有的pcl平面分割算法的区别在于:在创建平面的时候考虑到了点云的颜色信息,输入对象为具有颜色信息的点云
void Ransac_Plane_With_Color(pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pc, int iter, double dis_thresh, int color_thresh, vector<int> &Res){
//输入点云的大小
int Size_pc = pc->points.size();
//平面拟合
for(int i = 0; i < iter; i++){
//临时参数初始化
int p1, p2, p3;
vector<int> Tmp;
//随机选取三个点,需满足:1、三点不共线;2、三点颜色近似
while(true){
//随机选取三个点
p1 = random()%Size_pc;
p2 = random()%Size_pc;
p3 = random()%Size_pc;
//是否存在相同的点
if(p1 == p2 || p1 == p3 || p2 == p3)
continue;
//判断三点是否共线
if( (pc->points[p1].x - pc->points[p2].x)*(pc->points[p1].y - pc->points[p3].y) == (pc->points[p1].x - pc->points[p3].x)*(pc->points[p1].y - pc->points[p2].y) )
continue;
//判断三点的颜色是否近似
if(abs(pc->points[p1].r - pc->points[p2].r) > color_thresh || abs(pc->points[p1].g - pc->points[p2].g) > color_thresh || abs(pc->points[p1].b - pc->points[p2].b) > color_thresh ||
abs(pc->points[p1].r - pc->points[p3].r) > color_thresh || abs(pc->points[p1].g - pc->points[p3].g) > color_thresh || abs(pc->points[p1].b - pc->points[p3].b) > color_thresh ||
abs(pc->points[p3].r - pc->points[p2].r) > color_thresh || abs(pc->points[p3].g - pc->points[p2].g) > color_thresh || abs(pc->points[p3].b - pc->points[p2].b) > color_thresh)
continue;
//找到合适的三个点
break;
}
//使用三个点,拟合一个平面
// # A = (y2 - y1)*(z3 - z1) - (z2 - z1)*(y3 - y1);
// # B = (x3 - x1)*(z2 - z1) - (x2 - x1)*(z3 - z1);
// # C = (x2 - x1)*(y3 - y1) - (x3 - x1)*(y2 - y1);
// 平面方程表达式:A*x + B*y + C*z + D = 0
double A = (pc->points[p2].y - pc->points[p1].y)*(pc->points[p3].z - pc->points[p1].z) - (pc->points[p2].z - pc->points[p1].z)*(pc->points[p3].y - pc->points[p1].y);
double B = (pc->points[p3].x - pc->points[p1].x)*(pc->points[p2].z - pc->points[p1].z) - (pc->points[p3].z - pc->points[p1].z)*(pc->points[p2].x - pc->points[p1].x);
double C = (pc->points[p3].y - pc->points[p1].y)*(pc->points[p2].x - pc->points[p1].x) - (pc->points[p3].x - pc->points[p1].x)*(pc->points[p2].y - pc->points[p1].y);
double D = -A*pc->points[p1].x - B*pc->points[p1].y - C*pc->points[p1].z;
//遍历整个输入点云,计算每个点云与平面的距离,并将内点存入临时变量点云中
double root = sqrt(A*A+B*B+C*C);
for(int i = 0; i < pc->points.size(); i++){
double dis = abs(A*pc->points[i].x + B*pc->points[i].y + C*pc->points[i].z + D)/root;
if(dis < dis_thresh)
Tmp.push_back(i);
}
//比较临时变量点云与res的大小,选取点云数多的点云,并释放new申请的空间
if(Tmp.size() > Res.size())
Tmp.swap(Res);
}
//返回最终结果
return;
}
上述代码和之前的代码的返回值有些不一样,需要改变的就可以自己改一下,对于上述代码在主函数的使用,如下:
(代码片段)
if(pcl::io::loadPCDFile("pc.pcd", *cloud_filtered) == -1 ){
PCL_ERROR(" Couldn't Find .pcd File! ");
return -1;
}
if(pcl::io::loadPCDFile("pc.pcd", *cloud_2) == -1 ){
PCL_ERROR(" Couldn't Find .pcd File! ");
return -1;
}
vector<int> Result;
Ransac_Plane_With_Color(cloud_filtered, 500, 0.03, 10, Result);
std::shared_ptr<std::vector<int>> index_ptr = std::make_shared<std::vector<int>>(Result);
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud(cloud_2);
extract.setIndices(index_ptr);
extract.setNegative(false);
extract.filter(*cloud_f);
上述代码其实还有点问题,这是因为extract的对象是clouf_2,而点云分割的对象是cloud_filtered,因此,还是只能提取出一个平面。所以,又写了一个简单的代码,根据索引将点云分成两部分,代码如下:
(代码片段)
//函数:对象:pcl::PointCloiud<pcl::PointXYZRGB>,根据索引,将输入点云中的点云删除,并返回索引对应的点云
pcl::PointCloud<pcl::PointXYZRGB>::Ptr Extract(pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pc, vector<int>& index){
pcl::PointCloud<pcl::PointXYZRGB>::Ptr res(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZRGB>);
for(int i = 0; i < index.size(); i++){
res->points.push_back(pc->points[index[i]]);
pc->points[index[i]].x = 0;
pc->points[index[i]].y = 0;
pc->points[index[i]].z = 0;
pc->points[index[i]].r = 0;
pc->points[index[i]].g = 0;
pc->points[index[i]].b = 0;
}
for(auto iter = begin(pc->points); iter < end(pc->points); iter++){
if(iter->x == 0 && iter->y == 0 && iter->z == 0 && iter->r == 0 && iter->g == 0 && iter->z == 0)
continue;
tmp->points.push_back(*iter);
}
tmp->swap(*pc);
return res;
}
这样,使用Extract函数和Ransac_Plane_With_Color函数就可以提取出多个点云平面。具体代码并没有应用,所以,不知道是否存在问题,大家如果发现问题和我交流。
最后,附一下CMakeLists.txt文件中的部分内容,记得自己刚开始学的时候,对于如何写第三方库的CMake文件甚是头疼,具体如下:
# Find PCL
find_package(PCL REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})
add_definitions(${PCL_DEFINITIONS})
#traget_link
add_executable(XX XXX.cpp)
target_link_libraries(XX ${PCL_LIBRARIES})
感兴趣的可以将对比试验都做一做,然后将多个平面提取的代码写一写,大家互相交流,共同进步。
参考资料:
1、一种平面提取的点云简化算法
https://www.cnblogs.com/lwngreat/p/4312749.html
2、基于RANSAC算法PCL点云拟合平面整理
https://blog.csdn.net/qq_18941713/article/details/84647887?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522162514730816780262587756%2522%252C%2522scm%2522%253A%252220140713.130102334…%2522%257D&request_id=162514730816780262587756&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2allsobaiduend~default-2-84647887.nonecase&utm_term=%E7%82%B9%E4%BA%91%E5%B9%B3%E9%9D%A2%E6%8B%9F%E5%90%88&spm=1018.2226.3001.4187