四元数、罗德里格斯公式、欧拉角、旋转矩阵推导和资料

写在前面

1、本文内容
四元数、罗德里格斯公式、欧拉角、旋转矩阵推导和资料
2、转载请注明出处：
https://blog.csdn.net/qq_41102371/article/details/126002167

资料

四元数
Understanding Quaternions 中文翻译《理解四元数》 https://www.qiujiawei.com/understanding-quaternions/
http://mars.cs.umn.edu/tr/reports/Trawny05b.pdf
几种三维空间旋转的表达方式转换
三维旋转：欧拉角、四元数、旋转矩阵、轴角之间的转换 https://zhuanlan.zhihu.com/p/45404840
彻底搞懂“旋转矩阵/欧拉角/四元数”，让你体会三维旋转之美 https://blog.csdn.net/weixin_45590473/article/details/122884112
展示欧拉角与四元数动态变换关系的网站
https://quaternions.online/

罗德里格斯公式

https://en.wikipedia.org/wiki/Rodrigues’_rotation_formula
罗德里格斯公式（Rodrigues Formula） https://blog.csdn.net/weixin_40215443/article/details/123950141
二维xy坐标旋转 https://blog.csdn.net/qq_41102371/article/details/116245483#t4

推导

先放这，有空来推一遍

几种表达方式

$K$是$k$的叉乘矩阵，即$\mathbf{K}={\mathbf{k}}^{\wedge }$,($\mathbf{k}$的叉乘矩阵也可表示为${\mathbf{k}}_{\times }$)
1、
$$\begin{array}{rl}\mathbf{R}& =\mathbf{I}+(1-\cos \theta ){\mathbf{K}}^2+(\sin \theta )\mathbf{K}\\ & =\mathbf{I}+(1-\cos \theta )[{\mathbf{k}}^{\wedge }{]}^2+(\sin \theta ){\mathbf{k}}^{\wedge }\end{array}$$
2、
由${\mathbf{K}}^2={\mathbf{k}}^{\wedge }{\mathbf{k}}^{\wedge }=\mathbf{k}{\mathbf{k}}^T-\mathbf{I}$
可得
$$\begin{array}{rl}\mathbf{R}& =\mathbf{I}+(1-\cos \theta )(\mathbf{k}{\mathbf{k}}^T-\mathbf{I})+(\sin \theta ){\mathbf{k}}^{\wedge }\\ & =\cos \theta \mathbf{I}+(1-\cos \theta )\mathbf{k}{\mathbf{k}}^T+(\sin \theta ){\mathbf{k}}^{\wedge }\end{array}$$
3、
高博的14讲中推导了指数映射

$$\mathbf{R}=exp(\theta {\mathbf{k}}^{\wedge })=\cos \theta \mathbf{I}+(1-\cos \theta )\mathbf{k}{\mathbf{k}}^T+(\sin \theta ){\mathbf{k}}^{\wedge }$$

四元数转旋转矩阵

根据http://mars.cs.umn.edu/tr/reports/Trawny05b.pdf，四元数转旋转矩阵为：

