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)