Open3D (C++ API)  0.18.0
Loading...
Searching...
No Matches
FillInLinearSystemImpl.h
Go to the documentation of this file.
1// ----------------------------------------------------------------------------
2// - Open3D: www.open3d.org -
3// ----------------------------------------------------------------------------
4// Copyright (c) 2018-2023 www.open3d.org
5// SPDX-License-Identifier: MIT
6// ----------------------------------------------------------------------------
7
8#pragma once
9
10#include <fstream>
11
16
17namespace open3d {
18namespace t {
19namespace pipelines {
20namespace slac {
21
22using namespace open3d::core::eigen_converter;
23using core::Tensor;
25
26// Reads pointcloud from filename, and loads on the device as
27// Tensor PointCloud of Float32 dtype.
28static PointCloud CreateTPCDFromFile(
29 const std::string& fname,
30 const core::Device& device = core::Device("CPU:0")) {
31 std::shared_ptr<open3d::geometry::PointCloud> pcd =
33 return PointCloud::FromLegacy(*pcd, core::Float32, device);
34}
35
36static void FillInRigidAlignmentTerm(Tensor& AtA,
37 Tensor& Atb,
38 Tensor& residual,
39 PointCloud& tpcd_i,
40 PointCloud& tpcd_j,
41 const Tensor& Ti,
42 const Tensor& Tj,
43 const int i,
44 const int j,
45 const float threshold) {
46 tpcd_i.Transform(Ti);
47 tpcd_j.Transform(Tj);
48
49 kernel::FillInRigidAlignmentTerm(AtA, Atb, residual,
50 tpcd_i.GetPointPositions(),
51 tpcd_j.GetPointPositions(),
52 tpcd_i.GetPointNormals(), i, j, threshold);
53}
54
55void FillInRigidAlignmentTerm(Tensor& AtA,
56 Tensor& Atb,
57 Tensor& residual,
58 const std::vector<std::string>& fnames,
59 const PoseGraph& pose_graph,
60 const SLACOptimizerParams& params,
61 const SLACDebugOption& debug_option) {
62 core::Device device(params.device_);
63
64 // Enumerate pose graph edges
65 for (auto& edge : pose_graph.edges_) {
66 int i = edge.source_node_id_;
67 int j = edge.target_node_id_;
68
69 std::string corres_fname = fmt::format("{}/{:03d}_{:03d}.npy",
70 params.GetSubfolderName(), i, j);
71 if (!utility::filesystem::FileExists(corres_fname)) {
72 utility::LogWarning("Correspondence {} {} skipped!", i, j);
73 continue;
74 }
75 Tensor corres_ij = Tensor::Load(corres_fname).To(device);
76 PointCloud tpcd_i = CreateTPCDFromFile(fnames[i], device);
77 PointCloud tpcd_j = CreateTPCDFromFile(fnames[j], device);
78
79 PointCloud tpcd_i_indexed(
80 tpcd_i.GetPointPositions().IndexGet({corres_ij.T()[0]}));
81 tpcd_i_indexed.SetPointNormals(
82 tpcd_i.GetPointNormals().IndexGet({corres_ij.T()[0]}));
83 PointCloud tpcd_j_indexed(
84 tpcd_j.GetPointPositions().IndexGet({corres_ij.T()[1]}));
85
86 Tensor Ti = EigenMatrixToTensor(pose_graph.nodes_[i].pose_)
87 .To(device, core::Float32);
88 Tensor Tj = EigenMatrixToTensor(pose_graph.nodes_[j].pose_)
89 .To(device, core::Float32);
90
91 FillInRigidAlignmentTerm(AtA, Atb, residual, tpcd_i_indexed,
92 tpcd_j_indexed, Ti, Tj, i, j,
93 params.distance_threshold_);
94
95 if (debug_option.debug_ && i >= debug_option.debug_start_node_idx_) {
96 VisualizePointCloudCorrespondences(tpcd_i, tpcd_j, corres_ij,
97 Tj.Inverse().Matmul(Ti));
98 }
99 }
100}
101
102static void FillInSLACAlignmentTerm(Tensor& AtA,
103 Tensor& Atb,
104 Tensor& residual,
105 ControlGrid& ctr_grid,
106 const PointCloud& tpcd_param_i,
107 const PointCloud& tpcd_param_j,
108 const Tensor& Ti,
109 const Tensor& Tj,
110 const int i,
111 const int j,
112 const int n_fragments,
113 const float threshold) {
114 // Parameterize: setup point cloud -> cgrid correspondences
115 Tensor cgrid_index_ps =
116 tpcd_param_i.GetPointAttr(ControlGrid::kGrid8NbIndices);
117 Tensor cgrid_ratio_ps =
118 tpcd_param_i.GetPointAttr(ControlGrid::kGrid8NbVertexInterpRatios);
119
120 Tensor cgrid_index_qs =
121 tpcd_param_j.GetPointAttr(ControlGrid::kGrid8NbIndices);
122 Tensor cgrid_ratio_qs =
123 tpcd_param_j.GetPointAttr(ControlGrid::kGrid8NbVertexInterpRatios);
124
125 // Deform with control grids
126 PointCloud tpcd_nonrigid_i = ctr_grid.Deform(tpcd_param_i);
127 PointCloud tpcd_nonrigid_j = ctr_grid.Deform(tpcd_param_j);
128
129 Tensor Cps = tpcd_nonrigid_i.GetPointPositions();
130 Tensor Cqs = tpcd_nonrigid_j.GetPointPositions();
131 Tensor Cnormal_ps = tpcd_nonrigid_i.GetPointNormals();
132
133 Tensor Ri = Ti.Slice(0, 0, 3).Slice(1, 0, 3);
134 Tensor ti = Ti.Slice(0, 0, 3).Slice(1, 3, 4);
135
136 Tensor Rj = Tj.Slice(0, 0, 3).Slice(1, 0, 3);
137 Tensor tj = Tj.Slice(0, 0, 3).Slice(1, 3, 4);
138
139 // Transform for required entries
140 Tensor Ti_Cps = (Ri.Matmul(Cps.T())).Add_(ti).T().Contiguous();
141 Tensor Tj_Cqs = (Rj.Matmul(Cqs.T())).Add_(tj).T().Contiguous();
142 Tensor Ri_Cnormal_ps = (Ri.Matmul(Cnormal_ps.T())).T().Contiguous();
143 Tensor RjT_Ri_Cnormal_ps =
144 (Rj.T().Matmul(Ri_Cnormal_ps.T())).T().Contiguous();
145
147 AtA, Atb, residual, Ti_Cps, Tj_Cqs, Cnormal_ps, Ri_Cnormal_ps,
148 RjT_Ri_Cnormal_ps, cgrid_index_ps, cgrid_index_qs, cgrid_ratio_ps,
149 cgrid_ratio_qs, i, j, n_fragments, threshold);
150}
151
152void FillInSLACAlignmentTerm(Tensor& AtA,
153 Tensor& Atb,
154 Tensor& residual,
155 ControlGrid& ctr_grid,
156 const std::vector<std::string>& fnames,
157 const PoseGraph& pose_graph,
158 const SLACOptimizerParams& params,
159 const SLACDebugOption& debug_option) {
160 core::Device device(params.device_);
161 int n_frags = pose_graph.nodes_.size();
162
163 // Enumerate pose graph edges.
164 for (auto& edge : pose_graph.edges_) {
165 int i = edge.source_node_id_;
166 int j = edge.target_node_id_;
167
168 std::string corres_fname = fmt::format("{}/{:03d}_{:03d}.npy",
169 params.GetSubfolderName(), i, j);
170 if (!utility::filesystem::FileExists(corres_fname)) {
171 utility::LogWarning("Correspondence {} {} skipped!", i, j);
172 continue;
173 }
174 Tensor corres_ij = Tensor::Load(corres_fname).To(device);
175
176 PointCloud tpcd_i = CreateTPCDFromFile(fnames[i], device);
177 PointCloud tpcd_j = CreateTPCDFromFile(fnames[j], device);
178
179 PointCloud tpcd_i_indexed(
180 tpcd_i.GetPointPositions().IndexGet({corres_ij.T()[0]}));
181 tpcd_i_indexed.SetPointNormals(
182 tpcd_i.GetPointNormals().IndexGet({corres_ij.T()[0]}));
183
184 PointCloud tpcd_j_indexed(
185 tpcd_j.GetPointPositions().IndexGet({corres_ij.T()[1]}));
186 tpcd_j_indexed.SetPointNormals(
187 tpcd_j.GetPointNormals().IndexGet({corres_ij.T()[1]}));
188
189 // Parameterize points in the control grid.
190 PointCloud tpcd_param_i = ctr_grid.Parameterize(tpcd_i_indexed);
191 PointCloud tpcd_param_j = ctr_grid.Parameterize(tpcd_j_indexed);
192
193 // Load poses.
194 auto Ti = EigenMatrixToTensor(pose_graph.nodes_[i].pose_)
195 .To(device, core::Float32);
196 auto Tj = EigenMatrixToTensor(pose_graph.nodes_[j].pose_)
197 .To(device, core::Float32);
198 auto Tij = EigenMatrixToTensor(edge.transformation_)
199 .To(device, core::Float32);
200
201 // Fill In.
202 FillInSLACAlignmentTerm(AtA, Atb, residual, ctr_grid, tpcd_param_i,
203 tpcd_param_j, Ti, Tj, i, j, n_frags,
204 params.distance_threshold_);
205
206 if (debug_option.debug_ && i >= debug_option.debug_start_node_idx_) {
207 VisualizePointCloudCorrespondences(tpcd_i, tpcd_j, corres_ij,
208 Tj.Inverse().Matmul(Ti));
209 VisualizePointCloudEmbedding(tpcd_param_i, ctr_grid);
210 VisualizePointCloudDeformation(tpcd_param_i, ctr_grid);
211 }
212 }
213}
214
216 Tensor& Atb,
217 Tensor& residual,
218 ControlGrid& ctr_grid,
219 int n_frags,
220 const SLACOptimizerParams& params,
221 const SLACDebugOption& debug_option) {
222 Tensor active_buf_indices, nb_buf_indices, nb_masks;
223 std::tie(active_buf_indices, nb_buf_indices, nb_masks) =
224 ctr_grid.GetNeighborGridMap();
225
226 Tensor positions_init = ctr_grid.GetInitPositions();
227 Tensor positions_curr = ctr_grid.GetCurrPositions();
228 kernel::FillInSLACRegularizerTerm(AtA, Atb, residual, active_buf_indices,
229 nb_buf_indices, nb_masks, positions_init,
230 positions_curr,
231 n_frags * params.regularizer_weight_,
232 n_frags, ctr_grid.GetAnchorIdx());
233 if (debug_option.debug_) {
234 VisualizeGridDeformation(ctr_grid);
235 }
236}
237
238} // namespace slac
239} // namespace pipelines
240} // namespace t
241} // namespace open3d
Definition Device.h:18
Definition Tensor.h:32
Tensor Inverse() const
Definition Tensor.cpp:1929
Tensor Add_(const Tensor &value)
Definition Tensor.cpp:1085
Tensor Slice(int64_t dim, int64_t start, int64_t stop, int64_t step=1) const
Definition Tensor.cpp:825
Tensor T() const
Expects input to be <= 2-D Tensor by swapping dimension 0 and 1.
Definition Tensor.cpp:1047
Tensor Contiguous() const
Definition Tensor.cpp:740
static Tensor Load(const std::string &file_name)
Load tensor from numpy's npy format.
Definition Tensor.cpp:1828
Tensor Matmul(const Tensor &rhs) const
Definition Tensor.cpp:1866
Tensor To(Dtype dtype, bool copy=false) const
Definition Tensor.cpp:707
Tensor IndexGet(const std::vector< Tensor > &index_tensors) const
Advanced indexing getter. This will always allocate a new Tensor.
Definition Tensor.cpp:873
A point cloud consists of point coordinates, and optionally point colors and point normals.
Definition PointCloud.h:36
PointCloud & Transform(const Eigen::Matrix4d &transformation) override
Apply transformation (4x4 matrix) to the geometry coordinates.
Definition PointCloud.cpp:60
Data structure defining the pose graph.
Definition PoseGraph.h:96
std::vector< PoseGraphNode > nodes_
List of PoseGraphNode.
Definition PoseGraph.h:108
std::vector< PoseGraphEdge > edges_
List of PoseGraphEdge.
Definition PoseGraph.h:110
A point cloud contains a list of 3D points.
Definition PointCloud.h:80
core::Tensor & GetPointNormals()
Get the value of the "normals" attribute. Convenience function.
Definition PointCloud.h:128
static PointCloud FromLegacy(const open3d::geometry::PointCloud &pcd_legacy, core::Dtype dtype=core::Float32, const core::Device &device=core::Device("CPU:0"))
Create a PointCloud from a legacy Open3D PointCloud.
Definition PointCloud.cpp:1029
core::Tensor & GetPointPositions()
Get the value of the "positions" attribute. Convenience function.
Definition PointCloud.h:122
void SetPointNormals(const core::Tensor &value)
Set the value of the "normals" attribute. Convenience function.
Definition PointCloud.h:178
Definition ControlGrid.h:30
int64_t GetAnchorIdx()
Definition ControlGrid.h:116
static const std::string kGrid8NbIndices
Definition ControlGrid.h:34
static const std::string kGrid8NbVertexInterpRatios
8 neighbor grid interpolation ratio for vertex per point.
Definition ControlGrid.h:36
core::Tensor GetInitPositions()
Get control grid original positions directly from tensor keys.
Definition ControlGrid.h:104
core::Tensor GetCurrPositions()
Definition ControlGrid.h:110
std::tuple< core::Tensor, core::Tensor, core::Tensor > GetNeighborGridMap()
Definition ControlGrid.cpp:115
geometry::PointCloud Parameterize(const geometry::PointCloud &pcd)
Definition ControlGrid.cpp:150
Definition EigenConverter.cpp:18
core::Tensor EigenMatrixToTensor(const Eigen::MatrixBase< Derived > &matrix)
Converts a eigen matrix of shape (M, N) with alignment A and type T to a tensor.
Definition EigenConverter.h:29
const Dtype Float32
Definition Dtype.cpp:42
std::shared_ptr< geometry::PointCloud > CreatePointCloudFromFile(const std::string &filename, const std::string &format, bool print_progress)
Definition PointCloudIO.cpp:69
void FillInSLACRegularizerTerm(core::Tensor &AtA, core::Tensor &Atb, core::Tensor &residual, const core::Tensor &grid_idx, const core::Tensor &grid_nbs_idx, const core::Tensor &grid_nbs_mask, const core::Tensor &positions_init, const core::Tensor &positions_curr, float weight, int n, int anchor_idx)
Definition FillInLinearSystem.cpp:133
void FillInSLACAlignmentTerm(core::Tensor &AtA, core::Tensor &Atb, core::Tensor &residual, const core::Tensor &Ti_ps, const core::Tensor &Tj_qs, const core::Tensor &normal_ps, const core::Tensor &Ri_normal_ps, const core::Tensor &RjT_Ri_normal_ps, const core::Tensor &cgrid_idx_ps, const core::Tensor &cgrid_idx_qs, const core::Tensor &cgrid_ratio_qs, const core::Tensor &cgrid_ratio_ps, int i, int j, int n, float threshold)
Definition FillInLinearSystem.cpp:69
void FillInRigidAlignmentTerm(core::Tensor &AtA, core::Tensor &Atb, core::Tensor &residual, const core::Tensor &Ti_ps, const core::Tensor &Tj_qs, const core::Tensor &Ri_normal_ps, int i, int j, float threshold)
Definition FillInLinearSystem.cpp:18
void VisualizePointCloudDeformation(const geometry::PointCloud &tpcd_param, ControlGrid &ctr_grid)
Definition Visualization.cpp:139
void VisualizeGridDeformation(ControlGrid &cgrid)
Definition Visualization.cpp:187
void VisualizePointCloudCorrespondences(const t::geometry::PointCloud &tpcd_i, const t::geometry::PointCloud &tpcd_j, const core::Tensor correspondences, const core::Tensor &T_ij)
Visualize pairs with correspondences.
Definition Visualization.cpp:46
void VisualizePointCloudEmbedding(t::geometry::PointCloud &tpcd_param, ControlGrid &ctr_grid, bool show_lines)
Definition Visualization.cpp:80
void FillInSLACRegularizerTerm(Tensor &AtA, Tensor &Atb, Tensor &residual, ControlGrid &ctr_grid, int n_frags, const SLACOptimizerParams &params, const SLACDebugOption &debug_option)
Definition FillInLinearSystemImpl.h:215
bool FileExists(const std::string &filename)
Definition FileSystem.cpp:270
Definition PinholeCameraIntrinsic.cpp:16
Definition SLACOptimizer.h:90
bool debug_
Enable debug.
Definition SLACOptimizer.h:92
int debug_start_node_idx_
Definition SLACOptimizer.h:96
float distance_threshold_
Distance threshold to filter inconsistent correspondences.
Definition SLACOptimizer.h:33
float regularizer_weight_
Weight of the regularizer.
Definition SLACOptimizer.h:39
std::string GetSubfolderName() const
Definition SLACOptimizer.h:46
core::Device device_
Device to use.
Definition SLACOptimizer.h:42