使用python 基于汽车环视相机RT矩阵绘制相机坐标系x,y,z轴在世界坐标系下朝向及相机位置-优快云博客

这里的RT为世界坐标系旋转平移到摄像机坐标系下的矩阵关系。世界坐标系以车体后轴中心为原点，向右为X,向前为Y,向上为Z正方向，车辆位于Z=0平面上。

首先安装matplotlib

pip install matplotlib


import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D

def draw_camera(ax, rt_matrix,name,color, length=10.0):
    # Extract camera position and orientation from RT matrix
    camera_position = -np.linalg.inv(rt_matrix[:, :3]).dot(rt_matrix[:, 3])
    camera_orientation = rt_matrix[:, :3]

    # Plot camera position
    ax.scatter(camera_position[0], camera_position[1], camera_position[2], c=color, marker='o', label=name)

    # Plot camera axes
    ax.quiver(camera_position[0], camera_position[1], camera_position[2],
              camera_orientation[0, 0], camera_orientation[1, 0], camera_orientation[2, 0],
              length=length, color='g', label='X-axis')

    ax.quiver(camera_position[0], camera_position[1], camera_position[2],
              camera_orientation[0, 1], camera_or