[小汪学习日记02]pcl基于模型的球拟合方法

已于 2023-09-01 15:24:15 修改
阅读量256 收藏
点赞数 2
分类专栏： pcl学习之路 文章标签： c++
于 2023-09-01 13:59:22 首次发布
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/ghgghhojhgg/article/details/132139540
版权

一、软件环境

        win10、pcl1.11.0、vs2019。

二、代码介绍

        刚学习完平面拟合，想自己试试根据算法写个球拟合的代码，另外不用更改pcl库的文件，放一个文件里，后续这样想加点约束条件来改进试试，其实就是RANSAC算法，利用mt19937来确定四个随机点，四点确定一个圆判断内外点。本人萌新，写的有些粗糙，请见谅。

三、算法介绍

算法参考链接：根据空间4点坐标求球心坐标 - 百度文库 (baidu.com)

四、详细代码

#include <pcl/io/pcd_io.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_sphere.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/point_types.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <boost/thread/thread.hpp>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/io/io.h>
#include <pcl/PolygonMesh.h>
#include <pcl/point_cloud.h>
#include <pcl/io/vtk_lib_io.h>
#include<pcl/features/normal_3d.h>
#include<pcl/features/principal_curvatures.h>
#include <vector>
#include <iostream>
#include <random>
using namespace std;
using namespace pcl;
int user_data;
void
viewerPsycho(pcl::visualization::PCLVisualizer& viewer)
{
    static unsigned count = 0;
    std::stringstream ss;
    ss << "Once per viewer loop: " << count++;
    viewer.removeShape("text", 0);
    viewer.addText(ss.str(), 200, 300, "text", 0);

    //FIXME: possible race condition here:
    user_data++;
}

