问题:
已知相机外参 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)
