pcl中set程序_PCL关键点（1）

#include #include#include#include#include#include#include#include#include#includetypedef pcl::PointXYZ PointType;//--------------------//-----Parameters-----//--------------------

float angular_resolution = 0.5f; angular_resolution为模拟的深度传感器的角度分辨率，即深度图像中一个像素对应的角度大小

float support_size = 0.2f; //点云大小的设置

pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::CAMERA_FRAME; //设置坐标系

bool setUnseenToMaxRange = false;bool rotation_invariant = true;//--------------//-----Help-----//--------------

voidprintUsage (const char*progName)

{

std::cout<< "\n\nUsage:"<\n\n"

<< "Options:\n"

<< "-------------------------------------------\n"

<< "-r angular resolution in degrees (default"<

<< "-c coordinate frame (default"<< (int)coordinate_frame<

<< "-m Treat all unseen points to max range\n"

<< "-s support size for the interest points (diameter of the used sphere -"

"default"<

<< "-o <0/1> switch rotational invariant version of the feature on/off"

<< "(default"<< (int)rotation_invariant<

<< "-h this help\n"

<< "\n\n";

}voidsetViewerPose (pcl::visualization::PCLVisualizer& viewer, const Eigen::Affine3f& viewer_pose) //设置视口的位姿

{

Eigen::Vector3f pos_vector= viewer_pose * Eigen::Vector3f (0, 0, 0); //视口的原点pos_vector

Eigen::Vector3f look_at_vector = viewer_pose.rotation () * Eigen::Vector3f (0, 0, 1) + pos_vector; //旋转+平移look_at_vector

Eigen::Vector3f up_vector = viewer_pose.rotation () * Eigen::Vector3f (0, -1, 0); //up_vector

viewer.setCameraPosition (pos_vector[0], pos_vector[1], pos_vector[2], //设置照相机的位姿

look_at_vector[0], look_at_vector[1], look_at_vector[2],

up_vector[0], up_vector[1], up_vector[2]);

}//--------------//-----Main-----//--------------

intmain (int argc, char**argv)

{//--------------------------------------//-----Parse Command Line Arguments-----//--------------------------------------

if (pcl::console::find_argument (argc, argv, "-h") >= 0)

{

printUsage (argv[0]);return 0;

}if (pcl::console::find_argument (argc, argv, "-m") >= 0)

{

setUnseenToMaxRange= true;

cout<< "Setting unseen values in range image to maximum range readings.\n";

}if (pcl::console::parse (argc, argv, "-o", rotation_invariant) >= 0)

cout<< "Switching rotation invariant feature version"<< (rotation_invariant ? "on" : "off")<= 0)

{

coordinate_frame=pcl::RangeImage::CoordinateFrame (tmp_coordinate_frame);

cout<< "Using coordinate frame"<< (int)coordinate_frame<

}if (pcl::console::parse (argc, argv, "-s", support_size) >= 0)

cout<< "Setting support size to"<= 0)

cout<< "Setting angular resolution to"<

angular_resolution=pcl::deg2rad (angular_resolution);//------------------------------------------------------------------//-----Read pcd file or create example point cloud if not given-----//------------------------------------------------------------------

pcl::PointCloud::Ptr point_cloud_ptr (new pcl::PointCloud);

pcl::PointCloud& point_cloud = *point_cloud_ptr;

pcl::PointCloud<:pointwithviewpoint>far_ranges;

Eigen::Affine3f scene_sensor_pose (Eigen::Affine3f::Identity ());

std::vector pcd_filename_indices = pcl::console::parse_file_extension_argument (argc, argv, "pcd");if (!pcd_filename_indices.empty ())

{

std::string filename = argv[pcd_filename_indices[0]];if (pcl::io::loadPCDFile (filename, point_cloud) == -1)

{

cerr<< "Was not able to open file \""<

printUsage (argv[0]);return 0;

}

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::string far_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 \""<

}else{

setUnseenToMaxRange= true;

cout<< "\nNo *.pcd file given => Genarating example point cloud.\n\n";for (float x=-0.5f; x<=0.5f; x+=0.01f)

{for (float y=-0.5f; y<=0.5f; y+=0.01f)

{

PointType point; 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;

}//-----------------------------------------------//-----Create RangeImage from the PointCloud-----//-----------------------------------------------

float noise_level = 0.0;float min_range = 0.0f;int border_size = 1;

boost::shared_ptr<: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 ();//--------------------------------------------//-----Open 3D viewer and add point cloud-----//--------------------------------------------

pcl::visualization::PCLVisualizer viewer ("3D Viewer");

viewer.setBackgroundColor (1, 1, 1);

pcl::visualization::PointCloudColorHandlerCustom<: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, "global");//PointCloudColorHandlerCustom 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 ());//--------------------------//-----Show range image-----//--------------------------

pcl::visualization::RangeImageVisualizer range_image_widget ("Range image");

range_image_widget.showRangeImage (range_image);/*********************************************************************************************************

创建RangeImageBorderExtractor对象，它是用来进行边缘提取的，因为NARF的第一步就是需要探测出深度图像的边缘，

*********************************************************************************************************/

//--------------------------------//-----Extract NARF keypoints-----//--------------------------------

pcl::RangeImageBorderExtractor range_image_border_extractor; //用来提取边缘

pcl::NarfKeypoint narf_keypoint_detector; //用来检测关键点

narf_keypoint_detector.setRangeImageBorderExtractor (&range_image_border_extractor); // narf_keypoint_detector.setRangeImage (&range_image);

narf_keypoint_detector.getParameters ().support_size= support_size; //设置NARF的参数

pcl::PointCloudkeypoint_indices;

narf_keypoint_detector.compute (keypoint_indices);

std::cout<< "Found"<

pcl::PointCloud<:pointxyz>::Ptr keypoints_ptr (new pcl::PointCloud<:pointxyz>);

pcl::PointCloud<:pointxyz>& keypoints = *keypoints_ptr;

keypoints.points.resize (keypoint_indices.points.size ());for (size_t i=0; i

keypoints.points[i].getVector3fMap ()=range_image.points[keypoint_indices.points[i]].getVector3fMap ();

pcl::visualization::PointCloudColorHandlerCustom<:pointxyz> keypoints_color_handler (keypoints_ptr, 0, 255, 0);

viewer.addPointCloud<:pointxyz> (keypoints_ptr, keypoints_color_handler, "keypoints");

viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,7, "keypoints");//------------------------------------------------------//-----Extract NARF descriptors for interest points-----//------------------------------------------------------

std::vectorkeypoint_indices2;

keypoint_indices2.resize (keypoint_indices.points.size ());for (unsigned int i=0; i

keypoint_indices2[i]=keypoint_indices.points[i];

pcl::NarfDescriptor narf_descriptor (&range_image, &keypoint_indices2);

narf_descriptor.getParameters ().support_size=support_size;

narf_descriptor.getParameters ().rotation_invariant=rotation_invariant;

pcl::PointCloud<:narf36>narf_descriptors;

narf_descriptor.compute (narf_descriptors);

cout<< "Extracted"<

<

while (!viewer.wasStopped ())

{

range_image_widget.spinOnce ();//process GUI events

viewer.spinOnce ();

pcl_sleep(0.01);

}

}