open3D源码解读第三篇_open3d mahalanobis distance

2021SC@SDUSC

open3D点云类

Open3D是一个非常棒的点云处理库，包含一系列常用的点云处理函数，而且代码非常干净易读。

本篇解读PointCloud和PointCloudIO两个与点云有关的基类。

PointCloud

点云类。点云由点坐标以及可选的点颜色和点法线组成。

• 注意点：

  1. 由于需要解析的代码较多，为使代码解读更加清晰，我将代码分析的详细过程写在代码段的注释中。

  2. 中文部分是源码解读，包含代码分析和遇到的问题。

• 类中部分函数（由于PointCloud类中函数较多，因此展示部分关键函数）：

  1. Clear函数必须要有，因为基类Geometry中定义了纯虚函数。

  2. VoxelDownSample，向下采样输入点云到输出点云与体素的函数。

  3. VoxelDownSampleAndTrace，函数使用geometry.PointCloud.VoxelDownSample进行降低取样（缩小取样）。并且记录下采样前的点云指数。

  4. UniformDownSample，将输入点云统一下采样到输出点云的函数。采样按照点的顺序执行，总是选择第0个点，而不是随机的。

  5. RandomDownSample，将输入点云随机下采样到输出点云的函数。

  6. ComputeMahalanobisDistance，计算输入点云中点的马氏距离的函数。

  7. ComputeNearestNeighborDistance，计算从一个点到输入点云中它最近的邻居的距离的函数。

  8. ComputeConvexHull，利用qhull算法计算点云的凸包的函数。

  9. ClusterDBSCAN，点云集群使用DBSCAN算法。

  10. SegmentPlane，使用RANSAC算法分割点云平面。

#pragma once

#include <Eigen/Core>
#include <memory>
#include <tuple>
#include <vector>

#include "open3d/geometry/Geometry3D.h"
#include "open3d/geometry/KDTreeSearchParam.h"

namespace open3d {

namespace camera {
class PinholeCameraIntrinsic;
}

namespace geometry {

class Image;
class RGBDImage;
class TriangleMesh;
class VoxelGrid;

/// \class PointCloud
/// 点云由点坐标、点颜色和点法线组成。
class PointCloud : public Geometry3D {
public:
    /// 默认构造函数。
    PointCloud() : Geometry3D(Geometry::GeometryType::PointCloud) {}
    /// 参数化构造函数。
    /// \param points 点坐标。
    PointCloud(const std::vector<Eigen::Vector3d> &points)
        : Geometry3D(Geometry::GeometryType::PointCloud), points_(points) {}
    ~PointCloud() override {}

public:
    /// Clear函数必须要有，因为基类Geometry中定义了纯虚函数。
    /// 参考资料：https://blog.csdn.net/hou09tian/article/details/108862875
    /// 当成员函数返回的为*this时，就写Geometry3D&，表示返回值是调用该成员函数的变量的引用
    PointCloud &Clear() override;
    bool IsEmpty() const override;
    Eigen::Vector3d GetMinBound() const override;
    Eigen::Vector3d GetMaxBound() const override;
    Eigen::Vector3d GetCenter() const override;
    AxisAlignedBoundingBox GetAxisAlignedBoundingBox() const override;
    OrientedBoundingBox GetOrientedBoundingBox() const override;
    PointCloud &Transform(const Eigen::Matrix4d &transformation) override;
    PointCloud &Translate(const Eigen::Vector3d &translation,
                          bool relative = true) override;
    PointCloud &Scale(const double scale,
                      const Eigen::Vector3d &center) override;
    PointCloud &Rotate(const Eigen::Matrix3d &R,
                       const Eigen::Vector3d &center) override;

    PointCloud &operator+=(const PointCloud &cloud);
    PointCloud operator+(const PointCloud &cloud) const;

    /// 如果点云包含点，返回true。
    bool HasPoints() const { return points_.size() > 0; }

    /// 如果点云包含点法线，返回true。
    bool HasNormals() const {
        return points_.size() > 0 && normals_.size() == points_.size();
    }

    /// 如果点云包含点颜色，返回true。
    bool HasColors() const {
        return points_.size() > 0 && colors_.size() == points_.size();
    }

    /// 如果点云包含每个点的协方差矩阵，则返回true。
    bool HasCovariances() const {
        return !points_.empty() && covariances_.size() == points_.size();
    }

    /// 归一化点法线长度。
    PointCloud &NormalizeNormals() {
        for (size_t i = 0; i < normals_.size(); i++) {
            normals_[i].normalize();
        }
        return *this;
    }