$$\begin{array}{rl}{}_G^L\mathbf{C}(\bar{q})& =(2q_4^2-1){\mathbf{I}}_{3\times 3}-2q_4[{\mathbf{q}}_{\times }]+2\mathbf{q}{\mathbf{q}}^T\\ & =(2q_4^2-1){\mathbf{I}}_{3\times 3}-2q_4[{\mathbf{q}}_{\times }]+2[{\mathbf{q}}_{\times }{]}^2+2|\mathbf{q}{|}^2\cdot {\mathbf{I}}_{3\times 3}\\ & =(2q_4^2+2|\mathbf{q}{|}^2-1){\mathbf{I}}_{3\times 3}-2q_4[{\mathbf{q}}_{\times }]+2[{\mathbf{q}}_{\times }{]}^2\\ & =(2q_4^2+2(q_1^2+q_2^2+q_3^2)-1){\mathbf{I}}_{3\times 3}-2q_4[{\mathbf{q}}_{\times }]+2[{\mathbf{q}}_{\times }{]}^2\\ & =(2-1){\mathbf{I}}_{3\times 3}-2q_4[{\mathbf{q}}_{\times }]+2[{\mathbf{q}}_{\times }{]}^2\\ & ={\mathbf{I}}_{3\times 3}-2q_4[{\mathbf{q}}_{\times }]+2[{\mathbf{q}}_{\times }{]}^2\end{array}$$
其中$\bar{q}=\left[\begin{array}{c}\mathbf{q}\\ q_4\end{array}\right]=[q_1,q_2,q_3,q_4{]}^T$，$\mathbf{q}=[q_1,q_2,q_3{]}^T$，$q_1^2+q_2^2+q_3^2+q_4^2=1$
则
$${}_G^{L}R=\left[\begin{array}{ccc}1-2q_2^2-2q_3^2& 2(q_1{q}_2+q_3{q}_4)& 2(q_1{q}_3-q_2{q}_4)\\ 2(q_1{q}_2-q_3{q}_4)& 1-2q_1^2-2q_3^2& 2(q_2{q}_3+q_1{q}_4)\\ 2(q_1{q}_3+q_2{q}_4)& 2(q_2{q}_3-q_1{q}_4)& 1-2q_1^2-2q_2^2\end{array}\right]$$
即将世界坐标系下的点$P^G$旋转到局部坐标系下的点$P^L$
$$P^L={}_G^{L}R{P}^G$$
而
三维旋转：欧拉角、四元数、旋转矩阵、轴角之间的转换 https://zhuanlan.zhihu.com/p/45404840中是将局部坐标系下的点$P^L$转换到世界坐标系下的$P^G$：
$${}_L^{G}R=\left[\begin{array}{ccc}1-2q_2^2-2q_3^2& 2(q_1{q}_2-q_3{q}_4)& 2(q_1{q}_3+q_2{q}_4)\\ 2(q_1{q}_2+q_3{q}_4)& 1-2q_1^2-2q_3^2& 2(q_2{q}_3-q_1{q}_4)\\ 2(q_1{q}_3-q_2{q}_4)& 2(q_2{q}_3+q_1{q}_4)& 1-2q_1^2-2q_2^2\end{array}\right]$$
$$P^G={}_L^{G}R{P}^L$$
这正好满足：
$${}_G^{L}R={{}_L^{G}R}^T({}_G^{L}R={{}_L^{G}R}^{-1})$$

近似

根据http://mars.cs.umn.edu/tr/reports/Trawny05b.pdf原文当$\delta \bar{q}$非常小时

以下为个人理解的推导
$$\begin{array}{rl}{}_G^L\mathbf{C}(\bar{q})& =(2q_4^2-1){\mathbf{I}}_{3\times 3}-2q_4[{\mathbf{q}}_{\times }]+2q{q}^T\\ & \approx (2\times 1^2-1){\mathbf{I}}_{3\times 3}-2\times 1[\frac{1}{2}\delta {\theta }_{\times }]+2\times 0_{3\times 3}\\ & ={\mathbf{I}}_{3\times 3}-[\delta {\theta }_{\times }]\end{array}$$
其中$q{q}^T\approx 0_{3\times 3}$

四元数与轴角

还是参考http://mars.cs.umn.edu/tr/reports/Trawny05b.pdf，

$$\bar{q}=\left[\begin{array}{c}\mathbf{q}\\ q_4\end{array}\right]=\left[\begin{array}{c}{q}_1\\ q_2\\ q_3\\ q_4\end{array}\right]=\left[\begin{array}{c}{k}_x\sin (\theta /2)\\ k_y\sin (\theta /2)\\ k_z\sin (\theta /2)\\ \cos (\theta /2)\end{array}\right]=\left[\begin{array}{c}\hat{\mathbf{k}}\sin (\theta /2)\\ \cos (\theta /2)\end{array}\right],\hat{\mathbf{k}}=[k_x,k_y,k_z{]}^T,q_4=\cos (\theta /2)$$

