前几天我和一位工程师聊了聊，我们都被一个与捆绑调整有关的问题难住了。为了复习，这里有一个很好的链接来解释这个问题：
http://homepages.inf.ed.ac.uk/rbf/CVonline/LOCAL_COPIES/ZISSERMAN/bundle/bundle.html
该问题需要对3n+11m参数进行优化。相机优化由5个固有相机参数组成，位置（x，y，z）为3个自由度，旋转（俯仰、偏航和滚转）为3自由度。
现在，当你真正开始实现这个算法时，一个旋转矩阵由9个数字的优化组成。欧拉轴定理说这9个数是相关的，总共只有3个自由度。
假设使用标准化四元数表示旋转。然后您可以对3个数字进行优化。相同DOF。
一种表示在计算上是否比另一种更高效、更好？使用旋转四元数在旋转矩阵上进行优化的变量会更少吗？
您永远不会优化超过9个数字！当然，这将是低效的。一个只需要3个参数的有效表示是使用群eSO（3）的李代数来参数化旋转矩阵R。如果你不熟悉李代数，这里有一个教程，以直观（但有时过于简单）的方式解释一切。为了用几句短句来解释它，在这种表示中，每个旋转矩阵R都被写成expmat(a*G_1+b*G_2+c*G_3)，其中expmat是矩阵指数，而G_i是SO（3）的李代数的"生成器"，即在恒等式上与S0（3）的切空间。因此，要估计旋转矩阵，只需要学习三个参数a,b,c。这大致相当于将旋转矩阵分解为围绕x,y,z的三个旋转，并估计这些旋转的三个角度。
一个尚未提及的解决方案是使用轴角度参数化。
基本上将旋转表示为单个三维矢量。向量的方向v/|v|是旋转轴，而范数|v|则是围绕该轴的旋转角度。
该方法直接具有3个自由度，不同于四元数的4个自由度。因此，对于四元数，您需要使用约束优化或附加参数化来获得3自由度。
我不熟悉@Ash的建议，但他在评论中提到，这只适用于小角度。轴角度表示没有此限制。
relative_random建议使用一个选项来优化轴角度参数化。然后，可以相对简单地计算导数，如本文所述。唯一的问题可能是，对于接近恒等式的旋转，可能会出现一些数值问题。
import numpy as np
def hat(v):
"""
vecotrized version of the hat function, creating for a vector its skew symmetric matrix.
Args:
v (np.array<float>(..., 3, 1)): The input vector.
Returns:
(np.array<float>(..., 3, 3)): The output skew symmetric matrix.
"""
E1 = np.array([[0., 0., 0.], [0., 0., -1.], [0., 1., 0.]])
E2 = np.array([[0., 0., 1.], [0., 0., 0.], [-1., 0., 0.]])
E3 = np.array([[0., -1., 0.], [1., 0., 0.], [0., 0., 0.]])
return v[..., 0:1, :] * E1 + v[..., 1:2, :] * E2 + v[..., 2:3, :] * E3
def exp(v, der=False):
"""
Vectorized version of the exponential map.
Args:
v (np.array<float>(..., 3, 1)): The input axis-angle vector.
der (bool, optional): Wether to output the derivative as well. Defaults to False.
Returns:
R (np.array<float>(..., 3, 3)): The corresponding rotation matrix.
[dR (np.array<float>(3, ..., 3, 3)): The derivative of each rotation matrix.
The matrix dR[i, ..., :, :] corresponds to
the derivative d R[..., :, :] / d v[..., i, :],
so the derivative of the rotation R gained
through the axis-angle vector v with respect
to v_i. Note that this is not a Jacobian of
any form but a vectorized version of derivatives.]
"""
n = np.linalg.norm(v, axis=-2, keepdims=True)
H = hat(v)
with np.errstate(all='ignore'):
R = np.identity(3) + (np.sin(n) / n) * H + ((1 - np.cos(n)) / n**2) * (H @ H)
R = np.where(n == 0, np.identity(3), R)
if der:
sh = (3,) + tuple(1 for _ in range(v.ndim - 2)) + (3, 1)
dR = np.swapaxes(np.expand_dims(v, axis=0), 0, -2) * H
dR = dR + hat(np.cross(v, ((np.identity(3) - R) @ np.identity(3).reshape(sh)), axis=-2))
dR = dR @ R
n = n**2 # redifinition
with np.errstate(all='ignore'):
dR = dR / n
dR = np.where(n == 0, hat(np.identity(3).reshape(sh)), dR)
return R, dR
else:
return R
# generate two sets of points which differ by a rotation
np.random.seed(1001)
n = 100 # number of points
p_1 = np.random.randn(n, 3, 1)
v = np.array([0.3, -0.2, 0.1]).reshape(3, 1) # the axis-angle vector
p_2 = exp(v) @ p_1 + np.random.randn(n, 3, 1) * 1e-2
# estimate v with least sqaures, so the objective function becomes:
# minimize v over f(v) = sum_[1<=i<=n] (||p_1_i - exp(v)p_2_i||^2)
# Due to the way least_squres is implemented we have to pass the
# individual residuals ||p_1_i - exp(v)p_2_i||^2 as ||p_1_i - exp(v)p_2_i||.
from scipy.optimize import least_squares
def loss(x):
R = exp(x.reshape(1, 3, 1))
y = p_2 - R @ p_1
y = np.linalg.norm(y, axis=-2).squeeze(-1)
return y
def d_loss(x):
R, d_R = exp(x.reshape(1, 3, 1), der=True)
y = p_2 - R @ p_1
d_y = -d_R @ p_1
d_y = np.sum(y * d_y, axis=-2) / np.linalg.norm(y, axis=-2)
d_y = d_y.squeeze(-1).T
return d_y
x0 = np.zeros((3))
res = least_squares(loss, x0, d_loss)
print('True axis-angle vector: {}'.format(v.reshape(-1)))
print('Estimated axis-angle vector: {}'.format(res.x))