OAK-D Absolute Depth Precision

Dear erik,

Thanks a lot for your reply, and sorry for my delay, the project has been on hold for a few weeks.

I did give it a try, as you suggested, at recording the bag file and replaying it with the realsense viewer. The results are satisfactory, as I can individuate the correct coordinates of the extremities of the pipe, as shown:

My problem is now: given that I can still correctly identify the circles thanks to a hough detection algorithm, how do I extrapolate such coordinates from the depth array? My naive implementation provides me with wrong results regarding depth.

That's what I tried:

def getFrame(queue):
  # Get frame from queue
  frame = queue.get()
  # Convert frame to OpenCV format and return
  return frame.getCvFrame()

def getMonoCamera(pipeline, isLeft):
  # Configure mono camera
  mono = pipeline.createMonoCamera()
  # Set Camera Resolution
  mono.setResolution(dai.MonoCameraProperties.SensorResolution.THE_400_P)
  if isLeft:
      # Get left camera
      mono.setBoardSocket(dai.CameraBoardSocket.LEFT)
  else :
      # Get right camera
      mono.setBoardSocket(dai.CameraBoardSocket.RIGHT)
  return mono

def getStereoPair(pipeline, monoLeft, monoRight):
    # Configure stereo pair for depth estimation
    stereo = pipeline.createStereoDepth()
    # Checks occluded pixels and marks them as invalid
    stereo.setLeftRightCheck(True)
    # Configure left and right cameras to work as a stereo pair
    monoLeft.out.link(stereo.left)
    monoRight.out.link(stereo.right)
    return stereo

if __name__ == '__main__':
    # Start defining a pipeline
    pipeline = dai.Pipeline()
    # Set up left and right cameras
    monoLeft = getMonoCamera(pipeline, isLeft = True)
    monoRight = getMonoCamera(pipeline, isLeft = False)
    # Combine left and right cameras to form a stereo pair
    stereo = getStereoPair(pipeline, monoLeft, monoRight)
    stereo.setLeftRightCheck(True)
    camRgb = pipeline.create(dai.node.ColorCamera)
    camRgb.setIspScale(1,3)
    camRgb.setResolution(dai.ColorCameraProperties.SensorResolution.THE_1080_P)
    xoutRgb = pipeline.create(dai.node.XLinkOut)
    xoutRgb.setStreamName("video")
    camRgb.video.link(xoutRgb.input)
    # Set XlinkOut for disparity, rectifiedLeft, and rectifiedRight
    xoutDisp = pipeline.createXLinkOut()
    xoutDisp.setStreamName("disparity")
    
    xoutRectifiedLeft = pipeline.createXLinkOut()
    xoutRectifiedLeft.setStreamName("rectifiedLeft")

    xoutRectifiedRight = pipeline.createXLinkOut()
    xoutRectifiedRight.setStreamName("rectifiedRight")

    stereo.disparity.link(xoutDisp.input)
    
    stereo.rectifiedLeft.link(xoutRectifiedLeft.input)
    stereo.rectifiedRight.link(xoutRectifiedRight.input)
    
    # Pipeline is defined, now we can connect to the device

    with dai.Device(pipeline) as device:
       
        # Output queues will be used to get the rgb frames and nn data from the outputs defined above
        disparityQueue = device.getOutputQueue(name="disparity", maxSize=1, blocking=False)
        rectifiedLeftQueue = device.getOutputQueue(name="rectifiedLeft", maxSize=1, blocking=False)
        rectifiedRightQueue = device.getOutputQueue(name="rectifiedRight", maxSize=1, blocking=False)
        # Calculate a multiplier for colormapping disparity map
        disparityMultiplier = 255 / stereo.getMaxDisparity()
        cv2.namedWindow("Stereo Pair")
        
        # Variable use to toggle between side by side view and one frame view.
        sideBySide = False
        def rescale_frame(frame, percent=75):
            width = int(frame.shape[1] * percent/ 100)
            height = int(frame.shape[0] * percent/ 100)
            dim = (640, 400)
            return cv2.resize(frame, dim, interpolation =cv2.INTER_AREA)

        while True:

            # Get disparity map
            disparity = getFrame(disparityQueue)
            depth = 441.25 * 7.5 / disparity 
            
            # Colormap disparity for display
            disparity_multiplied = (disparity * disparityMultiplier).astype(np.uint8)
            disparity_multiplied = cv2.applyColorMap(disparity_multiplied, cv2.COLORMAP_JET)
            
            # Get left and right rectified frame
            leftFrame = getFrame(rectifiedLeftQueue)
            rightFrame = getFrame(rectifiedRightQueue)

            blurredLeft = cv2.GaussianBlur(leftFrame, (5, 5), 0)
            blurredRight = cv2.GaussianBlur(rightFrame, (5, 5), 0)

            imLeft = cv2.adaptiveThreshold(blurredLeft, 255, cv2.ADAPTIVE_THRESH_GAUSSIAN_C, cv2.THRESH_BINARY_INV, 11, 2)
            imRight = cv2.adaptiveThreshold(blurredRight, 255, cv2.ADAPTIVE_THRESH_GAUSSIAN_C, cv2.THRESH_BINARY_INV, 11, 2)
            try:
                circles = cv2.HoughCircles(blurredRight,
                            cv2.HOUGH_GRADIENT_ALT,
                            minDist=15,
                            dp=1.6,
                            param1=300,
                            param2=0.9,
                            minRadius=0,
                            maxRadius=-1)
                circles = np.uint16(np.around(circles))
                for i in circles[0,:]:
                    # draw the outer circle
                    cv2.circle(rightFrame,(i[0],i[1]),i[2],(255,0,255),1)
                    cv2.circle(disparity_multiplied,(i[0],i[1]),i[2],(255,0,255),1)
                    # draw the center of the circle
                cv2.circle(rightFrame,(i[0],i[1]),1,(255,0,255),1)
                cv2.circle(disparity_multiplied,(i[0],i[1]),1,(255,0,255),1)
                xmin_depth = i[0]-i[2]
                xmax_depth = i[0]+ i[2]
                ymin_depth = i[1]-i[2]
                ymax_depth = i[1]+i[2]
                # print(i[0],i[1],i[2]) 
                # print(xmin_depth,xmax_depth,ymin_depth,ymax_depth)
                
                # print(depth[i[1],i[0]])
                # depth_cut = depth[ymin_depth:ymax_depth,xmin_depth:xmax_depth].astype(float)
                # print(depth_cut)
                dist = depth[i[1],i[0]]
                
                print("Detected {0:.2} centimeters away.".format(dist))    
            except:
                pass

Also, even if this would work for the depth estimate, which does not, I still am not sure on how to get the same for the x and y coordinate.

Any help or clarification would be greatly appreaciated.

Thanks a lot erik ,

I was able to get some results adapting the demo you proposed me.

I still have a couple of doubts:

1. I calculated the hough circles pixel coordinates based on the right image output of the camera, does this mean that the coordinates are expressed in the reference frame of the right camera sensor?

2. The value of the Z coordinate is expressed as the length of the orthogonal segment starting at X,Y point and reaching the object, or is it the length of the segment starting at 0,0 and reaching the object? I guess the first option, but I am asking just to be sure.