参考引用

• Point Cloud Library
• 黑马机器人 | PCL-3D点云

PCL点云库学习笔记（文章链接汇总）

1. 引言

• 关键点也称为兴趣点，它是 2D 图像或 3D 点云或曲面模型上,可以通过检测标准来获取的具有稳定性、区别性的点集。从技术上来说，关键点的数量比原始点云或图像的数据量少很多，其与局部特征描述子结合组成关键点描述子。常用来构成原始数据的紧凑表示，具有代表性与描述性，从而加快后续识别、追踪等对数据的处理速度

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

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

• 为了满足上述要求，可以通过以下检测步骤来进行关键点提取

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

2. 代码实现

• narf_keypoint_extraction.cpp

#include 

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

typedef pcl::PointXYZ PointType;

// -----Parameters-----
float angular_resolution = 0.5f;
float support_size = 0.2f;
pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::CAMERA_FRAME;
// 如果无法提供超出这些应该观察到的传感器范围的点
// 则可以使用 setUnseenToMaxRange 函数，将那些点设置为最大深度
bool setUnseenToMaxRange = false;

// // 命令帮助提示
void printUsage(const char *progName) {
    std::cout << "\n\nUsage: " << progName << " [options] \n\n"
              << "Options:\n"
              << "-------------------------------------------\n"
              << "-r    angular resolution in degrees (default " << angular_resolution << ")\n"    // 指定角度分辨率的浮点数值，默认值为 angular_resolution
              << "-c      coordinate frame (default " << (int) coordinate_frame << ")\n"    // 指定坐标系的整数值，默认值为 coordinate_frame
              << "-m           Treat all unseen points as maximum range readings\n"    // 将所有未被观察到的点视为最大距离处理
              << "-s    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.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) {
    // 解析命令行参数（执行时在 ./range_image_border_extraction.cpp 后添加）
    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 文件；如果没有输入 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 ()); // 申明传感器的位置是一个 4*4 的仿射变换
    // 解析命令行参数，并返回所有 .pcd 扩展名的文件的索引列表
    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]]; // 保存从命令行参数获取到的第一个 PCD 文件名
        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：可以表示平移、旋转等多种变换，并且支持矩阵乘法来组合多个变换
        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_);
        // 去除文件扩展名，并添加 "_far_ranges.pcd" 来创建输出文件名
        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 => 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.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;
    // 使用智能指针 boost::shared_ptr 管理内存，确保在不需要该对象时自动释放其占用的空间
    boost::shared_ptr<pcl::RangeImage> 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();

    // 设置点云可视化
    pcl::visualization::PCLVisualizer viewer("3D Viewer");
    viewer.setBackgroundColor(1, 1, 1);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointWithRange> range_image_color_handler(range_image_ptr,
                                                                                                    255, 0, 0);
    viewer.addPointCloud(range_image_ptr, range_image_color_handler, "range image");
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "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 ());

    // 显示深度图像
    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);  // 设置要用于关键点检测的 RangeImage 对象
        // support_size 是一个变量，其值被用于设置支持半径的大小参数
    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";

    // -----Show keypoints in range image widget-----
    //for (size_t i=0; i
    //range_image_widget.markPoint (keypoint_indices.points[i]%range_image.width,
    //keypoint_indices.points[i]/range_image.width);

    // 提取出的关键点可视化
    pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints_ptr(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ> &keypoints = *keypoints_ptr;
    // 调整 keypoints 对象中的点数以与索引点数相同，以存储从 range_image 中提取的关键点
    keypoints.points.resize(keypoint_indices.points.size());
    // 对于每个索引点，代码将在 range_image 中找到相应的点，并将其复制到 keypoints 数据集中
        // getVector3fMap() 方法用于获取三维向量的映射，并将其设置为新点的位置
    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, 5, "keypoints");

    while (!viewer.wasStopped()) {
        range_image_widget.spinOnce();  // process GUI events
        viewer.spinOnce();
        pcl_sleep(0.01);
    }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181

• 配置文件 CMakeLists.txt

  cmake_minimum_required(VERSION 3.5 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
  2
  3
  4
  5
  6
  7
  8
  9
  10
  11
  12

• 编译并执行

  $ mkdir build
  $ cd build
  $ cmake ..
  $ make
  
  $ ./narf_keypoint_extraction -m ../table_scene_lms400.pcd
  1
  2
  3
  4
  5
  6

  # 输出结果（见下图，绿色的点为提取的关键点）
  Found 37 key points.
  1
  2