相机位姿python 相机位姿 4x3

相机位姿变换

• 项目场景
• 相机位姿
• 旋转变换

• 求平面方程
• 求法向量
• 求旋转矩阵

• 平移变换
• 尺度变换
• 变换顺序

---

项目场景

将自定义数据的影像数据规范化到 2×2 的一个立方体内进行训练。由于影像的相机位置近似在一个平面上，且场景主要在相机位置下方。所以也就是说要把这些相机位姿变换到立方体的上方的表面上。这样训练对象就能落到训练场景内，并在场景内近似居中，有利于训练。其实就是一个求七参数（三个平移量、三个旋转角、一个尺度因子）进行空间直角坐标系转换的过程。由于metashape或者colmap等通过匹配导出的相机位姿通常是以第一张相片的相机坐标系为参考的，那么在进一步处理时就需要对其进行位姿变换（另外也要注意模型中使用的相机坐标系与原始相机坐标系中z轴方向是否一致）。

相机位姿

相机位姿（位置和姿态）由相机的外参（extrinsic matrix）决定，投影属性由相机的内参（intrinsic matrix）决定。
相机外参可以用一个 4×4 矩阵来表示，作用是将世界坐标系的点变换到相机坐标系下。所以也常把相机外参矩阵叫做world-to-camera (w2c) 矩阵。相机外参的逆矩阵被称为camera-to-world (c2w) 矩阵，其作用是把相机坐标系的点变换到世界坐标系，也就是常说的位姿矩阵 (poses) 。

c2w矩阵是一个 4x4 的矩阵，c2w[:3,:3]是旋转矩阵R（同时也代表相机姿态，其三个列向量分别代表相机坐标系xyz轴的方向），c2w[:3,3]是平移向量T，c2w[3,3]是尺度因子S。

图及描述参考这篇文章，其对相机参数以及NeRF代码解读得很好，推荐阅读。

---

旋转变换

由于我需要处理的影像的位置基本上是在一个平面上，所以接下来以平面法向量为例计算旋转矩阵来进行旋转变换。

求平面方程

def fit_a_plane(x2, y2, z2):
	"""拟合平面"""
    # 创建系数矩阵A
    A = np.zeros((3, 3))
    for i in range(len(x2)):
        A[0, 0] = A[0, 0] + x2[i] ** 2
        A[0, 1] = A[0, 1] + x2[i] * y2[i]
        A[0, 2] = A[0, 2] + x2[i]
        A[1, 0] = A[0, 1]
        A[1, 1] = A[1, 1] + y2[i] ** 2
        A[1, 2] = A[1, 2] + y2[i]
        A[2, 0] = A[0, 2]
        A[2, 1] = A[1, 2]
        A[2, 2] = len(x2)

    # 创建b矩阵
    b = np.zeros((3, 1))
    for i in range(len(x2)):
        b[0, 0] = b[0, 0] + x2[i] * z2[i]
        b[1, 0] = b[1, 0] + y2[i] * z2[i]
        b[2, 0] = b[2, 0] + z2[i]


    # 求解X矩阵
    A_inv = np.linalg.inv(A)
    X = np.dot(A_inv, b)
    # print('平面拟合结果为：z = %.3f * x + %.3f * y + %.3f' % (X[0, 0], X[1, 0], X[2, 0]))

    # 计算方差
    R = 0
    for i in range(len(x2)):
        R = R + (X[0, 0] * x2[i] + X[1, 0] * y2[i] + X[2, 0] - z2[i]) ** 2
    # print('方差为：%.*f' % (3, R))

    return [X[0, 0], X[1, 0], X[2, 0]]

求法向量

def get_normal_vector(point1, point2, point3):
    '''三个点计算平面法向量'''
    vect1 = np.array(point2) - np.array(point1)
    vect2 = np.array(point3) - np.array(point1)
    norm_vect = np.cross(vect1, vect2)
    return norm_vect

求旋转矩阵

def get_R_matrix( vector_src, vector_tgt):
    """计算两平面(法向量)间的旋转矩阵"""    
    vector_src  =  vector_src / np.linalg.norm( vector_src)
    vector_tgt =  vector_tgt / np.linalg.norm( vector_tgt)
    
    c = np.dot( vector_src,  vector_tgt)
    n_vector = np.cross( vector_src,  vector_tgt)
    
    n_vector_invert = np.array((
        [0, -n_vector[2], n_vector[1]],
        [n_vector[2], 0, -n_vector[0]],
        [-n_vector[1], n_vector[0], 0]))
    
    I = np.eye(3)
    R_w2c = I + n_vector_invert + np.dot(n_vector_invert, n_vector_invert) / (1 + c)
    return R_w2c

