Skip to content

Commit 7ad3378

Browse files
DamienGilliardDamienGilliard
authored
Merge pull request #186 from diffCheckOrg/feature/export_pose_csv
Feature/export pose csv
2 parents 08d0b8d + 610dc87 commit 7ad3378

18 files changed

Lines changed: 363 additions & 148 deletions

File tree

‎CMakeLists.txt‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -10,6 +10,9 @@ include(options)
1010
# disabling warnings
1111
if (MSVC)
1212
add_compile_options(/wd4003)
13+
# Disable _SECURE_SCL to avoid issues with stdext::checked_array_iterator
14+
# in newer MSVC versions with Open3D's bundled fmt library
15+
add_compile_definitions(_SECURE_SCL=0 _HAS_STDEXT_CHECKED_ARRAY_ITERATOR=0)
1316
endif()
1417

1518
# check that the -DCMAKE_BUILD_TYPE is set

‎cmake/copy_dlls_script.cmake‎

Lines changed: 18 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,18 @@
1+
# This script is executed at build time to copy DLLs
2+
# It uses variables passed from the main CMake configuration
3+
4+
# Remove old DLLs from the destination directory
5+
file(GLOB old_dlls "${DIR_TO_CLEAN}/*.dll")
6+
if(old_dlls)
7+
file(REMOVE ${old_dlls})
8+
endif()
9+
10+
# Get all DLLs from the source directory (evaluated at build time)
11+
file(GLOB dll_files "${SRC_DIR}/*.dll")
12+
13+
# Copy each DLL to the destination directory
14+
foreach(dll_file ${dll_files})
15+
get_filename_component(dll_name ${dll_file} NAME)
16+
message(STATUS "Copying ${dll_name} to ${DST_DIR}")
17+
file(COPY ${dll_file} DESTINATION ${DST_DIR})
18+
endforeach()

‎cmake/external_tools.cmake‎

Lines changed: 15 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -232,19 +232,19 @@ endfunction()
232232

