python 点云从相机坐标系到世界坐标系/ 世界坐标系到相机坐标系下的变换

问题:

已知相机外参 extr 和 相机坐标系下的点云 pc 求解世界坐标系下的 点云 pc

或

已知相机外参 extr 和 世界坐标系下的点云 pc 求解相机坐标系下的 点云 pc

notes:

1. 一些经验之谈: 一般世界坐标系到相机坐标系的变换会发生在模拟器中(典型如pybullet, unity等), 因为模拟器中的世界坐标系原点坐标是(0, 0, 0). 但真机上使用该变换就需要采用相机标定以确定相机外参

2. 相机外参 extr: 不同模拟器有不同的表示形式 一般采用 extr的shape为(4, 4) 的矩阵来描述 (此时 相机的旋转R = extr[:3, :3], 相机的平移T = extr[:3, 3]), pybullet 中的extr采用 四元数 Q + 平移 T 的方式 描述 shape 为 (1, 7), 此时 相机的旋转 Q = extr[:, :4], 相机的平移 T = extr[:, 4:]

3. 上述表达中缩写的全称 extr --> extrinsic (外参),   pc -- > point cloud (点云)

4. 一些人会混淆 点云 和 世界坐标系 这两个概念 (而正确的认知应是世界坐标系下的点云, 与相机坐标系下的点云), 因为他们可能需要完成一些任务, 典型如测像素中两点对应真实世界中的两点的距离, 会出现一种思想:  世界中的两点距离 一定是需要"世界"坐标系的(二者正好均含世界二字).然而, 如果仅是相机测距的话仅需要在相机坐标系下的点云计算即可, 见python 深度图转点云_tycoer的博客-CSDN博客_深度图像转点云python 中的相机测距部分

import numpy as np
from scipy.spatial.transform import Rotation


def pc_cam_to_pc_world(pc, extrinsic):
    # extrinsic 中旋转的表达形式为旋转矩阵
    # pc shape (n , 3)
    # extrinsic shape (4, 4)
    extr_inv = np.linalg.inv(extrinsic)  # 求逆
    R = extr_inv[:3, :3]
    T = extr_inv[:3, 3]
    pc = (R @ pc.T).T + T # 注意 R 需要左乘, 右乘会得到错误结果
    return pc

def pc_world_to_pc_cam(pc, extrinsic):
    R = extrinsic[:3, :3]
    T = extrinsic[:3, 3]
    pc = (R @ pc.T).T + T
    return pc



if __name__ == '__main__':
    # 造数据
    np.random.seed(0)
    extr = np.eye(4)
    R = np.random.rand(3, 3)
    # # 如果旋转部分采用了四元数
    # from scipy.spatial.transform import Rotation
    # Q = np.random.rand(1, 4)
    # R = Rotation.from_quat(Q).as_matrix()

    T = np.random.rand(3, 1)
    extr[:3, :3] = R
    extr[:3, 3:] = T

    pc_cam = np.random.rand(1000, 3)
    pc_world = pc_cam_to_pc_world(pc_cam, extr)
    # 可以看到 pc_cam 和 pc_cam_val 的 值相同
    pc_cam_val = pc_world_to_pc_cam(pc_world, extr)