35

私は、オブジェクトが最初の画像からある程度回転している単一のオブジェクトの2つの画像を使用しています。

各画像のPOSEを計算し、Rodergues()を使用して回転ベクトルをMatrixに変換しました。では、最初の位置からどれだけ回転したかを計算して確認するにはどうすればよいですか?

私は多くの方法を試しましたが、答えは近くありませんでした

編集:私のカメラは固定されており、オブジェクトだけが動いています。

4

8 に答える 8

73

次の式を使用して、回転行列からオイラー角を取得できます。

与えられた3×3回転行列

ここに画像の説明を入力してください

3つのオイラー角は

ここに画像の説明を入力してください

ここに画像の説明を入力してください

ここに画像の説明を入力してください

ここで、atan2は同じアークタンジェント関数であり、象限チェックがあり、通常はCまたはMatlabにあります。

注:y軸の周りの角度が正確に+/- 90°の場合は、注意が必要です。その場合、最初の列と最後の行のすべての要素は、下隅の1または-1を除いて、0になります(cos(1)= 0)。1つの解決策は、x軸の周りの回転を180°に固定し、atan2(r_12、-r_22)からz軸の周りの角度を計算することです。

https://www.geometrictools.com/Documentation/EulerAngles.pdfも参照してください。これには、オイラー角の6つの異なる次数の実装が含まれています。

于 2013-02-22T17:01:31.470 に答える
21

Rが(3x3)回転行列の場合、回転角はacos((tr(R -1)/ 2)になります。ここで、tr(R)は行列のトレース(つまり、対角要素の合計)です。 )。

それがあなたが求めたものです。私はそれがあなたが望むものではない可能性が90%あると見積もっています。

于 2013-02-22T15:50:05.370 に答える
6

同じ問題に取り組んでいたので、ここに貢献したいと思います。3D回転行列(3x3)を対応するロール(Rx)、ピッチ(Ry)、ヨー(Rz)の角度に変換するための純粋なPython実装を投稿することで、上記の回答に価値を追加します。

参照擬似コード: https ://www.gregslabaugh.net/publications/euler.pdf (リンクはやや無効になっています/ 2021年に壊れています...しかし、完全を期すためにここに含めます)

参照問題の設定: 3x3の回転行列があり、オイラー角を度単位で抽出するとします。スクリプトで何が起こっているのかを簡単に解読できるように、Pythonの実装を可能な限り「明白」にします。それぞれのコーダーは、自分の用途に合わせて最適化できます。

前提条件:最初にx軸を中心に回転し、次にy軸、最後にz軸を回転します。このコードスニペットを適応させるときは、この順序付けの定義を尊重する必要があります。

"""
Illustration of the rotation matrix / sometimes called 'orientation' matrix
R = [ 
       R11 , R12 , R13, 
       R21 , R22 , R23,
       R31 , R32 , R33  
    ]

REMARKS: 
1. this implementation is meant to make the mathematics easy to be deciphered
from the script, not so much on 'optimized' code. 
You can then optimize it to your own style. 

2. I have utilized naval rigid body terminology here whereby; 
2.1 roll -> rotation about x-axis 
2.2 pitch -> rotation about the y-axis 
2.3 yaw -> rotation about the z-axis (this is pointing 'upwards') 
"""
from math import (
    asin, pi, atan2, cos 
)

if R31 != 1 and R31 != -1: 
     pitch_1 = -1*asin(R31)
     pitch_2 = pi - pitch_1 
     roll_1 = atan2( R32 / cos(pitch_1) , R33 /cos(pitch_1) ) 
     roll_2 = atan2( R32 / cos(pitch_2) , R33 /cos(pitch_2) ) 
     yaw_1 = atan2( R21 / cos(pitch_1) , R11 / cos(pitch_1) )
     yaw_2 = atan2( R21 / cos(pitch_2) , R11 / cos(pitch_2) ) 

     # IMPORTANT NOTE here, there is more than one solution but we choose the first for this case for simplicity !
     # You can insert your own domain logic here on how to handle both solutions appropriately (see the reference publication link for more info). 
     pitch = pitch_1 
     roll = roll_1
     yaw = yaw_1 
else: 
     yaw = 0 # anything (we default this to zero)
     if R31 == -1: 
        pitch = pi/2 
        roll = yaw + atan2(R12,R13) 
     else: 
        pitch = -pi/2 
        roll = -1*yaw + atan2(-1*R12,-1*R13) 

# convert from radians to degrees
roll = roll*180/pi 
pitch = pitch*180/pi
yaw = yaw*180/pi 

rxyz_deg = [roll , pitch , yaw] 

これが仲間のコーダーに役立つことを願っています!

于 2020-10-13T13:28:36.080 に答える
3

