手写VIO第一讲习题:第二题, 更新方式验证

#include<iostream>
#include<cmath>
#include"/usr/include/eigen3/Eigen/Core"
//包含几何模块
#include"/usr/include/eigen3/Eigen/Geometry"
#include "/usr/local/include/sophus/so3.h"
using namespace std;

int main(int argc,char **argv)
{

    Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(1,1,1)).toRotationMatrix();
    Sophus::SO3 SO3_R(R);
    Sophus::SO3 SO3_q(R);
    cout<<"SO(3) from matrix: "<<SO3_R<<endl;
    cout<<"SO(3) from quaterniond: "<<SO3_q<<endl;

    Eigen::Quaterniond q(R);
    Eigen::Vector3d w(0.01,0.02,0.03);//小量w
    Eigen::Matrix3d rotation_matrix3;

    //扰动更新
    Sophus::SO3 SO3_updated = SO3_R*Sophus::SO3::exp(w);
    cout<<"扰动更新结果: "<<SO3_updated<<endl;

    //四元数更新
    Eigen::Quaterniond q_update(1,0.005,0.01,0.015);
    q_update.normalize();//归一化
    Eigen::Quaterniond q_updated;
    q_updated = q*q_update;
    Sophus::SO3 SO3_qupdated(q_updated);
    cout << "四元数更新结果:" << SO3_qupdated;
    return 0;
}

 

  • 1
    点赞
  • 4
    收藏
    觉得还不错? 一键收藏
  • 3
    评论
评论 3
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值