三次元座標からの回転角の導出において、最適化する手法について

現在、変位前後の対応点群から、回転角と並進量を同時に求める際に使う最適化の方法について悩んでいます。

Newton-Raphson法を用いて繰り返し演算により3次元の回転角α、β、γ、そして並進量dx、dy、dzを同時に計算させるプログラムを作ってみたのですが、うまく収束しませんでした。
数値計算などで使われる有効な最適化手法や、そのライブラリなどあれば教えていただけるとうれしいです。

だいたい以下のようなことをしています。

変位前の座標がPi(xi,yi,zi)であった点が変位してそれぞれP'i(x'i,y'i,z'i)に移ったとします。
これらの座標点の間には、回転行列と並進量を用いて

x'i = m11*xi + m12*yi + m13*zi + dx
y'i = m21*xi + m22*yi + m23*zi + dy
z'i = m31*xi + m32*yi + m33*zi + dz

の関係があるとする。
（ただし、m11などはｚ軸まわりにγ→y軸まわりにβ→x軸まわりにαの順に回転したときのcosとsinの掛け算）

Newton-Raphson法において、評価関数は以下のようにおきました。

Σ{ x'i - ( m11*xi + m12*yi + m13*zi + dx ) }^2
+ { y'i - ( m21*xi + m22*yi + m23*zi + dy ) }^2
+ { z'i - ( m31*xi + m32*yi + m33*zi + dz ) }^2

評価関数が誤差に対して敏感すぎるということも考えられますが、最適化についてまったく専門外な私にはどこが問題なのかわかりません。

ぜひご指導よろしくお願いします。

No.4 ベストアンサー 回答者： ddtddtddt 回答日時： 2008/02/21 13:04

　#2です。

#3さんの仰るとおりだと思います。以下は、私ならこうします、の例です。

　変換の回転成分をＡをしたとき、#2の方法で、サンプル点から単純に逆算したＡは誤差εを持つので、直行条件、

　　(Ａ＋ε)・(Ａ＋ε)^t＝Ｅ

を付けます。ここで^tは転置，Ｅは単位行列で、さらにＡの値は先に求めておきます。上記を2次の微少量を無視して展開すれば、

　　ε・Ａ^t＋Ａ・ε^t＝Ｅ－Ａ・Ａ^t

となります。Ａを先に求めておいたので、成分への展開は多少面倒ですが、上記も線形です。εを求め、ＡをＡ＋εに修正し、もともとの変換条件に持ち込んで、平行移動成分を修正します。
　サンプル点の座標には手を付けません。手を付けると、際限がなくなるからです。かつ修正は一回にとどめます。

　これ以上の事をやるくらいなら、その前に、サンプル点の精度を上げるべきだというのが、自分の意見です。でもそれも出来ない場合は、仕方なく、非線形計算に入ります。

No.3 回答者： arrysthmia 回答日時： 2008/02/20 20:28

{ m_ij } が回転行列であるという条件を、どのように定式化しましたか？

単に３行３列の行列としたのでは、自由度が高すぎます。
恐らく、この事によって、評価関数の極小値が広義極小となってしまい、
極小値を与える空間の中で、反復法の逐次解がウロウロしてしまう…
というのが、「うまく収束しませんでした」の事態ではないかと思います。
条件式を追加しましょう。

質問の評価関数を { m_ij, dx, dy, dz } の１２変数関数と考えた場合、
∇評価関数＝０ という方程式は一次方程式になりますから、
臨界点が複数あったとしても、それらは R^12 の線形部分空間であって、
単連結です。よって、この問題の場合、local minimumへのトラッピングは
起こりそうにありません。

No.2 回答者： ddtddtddt 回答日時： 2008/02/18 12:58

　#1さんと基本的には同じです。

x'i = m11*xi + m12*yi + m13*zi + dx
y'i = m21*xi + m22*yi + m23*zi + dy
z'i = m31*xi + m32*yi + m33*zi + dz

は線形方程式なので、わざわざNewton-Raphson法などを用いずに、そのまま線形連立方程式と考える事ができます。ただし、サンプル点は4点以上（自由度12以上）ある事を前提とします。

　自由度12の場合は、解は一意です。結果の正しさは、4点の誤差の精度を越えるものではありません（それ以上の事はできません）。
　自由度が12を越える場合は条件過多になりますが、残差ベクトル最小の条件を付けて解く事は可能です（線形の最小２乗法と同じ）。
　残差ベクトル最小の条件は、代数的操作に帰着されるので、local minimumにはまる危険は少ないと思いますが、結果が満足できるかどうかは、やはり問題の状況次第です。

　なお、残差ベクトル最小の評価関数は、
Σ{ x'i - ( m11*xi + m12*yi + m13*zi + dx ) }^2
+ { y'i - ( m21*xi + m22*yi + m23*zi + dy ) }^2
+ { z'i - ( m31*xi + m32*yi + m33*zi + dz ) }^2

と同じです。

No.1 回答者： stomachman 回答日時： 2008/02/18 11:34

　変位を与える空間変換に全く誤差がないのであれば、３個の点の座標を使って解析的に変換を決定できる。

ひとつの点を原点にし、2つ目の点でx軸の方向を決め、2つ目と3つ目の外積でy軸の方向を決めてやれば良い訳です。
　なのにわざわざ非線形最小二乗法を使うのは、変位に誤差が入っているからでしょう。で、その誤差の程度が問題です。
　点の間隔に比べて誤差が小さい場合、互いに離れた３点を選んで、上記のように解析的に変換を決定し、それを出発値にして非線形最小二乗法（どんな手法でも良い）を適用すればいとも簡単に収束します。
　しかし、誤差が非常に大きくて、しかも点の個数があまり多くない場合には、評価関数が沢山の極値（local minimum）を持つようになることがあり、するとgradientを計算すると0に近くなってしまうことがある。その場合、空間的に近い点同士で点を３つのグループに分け、グループごとの位置の平均を取って、これらを３つの点だと思って解析的に変換を決定して出発値にする、という方法は、まるでお門違いのlocal minimumを避けるのに有効です。
　また、反復の度に評価関数が以前より小さくなったかどうかを確認し、もし小さくなっていなければ小さくなるまで修正量を半分にしていく、などの安定化の手法を組み込むと良いです。
　しかしながら、local minimumに落ち込んでしまう恐れは常にあります。本当に最小を探したいのか、あるいはlocal minimumでも構わないのかは、応用によります。