Skip to content

Commit c9277c7

Browse files
DamienGilliardDamienGilliard
committed
WIP: segmentation now using triangle faces + small fixes
1 parent 8bff20f commit c9277c7

File tree

3 files changed

+123
-61
lines changed

3 files changed

+123
-61
lines changed

‎src/diffCheck/segmentation/DFSegmentation.cc‎

Lines changed: 73 additions & 42 deletions
Original file line numberDiff line numberDiff line change
@@ -16,24 +16,34 @@ namespace diffCheck::segmentation
1616
int radiusNeighborhoodSize)
1717
{
1818
std::vector<std::shared_ptr<geometry::DFPointCloud>> segments;
19+
open3d::geometry::PointCloud o3dPointCloud;
1920
cilantro::PointCloud3f cilantroPointCloud;
20-
21+
22+
// Convert the point cloud to open3d point cloud
23+
for (int i = 0; i < pointCloud.Points.size(); i++)
24+
{
25+
o3dPointCloud.points_.push_back(pointCloud.Points[i]);
26+
}
27+
28+
// estimate normals
29+
o3dPointCloud.EstimateNormals(open3d::geometry::KDTreeSearchParamHybrid(50 * voxelSize, 80));
30+
o3dPointCloud.NormalizeNormals();
31+
std::shared_ptr<open3d::geometry::PointCloud> voxelizedO3DPointCloud = o3dPointCloud.VoxelDownSample(voxelSize);
32+
2133
// Convert the point cloud to cilantro point cloud
22-
for (int i = 0; i < pointCloud.Points.size(); i++)
34+
for (int i = 0; i < voxelizedO3DPointCloud->points_.size(); i++)
2335
{
2436
cilantroPointCloud.points.conservativeResize(3, cilantroPointCloud.points.cols() + 1);
25-
cilantroPointCloud.points.rightCols(1) = pointCloud.Points[i].cast<float>();
37+
cilantroPointCloud.points.rightCols(1) = voxelizedO3DPointCloud->points_[i].cast<float>();
38+
cilantroPointCloud.normals.conservativeResize(3, cilantroPointCloud.normals.cols() + 1);
39+
cilantroPointCloud.normals.rightCols(1) = voxelizedO3DPointCloud->normals_[i].cast<float>();
2640
}
2741

2842
// segment the point cloud using knn or radius neighborhood
2943
if (useKnnNeighborhood)
3044
{
3145
cilantro::KNNNeighborhoodSpecification<int> neighborhood(knnNeighborhoodSize);
3246

33-
// conpute the normals and downsample
34-
cilantroPointCloud.estimateNormals(neighborhood, false);
35-
cilantroPointCloud.gridDownsample(voxelSize);
36-
3747
// Similarity evaluator
3848
cilantro::NormalsProximityEvaluator<float, 3> similarityEvaluator(
3949
cilantroPointCloud.normals,
@@ -62,10 +72,6 @@ namespace diffCheck::segmentation
6272
else
6373
{
6474
cilantro::RadiusNeighborhoodSpecification<float> neighborhood(radiusNeighborhoodSize);
65-
66-
// conpute the normals and downsample
67-
cilantroPointCloud.estimateNormals(neighborhood, false);
68-
cilantroPointCloud.gridDownsample(voxelSize);
6975

7076
// Similarity evaluator
7177
cilantro::NormalsProximityEvaluator<float, 3> similarityEvaluator(
@@ -107,35 +113,34 @@ namespace diffCheck::segmentation
107113
std::vector<std::shared_ptr<geometry::DFPointCloud>> segmentsRemainder;
108114

109115
// iterate through the mesh faces given as function argument
110-
for (auto face : meshFaces)
116+
for (std::shared_ptr<diffCheck::geometry::DFMesh> face : meshFaces)
111117
{
112118
std::shared_ptr<geometry::DFPointCloud> correspondingSegment;
113-
119+
std::shared_ptr<open3d::geometry::TriangleMesh> o3dFace;
120+
o3dFace = face->Cvt2O3DTriangleMesh();
121+
114122
// Getting the center of the mesh face
115-
Eigen::Vector3d faceCenter = Eigen::Vector3d::Zero();
116-
for (auto vertex : face->Vertices){faceCenter += vertex;}
117-
faceCenter /= face->GetNumVertices();
123+
Eigen::Vector3d faceCenter;
124+
open3d::geometry::OrientedBoundingBox orientedBoundingBox = o3dFace->GetMinimalOrientedBoundingBox();
125+
faceCenter = orientedBoundingBox.GetCenter();
118126

119127
if (face->NormalsFace.size() == 0)
120128
{
121-
std::cout << "face has no normals, computing normals" << std::endl;
122-
std::shared_ptr<open3d::geometry::TriangleMesh> o3dFace = face->Cvt2O3DTriangleMesh();
123129
o3dFace->ComputeTriangleNormals();
124130
face->NormalsFace.clear();
125131
for (auto normal : o3dFace->triangle_normals_)
126132
{
127133
face->NormalsFace.push_back(normal.cast<double>());
128134
}
129135
}
130-
136+
131137
// Getting the normal of the mesh face
132138
Eigen::Vector3d faceNormal = face->NormalsFace[0];
133139

134140
// iterate through the segments to find the closest ones compared to the face center taking the normals into account
135-
Eigen::Vector3d segmentCenter = Eigen::Vector3d::Zero();
136-
Eigen::Vector3d segmentNormal = Eigen::Vector3d::Zero();
137-
double faceDistance = (segments[0]->Points[0] - faceCenter).norm() / std::pow(std::abs(segments[0]->Normals[0].dot(faceNormal)), 2);
138-
int segmentIndex = 0;
141+
Eigen::Vector3d segmentCenter;
142+
Eigen::Vector3d segmentNormal;
143+
double faceDistance = std::numeric_limits<double>::max();
139144
for (auto segment : segments)
140145
{
141146
for (auto point : segment->Points)
@@ -144,40 +149,66 @@ namespace diffCheck::segmentation
144149
}
145150
segmentCenter /= segment->GetNumPoints();
146151

147-
if (segment->Normals.size() == 0)
148-
{
149-
std::shared_ptr<open3d::geometry::PointCloud> o3dSegment = segment->Cvt2O3DPointCloud();
150-
o3dSegment->EstimateNormals();
151-
segment->Normals.clear();
152-
for (auto normal : o3dSegment->normals_)
153-
{
154-
segment->Normals.push_back(normal.cast<double>());
155-
}
156-
}
157152
for (auto normal : segment->Normals)
158153
{
159154
segmentNormal += normal;
160155
}
161156
segmentNormal.normalize();
162157

163-
double currentDistance = (faceCenter - segmentCenter).norm() / std::pow(std::abs(segmentNormal.dot(faceNormal)), 2);
158+
double currentDistance = (faceCenter - segmentCenter).norm() / std::abs(segmentNormal.dot(faceNormal));
164159
// if the distance is smaller than the previous one, update the distance and the corresponding segment
165160
if (currentDistance < faceDistance)
166161
{
167162
correspondingSegment = segment;
168-
// storing previous segment in the remainder vector
169163
faceDistance = currentDistance;
170164
}
171-
segmentIndex++;
172165
}
173-
// remove the segment fron the segments vector
174-
segments.erase(std::remove(segments.begin(), segments.end(), correspondingSegment), segments.end());
175166

176-
// Add the closest points of the corresponding segment to the unified point cloud
177-
for (auto point : correspondingSegment->Points)
167+
// get the triangles of the face. This is to check if the point is in the face
168+
std::vector<Eigen::Vector3i> faceTriangles = o3dFace->triangles_;
169+
170+
for (Eigen::Vector3d point : correspondingSegment->Points)
178171
{
179-
unifiedPointCloud->Points.push_back(point);
180-
correspondingSegment->Points.erase(std::remove(correspondingSegment->Points.begin(), correspondingSegment->Points.end(), point), correspondingSegment->Points.end());
172+
bool pointInFace = false;
173+
/*
174+
To check if the point is in the face, we take into account all the triangles forming the face.
175+
We calculate the area of each triangle, then check if the sum of the areas of the tree triangles
176+
formed by two of the points of the referencr triangle and our point is equal to the reference triangle area
177+
(within a user-defined margin). If it is the case, the triangle is in the face.
178+
*/
179+
for (Eigen::Vector3i triangle : faceTriangles)
180+
{
181+
// reference triangle
182+
Eigen::Vector3d v0 = o3dFace->vertices_[triangle[0]].cast<double>();
183+
Eigen::Vector3d v1 = o3dFace->vertices_[triangle[1]].cast<double>();
184+
Eigen::Vector3d v2 = o3dFace->vertices_[triangle[2]].cast<double>();
185+
Eigen::Vector3d n = (v1 - v0).cross(v2 - v0);
186+
double referenceTriangleArea = n.norm()/2.0;
187+
188+
// triangle 1
189+
Eigen::Vector3d n1 = (v1 - v0).cross(point - v0);
190+
double area1 = n1.norm()/2.0;
191+
192+
// triangle 2
193+
Eigen::Vector3d n2 = (v2 - v1).cross(point - v1);
194+
double area2 = n2.norm()/2.0;
195+
196+
// triangle 3
197+
Eigen::Vector3d n3 = (v0 - v2).cross(point - v2);
198+
double area3 = n3.norm()/2.0;
199+
200+
if (std::abs((referenceTriangleArea - (area1 + area2 + area3)) / referenceTriangleArea) < associationThreshold)
201+
{
202+
pointInFace = true;
203+
break;
204+
}
205+
}
206+
if (pointInFace)
207+
{
208+
unifiedPointCloud->Points.push_back(point);
209+
correspondingSegment->Points.erase(std::remove(correspondingSegment->Points.begin(), correspondingSegment->Points.end(), point), correspondingSegment->Points.end());
210+
}
211+
// correspondingSegment->Points.erase(std::remove(correspondingSegment->Points.begin(), correspondingSegment->Points.end(), point), correspondingSegment->Points.end());
181212
}
182213
}
183214
return std::make_tuple(unifiedPointCloud, segments);

‎src/diffCheck/segmentation/DFSegmentation.hh‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -29,6 +29,7 @@ namespace diffCheck::segmentation
2929
* @brief From a vector of mesh faces, finds in a vector of cloud segments the corresponding segments as a unified point cloud
3030
* @param meshFaces the mesh faces to compare
3131
* @param segments the segments to compare
32+
* @param associationThreshold the threshold to consider a segment as a match. A lower number will result in less points being associated to those mesh faces
3233
* @return std::tuple<std::shared_ptr<geometry::DFPointCloud>, std::vector<std::shared_ptr<geometry::DFPointCloud>> > the unified point cloud and the corresponding segments as a tuple
3334
*/
3435
static std::tuple<std::shared_ptr<geometry::DFPointCloud>, std::vector<std::shared_ptr<geometry::DFPointCloud>>> AssociateSegments(

‎src/diffCheckApp.cc‎

Lines changed: 49 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -3,58 +3,88 @@
33
#include <iostream>
44
#include <fstream>
55

6+
// checking computation time
7+
#include <chrono>
68

79
int main()
810
{
11+
auto initTime = std::chrono::high_resolution_clock::now();
12+
913
std::shared_ptr<diffCheck::geometry::DFPointCloud> pcdSrc = std::make_shared<diffCheck::geometry::DFPointCloud>();
1014
std::vector<std::shared_ptr<diffCheck::geometry::DFMesh>> meshSrc = std::vector<std::shared_ptr<diffCheck::geometry::DFMesh>>();
1115
std::vector<std::shared_ptr<diffCheck::geometry::DFPointCloud>> segments;
1216
std::vector<std::string> meshPaths;
1317

14-
std::string meshesFolderPath = R"(C:\Users\localuser\Desktop\other_meshes_for_diffCheck\)";
18+
std::string meshesFolderPath = R"(C:\Users\localuser\Desktop\again_other_meshes_for_diffCheck\)";
1519

16-
for (int i = 1; i <= 14; i++)
20+
for (int i = 1; i <= 4; i++)
1721
{
1822
std::string meshPath = meshesFolderPath + std::to_string(i) + ".ply";
1923
std::shared_ptr<diffCheck::geometry::DFMesh> mesh = std::make_shared<diffCheck::geometry::DFMesh>();
2024
mesh->LoadFromPLY(meshPath);
2125
meshSrc.push_back(mesh);
2226
}
2327

24-
std::string pathPcdSrc = R"(C:\Users\localuser\Downloads\02_points_with_errors_1e6_pts.ply)";
28+
std::string pathPcdSrc = R"(C:\Users\localuser\Desktop\cleaned_AC_pc_90deg.ply)";
2529

2630
pcdSrc->LoadFromPLY(pathPcdSrc);
2731

28-
segments = diffCheck::segmentation::DFSegmentation::NormalBasedSegmentation(*pcdSrc, 5, 1, 20, true, 50, 200);
32+
segments = diffCheck::segmentation::DFSegmentation::NormalBasedSegmentation(*pcdSrc, 0.01, 4, 20, true, 50, 2);
2933
std::cout << "number of segments:" << segments.size()<< std::endl;
3034

3135
std::tuple<std::shared_ptr<diffCheck::geometry::DFPointCloud>, std::vector<std::shared_ptr<diffCheck::geometry::DFPointCloud>>> unifiedSegments =
32-
diffCheck::segmentation::DFSegmentation::AssociateSegments(meshSrc, segments, 50);
33-
36+
diffCheck::segmentation::DFSegmentation::AssociateSegments(meshSrc, segments, 0.1);
3437

3538
diffCheck::visualizer::Visualizer vis;
36-
for (int i = 0; i < 1; i++)
39+
for (auto segment : std::get<1>(unifiedSegments))
3740
{
3841
// colorize the segments with random colors
3942
double r = static_cast<double>(rand()) / RAND_MAX;
4043
double g = static_cast<double>(rand()) / RAND_MAX;
41-
double b = static_cast<double>(rand()) / RAND_MAX;
42-
for (int i = 0; i < std::get<0>(unifiedSegments)->Points.size(); i++)
43-
{
44-
std::get<0>(unifiedSegments)->Colors.push_back(Eigen::Vector3d(r, g, b));
45-
}
46-
vis.AddPointCloud(std::get<0>(unifiedSegments));
44+
double b = static_cast<double>(rand()) / RAND_MAX;
4745

48-
for (auto segemt : std::get<1>(unifiedSegments))
46+
for (int i = 0; i < segment->Points.size(); i++)
4947
{
50-
for (int i = 0; i < segemt->Points.size(); i++)
51-
{
52-
segemt->Colors.push_back(Eigen::Vector3d(0, 0, 1));
53-
}
54-
vis.AddPointCloud(segemt);
48+
segment->Colors.push_back(Eigen::Vector3d(r, g, b));
5549
}
50+
vis.AddPointCloud(segment);
51+
52+
// // colorize the segments with random colors
53+
// double r = static_cast<double>(rand()) / RAND_MAX;
54+
// double g = static_cast<double>(rand()) / RAND_MAX;
55+
// double b = static_cast<double>(rand()) / RAND_MAX;
56+
// for (int i = 0; i < std::get<0>(unifiedSegments)->Points.size(); i++)
57+
// {
58+
// std::get<0>(unifiedSegments)->Colors.push_back(Eigen::Vector3d(r, g, b));
59+
// }
60+
// vis.AddPointCloud(std::get<0>(unifiedSegments));
61+
62+
// for (auto segemt : std::get<1>(unifiedSegments))
63+
// {
64+
// for (int i = 0; i < segemt->Points.size(); i++)
65+
// {
66+
// segemt->Colors.push_back(Eigen::Vector3d(0, 0, 1));
67+
// }
68+
// vis.AddPointCloud(segemt);
69+
// }
70+
71+
}
72+
for(auto mesh : meshSrc)
73+
{
74+
vis.AddMesh(mesh);
75+
}
5676

77+
auto unified = std::get<0>(unifiedSegments);
78+
for (int i = 0; i < unified->Points.size(); i++)
79+
{
80+
unified->Colors.push_back(Eigen::Vector3d(0, 0, 0));
5781
}
82+
vis.AddPointCloud(unified);
83+
84+
auto endTime = std::chrono::high_resolution_clock::now();
85+
auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(endTime - initTime);
86+
std::cout << "Computation time:" << duration.count() << std::endl;
87+
5888
vis.Run();
5989
return 0;
6090
}

0 commit comments

Comments
 (0)