diff --git a/src/diffCheck/segmentation/DFSegmentation.cc b/src/diffCheck/segmentation/DFSegmentation.cc index 91a11691..d79cba5b 100644 --- a/src/diffCheck/segmentation/DFSegmentation.cc +++ b/src/diffCheck/segmentation/DFSegmentation.cc @@ -97,11 +97,12 @@ namespace diffCheck::segmentation std::vector> DFSegmentation::AssociateClustersToMeshes( bool isCylinder, + bool discriminatePoints, std::vector> referenceMesh, std::vector> &clusters, double angleThreshold, double associationThreshold, - double angleAssociationThreshold) + double maximumFaceSegmentDistance) { std::vector> faceSegments = std::vector>(); @@ -270,12 +271,12 @@ namespace diffCheck::segmentation for (auto normal : segment->Normals){segmentNormal += normal;} 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 the distance is smaller than the previous one, update the distance and the corresponding segment - if (std::abs(sin(acos(faceNormal.dot(segmentNormal)))) < angleThreshold && currentDistance * (angleAssociationThreshold + std::abs(faceNormal.dot((faceCenter - segmentCenter) / (faceCenter - segmentCenter).norm()))) < faceDistance) + if (std::abs(sin(acos(faceNormal.dot(segmentNormal)))) < angleThreshold && currentDitanceOrthogonalToFace < maximumFaceSegmentDistance && currentDitanceOrthogonalToFace < faceDistance) { correspondingSegment = segment; - faceDistance = currentDistance * (angleAssociationThreshold + std::abs(faceNormal.dot((faceCenter - segmentCenter) / (faceCenter - segmentCenter).norm()))); + faceDistance = currentDitanceOrthogonalToFace; } } @@ -289,8 +290,32 @@ namespace diffCheck::segmentation for (Eigen::Vector3d point : correspondingSegment->Points) { - bool pointInFace = false; - if (face->IsPointOnFace(point, associationThreshold)) + 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))] + ); + if (hasColors) + { + facePoints->Colors.push_back( + correspondingSegment->Colors[std::distance( + correspondingSegment->Points.begin(), + std::find(correspondingSegment->Points.begin(), + correspondingSegment->Points.end(), + point))] + ); + } + } + } + else { facePoints->Points.push_back(point); facePoints->Normals.push_back( @@ -330,12 +355,13 @@ namespace diffCheck::segmentation void DFSegmentation::CleanUnassociatedClusters( bool isCylinder, + bool discriminatePoints, std::vector> &unassociatedClusters, std::vector>> &existingPointCloudSegments, std::vector>> meshes, double angleThreshold, double associationThreshold, - double angleAssociationThreshold) + double maximumFaceSegmentDistance) { if (unassociatedClusters.size() == 0) { @@ -443,7 +469,7 @@ namespace diffCheck::segmentation 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 * (angleAssociationThreshold + std::abs(faceNormal.dot((faceCenter - clusterCenter) / (faceCenter - clusterCenter).norm()))) < distance) + if (std::abs(sin(acos(faceNormal.dot(clusterNormal)))) < angleThreshold && currentDistance < maximumFaceSegmentDistance && currentDistance * (std::abs(faceNormal.dot((faceCenter - clusterCenter) / (faceCenter - clusterCenter).norm()))) < distance) { goodMeshIndex = meshIndex; goodFaceIndex = faceIndex; @@ -477,12 +503,23 @@ namespace diffCheck::segmentation completed_segment->Colors.push_back(cluster->Colors[std::distance(cluster->Points.begin(), std::find(cluster->Points.begin(), cluster->Points.end(), point))]); } else - if (correspondingMeshFace->IsPointOnFace(point, associationThreshold)) + { + if (discriminatePoints) + { + 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))]); + } + } + 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))]); } + } } std::vector indicesToRemove; diff --git a/src/diffCheck/segmentation/DFSegmentation.hh b/src/diffCheck/segmentation/DFSegmentation.hh index cb453158..a11b465d 100644 --- a/src/diffCheck/segmentation/DFSegmentation.hh +++ b/src/diffCheck/segmentation/DFSegmentation.hh @@ -28,38 +28,42 @@ namespace diffCheck::segmentation public: ///< segmentation refinement methods /** @brief Associates point cloud segments to mesh faces and merges them. It uses the center of mass of the segments and the mesh faces to find correspondances. For each mesh face it then iteratively associate the points of the segment that are actually on the mesh face. * @param isCylinder a boolean to indicate if the model is a cylinder. If true, the method will use the GetCenterAndAxis method of the mesh to find the center and axis of the mesh. based on that, we only want points that have normals more or less perpendicular to the cylinder axis. + * @param discriminatePoints a boolean to indicate if we want to discriminate points based on their normal direction when associating clusters to mesh faces. If true, only points that have normals more or less aligned with the face normal will be considered for association. * @param referenceMesh the vector of mesh faces to associate with the segments. It is a representation of a beam and its faces. * @param clusters the vector of clusters from cilantro to associate with the mesh faces of the reference mesh * @param angleThreshold the threshold to consider the a cluster as potential candidate for association. the value passed is the minimum sine of the angles. A value of 0 requires perfect alignment (angle = 0), while a value of 0.1 allows an angle of 5.7 degrees. * @param associationThreshold the threshold to consider the points of a segment and a mesh face as associable. It is the ratio between the surface of the closest mesh triangle and the sum of the areas of the three triangles that form the rest of the pyramid described by the mesh triangle and the point we want to associate or not. The lower the number, the more strict the association will be and some poinnts on the mesh face might be wrongfully excluded. - * @param angleAssociationThreshold a number to indicate how much distance in the plane of the face should be favored, compared to distance orthogonal to the face normal. If set to 0, any face in the same plane as the face will be considered as having a distance of 0. If set to a high value (e.g. 1000000), no difference will be made between distance in the plane of the face and orthogonal to it. Default is 0.5 + * @param maximumFaceSegmentDistance the maximum distance a segment's center of mass can be perpendicularly to a mesh face * @return std::shared_ptr The unified segments */ static std::vector> DFSegmentation::AssociateClustersToMeshes( bool isCylinder, + bool discriminatePoints, std::vector> referenceMesh, std::vector> &clusters, double angleThreshold = 0.1, double associationThreshold = 0.1, - double angleAssociationThreshold = 0.5); + double maximumFaceSegmentDistance = 0.05); /** @brief Iterated through clusters and finds the corresponding mesh face. It then associates the points of the cluster that are on the mesh face to the segment already associated with the mesh face. * @param isCylinder a boolean to indicate if the model is a cylinder. If true, the method will use the GetCenterAndAxis method of the mesh to find the center and axis of the mesh. based on that, we only want points that have normals more or less perpendicular to the cylinder axis. + * @param discriminatePoints a boolean to indicate if we want to discriminate points based on their normal direction when associating clusters to mesh faces. If true, only points that have normals more or less aligned with the face normal will be considered for association. * @param unassociatedClusters the clusters from the normal-based segmentatinon that haven't been associated yet. * @param existingPointCloudSegments the already associated segments per mesh face. * @param meshes the mesh faces for all the model. This is used to associate the clusters to the mesh faces. * @param angleThreshold the threshold to consider the a cluster as potential candidate for association. the value passed is the minimum sine of the angles. A value of 0 requires perfect alignment (angle = 0), while a value of 0.1 allows an angle of 5.7 degrees. * @param associationThreshold the threshold to consider the points of a segment and a mesh face as associable. It is the ratio between the surface of the closest mesh triangle and the sum of the areas of the three triangles that form the rest of the pyramid described by the mesh triangle and the point we want to associate or not. The lower the number, the more strict the association will be and some poinnts on the mesh face might be wrongfully excluded. - * @param angleAssociationThreshold a number to indicate how much distance in the plane of the face should be favored, compared to distance orthogonal to the face normal. If set to 0, any face in the same plane as the face will be considered as having a distance of 0. If set to a high value (e.g. 1000000), no difference will be made between distance in the plane of the face and orthogonal to it. Default is 0.5 + * @param maximumFaceSegmentDistance the maximum distance a segment's center of mass can be perpendicularly to a mesh face * @return void */ static void DFSegmentation::CleanUnassociatedClusters( bool isCylinder, + bool discriminatePoints, std::vector> &unassociatedClusters, std::vector>> &existingPointCloudSegments, std::vector>> meshes, double angleThreshold = 0.1, double associationThreshold = 0.1, - double angleAssociationThreshold = 0.5); + double maximumFaceSegmentDistance = 0.05); }; } // namespace diffCheck::segmentation \ No newline at end of file diff --git a/src/diffCheckBindings.cc b/src/diffCheckBindings.cc index e1a7a081..590c1662 100644 --- a/src/diffCheckBindings.cc +++ b/src/diffCheckBindings.cc @@ -226,18 +226,20 @@ PYBIND11_MODULE(diffcheck_bindings, m) { .def_static("associate_clusters", &diffCheck::segmentation::DFSegmentation::AssociateClustersToMeshes, py::arg("is_roundwood"), + py::arg("discriminate_points"), py::arg("reference_mesh"), py::arg("unassociated_clusters"), py::arg("angle_threshold") = 0.1, py::arg("association_threshold") = 0.1, - py::arg("angle_association_threshold") = 0.5) + py::arg("maximum_face_segment_distance") = 0.05) .def_static("clean_unassociated_clusters", &diffCheck::segmentation::DFSegmentation::CleanUnassociatedClusters, py::arg("is_roundwood"), + py::arg("discriminate_points"), py::arg("unassociated_clusters"), py::arg("associated_clusters"), py::arg("reference_mesh"), py::arg("angle_threshold") = 0.1, py::arg("association_threshold") = 0.1, - py::arg("angle_association_threshold") = 0.5); + py::arg("maximum_face_segment_distance") = 0.05); } diff --git a/src/gh/components/DF_CAD_segmentator/code.py b/src/gh/components/DF_CAD_segmentator/code.py index 81bc8e94..13371761 100644 --- a/src/gh/components/DF_CAD_segmentator/code.py +++ b/src/gh/components/DF_CAD_segmentator/code.py @@ -9,7 +9,7 @@ from diffCheck.diffcheck_bindings import dfb_segmentation -from diffCheck.diffcheck_bindings import dfb_geometry +from diffCheck.diffcheck_bindings import dfb_geometry, dfb_registrations from diffCheck import df_cvt_bindings @@ -17,11 +17,14 @@ class DFCADSegmentator(component): def RunScript(self, - i_clouds: System.Collections.Generic.IList[Rhino.Geometry.PointCloud], - i_assembly, - i_angle_threshold: float = 0.1, - i_association_threshold: float = 0.1, - i_angle_association_threshold: float = 0.5): + i_clouds: System.Collections.Generic.List[Rhino.Geometry.PointCloud], + i_assembly, + i_angle_threshold: float, + i_association_threshold: float, + i_maximum_face_segment_distance: float, + i_radius_normal_estimation: float, + i_make_registration: bool, + i_max_correspondence_distance_icp: float): if i_clouds is None or i_assembly is None: self.AddRuntimeMessage(RML.Warning, "Please provide a cloud and an assembly to segment.") @@ -30,44 +33,98 @@ def RunScript(self, i_angle_threshold = 0.1 if i_association_threshold is None: i_association_threshold = 0.1 - if i_angle_association_threshold is None: - i_angle_association_threshold = 0.5 + if i_radius_normal_estimation is None: + i_radius_normal_estimation = 0.01 + if i_make_registration is None: + i_make_registration = True + if i_max_correspondence_distance_icp is None: + i_max_correspondence_distance_icp = 0.1 + if i_maximum_face_segment_distance is None: + i_maximum_face_segment_distance = 0.1 + o_face_clusters = [] + transforms = [] df_clusters = [] # we make a deepcopy of the input clouds df_clouds = [df_cvt_bindings.cvt_rhcloud_2_dfcloud(cloud.Duplicate()) for cloud in i_clouds] + df_merged_cloud = dfb_geometry.DFPointCloud() + df_merged_cloud.remove_statistical_outliers(100, 1.5) + for pc in df_clouds: + df_merged_cloud.add_points(pc) df_beams = i_assembly.beams - df_asssociated_cluster_faces_per_beam = [] - for df_b in df_beams: + rh_meshes = [] + for i, df_b in enumerate(df_beams): + rh_b_mesh_faces = [df_b_f.to_mesh() for df_b_f in df_b.side_faces] + for df_j_face in df_b.joint_faces: + rh_b_mesh_faces.append(df_j_face.to_mesh()) + rh_mesh = Rhino.Geometry.Mesh() + for rh_b_mesh_face in rh_b_mesh_faces: + rh_mesh.Append(rh_b_mesh_face) + + df_b_mesh = df_cvt_bindings.cvt_rhmesh_2_dfmesh(rh_mesh) + df_sampled_cloud = df_b_mesh.sample_points_uniformly(1000) + df_sampled_cloud.estimate_normals(use_cilantro_evaluator=False, + search_radius = i_radius_normal_estimation, + ) + if i_make_registration: + transform = dfb_registrations.DFRefinedRegistration.O3DICP( + source=df_sampled_cloud, + target=df_merged_cloud, + max_correspondence_distance= i_max_correspondence_distance_icp, + max_iteration = 1000 + ) + + df_xform = transform.transformation_matrix + rh_xform = Rhino.Geometry.Transform() + for i in range(4): + for j in range(4): + rh_xform[i, j] = df_xform[i, j] + + else: + rh_xform = Rhino.Geometry.Transform(1) + transforms.append(rh_xform) + + df_asssociated_cluster_faces_per_beam = [] + for i, df_b in enumerate(df_beams): 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)): + sucess = rh_b_mesh_faces[j].Transform(transforms[i]) + if sucess: + rh_test_mesh.Append(rh_b_mesh_faces[j]) + rh_meshes.append(rh_test_mesh) df_b_mesh_faces = [df_cvt_bindings.cvt_rhmesh_2_dfmesh(rh_b_mesh_face) for rh_b_mesh_face in rh_b_mesh_faces] # different association depending on the type of beam - df_asssociated_cluster_faces = dfb_segmentation.DFSegmentation.associate_clusters( + df_new_asssociated_cluster_faces = dfb_segmentation.DFSegmentation.associate_clusters( is_roundwood=df_b.is_roundwood, + discriminate_points=True, reference_mesh=df_b_mesh_faces, unassociated_clusters=df_clouds, angle_threshold=i_angle_threshold, association_threshold=i_association_threshold, - angle_association_threshold=i_angle_association_threshold + maximum_face_segment_distance=i_maximum_face_segment_distance ) - df_asssociated_cluster_faces_per_beam.append(df_asssociated_cluster_faces) + df_asssociated_cluster_faces_per_beam.append(df_new_asssociated_cluster_faces) for i, df_b in enumerate(df_beams): o_face_clusters.append([]) rh_b_mesh_faces = [df_b_f.to_mesh() for df_b_f in df_b.side_faces] + for j in range(len(rh_b_mesh_faces)): + rh_b_mesh_faces[j].Transform(transforms[i]) df_b_mesh_faces = [df_cvt_bindings.cvt_rhmesh_2_dfmesh(rh_b_mesh_face) for rh_b_mesh_face in rh_b_mesh_faces] dfb_segmentation.DFSegmentation.clean_unassociated_clusters( is_roundwood=df_b.is_roundwood, + discriminate_points=True, unassociated_clusters=df_clouds, associated_clusters=[df_asssociated_cluster_faces_per_beam[i]], reference_mesh=[df_b_mesh_faces], angle_threshold=i_angle_threshold, association_threshold=i_association_threshold, - angle_association_threshold=i_angle_association_threshold + maximum_face_segment_distance=i_maximum_face_segment_distance ) o_face_clusters[-1] = [df_cvt_bindings.cvt_dfcloud_2_rhcloud(cluster) for cluster in df_asssociated_cluster_faces_per_beam[i]] diff --git a/src/gh/components/DF_CAD_segmentator/metadata.json b/src/gh/components/DF_CAD_segmentator/metadata.json index ddfe81bd..2146f6a1 100644 --- a/src/gh/components/DF_CAD_segmentator/metadata.json +++ b/src/gh/components/DF_CAD_segmentator/metadata.json @@ -62,9 +62,45 @@ "typeHintID": "float" }, { - "name": "i_angle_association_threshold", - "nickname": "i_angle_association_threshold", - "description": "A number to indicate how much distance in the plane of the face should be favored, compared to distance orthogonal to the face normal. Default is 0.5", + "name": "i_maximum_face_segment_distance", + "nickname": "i_maximum_face_segment_distance", + "description": "The maximum correspondence distance for the segmentation. Default is 0.1", + "optional": false, + "allowTreeAccess": true, + "showTypeHints": true, + "scriptParamAccess": "item", + "wireDisplay": "default", + "sourceCount": 0, + "typeHintID": "float" + }, + { + "name": "i_radius_normal_estimation", + "nickname": "i_radius_normal_estimation", + "description": "The radius used for normal estimation. Default is 0.01", + "optional": true, + "allowTreeAccess": true, + "showTypeHints": true, + "scriptParamAccess": "item", + "wireDisplay": "default", + "sourceCount": 0, + "typeHintID": "float" + }, + { + "name": "i_make_registration", + "nickname": "i_make_registration", + "description": "Whether to make a registration of the clusters on the mesh faces. Default is True.", + "optional": true, + "allowTreeAccess": true, + "showTypeHints": true, + "scriptParamAccess": "item", + "wireDisplay": "default", + "sourceCount": 0, + "typeHintID": "bool" + }, + { + "name": "i_max_correspondence_distance_icp", + "nickname": "i_max_correspondence_distance_icp", + "description": "The maximum correspondence distance for the ICP algorithm. Default is 0.1", "optional": true, "allowTreeAccess": true, "showTypeHints": true, diff --git a/src/gh/components/DF_pose_estimation/code.py b/src/gh/components/DF_pose_estimation/code.py index b1a9fab1..60af0e56 100644 --- a/src/gh/components/DF_pose_estimation/code.py +++ b/src/gh/components/DF_pose_estimation/code.py @@ -47,24 +47,20 @@ def RunScript(self, continue rh_face_normals.append(Rhino.Geometry.Vector3d(plane_normal[0], plane_normal[1], plane_normal[2])) - df_bb_points = df_cloud.get_axis_aligned_bounding_box() - df_bb_centroid = (df_bb_points[0] + df_bb_points[1]) / 2 - rh_bb_centroid = Rhino.Geometry.Point3d(df_bb_centroid[0], df_bb_centroid[1], df_bb_centroid[2]) - + df_bb_points = df_cloud.get_tight_bounding_box() + df_bb_centroid = sum(df_bb_points)/len(df_bb_points) + rh_tentative_bb_centroid = Rhino.Geometry.Point3d(df_bb_centroid[0], df_bb_centroid[1], df_bb_centroid[2]) new_xDirection, new_yDirection = df_poses.select_vectors(rh_face_normals, i_assembly.beams[i].plane.XAxis, i_assembly.beams[i].plane.YAxis) - if not new_yDirection: - df_beam_pc = dfb_geometry.DFPointCloud() - for face_cloud in face_clouds: - df_face_cloud = df_cvt_bindings.cvt_rhcloud_2_dfcloud(face_cloud) - df_beam_pc.add_points(df_face_cloud) - corners = df_beam_pc.get_tight_bounding_box() - rh_corners = [Rhino.Geometry.Point3d(pt[0], pt[1], pt[2]) for pt in corners] - plane = Rhino.Geometry.Plane.CreateFromPoints(rh_corners[0],rh_corners[1],rh_corners[2]) - box = Rhino.Geometry.Box(plane, rh_corners) - longest_edge = sorted(box.ToBrep().Edges, key=lambda e: e.GetLength())[-1] - longest_edge_direction = longest_edge.TangentAtEnd - new_yDirection = Rhino.Geometry.Vector3d.CrossProduct(new_xDirection, longest_edge_direction) + rh_tentative_plane = Rhino.Geometry.Plane(rh_tentative_bb_centroid, new_yDirection, new_xDirection) + + rh_beam_cloud = Rhino.Geometry.PointCloud() + for face_cloud in face_clouds: + rh_beam_cloud.Merge(face_cloud) + + rh_bbox = rh_beam_cloud.GetBoundingBox(rh_tentative_plane) + rh_bbox.Transform(Rhino.Geometry.Transform.PlaneToPlane(Rhino.Geometry.Plane.WorldXY, rh_tentative_plane)) + rh_bb_centroid = rh_bbox.Center pose = df_poses.DFPose( origin = [rh_bb_centroid.X, rh_bb_centroid.Y, rh_bb_centroid.Z],