Skip to content

Commit fc27476

Browse files
DamienGilliardDamienGilliard
committed
WIP: EvaluateRegistration static method added and default parameters of Ransac methods now exposed
1 parent fcffcb4 commit fc27476

File tree

3 files changed

+130
-93
lines changed

3 files changed

+130
-93
lines changed

‎src/diffCheck/registration/globalregistration.cc‎

Lines changed: 66 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -14,8 +14,26 @@ namespace diffCheck::registration
1414
return distances;
1515
}
1616

17+
std::vector<double> GlobalRegistration::EvaluateRegistrations(std::shared_ptr<geometry::DFPointCloud> source,
18+
std::shared_ptr<geometry::DFPointCloud> target,
19+
std::vector<Eigen::Matrix<double, 4, 4>> transforms)
20+
{
21+
std::vector<double> errors;
22+
for(int i = 0; i < transforms.size(); i++)
23+
{
24+
std::shared_ptr<open3d::geometry::PointCloud> o3DPointCloud = source->Cvt2O3DPointCloud();
25+
std::shared_ptr<open3d::geometry::PointCloud> o3DPointCloudAfterTrans = std::make_shared<open3d::geometry::PointCloud>(o3DPointCloud->Transform(transforms[i]));
26+
std::shared_ptr<geometry::DFPointCloud> dfPointCloudPtrAfterTrans = std::make_shared<geometry::DFPointCloud>();
27+
dfPointCloudPtrAfterTrans->Cvt2DFPointCloud(o3DPointCloudAfterTrans);
28+
std::vector<double> registrationErrors = ComputeP2PDistance(dfPointCloudPtrAfterTrans, target);
29+
errors.push_back(std::accumulate(registrationErrors.begin(), registrationErrors.end(), 0.0) / registrationErrors.size());
30+
}
31+
return errors;
32+
};
33+
1734
open3d::pipelines::registration::RegistrationResult GlobalRegistration::O3DFastGlobalRegistrationFeatureMatching(std::shared_ptr<geometry::DFPointCloud> source,
1835
std::shared_ptr<geometry::DFPointCloud> target,
36+
bool voxelise,
1937
double voxelSize,
2038
double radiusKDTreeSearch,
2139
int maxNeighborKDTreeSearch,
@@ -26,8 +44,11 @@ namespace diffCheck::registration
2644
std::shared_ptr<open3d::geometry::PointCloud> sourceO3D = source->Cvt2O3DPointCloud();
2745
std::shared_ptr<open3d::geometry::PointCloud> targetO3D = target->Cvt2O3DPointCloud();
2846

29-
sourceO3D->VoxelDownSample(voxelSize);
30-
targetO3D->VoxelDownSample(voxelSize);
47+
if (voxelise)
48+
{
49+
sourceO3D->VoxelDownSample(voxelSize);
50+
targetO3D->VoxelDownSample(voxelSize);
51+
}
3152

3253
std::shared_ptr<open3d::pipelines::registration::Feature> sourceFPFHFeatures = open3d::pipelines::registration::ComputeFPFHFeature(*sourceO3D,
3354
open3d::geometry::KDTreeSearchParamHybrid(radiusKDTreeSearch, maxNeighborKDTreeSearch));
@@ -49,6 +70,7 @@ namespace diffCheck::registration
4970

5071
open3d::pipelines::registration::RegistrationResult GlobalRegistration::O3DFastGlobalRegistrationBasedOnCorrespondence(std::shared_ptr<geometry::DFPointCloud> source,
5172
std::shared_ptr<geometry::DFPointCloud> target,
73+
bool voxelise,
5274
double voxelSize,
5375
double radiusKDTreeSearch,
5476
int maxNeighborKDTreeSearch,
@@ -59,8 +81,11 @@ namespace diffCheck::registration
5981
std::shared_ptr<open3d::geometry::PointCloud> sourceO3D = source->Cvt2O3DPointCloud();
6082
std::shared_ptr<open3d::geometry::PointCloud> targetO3D = target->Cvt2O3DPointCloud();
6183

62-
sourceO3D->VoxelDownSample(voxelSize);
63-
targetO3D->VoxelDownSample(voxelSize);
84+
if (voxelise)
85+
{
86+
sourceO3D->VoxelDownSample(voxelSize);
87+
targetO3D->VoxelDownSample(voxelSize);
88+
}
6489

6590
std::shared_ptr<open3d::pipelines::registration::FastGlobalRegistrationOption> option = std::make_shared<open3d::pipelines::registration::FastGlobalRegistrationOption>();
6691
option->maximum_correspondence_distance_ = maxCorrespondenceDistance;
@@ -85,23 +110,33 @@ namespace diffCheck::registration
85110

86111
open3d::pipelines::registration::RegistrationResult GlobalRegistration::O3DRansacOnCorrespondence(std::shared_ptr<geometry::DFPointCloud> source,
87112
std::shared_ptr<geometry::DFPointCloud> target,
113+
bool voxelise,
88114
double voxelSize,
89115
double radiusKDTreeSearch,
90116
int maxNeighborKDTreeSearch,
91117
double maxCorrespondenceDistance,
92-
int correspondenceSetSize)
118+
int ransacN,
119+
double correspondenceCheckerDistance ,
120+
int ransacMaxIteration,
121+
double ransacConfidenceThreshold)
93122
{
94123
std::shared_ptr<open3d::geometry::PointCloud> sourceO3D = source->Cvt2O3DPointCloud();
95124
std::shared_ptr<open3d::geometry::PointCloud> targetO3D = target->Cvt2O3DPointCloud();
96125

97-
sourceO3D->VoxelDownSample(voxelSize);
98-
targetO3D->VoxelDownSample(voxelSize);
126+
if (voxelise)
127+
{
128+
sourceO3D->VoxelDownSample(voxelSize);
129+
targetO3D->VoxelDownSample(voxelSize);
130+
}
99131

100132
std::shared_ptr<open3d::pipelines::registration::Feature> sourceFPFHFeatures = open3d::pipelines::registration::ComputeFPFHFeature(*sourceO3D,
101133
open3d::geometry::KDTreeSearchParamHybrid(radiusKDTreeSearch, maxNeighborKDTreeSearch));
102134
std::shared_ptr<open3d::pipelines::registration::Feature> targetFPFHFeatures = open3d::pipelines::registration::ComputeFPFHFeature(*targetO3D,
103135
open3d::geometry::KDTreeSearchParamHybrid(radiusKDTreeSearch, maxNeighborKDTreeSearch));
104136

137+
std::vector<std::reference_wrapper<const open3d::pipelines::registration::CorrespondenceChecker>> correspondanceChecker;
138+
open3d::pipelines::registration::CorrespondenceCheckerBasedOnDistance checkerOnDistance = open3d::pipelines::registration::CorrespondenceCheckerBasedOnDistance(correspondenceCheckerDistance);
139+
correspondanceChecker.push_back(checkerOnDistance);
105140

106141
open3d::pipelines::registration::CorrespondenceSet correspondanceset;
107142
correspondanceset = open3d::pipelines::registration::CorrespondencesFromFeatures(*sourceFPFHFeatures, *targetFPFHFeatures);
@@ -111,33 +146,52 @@ namespace diffCheck::registration
111146
correspondanceset,
112147
maxCorrespondenceDistance,
113148
open3d::pipelines::registration::TransformationEstimationPointToPoint(),
114-
correspondenceSetSize);
149+
ransacN,
150+
correspondanceChecker,
151+
open3d::pipelines::registration::RANSACConvergenceCriteria(ransacMaxIteration, ransacConfidenceThreshold));
115152
return result;
116153
}
117154

