目录
欧拉角转旋转矩阵(右手定则逆时针旋转)
因此，从一个坐标系oxyz 转换到另一个坐标系txyz， 转换矩阵为:
欧拉角转换乘矩阵，一个点(1,0,0) 从一个坐标系转换到另一个坐标系下的例子如下
(自己写的初始例子)
#include <iostream>
#include <cmath>
using namespace std;
/*
作用：2个3x3 矩阵点乘，
输入: 3x3矩阵m1, 3x3矩阵m2
输出: 矩阵m1 点乘 m2 后的3x3 矩阵m_target
*/
void matrixDot(float m1[][3], float m2[][3],float (*m_target)[3] )
{
//float m_target[3][3];
m_target[0][0]=m1[0][0]*m2[0][0]+m1[0][1]*m2[1][0]+m1[0][2]*m2[2][0];
m_target[0][1]=m1[0][0]*m2[0][1]+m1[0][1]*m2[1][1]+m1[0][2]*m2[2][1];
m_target[0][2]=m1[0][0]*m2[0][2]+m1[0][1]*m2[1][2]+m1[0][2]*m2[2][2];
m_target[1][0]=m1[1][0]*m2[0][0]+m1[1][1]*m2[1][0]+m1[1][2]*m2[2][0];
m_target[1][1]=m1[1][0]*m2[0][1]+m1[1][1]*m2[1][1]+m1[1][2]*m2[2][1];
m_target[1][2]=m1[1][0]*m2[0][2]+m1[1][1]*m2[1][2]+m1[1][2]*m2[2][2];
m_target[2][0]=m1[2][0]*m2[0][0]+m1[2][1]*m2[1][0]+m1[2][2]*m2[2][0];
m_target[2][1]=m1[2][0]*m2[0][1]+m1[2][1]*m2[1][1]+m1[2][2]*m2[2][1];
m_target[2][2]=m1[2][0]*m2[0][2]+m1[2][1]*m2[1][2]+m1[2][2]*m2[2][2];
//return m_target;
}
/*
作用：点point 左乘旋转3x3矩阵matrix, 从一个坐标系转换到另一个坐标系的点
输入: 3x3旋转矩阵matrix, 起始点point， 是一个3x1的矩阵
输出: 3x1 目标点target_point
*/
void matrixDot(float matrix[][3], float point[][1],float (*target_point)[1] )
{
//float m_target[3][3];
target_point[0][0]=matrix[0][0]*point[0][0]+matrix[0][1]*point[1][0]+matrix[0][2]*point[2][0];
target_point[1][0]=matrix[1][0]*point[0][0]+matrix[1][1]*point[1][0]+matrix[1][2]*point[2][0];
target_point[2][0]=matrix[2][0]*point[0][0]+matrix[2][1]*point[1][0]+matrix[2][2]*point[2][0];
}
/*
作用: 欧拉角转换成旋转矩阵，顺序是先绕x轴逆时针(正向)旋转roll 角， 再绕y 轴逆时针旋转pitch 角
最后绕z轴逆时针旋转yaw 角，注意必须按顺序旋转，顺序不同，结果不一样
参数：单位弧度，绕x轴旋转的roll 角，绕y轴旋转的pitch角，绕z轴旋转的yaw 角
输出: 3x3 的旋转矩阵，某个点通过左乘旋转矩阵，从一个坐标系下转换到另一个坐标系下的点。
*/
void eulerToMatrix3x3(float yaw, float pitch,float roll,float(*c_target)[3])
{
//绕x 轴逆时针旋转矩阵
float c_roll[3][3];
c_roll[0][0]=1; c_roll[0][1]=0; c_roll[0][2]=0;
c_roll[1][0]=0; c_roll[1][1]=cos(roll); c_roll[1][2]=sin(roll);
c_roll[2][0]=0; c_roll[2][1]=-sin(roll); c_roll[2][2]=cos(roll);
//绕y 轴逆时针旋转矩阵
float c_pitch[3][3];
c_pitch[0][0]=cos(pitch); c_pitch[0][1]=0; c_pitch[0][2]=-sin(pitch);
c_pitch[1][0]=0; c_pitch[1][1]=1; c_pitch[1][2]=0;
c_pitch[2][0]=sin(pitch); c_pitch[2][1]=0; c_pitch[2][2]=cos(pitch);
//绕z 轴逆时针旋转矩阵
float c_yaw[3][3];
c_yaw[0][0]=cos(yaw); c_yaw[0][1]=sin(yaw); c_yaw[0][2]=0;
c_yaw[1][0]=-sin(yaw); c_yaw[1][1]=cos(yaw); c_yaw[1][2]=0;
c_yaw[2][0]=0; c_yaw[2][1]=0; c_yaw[2][2]=1;
float c_tmp[3][3]={0};
matrixDot(c_pitch,c_roll,c_tmp);
//float c_target[3][3]={0};
matrixDot(c_yaw,c_tmp,c_target);
//return c_target;
}
int main()
{
//设置初始点是(1,0,0)
float point[3][1];
point[0][0]=1; point[1][0]=0; point[2][0]=0;
#if 0 //绕y轴逆时针旋转-90度，(1,0,0)——>(0,0,-1)
float matrix[3][3];
float yaw=0.0;
float pitch=-M_PI_2;
float roll =0.0;
eulerToMatrix3x3(yaw,pitch,roll,matrix);
float target_point[3][1];
matrixDot(matrix,point,target_point);
cout << "target_point=(" <<target_point[0][0]<<","
<<target_point[1][0]<<","<<target_point[2][0]<<")"<< endl;
#endif
#if 0 //绕y轴逆时针旋转90度，(1,0,0)——>(0,0,1)
float matrix[3][3];
float yaw=0.0;
float pitch=M_PI_2;
float roll =0.0;
eulerToMatrix3x3(yaw,pitch,roll,matrix);
float target_point[3][1];
matrixDot(matrix,point,target_point);
cout << "target_point=(" <<target_point[0][0]<<","
<<target_point[1][0]<<","<<target_point[2][0]<<")"<< endl;
#endif
//绕x轴逆时针旋转90度,绕y轴旋转0度，再绕z轴逆时针旋转90度，(1,0,0)——>(0,-1,0)
float matrix[3][3];
float yaw=M_PI_2;
float pitch=0.0;
float roll =M_PI_2;
eulerToMatrix3x3(yaw,pitch,roll,matrix);
float target_point[3][1];
matrixDot(matrix,point,target_point);
cout << "target_point=(" <<target_point[0][0]<<","
<<target_point[1][0]<<","<<target_point[2][0]<<")"<< endl;
return 0;
}欧拉角转旋转矩阵(左手定则顺时针旋转)
在ROS geometry-indigo-devel tf 转换中，欧拉角 转换到 旋转矩阵用的是顺时针旋转（左手定则）
原理如下:
ros代码实现如下
/** @brief Set the matrix from euler angles YPR around ZYX axes
* @param eulerZ Yaw aboud Z axis
* @param eulerY Pitch around Y axis
* @param eulerX Roll about X axis
*
* These angles are used to produce a rotation matrix. The euler
* angles are applied in ZYX order. I.e a vector is first rotated
* about X then Y and then Z
旋转矩阵是: 按顺时针旋转，左手定则， (刚好和右手定则相反)
cosr*cosy , sinp*sinr*cosy - cosr*siny , sinp*cosr*cosy + sinr*siny
cosp*siny , sinp*sinr*siny + cosr*cosy , sinp*cosr*siny - sinr*cosy
-sinp , cosp*sinr , cosp*cosr
**/
void setEulerYPR(tfScalar eulerZ, tfScalar eulerY,tfScalar eulerX) {
tfScalar ci ( tfCos(eulerX));
tfScalar cj ( tfCos(eulerY));
tfScalar ch ( tfCos(eulerZ));
tfScalar si ( tfSin(eulerX));
tfScalar sj ( tfSin(eulerY));
tfScalar sh ( tfSin(eulerZ));
tfScalar cc = ci * ch;
tfScalar cs = ci * sh;
tfScalar sc = si * ch;
tfScalar ss = si * sh;
setValue(cj * ch, sj * sc - cs, sj * cc + ss,
cj * sh, sj * ss + cc, sj * cs - sc,
-sj, cj * si, cj * ci);
}
void setValue(const tfScalar& xx, const tfScalar& xy, const tfScalar& xz,
const tfScalar& yx, const tfScalar& yy, const tfScalar& yz,
const tfScalar& zx, const tfScalar& zy, const tfScalar& zz)
{
m_el[0].setValue(xx,xy,xz);
m_el[1].setValue(yx,yy,yz);
m_el[2].setValue(zx,zy,zz);
}旋转矩阵转欧拉角
/*
* 作用： 通过旋转矩阵，解算出欧拉角yaw,pitch, roll
*/
void getEulerYPR(double& yaw, double& pitch, double& roll) const
{
pitch = -asin(m_el[2].x());
roll = atan2(m_el[2].y()/cos(pitch), m_el[2].z()/cos(pitch));
yaw = atan2(m_el[1].x()/cos(pitch), m_el[0].x()/cos(pitch));
}
四元数转旋转矩阵
推理过程:
旋转矩阵转四元数
已知旋转矩阵：
则求解四元数时根据的方法就是从四元数转旋转矩阵的公式中得到：
但从上式中是无法确定正负号的，所以又有：
这样只要得到q0到q4中的任意一个就能根据上面的关系求出剩余3个分量的值，假设我们先求q0的值，则有：
从上式中可以看到，求得的四元数有两个，但他们表示的是同一种旋转关系，至于先求q0到q4中的哪个值，在实际使用时应该全部一起求，看哪个值大，就选取哪个，以防止某一项在出现0时无法计算的情况
四元数转欧拉角
已知四元数：
欧拉角为：
但是当β角度为90度时，四元数反向计算欧拉角时会出现奇点，是无法计算的。因为这时候简化后的四元数是这样的：
所以atan2中后面那一项就变成了0：
这时候我们通常令α=0，然后解出欧拉角的值
欧拉角转四元数
具体推导如下图:
四元数球面线性插值
具体原理参考: https://www.cnblogs.com/21207-iHome/p/6952004.html
666
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