bool isPointDuplicate(const pcl::PointXYZ& point, const std::vector<pcl::PointXYZ>& selectedPoints)
{
    for (const auto& selectedPoint : selectedPoints)
    {
        if (point.x == selectedPoint.x && point.y == selectedPoint.y && point.z == selectedPoint.z)
        {
            return true;
        }
    }
    return false;
}
int main()
{
    pcl::PointCloud<pcl::PointNormal>::Ptr cloud_inputsb(new pcl::PointCloud<pcl::PointNormal>);
    if (pcl::io::loadPCDFile("D://code//Project5//normal_with_cur.pcd", *cloud_inputsb) < 0)
    {
        PCL_ERROR("点云读取失败！\n");
        return -1;
    }
    cout << "加载点云点数：" << cloud_inputsb->points.size() << endl;
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_input(new pcl::PointCloud<pcl::PointXYZ>);
    if (pcl::io::loadPCDFile("D://code//Project5//1.pcd", *cloud_input) < 0)
    {
        PCL_ERROR("点云读取失败！\n");
        return -1;
    }
    cout << "加载点云点数：" << cloud_input->points.size() << endl;

    std::random_device rd;
    std::mt19937 gen(rd());
    std::uniform_int_distribution<> dist(0, cloud_input->size() - 1);
    double jieshou_x = 0;
    double jieshou_y = 0;
    double jieshou_z = 0;
    double banjing = 0;
    double zuijia_x = 0;
    double zuijia_y = 0;
    double zuijia_z = 0;
    double zuijia_banjing = 0;
    double zuijia_neidianlv;
    double distance = 0;
    double neidianlv_indices = 0;
    double neidianlv_index = 0;
    int diedaicishu = 10000;
    double x_centre = 0;
    double y_centre = 0;
    double z_centre = 0;
    for (int i = 0; i < diedaicishu; i++)
    {
        pcl::PointCloud<pcl::PointXYZ>::Ptr neidianji(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::PointCloud<pcl::PointXYZ>::Ptr waidianji(new pcl::PointCloud<pcl::PointXYZ>);
        neidianji->resize(cloud_input->size());
        waidianji->resize(cloud_input->size());
        double neidianjishu = 0;
        double waidianjishu = 0;

        std::vector<pcl::PointXYZ> selectedPoints;
        while (selectedPoints.size() < 4)
        {
            int index = dist(gen);
            pcl::PointXYZ selectedPoint = cloud_input->points[index];
            if (!isPointDuplicate(selectedPoint, selectedPoints))
            {
                selectedPoints.push_back(selectedPoint);
                std::cout << "Selected point: (" << selectedPoint.x << ", " << selectedPoint.y << ", " << selectedPoint.z << ")" << std::endl;
            }
        }
        double p1x = selectedPoints[0].x;
        double p1y = selectedPoints[0].y;
        double p1z = selectedPoints[0].z;
        double p2x = selectedPoints[1].x;
        double p2y = selectedPoints[1].y;
        double p2z = selectedPoints[1].z;
        double p3x = selectedPoints[2].x;
        double p3y = selectedPoints[2].y;
        double p3z = selectedPoints[2].z;
        double p4x = selectedPoints[3].x;
        double p4y = selectedPoints[3].y;
        double p4z = selectedPoints[3].z;
        double a = p1x - p2x, b = p1y - p2y, c = p1z - p2z;
        double a1 = p3x - p4x, b1 = p3y - p4y, c1 = p3z - p4z;
        double a2 = p2x - p3x, b2 = p2y - p3y, c2 = p2z - p3z;
        double A = p1x * p1x - p2x * p2x;
        double B = p1y * p1y - p2y * p2y;
        double C = p1z * p1z - p2z * p2z;
        double A1 = p3x * p3x - p4x * p4x;
        double B1 = p3y * p3y - p4y * p4y;
        double C1 = p3z * p3z - p4z * p4z;
        double A2 = p2x * p2x - p3x * p3x;
        double B2 = p2y * p2y - p3y * p3y;
        double C2 = p2z * p2z - p3z * p3z;
        double P = (A + B + C) / 2;
        double Q = (A1 + B1 + C1) / 2;
        double R = (A2 + B2 + C2) / 2;
        double D = a * b1 * c2 + a2 * b * c1 + c * a1 * b2 - (a2 * b1 * c + a1 * b * c2 + a * b2 * c1);
        double Dx = P * b1 * c2 + b * c1 * R + c * Q * b2 - (c * b1 * R + P * c1 * b2 + Q * b * c2);
        double Dy = a * Q * c2 + P * c1 * a2 + c * a1 * R - (c * Q * a2 + a * c1 * R + c2 * P * a1);
        double Dz = a * b1 * R + b * Q * a2 + P * a1 * b2 - (a2 * b1 * P + a * Q * b2 + R * b * a1);
        if (D == 0) {
            cerr << "四点共面" << endl;
        }
        else {
            x_centre = Dx / D;
            y_centre = Dy / D;
            z_centre = Dz / D;
            banjing = sqrt((p1x - x_centre) * (p1x - x_centre) +
                (p1y - y_centre) * (p1y - y_centre) +
                (p1z - z_centre) * (p1z - z_centre));

        }
        std::cout << "x坐标" << x_centre << "y坐标" << y_centre << "z坐标" << z_centre << "半径为" << banjing << std::endl;
        double array01[629];
        for (int j = 0; j < cloud_input->size(); j++)
        {
            distance = sqrt(((cloud_input->points[j].x - x_centre) * (cloud_input->points[j].x - x_centre) +
                (cloud_input->points[j].y - y_centre) * (cloud_input->points[j].y - y_centre) +
                (cloud_input->points[j].z - z_centre) * (cloud_input->points[j].z - z_centre))) - banjing;
            array01[j] = distance;
            if (distance <= 0.1 && distance >= -0.1)
            {
                neidianji->points[neidianjishu].x = cloud_input->points[j].x;
                neidianji->points[neidianjishu].y = cloud_input->points[j].y;
                neidianji->points[neidianjishu].z = cloud_input->points[j].z;
                neidianjishu = neidianjishu + 1;
            }
            else
            {
                waidianji->points[waidianjishu].x = cloud_input->points[j].x;
                waidianji->points[waidianjishu].y = cloud_input->points[j].y;
                waidianji->points[waidianjishu].z = cloud_input->points[j].z;
                waidianjishu = waidianjishu + 1;
            }
        }

        std::cout << "内点数为：" << neidianjishu << std::endl;
        std::cout << "外点数为：" << waidianjishu << std::endl;
        float neidianlv = 100 * neidianjishu / (neidianjishu + waidianjishu);
        std::cout << neidianlv << std::endl;
        neidianlv_indices = neidianlv;
        jieshou_x = x_centre;
        jieshou_y = y_centre;
        jieshou_z = z_centre;
        if (neidianlv >= 70)
        {
            zuijia_x = x_centre;
            zuijia_y = y_centre;
            zuijia_z = z_centre;
            zuijia_banjing = banjing;
            zuijia_neidianlv = neidianlv_indices;

            pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("拟合结果"));
            viewer->addPointCloud<pcl::PointXYZ>(neidianji, "cloud");
            viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 1, 1, "cloud");
            viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud");
            viewer->addPointCloud<pcl::PointXYZ>(waidianji, "sphere");
            viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 0, 0, "sphere");
            viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "sphere");
            while (!viewer->wasStopped())
            {
                viewer->spinOnce(100);
            }
            break;
        }
        if (neidianlv_indices >= neidianlv_index)
        {
            zuijia_x = jieshou_x;
            zuijia_y = jieshou_y;
            zuijia_z = jieshou_z;
            zuijia_banjing = banjing;
            zuijia_neidianlv = neidianlv_indices;
            neidianlv_index = neidianlv_indices;
            zuijia_x = jieshou_x;
            jieshou_x = 0;
        }
        std::cout << "hhhhhh" << std::endl;
    }
    std::cout << "最佳拟合球参数：" << "球心坐标x" << zuijia_x << "球心坐标y" << zuijia_y << "球心坐标z" << zuijia_z << "球半径" << zuijia_banjing << std::endl;
    std::cout << "内点率为" << zuijia_neidianlv << std::endl;





    pcl::PointCloud<pcl::PointXYZ>::Ptr neidian(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr waidian(new pcl::PointCloud<pcl::PointXYZ>);
    neidian->resize(cloud_input->size());
    waidian->resize(cloud_input->size());
    double neidian02 = 0;
    double waidian02 = 0;
    double array02[629];
    for (int k = 0; k < cloud_input->size(); k++)
    {
        distance = sqrt(((cloud_input->points[k].x - zuijia_x) * (cloud_input->points[k].x - zuijia_x) +
            (cloud_input->points[k].y - zuijia_y) * (cloud_input->points[k].y - zuijia_y) +
            (cloud_input->points[k].z - zuijia_z) * (cloud_input->points[k].z - zuijia_z))) - zuijia_banjing;
        array02[k] = distance;
        if (distance <= 0.1 && distance >= -0.1)
        {
            neidian->points[neidian02].x = cloud_input->points[k].x;
            neidian->points[neidian02].y = cloud_input->points[k].y;
            neidian->points[neidian02].z = cloud_input->points[k].z;
            neidian02 = neidian02 + 1;
        }
        else
        {
            waidian->points[waidian02].x = cloud_input->points[k].x;
            waidian->points[waidian02].y = cloud_input->points[k].y;
            waidian->points[waidian02].z = cloud_input->points[k].z;
            waidian02 = waidian02 + 1;
        }
    }

    pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("拟合结果"));
    viewer->addPointCloud<pcl::PointXYZ>(neidian, "cloud");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 1, 1, "cloud");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud");
    viewer->addPointCloud<pcl::PointXYZ>(waidian, "sphere");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 0, 0, "sphere");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "sphere");
    while (!viewer->wasStopped())
    {
        viewer->spinOnce(100);
    }

    return 0;
}