    /// 为PointCloud中的每个点分配相同的颜色。  
    ///
    /// \param color RGB颜色的点。
    PointCloud &PaintUniformColor(const Eigen::Vector3d &color) {
        ResizeAndPaintUniformColor(colors_, points_.size(), color);
        return *this;
    }

    ///删除点云中所有具有nan entry或infinite entries的点。
    ///并且删除相应的法线和颜色条目。
    ///
    /// \param remove_nan 从PointCloud中删除NaN值。
    /// \param remove_infinite 从PointCloud中删除infinite值。
    PointCloud &RemoveNonFinitePoints(bool remove_nan = true,
                                      bool remove_infinite = true);

    /// 选择点从输入点云到输出点云的函数。
    /// 有被选中的指标的点。
    ///
    /// \param indices 将要选择的点的指标。
    /// \param invert 设置为True时，反转索引的选择。
    std::shared_ptr<PointCloud> SelectByIndex(
            const std::vector<size_t> &indices, bool invert = false) const;

    /// 向下采样输入点云到输出点云与体素的函数。
    /// normal和colors如果存在，则取平均值。
    ///
    /// \param voxel_size 定义体素网格的分辨率，值越小输出点云越密集。
    std::shared_ptr<PointCloud> VoxelDownSample(double voxel_size) const;

    /// 函数使用geometry.PointCloud.VoxelDownSample进行降低取样（缩小取样）。
    /// 并且记录下采样前的点云指数。
    ///
    /// \param voxel_size 下采样到的体素大小。
    /// \param min_bound 体素边界的最小坐标
    /// \param max_bound 体素边界的最大坐标
    /// \param approximate_class 是否近似。
    std::tuple<std::shared_ptr<PointCloud>,
               Eigen::MatrixXi,
               std::vector<std::vector<int>>>
    VoxelDownSampleAndTrace(double voxel_size,
                            const Eigen::Vector3d &min_bound,
                            const Eigen::Vector3d &max_bound,
                            bool approximate_class = false) const;

    /// 将输入点云统一下采样到输出点云的函数。
    /// 采样按照点的顺序执行，总是选择第0个点，而不是随机的。
    ///
    /// \param every_k_points 采样率，选择的点指标是[0,k,2k，…]。
    std::shared_ptr<PointCloud> UniformDownSample(size_t every_k_points) const;

    /// 将输入点云随机下采样到输出点云的函数。
    /// 采样是通过随机选择点云中点的索引来执行的。
    ///
    /// \param sampling_ratio 采样是通过随机选择点云中点的索引来执行的。
    std::shared_ptr<PointCloud> RandomDownSample(double sampling_ratio) const;
    
    /// ？？？剪裁点云到输出点云的函数。
    /// 所有坐标在边界框bbox之外的点都会被剪切。
    ///
    /// \param bbox AxisAlignedBoundingBox to crop points.
    std::shared_ptr<PointCloud> Crop(const AxisAlignedBoundingBox &bbox) const;


    /// ？？？剪裁点云到输出点云的函数。
    /// 所有坐标在边界框bbox之外的点都会被剪切。
    ///
    /// \param bbox OrientedBoundingBox to crop points.
    std::shared_ptr<PointCloud> Crop(const OrientedBoundingBox &bbox) const;

    /// 移除给定半径范围内小于nb_points的点的函数。
    ///
    /// \param nb_points 半径内的点数。
    /// \param search_radius 球面的半径。
    std::tuple<std::shared_ptr<PointCloud>, std::vector<size_t>>
    RemoveRadiusOutliers(size_t nb_points, double search_radius) const;

    /// 移除给定半径范围内小于nb_points的点的函数。
    ///
    /// \param nb_neighbors 半径内的点数。
    /// \param std_ratio 球体的半径。
    std::tuple<std::shared_ptr<PointCloud>, std::vector<size_t>>
    RemoveStatisticalOutliers(size_t nb_neighbors, double std_ratio) const;

    /// 计算点云的法线的函数。
    /// 如果存在法线，则法线是针对输入点云的。
    ///
    /// \param search_param KDTree的邻居搜索参数。
    /// \param fast_normal_computation 如果为真，则正态估计使用非迭代方法从协方差矩阵中提取特征向量。这更快，但在数值上不那么稳定。
    void EstimateNormals(
            const KDTreeSearchParam &search_param = KDTreeSearchParamKNN(),
            bool fast_normal_computation = true);

