Merge pull request #822 from luxonis/custom_mesh

Erol444 · web-flow · commit 9f11020d7b09 · 2023-05-10T13:39:23.000+02:00
Custom mesh example/docs
diff --git a/docs/requirements.txt b/docs/requirements.txt
@@ -8,4 +8,5 @@ sphinx-autopackagesummary==1.3
 autodocsumm==0.2.2
 pathlib==1.0.1
 jinja2==3.0.3
+urllib3==1.26.15 # New urllib version breaks sphinx
 -r ./requirements_mkdoc.txt
diff --git a/docs/source/samples/StereoDepth/stereo_depth_custom_mesh.rst b/docs/source/samples/StereoDepth/stereo_depth_custom_mesh.rst
@@ -0,0 +1,52 @@
+Stereo Depth custom Mesh
+========================
+
+This example shows how you can load custom mesh to the device and use it for depth calculation.
+In this example, mesh files are generated from camera calibration data, but you can also use
+your own mesh files.
+
+By default, :ref:`StereoDepth` will use the same logic as inside the ``def getMesh()`` to calculate
+mesh files whenever horizontal FOV is larger than 90°. You could also force calculate the mesh using:
+
+.. code-block:: python
+
+    stereo = pipeline.create(dai.node.StereoDepth)
+    # Enable mesh calculation to correct distortion:
+    stereo.enableDistortionCorrection(True)
+
+
+StereoDepth node also allows you to load mesh files directly from a file path:
+
+.. code-block:: python
+
+    stereo = pipeline.create(dai.node.StereoDepth)
+    stereo.loadMeshFiles('path/to/left_mesh', 'path/to/right_mesh')
+
+Demo
+####
+
+.. image:: https://github.com/luxonis/depthai-python/assets/18037362/f2031bd4-0748-4a06-abb1-b52e9a17134e
+
+On the image above you can see that the rectified frame isn't as wide FOV as the original one,
+that's because the distortion correction is applied (in this case via custom mesh files), so the
+disparity matching can be performed correctly.
+
+Setup
+#####
+
+.. include::  /includes/install_from_pypi.rst
+
+Source code
+###########
+
+.. tabs::
+
+    .. tab:: Python
+
+        Also `available on GitHub <https://github.com/luxonis/depthai-python/blob/main/examples/StereoDepth/stereo_depth_custom_mesh.py>`__
+
+        .. literalinclude:: ../../../../examples/StereoDepth/stereo_depth_custom_mesh.py
+           :language: python
+           :linenos:
+
+.. include::  /includes/footer-short.rst
diff --git a/docs/source/tutorials/code_samples.rst b/docs/source/tutorials/code_samples.rst
@@ -137,6 +137,7 @@ are presented with code.
 - :ref:`Depth Crop Control` - Demonstrates how to control cropping of depth frames from the host
 - :ref:`Depth Post-Processing` - Depth post-processing filters
 - :ref:`Depth Preview` - Displays colorized stereo disparity
+- :ref:`Stereo Depth custom Mesh` - Calculate and load custom mesh for stereo depth calculation
 - :ref:`Stereo Depth from host` - Generates stereo depth frame from a set of mono images from the host
 - :ref:`Stereo Depth Video` - An extended version of **Depth Preview**
 - :ref:`RGB Depth alignment` - Displays RGB depth aligned frames
