105 lines
3.8 KiB
C++
Executable File
105 lines
3.8 KiB
C++
Executable File
// ----------------------------------------------------------------------------
|
|
// - Open3D: www.open3d.org -
|
|
// ----------------------------------------------------------------------------
|
|
// Copyright (c) 2018-2023 www.open3d.org
|
|
// SPDX-License-Identifier: MIT
|
|
// ----------------------------------------------------------------------------
|
|
|
|
#pragma once
|
|
|
|
#include "open3d/geometry/VoxelGrid.h"
|
|
#include "open3d/pipelines/integration/TSDFVolume.h"
|
|
|
|
namespace open3d {
|
|
|
|
namespace geometry {
|
|
|
|
class TSDFVoxel : public Voxel {
|
|
public:
|
|
TSDFVoxel() : Voxel() {}
|
|
TSDFVoxel(const Eigen::Vector3i &grid_index) : Voxel(grid_index) {}
|
|
TSDFVoxel(const Eigen::Vector3i &grid_index, const Eigen::Vector3d &color)
|
|
: Voxel(grid_index, color) {}
|
|
~TSDFVoxel() {}
|
|
|
|
public:
|
|
float tsdf_ = 0;
|
|
float weight_ = 0;
|
|
};
|
|
|
|
} // namespace geometry
|
|
|
|
namespace pipelines {
|
|
namespace integration {
|
|
|
|
/// \class UniformTSDFVolume
|
|
///
|
|
/// \brief UniformTSDFVolume implements the classic TSDF volume with uniform
|
|
/// voxel grid (Curless and Levoy 1996).
|
|
class UniformTSDFVolume : public TSDFVolume {
|
|
public:
|
|
UniformTSDFVolume(double length,
|
|
int resolution,
|
|
double sdf_trunc,
|
|
TSDFVolumeColorType color_type,
|
|
const Eigen::Vector3d &origin = Eigen::Vector3d::Zero());
|
|
~UniformTSDFVolume() override;
|
|
|
|
public:
|
|
void Reset() override;
|
|
void Integrate(const geometry::RGBDImage &image,
|
|
const camera::PinholeCameraIntrinsic &intrinsic,
|
|
const Eigen::Matrix4d &extrinsic) override;
|
|
std::shared_ptr<geometry::PointCloud> ExtractPointCloud() override;
|
|
std::shared_ptr<geometry::TriangleMesh> ExtractTriangleMesh() override;
|
|
|
|
/// Debug function to extract the voxel data into a VoxelGrid
|
|
std::shared_ptr<geometry::PointCloud> ExtractVoxelPointCloud() const;
|
|
/// Debug function to extract the voxel data VoxelGrid
|
|
std::shared_ptr<geometry::VoxelGrid> ExtractVoxelGrid() const;
|
|
/// Debug function to extract the volume TSDF data into a vector array
|
|
std::vector<Eigen::Vector2d> ExtractVolumeTSDF() const;
|
|
/// Debug function to extract the volume color data into a vector array
|
|
std::vector<Eigen::Vector3d> ExtractVolumeColor() const;
|
|
/// Debug function to inject voxel TSDF data into the volume
|
|
void InjectVolumeTSDF(const std::vector<Eigen::Vector2d> &sharedvoxels);
|
|
/// Debug function to inject voxel Color data into the volume
|
|
void InjectVolumeColor(const std::vector<Eigen::Vector3d> &sharedcolors);
|
|
|
|
/// Faster Integrate function that uses depth_to_camera_distance_multiplier
|
|
/// precomputed from camera intrinsic
|
|
void IntegrateWithDepthToCameraDistanceMultiplier(
|
|
const geometry::RGBDImage &image,
|
|
const camera::PinholeCameraIntrinsic &intrinsic,
|
|
const Eigen::Matrix4d &extrinsic,
|
|
const geometry::Image &depth_to_camera_distance_multiplier);
|
|
|
|
inline int IndexOf(int x, int y, int z) const {
|
|
return x * resolution_ * resolution_ + y * resolution_ + z;
|
|
}
|
|
|
|
inline int IndexOf(const Eigen::Vector3i &xyz) const {
|
|
return IndexOf(xyz(0), xyz(1), xyz(2));
|
|
}
|
|
|
|
public:
|
|
std::vector<geometry::TSDFVoxel> voxels_;
|
|
Eigen::Vector3d origin_;
|
|
/// Total length, where voxel_length = length / resolution.
|
|
double length_;
|
|
/// Resolution over the total length, where voxel_length = length /
|
|
/// resolution.
|
|
int resolution_;
|
|
/// Number of voxels present.
|
|
int voxel_num_;
|
|
|
|
private:
|
|
Eigen::Vector3d GetNormalAt(const Eigen::Vector3d &p);
|
|
|
|
double GetTSDFAt(const Eigen::Vector3d &p);
|
|
};
|
|
|
|
} // namespace integration
|
|
} // namespace pipelines
|
|
} // namespace open3d
|