7

既知の単位ベクトルを持つ 2 つのデカルト座標系があります。

システム A(x_A,y_A,z_A)

と

システム B(x_B、y_B、z_B)

両方のシステムが同じオリジン (0,0,0) を共有しています。システム B のベクトルをシステム A で表現できるように、四元数を計算しようとしています。

私は四元数の数学的概念に精通しています。ここから必要な数学を既に実装しています: http://content.gpwiki.org/index.php/OpenGL%3aTutorials%3aUsing_Quaternions_to_represent_rotation

考えられる解決策の 1 つは、オイラー角を計算し、それらを 3 つのクォータニオンに使用することです。それらを乗算すると最終的な結果が得られるため、ベクトルを変換できます。

v(A) = q*v(B)*q_conj

しかし、これには再びジンバル ロックが組み込まれるため、当初はオイラー角を使用しなかったのです。

これを解決する方法はありますか？

4

6 に答える 6

7

ある座標系から別の座標系への最適な変換を表す四元数は、このホワイト ペーパーで説明されている方法で計算できます。

ポール J. ベスルとニール D. マッケイ「3D 形状の登録方法」、センサー フュージョン IV: コントロール パラダイムとデータ構造、586 (1992 年 4 月 30 日)。http://dx.doi.org/10.1117/12.57955

この論文はオープン アクセスではありませんが、Python の実装を紹介できます。

def get_quaternion(lst1,lst2,matchlist=None):
    if not matchlist:
        matchlist=range(len(lst1))
    M=np.matrix([[0,0,0],[0,0,0],[0,0,0]])

    for i,coord1 in enumerate(lst1):
        x=np.matrix(np.outer(coord1,lst2[matchlist[i]]))
        M=M+x

    N11=float(M[0][:,0]+M[1][:,1]+M[2][:,2])
    N22=float(M[0][:,0]-M[1][:,1]-M[2][:,2])
    N33=float(-M[0][:,0]+M[1][:,1]-M[2][:,2])
    N44=float(-M[0][:,0]-M[1][:,1]+M[2][:,2])
    N12=float(M[1][:,2]-M[2][:,1])
    N13=float(M[2][:,0]-M[0][:,2])
    N14=float(M[0][:,1]-M[1][:,0])
    N21=float(N12)
    N23=float(M[0][:,1]+M[1][:,0])
    N24=float(M[2][:,0]+M[0][:,2])
    N31=float(N13)
    N32=float(N23)
    N34=float(M[1][:,2]+M[2][:,1])
    N41=float(N14)
    N42=float(N24)
    N43=float(N34)

    N=np.matrix([[N11,N12,N13,N14],\
              [N21,N22,N23,N24],\
              [N31,N32,N33,N34],\
              [N41,N42,N43,N44]])


    values,vectors=np.linalg.eig(N)
    w=list(values)
    mw=max(w)
    quat= vectors[:,w.index(mw)]
    quat=np.array(quat).reshape(-1,).tolist()
    return quat

この関数は、探していた四元数を返します。引数 lst1 と lst2 は、すべての配列が 3D ベクトルを表す numpy.arrays のリストです。両方のリストの長さが 3 である (直交単位ベクトルを含む) 場合、クォータニオンは正確な変換である必要があります。より長いリストを指定すると、両方のポイント セットの差を最小にするクォータニオンが得られます。オプションの matchlist 引数は、lst2 のどの点を lst1 のどの点に変換する必要があるかを関数に伝えるために使用されます。一致リストが指定されていない場合、関数は、lst1 の最初の点が lst2 の最初の点と一致する必要があると想定します...

C++ の 3 つのポイントのセットに対する同様の関数は次のとおりです。

#include <Eigen/Dense>
#include <Eigen/Geometry>

using namespace Eigen;

/// Determine rotation quaternion from coordinate system 1 (vectors
/// x1, y1, z1) to coordinate system 2 (vectors x2, y2, z2)
Quaterniond QuaternionRot(Vector3d x1, Vector3d y1, Vector3d z1,
                          Vector3d x2, Vector3d y2, Vector3d z2) {

    Matrix3d M = x1*x2.transpose() + y1*y2.transpose() + z1*z2.transpose();

    Matrix4d N;
    N << M(0,0)+M(1,1)+M(2,2)   ,M(1,2)-M(2,1)          , M(2,0)-M(0,2)         , M(0,1)-M(1,0),
         M(1,2)-M(2,1)          ,M(0,0)-M(1,1)-M(2,2)   , M(0,1)+M(1,0)         , M(2,0)+M(0,2),
         M(2,0)-M(0,2)          ,M(0,1)+M(1,0)          ,-M(0,0)+M(1,1)-M(2,2)  , M(1,2)+M(2,1),
         M(0,1)-M(1,0)          ,M(2,0)+M(0,2)          , M(1,2)+M(2,1)         ,-M(0,0)-M(1,1)+M(2,2);

    EigenSolver<Matrix4d> N_es(N);
    Vector4d::Index maxIndex;
    N_es.eigenvalues().real().maxCoeff(&maxIndex);

    Vector4d ev_max = N_es.eigenvectors().col(maxIndex).real();

    Quaterniond quat(ev_max(0), ev_max(1), ev_max(2), ev_max(3));
    quat.normalize();

    return quat;
}
于 2014-05-20T13:01:14.940 に答える
1

私はちょうどこの同じ問題に遭遇しました。私は解決策に向かっていましたが、行き詰まりました。

したがって、両方の座標系で知られている 2 つのベクトルが必要になります。私の場合、デバイスの座標系 (重力と磁場) に 2 つの正規直交ベクトルがあり、デバイス座標からグローバル方向 (North は正の Y、"up" は正の方向) に回転するクォータニオンを見つけたいと考えています。正の Z)。したがって、私の場合、デバイス座標空間でベクトルを測定し、ベクトル自体を定義して、グローバル システムの正規直交基底を形成しています。

そうは言っても、四元数の軸角度の解釈を検討してください。グローバル座標に一致するように、デバイスの座標をある角度だけ回転させることができるベクトル V があります。(負の) 重力ベクトルを G、磁場を M と呼びます (どちらも正規化されています)。

V、G、M はすべて単位球上の点を表します。Z_dev と Y_dev (デバイスの座標系の Z ベースと Y ベース) も同様です。目標は、G を Z_dev に、M を Y_dev にマッピングする回転を見つけることです。V が G を Z_dev に回転させるには、G と V によって定義されるポイント間の距離は、V と Z_dev によって定義されるポイント間の距離と同じでなければなりません。方程式では:

|V - G| = |V - Z_dev|

この方程式の解は平面を形成します (すべての点が G と Z_dev から等距離にある)。しかし、V は単位長に制限されています。つまり、解は原点を中心とするリングであり、点の数は無限です。

しかし、同じ状況が Y_dev、M、および V にも当てはまります。

|V - M| = |V - Y_dev|

これに対する解決策も、原点を中心としたリングです。これらの環には 2 つの交点があり、一方は他方の負の値です。どちらも有効な回転軸です (回転角度が負の場合もあります)。

上記の 2 つの方程式を使用し、これらのベクトルのそれぞれが単位長であるという事実から、V を解くことができるはずです。

次に、回転する角度を見つける必要があります。これは、V から対応するベース (私にとっては G と Z_dev) に向かうベクトルを使用して実行できるはずです。

最終的に、私は V を解く代数の終わりに向けて夢中になりました.. しかし、いずれにせよ、必要なものはすべてここにあると思います.

于 2014-06-06T14:49:55.180 に答える