用matplotlib 根据变换矩阵和照相机位置计算方位角和仰角

假设你已经获得了变换矩阵 $T$ 和照相机位置 $p$，可以使用以下代码计算方位角和仰角，并绘制一个3D坐标轴以及照相机位置和朝向：

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

# 定义照相机位置和朝向
p = np.array([0, 0, 0])
v = np.array([0, 0, 1])

# 计算方位角和仰角
d = np.dot(T[:3, :3], v)
theta = np.arctan2(d[0], d[2])
phi = np.arccos(d[1] / np.linalg.norm(d))

# 创建3D坐标轴
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim(-1, 1)
ax.set_ylim(-1, 1)
ax.set_zlim(-1, 1)
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')

# 绘制照相机位置
ax.scatter(p[0], p[1], p[2], c='r', marker='o')

# 绘制照相机朝向
u, v = np.mgrid[0:2*np.pi:20j, 0:np.pi:10j]
x = np.sin(v) * np.cos(u)
y = np.sin(v) * np.sin(u)
z = np.cos(v)
R = np.array([[np.cos(theta), 0, np.sin(theta)],
              [0, 1, 0],
              [-np.sin(theta), 0, np.cos(theta)]])
r = np.dot(R, np.array([x.flatten(), y.flatten(), z.flatten()]))
ax.plot_surface(r[0].reshape(x.shape), r[1].reshape(x.shape), r[2].reshape(x.shape), alpha=0.3)

# 显示图像
plt.show()

这里使用了 `np.dot` 函数计算照相机朝向在变换矩阵下的方向向量，然后使用 `np.arctan2` 和 `np.arccos` 函数计算方位角和仰角。然后使用 `mpl_toolkits.mplot3d` 模块绘制了一个3D坐标轴，以及照相机位置和朝向。