Skip to content

Commit 0e11970

Browse files
DamienGilliardDamienGilliard
committed
WIP-ADD: Registrations tested and produce csv for error and computation time. python plotter added
1 parent 7007eab commit 0e11970

File tree

4 files changed

+268
-55
lines changed

4 files changed

+268
-55
lines changed

‎plotter.py‎

Lines changed: 26 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,26 @@
1+
import numpy as np
2+
import matplotlib.pyplot as plt
3+
4+
def main():
5+
# open csv file
6+
time_data = np.genfromtxt('times.csv', delimiter=',', skip_header=1)
7+
error_data = np.genfromtxt('errors.csv', delimiter=',', skip_header=1)
8+
x = np.linspace(1, len(time_data), len(time_data))
9+
plt.plot(x, time_data)
10+
plt.xlabel('Iteration')
11+
plt.ylabel('Time [s]')
12+
plt.title('Time vs Iteration: additive 00')
13+
plt.legend(["FGR Feature Matching","FGR Correspondance","Ransac Correspondance","Ransac Feature Matching"])
14+
plt.show()
15+
16+
17+
plt.plot(x, error_data)
18+
plt.xlabel('Iteration')
19+
plt.ylabel('Mean error [mm]')
20+
plt.title('Error vs Iteration: additive 00')
21+
plt.legend(["FGR Feature Matching","FGR Correspondance","Ransac Correspondance","Ransac Feature Matching"])
22+
plt.show()
23+
24+
25+
if __name__ == '__main__':
26+
main()

‎src/diffCheck/registration/registration.cc‎

Lines changed: 58 additions & 28 deletions
Original file line numberDiff line numberDiff line change
@@ -13,32 +13,18 @@ namespace diffCheck::registration
1313
distances = O3DSourcePointCloud->ComputePointCloudDistance(*O3DTargetPointCloud);
1414
return distances;
1515
}
16-
/*
17-
Documentation on Fast Point Feature Historigrams: https://pcl.readthedocs.io/projects/tutorials/en/latest/fpfh_estimation.html
18-
19-
Very simply, point features are values computed on a point cloud (for example the normal of a point, the curvature, etc.).
20-
point features historigrams generalize this concept by computing point features in a local neighborhood of a point, stored as higher-dimentional historigrams.
21-
22-
For example, for a given point, you take all the neighboring points within a given radius, and create a complete graph on those vertices.
23-
then for each edge of the graph you compute features that are then stored in a historigram of the original center point from which the sphere and the graph where built.
24-
https://pcl.readthedocs.io/projects/tutorials/en/latest/pfh_estimation.html#pfh-estimation proposes a simple example of such a historigram.
25-
26-
PCL's documentation refers to this 2009 TUM PhD thesis (but largely outside the scope of our work): https://mediatum.ub.tum.de/doc/800632/941254.pdf
27-
28-
Quite important for us: the resultant hyperspace is dependent on the quality of the surface normal estimations at each point (if pc noisy, historigram different).
29-
*/
3016
open3d::pipelines::registration::RegistrationResult Registration::O3DFastGlobalRegistrationFeatureMatching(std::shared_ptr<geometry::DFPointCloud> source, std::shared_ptr<geometry::DFPointCloud> target)
3117
{
3218
std::shared_ptr<open3d::geometry::PointCloud> sourceO3D = source->Cvt2O3DPointCloud();
3319
std::shared_ptr<open3d::geometry::PointCloud> targetO3D = target->Cvt2O3DPointCloud();
3420

35-
sourceO3D->RandomDownSample(0.1);
36-
targetO3D->RandomDownSample(0.1);
21+
sourceO3D->VoxelDownSample(0.01);
22+
targetO3D->VoxelDownSample(0.01);
3723

3824
std::shared_ptr<open3d::pipelines::registration::Feature> sourceFPFHFeatures = open3d::pipelines::registration::ComputeFPFHFeature(*sourceO3D,
39-
open3d::geometry::KDTreeSearchParamHybrid(0.25, 30));
25+
open3d::geometry::KDTreeSearchParamHybrid(3, 50));
4026
std::shared_ptr<open3d::pipelines::registration::Feature> targetFPFHFeatures = open3d::pipelines::registration::ComputeFPFHFeature(*targetO3D,
41-
open3d::geometry::KDTreeSearchParamHybrid(0.25, 30));
27+
open3d::geometry::KDTreeSearchParamHybrid(0.25, 50));
4228
std::shared_ptr<open3d::pipelines::registration::FastGlobalRegistrationOption> option = std::make_shared<open3d::pipelines::registration::FastGlobalRegistrationOption>();
4329
option->maximum_correspondence_distance_ = 0.05;
4430
option->iteration_number_ = 100;
@@ -52,28 +38,24 @@ namespace diffCheck::registration
5238