转换代码

欧拉角转旋转矩阵

#include <Eigen/Core>
#include <Eigen/Dense>
#include <iostream>
#define PI 3.1415926

int main(int argc, char* argv[]){
    
    
    std::cout<<PI<<std::endl;
    if(argc<4){
    
    
        std::cout<<"please input a 3x1 vector,for example:\neuler2rt 45 30 60"<<std::endl;
        return 0;  
    }
    
    Eigen::Vector3d eulerAngle(atof(argv[1]),atof(argv[2]),atof(argv[3]));
    std::cout<<"eulerAngle:\nx: "<<eulerAngle[0]<<"    y: "<<eulerAngle[1]<<"    z: "<<eulerAngle[2]<<std::endl;
    eulerAngle=eulerAngle/180*PI;


    Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();
        Eigen::AngleAxisd rollAngle(Eigen::AngleAxisd(eulerAngle[0],Eigen::Vector3d::UnitX()));
    Eigen::AngleAxisd pitchAngle(Eigen::AngleAxisd(eulerAngle[1],Eigen::Vector3d::UnitY()));
    Eigen::AngleAxisd yawAngle(Eigen::AngleAxisd(eulerAngle[2],Eigen::Vector3d::UnitZ()));


    rotation_matrix=rollAngle*pitchAngle*yawAngle;

    std::cout<<"rotation_matrix:\n"<<rotation_matrix<<std::endl;
    Eigen::Vector3d eulerAngle2=rotation_matrix.eulerAngles(0,1,2);
     std::cout<<"eulerAngle:\nx: "<<eulerAngle2[0]/PI*180<<"    y: "<<eulerAngle2[1]/PI*180<<"    z: "<<eulerAngle2[2]/PI*180<<std::endl;

    return 0;
}

使用

erluer2rt 30 45 60

旋转矩阵转欧拉角和四元数

#include <Eigen/Core>
#include <Eigen/Dense>
#include <iostream>
#include <fstream>
#define PI 3.1415926

int main(int argc, char* argv[]){
    
    
    if(argc<2){
    
    
        std::cout<<"please input a file including a 3x3 matrix"<<std::endl;
        return 0;  
    }
    std::string filename;
    filename=argv[1];  
    std::ifstream fin;
    fin.open(filename,std::ios::in);
    if(!fin.is_open()){
    
    
        std::cout<<"read file "<<filename<<" failed."<<std::endl;
        return 0;
    }
    Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();

    for (std::size_t i = 0; i < 3; ++i) {
    
    
        for (std::size_t j = 0; j < 3; ++j) {
    
    
            double value;
            fin >> value;
            // std::cout<<value<<" ";
            rotation_matrix(i, j) = value;
        }
    }
    std::cout<<std::endl;
	fin.close();

    std::cout<<"rotation_matrix:\n"<<rotation_matrix<<std::endl;
    Eigen::Vector3d eulerAngle=rotation_matrix.eulerAngles(0,1,2);
    // std::cout<<eulerAngle<<std::endl;
    std::cout<<"eulerAngle:\nx: "<<eulerAngle[0]/PI*180<<"    y: "<<eulerAngle[1]/PI*180<<"    z: "<<eulerAngle[2]/PI*180<<std::endl;

	//转四元数
    Eigen::Quaterniond rotation_q(rotation_matrix);
    std::cout<<"rotation_q:\nx: "<<rotation_q.x()<<" y:"<<rotation_q.y()<<" z:"<<rotation_q.z()<<" w:"<<rotation_q.w()<<" "<<std::endl;


    return 0;
}

使用

rt2euler r_3x3.txt

其中r_3x3.txt格式如下

1 0 0
0 1 0
0 0 1

参考

使用Eigen实现四元数、欧拉角、旋转矩阵、旋转向量之间的转换
http://t.zoukankan.com/long5683-p-14373627.html

完

如有错漏，敬请指正
--------------------------------------------------------------------------------------------202207