open3D源码解读第二篇

最新推荐文章于 2022-12-23 21:08:33 发布

purposewyt

最新推荐文章于 2022-12-23 21:08:33 发布

阅读量954

点赞数

文章标签：几何学 c++

本文链接：https://blog.csdn.net/weixin_47309740/article/details/121904402

版权

软件工程应用与实践专栏收录该内容

14 篇文章 5 订阅

订阅专栏

2021SC@SDUSC

文章目录

open3D Geometry2

open3D Geometry2

本片分析了点云处理的3D几何图形的基础几何体类。包括Geometry3D.h和Geometry3D.cpp。

Geometry3D.h

注意点：
1. 由于需要解析的代码较多，为使代码解读更加清晰，我将代码分析的详细过程写在代码段的注释中。
2. 中文部分是源码解读，包含代码分析和遇到的问题。
一些关键的类或函数：
1. Clear函数必须要有，因为基类Geometry中定义了纯虚函数。
2. Geometry3D中定义了许多虚函数，这些虚函数是给3D几何类型定义的，要求不同的几何类型在其内部实现。const的含义是说返回值是一个const不可修改，=0的含义是说基类的虚函数未做实现的时候要加=0，并不是const=0，这两个要分开理解。
3. ResizeAndPaintUniformColor函数，调整颜色向量的大小并绘制统一的颜色。
4. TransformPoints函数，用变换矩阵变换所有点。
5. TransformNormals函数，用变换矩阵变换法线。
6. TranslatePoints函数，对几何坐标应用平移。
7. ScalePoints函数，用比例因子比例缩放所有点的坐标。
8. RotatePoints函数，通过旋转矩阵R旋转所有的点。
9. RotateNormals函数，通过旋转矩阵R旋转所有法线。

#pragma once

#include <Eigen/Core>
#include <Eigen/Geometry>

#include "open3d/geometry/Geometry.h"
#include "open3d/utility/Eigen.h"

namespace open3d {
namespace geometry {

class AxisAlignedBoundingBox;
class OrientedBoundingBox;

/// \class Geometry3D
/// 用于3D几何图形的基本几何类。
/// 主要的三维几何图形类，从几何基类派生所有数据。
/// 这里定义了一个叫Geometry3D的类，继承了基类Geometry，这个类也是个基类。
class Geometry3D : public Geometry {
public:
    /// 参考资料：https://blog.csdn.net/qq_43808700/article/details/103539401
    /// 这里析构函数的override写不写都行，写不写都行的时候就写上
    /// 基类析构函数为虚函数时，派生类的析构函数写不写override都行，因为写不写编译器都会帮你override一下
    /// ？？存疑？？override是否为必须？？
    ~Geometry3D() override {}

protected:
    /// \brief Parameterized Constructor.
    /// 构造函数，继承基类Geometry，这里要求实例化Geometry3D时要给定GeometryType，变量叫type，同时继承自Geometry是要指定dim为3，因为我们定义这个基类是3D的类。
    Geometry3D(GeometryType type) : Geometry(type, 3) {}

public:
    /// Clear函数必须要有，因为基类Geometry中定义了纯虚函数。
    /// 参考资料：https://blog.csdn.net/hou09tian/article/details/108862875
    /// 当成员函数返回的为*this时，就写Geometry3D&，表示返回值是调用该成员函数的变量的引用。
    Geometry3D& Clear() override = 0;
    bool IsEmpty() const override = 0;
    /// 参考资料：https://blog.csdn.net/weixin_44001521/article/details/104394353
    /// 下面的虚函数是给3D几何类型定义的，要求不同的几何类型在其内部实现。const的含义是说返回值是一个const不可修改，=0的含义是说基类的虚函数未做实现的时候要加=0，并不是const=0，这两个要分开理解。
    /// 返回几何坐标的最小边界。
    virtual Eigen::Vector3d GetMinBound() const = 0;
    /// 返回几何坐标的最大边界。
    virtual Eigen::Vector3d GetMaxBound() const = 0;
    /// 返回几何坐标的中心。
    virtual Eigen::Vector3d GetCenter() const = 0;
    /// 返回几何图形的轴对齐边框。
    virtual AxisAlignedBoundingBox GetAxisAlignedBoundingBox() const = 0;
    /// 返回几何图形的一个定向边框。
    virtual OrientedBoundingBox GetOrientedBoundingBox() const = 0;
    /// 传入矩阵的引用，避免浪费内存，加const防止被错误修改
    /// Geometry3D&表示return的是*this
    virtual Geometry3D& Transform(const Eigen::Matrix4d& transformation) = 0;

