【从理论到代码】旋转矩阵与欧拉角一

本篇主要是结合odom坐标系与相机坐标系之间的转换，可以用于将odom数据与视觉slam进行融合时的位姿计算；

主要分为两部分，第一部分讲述旋转矩阵与欧拉角之间的转换；第二部分讲述如何将odom的位移和角度转换到相机坐标系下；

假设空间中的任意一点

绕Z轴旋转了度，那么求旋转后的坐标，这里我直接给出自己的推导:

假设旋转之后的点为，A点与X轴的夹角为beta，A点到原点的距离为L，那么我们可以得出以下几条式子：

按照三角函数的倍角公式展开代入L和的表达式化简之后有

写出矩阵形式有

尽管图示中仅仅表示的是旋转一个锐角的情形，但是我们推导中使用的是三角函数的基本定义来计算坐标的，因此当旋转的角度是任意角度（例如大于180度，导致A’点进入到第四象限）结论仍然是成立的。

同理可得其他两个轴的旋转矩阵如下

绕x轴旋转的alpha矩阵：

绕y轴旋转的alha矩阵：

由角度值转换成旋转矩阵的过程中需要注意两种情况，一种是绕固定轴旋转，一种是绕动态轴旋转。

以slam中的机器人的odom坐标系和相机坐标系为例：

这两种坐标系的关系图如下所示：（两个坐标系都符合右手法则）

以机器人的前进方向来说，当机器在向前方行进时，在坐标系中表示，即为向odom的x方向前进，向相机坐标系的z轴方向前进。

同理，odom坐标系的y轴正方向，与相机坐标系的x轴整方向差一个负号，表示成公式即为（不考虑平移）：

cam.x = -odom.y;
cam.y = -odom.z;
cam.z = odom.x;

这种从odom坐标系到cam坐标系的关系用固定轴和动态轴旋转方式表示分别如下：

对应在ORBSLAM中R矩阵的书写方式为Roc，即从odom坐标系旋转到cam坐标系

固定轴：严格按照[x,y,z]的顺序：先绕odom坐标系的x轴逆时针旋转90度，再绕odom坐标系的y轴旋转0度，再绕odom坐标系的z轴旋转-90度；

动轴：严格按照[z,y,x]的顺序：线绕odom的坐标系的z轴逆时针旋转-90度，再绕新odom的y轴旋转0度，再绕新odom的x轴逆时针旋转90度；

每一次旋转都会生成一个旋转矩阵，在拼接成最终的旋转矩阵时的顺序，都是Rotation = rotationZ*rotaionY*rotaionX;

因此，最后的R= [0, 0, 1; -1, 0, 0; 0,-1,0]

用Eigen来生成这个矩阵就是： (这里需要注意，在Eigen中传入的值是弧度值，而非角度值，所以要将-90 度转换为 -90.0*M_PI/180)

Eigen::Vector3d xyztheta(-90.0*M_PI/180, 0 ,-90*M_PI/180);
Eigen::Matrix<double,3,3> Reigen;
Reigen = Eigen::AngleAxisd(xyzangles[2],Eigen::Vector3d::UnitZ()) *Eigen::AngleAxisd(xyzangles[1],Eigen::Vector3d::UnitY())*Eigen::AngleAxisd(xyzangles[0],Eigen::Vector3d::UnitX());

这样计算出来的旋转矩阵

同理，如果已知旋转矩阵，想要转换成物体旋转各个表示方法之间的转换举例：

以cam1 为参考，其R矩阵为单位阵， cam2相对与cam1之间的旋转矩阵为RotationMatrix，如下：

RotationMatrix << 0.539026 ,- 0.0785835, 0.838615,0.0609289 ,0.996668, 0.0542316,- 0.840082, 0.0218637, 0.542018;
Eigen::Vector3d euler_angles=RotationMatrix.eulerAngles(2,1,0);std::cout << "yaw(Z) pitch(Y) roll(X)=\n" << euler_angles.transpose() << std::endl;

将旋转矩阵转换成为欧拉角之后 yaw(Z) pitch(Y) roll(X)=0.112557 0.997435 0.0403156
这三个数分别代表了cam2绕cam1的各个轴之间的旋转弧度值，其范围为-Π<x<Π ，因此实际的角度值分别为0.112557/pi*180; 0.997435/pi*180 0.0403156/pi*180

第二部分: 利用两个时刻odom的位姿差估算相机的位姿

其中b1为t1时刻的odom坐标，b2为t2时刻的odom坐标

