Feature/export pose csv

CMakeLists.txt

@@ -10,6 +10,9 @@ include(options)
 # disabling warnings
 if (MSVC)
     add_compile_options(/wd4003)
+    # Disable _SECURE_SCL to avoid issues with stdext::checked_array_iterator
+    # in newer MSVC versions with Open3D's bundled fmt library
+    add_compile_definitions(_SECURE_SCL=0 _HAS_STDEXT_CHECKED_ARRAY_ITERATOR=0)
 endif()
 
 # check that the -DCMAKE_BUILD_TYPE is set

cmake/copy_dlls_script.cmake

@@ -0,0 +1,18 @@
+# This script is executed at build time to copy DLLs
+# It uses variables passed from the main CMake configuration
+
+# Remove old DLLs from the destination directory
+file(GLOB old_dlls "${DIR_TO_CLEAN}/*.dll")
+if(old_dlls)
+    file(REMOVE ${old_dlls})
+endif()
+
+# Get all DLLs from the source directory (evaluated at build time)
+file(GLOB dll_files "${SRC_DIR}/*.dll")
+
+# Copy each DLL to the destination directory
+foreach(dll_file ${dll_files})
+    get_filename_component(dll_name ${dll_file} NAME)
+    message(STATUS "Copying ${dll_name} to ${DST_DIR}")
+    file(COPY ${dll_file} DESTINATION ${DST_DIR})
+endforeach()

cmake/external_tools.cmake

@@ -232,19 +232,19 @@ endfunction()
 
 # ------------------------------------------------------------------------------
 function (copy_dlls directory_to_copy_dlls post_build_target)
