update

combine_all_pts.py

@@ -0,0 +1,15 @@
+import numpy as np
+import os
+
+if __name__ == "__main__":
+    pts_dir_path = "/home/yan20/nbv_rec/project/franka_control/temp_output/cad_model_world/pts"
+    pts_dir = os.listdir(pts_dir_path)
+    pts_list = []
+    for i in range(len(pts_dir)):
+        pts_path = os.path.join(pts_dir_path, pts_dir[i])
+        pts = np.loadtxt(pts_path)
+        pts_list.append(pts)
+    combined_pts = np.vstack(pts_list)
+    path = "/home/yan20/nbv_rec/project/franka_control"
+    np.savetxt(os.path.join(path, "combined_pts.txt"), combined_pts)
+    

configs/cad_close_loop_config.yaml

@@ -19,8 +19,8 @@ runner:
     max_shot_view_num: 50
     min_shot_new_pts_num: 10
     min_coverage_increase: 0.001  
-    max_view: 512
+    max_view: 64
-    min_view: 128
+    min_view: 32
     max_diag: 0.7
     min_diag: 0.01
     random_view_ratio: 0

runners/cad_close_loop_strategy.py

@@ -44,7 +44,7 @@ class CADCloseLoopStrategyRunner(Runner):
             "min_cam_table_included_degree"
         ]
         self.max_shot_view_num = self.generate_config["max_shot_view_num"]
-        self.min_shot_new_pts_num = self.generate_config["max_shot_new_pts_num"]
+        self.min_shot_new_pts_num = self.generate_config["min_shot_new_pts_num"]
         self.min_coverage_increase = self.generate_config["min_coverage_increase"]
         
         self.random_view_ratio = self.generate_config["random_view_ratio"]
@@ -151,12 +151,16 @@ class CADCloseLoopStrategyRunner(Runner):
                 scan_points_idx_list.append(indices)
 
             """ close-loop strategy """
-            scanned_pts = [first_real_world_pts]
+            scanned_pts = PtsUtil.voxel_downsample_point_cloud(
+                first_splitted_real_world_pts, self.voxel_size
+            )
+            shot_pts_list = [first_splitted_real_world_pts]
             history_indices = []
             last_coverage = 0
-            last_covered_num = 0
             Log.info("[Part 4/4] start close-loop control")
+            cnt = 0
             while True:
+                #import ipdb; ipdb.set_trace()
                 next_best_view, next_best_coverage, next_best_covered_num = (
                     ReconstructionUtil.compute_next_best_view_with_overlap(
                         scanned_pts,
@@ -164,8 +168,7 @@ class CADCloseLoopStrategyRunner(Runner):
                         history_indices,
                         scan_points_idx_list,
                         threshold=self.voxel_size,
-                        soft_overlap_threshold=self.soft_overlap_threshold,
+                        overlap_area_threshold=25,
-                        hard_overlap_threshold=self.hard_overlap_threshold,
                         scan_points_threshold=self.scan_points_threshold,
                     )
                 )
@@ -176,28 +179,50 @@ class CADCloseLoopStrategyRunner(Runner):
                 ''' get world pts '''
                 time.sleep(0.5)
                 view_data = CommunicateUtil.get_view_data()
-                world_shot_pts = ViewUtil.get_pts(view_data)
+                if view_data is None:
-                scanned_pts.append(world_shot_pts)
+                    Log.error("No view data received")
+                    continue
+                cam_shot_pts = ViewUtil.get_pts(view_data)
+                world_shot_pts = PtsUtil.transform_point_cloud(
+                    cam_shot_pts, first_cam_to_real_world
+                )
+                _, world_splitted_shot_pts = self.split_scan_pts_and_obj_pts(
+                    world_shot_pts
+                )
+                shot_pts_list.append(world_splitted_shot_pts)
+                
+                debug_dir = os.path.join(temp_dir, "debug")
+                if not os.path.exists(debug_dir):
+                    os.makedirs(debug_dir)
+                np.savetxt(os.path.join(debug_dir,  f"shot_pts_{cnt}.txt"), world_splitted_shot_pts)
+                np.savetxt(os.path.join(debug_dir,  f"render_pts_{cnt}.txt"), sample_view_pts_list[next_best_view])
+                #real_world_to_cad = PtsUtil.register(first_splitted_real_world_pts, cad_model)
+                #import ipdb; ipdb.set_trace()
+                last_scanned_pts_num = scanned_pts.shape[0]
+                new_scanned_pts = PtsUtil.voxel_downsample_point_cloud(
+                    np.vstack([scanned_pts, world_splitted_shot_pts]), self.voxel_size
+                )
+                new_scanned_pts_num = new_scanned_pts.shape[0]
                 history_indices.append(scan_points_idx_list[next_best_view])
