相机位姿估计：从2D图像到3D世界
寻找相机的"定位"
我们已经知道如何通过本质矩阵找到两个相机之间的几何关系，但如何从中恢复出相机的具体位置和朝向呢？这就是相机位姿估计要解决的问题。
3D视觉趣闻
- 最早的3D点云技术可以追溯到1970年代，当时美国麻省理工学院的研究人员使用激光雷达创建了简单的3D环境地图。
- 电影《阿凡达》中潘多拉星球的许多场景都使用了3D点云技术来构建逼真的虚拟环境。
- 相机位姿估计技术在AR(增强现实)中至关重要——它能让虚拟物体准确地"附着"在真实世界中。
- 有趣的是，人类大脑也在不断进行着"位姿估计"——我们能感知自己在空间中的位置和朝向，即使闭上眼睛也能大致判断周围环境。
让我们一起探索如何从本质矩阵出发，破解相机位姿的奥秘！
核心知识点讲解
1 什么是相机位姿？
相机位姿（Pose）是指相机在世界坐标系中的位置和朝向。
在3D视觉中，我们通常用旋转矩阵(R)和平移向量(t)来表示相机位姿：
- 旋转矩阵(R)：描述相机的朝向，是一个3×3的正交矩阵
- 平移向量(t)：描述相机的位置，是一个3×1的向量
🧭 方向与位置：如果把相机比作一个人，那么旋转矩阵就像人的头部朝向（看哪个方向），平移向量就像人的站立位置（在哪里）。
2 如何从本质矩阵恢复位姿？
我们已经知道本质矩阵的定义：\( E = [t]_{\times} R \)。现在，我们需要从E中"分解"出旋转矩阵R和平移向量t。
第一步：奇异值分解（SVD）
对本质矩阵E进行奇异值分解（SVD）：
对于本质矩阵，奇异值矩阵S具有特殊形式：\( S = \begin{bmatrix} \sigma & 0 & 0 \\ 0 & \sigma & 0 \\ 0 & 0 & 0 \end{bmatrix} \)，其中σ是一个非零常数。
第二步：构造旋转矩阵
我们可以构造两个可能的旋转矩阵：
这里 \( W = \begin{bmatrix} 0 & -1 & 0 \\ 1 & 0 & 0 \\ 0 & 0 & 1 \end{bmatrix} \)。
第三步：构造平移向量
平移向量t可以从U矩阵的最后一列得到：
第四步：解决歧义性
从上面的步骤，我们会得到四种可能的位姿解。为了确定哪个解是正确的，我们需要进行可视性检查（cheirality check）：
- 将3D点投影到两个相机视图中
- 检查投影点是否位于图像平面的前方（Z坐标为正）
- 满足这个条件的解就是正确的位姿
🧩 解的歧义性：为什么会有四种可能的解？这是因为本质矩阵只描述了相机之间的相对运动，而没有确定绝对的尺度和方向。可视性检查就像是一把"钥匙"，帮助我们从四个可能的解中找到正确的那个。
动手实践：用Python实现相机位姿估计
下面我们用OpenCV来实现相机位姿估计。这段代码将：
- 生成模拟的3D点和相机位姿
- 投影3D点到两个相机视图中，生成2D对应点
- 计算本质矩阵
- 从本质矩阵中恢复相机位姿
- 验证恢复的位姿是否正确
import numpy as np
import cv2
import matplotlib.pyplot as plt
# 设置中文字体
plt.rcParams["font.family"] = ["SimHei", "WenQuanYi Micro Hei", "Heiti TC"]
class CameraPoseEstimation:
"""
相机位姿估计类：从本质矩阵恢复相机位姿
"""
def __init__(self):
# 设置随机种子，确保结果可复现
np.random.seed(42)
def generate_3d_points(self, num_points=100, scale=5.0):
"""
生成随机的3D点
:param num_points: 点的数量
:param scale: 点云的尺度
:return: 3D点矩阵 (3, N)
"""
points = np.random.rand(3, num_points) * scale - scale / 2
return points
def project_points(self, points_3d, K, R, t):
"""
将3D点投影到图像平面
:param points_3d: 3D点矩阵 (3, N)
:param K: 相机内参矩阵 (3, 3)
:param R: 旋转矩阵 (3, 3)
:param t: 平移向量 (3, 1)
:return: 2D投影点矩阵 (2, N)
"""
# 构造投影矩阵
P = K @ np.hstack((R, t))
# 转换为齐次坐标
points_3d_hom = np.vstack((points_3d, np.ones((1, points_3d.shape[1]))))
# 投影
points_2d_hom = P @ points_3d_hom
# 转换回非齐次坐标
points_2d = points_2d_hom[:2, :] / points_2d_hom[2, :]
return points_2d
def add_noise(self, points_2d, noise_level=0.5):
"""
为2D点添加噪声
:param points_2d: 2D点矩阵 (2, N)
:param noise_level: 噪声水平
:return: 带噪声的2D点矩阵 (2, N)
"""
noise = np.random.randn(2, points_2d.shape[1]) * noise_level
return points_2d + noise
def estimate_pose_from_essential_matrix(self, points1, points2, K):
"""
从本质矩阵估计相机位姿
:param points1: 第一幅图像中的点 (2, N)
:param points2: 第二幅图像中的点 (2, N)
:param K: 相机内参矩阵 (3, 3)
:return: R (旋转矩阵), t (平移向量)
"""
# 转换为归一化坐标
K_inv = np.linalg.inv(K)
points1_normalized = K_inv @ np.vstack((points1, np.ones((1, points1.shape[1]))))
points2_normalized = K_inv @ np.vstack((points2, np.ones((1, points2.shape[1]))))
points1_normalized = points1_normalized[:2, :]
points2_normalized = points2_normalized[:2, :]
# 计算本质矩阵
E, mask = cv2.findEssentialMat(points1_normalized.T, points2_normalized.T, method=cv2.RANSAC, prob=0.999, threshold=0.001)
# 从本质矩阵恢复位姿
_, R, t, mask = cv2.recoverPose(E, points1_normalized.T, points2_normalized.T)
return R, t, E
def run_demo(self):
"""
运行相机位姿估计演示
"""
# 1. 设置相机内参
fx, fy = 800, 800
cx, cy = 320, 240
K = np.array([[fx, 0, cx],
[0, fy, cy],
[0, 0, 1]])
# 2. 生成3D点
num_points = 100
points_3d = self.generate_3d_points(num_points)
# 3. 设置真实的相机位姿
# 相机1：位于原点，朝向Z轴正方向
R1 = np.eye(3)
t1 = np.zeros((3, 1))
# 相机2：绕Y轴旋转30度，沿X轴平移2个单位
angle = np.radians(30)
R2 = np.array([[np.cos(angle), 0, np.sin(angle)],
[0, 1, 0],
[-np.sin(angle), 0, np.cos(angle)]])
t2 = np.array([[2.0], [0.0], [0.0]])
# 4. 投影3D点到两个相机
points1 = self.project_points(points_3d, K, R1, t1)
points2 = self.project_points(points_3d, K, R2, t2)
# 5. 添加噪声
points1_noisy = self.add_noise(points1)
points2_noisy = self.add_noise(points2)
# 6. 从本质矩阵估计位姿
R_est, t_est, E = self.estimate_pose_from_essential_matrix(points1_noisy, points2_noisy, K)
# 7. 结果可视化
self.visualize_results(points_3d, points1_noisy, points2_noisy, R2, t2, R_est, t_est)
def visualize_results(self, points_3d, points1, points2, R_gt, t_gt, R_est, t_est):
"""
可视化位姿估计结果
"""
fig = plt.figure(figsize=(15, 10))
# 1. 绘制3D点
ax1 = fig.add_subplot(221, projection='3d')
ax1.scatter(points_3d[0, :], points_3d[1, :], points_3d[2, :], c='b', marker='.')
ax1.set_xlabel('X')
ax1.set_ylabel('Y')
ax1.set_zlabel('Z')
ax1.set_title('3D点云')
# 2. 绘制第一幅图像中的点
ax2 = fig.add_subplot(222)
ax2.scatter(points1[0, :], points1[1, :], c='r', marker='.')
ax2.set_xlabel('X')
ax2.set_ylabel('Y')
ax2.set_title('第一幅图像中的点')
ax2.invert_yaxis() # 图像坐标系的Y轴向下
# 3. 绘制第二幅图像中的点
ax3 = fig.add_subplot(223)
ax3.scatter(points2[0, :], points2[1, :], c='g', marker='.')
ax3.set_xlabel('X')
ax3.set_ylabel('Y')
ax3.set_title('第二幅图像中的点')
ax3.invert_yaxis() # 图像坐标系的Y轴向下
# 4. 绘制相机位姿比较
ax4 = fig.add_subplot(224, projection='3d')
# 绘制3D点
ax4.scatter(points_3d[0, :], points_3d[1, :], points_3d[2, :], c='b', marker='.')
# 绘制真实相机位姿
self.plot_camera(ax4, np.eye(3), np.zeros((3, 1)), 'r', '真实相机1')
self.plot_camera(ax4, R_gt, t_gt, 'g', '真实相机2')
# 绘制估计相机位姿
self.plot_camera(ax4, R_est, t_est, 'm', '估计相机2')
ax4.set_xlabel('X')
ax4.set_ylabel('Y')
ax4.set_zlabel('Z')
ax4.set_title('相机位姿比较')
ax4.legend()
plt.tight_layout()
plt.show()
# 打印估计结果
print("真实旋转矩阵 R:")
print(R_gt)
print("\n估计旋转矩阵 R_est:")
print(R_est)
print("\n真实平移向量 t:")
print(t_gt)
print("\n估计平移向量 t_est:")
print(t_est)
def plot_camera(self, ax, R, t, color, label, scale=0.5):
"""
在3D图中绘制相机
"""
# 相机中心
center = t.flatten()
ax.scatter(center[0], center[1], center[2], c=color, s=50, marker='o', label=label)
# 相机坐标轴
axes = scale * R
x_axis = center + axes[:, 0]
y_axis = center + axes[:, 1]
z_axis = center + axes[:, 2]
ax.plot([center[0], x_axis[0]], [center[1], x_axis[1]], [center[2], x_axis[2]], c='r')
ax.plot([center[0], y_axis[0]], [center[1], y_axis[1]], [center[2], y_axis[2]], c='g')
ax.plot([center[0], z_axis[0]], [center[1], z_axis[1]], [center[2], z_axis[2]], c='b')
# 相机视锥
vertices = scale * np.array([[-1, -1, 2],
[1, -1, 2],
[1, 1, 2],
[-1, 1, 2]])
vertices = R @ vertices.T + t
vertices = vertices.T
faces = [[0, 1, 2], [0, 2, 3], [0, 1, 1], [1, 2, 2], [2, 3, 3], [3, 0, 0]]
for face in faces:
x = [vertices[face[0], 0], vertices[face[1], 0], vertices[face[2], 0], vertices[face[0], 0]]
y = [vertices[face[0], 1], vertices[face[1], 1], vertices[face[2], 1], vertices[face[0], 1]]
z = [vertices[face[0], 2], vertices[face[1], 2], vertices[face[2], 2], vertices[face[0], 2]]
ax.plot(x, y, z, c=color, alpha=0.3)
def main():
demo = CameraPoseEstimation()
demo.run_demo()
if __name__ == "__main__":
main()
💡 代码小提示：这段代码使用了OpenCV的findEssentialMat和recoverPose函数来计算本质矩阵和恢复相机位姿。findEssentialMat函数使用RANSAC算法来鲁棒地估计本质矩阵，而recoverPose函数则从本质矩阵中恢复相机位姿并解决歧义性问题。
互动演示：探索相机位姿变化
下面我们提供一个简单的互动演示，你可以调整相机的旋转角度和平移距离，实时观察这些参数如何影响相机的位姿和3D点的投影。
相机1视图
相机2视图
3D场景与相机位姿
知识点小结
恭喜你！通过这节课，你已经掌握了相机位姿估计的基本原理和实现方法：
核心概念
- 相机位姿由旋转矩阵(R)和平移向量(t)表示
- 可以从本质矩阵中恢复相机位姿
- 从本质矩阵恢复位姿会产生四种可能的解
- 可视性检查(cheirality check)用于确定正确的解
实现步骤
- 计算本质矩阵E
- 对E进行奇异值分解(SVD)
- 构造可能的旋转矩阵和平移向量
- 进行可视性检查，选择正确的解
位姿估计的应用场景
自动驾驶
通过估计相机位姿，自动驾驶汽车可以确定自己的位置和行驶方向，实现定位和导航。
增强现实(AR)
AR系统需要准确估计相机位姿，才能将虚拟物体正确地叠加到真实世界中。
机器人导航
机器人通过相机位姿估计可以了解自己在环境中的位置，实现自主导航和避障。
3D重建
在从多张图像重建3D场景时，相机位姿估计是关键的一步，决定了重建的准确性。