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)