/*
mTcw_1: t1时刻相机的位姿
DR_del_th : t2时刻的角度 - t1时刻的角度
DR_del_x: t2时刻的x值 - t1时刻的x值， DR_del_y同理
*/void Tracking::TrackWithDR()
{cv::Mat Tc1w = mTcw_1.clone();cv::Mat Tb2b1;cv::Mat Tb1b2 = cv::Mat::eye(4,4,CV_32F);cv::Mat Rb1b2 = cv::Mat::eye(3,3,CV_32F);cv::Mat tb2inb1(3,1,CV_32F);float dth_rad = DR_del_th; Rb1b2.at<float>(0,0) = cos(dth_rad);Rb1b2.at<float>(0,1) = -sin(dth_rad);Rb1b2.at<float>(1,0) = sin(dth_rad);Rb1b2.at<float>(1,1) = cos(dth_rad);cout << "Rb1b2: " << Rb1b2 << endl;float d = sqrt(DR_del_x*DR_del_x + DR_del_y*DR_del_y);float thc = atan2(DR_del_y, DR_del_x) - DR_th;float d_xr = d * cos(thc); //在baselink1的坐标系中baselink2的位移float d_yr = d * sin(thc);tb2inb1.at<float>(0) = d_xr;tb2inb1.at<float>(1) = d_yr;tb2inb1.at<float>(2) = 0;Rb1b2.copyTo(Tb1b2.rowRange(0,3).colRange(0,3));tb2inb1.copyTo(Tb1b2.rowRange(0,3).col(3));cout << "Tb1b2: " << Tb1b2 << endl;Tb2b1 = Tb1b2.inv();cout << "Tb2b1: " << Tb2b1 << endl;cv::Mat Rbc(3,3,CV_32F);float rot_x = -90.0*M_PI/180;float rot_y = 0;float rot_z = -90.0*M_PI/180;Eigen::Vector3d xyzangles(rot_x,rot_y,rot_z);Eigen::Matrix<double,3,3> Rbc_Eigen = Converter::angletocvMat(xyzangles);Eigen::Matrix<double,3,1> tcinb(mDistCamFromCenter_h,0,mDistCamFromCenter_v);cv::Mat Tbc = Converter::toCvSE3(Rbc_Eigen,tcinb);cout << "Tbc: " << Tbc << endl;cv::Mat Tb1w = Tbc* Tc1w; cv::Mat Tb2w = Tb2b1*Tb1w;cv::Mat Tc2w = Tbc.inv()*Tb2w;cout << "Tc2w: " << Tc2w << endl; mTcw_1 = Tc2w.clone();mDR_Tcw = mTcw_1.clone();cout << "new set mDR_Tcw " << mDR_Tcw << endl;mpMapDrawer->SetCurrentCameraPose(mDR_Tcw);}

【从理论到代码】旋转矩阵与欧拉角一相关推荐

旋转矩阵与欧拉角的相互转换及代码
这篇博客将会分享旋转矩阵和欧拉角的相互转换. 三维旋转矩阵有三个自由度,旋转能够使用多种方法表示(旋转矩阵,欧拉角,四元数,轴角,李群与李代数),比如一个3x3的矩阵,比如四元数,甚至可以将旋转表示为 ...
旋转矩阵、欧拉角、四元数理论及其转换关系
旋转矩阵.欧拉角.四元数理论及其转换关系 author@jason_ql(lql0716) http://blog.csdn.net/lql0716 1.概述旋转矩阵.欧拉角.四元数主要用于表示坐标 ...
旋转矩阵与欧拉角之间的转换
简介: 对于欧拉角与旋转矩阵之间的转换公式和程序实现进行了测试.也显示了这其中的转换关系的复杂性,来自于欧拉角的方向.范围.转换顺序.这在实际应用中需要特别的关注. 关键词: 欧拉角,旋转矩阵 #m ...
旋转矩阵、欧拉角、四元数比较
旋转矩阵.欧拉角.四元数主要用于:向量的旋转.坐标系之间的转换.角位移计算.方位的平滑插值计算. 不同的方位表示方法适用于不同的情况.下面是我们对合理选择格式的一些建议: l 欧拉角最容易使用.当需 ...
旋转矩阵、欧拉角、旋转矢量及四元数的介绍和工程应用
本篇文章首发于微信公众号:无人机开发.更多无人机技术相关文章请关注此公众号,有问题也可在公众号底部添加个人微信进行交流. 1.前言从事导航.制导或者控制时,经常需要将各个物理矢量从A坐标系转换至B坐 ...
旋转矩阵转欧拉角（二自由度约束）
旋转矩阵转欧拉角 1 关于欧拉角 2 转换公式推导 2.1 由欧拉角构造旋转矩阵 2.2 由旋转矩阵推算欧拉角 2.2.1 一般情况 2.2.2 约束滚转角 3 转换代码(C++) 3.1 欧拉角-- ...
matlab 欧拉角方向余弦,旋转矩阵、欧拉角之间转换
学习过程中涉及欧拉角和旋转矩阵的转换,索性整理学习一下欧拉角四元数和旋转矩阵的概念以及matlab中的互相转换本文摘自各大课本,博客,自己学习整理使用,侵删 MATLAB矩阵乘法从左到右依次相乘用 ...
从PointNet到PointNet++理论及代码详解
从PointNet到PointNet++理论及代码详解 1. 点云是什么 1.1 三维数据的表现形式 1.2 为什么使用点云 1.3 点云上以往的相关工作 2. PointNet 2.1 基于点云的置 ...
车辆姿态表达：旋转矩阵、欧拉角、四元数的转换以及eigen、matlab、pathon方法实现
目录 1 概述 2 原理 2.1 旋转矩阵 2.1.1 绕x轴旋转 2.1.2 绕y轴旋转 2.1.3 绕z轴旋转 2.2 欧拉角 2.2.1 基本思想 2.2.2 欧拉角的缺点 2.3 四元数 2. ...

【从理论到代码】旋转矩阵与欧拉角一

【从理论到代码】旋转矩阵与欧拉角一相关推荐

最新文章

热门文章

【从理论到代码】旋转矩阵与欧拉角 一

【从理论到代码】旋转矩阵与欧拉角 一相关推荐

最新文章

热门文章

【从理论到代码】旋转矩阵与欧拉角一

【从理论到代码】旋转矩阵与欧拉角一相关推荐