Skip to content

Commit 415ab6c

Browse files
9and39and3
authored
Merge pull request #14 from diffCheckOrg/new_refined_registration
New refined registration
2 parents a002bb7 + 1a527cd commit 415ab6c

File tree

4 files changed

+219
-115
lines changed

4 files changed

+219
-115
lines changed

‎src/diffCheck.hh‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -13,3 +13,4 @@ const diffCheck::Log LOG = diffCheck::Log();
1313
#include "diffCheck/visualizer.hh"
1414
#include "diffCheck/transformation/DFTransformation.hh"
1515
#include "diffCheck/registrations/DFGlobalRegistrations.hh"
16+
#include "diffCheck/registrations/DFRefinedRegistration.hh"

‎src/diffCheck/registrations/DFRefinedRegistration.cc‎

Lines changed: 103 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,103 @@
1+
#include "DFRefinedRegistration.hh"
2+
3+
4+
namespace diffCheck::registration
5+
{
6+
diffCheck::transformation::DFTransformation RefinedRegistration::O3DICP(
7+
std::shared_ptr<geometry::DFPointCloud> source,
8+
std::shared_ptr<geometry::DFPointCloud> target,
9+
double maxCorrespondenceDistance,
10+
bool scalingForPointToPointTransformationEstimation,
11+
double relativeFitness,
12+
double relativeRMSE,
13+
int maxIteration,
14+
bool usePointToPlane)
15+
{
16+
std::vector<Eigen::Vector3d> minMax = source->ComputeBoundingBox();
17+
double scale = (minMax[1] - minMax[0]).norm();
18+
double absoluteMaxCorrespondenceDistance = maxCorrespondenceDistance * scale;
19+
20+
std::shared_ptr<open3d::geometry::PointCloud> O3Dsource = source->Cvt2O3DPointCloud();
21+
std::shared_ptr<open3d::geometry::PointCloud> O3Dtarget = target->Cvt2O3DPointCloud();
22+
Eigen::Matrix4d initialTransformation = Eigen::Matrix4d::Identity();
23+
open3d::pipelines::registration::ICPConvergenceCriteria criteria
24+
= open3d::pipelines::registration::ICPConvergenceCriteria(
25+
relativeFitness,
26+
relativeRMSE,
27+
maxIteration);
28+
29+
open3d::pipelines::registration::RegistrationResult result;
30+
31+
if(usePointToPlane)
32+
{
33+
O3Dsource->EstimateNormals();
34+
O3Dtarget->EstimateNormals();
35+
open3d::pipelines::registration::TransformationEstimationPointToPlane transformation_estimation
36+
= open3d::pipelines::registration::TransformationEstimationPointToPlane();
37+
result = open3d::pipelines::registration::RegistrationICP(
38+
*O3Dsource,
39+
*O3Dtarget,
40+
absoluteMaxCorrespondenceDistance,
41+
initialTransformation,
42+
transformation_estimation,
43+
criteria);
44+
}
45+
else
46+
{
47+
open3d::pipelines::registration::TransformationEstimationPointToPoint transformation_estimation
48+
= open3d::pipelines::registration::TransformationEstimationPointToPoint(scalingForPointToPointTransformationEstimation);
49+
result = open3d::pipelines::registration::RegistrationICP(
50+
*O3Dsource,
51+
*O3Dtarget,
52+
absoluteMaxCorrespondenceDistance,
53+
initialTransformation,
54+
transformation_estimation,
55+
criteria);
56+
}
57+
58+
diffCheck::transformation::DFTransformation transformation
59+
= diffCheck::transformation::DFTransformation(result.transformation_);
60+
return transformation;
61+
}
62+
diffCheck::transformation::DFTransformation RefinedRegistration::O3DGeneralizedICP(
63+
std::shared_ptr<geometry::DFPointCloud> source,
64+
std::shared_ptr<geometry::DFPointCloud> target,
65+
double maxCorrespondenceDistance,
66+
int maxIteration,
67+
double relativeFitness,
68+
double relativeRMSE)
69+
{
70+
std::vector<Eigen::Vector3d> minMax = source->ComputeBoundingBox();
71+
double scale = (minMax[1] - minMax[0]).norm();
72+
double absoluteMaxCorrespondenceDistance = maxCorrespondenceDistance * scale;
73+
74+
std::shared_ptr<open3d::geometry::PointCloud> O3Dsource = source->Cvt2O3DPointCloud();
75+
std::shared_ptr<open3d::geometry::PointCloud> O3Dtarget = target->Cvt2O3DPointCloud();
76+
77+
O3Dsource->EstimateCovariances();
78+
O3Dtarget->EstimateCovariances();
79+
80+
Eigen::Matrix4d initialTransformation = Eigen::Matrix4d::Identity();
81+
open3d::pipelines::registration::ICPConvergenceCriteria criteria
82+
= open3d::pipelines::registration::ICPConvergenceCriteria(
83+
relativeFitness,
84+
relativeRMSE,
85+
maxIteration);
86+
87+
open3d::pipelines::registration::TransformationEstimationForGeneralizedICP transformation_estimation
88+
= open3d::pipelines::registration::TransformationEstimationForGeneralizedICP();
89+
90+
open3d::pipelines::registration::RegistrationResult result
91+
= open3d::pipelines::registration::RegistrationGeneralizedICP(
92+
*O3Dsource,
93+
*O3Dtarget,
94+
absoluteMaxCorrespondenceDistance,
95+
initialTransformation,
96+
transformation_estimation,
97+
criteria);
98+
99+
diffCheck::transformation::DFTransformation transformation
100+
= diffCheck::transformation::DFTransformation(result.transformation_);
101+
return transformation;
102+
}
103+
}

