旋转向量与旋转矩阵的相互转化

公式

1. 旋转向量求解旋转矩阵

当刚体在三维空间中运动时,如果已知旋转向量,根据罗德里格斯公式是比较容易求得旋转矩阵的.
罗德里格斯公式如图所示

其中,I 是单位矩阵,n 是旋转向量的单位向量, theta是旋转向量的模长.

2. 旋转矩阵求解旋转向量
如果已知旋转矩阵,求解旋转向量时,theta是比较容易求解的.根据上图,对等式两端取迹便可以得到旋转向量的模长


记旋转向量的单位向量为 r(rx, ry, rz) ,通过下图公式便可求解得出 r 向量的反对成矩阵,即可得出 r 向量

代码

这里用代码简单的求解一下

#include <iostream>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

int main() 
{
    double angle = M_PI / 6.;   //X轴向的转角
    AngleAxisd angleAxisd(angle, Vector3d(1, 0, 0));
    
    {
        cout << "根据旋转向量求解旋转矩阵..." << endl;
        double theta = angleAxisd.angle();
        Vector3d n = angleAxisd.axis();
        
        cout<< "theta is " << theta << endl;
        cout<< "n is " << n << endl;

        Matrix3d n_hat;
        n_hat << 0, -n[2], n[1],
                n[2], 0, -n[0],
                -n[1], n[0], 0;
        
        Matrix3d R_solved = cos(theta) * Matrix3d::Identity() + (1 - cos(theta)) * n * n.transpose() + sin(theta) * n_hat;
        cout << "R_solved is " << R_solved << endl;
    }

用OpenCV Documention上的方法求解旋转向量
点击此处搜索Rodrigues

    {
        cout<< "根据旋转矩阵求解旋转向量..." << endl;
        Matrix3d R = angleAxisd.toRotationMatrix();
        double theta = acos((R.trace() - 1) * 0.5);	//待求的旋转向量的模长

        Matrix3d Right = (R - R.transpose()) * 0.5 / sin(theta);
        Vector3d r;	//待求的旋转向量的单位向量
        r[0] = (Right(2,1) - Right(1,2))*0.5;
        r[1] = (Right(0,2) - Right(2,0))*0.5;
        r[2] = (Right(1,0) - Right(0,1))*0.5;
        cout<<"angle is "<<theta<<"\nn is "<< r <<endl;
    }
    
    return 0;
}

用求特征值为1对应的特征向量方法求解旋转向量

    {
        cout<< "根据旋转矩阵求解旋转向量..." << endl;
        //此处RR和n是相互对应的,用所求的旋转单位向量同n对比验证
        Matrix3d RR;
        RR << -0.036255, 0.978364, -0.203692,
                0.998304,0.026169, -0.051994,
                -0.045539,-0.205232,-0.977653;
        Vector3d n(-2.100418,-2.167796,0.273330);
        double theta = acos((RR.trace() - 1) * 0.5);

        EigenSolver<Matrix3d> solver(RR);
        Matrix3cd vectors = solver.eigenvectors();
        Vector3cd values = solver.eigenvalues();
        cout<<"vectors is "<<vectors<<"\n values is "<<values<<endl;
        
        n.normalize();
        cout<<"\nn is "<<n<<endl;

        Vector3d x;
        for (int i = 0; i < 3; ++i)
        {
            if(abs(values[i].real()-1.)<1e-5)
            {
                x(0) = vectors(0,i).real();
                x(1) = vectors(1,i).real();
                x(2) = vectors(2,i).real();
                x.normalize();
                break;
            }
        }
        cout<<"\n所求的旋转单位向量x is "<<x<<endl;
        cout<<"验证(R*x = x) is "<<RR.cast<double>()*x<<endl;
    }