5339
return result;
5440
}
55-
/*
56-
Very little information on this registration method compared to the previous one.
57-
If I understand correctly, this method finds keypoints in the FPFH hyperspaces of the source and target point clouds and then tries to match them.
58-
https://pcl.readthedocs.io/projects/tutorials/en/latest/correspondence_grouping.html
59-
*/
41+
6042
open3d::pipelines::registration::RegistrationResult Registration::O3DFastGlobalRegistrationBasedOnCorrespondence(std::shared_ptr<geometry::DFPointCloud> source, std::shared_ptr<geometry::DFPointCloud> target)
6143
{
6244
std::shared_ptr<open3d::geometry::PointCloud> sourceO3D = source->Cvt2O3DPointCloud();
6345
std::shared_ptr<open3d::geometry::PointCloud> targetO3D = target->Cvt2O3DPointCloud();
6446

65-
sourceO3D->RandomDownSample(0.1);
66-
targetO3D->RandomDownSample(0.1);
47+
sourceO3D->VoxelDownSample(0.01);
48+
targetO3D->VoxelDownSample(0.01);
6749

6850
std::shared_ptr<open3d::pipelines::registration::FastGlobalRegistrationOption> option = std::make_shared<open3d::pipelines::registration::FastGlobalRegistrationOption>();
6951
option->maximum_correspondence_distance_ = 0.05;
7052
option->iteration_number_ = 100;
7153
option->maximum_tuple_count_ = 500;
7254

7355
std::shared_ptr<open3d::pipelines::registration::Feature> sourceFPFHFeatures = open3d::pipelines::registration::ComputeFPFHFeature(*sourceO3D,
74-
open3d::geometry::KDTreeSearchParamHybrid(0.25, 30));
56+
open3d::geometry::KDTreeSearchParamHybrid(3, 50));
7557
std::shared_ptr<open3d::pipelines::registration::Feature> targetFPFHFeatures = open3d::pipelines::registration::ComputeFPFHFeature(*targetO3D,
76-
open3d::geometry::KDTreeSearchParamHybrid(0.25, 30));
58+
open3d::geometry::KDTreeSearchParamHybrid(3, 50));
7759

7860

7961
open3d::pipelines::registration::CorrespondenceSet correspondanceset;
@@ -85,4 +67,52 @@ namespace diffCheck::registration
8567
*option);
8668
return result;
8769
}
88-
}
70+
71+
open3d::pipelines::registration::RegistrationResult Registration::O3DRansacOnCorrespondence(std::shared_ptr<geometry::DFPointCloud> source, std::shared_ptr<geometry::DFPointCloud> target)
72+
{
73+
std::shared_ptr<open3d::geometry::PointCloud> sourceO3D = source->Cvt2O3DPointCloud();
74+
std::shared_ptr<open3d::geometry::PointCloud> targetO3D = target->Cvt2O3DPointCloud();
75+
76+
sourceO3D->VoxelDownSample(0.01);
77+
targetO3D->VoxelDownSample(0.01);
78+
79+
std::shared_ptr<open3d::pipelines::registration::Feature> sourceFPFHFeatures = open3d::pipelines::registration::ComputeFPFHFeature(*sourceO3D,
80+
open3d::geometry::KDTreeSearchParamHybrid(3, 50));
81+
std::shared_ptr<open3d::pipelines::registration::Feature> targetFPFHFeatures = open3d::pipelines::registration::ComputeFPFHFeature(*targetO3D,
82+
open3d::geometry::KDTreeSearchParamHybrid(3, 50));
83+
84+
85+
open3d::pipelines::registration::CorrespondenceSet correspondanceset;
86+
correspondanceset = open3d::pipelines::registration::CorrespondencesFromFeatures(*sourceFPFHFeatures, *targetFPFHFeatures);
87+
88+
auto result = open3d::pipelines::registration::RegistrationRANSACBasedOnCorrespondence(*sourceO3D,
89+
*targetO3D,
90+
correspondanceset,
91+
0.05,
92+
open3d::pipelines::registration::TransformationEstimationPointToPoint(),
93+
200);
94+
return result;
95+
}
96+
97+
open3d::pipelines::registration::RegistrationResult Registration::O3DRansacOnFeatureMatching(std::shared_ptr<geometry::DFPointCloud> source, std::shared_ptr<geometry::DFPointCloud> target)
98+
{
99+
std::shared_ptr<open3d::geometry::PointCloud> sourceO3D = source->Cvt2O3DPointCloud();
100+
std::shared_ptr<open3d::geometry::PointCloud> targetO3D = target->Cvt2O3DPointCloud();
101+
102+
sourceO3D->VoxelDownSample(0.01);
103+
targetO3D->VoxelDownSample(0.01);
104+
105+
std::shared_ptr<open3d::pipelines::registration::Feature> sourceFPFHFeatures = open3d::pipelines::registration::ComputeFPFHFeature(*sourceO3D,
106+
open3d::geometry::KDTreeSearchParamHybrid(3, 50));
107+
std::shared_ptr<open3d::pipelines::registration::Feature> targetFPFHFeatures = open3d::pipelines::registration::ComputeFPFHFeature(*targetO3D,
108+
open3d::geometry::KDTreeSearchParamHybrid(3, 50));
109+
auto result = open3d::pipelines::registration::RegistrationRANSACBasedOnFeatureMatching(*sourceO3D,
110+
*targetO3D,
111+
*sourceFPFHFeatures,
112+
*targetFPFHFeatures,
113+
false,
114+
0.05);
115+
116+
return result;
117+
}
118+
}

