Optimization on SO(3)

This page demonstrates a method for iterative optimization of a function from SO(3) to the real numbers using Python. The theory behind the method is described in

@article{taylor1994minimization,
  title={Minimization on the Lie group SO (3) and related manifolds},
  author={Taylor, Camillo J and Kriegman, David J},
  journal={Yale University},
  volume={16},
  pages={155},
  year={1994}
}

Consider the vectors uⱼ ∈ R³ and vⱼ ∈ R³ indexed by j. The code below finds the rotation matrix R ∈ SO(3) that gives the maximal value of the function f(R) = ∑ⱼ (uⱼᵀ R vⱼ )^2.

The output from the program shows that Rn ∈ SO(3) comes closer to the desired Rd ∈ SO(3) for each iteration.

optimize.py

import numpy
from scipy.linalg import expm
numpy.set_printoptions(formatter={'float': '{: 0.3f}'.format})

def cv(vec):
    return numpy.array(vec, dtype=float).reshape(-1,1)

def hat(v):
    return numpy.array([[0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0]])

# R desired. This is the rotation matrix that we want to recover
# by finding the maximal value of f(R).
Rd = expm(hat([0.4, 0, 0])) @ expm(hat([0, 0.2, 0])) @ expm(hat([0, 0, -0.2]))

v_vecs = [cv([0, 0, 1]),
          cv([0, 1, 0]),
          cv([1, 0, 0])]
u_vecs = [Rd @ v for v in v_vecs]


def f(R):
    Σ = 0.0
    for u,v in zip(u_vecs, v_vecs):
        Σ += (u.T @ R @ v)[0,0]**2
    return Σ

def df_domega(R):
    Σ1, Σ2, Σ3 = 0.0, 0.0, 0.0
    ω1, ω2, ω3 = hat([1, 0, 0]), hat([0, 1, 0]), hat([0, 0, 1])
    for u,v in zip(u_vecs, v_vecs):
        Σ1 += (u.T @ R @ v)[0,0] * (u.T @ R @ ω1 @ v)[0,0]
        Σ2 += (u.T @ R @ v)[0,0] * (u.T @ R @ ω2 @ v)[0,0]
        Σ3 += (u.T @ R @ v)[0,0] * (u.T @ R @ ω3 @ v)[0,0]
    return Σ1, Σ2, Σ3

def print_result(Rn):
    diff = Rd.T @ Rn
    output = '''Fasit: {0}  Rn: {3}  diff: {6}
       {1}      {4}        {7}
       {2}      {5}        {8}'''.format(Rd[0], Rd[1], Rd[2], Rn[0], Rn[1], Rn[2], diff[0], diff[1], diff[2])
    print(output)


Rn = numpy.eye(3)

def step(Rn):
    dω = df_domega(Rn)
    return Rn @ expm(hat(dω) * 0.04)

for i in range(100):
    Rn = step(Rn)
    print_result(Rn)