#include iostream #include Eigen/Core #include Eigen/Geometry #include sophus/so3.h #include sophus/se3.h using namespace std; int main( int argc, char** argv ) // 沿Z轴转90度的旋转矩阵 Eigen::Matrix3d R Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(0,0,1)).toRotationMatrix(); std::cout R : endl R endl; Sophus::SO3 SO3_R(R); // 从旋转矩阵构造Sophus::SO(3) Eigen::Quaterniond q(R); // 从旋转矩阵构造四元数 // 李代数更新 Eigen::Vector3d update_so3(0.01, 0.02, 0.03); //更新量 Sophus::SO3 SO3_updated SO3_R * Sophus::SO3::exp(update_so3); std::cout SO3 updated endl SO3_updated.matrix() endl; //四元数更新 Eigen::Quaterniond q_update(1, update_so3(0)/2, update_so3(1)/2, update_so3(2)/2);  Eigen::Quaterniond q_updated (q * q_update).normalized(); //四元数归一化 std::cout q2R endl q_updated.toRotationMatrix() endl; return 0;

CMakeLists.txt:

SET (PROJECT_NAME imuSimWithPointLine)PROJECT(${PROJECT_NAME})CMAKE_MINIMUM_REQUIRED(VERSION 2.8)SET(CMAKE_BUILD_TYPE Debug) # Release, RelWithDebInfo , DebugSET(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)SET(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)set(CMAKE_CXX_FLAGS ${CMAKE_CXX_FLAGS} -std c 11 )#set(CMAKE_C_FLAGS ${CMAKE_C_FLAGS} -Wall -O3 -march native )#set(CMAKE_CXX_FLAGS ${CMAKE_CXX_FLAGS} -Wall -O3 -march native )SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${PROJECT_SOURCE_DIR}/cmake_modules/ )find_package(Eigen3 REQUIRED)FIND_PACKAGE(OpenCV REQUIRED)FIND_PACKAGE(Sophus REQUIRED)include_directories(${EIGEN3_INCLUDE_DIR}${SOPHUS_INCLUDE_DIR} # for both sophus and geographiclib${OPENCV_INCLUDE_DIR}LIST(APPEND LINK_LIBS${OpenCV_LIBS}${Sophus_LIBRARIES}ADD_EXECUTABLE(homework src/home_work.cpp)TARGET_LINK_LIBRARIES (homework ${LINK_LIBS})