Open3D (C++ API)  0.19.0
Loading...
Searching...
No Matches
TransformationEstimation.h
Go to the documentation of this file.
1// ----------------------------------------------------------------------------
2// - Open3D: www.open3d.org -
3// ----------------------------------------------------------------------------
4// Copyright (c) 2018-2024 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"
21
22namespace open3d {
23
24namespace t {
25namespace geometry {
26class PointCloud;
27}
28
29namespace pipelines {
30namespace registration {
31
32namespace {
33
34// Minimum time period (sec) between two sequential scans for Doppler ICP.
35constexpr 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
53public:
57
58public:
60 const = 0;
61
71 virtual double ComputeRMSE(const geometry::PointCloud &source,
73 const core::Tensor &correspondences) const = 0;
88 const geometry::PointCloud &source,
90 const core::Tensor &correspondences,
91 const core::Tensor &current_transform =
93 const std::size_t iteration = 0) const = 0;
94};
95
101public:
102 // TODO: support with_scaling.
105
106public:
108 const override {
109 return type_;
110 };
120 double ComputeRMSE(const geometry::PointCloud &source,
122 const core::Tensor &correspondences) const override;
123
138 const geometry::PointCloud &source,
140 const core::Tensor &correspondences,
141 const core::Tensor &current_transform =
143 const std::size_t iteration = 0) const override;
144
145private:
146 const TransformationEstimationType type_ =
148};
149
155public:
159
165 : kernel_(kernel) {}
166
167public:
169 const override {
170 return type_;
171 };
172
183 double ComputeRMSE(const geometry::PointCloud &source,
185 const core::Tensor &correspondences) const override;
186
202 const geometry::PointCloud &source,
204 const core::Tensor &correspondences,
205 const core::Tensor &current_transform =
207 const std::size_t iteration = 0) const override;
208
209public:
212
213private:
214 const TransformationEstimationType type_ =
216};
217
227public:
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
251public:
263 double ComputeRMSE(const geometry::PointCloud &source,
265 const core::Tensor &correspondences) const override;
266
283 const geometry::PointCloud &source,
285 const core::Tensor &correspondences,
286 const core::Tensor &current_transform =
288 const std::size_t iteration = 0) const override;
289
290public:
291 double lambda_geometric_ = 0.968;
294
295private:
296 const TransformationEstimationType type_ =
298};
299
309public:
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
376public:
388 double ComputeRMSE(const geometry::PointCloud &source,
390 const core::Tensor &correspondences) const override;
391
408 const geometry::PointCloud &source,
410 const core::Tensor &correspondences,
411 const core::Tensor &current_transform =
413 const std::size_t iteration = 0) const override;
414
415public:
417 double period_{0.1};
419 double lambda_doppler_{0.01};
439
440private:
441 const TransformationEstimationType type_ =
443};
444
445} // namespace registration
446} // namespace pipelines
447} // namespace t
448} // namespace open3d
double t
Definition SurfaceReconstructionPoisson.cpp:172
Real target
Definition SurfaceReconstructionPoisson.cpp:267
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:81
~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
TransformationEstimationType
Definition TransformationEstimation.h:39
Definition PinholeCameraIntrinsic.cpp:16