diff --git a/src/gh/components/DF_CAD_segmentator/code.py b/src/gh/components/DF_CAD_segmentator/code.py index 13371761..e7654613 100644 --- a/src/gh/components/DF_CAD_segmentator/code.py +++ b/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_initial_pose = df_b.plane + if beam_initial_pose.Transform(transforms[i]): + beam_detected_pose = beam_initial_pose + o_poses_from_icp.append(beam_detected_pose) + 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] diff --git a/src/gh/components/DF_CAD_segmentator/metadata.json b/src/gh/components/DF_CAD_segmentator/metadata.json index 2146f6a1..e47901bb 100644 --- a/src/gh/components/DF_CAD_segmentator/metadata.json +++ b/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 be empty.", + "optional": false, + "sourceCount": 0, + "graft": false } ] } diff --git a/src/gh/components/DF_csv_exporter/code.py b/src/gh/components/DF_csv_exporter/code.py index b27f5671..fcf694da 100644 --- a/src/gh/components/DF_csv_exporter/code.py +++ b/src/gh/components/DF_csv_exporter/code.py @@ -8,7 +8,7 @@ from ghpythonlib.componentbase import executingcomponent as component import Grasshopper as gh -from diffCheck.df_error_estimation import DFInvalidData, DFVizResults +from diffCheck.df_error_estimation import DFInvalidData, DFVizResults, DFPoseResults def add_bool_toggle(self, @@ -170,21 +170,50 @@ def RunScript(self, if i_dump: os.makedirs(i_export_dir, exist_ok=True) - self.prefix = i_result.analysis_type - - if i_export_seperate_files: - for idx in range(len(i_result.source)): - element_id = self._get_id(idx, i_result) - csv_analysis_path = os.path.join(i_export_dir, f"{i_file_name}_{self.prefix}_{element_id}.csv") - rows = [self._prepare_row(idx, i_result)] - self._write_csv(csv_analysis_path, rows) + if isinstance(i_result, DFVizResults): + self.prefix = i_result.analysis_type + + if i_export_seperate_files: + for idx in range(len(i_result.source)): + element_id = self._get_id(idx, i_result) + csv_analysis_path = os.path.join(i_export_dir, f"{i_file_name}_{self.prefix}_{element_id}.csv") + rows = [self._prepare_row(idx, i_result)] + self._write_csv(csv_analysis_path, rows) + if i_export_distances: + csv_distances_path = os.path.join(i_export_dir, f"{i_file_name}_{self.prefix}_{element_id}_distances.csv") + self._write_csv(csv_distances_path, rows, is_writing_only_distances=True) + else: + csv_analysis_path = os.path.join(i_export_dir, f"{i_file_name}.csv") + merged_rows = [self._prepare_row(idx, i_result) for idx in range(len(i_result.source))] + self._write_csv(csv_analysis_path, merged_rows) if i_export_distances: - csv_distances_path = os.path.join(i_export_dir, f"{i_file_name}_{self.prefix}_{element_id}_distances.csv") - self._write_csv(csv_distances_path, rows, is_writing_only_distances=True) - else: - csv_analysis_path = os.path.join(i_export_dir, f"{i_file_name}.csv") - merged_rows = [self._prepare_row(idx, i_result) for idx in range(len(i_result.source))] - self._write_csv(csv_analysis_path, merged_rows) - if i_export_distances: - csv_distances_path = os.path.join(i_export_dir, f"{i_file_name}_distances.csv") - self._write_csv(csv_distances_path, merged_rows, is_writing_only_distances=True) + csv_distances_path = os.path.join(i_export_dir, f"{i_file_name}_distances.csv") + self._write_csv(csv_distances_path, merged_rows, is_writing_only_distances=True) + + elif isinstance(i_result, DFPoseResults): + data_dicts = [] + csv_analysis_path = os.path.join(i_export_dir, f"{i_file_name}_pose_data.csv") + elem_names, elem_last_dist_err, elem_last_rot_err, assembly_dist_err_hist, assembly_rot_err_hist = i_result.compute_history_pose_errors() + for i in range(len(elem_names)): + dist_err_list = [] + rot_err_list = [] + for data in assembly_dist_err_hist[i]: + if data : + dist_err_list.append(float(data)) + else: + dist_err_list.append("nan") + for data in assembly_rot_err_hist[i]: + if data: + rot_err_list.append(float(data)) + else: + rot_err_list.append("nan") + + data_dict = { + "element_name": elem_names[i], + "last_distance_error": elem_last_dist_err[i], + "last_rotation_error": elem_last_rot_err[i], + "assembly_distance_error_history": dist_err_list, + "assembly_rotation_error_history": rot_err_list + } + data_dicts.append(data_dict) + self._write_csv(csv_analysis_path, data_dicts) diff --git a/src/gh/components/DF_pose_comparison/code.py b/src/gh/components/DF_pose_comparison/code.py index 267afd9d..bf3da573 100644 --- a/src/gh/components/DF_pose_comparison/code.py +++ b/src/gh/components/DF_pose_comparison/code.py @@ -7,6 +7,8 @@ import ghpythonlib.treehelpers as th import diffCheck.df_geometries +import diffCheck.df_error_estimation + import numpy def compute_comparison(measured_pose, cad_pose): @@ -57,6 +59,7 @@ def RunScript(self, i_assembly: diffCheck.df_geometries.DFAssembly, i_measured_p o_distances[beam_id].append(dist) o_angles[beam_id].append(angle) o_transforms_cad_to_measured[beam_id].append(transform_cad_to_measured) + else: i_measured_planes.Flatten() measured_plane_list = th.tree_to_list(i_measured_planes) @@ -67,7 +70,12 @@ def RunScript(self, i_assembly: diffCheck.df_geometries.DFAssembly, i_measured_p o_angles.append(angle) o_transforms_cad_to_measured.append(transform_cad_to_measured) + + df_poses_assembly = diffCheck.df_poses.DFPosesAssembly().from_gh_tree(i_measured_planes) + o_result = diffCheck.df_error_estimation.DFErrorEstimation(i_assembly) + o_result.add_history(df_poses_assembly) + if bc == 1: - return o_distances, o_angles, o_transforms_cad_to_measured + return o_distances, o_angles, o_transforms_cad_to_measured, o_result else: - return th.list_to_tree(o_distances), th.list_to_tree(o_angles), th.list_to_tree(o_transforms_cad_to_measured) + return th.list_to_tree(o_distances), th.list_to_tree(o_angles), th.list_to_tree(o_transforms_cad_to_measured), o_result diff --git a/src/gh/components/DF_pose_comparison/metadata.json b/src/gh/components/DF_pose_comparison/metadata.json index c9ad3a8e..e3130c95 100644 --- a/src/gh/components/DF_pose_comparison/metadata.json +++ b/src/gh/components/DF_pose_comparison/metadata.json @@ -61,6 +61,14 @@ "optional": false, "sourceCount": 0, "graft": false + }, + { + "name": "o_result", + "nickname": "o_result", + "description": "A DFPoseResults object containing detailed error information for each pose comparison.", + "optional": false, + "sourceCount": 0, + "graft": false } ] } diff --git a/src/gh/components/DF_pose_estimation/code.py b/src/gh/components/DF_pose_estimation/code.py index 7db7703b..3ebc8db5 100644 --- a/src/gh/components/DF_pose_estimation/code.py +++ b/src/gh/components/DF_pose_estimation/code.py @@ -17,6 +17,7 @@ class DFPoseEstimation(component): def RunScript(self, i_face_clouds: Grasshopper.DataTree[Rhino.Geometry.PointCloud], i_assembly, + i_poses_from_icp: list, i_reset: bool, i_save: bool): @@ -35,40 +36,47 @@ def RunScript(self, all_poses_this_time = [] for i, face_clouds in enumerate(clusters_per_beam): try: - df_cloud = dfb_geometry.DFPointCloud() - - rh_face_normals = [] - for face_cloud in face_clouds: - df_face_cloud = df_cvt_bindings.cvt_rhcloud_2_dfcloud(face_cloud) - df_cloud.add_points(df_face_cloud) - plane_normal = df_face_cloud.fit_plane_ransac() - if all(plane_normal) == 0: - ghenv.Component.AddRuntimeMessage(RML.Warning, f"There was a missing face in the cloud of beam {i}: the face was skipped in the pose estimation of that beam") # noqa: F821 - continue - rh_face_normals.append(Rhino.Geometry.Vector3d(plane_normal[0], plane_normal[1], plane_normal[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) - rh_tentative_plane = Rhino.Geometry.Plane(rh_tentative_bb_centroid, new_xDirection, new_yDirection) - - 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], - xDirection = [new_xDirection.X, new_xDirection.Y, new_xDirection.Z], - yDirection = [new_yDirection.X, new_yDirection.Y, new_yDirection.Z]) - all_poses_this_time.append(pose) - plane = Rhino.Geometry.Plane(origin = rh_bb_centroid, xDirection=new_xDirection, yDirection=new_yDirection) - planes.append(plane) + if len(i_poses_from_icp) > i and i_poses_from_icp[i] is not None: + # if there is a pose from ICP for this beam, use it directly without processing the cloud + planes.append(i_poses_from_icp[i]) + all_poses_this_time.append(df_poses.DFPose.from_rh_plane(i_poses_from_icp[i])) + continue + + else: + df_cloud = dfb_geometry.DFPointCloud() + + rh_face_normals = [] + for face_cloud in face_clouds: + df_face_cloud = df_cvt_bindings.cvt_rhcloud_2_dfcloud(face_cloud) + df_cloud.add_points(df_face_cloud) + plane_normal = df_face_cloud.fit_plane_ransac() + if all(plane_normal) == 0: + ghenv.Component.AddRuntimeMessage(RML.Warning, f"There was a missing face in the cloud of beam {i}: the face was skipped in the pose estimation of that beam") # noqa: F821 + continue + rh_face_normals.append(Rhino.Geometry.Vector3d(plane_normal[0], plane_normal[1], plane_normal[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) + rh_tentative_plane = Rhino.Geometry.Plane(rh_tentative_bb_centroid, new_xDirection, new_yDirection) + + 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], + xDirection = [new_xDirection.X, new_xDirection.Y, new_xDirection.Z], + yDirection = [new_yDirection.X, new_yDirection.Y, new_yDirection.Z]) + all_poses_this_time.append(pose) + plane = Rhino.Geometry.Plane(origin = rh_bb_centroid, xDirection=new_xDirection, yDirection=new_yDirection) + planes.append(plane) except Exception as e: # Any unexpected error on this cloud, skip it and keep going diff --git a/src/gh/components/DF_pose_estimation/metadata.json b/src/gh/components/DF_pose_estimation/metadata.json index f7c780ae..f09bc53f 100644 --- a/src/gh/components/DF_pose_estimation/metadata.json +++ b/src/gh/components/DF_pose_estimation/metadata.json @@ -37,6 +37,18 @@ "sourceCount": 0, "typeHintID": "ghdoc" }, + { + "name": "i_poses_from_icp", + "nickname": "i_poses_from_icp", + "description": "The optional poses detected in the CAD segmentation component though ICP. If provided, the pose will not be re-calculated using the faces normals.", + "optional": true, + "allowTreeAccess": true, + "showTypeHints": true, + "scriptParamAccess": "list", + "wireDisplay": "default", + "sourceCount": 0, + "typeHintID": "ghdoc" + }, { "name": "i_reset", "nickname": "i_reset", diff --git a/src/gh/diffCheck/diffCheck/df_error_estimation.py b/src/gh/diffCheck/diffCheck/df_error_estimation.py index b9ab03c0..e4ec22dc 100644 --- a/src/gh/diffCheck/diffCheck/df_error_estimation.py +++ b/src/gh/diffCheck/diffCheck/df_error_estimation.py @@ -19,6 +19,7 @@ from diffCheck import diffcheck_bindings # type: ignore from diffCheck import df_cvt_bindings from diffCheck.df_geometries import DFAssembly +from diffCheck.df_poses import DFPosesBeam, DFPose class NumpyEncoder(json.JSONEncoder): @@ -268,6 +269,63 @@ def analysis_type(self): self._analysis_type = self._compute_dfresult_type() return self._analysis_type +class DFPoseResults(): + """ + This class compiles the results of the pose estimation into one object + """ + def __init__(self, assembly: DFAssembly): + self.assembly = assembly + self.pose_history : dict[str, DFPosesBeam] = dict() + self.last_poses : dict[str, DFPose] = dict() + + def add_history(self, pose_history : dict[str, DFPosesBeam]): + """ + The pose history is a dictionnary where the keys are the element names ("element_0", "element_1", etc), + and the values are DFPosesBeam objects containing a dictionnary of poses for each element. + """ + self.pose_history = pose_history + for element in pose_history: + self.last_poses[element] = pose_history[element].poses_dictionary[list(pose_history[element].poses_dictionary.keys())[-1]] + + def add_last_poses(self, last_poses : dict[str, DFPose]): + """ + Adds a dictionnary of the last poses for each element. The keys are the element names_0", "element_1", etc), + """ + self.last_poses = last_poses + + def compute_history_pose_errors(self): + """ + This function computes the error of the pose estimation for each element at each step, compared to the poses defined in the assembly. + """ + element_names = [] + element_last_dist_errors = [] + element_last_rot_errors = [] + assembly_dist_error_history = [] + assembly_rot_error_history = [] + + for element_name, df_poses_beam in self.pose_history.items(): + element_index = int(element_name.split("_")[1]) + df_beam = self.assembly.beams[element_index] + df_beam_pose_plane = df_beam.plane + dist_error_history = [] + rot_error_history = [] + for pose_name, pose in df_poses_beam.poses_dictionary.items(): + if pose is None: + dist_error_history.append(None) + rot_error_history.append(None) + continue + else: + dist, angle, transform_error = pose.compare_to_rh_plane(df_beam_pose_plane) + dist_error_history.append(dist) + rot_error_history.append(angle) + assembly_dist_error_history.append(dist_error_history) + assembly_rot_error_history.append(rot_error_history) + element_names.append(element_name) + element_last_dist_errors.append(dist_error_history[-1]) + element_last_rot_errors.append(rot_error_history[-1]) + return element_names, element_last_dist_errors, element_last_rot_errors, assembly_dist_error_history, assembly_rot_error_history + + # FIXME: ths is currently broken, we need to fix it def df_cloud_2_df_cloud_comparison( assembly: DFAssembly, diff --git a/src/gh/diffCheck/diffCheck/df_poses.py b/src/gh/diffCheck/diffCheck/df_poses.py index eee7a694..a1de4509 100644 --- a/src/gh/diffCheck/diffCheck/df_poses.py +++ b/src/gh/diffCheck/diffCheck/df_poses.py @@ -3,6 +3,7 @@ import Rhino import json +import numpy from dataclasses import dataclass, field # use a key and not all the sticky @@ -21,6 +22,19 @@ class DFPose: xDirection: list yDirection: list + @staticmethod + def from_rh_plane(rh_plane): + """ + Create a DFPose object from a Rhino Plane object. + + :param rh_plane: the Rhino Plane to convert + :return: a DFPose object representing the same pose as the input Rhino Plane + """ + return DFPose( + origin = [rh_plane.Origin.X, rh_plane.Origin.Y, rh_plane.Origin.Z], + xDirection = [rh_plane.XAxis.X, rh_plane.XAxis.Y, rh_plane.XAxis.Z], + yDirection = [rh_plane.YAxis.X, rh_plane.YAxis.Y, rh_plane.YAxis.Z]) + def to_rh_plane(self): """ Convert the pose to a Rhino Plane object. @@ -30,6 +44,31 @@ def to_rh_plane(self): yDirection = Rhino.Geometry.Vector3d(self.yDirection[0], self.yDirection[1], self.yDirection[2]) return Rhino.Geometry.Plane(origin, xDirection, yDirection) + def compare_to_rh_plane(self, rh_plane): + """ + Compare this pose to another pose and return the differences in origin, xDirection and yDirection. + + :param rh_plane: the Rhino Plane to compare to + :return: a tuple containing the distance between the origins, the angle between the xDirections and the rhino transform to go from the compared pose to this pose. + """ + other_origin = rh_plane.Origin + measured_origin = self.to_rh_plane().Origin + distance = other_origin.DistanceTo(measured_origin) + + # Compare the orientations using the formula: $$ \theta = \arccos\left(\frac{\text{trace}(R_{\text{pred}}^T R_{\text{meas}}) - 1}{2}\right) $$ + transform_o_to_other = Rhino.Geometry.Transform.PlaneToPlane(Rhino.Geometry.Plane.WorldXY, rh_plane) + transform_o_to_current = Rhino.Geometry.Transform.PlaneToPlane(Rhino.Geometry.Plane.WorldXY, self.to_rh_plane()) + np_transform_o_to_other = numpy.array(transform_o_to_other.ToDoubleArray(rowDominant=True)).reshape((4, 4)) + np_transform_o_to_measured = numpy.array(transform_o_to_current.ToDoubleArray(rowDominant=True)).reshape((4, 4)) + + R_other = np_transform_o_to_other[:3, :3] + R_measured = np_transform_o_to_measured[:3, :3] + R_rel = numpy.dot(R_other.T, R_measured) + theta = numpy.arccos(numpy.clip((numpy.trace(R_rel) - 1) / 2, -1.0, 1.0)) + + transform_other_to_current_plane = Rhino.Geometry.Transform.PlaneToPlane(rh_plane, self.to_rh_plane()) + return distance, theta, transform_other_to_current_plane + @dataclass class DFPosesBeam: """ @@ -117,6 +156,31 @@ def to_gh_tree(self): list_of_poses.append(list_of_pose_of_element) return th.list_to_tree(list_of_poses) + def from_gh_tree(self, gh_tree): + """ + Load the assembly poses from a Grasshopper tree structure. + + :param gh_tree: the Grasshopper tree containing the poses in the form of Rhino Planes + """ + bc = gh_tree.BranchCount + if bc > 1: + list_of_poses = th.tree_to_list(gh_tree) + else: + gh_tree.Flatten() + list_of_poses = [th.tree_to_list(gh_tree)] + n_poses = len(list_of_poses[0]) if list_of_poses else 0 + for i in range(n_poses): + new_poses = [] + for poses_of_element in list_of_poses: + if poses_of_element[i] is None: + new_poses.append(None) + continue + new_poses.append(DFPose( + origin = [poses_of_element[i].Origin.X, poses_of_element[i].Origin.Y, poses_of_element[i].Origin.Z], + xDirection = [poses_of_element[i].XAxis.X, poses_of_element[i].XAxis.Y, poses_of_element[i].XAxis.Z], + yDirection = [poses_of_element[i].YAxis.X, poses_of_element[i].YAxis.Y, poses_of_element[i].YAxis.Z])) + self.add_step(new_poses) + def compute_dot_product(v1, v2): """