Open3D (C++ API)  0.18.0+252c867
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 
17 namespace open3d {
18 namespace t {
19 namespace pipelines {
20 namespace slac {
21 
22 using namespace open3d::core::eigen_converter;
23 using core::Tensor;
24 using t::geometry::PointCloud;
25 
26 // Reads pointcloud from filename, and loads on the device as
27 // Tensor PointCloud of Float32 dtype.
28 static 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 
36 static 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 
55 void 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 
102 static 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 
152 void 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
filament::Texture::InternalFormat format
Definition: FilamentResourceManager.cpp:195
#define LogWarning(...)
Definition: Logging.h:60
Definition: Device.h:18
Definition: Tensor.h:32
Tensor Inverse() const
Definition: Tensor.cpp:1929
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
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
core::Tensor & GetPointPositions()
Get the value of the "positions" attribute. Convenience function.
Definition: PointCloud.h:122
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
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