    /// 定向点云的法线的函数。
    ///
    /// \param orientation_reference 法线是相对于orientation_reference定向的。
    void OrientNormalsToAlignWithDirection(
            const Eigen::Vector3d &orientation_reference =
                    Eigen::Vector3d(0.0, 0.0, 1.0));

    /// 通过另一种方法定向点云的法线的函数。
    /// \param camera_location 法线指向camera_location。
    void OrientNormalsTowardsCameraLocation(
            const Eigen::Vector3d &camera_location = Eigen::Vector3d::Zero());

    /// 按照Hoppe等人在1992年“从无组织点进行曲面重构”中描述的，基于统一的切平面的统一定向估计法线的函数。
    ///
    /// \param k k最近邻，用于正常传播的图重构。
    void OrientNormalsConsistentTangentPlane(size_t k);

    /// 计算点云之间的点对点距离的函数。
    /// 对于源点云中的每个点，计算到目标点云的距离。
    ///
    /// \param target 目标点云。
    std::vector<double> ComputePointCloudDistance(const PointCloud &target);

    /// 计算点云中每一点的协方差矩阵的静态函数。该函数不会改变PointCloud的输入，只是输出协方差矩阵。
    ///
    ///
    /// \param input 用于协方差计算的PointCloud。
    /// \param search_param 用于邻域搜索的KDTree搜索参数。
    static std::vector<Eigen::Matrix3d> EstimatePerPointCovariances(
            const PointCloud &input,
            const KDTreeSearchParam &search_param = KDTreeSearchParamKNN());

    /// 计算点云中每个点的协方差矩阵的函数。
    ///
    /// \param search_param 用于邻域搜索的KDTree搜索参数。
    void EstimateCovariances(
            const KDTreeSearchParam &search_param = KDTreeSearchParamKNN());

    /// 计算点云的均值和协方差矩阵的函数。
    std::tuple<Eigen::Vector3d, Eigen::Matrix3d> ComputeMeanAndCovariance()
            const;

    /// 计算输入点云中点的马氏距离的函数。
    ///
    /// See: https://en.wikipedia.org/wiki/Mahalanobis_distance
    std::vector<double> ComputeMahalanobisDistance() const;

    /// 计算从一个点到输入点云中它最近的邻居的距离的函数。
    std::vector<double> ComputeNearestNeighborDistance() const;

    /// 利用qhull算法计算点云的凸包的函数。
    std::tuple<std::shared_ptr<TriangleMesh>, std::vector<size_t>>
    ComputeConvexHull() const;

    /// \brief This is an implementation of the Hidden Point Removal operator
    /// described in Katz et. al. 'Direct Visibility of Point Sets', 2007.
    ///
    /// Additional information about the choice of radius
    /// for noisy point clouds can be found in Mehra et. al. 'Visibility of
    /// Noisy Point Cloud Data', 2010.
    ///
    /// \param camera_location 从那个位置看不见的所有点都将被移除。
    /// \param radius 球面投影的半径。
    std::tuple<std::shared_ptr<TriangleMesh>, std::vector<size_t>>
    HiddenPointRemoval(const Eigen::Vector3d &camera_location,
                       const double radius) const;

    /// 点云集群使用DBSCAN算法。
    /// Ester et al., "A Density-Based Algorithm for Discovering Clusters
    /// in Large Spatial Databases with Noise", 1996
    ///
    /// 返回一个点标签列表，-1根据算法表示噪声。
    ///
    /// \param eps 密度参数，用于寻找相邻点。
    /// \param min_points 形成一个集群的最小点数。
    /// \param print_progress 如果为true，过程将在控制台中可视化。
    std::vector<int> ClusterDBSCAN(double eps,
                                   size_t min_points,
                                   bool print_progress = false) const;

    /// 使用RANSAC算法分割点云平面。
    ///
    /// \param distance_threshold 一个点到平面模型的最大距离，并且被认为是一个内链。
    /// \param ransac_n 在每次迭代中考虑内嵌的初始点数。
    /// \param num_iterations \迭代次数。
    /// \return \返回平面模型ax + by + cz + d = 0和平面内层的索引。
    std::tuple<Eigen::Vector4d, std::vector<size_t>> SegmentPlane(
            const double distance_threshold = 0.01,
            const int ransac_n = 3,
            const int num_iterations = 100) const;

