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, v], [v, 0, -v], [-v, v, 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, Rd, Rd, Rn, Rn, Rn, diff, diff, diff)
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)
```