Open3D (C++ API)  0.16.0
FillInLinearSystemImpl.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-2021 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
29#include <fstream>
30
35
36namespace open3d {
37namespace t {
38namespace pipelines {
39namespace slac {
40
41using namespace open3d::core::eigen_converter;
42using core::Tensor;
44
45// Reads pointcloud from filename, and loads on the device as
46// Tensor PointCloud of Float32 dtype.
47static PointCloud CreateTPCDFromFile(
48 const std::string& fname,
49 const core::Device& device = core::Device("CPU:0")) {
50 std::shared_ptr<open3d::geometry::PointCloud> pcd =
52 return PointCloud::FromLegacy(*pcd, core::Float32, device);
53}
54
55static void FillInRigidAlignmentTerm(Tensor& AtA,
56 Tensor& Atb,
57 Tensor& residual,
58 PointCloud& tpcd_i,
59 PointCloud& tpcd_j,
60 const Tensor& Ti,
61 const Tensor& Tj,
62 const int i,
63 const int j,
64 const float threshold) {
65 tpcd_i.Transform(Ti);
66 tpcd_j.Transform(Tj);
67
68 kernel::FillInRigidAlignmentTerm(AtA, Atb, residual,
69 tpcd_i.GetPointPositions(),
70 tpcd_j.GetPointPositions(),
71 tpcd_i.GetPointNormals(), i, j, threshold);
72}
73
74void FillInRigidAlignmentTerm(Tensor& AtA,
75 Tensor& Atb,
76 Tensor& residual,
77 const std::vector<std::string>& fnames,
78 const PoseGraph& pose_graph,
79 const SLACOptimizerParams& params,
80 const SLACDebugOption& debug_option) {
81 core::Device device(params.device_);
82
83 // Enumerate pose graph edges
84 for (auto& edge : pose_graph.edges_) {
85 int i = edge.source_node_id_;
86 int j = edge.target_node_id_;
87
88 std::string corres_fname = fmt::format("{}/{:03d}_{:03d}.npy",
89 params.GetSubfolderName(), i, j);
90 if (!utility::filesystem::FileExists(corres_fname)) {
91 utility::LogWarning("Correspondence {} {} skipped!", i, j);
92 continue;
93 }
94 Tensor corres_ij = Tensor::Load(corres_fname).To(device);
95 PointCloud tpcd_i = CreateTPCDFromFile(fnames[i], device);
96 PointCloud tpcd_j = CreateTPCDFromFile(fnames[j], device);
97
98 PointCloud tpcd_i_indexed(
99 tpcd_i.GetPointPositions().IndexGet({corres_ij.T()[0]}));
100 tpcd_i_indexed.SetPointNormals(
101 tpcd_i.GetPointNormals().IndexGet({corres_ij.T()[0]}));
102 PointCloud tpcd_j_indexed(
103 tpcd_j.GetPointPositions().IndexGet({corres_ij.T()[1]}));
104
105 Tensor Ti = EigenMatrixToTensor(pose_graph.nodes_[i].pose_)
106 .To(device, core::Float32);
107 Tensor Tj = EigenMatrixToTensor(pose_graph.nodes_[j].pose_)
108 .To(device, core::Float32);
109
110 FillInRigidAlignmentTerm(AtA, Atb, residual, tpcd_i_indexed,
111 tpcd_j_indexed, Ti, Tj, i, j,
112 params.distance_threshold_);
113
114 if (debug_option.debug_ && i >= debug_option.debug_start_node_idx_) {
115 VisualizePointCloudCorrespondences(tpcd_i, tpcd_j, corres_ij,
116 Tj.Inverse().Matmul(Ti));
117 }
118 }
119}
120
121static void FillInSLACAlignmentTerm(Tensor& AtA,
122 Tensor& Atb,
123 Tensor& residual,
124 ControlGrid& ctr_grid,
125 const PointCloud& tpcd_param_i,
126 const PointCloud& tpcd_param_j,
127 const Tensor& Ti,
128 const Tensor& Tj,
129 const int i,
130 const int j,
131 const int n_fragments,
132 const float threshold) {
133 // Parameterize: setup point cloud -> cgrid correspondences
134 Tensor cgrid_index_ps =
135 tpcd_param_i.GetPointAttr(ControlGrid::kGrid8NbIndices);
136 Tensor cgrid_ratio_ps =
137 tpcd_param_i.GetPointAttr(ControlGrid::kGrid8NbVertexInterpRatios);
138
139 Tensor cgrid_index_qs =
140 tpcd_param_j.GetPointAttr(ControlGrid::kGrid8NbIndices);
141 Tensor cgrid_ratio_qs =
142 tpcd_param_j.GetPointAttr(ControlGrid::kGrid8NbVertexInterpRatios);
143
144 // Deform with control grids
145 PointCloud tpcd_nonrigid_i = ctr_grid.Deform(tpcd_param_i);
146 PointCloud tpcd_nonrigid_j = ctr_grid.Deform(tpcd_param_j);
147
148 Tensor Cps = tpcd_nonrigid_i.GetPointPositions();
149 Tensor Cqs = tpcd_nonrigid_j.GetPointPositions();
150 Tensor Cnormal_ps = tpcd_nonrigid_i.GetPointNormals();
151
152 Tensor Ri = Ti.Slice(0, 0, 3).Slice(1, 0, 3);
153 Tensor ti = Ti.Slice(0, 0, 3).Slice(1, 3, 4);
154
155 Tensor Rj = Tj.Slice(0, 0, 3).Slice(1, 0, 3);
156 Tensor tj = Tj.Slice(0, 0, 3).Slice(1, 3, 4);
157
158 // Transform for required entries
159 Tensor Ti_Cps = (Ri.Matmul(Cps.T())).Add_(ti).T().Contiguous();
160 Tensor Tj_Cqs = (Rj.Matmul(Cqs.T())).Add_(tj).T().Contiguous();
161 Tensor Ri_Cnormal_ps = (Ri.Matmul(Cnormal_ps.T())).T().Contiguous();
162 Tensor RjT_Ri_Cnormal_ps =
163 (Rj.T().Matmul(Ri_Cnormal_ps.T())).T().Contiguous();
164
166 AtA, Atb, residual, Ti_Cps, Tj_Cqs, Cnormal_ps, Ri_Cnormal_ps,
167 RjT_Ri_Cnormal_ps, cgrid_index_ps, cgrid_index_qs, cgrid_ratio_ps,
168 cgrid_ratio_qs, i, j, n_fragments, threshold);
169}
170
171void FillInSLACAlignmentTerm(Tensor& AtA,
172 Tensor& Atb,
173 Tensor& residual,
174 ControlGrid& ctr_grid,
175 const std::vector<std::string>& fnames,
176 const PoseGraph& pose_graph,
177 const SLACOptimizerParams& params,
178 const SLACDebugOption& debug_option) {
179 core::Device device(params.device_);
180 int n_frags = pose_graph.nodes_.size();
181
182 // Enumerate pose graph edges.
183 for (auto& edge : pose_graph.edges_) {
184 int i = edge.source_node_id_;
185 int j = edge.target_node_id_;
186
187 std::string corres_fname = fmt::format("{}/{:03d}_{:03d}.npy",
188 params.GetSubfolderName(), i, j);
189 if (!utility::filesystem::FileExists(corres_fname)) {
190 utility::LogWarning("Correspondence {} {} skipped!", i, j);
191 continue;
192 }
193 Tensor corres_ij = Tensor::Load(corres_fname).To(device);
194
195 PointCloud tpcd_i = CreateTPCDFromFile(fnames[i], device);
196 PointCloud tpcd_j = CreateTPCDFromFile(fnames[j], device);
197
198 PointCloud tpcd_i_indexed(
199 tpcd_i.GetPointPositions().IndexGet({corres_ij.T()[0]}));
200 tpcd_i_indexed.SetPointNormals(
201 tpcd_i.GetPointNormals().IndexGet({corres_ij.T()[0]}));
202
203 PointCloud tpcd_j_indexed(
204 tpcd_j.GetPointPositions().IndexGet({corres_ij.T()[1]}));
205 tpcd_j_indexed.SetPointNormals(
206 tpcd_j.GetPointNormals().IndexGet({corres_ij.T()[1]}));
207
208 // Parameterize points in the control grid.
209 PointCloud tpcd_param_i = ctr_grid.Parameterize(tpcd_i_indexed);
210 PointCloud tpcd_param_j = ctr_grid.Parameterize(tpcd_j_indexed);
211
212 // Load poses.
213 auto Ti = EigenMatrixToTensor(pose_graph.nodes_[i].pose_)
214 .To(device, core::Float32);
215 auto Tj = EigenMatrixToTensor(pose_graph.nodes_[j].pose_)
216 .To(device, core::Float32);
217 auto Tij = EigenMatrixToTensor(edge.transformation_)
218 .To(device, core::Float32);
219
220 // Fill In.
221 FillInSLACAlignmentTerm(AtA, Atb, residual, ctr_grid, tpcd_param_i,
222 tpcd_param_j, Ti, Tj, i, j, n_frags,
223 params.distance_threshold_);
224
225 if (debug_option.debug_ && i >= debug_option.debug_start_node_idx_) {
226 VisualizePointCloudCorrespondences(tpcd_i, tpcd_j, corres_ij,
227 Tj.Inverse().Matmul(Ti));
228 VisualizePointCloudEmbedding(tpcd_param_i, ctr_grid);
229 VisualizePointCloudDeformation(tpcd_param_i, ctr_grid);
230 }
231 }
232}
233
235 Tensor& Atb,
236 Tensor& residual,
237 ControlGrid& ctr_grid,
238 int n_frags,
239 const SLACOptimizerParams& params,
240 const SLACDebugOption& debug_option) {
241 Tensor active_buf_indices, nb_buf_indices, nb_masks;
242 std::tie(active_buf_indices, nb_buf_indices, nb_masks) =
243 ctr_grid.GetNeighborGridMap();
244
245 Tensor positions_init = ctr_grid.GetInitPositions();
246 Tensor positions_curr = ctr_grid.GetCurrPositions();
247 kernel::FillInSLACRegularizerTerm(AtA, Atb, residual, active_buf_indices,
248 nb_buf_indices, nb_masks, positions_init,
249 positions_curr,
250 n_frags * params.regularizer_weight_,
251 n_frags, ctr_grid.GetAnchorIdx());
252 if (debug_option.debug_) {
253 VisualizeGridDeformation(ctr_grid);
254 }
255}
256
257} // namespace slac
258} // namespace pipelines
259} // namespace t
260} // namespace open3d
filament::Texture::InternalFormat format
Definition: FilamentResourceManager.cpp:214
#define LogWarning(...)
Definition: Logging.h:79
Definition: Device.h:37
Definition: Tensor.h:51
Tensor Inverse() const
Definition: Tensor.cpp:1910
Tensor Add_(const Tensor &value)
Definition: Tensor.cpp:1066
Tensor Slice(int64_t dim, int64_t start, int64_t stop, int64_t step=1) const
Definition: Tensor.cpp:843
Tensor T() const
Expects input to be <= 2-D Tensor by swapping dimension 0 and 1.
Definition: Tensor.cpp:1028
Tensor Contiguous() const
Definition: Tensor.cpp:758
static Tensor Load(const std::string &file_name)
Load tensor from numpy's npy format.
Definition: Tensor.cpp:1809
Tensor Matmul(const Tensor &rhs) const
Definition: Tensor.cpp:1847
Tensor To(Dtype dtype, bool copy=false) const
Definition: Tensor.cpp:725
Tensor IndexGet(const std::vector< Tensor > &index_tensors) const
Advanced indexing getter. This will always allocate a new Tensor.
Definition: Tensor.cpp:891
A point cloud consists of point coordinates, and optionally point colors and point normals.
Definition: PointCloud.h:55
PointCloud & Transform(const Eigen::Matrix4d &transformation) override
Apply transformation (4x4 matrix) to the geometry coordinates.
Definition: PointCloud.cpp:74
Data structure defining the pose graph.
Definition: PoseGraph.h:115
std::vector< PoseGraphNode > nodes_
List of PoseGraphNode.
Definition: PoseGraph.h:127
std::vector< PoseGraphEdge > edges_
List of PoseGraphEdge.
Definition: PoseGraph.h:129
A point cloud contains a list of 3D points.
Definition: PointCloud.h:99
core::Tensor & GetPointNormals()
Get the value of the "normals" attribute. Convenience function.
Definition: PointCloud.h:147
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:886
core::Tensor & GetPointPositions()
Get the value of the "positions" attribute. Convenience function.
Definition: PointCloud.h:141
void SetPointNormals(const core::Tensor &value)
Set the value of the "normals" attribute. Convenience function.
Definition: PointCloud.h:197
Definition: ControlGrid.h:49
int64_t GetAnchorIdx()
Definition: ControlGrid.h:135
static const std::string kGrid8NbIndices
Definition: ControlGrid.h:53
static const std::string kGrid8NbVertexInterpRatios
8 neighbor grid interpolation ratio for vertex per point.
Definition: ControlGrid.h:55
core::Tensor GetInitPositions()
Get control grid original positions directly from tensor keys.
Definition: ControlGrid.h:123
core::Tensor GetCurrPositions()
Definition: ControlGrid.h:129
std::tuple< core::Tensor, core::Tensor, core::Tensor > GetNeighborGridMap()
Definition: ControlGrid.cpp:134
geometry::PointCloud Parameterize(const geometry::PointCloud &pcd)
Definition: ControlGrid.cpp:169
Definition: EigenConverter.cpp:37
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:48
const Dtype Float32
Definition: Dtype.cpp:61
std::shared_ptr< geometry::PointCloud > CreatePointCloudFromFile(const std::string &filename, const std::string &format, bool print_progress)
Definition: PointCloudIO.cpp:68
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:149
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:86
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:36
void VisualizePointCloudDeformation(const geometry::PointCloud &tpcd_param, ControlGrid &ctr_grid)
Definition: Visualization.cpp:158
void VisualizeGridDeformation(ControlGrid &cgrid)
Definition: Visualization.cpp:206
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:65
void VisualizePointCloudEmbedding(t::geometry::PointCloud &tpcd_param, ControlGrid &ctr_grid, bool show_lines)
Definition: Visualization.cpp:99
void FillInSLACRegularizerTerm(Tensor &AtA, Tensor &Atb, Tensor &residual, ControlGrid &ctr_grid, int n_frags, const SLACOptimizerParams &params, const SLACDebugOption &debug_option)
Definition: FillInLinearSystemImpl.h:234
bool FileExists(const std::string &filename)
Definition: FileSystem.cpp:288
Definition: PinholeCameraIntrinsic.cpp:35
Definition: SLACOptimizer.h:109
bool debug_
Enable debug.
Definition: SLACOptimizer.h:111
int debug_start_node_idx_
Definition: SLACOptimizer.h:115
float distance_threshold_
Distance threshold to filter inconsistent correspondences.
Definition: SLACOptimizer.h:52
float regularizer_weight_
Weight of the regularizer.
Definition: SLACOptimizer.h:58
std::string GetSubfolderName() const
Definition: SLACOptimizer.h:65
core::Device device_
Device to use.
Definition: SLACOptimizer.h:61