Skip to content

Commit 5bd68c8

Browse files
DamienGilliardDamienGilliard
committed
feat: various small improvements on segmentation implementation (no change to header)
1 parent eccaf03 commit 5bd68c8

1 file changed

Lines changed: 42 additions & 64 deletions

File tree

‎src/diffCheck/segmentation/DFSegmentation.cc‎

Lines changed: 42 additions & 64 deletions
Original file line numberDiff line numberDiff line change
@@ -255,21 +255,18 @@ namespace diffCheck::segmentation
255255
}
256256
for (auto segment : clusters)
257257
{
258-
Eigen::Vector3d segmentCenter;
259-
Eigen::Vector3d segmentNormal;
258+
Eigen::Vector3d segmentNormal = Eigen::Vector3d::Zero();
260259

261260
if (segment->GetNumPoints() == 0)
262261
{
263262
DIFFCHECK_WARN("Empty segment. Skipping the segment.");
264263
continue;
265264
}
266-
segmentCenter = segment->GetAxixAlignedBoundingBox()[0] + (segment->GetAxixAlignedBoundingBox()[1] - segment->GetAxixAlignedBoundingBox()[0])/2.0;
265+
Eigen::Vector3d segmentCenter = segment->GetAxixAlignedBoundingBox()[0] + (segment->GetAxixAlignedBoundingBox()[1] - segment->GetAxixAlignedBoundingBox()[0])/2.0;
267266

268267
for (auto normal : segment->Normals){segmentNormal += normal;}
269268
segmentNormal.normalize();
270-
double currentDistance = (faceCenter - segmentCenter).norm();
271269
double currentDitanceOrthogonalToFace = std::abs((faceCenter - segmentCenter).dot(faceNormal));
272-
double currentAngle = std::abs(sin(acos(faceNormal.dot(faceCenter - segmentCenter))));
273270
if (std::abs(sin(acos(faceNormal.dot(segmentNormal)))) < angleThreshold
274271
&& currentDitanceOrthogonalToFace < maximumFaceSegmentDistance
275272
&& currentDitanceOrthogonalToFace < faceDistance)
@@ -287,64 +284,47 @@ namespace diffCheck::segmentation
287284
}
288285
bool hasColors = correspondingSegment->GetNumColors() > 0;
289286

290-
for (Eigen::Vector3d point : correspondingSegment->Points)
287+
std::vector<int> indicesToRemove;
288+
for (size_t i = 0; i < correspondingSegment->Points.size(); i++)
291289
{
290+
const Eigen::Vector3d& point = correspondingSegment->Points[i];
291+
292292
if (discriminatePoints)
293293
{
294-
bool pointInFace = false;
295294
if (face->IsPointOnFace(point, associationThreshold))
296295
{
297296
facePoints->Points.push_back(point);
298-
facePoints->Normals.push_back(
299-
correspondingSegment->Normals[std::distance(
300-
correspondingSegment->Points.begin(),
301-
std::find(correspondingSegment->Points.begin(),
302-
correspondingSegment->Points.end(),
303-
point))]
304-
);
297+
facePoints->Normals.push_back(correspondingSegment->Normals[i]);
305298
if (hasColors)
306299
{
307-
facePoints->Colors.push_back(
308-
correspondingSegment->Colors[std::distance(
309-
correspondingSegment->Points.begin(),
310-
std::find(correspondingSegment->Points.begin(),
311-
correspondingSegment->Points.end(),
312-
point))]
313-
);
300+
facePoints->Colors.push_back(correspondingSegment->Colors[i]);
314301
}
302+
indicesToRemove.push_back(i);
315303
}
316304
}
317305
else
318306
{
319307
facePoints->Points.push_back(point);
320-
facePoints->Normals.push_back(
321-
correspondingSegment->Normals[std::distance(
322-
correspondingSegment->Points.begin(),
323-
std::find(correspondingSegment->Points.begin(),
324-
correspondingSegment->Points.end(),
325-
point))]
326-
);
308+
facePoints->Normals.push_back(correspondingSegment->Normals[i]);
327309
if (hasColors)
328310
{
329-
facePoints->Colors.push_back(
330-
correspondingSegment->Colors[std::distance(
331-
correspondingSegment->Points.begin(),
332-
std::find(correspondingSegment->Points.begin(),
333-
correspondingSegment->Points.end(),
334-
point))]
335-
);
311+
facePoints->Colors.push_back(correspondingSegment->Colors[i]);
336312
}
313+
indicesToRemove.push_back(i);
337314
}
338315
}
339316

340-
for(Eigen::Vector3d point : facePoints->Points)
317+
for (auto it = indicesToRemove.rbegin(); it != indicesToRemove.rend(); ++it)
341318
{
342-
correspondingSegment->Points.erase(
343-
std::remove(
344-
correspondingSegment->Points.begin(),
345-
correspondingSegment->Points.end(),
346-
point),
347-
correspondingSegment->Points.end());
319+
int i = *it;
320+
321+
correspondingSegment->Points.erase(correspondingSegment->Points.begin() + i);
322+
correspondingSegment->Normals.erase(correspondingSegment->Normals.begin() + i);
323+
324+
if (hasColors)
325+
{
326+
correspondingSegment->Colors.erase(correspondingSegment->Colors.begin() + i);
327+
}
348328
}
349329
faceSegments.push_back(facePoints);
350330
}
@@ -372,7 +352,7 @@ namespace diffCheck::segmentation
372352
for (std::shared_ptr<geometry::DFPointCloud> cluster : unassociatedClusters)
373353
{
374354
std::shared_ptr<geometry::DFMesh> correspondingMeshFace;
375-
Eigen::Vector3d clusterCenter;
355+
Eigen::Vector3d clusterCenter = Eigen::Vector3d::Zero();
376356
Eigen::Vector3d clusterNormal = Eigen::Vector3d::Zero();
377357

378358
if (cluster->GetNumPoints() == 0)
@@ -390,7 +370,7 @@ namespace diffCheck::segmentation
390370
DIFFCHECK_WARN("No meshes to associate with the clusters. Skipping the cluster.");
391371
continue;
392372
}
393-
for (Eigen::Vector3d point : cluster->Points)
373+
for (const Eigen::Vector3d& point : cluster->Points)
394374
{
395375
clusterCenter += point;
396376
}
@@ -466,11 +446,13 @@ namespace diffCheck::segmentation
466446