最后的效果如下，白色为拟合的球体。

最佳拟合球参数：球心坐标x86.8652球心坐标y60.0072球心坐标z86.8682球半径9.96032
内点率为27.9809

五、pcd文件链接

链接：https://pan.baidu.com/s/13IJMh6ALlqAMAngaKVc80g 
提取码：acdl 
--来自百度网盘超级会员V4的分享

六、总结

        如有不足，请各位大佬批评指正。

一日不学习浑身难受的小汪
关注
  • 2
    点赞
  • 踩
  • 0
    收藏
    觉得还不错? 一键收藏
  • 1
    评论
专栏目录
PCL :实现RANSAC拟合空间3D球体（附完整源码）
希望我的博客，能帮上你解决学习中工作中所遇到的问题
09-17 167
PCL :实现RANSAC拟合空间3D球体（附完整源码）
PCL：RANSAC 圆拟合（二维圆 + 空间圆）
孙悟空
07-12 8018
本文介绍了PCL中RANSAC二维圆拟合与空间圆拟合的详细过程
基于RANSAC算法PCL点云拟合平面整理
热门推荐
洛北辰南的博客
11-30 3万+
文章目录前言首先讲一下PCL是什么接下来是RANSAC算法解决思路PCL拟合平面手写RANSAC算法代码流程然而，出现了问题！！！发现问题mt19937 is what？ 前言 这个项目，跟我专业不大相关，准确的说，是帮助学姐的做一做毕业设计的实际工作（平时有点闲，找点事做）。个人比较喜欢算法一类的，喜欢看别人的算法，改进、完善，研究如何应用到自己的功能中，精简前人的成果中对我冗余的部分。 也就是...
PCL求解规则点云的拟合圆
Candyerer的博客
11-06 6938
该文章引用自https://blog.csdn.net/Asher_zheng/article/details/100019353，https://blog.csdn.net/baidu_40840693/article/details/103928636 仅作为学习记录 前言 PCL中可通过RANSAC算法来计算一些规则点云的数学模型，并返回模型的参数，因此RANSAC算法可用来从点云中提取圆形点云并得到圆形点云的圆心、半径及圆所在平面的法向量（适合于3D Circle）。在PCL的RANSAC算法模型中
【PCL自学：RANSAC】如何使用PCL随机一致性模型(Ransac)拟合球和直线
斯坦福的兔子的博客
05-04 2107
如何使用PCL随机一致性模型1、什么是随即一致性算法2、在PCL中如何使用RANSAC 1、什么是随即一致性算法   Random Sample Consensus的缩写是RANSAC，它是一种从一组包含异常值的数据中估计数学模型参数的迭代方法。这个算法是由Fischler和Bolles在1981年发表的。RANSAC算法假设我们正在查看的所有数据都由内值(正确值)和异常值组成。内置可以用带有一组特定参数值的模型(例如直线、面、圆柱等形状函数)来解释，而异常值在任何情况下都不适合该模型。另一个必要的假设是，
三维空间圆拟合——center:XYZ Radius
04-19
三维空间圆拟合出XYZ和Radius值 建议：有多个点，三三分成几部分，多次拟合求取平均值，这样精度会更好。
PCL RANSAC拟合二维圆（C++详细过程版）
点云侠的博客
03-07 1094
RANSAC拟合二维圆（C++详细过程版）
PCL：实现最小二乘拟合二维圆（附完整源码）
希望我的博客，能帮上你解决学习中工作中所遇到的问题
10-12 273
PCL：实现最小二乘拟合二维圆（附完整源码）
PCL点云模型文件，可以用PCL库读取学习
最新发布
03-06
PCL点云模型文件，可以用PCL库读取学习
基于PCL、VTK的切片法计算三维模型的体积
07-22
采用C++ 切片法计算封闭三维模型（三角形网格模型）的体积。计算速度快，计算准确。
PCL Ransac 点云平面拟合 C++
02-25
利用点云库PCL，使用VS2015完成的C++代码，测试文件（.obj）已经在本站上传资源，供大家交流，如有问题欢迎多提宝贵意见 对于不平整表面，利用ransac平面拟合，然后将三维不平整表面近似为一个平面，并将表面上的点...
基于最小二乘法拟合平面得到点云曲面法矢估计
10-14
使用最小二乘法拟合平面，借助pcl点云库中的估计法矢的类来得到模型中点云曲面法矢估计。 是一个较为简单，常用的代码。txt文件
PCL 点云的圆柱拟合.md
05-19
1、获取点云后的圆柱拟合,整合成一个函数，只需要设置拟合的参数即可。 2、拟合完成后如何得到圆柱的参数。...3、PCL 点云库的圆柱拟合只能获得轴向，轴线上一点 ，提供获得圆柱的起点、终点的思路。
3D视觉测量：PCL空间圆孔拟合圆（附源码）
lddx_123456的博客
09-03 637
空间圆拟合（Spatial Circle Fitting）是一个用于拟合三维空间中的圆的数学和计算机视觉技术。
PCL RANSAC拟合球面和平面
m0_51204289的博客
09-19 592
PCL中 RANSAC拟合球面和平面的简单使用案例
Ceres&PCL 拟合二维圆
dayuhaitang1的博客
03-28 185
Ceres&PCL 拟合二维圆
pcl 点云数据平滑拟合——基于b样条曲线的方法
12-02
而基于b样条曲线的方法就是其中一种常用的平滑拟合方法。 在点云数据平滑拟合中，基于b样条曲线的方法通过将点云数据分段连接，使用多个b样条曲线来逼近原始数据点的形状。首先，需要将点云数据按照一定的规则划分...

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

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

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

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

抵扣说明：

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

余额充值