参考までに、このコードはMATLABでオイラー角を計算します。

function Eul = RotMat2Euler(R)

if R(1,3) == 1 | R(1,3) == -1
  %special case
  E3 = 0; %set arbitrarily
  dlta = atan2(R(1,2),R(1,3));
  if R(1,3) == -1
    E2 = pi/2;
    E1 = E3 + dlta;
  else
    E2 = -pi/2;
    E1 = -E3 + dlta;
  end
else
  E2 = - asin(R(1,3));
  E1 = atan2(R(2,3)/cos(E2), R(3,3)/cos(E2));
  E3 = atan2(R(1,2)/cos(E2), R(1,1)/cos(E2));
end

Eul = [E1 E2 E3];

グレアム・テイラー、ジェフ・ヒントン、サム・ロワイスによって提供されたコード。詳細については、こちらをご覧ください

于 2017-09-28T03:33:43.653 に答える
1

R1cとR2cを計算した2つの回転行列とします。これらは、ポーズ1と2のオブジェクトからカメラフレームへの回転を表します(したがって、2番目のc接尾辞)。必要な回転行列は、ポーズ1からポーズ2、つまりR12です。それを計算するには、頭の中で、オブジェクトをpose_1からcameraに、次にcameraからpose_2に回転させる必要があります。後者の回転は、R2cによって抑制されたpose_2-to-cameraの逆であるため、次のようになります。

R12 = R1c * inv(R2c)

次に、行列R12から、Rodiguezの式を使用して回転角度と回転軸を計算できます。

于 2013-02-24T18:44:59.710 に答える
1

主に特定の座標軸を中心に回転するとします(人がカメラの前に立ち、回転行列を取得するために回転します)。次のコードを試してください。

float calc_angle(Eigen::Matrix3f &R_, int axis_)
{
    //! the coordinate system is consistent with "vedo"
    //! suppose it mainly rotates around a certain coordinate axis(X/Y/Z)
    
    Eigen::Vector3f aX_(1.0f, 0.0f, 0.0f);
    Eigen::Vector3f aY_(0.0f, 1.0f, 0.0f);
    Eigen::Vector3f aZ_(0.0f, 0.0f, 1.0f);

    Eigen::Vector3f v0_, v1_;
    int axis_contrary_[2];
    switch (axis_)
    {
    case 0 /* x */:
        axis_contrary_[0] = 1;
        axis_contrary_[1] = 2;
        v0_ = aY_;
        v1_ = aZ_;
        break;
    case 1 /* y */:
        axis_contrary_[0] = 0;
        axis_contrary_[1] = 2;
        v0_ = aX_;
        v1_ = aZ_;
        break;
    case 2 /* z */:
        axis_contrary_[0] = 0;
        axis_contrary_[1] = 1;
        v0_ = aX_;
        v1_ = aY_;
        break;
    }

    Eigen::Vector3f v0_new_ = R_ * v0_; //R_.col(axis_contrary_[0]);
    v0_new_(axis_) = 0.0f;
    v0_new_.normalize();

    Eigen::Vector3f v1_new_ = R_ * v1_; //R_.col(axis_contrary_[1]);
    v1_new_(axis_) = 0.0f;
    v1_new_.normalize();

    Eigen::Vector3f v2_new_0_ = v0_.cross(v0_new_);
    Eigen::Vector3f v2_new_1_ = v1_.cross(v1_new_);
    bool is_reverse = ((v2_new_0_[axis_] + v2_new_1_[axis_]) / 2.0f < 0.0f);

    float cos_theta_0_ = v0_new_(axis_contrary_[0]);
    float cos_theta_1_ = v1_new_(axis_contrary_[1]);
    float theta_0_ = std::acos(cos_theta_0_) / 3.14f * 180.0f;
    float theta_1_ = std::acos(cos_theta_1_) / 3.14f * 180.0f;
    // std::cout << "theta_0_: " << theta_0_ << std::endl;
    // std::cout << "theta_1_: " << theta_1_ << std::endl;
    float theta_ = (theta_0_ + theta_1_) / 2.0f;

    float deg_;
    if (!is_reverse)
    {
        deg_ = theta_;
    }
    else
    {
        deg_ = 360.0f - theta_;
    }

    return deg_;
}

次のコードで視覚化できます。

import numpy as np
from glob import glob
from vedo import *

path_folder = ".../data/20210203_175550/res_R"
path_R_ALL = sorted(glob(path_folder + "/*_R.txt"))
path_t_ALL = sorted(glob(path_folder + "/*_t.txt"))

o = np.array([0, 0, 0])
x = np.mat([1, 0, 0]).T
y = np.mat([0, 1, 0]).T
z = np.mat([0, 0, 1]).T