-  message (STATUS "Erasing old DLLs and copy new ones to ${directory_to_copy_dlls}")
-  file(GLOB files ${directory_to_copy_dlls}/*.dll)
-  foreach(file ${files})
-      message(STATUS "Removing ${file}")
-      file(REMOVE ${file})
-  endforeach()
-  file(GLOB files ${CMAKE_BINARY_DIR}/bin/${CMAKE_BUILD_TYPE}/*.dll)
-  foreach(file ${files})
-      message(STATUS "Copying ${file} to ${directory_to_copy_dlls}")
-      add_custom_command(TARGET ${post_build_target} POST_BUILD
-          COMMAND ${CMAKE_COMMAND} -E copy
-          ${file}
-          ${directory_to_copy_dlls}
-          )
-  endforeach()
+  message (STATUS "Configuring DLL copy to ${directory_to_copy_dlls} at build time")
+
+  # Get the path to the script relative to the project source dir
+  set(COPY_DLLS_SCRIPT ${PROJECT_SOURCE_DIR}/cmake/copy_dlls_script.cmake)
+
+  # Add a post-build command that will copy DLLs at build time
+  # This ensures the file list is evaluated at build time, not configure time
+  add_custom_command(TARGET ${post_build_target} POST_BUILD
+      COMMAND ${CMAKE_COMMAND}
+      -DDIR_TO_CLEAN="${directory_to_copy_dlls}"
+      -DSRC_DIR="${CMAKE_BINARY_DIR}/bin/${CMAKE_BUILD_TYPE}"
+      -DDST_DIR="${directory_to_copy_dlls}"
+      -P "${COPY_DLLS_SCRIPT}"
+      COMMENT "Copying DLLs to ${directory_to_copy_dlls}"
+      )
 endfunction()

src/diffCheck/geometry/DFMesh.cc

@@ -112,10 +112,12 @@ namespace diffCheck::geometry
             Eigen::Vector3d v1 = this->Vertices[triangle[1]];
             Eigen::Vector3d v2 = this->Vertices[triangle[2]];
             Eigen::Vector3d n = (v1 - v0).cross(v2 - v0);
-            n.normalize();
+            double n2 = n.squaredNorm();
 
-            // Project the point onto the plane of the triangle
-            Eigen::Vector3d projectedPoint = point - n * (n.dot(point - v0));
+            // Handle degenerate triangle
+            if (n2 < 1e-20){continue;}// skip this triangle
+
+            Eigen::Vector3d projectedPoint = point - n * (n.dot(point - v0) / n2);
 
             // Compute vectors
             Eigen::Vector3d v0v1 = v1 - v0;
@@ -130,16 +132,21 @@ namespace diffCheck::geometry
             double dot12 = v0v1.dot(v0p);
 
             // 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
-            double invDenom = 1.0 / (dot00 * dot11 - dot01 * dot01);
+            double denom = dot00 * dot11 - dot01 * dot01;
+            if (std::abs(denom) < 1e-20)
+                continue;
+
+            double invDenom = 1.0 / denom;
             double u = (dot11 * dot02 - dot01 * dot12) * invDenom;
             double v = (dot00 * dot12 - dot01 * dot02) * invDenom;
 
             // Check if point is in triangle
-            if ((u >= -associationThreshold / 100) && (v >= -associationThreshold / 100) && (u + v <= 1 + associationThreshold / 100))
+            double epsilon = 1e-6;
+
+            if ((u >= -epsilon) && (v >= -epsilon) && (u + v <= 1 + epsilon))
             {
                 // Check if the point is close enough to the face
-                double maxProjectionDistance = associationThreshold * std::min({(v1 - v0).norm(), (v2 - v1).norm(), (v0 - v2).norm()}) ;
-                if ((projectedPoint - point).norm() < maxProjectionDistance)
+                if ((projectedPoint - point).squaredNorm() < associationThreshold * associationThreshold)
                 {
                     return true;
                 }

src/diffCheck/geometry/DFPointCloud.cc

@@ -156,7 +156,7 @@ namespace diffCheck::geometry
             }
             for (auto &normal : O3DPointCloud->normals_)
             {
-                if(normal.z() < -0.8)
+                if(normal.z() < -0.1)
                 {
                     normal = -normal;
                 }
@@ -173,7 +173,7 @@ namespace diffCheck::geometry
             this->Normals.clear();
             for (int i = 0; i < cilantroPointCloud->normals.cols(); i++)
             {
-                if(cilantroPointCloud->normals.col(i).z() < -0.8)
+                if(cilantroPointCloud->normals.col(i).z() < -0.1)
                 {
                     cilantroPointCloud->normals.col(i) = -cilantroPointCloud->normals.col(i);
                 }

src/diffCheck/geometry/DFPointCloud.hh

@@ -145,13 +145,13 @@ namespace diffCheck::geometry
          * 
          * @param targetSize the target size of the cloud
          */
-
         void DownsampleBySize(int targetSize);
+
         /**
          * @brief Get the tight bounding box of the point cloud
          * 
-         * @return std::vector<Eigen::Vector3d> A vector of two Eigen::Vector3d, with the first one being the minimum
-         * point and the second one the maximum point of the bounding box.
+         * @return std::vector<Eigen::Vector3d> A vector of eight Eigen::Vector3d, representing the corners of the bounding box. 
+         * The order of the corners is as follows:
          *  ///      ------- x
          *  ///     /|
          *  ///    / |

src/diffCheck/segmentation/DFSegmentation.cc

@@ -255,21 +255,23 @@ namespace diffCheck::segmentation
                 }
                 for (auto segment : clusters)
                 {   
-                    Eigen::Vector3d segmentCenter;
-                    Eigen::Vector3d segmentNormal;
+                    Eigen::Vector3d segmentNormal = Eigen::Vector3d::Zero();
 
                     if (segment->GetNumPoints() == 0)
                     {
                         DIFFCHECK_WARN("Empty segment. Skipping the segment.");
                         continue;
                     }
-                    segmentCenter = segment->GetAxixAlignedBoundingBox()[0] + (segment->GetAxixAlignedBoundingBox()[1] - segment->GetAxixAlignedBoundingBox()[0])/2.0;
+                    Eigen::Vector3d segmentCenter = segment->GetAxixAlignedBoundingBox()[0] + (segment->GetAxixAlignedBoundingBox()[1] - segment->GetAxixAlignedBoundingBox()[0])/2.0;
 
                     for (auto normal : segment->Normals){segmentNormal += normal;}
+                    if (segmentNormal.norm() == 0)
+                    {
+                        DIFFCHECK_WARN("Segment normal is zero. Skipping the segment.");
+                        continue;
+                    }
                     segmentNormal.normalize();
-                    double currentDistance = (faceCenter - segmentCenter).norm();
                     double currentDitanceOrthogonalToFace = std::abs((faceCenter - segmentCenter).dot(faceNormal));
-                    double currentAngle = std::abs(sin(acos(faceNormal.dot(faceCenter - segmentCenter))));
                     if (std::abs(sin(acos(faceNormal.dot(segmentNormal)))) < angleThreshold 
                         && currentDitanceOrthogonalToFace < maximumFaceSegmentDistance  
                         && currentDitanceOrthogonalToFace < faceDistance)
@@ -287,64 +289,47 @@ namespace diffCheck::segmentation
                 }
                 bool hasColors = correspondingSegment->GetNumColors() > 0;
 
-                for (Eigen::Vector3d point : correspondingSegment->Points)
+                std::vector<int> indicesToRemove;
+                for (size_t i = 0; i < correspondingSegment->Points.size(); i++)
                 {
+                    const Eigen::Vector3d& point = correspondingSegment->Points[i];
+
                     if (discriminatePoints)
                     {
-                        bool pointInFace = false;
                         if (face->IsPointOnFace(point, associationThreshold))
                         {
                             facePoints->Points.push_back(point);
-                            facePoints->Normals.push_back(
-                                correspondingSegment->Normals[std::distance(
-                                    correspondingSegment->Points.begin(), 
-                                    std::find(correspondingSegment->Points.begin(), 
-                                    correspondingSegment->Points.end(), 
-                                    point))]
-                                );
+                            facePoints->Normals.push_back(correspondingSegment->Normals[i]);
                             if (hasColors)
                             {
-                                facePoints->Colors.push_back(
-                                    correspondingSegment->Colors[std::distance(
-                                        correspondingSegment->Points.begin(), 
-                                        std::find(correspondingSegment->Points.begin(), 
-                                        correspondingSegment->Points.end(), 
-                                        point))]
-                                    );
+                                facePoints->Colors.push_back(correspondingSegment->Colors[i]);
                             }
+                            indicesToRemove.push_back(i);
                         }
                     }
                     else
                     {
                         facePoints->Points.push_back(point);
-                        facePoints->Normals.push_back(
-                            correspondingSegment->Normals[std::distance(
-                                correspondingSegment->Points.begin(), 
-                                std::find(correspondingSegment->Points.begin(), 
-                                correspondingSegment->Points.end(), 
-                                point))]
-                            );
+                        facePoints->Normals.push_back(correspondingSegment->Normals[i]);
                         if (hasColors)
                         {
-                            facePoints->Colors.push_back(
-                                correspondingSegment->Colors[std::distance(
-                                    correspondingSegment->Points.begin(), 
-                                    std::find(correspondingSegment->Points.begin(), 
-                                    correspondingSegment->Points.end(), 
-                                    point))]
-                                );
+                            facePoints->Colors.push_back(correspondingSegment->Colors[i]);
                         }
+                        indicesToRemove.push_back(i);
                     }
                 }
 
-                for(Eigen::Vector3d point : facePoints->Points)
+                for (auto it = indicesToRemove.rbegin(); it != indicesToRemove.rend(); ++it)
                 {
-                    correspondingSegment->Points.erase(
-                        std::remove(
-                            correspondingSegment->Points.begin(), 
-                            correspondingSegment->Points.end(), 
-                            point), 
-                        correspondingSegment->Points.end());
+                    int i = *it;
+
+                    correspondingSegment->Points.erase(correspondingSegment->Points.begin() + i);
+                    correspondingSegment->Normals.erase(correspondingSegment->Normals.begin() + i);
+
+                    if (hasColors)
+                    {
+                        correspondingSegment->Colors.erase(correspondingSegment->Colors.begin() + i);
+                    }
                 }
                 faceSegments.push_back(facePoints);
             }
@@ -372,7 +357,7 @@ namespace diffCheck::segmentation
             for (std::shared_ptr<geometry::DFPointCloud> cluster : unassociatedClusters)
             {
                 std::shared_ptr<geometry::DFMesh> correspondingMeshFace;
-                Eigen::Vector3d clusterCenter;
+                Eigen::Vector3d clusterCenter = Eigen::Vector3d::Zero();
                 Eigen::Vector3d clusterNormal = Eigen::Vector3d::Zero();
 
                 if (cluster->GetNumPoints() == 0)
@@ -390,7 +375,7 @@ namespace diffCheck::segmentation
                     DIFFCHECK_WARN("No meshes to associate with the clusters. Skipping the cluster.");
                     continue;
                 }
-                for (Eigen::Vector3d point : cluster->Points)
+                for (const Eigen::Vector3d& point : cluster->Points)
                 {
                     clusterCenter += point;
                 }
@@ -466,11 +451,13 @@ namespace diffCheck::segmentation
 
                             double dotProduct = clusterNormal.dot((clusterCenter - faceCenter).normalized());
                             dotProduct = std::max(-1.0, std::min(1.0, dotProduct));
-                            double clusterNormalToJunctionLineAngle = std::acos(dotProduct);
 
-                            double currentDistance = (clusterCenter - faceCenter).norm() * std::abs(std::cos(clusterNormalToJunctionLineAngle))
-                            / std::min(std::abs(clusterNormal.dot(faceNormal)), 0.05) ;
-                            if (std::abs(sin(acos(faceNormal.dot(clusterNormal)))) < angleThreshold && currentDistance < maximumFaceSegmentDistance && currentDistance * (std::abs(faceNormal.dot((faceCenter - clusterCenter) / (faceCenter - clusterCenter).norm()))) < distance)
+                            double anglePenalty = 100*std::abs(clusterNormal.dot(faceNormal));
+                            double currentDistance = (clusterCenter - faceCenter).norm() * (.1 + std::abs(dotProduct)) / std::max(anglePenalty, 1.0);
+                            double normalAlignment = std::abs(faceNormal.dot(clusterNormal));
+                            if (std::abs(std::sqrt(1.0 - normalAlignment * normalAlignment)) < angleThreshold 
+                            && currentDistance < maximumFaceSegmentDistance 
+                            && currentDistance < distance)
                             {
                                 goodMeshIndex = meshIndex;
                                 goodFaceIndex = faceIndex;
@@ -495,13 +482,16 @@ namespace diffCheck::segmentation
                 }
                 std::shared_ptr<geometry::DFPointCloud> completed_segment = existingPointCloudSegments[goodMeshIndex][goodFaceIndex];
 
-                for (Eigen::Vector3d point : cluster->Points)
+                std::vector<int> indicesToRemove;
+                for (size_t i = 0; i <  cluster->Points.size(); i++)
                 {
+                    const Eigen::Vector3d& point = cluster->Points[i];
                     if(isCylinder)
                     {
                         completed_segment->Points.push_back(point);
-                        completed_segment->Normals.push_back(cluster->Normals[std::distance(cluster->Points.begin(), std::find(cluster->Points.begin(), cluster->Points.end(), point))]);
-                        completed_segment->Colors.push_back(cluster->Colors[std::distance(cluster->Points.begin(), std::find(cluster->Points.begin(), cluster->Points.end(), point))]);
+                        completed_segment->Normals.push_back(cluster->Normals[i]);
+                        completed_segment->Colors.push_back(cluster->Colors[i]);
+                        indicesToRemove.push_back(i);
                     }
                     else
                     {
@@ -510,27 +500,20 @@ namespace diffCheck::segmentation
                             if (correspondingMeshFace->IsPointOnFace(point, associationThreshold))
                             {
                                 completed_segment->Points.push_back(point);
-                                completed_segment->Normals.push_back(cluster->Normals[std::distance(cluster->Points.begin(), std::find(cluster->Points.begin(), cluster->Points.end(), point))]);
-                                completed_segment->Colors.push_back(cluster->Colors[std::distance(cluster->Points.begin(), std::find(cluster->Points.begin(), cluster->Points.end(), point))]);
+                                completed_segment->Normals.push_back(cluster->Normals[i]);
+                                completed_segment->Colors.push_back(cluster->Colors[i]);
+                                indicesToRemove.push_back(i);
                             }
                         }
                         else
                         {
                             completed_segment->Points.push_back(point);
-                            completed_segment->Normals.push_back(cluster->Normals[std::distance(cluster->Points.begin(), std::find(cluster->Points.begin(), cluster->Points.end(), point))]);
-                            completed_segment->Colors.push_back(cluster->Colors[std::distance(cluster->Points.begin(), std::find(cluster->Points.begin(), cluster->Points.end(), point))]);
+                            completed_segment->Normals.push_back(cluster->Normals[i]);
+                            completed_segment->Colors.push_back(cluster->Colors[i]);
+                            indicesToRemove.push_back(i);
                         }
                     }
                 }
-                std::vector<int> indicesToRemove;
-
-                for (int i = 0; i < cluster->Points.size(); ++i) 
-                {
-                    if (std::find(completed_segment->Points.begin(), completed_segment->Points.end(), cluster->Points[i]) != completed_segment->Points.end()) 
-                    {
-                        indicesToRemove.push_back(i);
-                    }
-                }
                 for (auto it = indicesToRemove.rbegin(); it != indicesToRemove.rend(); ++it) 
                 {
                     std::swap(cluster->Points[*it], cluster->Points.back());

src/gh/components/DF_CAD_segmentator/code.py

@@ -43,6 +43,7 @@ def RunScript(self,
             i_maximum_face_segment_distance = 0.1
 
         o_face_clusters = []
+        o_poses_from_icp = []
         transforms = []
         df_clusters = []
         # we make a deepcopy of the input clouds
@@ -88,6 +89,11 @@ def RunScript(self,
 
         df_asssociated_cluster_faces_per_beam = []
         for i, df_b in enumerate(df_beams):
+            beam_detected_pose = Rhino.Geometry.Plane(df_b.plane)
+            if beam_detected_pose.Transform(transforms[i]):
+                o_poses_from_icp.append(beam_detected_pose)
+            else:
+                o_poses_from_icp.append(None)
             rh_b_mesh_faces = [df_b_f.to_mesh() for df_b_f in df_b.side_faces]
             rh_test_mesh = Rhino.Geometry.Mesh()
             for j in range(len(rh_b_mesh_faces)):
@@ -144,4 +150,4 @@ def RunScript(self,
 
         o_face_clouds = th.list_to_tree(o_face_clusters)
 
-        return [o_beam_clouds, o_face_clouds]
+        return [o_beam_clouds, o_face_clouds, o_poses_from_icp]

src/gh/components/DF_CAD_segmentator/metadata.json

@@ -126,6 +126,14 @@
                 "optional": false,
                 "sourceCount": 0,
                 "graft": false
+            },
+            {
+                "name": "o_poses_from_icp",
+                "nickname": "o_poses_from_icp",
+                "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).", 
+                "optional": false,
+                "sourceCount": 0,
+                "graft": false
             }
         ]
     }