平移变换

这个就很简单了，目标中心坐标与相机位置坐标均值作差就得到了平移矩阵

T_move = center_tgt - p_mean

尺度变换

这个也简单，目标尺度与当前所有相机位置边界框尺寸之比就是了

scene_scale = scale_tgt / (p_max - p_min)

变换顺序

1. 先旋转
2. 再缩放
3. 再平移

需要注意的是，一定要把姿态考虑进去，在旋转时对姿态进行同步处理。

以下代码以z=1为目标平面，center_tgt=[0,0,1]为目标中心点坐标。poses为所有相机的c2w位姿矩阵。

# File      :transform_poses.py
# Auther    :WooChi
# Time      :2023/03/15
# Version   :1.0
# Function  :坐标变换到固定场景

def poses_transform(poses,center_tgt=np.array([0, 0, 1])):
	
    # 1. 旋转变换
    # 1.1 拟合平面方程
    f_p = fit_a_plane(poses[:, 0, 3], poses[:, 1, 3], poses[:, 2, 3])

    # 在平面上拿出三个点
    points = np.array([[-1., -1., 0.],
                       [-1., 1., 0.],
                       [1., 1., 0.]])

    points[0, 2] = points[0, 0] * f_p[0] + points[0, 1] * f_p[1] + f_p[2]
    points[1, 2] = points[1, 0] * f_p[0] + points[1, 1] * f_p[1] + f_p[2]
    points[2, 2] = points[2, 0] * f_p[0] + points[2, 1] * f_p[1] + f_p[2]

    # 1.2 计算法向量
    normal_p = get_normal_vector(points[0, :], points[1, :], points[2, :])

    # 目标平面的法向量
    normal_cube = np.array([0., 0., -1.])
    
	# 1.3 求旋转矩阵
    R_w2c = get_R_matrix(normal_p, normal_cube)

    # 1.4 对位置进行旋转变换
    # 先从位姿矩阵中取出位置坐标
    p_src = np.zeros(shape=(len(poses[:, 0, 3]), 3))
    p_src[:, 0] = poses[:, 0, 3]
    p_src[:, 1] = poses[:, 1, 3]
    p_src[:, 2] = poses[:, 2, 3]
	# 再对位置坐标进行旋转变换
    p_new = np.dot(R_w2c, np.transpose(p_src))
    
    # 把变换后的相机位置坐标放到位姿矩阵中
    poses_new = poses.copy()
    poses_new[:, 0, 3] = p_new[0, :]
    poses_new[:, 1, 3] = p_new[1, :]
    poses_new[:, 2, 3] = p_new[2, :]
	
	# 1.5 对姿态进行旋转变换，其实就是对三个列向量（坐标轴）进行旋转变换
    poses_new[:, :3, 0] = np.dot(R_w2c, np.transpose(poses_new[:, :3, 0])).transpose()
    poses_new[:, :3, 1] = np.dot(R_w2c, np.transpose(poses_new[:, :3, 1])).transpose()
    poses_new[:, :3, 2] = np.dot(R_w2c, np.transpose(poses_new[:, :3, 2])).transpose()
	

	# 2. 缩放变换 
    # 2.1 求相机位置坐标所占空间大小
    max_vertices = np.max(p_new, axis=1)
    min_vertices = np.min(p_new, axis=1)
    
    # 2.2 求缩放因子，目标尺寸为2
    scene_scale = 2 / (np.max(max_vertices - min_vertices))

    
    # 2.2. 对位置进行缩放变换
    poses_new[:, :3, 3] *= scene_scale
    p_new[0, :] = poses_new[:, 0, 3]
    p_new[1, :] = poses_new[:, 1, 3]
    p_new[2, :] = poses_new[:, 2, 3]
  

    # 3. 对位置进行平移变换
    T_move = np.array([center_tgt - np.mean(p_new, axis=1)])
    p_new = p_new + T_move.transpose()
	
    poses_new[:, 0, 3] = p_new[0, :]
    poses_new[:, 1, 3] = p_new[1, :]
    poses_new[:, 2, 3] = p_new[2, :]

    return poses_new

---

平面拟合

位姿变换

---

nice.