+                scanned_pts = new_scanned_pts
                 Log.info(
-                    f"Current rec pts num: {len(scanned_pts)}, Best cover pts: {next_best_covered_num}, Best coverage: {next_best_coverage}"
+                    f"Next Best cover pts: {next_best_covered_num}, Best coverage: {next_best_coverage}"
                 )
                 
                 coverage_rate_increase = next_best_coverage - last_coverage
                 if coverage_rate_increase < self.min_coverage_increase:
                     Log.info(f"Coverage rate = {coverage_rate_increase} < {self.min_coverage_increase}, stop scanning")
-                    break
+                    # break
                 last_coverage = next_best_coverage
                 
-                new_added_pts_num = next_best_covered_num - last_covered_num
+                new_added_pts_num = new_scanned_pts_num - last_scanned_pts_num
                 if new_added_pts_num < self.min_shot_new_pts_num:
-                    Log.info(f"New added pts num = {new_added_pts_num} < {self.min_shot_new_pts_num}, stop scanning")
+                    Log.info(f"New added pts num = {new_added_pts_num} < {self.min_shot_new_pts_num}")
-                    break
+                    #ipdb.set_trace()
-                last_covered_num = next_best_covered_num
+                if len(shot_pts_list) >= self.max_shot_view_num:
-                
+                    Log.info(f"Scanned view num = {len(shot_pts_list)} >= {self.max_shot_view_num}, stop scanning")
-                if len(scanned_pts) >= self.max_shot_view_num:
+                    #break
-                    Log.info(f"Scanned view num = {len(scanned_pts)} >= {self.max_shot_view_num}, stop scanning")
+                cnt += 1
-                    break
                 
             Log.success("[Part 4/4] finish close-loop control")
 

utils/pts_util.py

@@ -117,66 +117,6 @@ class PtsUtil:
         filtered_sampled_points = sampled_points[idx]
         return filtered_sampled_points[:, :3]
 
