Tof

examples/camera_wrappers_examples/live_pcl.py

@@ -1,13 +1,12 @@
 import argparse
 
 import cv2
-import numpy as np
-from pollen_vision.camera_wrappers.depthai import SDKWrapper
+from pollen_vision.camera_wrappers import SDKWrapper
 from pollen_vision.camera_wrappers.depthai.utils import (
     get_config_file_path,
     get_config_files_names,
 )
-from pollen_vision.perception.utils.pcl_visualizer import PCLVisualizer
+from pollen_vision.utils.pcl_visualizer import PCLVisualizer
 
 valid_configs = get_config_files_names()
 
@@ -23,23 +22,19 @@
 
 w = SDKWrapper(get_config_file_path(args.config), compute_depth=True)
 
-K = w.cam_config.get_K_left()
+K = w.get_K()
 P = PCLVisualizer(K)
 
 while True:
     data, lat, _ = w.get_data()
-    print(lat["depthNode_left"], lat["depthNode_right"])
 
     depth = data["depth"]
     rgb = data["left"]
-    disparity = data["disparity"]
+
     P.update(cv2.cvtColor(rgb, cv2.COLOR_BGR2RGB), depth)
 
-    disparity = (disparity * (255 / w.depth_max_disparity)).astype(np.uint8)
-    disparity = cv2.applyColorMap(disparity, cv2.COLORMAP_JET)
-    cv2.imshow("disparity", disparity)
-    cv2.imshow("left", data["depthNode_left"])
-    cv2.imshow("right", data["depthNode_right"])
+    cv2.imshow("depth", depth)
+    cv2.imshow("rgb", rgb)
 
     key = cv2.waitKey(1)
     P.tick()

examples/camera_wrappers_examples/live_pcl_orbbec.py

@@ -1,6 +1,6 @@
 import cv2
 from pollen_vision.camera_wrappers.orbbec.orbbec_wrapper import OrbbecWrapper
-from pollen_vision.perception.utils.pcl_visualizer import PCLVisualizer
+from pollen_vision.utils.pcl_visualizer import PCLVisualizer
 
 w = OrbbecWrapper()
 

examples/camera_wrappers_examples/live_pcl_tof.py

@@ -0,0 +1,37 @@
+import argparse
+
+import cv2
+from pollen_vision.camera_wrappers import TOFWrapper
+from pollen_vision.camera_wrappers.depthai.utils import (
+    get_config_file_path,
+    get_config_files_names,
+)
+from pollen_vision.utils.pcl_visualizer import PCLVisualizer
+
+valid_configs = get_config_files_names()
+
+argParser = argparse.ArgumentParser(description="depth wrapper example")
+argParser.add_argument(
+    "--config",
+    type=str,
+    required=True,
+    choices=valid_configs,
+    help=f"Configutation file name : {valid_configs}",
+)
+args = argParser.parse_args()
+
+w = TOFWrapper(get_config_file_path(args.config), crop=False, fps=30, create_pointcloud=True)
+
+P = PCLVisualizer()
+P.add_frame("origin")
+
+
+while True:
+    data, lat, _ = w.get_data()
+
+    rgb = data["left"]
+
+    P.update(cv2.cvtColor(rgb, cv2.COLOR_BGR2RGB), points=data["pointcloud"])
+
+    key = cv2.waitKey(1)
+    P.tick()

examples/camera_wrappers_examples/tof_example.py

@@ -0,0 +1,25 @@
+import cv2
+import depthai as dai
+from pollen_vision.camera_wrappers import TOFWrapper
+from pollen_vision.camera_wrappers.depthai.utils import (
+    colorizeDepth,
+    get_config_file_path,
+)
+
+t = TOFWrapper(get_config_file_path("CONFIG_IMX296_TOF"), fps=30, crop=False)
+
+print(dai.__version__)
+while True:
+    data, _, _ = t.get_data()
+    left = data["left"]
+
+    depth = data["depth"]
+    tof_intensity = data["tof_intensity"]
+
+    cv2.imshow("left", left)
+    cv2.imshow("tof intensity", tof_intensity)
+    colorized_depth = colorizeDepth(data["depth"])
+    blended = cv2.addWeighted(left, 0.5, colorized_depth, 0.5, 0)
+    cv2.imshow("blended", blended)
+    cv2.imshow("colorized_depth", colorized_depth)
+    cv2.waitKey(1)

pollen_vision/config_files_vision/CONFIG_AR0234_TOF.json

@@ -0,0 +1,6 @@
+{
+  "socket_to_name": { "CAM_B": "right", "CAM_C": "left", "CAM_D": "tof" },
+  "inverted": false,
+  "fisheye": true,
+  "mono": false
+}

pollen_vision/config_files_vision/CONFIG_IMX296_TOF.json

@@ -0,0 +1,10 @@
+{
+  "socket_to_name": {
+    "CAM_B": "right",
+    "CAM_C": "left",
+    "CAM_D": "tof"
+  },
+  "inverted": false,
+  "fisheye": true,
+  "mono": false
+}

pollen_vision/pollen_vision/camera_wrappers/__init__.py

@@ -1,6 +1,7 @@
 from .camera_wrapper import CameraWrapper  # noqa: F401
 from .depthai.sdk import SDKWrapper  # noqa: F401
 from .depthai.teleop import TeleopWrapper  # noqa: F401
+from .depthai.tof import TOFWrapper  # noqa: F401
 
 # from .pollen_sdk_camera.pollen_sdk_camera_wrapper import (  # noqa: F401
 #     PollenSDKCameraWrapper,

pollen_vision/pollen_vision/camera_wrappers/depthai/__init__.py

@@ -1,2 +1,3 @@
 from .sdk import SDKWrapper  # noqa: F401
 from .teleop import TeleopWrapper  # noqa: F401
