Skip to content

Commit e4c2bce

Browse files
DamienGilliardDamienGilliard
committed
WIP-ADD: DFTransformation datatype created and code adapted to use this datatype
1 parent fc27476 commit e4c2bce

File tree

6 files changed

+74
-37
lines changed

6 files changed

+74
-37
lines changed

‎src/diffCheck.hh‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -8,4 +8,5 @@
88
#include "diffCheck/geometry/DFMesh.hh"
99
#include "diffCheck/IOManager.hh"
1010
#include "diffCheck/visualizer.hh"
11-
#include "diffCheck/registration/globalRegistration.hh"
11+
#include "diffCheck/transformation/DFTransformation.hh"
12+
#include "diffCheck/registration/globalRegistration.hh"

‎src/diffCheck/registration/globalregistration.cc‎

Lines changed: 20 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -31,7 +31,7 @@ namespace diffCheck::registration
3131
return errors;
3232
};
3333

34-
open3d::pipelines::registration::RegistrationResult GlobalRegistration::O3DFastGlobalRegistrationFeatureMatching(std::shared_ptr<geometry::DFPointCloud> source,
34+
diffCheck::transformation::DFTransformation GlobalRegistration::O3DFastGlobalRegistrationFeatureMatching(std::shared_ptr<geometry::DFPointCloud> source,
3535
std::shared_ptr<geometry::DFPointCloud> target,
3636
bool voxelise,
3737
double voxelSize,
@@ -59,16 +59,18 @@ namespace diffCheck::registration
5959
option->iteration_number_ = iterationNumber;
6060
option->maximum_tuple_count_ = maxTupleCount;
6161

62-
auto result = open3d::pipelines::registration::FastGlobalRegistrationBasedOnFeatureMatching(*sourceO3D,
62+
open3d::pipelines::registration::RegistrationResult result = open3d::pipelines::registration::FastGlobalRegistrationBasedOnFeatureMatching(*sourceO3D,
6363
*targetO3D,
6464
*sourceFPFHFeatures,
6565
*targetFPFHFeatures,
6666
*option);
67+
diffCheck::transformation::DFTransformation transformation = diffCheck::transformation::DFTransformation();
68+
transformation.transformationMatrix = result.transformation_;
6769

68-
return result;
70+
return transformation;
6971
}
7072

71-
open3d::pipelines::registration::RegistrationResult GlobalRegistration::O3DFastGlobalRegistrationBasedOnCorrespondence(std::shared_ptr<geometry::DFPointCloud> source,
73+
diffCheck::transformation::DFTransformation GlobalRegistration::O3DFastGlobalRegistrationBasedOnCorrespondence(std::shared_ptr<geometry::DFPointCloud> source,
7274
std::shared_ptr<geometry::DFPointCloud> target,
7375
bool voxelise,
7476
double voxelSize,
@@ -105,10 +107,13 @@ namespace diffCheck::registration
105107
*targetO3D,
106108
correspondanceset,
107109
*option);
108-
return result;
110+
diffCheck::transformation::DFTransformation transformation = diffCheck::transformation::DFTransformation();
111+
transformation.transformationMatrix = result.transformation_;
112+
113+
return transformation;
109114
}
110115

111-
open3d::pipelines::registration::RegistrationResult GlobalRegistration::O3DRansacOnCorrespondence(std::shared_ptr<geometry::DFPointCloud> source,
116+
diffCheck::transformation::DFTransformation GlobalRegistration::O3DRansacOnCorrespondence(std::shared_ptr<geometry::DFPointCloud> source,
112117
std::shared_ptr<geometry::DFPointCloud> target,
113118
bool voxelise,
114119
double voxelSize,
@@ -149,10 +154,13 @@ namespace diffCheck::registration
149154
ransacN,
150155
correspondanceChecker,
151156
open3d::pipelines::registration::RANSACConvergenceCriteria(ransacMaxIteration, ransacConfidenceThreshold));
152-
return result;
157+
diffCheck::transformation::DFTransformation transformation = diffCheck::transformation::DFTransformation();
158+
transformation.transformationMatrix = result.transformation_;
159+
160+
return transformation;
153161
}
154162

155-
open3d::pipelines::registration::RegistrationResult GlobalRegistration::O3DRansacOnFeatureMatching(std::shared_ptr<geometry::DFPointCloud> source,
163+
diffCheck::transformation::DFTransformation GlobalRegistration::O3DRansacOnFeatureMatching(std::shared_ptr<geometry::DFPointCloud> source,
156164
std::shared_ptr<geometry::DFPointCloud> target,
157165
bool voxelise,
158166
double voxelSize,
@@ -193,6 +201,9 @@ namespace diffCheck::registration
193201
correspondanceChecker,
194202
open3d::pipelines::registration::RANSACConvergenceCriteria(ransacMaxIteration, ransacConfidenceThreshold));
195203

196-
return result;
204+
diffCheck::transformation::DFTransformation transformation = diffCheck::transformation::DFTransformation();
205+
transformation.transformationMatrix = result.transformation_;
206+
207+
return transformation;
197208
}
198209
}

‎src/diffCheck/registration/globalregistration.hh‎

Lines changed: 15 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -14,11 +14,12 @@ class GlobalRegistration
1414
/**
1515
* @brief Compute the "point to point" distance between two point clouds.
1616
*
17-
* For every point in the source point cloud it looks in the KDTree of the target point cloud and finds the closest point.
17+
* For every point in the source point cloud, it looks in the KDTree of the target point cloud and finds the closest point.
1818
* It returns a vector of distances, one for each point in the source point cloud.
1919
*
2020
* @param source The source diffCheck point cloud
2121
* @param target The target diffCheck point cloud
22+
* @return std::vector<double> A vector of distances, one for each point in the source point cloud.
2223
*
2324
* @see https://github.com/isl-org/Open3D/blob/main/cpp/open3d/geometry/PointCloud.cpp
2425
*/
@@ -29,7 +30,8 @@ class GlobalRegistration
2930
*
3031
* @param source The source diffCheck point cloud
3132
* @param target The target diffCheck point cloud
32-
* @param transform The vector of transformation matrix to apply to the source point cloud.
33+
* @param transform The vector of transformation matrix we want to evaluate. they are applied to the source point cloud.
34+
* @return std::vector<double> A vector of mean distances, one for each transform.
3335
*/
3436

3537
static std::vector<double> EvaluateRegistrations(std::shared_ptr<geometry::DFPointCloud> source,
@@ -40,7 +42,7 @@ class GlobalRegistration
4042
* @brief Fast Global Registration based on Feature Matching using (Fast) Point Feature Histograms (FPFH) on the source and target point clouds
4143
*
4244
* Very simply, point features are values computed on a point cloud (for example the normal of a point, the curvature, etc.).
43-
* point features historigrams generalize this concept by computing point features in a local neighborhood of a point, stored as higher-dimentional historigrams.
45+
* point features historigrams generalize this concept by computing point features in a local neighborhood of a point, and are stored as higher-dimentional historigrams. Those historigrams are then used to compute a transformation between the source and target point clouds.
4446
*
4547
* @note The FPFH hyperspace is dependent on the quality of the surface normal estimations at each point (if pc noisy, historigram different).
4648
*
@@ -52,12 +54,14 @@ class GlobalRegistration
5254
* @param maxCorrespondenceDistance the maximum distance between correspondences. A higher value will result in more correspondences, but potentially include wrong ones.
5355
* @param iterationNumber the number of iterations to run the RanSaC registration algorithm. A higher value will take more time to compute but increases the chances of finding a good transformation. As parameter of the FastGlobalRegistrationOption options
5456
* @param maxTupleCount the maximum number of tuples to consider in the FPFH hyperspace. A higher value will result in heavier computation but potentially more precise. As parameter of the FastGlobalRegistrationOption options
57+
* @return diffCheck::transformation::DFTransformation The result of the registration, containing the transformation matrix and the fitness score.
5558
*
59+
* @see https://www.open3d.org/docs/latest/cpp_api/classopen3d_1_1pipelines_1_1registration_1_1_registration_result.html#a6722256f1f3ddccb2c4ec8d724693974 for more information on the RegistrationResult object
5660
* @see https://link.springer.com/content/pdf/10.1007/978-3-319-46475-6_47.pdf for the original article on Fast Global Registration
5761
* @see https://pcl.readthedocs.io/projects/tutorials/en/latest/pfh_estimation.html#pfh-estimation for more information on PFH (from PCL, not Open3D)
5862
* @see https://mediatum.ub.tum.de/doc/800632/941254.pdf for in-depth documentation on the theory
5963
*/
60-
static open3d::pipelines::registration::RegistrationResult O3DFastGlobalRegistrationFeatureMatching(std::shared_ptr<geometry::DFPointCloud> source,
64+
static diffCheck::transformation::DFTransformation O3DFastGlobalRegistrationFeatureMatching(std::shared_ptr<geometry::DFPointCloud> source,
6165
std::shared_ptr<geometry::DFPointCloud> target,
6266
bool voxelize = true,
6367
double voxelSize = 0.01,
@@ -82,10 +86,11 @@ class GlobalRegistration
8286
* @param maxNeighborKDTreeSearch the maximum number of neighbors to search for in the KDTree. It is used for the calculation of FPFHFeatures
8387
* @param iterationNumber the number of iterations to run the Fast Global Registration algorithm.
8488
* @param maxTupleCount the maximum number of tuples to consider in the FPFH hyperspace
89+
* @return diffCheck::transformation::DFTransformation The result of the registration, containing the transformation matrix and the fitness score.
8590
*
8691
* @see https://pcl.readthedocs.io/projects/tutorials/en/latest/correspondence_grouping.html (from PCL, not Open3D)
8792
*/
88-
static open3d::pipelines::registration::RegistrationResult O3DFastGlobalRegistrationBasedOnCorrespondence(std::shared_ptr<geometry::DFPointCloud> source,
93+
static diffCheck::transformation::DFTransformation O3DFastGlobalRegistrationBasedOnCorrespondence(std::shared_ptr<geometry::DFPointCloud> source,
8994
std::shared_ptr<geometry::DFPointCloud> target,
9095
bool voxelize = true,
9196
double voxelSize = 0.01,
@@ -110,8 +115,9 @@ class GlobalRegistration
110115
* @param maxNeighborKDTreeSearch the maximum number of neighbors to search for in the KDTree. It is used for the calculation of FPFHFeatures
111116
* @param maxCorrespondenceDistance the maximum distance between correspondences in the FPFH space. A higher value will result in more correspondences, but potentially include wrong ones.
112117
* @param correspondenceSetSize the number of correspondences to consider in the Ransac algorithm
118+
* @return diffCheck::transformation::DFTransformation The result of the registration, containing the transformation matrix and the fitness score.
113119
*/
114-
static open3d::pipelines::registration::RegistrationResult O3DRansacOnCorrespondence(std::shared_ptr<geometry::DFPointCloud> source,
120+
static diffCheck::transformation::DFTransformation O3DRansacOnCorrespondence(std::shared_ptr<geometry::DFPointCloud> source,
115121
std::shared_ptr<geometry::DFPointCloud> target,
116122
bool voxelize = true,
117123
double voxelSize = 0.01,
@@ -136,10 +142,11 @@ class GlobalRegistration
136142
* @param transformationEstimation the transformation estimation method to use. By default, it uses a point to point transformation estimation.
137143
* @param ransacN the number of points to sample in the source point cloud. A higher value can result in a more precise transformation, but will take more time to compute.
138144
* @param correspondenceCheckersDistance the maximum distance between correspondances in the FPFH space before testing a RanSaC model.
145+
* @return diffCheck::transformation::DFTransformation The result of the registration, containing the transformation matrix and the fitness score.
139146
*
140147
* @see https://www.open3d.org/docs/release/tutorial/pipelines/global_registration.html#RANSAC (from PCL, not Open3D)
141148
*/
142-
static open3d::pipelines::registration::RegistrationResult O3DRansacOnFeatureMatching(std::shared_ptr<geometry::DFPointCloud> source,
149+
static diffCheck::transformation::DFTransformation O3DRansacOnFeatureMatching(std::shared_ptr<geometry::DFPointCloud> source,
143150
std::shared_ptr<geometry::DFPointCloud> target,
144151
bool voxelize = true,
145152
double voxelSize = 0.01,
@@ -151,6 +158,6 @@ class GlobalRegistration
151158
double correspondenceCheckerDistance = 0.05,
152159
int ransacMaxIteration = 1000,
153160
double ransacConfidenceThreshold = 0.999);
154-
155161
};
162+
156163
}

‎src/diffCheck/transformation/DFTransformation.cc‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,7 @@
1+
#include "DFTransformation.hh"
2+
3+
namespace diffCheck::transformation
4+
{
5+
6+
7+
}

‎src/diffCheck/transformation/DFTransformation.hh‎

Lines changed: 25 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,25 @@
1+
#pragma once
2+
#include <Eigen/Core>
3+
4+
namespace diffCheck::transformation
5+
{
6+
class DFTransformation
7+
{
8+
public:
9+
/**
10+
* @brief 4x4 Transformation matrix for point clouds
11+
*/
12+
Eigen::Matrix4d transformationMatrix;
13+
14+
/**
15+
* @brief 3x3 Rotation matrix for point clouds
16+
*/
17+
Eigen::Matrix3d rotationMatrix;
18+
19+
/**
20+
* @brief 3x1 Translation vector for point clouds
21+
*/
22+
Eigen::Vector3d translationVector;
23+
24+
};
25+
}

‎src/diffCheckApp.cc‎

Lines changed: 5 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -65,26 +65,26 @@ int iterations = 50;
6565
std::vector<Eigen::Matrix<double, 4, 4>> registrationResults;
6666
// Testing the Fast Global Registration on Feature Matching method
6767
std::shared_ptr<diffCheck::geometry::DFPointCloud> dfPointCloudPtrAfterReg_1 = std::make_shared<diffCheck::geometry::DFPointCloud>();
68-
auto result_1 = diffCheck::registration::GlobalRegistration::O3DFastGlobalRegistrationBasedOnCorrespondence(dfPointCloudPtrAfterTrans, dfPointCloudPtrGroundTruth);
69-
Eigen::Matrix<double, 4, 4> transformation = result_1.transformation_;
68+
diffCheck::transformation::DFTransformation result_1 = diffCheck::registration::GlobalRegistration::O3DFastGlobalRegistrationBasedOnCorrespondence(dfPointCloudPtrAfterTrans, dfPointCloudPtrGroundTruth);
69+
Eigen::Matrix<double, 4, 4> transformation = result_1.transformationMatrix;
7070
registrationResults.push_back(transformation);
7171

7272
// Testing the Fast Global Registration on Correspondance method
7373
std::shared_ptr<diffCheck::geometry::DFPointCloud> dfPointCloudPtrAfterReg_2 = std::make_shared<diffCheck::geometry::DFPointCloud>();
7474
auto result_2 = diffCheck::registration::GlobalRegistration::O3DFastGlobalRegistrationBasedOnCorrespondence(dfPointCloudPtrAfterTrans, dfPointCloudPtrGroundTruth);
75-
Eigen::Matrix<double, 4, 4> transformation_2 = result_2.transformation_;
75+
Eigen::Matrix<double, 4, 4> transformation_2 = result_2.transformationMatrix;
7676
registrationResults.push_back(transformation_2);
7777

7878
// Testing the Ransac registration based on correspondance method
7979
std::shared_ptr<diffCheck::geometry::DFPointCloud> dfPointCloudPtrAfterReg_3 = std::make_shared<diffCheck::geometry::DFPointCloud>();
8080
auto result_3 = diffCheck::registration::GlobalRegistration::O3DRansacOnCorrespondence(dfPointCloudPtrAfterTrans, dfPointCloudPtrGroundTruth);
81-
Eigen::Matrix<double, 4, 4> transformation_3 = result_3.transformation_;
81+
Eigen::Matrix<double, 4, 4> transformation_3 = result_3.transformationMatrix;
8282
registrationResults.push_back(transformation_3);
8383

8484
// Testing the Ransac registration based on Feature Matching method
8585
std::shared_ptr<diffCheck::geometry::DFPointCloud> dfPointCloudPtrAfterReg_4 = std::make_shared<diffCheck::geometry::DFPointCloud>();
8686
auto result_4 = diffCheck::registration::GlobalRegistration::O3DRansacOnFeatureMatching(dfPointCloudPtrAfterTrans, dfPointCloudPtrGroundTruth);
87-
Eigen::Matrix<double, 4, 4> transformation_4 = result_4.transformation_;
87+
Eigen::Matrix<double, 4, 4> transformation_4 = result_4.transformationMatrix;
8888
registrationResults.push_back(transformation_4);
8989

9090
std::cout<<"Iteration: "<<i<<" "<<std::flush;
@@ -93,20 +93,6 @@ int iterations = 50;
9393
std::vector<double> errors = diffCheck::registration::GlobalRegistration::EvaluateRegistrations(dfPointCloudPtrAfterTrans, dfPointCloudPtr, registrationResults);
9494
std::cout<<"Errors: FGRCorrespondence "<<errors[0]<<", FGRFeatureMatching: "<<errors[1]<<", RanSaC Correspondence: "<<errors[2]<<", RanSaC FeatureMatching: "<<errors[3]<<std::endl;
9595
}
96-
/*
97-
// write the errors and computation times to 2 csv files with one column per method
98-
std::ofstream fileErrors("errors.csv");
99-
std::ofstream fileTimes("times.csv");
100-
fileErrors<<"FGR Feature Matching,FGR Correspondance,Ransac Correspondance,Ransac Feature Matching"<<std::endl;
101-
fileTimes<<"FGR Feature Matching,FGR Correspondance,Ransac Correspondance,Ransac Feature Matching"<<std::endl;
102-
for (int i = 0; i < transformations.size(); i++)
103-
{
104-
fileErrors<<errorsFGRFeatureMatching[i]<<","<<errorsFGRCorrespondance[i]<<","<<errorsRansacCorrespondance[i]<<","<<errorsRansacFeatureMatching[i]<<std::endl;
105-
fileTimes<<timesFGRFeatureMatching[i]<<","<<timesFGRCorrespondance[i]<<","<<timesRansacCorrespondance[i]<<","<<timesRansacFeatureMatching[i]<<std::endl;
106-
}
107-
fileErrors.close();
108-
fileTimes.close();
109-
*/
11096

11197
return 0;
11298
}

0 commit comments

Comments
 (0)