Plotting the normal vector of a plane in OpenCV

edit

I'm using a 2D barcode to identify a plane in 3D space and I want to plot its normal with respect to its center.

This is the code I'm using to compute the normal

def compute_normal(camera, board, bounds, frame):
    extrinsics, ip, op = get_extrinsics(camera, frame, board, bounds)
    extrinsic = extrinsics[0]

    if not extrinsic: return [], None

    camera_transform = M_from_extrinsic(extrinsic)
    transformed_board = transform_board(board, camera_transform)

    # change this to be the corners of any board, not just 4-square

    p1 = transformed_board[3][:3]
    p2 = transformed_board[6][:3]
    p3 = transformed_board[8][:3]

    l1 = p2 - p1
    l2 = p3 - p1

    normal = np.cross(l1, l2)

    return normal, extrinsic

board is simply a 3xN array of object points (everything is backed by solvePnP). The points p1, p2, p3 are the top left, top right, and bottom left of an ideal calibration board. The circled points are the ones I am choosing for my computation. The other points are more corners which are not being considered for this particular question.

After I compute the normal, this is how I plot it.

normal, extrinsic = compute_normal(camera0, board0, [[0, 4]], im)

if len(normal):

    projected_center, jac0 = cv2.projectPoints(board_center, extrinsic.rvec, extrinsic.tvec, camera0.mtx, camera0.dist)
    projected_norm, jac1 = cv2.projectPoints(np.array([normal]), extrinsic.rvec, extrinsic.tvec, camera0.mtx, camera0.dist)

    origin = clean_image_points(projected_center.reshape(1, 2))[0]
    vec = clean_image_points(projected_norm.reshape(1, 2))[0]

    cv2.arrowedLine(im, origin, vec, (0, 255, 0), 5)

However, I'm getting a result that looks like this

As you can see, the green line is pointing in some direction where it should be pointing towards the screen, since the board is facing the camera more or less head on.

I can confirm that calculating the extrinsics is working fine since I am using it in several other places. I'm just not sure about the normal computation and the plotting.

Thanks for any assistance!