点云平面拟合和球面拟合

一、介绍

In this tutorial we learn how to use a RandomSampleConsensus with a plane model to obtain the cloud fitting to this model.

二、代码

#include <iostream>
#include <thread>
#include <pcl/point_types.h>
#include <pcl/common/io.h>
#include <pcl/point_cloud.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/sample_consensus/sac_model_sphere.h>
#include <pcl/visualization/pcl_visualizer.h>using namespace std::chrono_literals;pcl::visualization::PCLVisualizer::Ptr simpleVis(pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud)
{pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));viewer->setBackgroundColor(0, 0, 0);viewer->addPointCloud<pcl::PointXYZ>(cloud, "sample cloud");viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud");viewer->addCoordinateSystem (1.0, "global");viewer->initCameraParameters();return (viewer);
}void ranFit()
{bool is_plane = true;bool is_show_final = false;pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptr final(new pcl::PointCloud<pcl::PointXYZ>);cloud->width = 500;cloud->height = 1;cloud->is_dense = false;cloud->points.resize(cloud->width * cloud->height);for (int i = 0; i < cloud->size(); ++i){if (is_plane==false){// x*x+y*y+z*z=1(*cloud)[i].x = 2.0 * rand() / (RAND_MAX + 1.0) - 1.0;(*cloud)[i].y = 2.0 * rand() / (RAND_MAX + 1.0) - 1.0;if ((*cloud)[i].x * (*cloud)[i].x + ((*cloud)[i].y * (*cloud)[i].y) > 1)(*cloud)[i].z = 2.0 * rand() / (RAND_MAX + 1.0) - 1.0;else if (i % 2 == 0)(*cloud)[i].z = sqrt(1 - ((*cloud)[i].x * (*cloud)[i].x) - ((*cloud)[i].y * (*cloud)[i].y));else(*cloud)[i].z = -sqrt(1 - ((*cloud)[i].x * (*cloud)[i].x) - ((*cloud)[i].y * (*cloud)[i].y));}else{// 0.5x+0.5y-z=0(*cloud)[i].x = rand() / (RAND_MAX + 1.0);(*cloud)[i].y = rand() / (RAND_MAX + 1.0);if (i % 3 == 0)(*cloud)[i].z = rand() / (RAND_MAX + 1.0);else(*cloud)[i].z = 0.5 * (*cloud)[i].x + 0.5 * (*cloud)[i].y;}}std::vector<int> inliers;std::vector<int> outliers;pcl::SampleConsensusModelSphere<pcl::PointXYZ>::Ptrmodel_s(new pcl::SampleConsensusModelSphere<pcl::PointXYZ>(cloud));pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptrmodel_p(new pcl::SampleConsensusModelPlane<pcl::PointXYZ>(cloud));Eigen::VectorXf coef;if (is_plane){pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_p);ransac.setDistanceThreshold(.01);ransac.computeModel();ransac.getInliers(inliers);ransac.getModelCoefficients(coef);}else{pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_s);ransac.setDistanceThreshold(.01);ransac.computeModel();ransac.getInliers(inliers);ransac.getModelCoefficients(coef);}std::cout << coef << std::endl;pcl::copyPointCloud(*cloud, inliers, *final);pcl::visualization::PCLVisualizer::Ptr viewer;if (is_show_final)viewer = simpleVis(final);elseviewer = simpleVis(cloud);while (!viewer->wasStopped()){viewer->spinOnce(100);std::this_thread::sleep_for(100ms);}
}int main()
{ranFit();return (0);
}