Eighth and Ninth Chapter

Signed-off-by: Hao Guo <[email protected]>

第九章/1 random_sample_consensus/source/CMakeLists.txt

@@ -0,0 +1,12 @@
+cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+
+project(random_sample_consensus)
+
+find_package(PCL 1.2 REQUIRED)
+
+include_directories(${PCL_INCLUDE_DIRS})
+link_directories(${PCL_LIBRARY_DIRS})
+add_definitions(${PCL_DEFINITIONS})
+
+add_executable (random_sample_consensus random_sample_consensus.cpp)
+target_link_libraries (random_sample_consensus ${PCL_LIBRARIES})

第九章/1 random_sample_consensus/source/random_sample_consensus.cpp

@@ -0,0 +1,104 @@
+#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)
+{
+ // --------------------------------------------
+ // -----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);
+ viewer->addPointCloud<pcl::PointXYZ> (cloud, "sample cloud");
+ viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud");
+ //viewer->addCoordinateSystem (1.0);
+ viewer->initCameraParameters ();
+ return (viewer);
+}
+
+int
+main(int argc, char** argv)
+{
+ srand(time(NULL));
+ // initialize PointClouds
+ pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
+ pcl::PointCloud<pcl::PointXYZ>::Ptr final (new pcl::PointCloud<pcl::PointXYZ>);
+
+ // populate our PointCloud with points
+ cloud->width = 5000;
+ cloud->height = 1;
+ cloud->is_dense = false;
+ cloud->points.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)
+ {
+ cloud->points[i].x = rand () / (RAND_MAX + 1.0);
+ cloud->points[i].y = rand () / (RAND_MAX + 1.0);
+ if (i % 5 == 0)
+ cloud->points[i].z = rand () / (RAND_MAX + 1.0);
+ 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
+ {
+ cloud->points[i].x = rand () / (RAND_MAX + 1.0);
+ cloud->points[i].y = rand () / (RAND_MAX + 1.0);
+ if( i % 5 == 0)
+ cloud->points[i].z = rand () / (RAND_MAX + 1.0);
+ else
+ cloud->points[i].z = -1 * (cloud->points[i].x + cloud->points[i].y);
+ }
+ }
+
+ std::vector<int> inliers;
+
+ // created RandomSampleConsensus object and compute the appropriated model
+ 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.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.computeModel();
+ ransac.getInliers(inliers);
+ }
+
+ // copies all inliers of the model computed to another PointCloud
+ pcl::copyPointCloud<pcl::PointXYZ>(*cloud, inliers, *final);
+
+ // creates the visualization object and adds either our orignial cloud or all of the inliers
+ // depending on the command line arguments specified.
+ boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
+ if (pcl::console::find_argument (argc, argv, "-f") >= 0 || pcl::console::find_argument (argc, argv, "-sf") >= 0)
+ viewer = simpleVis(final);
+ else
+ viewer = simpleVis(cloud);
+ while (!viewer->wasStopped ())
+ {
+ viewer->spinOnce (100);
+ boost::this_thread::sleep (boost::posix_time::microseconds (100000));
+ }
+ return 0;
+ }

第八章/1 narf_keypoint_extraction/source/CMakeLists.txt

@@ -0,0 +1,8 @@
+cmake_minimum_required(VERSION 2.6 FATAL_ERROR)
+project(narf_keypoint_extraction)
+find_package(PCL 1.3 REQUIRED)
+include_directories(${PCL_INCLUDE_DIRS})
+link_directories(${PCL_LIBRARY_DIRS})
+add_definitions(${PCL_DEFINITIONS})
+add_executable(narf_keypoint_extraction narf_keypoint_extraction.cpp)
+target_link_libraries(narf_keypoint_extraction ${PCL_LIBRARIES})

第八章/1 narf_keypoint_extraction/source/narf_keypoint_extraction.cpp

