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:
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)

