第一章 点云数据采集
第二章 点云滤波
第二章 点云降采样

---

1.点云关键点是什么？

关键点也称为兴趣点，它是2D图像、3D点云或曲面模型上，可以通过定义检测标准来获取的具有稳定性、区别性的点集。

我们获得的数据量大，特别是几十万个以上的点云，里面有很多冗余数据，会导致处理起来比较耗时。
降采样是一种有效的减少数据、缩减计算量的方法。

2.关键点检测算法(本数据单位为mm，通常数据单位为m，取半径之类得参数要除1000)

2.1 ISS关键点

ISS 关键点通过计算每个点与其近邻点的曲率变化，得到该点的稳定性和自适应尺度，从而提取稳定性和尺度合适的关键点。

Open3d(需要ply格式)

import open3d as o3d
import numpy as np
import time
class o3dtut:
    def get_bunny_mesh():
        mesh = o3d.io.read_triangle_mesh("second_radius_cloud.ply")
        mesh.compute_vertex_normals()
        return mesh
    def keypoints_to_spheres(keypoints):

        spheres = o3d.geometry.TriangleMesh()
        for keypoint in keypoints.points:
         sphere = o3d.geometry.TriangleMesh.create_sphere(radius=10)
         sphere.translate(keypoint)
         spheres += sphere
         spheres.paint_uniform_color([1.0, 0.0, 0.0])
        return spheres


mesh = o3dtut.get_bunny_mesh()
pcd = o3d.geometry.PointCloud()
pcd.points = mesh.vertices
tic = time.time()
keypoints = o3d.geometry.keypoint.compute_iss_keypoints(pcd,
                                                        salient_radius=10,
                                                        non_max_radius=10,
                                                        gamma_21=0.5,
                                                        gamma_32=0.5)
toc = 1000 * (time.time() - tic)
print("ISS Computation took {:.0f} [ms]".format(toc))

mesh.compute_vertex_normals()
mesh.paint_uniform_color([0.0, 0.0, 1.0])
o3d.visualization.draw_geometries([o3dtut.keypoints_to_spheres(keypoints), mesh],window_name="ISS检测",width=800,height=800)

PCL

#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/keypoints/iss_3d.h>
#include <pcl/visualization/pcl_visualizer.h>

int main(int argc, char** argv)
{
    // 读取点云数据
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
    if (pcl::io::loadPCDFile<pcl::PointXYZ>("second_radius_cloud.pcd", *cloud) == -1)
    {
        PCL_ERROR("Couldn't read the input PCD file \n");
        return (-1);
    }

    // ISS关键点检测参数
    double salient_radius = 10;
    double non_max_radius = 10;
    double gamma_21 = 0.5;
    double gamma_32 = 0.5;
    double min_neighbors = 5;
    int threads = 4;

    // 执行ISS关键点检测
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
    pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::ISSKeypoint3D<pcl::PointXYZ, pcl::PointXYZ> iss_detector;
    iss_detector.setSearchMethod(tree);
    iss_detector.setSalientRadius(salient_radius);
    iss_detector.setNonMaxRadius(non_max_radius);
    iss_detector.setThreshold21(gamma_21);
    iss_detector.setThreshold32(gamma_32);
    iss_detector.setMinNeighbors(min_neighbors);
    iss_detector.setNumberOfThreads(threads);
    iss_detector.setInputCloud(cloud);
    iss_detector.compute(*keypoints);

    // 可视化
    pcl::visualization::PCLVisualizer viewer("ISS Keypoints");
    viewer.setBackgroundColor(0, 0, 0);
    viewer.addPointCloud<pcl::PointXYZ>(cloud, "cloud");
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud");
    viewer.addPointCloud<pcl::PointXYZ>(keypoints, "keypoints");
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "keypoints");
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 0, 0, "keypoints");
    while (!viewer.wasStopped())
    {
        viewer.spinOnce(100);
    }

    return 0;
}

2.2 Harris关键点