@@ -0,0 +1,174 @@
+/* \author Bastian Steder */
+
+#include <iostream>
+#include <boost/thread/thread.hpp>
+#include <pcl/range_image/range_image.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/visualization/range_image_visualizer.h>
+#include <pcl/visualization/pcl_visualizer.h>
+#include <pcl/features/range_image_border_extractor.h>
+#include <pcl/keypoints/narf_keypoint.h>
+#include <pcl/console/parse.h>
+
+typedefpcl::PointXYZPointType;
+//参数
+floatangular_resolution=0.5f;
+floatsupport_size=0.2f;
+pcl::RangeImage::CoordinateFramecoordinate_frame=pcl::RangeImage::CAMERA_FRAME;
+boolsetUnseenToMaxRange=false;
+//打印帮助
+void
+printUsage(constchar*progName)
+{
+std::cout<<"\n\nUsage: "<<progName<<" [options] <scene.pcd>\n\n"
+<<"Options:\n"
+<<"-------------------------------------------\n"
+<<"-r <float> angular resolution in degrees (default "<<angular_resolution<<")\n"
+<<"-c <int> coordinate frame (default "<<(int)coordinate_frame<<")\n"
+<<"-m Treat all unseen points as maximum range readings\n"
+<<"-s <float> support size for the interest points (diameter of the used sphere - "
+<<"default "<<support_size<<")\n"
+<<"-h this help\n"
+<<"\n\n";
+}
+
+void
+setViewerPose(pcl::visualization::PCLVisualizer&viewer,constEigen::Affine3f&viewer_pose)
+{
+Eigen::Vector3fpos_vector=viewer_pose*Eigen::Vector3f(0,0,0);
+Eigen::Vector3flook_at_vector=viewer_pose.rotation()*Eigen::Vector3f(0,0,1)+pos_vector;
+Eigen::Vector3fup_vector=viewer_pose.rotation()*Eigen::Vector3f(0,-1,0);
+viewer.camera_.pos[0]=pos_vector[0];
+viewer.camera_.pos[1]=pos_vector[1];
+viewer.camera_.pos[2]=pos_vector[2];
+viewer.camera_.focal[0]=look_at_vector[0];
+viewer.camera_.focal[1]=look_at_vector[1];
+viewer.camera_.focal[2]=look_at_vector[2];
+viewer.camera_.view[0]=up_vector[0];
+viewer.camera_.view[1]=up_vector[1];
+viewer.camera_.view[2]=up_vector[2];
+viewer.updateCamera();
+}
+
+// -----Main-----
+int
+main(intargc,char**argv)
+{
+//解析命令行参数
+if(pcl::console::find_argument(argc,argv,"-h")>=0)
+{
+printUsage(argv[0]);
+return0;
+}
+if(pcl::console::find_argument(argc,argv,"-m")>=0)
+{
+setUnseenToMaxRange=true;
+cout<<"Setting unseen values in range image to maximum range readings.\n";
+}
+inttmp_coordinate_frame;
+if(pcl::console::parse(argc,argv,"-c",tmp_coordinate_frame)>=0)
+{
+coordinate_frame=pcl::RangeImage::CoordinateFrame(tmp_coordinate_frame);
+cout<<"Using coordinate frame "<<(int)coordinate_frame<<".\n";
+}
+if(pcl::console::parse(argc,argv,"-s",support_size)>=0)
+cout<<"Setting support size to "<<support_size<<".\n";
+if(pcl::console::parse(argc,argv,"-r",angular_resolution)>=0)
+cout<<"Setting angular resolution to "<<angular_resolution<<"deg.\n";
+angular_resolution=pcl::deg2rad(angular_resolution);
+//读取给定的pcd文件或者自行创建随机点云
+pcl::PointCloud<PointType>::Ptrpoint_cloud_ptr(newpcl::PointCloud<PointType>);
+pcl::PointCloud<PointType>&point_cloud=*point_cloud_ptr;
+pcl::PointCloud<pcl::PointWithViewpoint>far_ranges;
+Eigen::Affine3fscene_sensor_pose(Eigen::Affine3f::Identity());
+std::vector<int>pcd_filename_indices=pcl::console::parse_file_extension_argument(argc,argv,"pcd");
+if(!pcd_filename_indices.empty())
+{
+std::stringfilename=argv[pcd_filename_indices[0]];
+if(pcl::io::loadPCDFile(filename,point_cloud)==-1)
+{
+cerr<<"Was not able to open file \""<<filename<<"\".\n";
+printUsage(argv[0]);
+return0;
+}
+scene_sensor_pose=Eigen::Affine3f(Eigen::Translation3f(point_cloud.sensor_origin_[0],
+point_cloud.sensor_origin_[1],
+point_cloud.sensor_origin_[2]))*
+Eigen::Affine3f(point_cloud.sensor_orientation_);
+std::stringfar_ranges_filename=pcl::getFilenameWithoutExtension(filename)+"_far_ranges.pcd";
+if(pcl::io::loadPCDFile(far_ranges_filename.c_str(),far_ranges)==-1)
+std::cout<<"Far ranges file \""<<far_ranges_filename<<"\" does not exists.\n";
+}
+else
+{
+setUnseenToMaxRange=true;
+cout<<"\nNo *.pcd file given =>Genarating example point cloud.\n\n";
+for(floatx=-0.5f;x<=0.5f;x+=0.01f)
+{
+for(floaty=-0.5f;y<=0.5f;y+=0.01f)
+{
+PointTypepoint;point.x=x;point.y=y;point.z=2.0f-y;
+point_cloud.points.push_back(point);
+}
+}
+point_cloud.width=(int)point_cloud.points.size();point_cloud.height=1;
+}
+//从点云创建距离图像
+floatnoise_level=0.0;
+floatmin_range=0.0f;
+intborder_size=1;
+boost::shared_ptr<pcl::RangeImage>range_image_ptr(newpcl::RangeImage);
+pcl::RangeImage&range_image=*range_image_ptr;
+range_image.createFromPointCloud(point_cloud,angular_resolution,pcl::deg2rad(360.0f),pcl::deg2rad(180.0f),
+scene_sensor_pose,coordinate_frame,noise_level,min_range,border_size);
+range_image.integrateFarRanges(far_ranges);
+if(setUnseenToMaxRange)
+range_image.setUnseenToMaxRange();
+// 创建3D点云可视化窗口，并显示点云
+pcl::visualization::PCLVisualizerviewer("3D Viewer");
+viewer.setBackgroundColor(1,1,1);
+pcl::visualization::PointCloudColorHandlerCustom<pcl::PointWithRange>range_image_color_handler(range_image_ptr,0,0,0);
+viewer.addPointCloud(range_image_ptr,range_image_color_handler,"range image");
+viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,1,"range image");
+//viewer.addCoordinateSystem (1.0f);
+//PointCloudColorHandlerCustom<PointType>point_cloud_color_handler (point_cloud_ptr, 150, 150, 150);
+//viewer.addPointCloud (point_cloud_ptr, point_cloud_color_handler, "original point cloud");
+viewer.initCameraParameters();
+setViewerPose(viewer,range_image.getTransformationToWorldSystem());
+// 显示距离图像
+pcl::visualization::RangeImageVisualizerrange_image_widget("Range image");
+range_image_widget.showRangeImage(range_image);
+
+//提取NARF关键点
+pcl::RangeImageBorderExtractorrange_image_border_extractor;
+pcl::NarfKeypointnarf_keypoint_detector(&range_image_border_extractor);
+narf_keypoint_detector.setRangeImage(&range_image);
+narf_keypoint_detector.getParameters().support_size=support_size;
+//narf_keypoint_detector.getParameters ().add_points_on_straight_edges = true;
+//narf_keypoint_detector.getParameters ().distance_for_additional_points = 0.5;
+
+pcl::PointCloud<int>keypoint_indices;
+narf_keypoint_detector.compute(keypoint_indices);
+std::cout<<"Found "<<keypoint_indices.points.size()<<" key points.\n";
+//在距离图像显示组件内显示关键点
+//for (size_ti=0; i<keypoint_indices.points.size (); ++i)
+//range_image_widget.markPoint (keypoint_indices.points[i]%range_image.width,
+//keypoint_indices.points[i]/range_image.width);
+//在3D窗口中显示关键点
+pcl::PointCloud<pcl::PointXYZ>::Ptrkeypoints_ptr(newpcl::PointCloud<pcl::PointXYZ>);
+pcl::PointCloud<pcl::PointXYZ>&keypoints=*keypoints_ptr;
+keypoints.points.resize(keypoint_indices.points.size());
+for(size_ti=0;i<keypoint_indices.points.size();++i)
+keypoints.points[i].getVector3fMap()=range_image.points[keypoint_indices.points[i]].getVector3fMap();
+
+pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>keypoints_color_handler(keypoints_ptr,0,255,0);
+viewer.addPointCloud<pcl::PointXYZ>(keypoints_ptr,keypoints_color_handler,"keypoints");
+viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,7,"keypoints");
+// 主循环
+while(!viewer.wasStopped())
+{
+range_image_widget.spinOnce();// process GUI events
+viewer.spinOnce();
+pcl_sleep(0.01);
+}
+}

