从零开始手写vio作业-ch1-编程实现旋转的更新(分别用旋转矩阵和四元数实现)

利用旋转矩阵、四元数 实现旋转的更新

公式：

上部分公式对应旋转矩阵的更新，下部分公式对应着四元数更新

其中 w对应着旋转小量 w=[0.01,0.02,0.03]T，也就是原始的旋转要更新w这么多的旋转
1、直接用Eigen 进行更新

对于旋转矩阵的更新来说，要用到下面的公式 ，该公式可参考slambook2 79p页
也就是罗德里格斯公式（Rodrigues’s Formula ）
注;对于小量w来说 ，它的旋转角度是小量向量的模长norm，旋转轴是小量向量除以旋转角度。

源码如下：

//
// Created by nnz on 2020/11/25.
//

/**
 * 该程序用来更新旋转-------初始旋转在某一小量的旋转的基础上更新
 **/
#include <iostream>
#include <Eigen/Core>
#include <Eigen/Geometry>
using namespace std;
using namespace Eigen;
int main(int argc,char **argv)
{
    
    
    Vector3d w(0.01,0.02,0.03);//旋转小量
    Matrix3d R=AngleAxisd(M_PI/4,Vector3d(0,0,1)).toRotationMatrix();//初始化旋转矩阵
    Quaterniond q(R);//初始化四元数
    cout<<"初始旋转矩阵："<<endl;
    cout<<R<<endl;
    cout<<"初始四元数："<<endl;
    cout<<q.coeffs().transpose() <<endl;//coeffs的顺序是(x,y,z,w) w为实部，前三者为虚部

    cout<<"*******************//利用Rodrigues's formula完成旋转矩阵更新***********************"<<endl;
    double theta=w.norm();//theat是旋转向量对应的旋转角度
    Vector3d n_w=w/theta;//n_w是旋转向量对应的旋转轴
    Matrix3d n_w_skew;   //n_w_skew旋转轴的反对称矩阵
    n_w_skew<<   0,    -n_w(2),    n_w(1),
            n_w(2),     0,     -n_w(0),
            -n_w(1),  n_w(0),      0;
    //Rodrigues's formula
    Matrix3d R_w=cos(theta)*Matrix3d::Identity()+(1-cos(theta))*n_w*n_w.transpose()+sin(theta)*n_w_skew;//Rodrigues's formula
    Matrix3d R_update=R*R_w;
    cout<<"更新后的旋转矩阵："<<endl;
    cout<<R_update<<endl;
    cout<<"*******************//利用Rodrigues's formula完成旋转矩阵更新***********************"<<endl;


    cout<<"******************* //利用四元数***********************"<<endl;
    Quaterniond q_w(1,w(0)/2,w(1)/2,w(2)/2);//小量角速度对应的四元数
    Quaterniond q_update=q*q_w;
    q_update.normalized();//单位四元数才可以表示三维旋转，所以必须归一化
    cout<<"更新后的四元数："<<endl;
    cout<<q_update.coeffs().transpose()<<endl;

    cout<<"******************* //两种方法的误差***********************"<<endl;
    cout<< R_update-q_update.toRotationMatrix() <<endl;
    return 0;
}

运行结果如下：

初始旋转矩阵：
 0.707107 -0.707107         0
 0.707107  0.707107         0
        0         0         1
初始四元数：
       0        0 0.382683  0.92388
*******************//利用Rodrigues's formula完成旋转矩阵更新***********************
更新后的旋转矩阵：
  0.685368  -0.727891  0.0211022
  0.727926   0.685616 0.00738758
-0.0198454  0.0102976    0.99975
*******************//利用Rodrigues's formula完成旋转矩阵更新***********************
******************* //利用四元数***********************
更新后的四元数：
0.000792563   0.0111522    0.396542    0.918139
******************* //两种方法的误差***********************
 0.000107524  0.000252387 -4.93703e-06
-0.000252391  0.000107495 -1.68692e-06
 4.64884e-06 -2.36817e-06  2.91653e-08

2、利用Sophus进行更新(也就是不用Rodrigues’s formula、利用李群进行更新)
源码如下：

//
// Created by nnz on 2020/11/25.
//
#include <iostream>
#include <sophus/se3.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>
using namespace std;
using namespace Eigen;
using namespace  Sophus;

int main(int argc,char **argv)
{
    
    
    Vector3d w(0.01,0.02,0.03);//旋转小量
    Matrix3d R=AngleAxisd(M_PI/4,Vector3d(0,0,1)).toRotationMatrix();//初始化旋转矩阵
    Quaterniond q(R);//初始化四元数
    cout<<"初始旋转矩阵："<<endl;
    cout<<R<<endl;
    cout<<"初始四元数："<<endl;
    cout<<q.coeffs().transpose() <<endl;//coeffs的顺序是(x,y,z,w) w为实部，前三者为虚部

    cout<<"*******************//利用SO3旋转矩阵更新***********************"<<endl;
    SO3d SO3_R(R);
    cout<<"SO3_R="<<endl;
    cout<<SO3_R.matrix()<<endl;
    SO3d update_SO3_R=SO3_R*SO3d::exp(w);
    cout<<"更新后的旋转矩阵："<<endl;
    cout<<update_SO3_R.matrix()<<endl;
    cout<<"*******************//利用SO3旋转矩阵更新***********************"<<endl;


    cout<<"******************* //利用四元数***********************"<<endl;
    Quaterniond q_w(1,w(0)/2,w(1)/2,w(2)/2);//小量角速度对应的四元数
    Quaterniond q_update=q*q_w;
    q_update.normalized();//单位四元数才可以表示三维旋转，所以必须归一化
    cout<<"更新后的四元数："<<endl;
    cout<<q_update.coeffs().transpose()<<endl;

    cout<<"******************* //两种方法的误差***********************"<<endl;
    cout<< update_SO3_R.matrix()-q_update.toRotationMatrix() <<endl;


    return 0;
}

运行结果如下：

初始旋转矩阵：
 0.707107 -0.707107         0
 0.707107  0.707107         0
        0         0         1
初始四元数：
       0        0 0.382683  0.92388
*******************//利用SO3旋转矩阵更新***********************
SO3_R=
 0.707107 -0.707107         0
 0.707107  0.707107         0
        0         0         1
更新后的旋转矩阵：
  0.685368  -0.727891  0.0211022
  0.727926   0.685616 0.00738758
-0.0198454  0.0102976    0.99975
*******************//利用SO3旋转矩阵更新***********************
******************* //利用四元数***********************
更新后的四元数：
0.000792563   0.0111522    0.396542    0.918139
******************* //两种方法的误差***********************
 0.000107524  0.000252387 -4.93703e-06
-0.000252391  0.000107495 -1.68692e-06
 4.64884e-06 -2.36817e-06  2.91653e-08