标签：family com div 注意 ade orm sop 表示 htm
请编程验证对于小量w = [0.01, 0.02, 0.03]T，通过李代数和四元数两种更新方法得到的结果接近。
这段代码本身不难，但是可能对于初学者，对Eigen库和Sophus库不太熟悉，这里我把两个库的官方使用说明网站贴上来
Eigen库编程文档
有了这两文档，使用Eigen和Sophus不会再有编程上的问题，
还有一点值得注意的是：我们都是使用单位四元数表示旋转，这在下面的代码里也有对应，就是
q_new.normalize();
感兴趣的同学可以尝试把这行代码注释掉，看看结果如何。
#include <Eigen/Dense> #include <iostream> #include "sophus/so3.hpp" using namespace std; using namespace Eigen; int main(){ Eigen::Matrix3dR = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(1,1,1)).toRotationMatrix(); Quaterniond q_raw(R); Quaterniond q_w(1.0,0.005,0.01,0.015); Quaterniond q_new = q_raw*q_w; q_new.normalize(); Matrix3d R_new_q = q_new.toRotationMatrix(); cout<<"R is "<< R_new_q << "by q"<<endl; Vector3d so3_w(0.01,0.02,0.03); Sophus::SO3 SO3_w = Sophus::SO3::exp(so3_w); Sophus::SO3 SO3_R_raw(R); Sophus::SO3 SO3_R_new_lie_l = SO3_R_raw * SO3_w; cout<<"R is "<< SO3_R_new_lie_l.matrix() << "by Lie"<<endl; }
下面是结果，可以看到，通过李代数和四元数的更新两个旋转矩阵确实相差不大
标签：family com div 注意 ade orm sop 表示 htm
原文地址：https://www.cnblogs.com/jxLuTech/p/11696975.html
