extrinsic

在Open3D中，open3d.camera.PinholeCameraParameters类可以用来表示针孔相机的参数。其中，extrinsic表示相机的外参，也就是相机位姿的参数。

外参包括相机的位移和旋转。在Open3D中，外参由4x4齐次变换矩阵表示，该矩阵可以将相机坐标系中的点变换到世界坐标系中。

具体地，外参矩阵的前三列表示相机坐标系的x、y、z轴在世界坐标系中的方向，即相机的旋转矩阵。最后一列表示相机在世界坐标系中的位置，即相机的位移向量。

因此，对于给定的一个相机位姿，可以通过如下代码将其表示为一个外参矩阵：

import numpy as np
from open3d.camera import PinholeCameraParameters

# 相机位姿的欧拉角表示
R = np.eye(3)  # 旋转矩阵
t = np.array([0, 0, 1])  # 位移向量
camera_pose = np.eye(4)
camera_pose[:3,:3] = R
camera_pose[:3,3] = t

# 构造PinholeCameraParameters对象
camera_params = PinholeCameraParameters()
camera_params.extrinsic = camera_pose

# 或者直接构造PinholeCameraParameters对象
camera_params = PinholeCameraParameters(
    intrinsic_matrix=np.eye(3),
    extrinsic=camera_pose,
    width=640,
    height=480
)

需要注意的是，在Open3D中，通常将相机的y轴朝上，x轴朝右，z轴朝前。因此，对于一个给定的位姿，需要根据相机坐标系和世界坐标系的方向关系来设置外参矩阵。比如，在世界坐标系中，相机位于$[0 1 2]^T$，其朝向为$[-1 0 0]^T$（相机坐标系中的z轴方向），可以通过如下代码来构造相应的外参矩阵：

import numpy as np
from open3d.camera import PinholeCameraParameters

# 构造旋转矩阵
z_axis = np.array([0., 0., 1.])
camera_z = np.array([-1., 0., 0.])
camera_y = np.cross(z_axis, camera_z)
camera_y /= np.linalg.norm(camera_y)
camera_x = np.cross(camera_y, camera_z)
camera_x /= np.linalg.norm(camera_x)
R = np.stack([camera_x, camera_y, camera_z])

# 构造外参矩阵
t = np.array([0, 1, 2])
camera_pose = np.eye(4)
camera_pose[:3,:3] = R
camera_pose[:3,3] = t

# 构造PinholeCameraParameters对象
camera_params = PinholeCameraParameters()
camera_params.extrinsic = camera_pose

在Open3D中，可以通过外参矩阵来实现相机位姿的变换。具体地，给定一个外参矩阵，可以将其应用到点的坐标上，从而将点从相机坐标系变换到世界坐标系（或者反过来），如下：

import numpy as np
from open3d.camera import PinholeCameraParameters

# 构造相机位姿
R = np.eye(3)
t = np.array([0, 0, 1])
camera_pose = np.eye(4)
camera_pose[:3,:3] = R
camera_pose[:3,3] = t

# 构造相机参数对象
camera_params = PinholeCameraParameters()
camera_params.extrinsic = camera_pose

# 定义一个在相机坐标系中的点
p_cam = np.array([0, 0, 1])

# 将点从相机坐标系变换到世界坐标系
p_world = np.matmul(camera_pose, np.append(p_cam, [1]))[:3]
# 或者
p_world_2 = camera_params.extrinsic @ np.append(p_cam, [1])[:4]

需要注意的是，在变换过程中，点的坐标应该是一个齐次坐标向量，即$[x, y, z, 1]^T$，变换结果也是一个齐次坐标向量。因此，在使用多个变换矩阵组合变换时，需要使用np.matmul或@运算符进行矩阵乘法，而不能使用np.dot函数。同时，需要对点的齐次坐标向量进行截取或缩放，从而得到正确的三维坐标。