PCL笔记八：关键点_pcl::pointcloud

关键点提取是2D和3D信息处理中不可或缺的关键技术。

NARF（Normal Aligned Radial Feature）关键点

NARF（Normal Aligned Radial Feature）关键点是为了从深度图像中识别物体而提出的，关键点探测的重要一步是减少特征提取时的搜索空间，把重点放在重要的结构上，对NARF关键点提取过程中有以下要求：

• 提取的过程必须将边缘以及物体表面变化信息考虑在内；
• 关键点的位置必须稳定，可以被重复探测，及时换了不同的视角；
• 关键点所在的位置必须有稳定的支持区域，可以计算描述子并进行唯一的法向量估计。

关键点探测步骤如下：

1. 遍历每个深度图像点，通过寻找在近邻区域有深度突变的位置进行边缘检测。
2. 遍历每个深度图像点，根据近邻区域的表面变化决定一种测度表面变化的系数，以及变化的主方向。
3. 根据第二步找到的主方向计算兴趣值，表征该方向与其他方向的不同，以及该处表面的变化情况，即改点有多稳定。
4. 对兴趣值进行平滑过滤。
5. 进行无最大值压缩找到最终的关键点，纪委NARF关键点。

Harris关键点：

Harris关键点检测通过计算图像点的Harris矩阵和矩阵对应的特征值来判断是否为关键点。如果Harris矩阵特征的两个特征值都很大，则该点为关键点。

Harris关键点检测算子对于图像旋转变换保持较好的检测重复率，但不适合尺寸变化的关键点检测。

点云中的3D Harris关键点借鉴了2D Harris关键点检测的思想，不过3D Harris关键点检测使用的是点云表面法向量的信息，而不是2D Harris关键点检测使用的图像梯度。

距离图像中提取NARF关键点

