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! "); 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 }
切勿转载！！！