如何在Matplotlib中基于4x4位姿矩阵设置相机观测视角

需要在Matplotlib的3D绘图中，将相机精准放置在由4x4变换矩阵定义的位姿上，确保相机沿**+Z轴方向**观测场景。目前参考生成的set_camera_from_pose_1与set_camera_from_pose_2函数无法满足需求，以下是完整可运行测试代码及修正方案。

问题根源 #

原函数默认相机沿**-Z轴**观测，与需求的+Z轴方向不符；同时view_init的参数计算逻辑未正确匹配相机坐标系到世界坐标系的转换关系，导致视角偏差。

修正后的解决方案 #

完整可运行代码 #

import requests
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d.art3d import Poly3DCollection


def load_obj(filename):
    '''Load vertices and faces from OBJ file'''
    vertices, faces = [], []
    data = requests.get(filename).text.split('\r\n')
    for line in data:
        if line.startswith('v '):
            vertices.append([float(x) for x in line.strip().split()[1:4]])
        elif line.startswith('f '):
            face = []
            for vertex in line.strip().split()[1:]:
                face.append(int(vertex.split('/')[0]) - 1)
            faces.append(face)
    return np.array(vertices), faces


def rotation_matrix(θx, θy, θz):
    Rx = np.array([
        [1, 0, 0],
        [0, np.cos(θx), -np.sin(θx)],
        [0, np.sin(θx), np.cos(θx)]
    ])
    Ry = np.array([
        [np.cos(θy), 0, np.sin(θy)],
        [0, 1, 0],
        [-np.sin(θy), 0, np.cos(θy)]
    ])
    Rz = np.array([
        [np.cos(θz), -np.sin(θz), 0],
        [np.sin(θz), np.cos(θz), 0],
        [0, 0, 1]
    ])

    R = Rz @ Ry @ Rx  # Combined rotation (extrinsic XYZ order)
    return R


def set_camera_from_pose_correct(ax, pose_matrix):
    # 提取旋转矩阵R和平移向量t
    R = pose_matrix[:3, :3]
    t = pose_matrix[:3, 3]
    
    # 相机位置（eye）是位姿的平移分量
    eye = t
    # 相机沿自身+Z轴观测，转换到世界坐标系就是R[:,2]
    forward_dir = R[:, 2]
    
    # 目标点：相机位置沿+Z轴方向移动一段距离
    target = eye + forward_dir
    
    # 计算视线方向向量
    view_dir = target - eye
    view_dir_norm = view_dir / np.linalg.norm(view_dir)
    
    # 计算azimuth：视线在XY平面投影与X轴的夹角（从X轴逆时针为正）
    azim = np.degrees(np.arctan2(view_dir_norm[1], view_dir_norm[0]))
    
    # 计算elevation：视线与XY平面的夹角（向上为正）
    elev = np.degrees(np.arcsin(view_dir_norm[2]))
    
    # 设置视角
    ax.view_init(elev=elev, azim=azim)
    
    # 调整缩放：确保场景在视野内，根据相机到目标的距离调整
    distance = np.linalg.norm(target - eye)
    ax.set_box_aspect(None, zoom=2 / distance)


def plot_pose(T, ax=None, label=None, axis_length=1.0, axis_width=2):
    ''' Visualize a pose from a 4x4 transformation matrix '''
    if ax is None:
        fig = plt.figure(figsize=(10, 8))
        ax = fig.add_subplot(111, projection='3d')

    # Extract position (translation) from the matrix
    origin = T[:3, 3]

    # Extract rotation (orientation) from the matrix
    x_axis, y_axis, z_axis = T[:3, 0], T[:3, 1], T[:3, 2]

    # Plot the three axes
    ax.quiver(origin[0], origin[1], origin[2], x_axis[0], x_axis[1], x_axis[2], color='red', length=axis_length, linewidth=axis_width, arrow_length_ratio=0.2)
    ax.quiver(origin[0], origin[1], origin[2], y_axis[0], y_axis[1], y_axis[2], color='green', length=axis_length, linewidth=axis_width, arrow_length_ratio=0.2)
    ax.quiver(origin[0], origin[1], origin[2], z_axis[0], z_axis[1], z_axis[2], color='blue', length=axis_length, linewidth=axis_width, arrow_length_ratio=0.2)

    # Plot origin point
    ax.scatter(origin[0], origin[1], origin[2], color='black', s=40, label='Origin')

    if label is not None:
        ax.text(origin[0], origin[1], origin[2], label, fontsize=10, va='top', ha='left')

    return ax


fig = plt.figure(figsize=(6, 6), facecolor='none')
ax = fig.add_subplot(111, projection='3d', facecolor='none')

# Load the 3D model
scale = 1.0
R = rotation_matrix(np.pi / 2, 0, np.pi / 2)
vertices, faces = load_obj('https://graphics.stanford.edu/~mdfisher/Data/Meshes/bunny.obj')
vertices -= vertices.min(axis=0)
vertices = vertices * scale
vertices = np.array([vertices[face] for face in faces])
mesh = Poly3DCollection(vertices, alpha=0.9, facecolor='cyan', edgecolor='darkgray', linewidths=0.5)
vertices = vertices @ R.T
mesh.set_verts(vertices.tolist())
ax.add_collection3d(mesh)

# Plot some pose(s)
angle = 0

pose1 = np.array([
    [np.cos(angle), -np.sin(angle), 0, 0.4],
    [np.sin(angle), np.cos(angle), 0, 0.3],
    [0, 0, 1, -0.3],
    [0, 0, 0, 1.0]
])

pose2 = np.array([
    [np.cos(angle), -np.sin(angle), 0, -0.5],
    [np.sin(angle), np.cos(angle), 0, -0.4],
    [0, 0, 1, 1.0],
    [0, 0, 0, 1.0]
])

camera_pose= np.array([
    [-0.1744385499559559516,  -0.7794457272190911112,  0.6016939010902186968, -0.2006076245670841973],
    [-0.9548445695049905257, -0.01535221630358679992, -0.2967088767485692724, -0.1520286928361865575],
    [ 0.2405058011277342034,  -0.6262816201793489634, -0.7415715015084093364,  0.7943007246282384193],
    [                     0,                       0,                      0,                      1]
])

plot_pose(pose1, ax, label='p1', axis_length=0.05)
plot_pose(pose2, ax, label='p2', axis_length=0.05)
plot_pose(camera_pose, ax, label='camera', axis_length=0.1, axis_width=1)

# Plot world frame for reference
plot_pose(np.eye(4), ax, axis_length=0.1, axis_width=1)

# 使用修正后的相机位姿设置函数
set_camera_from_pose_correct(ax, camera_pose)

ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')

plt.tight_layout()
plt.show()

核心逻辑说明 #

1. 位姿解析：从4x4矩阵中提取旋转矩阵R和平移向量t，相机位置直接取t。
2. 视线方向：相机沿自身+Z轴观测，对应世界坐标系下的方向为R[:,2]。
3. 视角计算：
   • azim：视线在XY平面投影与X轴的夹角，通过arctan2计算后转换为角度。
   • elev：视线与XY平面的夹角，通过arcsin计算后转换为角度，确保匹配+Z轴观测方向。
4. 缩放调整：根据相机到目标点的距离动态调整缩放因子，保证场景完整显示。

内容的提问来源于stack exchange，提问作者dibyendu