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

公式

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;}

旋转向量与旋转矩阵的相互转化相关推荐

方向向量转欧拉角_【姿态表示】旋转向量、旋转矩阵、欧拉角、四元数
1. 旋转矩阵与旋转向量旋转矩阵(Rotation Matrix)用 9 个量描述旋转的3个自由度,有冗余: 9 个量是有约束的:必须是正交矩阵,且行列式为 1 旋转向量(Rotation Vecto ...
罗德里格斯公式推导（轴角与旋转矩阵的关系）以及四元数与旋转向量、旋转矩阵、欧拉角之间的转换关系
罗德里格斯公式推导(轴角与旋转矩阵的关系) 意义:罗德里格斯公式表示旋转向量到旋转矩阵之间爱你的转换关系旋转向量:一个向量,方向与旋转轴一致,长度等于旋转角度空间中任意旋转都可以用一个旋转轴和一个 ...
C++ Eigen 库中旋转向量、旋转矩阵、欧拉角、四元数的定义及互相转换
今天看师兄写的使用力反馈设备操作机械臂的代码,里边涉及到了Eigen 库中的旋转变换,表征旋转变换的有旋转向量Eigen::AngleAxisd.欧拉角Eigen::Vector3d.旋转矩阵Eige ...
罗德里格斯（Rodrigues）旋转向量转旋转矩阵
转载于:https://blog.csdn.net/qq_22235957/article/details/80461290 旋转向量中V的单位向量代表方向,模代表角度,通过罗德里格斯方程可以将旋转向 ...
详解基于罗德里格斯（Rodrigues）公式由旋转向量到旋转矩阵的 Python 实现
文章目录旋转向量 rotation vector 旋转矩阵 rotation matrix 罗德里格斯公式 Rodrigues' formula 基于 Python 和 NumPy 实现 Rodri ...
机械臂 tcp 坐标转旋转矩阵（旋转向量转旋转矩阵）
C++ Eigen版本 #include <Eigen/Core> #include <Eigen/Eigen> #include <Eigen/Geometry> ...
python 实现罗德里格斯公式Rodrigues 旋转向量到旋转矩阵转化
罗德里格斯公式Rodrigues: 代码: import numpy as npdef rodrigues_rotation(r, theta):# n旋转轴[3x1]# theta为旋转角度# 旋转 ...
从旋转向量到旋转矩阵的转化公式
这个博客是公式详解: https://www.cnblogs.com/xpvincent/archive/2013/02/15/2912836.html Eigen::Matrix3d constru ...
罗德里格斯公式推导，以及如何使用cv2.Rodrigues进行旋转矩阵和旋转向量之间的相互转化
罗德里格斯公式推导,以及如何使用cv2.Rodrigues进行旋转矩阵和旋转向量之间的相互转化 1 罗德里格斯公式推导 2 cv2.Rodrigues进行旋转矩阵和旋转向量之间的相互转化 1 罗德里格 ...

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

公式

代码

旋转向量与旋转矩阵的相互转化相关推荐

最新文章

热门文章