【点云PCL入门】关键点提取

1.深度图像中提取NARF关键点

#include<iostream>
#include <pcl/range_image/range_image.h>
#include <pcl/io/pcd_io.h>
#include <pcl/features/range_image_border_extractor.h>
#include <pcl/visualization/range_image_visualizer.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/keypoints/narf_keypoint.h>

// -----Parameters-----
float angular_resolution = 0.5f;
float support_size = 0.2f;
pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::CAMERA_FRAME;

int main(int argc, char* argv[])
{
	angular_resolution = pcl::deg2rad(angular_resolution);
    // -----Read pcd file or create example point cloud if not given-----
	pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>&point_cloud = *point_cloud_ptr;
	pcl::PointCloud<pcl::PointWithViewpoint>far_ranges;//远距离点云数据
	Eigen::Affine3f scense_sensor_pose(Eigen::Affine3f::Identity());
	
	if(pcl::io::loadPCDFile("./frame_00000.pcd",point_cloud)==-1)
	{
		std::cerr << "Was not able to open file \n";
		return 0;
	}
	scense_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_);

  // -----Create RangeImage from the PointCloud-----
	float noise_level = 0.0;
	float min_range = 0.0;
	int border_size = 2;
	pcl::RangeImage::Ptr range_image_ptr(new pcl::RangeImage);
	pcl::RangeImage&range_image = *range_image_ptr;
	range_image.createFromPointCloud(point_cloud, angular_resolution, pcl::deg2rad(360.0), pcl::deg2rad(180.0), scense_sensor_pose, coordinate_frame, noise_level, min_range, border_size);
	//range_image.integrateFarRanges(far_ranges);
	range_image.setUnseenToMaxRange();//不可见区域变为最大范围读数，识别出边界

    // -----Open 3D viewer and add point cloud-----
	pcl::visualization::PCLVisualizer viewer("3D Viewer");
	viewer.setBackgroundColor(1, 1, 1); 
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointWithRange>range_image_color_handle(range_image_ptr,0,0,0);
	viewer.addPointCloud(range_image_ptr, range_image_color_handle, "range image");
	viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "range image");
	viewer.initCameraParameters();
	// -----Show range image-----
	pcl::visualization::RangeImageVisualizer range_image_widget("Range image");
	range_image_widget.showRangeImage(range_image);
	// -----Extract NARF keypoints-----
	pcl::RangeImageBorderExtractor range_image_border_extractor;
	pcl::NarfKeypoint narf_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.size() << " key points.\n";

	// -----Show keypoints in 3D viewer-----
	pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints_ptr(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>& keypoints = *keypoints_ptr;
	keypoints.resize(keypoint_indices.size());
	for (std::size_t i = 0; i < keypoint_indices.size(); ++i)
		keypoints[i].getVector3fMap() = range_image[keypoint_indices[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);
	}
	return 0;
}

2.SIFT关键点提取

#include<iostream>
#include<pcl/io/pcd_io.h>
#include<pcl/point_types.h>
#include<pcl/keypoints/sift_keypoint.h>
#include <pcl/features/normal_3d.h>
#include<pcl/visualization/pcl_visualizer.h>

//pcl中sift特征需要返回强度信息，改为如下:
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("./pig.pcd", *cloud_xyz);
	//stof字符串转float
	const float min_scale = 0.01;//设置尺寸空间中最小尺度的标准偏差
	const int n_octaves = 6;//高斯金字塔中组的数目
	const int n_scales_per_octave = 4;//每组计算的尺度数目
	const float min_contrast = 0.01;

	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::PointWithScale的数据转换为点类型pcl::PointXYZ的数据
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_temp(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::copyPointCloud(result, *cloud_temp);

	//可视化输入点云和关键点
	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.Harris关键点提取

#include<iostream>
#include<pcl/io/pcd_io.h>
#include<pcl/visualization/pcl_visualizer.h>
#include<pcl/keypoints/harris_3D.h>

int main(int argc, char* argv[])
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::io::loadPCDFile("./roorm.pcd", *input_cloud);
	pcl::PCDWriter writer;
	float r_normal=0.1;
	float r_keypoint=0.1;

	//包含强度信息
	pcl::PointCloud<pcl::PointXYZI>::Ptr Harris_keypoints(new pcl::PointCloud<pcl::PointXYZI>);
	pcl::HarrisKeypoint3D<pcl::PointXYZ,pcl::PointXYZI,pcl::Normal>* harris_detector=new  pcl::HarrisKeypoint3D<pcl::PointXYZ, pcl::PointXYZI, pcl::Normal>;
	harris_detector->setRadius(r_normal);//设置法向量估计的半径
	harris_detector->setRadiusSearch(r_keypoint);//设置关键点估计的近邻搜索半径
	harris_detector->setInputCloud(input_cloud);
	harris_detector->compute(*Harris_keypoints);

	cout << "Harris_keypoints的大小是" << Harris_keypoints->size() << endl;
	writer.write<pcl::PointXYZI>("Harris_keypoints.pcd", *Harris_keypoints, false);

	pcl::visualization::PCLVisualizer visu3("clouds");
	visu3.setBackgroundColor(255, 255, 255);
	visu3.addPointCloud(Harris_keypoints, pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZI>(Harris_keypoints, 0.0, 0.0, 255.0), "Harris_keypoints");
	visu3.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 8, "Harris_keypoints");
	visu3.addPointCloud(input_cloud, "input_cloud");
	visu3.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0, 0, 0, "input_cloud");
	visu3.initCameraParameters();
	visu3.spin();

}

参考

1.https://pcl.readthedocs.io/projects/tutorials/en/latest/index.html#keypoints