2021SC@SDUSC
open3D Octree
Octree(八叉树),是一种用于描述三维空间的树状数据结构。八叉树的每个节点表示一个正方体的体积元素,每个节点有八个子节点,将八个子节点所表示的体积元素加在一起就等于父节点的体积。八叉树是四叉树在三维空间上的扩展,二维上我们有四个象限,而三维上,我们有8个卦限。八叉树主要用于空间划分和最近邻搜索。
本篇分析了Octree和OctreeIO两个与Octree相关的基本类。
Octree
注意点:
- 由于需要解析的代码较多,为使代码解读更加清晰,我将代码分析的详细过程写在代码段的注释中。
- 中文部分是源码解读,包含代码分析和遇到的问题。
#pragma once
#include <cstddef>
#include <memory>
#include <vector>
#include "open3d/geometry/Geometry3D.h"
#include "open3d/utility/IJsonConvertible.h"
namespace open3d {
namespace geometry {
class PointCloud;
class VoxelGrid;
/// \class OctreeNodeInfo
/// OctreeNode的信息。
/// OctreeNodeInfo是动态计算的,而不是存储在Node中的。
class OctreeNodeInfo {
public:
///默认构造函数。
///
///初始化所有值为0。
OctreeNodeInfo() : origin_(0, 0, 0), size_(0), depth_(0), child_index_(0) {}
///参数化构造函数。
///
///参数origin 节点的起始坐标
///参数size 节点大小。
///参数depth 节点到根的深度。根的深度是0。
///参数child_index 节点自身的子索引。
OctreeNodeInfo(const Eigen::Vector3d& origin,
const double& size,
const size_t& depth,
const size_t& child_index)
: origin_(origin),
size_(size),
depth_(depth),
child_index_(child_index) {}
~OctreeNodeInfo() {}
public:
///节点的起始坐标。
Eigen::Vector3d origin_;
///节点大小。
double size_;
///节点到根的深度。根的深度从0开始。
size_t depth_;
///节点的子索引。对于非根节点,child_index为0~7;
///根节点的child_index为-1。
size_t child_index_;
};
/// \class OctreeNode
/// 八叉树节点的基类。
///
/// 设计决策:不存储节点的起源和大小。
/// -优点:更好的空间效率。
/// -缺点:重新计算原点和大小时需要遍历。
class OctreeNode : public utility::IJsonConvertible {
public:
///默认构造函数。
OctreeNode() {}
virtual ~OctreeNode() {}
///通过解析json值来构造一个OctreeNode的工厂函数。
static std::shared_ptr<OctreeNode> ConstructFromJsonValue(
const Json::Value& value);
};
/// \class OctreeInternalNode
/// OctreeInternalNode类,包含OctreeNode子节点。
///
/// 子节点排序约定如下所示:
/// 对于每个实例,假设:
/// - root_node: origin == (0,0,0), size == 2
///
/// 然后:
/// - children_[0]: origin == (1,0,0), size == 1
/// - children_[1]: origin == (1,0,0), size == 1,沿着x轴紧挨着child 0
/// - children_[2]: origin == (0,1,0), size == 1,沿着y轴紧挨着child 0
/// - children_[3]: origin == (1,1,0), size == 1,在X-Y平面
/// - children_[4]: origin == (0,0,1), size == 1,沿着z轴紧挨着child 0
/// - children_[5]: origin == (1,0,1), size == 1,在X-Z平面
/// - children_[6]: origin == (0,1,1), size == 1,在Y-Z平面
/// - children_[7]: origin == (1,1,1), size == 1,最远从child 0
class OctreeInternalNode : public OctreeNode {
public:
///默认构造函数。
OctreeInternalNode() : children_(8) {}
static std::shared_ptr<OctreeNodeInfo> GetInsertionNodeInfo(
const std::shared_ptr<OctreeNodeInfo>& node_info,
const Eigen::Vector3d& point);
///获取初始化OctreeInternalNode的lambda函数。
///调用init函数时,创建一个空的OctreeInternalNode。
static std::function<std::shared_ptr<OctreeInternalNode>()>
GetInitFunction();
///获取lambda函数来更新OctreeInternalNode。
///这个update函数什么也不做。
static std::function<void(std::shared_ptr<OctreeInternalNode>)>
GetUpdateFunction();
bool ConvertToJsonValue(Json::Value& value) const override;
bool ConvertFromJsonValue(const Json::Value& value) override;
public:
/// Pybind11使用vector代替C-array,否则,需要定义更多的辅助函数
/// 详见:https://github.com/pybind/pybind11/issues/546#issuecomment-265707318
/// 子节点列表。
std::vector<std::shared_ptr<OctreeNode>> children_;
};
/// \class OctreeInternalPointNode
/// OctreeInternalPointNode类是一个包含索引列表的OctreeInternalNode类,索引列表是它所有子节点的索引列表的并集。
class OctreeInternalPointNode : public OctreeInternalNode {
public:
///默认构造函数。
OctreeInternalPointNode() : OctreeInternalNode() {}
///获取用于初始化OctreeInternalPointNode的lambda函数。
///调用init函数时,创建一个空的OctreeInternalPointNode。
static std::function<std::shared_ptr<OctreeInternalNode>()>
GetInitFunction();
/// 获取用于更新OctreeInternalPointNode的lambda函数。
///调用时,update函数将输入点索引添加到相应节点的子点索引列表中。
static std::function<void(std::shared_ptr<OctreeInternalNode>)>
GetUpdateFunction(size_t idx);
bool ConvertToJsonValue(Json::Value& value) const override;
bool ConvertFromJsonValue(const Json::Value& value) override;
public:
///与该节点的任何子节点关联的点的索引
std::vector<size_t> indices_;
};
/// \class OctreeLeafNode
/// OctreeLeafNode的基类。
class OctreeLeafNode : public OctreeNode {
public:
virtual bool operator==(const OctreeLeafNode& other) const = 0;
///克隆这个OctreeLeafNode。
virtual std::shared_ptr<OctreeLeafNode> Clone() const = 0;
};
/// \class OctreeColorLeafNode
/// OctreeColorLeafNode类是一个包含颜色的OctreeLeafNode。
class OctreeColorLeafNode : public OctreeLeafNode {
public:
bool operator==(const OctreeLeafNode& other) const override;
///克隆这个OctreeLeafNode。
std::shared_ptr<OctreeLeafNode> Clone() const override;
/// 获取用于初始化OctreeLeafNode的lambda函数。
/// 调用init函数时,创建一个空的OctreeColorLeafNode。
static std::function<std::shared_ptr<OctreeLeafNode>()> GetInitFunction();
///获取用于更新OctreeLeafNode的lambda函数。
///当被调用时,update函数用输入颜色更新相应的节点。
///
///参数color 节点颜色。
static std::function<void(std::shared_ptr<OctreeLeafNode>)>
GetUpdateFunction(const Eigen::Vector3d& color);
bool ConvertToJsonValue(Json::Value& value) const override;
bool ConvertFromJsonValue(const Json::Value& value) override;
/// TODO:用lambda函数处理节点的数据柔性(flexible data)。???
///
/// 节点的颜色。
Eigen::Vector3d color_ = Eigen::Vector3d(0, 0, 0);
};
/// \class OctreePointColorLeafNode
/// OctreePointColorLeafNode类是一个OctreeColorLeafNode类,它包含一个与此叶节点中包含的点云点对应的索引列表。
class OctreePointColorLeafNode : public OctreeColorLeafNode {
public:
bool operator==(const OctreeLeafNode& other) const override;
///克隆这个OctreeLeafNode。
std::shared_ptr<OctreeLeafNode> Clone() const override;
///获取lambda函数以初始化OctreeLeafNode。
///调用init函数时,创建一个空的OctreePointColorLeafNode。
static std::function<std::shared_ptr<OctreeLeafNode>()> GetInitFunction();
///获取lambda函数来更新OctreeLeafNode。
///调用update函数时,将点云的点索引添加到相应的节点索引列表中。
///参数index 点云与节点关联的点索引。
///参数color 节点的颜色。
static std::function<void(std::shared_ptr<OctreeLeafNode>)>
GetUpdateFunction(size_t index, const Eigen::Vector3d& color);
bool ConvertToJsonValue(Json::Value& value) const override;
bool ConvertFromJsonValue(const Json::Value& value) override;
///与该节点关联的点索引。
std::vector<size_t> indices_;
};
/// \class Octree
/// 八叉树数据结构。
class Octree : public Geometry3D, public utility::IJsonConvertible {
public:
///默认构造函数。
Octree()
: Geometry3D(Geometry::GeometryType::Octree),
origin_(0, 0, 0),
size_(0),
max_depth_(0) {}
///参数构造函数。
///
///参数max_depth 设置八叉树的最大深度的值。
Octree(const size_t& max_depth)
: Geometry3D(Geometry::GeometryType::Octree),
origin_(0, 0, 0),
size_(0),
max_depth_(max_depth) {}
///参数构造函数。
///
///参数max_depth 设置八叉树的最大深度值。
///参数origin 设置八叉树的全局最小边界。
///参数size 设置整个八叉树的边框大小。
Octree(const size_t& max_depth,
const Eigen::Vector3d& origin,
const double& size)
: Geometry3D(Geometry::GeometryType::Octree),
origin_(origin),
size_(size),
max_depth_(max_depth) {}
Octree(const Octree& src_octree);
~Octree() override {}
public:
Octree& 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;
Octree& Transform(const Eigen::Matrix4d& transformation) override;
Octree& Translate(const Eigen::Vector3d& translation,
bool relative = true) override;
Octree& Scale(const double scale, const Eigen::Vector3d& center) override;
Octree& Rotate(const Eigen::Matrix3d& R,
const Eigen::Vector3d& center) override;
bool ConvertToJsonValue(Json::Value& value) const override;
bool ConvertFromJsonValue(const Json::Value& value) override;
public:
///从点云转换八叉树。
///
///参数point_cloud 输入点云。
///参数size_expand 一个小的扩展大小,使得八叉树略大于原始的点云边界,以容纳所有点。
void ConvertFromPointCloud(const geometry::PointCloud& point_cloud,
double size_expand = 0.01);
///八叉树的根。
std::shared_ptr<OctreeNode> root_node_ = nullptr;
///全局最小边界。一个点在origin_ <= point < origin_ + size_的范围内。
Eigen::Vector3d origin_;
///整个八叉树的边框大小。一个点在origin_ <= point < origin_ + size_的范围内。
double size_;
///八叉树的最大深度。深度定义为从最深的叶节点到根的距离。只有根节点的树深度为0。
size_t max_depth_;
///插入一个点到八叉树。
///
///参数point 点的坐标。
///参数fl_init初始化fcn,用于创建与点关联的新叶节点(如果需要)。
///参数fl_update更新用于更新与该点关联的叶节点的fcn。
///参数fi_init初始化fcn用于创建一个新的内部节点(如果需要),它是点的叶节点的祖先。如果省略,则使用默认的OctreeInternalNode函数。
///参数fi_update用于更新内部节点的fcn,它是点的叶节点的祖先。如果省略,则使用默认的OctreeInternalNode函数。
void InsertPoint(
const Eigen::Vector3d& point,
const std::function<std::shared_ptr<OctreeLeafNode>()>& fl_init,
const std::function<void(std::shared_ptr<OctreeLeafNode>)>&
fl_update,
const std::function<std::shared_ptr<OctreeInternalNode>()>&
fi_init = nullptr,
const std::function<void(std::shared_ptr<OctreeInternalNode>)>&
fi_update = nullptr);
///从根目录遍历Octree,并为每个节点调用回调函数。
///
///参数f 每次遍历内部/叶节点时触发的回调。
///参数提供关于正在遍历的节点和其他特定于节点的数据的信息。布尔返回值用于提前停止。
///如果f返回true,则不会遍历该节点的子节点。
void Traverse(
const std::function<bool(const std::shared_ptr<OctreeNode>&,
const std::shared_ptr<OctreeNodeInfo>&)>&
f);
///Traverse的Const版本。从根目录遍历Octree,并为每个节点调用回调函数。
///
///参数f 每次遍历内部/叶节点时触发的回调。
///参数提供关于正在遍历的节点和其他特定于节点的数据的信息。布尔返回值用于提前停止。
///如果f返回true,则不会遍历该节点的子节点。
void Traverse(
const std::function<bool(const std::shared_ptr<OctreeNode>&,
const std::shared_ptr<OctreeNodeInfo>&)>&
f) const;
std::pair<std::shared_ptr<OctreeLeafNode>, std::shared_ptr<OctreeNodeInfo>>
///返回查询点所在的leaf OctreeNode和OctreeNodeInfo。
///
///参数point 点的坐标。
LocateLeafNode(const Eigen::Vector3d& point) const;
///如果点在origin <= point < origin + size范围内,则返回true。
///
///参数point 点的坐标。
///参数origin 起始坐标。
///参数size 八叉树的大小。
static bool IsPointInBound(const Eigen::Vector3d& point,
const Eigen::Vector3d& origin,
const double& size);
///如果八叉树完全相同,返回true,用于测试。
bool operator==(const Octree& other) const;
///转换为VoxelGrid。
std::shared_ptr<geometry::VoxelGrid> ToVoxelGrid() const;
///从体素网格转换。
void CreateFromVoxelGrid(const geometry::VoxelGrid& voxel_grid);
private:
static void TraverseRecurse(
const std::shared_ptr<OctreeNode>& node,
const std::shared_ptr<OctreeNodeInfo>& node_info,
const std::function<bool(const std::shared_ptr<OctreeNode>&,
const std::shared_ptr<OctreeNodeInfo>&)>&
f);
void InsertPointRecurse(
const std::shared_ptr<OctreeNode>& node,
const std::shared_ptr<OctreeNodeInfo>& node_info,
const Eigen::Vector3d& point,
const std::function<std::shared_ptr<OctreeLeafNode>()>& f_l_init,
const std::function<void(std::shared_ptr<OctreeLeafNode>)>&
f_l_update,
const std::function<std::shared_ptr<OctreeInternalNode>()>&
f_i_init,
const std::function<void(std::shared_ptr<OctreeInternalNode>)>&
f_i_update);
};
} // namespace geometry
} // namespace open3d
OctreeIO
注:注释中文部分是源码解读。
#pragma once
#include <string>
#include "open3d/geometry/Octree.h"
namespace open3d {
namespace io {
///从文件中创建八叉树的工厂函数。
///如果读取失败,返回一个空八叉树。
std::shared_ptr<geometry::Octree> CreateOctreeFromFile(
const std::string &filename, const std::string &format = "auto");
///从文件中读取八叉树的一般入口。
///该函数基于扩展名filename调用read函数。
///如果读取成功返回true,否则返回false。
bool ReadOctree(const std::string &filename,
geometry::Octree &octree,
const std::string &format = "auto");
///将八叉树写入文件的一般入口。
///函数调用基于扩展名filename的write函数。
///如果write函数支持二进制编码和压缩,则使用后两个参数。否则它们将被忽略。
///如果写函数成功返回true,否则返回false。
bool WriteOctree(const std::string &filename, const geometry::Octree &octree);
bool ReadOctreeFromJson(const std::string &filename, geometry::Octree &octree);
bool WriteOctreeToJson(const std::string &filename,
const geometry::Octree &octree);
} // namespace io
} // namespace open3d
小结
本篇对open3D中Octree相关的源码进行了分析,使得我对open3D实现Octree有个更深的理解。下一篇是我们小组对之前所学的部分知识的应用。