opencv 如何从旋转矩阵计算Angular

kadbb459  于 2023-01-05  发布在  Angular
关注(0)|答案(9)|浏览(222)

我使用两个图像的单一对象的对象是旋转一定程度上从它的第一个图像。
我已经计算了每张图像的POSE,并使用Rodergues()将旋转矢量转换为Matrix。现在我如何计算并查看它从第一个位置旋转了多少?
我试过很多方法,但答案都很接近
编辑:我的相机是固定的,只有物体在移动。

mum43rcc

mum43rcc1#

我们可以用下面的公式从旋转矩阵得到欧拉角。
给定一个3×3旋转矩阵

3个欧拉角为

这里atan 2是相同的反正切函数,带有象限检查,通常在C或Matlab中找到。
注意:如果绕y轴的Angular 正好为+/-90°,则必须注意。在这种情况下,第一列和最后一行中的所有元素(除了下角的元素,它是1或-1)都将为0(cos(1)=0)。一种解决方案是将绕x轴的旋转固定为180°,并根据下式计算绕z轴的Angular :atan2(r_12,-r_22)。
另请参见https://www.geometrictools.com/Documentation/EulerAngles.pdf,其中包括六个不同阶数的欧拉角的实现。

p3rjfoxz

p3rjfoxz2#

如果R是(3x 3)旋转矩阵,则旋转Angular 将为acos((tr(R)-1)/2),其中tr(R)是矩阵的迹(即对角线元素之和)。
这是你要求的我估计有90%的可能不是你想要的。

ozxc1zmp

ozxc1zmp3#

我想在这里发表一点意见,因为我也在研究同样的问题。我通过发布一个纯Python实现来将3-D旋转矩阵(3x3)转换为相应的滚动(Rx)、俯仰(Ry)、偏航(Rz)Angular ,从而为上述答案增添价值。

    • 参考问题设置:**假设我们有一个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]

希望这对其他的程序员有帮助!

qacovj5a

qacovj5a4#

以下代码在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];

代码由Graham Taylor、Geoff欣顿和Sam Roweis提供。有关详细信息,请参见here

fdbelqdn

fdbelqdn5#

假设R1c和R2c是你计算的2个旋转矩阵,它们分别表示从姿态1和2的物体到相机帧的旋转(因此有第二个后缀c)。您需要的旋转矩阵是从pose 1到pose 2,即R12。要计算它,您必须在头脑中旋转对象,使其从pose_1到camera,然后从摄像机旋转到pose_2。后者的旋转与R2c所表示的pose_2到摄像机的旋转相反,因此:
第一个月
从矩阵R12中,您可以使用Rodiguez公式计算Angular 和旋转轴。

v8wbuo2f

v8wbuo2f6#

假设它主要是绕某个坐标轴旋转(一个人站在摄像头前,绕着旋转得到旋转矩阵),试试下面的代码:

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)
fjaof16o

fjaof16o7#

对于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()
00jrzges

00jrzges8#

我猜你想知道如何从旋转矩阵计算精确的Angular 。首先,你应该决定命令(XYZ,ZYZ,ZXZ等)从旋转的乘积的结果,你可以采取反正弦函数的Angular 。(使用旋转矩阵你已经采取了!)

pepwfjgg

pepwfjgg9#

对于Matlab用户,有一个内置函数:

rot2m2eul()

它是inverse

eul2rotm()

相关问题