467447
double dotProduct = clusterNormal.dot((clusterCenter - faceCenter).normalized());
468448
dotProduct = std::max(-1.0, std::min(1.0, dotProduct));
469-
double clusterNormalToJunctionLineAngle = std::acos(dotProduct);
470449

471-
double currentDistance = (clusterCenter - faceCenter).norm() * std::abs(std::cos(clusterNormalToJunctionLineAngle))
472-
/ std::min(std::abs(clusterNormal.dot(faceNormal)), 0.05) ;
473-
if (std::abs(sin(acos(faceNormal.dot(clusterNormal)))) < angleThreshold && currentDistance < maximumFaceSegmentDistance && currentDistance * (std::abs(faceNormal.dot((faceCenter - clusterCenter) / (faceCenter - clusterCenter).norm()))) < distance)
450+
double anglePenalty = 100*std::abs(clusterNormal.dot(faceNormal));
451+
double currentDistance = (clusterCenter - faceCenter).norm() * (.1 + std::abs(dotProduct)) / std::max(anglePenalty, 1.0);
452+
double normalAlignment = std::abs(faceNormal.dot(clusterNormal));
453+
if (std::abs(std::sqrt(1.0 - normalAlignment * normalAlignment)) < angleThreshold
454+
&& currentDistance < maximumFaceSegmentDistance
455+
&& currentDistance < distance)
474456
{
475457
goodMeshIndex = meshIndex;
476458
goodFaceIndex = faceIndex;
@@ -495,13 +477,16 @@ namespace diffCheck::segmentation
495477
}
496478
std::shared_ptr<geometry::DFPointCloud> completed_segment = existingPointCloudSegments[goodMeshIndex][goodFaceIndex];
497479

498-
for (Eigen::Vector3d point : cluster->Points)
480+
std::vector<int> indicesToRemove;
481+
for (size_t i = 0; i < cluster->Points.size(); i++)
499482
{
483+
const Eigen::Vector3d& point = cluster->Points[i];
500484
if(isCylinder)
501485
{
502486
completed_segment->Points.push_back(point);
503-
completed_segment->Normals.push_back(cluster->Normals[std::distance(cluster->Points.begin(), std::find(cluster->Points.begin(), cluster->Points.end(), point))]);
504-
completed_segment->Colors.push_back(cluster->Colors[std::distance(cluster->Points.begin(), std::find(cluster->Points.begin(), cluster->Points.end(), point))]);
487+
completed_segment->Normals.push_back(cluster->Normals[i]);
488+
completed_segment->Colors.push_back(cluster->Colors[i]);
489+
indicesToRemove.push_back(i);
505490
}
506491
else
507492
{
@@ -510,27 +495,20 @@ namespace diffCheck::segmentation
510495
if (correspondingMeshFace->IsPointOnFace(point, associationThreshold))
511496
{
512497
completed_segment->Points.push_back(point);
513-
completed_segment->Normals.push_back(cluster->Normals[std::distance(cluster->Points.begin(), std::find(cluster->Points.begin(), cluster->Points.end(), point))]);
514-
completed_segment->Colors.push_back(cluster->Colors[std::distance(cluster->Points.begin(), std::find(cluster->Points.begin(), cluster->Points.end(), point))]);
498+
completed_segment->Normals.push_back(cluster->Normals[i]);
499+
completed_segment->Colors.push_back(cluster->Colors[i]);
500+
indicesToRemove.push_back(i);
515501
}
516502
}
517503
else
518504
{
519505
completed_segment->Points.push_back(point);
520-
completed_segment->Normals.push_back(cluster->Normals[std::distance(cluster->Points.begin(), std::find(cluster->Points.begin(), cluster->Points.end(), point))]);
521-
completed_segment->Colors.push_back(cluster->Colors[std::distance(cluster->Points.begin(), std::find(cluster->Points.begin(), cluster->Points.end(), point))]);
506+
completed_segment->Normals.push_back(cluster->Normals[i]);
507+
completed_segment->Colors.push_back(cluster->Colors[i]);
508+
indicesToRemove.push_back(i);
522509
}
523510
}
524511
}
525-
std::vector<int> indicesToRemove;
526-
527-
for (int i = 0; i < cluster->Points.size(); ++i)
528-
{
529-
if (std::find(completed_segment->Points.begin(), completed_segment->Points.end(), cluster->Points[i]) != completed_segment->Points.end())
530-
{
531-
indicesToRemove.push_back(i);
532-
}
533-
}
534512
for (auto it = indicesToRemove.rbegin(); it != indicesToRemove.rend(); ++it)
535513
{
536514
std::swap(cluster->Points[*it], cluster->Points.back());

0 commit comments

Comments
 (0)