自己解決しました

http://www.fractal.is.tohoku.ac.jp/okatani/pub/cvim.pdf
ここの最後の回転の差分表示を使えばできました。
つまり、線形解法で求まった回転行列Rを固定して、
最適化をかけるパラメータを３つのオイラー角を用意して、
それからできる回転行列δRを用いて、RδRとすれば回転行列の直交性を保ったまま
最適化が可能です。