-    @staticmethod
-    def old_register(pcl: np.ndarray, model: trimesh.Trimesh, voxel_size=0.002):
-        radius_normal = voxel_size * 3
-        pipreg = o3d.pipelines.registration
-        model_pcd = o3d.geometry.PointCloud()
-        model_pcd.points = o3d.utility.Vector3dVector(model.vertices)
-        model_downsampled = model_pcd.voxel_down_sample(voxel_size)
-        model_downsampled.estimate_normals(
-            search_param=o3d.geometry.KDTreeSearchParamHybrid(
-                radius=radius_normal, max_nn=30
-            )
-        )
-        model_fpfh = pipreg.compute_fpfh_feature(
-            model_downsampled,
-            o3d.geometry.KDTreeSearchParamHybrid(radius=radius_normal, max_nn=100),
-        )
-
-        source_pcd = o3d.geometry.PointCloud()
-        source_pcd.points = o3d.utility.Vector3dVector(pcl)
-        source_downsampled = source_pcd.voxel_down_sample(voxel_size)
-        source_downsampled.estimate_normals(
-            search_param=o3d.geometry.KDTreeSearchParamHybrid(
-                radius=radius_normal, max_nn=30
-            )
-        )
-        source_fpfh = pipreg.compute_fpfh_feature(
-            source_downsampled,
-            o3d.geometry.KDTreeSearchParamHybrid(radius=radius_normal, max_nn=100),
-        )
-
-        reg_ransac = pipreg.registration_ransac_based_on_feature_matching(
-            source_downsampled,
-            model_downsampled,
-            source_fpfh,
-            model_fpfh,
-            mutual_filter=True,
-            max_correspondence_distance=voxel_size * 2,
-            estimation_method=pipreg.TransformationEstimationPointToPoint(False),
-            ransac_n=4,
-            checkers=[pipreg.CorrespondenceCheckerBasedOnEdgeLength(0.9)],
-            criteria=pipreg.RANSACConvergenceCriteria(4000000, 500),
-        )
-        
-        reg_icp = pipreg.registration_icp(
-            source_downsampled,
-            model_downsampled,
-            voxel_size/2,
-            reg_ransac.transformation,
-            pipreg.TransformationEstimationPointToPlane(),
-            pipreg.ICPConvergenceCriteria(max_iteration=2000),
-        )
-        
-        return reg_icp.transformation
-    
-    @staticmethod
-    def chamfer_distance(pcl_a, pcl_b):
-        distances = np.linalg.norm(pcl_a[:, None] - pcl_b, axis=2)
-        min_distances = np.min(distances, axis=1)
-        return np.sum(min_distances)
-    
     @staticmethod
     def multi_scale_icp(
         source, target, voxel_size_range, init_transformation=None, steps=20

utils/reconstruction_util.py

@@ -13,15 +13,14 @@ class ReconstructionUtil:
         return coverage_rate, covered_points_num
     
     @staticmethod
-    def compute_overlap_rate(new_point_cloud, combined_point_cloud, threshold=0.01):
+    def check_overlap(new_point_cloud, combined_point_cloud, overlap_area_threshold=25, voxel_size=0.01):
         kdtree = cKDTree(combined_point_cloud)
         distances, _ = kdtree.query(new_point_cloud)
-        overlapping_points = np.sum(distances < threshold*2)
+        overlapping_points = np.sum(distances < voxel_size*2)
-        if new_point_cloud.shape[0] == 0:
+        cm = 0.01
-            overlap_rate = 0
+        voxel_size_cm = voxel_size / cm
-        else:
+        overlap_area = overlapping_points * voxel_size_cm * voxel_size_cm
-            overlap_rate = overlapping_points / new_point_cloud.shape[0]
+        return overlap_area > overlap_area_threshold
-        return overlap_rate
 
 
     @staticmethod
@@ -122,7 +121,7 @@ class ReconstructionUtil:
         return view_sequence, remaining_views, combined_point_cloud
 
     @staticmethod
-    def compute_next_best_view_with_overlap(scanned_pts:list, point_cloud_list, history_indices, scan_points_indices_list, threshold=0.01, soft_overlap_threshold=0.5, hard_overlap_threshold=0.7, scan_points_threshold=5):
+    def compute_next_best_view_with_overlap(scanned_pts, point_cloud_list, history_indices, scan_points_indices_list, threshold=0.01, overlap_area_threshold=25, scan_points_threshold=5):
         max_rec_pts = np.vstack(point_cloud_list)
         downsampled_max_rec_pts = PtsUtil.voxel_downsample_point_cloud(max_rec_pts, threshold)
         best_view = None
@@ -132,14 +131,17 @@ class ReconstructionUtil:
             if point_cloud_list[view].shape[0] == 0:
                 continue
             new_scan_points_indices = scan_points_indices_list[view]
+            
             if not ReconstructionUtil.check_scan_points_overlap(history_indices, new_scan_points_indices, scan_points_threshold):
-                overlap_threshold = hard_overlap_threshold
+                curr_overlap_area_threshold = overlap_area_threshold
             else:
-                overlap_threshold = soft_overlap_threshold
+                curr_overlap_area_threshold = overlap_area_threshold * 0.5
-            overlap_rate = ReconstructionUtil.compute_overlap_rate(point_cloud_list[view], scanned_pts, threshold)
+                
-            if overlap_rate < overlap_threshold:
+            if not ReconstructionUtil.check_overlap(point_cloud_list[view], scanned_pts, overlap_area_threshold = curr_overlap_area_threshold, voxel_size=threshold):
                 continue
-            new_combined_point_cloud = np.vstack(scanned_pts + [point_cloud_list[view]])
+            
+            
+            new_combined_point_cloud = np.vstack([scanned_pts ,point_cloud_list[view]])
             new_downsampled_combined_point_cloud = PtsUtil.voxel_downsample_point_cloud(new_combined_point_cloud,threshold)
             new_coverage, new_covered_num = ReconstructionUtil.compute_coverage_rate(downsampled_max_rec_pts, new_downsampled_combined_point_cloud, threshold)
             if new_coverage > best_coverage:

utils/view_util.py

@@ -91,6 +91,7 @@ class CameraData:
         points = np.linalg.inv(self.intrinsics) @ points                          # 3xN
         points = points.T                                                         # Nx3
         points = points * self.depth_image.flatten()[:, np.newaxis]               # Nx3
+        points = points[points[:, 2] > 0]                                         # Nx3
         return points
     
     @property