118155
open3d::pipelines::registration::RegistrationResult GlobalRegistration::O3DRansacOnFeatureMatching(std::shared_ptr<geometry::DFPointCloud> source,
119156
std::shared_ptr<geometry::DFPointCloud> target,
157+
bool voxelise,
120158
double voxelSize,
121159
double radiusKDTreeSearch,
122160
int maxNeighborKDTreeSearch,
123-
double maxCorrespondenceDistance)
161+
double maxCorrespondenceDistance,
162+
open3d::pipelines::registration::TransformationEstimationPointToPoint transformationEstimation,
163+
int ransacN,
164+
double correspondenceCheckerDistance ,
165+
int ransacMaxIteration,
166+
double ransacConfidenceThreshold)
124167
{
125168
std::shared_ptr<open3d::geometry::PointCloud> sourceO3D = source->Cvt2O3DPointCloud();
126169
std::shared_ptr<open3d::geometry::PointCloud> targetO3D = target->Cvt2O3DPointCloud();
127170

128-
sourceO3D->VoxelDownSample(voxelSize);
129-
targetO3D->VoxelDownSample(voxelSize);
171+
if (voxelise)
172+
{
173+
sourceO3D->VoxelDownSample(voxelSize);
174+
targetO3D->VoxelDownSample(voxelSize);
175+
}
130176

131177
std::shared_ptr<open3d::pipelines::registration::Feature> sourceFPFHFeatures = open3d::pipelines::registration::ComputeFPFHFeature(*sourceO3D,
132178
open3d::geometry::KDTreeSearchParamHybrid(radiusKDTreeSearch, maxNeighborKDTreeSearch));
133179
std::shared_ptr<open3d::pipelines::registration::Feature> targetFPFHFeatures = open3d::pipelines::registration::ComputeFPFHFeature(*targetO3D,
134180
open3d::geometry::KDTreeSearchParamHybrid(radiusKDTreeSearch, maxNeighborKDTreeSearch));
181+
std::vector<std::reference_wrapper<const open3d::pipelines::registration::CorrespondenceChecker>> correspondanceChecker;
182+
open3d::pipelines::registration::CorrespondenceCheckerBasedOnDistance checkerOnDistance = open3d::pipelines::registration::CorrespondenceCheckerBasedOnDistance(correspondenceCheckerDistance);
183+
correspondanceChecker.push_back(checkerOnDistance);
184+
135185
auto result = open3d::pipelines::registration::RegistrationRANSACBasedOnFeatureMatching(*sourceO3D,
136186
*targetO3D,
137187
*sourceFPFHFeatures,
138188
*targetFPFHFeatures,
139189
false,
140-
maxCorrespondenceDistance);
190+
maxCorrespondenceDistance,
191+
transformationEstimation,
192+
ransacN,
193+
correspondanceChecker,
194+
open3d::pipelines::registration::RANSACConvergenceCriteria(ransacMaxIteration, ransacConfidenceThreshold));
141195

142196
return result;
143197
}

0 commit comments

Comments
 (0)