diff --git a/.gitignore b/.gitignore index f2b3d15..8e43848 100644 --- a/.gitignore +++ b/.gitignore @@ -19,6 +19,7 @@ dist *.TimeRecord +*.so *.pkl *.mp4 data @@ -26,4 +27,6 @@ checkpoints evaluation/output data_example -*.zip \ No newline at end of file +*.zip + +test.py \ No newline at end of file diff --git a/ace/train_ace.py b/ace/train_ace.py index f165581..9b7ce8e 100755 --- a/ace/train_ace.py +++ b/ace/train_ace.py @@ -25,6 +25,6 @@ def train_ace(scene, output_map_file): torch.cuda.empty_cache() if __name__ == '__main__': - scene = Path("/home/yanzj/workspace/code1/DovSG/data/company") + scene = Path("/home/yanzj/workspace/code/DovSG/data/company") output_map_file = scene / "ace/ace.pt" train_ace(scene, output_map_file) diff --git a/dovsg/controller.py b/dovsg/controller.py index 6a99372..e42a3a5 100644 --- a/dovsg/controller.py +++ b/dovsg/controller.py @@ -66,7 +66,7 @@ def __init__( nb_neighbors: int=35, std_ratio: float=1.5, - socket_ip: str="192.168.1.54", + socket_ip: str="192.168.1.50", socket_port: str="9999" ): @@ -722,8 +722,7 @@ def get_align_observations(self, use_inlier_mask=False, show_align=False, just_w print("===> get observations from robot.") _, observations = self.get_observations(just_wrist=just_wrist, save_name=save_name) - rough_poses = self.test_ace(observations) - + for name, obs in observations.items(): point = obs["point"] rgb = obs["rgb"] @@ -732,7 +731,7 @@ def get_align_observations(self, use_inlier_mask=False, show_align=False, just_w inlier_mask = get_inlier_mask(point=point, color=rgb, mask=mask) mask = np.logical_and(mask, inlier_mask) obs["mask"] = mask - obs["pose"] = rough_poses[name] + # obs["pose"] = rough_poses[name] if self_align: observations = self.self_align_observations(observations) @@ -740,6 +739,9 @@ def get_align_observations(self, use_inlier_mask=False, show_align=False, just_w is_success = True # align observations from base coord to world coord if align_to_world: + rough_poses = self.test_ace(observations) + for name, obs in observations.items(): + obs["pose"] = rough_poses[name] observations, is_success = self.correct_pose_observations(observations) if show_align and is_success: self.show_pointcloud_for_align(observations) @@ -1111,6 +1113,9 @@ def run_tasks(self, tasks: Union[List[dict]]): align_to_world=True, save_name="0_start", ) + if not correct_success: + assert 1 == 0, "Init Pose Error!" + init_position, init_rotation = self.get_current_position(observations) current_position = init_position current_rotation = init_rotation @@ -1149,8 +1154,8 @@ def run_tasks(self, tasks: Union[List[dict]]): # just after pick up and place, update scene if task["action"] in ["Pick up", "Place"]: if not correct_success: - print("relocalize error.") - exit(0) + assert 1 == 0, "relocalize error!" + self.update_scene(observations=observations) end_time = time.time() print(f"Step spend time is: {end_time - start_time}") diff --git a/dovsg/memory/scene_graph/1.py b/dovsg/memory/scene_graph/1.py deleted file mode 100644 index ed43e5c..0000000 --- a/dovsg/memory/scene_graph/1.py +++ /dev/null @@ -1,696 +0,0 @@ -import os -import numpy as np -from dovsg.memory.view_dataset import ViewDataset -from dovsg.memory.instances.instance_utils import MapObjectList -from dovsg.memory.instances.instance_utils import get_bbox -from dovsg.memory.scene_graph.graph import ObjectNode, SceneGraph -import alphashape -from shapely.geometry import Polygon, MultiPolygon -import copy -import open3d as o3d -from tqdm import tqdm -from typing import Union -import faiss -from scipy.spatial import ConvexHull, Delaunay - -def _get_cross_prod_mat(pVec_Arr): - # pVec_Arr shape (3) - qCross_prod_mat = np.array( - [ - [0, -pVec_Arr[2], pVec_Arr[1]], - [pVec_Arr[2], 0, -pVec_Arr[0]], - [-pVec_Arr[1], pVec_Arr[0], 0], - ] - ) - return qCross_prod_mat - -def _caculate_align_mat(pVec_Arr): - scale = np.linalg.norm(pVec_Arr) - pVec_Arr = pVec_Arr / scale - # must ensure pVec_Arr is also a unit vec. - z_unit_Arr = np.array([0, 0, 1]) - z_mat = _get_cross_prod_mat(z_unit_Arr) - - z_c_vec = np.matmul(z_mat, pVec_Arr) - z_c_vec_mat = _get_cross_prod_mat(z_c_vec) - if np.dot(z_unit_Arr, pVec_Arr) == -1: - qTrans_Mat = -np.eye(3, 3) - elif np.dot(z_unit_Arr, pVec_Arr) == 1: - qTrans_Mat = np.eye(3, 3) - else: - qTrans_Mat = ( - np.eye(3, 3) - + z_c_vec_mat - + np.matmul(z_c_vec_mat, z_c_vec_mat) / (1 + np.dot(z_unit_Arr, pVec_Arr)) - ) - qTrans_Mat *= scale - return qTrans_Mat - - -# Borrow ideas and codes from H. Sánchez's answer -# https://stackoverflow.com/questions/59026581/create-arrows-in-open3d -def getArrowMesh(origin=[0, 0, 0], end=None, color=[0, 0, 0]): - vec_Arr = np.array(end) - np.array(origin) - vec_len = np.linalg.norm(vec_Arr) - # mesh_arrow = o3d.geometry.TriangleMesh.create_arrow( - # cone_height=0.2 * vec_len, - # cone_radius=0.08, - # cylinder_height=1.0 * vec_len, - # cylinder_radius=0.04, - # ) - mesh_arrow = o3d.geometry.TriangleMesh.create_arrow( - cone_height=0.02 * vec_len, - cone_radius=0.008, - cylinder_height=0.10 * vec_len, - cylinder_radius=0.004, - ) - mesh_arrow.paint_uniform_color(color) - rot_mat = _caculate_align_mat(vec_Arr / vec_len) - mesh_arrow.rotate(rot_mat, center=np.array([0, 0, 0])) - mesh_arrow.translate(np.array(origin)) - return mesh_arrow - -class SceneGraphProcesser: - def __init__( - self, - part_level_classes: list, - resolution: float=0.01, - neighbour_num: int=5, - stand_floor_threshold: float=0.15, - alphashape_alpha: float=1, - alpha_shape_overleaf_rate_threshold: float=0.6, - part_intersection_rate_threshold: float=0.2, - inside_threshold: float=0.95 - ): - self.part_level_classes = part_level_classes - self.resolution = resolution - self.neighbour_num = neighbour_num - self.stand_floor_threshold = stand_floor_threshold - self.alphashape_alpha = alphashape_alpha - self.alpha_shape_overleaf_rate_threshold = alpha_shape_overleaf_rate_threshold - self.part_intersection_rate_threshold = part_intersection_rate_threshold - self.inside_threshold = inside_threshold - - self.root_node_class = "floor" - self.root_node_id = "floor_0" - - - def get_voxel_neighbours(self, voxels, size): - offsets = np.arange(-size, size + 1) - # Create a 3x3x3 grid of offsets - di, dj, dk = np.meshgrid(offsets, offsets, offsets, indexing="ij") - # Flatten the grids and stack them to create a list of offset vectors - offset_vectors = np.stack((di.ravel(), dj.ravel(), dk.ravel()), axis=-1) - # Remove the (0,0,0) offset (central voxel) - offset_vectors = offset_vectors[~np.all(offset_vectors == 0, axis=1)] - # Apply the offsets to the voxel coordinates using broadcasting - neighbours = voxels[:, np.newaxis, :] + offset_vectors - valid_mask = np.all((neighbours >= 0) & (neighbours < self.view_dataset.voxel_num), axis=2) - return self.view_dataset.voxel_to_index(neighbours), valid_mask - - def get_handle_info( - self, - instance_object, - parent_instance_object, - sibling_instance_objects, - visualize=False - ): - # Get the handle pose - - # handle_pcd = instance.index_to_pcd(instance.voxel_indexes) - voxel_indexes = instance_object["indexes"] - handle_point = self.view_dataset.index_to_point(voxel_indexes) - - # Currently the handle points are kind of clean - handle_center = np.mean(handle_point, axis=0) - # PCA to get the handle most obvious direction - centralized_points = handle_point - handle_center - cov_matrix = np.cov(centralized_points, rowvar=False) - eigenvalues, eigenvectors = np.linalg.eigh( - cov_matrix + np.eye(cov_matrix.shape[0]) * 1e-10 - ) - handle_direction = eigenvectors[:, -1] - floor_normal = np.array([0, 0, 1]) - # Make the handle direciton prependicular or parallel to the floor - if np.abs(np.dot(handle_direction, floor_normal)) > 0.7: - handle_direction = floor_normal - else: - handle_direction = np.cross( - floor_normal, np.cross(handle_direction, floor_normal) - ) - handle_direction /= np.linalg.norm(handle_direction) - - # Get the neightbour points of the handle - - voxels = self.view_dataset.index_to_voxel(instance_object["indexes"]) - neighbours, valid_mask = self.get_voxel_neighbours(voxels, self.neighbour_num) - - # Only consider the neighbours on the parent instance - # valid_mask *= np.isin(neighbours, list(parent_instance.voxel_indexes)) - valid_mask *= np.isin(neighbours, parent_instance_object["indexes"]) - - all_neighbour_indexes = np.array(list(set(neighbours[valid_mask].tolist()))) - all_neighbour_indexes = all_neighbour_indexes[ - ~np.isin(all_neighbour_indexes, voxel_indexes) - ] - # neighbour_pcd = self.index_to_pcd(all_neighbour_indexes).tolist() - neighbour_point = self.view_dataset.index_to_point(all_neighbour_indexes).tolist() - - # Estimate the normals - neighbour_pc = o3d.geometry.PointCloud() - - # neighbour_pc.points = o3d.utility.Vector3dVector(neighbour_pcd) - neighbour_pc.points = o3d.utility.Vector3dVector(neighbour_point) - - neighbour_pc.estimate_normals( - search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.08, max_nn=20) - ) - neighbour_normals = np.asarray(neighbour_pc.normals) - rounded_normals = np.round(neighbour_normals, decimals=3) - # Remove the normals prependicular to the floor - mask = np.abs(np.dot(rounded_normals, floor_normal)) < 0.7 - rounded_normals = rounded_normals[mask] - # Remove the normals parallel to the handle direction - mask = np.abs(np.dot(rounded_normals, handle_direction)) < 0.7 - rounded_normals = rounded_normals[mask] - unique, counts = np.unique(rounded_normals, axis=0, return_counts=True) - open_direction = unique[np.argmax(counts)] - # Use heuristic to determine the reference direction - reference_direction = np.zeros(3) - handle_center - reference_direction /= np.linalg.norm(reference_direction) - if np.dot(open_direction, reference_direction) < 0: - open_direction = -open_direction - # Make the open direction perpendicular to the [0, 0, 1] - open_direction = np.cross(floor_normal, np.cross(open_direction, floor_normal)) - open_direction /= np.linalg.norm(open_direction) - # Refine the handle direction using the open direction - # Use heuristic to judge the joint type, if vertical, revolute, or prismatic - if np.abs(np.dot(handle_direction, floor_normal)) < 0.7: - handle_direction = np.cross(open_direction, floor_normal) - joint_type = "prismatic" - else: - joint_type = "revolute" - - joint_info = {} - if joint_type == "revolute": - joint_axis = handle_direction - # Get the side_direction - side_direction = np.cross(handle_direction, open_direction) - # Get the distance of the handle center to the parent node boundary in the side direction - # parent_points = parent_instance.index_to_pcd(parent_instance.voxel_indexes) - - parent_points = self.view_dataset.index_to_point(parent_instance_object["indexes"]) - - # Get the distance between the handle center to the parent node boundary - distances = np.dot(parent_points - handle_center, side_direction) - max_distance = np.max(distances) - min_distance = np.min(distances) - # Judge if there is another handle on the side - flag_current = False - flag_reverse = False - # for sibling_instance in sibling_instances: - for sibling_instance_object in sibling_instance_objects: - if sibling_instance_object["class_id"] == "handle": - distance_up = np.dot( - np.mean( - self.view_dataset.index_to_point( - sibling_instance_object["indexes"] - ), - axis=0, - ) - - handle_center, - floor_normal, - ) - if np.abs(distance_up) > 0.07: - continue - distance_current = np.dot( - np.mean( - self.view_dataset.index_to_point( - sibling_instance_object["indexes"] - ), - axis=0, - ) - - handle_center, - side_direction, - ) - if distance_current > 0: - flag_current = True - distance_reverse = np.dot( - np.mean( - self.view_dataset.index_to_point( - sibling_instance_object["indexes"] - ), - axis=0, - ) - - handle_center, - -side_direction, - ) - if distance_reverse > 0: - flag_reverse = True - - if flag_current: - side_direction = -side_direction - joint_axis = -joint_axis - elif not flag_reverse and max_distance < np.abs(min_distance): - side_direction = -side_direction - joint_axis = -joint_axis - - # Find the points in the farthest open directions - open_distances = np.dot(parent_points, open_direction) - max_distance = np.dot(handle_center, open_direction) - potential_points = parent_points[ - np.logical_and( - open_distances < max_distance, open_distances > max_distance - 0.05 - ) - ] - # Find the points in the farthest side directions - side_distances = np.dot(potential_points, side_direction) - max_index = np.argmax(side_distances) - joint_info["side_direction"] = side_direction - joint_info["joint_origin"] = potential_points[max_index] - joint_info["joint_axis"] = joint_axis - - if visualize: - # scene_pcd, scene_color = self.view_dataset.indexes_colors_mapping_dict - # # Test the estimated normal of current scen first - # scene_pc = o3d.geometry.PointCloud() - # scene_pc.points = o3d.utility.Vector3dVector(scene_pcd) - # scene_pc.colors = o3d.utility.Vector3dVector(scene_color) - # o3d.visualization.draw_geometries( - # [scene_pc, neighbour_pc], point_show_normal=True - # ) - - scene_pc = self.view_dataset.index_to_pcd(list(self.view_dataset.indexes_colors_mapping_dict.keys())) - - arrow1 = getArrowMesh( - origin=handle_center, - end=handle_center + handle_direction, - color=[1, 0, 0], - ) - arrow2 = getArrowMesh( - origin=handle_center, - end=handle_center + open_direction, - color=[0, 1, 0], - ) - extra = [arrow1, arrow2] - if joint_type == "revolute": # revolute表示旋转关节，Prismatic表示平移关节 - arrow3 = getArrowMesh( - origin=joint_info["joint_origin"], - end=joint_info["joint_origin"] + joint_info["joint_axis"], - color=[0, 0, 0], - ) - extra.append(arrow3) - # visualize_pc(scene_pcd, color=scene_color, extra=extra) - o3d.visualization.draw_geometries( - [scene_pc] + extra, point_show_normal=True - ) - - return handle_center, handle_direction, open_direction, joint_type, joint_info - - def build_scene_graph(self, view_dataset: ViewDataset, instance_objects: MapObjectList, instance_scene_graph: Union[SceneGraph, None]=None): - self.view_dataset = view_dataset - instance_objects_copy = copy.deepcopy(instance_objects) - - # use floor node as root node - # floor node is not real exist, we can assume it as z-axis is 0 - # self.root_node_class = "floor" - # self.root_node_id = "floor_0" - if instance_scene_graph is None: - root_node = ObjectNode( - parent=None, - node_class=self.root_node_class, - node_id=self.root_node_id - ) - instance_scene_graph = SceneGraph(root_node=root_node) - - - class_id_to_instance_object = {ins_obj["class_id"]: ins_obj for ins_obj in instance_objects_copy} - - # # object label which is not include and part level labels and not been align to scene graph - # needed_align_node_class_ids = [ins_obj["class_id"] for ins_obj in instance_objects_copy - # if ins_obj["class_id"] not in added_instance_class_ids - # and ins_obj["class_id"] not in self.part_level_classes] - - - """At first, we process non-part level object""" - # just include non-part object - label_to_object_information_mapping = {} - for idx, ins_obj in tqdm(enumerate(instance_objects_copy), - total=len(instance_objects_copy), desc="label non-part object information mapping"): - class_name = ins_obj["class_name"] - class_id = ins_obj["class_id"] - # at first, don't process part level labels - if class_name in self.part_level_classes: - continue - lower_z = np.min(self.view_dataset.index_to_point(ins_obj["indexes"]), axis=0)[2] - xy_axis_unique = np.unique(self.view_dataset.index_to_point(ins_obj["indexes"])[:, :2], axis=0) - # Perturb the points a bit to avoid colinearity - xy_axis_unique += np.random.normal(0, 4e-3, xy_axis_unique.shape) - alpha_shape = alphashape.alphashape(xy_axis_unique, alpha=self.alphashape_alpha) - - if isinstance(alpha_shape, MultiPolygon): - alpha_shape = max(alpha_shape.geoms, key=lambda shape: shape.area) - if alpha_shape is None: assert 1 == 1 - label_to_object_information_mapping[class_id] = { - "ins_obj": ins_obj, - "lower_z": lower_z, - "alpha_shape": alpha_shape - } - - # sorted by lower z-axis value for improve efficiency - label_to_object_information_mapping = dict( - sorted( - label_to_object_information_mapping.items(), - key=lambda x: x[1]["lower_z"]) - ) - - for class_id, info in label_to_object_information_mapping.items(): - if class_id not in instance_scene_graph.object_nodes.keys(): - if info["lower_z"] < self.stand_floor_threshold: - ins_obj = info["ins_obj"] - assert ins_obj["class_id"] == class_id - instance_scene_graph.add_node( - parent=instance_scene_graph.root, - node_class=ins_obj["class_name"], - node_id=ins_obj["class_id"], - parent_relation="on", - is_part=False - ) - - need_align_class_id_list = [class_id for class_id in label_to_object_information_mapping.keys() - if class_id not in instance_scene_graph.object_nodes.keys()] - - # For other objects not directly on the table, find the object beneath it - while len(need_align_class_id_list) > 0: - for class_id in need_align_class_id_list: - # if class_id == "table_17": - # assert 1 == 1 - # cardboard_box_62 = self.view_dataset.index_to_pcd(label_to_object_information_mapping["cardboard box_62"]["ins_obj"]["indexes"]) - # cardboard_box_65 = self.view_dataset.index_to_pcd(label_to_object_information_mapping["cardboard box_65"]["ins_obj"]["indexes"]) - # bottle_8 = self.view_dataset.index_to_pcd(label_to_object_information_mapping["bottle_8"]["ins_obj"]["indexes"]) - # ink_0 = self.view_dataset.index_to_pcd(label_to_object_information_mapping["ink_0"]["ins_obj"]["indexes"]) - - # coordinate_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.3, origin=[0, 0, 0]) - # cardboard_box_62.paint_uniform_color([1, 0, 0]) - # cardboard_box_65.paint_uniform_color([0.5, 0, 0]) - # bottle_8.paint_uniform_color([0, 1, 0]) - # ink_0.paint_uniform_color([0, 0, 1]) - # o3d.visualization.draw_geometries([cardboard_box_62, cardboard_box_65, bottle_8, ink_0, coordinate_frame]) - - info = label_to_object_information_mapping[class_id] - ins_obj = info["ins_obj"] - lower_z = info["lower_z"] - alpha_shape = info["alpha_shape"] - bottom_ins_objs = [] - for other_class_id, other_info in label_to_object_information_mapping.items(): - if class_id != other_class_id: - other_ins_obj = other_info["ins_obj"] - other_lower_z = other_info["lower_z"] - other_alpha_shape = other_info["alpha_shape"] - - inter_rate = alpha_shape.intersection(other_alpha_shape).area / alpha_shape.area \ - if alpha_shape.area > 0 else 0 - - if inter_rate > self.alpha_shape_overleaf_rate_threshold and other_lower_z < lower_z: - print(f"{class_id} {other_class_id}, overlap rate {inter_rate}, \ - bottom distance:{lower_z - other_lower_z}") - - bottom_ins_objs.append([other_ins_obj, lower_z - other_lower_z]) - - - if len(bottom_ins_objs) > 0: - bottom_ins_obj = sorted(bottom_ins_objs, key=lambda x: x[1])[0][0] - # if bottom object has not been add to scene graph, continue it - if bottom_ins_obj["class_id"] not in instance_scene_graph.object_nodes.keys(): - continue - - parent_node = instance_scene_graph.object_nodes[bottom_ins_obj["class_id"]] - assert ins_obj["class_id"] == class_id - instance_scene_graph.add_node( - parent=parent_node, - node_class=ins_obj["class_name"], - node_id=ins_obj["class_id"], - parent_relation="on", - is_part=False - ) - else: # That is, if the object has no other objects at the bottom, it still belongs to the floor - instance_scene_graph.add_node( - instance_scene_graph.root, - node_class=ins_obj["class_name"], - node_id=ins_obj["class_id"], - parent_relation="on", - is_part=False - ) - need_align_class_id_list.remove(class_id) - - - """Now, we process part level object""" - part_instance_objects = [ins_obj for ins_obj in instance_objects_copy - if ins_obj["class_name"] in self.part_level_classes] - - # process part-level instance objects - # def find_parent_instance_object(part_ins_obj): - # # just match with non-part objec - # # part_ins_obj is not process in instnace process with unique indexes - # part_ins_obj_indexes = part_ins_obj["indexes"] - # parent_class_ids = [] - # for class_id, info in label_to_object_information_mapping.items(): - # ins_obj = info["ins_obj"] - # ins_obj_indexes = ins_obj["indexes"] - # part_intersection_rate = len(np.intersect1d(part_ins_obj_indexes, ins_obj_indexes)) / len(part_ins_obj_indexes) - # # class_ids.append() - # if part_intersection_rate > 0: - # parent_class_ids.append([class_id, part_intersection_rate]) - # # if part_intersection_rate > self.part_intersection_rate_threshold: - # # return class_id - # # raise ValueError("Cannot find the parent object") - # parent_class_id = sorted(parent_class_ids, key=lambda x: x[1], reverse=True)[0][0] - # return parent_class_id - - def find_parent_instance_object(part_ins_obj): - # Get the indexes of the part instance object - part_ins_obj_indexes = part_ins_obj["indexes"] - - # Convert part object indexes into actual point coordinates - part_points = self.view_dataset.index_to_point(part_ins_obj_indexes) - n_part_points = part_points.shape[0] - - # coordinate_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.3, origin=[0, 0, 0]) - # o3d.visualization.draw_geometries([self.view_dataset.index_to_pcd(part_ins_obj_indexes), coordinate_frame]) - - # Initialize a FAISS L2 distance index for fast nearest neighbor search - index = faiss.IndexFlatL2(part_points.shape[1]) - index.add(part_points) # Add part object's points to the FAISS index - - parent_class_ids = [] - # Iterate through all the objects to find the best parent candidate - for class_id, info in label_to_object_information_mapping.items(): - ins_obj = info["ins_obj"] - ins_obj_indexes = ins_obj["indexes"] - - # Convert parent object indexes into point coordinates - parent_points = self.view_dataset.index_to_point(ins_obj_indexes) - - # Use FAISS to search for the nearest points in the part object for each parent point - D, I = index.search(parent_points, 1) # D contains the distances, I contains the indices of nearest points - - # Calculate the ratio of points within the threshold distance - threshold = self.resolution * 10 # Define the distance threshold (e.g., 0.01 units) - close_points_rate = np.sum(D < threshold ** 2) / len(D) # Compute the proportion of points below the threshold distance - - # If any points satisfy the distance condition, record the parent candidate - if close_points_rate > 0: - parent_class_ids.append([class_id, close_points_rate]) - - # If parent candidates are found, select the one with the highest proportion of close points - if parent_class_ids: - parent_class_id = sorted(parent_class_ids, key=lambda x: x[1], reverse=True)[0][0] - return parent_class_id - else: - # Raise an error if no suitable parent object is found - raise ValueError("Cannot find a suitable parent object") - - - for part_ins_obj in tqdm(part_instance_objects, - total=len(part_instance_objects), desc="part-level object process"): - # find parent objects - parent_class_id = find_parent_instance_object(part_ins_obj) - instance_scene_graph.add_node( - instance_scene_graph.object_nodes[parent_class_id], - node_class=part_ins_obj["class_name"], - node_id=part_ins_obj["class_id"], - parent_relation="belong", - is_part=True - ) - - # Analyze the attributes of the handles - for part_ins_obj in tqdm(part_instance_objects, total=len(part_instance_objects), desc="handle process"): - if part_ins_obj["class_name"] != "handle": - continue - - part_node = instance_scene_graph.object_nodes[part_ins_obj["class_id"]] - parent_node = part_node.parent - sibling_nodes = parent_node.children.values() - - parent_ins_obj = class_id_to_instance_object[parent_node.node_id] - sibling_ins_objs = [class_id_to_instance_object[node.node_id] for node in sibling_nodes] - - ( - part_node.handle_center, - part_node.handle_direction, - part_node.open_direction, - part_node.joint_type, - joint_info, - ) = self.get_handle_info( - instance_object=part_ins_obj, - parent_instance_object=parent_ins_obj, - sibling_instance_objects=sibling_ins_objs, - visualize=False, - ) - - if part_node.joint_type == "revolute": - part_node.node_label = "door_handle" - part_node.joint_axis = joint_info["joint_axis"] - part_node.joint_origin = joint_info["joint_origin"] - part_node.side_direction = joint_info["side_direction"] - elif part_node.joint_type == "prismatic": - part_node.node_label = "drawer_handle" - - - # def collect_all_descendants(node: ObjectNode): - # """Collect all descendants of a given node recursively.""" - # descendants = [] - # for child in node.children.values(): - # descendants.append(child) - # descendants.extend(collect_all_descendants(child)) # Recursively add child nodes - # return descendants - - # Analyze the inside objects while parent object has part-level object - - part_level_parent_nodes = set() - for class_id, node in instance_scene_graph.object_nodes.items(): - if class_id == instance_scene_graph.root.node_id: - continue - ins_obj = class_id_to_instance_object[class_id] - - # Check if the node has a parent with part-level children - parent_node = node.parent - if parent_node is None: - continue - - part_level_children = [ - child for child in parent_node.children.values() if child.is_part - ] - - if not part_level_children: - continue # Skip if parent node does not have part-level objects - - part_level_parent_nodes.add(parent_node) - - for parent_node in part_level_parent_nodes: - # Get the point cloud of the parent object - parent_ins_obj = class_id_to_instance_object[parent_node.node_id] - parent_points = self.view_dataset.index_to_point(parent_ins_obj["indexes"]) - - delaunay_tri = Delaunay(parent_points) - # all_descendants = collect_all_descendants(parent_node) - - def extract_surface_triangles(delaunay): - triangles = [] - for tetrahedron in delaunay.simplices: - triangles.append([tetrahedron[0], tetrahedron[1], tetrahedron[2]]) - triangles.append([tetrahedron[0], tetrahedron[1], tetrahedron[3]]) - triangles.append([tetrahedron[0], tetrahedron[2], tetrahedron[3]]) - triangles.append([tetrahedron[1], tetrahedron[2], tetrahedron[3]]) - return np.array(triangles) - surface_triangles = extract_surface_triangles(delaunay_tri) - parent_pcd = o3d.geometry.PointCloud() - parent_pcd.points = o3d.utility.Vector3dVector(parent_points) - parent_pcd.paint_uniform_color([121/255, 255/255, 226/255]) - triangles = o3d.utility.Vector3iVector(surface_triangles) - triangle_mesh = o3d.geometry.TriangleMesh() - triangle_mesh.vertices = o3d.utility.Vector3dVector(parent_points) - triangle_mesh.triangles = triangles - triangle_mesh.paint_uniform_color([121/255, 255/255, 226/255]) - o3d.visualization.draw_geometries([parent_pcd, triangle_mesh]) - - - - # Check all descendant of the parent node (excluding part-level objects) - # using delaunay to select the object that satisfies the requirements - for child_node in parent_node.children.values(): - if child_node.is_part: - continue # Skip part-level objects - - # Get the point cloud of the child object - child_instance = class_id_to_instance_object[child_node.node_id] - child_points = self.view_dataset.index_to_point(child_instance["indexes"]) - # just inner is valid - inside_mask = delaunay_tri.find_simplex(child_points) > 0 - - - if inside_mask.sum() > self.inside_threshold * len(child_points): - # If the condition holds, set the parent_relationship to 'inside' and update parent - child_node.parent_relation = "inside" - print(f"Node {child_node.node_id} is inside {parent_node.node_id}") - - return instance_scene_graph - - def calculate_max_generation(self, node, current_generation=1): - "find node children to get all affected node" - max_generation = current_generation - for child in list(node.children.values()): - child_generation = self.calculate_max_generation(child, current_generation + 1) - max_generation = max(max_generation, child_generation) - return max_generation - - def update_scene_graph(self, view_dataset: ViewDataset, instance_objects: MapObjectList, history_scene_graph: SceneGraph): - class_id_to_instance_object = {ins_obj["class_id"]: ins_obj for ins_obj in instance_objects} - last_step_class_ids = list(history_scene_graph.object_nodes.keys()) - # remove root class_id - last_step_class_ids.remove(self.root_node_id) - this_step_class_ids = list(class_id_to_instance_object.keys()) - del_class_ids = np.setdiff1d(last_step_class_ids, this_step_class_ids) - new_class_ids = np.setdiff1d(this_step_class_ids, last_step_class_ids) - - affected_node_class_ids = [] - - for del_class_id in del_class_ids: - node = history_scene_graph.object_nodes[del_class_id] - for object_node in history_scene_graph.object_nodes.values(): - if object_node.parent == node: - affected_node_class_ids.append(object_node.node_id) - - delete_node_class_ids = np.union1d(del_class_ids, affected_node_class_ids) - - # Sort by generation, the smaller the generation, the first to be deleted - delete_node_generation_mapping = {} - for del_class_id in delete_node_class_ids: - node = history_scene_graph.object_nodes[del_class_id] - delete_node_generation_mapping[del_class_id] = { - "node": node, - "generation": self.calculate_max_generation(node), - } - print(del_class_id, self.calculate_max_generation(node)) - - delete_node_generation_mapping = dict(sorted( - delete_node_generation_mapping.items(), - key=lambda item: item[1]['generation'] - )) - - for key, value in delete_node_generation_mapping.items(): - if value["generation"] > 1: - assert 1 == 1 - - object_node = value["node"] - print(value["generation"], object_node.node_id) - object_node.delete() - del history_scene_graph.object_nodes[object_node.node_id] - - instance_scene_graph = self.build_scene_graph( - view_dataset=view_dataset, - instance_objects=instance_objects, - instance_scene_graph=history_scene_graph - ) - assert len(instance_scene_graph.object_nodes) == len(class_id_to_instance_object) + 1 - return instance_scene_graph \ No newline at end of file diff --git a/dovsg/memory/scene_graph/scene_graph_processer.py b/dovsg/memory/scene_graph/scene_graph_processer.py index c0c56d6..2dbcd36 100644 --- a/dovsg/memory/scene_graph/scene_graph_processer.py +++ b/dovsg/memory/scene_graph/scene_graph_processer.py @@ -346,7 +346,7 @@ def build_scene_graph(self, view_dataset: ViewDataset, instance_objects: MapObje if isinstance(alpha_shape, MultiPolygon): alpha_shape = max(alpha_shape.geoms, key=lambda shape: shape.area) - if alpha_shape is None: assert 1 == 1 + label_to_object_information_mapping[class_id] = { "ins_obj": ins_obj, "lower_z": lower_z, @@ -698,9 +698,6 @@ def update_scene_graph(self, view_dataset: ViewDataset, instance_objects: MapObj )) for key, value in delete_node_generation_mapping.items(): - if value["generation"] > 1: - assert 1 == 1 - object_node = value["node"] print(value["generation"], object_node.node_id) object_node.delete() diff --git a/dovsg/scripts/zmq_socket.py b/dovsg/scripts/zmq_socket.py index e705b93..73cb046 100644 --- a/dovsg/scripts/zmq_socket.py +++ b/dovsg/scripts/zmq_socket.py @@ -39,7 +39,7 @@ def close(self): class ZmqSocket: - def __init__(self, ip="192.168.1.54", port="9999"): + def __init__(self, ip="192.168.1.50", port="9999"): self.client = ZmqClientSocket(ip, port) def send_info(self, info, type): # type are include in [paths, pose, frame, over] diff --git a/hardcode/arm/xarm6.py b/hardcode/arm/xarm6.py index 9400796..ac9b395 100644 --- a/hardcode/arm/xarm6.py +++ b/hardcode/arm/xarm6.py @@ -213,6 +213,4 @@ def is_alive(self): # ] # for pose in poses: # # Move to the pose and wait for 5s to make it stable - # xarm.move_to_pose(pose=pose, wait=True, ignore_error=True) - - assert 1 == 1 \ No newline at end of file + # xarm.move_to_pose(pose=pose, wait=True, ignore_error=True) \ No newline at end of file