    /// 工厂函数，用于从深度图像和相机模型创建点云。
    /// 
    /// 在(u, v)图像坐标处给定深度值d，对应的3d点为:z = d /depth_scale\n x = (u - cx) * z / fx\n y = (v - cy) * z     /// fy\n
    ///
    /// \param depth 输入的深度图像可以是浮动图像，也可以是uint16_t图像。
    /// \param intrinsic 相机的固有参数。
    /// \param extrinsic 相机的外部参数。
    /// \param depth_scale 深度由1 /depth_scale缩放。
    /// \param depth_trunc 在depth_trunc距离截断。
    /// \param stride 支持粗点云提取的采样因子。
    ///
    /// \return 如果转换失败，则为空点云。
    ///
    /// 如果project_valid_depth_only为true，返回点云，它没有nan点。如果值为false，返回点云，它为每个像素有一个点，而无效的深度将导致NaN点。
    static std::shared_ptr<PointCloud> CreateFromDepthImage(
            const Image &depth,
            const camera::PinholeCameraIntrinsic &intrinsic,
            const Eigen::Matrix4d &extrinsic = Eigen::Matrix4d::Identity(),
            double depth_scale = 1000.0,
            double depth_trunc = 1000.0,
            int stride = 1,
            bool project_valid_depth_only = true);

    /// 工厂函数，用于从RGB-D图像和相机模型创建点云。
    ///
    /// 在(u, v)图像坐标处给定深度值d，对应的3d点为:z=d/depth_scale\nx=(u - cx) * z / fx\n y = (v - cy)*z\fy\n。
    ///
    /// \param image 输入图像。
    /// \param intrinsic 相机的固有参数。
    /// \param extrinsic 相机的外部参数。
    /// \return 如果转换失败，则为空点云。
    ///
    /// 如果\param project_valid_depth_only为true，返回点云，它没有nan点。如果值为false，返回点云，该点云为每个像素有一     /// 个点，而无效的深度将导致NaN点。
    static std::shared_ptr<PointCloud> CreateFromRGBDImage(
            const RGBDImage &image,
            const camera::PinholeCameraIntrinsic &intrinsic,
            const Eigen::Matrix4d &extrinsic = Eigen::Matrix4d::Identity(),
            bool project_valid_depth_only = true);

    /// 从VoxelGrid创建PointCloud的函数。
    /// 它使用原始点云坐标(相对于体素网格的中心)将体素中心转换为3D点。
    ///
    /// \param voxel_grid 输入VoxelGrid。
    std::shared_ptr<PointCloud> CreateFromVoxelGrid(
            const VoxelGrid &voxel_grid);

public:
    /// 点坐标
    std::vector<Eigen::Vector3d> points_;
    /// 点法线
    std::vector<Eigen::Vector3d> normals_;
    /// RGB颜色的点
    std::vector<Eigen::Vector3d> colors_;
    /// 每个点的协方差矩阵
    std::vector<Eigen::Matrix3d> covariances_;
};

}  // namespace geometry
}  // namespace open3d
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
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326

PointCloudIO

注：注释中文部分是源码解读，包含分析和遇到的问题

#pragma once

#include <string>

#include "open3d/geometry/PointCloud.h"

namespace open3d {
namespace io {

///工厂函数，从一个文件创建一个点云。
///如果读取失败，返回一个空的pointcloud。
std::shared_ptr<geometry::PointCloud> CreatePointCloudFromFile(
        const std::string &filename,
        const std::string &format = "auto",
        bool print_progress = false);

/// \struct ReadPointCloudOption
/// ReadPointCloud的可选参数
struct ReadPointCloudOption {
    ReadPointCloudOption(
            //注意:当你更新默认值时，更新pybind/io/class_io.cpp中的文档字符串
            std::string format = "auto",
            bool remove_nan_points = false,
            bool remove_infinite_points = false,
            bool print_progress = false,
            std::function<bool(double)> update_progress = {})
        : format(format),
          remove_nan_points(remove_nan_points),
          remove_infinite_points(remove_infinite_points),
          print_progress(print_progress),
          update_progress(update_progress){};
    ReadPointCloudOption(std::function<bool(double)> up)
        : ReadPointCloudOption() {
        update_progress = up;
    };
    ///指定文件内容的格式(以及使用什么加载器)，默认的auto意味着关闭文件扩展名。
    std::string format;
    ///是否删除所有具有nan的点。
    bool remove_nan_points;
    ///是否删除所有带正负inf的点。
    bool remove_infinite_points;
    ///将加载进度打印到stdout。
    ///如果你想要有自己的进度指示器或者取消加载，也可以查看update_progress。
    bool print_progress;
    ///读取正在进行时调用的回调，参数是完成的百分比。
    ///返回true表示继续加载，false表示试图停止加载和清理。
    std::function<bool(double)> update_progress;
};