233233
# ------------------------------------------------------------------------------
234234
function (copy_dlls directory_to_copy_dlls post_build_target)
235-
message (STATUS "Erasing old DLLs and copy new ones to ${directory_to_copy_dlls}")
236-
file(GLOB files ${directory_to_copy_dlls}/*.dll)
237-
foreach(file ${files})
238-
message(STATUS "Removing ${file}")
239-
file(REMOVE ${file})
240-
endforeach()
241-
file(GLOB files ${CMAKE_BINARY_DIR}/bin/${CMAKE_BUILD_TYPE}/*.dll)
242-
foreach(file ${files})
243-
message(STATUS "Copying ${file} to ${directory_to_copy_dlls}")
244-
add_custom_command(TARGET ${post_build_target} POST_BUILD
245-
COMMAND ${CMAKE_COMMAND} -E copy
246-
${file}
247-
${directory_to_copy_dlls}
248-
)
249-
endforeach()
235+
message (STATUS "Configuring DLL copy to ${directory_to_copy_dlls} at build time")
236+
237+
# Get the path to the script relative to the project source dir
238+
set(COPY_DLLS_SCRIPT ${PROJECT_SOURCE_DIR}/cmake/copy_dlls_script.cmake)
239+
240+
# Add a post-build command that will copy DLLs at build time
241+
# This ensures the file list is evaluated at build time, not configure time
242+
add_custom_command(TARGET ${post_build_target} POST_BUILD
243+
COMMAND ${CMAKE_COMMAND}
244+
-DDIR_TO_CLEAN="${directory_to_copy_dlls}"
245+
-DSRC_DIR="${CMAKE_BINARY_DIR}/bin/${CMAKE_BUILD_TYPE}"
246+
-DDST_DIR="${directory_to_copy_dlls}"
247+
-P "${COPY_DLLS_SCRIPT}"
248+
COMMENT "Copying DLLs to ${directory_to_copy_dlls}"
249+
)
250250
endfunction()

‎src/diffCheck/geometry/DFMesh.cc‎

Lines changed: 14 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -112,10 +112,12 @@ namespace diffCheck::geometry
112112
Eigen::Vector3d v1 = this->Vertices[triangle[1]];
113113
Eigen::Vector3d v2 = this->Vertices[triangle[2]];
114114
Eigen::Vector3d n = (v1 - v0).cross(v2 - v0);
115-
n.normalize();
115+
double n2 = n.squaredNorm();
116116

117-
// Project the point onto the plane of the triangle
118-
Eigen::Vector3d projectedPoint = point - n * (n.dot(point - v0));
117+
// Handle degenerate triangle
118+
if (n2 < 1e-20){continue;}// skip this triangle
119+
120+
Eigen::Vector3d projectedPoint = point - n * (n.dot(point - v0) / n2);
119121

120122
// Compute vectors
121123
Eigen::Vector3d v0v1 = v1 - v0;
@@ -130,16 +132,21 @@ namespace diffCheck::geometry
130132
double dot12 = v0v1.dot(v0p);
131133

132134
// create u,v isoparametric mapping to the triangle where (u,v) = (1,0) if projectedPoint = v2, (u,v) = (0,1) if projectedPoint = v1 and (u,v) = (0,0) if projectedPoint = v0
133-
double invDenom = 1.0 / (dot00 * dot11 - dot01 * dot01);
135+
double denom = dot00 * dot11 - dot01 * dot01;
136+
if (std::abs(denom) < 1e-20)
137+
continue;
138+
139+
double invDenom = 1.0 / denom;
134140
double u = (dot11 * dot02 - dot01 * dot12) * invDenom;
135141
double v = (dot00 * dot12 - dot01 * dot02) * invDenom;
136142

137143
// Check if point is in triangle
138-
if ((u >= -associationThreshold / 100) && (v >= -associationThreshold / 100) && (u + v <= 1 + associationThreshold / 100))
144+
double epsilon = 1e-6;
145+
146+
if ((u >= -epsilon) && (v >= -epsilon) && (u + v <= 1 + epsilon))
139147
{
140148
// Check if the point is close enough to the face
141-
double maxProjectionDistance = associationThreshold * std::min({(v1 - v0).norm(), (v2 - v1).norm(), (v0 - v2).norm()}) ;
142-
if ((projectedPoint - point).norm() < maxProjectionDistance)
149+
if ((projectedPoint - point).squaredNorm() < associationThreshold * associationThreshold)
143150
{
144151
return true;
145152
}

‎src/diffCheck/geometry/DFPointCloud.cc‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -156,7 +156,7 @@ namespace diffCheck::geometry
156156
}
157157
for (auto &normal : O3DPointCloud->normals_)
158158
{
159-
if(normal.z() < -0.8)
159+
if(normal.z() < -0.1)
160160
{
161161
normal = -normal;
162162
}
@@ -173,7 +173,7 @@ namespace diffCheck::geometry
173173
this->Normals.clear();
174174
for (int i = 0; i < cilantroPointCloud->normals.cols(); i++)
175175
{
176-
if(cilantroPointCloud->normals.col(i).z() < -0.8)
176+
if(cilantroPointCloud->normals.col(i).z() < -0.1)
177177
{
178178
cilantroPointCloud->normals.col(i) = -cilantroPointCloud->normals.col(i);
179179
}

‎src/diffCheck/geometry/DFPointCloud.hh‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -145,13 +145,13 @@ namespace diffCheck::geometry
145145
*
146146
* @param targetSize the target size of the cloud
147147
*/
148-
149148
void DownsampleBySize(int targetSize);
149+
150150
/**
151151
* @brief Get the tight bounding box of the point cloud
152152
*
153-
* @return std::vector<Eigen::Vector3d> A vector of two Eigen::Vector3d, with the first one being the minimum
154-
* point and the second one the maximum point of the bounding box.
153+
* @return std::vector<Eigen::Vector3d> A vector of eight Eigen::Vector3d, representing the corners of the bounding box.
154+
* The order of the corners is as follows:
155155
* /// ------- x
156156
* /// /|
157157
* /// / |

‎src/diffCheck/segmentation/DFSegmentation.cc‎

Lines changed: 47 additions & 64 deletions
Original file line numberDiff line numberDiff line change
@@ -255,21 +255,23 @@ 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;}
268+
if (segmentNormal.norm() == 0)
269+
{
270+
DIFFCHECK_WARN("Segment normal is zero. Skipping the segment.");
271+
continue;
272+
}
269273
segmentNormal.normalize();
270-
double currentDistance = (faceCenter - segmentCenter).norm();
271274
double currentDitanceOrthogonalToFace = std::abs((faceCenter - segmentCenter).dot(faceNormal));
272-
double currentAngle = std::abs(sin(acos(faceNormal.dot(faceCenter - segmentCenter))));
273275
if (std::abs(sin(acos(faceNormal.dot(segmentNormal)))) < angleThreshold
274276
&& currentDitanceOrthogonalToFace < maximumFaceSegmentDistance
275277
&& currentDitanceOrthogonalToFace < faceDistance)
@@ -287,64 +289,47 @@ namespace diffCheck::segmentation
287289
}
288290
bool hasColors = correspondingSegment->GetNumColors() > 0;
289291

290-
for (Eigen::Vector3d point : correspondingSegment->Points)
292+
std::vector<int> indicesToRemove;
293+
for (size_t i = 0; i < correspondingSegment->Points.size(); i++)
291294
{
295+
const Eigen::Vector3d& point = correspondingSegment->Points[i];
296+
292297
if (discriminatePoints)
293298
{
294-
bool pointInFace = false;
295299
if (face->IsPointOnFace(point, associationThreshold))
296300
{
297301
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-
);
302+
facePoints->Normals.push_back(correspondingSegment->Normals[i]);
305303
if (hasColors)
306304
{
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-
);
305+
facePoints->Colors.push_back(correspondingSegment->Colors[i]);
314306
}
307+
indicesToRemove.push_back(i);
315308
}
316309
}
317310
else
318311
{
319312
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-
);
313+
facePoints->Normals.push_back(correspondingSegment->Normals[i]);
327314
if (hasColors)
328315
{
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-
);
316+
facePoints->Colors.push_back(correspondingSegment->Colors[i]);
336317
}
318+
indicesToRemove.push_back(i);
337319
}
338320
}
339321

340-
for(Eigen::Vector3d point : facePoints->Points)
322+
for (auto it = indicesToRemove.rbegin(); it != indicesToRemove.rend(); ++it)
341323
{
342-
correspondingSegment->Points.erase(
343-
std::remove(
344-
correspondingSegment->Points.begin(),
345-
correspondingSegment->Points.end(),
346-
point),
347-
correspondingSegment->Points.end());
324+
int i = *it;
325+
326+
correspondingSegment->Points.erase(correspondingSegment->Points.begin() + i);
327+
correspondingSegment->Normals.erase(correspondingSegment->Normals.begin() + i);
328+
329+
if (hasColors)
330+
{
331+
correspondingSegment->Colors.erase(correspondingSegment->Colors.begin() + i);
332+
}
348333
}
349334
faceSegments.push_back(facePoints);
350335
}
@@ -372,7 +357,7 @@ namespace diffCheck::segmentation
372357
for (std::shared_ptr<geometry::DFPointCloud> cluster : unassociatedClusters)
373358
{
374359
std::shared_ptr<geometry::DFMesh> correspondingMeshFace;
375-
Eigen::Vector3d clusterCenter;
360+
Eigen::Vector3d clusterCenter = Eigen::Vector3d::Zero();
376361
Eigen::Vector3d clusterNormal = Eigen::Vector3d::Zero();
377362

378363
if (cluster->GetNumPoints() == 0)
@@ -390,7 +375,7 @@ namespace diffCheck::segmentation
390375
DIFFCHECK_WARN("No meshes to associate with the clusters. Skipping the cluster.");
391376
continue;
392377
}
393-
for (Eigen::Vector3d point : cluster->Points)
378+
for (const Eigen::Vector3d& point : cluster->Points)
394379
{
395380
clusterCenter += point;
396381
}
@@ -466,11 +451,13 @@ namespace diffCheck::segmentation
466451

467452
double dotProduct = clusterNormal.dot((clusterCenter - faceCenter).normalized());
468453
dotProduct = std::max(-1.0, std::min(1.0, dotProduct));
469-
double clusterNormalToJunctionLineAngle = std::acos(dotProduct);
470454

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)
455+
double anglePenalty = 100*std::abs(clusterNormal.dot(faceNormal));
456+
double currentDistance = (clusterCenter - faceCenter).norm() * (.1 + std::abs(dotProduct)) / std::max(anglePenalty, 1.0);
457+
double normalAlignment = std::abs(faceNormal.dot(clusterNormal));
458+
if (std::abs(std::sqrt(1.0 - normalAlignment * normalAlignment)) < angleThreshold
459+
&& currentDistance < maximumFaceSegmentDistance
460+
&& currentDistance < distance)
474461
{
475462
goodMeshIndex = meshIndex;
476463
goodFaceIndex = faceIndex;
@@ -495,13 +482,16 @@ namespace diffCheck::segmentation
495482
}
496483
std::shared_ptr<geometry::DFPointCloud> completed_segment = existingPointCloudSegments[goodMeshIndex][goodFaceIndex];
497484

498-
for (Eigen::Vector3d point : cluster->Points)
485+
std::vector<int> indicesToRemove;
486+
for (size_t i = 0; i < cluster->Points.size(); i++)
499487
{
488+
const Eigen::Vector3d& point = cluster->Points[i];
500489
if(isCylinder)
501490
{
502491
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))]);
492+
completed_segment->Normals.push_back(cluster->Normals[i]);
493+
completed_segment->Colors.push_back(cluster->Colors[i]);
494+
indicesToRemove.push_back(i);
505495
}
506496
else
507497
{
@@ -510,27 +500,20 @@ namespace diffCheck::segmentation
510500
if (correspondingMeshFace->IsPointOnFace(point, associationThreshold))
511501
{
512502
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))]);
503+
completed_segment->Normals.push_back(cluster->Normals[i]);
504+
completed_segment->Colors.push_back(cluster->Colors[i]);
505+
indicesToRemove.push_back(i);
515506
}
516507
}
517508
else
518509
{
519510
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))]);
511+
completed_segment->Normals.push_back(cluster->Normals[i]);
512+
completed_segment->Colors.push_back(cluster->Colors[i]);
513+
indicesToRemove.push_back(i);
522514
}
523515
}
524516
}
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-
}
534517
for (auto it = indicesToRemove.rbegin(); it != indicesToRemove.rend(); ++it)
535518
{
536519
std::swap(cluster->Points[*it], cluster->Points.back());

‎src/gh/components/DF_CAD_segmentator/code.py‎

Lines changed: 7 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -43,6 +43,7 @@ def RunScript(self,
4343
i_maximum_face_segment_distance = 0.1
4444

4545
o_face_clusters = []
46+
o_poses_from_icp = []
4647
transforms = []
4748
df_clusters = []
4849
# we make a deepcopy of the input clouds
@@ -88,6 +89,11 @@ def RunScript(self,
8889

8990
df_asssociated_cluster_faces_per_beam = []
9091
for i, df_b in enumerate(df_beams):
92+
beam_detected_pose = Rhino.Geometry.Plane(df_b.plane)
93+
if beam_detected_pose.Transform(transforms[i]):
94+
o_poses_from_icp.append(beam_detected_pose)
95+
else:
96+
o_poses_from_icp.append(None)
9197
rh_b_mesh_faces = [df_b_f.to_mesh() for df_b_f in df_b.side_faces]
9298
rh_test_mesh = Rhino.Geometry.Mesh()
9399
for j in range(len(rh_b_mesh_faces)):
@@ -144,4 +150,4 @@ def RunScript(self,
144150

145151
o_face_clouds = th.list_to_tree(o_face_clusters)
146152

147-
return [o_beam_clouds, o_face_clouds]
153+
return [o_beam_clouds, o_face_clouds, o_poses_from_icp]

‎src/gh/components/DF_CAD_segmentator/metadata.json‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -126,6 +126,14 @@
126126
"optional": false,
127127
"sourceCount": 0,
128128
"graft": false
129+
},
130+
{
131+
"name": "o_poses_from_icp",
132+
"nickname": "o_poses_from_icp",
133+
"description": "The list of poses resulting from the ICP registration. If i_make_registration is False, this output will contain the original assembly beam planes (no ICP applied).",
134+
"optional": false,
135+
"sourceCount": 0,
136+
"graft": false
129137
}
130138
]
131139
}

0 commit comments

Comments
 (0)