RANSAC(随机采样一致性算法)代码实现(C++版本)

基于点云分割的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

  • 1
    点赞
  • 22
    收藏
    觉得还不错? 一键收藏
  • 0
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值