Skip to content

Commit 1a527cd

Browse files
DamienGilliardDamienGilliard
committed
ADD-CAP: Refined registration implemented and logger usage
1 parent 9cd9d77 commit 1a527cd

File tree

1 file changed

+52
-115
lines changed

1 file changed

+52
-115
lines changed

‎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)