Harris关键点检测通过计算每个点的协方差矩阵，求解特征值和特征向量，来判断该点是否为关键点。如果Harris矩阵的两个特征值都很大，则该点是关键点。具有较好的旋转不变性和尺度不变性。

PCL

#include
#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
#include
#include <pcl/console/parse.h>
using namespace std;

int main()
{
pcl::PointCloudpcl::PointXYZ::Ptr input_cloud (new pcl::PointCloudpcl::PointXYZ);
pcl::io::loadPCDFile (“second_radius_cloud.pcd”, *input_cloud);
pcl::PCDWriter writer;
float r_normal=10;//法向量估计的半径
float r_keypoint=40;//关键点估计的近邻搜索半径

typedef pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZI> ColorHandlerT3;

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);//结果存放在Harris_keypoints
pcl::visualization::PCLVisualizer viewer("clouds");
viewer.setBackgroundColor(255,255,255);
viewer.addPointCloud (Harris_keypoints, ColorHandlerT3 (Harris_keypoints, 0.0, 0.0, 255.0), "Harris_keypoints");
viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,8,"Harris_keypoints");
viewer.addPointCloud(input_cloud,"input_cloud");
viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR,0,0,0,"input_cloud");
viewer.initCameraParameters();
viewer.saveScreenshot("screenshot.png");
viewer.spin ();

}

2.3 NAFR关键点

NARF算法是一种基于法向量的点云关键点提取算法。它通过将点云投影到二维图像上，并计算每个像素周围梯度直方图，来寻找具有唯一性和重复性的关键点。

PCL
注意修改角度范围和角度分辨率

#include <iostream>
 #include <pcl/range_image/range_image.h> // 深度图头文件
 #include <pcl/io/pcd_io.h>               // PCD文件读取头
 #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> // NARF关键点计算头
 #include <pcl/features/narf_descriptor.h>// NARF描述子头
 #include <pcl/console/parse.h>           // 命令行解析头
 #include <pcl/common/file_io.h> // 用于获取没有拓展名的文件
 
 typedef pcl::PointXYZ PointType;
 
 float angular_resolution = 0.1875f; // 角度分辨率
 float support_size = 20.0f; // 关键点计算范围（计算范围球的半径）
 pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::CAMERA_FRAME; // 深度图坐标系
 bool setUnseenToMaxRange = false;  // 不设置深度范围
 bool rotation_invariant = true;	// 是否保持旋转不变性
 
 // --------------
 // -----输出帮助信息-----
// --------------
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 to max range\n"
             << "-s <float>   support size for the interest points (diameter of the used sphere - "
                                                                   "default "<<support_size<<")\n"
             << "-o <0/1>     switch rotational invariant version of the feature on/off"
             <<               " (default "<< (int)rotation_invariant<<")\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.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]);
 }
 

 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;
     std::cout << "Setting unseen values in range image to maximum range readings.\n";
   }
   if (pcl::console::parse (argc, argv, "-o", rotation_invariant) >= 0)
     std::cout << "Switching rotation invariant feature version "<< (rotation_invariant ? "on" : "off")<<".\n";
   int tmp_coordinate_frame;
   if (pcl::console::parse (argc, argv, "-c", tmp_coordinate_frame) >= 0)
   {
     coordinate_frame = pcl::RangeImage::CoordinateFrame (tmp_coordinate_frame);
     std::cout << "Using coordinate frame "<< (int)coordinate_frame<<".\n";
   }
   if (pcl::console::parse (argc, argv, "-s", support_size) >= 0)
     std::cout << "Setting support size to "<<support_size<<".\n";
   if (pcl::console::parse (argc, argv, "-r", angular_resolution) >= 0)
     std::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)
    {
      std::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)+".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;
    std::cout << "\nNo *.pcd file given => Generating 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.push_back (point);
      }
    }
    point_cloud.width = point_cloud.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 (60.0f), pcl::deg2rad (45.0f),
                                   scene_sensor_pose, coordinate_frame, noise_level, min_range, border_size);
  range_image.integrateFarRanges (far_ranges);
  if (setUnseenToMaxRange)
    range_image.setUnseenToMaxRange ();
  
  // 显示点云
  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.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;  
 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;
  
  pcl::PointCloud<int> keypoint_indices;// 关键点序列
 narf_keypoint_detector.compute (keypoint_indices);// 计算narf关键点，并按照序列排序
  std::cout << "Found "<<keypoint_indices.size ()<<" key points.\n";


  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");
  viewer.initCameraParameters();
  viewer.saveScreenshot("screenshot.png");
  // 为这些关键点计算NARF描述子
  std::vector<int> keypoint_indices2;
  keypoint_indices2.resize (keypoint_indices.size ());
  for (unsigned int i=0; i<keypoint_indices.size (); ++i) 
    keypoint_indices2[i]=keypoint_indices[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<pcl::Narf36> narf_descriptors;
 narf_descriptor.compute (narf_descriptors);
  std::cout << "Extracted "<<narf_descriptors.size ()<<" descriptors for "
                      <<keypoint_indices.size ()<< " keypoints.\n";
  while (!viewer.wasStopped ())
  {
    viewer.spinOnce ();
  }
}

