Stereo Depth Video¶
This example is an upgraded Depth Preview. It has higher resolution (720p), each frame can be shown (mono left-right, rectified left-right, disparity and depth). There are 6 modes which you can select inside the code:
withDepth: if you turn it off it will became Mono Preview, so it will show only the 2 mono cameras
outputDepth: if you turn it on it will show the depth
lrcheck: used for better occlusion handling. For more information click here
extended: suitable for short range objects. For more information click here
subpixel: suitable for long range. For more information click here
Stereo Alpha Param¶
With –alpha you can select scaling during the rectification. Values between 0 and 1:
0.0 (default value) means that the rectified images are zoomed and shifted so that only valid pixels are visible (no black areas after rectification)
1.0 means that the rectified image is decimated and shifted so that all the pixels from the original images from the cameras are retained in the rectified images (no source image pixels are lost).
Similar samples:
Setup¶
Please run the install script to download all required dependencies. Please note that this script must be ran from git context, so you have to download the depthai-python repository first and then run the script
git clone https://github.com/luxonis/depthai-python.git
cd depthai-python/examples
python3 install_requirements.py
For additional information, please follow installation guide
Source code¶
Also available on GitHub
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 | #!/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.",
)
parser.add_argument(
"-rect",
"--out_rectified",
default=False,
action="store_true",
help="Generate and display rectified streams",
)
parser.add_argument(
"-lr",
"--lrcheck",
default=False,
action="store_true",
help="Better handling for occlusions",
)
parser.add_argument(
"-e",
"--extended",
default=False,
action="store_true",
help="Closer-in minimum depth, disparity range is doubled",
)
parser.add_argument(
"-s",
"--subpixel",
default=False,
action="store_true",
help="Better accuracy for longer distance, fractional disparity 32-levels",
)
parser.add_argument(
"-m",
"--median",
type=str,
default="7x7",
help="Choose the size of median filtering. Options: OFF | 3x3 | 5x5 | 7x7 (default)",
)
parser.add_argument(
"-d",
"--depth",
default=False,
action="store_true",
help="Display depth frames",
)
parser.add_argument(
"-swlr",
"--swap_left_right",
default=False,
action="store_true",
help="Swap left right frames",
)
parser.add_argument(
"-a",
"--alpha",
type=float,
default=None,
help="Alpha scaling parameter to increase FOV",
)
args = parser.parse_args()
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]
meshDirectory = args.mesh_dir # Output dir for mesh files
generateMesh = args.load_mesh # Load mesh files
outRectified = args.out_rectified # Output and display rectified streams
lrcheck = args.lrcheck # Better handling for occlusions
extended = args.extended # Closer-in minimum depth, disparity range is doubled
subpixel = args.subpixel # Better accuracy for longer distance, fractional disparity 32-levels
depth = args.depth # Display depth frames
medianMap = {
"OFF": dai.StereoDepthProperties.MedianFilter.MEDIAN_OFF,
"3x3": dai.StereoDepthProperties.MedianFilter.KERNEL_3x3,
"5x5": dai.StereoDepthProperties.MedianFilter.KERNEL_5x5,
"7x7": dai.StereoDepthProperties.MedianFilter.KERNEL_7x7,
}
if args.median not in medianMap:
exit("Unsupported median size!")
median = medianMap[args.median]
print("StereoDepth config options:")
print(f" Resolution: {resolution['w']}x{resolution['h']}")
print(" Left-Right check: ", lrcheck)
print(" Extended disparity:", extended)
print(" Subpixel: ", subpixel)
print(" Median filtering: ", median)
print(" Generating mesh files: ", generateMesh)
print(" Outputting mesh files to: ", meshDirectory)
def getMesh(calibData):
M1 = np.array(calibData.getCameraIntrinsics(dai.CameraBoardSocket.CAM_B, resolution[0], resolution[1]))
d1 = np.array(calibData.getDistortionCoefficients(dai.CameraBoardSocket.CAM_B))
R1 = np.array(calibData.getStereoLeftRectificationRotation())
M2 = np.array(calibData.getCameraIntrinsics(dai.CameraBoardSocket.CAM_C, resolution[0], resolution[1]))
d2 = np.array(calibData.getDistortionCoefficients(dai.CameraBoardSocket.CAM_C))
R2 = np.array(calibData.getStereoRightRectificationRotation())
mapXL, mapYL = cv2.initUndistortRectifyMap(M1, d1, R1, M2, resolution, cv2.CV_32FC1)
mapXR, mapYR = cv2.initUndistortRectifyMap(M2, d2, R2, M2, resolution, 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 getDisparityFrame(frame, cvColorMap):
maxDisp = stereo.initialConfig.getMaxDisparity()
disp = (frame * (255.0 / maxDisp)).astype(np.uint8)
disp = cv2.applyColorMap(disp, cvColorMap)
return disp
device = dai.Device()
calibData = device.readCalibration()
print("Creating Stereo Depth pipeline")
pipeline = dai.Pipeline()
camLeft = pipeline.create(dai.node.MonoCamera)
camRight = pipeline.create(dai.node.MonoCamera)
stereo = pipeline.create(dai.node.StereoDepth)
xoutLeft = pipeline.create(dai.node.XLinkOut)
xoutRight = pipeline.create(dai.node.XLinkOut)
xoutDisparity = pipeline.create(dai.node.XLinkOut)
xoutDepth = pipeline.create(dai.node.XLinkOut)
xoutRectifLeft = pipeline.create(dai.node.XLinkOut)
xoutRectifRight = pipeline.create(dai.node.XLinkOut)
if args.swap_left_right:
camLeft.setCamera("right")
camRight.setCamera("left")
else:
camLeft.setCamera("left")
camRight.setCamera("right")
for monoCam in (camLeft, camRight): # Common config
monoCam.setResolution(resolution['res'])
# monoCam.setFps(20.0)
stereo.setDefaultProfilePreset(dai.node.StereoDepth.PresetMode.HIGH_DENSITY)
stereo.initialConfig.setMedianFilter(median) # KERNEL_7x7 default
stereo.setRectifyEdgeFillColor(0) # Black, to better see the cutout
stereo.setLeftRightCheck(lrcheck)
stereo.setExtendedDisparity(extended)
stereo.setSubpixel(subpixel)
if args.alpha is not None:
stereo.setAlphaScaling(args.alpha)
config = stereo.initialConfig.get()
config.postProcessing.brightnessFilter.minBrightness = 0
stereo.initialConfig.set(config)
xoutLeft.setStreamName("left")
xoutRight.setStreamName("right")
xoutDisparity.setStreamName("disparity")
xoutDepth.setStreamName("depth")
xoutRectifLeft.setStreamName("rectifiedLeft")
xoutRectifRight.setStreamName("rectifiedRight")
camLeft.out.link(stereo.left)
camRight.out.link(stereo.right)
stereo.syncedLeft.link(xoutLeft.input)
stereo.syncedRight.link(xoutRight.input)
stereo.disparity.link(xoutDisparity.input)
if depth:
stereo.depth.link(xoutDepth.input)
if outRectified:
stereo.rectifiedLeft.link(xoutRectifLeft.input)
stereo.rectifiedRight.link(xoutRectifRight.input)
streams = ["left", "right"]
if outRectified:
streams.extend(["rectifiedLeft", "rectifiedRight"])
streams.append("disparity")
if depth:
streams.append("depth")
cvColorMap = cv2.applyColorMap(np.arange(256, dtype=np.uint8), cv2.COLORMAP_JET)
cvColorMap[0] = [0, 0, 0]
print("Creating DepthAI device")
with device:
device.startPipeline(pipeline)
# Create a receive queue for each stream
qList = [device.getOutputQueue(stream, 8, blocking=False) for stream in streams]
while True:
for q in qList:
name = q.getName()
frame = q.get().getCvFrame()
if name == "depth":
frame = frame.astype(np.uint16)
elif name == "disparity":
frame = getDisparityFrame(frame, cvColorMap)
cv2.imshow(name, frame)
if cv2.waitKey(1) == ord("q"):
break
|
Also available on GitHub
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 | #include <iostream>
// Includes common necessary includes for development using depthai library
#include "depthai/depthai.hpp"
static std::atomic<bool> withDepth{true};
static std::atomic<bool> outputDepth{false};
static std::atomic<bool> outputRectified{true};
static std::atomic<bool> lrcheck{true};
static std::atomic<bool> extended{false};
static std::atomic<bool> subpixel{false};
int main() {
using namespace std;
// Create pipeline
dai::Pipeline pipeline;
// Define sources and outputs
auto monoLeft = pipeline.create<dai::node::MonoCamera>();
auto monoRight = pipeline.create<dai::node::MonoCamera>();
auto stereo = withDepth ? pipeline.create<dai::node::StereoDepth>() : nullptr;
auto xoutLeft = pipeline.create<dai::node::XLinkOut>();
auto xoutRight = pipeline.create<dai::node::XLinkOut>();
auto xoutDisp = pipeline.create<dai::node::XLinkOut>();
auto xoutDepth = pipeline.create<dai::node::XLinkOut>();
auto xoutRectifL = pipeline.create<dai::node::XLinkOut>();
auto xoutRectifR = pipeline.create<dai::node::XLinkOut>();
// XLinkOut
xoutLeft->setStreamName("left");
xoutRight->setStreamName("right");
if(withDepth) {
xoutDisp->setStreamName("disparity");
xoutDepth->setStreamName("depth");
xoutRectifL->setStreamName("rectified_left");
xoutRectifR->setStreamName("rectified_right");
}
// Properties
monoLeft->setResolution(dai::MonoCameraProperties::SensorResolution::THE_720_P);
monoLeft->setCamera("left");
monoRight->setResolution(dai::MonoCameraProperties::SensorResolution::THE_720_P);
monoRight->setCamera("right");
if(withDepth) {
// StereoDepth
stereo->setDefaultProfilePreset(dai::node::StereoDepth::PresetMode::HIGH_DENSITY);
stereo->setRectifyEdgeFillColor(0); // black, to better see the cutout
// stereo->setInputResolution(1280, 720);
stereo->initialConfig.setMedianFilter(dai::MedianFilter::KERNEL_5x5);
stereo->setLeftRightCheck(lrcheck);
stereo->setExtendedDisparity(extended);
stereo->setSubpixel(subpixel);
// Linking
monoLeft->out.link(stereo->left);
monoRight->out.link(stereo->right);
stereo->syncedLeft.link(xoutLeft->input);
stereo->syncedRight.link(xoutRight->input);
stereo->disparity.link(xoutDisp->input);
if(outputRectified) {
stereo->rectifiedLeft.link(xoutRectifL->input);
stereo->rectifiedRight.link(xoutRectifR->input);
}
if(outputDepth) {
stereo->depth.link(xoutDepth->input);
}
} else {
// Link plugins CAM -> XLINK
monoLeft->out.link(xoutLeft->input);
monoRight->out.link(xoutRight->input);
}
// Connect to device and start pipeline
dai::Device device(pipeline);
auto leftQueue = device.getOutputQueue("left", 8, false);
auto rightQueue = device.getOutputQueue("right", 8, false);
auto dispQueue = withDepth ? device.getOutputQueue("disparity", 8, false) : nullptr;
auto depthQueue = withDepth ? device.getOutputQueue("depth", 8, false) : nullptr;
auto rectifLeftQueue = withDepth ? device.getOutputQueue("rectified_left", 8, false) : nullptr;
auto rectifRightQueue = withDepth ? device.getOutputQueue("rectified_right", 8, false) : nullptr;
// Disparity range is used for normalization
float disparityMultiplier = withDepth ? 255 / stereo->initialConfig.getMaxDisparity() : 0;
while(true) {
auto left = leftQueue->get<dai::ImgFrame>();
cv::imshow("left", left->getFrame());
auto right = rightQueue->get<dai::ImgFrame>();
cv::imshow("right", right->getFrame());
if(withDepth) {
auto disparity = dispQueue->get<dai::ImgFrame>();
cv::Mat disp(disparity->getCvFrame());
disp.convertTo(disp, CV_8UC1, disparityMultiplier); // Extend disparity range
cv::imshow("disparity", disp);
cv::Mat disp_color;
cv::applyColorMap(disp, disp_color, cv::COLORMAP_JET);
cv::imshow("disparity_color", disp_color);
if(outputDepth) {
auto depth = depthQueue->get<dai::ImgFrame>();
cv::imshow("depth", depth->getCvFrame());
}
if(outputRectified) {
auto rectifL = rectifLeftQueue->get<dai::ImgFrame>();
cv::imshow("rectified_left", rectifL->getFrame());
auto rectifR = rectifRightQueue->get<dai::ImgFrame>();
cv::imshow("rectified_right", rectifR->getFrame());
}
}
int key = cv::waitKey(1);
if(key == 'q' || key == 'Q') {
return 0;
}
}
return 0;
}
|