第八章/2 siftsource/source/CMakeLists.txt

@@ -0,0 +1,13 @@
+cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+
+project(Siftdetect)
+
+find_package(PCL 1.7 REQUIRED)
+include_directories(${PCL_INCLUDE_DIRS})
+link_directories(${PCL_LIBRARY_DIRS})
+add_definitions(${PCL_DEFINITIONS})
+
+add_executable (Siftdetect Siftdetect.cpp)
+target_link_libraries (Siftdetect ${PCL_LIBRARIES})
+
+

第八章/2 siftsource/source/Siftdetect.cpp

@@ -0,0 +1,63 @@
+#include <iostream>
+#include <pcl/io/pcd_io.h>
+#include <pcl/point_types.h>
+#include <pcl/common/io.h>
+#include <pcl/keypoints/sift_keypoint.h>
+#include <pcl/features/normal_3d.h>
+#include <pcl/visualization/pcl_visualizer.h>
+#include <pcl/console/time.h>
+using namespace std;
+
+namespace pcl
+{
+ template<>
+ struct SIFTKeypointFieldSelector<PointXYZ>
+ {
+ inline float
+ operator () (const PointXYZ &p) const
+ {
+ return p.z;
+ }
+ };
+}
+
+int
+main(int argc, char *argv[])
+{
+
+ pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz (new pcl::PointCloud<pcl::PointXYZ>);
+ pcl::io::loadPCDFile (argv[1], *cloud_xyz);
+
+ const float min_scale = stof(argv[2]); 
+ const int n_octaves = stof(argv[3]); 
+ const int n_scales_per_octave = stof(argv[4]); 
+ const float min_contrast = stof(argv[5]); 
+
+ pcl::SIFTKeypoint<pcl::PointXYZ, pcl::PointWithScale> sift;//创建sift关键点检测对象
+ pcl::PointCloud<pcl::PointWithScale> result;
+ sift.setInputCloud(cloud_xyz);//设置输入点云
+ pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ> ());
+ sift.setSearchMethod(tree);//创建一个空的kd树对象tree，并把它传递给sift检测对象
+ sift.setScales(min_scale, n_octaves, n_scales_per_octave);//指定搜索关键点的尺度范围
+ sift.setMinimumContrast(min_contrast);//设置限制关键点检测的阈值
+ sift.compute(result);//执行sift关键点检测，保存结果在result
+
+ pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_temp (new pcl::PointCloud<pcl::PointXYZ>);
+ copyPointCloud(result, *cloud_temp);//将点类型pcl::PointWithScale的数据转换为点类型pcl::PointXYZ的数据
+
+ //可视化输入点云和关键点
+ pcl::visualization::PCLVisualizer viewer("Sift keypoint");
+ viewer.setBackgroundColor( 255, 255, 255 );
+ viewer.addPointCloud(cloud_xyz, "cloud");
+ viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR,0,0,0,"cloud");
+ viewer.addPointCloud(cloud_temp, "keypoints");
+ viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 9, "keypoints");
+ viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR,0,0,255,"keypoints");
+
+ while(!viewer.wasStopped ())
+ {
+ viewer.spinOnce ();
+ }
+ return 0;
+
+}

第八章/3/CMakeLists.txt

@@ -0,0 +1,13 @@
+cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+
+project(pcl_test)
+
+find_package(PCL 1.7 REQUIRED)
+include_directories(${PCL_INCLUDE_DIRS})
+link_directories(${PCL_LIBRARY_DIRS})
+add_definitions(${PCL_DEFINITIONS})
+
+add_executable (pcl_test pcl_test.cpp)
+target_link_libraries (pcl_test ${PCL_LIBRARIES})
+
+