2. 点云到平面的映射方法

 1 #include <iostream>
 2 #include <pcl/io/pcd_io.h>
 3 #include <pcl/point_types.h>
 4 #include <pcl/ModelCoefficients.h>
 5 #include <pcl/filters/project_inliers.h>
 6 #include <pcl/visualization/pcl_visualizer.h>
 7 
 8 using namespace pcl;
 9 using namespace std;
10 typedef pcl::PointXYZ PointT;
11 typedef pcl::PointCloud<PointT> PointCloud;
12 
13 int main(int argc, char** argv)
14 {
15 
16     //*******点云往平面投影的方法**********
17     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
18     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_projected(new pcl::PointCloud<pcl::PointXYZ>);
19 
20     if (pcl::io::loadPCDFile("C:/Users/18148/Desktop/qq3.pcd", *cloud) == -1){
21         PCL_ERROR("Could not read pcd file!\n");
22         return -1;
23     }
24 
25     std::cerr << "Cloud before projection: " << std::endl;
26     /*for (size_t i = 0; i < cloud->points.size(); ++i)
27         std::cerr << "    " << cloud->points[i].x << " "
28         << cloud->points[i].y << " "
29         << cloud->points[i].z << std::endl;
30 */
31     // 定义模型系数对象，并填充对应的数据Create a set of planar coefficients with X=Y=0,Z=1
32     pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());
33     coefficients->values.resize(4);
34     coefficients->values[0] = 0.5;
35     coefficients->values[1] = 1;
36     coefficients->values[2] = 1;
37     coefficients->values[3] = 0;
38 
39     // Create the filtering object
40     pcl::ProjectInliers<pcl::PointXYZ> proj;//创建投影滤波对象
41     proj.setModelType(pcl::SACMODEL_PLANE);//设置对象对应的投影模型
42     proj.setInputCloud(cloud);//设置输入点云
43     proj.setModelCoefficients(coefficients);//设置模型对应的系数
44     proj.filter(*cloud_projected);//执行投影滤波存储结果cloud_projected
45 
46     std::cerr << "Cloud after projection: " << std::endl;
47     //for (size_t i = 0; i < cloud_projected->points.size(); ++i)
48     //    std::cerr << "    " << cloud_projected->points[i].x << " "
49     //    << cloud_projected->points[i].y << " "
50     //    << cloud_projected->points[i].z << std::endl;
51 
52     boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
53     viewer->setBackgroundColor(0, 0, 0);
54     //pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cloud_pointsPtr);
55     pcl::visualization::PointCloudColorHandlerCustom<PointT> blue(cloud, 0, 0, 255);
56     viewer->addPointCloud<PointT>(cloud, blue, "sample cloud");
57     viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");
58 
59     pcl::visualization::PointCloudColorHandlerCustom<PointT> red(cloud_projected, 255, 0, 0);
60     viewer->addPointCloud<PointT>(cloud_projected, red, "sample cloud2");
61     viewer->addCoordinateSystem(1.0);
62     viewer->initCameraParameters();
63 
64     while (!viewer->wasStopped())
65     {
66         viewer->spinOnce(100);
67         boost::this_thread::sleep(boost::posix_time::microseconds(100000));
68     }
69     system("pause");
70     return 0;
71 }