Open3D (C++ API)  0.15.1
Octree.h
Go to the documentation of this file.
1 // ----------------------------------------------------------------------------
2 // - Open3D: www.open3d.org -
3 // ----------------------------------------------------------------------------
4 // The MIT License (MIT)
5 //
6 // Copyright (c) 2018-2021 www.open3d.org
7 //
8 // Permission is hereby granted, free of charge, to any person obtaining a copy
9 // of this software and associated documentation files (the "Software"), to deal
10 // in the Software without restriction, including without limitation the rights
11 // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
12 // copies of the Software, and to permit persons to whom the Software is
13 // furnished to do so, subject to the following conditions:
14 //
15 // The above copyright notice and this permission notice shall be included in
16 // all copies or substantial portions of the Software.
17 //
18 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
19 // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
20 // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
21 // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
22 // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
23 // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
24 // IN THE SOFTWARE.
25 // ----------------------------------------------------------------------------
26 
27 #pragma once
28 
29 #include <cstddef>
30 #include <memory>
31 #include <vector>
32 
35 
36 namespace open3d {
37 namespace geometry {
38 
39 class PointCloud;
40 class VoxelGrid;
41 
48 public:
52  OctreeNodeInfo() : origin_(0, 0, 0), size_(0), depth_(0), child_index_(0) {}
53 
60  OctreeNodeInfo(const Eigen::Vector3d& origin,
61  const double& size,
62  const size_t& depth,
63  const size_t& child_index)
64  : origin_(origin),
65  size_(size),
66  depth_(depth),
67  child_index_(child_index) {}
69 
70 public:
72  Eigen::Vector3d origin_;
74  double size_;
76  size_t depth_;
79  size_t child_index_;
80 };
81 
90 public:
94  virtual ~OctreeNode() {}
95 
97  static std::shared_ptr<OctreeNode> ConstructFromJsonValue(
98  const Json::Value& value);
99 };
100 
120 public:
124  static std::shared_ptr<OctreeNodeInfo> GetInsertionNodeInfo(
125  const std::shared_ptr<OctreeNodeInfo>& node_info,
126  const Eigen::Vector3d& point);
127 
132  static std::function<std::shared_ptr<OctreeInternalNode>()>
133  GetInitFunction();
134 
138  static std::function<void(std::shared_ptr<OctreeInternalNode>)>
140 
141  bool ConvertToJsonValue(Json::Value& value) const override;
142  bool ConvertFromJsonValue(const Json::Value& value) override;
143 
144 public:
149  std::vector<std::shared_ptr<OctreeNode>> children_;
150 };
151 
157 public:
161 
166  static std::function<std::shared_ptr<OctreeInternalNode>()>
167  GetInitFunction();
168 
173  static std::function<void(std::shared_ptr<OctreeInternalNode>)>
174  GetUpdateFunction(size_t idx);
175 
176  bool ConvertToJsonValue(Json::Value& value) const override;
177  bool ConvertFromJsonValue(const Json::Value& value) override;
178 
179 public:
181  std::vector<size_t> indices_;
182 };
183 
187 class OctreeLeafNode : public OctreeNode {
188 public:
189  virtual bool operator==(const OctreeLeafNode& other) const = 0;
191  virtual std::shared_ptr<OctreeLeafNode> Clone() const = 0;
192 };
193 
198 public:
199  bool operator==(const OctreeLeafNode& other) const override;
200 
202  std::shared_ptr<OctreeLeafNode> Clone() const override;
203 
208  static std::function<std::shared_ptr<OctreeLeafNode>()> GetInitFunction();
209 
216  static std::function<void(std::shared_ptr<OctreeLeafNode>)>
217  GetUpdateFunction(const Eigen::Vector3d& color);
218 
219  bool ConvertToJsonValue(Json::Value& value) const override;
220  bool ConvertFromJsonValue(const Json::Value& value) override;
223  Eigen::Vector3d color_ = Eigen::Vector3d(0, 0, 0);
224 };
225 
232 public:
233  bool operator==(const OctreeLeafNode& other) const override;
235  std::shared_ptr<OctreeLeafNode> Clone() const override;
236 
241  static std::function<std::shared_ptr<OctreeLeafNode>()> GetInitFunction();
242 
250  static std::function<void(std::shared_ptr<OctreeLeafNode>)>
251  GetUpdateFunction(size_t index, const Eigen::Vector3d& color);
252 
253  bool ConvertToJsonValue(Json::Value& value) const override;
254  bool ConvertFromJsonValue(const Json::Value& value) override;
255 
257  std::vector<size_t> indices_;
258 };
259 
264 public:
268  origin_(0, 0, 0),
269  size_(0),
270  max_depth_(0) {}
274  Octree(const size_t& max_depth)
276  origin_(0, 0, 0),
277  size_(0),
278  max_depth_(max_depth) {}
284  Octree(const size_t& max_depth,
285  const Eigen::Vector3d& origin,
286  const double& size)
288  origin_(origin),
289  size_(size),
290  max_depth_(max_depth) {}
291  Octree(const Octree& src_octree);
292  ~Octree() override {}
293 
294 public:
295  Octree& Clear() override;
296  bool IsEmpty() const override;
297  Eigen::Vector3d GetMinBound() const override;
298  Eigen::Vector3d GetMaxBound() const override;
299  Eigen::Vector3d GetCenter() const override;
302  bool robust = false) const override;
303  Octree& Transform(const Eigen::Matrix4d& transformation) override;
304  Octree& Translate(const Eigen::Vector3d& translation,
305  bool relative = true) override;
306  Octree& Scale(const double scale, const Eigen::Vector3d& center) override;
307  Octree& Rotate(const Eigen::Matrix3d& R,
308  const Eigen::Vector3d& center) override;
309  bool ConvertToJsonValue(Json::Value& value) const override;
310  bool ConvertFromJsonValue(const Json::Value& value) override;
311 
312 public:
319  void ConvertFromPointCloud(const geometry::PointCloud& point_cloud,
320  double size_expand = 0.01);
321 
323  std::shared_ptr<OctreeNode> root_node_ = nullptr;
324 
327  Eigen::Vector3d origin_;
328 
331  double size_;
332 
335  size_t max_depth_;
336 
350  void InsertPoint(
351  const Eigen::Vector3d& point,
352  const std::function<std::shared_ptr<OctreeLeafNode>()>& fl_init,
353  const std::function<void(std::shared_ptr<OctreeLeafNode>)>&
354  fl_update,
355  const std::function<std::shared_ptr<OctreeInternalNode>()>&
356  fi_init = nullptr,
357  const std::function<void(std::shared_ptr<OctreeInternalNode>)>&
358  fi_update = nullptr);
359 
367  void Traverse(
368  const std::function<bool(const std::shared_ptr<OctreeNode>&,
369  const std::shared_ptr<OctreeNodeInfo>&)>&
370  f);
371 
379  void Traverse(
380  const std::function<bool(const std::shared_ptr<OctreeNode>&,
381  const std::shared_ptr<OctreeNodeInfo>&)>&
382  f) const;
383 
384  std::pair<std::shared_ptr<OctreeLeafNode>, std::shared_ptr<OctreeNodeInfo>>
385 
390  LocateLeafNode(const Eigen::Vector3d& point) const;
391 
398  static bool IsPointInBound(const Eigen::Vector3d& point,
399  const Eigen::Vector3d& origin,
400  const double& size);
401 
403  bool operator==(const Octree& other) const;
404 
406  std::shared_ptr<geometry::VoxelGrid> ToVoxelGrid() const;
407 
409  void CreateFromVoxelGrid(const geometry::VoxelGrid& voxel_grid);
410 
411 private:
412  static void TraverseRecurse(
413  const std::shared_ptr<OctreeNode>& node,
414  const std::shared_ptr<OctreeNodeInfo>& node_info,
415  const std::function<bool(const std::shared_ptr<OctreeNode>&,
416  const std::shared_ptr<OctreeNodeInfo>&)>&
417  f);
418 
419  void InsertPointRecurse(
420  const std::shared_ptr<OctreeNode>& node,
421  const std::shared_ptr<OctreeNodeInfo>& node_info,
422  const Eigen::Vector3d& point,
423  const std::function<std::shared_ptr<OctreeLeafNode>()>& f_l_init,
424  const std::function<void(std::shared_ptr<OctreeLeafNode>)>&
425  f_l_update,
426  const std::function<std::shared_ptr<OctreeInternalNode>()>&
427  f_i_init,
428  const std::function<void(std::shared_ptr<OctreeInternalNode>)>&
429  f_i_update);
430 };
431 
432 } // namespace geometry
433 } // namespace open3d
math::float4 color
Definition: LineSetBuffers.cpp:64
A bounding box that is aligned along the coordinate axes.
Definition: BoundingVolume.h:155
The base geometry class for 3D geometries.
Definition: Geometry3D.h:47
The base geometry class.
Definition: Geometry.h:37
GeometryType
Specifies possible geometry types.
Definition: Geometry.h:42
OctreeColorLeafNode class is an OctreeLeafNode containing color.
Definition: Octree.h:197
bool ConvertToJsonValue(Json::Value &value) const override
Definition: Octree.cpp:251
Eigen::Vector3d color_
Definition: Octree.h:223
bool operator==(const OctreeLeafNode &other) const override
Definition: Octree.cpp:241
std::shared_ptr< OctreeLeafNode > Clone() const override
Clone this OctreeLeafNode.
Definition: Octree.cpp:235
static std::function< std::shared_ptr< OctreeLeafNode >)> GetInitFunction()
Get lambda function for initializing OctreeLeafNode.
Definition: Octree.cpp:213
bool ConvertFromJsonValue(const Json::Value &value) override
Definition: Octree.cpp:256
static std::function< void(std::shared_ptr< OctreeLeafNode >)> GetUpdateFunction(const Eigen::Vector3d &color)
Get lambda function for updating OctreeLeafNode.
Definition: Octree.cpp:220
Octree datastructure.
Definition: Octree.h:263
void ConvertFromPointCloud(const geometry::PointCloud &point_cloud, double size_expand=0.01)
Convert octree from point cloud.
Definition: Octree.cpp:558
std::shared_ptr< OctreeNode > root_node_
Root of the octree.
Definition: Octree.h:323
Eigen::Vector3d GetCenter() const override
Returns the center of the geometry coordinates.
Definition: Octree.cpp:521
bool IsEmpty() const override
Returns true iff the geometry is empty.
Definition: Octree.cpp:503
bool ConvertToJsonValue(Json::Value &value) const override
Definition: Octree.cpp:768
Octree & Clear() override
Clear all elements in the geometry.
Definition: Octree.cpp:495
bool operator==(const Octree &other) const
Returns true if the Octree is completely the same, used for testing.
Definition: Octree.cpp:400
Eigen::Vector3d origin_
Definition: Octree.h:327
std::pair< std::shared_ptr< OctreeLeafNode >, std::shared_ptr< OctreeNodeInfo > > LocateLeafNode(const Eigen::Vector3d &point) const
Returns leaf OctreeNode and OctreeNodeInfo where the querypoint should reside.
Definition: Octree.cpp:739
Eigen::Vector3d GetMinBound() const override
Returns min bounds for geometry coordinates.
Definition: Octree.cpp:505
bool ConvertFromJsonValue(const Json::Value &value) override
Definition: Octree.cpp:782
Octree & Rotate(const Eigen::Matrix3d &R, const Eigen::Vector3d &center) override
Apply rotation to the geometry coordinates and normals. Given a rotation matrix , and center ,...
Definition: Octree.cpp:552
double size_
Definition: Octree.h:331
AxisAlignedBoundingBox GetAxisAlignedBoundingBox() const override
Returns an axis-aligned bounding box of the geometry.
Definition: Octree.cpp:525
void CreateFromVoxelGrid(const geometry::VoxelGrid &voxel_grid)
Convert from voxel grid.
Definition: Octree.cpp:803
Octree & Transform(const Eigen::Matrix4d &transformation) override
Apply transformation (4x4 matrix) to the geometry coordinates.
Definition: Octree.cpp:537
Octree & Translate(const Eigen::Vector3d &translation, bool relative=true) override
Apply translation to the geometry coordinates.
Definition: Octree.cpp:542
std::shared_ptr< geometry::VoxelGrid > ToVoxelGrid() const
Convert to VoxelGrid.
Definition: Octree.cpp:762
OrientedBoundingBox GetOrientedBoundingBox(bool robust=false) const override
Definition: Octree.cpp:532
static bool IsPointInBound(const Eigen::Vector3d &point, const Eigen::Vector3d &origin, const double &size)
Return true if point within bound, that is, origin <= point < origin + size.
Definition: Octree.cpp:675
void Traverse(const std::function< bool(const std::shared_ptr< OctreeNode > &, const std::shared_ptr< OctreeNodeInfo > &)> &f)
DFS traversal of Octree from the root, with callback function called for each node.
Definition: Octree.cpp:683
size_t max_depth_
Definition: Octree.h:335
Octree(const size_t &max_depth)
Parameterized Constructor.
Definition: Octree.h:274
Octree(const size_t &max_depth, const Eigen::Vector3d &origin, const double &size)
Parameterized Constructor.
Definition: Octree.h:284
~Octree() override
Definition: Octree.h:292
Octree()
Default Constructor.
Definition: Octree.h:266
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)
Insert a point to the octree.
Definition: Octree.cpp:591
Octree & Scale(const double scale, const Eigen::Vector3d &center) override
Apply scaling to the geometry coordinates. Given a scaling factor , and center , a given point is tr...
Definition: Octree.cpp:547
Eigen::Vector3d GetMaxBound() const override
Returns max bounds for geometry coordinates.
Definition: Octree.cpp:513
OctreeInternalNode class, containing OctreeNode children.
Definition: Octree.h:119
std::vector< std::shared_ptr< OctreeNode > > children_
Definition: Octree.h:149
static std::shared_ptr< OctreeNodeInfo > GetInsertionNodeInfo(const std::shared_ptr< OctreeNodeInfo > &node_info, const Eigen::Vector3d &point)
Definition: Octree.cpp:71
static std::function< std::shared_ptr< OctreeInternalNode >)> GetInitFunction()
Get lambda function for initializing OctreeInternalNode.
Definition: Octree.cpp:95
static std::function< void(std::shared_ptr< OctreeInternalNode >)> GetUpdateFunction()
Get lambda function for updating OctreeInternalNode.
Definition: Octree.cpp:102
bool ConvertFromJsonValue(const Json::Value &value) override
Definition: Octree.cpp:122
bool ConvertToJsonValue(Json::Value &value) const override
Definition: Octree.cpp:106
OctreeInternalNode()
Default Constructor.
Definition: Octree.h:123
OctreeInternalPointNode class is an OctreeInternalNode containing a list of indices which is the unio...
Definition: Octree.h:156
OctreeInternalPointNode()
Default Constructor.
Definition: Octree.h:160
std::vector< size_t > indices_
Indices of points associated with any children of this node.
Definition: Octree.h:181
bool ConvertFromJsonValue(const Json::Value &value) override
Definition: Octree.cpp:186
static std::function< std::shared_ptr< OctreeInternalNode >)> GetInitFunction()
Get lambda function for initializing OctreeInternalPointNode.
Definition: Octree.cpp:143
bool ConvertToJsonValue(Json::Value &value) const override
Definition: Octree.cpp:165
OctreeLeafNode base class.
Definition: Octree.h:187
virtual bool operator==(const OctreeLeafNode &other) const =0
virtual std::shared_ptr< OctreeLeafNode > Clone() const =0
Clone this OctreeLeafNode.
The base class for octree node.
Definition: Octree.h:89
OctreeNode()
Default Constructor.
Definition: Octree.h:93
static std::shared_ptr< OctreeNode > ConstructFromJsonValue(const Json::Value &value)
Factory function to construct an OctreeNode by parsing the json value.
Definition: Octree.cpp:43
virtual ~OctreeNode()
Definition: Octree.h:94
OctreeNode's information.
Definition: Octree.h:47
size_t child_index_
Definition: Octree.h:79
~OctreeNodeInfo()
Definition: Octree.h:68
OctreeNodeInfo(const Eigen::Vector3d &origin, const double &size, const size_t &depth, const size_t &child_index)
Parameterized Constructor.
Definition: Octree.h:60
double size_
Size of the node.
Definition: Octree.h:74
OctreeNodeInfo()
Default Constructor.
Definition: Octree.h:52
Eigen::Vector3d origin_
Origin coordinate of the node.
Definition: Octree.h:72
size_t depth_
Depth of the node to the root. The root is of depth 0.
Definition: Octree.h:76
OctreePointColorLeafNode class is an OctreeColorLeafNode containing a list of indices corresponding t...
Definition: Octree.h:231
static std::function< std::shared_ptr< OctreeLeafNode >)> GetInitFunction()
Get lambda function for initializing OctreeLeafNode.
Definition: Octree.cpp:270
bool ConvertToJsonValue(Json::Value &value) const override
Definition: Octree.cpp:316
std::vector< size_t > indices_
Associated point indices with this node.
Definition: Octree.h:257
static std::function< void(std::shared_ptr< OctreeLeafNode >)> GetUpdateFunction(size_t index, const Eigen::Vector3d &color)
Get lambda function for updating OctreeLeafNode.
Definition: Octree.cpp:277
bool operator==(const OctreeLeafNode &other) const override
Definition: Octree.cpp:303
bool ConvertFromJsonValue(const Json::Value &value) override
Definition: Octree.cpp:326
std::shared_ptr< OctreeLeafNode > Clone() const override
Clone this OctreeLeafNode.
Definition: Octree.cpp:296
A bounding box oriented along an arbitrary frame of reference.
Definition: BoundingVolume.h:44
A point cloud consists of point coordinates, and optionally point colors and point normals.
Definition: PointCloud.h:55
VoxelGrid is a collection of voxels which are aligned in grid.
Definition: VoxelGrid.h:80
Definition: IJsonConvertible.h:57
int size
Definition: FilePCD.cpp:59
const char const char value recording_handle imu_sample void
Definition: K4aPlugin.cpp:269
Definition: PinholeCameraIntrinsic.cpp:35