‎src/diffCheck/registration/registration.hh‎

Lines changed: 39 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -8,11 +8,49 @@ namespace diffCheck::registration{
88
class Registration
99
{
1010
public:
11+
12+
std::vector<double> ComputeP2PDistance(std::shared_ptr<geometry::DFPointCloud> source, std::shared_ptr<geometry::DFPointCloud> target);
13+
14+
/*
15+
Documentation on Fast Point Feature Historigrams: https://pcl.readthedocs.io/projects/tutorials/en/latest/fpfh_estimation.html
16+
17+
Very simply, point features are values computed on a point cloud (for example the normal of a point, the curvature, etc.).
18+
point features historigrams generalize this concept by computing point features in a local neighborhood of a point, stored as higher-dimentional historigrams.
19+
20+
For example, for a given point, you take all the neighboring points within a given radius, and create a complete graph on those vertices.
21+
then for each edge of the graph you compute features that are then stored in a historigram of the original center point from which the sphere and the graph where built.
22+
https://pcl.readthedocs.io/projects/tutorials/en/latest/pfh_estimation.html#pfh-estimation proposes a simple example of such a historigram.
1123
24+
PCL's documentation refers to this 2009 TUM PhD thesis (but largely outside the scope of our work): https://mediatum.ub.tum.de/doc/800632/941254.pdf
25+
26+
Quite important for us: the resultant hyperspace is dependent on the quality of the surface normal estimations at each point (if pc noisy, historigram different).
27+
*/
1228
open3d::pipelines::registration::RegistrationResult O3DFastGlobalRegistrationFeatureMatching(std::shared_ptr<geometry::DFPointCloud> source, std::shared_ptr<geometry::DFPointCloud> target);
1329

30+
/*
31+
Very little information on this registration method compared to the previous one.
32+
If I understand correctly, this method finds keypoints in the FPFH hyperspaces of the source and target point clouds and then tries to match them.
33+
https://pcl.readthedocs.io/projects/tutorials/en/latest/correspondence_grouping.html
34+
*/
1435
open3d::pipelines::registration::RegistrationResult O3DFastGlobalRegistrationBasedOnCorrespondence(std::shared_ptr<geometry::DFPointCloud> source, std::shared_ptr<geometry::DFPointCloud> target);
36+
/*
37+
Ransac registration based on correspondence:
38+
Correspondances are computed between the source and target point clouds.
39+
Then, a transformation is computed that minimizes the error between the correspondances.
40+
If the error is above a certain threshold, the transformation is discarded and a new one is computed.
1541
16-
std::vector<double> ComputeP2PDistance(std::shared_ptr<geometry::DFPointCloud> source, std::shared_ptr<geometry::DFPointCloud> target);
42+
In practice, Open3D gives little information about the feature correspondence
43+
44+
*/
45+
open3d::pipelines::registration::RegistrationResult Registration::O3DRansacOnCorrespondence(std::shared_ptr<geometry::DFPointCloud> source, std::shared_ptr<geometry::DFPointCloud> target);
46+
/*
47+
Ransac registration based on Feature Matching
48+
https://www.open3d.org/docs/release/tutorial/pipelines/global_registration.html#RANSAC
49+
*/
50+
open3d::pipelines::registration::RegistrationResult Registration::O3DRansacOnFeatureMatching(std::shared_ptr<geometry::DFPointCloud> source, std::shared_ptr<geometry::DFPointCloud> target);
51+
52+
53+
54+
1755
};
1856
}

0 commit comments

Comments
 (0)