+from .tof import TOFWrapper  # noqa: F401

pollen_vision/pollen_vision/camera_wrappers/depthai/calibration/.gitignore

@@ -0,0 +1,2 @@
+*.json
+calib_images/*

pollen_vision/pollen_vision/camera_wrappers/depthai/calibration/acquire.py

@@ -3,7 +3,7 @@
 
 import cv2
 import numpy as np
-from pollen_vision.camera_wrappers.depthai import SDKWrapper
+from pollen_vision.camera_wrappers.depthai import SDKWrapper, TOFWrapper
 from pollen_vision.camera_wrappers.depthai.utils import (
     get_config_file_path,
     get_config_files_names,
@@ -24,14 +24,22 @@
     default="./calib_images/",
     help="Directory where the acquired images are stored (default ./calib_images/)",
 )
+argParser.add_argument("--tof", action="store_true", help="Has tof sensor ?")
 args = argParser.parse_args()
 
-w = SDKWrapper(get_config_file_path(args.config), compute_depth=False, rectify=False)
+if not args.tof:
+    w = SDKWrapper(get_config_file_path(args.config), compute_depth=False, rectify=False)
+else:
+    w = TOFWrapper(get_config_file_path("CONFIG_IMX296_TOF"), fps=30, noalign=True, rectify=False)
+
 
 left_path = os.path.join(args.imagesPath, "left")
 right_path = os.path.join(args.imagesPath, "right")
 os.makedirs(left_path, exist_ok=True)
 os.makedirs(right_path, exist_ok=True)
+if args.tof:
+    tof_path = os.path.join(args.imagesPath, "tof")
+    os.makedirs(tof_path, exist_ok=True)
 
 print("Press return to save an image pair.")
 print("(Keep the focus on the opencv window for the inputs to register.)")
@@ -44,12 +52,20 @@
     for name in data.keys():
         _data[name] = data[name]
 
-    concat = np.hstack((_data["left"], _data["right"]))
-    cv2.imshow(name, cv2.resize(concat, (0, 0), fx=0.5, fy=0.5))
+    if args.tof:
+        tof_intensity = _data["tof_intensity"]
+        tof_intensity = cv2.resize(tof_intensity, _data["left"].shape[:2][::-1])
+        tof_intensity = np.dstack((tof_intensity, tof_intensity, tof_intensity))
+        concat = np.hstack((_data["left"], _data["right"], tof_intensity))
+    else:
+        concat = np.hstack((_data["left"], _data["right"]))
+    cv2.imshow("concat", cv2.resize(concat, (0, 0), fx=0.5, fy=0.5))
     key = cv2.waitKey(1)
     if key == 13:
         cv2.imwrite(os.path.join(left_path, str(i) + ".png"), data["left"])
         cv2.imwrite(os.path.join(right_path, str(i) + ".png"), data["right"])
+        if args.tof:
+            cv2.imwrite(os.path.join(tof_path, str(i) + ".png"), data["tof_intensity"])
         print("Saved image pair ", i, "to", left_path, " and ", right_path)
         i += 1
     elif key == 27 or key == ord("q"):

pollen_vision/pollen_vision/camera_wrappers/depthai/calibration/check_epilines.py

@@ -2,8 +2,9 @@
 import logging
 
 import cv2
+import numpy as np
 from cv2 import aruco
-from pollen_vision.camera_wrappers.depthai import SDKWrapper
+from pollen_vision.camera_wrappers.depthai import SDKWrapper, TOFWrapper
 from pollen_vision.camera_wrappers.depthai.utils import (
     drawEpiLines,
     get_config_file_path,
@@ -21,11 +22,15 @@
     choices=valid_configs,
     help=f"Configutation file name : {valid_configs}",
 )
+argParser.add_argument("--tof", action="store_true", help="Has tof sensor ?")
 args = argParser.parse_args()
 
 ARUCO_DICT = aruco.getPredefinedDictionary(aruco.DICT_4X4_1000)
 
-w = SDKWrapper(get_config_file_path(args.config), rectify=True, resize=(1280, 720), fps=60)
+if not args.tof:
+    w = SDKWrapper(get_config_file_path(args.config), rectify=True, resize=(1280, 720), fps=60)
+else:
+    w = TOFWrapper(get_config_file_path("CONFIG_IMX296_TOF"), fps=30, rectify=True)
 
 
 while True:
@@ -35,8 +40,15 @@
     for name in data.keys():
         _data[name] = data[name]
     epi = drawEpiLines(_data["left"], _data["right"], ARUCO_DICT)
-    epi = cv2.resize(epi, (0, 0), fx=0.9, fy=0.9)
+    epi = cv2.resize(epi, (0, 0), fx=0.5, fy=0.5)
     cv2.imshow("epi", epi)
+
+    if args.tof:
+        right_resized = cv2.resize(_data["right"], _data["tof_intensity"].shape[:2][::-1])
+        tof_im = _data["tof_intensity"]
+        tof_im = np.dstack((tof_im, tof_im, tof_im))
+        epi_right_tof = drawEpiLines(right_resized, tof_im, ARUCO_DICT)
+        cv2.imshow("epi_right_tof", epi_right_tof)
     key = cv2.waitKey(1)
 
     if key == 27 or key == ord("q"):

pollen_vision/pollen_vision/camera_wrappers/depthai/calibration/flash.py

@@ -24,7 +24,8 @@
     required=True,
     help="Path to the calibration json file",
 )
+argParser.add_argument("--tof", action="store_true", help="Has tof sensor ?")
 args = argParser.parse_args()
 
 w = SDKWrapper(get_config_file_path(args.config))
-w.flash(args.calib_json_file)
+w.flash(args.calib_json_file, tof=args.tof)