SciPy 中的大规模捆绑调整¶
| 日期 | 2016-10-22（最后修改） |
|---|
捆绑调整问题出现在 3-D 重建中，它可以表述如下（摘自 https://en.wikipedia.org/wiki/Bundle_adjustment）
给定一组从不同视角描绘多个 3D 点的图像，捆绑调整可以定义为同时优化描述场景几何形状的 3D 坐标以及用于获取图像的相机(s)的相对运动参数和光学特性的问题，根据涉及所有点对应图像投影的优化标准。
更准确地说。我们有一组在现实世界中定义的点，它们在某个先验选择的“世界坐标系”中由其坐标 (X, Y, Z) 定义。我们用不同的相机拍摄这些点，这些相机以其相对于世界坐标系的方位和平移以及焦距和两个径向畸变参数（总共 9 个参数）为特征。然后，我们精确地测量相机在图像上投影的点的 2-D 坐标 (x, y)。我们的任务是通过最小化重投影误差的平方和来优化原始点的 3-D 坐标以及相机参数。
令 $\pmb{P} = (X, Y, Z)^T$ - 点的半径向量，$\pmb{R}$ - 相机的旋转矩阵，$\pmb{t}$ - 相机的平移向量，$f$ - 它的焦距，$k_1, k_2$ - 它的畸变参数。然后，重投影如下进行
\begin{align} \pmb{Q} = \pmb{R} \pmb{P} + \pmb{t} \\ \pmb{q} = -\begin{pmatrix} Q_x / Q_z \\ Q_y / Q_z \end{pmatrix} \\ \pmb{p} = f (1 + k_1 \lVert \pmb{q} \rVert^2 + k_2 \lVert \pmb{q} \rVert^4) \pmb{q} \end{align}得到的向量 $\pmb{p}=(x, y)^T$ 包含原始点的图像坐标。这种模型被称为“针孔相机模型”，我在此找到了一些关于此主题的非常好的笔记 http://www.comp.nus.edu.sg/~cs4243/lecture/camera.pdf
现在让我们开始解决一些真实的束调整问题。我们将从 http://grail.cs.washington.edu/projects/bal/ 中获取一个问题。
from __future__ import print_function
import urllib
import bz2
import os
import numpy as np
首先下载数据文件
BASE_URL = "http://grail.cs.washington.edu/projects/bal/data/ladybug/"
FILE_NAME = "problem-49-7776-pre.txt.bz2"
URL = BASE_URL + FILE_NAME
if not os.path.isfile(FILE_NAME):
urllib.request.urlretrieve(URL, FILE_NAME)
现在从文件中读取数据
def read_bal_data(file_name):
with bz2.open(file_name, "rt") as file:
n_cameras, n_points, n_observations = map(
int, file.readline().split())
camera_indices = np.empty(n_observations, dtype=int)
point_indices = np.empty(n_observations, dtype=int)
points_2d = np.empty((n_observations, 2))
for i in range(n_observations):
camera_index, point_index, x, y = file.readline().split()
camera_indices[i] = int(camera_index)
point_indices[i] = int(point_index)
points_2d[i] = [float(x), float(y)]
camera_params = np.empty(n_cameras * 9)
for i in range(n_cameras * 9):
camera_params[i] = float(file.readline())
camera_params = camera_params.reshape((n_cameras, -1))
points_3d = np.empty(n_points * 3)
for i in range(n_points * 3):
points_3d[i] = float(file.readline())
points_3d = points_3d.reshape((n_points, -1))
return camera_params, points_3d, camera_indices, point_indices, points_2d
camera_params, points_3d, camera_indices, point_indices, points_2d = read_bal_data(FILE_NAME)
这里我们有 numpy 数组
camera_params形状为(n_cameras, 9)，包含所有相机的参数初始估计。每行的前 3 个分量构成旋转向量 (https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula)，接下来的 3 个分量构成平移向量，然后是焦距和两个畸变参数。points_3d形状为(n_points, 3)，包含世界坐标系中点坐标的初始估计。camera_ind形状为(n_observations,)，包含每个观测中涉及的相机的索引（从 0 到n_cameras - 1）。point_ind形状为(n_observations,)，包含每个观测中涉及的点的索引（从 0 到n_points - 1）。points_2d形状为(n_observations, 2)，包含每个观测中投影到图像上的点的测量二维坐标。
数字如下
n_cameras = camera_params.shape[0]
n_points = points_3d.shape[0]
n = 9 * n_cameras + 3 * n_points
m = 2 * points_2d.shape[0]
print("n_cameras: {}".format(n_cameras))
print("n_points: {}".format(n_points))
print("Total number of parameters: {}".format(n))
print("Total number of residuals: {}".format(m))
我们选择了一个相对较小的问题来减少计算时间，但 scipy 的算法能够解决更大的问题，尽管所需时间会按比例增长。
现在定义一个返回残差向量的函数。我们使用 numpy 向量化计算
def rotate(points, rot_vecs):
"""Rotate points by given rotation vectors.
Rodrigues' rotation formula is used.
"""
theta = np.linalg.norm(rot_vecs, axis=1)[:, np.newaxis]
with np.errstate(invalid='ignore'):
v = rot_vecs / theta
v = np.nan_to_num(v)
dot = np.sum(points * v, axis=1)[:, np.newaxis]
cos_theta = np.cos(theta)
sin_theta = np.sin(theta)
return cos_theta * points + sin_theta * np.cross(v, points) + dot * (1 - cos_theta) * v
def project(points, camera_params):
"""Convert 3-D points to 2-D by projecting onto images."""
points_proj = rotate(points, camera_params[:, :3])
points_proj += camera_params[:, 3:6]
points_proj = -points_proj[:, :2] / points_proj[:, 2, np.newaxis]
f = camera_params[:, 6]
k1 = camera_params[:, 7]
k2 = camera_params[:, 8]
n = np.sum(points_proj**2, axis=1)
r = 1 + k1 * n + k2 * n**2
points_proj *= (r * f)[:, np.newaxis]
return points_proj
def fun(params, n_cameras, n_points, camera_indices, point_indices, points_2d):
"""Compute residuals.
`params` contains camera parameters and 3-D coordinates.
"""
camera_params = params[:n_cameras * 9].reshape((n_cameras, 9))
points_3d = params[n_cameras * 9:].reshape((n_points, 3))
points_proj = project(points_3d[point_indices], camera_params[camera_indices])
return (points_proj - points_2d).ravel()
您可以看到计算 fun 的雅可比矩阵很麻烦，因此我们将依赖于有限差分近似。为了使此过程在时间上可行，我们提供了雅可比矩阵稀疏结构（即标记已知非零的元素）
from scipy.sparse import lil_matrix
def bundle_adjustment_sparsity(n_cameras, n_points, camera_indices, point_indices):
m = camera_indices.size * 2
n = n_cameras * 9 + n_points * 3
A = lil_matrix((m, n), dtype=int)
i = np.arange(camera_indices.size)
for s in range(9):
A[2 * i, camera_indices * 9 + s] = 1
A[2 * i + 1, camera_indices * 9 + s] = 1
for s in range(3):
A[2 * i, n_cameras * 9 + point_indices * 3 + s] = 1
A[2 * i + 1, n_cameras * 9 + point_indices * 3 + s] = 1
return A
现在我们准备运行优化。让我们可视化使用初始参数评估的残差。
%matplotlib inline
import matplotlib.pyplot as plt
x0 = np.hstack((camera_params.ravel(), points_3d.ravel()))
f0 = fun(x0, n_cameras, n_points, camera_indices, point_indices, points_2d)
plt.plot(f0)
A = bundle_adjustment_sparsity(n_cameras, n_points, camera_indices, point_indices)
import time
from scipy.optimize import least_squares
t0 = time.time()
res = least_squares(fun, x0, jac_sparsity=A, verbose=2, x_scale='jac', ftol=1e-4, method='trf',
args=(n_cameras, n_points, camera_indices, point_indices, points_2d))
t1 = time.time()
print("Optimization took {0:.0f} seconds".format(t1 - t0))
设置 scaling='jac' 的目的是自动缩放变量并使它们对成本函数的影响相等（显然，相机参数和点的坐标是截然不同的实体）。这个选项被证明对成功进行捆绑调整至关重要。
现在让我们绘制找到的解的残差
plt.plot(res.fun)
现在我们看到了残差的更清晰的图像，平均值非常接近零。还有一些尖峰。这可以通过数据中的异常值来解释，或者可能是算法找到了一个局部最小值（尽管是一个非常好的最小值）或者没有收敛到足够程度。请注意，该算法使用雅可比有限差分近似，这可能会在最小值附近阻碍进度，因为精度不足（但同样，为这个问题计算精确的雅可比矩阵非常困难）。
章节作者：Nikolay Mayorov