Open3D (C++ API)  0.12.0
UniformTSDFVolume.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 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 
31 
32 namespace open3d {
33 
34 namespace geometry {
35 
36 class TSDFVoxel : public Voxel {
37 public:
38  TSDFVoxel() : Voxel() {}
39  TSDFVoxel(const Eigen::Vector3i &grid_index) : Voxel(grid_index) {}
40  TSDFVoxel(const Eigen::Vector3i &grid_index, const Eigen::Vector3d &color)
41  : Voxel(grid_index, color) {}
43 
44 public:
45  float tsdf_ = 0;
46  float weight_ = 0;
47 };
48 
49 } // namespace geometry
50 
51 namespace pipelines {
52 namespace integration {
53 
58 class UniformTSDFVolume : public TSDFVolume {
59 public:
60  UniformTSDFVolume(double length,
61  int resolution,
62  double sdf_trunc,
63  TSDFVolumeColorType color_type,
64  const Eigen::Vector3d &origin = Eigen::Vector3d::Zero());
65  ~UniformTSDFVolume() override;
66 
67 public:
68  void Reset() override;
69  void Integrate(const geometry::RGBDImage &image,
70  const camera::PinholeCameraIntrinsic &intrinsic,
71  const Eigen::Matrix4d &extrinsic) override;
72  std::shared_ptr<geometry::PointCloud> ExtractPointCloud() override;
73  std::shared_ptr<geometry::TriangleMesh> ExtractTriangleMesh() override;
74 
76  std::shared_ptr<geometry::PointCloud> ExtractVoxelPointCloud() const;
78  std::shared_ptr<geometry::VoxelGrid> ExtractVoxelGrid() const;
79 
82  void IntegrateWithDepthToCameraDistanceMultiplier(
83  const geometry::RGBDImage &image,
84  const camera::PinholeCameraIntrinsic &intrinsic,
85  const Eigen::Matrix4d &extrinsic,
86  const geometry::Image &depth_to_camera_distance_multiplier);
87 
88  inline int IndexOf(int x, int y, int z) const {
89  return x * resolution_ * resolution_ + y * resolution_ + z;
90  }
91 
92  inline int IndexOf(const Eigen::Vector3i &xyz) const {
93  return IndexOf(xyz(0), xyz(1), xyz(2));
94  }
95 
96 public:
97  std::vector<geometry::TSDFVoxel> voxels_;
98  Eigen::Vector3d origin_;
100  double length_;
106 
107 private:
108  Eigen::Vector3d GetNormalAt(const Eigen::Vector3d &p);
109 
110  double GetTSDFAt(const Eigen::Vector3d &p);
111 };
112 
113 } // namespace integration
114 } // namespace pipelines
115 } // namespace open3d
~TSDFVoxel()
Definition: UniformTSDFVolume.h:42
Definition: UniformTSDFVolume.h:36
std::vector< geometry::TSDFVoxel > voxels_
Definition: UniformTSDFVolume.h:97
Contains the pinhole camera intrinsic parameters.
Definition: PinholeCameraIntrinsic.h:51
int IndexOf(int x, int y, int z) const
Definition: UniformTSDFVolume.h:88
math::float4 color
Definition: LineSetBuffers.cpp:64
int voxel_num_
Number of voxels present.
Definition: UniformTSDFVolume.h:105
RGBDImage is for a pair of registered color and depth images,.
Definition: RGBDImage.h:46
TSDFVolumeColorType
Definition: TSDFVolume.h:41
UniformTSDFVolume implements the classic TSDF volume with uniform voxel grid (Curless and Levoy 1996)...
Definition: UniformTSDFVolume.h:58
Definition: PinholeCameraIntrinsic.cpp:35
TSDFVoxel()
Definition: UniformTSDFVolume.h:38
int IndexOf(const Eigen::Vector3i &xyz) const
Definition: UniformTSDFVolume.h:92
Base class of the Truncated Signed Distance Function (TSDF) volume.
Definition: TSDFVolume.h:61
Base Voxel class, containing grid id and color.
Definition: VoxelGrid.h:54
float weight_
Definition: UniformTSDFVolume.h:46
double length_
Total length, where voxel_length = length / resolution.
Definition: UniformTSDFVolume.h:100
The Image class stores image with customizable width, height, num of channels and bytes per channel...
Definition: Image.h:53
int resolution_
Definition: UniformTSDFVolume.h:103
float tsdf_
Definition: UniformTSDFVolume.h:45
TSDFVoxel(const Eigen::Vector3i &grid_index)
Definition: UniformTSDFVolume.h:39
TSDFVoxel(const Eigen::Vector3i &grid_index, const Eigen::Vector3d &color)
Definition: UniformTSDFVolume.h:40
Eigen::Vector3d origin_
Definition: UniformTSDFVolume.h:98