Open3D (C++ API)  0.18.0+252c867
TransformationEstimation.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 <cmath>
11 #include <memory>
12 #include <string>
13 #include <utility>
14 #include <vector>
15 
16 #include "open3d/core/Tensor.h"
20 #include "open3d/utility/Logging.h"
21 
22 namespace open3d {
23 
24 namespace t {
25 namespace geometry {
26 class PointCloud;
27 }
28 
29 namespace pipelines {
30 namespace registration {
31 
32 namespace {
33 
34 // Minimum time period (sec) between two sequential scans for Doppler ICP.
35 constexpr double kMinTimePeriod{1e-3};
36 
37 } // namespace
38 
40  Unspecified = 0,
41  PointToPoint = 1,
42  PointToPlane = 2,
43  ColoredICP = 3,
44  DopplerICP = 4,
45 };
46 
53 public:
57 
58 public:
60  const = 0;
61 
71  virtual double ComputeRMSE(const geometry::PointCloud &source,
72  const geometry::PointCloud &target,
73  const core::Tensor &correspondences) const = 0;
88  const geometry::PointCloud &source,
89  const geometry::PointCloud &target,
90  const core::Tensor &correspondences,
91  const core::Tensor &current_transform =
93  const std::size_t iteration = 0) const = 0;
94 };
95 
101 public:
102  // TODO: support with_scaling.
105 
106 public:
108  const override {
109  return type_;
110  };
120  double ComputeRMSE(const geometry::PointCloud &source,
121  const geometry::PointCloud &target,
122  const core::Tensor &correspondences) const override;
123 
138  const geometry::PointCloud &source,
139  const geometry::PointCloud &target,
140  const core::Tensor &correspondences,
141  const core::Tensor &current_transform =
143  const std::size_t iteration = 0) const override;
144 
145 private:
146  const TransformationEstimationType type_ =
148 };
149 
155 public:
159 
165  : kernel_(kernel) {}
166 
167 public:
169  const override {
170  return type_;
171  };
172 
183  double ComputeRMSE(const geometry::PointCloud &source,
184  const geometry::PointCloud &target,
185  const core::Tensor &correspondences) const override;
186 
202  const geometry::PointCloud &source,
203  const geometry::PointCloud &target,
204  const core::Tensor &correspondences,
205  const core::Tensor &current_transform =
207  const std::size_t iteration = 0) const override;
208 
209 public:
212 
213 private:
214  const TransformationEstimationType type_ =
216 };
217 
227 public:
229 
237  double lambda_geometric = 0.968,
238  const RobustKernel &kernel =
240  : lambda_geometric_(lambda_geometric), kernel_(kernel) {
241  if (lambda_geometric_ < 0 || lambda_geometric_ > 1.0) {
242  lambda_geometric_ = 0.968;
243  }
244  }
245 
247  const override {
248  return type_;
249  };
250 
251 public:
263  double ComputeRMSE(const geometry::PointCloud &source,
264  const geometry::PointCloud &target,
265  const core::Tensor &correspondences) const override;
266 
283  const geometry::PointCloud &source,
284  const geometry::PointCloud &target,
285  const core::Tensor &correspondences,
286  const core::Tensor &current_transform =
288  const std::size_t iteration = 0) const override;
289 
290 public:
291  double lambda_geometric_ = 0.968;
294 
295 private:
296  const TransformationEstimationType type_ =
298 };
299 
309 public:
311 
336  const double period = 0.1,
337  const double lambda_doppler = 0.01,
338  const bool reject_dynamic_outliers = false,
339  const double doppler_outlier_threshold = 2.0,
340  const std::size_t outlier_rejection_min_iteration = 2,
341  const std::size_t geometric_robust_loss_min_iteration = 0,
342  const std::size_t doppler_robust_loss_min_iteration = 2,
343  const RobustKernel &geometric_kernel =
345  const RobustKernel &doppler_kernel =
347  const core::Tensor &transform_vehicle_to_sensor =
349  : period_(period),
350  lambda_doppler_(lambda_doppler),
351  reject_dynamic_outliers_(reject_dynamic_outliers),
352  doppler_outlier_threshold_(doppler_outlier_threshold),
353  outlier_rejection_min_iteration_(outlier_rejection_min_iteration),
355  geometric_robust_loss_min_iteration),
356  doppler_robust_loss_min_iteration_(doppler_robust_loss_min_iteration),
357  geometric_kernel_(geometric_kernel),
358  doppler_kernel_(doppler_kernel),
359  transform_vehicle_to_sensor_(transform_vehicle_to_sensor) {
360  core::AssertTensorShape(transform_vehicle_to_sensor, {4, 4});
361 
362  if (std::abs(period) < kMinTimePeriod) {
363  utility::LogError("Time period too small.");
364  }
365 
366  if (lambda_doppler_ < 0 || lambda_doppler_ > 1.0) {
367  lambda_doppler_ = 0.01;
368  }
369  }
370 
372  const override {
373  return type_;
374  };
375 
376 public:
388  double ComputeRMSE(const geometry::PointCloud &source,
389  const geometry::PointCloud &target,
390  const core::Tensor &correspondences) const override;
391 
408  const geometry::PointCloud &source,
409  const geometry::PointCloud &target,
410  const core::Tensor &correspondences,
411  const core::Tensor &current_transform =
413  const std::size_t iteration = 0) const override;
414 
415 public:
417  double period_{0.1};
419  double lambda_doppler_{0.01};
439 
440 private:
441  const TransformationEstimationType type_ =
443 };
444 
445 } // namespace registration
446 } // namespace pipelines
447 } // namespace t
448 } // namespace open3d
#define LogError(...)
Definition: Logging.h:48
#define AssertTensorShape(tensor,...)
Definition: TensorCheck.h:58
Definition: Device.h:18
Definition: Tensor.h:32
static Tensor Eye(int64_t n, Dtype dtype, const Device &device)
Create an identity matrix of size n x n.
Definition: Tensor.cpp:386
A point cloud contains a list of 3D points.
Definition: PointCloud.h:80
~TransformationEstimationForColoredICP() override
Definition: TransformationEstimation.h:228
double ComputeRMSE(const geometry::PointCloud &source, const geometry::PointCloud &target, const core::Tensor &correspondences) const override
Computes RMSE (double) for ColoredICP method, between two pointclouds, given correspondences.
Definition: TransformationEstimation.cpp:167
core::Tensor ComputeTransformation(const geometry::PointCloud &source, const geometry::PointCloud &target, const core::Tensor &correspondences, const core::Tensor &current_transform=core::Tensor::Eye(4, core::Float64, core::Device("CPU:0")), const std::size_t iteration=0) const override
Estimates the transformation matrix for ColoredICP method, a tensor of shape {4, 4},...
Definition: TransformationEstimation.cpp:255
TransformationEstimationForColoredICP(double lambda_geometric=0.968, const RobustKernel &kernel=RobustKernel(RobustKernelMethod::L2Loss, 1.0, 1.0))
Constructor.
Definition: TransformationEstimation.h:236
double lambda_geometric_
Definition: TransformationEstimation.h:291
TransformationEstimationType GetTransformationEstimationType() const override
Definition: TransformationEstimation.h:246
RobustKernel kernel_
RobustKernel for outlier rejection.
Definition: TransformationEstimation.h:293
core::Tensor transform_vehicle_to_sensor_
Definition: TransformationEstimation.h:437
std::size_t doppler_robust_loss_min_iteration_
Definition: TransformationEstimation.h:429
TransformationEstimationType GetTransformationEstimationType() const override
Definition: TransformationEstimation.h:371
double lambda_doppler_
Factor that weighs the Doppler residual term in DICP objective.
Definition: TransformationEstimation.h:419
std::size_t geometric_robust_loss_min_iteration_
Number of iterations of ICP after which robust loss kicks in.
Definition: TransformationEstimation.h:428
core::Tensor ComputeTransformation(const geometry::PointCloud &source, const geometry::PointCloud &target, const core::Tensor &correspondences, const core::Tensor &current_transform=core::Tensor::Eye(4, core::Float64, core::Device("CPU:0")), const std::size_t iteration=0) const override
Estimates the transformation matrix for DopplerICP method, a tensor of shape {4, 4},...
Definition: TransformationEstimation.cpp:342
double doppler_outlier_threshold_
Definition: TransformationEstimation.h:424
TransformationEstimationForDopplerICP(const double period=0.1, const double lambda_doppler=0.01, const bool reject_dynamic_outliers=false, const double doppler_outlier_threshold=2.0, const std::size_t outlier_rejection_min_iteration=2, const std::size_t geometric_robust_loss_min_iteration=0, const std::size_t doppler_robust_loss_min_iteration=2, const RobustKernel &geometric_kernel=RobustKernel(RobustKernelMethod::L2Loss, 1.0, 1.0), const RobustKernel &doppler_kernel=RobustKernel(RobustKernelMethod::L2Loss, 1.0, 1.0), const core::Tensor &transform_vehicle_to_sensor=core::Tensor::Eye(4, core::Float64, core::Device("CPU:0")))
Constructor.
Definition: TransformationEstimation.h:335
double period_
Time period (in seconds) between the source and the target point clouds.
Definition: TransformationEstimation.h:417
bool reject_dynamic_outliers_
Whether or not to prune dynamic point outlier correspondences.
Definition: TransformationEstimation.h:421
RobustKernel doppler_kernel_
Definition: TransformationEstimation.h:433
~TransformationEstimationForDopplerICP() override
Definition: TransformationEstimation.h:310
std::size_t outlier_rejection_min_iteration_
Number of iterations of ICP after which outlier rejection is enabled.
Definition: TransformationEstimation.h:426
double ComputeRMSE(const geometry::PointCloud &source, const geometry::PointCloud &target, const core::Tensor &correspondences) const override
Computes RMSE (double) for DopplerICP method, between two pointclouds, given correspondences.
Definition: TransformationEstimation.cpp:307
RobustKernel geometric_kernel_
RobustKernel for outlier rejection.
Definition: TransformationEstimation.h:431
Definition: TransformationEstimation.h:52
virtual ~TransformationEstimation()
Definition: TransformationEstimation.h:56
virtual TransformationEstimationType GetTransformationEstimationType() const =0
virtual core::Tensor ComputeTransformation(const geometry::PointCloud &source, const geometry::PointCloud &target, const core::Tensor &correspondences, const core::Tensor &current_transform=core::Tensor::Eye(4, core::Float64, core::Device("CPU:0")), const std::size_t iteration=0) const =0
TransformationEstimation()
Default Constructor.
Definition: TransformationEstimation.h:55
virtual double ComputeRMSE(const geometry::PointCloud &source, const geometry::PointCloud &target, const core::Tensor &correspondences) const =0
TransformationEstimationPointToPlane()
Default constructor.
Definition: TransformationEstimation.h:157
~TransformationEstimationPointToPlane() override
Definition: TransformationEstimation.h:158
TransformationEstimationType GetTransformationEstimationType() const override
Definition: TransformationEstimation.h:168
double ComputeRMSE(const geometry::PointCloud &source, const geometry::PointCloud &target, const core::Tensor &correspondences) const override
Computes RMSE (double) for PointToPlane method, between two pointclouds, given correspondences.
Definition: TransformationEstimation.cpp:99
RobustKernel kernel_
RobustKernel for outlier rejection.
Definition: TransformationEstimation.h:211
core::Tensor ComputeTransformation(const geometry::PointCloud &source, const geometry::PointCloud &target, const core::Tensor &correspondences, const core::Tensor &current_transform=core::Tensor::Eye(4, core::Float64, core::Device("CPU:0")), const std::size_t iteration=0) const override
Estimates the transformation matrix for PointToPlane method, a tensor of shape {4,...
Definition: TransformationEstimation.cpp:134
TransformationEstimationPointToPlane(const RobustKernel &kernel)
Constructor that takes as input a RobustKernel.
Definition: TransformationEstimation.h:164
TransformationEstimationPointToPoint()
Definition: TransformationEstimation.h:103
TransformationEstimationType GetTransformationEstimationType() const override
Definition: TransformationEstimation.h:107
double ComputeRMSE(const geometry::PointCloud &source, const geometry::PointCloud &target, const core::Tensor &correspondences) const override
Computes RMSE (double) for PointToPoint method, between two pointclouds, given correspondences.
Definition: TransformationEstimation.cpp:39
~TransformationEstimationPointToPoint() override
Definition: TransformationEstimation.h:104
core::Tensor ComputeTransformation(const geometry::PointCloud &source, const geometry::PointCloud &target, const core::Tensor &correspondences, const core::Tensor &current_transform=core::Tensor::Eye(4, core::Float64, core::Device("CPU:0")), const std::size_t iteration=0) const override
Estimates the transformation matrix for PointToPoint method, a tensor of shape {4,...
Definition: TransformationEstimation.cpp:70
const Dtype Float64
Definition: Dtype.cpp:43
const char const char value recording_handle imu_sample recording_handle uint8_t size_t data_size k4a_record_configuration_t config target_format k4a_capture_t capture_handle k4a_imu_sample_t imu_sample playback_handle k4a_logging_message_cb_t void min_level device_handle k4a_imu_sample_t timeout_in_ms capture_handle capture_handle capture_handle image_handle temperature_c k4a_image_t image_handle uint8_t image_handle image_handle image_handle image_handle image_handle timestamp_usec white_balance image_handle k4a_device_configuration_t config device_handle char size_t serial_number_size bool int32_t int32_t int32_t int32_t k4a_color_control_mode_t default_mode value const const k4a_calibration_t calibration char size_t
Definition: K4aPlugin.cpp:719
TransformationEstimationType
Definition: TransformationEstimation.h:39
Definition: PinholeCameraIntrinsic.cpp:16