def next_frame(self):
self.fps.update()
if self.input_type == "rgb":
# Send cropping information to manip node on device
cfg = dai.ImageManipConfig()
points = [
[self.crop_region.xmin, self.crop_region.ymin],
[self.crop_region.xmax - 1, self.crop_region.ymin],
[self.crop_region.xmax - 1, self.crop_region.ymax - 1],
[self.crop_region.xmin, self.crop_region.ymax - 1],
]
point2fList = []
for p in points:
pt = dai.Point2f()
pt.x, pt.y = p[0], p[1]
point2fList.append(pt)
cfg.setWarpTransformFourPoints(point2fList, False)
cfg.setResize(self.pd_input_length, self.pd_input_length)
cfg.setFrameType(dai.ImgFrame.Type.RGB888p)
self.q_manip_cfg.send(cfg)
# Get the device camera frame if wanted
if self.laconic:
frame = np.zeros((self.frame_size, self.frame_size, 3),
dtype=np.uint8)
else:
in_video = self.q_video.get()
frame = in_video.getCvFrame()
else:
if self.input_type == "image":
frame = self.img.copy()
else:
ok, frame = self.cap.read()
if not ok:
return None, None
# Cropping of the video frame
cropped = self.crop_and_resize(frame, self.crop_region)
cropped = cv2.cvtColor(cropped,
cv2.COLOR_BGR2RGB).transpose(2, 0, 1)
frame_nn = dai.ImgFrame()
frame_nn.setTimestamp(time.monotonic())
frame_nn.setWidth(self.pd_input_length)
frame_nn.setHeight(self.pd_input_length)
frame_nn.setData(cropped)
self.q_pd_in.send(frame_nn)
# Get result from device
inference = self.q_pd_out.get()
body = self.pd_postprocess(inference)
self.crop_region = body.next_crop_region
# Statistics
if self.stats:
self.nb_frames += 1
self.nb_pd_inferences += 1
return frame, body
def _create_depth(self, pipeline, using_nn):
'''!
Creates the depth node.
Uses the appropiate settings for the configs and existing pipeline.
Parameters:
@param pipeline: The pipeline to add the node to
@param using_nn: Whether the pipeline is using a neural network or not
'''
if self.depth:
depth_node = pipeline.createStereoDepth()
depth_node.setLeftRightCheck(True)
depth_node.setConfidenceThreshold(200)
if (not using_nn) and self._configs["depth_close"] == True: #See close -> Extended
depth_node.setExtendedDisparity(True)
depth_node.setSubpixel(False)
elif (not using_nn) and self._configs["depth_far"] == True: #See longer -> Subpixel
depth_node.setSubpixel(True)
depth_node.setExtendedDisparity(False)
else:
depth_node.setDepthAlign(dai.CameraBoardSocket.RGB)
#depth_node.setDepthAlign(dai.StereoDepthProperties.DepthAlign.CENTER)
depth_node.setSubpixel(False)
depth_node.setExtendedDisparity(False)
self.nodes[OAK_Stage.DEPTH] = depth_node
if self._configs["depth_host"] == False:
spatialLocationCalculator = pipeline.createSpatialLocationCalculator()
topLeft = dai.Point2f(0.4, 0.4)
bottomRight = dai.Point2f(0.6, 0.6)
config = dai.SpatialLocationCalculatorConfigData()
config.depthThresholds.lowerThreshold = 100
config.depthThresholds.upperThreshold = 10000
config.roi = dai.Rect(topLeft, bottomRight)
spatialLocationCalculator.setWaitForConfigInput(False)
spatialLocationCalculator.initialConfig.addROI(config)
depth_node.disparity.link(spatialLocationCalculator.inputDepth)
self.nodes["spartial_location_calculator"] = spatialLocationCalculator
# StereoDepth stereo.setOutputDepth(outputDepth) stereo.setOutputRectified(outputRectified) stereo.setConfidenceThreshold(255) stereo.setLeftRightCheck(lrcheck) stereo.setSubpixel(subpixel) monoLeft.out.link(stereo.left) monoRight.out.link(stereo.right) spatialLocationCalculator.passthroughDepth.link(xoutDepth.input) stereo.depth.link(spatialLocationCalculator.inputDepth) topLeft = dai.Point2f(0.4, 0.4) bottomRight = dai.Point2f(0.6, 0.6) # topLeft2 = dai.Point2f(0.6, 0.4) # bottomRight2 = dai.Point2f(0.8, 0.6) # topLeft = dai.Point2f(0.1, 0.1) # bottomRight = dai.Point2f(0.9, 0.9) spatialLocationCalculator.setWaitForConfigInput(False) config = dai.SpatialLocationCalculatorConfigData() config.depthThresholds.lowerThreshold = 100 config.depthThresholds.upperThreshold = 10000 config.roi = dai.Rect(topLeft, bottomRight) spatialLocationCalculator.initialConfig.addROI(config) spatialLocationCalculator.out.link(xoutSpatialData.input) xinSpatialCalcConfig.out.link(spatialLocationCalculator.inputConfig)
warpIdx = (warpIdx + 1) % len(warpList)
testFourPt = True
testDescription = warpList[warpIdx][2]
print("Warp 4-point transform: ", testDescription)
elif key == ord('h'):
printControls()
# Send an updated config with continuous rotate, or after a key press
if key >= 0 or (not testFourPt and abs(rotateRate) > 0.0001):
cfg = dai.ImageManipConfig()
if testFourPt:
test = warpList[warpIdx]
points, normalized = test[0], test[1]
point2fList = []
for p in points:
pt = dai.Point2f()
pt.x, pt.y = p[0], p[1]
point2fList.append(pt)
cfg.setWarpTransformFourPoints(point2fList, normalized)
else:
angleDeg += rotateRate
rotatedRect = ((320, 240), (400, 400), angleDeg)
rr = dai.RotatedRect()
rr.center.x, rr.center.y = rotatedRect[0]
rr.size.width, rr.size.height = rotatedRect[1]
rr.angle = rotatedRect[2]
cfg.setCropRotatedRect(rr, False)
if resizeFactor > 0:
cfg.setResize(resizeX, resizeY)
# cfg.setWarpBorderFillColor(255, 0, 0)
# cfg.setWarpBorderReplicatePixels()
def get3DPosition(self, point_x,point_y, size):
'''!
Copmutes the 3D postiion of a point in the image.
Parameters:
@param point_x: x coordinate of the point
@param point_y: y coordinate of the point
@param size: size of the image of the coordinates
'''
try:
self._depth_frame
except:
return np.array([np.inf,np.inf,np.inf] ,dtype=np.float64)
point_x *= self._depth_frame.shape[0]/size[0]
point_y *= self._depth_frame.shape[1]/size[1]
x_pixel = np.array([point_x,point_y,1.0],dtype=np.float64)
point_x = int(point_x)
point_y = int(point_y)
x_space = np.zeros(3,dtype=np.float64)
n_pixel = 0
color_calib_size = self.camera_calibration_size[OAK_Stage.COLOR]
color_intrinsic = self.camera_intrinsics[OAK_Stage.COLOR]
scale = [self._depth_frame.shape[0]/color_calib_size[0], self._depth_frame.shape[1]/color_calib_size[1]]
K = color_intrinsic.copy()
K[0] *= scale[0]
K[1] *= scale[1]
k_inv = np.linalg.inv(K)
n_point = self._configs["depth_roi_size"]
x_min = point_x-(n_point//2)-1
x_max = point_x+(n_point//2)
y_min = point_y-(n_point//2)-1
y_max = point_y+(n_point//2)
if x_min < 0:
x_min = 0
if y_min < 0:
y_min = 0
if x_max >= self._depth_frame.shape[0]:
x_max = self._depth_frame.shape[0]
if y_max >= self._depth_frame.shape[1]:
y_max = self._depth_frame.shape[1]
if self._configs["depth_host"] == False:
topLeft = dai.Point2f(x_min, y_min)
bottomRight = dai.Point2f(x_max, y_max)
config = dai.SpatialLocationCalculatorConfigData()
config.depthThresholds.lowerThreshold = 100
config.depthThresholds.upperThreshold = 10000
config.roi = dai.Rect(topLeft, bottomRight).normalize(width=self._depth_frame.shape[1], height=self._depth_frame.shape[0])
cfg = dai.SpatialLocationCalculatorConfig()
cfg.addROI(config)
self._oak_input_queue["spatialConfig"].send(cfg)
spatialdata = self._spatial_queue.get()
x_space = np.zeros(3, dtype = np.float64)
x_space[0] = spatialdata[0].spatialCoordinates.x
x_space[1] = spatialdata[0].spatialCoordinates.y
x_space[2] = spatialdata[0].spatialCoordinates.z
return []
samples = self._depth_frame[x_min:x_max,y_min:y_max].flatten()
samples = samples[samples != 0]
samples = samples[samples>100]
if samples.shape[0] == 0:
return np.array([np.inf,np.inf,np.inf] ,dtype=np.float64)
z = 0
if self._configs["depth_point_mode"] == "median":
z = np.median(samples)
elif self._configs["depth_point_mode"] == "mean":
z = np.mean(samples)
elif self._configs["depth_point_mode"] == "min":
samples = np.sort(samples)
z = np.mean(samples[:int(samples.shape[0]*0.1)])
elif self._configs["depth_point_mode"] == "max":
samples = np.sort(samples)
z = np.median(samples[int(samples.shape[0]*0.9):])
else:
hist, edge = np.histogram(samples, bins=1000, range=(0.0,5000.0), density=True)
a = []
for i in range(edge.shape[0]-1):
a.append((edge[i]+edge[i+1])/2)
edge = np.array(a)
best_z = 0
best_z_inliers = 0
for i in range(16):
index = int(np.random.rand(1)[0]*samples.shape[0])
z_test = samples[index]
if i == 0:
z_test = np.median(samples)
if i == 1:
z_test = np.mean(samples)
z_min = z_test-100.0
z_max = z_test+100.0
mask = np.logical_and(edge>z_min,edge<z_max)
values = edge[mask]
weights = hist[np.where(mask)]
if np.sum(weights) == 0:
continue
#z_test = np.average(values, weights=weights)
n_inlier = values.shape[0]
if n_inlier > best_z_inliers:
best_z_inliers = n_inlier
best_z = z_test
z = best_z
if z == 0:
return np.array([np.inf,np.inf,np.inf] ,dtype=np.float64)
x_space = k_inv @ (z*x_pixel)
return x_space/1000.0
# Left mono camera
left = pipeline.createMonoCamera()
left.setResolution(dai.MonoCameraProperties.SensorResolution.THE_400_P)
left.setBoardSocket(dai.CameraBoardSocket.LEFT)
stereo = pipeline.createStereoDepth()
stereo.setConfidenceThreshold(200)
stereo.setOutputDepth(True)
stereo.setOutputRectified(True)
stereo.setRectifyMirrorFrame(False)
left.out.link(stereo.left)
right.out.link(stereo.right)
# Depth output is 640x400. Rectified frame that gets resized has 1:1 (w:h) ratio
# 400/640=0.625 1-0.625=0.375 0.375/2=0.1875 1-0.1875=0.8125
topLeft = dai.Point2f(0.1875, 0)
bottomRight = dai.Point2f(0.8125, 1)
# This ROI will convert 640x400 depth frame into 400x400 depth frame, which we will resize on the host to match NN out
crop_depth = pipeline.createImageManip()
crop_depth.initialConfig.setCropRect(topLeft.x, topLeft.y, bottomRight.x, bottomRight.y)
stereo.depth.link(crop_depth.inputImage)
# Create depth output
xout_depth = pipeline.createXLinkOut()
xout_depth.setStreamName("depth")
crop_depth.out.link(xout_depth.input)
# Right rectified -> NN input to have the most accurate NN output/stereo mapping
manip = pipeline.createImageManip()
manip.setResize(nn_shape,nn_shape)
manip.setFrameType(dai.RawImgFrame.Type.BGR888p)
# StereoDepth
stereo.setOutputDepth(outputDepth)
stereo.setOutputRectified(outputRectified)
stereo.setConfidenceThreshold(255)
stereo.setLeftRightCheck(lrcheck)
stereo.setSubpixel(subpixel)
monoLeft.out.link(stereo.left)
monoRight.out.link(stereo.right)
spatialLocationCalculator.passthroughDepth.link(xoutDepth.input)
stereo.depth.link(spatialLocationCalculator.inputDepth)
topLeft = dai.Point2f(0.5, 0.5)
bottomRight = dai.Point2f(0.6, 0.6)
spatialLocationCalculator.setWaitForConfigInput(False)
config = dai.SpatialLocationCalculatorConfigData()
config.depthThresholds.lowerThreshold = 100
config.depthThresholds.upperThreshold = 10000
config.roi = dai.Rect(topLeft, bottomRight)
spatialLocationCalculator.initialConfig.addROI(config)
spatialLocationCalculator.out.link(xoutSpatialData.input)
xinSpatialCalcConfig.out.link(spatialLocationCalculator.inputConfig)
# Pipeline is defined, now we can connect to the device
with dai.Device(pipeline) as device:
device.startPipeline()
monoLeft = pipeline.create(dai.node.MonoCamera)
manipRight = pipeline.create(dai.node.ImageManip)
manipLeft = pipeline.create(dai.node.ImageManip)
controlIn = pipeline.create(dai.node.XLinkIn)
configIn = pipeline.create(dai.node.XLinkIn)
manipOutRight = pipeline.create(dai.node.XLinkOut)
manipOutLeft = pipeline.create(dai.node.XLinkOut)
controlIn.setStreamName('control')
configIn.setStreamName('config')
manipOutRight.setStreamName("right")
manipOutLeft.setStreamName("left")
# Crop range
topLeft = dai.Point2f(0.2, 0.2)
bottomRight = dai.Point2f(0.8, 0.8)
# Properties
monoRight.setBoardSocket(dai.CameraBoardSocket.RIGHT)
monoLeft.setBoardSocket(dai.CameraBoardSocket.LEFT)
monoRight.setResolution(dai.MonoCameraProperties.SensorResolution.THE_720_P)
monoLeft.setResolution(dai.MonoCameraProperties.SensorResolution.THE_720_P)
manipRight.initialConfig.setCropRect(topLeft.x, topLeft.y, bottomRight.x, bottomRight.y)
manipLeft.initialConfig.setCropRect(topLeft.x, topLeft.y, bottomRight.x, bottomRight.y)
manipRight.setMaxOutputFrameSize(monoRight.getResolutionHeight()*monoRight.getResolutionWidth()*3)
# Linking
monoRight.out.link(manipRight.inputImage)
monoLeft.out.link(manipLeft.inputImage)
controlIn.out.link(monoRight.inputControl)