利用旋转矩阵、四元数 实现旋转的更新
公式:
上部分公式对应旋转矩阵的更新,下部分公式对应着四元数更新
其中 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