代码出处:
http://pointclouds.org/documentation/tutorials/random_sample_consensus.php#random-sample-consensus
**算法介绍:**https://www.cnblogs.com/li-yao7758258/p/6477007.html
直接记录代码,供后续参考学习。
/** Filename: random_sample_consensus.cpp
** Date: 2018-4-18
**Description:
**/
#include "stdafx.h"
#include <iostream>
#include <pcl\console\parse.h>
#include <pcl\filters\extract_indices.h>
#include <pcl\io\pcd_io.h>
#include <pcl\point_types.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>
#include <boost\thread\thread.hpp>
boost::shared_ptr<pcl::visualization::PCLVisualizer> simpleVis(pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud,int v)
{
// -----------------------------------------------
// ---------Open 3D viewer and add point cloud ---
// -----------------------------------------------
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
viewer->setBackgroundColor(0, 0, 0,v);//black
viewer->addPointCloud(cloud,"sample cloud",v);
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "sample cloud",v);
viewer->addCoordinateSystem(1000.0, "global",v);
viewer->initCameraParameters();
return viewer;
}
int main(int argc, char* argv[])
{
// 初始化点云
pcl::PointCloud<pcl::PointXYZ> ::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr final(new pcl::PointCloud<pcl::PointXYZ>);
//填充点云数据
cloud->width =5000;
cloud->height = 1;
cloud->is_dense = false;
cloud->resize(cloud->width * cloud->height);
for (size_t i = 0; i < cloud->points.size(); i++)
{
if (pcl::console::find_argument(argc, argv, "-s") >= 0 || pcl::console::find_argument(argc, argv, "-sf") >= 0)
{
printf("随机生成 s or sf \n");
cloud->points[i].x = 1024 * rand() / (RAND_MAX + 1.0f);
cloud->points[i].y = 1024 * rand() / (RAND_MAX + 1.0f);
if (i % 5 == 0)
cloud->points[i].z = 1024 * rand() / (RAND_MAX + 1.0f);
else if (i % 2 == 0)
cloud->points[i].z = sqrt(1 - (cloud->points[i].x * cloud->points[i].x)
- (cloud->points[i].y * cloud->points[i].y));
else
cloud->points[i].z = - sqrt(1 - (cloud->points[i].x * cloud->points[i].x)
- (cloud->points[i].y * cloud->points[i].y));
}
else
{
printf("others \n");
cloud->points[i].x = 1024 * rand() / (RAND_MAX + 1.0);
cloud->points[i].y = 1024 * rand() / (RAND_MAX + 1.0);
if (i % 3 == 0)
cloud->points[i].z = -1 * (cloud->points[i].x + cloud->points[i].y);
else
cloud->points[i].z = 1024 * rand() / (RAND_MAX + 1.0);
}
}
//for (size_t i = 0; i < cloud->points.size(); i++)
//{
// printf("点云数据:\n");
// printf("%6.3f %6.3f %6.3f\n", cloud->points[i].x, cloud->points[i].y, cloud->points[i].z);
//}
printf("\n\n 点云大小为: %d \n", cloud->points.size());
std::vector<int> inliers; //有效数据
//创建 RANSAC 对象 并计算合适模型
pcl::SampleConsensusModelSphere<pcl::PointXYZ> ::Ptr
model_s(new pcl::SampleConsensusModelSphere<pcl::PointXYZ>(cloud));
pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr
model_p(new pcl::SampleConsensusModelPlane<pcl::PointXYZ>(cloud));
//平面采样一致性
if (pcl::console::find_argument(argc, argv, "-f") >= 0)
{
pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_p);
ransac.setDistanceThreshold(.01);
//ransac.setMaxIterations(30);
ransac.computeModel();
ransac.getInliers(inliers);
}
//球面采样一致性
else if (pcl::console::find_argument(argc,argv,"-sf")>=0)
{
pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_s);
ransac.setDistanceThreshold(.01);
//ransac.setMaxIterations(30);
ransac.computeModel();
ransac.getInliers(inliers);
}
std::cout << "采样后点云数据大小11:" << inliers.size()<< std::endl;
//将所有计算有效点云数据复制到另一个点云
pcl::copyPointCloud<pcl::PointXYZ>(*cloud, inliers, *final);
std::cout << "采样后点云数据大小:" << final->points.size() << std::endl;
//创建可视化对象,
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D viewer"));
int v1(0);
int v2(1);
viewer->createViewPort(0, 0, 0.5, 1, v1);
viewer->createViewPort(0.5, 0, 1, 1, v2);
if (pcl::console::find_argument(argc, argv, "-f") >= 0 || pcl::console::find_argument(argc, argv, "-sf") >= 0)
{
//viewer = simpleVis(cloud, v1);
//viewer = simpleVis(final, v2);
std::cout << "调用-f || -sf:" << std::endl;
viewer->setBackgroundColor(0, 0, 0, v1);//black
viewer->addPointCloud(cloud, "sample cloud", v1);
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "sample cloud", v1);
viewer->addCoordinateSystem(1000.0, "global", v1);
viewer->setBackgroundColor(0, 0, 0, v2);//black
viewer->addPointCloud(final, "Final cloud", v2);
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "Final cloud", v2);
viewer->addCoordinateSystem(1000.0, "global_v2", v2);
//viewer->initCameraParameters();
}
else
{
//viewer = simpleVis(cloud, v1);
//viewer = simpleVis(cloud, v2);
std::cout << "调用qita" << std::endl;
viewer->setBackgroundColor(0, 0, 0, v1);//black
viewer->addPointCloud(cloud, "sample cloud", v1);
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "sample cloud", v1);
viewer->addCoordinateSystem(1000.0, "global", v1);
viewer->setBackgroundColor(0, 0, 0, v2);//black
viewer->addPointCloud(cloud, "Final cloud", v2);
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "Final cloud", v2);
viewer->addCoordinateSystem(1000.0, "global_v2", v2);
//viewer->initCameraParameters();
}
while (!viewer->wasStopped())
{
viewer->spinOnce(100);
boost::this_thread::sleep(boost::posix_time::microseconds(100000));
}
system("pause");
return 0;
}