/* \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>
#include <pcl/common/file_io.h>   // for getFilenameWithoutExtension
 
#ifdef WIN32
#define pcl_sleep(x) Sleep(1000*(x))
#else
#define pcl_sleep(x) sleep(x)
#endif
 
 
typedef pcl::PointXYZ PointType;
//参数
float angular_resolution = 0.5f;
float support_size = 0.2f;
pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::CAMERA_FRAME;
bool setUnseenToMaxRange = false;
//打印帮助
 
void
printUsage(const char* 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, const Eigen::Affine3f& viewer_pose)
{
	Eigen::Vector3f pos_vector = viewer_pose * Eigen::Vector3f(0, 0, 0);
	Eigen::Vector3f look_at_vector = viewer_pose.rotation() * Eigen::Vector3f(0, 0, 1) + pos_vector;
	Eigen::Vector3f up_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.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]);
	//viewer.updateCamera();
}
 
// -----Main-----
int
main(int argc, char** argv)
{
	//解析命令行参数
	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";
	}
	int tmp_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>::Ptr point_cloud_ptr(new pcl::PointCloud<PointType>);
	pcl::PointCloud<PointType>& point_cloud = *point_cloud_ptr;
	pcl::PointCloud<pcl::PointWithViewpoint>far_ranges;
	Eigen::Affine3f scene_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::string filename = 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]);
			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 \"" << far_ranges_filename << "\" does not exists.\n";
	}
	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;
	}
 
	//从点云创建距离图像
	float noise_level = 0.0;
	float min_range = 0.0f;
	int border_size = 1;
	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.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::PCLVisualizer viewer("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::RangeImageVisualizer range_image_widget("Range image");
	range_image_widget.showRangeImage(range_image);
 
	//提取NARF关键点
	pcl::RangeImageBorderExtractor range_image_border_extractor;  // 用来边缘提取。NARF第一步就是需要探测出深度图像的边缘。
	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.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>::Ptr keypoints_ptr(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>& keypoints = *keypoints_ptr;
	keypoints.points.resize(keypoint_indices.points.size());
	for (size_t i = 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);
	}
}

SIFT关键点提取

SIFT：尺度不变特征变换（Scale-invariant feature transform, SIFT）。局部特征描述子。

#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::PointWithScale。
 
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_temp(new pcl::PointCloud<pcl::PointXYZ>);
    // 为了后期处理与显示的需要。需要将SIFT关键点检测结果转换为点类型为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;
 
}

SIFT关键点检测队形相关的参数：

setScales函数用于指定搜索关键点的尺度范围。

sift.setScales(min_scale, n_octaves, n_scales_per_octave);//指定搜索关键点的尺度范围：

• min_scale：用于设置尺度空间中最小尺度的标准偏差，
• 参数octaves是高斯金字塔中组（Octave）的数目，
• 参数nr_scales_per_octave是每组（Octave）计算的尺度（scale）数目。

Harris关键点提取

既可以提取角点也可以提取边缘点。3D Harris角点检测利用的是点云法向量的信息。

#include <iostream>
#include <pcl\io\pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/io/io.h>
#include <pcl/keypoints/harris_3D.h>  //harris特征点估计类头文件声明
#include <cstdlib>
#include <vector>
#include <pcl/console/parse.h>
using namespace std;
 
 
 
int main(int argc, char* argv[])
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::io::loadPCDFile(argv[1], *input_cloud);
	pcl::PCDWriter writer;
	float r_normal;
	float r_keypoint;
 
	r_normal = stof(argv[2]);
	r_keypoint = stof(argv[3]);
 
	typedef pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZI> ColorHandlerT3;
 
	// 创建关键点估计对象，用于保存Harris关键点。
	pcl::PointCloud<pcl::PointXYZI>::Ptr Harris_keypoints(new pcl::PointCloud<pcl::PointXYZI>());
	// Harris特征检测对象
	pcl::HarrisKeypoint3D<pcl::PointXYZ, pcl::PointXYZI, pcl::Normal>* harris_detector = new pcl::HarrisKeypoint3D<pcl::PointXYZ, pcl::PointXYZI, pcl::Normal>;
 
	//harris_detector->setNonMaxSupression(true);
	harris_detector->setRadius(r_normal);  // 法向量估计的半径。
	harris_detector->setRadiusSearch(r_keypoint);  // 关键点估计的近邻搜索半径。
	harris_detector->setInputCloud(input_cloud);
	//harris_detector->setNormals(normal_source);
	//harris_detector->setMethod(pcl::HarrisKeypoint3D<pcl::PointXYZRGB,pcl::PointXYZI>::LOWE);
	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, ColorHandlerT3(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.spin();
}

设置【项目】【属性】【配置属性】【调试】【命令参数】：room.pcd 0.1 0.1

基于对应点分类的对象识别

对应点聚类算法：利用特征匹配得到场景中的对应点，基于对应点聚类为待识别模型实例。

算法输出的每个聚类，代表场景中的一个模型实例假设，同时，对应一个变换矩阵，是该模型实例假设对应的位姿估计。

#include <tchar.h>
 
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/correspondence.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/features/shot_omp.h>
#include <pcl/features/board.h>
#include <pcl/keypoints/uniform_sampling.h>
#include <pcl/recognition/cg/hough_3d.h>
#include <pcl/recognition/cg/geometric_consistency.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/kdtree/impl/kdtree_flann.hpp>
#include <pcl/common/transforms.h>
#include <pcl/console/parse.h>
#include <pcl/filters/uniform_sampling.h>   // class "pcl::UniformSampling<PointType>" 没有成员 "compute"
 
typedef pcl::PointXYZRGBA PointType;
typedef pcl::Normal NormalType;
typedef pcl::ReferenceFrame RFType;
typedef pcl::SHOT352 DescriptorType;
 
std::string model_filename_;
std::string scene_filename_;
 
//Algorithm params
bool show_keypoints_(false);
bool show_correspondences_(false);
bool use_cloud_resolution_(false);
bool use_hough_(true);
float model_ss_(0.01f);
float scene_ss_(0.03f);
float rf_rad_(0.015f);
float descr_rad_(0.02f);
float cg_size_(0.01f);
float cg_thresh_(5.0f);
 
# define pcl_isfinite(x) std::isfinite(x)
 
 
void
showHelp(char* filename)
{
    std::cout << std::endl;
    std::cout << "***************************************************************************" << std::endl;
    std::cout << "*                                                                         *" << std::endl;
    std::cout << "*             Correspondence Grouping Tutorial - Usage Guide              *" << std::endl;
    std::cout << "*                                                                         *" << std::endl;
    std::cout << "***************************************************************************" << std::endl << std::endl;
    std::cout << "Usage: " << filename << " model_filename.pcd scene_filename.pcd [Options]" << std::endl << std::endl;
    std::cout << "Options:" << std::endl;
    std::cout << "     -h:                     Show this help." << std::endl;
    std::cout << "     -k:                     Show used keypoints." << std::endl;
    std::cout << "     -c:                     Show used correspondences." << std::endl;
    std::cout << "     -r:                     Compute the model cloud resolution and multiply" << std::endl;
    std::cout << "                             each radius given by that value." << std::endl;
    std::cout << "     --algorithm (Hough|GC): Clustering algorithm used (default Hough)." << std::endl;
    std::cout << "     --model_ss val:         Model uniform sampling radius (default 0.01)" << std::endl;
    std::cout << "     --scene_ss val:         Scene uniform sampling radius (default 0.03)" << std::endl;
    std::cout << "     --rf_rad val:           Reference frame radius (default 0.015)" << std::endl;
    std::cout << "     --descr_rad val:        Descriptor radius (default 0.02)" << std::endl;
    std::cout << "     --cg_size val:          Cluster size (default 0.01)" << std::endl;
    std::cout << "     --cg_thresh val:        Clustering threshold (default 5)" << std::endl << std::endl;
}
 
void
parseCommandLine(int argc, char* argv[])
{
    //Show help
    if (pcl::console::find_switch(argc, argv, "-h"))
    {
        showHelp(argv[0]);
        exit(0);
    }
 
    //Model & scene filenames
    std::vector<int> filenames;
    filenames = pcl::console::parse_file_extension_argument(argc, argv, ".pcd");
    if (filenames.size() != 2)
    {
        std::cout << "Filenames missing.\n";
        showHelp(argv[0]);
        exit(-1);
    }
 
    model_filename_ = argv[filenames[0]];
    scene_filename_ = argv[filenames[1]];
 
    //Program behavior
    if (pcl::console::find_switch(argc, argv, "-k"))
    {
        show_keypoints_ = true;
    }
    if (pcl::console::find_switch(argc, argv, "-c"))
    {
        show_correspondences_ = true;
    }
    if (pcl::console::find_switch(argc, argv, "-r"))
    {
        // 所有参数将于点云分辨率相乘，得到最终使用的参数。
        use_cloud_resolution_ = true;
    }
 
    std::string used_algorithm;
    if (pcl::console::parse_argument(argc, argv, "--algorithm", used_algorithm) != -1)
    {
        if (used_algorithm.compare("Hough") == 0)
        {
            use_hough_ = true;
        }
        else if (used_algorithm.compare("GC") == 0)
        {
            use_hough_ = false;
        }
        else
        {
            std::cout << "Wrong algorithm name.\n";
            showHelp(argv[0]);
            exit(-1);
        }
    }
 
    //General parameters
    pcl::console::parse_argument(argc, argv, "--model_ss", model_ss_);
    pcl::console::parse_argument(argc, argv, "--scene_ss", scene_ss_);
    pcl::console::parse_argument(argc, argv, "--rf_rad", rf_rad_);
    pcl::console::parse_argument(argc, argv, "--descr_rad", descr_rad_);
    pcl::console::parse_argument(argc, argv, "--cg_size", cg_size_);
    pcl::console::parse_argument(argc, argv, "--cg_thresh", cg_thresh_);
}
 
// 计算点云的空间分辨率：算出输入点云的每个点与其临近点距离的平均值。
double
computeCloudResolution(const pcl::PointCloud<PointType>::ConstPtr& cloud)
{
    double res = 0.0;
    int n_points = 0;
    int nres;
    std::vector<int> indices(2);
    std::vector<float> sqr_distances(2);
    pcl::search::KdTree<PointType> tree;
    tree.setInputCloud(cloud);
 
    for (size_t i = 0; i < cloud->size(); ++i)
    {
        if (!pcl_isfinite((*cloud)[i].x))
        {
            continue;
        }
        //Considering the second neighbor since the first is the point itself.
        nres = tree.nearestKSearch(i, 2, indices, sqr_distances);
        if (nres == 2)
        {
            res += sqrt(sqr_distances[1]);
            ++n_points;
        }
    }
    if (n_points != 0)
    {
        res /= n_points;
    }
    return res;
}
 
int
main(int argc, char* argv[])
{
    parseCommandLine(argc, argv);
 
    pcl::PointCloud<PointType>::Ptr model(new pcl::PointCloud<PointType>());
    pcl::PointCloud<PointType>::Ptr model_keypoints(new pcl::PointCloud<PointType>());
    pcl::PointCloud<PointType>::Ptr scene(new pcl::PointCloud<PointType>());
    pcl::PointCloud<PointType>::Ptr scene_keypoints(new pcl::PointCloud<PointType>());
    pcl::PointCloud<NormalType>::Ptr model_normals(new pcl::PointCloud<NormalType>());
    pcl::PointCloud<NormalType>::Ptr scene_normals(new pcl::PointCloud<NormalType>());
    pcl::PointCloud<DescriptorType>::Ptr model_descriptors(new pcl::PointCloud<DescriptorType>());
    pcl::PointCloud<DescriptorType>::Ptr scene_descriptors(new pcl::PointCloud<DescriptorType>());
 
    //
    //  Load clouds
    //
    if (pcl::io::loadPCDFile(model_filename_, *model) < 0)
    {
        std::cout << "Error loading model cloud." << std::endl;
        showHelp(argv[0]);
        return (-1);
    }
    if (pcl::io::loadPCDFile(scene_filename_, *scene) < 0)
    {
        std::cout << "Error loading scene cloud." << std::endl;
        showHelp(argv[0]);
        return (-1);
    }
 
    //
    //  Set up resolution invariance
    //
    if (use_cloud_resolution_)
    {
        float resolution = static_cast<float> (computeCloudResolution(model));
        if (resolution != 0.0f)
        {
            model_ss_ *= resolution;
            scene_ss_ *= resolution;
            rf_rad_ *= resolution;
            descr_rad_ *= resolution;
            cg_size_ *= resolution;
        }
 
        std::cout << "Model resolution:       " << resolution << std::endl;
        std::cout << "Model sampling size:    " << model_ss_ << std::endl;
        std::cout << "Scene sampling size:    " << scene_ss_ << std::endl;
        std::cout << "LRF support radius:     " << rf_rad_ << std::endl;
        std::cout << "SHOT descriptor radius: " << descr_rad_ << std::endl;
        std::cout << "Clustering bin size:    " << cg_size_ << std::endl << std::endl;
    }
 
    //
    //  Compute Normals
    // 计算场景和模型的每一个点的法向量，
    pcl::NormalEstimationOMP<PointType, NormalType> norm_est;
    norm_est.setKSearch(10);   // 计算法向量时，对于每一个点利用10个临近点，
    // 该临近点设置的数量是一个经验值，已被证实可以较好的应用于Kinect等获取的数据分辨率;但对于非常稠密的点云数据来说，可以用近似半径来控制 或设置其他参数，很大程度上取决于数据。
    norm_est.setInputCloud(model);
    norm_est.compute(*model_normals);
 
    norm_est.setInputCloud(scene);
    norm_est.compute(*scene_normals);
 
    //
    //  Downsample Clouds to Extract keypoints
    // 为了获取较为稀疏的关键点，下采样，
    pcl::PointCloud<int> sampled_indices;
 
    pcl::UniformSampling<PointType> uniform_sampling;
    uniform_sampling.setInputCloud(model);
    uniform_sampling.setRadiusSearch(model_ss_);  // 均匀采样的半径：默认值：0.03
    //uniform_sampling.compute(sampled_indices);
    //pcl::copyPointCloud(*model, sampled_indices.points, *model_keypoints);
    uniform_sampling.filter(*model_keypoints);
 
    std::cout << "Model total points: " << model->size() << "; Selected Keypoints: " << model_keypoints->size() << std::endl;
 
    uniform_sampling.setInputCloud(scene);
    uniform_sampling.setRadiusSearch(scene_ss_);
    //uniform_sampling.compute(sampled_indices);
    //pcl::copyPointCloud(*scene, sampled_indices.points, *scene_keypoints);
    uniform_sampling.filter(*scene_keypoints);
    std::cout << "Scene total points: " << scene->size() << "; Selected Keypoints: " << scene_keypoints->size() << std::endl;
 
 
    //
    //  Compute Descriptor for keypoints
    // 为场景和模型的每个关键点建立特征描述子，计算每个模型和场景的关键点的3D描述子。SHOT描述子。
    pcl::SHOTEstimationOMP<PointType, NormalType, DescriptorType> descr_est;
    descr_est.setRadiusSearch(descr_rad_);   // 描述子描述区域范围大小。
 
    descr_est.setInputCloud(model_keypoints);
    descr_est.setInputNormals(model_normals);
    descr_est.setSearchSurface(model);
    descr_est.compute(*model_descriptors);
 
    descr_est.setInputCloud(scene_keypoints);
    descr_est.setInputNormals(scene_normals);
    descr_est.setSearchSurface(scene);
    descr_est.compute(*scene_descriptors);
 
    // 利用KD树结构找到模型和场景的对应点。
    //  Find Model-Scene Correspondences with KdTree
    //模型和场景描述子点云之间对应点对集合。
    pcl::CorrespondencesPtr model_scene_corrs(new pcl::Correspondences());
    // 构造模型描述子点云的KdTreeFLANN，
    pcl::KdTreeFLANN<DescriptorType> match_search;
    match_search.setInputCloud(model_descriptors);
 
    //  For each scene keypoint descriptor, find nearest neighbor into the model keypoints descriptor cloud and add it to the correspondences vector.// 最近邻模型描述子 添加到 对应场景描述子 的 向量中。
    for (size_t i = 0; i < scene_descriptors->size(); ++i)
    {
        std::vector<int> neigh_indices(1);
        std::vector<float> neigh_sqr_dists(1);
        if (!pcl_isfinite(scene_descriptors->at(i).descriptor[0])) //skipping NaNs
        {
            continue;
        }
        int found_neighs = match_search.nearestKSearch(scene_descriptors->at(i), 1, neigh_indices, neigh_sqr_dists);
        if (found_neighs == 1 && neigh_sqr_dists[0] < 0.25f) //  add match only if the squared descriptor distance is less than 0.25 (SHOT descriptor distances are between 0 and 1 by design)
        // 当描述子平均距离小于0.25,添加匹配点，SHOT描述子本身设计使其距离保持在0到1之间。
        {
            pcl::Correspondence corr(neigh_indices[0], static_cast<int> (i), neigh_sqr_dists[0]);
            model_scene_corrs->push_back(corr);
        }
    }
    std::cout << "Correspondences found: " << model_scene_corrs->size() << std::endl;
 
    //
    //  Actual Clustering聚类。
    //
    std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > rototranslations;
    std::vector<pcl::Correspondences> clustered_corrs;
 
    //  Using Hough3D   Hough投票过程。
    if (use_hough_)
    {
        //
        //  Compute (Keypoints) Reference Frames only for Hough
        // 利用Hough算法，需要计算关键点的局部参考坐标系。
        pcl::PointCloud<RFType>::Ptr model_rf(new pcl::PointCloud<RFType>());
        pcl::PointCloud<RFType>::Ptr scene_rf(new pcl::PointCloud<RFType>());
        // 局部参考坐标系LRF。
        pcl::BOARDLocalReferenceFrameEstimation<PointType, NormalType, RFType> rf_est;
        rf_est.setFindHoles(true);
        rf_est.setRadiusSearch(rf_rad_);  // 估计局部参考坐标系时，当前点的领域搜索半径。
 
        rf_est.setInputCloud(model_keypoints);
        rf_est.setInputNormals(model_normals);
        rf_est.setSearchSurface(model);
        rf_est.compute(*model_rf);
 
        rf_est.setInputCloud(scene_keypoints);
        rf_est.setInputNormals(scene_normals);
        rf_est.setSearchSurface(scene);
        rf_est.compute(*scene_rf);
 
        //  Clustering聚类。
        pcl::Hough3DGrouping<PointType, PointType, RFType, RFType> clusterer;
        clusterer.setHoughBinSize(cg_size_);  // Hough空间的采样间隔
        clusterer.setHoughThreshold(cg_thresh_);  // Hough空间确定是否有实例存在的最少票数阈值。
        clusterer.setUseInterpolation(true);  // 是否对投票在Hough空间进行插值计算。
        clusterer.setUseDistanceWeight(false);  // 在投票时是否将对应点之间的距离作为权重参与计算。
 
        clusterer.setInputCloud(model_keypoints);
        clusterer.setInputRf(model_rf);  // 设置模型对应的LRF
        clusterer.setSceneCloud(scene_keypoints);
        clusterer.setSceneRf(scene_rf);  // 设置场景对应的LRF
        clusterer.setModelSceneCorrespondences(model_scene_corrs);  // 设置模型与场景的对应点对集合。
 
        //clusterer.cluster (clustered_corrs);
        clusterer.recognize(rototranslations, clustered_corrs);  // 结果包含 变换矩阵 和对应点 聚类 结果。
    }
    else // Using GeometricConsistency  // 使用几何一致性聚类
    {
        pcl::GeometricConsistencyGrouping<PointType, PointType> gc_clusterer;  // 算法实例。
        gc_clusterer.setGCSize(cg_size_);  // 检查几何一致性时的空间分辨率。
        gc_clusterer.setGCThreshold(cg_thresh_);  // 设置最小的聚类数量。
 
        gc_clusterer.setInputCloud(model_keypoints);  // 设置模型关键点。
        gc_clusterer.setSceneCloud(scene_keypoints);  // 设置场景关键点。
        gc_clusterer.setModelSceneCorrespondences(model_scene_corrs);
 
        //gc_clusterer.cluster (clustered_corrs);
        gc_clusterer.recognize(rototranslations, clustered_corrs);// 结果包含 变换矩阵 和对应点 聚类 结果。
    }
 
    //
    //  Output results
    // 识别算法返回一个Eigen::Matrix4f类型的矩阵向量，该矩阵向量代表场景中找到模型的每个实例的变换矩阵（旋转矩阵 + 平移向量）；
    // 识别算法还返回对应的支持每个模型实例的对应点对聚类，以向量形式保存，该向量的每个元素依次都是对应点对的集合，这些集合代表与场景中具体实例相关联的对应点。
    std::cout << "Model instances found: " << rototranslations.size() << std::endl;
    for (size_t i = 0; i < rototranslations.size(); ++i)
    {
        std::cout << "\n    Instance " << i + 1 << ":" << std::endl;
        std::cout << "        Correspondences belonging to this instance: " << clustered_corrs[i].size() << std::endl;
 
        // Print the rotation matrix and translation vector
        Eigen::Matrix3f rotation = rototranslations[i].block<3, 3>(0, 0);
        Eigen::Vector3f translation = rototranslations[i].block<3, 1>(0, 3);
 
        printf("\n");
        printf("            | %6.3f %6.3f %6.3f | \n", rotation(0, 0), rotation(0, 1), rotation(0, 2));
        printf("        R = | %6.3f %6.3f %6.3f | \n", rotation(1, 0), rotation(1, 1), rotation(1, 2));
        printf("            | %6.3f %6.3f %6.3f | \n", rotation(2, 0), rotation(2, 1), rotation(2, 2));
        printf("\n");
        printf("        t = < %0.3f, %0.3f, %0.3f >\n", translation(0), translation(1), translation(2));
    }
 
    //
    //  Visualization
    //
    pcl::visualization::PCLVisualizer viewer("点云库PCL学习教程第二版-基于对应点聚类的3D模型识别");
    viewer.addPointCloud(scene, "scene_cloud");
    viewer.setBackgroundColor(255, 255, 255);
    pcl::PointCloud<PointType>::Ptr off_scene_model(new pcl::PointCloud<PointType>());
    pcl::PointCloud<PointType>::Ptr off_scene_model_keypoints(new pcl::PointCloud<PointType>());
 
    if (show_correspondences_ || show_keypoints_)
    {
        //  We are translating the model so that it doesn't end in the middle of the scene representation
        pcl::transformPointCloud(*model, *off_scene_model, Eigen::Vector3f(0, 0, 0), Eigen::Quaternionf(1, 0, 0, 0));
        pcl::transformPointCloud(*model_keypoints, *off_scene_model_keypoints, Eigen::Vector3f(0, 0, 0), Eigen::Quaternionf(1, 0, 0, 0));
        pcl::visualization::PointCloudColorHandlerCustom<PointType> off_scene_model_color_handler(off_scene_model, 0, 255, 0);
        viewer.addPointCloud(off_scene_model, off_scene_model_color_handler, "off_scene_model");
    }
    if (show_keypoints_)
    {
        pcl::visualization::PointCloudColorHandlerCustom<PointType> scene_keypoints_color_handler(scene_keypoints, 0, 0, 255);
        viewer.addPointCloud(scene_keypoints, scene_keypoints_color_handler, "scene_keypoints");
        viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "scene_keypoints");
        pcl::visualization::PointCloudColorHandlerCustom<PointType> off_scene_model_keypoints_color_handler(off_scene_model_keypoints, 0, 0, 255);
        viewer.addPointCloud(off_scene_model_keypoints, off_scene_model_keypoints_color_handler, "off_scene_model_keypoints");
        viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "off_scene_model_keypoints");
    }
    for (size_t i = 0; i < rototranslations.size(); ++i)
    {
        pcl::PointCloud<PointType>::Ptr rotated_model(new pcl::PointCloud<PointType>());
        pcl::transformPointCloud(*model, *rotated_model, rototranslations[i]);
        std::stringstream ss_cloud;
        ss_cloud << "instance" << i;
        pcl::visualization::PointCloudColorHandlerCustom<PointType> rotated_model_color_handler(rotated_model, 255, 0, 0);
        viewer.addPointCloud(rotated_model, rotated_model_color_handler, ss_cloud.str());
        if (show_correspondences_)
        {
            for (size_t j = 0; j < clustered_corrs[i].size(); ++j)
            {
                std::stringstream ss_line;
                ss_line << "correspondence_line" << i << "_" << j;
                PointType& model_point = off_scene_model_keypoints->at(clustered_corrs[i][j].index_query);
                PointType& scene_point = scene_keypoints->at(clustered_corrs[i][j].index_match);
                //  We are drawing a line for each pair of clustered correspondences found between the model and the scene
                viewer.addLine<PointType, PointType>(model_point, scene_point, 0, 255, 0, ss_line.str());
            }
        }
    }
    while (!viewer.wasStopped())
    {
        viewer.spinOnce();
    }
    return (0);
}

需要在 

添加参数如下： 

milk_pose_changed.pcd milk_cartoon_all_small_clorox.pcd -k -c

 bug记录。