    /// 对几何坐标应用平移。
    /// \param translation 用于变换几何图形的三维向量。
    /// \param relative 如果为true，则平移直接应用于几何图形。否则，几何中心被平移。
    virtual Geometry3D& Translate(const Eigen::Vector3d& translation,
                                  bool relative = true) = 0;
    /// 对几何坐标应用缩放。
    /// 给定一个比例因子s，中心c，一个给定的点
    /// \f$p\f$ is transformed according to \f$s (p - c) + c\f$.
    ///
    /// \param scale 与几何图形的点/顶点相乘的比例参数。
    /// \param center 缩放中心，用于调整几何图形的大小。
    virtual Geometry3D& Scale(const double scale,
                              const Eigen::Vector3d& center) = 0;

    /// \brief Apply rotation to the geometry coordinates and normals.
    /// Given a rotation matrix \f$R\f$, and center \f$c\f$, a given point
    /// \f$p\f$ is transformed according to \f$R (p - c) + c\f$.
    ///
    /// \param R A 3x3 rotation matrix
    /// \param center Rotation center that is used for the rotation.
    /// 这个有两个Rotate函数，输入不同，即函数重载，第一个=0即纯虚函数，在源文件中未做实现。
    /// 第二个做了实现(其实是在第二个Rotate中调用第一个Rotate，具体见cpp源文件)
    virtual Geometry3D& Rotate(const Eigen::Matrix3d& R,
                               const Eigen::Vector3d& center) = 0;

    /// 以下是获得以不同的旋转方式得到的旋转矩阵。
    virtual Geometry3D& Rotate(const Eigen::Matrix3d& R);

    /// Get Rotation Matrix from XYZ RotationType.
    static Eigen::Matrix3d GetRotationMatrixFromXYZ(
            const Eigen::Vector3d& rotation);
    /// Get Rotation Matrix from YZX RotationType.
    static Eigen::Matrix3d GetRotationMatrixFromYZX(
            const Eigen::Vector3d& rotation);
    /// Get Rotation Matrix from ZXY RotationType.
    static Eigen::Matrix3d GetRotationMatrixFromZXY(
            const Eigen::Vector3d& rotation);
    /// Get Rotation Matrix from XZY RotationType.
    static Eigen::Matrix3d GetRotationMatrixFromXZY(
            const Eigen::Vector3d& rotation);
    /// Get Rotation Matrix from ZYX RotationType.
    static Eigen::Matrix3d GetRotationMatrixFromZYX(
            const Eigen::Vector3d& rotation);
    /// Get Rotation Matrix from YXZ RotationType.
    static Eigen::Matrix3d GetRotationMatrixFromYXZ(
            const Eigen::Vector3d& rotation);
    /// Get Rotation Matrix from AxisAngle RotationType.
    static Eigen::Matrix3d GetRotationMatrixFromAxisAngle(
            const Eigen::Vector3d& rotation);
    /// Get Rotation Matrix from Quaternion.
    static Eigen::Matrix3d GetRotationMatrixFromQuaternion(
            const Eigen::Vector4d& rotation);
    
protected:
    /// 计算列表点的最小界。
    Eigen::Vector3d ComputeMinBound(
            const std::vector<Eigen::Vector3d>& points) const;
    /// 计算列表点的最大边界。
    Eigen::Vector3d ComputeMaxBound(
            const std::vector<Eigen::Vector3d>& points) const;
    /// 计算机中心的一列点。
    Eigen::Vector3d ComputeCenter(
            const std::vector<Eigen::Vector3d>& points) const;

    /// 调整颜色向量的大小并绘制统一的颜色。
    ///
    /// \param colors 一个特征向量数组在RGB中指定颜色。
    /// \param size 颜色数组的结果大小。
    /// \param color 最后被填涂的颜色。
    void ResizeAndPaintUniformColor(std::vector<Eigen::Vector3d>& colors,
                                    const size_t size,
                                    const Eigen::Vector3d& color) const;

    /// 用变换矩阵变换所有点。
    ///
    /// \param transformation 4x4矩阵的变换。
    /// \param points 要转换的点的列表
    void TransformPoints(const Eigen::Matrix4d& transformation,
                         std::vector<Eigen::Vector3d>& points) const;

    /// 用变换矩阵变换法线。
    ///
    /// \param transformation 4x4 matrix for transformation.
    /// \param normals A list of normals to be transformed.
    void TransformNormals(const Eigen::Matrix4d& transformation,
                          std::vector<Eigen::Vector3d>& normals) const;
    /// 对几何坐标应用平移。
    ///
    /// \param translation 用于变换几何形状的三维向量。
    /// \param points 要转换的点的列表
    /// \param relative If `true`, the \p translation is directly applied to the
    /// \p points. Otherwise, the center of the \p points is moved to the \p
    /// translation.
    void TranslatePoints(const Eigen::Vector3d& translation,
                         std::vector<Eigen::Vector3d>& points,
                         bool relative) const;