vp = Plotter(axes=4)
vp += Box((0, 0, 0), 3, 3, 3, alpha=0.1)
for i, (path_R, path_t) in enumerate(zip(path_R_ALL, path_t_ALL)):
    R = np.loadtxt(path_R)
    R = np.mat(R.reshape(3, 3)).T
    # t = np.loadtxt(path_t)
    # t = np.mat(t).T

    Ax = Line(o, R*x, c="r")
    Ay = Line(o, R*y, c="g")
    Az = Line(o, R*z, c="b")
    vp += Ax
    vp += Ay
    vp += Az
    vp.show(interactive=1)
    vp -= Ax
    vp -= Ay
    vp -= Az

    x_new = R*x
    x_new[1] = 0
    x_new = x_new / np.linalg.norm(x_new)
    # print("x_new:", x_new)

    z_new = R*z
    z_new[1] = 0
    z_new = z_new / np.linalg.norm(z_new)
    # print("z_new:", z_new)

    cos_thetaX = x.T * x_new
    thetaX = np.arccos(cos_thetaX) / 3.14 * 180

    cos_thetaZ = z.T * z_new
    thetaZ = np.arccos(cos_thetaZ) / 3.14 * 180

    # print(x, x_new)
    tmpX = np.cross(x.T, x_new.T)
    # print("tmpX:", tmpX)
    if tmpX[0][1] < 0:
        thetaX = 360 - thetaX

    tmpZ = np.cross(z.T, z_new.T)
    # print("tmpZ:", tmpZ)
    if tmpZ[0][1] < 0:
        thetaZ = 360 - thetaZ
    # print(i, tmpX, tmpZ)
    print(i, thetaX, thetaZ)
于 2021-05-13T03:32:53.603 に答える
1

2Dの場合、Pythonコード。

    import numpy as np
    import matplotlib.pyplot as plt

    def get_random_a(r = 3, centre_x = 5, centre_y = 5):
        angle = np.random.uniform(low=0.0, high=2 * np.pi)
        x = np.cos(angle) * r 
        y = np.sin(angle) * r 

        x += centre_x
        y += centre_y 

        return x, y 

    def norm(x):
        return np.sqrt(x[0] ** 2 + x[1] ** 2) 

    def normalize_vector(x):
        return x / norm(x)



    def rotate_A_onto_B(vector_a, vector_b ):

        A = normalize_vector(vector_a)
        B = normalize_vector(vector_b)

        cos_theta = np.dot(A, B)
        sin_theta = np.cross(A, B)

        theta = np.arctan2(sin_theta, cos_theta)

        M = np.array ( [[np.cos(theta ), -np.sin(theta)],
                        [np.sin(theta), np.cos(theta) ]
                       ])

        M_dash = np.array( [ [cos_theta, -sin_theta], 
                        [sin_theta, cos_theta]
                      ])

        print( f" np all close of M and M_dash : {np.allclose(M, M_dash)}" )

        vector_a = vector_a[:, np.newaxis]
        rotated_vector_a = np.dot(M, vector_a)
        return rotated_vector_a.squeeze()


    #--------------
    #----------------
    centre_x, centre_y = 5, 5
    r = 3 
    b = (centre_x, centre_y - r) 

    vector_b = np.array ( ( b[0] - centre_x, b[1] - centre_y ) )
    x, y = get_random_a(r, centre_x, centre_y)
    vector_a = np.array ( ( x - centre_x, y - centre_y ) )

    rotated_vector_a = rotate_A_onto_B(vector_a, vector_b)
    print("is the answer corrent ? ", np.allclose(rotated_vector_a, vector_b))

    print(rotated_vector_a)

    # plot 
    plt.plot( [centre_x, x], [ centre_y, y ]  )
    plt.plot( [centre_x, b[0]], [centre_y, b[1]] )

    plt.scatter( [centre_x, x, b[0] ], [ centre_y, y, b[1] ], c = "r")

    plt.text(centre_x, centre_y, f"centre : {centre_x, centre_y}")
    plt.text(x, y, f"a : {x, y}")
    plt.text(b[0], b[1], f"b : {b[0], b[1]}")

    plt.xlim(left = centre_x - r - 1, right = centre_x + r + 1 )
    plt.ylim(bottom= centre_y -r - 1 , top = centre_y + r +1 )

    plt.show()
于 2021-05-24T17:35:27.240 に答える
1

回転行列から正確な角度を計算する方法を知りたいと思います。まず、回転の積の結果から(XYZ、ZYZ、ZXZなど)を注文することを決定する必要があります。逆正弦関数によって角度をとることができます。(すでに取った回転行列を使用してください!)

于 2021-07-20T00:55:46.360 に答える