‎src/diffCheck/registrations/DFRefinedRegistration.hh‎

Lines changed: 63 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,63 @@
1+
#pragma once
2+
# include "diffCheck.hh"
3+
4+
# include <open3d/pipelines/registration/Registration.h>
5+
6+
namespace diffCheck::registration
7+
{
8+
class RefinedRegistration
9+
{
10+
public: ///< open3d registration methods
11+
/**
12+
* @brief Perform ICP registration using Open3D
13+
*
14+
* The ICP registration looks for points in the target point cloud that are closest to the source
15+
* point cloud and computes the transformation that minimizes the distance between the two point clouds.
16+
* The way the distance is calculated is either with point to point or point to plane methods.
17+
*
18+
* @param source DFPointCloud source point cloud
19+
* @param target DFPointCloud Target point cloud
20+
* @param maxCorrespondenceDistance Maximum relative correspondence distance. 0.01 means 1% of the bounding box diagonal
21+
* @param scalingForPointToPointTransformationEstimation Enable scaling for point-to-point transformation estimation. by default it is false, to only allow rigid transformations
22+
* @param maxIteration Maximum number of ICP iterations to use in the p2p transformation estimation
23+
* @param relativeFitness Threshold for relative fitness to use in the p2p transformation estimation
24+
* @param relativeRMSE Threshold for relative RMSE to use in the p2p transformation estimation
25+
* @param usePointToPlane Use point-to-plane ICP instead of point-to-point. This replaces the p2p with the point-to-plane transformation estimation.
26+
* @return diffCheck::transformation::DFTransformation
27+
*/
28+
static diffCheck::transformation::DFTransformation O3DICP(
29+
std::shared_ptr<geometry::DFPointCloud> source,
30+
std::shared_ptr<geometry::DFPointCloud> target,
31+
double maxCorrespondenceDistance = 0.05,
32+
bool scalingForPointToPointTransformationEstimation = false,
33+
double relativeFitness = 1e-6,
34+
double relativeRMSE = 1e-6,
35+
int maxIteration = 30,
36+
bool usePointToPlane = false);
37+
38+
/**
39+
* @brief Perform Generalized ICP registration using Open3D
40+
*
41+
* The Generalized ICP registration additionally identifies planes in the source and target
42+
* point clouds and uses them to improve the registration. According to the original 2009 paper,
43+
* it can be seen as a "plane-to-plane" ICP. The paper is called "Generalized-ICP" and is referenced below.
44+
*
45+
* @param source DFPointCloud source point cloud
46+
* @param target DFPointCloud Target point cloud
47+
* @param maxCorrespondenceDistance Maximum relative correspondence distance. 0.01 means 1% of the bounding box diagonal
48+
* @param maxIteration Maximum number of ICP iterations to use in the p2p transformation estimation
49+
* @param relativeFitness Threshold for relative fitness to use in the p2p transformation estimation
50+
* @param relativeRMSE Threshold for relative RMSE to use in the p2p transformation estimation
51+
* @return diffCheck::transformation::DFTransformation
52+
*
53+
* @see http://dx.doi.org/10.15607/RSS.2009.V.021 for more information
54+
*/
55+
static diffCheck::transformation::DFTransformation O3DGeneralizedICP(
56+
std::shared_ptr<geometry::DFPointCloud> source,
57+
std::shared_ptr<geometry::DFPointCloud> target,
58+
double maxCorrespondenceDistance = 0.05,
59+
int maxIteration = 30,
60+
double relativeFitness = 1e-6,
61+
double relativeRMSE = 1e-6);
62+
};
63+
}

