UNWARP_ALPHA = 0.85
def CreateUnWarpMesh(calib_data, in_size, out_size, mesh_cell_size):
camera_intrinsic = np.array(calib_data.getCameraIntrinsics(dai.CameraBoardSocket.RGB))
distortion_coeff = np.array(calib_data.getDistortionCoefficients(dai.CameraBoardSocket.RGB))[:8]
print(camera_intrinsic)
print(distortion_coeff)
virtual_camera, _ = cv2.getOptimalNewCameraMatrix(
camera_intrinsic,
distortion_coeff,
in_size,
UNWARP_ALPHA,
out_size,
True
)
mapX, mapY = cv2.initUndistortRectifyMap(
camera_intrinsic,
distortion_coeff,
None,
virtual_camera,
out_size,
cv2.CV_32FC1
)
mesh0 = []
# Creates subsampled mesh which will be loaded on to device to undistort the image
for y in range(mapX.shape[0] + 1): # iterating over height of the image
if y % mesh_cell_size == 0:
rowLeft = []
for x in range(mapX.shape[1]): # iterating over width of the image
if x % mesh_cell_size == 0:
if y == mapX.shape[0] and x == mapX.shape[1]:
rowLeft.append(mapX[y - 1, x - 1])
rowLeft.append(mapY[y - 1, x - 1])
elif y == mapX.shape[0]:
rowLeft.append(mapX[y - 1, x])
rowLeft.append(mapY[y - 1, x])
elif x == mapX.shape[1]:
rowLeft.append(mapX[y, x - 1])
rowLeft.append(mapY[y, x - 1])
else:
rowLeft.append(mapX[y, x])
rowLeft.append(mapY[y, x])
if (mapX.shape[1] % mesh_cell_size) % 2 != 0:
rowLeft.append(0)
rowLeft.append(0)
mesh0.append(rowLeft)
mesh0 = np.array(mesh0)
bb_m_w = mesh0.shape[1] // 2
bb_m_h = mesh0.shape[0]
mesh0.resize(bb_m_w * bb_m_h, 2)
bb_mesh = list(map(tuple, mesh0))
return bb_mesh, bb_m_w, bb_m_h
def CreateUnWarpNode(type, pipeline, calib_data, in_size, out_size, mesh_cell_size):
bb_mesh , bb_m_w, bb_m_h = CreateUnWarpMesh(calib_data, in_size, out_size, mesh_cell_size)
unwarp = pipeline.create(type)
unwarp.setWarpMesh(bb_mesh, bb_m_w, bb_m_h)
unwarp.setMaxOutputFrameSize(out_size[0] * out_size[1] * 3)
unwarp.inputImage.setBlocking(False)
unwarp.inputImage.setQueueSize(1)
return unwarp
device = dai.Device()
# device.setIrFloodLightBrightness(1500)
device.setIrLaserDotProjectorBrightness(1000)
device.setLogLevel(dai.LogLevel.DEBUG)
device.setLogOutputLevel(dai.LogLevel.DEBUG)
pipeline = dai.Pipeline()
calib_data = device.readCalibration2()
BaseStream.device = device
BaseStream.pipeline = pipeline
pipeline.setOpenVINOVersion(dai.OpenVINO.Version.VERSION_2022_1)
cam_rgb = pipeline.create(dai.node.ColorCamera)
cam_rgb.setResolution(dai.ColorCameraProperties.SensorResolution.THE_800_P) #THE_800_P
cam_rgb.setInterleaved(False)
cam_rgb.setFps(CAMERA_FPS)
cam_rgb.setBoardSocket(dai.CameraBoardSocket.CAM_A)
cam_rgb.setIspScale(1,1)
rgb_frame_size = cam_rgb.getIspSize()
cam_rgb.setPreviewKeepAspectRatio(True)
cam_rgb.setPreviewSize(rgb_frame_size)
left = pipeline.createMonoCamera()
left.setBoardSocket(dai.CameraBoardSocket.CAM_B)
left.setResolution(dai.MonoCameraProperties.SensorResolution.THE_800_P)
left.setFps(CAMERA_FPS)
right = pipeline.createMonoCamera()
right.setBoardSocket(dai.CameraBoardSocket.CAM_C)
right.setResolution(dai.MonoCameraProperties.SensorResolution.THE_800_P)
right.setFps(CAMERA_FPS)
stereo = pipeline.createStereoDepth()
stereo.setConfidenceThreshold(230)
# LR-check is required for depth alignment
stereo.setLeftRightCheck(True)
# stereo.setDepthAlign(dai.CameraBoardSocket.RGB) #THIS UNWARP THE IMAGE CROPPING IT
stereo.setDepthAlign(dai.StereoDepthConfig.AlgorithmControl.DepthAlign.CENTER) #THIS IS THE BEST OPTION I FOUND
stereo.setRectification(True)
stereo.setAlphaScaling(UNWARP_ALPHA)
stereo.setRectifyEdgeFillColor(0)
# MEDIAN_OFF necessary in depthai 2.7.2.
# Otherwise : [critical] Fatal error. Please report to developers. Log: 'StereoSipp' '533'
# stereo.setMedianFilter(dai.StereoDepthProperties.MedianFilter.MEDIAN_OFF)
left.out.link(stereo.left)
right.out.link(stereo.right)
spatial_location_calculator = pipeline.createSpatialLocationCalculator()
spatial_location_calculator.setWaitForConfigInput(True)
spatial_location_calculator.inputDepth.setBlocking(False)
spatial_location_calculator.inputDepth.setQueueSize(1)
stereo.depth.link(spatial_location_calculator.inputDepth)
UNWARP_IMAGE_SIZE = (cam_rgb.getPreviewWidth(), cam_rgb.getPreviewHeight())
unwarp_full_node : dai.node.Warp = CreateUnWarpNode(dai.node.Warp, pipeline, calib_data, cam_rgb.getPreviewSize(), UNWARP_IMAGE_SIZE, 16)
# unwarp_full_node.setOutputSize(cam_rgb.getPreviewSize())
[cam_rgb.preview.link](http://cam_rgb.preview.link/)(unwarp_full_node.inputImage)
manip_square_crop = pipeline.create(dai.node.ImageManip)
manip_square_crop.inputImage.setBlocking(False)
manip_square_crop.inputImage.setQueueSize(1)
min_size = min(*UNWARP_IMAGE_SIZE)
manip_square_crop.setMaxOutputFrameSize(min_size * min_size * 3)
manip_square_crop.initialConfig.setCenterCrop(1,1)
[unwarp_full_node.out.link](http://unwarp_full_node.out.link/)(manip_square_crop.inputImage)