diff --git a/examples/StereoDepth/stereo_depth_custom_mesh.py b/examples/StereoDepth/stereo_depth_custom_mesh.py
@@ -0,0 +1,152 @@
+#!/usr/bin/env python3
+
+import cv2
+import numpy as np
+import depthai as dai
+import argparse
+
+parser = argparse.ArgumentParser()
+parser.add_argument("-res", "--resolution", type=str, default="720",
+    help="Sets the resolution on mono cameras. Options: 800 | 720 | 400")
+parser.add_argument("-md", "--mesh_dir", type=str, default=None,
+    help="Output directory for mesh files. If not specified mesh files won't be saved")
+parser.add_argument("-lm", "--load_mesh", default=False, action="store_true",
+    help="Read camera intrinsics, generate mesh files and load them into the stereo node.")
+args = parser.parse_args()
+
+meshDirectory = args.mesh_dir  # Output dir for mesh files
+generateMesh = args.load_mesh  # Load mesh files
+RES_MAP = {
+    '800': {'w': 1280, 'h': 800, 'res': dai.MonoCameraProperties.SensorResolution.THE_800_P },
+    '720': {'w': 1280, 'h': 720, 'res': dai.MonoCameraProperties.SensorResolution.THE_720_P },
+    '400': {'w': 640, 'h': 400, 'res': dai.MonoCameraProperties.SensorResolution.THE_400_P }
+}
+if args.resolution not in RES_MAP:
+    exit("Unsupported resolution!")
+
+resolution = RES_MAP[args.resolution]
+
+def getMesh(calibData):
+    M1 = np.array(calibData.getCameraIntrinsics(dai.CameraBoardSocket.CAM_B, resolution['w'], resolution['h']))
+    d1 = np.array(calibData.getDistortionCoefficients(dai.CameraBoardSocket.CAM_B))
+    R1 = np.array(calibData.getStereoLeftRectificationRotation())
+    M2 = np.array(calibData.getCameraIntrinsics(dai.CameraBoardSocket.CAM_C, resolution['w'], resolution['h']))
+    d2 = np.array(calibData.getDistortionCoefficients(dai.CameraBoardSocket.CAM_C))
+    R2 = np.array(calibData.getStereoRightRectificationRotation())
+    mapXL, mapYL = cv2.initUndistortRectifyMap(M1, d1, R1, M2, (resolution['w'], resolution['h']), cv2.CV_32FC1)
+    mapXR, mapYR = cv2.initUndistortRectifyMap(M2, d2, R2, M2, (resolution['w'], resolution['h']), cv2.CV_32FC1)
+
+    meshCellSize = 16
+    meshLeft = []
+    meshRight = []
+
+    for y in range(mapXL.shape[0] + 1):
+        if y % meshCellSize == 0:
+            rowLeft = []
+            rowRight = []
+            for x in range(mapXL.shape[1] + 1):
+                if x % meshCellSize == 0:
+                    if y == mapXL.shape[0] and x == mapXL.shape[1]:
+                        rowLeft.append(mapYL[y - 1, x - 1])
+                        rowLeft.append(mapXL[y - 1, x - 1])
+                        rowRight.append(mapYR[y - 1, x - 1])
+                        rowRight.append(mapXR[y - 1, x - 1])
+                    elif y == mapXL.shape[0]:
+                        rowLeft.append(mapYL[y - 1, x])
+                        rowLeft.append(mapXL[y - 1, x])
+                        rowRight.append(mapYR[y - 1, x])
+                        rowRight.append(mapXR[y - 1, x])
+                    elif x == mapXL.shape[1]:
+                        rowLeft.append(mapYL[y, x - 1])
+                        rowLeft.append(mapXL[y, x - 1])
+                        rowRight.append(mapYR[y, x - 1])
+                        rowRight.append(mapXR[y, x - 1])
+                    else:
+                        rowLeft.append(mapYL[y, x])
+                        rowLeft.append(mapXL[y, x])
+                        rowRight.append(mapYR[y, x])
+                        rowRight.append(mapXR[y, x])
+            if (mapXL.shape[1] % meshCellSize) % 2 != 0:
+                rowLeft.append(0)
+                rowLeft.append(0)
+                rowRight.append(0)
+                rowRight.append(0)
+
+            meshLeft.append(rowLeft)
+            meshRight.append(rowRight)
+
+    meshLeft = np.array(meshLeft)
+    meshRight = np.array(meshRight)
+
+    return meshLeft, meshRight
+
+def saveMeshFiles(meshLeft, meshRight, outputPath):
+    print("Saving mesh to:", outputPath)
+    meshLeft.tofile(outputPath + "/left_mesh.calib")
+    meshRight.tofile(outputPath + "/right_mesh.calib")
+
+
+def create_pipeline(device: dai.Device) -> dai.Pipeline:
+    calibData = device.readCalibration()
+    print("Creating Stereo Depth pipeline")
+    pipeline = dai.Pipeline()
+
+    camLeft = pipeline.create(dai.node.MonoCamera)
+    camLeft.setBoardSocket(dai.CameraBoardSocket.LEFT)
+
+    camRight = pipeline.create(dai.node.MonoCamera)
+    camRight.setBoardSocket(dai.CameraBoardSocket.RIGHT)
+
+    xoutRight = pipeline.create(dai.node.XLinkOut)
+    xoutRight.setStreamName("right")
+    camRight.out.link(xoutRight.input)
+
+    for monoCam in (camLeft, camRight):  # Common config
+        monoCam.setResolution(resolution['res'])
+        # monoCam.setFps(20.0)
+
+    stereo = pipeline.create(dai.node.StereoDepth)
+    camLeft.out.link(stereo.left)
+    camRight.out.link(stereo.right)
+    stereo.setDefaultProfilePreset(dai.node.StereoDepth.PresetMode.HIGH_DENSITY)
+    stereo.setRectifyEdgeFillColor(0)  # Black, to better see the cutout
+    stereo.setLeftRightCheck(True)
+    stereo.setExtendedDisparity(True)
+
+    
+
+
+    xoutDisparity = pipeline.create(dai.node.XLinkOut)
+    xoutDisparity.setStreamName("disparity")
+    stereo.disparity.link(xoutDisparity.input)
+
+    xoutRectifRight = pipeline.create(dai.node.XLinkOut)
+    xoutRectifRight.setStreamName("rectifiedRight")
+    stereo.rectifiedRight.link(xoutRectifRight.input)
+
+    # Create custom meshes from calibration data. Here you could also
+    # load your own mesh files, or generate them in any other way.
+    leftMesh, rightMesh = getMesh(calibData)
+    if generateMesh:
+        meshLeft = list(leftMesh.tobytes())
+        meshRight = list(rightMesh.tobytes())
+        # Load mesh data to the StereoDepth node
+        stereo.loadMeshData(meshLeft, meshRight)
+
+    if meshDirectory is not None:
+        saveMeshFiles(leftMesh, rightMesh, meshDirectory)
+    return pipeline
+
+with dai.Device() as device:
+    device.startPipeline(create_pipeline(device))
+
+    # Create a receive queue for each stream
+    qList = [device.getOutputQueue(stream, 8, blocking=False) for stream in ['right', 'rectifiedRight', 'disparity']]
+
+    while True:
+        for q in qList:
+            name = q.getName()
+            frame = q.get().getCvFrame()
+            cv2.imshow(name, frame)
+        if cv2.waitKey(1) == ord("q"):
+            break
diff --git a/examples/StereoDepth/stereo_depth_video.py b/examples/StereoDepth/stereo_depth_video.py
@@ -85,11 +85,16 @@
 )
 args = parser.parse_args()
 
