1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
|
// ----------------------------------------------------------------------------
// - Open3D: www.open3d.org -
// ----------------------------------------------------------------------------
// Copyright (c) 2018-2024 www.open3d.org
// SPDX-License-Identifier: MIT
// ----------------------------------------------------------------------------
#include "open3d/Open3D.h"
void PrintHelp() {
using namespace open3d;
PrintOpen3DVersion();
// clang-format off
utility::LogInfo("Usage:");
utility::LogInfo(" > OfflineSLAM [options]");
utility::LogInfo(" Given an RGBD image sequence, perform frame-to-model tracking and mapping, and reconstruct the surface.");
utility::LogInfo("");
utility::LogInfo("Basic options:");
utility::LogInfo(" --color_folder_path");
utility::LogInfo(" --depth_folder_path");
utility::LogInfo(" --intrinsic_path [camera_intrinsic]");
utility::LogInfo(" --voxel_size [=0.0058 (m)]");
utility::LogInfo(" --depth_scale [=1000.0]");
utility::LogInfo(" --depth_max [=3.0]");
utility::LogInfo(" --trunc_voxel_multiplier [=8.0]");
utility::LogInfo(" --block_count [=10000]");
utility::LogInfo(" --device [CPU:0]");
utility::LogInfo(" --pointcloud [file path to save the extracted pointcloud]");
utility::LogInfo(" --mesh [file path to save the extracted mesh]");
utility::LogInfo(" --vis [whether to visualize the result]");
utility::LogInfo("To run similar example with a default dataset, try the `OnlineSLAMRGBD` example");
// clang-format on
utility::LogInfo("");
}
int main(int argc, char* argv[]) {
using namespace open3d;
using core::Tensor;
using t::geometry::Image;
using t::geometry::PointCloud;
utility::SetVerbosityLevel(utility::VerbosityLevel::Info);
if (argc < 1 ||
utility::ProgramOptionExistsAny(argc, argv, {"-h", "--help"})) {
PrintHelp();
return 1;
}
// Device
std::string device_code = "CPU:0";
if (utility::ProgramOptionExists(argc, argv, "--device")) {
device_code = utility::GetProgramOptionAsString(argc, argv, "--device");
}
core::Device device(device_code);
utility::LogInfo("Using device: {}", device.ToString());
// Input RGBD files
std::string color_folder_path = utility::GetProgramOptionAsString(
argc, argv, "--color_folder_path", "");
std::string depth_folder_path = utility::GetProgramOptionAsString(
argc, argv, "--depth_folder_path", "");
std::vector<std::string> color_filenames, depth_filenames;
if (color_folder_path.empty() || depth_folder_path.empty()) {
utility::LogInfo("Using default RGBD sample dataset.");
data::SampleRedwoodRGBDImages sample_rgbd_data;
color_filenames = sample_rgbd_data.GetColorPaths();
depth_filenames = sample_rgbd_data.GetDepthPaths();
} else {
utility::filesystem::ListFilesInDirectory(color_folder_path,
color_filenames);
utility::filesystem::ListFilesInDirectory(depth_folder_path,
depth_filenames);
if (color_filenames.size() != depth_filenames.size()) {
utility::LogError(
"Numbers of color and depth files mismatch. Please provide "
"folders with same number of images.");
}
std::sort(color_filenames.begin(), color_filenames.end());
std::sort(depth_filenames.begin(), depth_filenames.end());
}
size_t n = color_filenames.size();
size_t iterations = static_cast<size_t>(
utility::GetProgramOptionAsInt(argc, argv, "--iterations", n));
iterations = std::min(n, iterations);
// Intrinsics
std::string intrinsic_path = utility::GetProgramOptionAsString(
argc, argv, "--intrinsic_path", "");
camera::PinholeCameraIntrinsic intrinsic = camera::PinholeCameraIntrinsic(
camera::PinholeCameraIntrinsicParameters::PrimeSenseDefault);
if (intrinsic_path.empty()) {
utility::LogWarning("Using default Primesense intrinsics");
} else if (!io::ReadIJsonConvertible(intrinsic_path, intrinsic)) {
utility::LogError("Unable to convert json to intrinsics.");
}
auto focal_length = intrinsic.GetFocalLength();
auto principal_point = intrinsic.GetPrincipalPoint();
Tensor intrinsic_t = Tensor::Init<double>(
{{focal_length.first, 0, principal_point.first},
{0, focal_length.second, principal_point.second},
{0, 0, 1}});
// VoxelBlock configurations
float voxel_size = static_cast<float>(utility::GetProgramOptionAsDouble(
argc, argv, "--voxel_size", 3.f / 512.f));
float trunc_voxel_multiplier =
static_cast<float>(utility::GetProgramOptionAsDouble(
argc, argv, "--trunc_voxel_multiplier", 8.0f));
int block_resolution = utility::GetProgramOptionAsInt(
argc, argv, "--block_resolution", 16);
int block_count =
utility::GetProgramOptionAsInt(argc, argv, "--block_count", 10000);
// Odometry configurations
float depth_scale = static_cast<float>(utility::GetProgramOptionAsDouble(
argc, argv, "--depth_scale", 1000.f));
float depth_max = static_cast<float>(
utility::GetProgramOptionAsDouble(argc, argv, "--depth_max", 3.f));
float depth_diff = static_cast<float>(utility::GetProgramOptionAsDouble(
argc, argv, "--depth_diff", 0.07f));
// Initialization
Tensor T_frame_to_model =
Tensor::Eye(4, core::Dtype::Float64, core::Device("CPU:0"));
t::pipelines::slam::Model model(voxel_size, block_resolution, block_count,
T_frame_to_model, device);
// Initialize frame
Image ref_depth = *t::io::CreateImageFromFile(depth_filenames[0]);
t::pipelines::slam::Frame input_frame(
ref_depth.GetRows(), ref_depth.GetCols(), intrinsic_t, device);
t::pipelines::slam::Frame raycast_frame(
ref_depth.GetRows(), ref_depth.GetCols(), intrinsic_t, device);
// Iterate over frames
for (size_t i = 0; i < iterations; ++i) {
utility::LogInfo("Processing {}/{}...", i, iterations);
// Load image into frame
Image input_depth = *t::io::CreateImageFromFile(depth_filenames[i]);
Image input_color = *t::io::CreateImageFromFile(color_filenames[i]);
input_frame.SetDataFromImage("depth", input_depth);
input_frame.SetDataFromImage("color", input_color);
bool tracking_success = true;
if (i > 0) {
auto result =
model.TrackFrameToModel(input_frame, raycast_frame,
depth_scale, depth_max, depth_diff);
core::Tensor translation =
result.transformation_.Slice(0, 0, 3).Slice(1, 3, 4);
double translation_norm = std::sqrt(
(translation * translation).Sum({0, 1}).Item<double>());
// TODO(wei): more systematical failure check.
// If the overlap is too small or translation is too high between
// two consecutive frames, it is likely that the tracking failed.
if (result.fitness_ >= 0.1 && translation_norm < 0.15) {
T_frame_to_model =
T_frame_to_model.Matmul(result.transformation_);
} else { // Don't update
tracking_success = false;
utility::LogWarning(
"Tracking failed for frame {}, fitness: {:.3f}, "
"translation: {:.3f}. Using previous frame's "
"pose.",
i, result.fitness_, translation_norm);
}
}
// Integrate
model.UpdateFramePose(i, T_frame_to_model);
if (tracking_success) {
model.Integrate(input_frame, depth_scale, depth_max,
trunc_voxel_multiplier);
}
model.SynthesizeModelFrame(raycast_frame, depth_scale, 0.1, depth_max,
trunc_voxel_multiplier, false);
}
if (utility::ProgramOptionExists(argc, argv, "--pointcloud")) {
std::string filename = utility::GetProgramOptionAsString(
argc, argv, "--pointcloud",
"pcd_" + device.ToString() + ".ply");
auto pcd = model.ExtractPointCloud();
auto pcd_legacy =
std::make_shared<open3d::geometry::PointCloud>(pcd.ToLegacy());
open3d::io::WritePointCloud(filename, *pcd_legacy);
if (utility::ProgramOptionExists(argc, argv, "--vis")) {
open3d::visualization::Draw({pcd_legacy}, "Extracted PointCloud.");
}
} else {
// If nothing is specified, draw and save the geometry as mesh.
std::string filename = utility::GetProgramOptionAsString(
argc, argv, "--mesh", "mesh_" + device.ToString() + ".ply");
auto mesh = model.ExtractTriangleMesh();
auto mesh_legacy = std::make_shared<open3d::geometry::TriangleMesh>(
mesh.ToLegacy());
open3d::io::WriteTriangleMesh(filename, *mesh_legacy);
if (utility::ProgramOptionExists(argc, argv, "--vis")) {
open3d::visualization::Draw({mesh_legacy}, "Extracted Mesh.");
}
}
}
|
