从零手写VIO-ch01课后习题答案和推导过程

#include<Eigen/Core>
#include<Eigen/Geometry>
#include"sophus/so3.hpp"
#include<random>
#include<iostream>

void quatdot(Eigen::Vector3d omega, Eigen::Quaterniond q1, Eigen::Quaterniond& q2){
    double omegax = omega[0];
    double omegay = omega[1];
    double omegaz = omega[2];
    Eigen::Quaterniond qtmp(1, 0.5 * omegax, 0.5 * omegay, 0.5 * omegaz);
    q2 = q1 * qtmp;
}

int main(){
    double q_array[4];
    for(int i = 0; i < 3; i++){
        std::mt19937 rng;
        rng.seed(std::random_device{}());
        std::uniform_real_distribution<double> dist_double(0.0, 1.0);
        q_array[i] = dist_double(rng);
    }

    Eigen::Quaterniond q(q_array[0], q_array[1], q_array[2], q_array[3]);
    q.normalize();
    Eigen::Matrix3d rotation_Mat = q.toRotationMatrix();
    Eigen::Quaterniond q2;
    Eigen::Vector3d omgea(0.01, 0.02, 0.03);
    quatdot(omgea, q, q2);
    Sophus::SO3d SO3_R(rotation_Mat);
    Sophus::SO3d SO3_R_upate = SO3_R*Sophus::SO3d::exp(omgea);//右扰动
    //Sophus::SO3d SO3_R_upate = Sophus::SO3d::exp(omgea)*SO3_R;//左扰动
    rotation_Mat = SO3_R_upate.matrix();
    Eigen::Matrix3d rotation_Mat2 = q2.toRotationMatrix();
    Eigen::Matrix3d delta_Mat = rotation_Mat2 - rotation_Mat;
    Eigen::Vector3d rpy = delta_Mat.eulerAngles(2, 1, 0);
    Eigen::AngleAxisd rot_z(rpy[0], Eigen::Vector3d::UnitZ());
    Eigen::AngleAxisd rot_y(rpy[1], Eigen::Vector3d::UnitY());
    Eigen::AngleAxisd rot_x(rpy[2], Eigen::Vector3d::UnitX());
    Eigen::AngleAxisd rot;
    rot = rot_z * rot_y * rot_x;
    Eigen::Matrix3d delta_Mat_calcu_from_rpy = rot.toRotationMatrix();
    std::cout<<"the output Mat is: "<<delta_Mat<<std::endl;
    std::cout<<"the rpy is: "<<rpy<<std::endl;
    std::cout<<"the delta_Mat_calcu_from_rpy is: "<<delta_Mat_calcu_from_rpy<<std::endl;
}

CMakeLists.txt文件：

cmake_minimum_required(VERSION 3.10)
project(HW1)
find_package(Eigen3 REQUIRED)
find_package(Sophus REQUIRED)

include_directories(${Sophus_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR})

add_executable(P2 P2.cpp)
target_link_libraries(P2 ${Sophus_LIBRARIES})

运行结果：


第一题答案和推导：

第二题答案和推导：