2.4 SIFT关键点

SIFT3D算法是一种基于高斯差分和尺度空间的点云关键点提取算法。它通过在多个尺度下对点云进行高斯滤波和差分操作，来提取稳定性和尺度不变性的关键点。

PCL

#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>
 
namespace pcl
{
	template<>
	struct SIFTKeypointFieldSelector<PointXYZ>
	{
		inline float
			operator () (const PointXYZ &p) const
		{
			return p.z;
		}
	};
}
 
int main()
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz(new pcl::PointCloud<pcl::PointXYZ>);
	if (pcl::io::loadPCDFile<pcl::PointXYZ>("second_radius_cloud.pcd", *cloud_xyz) == -1) // load the file
	{
		PCL_ERROR("Couldn't read file");
		return -1;
	}
	std::cout << "points: " << cloud_xyz->points.size() << std::endl;
 
	// Parameters for sift computation
	const float min_scale = 5.0f; //the standard deviation of the smallest scale in the scale space
	const int n_octaves = 6;//the number of octaves (i.e. doublings of scale) to compute
	const int n_scales_per_octave = 4;//the number of scales to compute within each octave
	const float min_contrast = 1.0f;//the minimum contrast required for detection
 
 
	pcl::console::TicToc time;
	time.tic();
	// Estimate the sift interest points using z values from xyz as the Intensity variants
	pcl::SIFTKeypoint<pcl::PointXYZ, pcl::PointWithScale> sift;
	pcl::PointCloud<pcl::PointWithScale> result;
	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
	sift.setSearchMethod(tree);
	sift.setScales(min_scale, n_octaves, n_scales_per_octave);
	sift.setMinimumContrast(min_contrast);
	sift.setInputCloud(cloud_xyz);
	sift.compute(result);
	std::cout << "Computing the SIFT points takes " << time.toc() / 1000 << "seconds" << std::endl;
	std::cout << "No of SIFT points in the result are " << result.points.size() << std::endl;
 
	// Copying the pointwithscale to pointxyz so as visualize the cloud
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_temp(new pcl::PointCloud<pcl::PointXYZ>);
	copyPointCloud(result, *cloud_temp);
	std::cout << "SIFT points in the result are " << cloud_temp->points.size() << std::endl;
	// Visualization of keypoints along with the original cloud
	pcl::visualization::PCLVisualizer viewer("PCL Viewer");
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> keypoints_color_handler(cloud_temp, 0, 255, 0);
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_color_handler(cloud_xyz, 255, 0, 0);
	viewer.setBackgroundColor(0.0, 0.0, 0.0);
	viewer.addPointCloud(cloud_xyz, cloud_color_handler, "cloud");//add point cloud
	viewer.addPointCloud(cloud_temp, keypoints_color_handler, "keypoints");//add the keypoints
	viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, "keypoints");
	while (!viewer.wasStopped())
	{
		viewer.spinOnce();
	}
	return 0;
 
}