Open3D (C++ API)
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 
30 
31 namespace open3d {
32 namespace integration {
33 
34 class UniformTSDFVolume : public TSDFVolume {
35 public:
36  UniformTSDFVolume(double length,
37  int resolution,
38  double sdf_trunc,
39  TSDFVolumeColorType color_type,
40  const Eigen::Vector3d &origin = Eigen::Vector3d::Zero());
41  ~UniformTSDFVolume() override;
42 
43 public:
44  void Reset() override;
45  void Integrate(const geometry::RGBDImage &image,
46  const camera::PinholeCameraIntrinsic &intrinsic,
47  const Eigen::Matrix4d &extrinsic) override;
48  std::shared_ptr<geometry::PointCloud> ExtractPointCloud() override;
49  std::shared_ptr<geometry::TriangleMesh> ExtractTriangleMesh() override;
50 
52  std::shared_ptr<geometry::PointCloud> ExtractVoxelPointCloud();
53 
57  const geometry::RGBDImage &image,
58  const camera::PinholeCameraIntrinsic &intrinsic,
59  const Eigen::Matrix4d &extrinsic,
60  const geometry::Image &depth_to_camera_distance_multiplier);
61 
62  inline int IndexOf(int x, int y, int z) const {
63  return x * resolution_ * resolution_ + y * resolution_ + z;
64  }
65 
66  inline int IndexOf(const Eigen::Vector3i &xyz) const {
67  return IndexOf(xyz(0), xyz(1), xyz(2));
68  }
69 
70 public:
71  Eigen::Vector3d origin_;
72  double length_;
75  std::vector<float> tsdf_;
76  std::vector<Eigen::Vector3f> color_;
77  std::vector<float> weight_;
78 
79 private:
80  Eigen::Vector3d GetNormalAt(const Eigen::Vector3d &p);
81 
82  double GetTSDFAt(const Eigen::Vector3d &p);
83 };
84 
85 } // namespace integration
86 } // namespace open3d
int IndexOf(const Eigen::Vector3i &xyz) const
Definition: UniformTSDFVolume.h:66
TSDFVolumeColorType
Definition: TSDFVolume.h:37
std::shared_ptr< geometry::PointCloud > ExtractPointCloud() override
Definition: UniformTSDFVolume.cpp:98
std::vector< float > weight_
Definition: UniformTSDFVolume.h:77
void Reset() override
Function to reset the TSDFVolume.
Definition: UniformTSDFVolume.cpp:56
UniformTSDFVolume(double length, int resolution, double sdf_trunc, TSDFVolumeColorType color_type, const Eigen::Vector3d &origin=Eigen::Vector3d::Zero())
Definition: UniformTSDFVolume.cpp:38
Definition: PinholeCameraIntrinsic.h:41
int resolution_
Definition: UniformTSDFVolume.h:73
Definition: TSDFVolume.h:50
std::shared_ptr< geometry::PointCloud > ExtractVoxelPointCloud()
Debug function to extract the voxel data into a point cloud.
Definition: UniformTSDFVolume.cpp:236
Definition: RGBDImage.h:38
Definition: UniformTSDFVolume.h:34
int voxel_num_
Definition: UniformTSDFVolume.h:74
void Integrate(const geometry::RGBDImage &image, const camera::PinholeCameraIntrinsic &intrinsic, const Eigen::Matrix4d &extrinsic) override
Function to integrate an RGB-D image into the volume.
Definition: UniformTSDFVolume.cpp:64
Definition: PinholeCameraIntrinsic.cpp:33
void IntegrateWithDepthToCameraDistanceMultiplier(const geometry::RGBDImage &image, const camera::PinholeCameraIntrinsic &intrinsic, const Eigen::Matrix4d &extrinsic, const geometry::Image &depth_to_camera_distance_multiplier)
Definition: UniformTSDFVolume.cpp:260
std::vector< float > tsdf_
Definition: UniformTSDFVolume.h:75
std::vector< Eigen::Vector3f > color_
Definition: UniformTSDFVolume.h:76
std::shared_ptr< geometry::TriangleMesh > ExtractTriangleMesh() override
Definition: UniformTSDFVolume.cpp:153
~UniformTSDFVolume() override
Definition: UniformTSDFVolume.cpp:54
double length_
Definition: UniformTSDFVolume.h:72
Eigen::Vector3d origin_
Definition: UniformTSDFVolume.h:71
Definition: Image.h:44
int IndexOf(int x, int y, int z) const
Definition: UniformTSDFVolume.h:62