OpenCV projectPoints results in Z axis being too long

Viewed 37

I'm trying to plot a rotated xyz axis using OpenCV projectPoints function. When testing rotation about the X and Y axis, I noticed that the Z axis is much longer than the X and Y axis when they should be the same length, but I am unsure why. Any help would be greatly appreciated!

Here are some images I generated:

Rotation by X Axis Rotation by Y Axis

Here is my code:

import numpy as np
import cv2
import sys

def rotByXAxis(angle):
    return np.array([
        [1,             0,              0],
        [0, np.cos(angle), -np.sin(angle)],
        [0, np.sin(angle),  np.cos(angle)],
    ], dtype=np.float32)

def rotByYAxis(angle):
    return np.array([
        [ np.cos(angle), 0, np.sin(angle)],
        [             0, 1,             0],
        [-np.sin(angle), 0, np.cos(angle)],
    ], dtype=np.float32)

def rotByZAxis(angle):
    return np.array([
        [ np.cos(angle), np.sin(angle), 0],
        [-np.sin(angle), np.cos(angle), 0],
        [             0,             0, 1],
    ], dtype=np.float32)

def createCanvas(self, height, width):
    blank_image = np.zeros((height, width, 3), np.uint8)
    blank_image[:, :] = (255, 255, 255)
    return blank_image

def draw3DAxis(self, image, rvec, tvec, cameraMatrix, scale=1, dist=None):
    """
    Draw a 6d of axis (XYZ -> RGB) in the given rotation and translation
    :param image - rgb numpy array
    :rvec - euler rotations, numpy array of length 3,
                    use cv2.Rodrigues(R)[0] to convert from rotation matrix
    :tvec - 3d translation vector, in meters (dtype must be float)
    :cameraMatrix - intrinsic calibration matrix , 3x3
    :scale - factor to control the axis lengths
    :dist - optional distortion coefficients, numpy array of length 4. If None distortion is ignored.
    """
    image = image.astype(np.float32)
    dist = np.zeros(4, dtype=float) if dist is None else dist
    if rvec.shape == (3, 3):
        rvec, _ = cv2.Rodrigues(rvec)
    points = scale * np.float32([
        [1, 0, 0],
        [0, 1, 0],
        [0, 0, 1],
        [0, 0, 0]
    ]).reshape(-1, 3)
    axis_points, _ = cv2.projectPoints(points, rvec, tvec, cameraMatrix, dist)
    print(axis_points)
    image = cv2.arrowedLine(
        image,
        tuple(int(e) for e in axis_points[3].ravel()),
        tuple(int(e) for e in axis_points[0].ravel()),
        (255, 0, 0),
        3,
        tipLength=0.01 * scale,
    )
    image = cv2.arrowedLine(
        image,
        tuple(int(e) for e in axis_points[3].ravel()),
        tuple(int(e) for e in axis_points[1].ravel()),
        (0, 255, 0),
        3,
        tipLength=0.01 * scale,
    )
    image = cv2.arrowedLine(
        image,
        tuple(int(e) for e in axis_points[3].ravel()),
        tuple(int(e) for e in axis_points[2].ravel()),
        (0, 0, 255),
        3,
        tipLength=0.01 * scale,
    )
    return image

if __name__ == "__main__":
    height = 300
    width = 400
    image = createCanvas(height, width)
    rvec = rotByZAxis(-pi/2)
    cameraMatrix = np.array([
        [1.0,   0,  width/2],
        [  0, 1.0, height/2],
        [  0,   0,    1.0  ]], dtype=np.float32)
    image = draw3DAxis(
        image,
        rvec=rvec,
        tvec=np.zeros(3, dtype=float),
        cameraMatrix=cameraMatrix,
        scale=20,
    )
    cv2.imshow("output", image)
    key = cv2.waitKey(0)
    if key:
        sys.exit(1)
0 Answers
Related