Enhance stereo depth develop merge

.gitignore

@@ -26,16 +26,22 @@ share/python-wheels/
 *.egg-info/
 .installed.cfg
 *.egg
+*darwin.so
 MANIFEST
-
+*.idea
 # DepthAI-Specific
 dataset/
 depthai.calib
+mesh_left.calib
+mesh_right.calib
+intrinsic.json
+intrinsic_calib_m2
 
 *.h264
 *.h265
 *.mkv
 *.mp4
 *.blob.sh*cmx*
 *.cmd
+*.mvcmd
 *.so

README.md

@@ -15,7 +15,7 @@ python3 -m pip install -r requirements.txt
 Optional:
 For command line autocomplete when pressing TAB, only bash interpreter supported now:
 Add to .bashrc:
-`echo 'eval "$(register-python-argcomplete depthai.py)"' >> ~/.bashrc`
+`echo 'eval "$(register-python-argcomplete depthai_demo.py)"' >> ~/.bashrc`
 
 If you use any other interpreter: https://kislyuk.github.io/argcomplete/
 

calibrate.py

@@ -156,7 +156,7 @@ def __init__(self):
                     'mono':
                     {
                         # 1280x720, 1280x800, 640x400 (binning enabled)
-                        'resolution_h': 720,
+                        'resolution_h': 800,
                         'fps': 30.0,
                     },
                 },

calibrate_and_test.py

@@ -24,7 +24,7 @@ def cleanup():
     args+="'"+arg+"' "
 
 calibrate_cmd = "python3 calibrate.py " + args
-test_cmd = """python3 depthai-demo.py -co '{"streams": [{"name": "depth_raw", "max_fps": 12.0}]}'"""
+test_cmd = """python3 depthai_demo.py -co '{"streams": [{"name": "depth_raw", "max_fps": 12.0}]}'"""
 
 
 atexit.register(cleanup)

consts/resource_paths.py

@@ -15,6 +15,14 @@ def relative_to_abs_path(relative_path):
 device_usb2_cmd_fpath = relative_to_abs_path('../depthai_usb2.cmd')
 boards_dir_path       = relative_to_abs_path('../resources/boards') + "/"
 custom_calib_fpath    = relative_to_abs_path('../resources/depthai.calib')
+left_mesh_fpath        = relative_to_abs_path('../resources/mesh_left.calib')
+right_mesh_fpath        = relative_to_abs_path('../resources/mesh_right.calib')
+
+right_map_x_fpath        = relative_to_abs_path('../resources/map_x_right.calib')
+right_map_y_fpath        = relative_to_abs_path('../resources/map_y_right.calib')
+left_map_x_fpath        = relative_to_abs_path('../resources/map_x_left.calib')
+left_map_y_fpath        = relative_to_abs_path('../resources/map_y_left.calib')
+
 nn_resource_path      = relative_to_abs_path('../resources/nn')+"/"
 blob_fpath            = relative_to_abs_path('../resources/nn/mobilenet-ssd/mobilenet-ssd.blob')
 blob_config_fpath     = relative_to_abs_path('../resources/nn/mobilenet-ssd/mobilenet-ssd.json')

depthai_demo.py

@@ -21,6 +21,7 @@
 
 from depthai_helpers.config_manager import DepthConfigManager
 from depthai_helpers.arg_manager import CliArgs
+from depthai_helpers.projector_3d import PointCloudVisualizer
 
 from depthai_helpers.object_tracker_handler import show_tracklets
 
@@ -126,6 +127,18 @@ def startLoop(self):
 
         self.reset_process_wd()
 
+        time_start = time()
+        def print_packet_info(packet):
+            meta = packet.getMetadata()
+            print("[{:.6f} {:15s}]".format(time()-time_start, packet.stream_name), end='')
+            if meta is not None:
+                print(" {:.6f}".format(meta.getTimestamp()), meta.getSequenceNum(), end='')
+                if not (packet.stream_name.startswith('disparity')
+                     or packet.stream_name.startswith('depth')):
+                    print('', meta.getCameraName(), end='')
+            print()
+            return
+
         for stream in stream_names:
             if stream in ["disparity", "disparity_color", "depth_raw"]:
                 cv2.namedWindow(stream)
@@ -134,6 +147,10 @@ def startLoop(self):
                 conf_thr_slider_max = 255
                 cv2.createTrackbar(trackbar_name, stream, conf_thr_slider_min, conf_thr_slider_max, self.on_trackbar_change)
                 cv2.setTrackbarPos(trackbar_name, stream, args['disparity_confidence_threshold'])
+
+        right_rectified = None
+        pcl_not_set = True
+
         ops = 0
         prevTime = time()
         while self.runThread:
@@ -158,6 +175,8 @@ def startLoop(self):
                     os._exit(10)
 
             for _, nnet_packet in enumerate(self.nnet_packets):
+                if args['verbose']: print_packet_info(nnet_packet)
+
                 meta = nnet_packet.getMetadata()
                 camera = 'rgb'
                 if meta != None:
@@ -171,6 +190,7 @@ def startLoop(self):
                 window_name = packet.stream_name
                 if packet.stream_name not in stream_names:
                     continue # skip streams that were automatically added
