open3D源码解读第三篇

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

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

小结

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