Skip to content

Commit fcc396e

Browse files
nicolaloibenjaminum
nicolaloi
and
benjaminum
authored
Unified cloud initializer pipeline for ICP (fixes segfault colored ICP) (#6942)
* Unified cloud initializer pipeline for ICP (fixes segfault colored ICP) * Update CHANGELOG --------- Co-authored-by: Benjamin Ummenhofer <[email protected]>
1 parent 2ae042a commit fcc396e

8 files changed

+152
-43
lines changed

‎CHANGELOG.md

+1
Original file line numberDiff line numberDiff line change
@@ -47,6 +47,7 @@
4747
- Fix projection of point cloud to Depth/RGBD image if no position attribute is provided (PR #6880)
4848
- Support lowercase types when reading PCD files (PR #6930)
4949
- Fix visualization/draw ICP example and add warnings (PR #6933)
50+
- Unified cloud initializer pipeline for ICP (fixes segfault colored ICP) (PR #6942)
5051
- Fix tensor EstimatePointWiseNormalsWithFastEigen3x3 (PR #6980)
5152
- Fix alpha shape reconstruction if alpha too small for point scale (PR #6998)
5253
- Fix render to depth image on Apple Retina displays (PR #7001)

‎cpp/open3d/pipelines/registration/ColoredICP.cpp

+30-16
Original file line numberDiff line numberDiff line change
@@ -211,15 +211,12 @@ double TransformationEstimationForColoredICP::ComputeRMSE(
211211
return residual;
212212
};
213213

214-
RegistrationResult RegistrationColoredICP(
214+
std::tuple<std::shared_ptr<const geometry::PointCloud>,
215+
std::shared_ptr<const geometry::PointCloud>>
216+
TransformationEstimationForColoredICP::InitializePointCloudsForTransformation(
215217
const geometry::PointCloud &source,
216218
const geometry::PointCloud &target,
217-
double max_distance,
218-
const Eigen::Matrix4d &init /* = Eigen::Matrix4d::Identity()*/,
219-
const TransformationEstimationForColoredICP &estimation
220-
/* = TransformationEstimationForColoredICP()*/,
221-
const ICPConvergenceCriteria
222-
&criteria /* = ICPConvergenceCriteria()*/) {
219+
double max_correspondence_distance) const {
223220
if (!target.HasNormals()) {
224221
utility::LogError(
225222
"ColoredICP requires target pointcloud to have normals.");
@@ -232,17 +229,34 @@ RegistrationResult RegistrationColoredICP(
232229
utility::LogError(
233230
"ColoredICP requires source pointcloud to have colors.");
234231
}
235-
236-
if (auto target_c = InitializePointCloudForColoredICP(
237-
target,
238-
geometry::KDTreeSearchParamHybrid(max_distance * 2.0, 30))) {
239-
return RegistrationICP(source, *target_c, max_distance, init,
240-
estimation, criteria);
241-
} else {
232+
std::shared_ptr<const geometry::PointCloud> source_initialized_c(
233+
&source, [](const geometry::PointCloud *) {});
234+
std::shared_ptr<geometry::PointCloud> target_initialized_c(
235+
InitializePointCloudForColoredICP(
236+
target, geometry::KDTreeSearchParamHybrid(
237+
max_correspondence_distance * 2.0, 30)));
238+
if (!source_initialized_c || !target_initialized_c) {
242239
utility::LogError(
243-
"Internal error: InitializePointCloudForColoredICP returns "
240+
"Internal error: InitializePointCloudsForTransformation "
241+
"returns "
244242
"nullptr.");
245-
};
243+
}
244+
return std::make_tuple(source_initialized_c,
245+
std::const_pointer_cast<const geometry::PointCloud>(
246+
target_initialized_c));
247+
}
248+
249+
RegistrationResult RegistrationColoredICP(
250+
const geometry::PointCloud &source,
251+
const geometry::PointCloud &target,
252+
double max_distance,
253+
const Eigen::Matrix4d &init /* = Eigen::Matrix4d::Identity()*/,
254+
const TransformationEstimationForColoredICP &estimation
255+
/* = TransformationEstimationForColoredICP()*/,
256+
const ICPConvergenceCriteria
257+
&criteria /* = ICPConvergenceCriteria()*/) {
258+
return RegistrationICP(source, target, max_distance, init, estimation,
259+
criteria);
246260
}
247261

248262
} // namespace registration

‎cpp/open3d/pipelines/registration/ColoredICP.h

+7
Original file line numberDiff line numberDiff line change
@@ -51,6 +51,13 @@ class TransformationEstimationForColoredICP : public TransformationEstimation {
5151
const geometry::PointCloud &target,
5252
const CorrespondenceSet &corres) const override;
5353

54+
std::tuple<std::shared_ptr<const geometry::PointCloud>,
55+
std::shared_ptr<const geometry::PointCloud>>
56+
InitializePointCloudsForTransformation(
57+
const geometry::PointCloud &source,
58+
const geometry::PointCloud &target,
59+
double max_correspondence_distance) const override;
60+
5461
public:
5562
double lambda_geometric_ = 0.968;
5663
/// shared_ptr to an Abstract RobustKernel that could mutate at runtime.

‎cpp/open3d/pipelines/registration/GeneralizedICP.cpp

+25-4
Original file line numberDiff line numberDiff line change
@@ -147,6 +147,29 @@ TransformationEstimationForGeneralizedICP::ComputeTransformation(
147147
return is_success ? extrinsic : Eigen::Matrix4d::Identity();
148148
}
149149

150+
std::tuple<std::shared_ptr<const geometry::PointCloud>,
151+
std::shared_ptr<const geometry::PointCloud>>
152+
TransformationEstimationForGeneralizedICP::
153+
InitializePointCloudsForTransformation(
154+
const geometry::PointCloud &source,
155+
const geometry::PointCloud &target,
156+
double max_correspondence_distance) const {
157+
std::shared_ptr<geometry::PointCloud> source_initialized_c =
158+
InitializePointCloudForGeneralizedICP(source, epsilon_);
159+
std::shared_ptr<geometry::PointCloud> target_initialized_c =
160+
InitializePointCloudForGeneralizedICP(target, epsilon_);
161+
if (!source_initialized_c || !target_initialized_c) {
162+
utility::LogError(
163+
"Internal error: InitializePointCloudsForTransformation "
164+
"returns "
165+
"nullptr.");
166+
}
167+
return std::make_pair(std::const_pointer_cast<const geometry::PointCloud>(
168+
source_initialized_c),
169+
std::const_pointer_cast<const geometry::PointCloud>(
170+
target_initialized_c));
171+
}
172+
150173
RegistrationResult RegistrationGeneralizedICP(
151174
const geometry::PointCloud &source,
152175
const geometry::PointCloud &target,
@@ -156,10 +179,8 @@ RegistrationResult RegistrationGeneralizedICP(
156179
&estimation /* = TransformationEstimationForGeneralizedICP()*/,
157180
const ICPConvergenceCriteria
158181
&criteria /* = ICPConvergenceCriteria()*/) {
159-
return RegistrationICP(
160-
*InitializePointCloudForGeneralizedICP(source, estimation.epsilon_),
161-
*InitializePointCloudForGeneralizedICP(target, estimation.epsilon_),
162-
max_correspondence_distance, init, estimation, criteria);
182+
return RegistrationICP(source, target, max_correspondence_distance, init,
183+
estimation, criteria);
163184
}
164185

165186
} // namespace registration

‎cpp/open3d/pipelines/registration/GeneralizedICP.h

+7
Original file line numberDiff line numberDiff line change
@@ -51,6 +51,13 @@ class TransformationEstimationForGeneralizedICP
5151
const geometry::PointCloud &target,
5252
const CorrespondenceSet &corres) const override;
5353

54+
std::tuple<std::shared_ptr<const geometry::PointCloud>,
55+
std::shared_ptr<const geometry::PointCloud>>
56+
InitializePointCloudsForTransformation(
57+
const geometry::PointCloud &source,
58+
const geometry::PointCloud &target,
59+
double max_correspondence_distance) const override;
60+
5461
public:
5562
/// Small constant representing covariance along the normal.
5663
double epsilon_ = 1e-3;

‎cpp/open3d/pipelines/registration/Registration.cpp

+12-23
Original file line numberDiff line numberDiff line change
@@ -117,45 +117,34 @@ RegistrationResult RegistrationICP(
117117
if (max_correspondence_distance <= 0.0) {
118118
utility::LogError("Invalid max_correspondence_distance.");
119119
}
120-
if ((estimation.GetTransformationEstimationType() ==
121-
TransformationEstimationType::PointToPlane ||
122-
estimation.GetTransformationEstimationType() ==
123-
TransformationEstimationType::ColoredICP) &&
124-
(!target.HasNormals())) {
125-
utility::LogError(
126-
"TransformationEstimationPointToPlane and "
127-
"TransformationEstimationColoredICP "
128-
"require pre-computed normal vectors for target PointCloud.");
129-
}
130-
if ((estimation.GetTransformationEstimationType() ==
131-
TransformationEstimationType::GeneralizedICP) &&
132-
(!target.HasCovariances() || !source.HasCovariances())) {
133-
utility::LogError(
134-
"TransformationEstimationForGeneralizedICP require "
135-
"pre-computed per point covariances matrices for source and "
136-
"target PointCloud.");
137-
}
120+
121+
auto [source_initialized_c, target_initialized_c] =
122+
estimation.InitializePointCloudsForTransformation(
123+
source, target, max_correspondence_distance);
138124

139125
Eigen::Matrix4d transformation = init;
140126
geometry::KDTreeFlann kdtree;
141-
kdtree.SetGeometry(target);
142-
geometry::PointCloud pcd = source;
127+
const geometry::PointCloud &target_initialized = *target_initialized_c;
128+
kdtree.SetGeometry(target_initialized);
129+
geometry::PointCloud pcd(*source_initialized_c);
130+
143131
if (!init.isIdentity()) {
144132
pcd.Transform(init);
145133
}
146134
RegistrationResult result;
147135
result = GetRegistrationResultAndCorrespondences(
148-
pcd, target, kdtree, max_correspondence_distance, transformation);
136+
pcd, target_initialized, kdtree, max_correspondence_distance,
137+
transformation);
149138
for (int i = 0; i < criteria.max_iteration_; i++) {
150139
utility::LogDebug("ICP Iteration #{:d}: Fitness {:.4f}, RMSE {:.4f}", i,
151140
result.fitness_, result.inlier_rmse_);
152141
Eigen::Matrix4d update = estimation.ComputeTransformation(
153-
pcd, target, result.correspondence_set_);
142+
pcd, target_initialized, result.correspondence_set_);
154143
transformation = update * transformation;
155144
pcd.Transform(update);
156145
RegistrationResult backup = result;
157146
result = GetRegistrationResultAndCorrespondences(
158-
pcd, target, kdtree, max_correspondence_distance,
147+
pcd, target_initialized, kdtree, max_correspondence_distance,
159148
transformation);
160149
if (std::abs(backup.fitness_ - result.fitness_) <
161150
criteria.relative_fitness_ &&

‎cpp/open3d/pipelines/registration/TransformationEstimation.cpp

+43
Original file line numberDiff line numberDiff line change
@@ -11,6 +11,7 @@
1111

1212
#include "open3d/geometry/PointCloud.h"
1313
#include "open3d/utility/Eigen.h"
14+
#include "open3d/utility/Logging.h"
1415

1516
namespace open3d {
1617
namespace pipelines {
@@ -42,6 +43,25 @@ Eigen::Matrix4d TransformationEstimationPointToPoint::ComputeTransformation(
4243
return Eigen::umeyama(source_mat, target_mat, with_scaling_);
4344
}
4445

46+
std::tuple<std::shared_ptr<const geometry::PointCloud>,
47+
std::shared_ptr<const geometry::PointCloud>>
48+
TransformationEstimationPointToPoint::InitializePointCloudsForTransformation(
49+
const geometry::PointCloud &source,
50+
const geometry::PointCloud &target,
51+
double max_correspondence_distance) const {
52+
std::shared_ptr<const geometry::PointCloud> source_initialized_c(
53+
&source, [](const geometry::PointCloud *) {});
54+
std::shared_ptr<const geometry::PointCloud> target_initialized_c(
55+
&target, [](const geometry::PointCloud *) {});
56+
if (!source_initialized_c || !target_initialized_c) {
57+
utility::LogError(
58+
"Internal error: InitializePointCloudsForTransformation "
59+
"returns "
60+
"nullptr.");
61+
}
62+
return std::make_tuple(source_initialized_c, target_initialized_c);
63+
}
64+
4565
double TransformationEstimationPointToPlane::ComputeRMSE(
4666
const geometry::PointCloud &source,
4767
const geometry::PointCloud &target,
@@ -89,6 +109,29 @@ Eigen::Matrix4d TransformationEstimationPointToPlane::ComputeTransformation(
89109
return is_success ? extrinsic : Eigen::Matrix4d::Identity();
90110
}
91111

112+
std::tuple<std::shared_ptr<const geometry::PointCloud>,
113+
std::shared_ptr<const geometry::PointCloud>>
114+
TransformationEstimationPointToPlane::InitializePointCloudsForTransformation(
115+
const geometry::PointCloud &source,
116+
const geometry::PointCloud &target,
117+
double max_correspondence_distance) const {
118+
if (!target.HasNormals()) {
119+
utility::LogError(
120+
"PointToPlaneICP requires target pointcloud to have normals.");
121+
}
122+
std::shared_ptr<const geometry::PointCloud> source_initialized_c(
123+
&source, [](const geometry::PointCloud *) {});
124+
std::shared_ptr<const geometry::PointCloud> target_initialized_c(
125+
&target, [](const geometry::PointCloud *) {});
126+
if (!source_initialized_c || !target_initialized_c) {
127+
utility::LogError(
128+
"Internal error: InitializePointCloudsForTransformation "
129+
"returns "
130+
"nullptr.");
131+
}
132+
return std::make_tuple(source_initialized_c, target_initialized_c);
133+
}
134+
92135
} // namespace registration
93136
} // namespace pipelines
94137
} // namespace open3d

‎cpp/open3d/pipelines/registration/TransformationEstimation.h

+27
Original file line numberDiff line numberDiff line change
@@ -67,6 +67,19 @@ class TransformationEstimation {
6767
const geometry::PointCloud &source,
6868
const geometry::PointCloud &target,
6969
const CorrespondenceSet &corres) const = 0;
70+
71+
/// Initialize the source and target point cloud for the transformation
72+
/// estimation.
73+
///
74+
/// \param source Source point cloud.
75+
/// \param target Target point cloud.
76+
/// \param max_correspondence_distance Maximum correspondence distance.
77+
virtual std::tuple<std::shared_ptr<const geometry::PointCloud>,
78+
std::shared_ptr<const geometry::PointCloud>>
79+
InitializePointCloudsForTransformation(
80+
const geometry::PointCloud &source,
81+
const geometry::PointCloud &target,
82+
double max_correspondence_distance) const = 0;
7083
};
7184

7285
/// \class TransformationEstimationPointToPoint
@@ -95,6 +108,13 @@ class TransformationEstimationPointToPoint : public TransformationEstimation {
95108
const geometry::PointCloud &target,
96109
const CorrespondenceSet &corres) const override;
97110

111+
std::tuple<std::shared_ptr<const geometry::PointCloud>,
112+
std::shared_ptr<const geometry::PointCloud>>
113+
InitializePointCloudsForTransformation(
114+
const geometry::PointCloud &source,
115+
const geometry::PointCloud &target,
116+
double max_correspondence_distance) const override;
117+
98118
public:
99119
/// \brief Set to True to estimate scaling, False to force scaling to be 1.
100120
///
@@ -137,6 +157,13 @@ class TransformationEstimationPointToPlane : public TransformationEstimation {
137157
const geometry::PointCloud &target,
138158
const CorrespondenceSet &corres) const override;
139159

160+
std::tuple<std::shared_ptr<const geometry::PointCloud>,
161+
std::shared_ptr<const geometry::PointCloud>>
162+
InitializePointCloudsForTransformation(
163+
const geometry::PointCloud &source,
164+
const geometry::PointCloud &target,
165+
double max_correspondence_distance) const override;
166+
140167
public:
141168
/// shared_ptr to an Abstract RobustKernel that could mutate at runtime.
142169
std::shared_ptr<RobustKernel> kernel_ = std::make_shared<L2Loss>();

0 commit comments

Comments
 (0)