+                if args['verbose']: print_packet_info(packet)
                 packetData = packet.getData()
                 if packetData is None:
                     print('Invalid packet data!')
@@ -195,8 +215,10 @@ def startLoop(self):
                     cv2.putText(nn_frame, "fps: " + str(frame_count_prev[window_name]), (25, 50), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 0, 0))
                     cv2.putText(nn_frame, "NN fps: " + str(frame_count_prev['nn'][camera]), (2, frame.shape[0]-4), cv2.FONT_HERSHEY_SIMPLEX, 0.4, (0, 255, 0))
                     cv2.imshow(window_name, nn_frame)
-                elif packet.stream_name == 'left' or packet.stream_name == 'right' or packet.stream_name == 'disparity':
+                elif packet.stream_name in ['left', 'right', 'disparity', 'rectified_left', 'rectified_right']:
                     frame_bgr = packetData
+                    if args['pointcloud'] and packet.stream_name == 'rectified_right':
+                        right_rectified = packetData
                     cv2.putText(frame_bgr, packet.stream_name, (25, 25), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 0, 0))
                     cv2.putText(frame_bgr, "fps: " + str(frame_count_prev[window_name]), (25, 50), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 0, 0))
                     if args['draw_bb_depth']:
@@ -216,6 +238,14 @@ def startLoop(self):
                             cv2.putText(frame, packet.stream_name, (25, 25), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 0, 255))
                             cv2.putText(frame, "fps: " + str(frame_count_prev[window_name]), (25, 50), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 0, 255))
                         else: # uint16
+                            if args['pointcloud'] and "depth_raw" in stream_names and "rectified_right" in stream_names and right_rectified is not None:
+                                if pcl_not_set:
+                                    pcl_converter = PointCloudVisualizer(self.device.get_right_intrinsic(), 1280, 720)
+                                    pcl_not_set =  False
+                                right_rectified = cv2.flip(right_rectified, 1)
+                                pcd = pcl_converter.rgbd_to_projection(frame, right_rectified)
+                                pcl_converter.visualize_pcd()
+
                             frame = (65535 // frame).astype(np.uint8)
                             #colorize depth map, comment out code below to obtain grayscale
                             frame = cv2.applyColorMap(frame, cv2.COLORMAP_HOT)

depthai_helpers/arg_manager.py

@@ -14,7 +14,8 @@ def _get_immediate_subdirectories(a_dir):
     return [name for name in os.listdir(a_dir)
             if os.path.isdir(os.path.join(a_dir, name))]
 
-_stream_choices = ("metaout", "previewout", "jpegout", "left", "right", "depth_raw", "disparity", "disparity_color", "meta_d2h", "object_tracker", "color")
+_stream_choices = ("metaout", "previewout", "jpegout", "left", "right", "depth_raw", "disparity", "disparity_color",
+                   "meta_d2h", "object_tracker", "rectified_left", "rectified_right", "color")
 _CNN_choices = _get_immediate_subdirectories(consts.resource_paths.nn_resource_path)
 
 def _stream_type(option):
@@ -104,6 +105,11 @@ def parse_args(self):
                             choices=range(0,256), metavar="[0-255]",
                             help="Disparity_confidence_threshold.")
 
+        parser.add_argument("-med", "--stereo_median_size", default=7, type=int, choices=[0, 3, 5, 7],
+                            help="Disparity / depth median filter kernel size (N x N) . 0 = filtering disabled. Default: %(default)s")
+
+        parser.add_argument("-lrc", "--stereo_lr_check", default=False, action="store_true",
+                        help="Enable stereo 'Left-Right check' feature.")
         parser.add_argument("-fv", "--field-of-view", default=None, type=float,
                             help="Horizontal field of view (HFOV) for the stereo cameras in [deg]. Default: 71.86deg.")
 
@@ -157,6 +163,8 @@ def parse_args(self):
         parser.add_argument("-sync", "--sync-video-meta", default=False, action="store_true",
                             help="Synchronize 'previewout' and 'metaout' streams")
 
+        parser.add_argument("-vv", "--verbose", default=False, action="store_true",
+                        help="Verbose, print info about received packets.")
         parser.add_argument("-s", "--streams",
                             nargs="+",
                             type=_stream_type,
@@ -171,6 +179,15 @@ def parse_args(self):
         parser.add_argument("-v", "--video", default=None, type=str, required=False,
                             help="Path where to save video stream (existing file will be overwritten)")
 
+        parser.add_argument("-pcl", "--pointcloud", default=False, action="store_true",
+                        help="Convert Depth map image to point clouds")
+
+        parser.add_argument("-mesh", "--use_mesh", default=False, action="store_true",
+                        help="use mesh for rectification of the stereo images")
+
+        parser.add_argument("-mirror_rectified", "--mirror_rectified", default='true', choices=['true', 'false'],
+                        help="Normally, rectified_left/_right are mirrored for Stereo engine constraints. "
+                             "If false, disparity/depth will be mirrored instead. Default: true")
         argcomplete.autocomplete(parser)
 
         options = parser.parse_args()