diff --git a/doc/_static/example_files/subtractive_gh_v1.gh b/doc/_static/example_files/subtractive_gh_v1.gh index bdc5feb1..83d94854 100644 Binary files a/doc/_static/example_files/subtractive_gh_v1.gh and b/doc/_static/example_files/subtractive_gh_v1.gh differ diff --git a/pyproject.toml b/pyproject.toml index 1b7616fe..032c09f7 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -20,7 +20,8 @@ module = [ "GH_IO.*", "clr.*", "diffcheck_bindings", - "diffCheck.diffcheck_bindings" + "diffCheck.diffcheck_bindings", + "ghpythonlib.*" ] ignore_missing_imports = true diff --git a/src/diffCheck.hh b/src/diffCheck.hh index 733801aa..31dec076 100644 --- a/src/diffCheck.hh +++ b/src/diffCheck.hh @@ -6,6 +6,9 @@ #include #include +#include + +#include // diffCheck includes #include "diffCheck/log.hh" diff --git a/src/diffCheck/IOManager.cc b/src/diffCheck/IOManager.cc index 2a857950..2da591a8 100644 --- a/src/diffCheck/IOManager.cc +++ b/src/diffCheck/IOManager.cc @@ -68,4 +68,11 @@ namespace diffCheck::io std::filesystem::path pathCloud = pathTestData / "test_pc_for_SOR_101pts_with_1_outlier.ply"; return pathCloud.string(); } + + std::string GetTwoConnectedPlanesPlyPath() + { + std::filesystem::path pathTestData = GetTestDataDir(); + std::filesystem::path pathCloud = pathTestData / "two_connected_planes_with_normals.ply"; + return pathCloud.string(); + } } // namespace diffCheck::io \ No newline at end of file diff --git a/src/diffCheck/IOManager.hh b/src/diffCheck/IOManager.hh index e22c029d..646ce511 100644 --- a/src/diffCheck/IOManager.hh +++ b/src/diffCheck/IOManager.hh @@ -42,4 +42,6 @@ namespace diffCheck::io std::string GetRoofQuarterPlyPath(); /// @brief Get the path to the plane point cloud with one outlier std::string GetPlanePCWithOneOutliers(); + /// @brief Get the path to the two connected planes ply test file + std::string GetTwoConnectedPlanesPlyPath(); } // namespace diffCheck::io \ No newline at end of file diff --git a/src/diffCheck/geometry/DFPointCloud.cc b/src/diffCheck/geometry/DFPointCloud.cc index 5b016eda..c371c06c 100644 --- a/src/diffCheck/geometry/DFPointCloud.cc +++ b/src/diffCheck/geometry/DFPointCloud.cc @@ -216,6 +216,76 @@ namespace diffCheck::geometry this->Normals.push_back(normal); } + std::vector DFPointCloud::GetPrincipalAxes(int nComponents) + { + std::vector principalAxes; + + if (! this->HasNormals()) + { + DIFFCHECK_WARN("The point cloud has no normals. Normals will be estimated with knn = 20."); + this->EstimateNormals(true, 20); + } + + // Convert normals to Eigen matrix + Eigen::Matrix normalMatrix(3, this->Normals.size()); + for (size_t i = 0; i < this->Normals.size(); ++i) + { + normalMatrix.col(i) = this->Normals[i].cast(); + } + + cilantro::KMeans kmeans(normalMatrix); + kmeans.cluster(nComponents); + + const cilantro::VectorSet3d& centroids = kmeans.getClusterCentroids(); + const std::vector& assignments = kmeans.getPointToClusterIndexMap(); + std::vector clusterSizes(nComponents, 0); + for (size_t i = 0; i < assignments.size(); ++i) + { + clusterSizes[assignments[i]]++; + } + // Sort clusters by size + std::vector> sortedClustersBySize(nComponents); + for (size_t i = 0; i < nComponents; ++i) + { + sortedClustersBySize[i] = {clusterSizes[i], centroids.col(i)}; + } + std::sort(sortedClustersBySize.begin(), sortedClustersBySize.end(), [](const auto& a, const auto& b) + { + return a.first > b.first; + }); + + for(size_t i = 0; i < nComponents; ++i) + { + if(principalAxes.size() == 0) + { + principalAxes.push_back(sortedClustersBySize[i].second); + } + else + { + bool isAlreadyPresent = false; + for (const auto& axis : principalAxes) + { + double dotProduct = std::abs(axis.dot(sortedClustersBySize[i].second)); + if (std::abs(dotProduct) > 0.7) // Threshold to consider as similar direction + { + isAlreadyPresent = true; + break; + } + } + if (!isAlreadyPresent) + { + principalAxes.push_back(sortedClustersBySize[i].second); + } + } + } + if (principalAxes.size() < 2) // Fallback to OBB if k-means fails to provide enough distinct axes + { + open3d::geometry::OrientedBoundingBox obb = this->Cvt2O3DPointCloud()->GetOrientedBoundingBox(); + principalAxes = {obb.R_.col(0), obb.R_.col(1), obb.R_.col(2)}; + } + return principalAxes; + } + void DFPointCloud::Crop(const Eigen::Vector3d &minBound, const Eigen::Vector3d &maxBound) { auto O3DPointCloud = this->Cvt2O3DPointCloud(); diff --git a/src/diffCheck/geometry/DFPointCloud.hh b/src/diffCheck/geometry/DFPointCloud.hh index 8a65d380..86658cc9 100644 --- a/src/diffCheck/geometry/DFPointCloud.hh +++ b/src/diffCheck/geometry/DFPointCloud.hh @@ -8,6 +8,8 @@ #include #include +#include + namespace diffCheck::geometry { @@ -89,6 +91,14 @@ namespace diffCheck::geometry */ void RemoveStatisticalOutliers(int nbNeighbors, double stdRatio); + /** + * @brief Get the nCompoments principal axes of the normals of the point cloud + * It is used to compute the pose of "boxy" point clouds. It relies on KMeans clustering to find the main axes of the point cloud. + * @param nComponents the number of components to compute (default 6, each of 3 main axes in both directions) + * @return std::vector the principal axes of the point cloud ordered by number of normals + */ + std::vector GetPrincipalAxes(int nComponents = 6); + /** * @brief Crop the point cloud to a bounding box defined by the min and max bounds * diff --git a/src/diffCheckBindings.cc b/src/diffCheckBindings.cc index 1e5ad07e..048a3370 100644 --- a/src/diffCheckBindings.cc +++ b/src/diffCheckBindings.cc @@ -61,6 +61,8 @@ PYBIND11_MODULE(diffcheck_bindings, m) { .def("remove_statistical_outliers", &diffCheck::geometry::DFPointCloud::RemoveStatisticalOutliers, py::arg("nb_neighbors"), py::arg("std_ratio")) + .def("get_principal_axes", &diffCheck::geometry::DFPointCloud::GetPrincipalAxes, + py::arg("n_components") = 6) .def("crop", (void (diffCheck::geometry::DFPointCloud::*)(const Eigen::Vector3d&, const Eigen::Vector3d&)) &diffCheck::geometry::DFPointCloud::Crop, diff --git a/src/gh/components/DF_pose_estimation/code.py b/src/gh/components/DF_pose_estimation/code.py new file mode 100644 index 00000000..4ab24063 --- /dev/null +++ b/src/gh/components/DF_pose_estimation/code.py @@ -0,0 +1,68 @@ +#! python3 + +from diffCheck import df_cvt_bindings +from diffCheck import df_poses + +import Rhino +from Grasshopper.Kernel import GH_RuntimeMessageLevel as RML + +from ghpythonlib.componentbase import executingcomponent as component +import System + + +class DFPoseEstimation(component): + def RunScript(self, + i_clouds: System.Collections.Generic.List[Rhino.Geometry.PointCloud], + i_assembly, + i_save: bool, + i_reset: bool): + + # ensure assembly has enough beams + if len(i_assembly.beams) < len(i_clouds): + ghenv.Component.AddRuntimeMessage(RML.Warning, "Assembly has fewer beams than input clouds") # noqa: F821 + return None, None + + planes = [] + all_poses_in_time = df_poses.DFPosesAssembly() + if i_reset: + all_poses_in_time.reset() + return None, None + + all_poses_this_time = [] + for i, cloud in enumerate(i_clouds): + try: + df_cloud = df_cvt_bindings.cvt_rhcloud_2_dfcloud(cloud) + if df_cloud is None: + return None, None + if not df_cloud.has_normals(): + ghenv.Component.AddRuntimeMessage(RML.Error, f"Point cloud {i} has no normals. Please compute the normals.") # noqa: F821 + + df_points = df_cloud.get_axis_aligned_bounding_box() + df_point = (df_points[0] + df_points[1]) / 2 + rh_point = Rhino.Geometry.Point3d(df_point[0], df_point[1], df_point[2]) + + axes = df_cloud.get_principal_axes(3) + vectors = [] + for axe in axes: + vectors.append(Rhino.Geometry.Vector3d(axe[0], axe[1], axe[2])) + + new_xDirection, new_yDirection = df_poses.select_vectors(vectors, i_assembly.beams[i].plane.XAxis, i_assembly.beams[i].plane.YAxis) + + pose = df_poses.DFPose( + origin = [rh_point.X, rh_point.Y, rh_point.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_point, xDirection=new_xDirection, yDirection=new_yDirection) + planes.append(plane) + except Exception as e: + # Any unexpected error on this cloud, skip it and keep going + ghenv.Component.AddRuntimeMessage(RML.Error, f"Cloud {i}: processing failed ({e}); skipping.") # noqa: F821 + planes.append(None) + all_poses_this_time.append(None) + continue + + if i_save: + all_poses_in_time.add_step(all_poses_this_time) + + return [planes, all_poses_in_time.to_gh_tree()] diff --git a/src/gh/components/DF_pose_estimation/icon.png b/src/gh/components/DF_pose_estimation/icon.png new file mode 100644 index 00000000..1240418f Binary files /dev/null and b/src/gh/components/DF_pose_estimation/icon.png differ diff --git a/src/gh/components/DF_pose_estimation/metadata.json b/src/gh/components/DF_pose_estimation/metadata.json new file mode 100644 index 00000000..60d1f363 --- /dev/null +++ b/src/gh/components/DF_pose_estimation/metadata.json @@ -0,0 +1,84 @@ +{ + "name": "DFPoseEstimation", + "nickname": "PoseEsimation", + "category": "diffCheck", + "subcategory": "PointCloud", + "description": "This compoment calculates the pose of a list of point clouds.", + "exposure": 4, + "instanceGuid": "a13c4414-f5df-46e6-beae-7054bb9c3e72", + "ghpython": { + "hideOutput": true, + "hideInput": true, + "isAdvancedMode": true, + "marshalOutGuids": true, + "iconDisplay": 2, + "inputParameters": [ + { + "name": "i_clouds", + "nickname": "i_clouds", + "description": "clouds whose pose is to be calculated", + "optional": false, + "allowTreeAccess": true, + "showTypeHints": true, + "scriptParamAccess": "list", + "wireDisplay": "default", + "sourceCount": 0, + "typeHintID": "pointcloud" + }, + { + "name": "i_assembly", + "nickname": "i_assembly", + "description": "The DFAssembly corresponding to the list of clouds.", + "optional": false, + "allowTreeAccess": true, + "showTypeHints": true, + "scriptParamAccess": "item", + "wireDisplay": "default", + "sourceCount": 0, + "typeHintID": "ghdoc" + }, + { + "name": "i_reset", + "nickname": "i_reset", + "description": "reset the history of the pose estimation", + "optional": true, + "allowTreeAccess": false, + "showTypeHints": true, + "scriptParamAccess": "item", + "wireDisplay": "default", + "sourceCount": 0, + "typeHintID": "bool" + }, + { + "name": "i_save", + "nickname": "i_save", + "description": "save the poses computed at this iteration", + "optional": true, + "allowTreeAccess": false, + "showTypeHints": true, + "scriptParamAccess": "item", + "wireDisplay": "default", + "sourceCount": 0, + "typeHintID": "bool" + } + ], + "outputParameters": [ + { + "name": "o_planes", + "nickname": "o_planes", + "description": "The resulting planes of the pose estimation in the last iteration.", + "optional": false, + "sourceCount": 0, + "graft": false + }, + { + "name": "o_history", + "nickname": "o_history", + "description": "The history of poses per elements.", + "optional": false, + "sourceCount": 0, + "graft": false + } + ] + } +} \ No newline at end of file diff --git a/src/gh/components/DF_truncate_assembly/code.py b/src/gh/components/DF_truncate_assembly/code.py new file mode 100644 index 00000000..9312b74f --- /dev/null +++ b/src/gh/components/DF_truncate_assembly/code.py @@ -0,0 +1,15 @@ +from ghpythonlib.componentbase import executingcomponent as component + +import diffCheck +import diffCheck.df_geometries + +class DFTruncateAssembly(component): + def RunScript(self, + i_assembly, + i_truncate_index: int): + beams = i_assembly.beams[:i_truncate_index] + name = i_assembly.name + + o_assembly = diffCheck.df_geometries.DFAssembly(name=name, beams=beams) + ghenv.Component.Message = f"number of beams: {len(o_assembly.beams)}" # noqa: F821 + return o_assembly diff --git a/src/gh/components/DF_truncate_assembly/icon.png b/src/gh/components/DF_truncate_assembly/icon.png new file mode 100644 index 00000000..d15d8142 Binary files /dev/null and b/src/gh/components/DF_truncate_assembly/icon.png differ diff --git a/src/gh/components/DF_truncate_assembly/metadata.json b/src/gh/components/DF_truncate_assembly/metadata.json new file mode 100644 index 00000000..ec63631d --- /dev/null +++ b/src/gh/components/DF_truncate_assembly/metadata.json @@ -0,0 +1,52 @@ +{ + "name": "DFTruncateAssembly", + "nickname": "TruncateAssembly", + "category": "diffCheck", + "subcategory": "Structure", + "description": "This component truncates an assembly.", + "exposure": 4, + "instanceGuid": "cf8af97f-dd84-40b6-af44-bf6aca7b941b", + "ghpython": { + "hideOutput": true, + "hideInput": true, + "isAdvancedMode": true, + "marshalOutGuids": true, + "iconDisplay": 2, + "inputParameters": [ + { + "name": "i_assembly", + "nickname": "i_assembly", + "description": "The assembly to be truncated.", + "optional": false, + "allowTreeAccess": true, + "showTypeHints": true, + "scriptParamAccess": "item", + "wireDisplay": "default", + "sourceCount": 0, + "typeHintID": "ghdoc" + }, + { + "name": "i_truncate_index", + "nickname": "i_truncate_index", + "description": "The index at which to truncate the assembly.", + "optional": false, + "allowTreeAccess": false, + "showTypeHints": true, + "scriptParamAccess": "item", + "wireDisplay": "default", + "sourceCount": 0, + "typeHintID": "int" + } + ], + "outputParameters": [ + { + "name": "o_assembly", + "nickname": "o_assembly", + "description": "The resulting assembly after truncation.", + "optional": false, + "sourceCount": 0, + "graft": false + } + ] + } +} \ No newline at end of file diff --git a/src/gh/diffCheck/diffCheck/df_geometries.py b/src/gh/diffCheck/diffCheck/df_geometries.py index 821a0849..541efcd5 100644 --- a/src/gh/diffCheck/diffCheck/df_geometries.py +++ b/src/gh/diffCheck/diffCheck/df_geometries.py @@ -101,6 +101,7 @@ def __post_init__(self): self._center: DFVertex = None # the normal of the face self._normal: typing.List[float] = None + self._area: float = None def __getstate__(self): state = self.__dict__.copy() @@ -261,6 +262,12 @@ def normal(self): self._normal = [normal_rg.X, normal_rg.Y, normal_rg.Z] return self._normal + @property + def area(self): + if self._area is None: + self._area = self.to_brep_face().ToBrep().GetArea() + return self._area + @dataclass class DFJoint: """ @@ -375,6 +382,7 @@ def __post_init__(self): self._center: rg.Point3d = None self._axis: rg.Line = self.compute_axis() + self.plane: rg.Plane = self.compute_plane() self._length: float = self._axis.Length self.__uuid = uuid.uuid4().int @@ -506,6 +514,28 @@ def compute_axis(self, is_unitized: bool = True) -> rg.Line: return axis_ln + def compute_plane(self) -> rg.Plane: + """ + This function computes the plane of the beam based on its axis and the first joint's center. + The plane is oriented along the beam's axis. + + :return plane: The plane of the beam + """ + if not self.joints: + raise ValueError("The beam has no joints to compute a plane") + + #main axis as defined above + main_direction = self.compute_axis().Direction + + #secondary axis as normal to the largest face of the beam + largest_face = max(self.faces, key=lambda f: f.area) + secondary_axis = largest_face.normal + secondary_vector = rg.Vector3d(secondary_axis[0], secondary_axis[1], secondary_axis[2]) + first_vector = rg.Vector3d.CrossProduct(main_direction, secondary_vector) + origin = self.center + + return rg.Plane(origin, first_vector, secondary_vector) + def compute_joint_distances_to_midpoint(self) -> typing.List[float]: """ This function computes the distances from the center of the beam to each joint. diff --git a/src/gh/diffCheck/diffCheck/df_poses.py b/src/gh/diffCheck/diffCheck/df_poses.py new file mode 100644 index 00000000..adaeee16 --- /dev/null +++ b/src/gh/diffCheck/diffCheck/df_poses.py @@ -0,0 +1,152 @@ +from scriptcontext import sticky as rh_sticky_dict +import ghpythonlib.treehelpers as th +import Rhino + +import json +from dataclasses import dataclass, field + +# use a key and not all the sticky +_STICKY_KEY = "df_poses" + +def _get_store(): + # returns private sub-dict inside rhino sticky + return rh_sticky_dict.setdefault(_STICKY_KEY, {}) + +@dataclass +class DFPose: + """ + This class represents the pose of a single element at a given time in the assembly process. + """ + origin: list + xDirection: list + yDirection: list + + def to_rh_plane(self): + """ + Convert the pose to a Rhino Plane object. + """ + origin = Rhino.Geometry.Point3d(self.origin[0], self.origin[1], self.origin[2]) + xDirection = Rhino.Geometry.Vector3d(self.xDirection[0], self.xDirection[1], self.xDirection[2]) + yDirection = Rhino.Geometry.Vector3d(self.yDirection[0], self.yDirection[1], self.yDirection[2]) + return Rhino.Geometry.Plane(origin, xDirection, yDirection) + +@dataclass +class DFPosesBeam: + """ + This class contains the poses of a single beam, at different times in the assembly process. + It also contains the number of faces detected for this element, based on which the poses are calculated. + """ + poses_dictionary: dict + n_faces: int = 3 + + def add_pose(self, pose: DFPose, step_number: int): + """ + Add a pose to the dictionary of poses. + """ + self.poses_dictionary[f"pose_{step_number}"] = pose + + def set_n_faces(self, n_faces: int): + """ + Set the number of faces detected for this element. + """ + self.n_faces = n_faces + +@dataclass +class DFPosesAssembly: + n_step: int = 0 + poses_per_element_dictionary: dict = field(default_factory=_get_store) + + """ + This class contains the poses of the different elements of the assembly, at different times in the assembly process. + """ + def __post_init__(self): + """ + Initialize the poses_per_element_dictionary with empty DFPosesBeam objects. + """ + lengths = [] + for element in self.poses_per_element_dictionary: + lengths.append(len(self.poses_per_element_dictionary[element].poses_dictionary)) + self.n_step = max(lengths) if lengths else 0 + + def add_step(self, new_poses: list[DFPose]): + for i, pose in enumerate(new_poses): + if f"element_{i}" not in self.poses_per_element_dictionary: + self.poses_per_element_dictionary[f"element_{i}"] = DFPosesBeam({}, 4) + for j in range(self.n_step): + self.poses_per_element_dictionary[f"element_{i}"].add_pose(None, j) + self.poses_per_element_dictionary[f"element_{i}"].add_pose(pose, self.n_step) + self.n_step += 1 + + def get_last_poses(self): + """ + Get the last poses of each element. + """ + if self.n_step == 0: + return None + last_poses = [] + for i in range(len(self.poses_per_element_dictionary)): + last_poses.append(self.poses_per_element_dictionary[f"element_{i}"].poses_dictionary[f"pose_{self.n_step-1}"]) + return last_poses + + def reset(self): + """ + Reset the assembly poses to the initial state. + """ + self.n_step = 0 + # clear only namespace + rh_sticky_dict[_STICKY_KEY] = {} + # refresh the local reference to the (now empty) store + self.poses_per_element_dictionary = _get_store() + + def save(self, file_path: str): + """ + Save the assembly poses to a JSON file. + """ + with open(file_path, 'w') as f: + json.dump(self.poses_per_element_dictionary, f, default=lambda o: o.__dict__, indent=4) + + def to_gh_tree(self): + """ + Convert the assembly poses to a Grasshopper tree structure. + """ + list_of_poses = [] + for element, poses in self.poses_per_element_dictionary.items(): + list_of_pose_of_element = [] + for pose in poses.poses_dictionary.values(): + list_of_pose_of_element.append(pose.to_rh_plane() if pose is not None else None) + list_of_poses.append(list_of_pose_of_element) + return th.list_to_tree(list_of_poses) + + +def compute_dot_product(v1, v2): + """ + Compute the dot product of two vectors. + """ + return (v1.X * v2.X) + (v1.Y * v2.Y) + (v1.Z * v2.Z) + + +def select_vectors(vectors, previous_xDirection, previous_yDirection): + """ + Select the vectors that are aligned with the xDirection and yDirection. + """ + if previous_xDirection is not None and previous_yDirection is not None: + sorted_vectors_by_alignment = sorted(vectors, key=lambda v: compute_dot_product(v, previous_xDirection), reverse=True) + new_xDirection = sorted_vectors_by_alignment[0] + else: + new_xDirection = vectors[0] + + condidates_for_yDirection = [] + for v in vectors: + if compute_dot_product(v, new_xDirection) ** 2 < 0.5: + condidates_for_yDirection.append(v) + if previous_xDirection is not None and previous_yDirection is not None: + sorted_vectors_by_perpendicularity = sorted(condidates_for_yDirection, key=lambda v: compute_dot_product(v, previous_yDirection), reverse=True) + new_xDirection = sorted_vectors_by_alignment[0] + new_yDirection = sorted_vectors_by_perpendicularity[0] - compute_dot_product(sorted_vectors_by_perpendicularity[0], new_xDirection) * new_xDirection + new_yDirection.Unitize() + else: + + sorted_vectors = sorted(vectors[1:], key=lambda v: compute_dot_product(v, new_xDirection)**2) + new_yDirection = sorted_vectors[0] - compute_dot_product(sorted_vectors[0], new_xDirection) * new_xDirection + new_yDirection.Unitize() + return new_xDirection, new_yDirection diff --git a/tests/unit_tests/DFPointCloudTest.cc b/tests/unit_tests/DFPointCloudTest.cc index d6d669d5..72b5a9db 100644 --- a/tests/unit_tests/DFPointCloudTest.cc +++ b/tests/unit_tests/DFPointCloudTest.cc @@ -219,4 +219,12 @@ TEST_F(DFPointCloudTestFixture, Transform) { //------------------------------------------------------------------------- // Others -//------------------------------------------------------------------------- \ No newline at end of file +//------------------------------------------------------------------------- + +TEST_F(DFPointCloudTestFixture, KMeansClusteringOfNormals) { + std::string path = diffCheck::io::GetTwoConnectedPlanesPlyPath(); + diffCheck::geometry::DFPointCloud dfPointCloud2Planes; + dfPointCloud2Planes.LoadFromPLY(path); + std::vector axes = dfPointCloud2Planes.GetPrincipalAxes(2); + EXPECT_TRUE((axes[0] - Eigen::Vector3d(0, 0, 1)).norm() < 1e-2 || (axes[1] - Eigen::Vector3d(0, 0, 1)).norm() < 1e-2); +} \ No newline at end of file