-resolutionMap = {"800": (1280, 800), "720": (1280, 720), "400": (640, 400)}
-if args.resolution not in resolutionMap:
+RES_MAP = {
+    '800': {'w': 1280, 'h': 800, 'res': dai.MonoCameraProperties.SensorResolution.THE_800_P },
+    '720': {'w': 1280, 'h': 720, 'res': dai.MonoCameraProperties.SensorResolution.THE_720_P },
+    '400': {'w': 640, 'h': 400, 'res': dai.MonoCameraProperties.SensorResolution.THE_400_P }
+}
+if args.resolution not in RES_MAP:
     exit("Unsupported resolution!")
 
-resolution = resolutionMap[args.resolution]
+resolution = RES_MAP[args.resolution]
+
 meshDirectory = args.mesh_dir  # Output dir for mesh files
 generateMesh = args.load_mesh  # Load mesh files
 
@@ -111,7 +116,7 @@
 median = medianMap[args.median]
 
 print("StereoDepth config options:")
-print("    Resolution:  ", resolution)
+print(f"    Resolution:  {resolution['w']}x{resolution['h']}")
 print("    Left-Right check:  ", lrcheck)
 print("    Extended disparity:", extended)
 print("    Subpixel:          ", subpixel)
@@ -188,7 +193,6 @@ def getDisparityFrame(frame, cvColorMap):
 
     return disp
 
