updated kitti360Viewer3D to visualize 3d bboxes along with pcd

kitti360scripts/viewer/kitti360Viewer3D.py

@@ -101,7 +101,7 @@ def __init__(self, seq=0, showStatic=True):
         sequence = '2013_05_28_drive_%04d_sync' % seq
         self.label3DPcdPath  = os.path.join(kitti360Path, 'data_3d_semantics')
         self.label3DBboxPath = os.path.join(kitti360Path, 'data_3d_bboxes')
-        #self.annotation3D = Annotation3D(self.label3DBboxPath, sequence)
+        self.annotation3D = Annotation3D(self.label3DBboxPath, sequence)
         self.annotation3DPly = Annotation3DPly(self.label3DPcdPath, sequence)
         self.sequence = sequence
 
@@ -319,16 +319,30 @@ def lookat(self, look_from, look_to, tmp = np.asarray([0, 0, 1])):
         if not len(v.bboxes):
             raise RuntimeError('No bounding boxes found! Please set KITTI360_DATASET in your environment path')
 
+        pcdFileList = v.annotation3DPly.pcdFileList 
         # group the bboxes by windows
         windows_unique = np.unique(np.array(v.bboxes_window), axis=0)
-        for window in windows_unique:
-            bboxes = [v.bboxes[i] for i in range(len(v.bboxes)) if v.bboxes_window[i][0]==window[0]]
+        for idx,window in enumerate(windows_unique):
+
+            # load point cloud for visualization
+            if not '%010d_%010d' % (window[0], window[1]) in pcdFileList[idx]:
+                raise RuntimeError("Window %s does not match with point cloud name %s! Please make sure that you downloaded the accumulated point clouds correctly." % (window, pcdFileList[idx]) )
+            pcd = v.loadWindow(pcdFileList[idx], args.mode)
+            pcdCenter = np.median(np.asarray(pcd.points),axis=0)
+            # filter out outliers in pcd
+            validPoints = np.where(np.linalg.norm(np.asarray(pcd.points)-pcdCenter,axis=1)<1e+3)[0]
+            pcd = pcd.select_by_index(validPoints)
+            pcdCenter = np.median(np.asarray(pcd.points),axis=0)
+
+            # load instance bounding boxes not too far away from the point cloud center
+            bboxes = [v.bboxes[i] for i in range(len(v.bboxes)) if (np.linalg.norm(np.mean(np.asarray(v.bboxes[i].vertices), axis=0)-pcdCenter)<120)]
             print('Visualizing %06d_%06d with %d objects' % (window[0], window[1], len(bboxes)))
             if len(bboxes)>args.max_bbox:
                 print('Randomly sample %d/%d bboxes for rendering efficiency' % (args.max_bbox, len(bboxes)))
                 random_list = np.random.permutation(len(bboxes))[:args.max_bbox]
                 bboxes = [bboxes[i] for i in random_list]
-            open3d.visualization.draw_geometries(bboxes)
+
+            open3d.visualization.draw_geometries(bboxes + [pcd])
 
     exit()