旋转非线性优化

Question

前几天，我和一位工程师聊了聊，我们俩都被一个与捆绑调整有关的问题搞糊涂了。对于复习器，这里有一个很好的链接来解释这个问题：

拷贝/ZISSERMAN/bundle/bundle.html

这个问题需要对3n+11m参数进行优化。摄像机优化包括5个内在的摄像机参数，3自由度的位置(x，y，z)和3自由度的旋转(俯仰，偏航和滚转)。

现在，当您实际执行这个算法时，旋转矩阵包含了对9个数字的优化。欧拉轴定理说，这9个数字是相关的，总体上只有3个自由度。

假设您使用规范化的四元数表示旋转。然后你有超过3个数字的优化。同样的自由度。

一种表示法在计算效率上比另一种更高吗？你会有更少的变量来优化使用旋转四元数的旋转矩阵吗？

Answer 1

你永远不会优化超过9个数字！当然，这将是低效的。一个只需要3个参数的有效表示是使用群R的李代数SO(3)参数化你的旋转矩阵。如果你不熟悉李代数，这里是一个以直观(但有时过于简化)的方式解释一切的教程。为了用几句短句来解释，在这个表示中，每个旋转矩阵R都写成expmat(a*G_1+b*G_2+c*G_3)，其中expmat是矩阵指数，G_i是SO(3)的李代数的“生成元”，即在恒等式上与SO(3)的切线空间。因此，要估计旋转矩阵，只需学习三个参数a,b,c。这大致相当于将旋转矩阵分解为围绕x,y,z的三个旋转，并估计这些旋转的三个角度。

Answer 2

一个尚未提到的解决方案是使用轴角参数化.

基本上，你把旋转表示成一个三维矢量。向量的方向是旋转轴，而范数x是围绕该轴旋转的角度。

与四元数的四元数4自由度不同，该方法直接有3自由度。因此，对于四元数，您需要使用约束优化或附加参数化来降低到3自由度。

我不太熟悉@Ash的建议，但他在评论中确实提到，它只适用于小角度。轴角表示没有这个限制。

Answer 3

一种选择是，正如relatively_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))