《自动驾驶中的slam技术》Ch2代码阅读

//
// Created by xiang on 22-12-29.
// Commented by WXL on 23-09-01.
//

#include <gflags/gflags.h>
#include <glog/logging.h>

#include "common/eigen_types.h"
#include "common/math_utils.h"
#include "tools/ui/pangolin_window.h"

/*********************************
 欧拉角定义：
 旋转向量转旋转矩阵            Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(0,0,1)).toRotationMatrix();
 旋转矩阵转四元数              Eigen::Quaterniond q(R);            // 或者四元数(从旋转矩阵构造)
 平移Eigen::Vector3d t(1,0,0);    // 沿X轴平移1


 SO(n) 特殊正交群       对应 n*n 的旋转矩阵  群(集合)
 SE(n+1) 特殊欧式群     对应 n*n 的旋转矩阵和 n*1的平移向量组合成的变换矩阵 群(集合)
 so(n)  对应的李代数为so(n)  n×1 列向量,使得向量和代数一一对应可以使用代数的更新方法来更新矩阵


 SO(3)  表示三维空间的 旋转矩阵 集合  3×3
 SE(3)  表示三维空间的 变换矩阵 集合  4×4
 李代数 so3的本质就是个三维向量，直接Eigen::Vector3d定义.3个旋转
 李代数 se3的本质就是个六维向量，直接Eigen::Matrix<double,6,1>定义,3个旋转 + 3个平移


 旋转向量定义的李群SO(3)       Sophus::SO3 SO3_v( 0, 0, M_PI/2 );  // 亦可从旋转向量构造  这里注意，不是旋转向量的三个坐标值，有点像欧拉角构造。
 旋转矩阵定义的李群SO(3)       Sophus::SO3 SO3_R(R);        // Sophus::SO(3)可以直接从旋转矩阵构造
 四元素定义的李群SO(3)         Sophus::SO3 SO3_q(q);
 从旋转矩阵和平移t构造SE3      Sophus::SE3 SE3_Rt(R,t);     // 从R,t构造SE(3)
 从四元素和平移t构造SE3        Sophus::SE3 SE3_qt(q,t);     // 从q,t构造SE(3)


 李代数so3为李群SO(3)的对数映射 Eigen::Vector3d so3 = SO3_R.log();
 李代数se(3)是一个6维向量,为李群SE3 的对数映射
 typedef Eigen::Matrix<double,6,1> Vector6d;// Vector6d指代Eigen::Matrix<double,6,1>
 Vector6d se3 = SE3_Rt.log();
 李代数指数映射成旋转矩阵对应的李群
 Eigen::Vector3d so33 (1, 1, 1);
 Sophus::SO3 SO3 =Sophus::SO3::exp(so33);


 Sophus::SO3::hat(so3) //hat为向量到反对称矩阵,相当于^运算.
 Sophus::SO3::vee( Sophus::SO3::hat(so3) ).transpose() //vee为反对称矩阵到向量,相当于下尖尖运算 .
 **********************************/


/// 本节程序演示一个正在作圆周运动的车辆
/// 车辆的角速度与线速度可以在flags中设置

DEFINE_double(angular_velocity, 10.0, "角速度（角度）制");
DEFINE_double(linear_velocity, 5.0, "车辆前进线速度 m/s");
DEFINE_bool(use_quaternion, false, "是否使用四元数计算");

int main(int argc, char** argv) {
    google::InitGoogleLogging(argv[0]);
    FLAGS_stderrthreshold = google::INFO;
    FLAGS_colorlogtostderr = true;
    google::ParseCommandLineFlags(&argc, &argv, true);

    /// 可视化
    sad::ui::PangolinWindow ui;
    if (ui.Init() == false) {
        return -1;
    }

    double angular_velocity_rad = FLAGS_angular_velocity * sad::math::kDEG2RAD;  // 弧度制角速度
    SE3 pose;                                                                    // TWB表示的位姿,初始化 pose translation （0，0，0）
    Vec3d omega(0, 0, angular_velocity_rad);                                     // 角速度矢量
    Vec3d v_body(FLAGS_linear_velocity, 0, 0);                                   // 本体系速度
    const double dt = 0.05;                                                   // 每次更新的时间

    while (ui.ShouldQuit() == false) {

        // 更新自身位置, pose.so3() 表示旋转矩阵 R,类似tf::Quaternion
        Vec3d v_world = pose.so3() * v_body;
        // 经过变换的直线速度乘以时间，就得到了距离
        pose.translation() += v_world * dt;

        // 更新自身旋转
        if (FLAGS_use_quaternion) {
            Quatd q = pose.unit_quaternion() * Quatd(1, 0.5 * omega[0] * dt, 0.5 * omega[1] * dt, 0.5 * omega[2] * dt);
            q.normalize();
            pose.so3() = SO3(q);
        } else {
            // 当前旋转位姿乘以旋转矩阵，就得到了当前的旋转位姿
            // omega*dt得到的三轴的变化量，通过so3::exp()得到变化的旋转矩阵
            pose.so3() = pose.so3() * SO3::exp(omega * dt);
        }

        LOG(INFO) << "pose: " << pose.translation().transpose();
        ui.UpdateNavState(sad::NavStated(0, pose, v_world));

        usleep(dt * 1e6);
    }

    ui.Quit();
    return 0;
}

参考资料1.
参考资料2 : https://juejin.cn/post/7062623440116826119
参考资料3 : https://www.zhihu.com/question/280293029