thank you for your reply.
I am using the code below.
(code)
#!/usr/bin/env python3
import cv2
import numpy as np
import depthai as dai
import argparse
import csv
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=True,
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()
resolutionMap = {"800": (1280, 800), "720": (1280, 720), "400": (640, 400)}
if args.resolution not in resolutionMap:
exit("Unsupported resolution!")
resolution = resolutionMap[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(" Resolution: ", resolution)
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.LEFT, resolution[0], resolution[1]))
d1 = np.array(calibData.getDistortionCoefficients(dai.CameraBoardSocket.LEFT))
R1 = np.array(calibData.getStereoLeftRectificationRotation())
M2 = np.array(calibData.getCameraIntrinsics(dai.CameraBoardSocket.RIGHT, resolution[0], resolution[1]))
d2 = np.array(calibData.getDistortionCoefficients(dai.CameraBoardSocket.RIGHT))
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)
camRgb = pipeline.create(dai.node.ColorCamera)
stereo = pipeline.create(dai.node.StereoDepth)
xoutRgb = pipeline.create(dai.node.XLinkOut)
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.setBoardSocket(dai.CameraBoardSocket.RIGHT)
camRight.setBoardSocket(dai.CameraBoardSocket.LEFT)
else:
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.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")
xoutRgb.setStreamName("rgb")
xoutDisparity.setStreamName("disparity")
xoutDepth.setStreamName("depth")
xoutRectifLeft.setStreamName("rectifiedLeft")
xoutRectifRight.setStreamName("rectifiedRight")
camRgb.setPreviewSize(640,480)
camRgb.setInterleaved(False)
camRgb.setColorOrder(dai.ColorCameraProperties.ColorOrder.RGB)
camLeft.out.link(stereo.left)
camRight.out.link(stereo.right)
camRgb.preview.link(xoutRgb.input)
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","disparity","depth","rgb"]
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")
import csv
with device:
device.startPipeline(pipeline)
qList = [device.getOutputQueue(stream, 8, blocking=False) for stream in streams]
depth_frame = None
disparity_frame = None
rgb_frame = None
while True:
for q in qList:
name = q.getName()
frame = q.get().getCvFrame()
if name == "rgb":
rgb_frame = frame.copy()
elif name == "depth":
frame = frame.astype(np.uint16)
depth_frame = frame.copy()
elif name == "disparity":
frame = getDisparityFrame(frame, cvColorMap)
disparity_frame = frame.copy()
cv2.imshow(name,frame)
key = cv2.waitKey(1)
if key == ord('s') and depth_frame is not None:
with open('depth.csv', 'w', newline='') as file:
writer = csv.writer(file)
writer.writerows(depth_frame.tolist())
print("Depth data written to CSV file")
if rgb_frame is not None:
cv2.imwrite('rgb_frame.png', rgb_frame)
print("RGB data written to png file")
elif key == ord('d') and disparity_frame is not None:
with open('disparity.csv', 'w', newline='') as file:
writer = csv.writer(file)
writer.writerows(disparity_frame.tolist())
print("Disparity data written to CSV file")
if rgb_frame is not None:
cv2.imwrite('rgb_frame.png', rgb_frame)
print("RGB data written to png file")
elif key == ord('q'):
break
--------------------------------------------
That's it. If any additional information is missing, please contact us.