    /// 用比例因子比例缩放所有点的坐标。
    ///
    /// \param scale 用于调整几何图形大小的比例因子
    /// \param points 要转换的点的列表
    /// \param center 缩放中心，用于调整几何图形的大小。
    void ScalePoints(const double scale,
                     std::vector<Eigen::Vector3d>& points,
                     const Eigen::Vector3d& center) const;

    /// 通过旋转矩阵R旋转所有的点。
    ///
    /// \param R 一个3x3的旋转矩阵，定义了旋转轴和围绕这个轴的角度的范数。
    /// \param points 要转换的点的列表。
    /// \param center 用于旋转的旋转中心。
    void RotatePoints(const Eigen::Matrix3d& R,
                      std::vector<Eigen::Vector3d>& points,
                      const Eigen::Vector3d& center) const;

    /// 通过旋转矩阵R旋转所有法线。
    ///
    /// \param R A 3x3 rotation matrix
    /// \param normals A list of normals to be transformed.
    /// \A list of normals to be transformed.
    void RotateNormals(const Eigen::Matrix3d& R,
                       std::vector<Eigen::Vector3d>& normals) const;
};

}  // namespace geometry
}  // namespace open3d

Geometry3D.cpp

#include "open3d/geometry/Geometry3D.h"

#include <Eigen/Dense>
#include <numeric>

#include "open3d/utility/Logging.h"

