Non-linear optimization for rotation - graphics

Question

I had a chat with an engineer the other day and we both were stumped on a question related to bundle adjustment. For a refresher, here is a good link explaining the problem:
http://homepages.inf.ed.ac.uk/rbf/CVonline/LOCAL_COPIES/ZISSERMAN/bundle/bundle.html
The problem requires optimization over 3n+11m parameters. The camera optimization consists of 5 intrinsic camera parameters, 3 DOF for position (x,y,z), and 3 DOF for rotation (pitch, yaw and roll).
Now, when you actually go about implementing this algorithm, a rotation matrix consists an optimization over 9 numbers. Euler's Axis Theorem says these 9 numbers are related and there are only 3 degrees of freedom overall.
Suppose you represent the rotation using a normalized quaternion. Then you have optimization over 3 numbers. Same DOF.
Is one representation more computationally efficient and better than the other? Will you have less variables to optimize using a rotation quaternion over rotation matrix?

Answer 1

You never optimize over 9 numbers! Of course this would be inefficient. One efficient representation in which you only need 3 parameters is to parametrize your rotation matrix R using the Lie algebra of the groupe SO(3). If you are not familiar with Lie algebra, here's a tutorial that explains everything in an intuitive (but sometimes oversimplified) manner. To explain it in a few short sentences, in this representation, each rotation matrix R is written as expmat(a*G_1+b*G_2+c*G_3) where expmat is the matrix exponential, and the G_i are the "generators" of the lie algebra of SO(3), i.e. the tangent space to SO(3) at the identity. Therefore, to estimate a rotation matrix, you only need to learn the three parameters a,b,c. This is roughly equivalent to decomposing your rotation matrix in three rotations around x,y,z and estimating the three angles of these rotations.

Answer 2

A solution not mentioned yet is to use axis-angle parameterization.
Basically, you represent the rotation as a single 3D vector. The direction v/|v| of the vector is the axis of rotation, and the norm |v| is the angle of rotation around that axis.
This method has 3 DOF directly, unlike quaternions' 4 DOF. So with quaternions, you need to use either constrained optimization or additional parameterization to get down to 3 DOF.
I'm not familiar with #Ash's suggestion, but he does mention in the comment that it only works for small angles. Axis-angle representation doesn't have this limitation.

Answer 3

One option is as relatively_random suggests to optimize over the axis-angle parameterization. The derivative can then, relatively simple, be computed as described in this paper. The only problem might be that some numerical issues might arise for rotations close to the identity.
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))