	///从文件中读取PointCloud的常规入口。
	///该函数基于扩展名filename调用read函数。
	///有关可以传递的其他选项，请参阅ReadPointCloudOption。
	///如果read函数成功则返回true，否则返回false。
bool ReadPointCloud(const std::string &filename,
                    geometry::PointCloud &pointcloud,
                    const ReadPointCloudOption &params = {});

/// \struct WritePointCloudOption
/// WritePointCloud的可选参数
struct WritePointCloudOption {
    enum class IsAscii : bool { Binary = false, Ascii = true };
    enum class Compressed : bool { Uncompressed = false, Compressed = true };
    WritePointCloudOption(
            //注意:当你更新默认值时，更新pybind/io/class_io.cpp中的文档字符串。
            IsAscii write_ascii = IsAscii::Binary,
            Compressed compressed = Compressed::Uncompressed,
            bool print_progress = false,
            std::function<bool(double)> update_progress = {})
        : write_ascii(write_ascii),
          compressed(compressed),
          print_progress(print_progress),
          update_progress(update_progress){};
    // 体现出WritePointCloud的兼容性
    WritePointCloudOption(bool write_ascii,
                          bool compressed = false,
                          bool print_progress = false,
                          std::function<bool(double)> update_progress = {})
        : write_ascii(IsAscii(write_ascii)),
          compressed(Compressed(compressed)),
          print_progress(print_progress),
          update_progress(update_progress){};
    WritePointCloudOption(std::function<bool(double)> up)
        : WritePointCloudOption() {
        update_progress = up;
    };
    ///是否以Ascii或二进制保存。
    ///一些存储程序/装置能够做到这两点，而另一些则忽略了这一点。
    IsAscii write_ascii;
    ///保存压缩文件还是未压缩文件。目前，只有PCD能够压缩，并且只有在使用IsAscii::Binary时可以，其他所有格式都忽略了这一点。
    Compressed compressed;
    /// 将加载进度打印到标准输出。如果想要有自己的进度指示器或者取消加载，可以查看update_progress。
    bool print_progress;
    /// 回调调用作为读取正在进行，参数是完成的百分比。
    /// 返回true表示继续加载，false表示尝试停止加载和清理。
    std::function<bool(double)> update_progress;
};

	///将PointCloud写入文件的常规入口。
	///该函数调用基于扩展名filename的write函数。
	///有关可以传递的其他选项，请参阅WritePointCloudOption。
	///如果写函数成功则返回true，否则返回false。
bool WritePointCloud(const std::string &filename,
                     const geometry::PointCloud &pointcloud,
                     const WritePointCloudOption &params = {});

bool ReadPointCloudFromXYZ(const std::string &filename,
                           geometry::PointCloud &pointcloud,
                           const ReadPointCloudOption &params);

bool WritePointCloudToXYZ(const std::string &filename,
                          const geometry::PointCloud &pointcloud,
                          const WritePointCloudOption &params);

bool ReadPointCloudFromXYZN(const std::string &filename,
                            geometry::PointCloud &pointcloud,
                            const ReadPointCloudOption &params);

bool WritePointCloudToXYZN(const std::string &filename,
                           const geometry::PointCloud &pointcloud,
                           const WritePointCloudOption &params);

bool ReadPointCloudFromXYZRGB(const std::string &filename,
                              geometry::PointCloud &pointcloud,
                              const ReadPointCloudOption &params);

bool WritePointCloudToXYZRGB(const std::string &filename,
                             const geometry::PointCloud &pointcloud,
                             const WritePointCloudOption &params);

bool ReadPointCloudFromPLY(const std::string &filename,
                           geometry::PointCloud &pointcloud,
                           const ReadPointCloudOption &params);

bool WritePointCloudToPLY(const std::string &filename,
                          const geometry::PointCloud &pointcloud,
                          const WritePointCloudOption &params);

bool ReadPointCloudFromPCD(const std::string &filename,
                           geometry::PointCloud &pointcloud,
                           const ReadPointCloudOption &params);

bool WritePointCloudToPCD(const std::string &filename,
                          const geometry::PointCloud &pointcloud,
                          const WritePointCloudOption &params);

bool ReadPointCloudFromPTS(const std::string &filename,
                           geometry::PointCloud &pointcloud,
                           const ReadPointCloudOption &params);

bool WritePointCloudToPTS(const std::string &filename,
                          const geometry::PointCloud &pointcloud,
                          const WritePointCloudOption &params);

}  // namespace io
}  // namespace open3d
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

小结

本次源码分析，我对点云的基类进行了分析，接下来将对体素的相关类进行分析，以及对点云展开应用。