namespace open3d {
namespace geometry {
/// 这里套娃了，用一个Rotate函数调用了另一个重载的Rotate函数；GetCenter是在Geometry3D的头文件中声明的。
Geometry3D& Geometry3D::Rotate(const Eigen::Matrix3d& R) {
    return Rotate(R, GetCenter());
}
// 得到最小的边界，即minx miny minz
Eigen::Vector3d Geometry3D::ComputeMinBound(
        const std::vector<Eigen::Vector3d>& points) const {
    // 如果points为空，就输出0，0，0
    if (points.empty()) {
        return Eigen::Vector3d(0.0, 0.0, 0.0);
    }
    // 参考资料：https://blog.csdn.net/qq_40803710/article/details/80273811
    // std::accumulate 用来做自定义数据类型的求和
    // 注意看上面网页里accumulate的模板实现，四个输入是：第一个iterator First，最后一个iterator Last，当前值val，自定义函数func
    // 这个函数会在迭代器中对当前值和迭代值取小值。学一下这里的std::accumulate的用法，很实用。
    return std::accumulate(
            points.begin(), points.end(), points[0],
            [](const Eigen::Vector3d& a, const Eigen::Vector3d& b) {
                return a.array().min(b.array()).matrix();
            });
}
// 实现方法同上
Eigen::Vector3d Geometry3D::ComputeMaxBound(
        const std::vector<Eigen::Vector3d>& points) const {
    if (points.empty()) {
        return Eigen::Vector3d(0.0, 0.0, 0.0);
    }
    return std::accumulate(
            points.begin(), points.end(), points[0],
            [](const Eigen::Vector3d& a, const Eigen::Vector3d& b) {
                return a.array().max(b.array()).matrix();
            });
}
Eigen::Vector3d Geometry3D::ComputeCenter(
        const std::vector<Eigen::Vector3d>& points) const {
    // 先定义center为0，0，0
    Eigen::Vector3d center(0, 0, 0);
    // 如果点集合为空就返回0，0，0
    if (points.empty()) {
        return center;
    }
    // 这里的std::accumulate是求和，在points迭代器中求和然后给到center中
    center = std::accumulate(points.begin(), points.end(), center);
    // 然后除以点的数量得到点集的center
    center /= double(points.size());
    return center;
}

void Geometry3D::ResizeAndPaintUniformColor(
        std::vector<Eigen::Vector3d>& colors,
        const size_t size,
        const Eigen::Vector3d& color) const {
    // 先对传入的引用colors
    colors.resize(size);
    Eigen::Vector3d clipped_color = color;
    if (color.minCoeff() < 0 || color.maxCoeff() > 1) {
        utility::LogWarning(
                "invalid color in PaintUniformColor, clipping to [0, 1]");
        clipped_color = clipped_color.array()
                                .max(Eigen::Vector3d(0, 0, 0).array())
                                .matrix();
        clipped_color = clipped_color.array()
                                .min(Eigen::Vector3d(1, 1, 1).array())
                                .matrix();
    }
    for (size_t i = 0; i < size; i++) {
        colors[i] = clipped_color;
    }
}

void Geometry3D::TransformPoints(const Eigen::Matrix4d& transformation,
                                 std::vector<Eigen::Vector3d>& points) const {
    // points是一个vector，auto& point是Vector3d类型点的引用，在for循环中处理每个点的旋转操作 
    // auto关键字处理引用类型时为 auto&                 
    for (auto& point : points) {
        Eigen::Vector4d new_point =
                transformation *
                Eigen::Vector4d(point(0), point(1), point(2), 1.0);
        // 用法参考：https://blog.csdn.net/u012541187/article/details/53420432
        // x.head<3>()的用法说明：即x(1:n)用于数组提取前n个[vector]
        // 关于为什么要除以new_point(3)：为了做矩阵乘法先将new_point齐次化，new_point(3)是scale这里为1.        
        point = new_point.head<3>() / new_point(3); // 取前三个值
    }
}

void Geometry3D::TransformNormals(const Eigen::Matrix4d& transformation,
                                  std::vector<Eigen::Vector3d>& normals) const {
    for (auto& normal : normals) {
        Eigen::Vector4d new_normal =
                transformation *
                Eigen::Vector4d(normal(0), normal(1), normal(2), 0.0);
        normal = new_normal.head<3>();
    }
}

void Geometry3D::TranslatePoints(const Eigen::Vector3d& translation,
                                 std::vector<Eigen::Vector3d>& points,
                                 bool relative) const {
    Eigen::Vector3d transform = translation;
    // !relative：物体的中心点移动到transform的位置；否则即整体移动transform
    if (!relative) {
        transform -= ComputeCenter(points);
    }
    // 
    for (auto& point : points) {
        point += transform;
    }
}

void Geometry3D::ScalePoints(const double scale,
                             std::vector<Eigen::Vector3d>& points,
                             const Eigen::Vector3d& center) const {
    for (auto& point : points) {
        // 先去中心化，scale后再平移到原中心，即以center为中心进行scale
        point = (point - center) * scale + center;
    }
}

void Geometry3D::RotatePoints(const Eigen::Matrix3d& R,
                              std::vector<Eigen::Vector3d>& points,
                              const Eigen::Vector3d& center) const {
    for (auto& point : points) {
    	// 先去中心化，旋转后再平移回去，即让物体绕着自身坐标系旋转，而不是绕着世界坐标系旋转
        point = R * (point - center) + center;
    }
}

void Geometry3D::RotateNormals(const Eigen::Matrix3d& R,
                               std::vector<Eigen::Vector3d>& normals) const {
    for (auto& normal : normals) {
        normal = R * normal;
    }
}

Eigen::Matrix3d Geometry3D::GetRotationMatrixFromXYZ(
        const Eigen::Vector3d& rotation) {
    return open3d::utility::RotationMatrixX(rotation(0)) *
           open3d::utility::RotationMatrixY(rotation(1)) *
           open3d::utility::RotationMatrixZ(rotation(2));
}

Eigen::Matrix3d Geometry3D::GetRotationMatrixFromYZX(
        const Eigen::Vector3d& rotation) {
    return open3d::utility::RotationMatrixY(rotation(0)) *
           open3d::utility::RotationMatrixZ(rotation(1)) *
           open3d::utility::RotationMatrixX(rotation(2));
}

Eigen::Matrix3d Geometry3D::GetRotationMatrixFromZXY(
        const Eigen::Vector3d& rotation) {
    return open3d::utility::RotationMatrixZ(rotation(0)) *
           open3d::utility::RotationMatrixX(rotation(1)) *
           open3d::utility::RotationMatrixY(rotation(2));
}

Eigen::Matrix3d Geometry3D::GetRotationMatrixFromXZY(
        const Eigen::Vector3d& rotation) {
    return open3d::utility::RotationMatrixX(rotation(0)) *
           open3d::utility::RotationMatrixZ(rotation(1)) *
           open3d::utility::RotationMatrixY(rotation(2));
}

Eigen::Matrix3d Geometry3D::GetRotationMatrixFromZYX(
        const Eigen::Vector3d& rotation) {
    return open3d::utility::RotationMatrixZ(rotation(0)) *
           open3d::utility::RotationMatrixY(rotation(1)) *
           open3d::utility::RotationMatrixX(rotation(2));
}

Eigen::Matrix3d Geometry3D::GetRotationMatrixFromYXZ(
        const Eigen::Vector3d& rotation) {
    return open3d::utility::RotationMatrixY(rotation(0)) *
           open3d::utility::RotationMatrixX(rotation(1)) *
           open3d::utility::RotationMatrixZ(rotation(2));
}

Eigen::Matrix3d Geometry3D::GetRotationMatrixFromAxisAngle(
        const Eigen::Vector3d& rotation) {
    const double phi = rotation.norm();
    return Eigen::AngleAxisd(phi, rotation / phi).toRotationMatrix();
}

Eigen::Matrix3d Geometry3D::GetRotationMatrixFromQuaternion(
        const Eigen::Vector4d& rotation) {
    return Eigen::Quaterniond(rotation(0), rotation(1), rotation(2),
                              rotation(3))
            .normalized()
            .toRotationMatrix();
}

}  // namespace geometry
}  // namespace open3d