点云到球面的映射方法

 1 #include <iostream>
 2 #include <cmath>
 3 #include <pcl/point_cloud.h>
 4 #include <pcl/io/pcd_io.h>
 5 #include <pcl/point_types.h>
 6 #include <pcl/ModelCoefficients.h>
 7 #include <pcl/filters/project_inliers.h>
 8 #include <pcl/visualization/pcl_visualizer.h>
 9 
10 using namespace pcl;
11 using namespace std;
12 
13 struct sphere
14 {
15     float centerX;
16     float centerY;
17     float centerZ;
18     float radius;
19 };
20 
21 
22 typedef pcl::PointXYZ PointT;
23 typedef pcl::PointCloud<PointT> PointCloud;
24 
25 
26 int main(int argc, char** argv)
27 {
28 
29     //*******点云往球面投影的方法**********
30     pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
31     pcl::PointCloud<PointT>::Ptr cloud_projected(new pcl::PointCloud<PointT>);
32 
33     if (pcl::io::loadPCDFile("qq3.pcd", *cloud) == -1){
34         PCL_ERROR("Could not read pcd file!\n");
35         return -1;
36     }
37 
38     std::cerr << "Cloud before projection: " << cloud->points.size() << std::endl;
39     PointT points;
40     sphere     SP;
41     SP.centerX = 0;
42     SP.centerY = 0;
43     SP.centerZ = 0;
44     SP.radius = 8;
45     for (size_t i = 0; i < cloud->points.size(); ++i)
46     {
47         float d = sqrt((cloud->points[i].x - SP.centerX)*(cloud->points[i].x - SP.centerX) + (cloud->points[i].y - SP.centerY)*(cloud->points[i].y - SP.centerY) + (cloud->points[i].z - SP.centerZ)*(cloud->points[i].z - SP.centerZ));
48         points.x = (cloud->points[i].x)*SP.radius / d;
49         points.y = (cloud->points[i].y)*SP.radius / d;
50         points.z = (cloud->points[i].z)*SP.radius / d;
51         cloud_projected->points.push_back(points);
52     }
53 
54     // 定义模型系数对象，并填充对应的数据
55 
56     std::cerr << "Cloud after projection: " << cloud_projected->points.size() << std::endl;
57     /*for (size_t i = 0; i < cloud_projected->points.size(); ++i)
58     std::cerr << "    " << cloud_projected->points[i].x << " "
59     << cloud_projected->points[i].y << " "
60     << cloud_projected->points[i].z << std::endl;*/
61 
62     boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
63     viewer->setBackgroundColor(0, 0, 0);
64     //pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cloud_pointsPtr);
65     pcl::visualization::PointCloudColorHandlerCustom<PointT> blue(cloud, 0, 0, 255);
66     viewer->addPointCloud<PointT>(cloud, blue, "sample cloud");
67     viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");
68 
69     pcl::visualization::PointCloudColorHandlerCustom<PointT> red(cloud_projected, 255, 0, 0);
70     viewer->addPointCloud<PointT>(cloud_projected, red, "sample cloud2");
71     viewer->addCoordinateSystem(1.0);
72     viewer->initCameraParameters();
73 
74     while (!viewer->wasStopped())
75     {
76         viewer->spinOnce(100);
77         boost::this_thread::sleep(boost::posix_time::microseconds(100000));
78     }
79     system("pause");
80 
81     return 0;
82 }

切勿转载！！！