‎src/diffCheckApp.cc‎

Lines changed: 52 additions & 115 deletions
Original file line numberDiff line numberDiff line change
@@ -1,129 +1,66 @@
1-
21
#include "diffCheck.hh"
32

43
#include <iostream>
54
#include <fstream>
65

7-
#include <Eigen/Core>
8-
#include <Open3D/Open3D.h>
96

107
int main()
118
{
12-
std::vector<Eigen::Vector3d> points;
13-
std::vector<Eigen::Vector3d> normals;
14-
std::vector<Eigen::Vector3d> colors;
15-
points.push_back(Eigen::Vector3d(0, 0, 0));
16-
points.push_back(Eigen::Vector3d(1, 0, 0));
17-
points.push_back(Eigen::Vector3d(0, 1, 0));
18-
19-
normals.push_back(Eigen::Vector3d(0, 0, 1));
20-
normals.push_back(Eigen::Vector3d(0, 0, 1));
21-
normals.push_back(Eigen::Vector3d(0, 0, 1));
22-
23-
colors.push_back(Eigen::Vector3d(1, 0, 0));
24-
colors.push_back(Eigen::Vector3d(0, 1, 0));
25-
colors.push_back(Eigen::Vector3d(0, 0, 1));
26-
27-
diffCheck::geometry::DFPointCloud dfPointCloud(points, colors, normals);
28-
29-
DIFFCHECK_INFO("Starting diffCheckApp...");
30-
31-
// create a sphere from o3d
32-
auto mesh = open3d::geometry::TriangleMesh::CreateSphere(1.0, 4);
33-
std::shared_ptr<diffCheck::geometry::DFMesh> dfMeshPtr = std::make_shared<diffCheck::geometry::DFMesh>();
34-
dfMeshPtr->Cvt2DFMesh(mesh);
35-
9+
// import clouds
3610
std::shared_ptr<diffCheck::geometry::DFPointCloud> dfPointCloudPtr = std::make_shared<diffCheck::geometry::DFPointCloud>();
37-
dfPointCloudPtr = dfMeshPtr->SampleCloudUniform(1000);
38-
39-
// visualize cloud
40-
std::shared_ptr<diffCheck::visualizer::Visualizer> visualizer = std::make_shared<diffCheck::visualizer::Visualizer>();
41-
visualizer->AddPointCloud(dfPointCloudPtr);
42-
visualizer->Run();
43-
44-
45-
46-
47-
48-
// // import clouds
49-
// std::shared_ptr<diffCheck::geometry::DFPointCloud> dfPointCloudPtr = std::make_shared<diffCheck::geometry::DFPointCloud>();
50-
// std::shared_ptr<diffCheck::geometry::DFPointCloud> dfPointCloudPtr_1 = std::make_shared<diffCheck::geometry::DFPointCloud>();
51-
// std::shared_ptr<diffCheck::geometry::DFPointCloud> dfPointCloudPtr_2 = std::make_shared<diffCheck::geometry::DFPointCloud>();
52-
// std::shared_ptr<diffCheck::geometry::DFPointCloud> dfPointCloudPtrWithoutNormals = std::make_shared<diffCheck::geometry::DFPointCloud>();
53-
// std::shared_ptr<diffCheck::geometry::DFPointCloud> dfPointCloudPtrGroundTruth = std::make_shared<diffCheck::geometry::DFPointCloud>();
54-
// std::shared_ptr<diffCheck::geometry::DFPointCloud> dfPointCloudPtrGroundTruthFromMesh = std::make_shared<diffCheck::geometry::DFPointCloud>();
55-
56-
// // std::string pathCloud = R"(C:\Users\andre\Downloads\scan_data_normals.ply\scan_data_normals.ply)";
57-
// // std::string pathMesh = R"(F:\diffCheck\temp\03_mesh.ply)";
58-
59-
// // create a sphere from o3d
60-
// auto mesh = open3d::geometry::TriangleMesh::CreateSphere(1.0, 4);
61-
62-
// DIFFCHECK_INFO("test");
63-
// DIFFCHECK_WARN("test");
64-
// DIFFCHECK_ERROR("test");
65-
// DIFFCHECK_FATAL("test");
66-
// std::string pathCloud = R"(C:\Users\localuser\Downloads\04_pt.ply)";
67-
// std::string pathCloud_1 = R"(C:\Users\localuser\Downloads\part_1_module.ply)";
68-
// std::string pathCloud_2 = R"(C:\Users\localuser\Downloads\part_2_module.ply)";
69-
// std::string pathMesh = R"(C:\Users\localuser\Downloads\04_mesh.ply)";
70-
71-
// dfMeshPtr->LoadFromPLY(pathMesh);
72-
// dfPointCloudPtr->LoadFromPLY(pathCloud);
73-
// dfPointCloudPtr_1->LoadFromPLY(pathCloud_1);
74-
// dfPointCloudPtr_2->LoadFromPLY(pathCloud_2);
75-
// dfPointCloudPtrGroundTruth->LoadFromPLY(pathCloud);
76-
77-
// // add noise to dfPointCloudPtr
78-
// std::vector<Eigen::Vector3d> boundingBoxPoints = dfPointCloudPtr->ComputeBoundingBox();
79-
// double meanInterval = (boundingBoxPoints[0] - boundingBoxPoints[1]).norm();
80-
// for (int i = 0; i < dfPointCloudPtr->Points.size(); i++)
81-
// {
82-
// dfPointCloudPtr->Points[i] += Eigen::Vector3d::Random() * 0.002 * meanInterval;
83-
// }
84-
85-
// std::shared_ptr<open3d::geometry::PointCloud> pcd_1 = dfPointCloudPtr->Cvt2O3DPointCloud();
86-
// if (pcd_1->normals_.size() > 0)
87-
// {
88-
// pcd_1->normals_.clear();
89-
// }
90-
// dfPointCloudPtrWithoutNormals->Cvt2DFPointCloud(pcd_1);
91-
92-
// // populate the mesh with points and store it in dfPointCloudPtrGroundTruthFromMesh
93-
// std::shared_ptr<open3d::geometry::PointCloud> pcd_2 = dfMeshPtr->Cvt2O3DTriangleMesh()->SamplePointsUniformly(50000);
94-
// dfPointCloudPtrGroundTruthFromMesh->Cvt2DFPointCloud(pcd_2);
95-
96-
// // create a rigid rotation matrix
97-
// Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
98-
// T.block<3, 3>(0, 0) = Eigen::AngleAxisd(1.57, Eigen::Vector3d::UnitX()).toRotationMatrix();
99-
// T(0, 3) = 50;
100-
// T(1, 3) = -100;
101-
// T(2, 3) = 100;
102-
// dfPointCloudPtrWithoutNormals->ApplyTransformation(diffCheck::transformation::DFTransformation(T));
103-
104-
// // create a copy of the point cloud so we can test both global registrations
105-
// std::shared_ptr<diffCheck::geometry::DFPointCloud> dfPointCloudPtrCopy = std::make_shared<diffCheck::geometry::DFPointCloud>(*dfPointCloudPtrWithoutNormals);
106-
107-
// // test global registrations Fast and RANSAC-based
108-
// std::vector<diffCheck::transformation::DFTransformation> registrationResults;
109-
// diffCheck::transformation::DFTransformation transformationA =
110-
// diffCheck::registrations::DFGlobalRegistrations::O3DFastGlobalRegistrationFeatureMatching(dfPointCloudPtr_1, dfPointCloudPtr_2);
111-
// std::cout << "Fast transformation: " << transformationA.TransformationMatrix << std::endl;
112-
// //dfPointCloudPtr_1->ApplyTransformation(transformationA);
113-
// registrationResults.push_back(transformationA);
114-
// diffCheck::transformation::DFTransformation transformationB =
115-
// diffCheck::registrations::DFGlobalRegistrations::O3DRansacOnFeatureMatching(dfPointCloudPtr_1, dfPointCloudPtr_2);
116-
// std::cout << "Ransac transformation: " << transformationB.TransformationMatrix << std::endl;
117-
// dfPointCloudPtr_1->ApplyTransformation(transformationB);
118-
// registrationResults.push_back(transformationB);
11+
std::shared_ptr<diffCheck::geometry::DFMesh> dfMeshPtr = std::make_shared<diffCheck::geometry::DFMesh>();
12+
std::shared_ptr<diffCheck::geometry::DFPointCloud> dfGroundTruth = std::make_shared<diffCheck::geometry::DFPointCloud>();
11913

120-
// // visualize cloud
121-
// std::shared_ptr<diffCheck::visualizer::Visualizer> visualizer = std::make_shared<diffCheck::visualizer::Visualizer>();
122-
// visualizer->AddPointCloud(dfPointCloudPtr_1);
123-
// visualizer->AddPointCloud(dfPointCloudPtr_2);
124-
// // visualizer->AddMesh(dfMeshPtr);
125-
// visualizer->Run();
14+
// std::string pathCloud = R"(C:\Users\andre\Downloads\scan_data_normals.ply\scan_data_normals.ply)";
15+
// std::string pathMesh = R"(F:\diffCheck\assets\dataset\mesh_fromRh_unfixedLength.ply)";
16+
// std::string pathMesh = R"(F:\diffCheck\temp\03_mesh.ply)";
12617

18+
// create a sphere from o3d
12719

20+
std::string pathCloud = R"(C:\Users\localuser\Downloads\04_pt.ply)";
21+
std::string pathMesh = R"(C:\Users\localuser\Downloads\04_mesh.ply)";
22+
// std::string pathMesh = R"(F:\diffCheck\temp\03_mesh.ply)";
23+
24+
dfMeshPtr->LoadFromPLY(pathMesh);
25+
dfPointCloudPtr->LoadFromPLY(pathCloud);
26+
27+
dfGroundTruth->Cvt2DFPointCloud(dfMeshPtr->Cvt2O3DTriangleMesh()->SamplePointsUniformly(10000));
28+
Eigen::Matrix4d transformation = Eigen::Matrix4d::Identity();
29+
transformation(0, 3) = 0.0;
30+
transformation(1, 3) = -0.02;
31+
transformation(2, 3) = 0.02;
32+
Eigen::Matrix3d rotation;
33+
rotation = Eigen::AngleAxisd(0.3, Eigen::Vector3d::UnitY());
34+
transformation.block<3, 3>(0, 0) = rotation * transformation.block<3, 3>(0, 0);
35+
36+
dfPointCloudPtr->ApplyTransformation(transformation);
37+
38+
diffCheck::transformation::DFTransformation simpleICPTransformation
39+
= diffCheck::registration::RefinedRegistration::O3DICP(
40+
dfPointCloudPtr,
41+
dfGroundTruth,
42+
0.05);
43+
44+
diffCheck::transformation::DFTransformation generalizedICPTransformation
45+
= diffCheck::registration::RefinedRegistration::O3DGeneralizedICP(
46+
dfPointCloudPtr,
47+
dfGroundTruth,
48+
0.05);
49+
dfPointCloudPtr->ApplyTransformation(simpleICPTransformation);
50+
51+
std::stringstream ss;
52+
ss << "Simple ICP Transformation:\n";
53+
ss << simpleICPTransformation.TransformationMatrix<<"\n";
54+
ss << "Generalized ICP Transformation:\n";
55+
ss << generalizedICPTransformation.TransformationMatrix;
56+
std::string matrixAsString = ss.str();
57+
DIFFCHECK_INFO(matrixAsString.c_str());
58+
59+
std::shared_ptr<diffCheck::visualizer::Visualizer> vis = std::make_shared<diffCheck::visualizer::Visualizer>();
60+
// vis->AddPointCloud(dfPointCloudPtr);
61+
vis->AddMesh(dfMeshPtr);
62+
vis->AddPointCloud(dfGroundTruth);
63+
vis->AddPointCloud(dfPointCloudPtr);
64+
vis->Run();
12865
return 0;
12966
}

0 commit comments

Comments
 (0)