旋转的非线性优化
Non-linear optimization for rotation
前几天我和一位工程师聊天，我们都被一个与捆绑调整相关的问题难住了。作为复习，这里有一个很好的 link 解释问题：
http://homepages.inf.ed.ac.uk/rbf/CVonline/LOCAL_COPIES/ZISSERMAN/bundle/bundle.html
该问题需要优化超过 3n+11m 个参数。相机优化包括 5 个固有相机参数、3 个位置自由度 (x、y、z) 和 3 个旋转自由度（俯仰、偏航和滚动）。
现在，当您实际着手实施此算法时，旋转矩阵包含对 9 个数字的优化。欧拉轴定理说这9个数是相关的，整体只有3个自由度。
假设您使用归一化四元数表示旋转。然后你对 3 个数字进行了优化。相同的自由度。
一种表示是否比另一种更高效、更好？在旋转矩阵上使用旋转四元数进行优化的变量会更少吗？
您永远不会优化超过 9 个数字！当然，这将是低效的。一种只需要 3 个参数的有效表示是使用群 SO(3) 的李代数参数化旋转矩阵 R。如果您不熟悉李代数，here 的教程以直观（但有时过于简单）的方式解释了所有内容。用几句话来解释一下，在这个表示中，每个旋转矩阵 R 写成 expmat(a*G_1+b*G_2+c*G_3) 其中 expmat 是矩阵指数， G_i 是 "generators" 的李代数 SO(3)，即切线 space 到 SO(3) 在恒等式.因此，估计一个旋转矩阵，只需要学习三个参数a,b,c。这大致相当于将你的旋转矩阵分解成围绕 x,y,z 的三个旋转并估计这些旋转的三个角度。
尚未提及的解决方案是使用轴角参数化。
基本上，您将旋转表示为单个 3D 矢量。方向 v/|v|矢量的是旋转轴，范数|v|是围绕该轴的旋转角度。
这种方法直接有3个自由度，不像四元数的4个自由度。因此，对于四元数，您需要使用约束优化或额外的参数化来降低到 3 自由度。
我不熟悉@Ash 的建议，但他确实在评论中提到它只适用于小角度。轴角表示没有这个限制
一个选项是 relatively_random 建议优化轴角参数化。然后可以相对简单地计算导数，如 this paper 中所述。唯一的问题可能是对于接近身份的旋转可能会出现一些数值问题。
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))
前几天我和一位工程师聊天，我们都被一个与捆绑调整相关的问题难住了。作为复习，这里有一个很好的 link 解释问题：
http://homepages.inf.ed.ac.uk/rbf/CVonline/LOCAL_COPIES/ZISSERMAN/bundle/bundle.html
该问题需要优化超过 3n+11m 个参数。相机优化包括 5 个固有相机参数、3 个位置自由度 (x、y、z) 和 3 个旋转自由度（俯仰、偏航和滚动）。
现在，当您实际着手实施此算法时，旋转矩阵包含对 9 个数字的优化。欧拉轴定理说这9个数是相关的，整体只有3个自由度。
假设您使用归一化四元数表示旋转。然后你对 3 个数字进行了优化。相同的自由度。
一种表示是否比另一种更高效、更好？在旋转矩阵上使用旋转四元数进行优化的变量会更少吗？
您永远不会优化超过 9 个数字！当然，这将是低效的。一种只需要 3 个参数的有效表示是使用群 SO(3) 的李代数参数化旋转矩阵 R。如果您不熟悉李代数，here 的教程以直观（但有时过于简单）的方式解释了所有内容。用几句话来解释一下，在这个表示中，每个旋转矩阵 R 写成 expmat(a*G_1+b*G_2+c*G_3) 其中 expmat 是矩阵指数， G_i 是 "generators" 的李代数 SO(3)，即切线 space 到 SO(3) 在恒等式.因此，估计一个旋转矩阵，只需要学习三个参数a,b,c。这大致相当于将你的旋转矩阵分解成围绕 x,y,z 的三个旋转并估计这些旋转的三个角度。
尚未提及的解决方案是使用轴角参数化。
基本上，您将旋转表示为单个 3D 矢量。方向 v/|v|矢量的是旋转轴，范数|v|是围绕该轴的旋转角度。
这种方法直接有3个自由度，不像四元数的4个自由度。因此，对于四元数，您需要使用约束优化或额外的参数化来降低到 3 自由度。
我不熟悉@Ash 的建议，但他确实在评论中提到它只适用于小角度。轴角表示没有这个限制
一个选项是 relatively_random 建议优化轴角参数化。然后可以相对简单地计算导数，如 this paper 中所述。唯一的问题可能是对于接近身份的旋转可能会出现一些数值问题。
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))