-
 device = dai.Device()
 calibData = device.readCalibration()
 print("Creating Stereo Depth pipeline")
@@ -211,15 +215,8 @@ def getDisparityFrame(frame, cvColorMap):
     camLeft.setBoardSocket(dai.CameraBoardSocket.LEFT)
     camRight.setBoardSocket(dai.CameraBoardSocket.RIGHT)
 
-res = (
-    dai.MonoCameraProperties.SensorResolution.THE_800_P
-    if resolution[1] == 800
-    else dai.MonoCameraProperties.SensorResolution.THE_720_P
-    if resolution[1] == 720
-    else dai.MonoCameraProperties.SensorResolution.THE_400_P
-)
 for monoCam in (camLeft, camRight):  # Common config
-    monoCam.setResolution(res)
+    monoCam.setResolution(resolution['res'])
     # monoCam.setFps(20.0)
 
 stereo.setDefaultProfilePreset(dai.node.StereoDepth.PresetMode.HIGH_DENSITY)
@@ -259,15 +256,6 @@ def getDisparityFrame(frame, cvColorMap):
 if depth:
     streams.append("depth")
 
-leftMesh, rightMesh = getMesh(calibData)
-if generateMesh:
-    meshLeft = list(leftMesh.tobytes())
-    meshRight = list(rightMesh.tobytes())
-    stereo.loadMeshData(meshLeft, meshRight)
-
-if meshDirectory is not None:
-    saveMeshFiles(leftMesh, rightMesh, meshDirectory)
-
 cvColorMap = cv2.applyColorMap(np.arange(256, dtype=np.uint8), cv2.COLORMAP_JET)
 cvColorMap[0] = [0, 0, 0]
 print("Creating DepthAI device")
diff --git a/utilities/cam_test.py b/utilities/cam_test.py
@@ -64,7 +64,7 @@ def socket_type_pair(arg):
                     "E.g: -cams rgb,m right,c . Default: rgb,c left,m right,m camd,c")
 parser.add_argument('-mres', '--mono-resolution', type=int, default=800, choices={480, 400, 720, 800},
                     help="Select mono camera resolution (height). Default: %(default)s")
-parser.add_argument('-cres', '--color-resolution', default='1080', choices={'720', '800', '1080', '1200', '4k', '5mp', '12mp', '48mp'},
+parser.add_argument('-cres', '--color-resolution', default='1080', choices={'720', '800', '1080', '1200', '4k', '5mp', '12mp', '13mp', '48mp'},
                     help="Select color camera resolution / height. Default: %(default)s")
 parser.add_argument('-rot', '--rotate', const='all', choices={'all', 'rgb', 'mono'}, nargs="?",
                     help="Which cameras to rotate 180 degrees. All if not filtered")
@@ -145,6 +145,7 @@ def socket_type_pair(arg):
     '4k':   dai.ColorCameraProperties.SensorResolution.THE_4_K,
     '5mp': dai.ColorCameraProperties.SensorResolution.THE_5_MP,
     '12mp': dai.ColorCameraProperties.SensorResolution.THE_12_MP,
+    '13mp': dai.ColorCameraProperties.SensorResolution.THE_13_MP,
     '48mp': dai.ColorCameraProperties.SensorResolution.THE_48_MP,
 }
 
