Add slam_in_autonomous_driving repo

workspace/slam_in_autonomous_driving/src/CMakeLists.txt

@@ -0,0 +1,12 @@
+add_subdirectory(common)
+add_subdirectory(ch2)
+add_subdirectory(ch3)
+add_subdirectory(ch4)
+add_subdirectory(ch5)
+add_subdirectory(ch6)
+add_subdirectory(ch7)
+add_subdirectory(ch8)
+add_subdirectory(ch9)
+add_subdirectory(ch10)
+
+add_subdirectory(tools)

workspace/slam_in_autonomous_driving/src/ch10/CMakeLists.txt

@@ -0,0 +1,15 @@
+add_library(${PROJECT_NAME}.ch10
+        fusion.cc
+        )
+
+target_link_libraries(${PROJECT_NAME}.ch10
+        ${PROJECT_NAME}.ch3
+        ${PROJECT_NAME}.ch7
+        ${third_party_libs}
+        )
+
+add_executable(run_fusion_offline run_fusion_offline.cc)
+target_link_libraries(run_fusion_offline
+        ${PROJECT_NAME}.ch10
+        ${third_party_libs}
+        )

workspace/slam_in_autonomous_driving/src/ch10/fusion.cc

@@ -0,0 +1,316 @@
+//
+// Created by xiang on 22-12-20.
+//
+#include <yaml-cpp/yaml.h>
+#include <execution>
+
+#include "common/lidar_utils.h"
+#include "fusion.h"
+
+namespace sad {
+
+Fusion::Fusion(const std::string& config_yaml) {
+    config_yaml_ = config_yaml;
+    StaticIMUInit::Options imu_init_options;
+    imu_init_options.use_speed_for_static_checking_ = false;  // 本节数据不需要轮速计
+    imu_init_ = StaticIMUInit(imu_init_options);
+    ndt_.setResolution(1.0);
+}
+
+bool Fusion::Init() {
+    // 地图原点
+    auto yaml = YAML::LoadFile(config_yaml_);
+    auto origin_data = yaml["origin"].as<std::vector<double>>();
+    map_origin_ = Vec3d(origin_data[0], origin_data[1], origin_data[2]);
+
+    // 地图目录
+    data_path_ = yaml["map_data"].as<std::string>();
+    LoadMapIndex();
+
+    // lidar和IMU消息同步
+    sync_ = std::make_shared<MessageSync>([this](const MeasureGroup& m) { ProcessMeasurements(m); });
+    sync_->Init(config_yaml_);
+
+    // lidar和IMU外参
+    std::vector<double> ext_t = yaml["mapping"]["extrinsic_T"].as<std::vector<double>>();
+    std::vector<double> ext_r = yaml["mapping"]["extrinsic_R"].as<std::vector<double>>();
+    Vec3d lidar_T_wrt_IMU = math::VecFromArray(ext_t);
+    Mat3d lidar_R_wrt_IMU = math::MatFromArray(ext_r);
+    TIL_ = SE3(lidar_R_wrt_IMU, lidar_T_wrt_IMU);
+
+    // ui
+    ui_ = std::make_shared<ui::PangolinWindow>();
+    ui_->Init();
+    ui_->SetCurrentScanSize(50);
+    return true;
+}
+
+void Fusion::ProcessRTK(GNSSPtr gnss) {
+    gnss->utm_pose_.translation() -= map_origin_;  // 减掉地图原点
+    last_gnss_ = gnss;
+}
+
+void Fusion::ProcessMeasurements(const MeasureGroup& meas) {
+    measures_ = meas;
+
+    if (imu_need_init_) {
+        TryInitIMU();
+        return;
+    }
+
+    /// 以下三步与LIO一致，只是align完成地图匹配工作
+    if (status_ == Status::WORKING) {
+        Predict();
+        Undistort();
+    } else {
+        scan_undistort_ = measures_.lidar_;
+    }
+
+    Align();
+}
+
+void Fusion::TryInitIMU() {
+    for (auto imu : measures_.imu_) {
+        imu_init_.AddIMU(*imu);
+    }
+
+    if (imu_init_.InitSuccess()) {
+        // 读取初始零偏，设置ESKF
+        sad::ESKFD::Options options;
+        // 噪声由初始化器估计
+        // options.gyro_var_ = sqrt(imu_init_.GetCovGyro()[0]);
+        // options.acce_var_ = sqrt(imu_init_.GetCovAcce()[0]);
+        options.update_bias_acce_ = false;
+        options.update_bias_gyro_ = false;
+        eskf_.SetInitialConditions(options, imu_init_.GetInitBg(), imu_init_.GetInitBa(), imu_init_.GetGravity());
+        imu_need_init_ = false;
+
+        LOG(INFO) << "IMU初始化成功";
+    }
+}
+
+void Fusion::Predict() {
+    imu_states_.clear();
+    imu_states_.emplace_back(eskf_.GetNominalState());
+
+    /// 对IMU状态进行预测
+    for (auto& imu : measures_.imu_) {
+        eskf_.Predict(*imu);
+        imu_states_.emplace_back(eskf_.GetNominalState());
+    }
+}
+
+void Fusion::Undistort() {
+    auto cloud = measures_.lidar_;
+    auto imu_state = eskf_.GetNominalState();  // 最后时刻的状态
+    SE3 T_end = SE3(imu_state.R_, imu_state.p_);
+
+    /// 将所有点转到最后时刻状态上
+    std::for_each(std::execution::par_unseq, cloud->points.begin(), cloud->points.end(), [&](auto& pt) {
+        SE3 Ti = T_end;
+        NavStated match;
+
+        // 根据pt.time查找时间，pt.time是该点打到的时间与雷达开始时间之差，单位为毫秒
+        math::PoseInterp<NavStated>(
+            measures_.lidar_begin_time_ + pt.time * 1e-3, imu_states_, [](const NavStated& s) { return s.timestamp_; },
+            [](const NavStated& s) { return s.GetSE3(); }, Ti, match);
+
+        Vec3d pi = ToVec3d(pt);
+        Vec3d p_compensate = TIL_.inverse() * T_end.inverse() * Ti * TIL_ * pi;
+
+        pt.x = p_compensate(0);
+        pt.y = p_compensate(1);
+        pt.z = p_compensate(2);
+    });
+    scan_undistort_ = cloud;
+}
+
+void Fusion::Align() {
+    FullCloudPtr scan_undistort_trans(new FullPointCloudType);
+    pcl::transformPointCloud(*scan_undistort_, *scan_undistort_trans, TIL_.matrix());
+    scan_undistort_ = scan_undistort_trans;
+    current_scan_ = ConvertToCloud<FullPointType>(scan_undistort_);
+    current_scan_ = VoxelCloud(current_scan_, 0.5);
+
+    if (status_ == Status::WAITING_FOR_RTK) {
+        // 若存在最近的RTK信号，则尝试初始化
+        if (last_gnss_ != nullptr) {
+            if (SearchRTK()) {
+                status_ == Status::WORKING;
+                ui_->UpdateScan(current_scan_, eskf_.GetNominalSE3());
+                ui_->UpdateNavState(eskf_.GetNominalState());
+            }
+        }
+    } else {
+        LidarLocalization();
+        ui_->UpdateScan(current_scan_, eskf_.GetNominalSE3());
+        ui_->UpdateNavState(eskf_.GetNominalState());
+    }
+}
+
+bool Fusion::SearchRTK() {
+    if (init_has_failed_) {
+        if ((last_gnss_->utm_pose_.translation() - last_searched_pos_.translation()).norm() < 20.0) {
+            LOG(INFO) << "skip this position";
+            return false;
+        }
+    }
+
+    // 由于RTK不带姿态，我们必须先搜索一定的角度范围
+    std::vector<GridSearchResult> search_poses;
+    LoadMap(last_gnss_->utm_pose_);
+
+    /// 由于RTK不带角度，这里按固定步长扫描RTK角度
+    double grid_ang_range = 360.0, grid_ang_step = 10;  // 角度搜索范围与步长
+    for (double ang = 0; ang < grid_ang_range; ang += grid_ang_step) {
+        SE3 pose(SO3::rotZ(ang * math::kDEG2RAD), Vec3d(0, 0, 0) + last_gnss_->utm_pose_.translation());
+        GridSearchResult gr;
+        gr.pose_ = pose;
+        search_poses.emplace_back(gr);
+    }
+
+    LOG(INFO) << "grid search poses: " << search_poses.size();
+    std::for_each(std::execution::par_unseq, search_poses.begin(), search_poses.end(),
+                  [this](GridSearchResult& gr) { AlignForGrid(gr); });
+
+    // 选择最优的匹配结果
+    auto max_ele = std::max_element(search_poses.begin(), search_poses.end(),
+                                    [](const auto& g1, const auto& g2) { return g1.score_ < g2.score_; });
+    LOG(INFO) << "max score: " << max_ele->score_ << ", pose: \n" << max_ele->result_pose_.matrix();
+    if (max_ele->score_ > rtk_search_min_score_) {
+        LOG(INFO) << "初始化成功, score: " << max_ele->score_ << ">" << rtk_search_min_score_;
+        status_ = Status::WORKING;
+
+        /// 重置滤波器状态
+        auto state = eskf_.GetNominalState();
+        state.R_ = max_ele->result_pose_.so3();
+        state.p_ = max_ele->result_pose_.translation();
+        state.v_.setZero();
+        eskf_.SetX(state, eskf_.GetGravity());
+
+        ESKFD::Mat18T cov;
+        cov = ESKFD::Mat18T::Identity() * 1e-4;
+        cov.block<12, 12>(6, 6) = Eigen::Matrix<double, 12, 12>::Identity() * 1e-6;
+        eskf_.SetCov(cov);
+
+        return true;
+    }
+
+    init_has_failed_ = true;
+    last_searched_pos_ = last_gnss_->utm_pose_;
+    return false;
+}
+
+void Fusion::AlignForGrid(sad::Fusion::GridSearchResult& gr) {
+    /// 多分辨率
+    pcl::NormalDistributionsTransform<PointType, PointType> ndt;
+    ndt.setTransformationEpsilon(0.05);
+    ndt.setStepSize(0.7);
+    ndt.setMaximumIterations(40);
+
+    ndt.setInputSource(current_scan_);
+    auto map = ref_cloud_;
+
+    CloudPtr output(new PointCloudType);
+    std::vector<double> res{10.0, 5.0, 4.0, 3.0};
+    Mat4f T = gr.pose_.matrix().cast<float>();
+    for (auto& r : res) {
+        auto rough_map = VoxelCloud(map, r * 0.1);
+        ndt.setInputTarget(rough_map);
+        ndt.setResolution(r);
+        ndt.align(*output, T);
+        T = ndt.getFinalTransformation();
+    }
+
+    gr.score_ = ndt.getTransformationProbability();
+    gr.result_pose_ = Mat4ToSE3(ndt.getFinalTransformation());
+}
+
+bool Fusion::LidarLocalization() {
+    SE3 pred = eskf_.GetNominalSE3();
+    LoadMap(pred);
+
+    ndt_.setInputCloud(current_scan_);
+    CloudPtr output(new PointCloudType);
+    ndt_.align(*output, pred.matrix().cast<float>());
+
+    SE3 pose = Mat4ToSE3(ndt_.getFinalTransformation());
+    eskf_.ObserveSE3(pose, 1e-1, 1e-2);
+
+    LOG(INFO) << "lidar loc score: " << ndt_.getTransformationProbability();
+
+    return true;
+}
+
+void Fusion::LoadMap(const SE3& pose) {
+    int gx = floor((pose.translation().x() - 50.0) / 100);
+    int gy = floor((pose.translation().y() - 50.0) / 100);
+    Vec2i key(gx, gy);
+
+    // 一个区域的周边地图，我们认为9个就够了
+    std::set<Vec2i, less_vec<2>> surrounding_index{
+        key + Vec2i(0, 0), key + Vec2i(-1, 0), key + Vec2i(-1, -1), key + Vec2i(-1, 1), key + Vec2i(0, -1),
+        key + Vec2i(0, 1), key + Vec2i(1, 0),  key + Vec2i(1, -1),  key + Vec2i(1, 1),
+    };
+
+    // 加载必要区域
+    bool map_data_changed = false;
+    int cnt_new_loaded = 0, cnt_unload = 0;
+    for (auto& k : surrounding_index) {
+        if (map_data_index_.find(k) == map_data_index_.end()) {
+            // 该地图数据不存在
+            continue;
+        }
+
+        if (map_data_.find(k) == map_data_.end()) {
+            // 加载这个区块
+            CloudPtr cloud(new PointCloudType);
+            pcl::io::loadPCDFile(data_path_ + std::to_string(k[0]) + "_" + std::to_string(k[1]) + ".pcd", *cloud);
+            map_data_.emplace(k, cloud);
+            map_data_changed = true;
+            cnt_new_loaded++;
+        }
+    }
+
+    // 卸载不需要的区域，这个稍微加大一点，不需要频繁卸载
+    for (auto iter = map_data_.begin(); iter != map_data_.end();) {
+        if ((iter->first - key).cast<float>().norm() > 3.0) {
+            // 卸载本区块
+            iter = map_data_.erase(iter);
+            cnt_unload++;
+            map_data_changed = true;
+        } else {
+            iter++;
+        }
+    }
+
+    LOG(INFO) << "new loaded: " << cnt_new_loaded << ", unload: " << cnt_unload;
+    if (map_data_changed) {
+        // rebuild ndt target map
+        ref_cloud_.reset(new PointCloudType);
+        for (auto& mp : map_data_) {
+            *ref_cloud_ += *mp.second;
+        }
+
+        LOG(INFO) << "rebuild global cloud, grids: " << map_data_.size();
+        ndt_.setInputTarget(ref_cloud_);
+    }
+
+    ui_->UpdatePointCloudGlobal(map_data_);
+}
+
+void Fusion::LoadMapIndex() {
+    std::ifstream fin(data_path_ + "/map_index.txt");
+    while (!fin.eof()) {
+        int x, y;
+        fin >> x >> y;
+        map_data_index_.emplace(Vec2i(x, y));
+    }
+    fin.close();
+}
+
+void Fusion::ProcessIMU(IMUPtr imu) { sync_->ProcessIMU(imu); }
+
+void Fusion::ProcessPointCloud(sensor_msgs::PointCloud2::Ptr cloud) { sync_->ProcessCloud(cloud); }
+
+}  // namespace sad

workspace/slam_in_autonomous_driving/src/ch10/fusion.h

@@ -0,0 +1,128 @@
+//
+// Created by xiang on 22-12-20.
+//
+
+#ifndef SLAM_IN_AUTO_DRIVING_FUSION_H
+#define SLAM_IN_AUTO_DRIVING_FUSION_H
+
+#include "ch3/eskf.hpp"
+#include "ch3/static_imu_init.h"
+#include "common/eigen_types.h"
+#include "common/gnss.h"
+#include "common/imu.h"
+#include "common/message_def.h"
+#include "common/point_types.h"
+
+#include "ch7/loosely_coupled_lio/cloud_convert.h"
+#include "ch7/loosely_coupled_lio/measure_sync.h"
+
+#include "tools/ui/pangolin_window.h"
+
+#include <pcl/registration/ndt.h>
+#include <sensor_msgs/PointCloud2.h>
+
+namespace sad {
+
+/**
+ * 第10章显示的高精度融合定位，融合IMU、RTK、激光点云定位功能
+ *
+ * - NOTE 一些IMU的异常处理没有加在这里，有可能会被IMU带歪。
+ */
+class Fusion {
+   public:
+    explicit Fusion(const std::string& config_yaml);
+
+    enum class Status {
+        WAITING_FOR_RTK,  // 等待初始的RTK
+        WORKING,          // 正常工作
+    };
+
+    /// 初始化，读取参数
+    bool Init();
+
+    /// 处理输入的RTK
+    void ProcessRTK(GNSSPtr gnss);
+    void ProcessIMU(IMUPtr imu);
+    void ProcessPointCloud(sensor_msgs::PointCloud2::Ptr cloud);
+
+   private:
+    /// 读取某个点附近的地图
+    void LoadMap(const SE3& pose);
+
+    /// 处理同步之后的IMU和雷达数据
+    void ProcessMeasurements(const MeasureGroup& meas);
+
+    /// 读取地图的索引文件
+    void LoadMapIndex();
+
+    /// 网格搜索时的结构
+    struct GridSearchResult {
+        SE3 pose_;
+        SE3 result_pose_;
+        double score_ = 0.0;
+    };
+
+    /// 在初始RTK附近搜索车辆位置
+    bool SearchRTK();
+
+    /// 对网格搜索的某个点进行配准，得到配准后位姿与配准分值
+    void AlignForGrid(GridSearchResult& gr);
+
+    /// 激光定位
+    bool LidarLocalization();
+
+    /// 使用IMU初始化
+    void TryInitIMU();
+
+    /// 利用IMU预测状态信息
+    /// 这段时间的预测数据会放入imu_states_里
+    void Predict();
+
+    /// 对measures_中的点云去畸变
+    void Undistort();
+
+    /// 执行一次配准和观测
+    void Align();
+
+    /// 标志位
+    Status status_ = Status::WAITING_FOR_RTK;
+
+    /// 数据
+    std::string config_yaml_;                          // 配置文件路径
+    Vec3d map_origin_ = Vec3d::Zero();                 // 地图原点
+    std::string data_path_;                            // 地图数据目录
+    std::set<Vec2i, less_vec<2>> map_data_index_;      // 哪些格子存在地图数据
+    std::map<Vec2i, CloudPtr, less_vec<2>> map_data_;  // 第9章建立的地图数据
+
+    std::shared_ptr<MessageSync> sync_ = nullptr;  // 消息同步器
+    StaticIMUInit imu_init_;                       // IMU静止初始化
+
+    /// 滤波器
+    ESKFD eskf_;
+    std::vector<NavStated> imu_states_;  // ESKF预测期间的状态
+
+    FullCloudPtr scan_undistort_{new FullPointCloudType()};  // scan after undistortion
+    CloudPtr current_scan_ = nullptr;
+
+    SE3 TIL_;
+    MeasureGroup measures_;  // sync IMU and lidar scan
+    GNSSPtr last_gnss_ = nullptr;
+
+    bool init_has_failed_ = false;  // 初始化是否失败过
+    SE3 last_searched_pos_;         // 上次搜索的GNSS位置
+
+    /// 激光定位
+    bool imu_need_init_ = true;     // 是否需要估计IMU初始零偏
+    CloudPtr ref_cloud_ = nullptr;  // NDT用于参考的点云
+    pcl::NormalDistributionsTransform<PointType, PointType> ndt_;
+
+    /// 参数
+    double rtk_search_min_score_ = 4.5;
+
+    // ui
+    std::shared_ptr<ui::PangolinWindow> ui_ = nullptr;
+};
+
+}  // namespace sad
+
+#endif  // SLAM_IN_AUTO_DRIVING_FUSION_H

workspace/slam_in_autonomous_driving/src/ch10/run_fusion_offline.cc

@@ -0,0 +1,48 @@
+//
+// Created by xiang on 22-12-20.
+//
+
+#include <gflags/gflags.h>
+#include <glog/logging.h>
+#include <yaml-cpp/yaml.h>
+
+#include "common/io_utils.h"
+#include "fusion.h"
+
+DEFINE_string(config_yaml, "./config/mapping.yaml", "配置文件");
+
+int main(int argc, char** argv) {
+    google::InitGoogleLogging(argv[0]);
+    FLAGS_stderrthreshold = google::INFO;
+    FLAGS_colorlogtostderr = true;
+    google::ParseCommandLineFlags(&argc, &argv, true);
+
+    sad::Fusion fusion(FLAGS_config_yaml);
+    if (!fusion.Init()) {
+        return -1;
+    }
+
+    auto yaml = YAML::LoadFile(FLAGS_config_yaml);
+    auto bag_path = yaml["bag_path"].as<std::string>();
+    sad::RosbagIO rosbag_io(bag_path, sad::DatasetType::NCLT);
+
+    /// 把各种消息交给fusion
+    rosbag_io
+        .AddAutoRTKHandle([&fusion](GNSSPtr gnss) {
+            fusion.ProcessRTK(gnss);
+            return true;
+        })
+        .AddAutoPointCloudHandle([&](sensor_msgs::PointCloud2::Ptr cloud) -> bool {
+            fusion.ProcessPointCloud(cloud);
+            return true;
+        })
+        .AddImuHandle([&](IMUPtr imu) {
+            fusion.ProcessIMU(imu);
+            return true;
+        })
+        .Go();
+
+    LOG(INFO) << "done.";
+    sleep(10);
+    return 0;
+}

workspace/slam_in_autonomous_driving/src/ch2/CMakeLists.txt

@@ -0,0 +1,5 @@
+add_executable(motion motion.cc)
+target_link_libraries(motion
+        ${PROJECT_NAME}.common
+        ${PROJECT_NAME}.tools
+        )

workspace/slam_in_autonomous_driving/src/ch2/motion.cc

@@ -0,0 +1,59 @@
+//
+// Created by xiang on 22-12-29.
+//
+
+#include <gflags/gflags.h>
+#include <glog/logging.h>
+
+#include "common/eigen_types.h"
+#include "common/math_utils.h"
+#include "tools/ui/pangolin_window.h"
+
+/// 本节程序演示一个正在作圆周运动的车辆
+/// 车辆的角速度与线速度可以在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表示的位姿
+    Vec3d omega(0, 0, angular_velocity_rad);                                     // 角速度矢量
+    Vec3d v_body(FLAGS_linear_velocity, 0, 0);                                   // 本体系速度
+    const double dt = 0.05;                                                      // 每次更新的时间
+
+    while (ui.ShouldQuit() == false) {
+        // 更新自身位置
+        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 {
+            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;
+}

workspace/slam_in_autonomous_driving/src/ch3/CMakeLists.txt

@@ -0,0 +1,32 @@
+add_executable(run_imu_integration
+        run_imu_integration.cc
+        )
+
+target_link_libraries(run_imu_integration
+        glog gflags ${PROJECT_NAME}.common
+        )
+
+add_executable(run_eskf_gins run_eskf_gins.cc)
+target_link_libraries(run_eskf_gins
+        glog gflags ${PROJECT_NAME}.common ${PROJECT_NAME}.ch3
+        )
+
+add_executable(process_gnss process_gnss.cc)
+target_link_libraries(process_gnss
+        glog gflags ${PROJECT_NAME}.common ${PROJECT_NAME}.ch3
+        )
+
+add_library(${PROJECT_NAME}.ch3
+        static_imu_init.cc
+        utm_convert.cc
+        # ieskf/nav_state_manifold.cc
+        # ieskf/ieskf.cc
+
+        ${PROJECT_SOURCE_DIR}/thirdparty/utm_convert/utm.cc
+        ${PROJECT_SOURCE_DIR}/thirdparty/utm_convert/tranmerc.cc
+        )
+
+target_link_libraries(${PROJECT_NAME}.ch3
+        glog gflags ${PROJECT_NAME}.common
+        )
+

workspace/slam_in_autonomous_driving/src/ch3/eskf.hpp

@@ -0,0 +1,336 @@
+//
+// Created by xiang on 2021/11/11.
+//
+
+#ifndef SLAM_IN_AUTO_DRIVING_ESKF_HPP
+#define SLAM_IN_AUTO_DRIVING_ESKF_HPP
+
+#include "common/eigen_types.h"
+#include "common/gnss.h"
+#include "common/imu.h"
+#include "common/math_utils.h"
+#include "common/nav_state.h"
+#include "common/odom.h"
+
+#include <glog/logging.h>
+#include <iomanip>
+
+namespace sad {
+
+/**
+ * 书本第3章介绍的误差卡尔曼滤波器
+ * 可以指定观测GNSS的读数，GNSS应该事先转换到车体坐标系
+ *
+ * 本书使用18维的ESKF，标量类型可以由S指定，默认取double
+ * 变量顺序：p, v, R, bg, ba, grav，与书本对应
+ * @tparam S    状态变量的精度，取float或double
+ */
+template <typename S = double>
+class ESKF {
+   public:
+    /// 类型定义
+    using SO3 = Sophus::SO3<S>;                     // 旋转变量类型
+    using VecT = Eigen::Matrix<S, 3, 1>;            // 向量类型
+    using Vec18T = Eigen::Matrix<S, 18, 1>;         // 18维向量类型
+    using Mat3T = Eigen::Matrix<S, 3, 3>;           // 3x3矩阵类型
+    using MotionNoiseT = Eigen::Matrix<S, 18, 18>;  // 运动噪声类型
+    using OdomNoiseT = Eigen::Matrix<S, 3, 3>;      // 里程计噪声类型
+    using GnssNoiseT = Eigen::Matrix<S, 6, 6>;      // GNSS噪声类型
+    using Mat18T = Eigen::Matrix<S, 18, 18>;        // 18维方差类型
+    using NavStateT = NavState<S>;                  // 整体名义状态变量类型
+
+    struct Options {
+        Options() = default;
+
+        /// IMU 测量与零偏参数
+        double imu_dt_ = 0.01;  // IMU测量间隔
+        // NOTE IMU噪声项都为离散时间，不需要再乘dt，可以由初始化器指定IMU噪声
+        double gyro_var_ = 1e-5;       // 陀螺测量标准差
+        double acce_var_ = 1e-2;       // 加计测量标准差
+        double bias_gyro_var_ = 1e-6;  // 陀螺零偏游走标准差
+        double bias_acce_var_ = 1e-4;  // 加计零偏游走标准差
+
+        /// 里程计参数
+        double odom_var_ = 0.5;
+        double odom_span_ = 0.1;        // 里程计测量间隔
+        double wheel_radius_ = 0.155;   // 轮子半径
+        double circle_pulse_ = 1024.0;  // 编码器每圈脉冲数
+
+        /// RTK 观测参数
+        double gnss_pos_noise_ = 0.1;                   // GNSS位置噪声
+        double gnss_height_noise_ = 0.1;                // GNSS高度噪声
+        double gnss_ang_noise_ = 1.0 * math::kDEG2RAD;  // GNSS旋转噪声
+
+        /// 其他配置
+        bool update_bias_gyro_ = true;  // 是否更新陀螺bias
+        bool update_bias_acce_ = true;  // 是否更新加计bias
+    };
+
+    /**
+     * 初始零偏取零
+     */
+    ESKF(Options option = Options()) : options_(option) { BuildNoise(option); }
+
+    /**
+     * 设置初始条件
+     * @param options 噪声项配置
+     * @param init_bg 初始零偏 陀螺
+     * @param init_ba 初始零偏 加计
+     * @param gravity 重力
+     */
+    void SetInitialConditions(Options options, const VecT& init_bg, const VecT& init_ba,
+                              const VecT& gravity = VecT(0, 0, -9.8)) {
+        BuildNoise(options);
+        options_ = options;
+        bg_ = init_bg;
+        ba_ = init_ba;
+        g_ = gravity;
+        cov_ = Mat18T::Identity() * 1e-4;
+    }
+
+    /// 使用IMU递推
+    bool Predict(const IMU& imu);
+
+    /// 使用轮速计观测
+    bool ObserveWheelSpeed(const Odom& odom);
+
+    /// 使用GPS观测
+    bool ObserveGps(const GNSS& gnss);
+
+    /**
+     * 使用SE3进行观测
+     * @param pose  观测位姿
+     * @param trans_noise 平移噪声
+     * @param ang_noise   角度噪声
+     * @return
+     */
+    bool ObserveSE3(const SE3& pose, double trans_noise = 0.1, double ang_noise = 1.0 * math::kDEG2RAD);
+
+    /// accessors
+    /// 获取全量状态
+    NavStateT GetNominalState() const { return NavStateT(current_time_, R_, p_, v_, bg_, ba_); }
+
+    /// 获取SE3 状态
+    SE3 GetNominalSE3() const { return SE3(R_, p_); }
+
+    /// 设置状态X
+    void SetX(const NavStated& x, const Vec3d& grav) {
+        current_time_ = x.timestamp_;
+        R_ = x.R_;
+        p_ = x.p_;
+        v_ = x.v_;
+        bg_ = x.bg_;
+        ba_ = x.ba_;
+        g_ = grav;
+    }
+
+    /// 设置协方差
+    void SetCov(const Mat18T& cov) { cov_ = cov; }
+
+    /// 获取重力
+    Vec3d GetGravity() const { return g_; }
+
+   private:
+    void BuildNoise(const Options& options) {
+        double ev = options.acce_var_;
+        double et = options.gyro_var_;
+        double eg = options.bias_gyro_var_;
+        double ea = options.bias_acce_var_;
+
+        double ev2 = ev;  // * ev;
+        double et2 = et;  // * et;
+        double eg2 = eg;  // * eg;
+        double ea2 = ea;  // * ea;
+
+        // 设置过程噪声
+        Q_.diagonal() << 0, 0, 0, ev2, ev2, ev2, et2, et2, et2, eg2, eg2, eg2, ea2, ea2, ea2, 0, 0, 0;
+
+        // 设置里程计噪声
+        double o2 = options_.odom_var_ * options_.odom_var_;
+        odom_noise_.diagonal() << o2, o2, o2;
+
+        // 设置GNSS状态
+        double gp2 = options.gnss_pos_noise_ * options.gnss_pos_noise_;
+        double gh2 = options.gnss_height_noise_ * options.gnss_height_noise_;
+        double ga2 = options.gnss_ang_noise_ * options.gnss_ang_noise_;
+        gnss_noise_.diagonal() << gp2, gp2, gh2, ga2, ga2, ga2;
+    }
+
+    /// 更新名义状态变量，重置error state
+    void UpdateAndReset() {
+        p_ += dx_.template block<3, 1>(0, 0);
+        v_ += dx_.template block<3, 1>(3, 0);
+        R_ = R_ * SO3::exp(dx_.template block<3, 1>(6, 0));
+
+        if (options_.update_bias_gyro_) {
+            bg_ += dx_.template block<3, 1>(9, 0);
+        }
+
+        if (options_.update_bias_acce_) {
+            ba_ += dx_.template block<3, 1>(12, 0);
+        }
+
+        g_ += dx_.template block<3, 1>(15, 0);
+
+        ProjectCov();
+        dx_.setZero();
+    }
+
+    /// 对P阵进行投影，参考式(3.63)
+    void ProjectCov() {
+        Mat18T J = Mat18T::Identity();
+        J.template block<3, 3>(6, 6) = Mat3T::Identity() - 0.5 * SO3::hat(dx_.template block<3, 1>(6, 0));
+        cov_ = J * cov_ * J.transpose();
+    }
+
+    /// 成员变量
+    double current_time_ = 0.0;  // 当前时间
+
+    /// 名义状态
+    VecT p_ = VecT::Zero();
+    VecT v_ = VecT::Zero();
+    SO3 R_;
+    VecT bg_ = VecT::Zero();
+    VecT ba_ = VecT::Zero();
+    VecT g_{0, 0, -9.8};
+
+    /// 误差状态
+    Vec18T dx_ = Vec18T::Zero();
+
+    /// 协方差阵
+    Mat18T cov_ = Mat18T::Identity();
+
+    /// 噪声阵
+    MotionNoiseT Q_ = MotionNoiseT::Zero();
+    OdomNoiseT odom_noise_ = OdomNoiseT::Zero();
+    GnssNoiseT gnss_noise_ = GnssNoiseT::Zero();
+
+    /// 标志位
+    bool first_gnss_ = true;  // 是否为第一个gnss数据
+
+    /// 配置项
+    Options options_;
+};
+
+using ESKFD = ESKF<double>;
+using ESKFF = ESKF<float>;
+
+template <typename S>
+bool ESKF<S>::Predict(const IMU& imu) {
+    assert(imu.timestamp_ >= current_time_);
+
+    double dt = imu.timestamp_ - current_time_;
+    if (dt > (5 * options_.imu_dt_) || dt < 0) {
+        // 时间间隔不对，可能是第一个IMU数据，没有历史信息
+        LOG(INFO) << "skip this imu because dt_ = " << dt;
+        current_time_ = imu.timestamp_;
+        return false;
+    }
+
+    // nominal state 递推
+    VecT new_p = p_ + v_ * dt + 0.5 * (R_ * (imu.acce_ - ba_)) * dt * dt + 0.5 * g_ * dt * dt;
+    VecT new_v = v_ + R_ * (imu.acce_ - ba_) * dt + g_ * dt;
+    SO3 new_R = R_ * SO3::exp((imu.gyro_ - bg_) * dt);
+
+    R_ = new_R;
+    v_ = new_v;
+    p_ = new_p;
+    // 其余状态维度不变
+
+    // error state 递推
+    // 计算运动过程雅可比矩阵 F，见(3.47)
+    // F实际上是稀疏矩阵，也可以不用矩阵形式进行相乘而是写成散装形式，这里为了教学方便，使用矩阵形式
+    Mat18T F = Mat18T::Identity();                                                 // 主对角线
+    F.template block<3, 3>(0, 3) = Mat3T::Identity() * dt;                         // p 对 v
+    F.template block<3, 3>(3, 6) = -R_.matrix() * SO3::hat(imu.acce_ - ba_) * dt;  // v对theta
+    F.template block<3, 3>(3, 12) = -R_.matrix() * dt;                             // v 对 ba
+    F.template block<3, 3>(3, 15) = Mat3T::Identity() * dt;                        // v 对 g
+    F.template block<3, 3>(6, 6) = SO3::exp(-(imu.gyro_ - bg_) * dt).matrix();     // theta 对 theta
+    F.template block<3, 3>(6, 9) = -Mat3T::Identity() * dt;                        // theta 对 bg
+
+    // mean and cov prediction
+    dx_ = F * dx_;  // 这行其实没必要算，dx_在重置之后应该为零，因此这步可以跳过，但F需要参与Cov部分计算，所以保留
+    cov_ = F * cov_.eval() * F.transpose() + Q_;
+    current_time_ = imu.timestamp_;
+    return true;
+}
+
+template <typename S>
+bool ESKF<S>::ObserveWheelSpeed(const Odom& odom) {
+    assert(odom.timestamp_ >= current_time_);
+    // odom 修正以及雅可比
+    // 使用三维的轮速观测，H为3x18，大部分为零
+    Eigen::Matrix<S, 3, 18> H = Eigen::Matrix<S, 3, 18>::Zero();
+    H.template block<3, 3>(0, 3) = Mat3T::Identity();
+
+    // 卡尔曼增益
+    Eigen::Matrix<S, 18, 3> K = cov_ * H.transpose() * (H * cov_ * H.transpose() + odom_noise_).inverse();
+
+    // velocity obs
+    double velo_l = options_.wheel_radius_ * odom.left_pulse_ / options_.circle_pulse_ * 2 * M_PI / options_.odom_span_;
+    double velo_r =
+        options_.wheel_radius_ * odom.right_pulse_ / options_.circle_pulse_ * 2 * M_PI / options_.odom_span_;
+    double average_vel = 0.5 * (velo_l + velo_r);
+
+    VecT vel_odom(average_vel, 0.0, 0.0);
+    VecT vel_world = R_ * vel_odom;
+
+    dx_ = K * (vel_world - v_);
+
+    // update cov
+    cov_ = (Mat18T::Identity() - K * H) * cov_;
+
+    UpdateAndReset();
+    return true;
+}
+
+template <typename S>
+bool ESKF<S>::ObserveGps(const GNSS& gnss) {
+    /// GNSS 观测的修正
+    assert(gnss.unix_time_ >= current_time_);
+
+    if (first_gnss_) {
+        R_ = gnss.utm_pose_.so3();
+        p_ = gnss.utm_pose_.translation();
+        first_gnss_ = false;
+        current_time_ = gnss.unix_time_;
+        return true;
+    }
+
+    assert(gnss.heading_valid_);
+    ObserveSE3(gnss.utm_pose_, options_.gnss_pos_noise_, options_.gnss_ang_noise_);
+    current_time_ = gnss.unix_time_;
+
+    return true;
+}
+
+template <typename S>
+bool ESKF<S>::ObserveSE3(const SE3& pose, double trans_noise, double ang_noise) {
+    /// 既有旋转，也有平移
+    /// 观测状态变量中的p, R，H为6x18，其余为零
+    Eigen::Matrix<S, 6, 18> H = Eigen::Matrix<S, 6, 18>::Zero();
+    H.template block<3, 3>(0, 0) = Mat3T::Identity();  // P部分
+    H.template block<3, 3>(3, 6) = Mat3T::Identity();  // R部分（3.66)
+
+    // 卡尔曼增益和更新过程
+    Vec6d noise_vec;
+    noise_vec << trans_noise, trans_noise, trans_noise, ang_noise, ang_noise, ang_noise;
+
+    Mat6d V = noise_vec.asDiagonal();
+    Eigen::Matrix<S, 18, 6> K = cov_ * H.transpose() * (H * cov_ * H.transpose() + V).inverse();
+
+    // 更新x和cov
+    Vec6d innov = Vec6d::Zero();
+    innov.template head<3>() = (pose.translation() - p_);          // 平移部分
+    innov.template tail<3>() = (R_.inverse() * pose.so3()).log();  // 旋转部分(3.67)
+
+    dx_ = K * innov;
+    cov_ = (Mat18T::Identity() - K * H) * cov_;
+
+    UpdateAndReset();
+    return true;
+}
+
+}  // namespace sad
+
+#endif  // SLAM_IN_AUTO_DRIVING_ESKF_HPP

workspace/slam_in_autonomous_driving/src/ch3/imu_integration.h

@@ -0,0 +1,60 @@
+//
+// Created by xiang on 2021/11/5.
+//
+
+#ifndef SLAM_IN_AUTO_DRIVING_IMU_INTEGRATION_H
+#define SLAM_IN_AUTO_DRIVING_IMU_INTEGRATION_H
+
+#include "common/eigen_types.h"
+#include "common/imu.h"
+#include "common/nav_state.h"
+
+namespace sad {
+
+/**
+ * 本程序演示单纯靠IMU的积分
+ */
+class IMUIntegration {
+   public:
+    IMUIntegration(const Vec3d& gravity, const Vec3d& init_bg, const Vec3d& init_ba)
+        : gravity_(gravity), bg_(init_bg), ba_(init_ba) {}
+
+    // 增加imu读数
+    void AddIMU(const IMU& imu) {
+        double dt = imu.timestamp_ - timestamp_;
+        if (dt > 0 && dt < 0.1) {
+            // 假设IMU时间间隔在0至0.1以内
+            p_ = p_ + v_ * dt + 0.5 * gravity_ * dt * dt + 0.5 * (R_ * (imu.acce_ - ba_)) * dt * dt;
+            v_ = v_ + R_ * (imu.acce_ - ba_) * dt + gravity_ * dt;
+            R_ = R_ * Sophus::SO3d::exp((imu.gyro_ - bg_) * dt);
+        }
+
+        // 更新时间
+        timestamp_ = imu.timestamp_;
+    }
+
+    /// 组成NavState
+    NavStated GetNavState() const { return NavStated(timestamp_, R_, p_, v_, bg_, ba_); }
+
+    SO3 GetR() const { return R_; }
+    Vec3d GetV() const { return v_; }
+    Vec3d GetP() const { return p_; }
+
+   private:
+    // 累计量
+    SO3 R_;
+    Vec3d v_ = Vec3d::Zero();
+    Vec3d p_ = Vec3d::Zero();
+
+    double timestamp_ = 0.0;
+
+    // 零偏，由外部设定
+    Vec3d bg_ = Vec3d::Zero();
+    Vec3d ba_ = Vec3d::Zero();
+
+    Vec3d gravity_ = Vec3d(0, 0, -9.8);  // 重力
+};
+
+}  // namespace sad
+
+#endif  // SLAM_IN_AUTO_DRIVING_IMU_INTEGRATION_H

workspace/slam_in_autonomous_driving/src/ch3/process_gnss.cc

@@ -0,0 +1,94 @@
+//
+// Created by xiang on 2022/1/4.
+//
+
+#include <glog/logging.h>
+#include <iomanip>
+#include <memory>
+
+#include "common/gnss.h"
+#include "common/io_utils.h"
+#include "tools/ui/pangolin_window.h"
+#include "utm_convert.h"
+
+DEFINE_string(txt_path, "./data/ch3/10.txt", "数据文件路径");
+
+// 以下参数仅针对本书提供的数据
+DEFINE_double(antenna_angle, 12.06, "RTK天线安装偏角（角度）");
+DEFINE_double(antenna_pox_x, -0.17, "RTK天线安装偏移X");
+DEFINE_double(antenna_pox_y, -0.20, "RTK天线安装偏移Y");
+DEFINE_bool(with_ui, true, "是否显示图形界面");
+
+/**
+ * 本程序演示如何处理GNSS数据
+ * 我们将GNSS原始读数处理成能够进行后续处理的6自由度Pose
+ * 需要处理UTM转换、RTK天线外参、坐标系转换三个步骤
+ *
+ * 我们将结果保存在文件中，然后用python脚本进行可视化
+ */
+
+int main(int argc, char** argv) {
+    google::InitGoogleLogging(argv[0]);
+    FLAGS_stderrthreshold = google::INFO;
+    FLAGS_colorlogtostderr = true;
+    google::ParseCommandLineFlags(&argc, &argv, true);
+
+    if (fLS::FLAGS_txt_path.empty()) {
+        return -1;
+    }
+
+    sad::TxtIO io(fLS::FLAGS_txt_path);
+
+    std::ofstream fout("./data/ch3/gnss_output.txt");
+    Vec2d antenna_pos(FLAGS_antenna_pox_x, FLAGS_antenna_pox_y);
+
+    auto save_result = [](std::ofstream& fout, double timestamp, const SE3& pose) {
+        auto save_vec3 = [](std::ofstream& fout, const Vec3d& v) { fout << v[0] << " " << v[1] << " " << v[2] << " "; };
+        auto save_quat = [](std::ofstream& fout, const Quatd& q) {
+            fout << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << " ";
+        };
+
+        fout << std::setprecision(18) << timestamp << " " << std::setprecision(9);
+        save_vec3(fout, pose.translation());
+        save_quat(fout, pose.unit_quaternion());
+        fout << std::endl;
+    };
+
+    std::shared_ptr<sad::ui::PangolinWindow> ui = nullptr;
+    if (FLAGS_with_ui) {
+        ui = std::make_shared<sad::ui::PangolinWindow>();
+        ui->Init();
+    }
+
+    bool first_gnss_set = false;
+    Vec3d origin = Vec3d::Zero();
+    io.SetGNSSProcessFunc([&](const sad::GNSS& gnss) {
+          sad::GNSS gnss_out = gnss;
+          if (sad::ConvertGps2UTM(gnss_out, antenna_pos, FLAGS_antenna_angle)) {
+              if (!first_gnss_set) {
+                  origin = gnss_out.utm_pose_.translation();
+                  first_gnss_set = true;
+              }
+
+              /// 减掉一个原点
+              gnss_out.utm_pose_.translation() -= origin;
+
+              save_result(fout, gnss_out.unix_time_, gnss_out.utm_pose_);
+
+              if (ui) {
+                  ui->UpdateNavState(
+                      sad::NavStated(gnss_out.unix_time_, gnss_out.utm_pose_.so3(), gnss_out.utm_pose_.translation()));
+                  usleep(1e3);
+              }
+          }
+      }).Go();
+
+    if (ui) {
+        while (!ui->ShouldQuit()) {
+            usleep(1e5);
+        }
+        ui->Quit();
+    }
+
+    return 0;
+}

workspace/slam_in_autonomous_driving/src/ch3/run_eskf_gins.cc

@@ -0,0 +1,154 @@
+//
+// Created by xiang on 2021/11/11.
+//
+
+#include "ch3/eskf.hpp"
+#include "ch3/static_imu_init.h"
+#include "common/io_utils.h"
+#include "tools/ui/pangolin_window.h"
+#include "utm_convert.h"
+
+#include <gflags/gflags.h>
+#include <glog/logging.h>
+#include <fstream>
+#include <iomanip>
+
+DEFINE_string(txt_path, "./data/ch3/10.txt", "数据文件路径");
+DEFINE_double(antenna_angle, 12.06, "RTK天线安装偏角（角度）");
+DEFINE_double(antenna_pox_x, -0.17, "RTK天线安装偏移X");
+DEFINE_double(antenna_pox_y, -0.20, "RTK天线安装偏移Y");
+DEFINE_bool(with_ui, true, "是否显示图形界面");
+DEFINE_bool(with_odom, false, "是否加入轮速计信息");
+
+/**
+ * 本程序演示使用RTK+IMU进行组合导航
+ */
+int main(int argc, char** argv) {
+    google::InitGoogleLogging(argv[0]);
+    FLAGS_stderrthreshold = google::INFO;
+    FLAGS_colorlogtostderr = true;
+    google::ParseCommandLineFlags(&argc, &argv, true);
+
+    if (fLS::FLAGS_txt_path.empty()) {
+        return -1;
+    }
+
+    // 初始化器
+    sad::StaticIMUInit imu_init;  // 使用默认配置
+    sad::ESKFD eskf;
+
+    sad::TxtIO io(FLAGS_txt_path);
+    Vec2d antenna_pos(FLAGS_antenna_pox_x, FLAGS_antenna_pox_y);
+
+    auto save_vec3 = [](std::ofstream& fout, const Vec3d& v) { fout << v[0] << " " << v[1] << " " << v[2] << " "; };
+    auto save_quat = [](std::ofstream& fout, const Quatd& q) {
+        fout << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << " ";
+    };
+
+    auto save_result = [&save_vec3, &save_quat](std::ofstream& fout, const sad::NavStated& save_state) {
+        fout << std::setprecision(18) << save_state.timestamp_ << " " << std::setprecision(9);
+        save_vec3(fout, save_state.p_);
+        save_quat(fout, save_state.R_.unit_quaternion());
+        save_vec3(fout, save_state.v_);
+        save_vec3(fout, save_state.bg_);
+        save_vec3(fout, save_state.ba_);
+        fout << std::endl;
+    };
+
+    std::ofstream fout("./data/ch3/gins.txt");
+    bool imu_inited = false, gnss_inited = false;
+
+    std::shared_ptr<sad::ui::PangolinWindow> ui = nullptr;
+    if (FLAGS_with_ui) {
+        ui = std::make_shared<sad::ui::PangolinWindow>();
+        ui->Init();
+    }
+
+    /// 设置各类回调函数
+    bool first_gnss_set = false;
+    Vec3d origin = Vec3d::Zero();
+
+    io.SetIMUProcessFunc([&](const sad::IMU& imu) {
+          /// IMU 处理函数
+          if (!imu_init.InitSuccess()) {
+              imu_init.AddIMU(imu);
+              return;
+          }
+
+          /// 需要IMU初始化
+          if (!imu_inited) {
+              // 读取初始零偏，设置ESKF
+              sad::ESKFD::Options options;
+              // 噪声由初始化器估计
+              options.gyro_var_ = sqrt(imu_init.GetCovGyro()[0]);
+              options.acce_var_ = sqrt(imu_init.GetCovAcce()[0]);
+              eskf.SetInitialConditions(options, imu_init.GetInitBg(), imu_init.GetInitBa(), imu_init.GetGravity());
+              imu_inited = true;
+              return;
+          }
+
+          if (!gnss_inited) {
+              /// 等待有效的RTK数据
+              return;
+          }
+
+          /// GNSS 也接收到之后，再开始进行预测
+          eskf.Predict(imu);
+
+          /// predict就会更新ESKF，所以此时就可以发送数据
+          auto state = eskf.GetNominalState();
+          if (ui) {
+              ui->UpdateNavState(state);
+          }
+
+          /// 记录数据以供绘图
+          save_result(fout, state);
+
+          usleep(1e3);
+      })
+        .SetGNSSProcessFunc([&](const sad::GNSS& gnss) {
+            /// GNSS 处理函数
+            if (!imu_inited) {
+                return;
+            }
+
+            sad::GNSS gnss_convert = gnss;
+            if (!sad::ConvertGps2UTM(gnss_convert, antenna_pos, FLAGS_antenna_angle) || !gnss_convert.heading_valid_) {
+                return;
+            }
+
+            /// 去掉原点
+            if (!first_gnss_set) {
+                origin = gnss_convert.utm_pose_.translation();
+                first_gnss_set = true;
+            }
+            gnss_convert.utm_pose_.translation() -= origin;
+
+            // 要求RTK heading有效，才能合入ESKF
+            eskf.ObserveGps(gnss_convert);
+
+            auto state = eskf.GetNominalState();
+            if (ui) {
+                ui->UpdateNavState(state);
+            }
+            save_result(fout, state);
+
+            gnss_inited = true;
+        })
+        .SetOdomProcessFunc([&](const sad::Odom& odom) {
+            /// Odom 处理函数，本章Odom只给初始化使用
+            imu_init.AddOdom(odom);
+            if (FLAGS_with_odom && imu_inited && gnss_inited) {
+                eskf.ObserveWheelSpeed(odom);
+            }
+        })
+        .Go();
+
+    while (ui && !ui->ShouldQuit()) {
+        usleep(1e5);
+    }
+    if (ui) {
+        ui->Quit();
+    }
+    return 0;
+}

workspace/slam_in_autonomous_driving/src/ch3/run_imu_integration.cc

@@ -0,0 +1,77 @@
+//
+// Created by xiang on 2021/11/5.
+//
+
+#include <glog/logging.h>
+#include <iomanip>
+
+#include "ch3/imu_integration.h"
+#include "common/io_utils.h"
+#include "tools/ui/pangolin_window.h"
+
+DEFINE_string(imu_txt_path, "./data/ch3/10.txt", "数据文件路径");
+DEFINE_bool(with_ui, true, "是否显示图形界面");
+
+/// 本程序演示如何对IMU进行直接积分
+/// 该程序需要输入data/ch3/下的文本文件，同时它将状态输出到data/ch3/state.txt中，在UI中也可以观察到车辆运动
+int main(int argc, char** argv) {
+    google::InitGoogleLogging(argv[0]);
+    FLAGS_stderrthreshold = google::INFO;
+    FLAGS_colorlogtostderr = true;
+    google::ParseCommandLineFlags(&argc, &argv, true);
+
+    if (FLAGS_imu_txt_path.empty()) {
+        return -1;
+    }
+
+    sad::TxtIO io(FLAGS_imu_txt_path);
+
+    // 该实验中，我们假设零偏已知
+    Vec3d gravity(0, 0, -9.8);  // 重力方向
+    Vec3d init_bg(00.000224886, -7.61038e-05, -0.000742259);
+    Vec3d init_ba(-0.165205, 0.0926887, 0.0058049);
+
+    sad::IMUIntegration imu_integ(gravity, init_bg, init_ba);
+
+    std::shared_ptr<sad::ui::PangolinWindow> ui = nullptr;
+    if (FLAGS_with_ui) {
+        ui = std::make_shared<sad::ui::PangolinWindow>();
+        ui->Init();
+    }
+
+    /// 记录结果
+    auto save_result = [](std::ofstream& fout, double timestamp, const Sophus::SO3d& R, const Vec3d& v,
+                          const Vec3d& p) {
+        auto save_vec3 = [](std::ofstream& fout, const Vec3d& v) { fout << v[0] << " " << v[1] << " " << v[2] << " "; };
+        auto save_quat = [](std::ofstream& fout, const Quatd& q) {
+            fout << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << " ";
+        };
+
+        fout << std::setprecision(18) << timestamp << " " << std::setprecision(9);
+        save_vec3(fout, p);
+        save_quat(fout, R.unit_quaternion());
+        save_vec3(fout, v);
+        fout << std::endl;
+    };
+
+    std::ofstream fout("./data/ch3/state.txt");
+    io.SetIMUProcessFunc([&imu_integ, &save_result, &fout, &ui](const sad::IMU& imu) {
+          imu_integ.AddIMU(imu);
+          save_result(fout, imu.timestamp_, imu_integ.GetR(), imu_integ.GetV(), imu_integ.GetP());
+          if (ui) {
+              ui->UpdateNavState(imu_integ.GetNavState());
+              usleep(1e2);
+          }
+      }).Go();
+
+    // 打开了可视化的话，等待界面退出
+    while (ui && !ui->ShouldQuit()) {
+        usleep(1e4);
+    }
+
+    if (ui) {
+        ui->Quit();
+    }
+
+    return 0;
+}

workspace/slam_in_autonomous_driving/src/ch3/static_imu_init.cc

@@ -0,0 +1,104 @@
+//
+// Created by xiang on 2021/11/11.
+//
+
+#include "ch3/static_imu_init.h"
+#include "common/math_utils.h"
+
+#include <glog/logging.h>
+
+namespace sad {
+
+bool StaticIMUInit::AddIMU(const IMU& imu) {
+    if (init_success_) {
+        return true;
+    }
+
+    if (options_.use_speed_for_static_checking_ && !is_static_) {
+        LOG(WARNING) << "等待车辆静止";
+        init_imu_deque_.clear();
+        return false;
+    }
+
+    if (init_imu_deque_.empty()) {
+        // 记录初始静止时间
+        init_start_time_ = imu.timestamp_;
+    }
+
+    // 记入初始化队列
+    init_imu_deque_.push_back(imu);
+
+    double init_time = imu.timestamp_ - init_start_time_;  // 初始化经过时间
+    if (init_time > options_.init_time_seconds_) {
+        // 尝试初始化逻辑
+        TryInit();
+    }
+
+    // 维持初始化队列长度
+    while (init_imu_deque_.size() > options_.init_imu_queue_max_size_) {
+        init_imu_deque_.pop_front();
+    }
+
+    current_time_ = imu.timestamp_;
+    return false;
+}
+
+bool StaticIMUInit::AddOdom(const Odom& odom) {
+    // 判断车辆是否静止
+    if (init_success_) {
+        return true;
+    }
+
+    if (odom.left_pulse_ < options_.static_odom_pulse_ && odom.right_pulse_ < options_.static_odom_pulse_) {
+        is_static_ = true;
+    } else {
+        is_static_ = false;
+    }
+
+    current_time_ = odom.timestamp_;
+    return true;
+}
+
+bool StaticIMUInit::TryInit() {
+    if (init_imu_deque_.size() < 10) {
+        return false;
+    }
+
+    // 计算均值和方差
+    Vec3d mean_gyro, mean_acce;
+    math::ComputeMeanAndCovDiag(init_imu_deque_, mean_gyro, cov_gyro_, [](const IMU& imu) { return imu.gyro_; });
+    math::ComputeMeanAndCovDiag(init_imu_deque_, mean_acce, cov_acce_, [this](const IMU& imu) { return imu.acce_; });
+
+    // 以acce均值为方向，取9.8长度为重力
+    LOG(INFO) << "mean acce: " << mean_acce.transpose();
+    gravity_ = -mean_acce / mean_acce.norm() * options_.gravity_norm_;
+
+    // 重新计算加计的协方差
+    math::ComputeMeanAndCovDiag(init_imu_deque_, mean_acce, cov_acce_,
+                                [this](const IMU& imu) { return imu.acce_ + gravity_; });
+
+    // 检查IMU噪声
+    if (cov_gyro_.norm() > options_.max_static_gyro_var) {
+        LOG(ERROR) << "陀螺仪测量噪声太大" << cov_gyro_.norm() << " > " << options_.max_static_gyro_var;
+        return false;
+    }
+
+    if (cov_acce_.norm() > options_.max_static_acce_var) {
+        LOG(ERROR) << "加计测量噪声太大" << cov_acce_.norm() << " > " << options_.max_static_acce_var;
+        return false;
+    }
+
+    // 估计测量噪声和零偏
+    init_bg_ = mean_gyro;
+    init_ba_ = mean_acce;
+
+    LOG(INFO) << "IMU 初始化成功，初始化时间= " << current_time_ - init_start_time_ << ", bg = " << init_bg_.transpose()
+              << ", ba = " << init_ba_.transpose() << ", gyro sq = " << cov_gyro_.transpose()
+              << ", acce sq = " << cov_acce_.transpose() << ", grav = " << gravity_.transpose()
+              << ", norm: " << gravity_.norm();
+    LOG(INFO) << "mean gyro: " << mean_gyro.transpose() << " acce: " << mean_acce.transpose();
+    init_success_ = true;
+    return true;
+}
+
+}  // namespace sad

workspace/slam_in_autonomous_driving/src/ch3/static_imu_init.h

@@ -0,0 +1,74 @@
+//
+// Created by xiang on 2021/11/11.
+//
+
+#ifndef SLAM_IN_AUTO_DRIVING_STATIC_IMU_INIT_H
+#define SLAM_IN_AUTO_DRIVING_STATIC_IMU_INIT_H
+
+#include "common/eigen_types.h"
+#include "common/imu.h"
+#include "common/odom.h"
+
+#include <deque>
+
+namespace sad {
+
+/**
+ * IMU水平静止状态下初始化器
+ * 使用方法：调用AddIMU, AddOdom添加数据，使用InitSuccess获取初始化是否成功
+ * 成功后，使用各Get函数获取内部参数
+ *
+ * 初始化器在每次调用AddIMU时尝试对系统进行初始化。在有odom的场合，初始化要求odom轮速读数接近零；没有时，假设车辆初期静止。
+ * 初始化器收集一段时间内的IMU读数，按照书本3.5.4节估计初始零偏和噪声参数，提供给ESKF或者其他滤波器
+ */
+class StaticIMUInit {
+   public:
+    struct Options {
+        Options() {}
+        double init_time_seconds_ = 10.0;     // 静止时间
+        int init_imu_queue_max_size_ = 2000;  // 初始化IMU队列最大长度
+        int static_odom_pulse_ = 5;           // 静止时轮速计输出噪声
+        double max_static_gyro_var = 0.5;     // 静态下陀螺测量方差
+        double max_static_acce_var = 0.05;    // 静态下加计测量方差
+        double gravity_norm_ = 9.81;          // 重力大小
+        bool use_speed_for_static_checking_ = true;  // 是否使用odom来判断车辆静止（部分数据集没有odom选项）
+    };
+
+    /// 构造函数
+    StaticIMUInit(Options options = Options()) : options_(options) {}
+
+    /// 添加IMU数据
+    bool AddIMU(const IMU& imu);
+    /// 添加轮速数据
+    bool AddOdom(const Odom& odom);
+
+    /// 判定初始化是否成功
+    bool InitSuccess() const { return init_success_; }
+
+    /// 获取各Cov, bias, gravity
+    Vec3d GetCovGyro() const { return cov_gyro_; }
+    Vec3d GetCovAcce() const { return cov_acce_; }
+    Vec3d GetInitBg() const { return init_bg_; }
+    Vec3d GetInitBa() const { return init_ba_; }
+    Vec3d GetGravity() const { return gravity_; }
+
+   private:
+    /// 尝试对系统初始化
+    bool TryInit();
+
+    Options options_;                 // 选项信息
+    bool init_success_ = false;       // 初始化是否成功
+    Vec3d cov_gyro_ = Vec3d::Zero();  // 陀螺测量噪声协方差（初始化时评估）
+    Vec3d cov_acce_ = Vec3d::Zero();  // 加计测量噪声协方差（初始化时评估）
+    Vec3d init_bg_ = Vec3d::Zero();   // 陀螺初始零偏
+    Vec3d init_ba_ = Vec3d::Zero();   // 加计初始零偏
+    Vec3d gravity_ = Vec3d::Zero();   // 重力
+    bool is_static_ = false;          // 标志车辆是否静止
+    std::deque<IMU> init_imu_deque_;  // 初始化用的数据
+    double current_time_ = 0.0;       // 当前时间
+    double init_start_time_ = 0.0;    // 静止的初始时间
+};
+
+}  // namespace sad
+
+#endif  // SLAM_IN_AUTO_DRIVING_STATIC_IMU_INIT_H

workspace/slam_in_autonomous_driving/src/ch3/utm_convert.cc

@@ -0,0 +1,84 @@
+//
+// Created by xiang on 2022/1/4.
+//
+
+#include "ch3/utm_convert.h"
+#include "common/math_utils.h"
+#include "utm_convert/utm.h"
+
+#include <glog/logging.h>
+
+namespace sad {
+
+bool LatLon2UTM(const Vec2d& latlon, UTMCoordinate& utm_coor) {
+    long zone = 0;
+    char char_north = 0;
+    long ret = Convert_Geodetic_To_UTM(latlon[0] * math::kDEG2RAD, latlon[1] * math::kDEG2RAD, &zone, &char_north,
+                                       &utm_coor.xy_[0], &utm_coor.xy_[1]);
+    utm_coor.zone_ = (int)zone;
+    utm_coor.north_ = char_north == 'N';
+
+    return ret == 0;
+}
+
+bool UTM2LatLon(const UTMCoordinate& utm_coor, Vec2d& latlon) {
+    bool ret = Convert_UTM_To_Geodetic((long)utm_coor.zone_, utm_coor.north_ ? 'N' : 'S', utm_coor.xy_[0],
+                                       utm_coor.xy_[1], &latlon[0], &latlon[1]);
+    latlon *= math::kRAD2DEG;
+    return ret == 0;
+}
+
+bool ConvertGps2UTM(GNSS& gps_msg, const Vec2d& antenna_pos, const double& antenna_angle, const Vec3d& map_origin) {
+    /// 经纬高转换为UTM
+    UTMCoordinate utm_rtk;
+    if (!LatLon2UTM(gps_msg.lat_lon_alt_.head<2>(), utm_rtk)) {
+        return false;
+    }
+    utm_rtk.z_ = gps_msg.lat_lon_alt_[2];
+
+    /// GPS heading 转成弧度
+    double heading = 0;
+    if (gps_msg.heading_valid_) {
+        heading = (90 - gps_msg.heading_) * math::kDEG2RAD;  // 北东地转到东北天
+    }
+
+    /// TWG 转到 TWB
+    SE3 TBG(SO3::rotZ(antenna_angle * math::kDEG2RAD), Vec3d(antenna_pos[0], antenna_pos[1], 0));
+    SE3 TGB = TBG.inverse();
+
+    /// 若指明地图原点，则减去地图原点
+    double x = utm_rtk.xy_[0] - map_origin[0];
+    double y = utm_rtk.xy_[1] - map_origin[1];
+    double z = utm_rtk.z_ - map_origin[2];
+    SE3 TWG(SO3::rotZ(heading), Vec3d(x, y, z));
+    SE3 TWB = TWG * TGB;
+
+    gps_msg.utm_valid_ = true;
+    gps_msg.utm_.xy_[0] = TWB.translation().x();
+    gps_msg.utm_.xy_[1] = TWB.translation().y();
+    gps_msg.utm_.z_ = TWB.translation().z();
+
+    if (gps_msg.heading_valid_) {
+        // 组装为带旋转的位姿
+        gps_msg.utm_pose_ = TWB;
+    } else {
+        // 组装为仅有平移的SE3
+        // 注意当安装偏移存在时，并不能实际推出车辆位姿
+        gps_msg.utm_pose_ = SE3(SO3(), TWB.translation());
+    }
+
+    return true;
+}
+
+bool ConvertGps2UTMOnlyTrans(GNSS& gps_msg) {
+    /// 经纬高转换为UTM
+    UTMCoordinate utm_rtk;
+    LatLon2UTM(gps_msg.lat_lon_alt_.head<2>(), utm_rtk);
+    gps_msg.utm_valid_ = true;
+    gps_msg.utm_.xy_ = utm_rtk.xy_;
+    gps_msg.utm_.z_ = gps_msg.lat_lon_alt_[2];
+    gps_msg.utm_pose_ = SE3(SO3(), Vec3d(gps_msg.utm_.xy_[0], gps_msg.utm_.xy_[1], gps_msg.utm_.z_));
+    return true;
+}
+
+}  // namespace sad

workspace/slam_in_autonomous_driving/src/ch3/utm_convert.h

@@ -0,0 +1,49 @@
+//
+// Created by xiang on 2022/1/4.
+//
+
+#ifndef SLAM_IN_AUTO_DRIVING_UTM_CONVERT_H
+#define SLAM_IN_AUTO_DRIVING_UTM_CONVERT_H
+
+#include "common/gnss.h"
+
+namespace sad {
+
+/**
+ * 计算本书的GNSS读数对应的UTM pose和六自由度Pose
+ * @param gnss_reading  输入gnss读数
+ * @param antenna_pos   安装位置
+ * @param antenna_angle 安装偏角
+ * @param map_origin    地图原点，指定时，将从UTM位置中减掉坐标原点
+ * @return
+ */
+bool ConvertGps2UTM(GNSS& gnss_reading, const Vec2d& antenna_pos, const double& antenna_angle,
+                    const Vec3d& map_origin = Vec3d::Zero());
+
+/**
+ * 仅转换平移部分的经纬度，不作外参和角度处理
+ * @param gnss_reading
+ * @return
+ */
+bool ConvertGps2UTMOnlyTrans(GNSS& gnss_reading);
+
+/**
+ * 经纬度转UTM
+ * NOTE 经纬度单位为度数
+ * @param latlon
+ * @param utm_coor
+ * @return
+ */
+bool LatLon2UTM(const Vec2d& latlon, UTMCoordinate& utm_coor);
+
+/**
+ * UTM转经纬度
+ * @param utm_coor
+ * @param latlon
+ * @return
+ */
+bool UTM2LatLon(const UTMCoordinate& utm_coor, Vec2d& latlon);
+
+}  // namespace sad
+
+#endif  // SLAM_IN_AUTO_DRIVING_UTM_CONVERT_H

workspace/slam_in_autonomous_driving/src/ch4/CMakeLists.txt

@@ -0,0 +1,19 @@
+add_library(${PROJECT_NAME}.ch4
+        gins_pre_integ.cc
+        imu_preintegration.cc
+        g2o_types.cc
+        )
+
+ADD_EXECUTABLE(test_preintegration test_preintegration.cc)
+ADD_TEST(test_preintegration test_preintegration)
+
+target_link_libraries(test_preintegration
+        gtest pthread glog gflags ${PROJECT_NAME}.ch4 ${PROJECT_NAME}.ch3 ${PROJECT_NAME}.common
+        )
+
+add_executable(run_gins_pre_integ run_gins_pre_integ.cc)
+target_link_libraries(run_gins_pre_integ
+        ${PROJECT_NAME}.ch3
+        ${PROJECT_NAME}.ch4
+        ${g2o_libs}
+        )

workspace/slam_in_autonomous_driving/src/ch4/g2o_types.cc

@@ -0,0 +1,138 @@
+//
+// Created by xiang on 23-1-19.
+//
+
+#include "ch4/g2o_types.h"
+#include "common/g2o_types.h"
+
+namespace sad {
+
+EdgeInertial::EdgeInertial(std::shared_ptr<IMUPreintegration> preinteg, const Vec3d& gravity, double weight)
+    : preint_(preinteg), dt_(preinteg->dt_) {
+    resize(6);  // 6个关联顶点
+    grav_ = gravity;
+    setInformation(preinteg->cov_.inverse() * weight);
+}
+
+void EdgeInertial::computeError() {
+    auto* p1 = dynamic_cast<const VertexPose*>(_vertices[0]);
+    auto* v1 = dynamic_cast<const VertexVelocity*>(_vertices[1]);
+    auto* bg1 = dynamic_cast<const VertexGyroBias*>(_vertices[2]);
+    auto* ba1 = dynamic_cast<const VertexAccBias*>(_vertices[3]);
+    auto* p2 = dynamic_cast<const VertexPose*>(_vertices[4]);
+    auto* v2 = dynamic_cast<const VertexVelocity*>(_vertices[5]);
+
+    Vec3d bg = bg1->estimate();
+    Vec3d ba = ba1->estimate();
+
+    const SO3 dR = preint_->GetDeltaRotation(bg);
+    const Vec3d dv = preint_->GetDeltaVelocity(bg, ba);
+    const Vec3d dp = preint_->GetDeltaPosition(bg, ba);
+
+    /// 预积分误差项（4.41）
+    const Vec3d er = (dR.inverse() * p1->estimate().so3().inverse() * p2->estimate().so3()).log();
+    Mat3d RiT = p1->estimate().so3().inverse().matrix();
+    const Vec3d ev = RiT * (v2->estimate() - v1->estimate() - grav_ * dt_) - dv;
+    const Vec3d ep = RiT * (p2->estimate().translation() - p1->estimate().translation() - v1->estimate() * dt_ -
+                            grav_ * dt_ * dt_ / 2) -
+                     dp;
+    _error << er, ev, ep;
+}
+
+void EdgeInertial::linearizeOplus() {
+    auto* p1 = dynamic_cast<const VertexPose*>(_vertices[0]);
+    auto* v1 = dynamic_cast<const VertexVelocity*>(_vertices[1]);
+    auto* bg1 = dynamic_cast<const VertexGyroBias*>(_vertices[2]);
+    auto* ba1 = dynamic_cast<const VertexAccBias*>(_vertices[3]);
+    auto* p2 = dynamic_cast<const VertexPose*>(_vertices[4]);
+    auto* v2 = dynamic_cast<const VertexVelocity*>(_vertices[5]);
+
+    Vec3d bg = bg1->estimate();
+    Vec3d ba = ba1->estimate();
+    Vec3d dbg = bg - preint_->bg_;
+
+    // 一些中间符号
+    const SO3 R1 = p1->estimate().so3();
+    const SO3 R1T = R1.inverse();
+    const SO3 R2 = p2->estimate().so3();
+
+    auto dR_dbg = preint_->dR_dbg_;
+    auto dv_dbg = preint_->dV_dbg_;
+    auto dp_dbg = preint_->dP_dbg_;
+    auto dv_dba = preint_->dV_dba_;
+    auto dp_dba = preint_->dP_dba_;
+
+    // 估计值
+    Vec3d vi = v1->estimate();
+    Vec3d vj = v2->estimate();
+    Vec3d pi = p1->estimate().translation();
+    Vec3d pj = p2->estimate().translation();
+
+    const SO3 dR = preint_->GetDeltaRotation(bg);
+    const SO3 eR = SO3(dR).inverse() * R1T * R2;
+    const Vec3d er = eR.log();
+    const Mat3d invJr = SO3::jr_inv(eR);
+
+    /// 雅可比矩阵
+    /// 注意有3个index, 顶点的，自己误差的，顶点内部变量的
+    /// 变量顺序：pose1(R1,p1), v1, bg1, ba1, pose2(R2,p2), v2
+    /// 残差顺序：eR, ev, ep，残差顺序为行，变量顺序为列
+
+    //       | R1 | p1 | v1 | bg1 | ba1 | R2 | p2 | v2 |
+    //  vert | 0       | 1  | 2   | 3   | 4       | 5  |
+    //  col  | 0    3  | 0  | 0   | 0   | 0    3  | 0  |
+    //    row
+    //  eR 0 |
+    //  ev 3 |
+    //  ep 6 |
+
+    /// 残差对R1, 9x3
+    _jacobianOplus[0].setZero();
+    // dR/dR1, 4.42
+    _jacobianOplus[0].block<3, 3>(0, 0) = -invJr * (R2.inverse() * R1).matrix();
+    // dv/dR1, 4.47
+    _jacobianOplus[0].block<3, 3>(3, 0) = SO3::hat(R1T * (vj - vi - grav_ * dt_));
+    // dp/dR1, 4.48d
+    _jacobianOplus[0].block<3, 3>(6, 0) = SO3::hat(R1T * (pj - pi - v1->estimate() * dt_ - 0.5 * grav_ * dt_ * dt_));
+
+    /// 残差对p1, 9x3
+    // dp/dp1, 4.48a
+    _jacobianOplus[0].block<3, 3>(6, 3) = -R1T.matrix();
+
+    /// 残差对v1, 9x3
+    _jacobianOplus[1].setZero();
+    // dv/dv1, 4.46a
+    _jacobianOplus[1].block<3, 3>(3, 0) = -R1T.matrix();
+    // dp/dv1, 4.48c
+    _jacobianOplus[1].block<3, 3>(6, 0) = -R1T.matrix() * dt_;
+
+    /// 残差对bg1
+    _jacobianOplus[2].setZero();
+    // dR/dbg1, 4.45
+    _jacobianOplus[2].block<3, 3>(0, 0) = -invJr * eR.inverse().matrix() * SO3::jr((dR_dbg * dbg).eval()) * dR_dbg;
+    // dv/dbg1
+    _jacobianOplus[2].block<3, 3>(3, 0) = -dv_dbg;
+    // dp/dbg1
+    _jacobianOplus[2].block<3, 3>(6, 0) = -dp_dbg;
+
+    /// 残差对ba1
+    _jacobianOplus[3].setZero();
+    // dv/dba1
+    _jacobianOplus[3].block<3, 3>(3, 0) = -dv_dba;
+    // dp/dba1
+    _jacobianOplus[3].block<3, 3>(6, 0) = -dp_dba;
+
+    /// 残差对pose2
+    _jacobianOplus[4].setZero();
+    // dr/dr2, 4.43
+    _jacobianOplus[4].block<3, 3>(0, 0) = invJr;
+    // dp/dp2, 4.48b
+    _jacobianOplus[4].block<3, 3>(6, 3) = R1T.matrix();
+
+    /// 残差对v2
+    _jacobianOplus[5].setZero();
+    // dv/dv2, 4,46b
+    _jacobianOplus[5].block<3, 3>(3, 0) = R1T.matrix();  // OK
+}
+
+}  // namespace sad

workspace/slam_in_autonomous_driving/src/ch4/g2o_types.h

@@ -0,0 +1,64 @@
+//
+// Created by xiang on 23-1-19.
+//
+
+#ifndef SLAM_IN_AUTO_DRIVING_CH4_G2O_TYPES_H
+#define SLAM_IN_AUTO_DRIVING_CH4_G2O_TYPES_H
+
+#include <g2o/core/base_binary_edge.h>
+#include <g2o/core/base_multi_edge.h>
+#include <g2o/core/base_unary_edge.h>
+#include <g2o/core/base_vertex.h>
+#include <g2o/core/robust_kernel.h>
+
+#include "ch4/imu_preintegration.h"
+#include "common/eigen_types.h"
+
+namespace sad {
+
+/// 与预积分相关的vertex, edge
+/**
+ * 预积分边
+ * 连接6个顶点：上一帧的pose, v, bg, ba，下一帧的pose, v
+ * 观测量为9维，即预积分残差, 顺序：R, v, p
+ * information从预积分类中获取，构造函数中计算
+ */
+class EdgeInertial : public g2o::BaseMultiEdge<9, Vec9d> {
+   public:
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+    /**
+     * 构造函数中需要指定预积分类对象
+     * @param preinteg  预积分对象指针
+     * @param gravity   重力矢量
+     * @param weight    权重
+     */
+    EdgeInertial(std::shared_ptr<IMUPreintegration> preinteg, const Vec3d& gravity, double weight = 1.0);
+
+    bool read(std::istream& is) override { return false; }
+    bool write(std::ostream& os) const override { return false; }
+
+    void computeError() override;
+    void linearizeOplus() override;
+
+    Eigen::Matrix<double, 24, 24> GetHessian() {
+        linearizeOplus();
+        Eigen::Matrix<double, 9, 24> J;
+        J.block<9, 6>(0, 0) = _jacobianOplus[0];
+        J.block<9, 3>(0, 6) = _jacobianOplus[1];
+        J.block<9, 3>(0, 9) = _jacobianOplus[2];
+        J.block<9, 3>(0, 12) = _jacobianOplus[3];
+        J.block<9, 6>(0, 15) = _jacobianOplus[4];
+        J.block<9, 3>(0, 21) = _jacobianOplus[5];
+        return J.transpose() * information() * J;
+    }
+
+   private:
+    const double dt_;
+    std::shared_ptr<IMUPreintegration> preint_ = nullptr;
+    Vec3d grav_;
+};
+
+}  // namespace sad
+
+#endif  // SLAM_IN_AUTO_DRIVING_G2O_TYPES_H

workspace/slam_in_autonomous_driving/src/ch4/gins_pre_integ.cc

@@ -0,0 +1,266 @@
+//
+// Created by xiang on 2021/7/19.
+//
+
+#include "ch4/gins_pre_integ.h"
+#include "ch4/g2o_types.h"
+#include "common/g2o_types.h"
+
+#include <glog/logging.h>
+
+#include <g2o/core/block_solver.h>
+#include <g2o/core/optimization_algorithm_levenberg.h>
+#include <g2o/core/robust_kernel.h>
+#include <g2o/solvers/eigen/linear_solver_eigen.h>
+
+namespace sad {
+
+void GinsPreInteg::AddImu(const IMU& imu) {
+    if (first_gnss_received_ && first_imu_received_) {
+        pre_integ_->Integrate(imu, imu.timestamp_ - last_imu_.timestamp_);
+    }
+
+    first_imu_received_ = true;
+    last_imu_ = imu;
+    current_time_ = imu.timestamp_;
+}
+
+void GinsPreInteg::SetOptions(sad::GinsPreInteg::Options options) {
+    double bg_rw2 = 1.0 / (options_.bias_gyro_var_ * options_.bias_gyro_var_);
+    options_.bg_rw_info_.diagonal() << bg_rw2, bg_rw2, bg_rw2;
+    double ba_rw2 = 1.0 / (options_.bias_acce_var_ * options_.bias_acce_var_);
+    options_.ba_rw_info_.diagonal() << ba_rw2, ba_rw2, ba_rw2;
+
+    double gp2 = options_.gnss_pos_noise_ * options_.gnss_pos_noise_;
+    double gh2 = options_.gnss_height_noise_ * options_.gnss_height_noise_;
+    double ga2 = options_.gnss_ang_noise_ * options_.gnss_ang_noise_;
+
+    options_.gnss_info_.diagonal() << 1.0 / ga2, 1.0 / ga2, 1.0 / ga2, 1.0 / gp2, 1.0 / gp2, 1.0 / gh2;
+    pre_integ_ = std::make_shared<IMUPreintegration>(options_.preinteg_options_);
+
+    double o2 = 1.0 / (options_.odom_var_ * options_.odom_var_);
+    options_.odom_info_.diagonal() << o2, o2, o2;
+
+    prior_info_.block<6, 6>(9, 9) = Mat6d ::Identity() * 1e6;
+
+    if (this_frame_) {
+        this_frame_->bg_ = options_.preinteg_options_.init_bg_;
+        this_frame_->ba_ = options_.preinteg_options_.init_ba_;
+    }
+}
+
+void GinsPreInteg::AddGnss(const GNSS& gnss) {
+    this_frame_ = std::make_shared<NavStated>(current_time_);
+    this_gnss_ = gnss;
+
+    if (!first_gnss_received_) {
+        if (!gnss.heading_valid_) {
+            // 要求首个GNSS必须有航向
+            return;
+        }
+
+        // 首个gnss信号，将初始pose设置为该gnss信号
+        this_frame_->timestamp_ = gnss.unix_time_;
+        this_frame_->p_ = gnss.utm_pose_.translation();
+        this_frame_->R_ = gnss.utm_pose_.so3();
+        this_frame_->v_.setZero();
+        this_frame_->bg_ = options_.preinteg_options_.init_bg_;
+        this_frame_->ba_ = options_.preinteg_options_.init_ba_;
+
+        pre_integ_ = std::make_shared<IMUPreintegration>(options_.preinteg_options_);
+
+        last_frame_ = this_frame_;
+        last_gnss_ = this_gnss_;
+        first_gnss_received_ = true;
+        current_time_ = gnss.unix_time_;
+        return;
+    }
+
+    // 积分到GNSS时刻
+    pre_integ_->Integrate(last_imu_, gnss.unix_time_ - current_time_);
+
+    current_time_ = gnss.unix_time_;
+    *this_frame_ = pre_integ_->Predict(*last_frame_, options_.gravity_);
+
+    Optimize();
+
+    last_frame_ = this_frame_;
+    last_gnss_ = this_gnss_;
+}
+
+void GinsPreInteg::AddOdom(const sad::Odom& odom) {
+    last_odom_ = odom;
+    last_odom_set_ = true;
+}
+
+void GinsPreInteg::Optimize() {
+    if (pre_integ_->dt_ < 1e-3) {
+        // 未得到积分
+        return;
+    }
+
+    using BlockSolverType = g2o::BlockSolverX;
+    using LinearSolverType = g2o::LinearSolverEigen<BlockSolverType::PoseMatrixType>;
+
+    auto* solver = new g2o::OptimizationAlgorithmLevenberg(
+        g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
+    g2o::SparseOptimizer optimizer;
+    optimizer.setAlgorithm(solver);
+
+    // 上时刻顶点， pose, v, bg, ba
+    auto v0_pose = new VertexPose();
+    v0_pose->setId(0);
+    v0_pose->setEstimate(last_frame_->GetSE3());
+    optimizer.addVertex(v0_pose);
+
+    auto v0_vel = new VertexVelocity();
+    v0_vel->setId(1);
+    v0_vel->setEstimate(last_frame_->v_);
+    optimizer.addVertex(v0_vel);
+
+    auto v0_bg = new VertexGyroBias();
+    v0_bg->setId(2);
+    v0_bg->setEstimate(last_frame_->bg_);
+    optimizer.addVertex(v0_bg);
+
+    auto v0_ba = new VertexAccBias();
+    v0_ba->setId(3);
+    v0_ba->setEstimate(last_frame_->ba_);
+    optimizer.addVertex(v0_ba);
+
+    // 本时刻顶点，pose, v, bg, ba
+    auto v1_pose = new VertexPose();
+    v1_pose->setId(4);
+    v1_pose->setEstimate(this_frame_->GetSE3());
+    optimizer.addVertex(v1_pose);
+
+    auto v1_vel = new VertexVelocity();
+    v1_vel->setId(5);
+    v1_vel->setEstimate(this_frame_->v_);
+    optimizer.addVertex(v1_vel);
+
+    auto v1_bg = new VertexGyroBias();
+    v1_bg->setId(6);
+    v1_bg->setEstimate(this_frame_->bg_);
+    optimizer.addVertex(v1_bg);
+
+    auto v1_ba = new VertexAccBias();
+    v1_ba->setId(7);
+    v1_ba->setEstimate(this_frame_->ba_);
+    optimizer.addVertex(v1_ba);
+
+    // 预积分边
+    auto edge_inertial = new EdgeInertial(pre_integ_, options_.gravity_);
+    edge_inertial->setVertex(0, v0_pose);
+    edge_inertial->setVertex(1, v0_vel);
+    edge_inertial->setVertex(2, v0_bg);
+    edge_inertial->setVertex(3, v0_ba);
+    edge_inertial->setVertex(4, v1_pose);
+    edge_inertial->setVertex(5, v1_vel);
+    auto* rk = new g2o::RobustKernelHuber();
+    rk->setDelta(200.0);
+    edge_inertial->setRobustKernel(rk);
+    optimizer.addEdge(edge_inertial);
+
+    // 两个零偏随机游走
+    auto* edge_gyro_rw = new EdgeGyroRW();
+    edge_gyro_rw->setVertex(0, v0_bg);
+    edge_gyro_rw->setVertex(1, v1_bg);
+    edge_gyro_rw->setInformation(options_.bg_rw_info_);
+    optimizer.addEdge(edge_gyro_rw);
+
+    auto* edge_acc_rw = new EdgeAccRW();
+    edge_acc_rw->setVertex(0, v0_ba);
+    edge_acc_rw->setVertex(1, v1_ba);
+    edge_acc_rw->setInformation(options_.ba_rw_info_);
+    optimizer.addEdge(edge_acc_rw);
+
+    // 上时刻先验
+    auto* edge_prior = new EdgePriorPoseNavState(*last_frame_, prior_info_);
+    edge_prior->setVertex(0, v0_pose);
+    edge_prior->setVertex(1, v0_vel);
+    edge_prior->setVertex(2, v0_bg);
+    edge_prior->setVertex(3, v0_ba);
+    optimizer.addEdge(edge_prior);
+
+    // GNSS边
+    auto edge_gnss0 = new EdgeGNSS(v0_pose, last_gnss_.utm_pose_);
+    edge_gnss0->setInformation(options_.gnss_info_);
+    optimizer.addEdge(edge_gnss0);
+
+    auto edge_gnss1 = new EdgeGNSS(v1_pose, this_gnss_.utm_pose_);
+    edge_gnss1->setInformation(options_.gnss_info_);
+    optimizer.addEdge(edge_gnss1);
+
+    // Odom边
+    EdgeEncoder3D* edge_odom = nullptr;
+    Vec3d vel_world = Vec3d::Zero();
+    Vec3d vel_odom = Vec3d::Zero();
+    if (last_odom_set_) {
+        // velocity obs
+        double velo_l =
+            options_.wheel_radius_ * last_odom_.left_pulse_ / options_.circle_pulse_ * 2 * M_PI / options_.odom_span_;
+        double velo_r =
+            options_.wheel_radius_ * last_odom_.right_pulse_ / options_.circle_pulse_ * 2 * M_PI / options_.odom_span_;
+        double average_vel = 0.5 * (velo_l + velo_r);
+        vel_odom = Vec3d(average_vel, 0.0, 0.0);
+        vel_world = this_frame_->R_ * vel_odom;
+
+        edge_odom = new EdgeEncoder3D(v1_vel, vel_world);
+        edge_odom->setInformation(options_.odom_info_);
+        optimizer.addEdge(edge_odom);
+
+        // 重置odom数据到达标志位，等待最新的odom数据
+        last_odom_set_ = false;
+    }
+
+    optimizer.setVerbose(options_.verbose_);
+    optimizer.initializeOptimization();
+    optimizer.optimize(20);
+
+    if (options_.verbose_) {
+        // 获取结果，统计各类误差
+        LOG(INFO) << "chi2/error: ";
+        LOG(INFO) << "preintegration: " << edge_inertial->chi2() << "/" << edge_inertial->error().transpose();
+        // LOG(INFO) << "gnss0: " << edge_gnss0->chi2() << ", " << edge_gnss0->error().transpose();
+        LOG(INFO) << "gnss1: " << edge_gnss1->chi2() << ", " << edge_gnss1->error().transpose();
+        LOG(INFO) << "bias: " << edge_gyro_rw->chi2() << "/" << edge_acc_rw->error().transpose();
+        LOG(INFO) << "prior: " << edge_prior->chi2() << "/" << edge_prior->error().transpose();
+        if (edge_odom) {
+            LOG(INFO) << "body vel: " << (v1_pose->estimate().so3().inverse() * v1_vel->estimate()).transpose();
+            LOG(INFO) << "meas: " << vel_odom.transpose();
+            LOG(INFO) << "odom: " << edge_odom->chi2() << "/" << edge_odom->error().transpose();
+        }
+    }
+
+    last_frame_->R_ = v0_pose->estimate().so3();
+    last_frame_->p_ = v0_pose->estimate().translation();
+    last_frame_->v_ = v0_vel->estimate();
+    last_frame_->bg_ = v0_bg->estimate();
+    last_frame_->ba_ = v0_ba->estimate();
+
+    this_frame_->R_ = v1_pose->estimate().so3();
+    this_frame_->p_ = v1_pose->estimate().translation();
+    this_frame_->v_ = v1_vel->estimate();
+    this_frame_->bg_ = v1_bg->estimate();
+    this_frame_->ba_ = v1_ba->estimate();
+
+    // 重置integ
+    options_.preinteg_options_.init_bg_ = this_frame_->bg_;
+    options_.preinteg_options_.init_ba_ = this_frame_->ba_;
+    pre_integ_ = std::make_shared<IMUPreintegration>(options_.preinteg_options_);
+}
+
+NavStated GinsPreInteg::GetState() const {
+    if (this_frame_ == nullptr) {
+        return {};
+    }
+
+    if (pre_integ_ == nullptr) {
+        return *this_frame_;
+    }
+
+    return pre_integ_->Predict(*this_frame_, options_.gravity_);
+}
+
+}  // namespace sad

workspace/slam_in_autonomous_driving/src/ch4/gins_pre_integ.h

@@ -0,0 +1,119 @@
+//
+// Created by xiang on 2021/7/19.
+//
+
+#ifndef MAPPING_DR_PRE_INTEG_H
+#define MAPPING_DR_PRE_INTEG_H
+
+#include <deque>
+#include <fstream>
+#include <memory>
+
+#include "common/eigen_types.h"
+#include "common/gnss.h"
+#include "common/imu.h"
+#include "common/math_utils.h"
+#include "common/odom.h"
+
+#include "ch4/imu_preintegration.h"
+namespace sad {
+
+/**
+ * 使用预积分优化的GINS
+ *
+ * 本章仍使用两帧模型，不会去做全量优化（尽管也可以做），两帧模型和ESKF更像一些
+ * IMU到达时，累计到预积分器中
+ * 每次RTK到达时，触发一次优化，并做边缘化。边缘化结果写入下一帧先验中。
+ * 里程计方面，使用最近时间的里程计信息作为速度观测量。
+ */
+class GinsPreInteg {
+   public:
+    /// GINS 配置项
+    struct Options {
+        Options() {}
+
+        Vec3d gravity_ = Vec3d(0, 0, -9.8);  // 重力方向
+
+        /// IMU相关噪声参数在preinteg内部配置
+        IMUPreintegration::Options preinteg_options_;
+
+        // 噪声
+        double bias_gyro_var_ = 1e-6;           // 陀螺零偏游走标准差
+        double bias_acce_var_ = 1e-4;           // 加计零偏游走标准差
+        Mat3d bg_rw_info_ = Mat3d::Identity();  // 陀螺随机游走信息阵
+        Mat3d ba_rw_info_ = Mat3d::Identity();  // 加计随机游走信息阵
+
+        double gnss_pos_noise_ = 0.1;                   // GNSS位置方差
+        double gnss_height_noise_ = 0.1;                // GNSS高度方差
+        double gnss_ang_noise_ = 1.0 * math::kDEG2RAD;  // GNSS角度方差
+        Mat6d gnss_info_ = Mat6d::Identity();           // 6D gnss信息矩阵
+
+        /// 轮速计相关
+        double odom_var_ = 0.05;
+        Mat3d odom_info_ = Mat3d::Identity();
+        double odom_span_ = 0.1;        // 里程计测量间隔
+        double wheel_radius_ = 0.155;   // 轮子半径
+        double circle_pulse_ = 1024.0;  // 编码器每圈脉冲数
+
+        bool verbose_ = true;  // 是否输出调试信息
+    };
+
+    /// Option 可以在构造时设置，也可以在后续设置
+    GinsPreInteg(Options options = Options()) : options_(options) { SetOptions(options_); }
+
+    /**
+     * IMU 处理函数，要求初始零偏已经设置过，再调用此函数
+     * @param imu imu读数
+     */
+    void AddImu(const IMU& imu);
+
+    /**
+     * GNSS 处理函数
+     * @param gnss
+     */
+    void AddGnss(const GNSS& gnss);
+
+    /**
+     * 轮速计处理函数
+     * @param odom
+     */
+    void AddOdom(const Odom& odom);
+
+    /// 设置gins的各种配置项，可以在构建时调用，也可以构造完成后，静止初始化结束时调用
+    void SetOptions(Options options);
+
+    /**
+     * 获取当前状态
+     * 如果IMU没有积分，就返回最后优化的状态
+     * 如果有，利用IMU积分，预测自己的状态
+     * @return
+     */
+    NavStated GetState() const;
+
+   private:
+    // 优化
+    void Optimize();
+
+    Options options_;
+    double current_time_ = 0.0;  // 当前时间
+
+    std::shared_ptr<IMUPreintegration> pre_integ_ = nullptr;
+    std::shared_ptr<NavStated> last_frame_ = nullptr;  // 上一个时刻状态
+    std::shared_ptr<NavStated> this_frame_ = nullptr;  // 当前时刻状态
+    Mat15d prior_info_ = Mat15d::Identity() * 1e2;     // 当前时刻先验
+
+    /// 两帧GNSS观测
+    GNSS last_gnss_;
+    GNSS this_gnss_;
+
+    IMU last_imu_;    // 上时刻IMU
+    Odom last_odom_;  // 上时刻odom
+    bool last_odom_set_ = false;
+
+    /// 标志位
+    bool first_gnss_received_ = false;  // 是否已收到第一个gnss信号
+    bool first_imu_received_ = false;   // 是否已收到第一个imu信号
+};
+}  // namespace sad
+
+#endif  // MAPPING_DR_PRE_INTEG_H

workspace/slam_in_autonomous_driving/src/ch4/imu_preintegration.cc

@@ -0,0 +1,89 @@
+#include "imu_preintegration.h"
+#include <glog/logging.h>
+
+namespace sad {
+
+IMUPreintegration::IMUPreintegration(Options options) {
+    bg_ = options.init_bg_;
+    ba_ = options.init_ba_;
+    const float ng2 = options.noise_gyro_ * options.noise_gyro_;
+    const float na2 = options.noise_acce_ * options.noise_acce_;
+    noise_gyro_acce_.diagonal() << ng2, ng2, ng2, na2, na2, na2;
+}
+
+void IMUPreintegration::Integrate(const IMU &imu, double dt) {
+    // 去掉零偏的测量
+    Vec3d gyr = imu.gyro_ - bg_;  // 陀螺
+    Vec3d acc = imu.acce_ - ba_;  // 加计
+
+    // 更新dv, dp, 见(4.13), (4.16)
+    dp_ = dp_ + dv_ * dt + 0.5f * dR_.matrix() * acc * dt * dt;
+    dv_ = dv_ + dR_ * acc * dt;
+
+    // dR先不更新，因为A, B阵还需要现在的dR
+
+    // 运动方程雅可比矩阵系数，A,B阵，见(4.29)
+    // 另外两项在后面
+    Eigen::Matrix<double, 9, 9> A;
+    A.setIdentity();
+    Eigen::Matrix<double, 9, 6> B;
+    B.setZero();
+
+    Mat3d acc_hat = SO3::hat(acc);
+    double dt2 = dt * dt;
+
+    // NOTE A, B左上角块与公式稍有不同
+    A.block<3, 3>(3, 0) = -dR_.matrix() * dt * acc_hat;
+    A.block<3, 3>(6, 0) = -0.5f * dR_.matrix() * acc_hat * dt2;
+    A.block<3, 3>(6, 3) = dt * Mat3d::Identity();
+
+    B.block<3, 3>(3, 3) = dR_.matrix() * dt;
+    B.block<3, 3>(6, 3) = 0.5f * dR_.matrix() * dt2;
+
+    // 更新各雅可比，见式(4.39)
+    dP_dba_ = dP_dba_ + dV_dba_ * dt - 0.5f * dR_.matrix() * dt2;                      // (4.39d)
+    dP_dbg_ = dP_dbg_ + dV_dbg_ * dt - 0.5f * dR_.matrix() * dt2 * acc_hat * dR_dbg_;  // (4.39e)
+    dV_dba_ = dV_dba_ - dR_.matrix() * dt;                                             // (4.39b)
+    dV_dbg_ = dV_dbg_ - dR_.matrix() * dt * acc_hat * dR_dbg_;                         // (4.39c)
+
+    // 旋转部分
+    Vec3d omega = gyr * dt;         // 转动量
+    Mat3d rightJ = SO3::jr(omega);  // 右雅可比
+    SO3 deltaR = SO3::exp(omega);   // exp后
+    dR_ = dR_ * deltaR;             // (4.9)
+
+    A.block<3, 3>(0, 0) = deltaR.matrix().transpose();
+    B.block<3, 3>(0, 0) = rightJ * dt;
+
+    // 更新噪声项
+    cov_ = A * cov_ * A.transpose() + B * noise_gyro_acce_ * B.transpose();
+
+    // 更新dR_dbg
+    dR_dbg_ = deltaR.matrix().transpose() * dR_dbg_ - rightJ * dt;  // (4.39a)
+
+    // 增量积分时间
+    dt_ += dt;
+}
+
+SO3 IMUPreintegration::GetDeltaRotation(const Vec3d &bg) { return dR_ * SO3::exp(dR_dbg_ * (bg - bg_)); }
+
+Vec3d IMUPreintegration::GetDeltaVelocity(const Vec3d &bg, const Vec3d &ba) {
+    return dv_ + dV_dbg_ * (bg - bg_) + dV_dba_ * (ba - ba_);
+}
+
+Vec3d IMUPreintegration::GetDeltaPosition(const Vec3d &bg, const Vec3d &ba) {
+    return dp_ + dP_dbg_ * (bg - bg_) + dP_dba_ * (ba - ba_);
+}
+
+NavStated IMUPreintegration::Predict(const sad::NavStated &start, const Vec3d &grav) const {
+    SO3 Rj = start.R_ * dR_;
+    Vec3d vj = start.R_ * dv_ + start.v_ + grav * dt_;
+    Vec3d pj = start.R_ * dp_ + start.p_ + start.v_ * dt_ + 0.5f * grav * dt_ * dt_;
+
+    auto state = NavStated(start.timestamp_ + dt_, Rj, pj, vj);
+    state.bg_ = bg_;
+    state.ba_ = ba_;
+    return state;
+}
+
+}  // namespace sad

workspace/slam_in_autonomous_driving/src/ch4/imu_preintegration.h

@@ -0,0 +1,79 @@
+#ifndef IMUTYPES_H
+#define IMUTYPES_H
+
+#include <mutex>
+#include <opencv2/core/core.hpp>
+#include <utility>
+#include <vector>
+
+#include "common/eigen_types.h"
+#include "common/imu.h"
+#include "common/nav_state.h"
+
+namespace sad {
+
+/**
+ * IMU 预积分器
+ *
+ * 调用Integrate来插入新的IMU读数，然后通过Get函数得到预积分的值
+ * 雅可比也可以通过本类获得，可用于构建g2o的边类
+ */
+class IMUPreintegration {
+   public:
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+    /// 参数配置项
+    /// 初始的零偏需要设置，其他可以不改
+    struct Options {
+        Options() {}
+        Vec3d init_bg_ = Vec3d::Zero();  // 初始零偏
+        Vec3d init_ba_ = Vec3d::Zero();  // 初始零偏
+        double noise_gyro_ = 1e-2;       // 陀螺噪声，标准差
+        double noise_acce_ = 1e-1;       // 加计噪声，标准差
+    };
+
+    IMUPreintegration(Options options = Options());
+
+    /**
+     * 插入新的IMU数据
+     * @param imu   imu 读数
+     * @param dt    时间差
+     */
+    void Integrate(const IMU &imu, double dt);
+
+    /**
+     * 从某个起始点开始预测积分之后的状态
+     * @param start 起始时时刻状态
+     * @return  预测的状态
+     */
+    NavStated Predict(const NavStated &start, const Vec3d &grav = Vec3d(0, 0, -9.81)) const;
+
+    /// 获取修正之后的观测量，bias可以与预积分时期的不同，会有一阶修正
+    SO3 GetDeltaRotation(const Vec3d &bg);
+    Vec3d GetDeltaVelocity(const Vec3d &bg, const Vec3d &ba);
+    Vec3d GetDeltaPosition(const Vec3d &bg, const Vec3d &ba);
+
+   public:
+    double dt_ = 0;                          // 整体预积分时间
+    Mat9d cov_ = Mat9d::Zero();              // 累计噪声矩阵
+    Mat6d noise_gyro_acce_ = Mat6d::Zero();  // 测量噪声矩阵
+
+    // 零偏
+    Vec3d bg_ = Vec3d::Zero();
+    Vec3d ba_ = Vec3d::Zero();
+
+    // 预积分观测量
+    SO3 dR_;
+    Vec3d dv_ = Vec3d::Zero();
+    Vec3d dp_ = Vec3d::Zero();
+
+    // 雅可比矩阵
+    Mat3d dR_dbg_ = Mat3d::Zero();
+    Mat3d dV_dbg_ = Mat3d::Zero();
+    Mat3d dV_dba_ = Mat3d::Zero();
+    Mat3d dP_dbg_ = Mat3d::Zero();
+    Mat3d dP_dba_ = Mat3d::Zero();
+};
+
+}  // namespace sad
+
+#endif  // IMUTYPES_H

workspace/slam_in_autonomous_driving/src/ch4/run_gins_pre_integ.cc

@@ -0,0 +1,152 @@
+//
+// Created by xiang on 2022/1/21.
+//
+
+#include "ch3/static_imu_init.h"
+#include "ch3/utm_convert.h"
+#include "ch4/gins_pre_integ.h"
+#include "common/io_utils.h"
+#include "tools/ui/pangolin_window.h"
+
+#include <gflags/gflags.h>
+#include <glog/logging.h>
+#include <fstream>
+#include <iomanip>
+
+/**
+ * 运行由预积分构成的GINS系统
+ */
+DEFINE_string(txt_path, "./data/ch3/10.txt", "数据文件路径");
+DEFINE_double(antenna_angle, 12.06, "RTK天线安装偏角（角度）");
+DEFINE_double(antenna_pox_x, -0.17, "RTK天线安装偏移X");
+DEFINE_double(antenna_pox_y, -0.20, "RTK天线安装偏移Y");
+DEFINE_bool(with_ui, true, "是否显示图形界面");
+DEFINE_bool(debug, false, "是否打印调试信息");
+
+int main(int argc, char** argv) {
+    google::InitGoogleLogging(argv[0]);
+    FLAGS_stderrthreshold = google::INFO;
+    FLAGS_colorlogtostderr = true;
+    google::ParseCommandLineFlags(&argc, &argv, true);
+
+    if (fLS::FLAGS_txt_path.empty()) {
+        return -1;
+    }
+
+    // 初始化器
+    sad::StaticIMUInit imu_init;  // 使用默认配置
+
+    sad::TxtIO io(fLS::FLAGS_txt_path);
+    Vec2d antenna_pos(fLD::FLAGS_antenna_pox_x, fLD::FLAGS_antenna_pox_y);
+
+    auto save_vec3 = [](std::ofstream& fout, const Vec3d& v) { fout << v[0] << " " << v[1] << " " << v[2] << " "; };
+    auto save_quat = [](std::ofstream& fout, const Quatd& q) {
+        fout << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << " ";
+    };
+
+    auto save_result = [&save_vec3, &save_quat](std::ofstream& fout, const sad::NavStated& save_state) {
+        fout << std::setprecision(18) << save_state.timestamp_ << " " << std::setprecision(9);
+        save_vec3(fout, save_state.p_);
+        save_quat(fout, save_state.R_.unit_quaternion());
+        save_vec3(fout, save_state.v_);
+        save_vec3(fout, save_state.bg_);
+        save_vec3(fout, save_state.ba_);
+        fout << std::endl;
+    };
+
+    std::ofstream fout("./data/ch4/gins_preintg.txt");
+    bool imu_inited = false, gnss_inited = false;
+
+    sad::GinsPreInteg::Options gins_options;
+    gins_options.verbose_ = FLAGS_debug;
+    sad::GinsPreInteg gins(gins_options);
+
+    bool first_gnss_set = false;
+    Vec3d origin = Vec3d::Zero();
+
+    std::shared_ptr<sad::ui::PangolinWindow> ui = nullptr;
+    if (FLAGS_with_ui) {
+        ui = std::make_shared<sad::ui::PangolinWindow>();
+        ui->Init();
+    }
+
+    /// 设置各类回调函数
+    io.SetIMUProcessFunc([&](const sad::IMU& imu) {
+          /// IMU 处理函数
+          if (!imu_init.InitSuccess()) {
+              imu_init.AddIMU(imu);
+              return;
+          }
+
+          /// 需要IMU初始化
+          if (!imu_inited) {
+              // 读取初始零偏，设置GINS
+              sad::GinsPreInteg::Options options;
+              options.preinteg_options_.init_bg_ = imu_init.GetInitBg();
+              options.preinteg_options_.init_ba_ = imu_init.GetInitBa();
+              options.gravity_ = imu_init.GetGravity();
+              gins.SetOptions(options);
+              imu_inited = true;
+              return;
+          }
+
+          if (!gnss_inited) {
+              /// 等待有效的RTK数据
+              return;
+          }
+
+          /// GNSS 也接收到之后，再开始进行预测
+          gins.AddImu(imu);
+
+          auto state = gins.GetState();
+          save_result(fout, state);
+          if (ui) {
+              ui->UpdateNavState(state);
+              usleep(5e2);
+          }
+      })
+        .SetGNSSProcessFunc([&](const sad::GNSS& gnss) {
+            /// GNSS 处理函数
+            if (!imu_inited) {
+                return;
+            }
+
+            sad::GNSS gnss_convert = gnss;
+            if (!sad::ConvertGps2UTM(gnss_convert, antenna_pos, FLAGS_antenna_angle) || !gnss_convert.heading_valid_) {
+                return;
+            }
+
+            /// 去掉原点
+            if (!first_gnss_set) {
+                origin = gnss_convert.utm_pose_.translation();
+                first_gnss_set = true;
+            }
+            gnss_convert.utm_pose_.translation() -= origin;
+
+            gins.AddGnss(gnss_convert);
+
+            auto state = gins.GetState();
+            save_result(fout, state);
+            if (ui) {
+                ui->UpdateNavState(state);
+                usleep(1e3);
+            }
+            gnss_inited = true;
+        })
+        .SetOdomProcessFunc([&](const sad::Odom& odom) {
+            imu_init.AddOdom(odom);
+
+            if (imu_inited && gnss_inited) {
+                gins.AddOdom(odom);
+            }
+        })
+        .Go();
+
+    while (ui && !ui->ShouldQuit()) {
+        usleep(1e5);
+    }
+    if (ui) {
+        ui->Quit();
+    }
+    return 0;
+}

workspace/slam_in_autonomous_driving/src/ch4/test_preintegration.cc

@@ -0,0 +1,397 @@
+//
+// Created by xiang on 2021/7/16.
+//
+
+#include <gflags/gflags.h>
+#include <glog/logging.h>
+#include <gtest/gtest.h>
+
+#include "ch3/eskf.hpp"
+#include "ch3/static_imu_init.h"
+#include "ch4/imu_preintegration.h"
+#include "ch4/g2o_types.h"
+#include "common/g2o_types.h"
+#include "common/io_utils.h"
+
+#include <g2o/core/block_solver.h>
+#include <g2o/core/optimization_algorithm_gauss_newton.h>
+#include <g2o/core/optimization_algorithm_levenberg.h>
+#include <g2o/core/robust_kernel.h>
+#include <g2o/solvers/eigen/linear_solver_eigen.h>
+
+DEFINE_string(txt_path, "./data/ch3/10.txt", "数据文件路径");
+DEFINE_double(antenna_angle, 12.06, "RTK天线安装偏角（角度）");
+DEFINE_double(antenna_pox_x, -0.17, "RTK天线安装偏移X");
+DEFINE_double(antenna_pox_y, -0.20, "RTK天线安装偏移Y");
+DEFINE_bool(with_ui, true, "是否显示图形界面");
+
+TEST(PREINTEGRATION_TEST, ROTATION_TEST) {
+    // 测试在恒定角速度运转下的预积分情况
+    double imu_time_span = 0.01;       // IMU测量间隔
+    Vec3d constant_omega(0, 0, M_PI);  // 角速度为180度/s，转1秒应该等于转180度
+    Vec3d gravity(0, 0, -9.8);         // Z 向上，重力方向为负
+
+    sad::NavStated start_status(0), end_status(1.0);
+    sad::IMUPreintegration pre_integ;
+
+    // 对比直接积分
+    Sophus::SO3d R;
+    Vec3d t = Vec3d::Zero();
+    Vec3d v = Vec3d::Zero();
+
+    for (int i = 1; i <= 100; ++i) {
+        double time = imu_time_span * i;
+        Vec3d acce = -gravity;  // 加速度计应该测量到一个向上的力
+        pre_integ.Integrate(sad::IMU(time, constant_omega, acce), imu_time_span);
+
+        sad::NavStated this_status = pre_integ.Predict(start_status, gravity);
+
+        t = t + v * imu_time_span + 0.5 * gravity * imu_time_span * imu_time_span +
+            0.5 * (R * acce) * imu_time_span * imu_time_span;
+        v = v + gravity * imu_time_span + (R * acce) * imu_time_span;
+        R = R * Sophus::SO3d::exp(constant_omega * imu_time_span);
+
+        // 验证在简单情况下，直接积分和预积分结果相等
+        EXPECT_NEAR(t[0], this_status.p_[0], 1e-2);
+        EXPECT_NEAR(t[1], this_status.p_[1], 1e-2);
+        EXPECT_NEAR(t[2], this_status.p_[2], 1e-2);
+
+        EXPECT_NEAR(v[0], this_status.v_[0], 1e-2);
+        EXPECT_NEAR(v[1], this_status.v_[1], 1e-2);
+        EXPECT_NEAR(v[2], this_status.v_[2], 1e-2);
+
+        EXPECT_NEAR(R.unit_quaternion().x(), this_status.R_.unit_quaternion().x(), 1e-4);
+        EXPECT_NEAR(R.unit_quaternion().y(), this_status.R_.unit_quaternion().y(), 1e-4);
+        EXPECT_NEAR(R.unit_quaternion().z(), this_status.R_.unit_quaternion().z(), 1e-4);
+        EXPECT_NEAR(R.unit_quaternion().w(), this_status.R_.unit_quaternion().w(), 1e-4);
+    }
+
+    end_status = pre_integ.Predict(start_status);
+
+    LOG(INFO) << "preinteg result: ";
+    LOG(INFO) << "end rotation: \n" << end_status.R_.matrix();
+    LOG(INFO) << "end trans: \n" << end_status.p_.transpose();
+    LOG(INFO) << "end v: \n" << end_status.v_.transpose();
+
+    LOG(INFO) << "direct integ result: ";
+    LOG(INFO) << "end rotation: \n" << R.matrix();
+    LOG(INFO) << "end trans: \n" << t.transpose();
+    LOG(INFO) << "end v: \n" << v.transpose();
+    SUCCEED();
+}
+
+TEST(PREINTEGRATION_TEST, ACCELERATION_TEST) {
+    // 测试在恒定加速度运行下的预积分情况
+    double imu_time_span = 0.01;     // IMU测量间隔
+    Vec3d gravity(0, 0, -9.8);       // Z 向上，重力方向为负
+    Vec3d constant_acce(0.1, 0, 0);  // x 方向上的恒定加速度
+
+    sad::NavStated start_status(0), end_status(1.0);
+    sad::IMUPreintegration pre_integ;
+
+    // 对比直接积分
+    Sophus::SO3d R;
+    Vec3d t = Vec3d::Zero();
+    Vec3d v = Vec3d::Zero();
+
+    for (int i = 1; i <= 100; ++i) {
+        double time = imu_time_span * i;
+        Vec3d acce = constant_acce - gravity;
+        pre_integ.Integrate(sad::IMU(time, Vec3d::Zero(), acce), imu_time_span);
+        sad::NavStated this_status = pre_integ.Predict(start_status, gravity);
+
+        t = t + v * imu_time_span + 0.5 * gravity * imu_time_span * imu_time_span +
+            0.5 * (R * acce) * imu_time_span * imu_time_span;
+        v = v + gravity * imu_time_span + (R * acce) * imu_time_span;
+
+        // 验证在简单情况下，直接积分和预积分结果相等
+        EXPECT_NEAR(t[0], this_status.p_[0], 1e-2);
+        EXPECT_NEAR(t[1], this_status.p_[1], 1e-2);
+        EXPECT_NEAR(t[2], this_status.p_[2], 1e-2);
+
+        EXPECT_NEAR(v[0], this_status.v_[0], 1e-2);
+        EXPECT_NEAR(v[1], this_status.v_[1], 1e-2);
+        EXPECT_NEAR(v[2], this_status.v_[2], 1e-2);
+
+        EXPECT_NEAR(R.unit_quaternion().x(), this_status.R_.unit_quaternion().x(), 1e-4);
+        EXPECT_NEAR(R.unit_quaternion().y(), this_status.R_.unit_quaternion().y(), 1e-4);
+        EXPECT_NEAR(R.unit_quaternion().z(), this_status.R_.unit_quaternion().z(), 1e-4);
+        EXPECT_NEAR(R.unit_quaternion().w(), this_status.R_.unit_quaternion().w(), 1e-4);
+    }
+
+    end_status = pre_integ.Predict(start_status);
+    LOG(INFO) << "preinteg result: ";
+    LOG(INFO) << "end rotation: \n" << end_status.R_.matrix();
+    LOG(INFO) << "end trans: \n" << end_status.p_.transpose();
+    LOG(INFO) << "end v: \n" << end_status.v_.transpose();
+
+    LOG(INFO) << "direct integ result: ";
+    LOG(INFO) << "end rotation: \n" << R.matrix();
+    LOG(INFO) << "end trans: \n" << t.transpose();
+    LOG(INFO) << "end v: \n" << v.transpose();
+    SUCCEED();
+}
+
+void Optimize(sad::NavStated& last_state, sad::NavStated& this_state, sad::GNSS& last_gnss, sad::GNSS& this_gnss,
+              std::shared_ptr<sad::IMUPreintegration>& preinteg, const Vec3d& grav);
+
+/// 使用ESKF的Predict, Update来验证预积分的优化过程
+TEST(PREINTEGRATION_TEST, ESKF_TEST) {
+    if (fLS::FLAGS_txt_path.empty()) {
+        FAIL();
+    }
+
+    // 初始化器
+    sad::StaticIMUInit imu_init;  // 使用默认配置
+    sad::ESKFD eskf;
+
+    sad::TxtIO io(FLAGS_txt_path);
+    Vec2d antenna_pos(FLAGS_antenna_pox_x, FLAGS_antenna_pox_y);
+
+    std::ofstream fout("./data/ch3/gins.txt");
+    bool imu_inited = false, gnss_inited = false;
+
+    /// 设置各类回调函数
+    bool first_gnss_set = false;
+    Vec3d origin = Vec3d::Zero();
+
+    std::shared_ptr<sad::IMUPreintegration> preinteg = nullptr;
+
+    sad::NavStated last_state;
+    bool last_state_set = false;
+
+    sad::GNSS last_gnss;
+    bool last_gnss_set = false;
+
+    io.SetIMUProcessFunc([&](const sad::IMU& imu) {
+          /// IMU 处理函数
+          if (!imu_init.InitSuccess()) {
+              imu_init.AddIMU(imu);
+              return;
+          }
+
+          /// 需要IMU初始化
+          if (!imu_inited) {
+              // 读取初始零偏，设置ESKF
+              sad::ESKFD::Options options;
+              // 噪声由初始化器估计
+              options.gyro_var_ = sqrt(imu_init.GetCovGyro()[0]);
+              options.acce_var_ = sqrt(imu_init.GetCovAcce()[0]);
+              eskf.SetInitialConditions(options, imu_init.GetInitBg(), imu_init.GetInitBa(), imu_init.GetGravity());
+
+              imu_inited = true;
+              return;
+          }
+
+          if (!gnss_inited) {
+              /// 等待有效的RTK数据
+              return;
+          }
+
+          /// GNSS 也接收到之后，再开始进行预测
+          double current_time = eskf.GetNominalState().timestamp_;
+          eskf.Predict(imu);
+
+          if (preinteg) {
+              preinteg->Integrate(imu, imu.timestamp_ - current_time);
+
+              if (last_state_set) {
+                  auto pred_of_preinteg = preinteg->Predict(last_state, eskf.GetGravity());
+                  auto pred_of_eskf = eskf.GetNominalState();
+
+                  /// 这两个预测值的误差应该非常接近
+                  EXPECT_NEAR((pred_of_preinteg.p_ - pred_of_eskf.p_).norm(), 0, 1e-2);
+                  EXPECT_NEAR((pred_of_preinteg.R_.inverse() * pred_of_eskf.R_).log().norm(), 0, 1e-2);
+                  EXPECT_NEAR((pred_of_preinteg.v_ - pred_of_eskf.v_).norm(), 0, 1e-2);
+              }
+          }
+      })
+        .SetGNSSProcessFunc([&](const sad::GNSS& gnss) {
+            /// GNSS 处理函数
+            if (!imu_inited) {
+                return;
+            }
+
+            sad::GNSS gnss_convert = gnss;
+            if (!sad::ConvertGps2UTM(gnss_convert, antenna_pos, FLAGS_antenna_angle) || !gnss_convert.heading_valid_) {
+                return;
+            }
+
+            /// 去掉原点
+            if (!first_gnss_set) {
+                origin = gnss_convert.utm_pose_.translation();
+                first_gnss_set = true;
+            }
+            gnss_convert.utm_pose_.translation() -= origin;
+
+            // 要求RTK heading有效，才能合入EKF
+            auto state_bef_update = eskf.GetNominalState();
+
+            eskf.ObserveGps(gnss_convert);
+
+            // 验证优化过程是否正确
+            if (last_state_set && last_gnss_set) {
+                auto update_state = eskf.GetNominalState();
+
+                LOG(INFO) << "state before eskf update: " << state_bef_update;
+                LOG(INFO) << "state after  eskf update: " << update_state;
+
+                auto state_pred = preinteg->Predict(last_state, eskf.GetGravity());
+                LOG(INFO) << "state in pred: " << state_pred;
+
+                Optimize(last_state, update_state, last_gnss, gnss_convert, preinteg, eskf.GetGravity());
+            }
+
+            last_state = eskf.GetNominalState();
+            last_state_set = true;
+
+            // 重置预积分
+            sad::IMUPreintegration::Options options;
+            options.init_bg_ = last_state.bg_;
+            options.init_ba_ = last_state.ba_;
+            preinteg = std::make_shared<sad::IMUPreintegration>(options);
+
+            gnss_inited = true;
+            last_gnss = gnss_convert;
+            last_gnss_set = true;
+        })
+        .SetOdomProcessFunc([&](const sad::Odom& odom) { imu_init.AddOdom(odom); })
+        .Go();
+
+    SUCCEED();
+}
+
+void Optimize(sad::NavStated& last_state, sad::NavStated& this_state, sad::GNSS& last_gnss, sad::GNSS& this_gnss,
+              std::shared_ptr<sad::IMUPreintegration>& pre_integ, const Vec3d& grav) {
+    assert(pre_integ != nullptr);
+
+    if (pre_integ->dt_ < 1e-3) {
+        // 未得到积分
+        return;
+    }
+
+    using BlockSolverType = g2o::BlockSolverX;
+    using LinearSolverType = g2o::LinearSolverEigen<BlockSolverType::PoseMatrixType>;
+
+    auto* solver = new g2o::OptimizationAlgorithmGaussNewton(
+        g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
+    g2o::SparseOptimizer optimizer;
+    optimizer.setAlgorithm(solver);
+
+    // 上时刻顶点， pose, v, bg, ba
+    auto v0_pose = new sad::VertexPose();
+    v0_pose->setId(0);
+    v0_pose->setEstimate(last_state.GetSE3());
+    optimizer.addVertex(v0_pose);
+
+    auto v0_vel = new sad::VertexVelocity();
+    v0_vel->setId(1);
+    v0_vel->setEstimate(last_state.v_);
+    optimizer.addVertex(v0_vel);
+
+    auto v0_bg = new sad::VertexGyroBias();
+    v0_bg->setId(2);
+    v0_bg->setEstimate(last_state.bg_);
+    optimizer.addVertex(v0_bg);
+
+    auto v0_ba = new sad::VertexAccBias();
+    v0_ba->setId(3);
+    v0_ba->setEstimate(last_state.ba_);
+    optimizer.addVertex(v0_ba);
+
+    // 本时刻顶点，pose, v, bg, ba
+    auto v1_pose = new sad::VertexPose();
+    v1_pose->setId(4);
+    v1_pose->setEstimate(this_state.GetSE3());
+    optimizer.addVertex(v1_pose);
+
+    auto v1_vel = new sad::VertexVelocity();
+    v1_vel->setId(5);
+    v1_vel->setEstimate(this_state.v_);
+    optimizer.addVertex(v1_vel);
+
+    auto v1_bg = new sad::VertexGyroBias();
+    v1_bg->setId(6);
+    v1_bg->setEstimate(this_state.bg_);
+    optimizer.addVertex(v1_bg);
+
+    auto v1_ba = new sad::VertexAccBias();
+    v1_ba->setId(7);
+    v1_ba->setEstimate(this_state.ba_);
+    optimizer.addVertex(v1_ba);
+
+    // 预积分边
+    auto edge_inertial = new sad::EdgeInertial(pre_integ, grav);
+    edge_inertial->setVertex(0, v0_pose);
+    edge_inertial->setVertex(1, v0_vel);
+    edge_inertial->setVertex(2, v0_bg);
+    edge_inertial->setVertex(3, v0_ba);
+    edge_inertial->setVertex(4, v1_pose);
+    edge_inertial->setVertex(5, v1_vel);
+
+    auto* rk = new g2o::RobustKernelHuber();
+    rk->setDelta(200.0);
+    edge_inertial->setRobustKernel(rk);
+
+    optimizer.addEdge(edge_inertial);
+    edge_inertial->computeError();
+    LOG(INFO) << "inertial init err: " << edge_inertial->chi2();
+
+    auto* edge_gyro_rw = new sad::EdgeGyroRW();
+    edge_gyro_rw->setVertex(0, v0_bg);
+    edge_gyro_rw->setVertex(1, v1_bg);
+    edge_gyro_rw->setInformation(Mat3d::Identity() * 1e6);
+    optimizer.addEdge(edge_gyro_rw);
+
+    edge_gyro_rw->computeError();
+    LOG(INFO) << "inertial bg rw: " << edge_gyro_rw->chi2();
+
+    auto* edge_acc_rw = new sad::EdgeAccRW();
+    edge_acc_rw->setVertex(0, v0_ba);
+    edge_acc_rw->setVertex(1, v1_ba);
+    edge_acc_rw->setInformation(Mat3d::Identity() * 1e6);
+    optimizer.addEdge(edge_acc_rw);
+
+    edge_acc_rw->computeError();
+    LOG(INFO) << "inertial ba rw: " << edge_acc_rw->chi2();
+
+    // GNSS边
+    auto edge_gnss0 = new sad::EdgeGNSS(v0_pose, last_gnss.utm_pose_);
+    edge_gnss0->setInformation(Mat6d::Identity() * 1e2);
+    optimizer.addEdge(edge_gnss0);
+
+    edge_gnss0->computeError();
+    LOG(INFO) << "gnss0 init err: " << edge_gnss0->chi2();
+
+    auto edge_gnss1 = new sad::EdgeGNSS(v1_pose, this_gnss.utm_pose_);
+    edge_gnss1->setInformation(Mat6d::Identity() * 1e2);
+    optimizer.addEdge(edge_gnss1);
+
+    edge_gnss1->computeError();
+    LOG(INFO) << "gnss1 init err: " << edge_gnss1->chi2();
+
+    optimizer.setVerbose(true);
+    optimizer.initializeOptimization();
+    optimizer.optimize(10);
+
+    sad::NavStated corr_state(this_state.timestamp_, v1_pose->estimate().so3(), v1_pose->estimate().translation(),
+                              v1_vel->estimate(), v1_bg->estimate(), v1_ba->estimate());
+    LOG(INFO) << "corr state in opt: " << corr_state;
+
+    // 获取结果，统计各类误差
+    LOG(INFO) << "chi2/error: ";
+    LOG(INFO) << "preintegration: " << edge_inertial->chi2() << "/" << edge_inertial->error().transpose();
+    LOG(INFO) << "gnss0: " << edge_gnss0->chi2() << ", " << edge_gnss0->error().transpose();
+    LOG(INFO) << "gnss1: " << edge_gnss1->chi2() << ", " << edge_gnss1->error().transpose();
+    LOG(INFO) << "bias: " << edge_gyro_rw->chi2() << "/" << edge_acc_rw->error().transpose();
+}
+
+int main(int argc, char** argv) {
+    google::InitGoogleLogging(argv[0]);
+    FLAGS_stderrthreshold = google::INFO;
+    FLAGS_colorlogtostderr = true;
+
+    testing::InitGoogleTest(&argc, argv);
+    google::ParseCommandLineFlags(&argc, &argv, true);
+    return RUN_ALL_TESTS();
+}

workspace/slam_in_autonomous_driving/src/ch5/CMakeLists.txt

@@ -0,0 +1,46 @@
+add_executable(point_cloud_load_and_vis point_cloud_load_and_vis.cc)
+target_link_libraries(point_cloud_load_and_vis
+        ${PCL_LIBRARIES}
+        ${GLOG_LIBRARIES}
+        gflags
+        )
+
+add_executable(pcd_to_bird_eye pcd_to_bird_eye.cc)
+target_link_libraries(pcd_to_bird_eye
+        ${PCL_LIBRARIES}
+        ${GLOG_LIBRARIES}
+        ${OpenCV_LIBS}
+        gflags
+        )
+
+add_executable(scan_to_range_image scan_to_range_image.cc)
+target_link_libraries(scan_to_range_image
+        ${PCL_LIBRARIES}
+        ${GLOG_LIBRARIES}
+        ${OpenCV_LIBS}
+        gflags
+        )
+
+add_library(${PROJECT_NAME}.ch5
+        bfnn.cc
+        kdtree.cc
+        octo_tree.cc
+        )
+target_link_libraries(${PROJECT_NAME}.ch5
+        tbb
+        )
+
+add_executable(linear_fitting linear_fitting.cc)
+target_link_libraries(linear_fitting
+        ${PCL_LIBRARIES}
+        ${GLOG_LIBRARIES}
+        ${OpenCV_LIBS}
+        gflags
+        )
+
+ADD_EXECUTABLE(test_nn test_nn.cc)
+ADD_TEST(test_nn test_bfnn)
+
+target_link_libraries(test_nn
+        gtest pthread glog gflags ${PROJECT_NAME}.ch5 ${PROJECT_NAME}.common ${PCL_LIBRARIES} tbb
+        )

workspace/slam_in_autonomous_driving/src/ch5/bfnn.cc

@@ -0,0 +1,80 @@
+//
+// Created by xiang on 2021/8/18.
+//
+
+#include "ch5/bfnn.h"
+#include <execution>
+
+namespace sad {
+
+int bfnn_point(CloudPtr cloud, const Vec3f& point) {
+    return std::min_element(cloud->points.begin(), cloud->points.end(),
+                            [&point](const PointType& pt1, const PointType& pt2) -> bool {
+                                return (pt1.getVector3fMap() - point).squaredNorm() <
+                                       (pt2.getVector3fMap() - point).squaredNorm();
+                            }) -
+           cloud->points.begin();
+}
+
+std::vector<int> bfnn_point_k(CloudPtr cloud, const Vec3f& point, int k) {
+    struct IndexAndDis {
+        IndexAndDis() {}
+        IndexAndDis(int index, double dis2) : index_(index), dis2_(dis2) {}
+        int index_ = 0;
+        double dis2_ = 0;
+    };
+
+    std::vector<IndexAndDis> index_and_dis(cloud->size());
+    for (int i = 0; i < cloud->size(); ++i) {
+        index_and_dis[i] = {i, (cloud->points[i].getVector3fMap() - point).squaredNorm()};
+    }
+    std::sort(index_and_dis.begin(), index_and_dis.end(),
+              [](const auto& d1, const auto& d2) { return d1.dis2_ < d2.dis2_; });
+    std::vector<int> ret;
+    std::transform(index_and_dis.begin(), index_and_dis.begin() + k, std::back_inserter(ret),
+                   [](const auto& d1) { return d1.index_; });
+    return ret;
+}
+
+void bfnn_cloud_mt(CloudPtr cloud1, CloudPtr cloud2, std::vector<std::pair<size_t, size_t>>& matches) {
+    // 先生成索引
+    std::vector<size_t> index(cloud2->size());
+    std::for_each(index.begin(), index.end(), [idx = 0](size_t& i) mutable { i = idx++; });
+
+    // 并行化for_each
+    matches.resize(index.size());
+    std::for_each(std::execution::par_unseq, index.begin(), index.end(), [&](auto idx) {
+        matches[idx].second = idx;
+        matches[idx].first = bfnn_point(cloud1, ToVec3f(cloud2->points[idx]));
+    });
+}
+
+void bfnn_cloud(CloudPtr cloud1, CloudPtr cloud2, std::vector<std::pair<size_t, size_t>>& matches) {
+    // 单线程版本
+    std::vector<size_t> index(cloud2->size());
+    std::for_each(index.begin(), index.end(), [idx = 0](size_t& i) mutable { i = idx++; });
+
+    matches.resize(index.size());
+    std::for_each(std::execution::seq, index.begin(), index.end(), [&](auto idx) {
+        matches[idx].second = idx;
+        matches[idx].first = bfnn_point(cloud1, ToVec3f(cloud2->points[idx]));
+    });
+}
+
+void bfnn_cloud_mt_k(CloudPtr cloud1, CloudPtr cloud2, std::vector<std::pair<size_t, size_t>>& matches, int k) {
+    // 先生成索引
+    std::vector<size_t> index(cloud2->size());
+    std::for_each(index.begin(), index.end(), [idx = 0](size_t& i) mutable { i = idx++; });
+
+    // 并行化for_each
+    matches.resize(index.size() * k);
+    std::for_each(std::execution::par_unseq, index.begin(), index.end(), [&](auto idx) {
+        auto v = bfnn_point_k(cloud1, ToVec3f(cloud2->points[idx]), k);
+        for (int i = 0; i < v.size(); ++i) {
+            matches[idx * k + i].first = v[i];
+            matches[idx * k + i].second = idx;
+        }
+    });
+}
+
+}  // namespace sad

workspace/slam_in_autonomous_driving/src/ch5/bfnn.h

@@ -0,0 +1,58 @@
+//
+// Created by xiang on 2021/8/18.
+//
+
+#ifndef SLAM_IN_AUTO_DRIVING_BFNN_H
+#define SLAM_IN_AUTO_DRIVING_BFNN_H
+
+#include "common/eigen_types.h"
+#include "common/point_types.h"
+
+#include <thread>
+
+namespace sad {
+
+/**
+ * Brute-force Nearest Neighbour
+ * @param cloud 点云
+ * @param point 待查找点
+ * @return 找到的最近点索引
+ */
+int bfnn_point(CloudPtr cloud, const Vec3f& point);
+
+/**
+ * Brute-force Nearest Neighbour, k近邻
+ * @param cloud 点云
+ * @param point 待查找点
+ * @param k 近邻数
+ * @return 找到的最近点索引
+ */
+std::vector<int> bfnn_point_k(CloudPtr cloud, const Vec3f& point, int k = 5);
+
+/**
+ * 对点云进行BF最近邻
+ * @param cloud1  目标点云
+ * @param cloud2  被查找点云
+ * @param matches 两个点云内的匹配关系
+ * @return
+ */
+void bfnn_cloud(CloudPtr cloud1, CloudPtr cloud2, std::vector<std::pair<size_t, size_t>>& matches);
+
+/**
+ * 对点云进行BF最近邻 多线程版本
+ * @param cloud1
+ * @param cloud2
+ * @param matches
+ */
+void bfnn_cloud_mt(CloudPtr cloud1, CloudPtr cloud2, std::vector<std::pair<size_t, size_t>>& matches);
+
+/**
+ * 对点云进行BF最近邻 多线程版本，k近邻
+ * @param cloud1
+ * @param cloud2
+ * @param matches
+ */
+void bfnn_cloud_mt_k(CloudPtr cloud1, CloudPtr cloud2, std::vector<std::pair<size_t, size_t>>& matches, int k = 5);
+}  // namespace sad
+
+#endif  // SLAM_IN_AUTO_DRIVING_BFNN_H

workspace/slam_in_autonomous_driving/src/ch5/gridnn.hpp

@@ -0,0 +1,215 @@
+//
+// Created by xiang on 2021/8/25.
+//
+
+#ifndef SLAM_IN_AUTO_DRIVING_GRID2D_HPP
+#define SLAM_IN_AUTO_DRIVING_GRID2D_HPP
+
+#include "common/eigen_types.h"
+#include "common/math_utils.h"
+#include "common/point_types.h"
+
+#include <glog/logging.h>
+#include <execution>
+#include <map>
+
+namespace sad {
+
+/**
+ * 栅格法最近邻
+ * @tparam dim 模板参数，使用2D或3D栅格
+ */
+template <int dim>
+class GridNN {
+   public:
+    using KeyType = Eigen::Matrix<int, dim, 1>;
+    using PtType = Eigen::Matrix<float, dim, 1>;
+
+    enum class NearbyType {
+        CENTER,  // 只考虑中心
+        // for 2D
+        NEARBY4,  // 上下左右
+        NEARBY8,  // 上下左右+四角
+
+        // for 3D
+        NEARBY6,  // 上下左右前后
+    };
+
+    /**
+     * 构造函数
+     * @param resolution 分辨率
+     * @param nearby_type 近邻判定方法
+     */
+    explicit GridNN(float resolution = 0.1, NearbyType nearby_type = NearbyType::NEARBY4)
+        : resolution_(resolution), nearby_type_(nearby_type) {
+        inv_resolution_ = 1.0 / resolution_;
+
+        // check dim and nearby
+        if (dim == 2 && nearby_type_ == NearbyType::NEARBY6) {
+            LOG(INFO) << "2D grid does not support nearby6, using nearby4 instead.";
+            nearby_type_ = NearbyType::NEARBY4;
+        } else if (dim == 3 && (nearby_type_ != NearbyType::NEARBY6 && nearby_type_ != NearbyType::CENTER)) {
+            LOG(INFO) << "3D grid does not support nearby4/8, using nearby6 instead.";
+            nearby_type_ = NearbyType::NEARBY6;
+        }
+
+        GenerateNearbyGrids();
+    }
+
+    /// 设置点云，建立栅格
+    bool SetPointCloud(CloudPtr cloud);
+
+    /// 获取最近邻
+    bool GetClosestPoint(const PointType& pt, PointType& closest_pt, size_t& idx);
+
+    /// 对比两个点云
+    bool GetClosestPointForCloud(CloudPtr ref, CloudPtr query, std::vector<std::pair<size_t, size_t>>& matches);
+    bool GetClosestPointForCloudMT(CloudPtr ref, CloudPtr query, std::vector<std::pair<size_t, size_t>>& matches);
+
+   private:
+    /// 根据最近邻的类型，生成附近网格
+    void GenerateNearbyGrids();
+
+    /// 空间坐标转到grid
+    KeyType Pos2Grid(const PtType& pt);
+
+    float resolution_ = 0.1;       // 分辨率
+    float inv_resolution_ = 10.0;  // 分辨率倒数
+
+    NearbyType nearby_type_ = NearbyType::NEARBY4;
+    std::unordered_map<KeyType, std::vector<size_t>, hash_vec<dim>> grids_;  //  栅格数据
+    CloudPtr cloud_;
+
+    std::vector<KeyType> nearby_grids_;  // 附近的栅格
+};
+
+// 实现
+template <int dim>
+bool GridNN<dim>::SetPointCloud(CloudPtr cloud) {
+    std::vector<size_t> index(cloud->size());
+    std::for_each(index.begin(), index.end(), [idx = 0](size_t& i) mutable { i = idx++; });
+
+    std::for_each(index.begin(), index.end(), [&cloud, this](const size_t& idx) {
+        auto pt = cloud->points[idx];
+        auto key = Pos2Grid(ToEigen<float, dim>(pt));
+        if (grids_.find(key) == grids_.end()) {
+            grids_.insert({key, {idx}});
+        } else {
+            grids_[key].emplace_back(idx);
+        }
+    });
+
+    cloud_ = cloud;
+    LOG(INFO) << "grids: " << grids_.size();
+    return true;
+}
+
+template <int dim>
+Eigen::Matrix<int, dim, 1> GridNN<dim>::Pos2Grid(const Eigen::Matrix<float, dim, 1>& pt) {
+    return pt.array().template round().template cast<int>();
+    // Eigen::Matrix<int, dim, 1> ret;
+    // for (int i = 0; i < dim; ++i) {
+    //     ret(i, 0) = round(pt[i] * inv_resolution_);
+    // }
+    // return ret;
+}
+
+template <>
+void GridNN<2>::GenerateNearbyGrids() {
+    if (nearby_type_ == NearbyType::CENTER) {
+        nearby_grids_.emplace_back(KeyType::Zero());
+    } else if (nearby_type_ == NearbyType::NEARBY4) {
+        nearby_grids_ = {Vec2i(0, 0), Vec2i(-1, 0), Vec2i(1, 0), Vec2i(0, 1), Vec2i(0, -1)};
+    } else if (nearby_type_ == NearbyType::NEARBY8) {
+        nearby_grids_ = {
+            Vec2i(0, 0),   Vec2i(-1, 0), Vec2i(1, 0),  Vec2i(0, 1), Vec2i(0, -1),
+            Vec2i(-1, -1), Vec2i(-1, 1), Vec2i(1, -1), Vec2i(1, 1),
+        };
+    }
+}
+
+template <>
+void GridNN<3>::GenerateNearbyGrids() {
+    if (nearby_type_ == NearbyType::CENTER) {
+        nearby_grids_.emplace_back(KeyType::Zero());
+    } else if (nearby_type_ == NearbyType::NEARBY6) {
+        nearby_grids_ = {KeyType(0, 0, 0),  KeyType(-1, 0, 0), KeyType(1, 0, 0), KeyType(0, 1, 0),
+                         KeyType(0, -1, 0), KeyType(0, 0, -1), KeyType(0, 0, 1)};
+    }
+}
+
+template <int dim>
+bool GridNN<dim>::GetClosestPoint(const PointType& pt, PointType& closest_pt, size_t& idx) {
+    // 在pt栅格周边寻找最近邻
+    std::vector<size_t> idx_to_check;
+    auto key = Pos2Grid(ToEigen<float, dim>(pt));
+
+    std::for_each(nearby_grids_.begin(), nearby_grids_.end(), [&key, &idx_to_check, this](const KeyType& delta) {
+        auto dkey = key + delta;
+        auto iter = grids_.find(dkey);
+        if (iter != grids_.end()) {
+            idx_to_check.insert(idx_to_check.end(), iter->second.begin(), iter->second.end());
+        }
+    });
+
+    if (idx_to_check.empty()) {
+        return false;
+    }
+
+    // brute force nn in cloud_[idx]
+    CloudPtr nearby_cloud(new PointCloudType);
+    std::vector<size_t> nearby_idx;
+    for (auto& idx : idx_to_check) {
+        nearby_cloud->points.template emplace_back(cloud_->points[idx]);
+        nearby_idx.emplace_back(idx);
+    }
+
+    size_t closest_point_idx = bfnn_point(nearby_cloud, ToVec3f(pt));
+    idx = nearby_idx.at(closest_point_idx);
+    closest_pt = cloud_->points[idx];
+
+    return true;
+}
+
+template <int dim>
+bool GridNN<dim>::GetClosestPointForCloud(CloudPtr ref, CloudPtr query,
+                                          std::vector<std::pair<size_t, size_t>>& matches) {
+    matches.clear();
+    std::vector<size_t> index(query->size());
+    std::for_each(index.begin(), index.end(), [idx = 0](size_t& i) mutable { i = idx++; });
+    std::for_each(index.begin(), index.end(), [this, &matches, &query](const size_t& idx) {
+        PointType cp;
+        size_t cp_idx;
+        if (GetClosestPoint(query->points[idx], cp, cp_idx)) {
+            matches.emplace_back(cp_idx, idx);
+        }
+    });
+
+    return true;
+}
+
+template <int dim>
+bool GridNN<dim>::GetClosestPointForCloudMT(CloudPtr ref, CloudPtr query,
+                                            std::vector<std::pair<size_t, size_t>>& matches) {
+    matches.clear();
+    // 与串行版本基本一样，但matches需要预先生成，匹配失败时填入非法匹配
+    std::vector<size_t> index(query->size());
+    std::for_each(index.begin(), index.end(), [idx = 0](size_t& i) mutable { i = idx++; });
+    matches.resize(index.size());
+
+    std::for_each(std::execution::par_unseq, index.begin(), index.end(), [this, &matches, &query](const size_t& idx) {
+        PointType cp;
+        size_t cp_idx;
+        if (GetClosestPoint(query->points[idx], cp, cp_idx)) {
+            matches[idx] = {cp_idx, idx};
+        } else {
+            matches[idx] = {math::kINVALID_ID, math::kINVALID_ID};
+        }
+    });
+
+    return true;
+}
+
+}  // namespace sad
+
+#endif  // SLAM_IN_AUTO_DRIVING_GRID2D_HPP

workspace/slam_in_autonomous_driving/src/ch5/kdtree.cc

@@ -0,0 +1,236 @@
+//
+// Created by xiang on 2021/9/22.
+//
+
+#include "ch5/kdtree.h"
+#include "common/math_utils.h"
+
+#include <glog/logging.h>
+#include <execution>
+#include <set>
+
+namespace sad {
+
+bool KdTree::BuildTree(const CloudPtr &cloud) {
+    if (cloud->empty()) {
+        return false;
+    }
+
+    cloud_.clear();
+    cloud_.resize(cloud->size());
+    for (size_t i = 0; i < cloud->points.size(); ++i) {
+        cloud_[i] = ToVec3f(cloud->points[i]);
+    }
+
+    Clear();
+    Reset();
+
+    IndexVec idx(cloud->size());
+    for (int i = 0; i < cloud->points.size(); ++i) {
+        idx[i] = i;
+    }
+
+    Insert(idx, root_.get());
+    return true;
+}
+
+void KdTree::Insert(const IndexVec &points, KdTreeNode *node) {
+    nodes_.insert({node->id_, node});
+
+    if (points.empty()) {
+        return;
+    }
+
+    if (points.size() == 1) {
+        size_++;
+        node->point_idx_ = points[0];
+        return;
+    }
+
+    IndexVec left, right;
+    if (!FindSplitAxisAndThresh(points, node->axis_index_, node->split_thresh_, left, right)) {
+        size_++;
+        node->point_idx_ = points[0];
+        return;
+    }
+
+    const auto create_if_not_empty = [&node, this](KdTreeNode *&new_node, const IndexVec &index) {
+        if (!index.empty()) {
+            new_node = new KdTreeNode;
+            new_node->id_ = tree_node_id_++;
+            Insert(index, new_node);
+        }
+    };
+
+    create_if_not_empty(node->left_, left);
+    create_if_not_empty(node->right_, right);
+}
+
+bool KdTree::GetClosestPoint(const PointType &pt, std::vector<int> &closest_idx, int k) {
+    if (k > size_) {
+        LOG(ERROR) << "cannot set k larger than cloud size: " << k << ", " << size_;
+        return false;
+    }
+    k_ = k;
+
+    std::priority_queue<NodeAndDistance> knn_result;
+    Knn(ToVec3f(pt), root_.get(), knn_result);
+
+    // 排序并返回结果
+    closest_idx.resize(knn_result.size());
+    for (int i = closest_idx.size() - 1; i >= 0; --i) {
+        // 倒序插入
+        closest_idx[i] = knn_result.top().node_->point_idx_;
+        knn_result.pop();
+    }
+    return true;
+}
+
+bool KdTree::GetClosestPointMT(const CloudPtr &cloud, std::vector<std::pair<size_t, size_t>> &matches, int k) {
+    matches.resize(cloud->size() * k);
+
+    // 索引
+    std::vector<int> index(cloud->size());
+    for (int i = 0; i < cloud->points.size(); ++i) {
+        index[i] = i;
+    }
+
+    std::for_each(std::execution::par_unseq, index.begin(), index.end(), [this, &cloud, &matches, &k](int idx) {
+        std::vector<int> closest_idx;
+        GetClosestPoint(cloud->points[idx], closest_idx, k);
+        for (int i = 0; i < k; ++i) {
+            matches[idx * k + i].second = idx;
+            if (i < closest_idx.size()) {
+                matches[idx * k + i].first = closest_idx[i];
+            } else {
+                matches[idx * k + i].first = math::kINVALID_ID;
+            }
+        }
+    });
+
+    return true;
+}
+
+void KdTree::Knn(const Vec3f &pt, KdTreeNode *node, std::priority_queue<NodeAndDistance> &knn_result) const {
+    if (node->IsLeaf()) {
+        // 如果是叶子，检查叶子是否能插入
+        ComputeDisForLeaf(pt, node, knn_result);
+        return;
+    }
+
+    // 看pt落在左还是右，优先搜索pt所在的子树
+    // 然后再看另一侧子树是否需要搜索
+    KdTreeNode *this_side, *that_side;
+    if (pt[node->axis_index_] < node->split_thresh_) {
+        this_side = node->left_;
+        that_side = node->right_;
+    } else {
+        this_side = node->right_;
+        that_side = node->left_;
+    }
+
+    Knn(pt, this_side, knn_result);
+    if (NeedExpand(pt, node, knn_result)) {  // 注意这里是跟自己比
+        Knn(pt, that_side, knn_result);
+    }
+}
+
+bool KdTree::NeedExpand(const Vec3f &pt, KdTreeNode *node, std::priority_queue<NodeAndDistance> &knn_result) const {
+    if (knn_result.size() < k_) {
+        return true;
+    }
+
+    if (approximate_) {
+        float d = pt[node->axis_index_] - node->split_thresh_;
+        if ((d * d) < knn_result.top().distance2_ * alpha_) {
+            return true;
+        } else {
+            return false;
+        }
+    } else {
+        // 检测切面距离，看是否有比现在更小的
+        float d = pt[node->axis_index_] - node->split_thresh_;
+        if ((d * d) < knn_result.top().distance2_) {
+            return true;
+        } else {
+            return false;
+        }
+    }
+}
+
+void KdTree::ComputeDisForLeaf(const Vec3f &pt, KdTreeNode *node,
+                               std::priority_queue<NodeAndDistance> &knn_result) const {
+    // 比较与结果队列的差异，如果优于最远距离，则插入
+    float dis2 = Dis2(pt, cloud_[node->point_idx_]);
+    if (knn_result.size() < k_) {
+        // results 不足k
+        knn_result.emplace(node, dis2);
+    } else {
+        // results等于k，比较current与max_dis_iter之间的差异
+        if (dis2 < knn_result.top().distance2_) {
+            knn_result.emplace(node, dis2);
+            knn_result.pop();
+        }
+    }
+}
+
+bool KdTree::FindSplitAxisAndThresh(const IndexVec &point_idx, int &axis, float &th, IndexVec &left, IndexVec &right) {
+    // 计算三个轴上的散布情况，我们使用math_utils.h里的函数
+    Vec3f var;
+    Vec3f mean;
+    math::ComputeMeanAndCovDiag(point_idx, mean, var, [this](int idx) { return cloud_[idx]; });
+    int max_i, max_j;
+    var.maxCoeff(&max_i, &max_j);
+    axis = max_i;
+    th = mean[axis];
+
+    for (const auto &idx : point_idx) {
+        if (cloud_[idx][axis] < th) {
+            // 中位数可能向左取整
+            left.emplace_back(idx);
+        } else {
+            right.emplace_back(idx);
+        }
+    }
+
+    // 边界情况检查：输入的points等于同一个值，上面的判定是>=号，所以都进了右侧
+    // 这种情况不需要继续展开，直接将当前节点设为叶子就行
+    // if (point_idx.size() > 1 && (left.empty() || right.empty())) {
+    //     return false;
+    // }
+
+    return true;
+}
+
+void KdTree::Reset() {
+    tree_node_id_ = 0;
+    root_.reset(new KdTreeNode());
+    root_->id_ = tree_node_id_++;
+    size_ = 0;
+}
+
+void KdTree::Clear() {
+    for (const auto &np : nodes_) {
+        if (np.second != root_.get()) {
+            delete np.second;
+        }
+    }
+
+    nodes_.clear();
+    root_ = nullptr;
+    size_ = 0;
+    tree_node_id_ = 0;
+}
+
+void KdTree::PrintAll() {
+    for (const auto &np : nodes_) {
+        auto node = np.second;
+        if (node->left_ == nullptr && node->right_ == nullptr) {
+            LOG(INFO) << "leaf node: " << node->id_ << ", idx: " << node->point_idx_;
+        } else {
+            LOG(INFO) << "node: " << node->id_ << ", axis: " << node->axis_index_ << ", th: " << node->split_thresh_;
+        }
+    }
+}
+
+}  // namespace sad

workspace/slam_in_autonomous_driving/src/ch5/kdtree.h

@@ -0,0 +1,133 @@
+//
+// Created by xiang on 2021/9/22.
+//
+
+#ifndef SLAM_IN_AUTO_DRIVING_KDTREE_H
+#define SLAM_IN_AUTO_DRIVING_KDTREE_H
+
+#include "common/eigen_types.h"
+#include "common/point_types.h"
+
+#include <glog/logging.h>
+#include <map>
+#include <queue>
+
+namespace sad {
+
+/// Kd树节点，二叉树结构，内部用祼指针，对外一个root的shared_ptr
+struct KdTreeNode {
+    int id_ = -1;
+    int point_idx_ = 0;            // 点的索引
+    int axis_index_ = 0;           // 分割轴
+    float split_thresh_ = 0.0;     // 分割位置
+    KdTreeNode* left_ = nullptr;   // 左子树
+    KdTreeNode* right_ = nullptr;  // 右子树
+
+    bool IsLeaf() const { return left_ == nullptr && right_ == nullptr; }  // 是否为叶子
+};
+
+/// 用于记录knn结果
+struct NodeAndDistance {
+    NodeAndDistance(KdTreeNode* node, float dis2) : node_(node), distance2_(dis2) {}
+    KdTreeNode* node_ = nullptr;
+    float distance2_ = 0;  // 平方距离，用于比较
+
+    bool operator<(const NodeAndDistance& other) const { return distance2_ < other.distance2_; }
+};
+
+/**
+ * 手写kd树
+ * 测试这个kd树的召回!
+ */
+class KdTree {
+   public:
+    explicit KdTree() = default;
+    ~KdTree() { Clear(); }
+
+    bool BuildTree(const CloudPtr& cloud);
+
+    /// 获取k最近邻
+    bool GetClosestPoint(const PointType& pt, std::vector<int>& closest_idx, int k = 5);
+
+    /// 并行为点云寻找最近邻
+    bool GetClosestPointMT(const CloudPtr& cloud, std::vector<std::pair<size_t, size_t>>& matches, int k = 5);
+
+    /// 这个被用于计算最近邻的倍数
+    void SetEnableANN(bool use_ann = true, float alpha = 0.1) {
+        approximate_ = use_ann;
+        alpha_ = alpha;
+    }
+
+    /// 返回节点数量
+    size_t size() const { return size_; }
+
+    /// 清理数据
+    void Clear();
+
+    /// 打印所有节点信息
+    void PrintAll();
+
+   private:
+    /// kdtree 构建相关
+    /**
+     * 在node处插入点
+     * @param points
+     * @param node
+     */
+    void Insert(const IndexVec& points, KdTreeNode* node);
+
+    /**
+     * 计算点集的分割面
+     * @param points 输入点云
+     * @param axis   轴
+     * @param th     阈值
+     * @param left   左子树
+     * @param right  右子树
+     * @return
+     */
+    bool FindSplitAxisAndThresh(const IndexVec& point_idx, int& axis, float& th, IndexVec& left, IndexVec& right);
+
+    void Reset();
+
+    /// 两个点的平方距离
+    static inline float Dis2(const Vec3f& p1, const Vec3f& p2) { return (p1 - p2).squaredNorm(); }
+
+    // Knn 相关
+    /**
+     * 检查给定点在kdtree node上的knn，可以递归调用
+     * @param pt     查询点
+     * @param node   kdtree 节点
+     */
+    void Knn(const Vec3f& pt, KdTreeNode* node, std::priority_queue<NodeAndDistance>& result) const;
+
+    /**
+     * 对叶子节点，计算它和查询点的距离，尝试放入结果中
+     * @param pt    查询点
+     * @param node  Kdtree 节点
+     */
+    void ComputeDisForLeaf(const Vec3f& pt, KdTreeNode* node, std::priority_queue<NodeAndDistance>& result) const;
+
+    /**
+     * 检查node下是否需要展开
+     * @param pt   查询点
+     * @param node Kdtree 节点
+     * @return true if 需要展开
+     */
+    bool NeedExpand(const Vec3f& pt, KdTreeNode* node, std::priority_queue<NodeAndDistance>& knn_result) const;
+
+    int k_ = 5;                                   // knn最近邻数量
+    std::shared_ptr<KdTreeNode> root_ = nullptr;  // 根节点
+    std::vector<Vec3f> cloud_;                    // 输入点云
+    std::unordered_map<int, KdTreeNode*> nodes_;  // for bookkeeping
+
+    size_t size_ = 0;       // 叶子节点数量
+    int tree_node_id_ = 0;  // 为kdtree node 分配id
+
+    // 近似最近邻
+    bool approximate_ = true;
+    float alpha_ = 0.1;
+};
+
+}  // namespace sad
+
+#endif  // SLAM_IN_AUTO_DRIVING_KDTREE_H

workspace/slam_in_autonomous_driving/src/ch5/linear_fitting.cc

@@ -0,0 +1,82 @@
+#include <gflags/gflags.h>
+#include <glog/logging.h>
+#include <opencv2/opencv.hpp>
+
+#include "common/eigen_types.h"
+#include "common/math_utils.h"
+
+DEFINE_int32(num_tested_points_plane, 10, "number of tested points in plane fitting");
+DEFINE_int32(num_tested_points_line, 100, "number of tested points in line fitting");
+DEFINE_double(noise_sigma, 0.01, "noise of generated samples");
+
+void PlaneFittingTest();
+void LineFittingTest();
+
+int main(int argc, char** argv) {
+    google::InitGoogleLogging(argv[0]);
+    FLAGS_stderrthreshold = google::INFO;
+    FLAGS_colorlogtostderr = true;
+    google::ParseCommandLineFlags(&argc, &argv, true);
+
+    LOG(INFO) << "testing plane fitting";
+    PlaneFittingTest();
+
+    LOG(INFO) << "testing line fitting";
+    LineFittingTest();
+}
+
+void PlaneFittingTest() {
+    Vec4d true_plane_coeffs(0.1, 0.2, 0.3, 0.4);
+    true_plane_coeffs.normalize();
+
+    std::vector<Vec3d> points;
+
+    // 随机生成仿真平面点
+    cv::RNG rng;
+    for (int i = 0; i < FLAGS_num_tested_points_plane; ++i) {
+        // 先生成一个随机点，计算第四维，增加噪声，再归一化
+        Vec3d p(rng.uniform(0.0, 1.0), rng.uniform(0.0, 1.0), rng.uniform(0.0, 1.0));
+        double n4 = -p.dot(true_plane_coeffs.head<3>()) / true_plane_coeffs[3];
+        p = p / (n4 + std::numeric_limits<double>::min());  // 防止除零
+        p += Vec3d(rng.gaussian(FLAGS_noise_sigma), rng.gaussian(FLAGS_noise_sigma), rng.gaussian(FLAGS_noise_sigma));
+
+        points.emplace_back(p);
+
+        // 验证在平面上
+        LOG(INFO) << "res of p: " << p.dot(true_plane_coeffs.head<3>()) + true_plane_coeffs[3];
+    }
+
+    Vec4d estimated_plane_coeffs;
+    if (sad::math::FitPlane(points, estimated_plane_coeffs)) {
+        LOG(INFO) << "estimated coeffs: " << estimated_plane_coeffs.transpose()
+                  << ", true: " << true_plane_coeffs.transpose();
+    } else {
+        LOG(INFO) << "plane fitting failed";
+    }
+}
+
+void LineFittingTest() {
+    // 直线拟合参数真值
+    Vec3d true_line_origin(0.1, 0.2, 0.3);
+    Vec3d true_line_dir(0.4, 0.5, 0.6);
+    true_line_dir.normalize();
+
+    // 随机生成直线点，利用参数方程
+    std::vector<Vec3d> points;
+    cv::RNG rng;
+    for (int i = 0; i < fLI::FLAGS_num_tested_points_line; ++i) {
+        double t = rng.uniform(-1.0, 1.0);
+        Vec3d p = true_line_origin + true_line_dir * t;
+        p += Vec3d(rng.gaussian(FLAGS_noise_sigma), rng.gaussian(FLAGS_noise_sigma), rng.gaussian(FLAGS_noise_sigma));
+
+        points.emplace_back(p);
+    }
+
+    Vec3d esti_origin, esti_dir;
+    if (sad::math::FitLine(points, esti_origin, esti_dir)) {
+        LOG(INFO) << "estimated origin: " << esti_origin.transpose() << ", true: " << true_line_origin.transpose();
+        LOG(INFO) << "estimated dir: " << esti_dir.transpose() << ", true: " << true_line_dir.transpose();
+    } else {
+        LOG(INFO) << "line fitting failed";
+    }
+}

workspace/slam_in_autonomous_driving/src/ch5/octo_tree.cc

@@ -0,0 +1,269 @@
+//
+// Created by xiang on 2021/9/27.
+//
+
+#include "octo_tree.h"
+#include "common/math_utils.h"
+
+#include <execution>
+
+namespace sad {
+
+bool OctoTree::BuildTree(const CloudPtr &cloud) {
+    if (cloud->empty()) {
+        return false;
+    }
+
+    cloud_.clear();
+    cloud_.resize(cloud->size());
+    for (size_t i = 0; i < cloud->points.size(); ++i) {
+        cloud_[i] = ToVec3f(cloud->points[i]);
+    }
+
+    Clear();
+    Reset();
+
+    IndexVec idx(cloud->size());
+    for (int i = 0; i < cloud->points.size(); ++i) {
+        idx[i] = i;
+    }
+
+    // 生成根节点的边界框
+    root_->box_ = ComputeBoundingBox();
+    Insert(idx, root_.get());
+
+    return true;
+}
+
+void OctoTree::Insert(const IndexVec &points, OctoTreeNode *node) {
+    nodes_.insert({node->id_, node});
+
+    if (points.empty()) {
+        return;
+    }
+
+    if (points.size() == 1) {
+        size_++;
+        node->point_idx_ = points[0];
+        return;
+    }
+
+    /// 只要点数不为1,就继续展开这个节点
+    std::vector<IndexVec> children_points;
+    ExpandNode(node, points, children_points);
+
+    /// 对子节点进行插入操作
+    for (size_t i = 0; i < 8; ++i) {
+        Insert(children_points[i], node->children[i]);
+    }
+}
+
+void OctoTree::ExpandNode(OctoTreeNode *node, const IndexVec &parent_idx, std::vector<IndexVec> &children_idx) {
+    children_idx.resize(8);
+    for (int i = 0; i < 8; ++i) {
+        node->children[i] = new OctoTreeNode();
+        node->children[i]->id_ = tree_node_id_++;
+    }
+
+    const Box3D &b = node->box_;  // 本节点的box
+    // 中心点
+    float c_x = 0.5 * (node->box_.min_[0] + node->box_.max_[0]);
+    float c_y = 0.5 * (node->box_.min_[1] + node->box_.max_[1]);
+    float c_z = 0.5 * (node->box_.min_[2] + node->box_.max_[2]);
+
+    // 8个外框示意图
+    // clang-format off
+    // 第一层：左上1 右上2 左下3 右下4
+    // 第二层：左上5 右上6 左下7 右下8
+    //     ---> x    /-------/-------/|
+    //    /|        /-------/-------/||
+    //   / |       /-------/-------/ ||
+    //  y  |z      |       |       | /|
+    //             |_______|_______|/|/
+    //             |       |       | /
+    //             |_______|_______|/
+    // clang-format on
+    node->children[0]->box_ = {b.min_[0], c_x, b.min_[1], c_y, b.min_[2], c_z};
+    node->children[1]->box_ = {c_x, b.max_[0], b.min_[1], c_y, b.min_[2], c_z};
+    node->children[2]->box_ = {b.min_[0], c_x, c_y, b.max_[1], b.min_[2], c_z};
+    node->children[3]->box_ = {c_x, b.max_[0], c_y, b.max_[1], b.min_[2], c_z};
+
+    node->children[4]->box_ = {b.min_[0], c_x, b.min_[1], c_y, c_z, b.max_[2]};
+    node->children[5]->box_ = {c_x, b.max_[0], b.min_[1], c_y, c_z, b.max_[2]};
+    node->children[6]->box_ = {b.min_[0], c_x, c_y, b.max_[1], c_z, b.max_[2]};
+    node->children[7]->box_ = {c_x, b.max_[0], c_y, b.max_[1], c_z, b.max_[2]};
+
+    // 把点云归到子节点中
+    for (int idx : parent_idx) {
+        const auto pt = cloud_[idx];
+        for (int i = 0; i < 8; ++i) {
+            if (node->children[i]->box_.Inside(pt)) {
+                children_idx[i].emplace_back(idx);
+                break;
+            }
+        }
+    }
+}
+
+Box3D OctoTree::ComputeBoundingBox() {
+    float min_values[3] = {std::numeric_limits<float>::max(),std::numeric_limits<float>::max(),std::numeric_limits<float>::max()};
+    float max_values[3] = {-std::numeric_limits<float>::max(),-std::numeric_limits<float>::max(),-std::numeric_limits<float>::max()};
+
+    for (const auto &p : cloud_) {
+        for (int i = 0; i < 3; ++i) {
+            max_values[i] = p[i] > max_values[i] ? p[i] : max_values[i];
+            min_values[i] = p[i] < min_values[i] ? p[i] : min_values[i];
+        }
+    }
+
+    return {min_values[0], max_values[0], min_values[1], max_values[1], min_values[2], max_values[2]};
+}
+
+bool OctoTree::GetClosestPoint(const PointType &pt, std::vector<int> &closest_idx, int k) const {
+    if (k > size_) {
+        LOG(ERROR) << "cannot set k larger than cloud size: " << k << ", " << size_;
+        return false;
+    }
+
+    std::priority_queue<NodeAndDistanceOcto> knn_result;
+    Knn(ToVec3f(pt), root_.get(), knn_result);
+
+    // 排序并返回结果
+    closest_idx.resize(knn_result.size());
+    for (int i = closest_idx.size() - 1; i >= 0; --i) {
+        // 倒序插入
+        closest_idx[i] = knn_result.top().node_->point_idx_;
+        knn_result.pop();
+    }
+    return true;
+}
+
+bool OctoTree::GetClosestPointMT(const CloudPtr &cloud, std::vector<std::pair<size_t, size_t>> &matches, int k) {
+    k_ = k;
+    matches.resize(cloud->size() * k);
+    // 索引
+    std::vector<int> index(cloud->size());
+    for (int i = 0; i < cloud->points.size(); ++i) {
+        index[i] = i;
+    }
+
+    std::for_each(std::execution::par_unseq, index.begin(), index.end(), [this, &cloud, &matches, &k](int idx) {
+        std::vector<int> closest_idx;
+        GetClosestPoint(cloud->points[idx], closest_idx, k);
+
+        for (int i = 0; i < k; ++i) {
+            matches[idx * k + i].second = idx;
+            if (i < closest_idx.size()) {
+                matches[idx * k + i].first = closest_idx[i];
+            } else {
+                matches[idx * k + i].first = math::kINVALID_ID;
+            }
+        }
+    });
+
+    return true;
+}
+
+void OctoTree::Clear() {
+    for (const auto &np : nodes_) {
+        if (np.second != root_.get()) {
+            delete np.second;
+        }
+    }
+
+    root_ = nullptr;
+    size_ = 0;
+    tree_node_id_ = 0;
+}
+
+void OctoTree::Reset() {
+    tree_node_id_ = 0;
+    root_.reset(new OctoTreeNode());
+    root_->id_ = tree_node_id_++;
+    size_ = 0;
+}
+
+void OctoTree::Knn(const Vec3f &pt, OctoTreeNode *node, std::priority_queue<NodeAndDistanceOcto> &knn_result) const {
+    if (node->IsLeaf()) {
+        if (node->point_idx_ != -1) {
+            // 如果是叶子，看该点是否为最近邻
+            ComputeDisForLeaf(pt, node, knn_result);
+            return;
+        }
+        return;
+    }
+
+    // 看pt落在哪一格，优先搜索pt所在的子树
+    // 然后再看其他子树是否需要搜索
+    // 如果pt在外边，优先搜索最近的子树
+    int idx_child = -1;
+    float min_dis = std::numeric_limits<float>::max();
+    for (int i = 0; i < 8; ++i) {
+        if (node->children[i]->box_.Inside(pt)) {
+            idx_child = i;
+            break;
+        } else {
+            float d = node->children[i]->box_.Dis(pt);
+            if (d < min_dis) {
+                idx_child = i;
+                min_dis = d;
+            }
+        }
+    }
+
+    // 先检查idx_child
+    Knn(pt, node->children[idx_child], knn_result);
+
+    // 再检查其他的
+    for (int i = 0; i < 8; ++i) {
+        if (i == idx_child) {
+            continue;
+        }
+
+        if (NeedExpand(pt, node->children[i], knn_result)) {
+            Knn(pt, node->children[i], knn_result);
+        }
+    }
+}
+
+void OctoTree::ComputeDisForLeaf(const Vec3f &pt, OctoTreeNode *node,
+                                 std::priority_queue<NodeAndDistanceOcto> &knn_result) const {
+    // 比较与结果队列的差异，如果优于最远距离，则插入
+    float dis2 = Dis2(pt, cloud_[node->point_idx_]);
+    if (knn_result.size() < k_) {
+        // results 不足k
+        knn_result.push({node, dis2});
+    } else {
+        // results等于k，比较current与max_dis_iter之间的差异
+        if (dis2 < knn_result.top().distance_) {
+            knn_result.push({node, dis2});
+            knn_result.pop();
+        }
+    }
+}
+
+bool OctoTree::NeedExpand(const Vec3f &pt, OctoTreeNode *node,
+                          std::priority_queue<NodeAndDistanceOcto> &knn_result) const {
+    if (knn_result.size() < k_) {
+        return true;
+    }
+
+    if (approximate_) {
+        float d = node->box_.Dis(pt);
+        if ((d * d) < knn_result.top().distance_ * alpha_) {
+            return true;
+        } else {
+            return false;
+        }
+    } else {
+        // 不用flann时，按通常情况查找
+        float d = node->box_.Dis(pt);
+        if ((d * d) < knn_result.top().distance_) {
+            return true;
+        } else {
+            return false;
+        }
+    }
+}
+
+}  // namespace sad

workspace/slam_in_autonomous_driving/src/ch5/octo_tree.h

@@ -0,0 +1,164 @@
+//
+// Created by xiang on 2021/9/27.
+//
+
+#ifndef SLAM_IN_AUTO_DRIVING_OCTO_TREE_H
+#define SLAM_IN_AUTO_DRIVING_OCTO_TREE_H
+
+#include "common/eigen_types.h"
+#include "common/point_types.h"
+
+#include <glog/logging.h>
+#include <map>
+#include <queue>
+
+namespace sad {
+
+// 3D Box 记录各轴上的最大最小值
+struct Box3D {
+    Box3D() = default;
+    Box3D(float min_x, float max_x, float min_y, float max_y, float min_z, float max_z)
+        : min_{min_x, min_y, min_z}, max_{max_x, max_y, max_z} {}
+
+    /// 判断pt是否在内部
+    bool Inside(const Vec3f& pt) const {
+        return pt[0] <= max_[0] && pt[0] >= min_[0] && pt[1] <= max_[1] && pt[1] >= min_[1] && pt[2] <= max_[2] &&
+               pt[2] >= min_[2];
+    }
+
+    /// 点到3D Box距离
+    /// 我们取外侧点到边界的最大值
+    float Dis(const Vec3f& pt) const {
+        float ret = 0;
+        for (int i = 0; i < 3; ++i) {
+            if (pt[i] < min_[i]) {
+                float d = min_[i] - pt[i];
+                ret = d > ret ? d : ret;
+            } else if (pt[i] > max_[i]) {
+                float d = pt[i] - max_[i];
+                ret = d > ret ? d : ret;
+            }
+        }
+
+        assert(ret >= 0);
+        return ret;
+    }
+
+    float min_[3] = {0};
+    float max_[3] = {0};
+};
+
+/// octo tree 节点
+struct OctoTreeNode {
+    int id_ = -1;
+    int point_idx_ = -1;                    // 点的索引，-1为无效
+    Box3D box_;                             // 边界框
+    OctoTreeNode* children[8] = {nullptr};  // 子节点
+
+    bool IsLeaf() const {
+        for (const OctoTreeNode* n : children) {
+            if (n != nullptr) {
+                return false;
+            }
+        }
+        return true;
+    }
+};
+
+/// 用于记录knn结果
+struct NodeAndDistanceOcto {
+    NodeAndDistanceOcto(OctoTreeNode* node, float dis2) : node_(node), distance_(dis2) {}
+    OctoTreeNode* node_ = nullptr;
+    float distance_ = 0;  // 平方距离，用于比较
+
+    bool operator<(const NodeAndDistanceOcto& other) const { return distance_ < other.distance_; }
+};
+
+class OctoTree {
+   public:
+    explicit OctoTree() = default;
+    ~OctoTree() { Clear(); }
+
+    bool BuildTree(const CloudPtr& cloud);
+
+    /// 获取k最近邻
+    bool GetClosestPoint(const PointType& pt, std::vector<int>& closest_idx, int k = 5) const;
+
+    /// 并行为点云寻找最近邻
+    bool GetClosestPointMT(const CloudPtr& cloud, std::vector<std::pair<size_t, size_t>>& match, int k = 5);
+
+    /// 设置近似最近邻参数
+    void SetApproximate(bool use_ann = true, float alpha = 0.1) {
+        approximate_ = use_ann;
+        alpha_ = alpha;
+    }
+
+    /// 返回节点数量
+    size_t size() const { return size_; }
+
+    /// 清理数据
+    void Clear();
+
+   private:
+    /// kdtree 构建相关
+    /**
+     * 在node处插入点
+     * @param points
+     * @param node
+     */
+    void Insert(const IndexVec& points, OctoTreeNode* node);
+
+    /// 为全局点云生成边界框
+    Box3D ComputeBoundingBox();
+
+    /**
+     * 展开一个节点
+     * @param [in] node 被展开的节点
+     * @param [in] parent_idx 父节点的点云索引
+     * @param [out] children_idx 子节点的点云索引
+     */
+    void ExpandNode(OctoTreeNode* node, const IndexVec& parent_idx, std::vector<IndexVec>& children_idx);
+
+    void Reset();
+
+    /// 两个点的平方距离
+    static inline float Dis2(const Vec3f& p1, const Vec3f& p2) { return (p1 - p2).squaredNorm(); }
+
+    // Knn 相关
+    /**
+     * 检查给定点在kdtree node上的knn，可以递归调用
+     * @param pt     查询点
+     * @param node   kdtree 节点
+     */
+    void Knn(const Vec3f& pt, OctoTreeNode* node, std::priority_queue<NodeAndDistanceOcto>& result) const;
+
+    /**
+     * 对叶子节点，计算它和查询点的距离，尝试放入结果中
+     * @param pt    查询点
+     * @param node  Kdtree 节点
+     */
+    void ComputeDisForLeaf(const Vec3f& pt, OctoTreeNode* node, std::priority_queue<NodeAndDistanceOcto>& result) const;
+
+    /**
+     * 检查node下是否需要展开
+     * @param pt   查询点
+     * @param node Kdtree 节点
+     * @return true if 需要展开
+     */
+    bool NeedExpand(const Vec3f& pt, OctoTreeNode* node, std::priority_queue<NodeAndDistanceOcto>& knn_result) const;
+
+    int k_ = 5;                                     // knn最近邻数量
+    std::shared_ptr<OctoTreeNode> root_ = nullptr;  // 叶子节点
+    std::vector<Vec3f> cloud_;                      // 输入点云
+    std::map<int, OctoTreeNode*> nodes_;            // for bookkeeping
+    size_t size_ = 0;                               // 叶子节点数量
+    int tree_node_id_ = 0;                          // 为kdtree node 分配id
+
+    // flann
+    bool approximate_ = false;
+    float alpha_ = 1.0;  // flann的距离倍数
+};
+
+}  // namespace sad
+
+#endif  // SLAM_IN_AUTO_DRIVING_OCTO_TREE_H

workspace/slam_in_autonomous_driving/src/ch5/pcd_to_bird_eye.cc

@@ -0,0 +1,85 @@
+//
+// Created by xiang on 2021/8/9.
+//
+
+#include <gflags/gflags.h>
+#include <glog/logging.h>
+
+#include <pcl/io/pcd_io.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
+
+using PointType = pcl::PointXYZI;
+using PointCloudType = pcl::PointCloud<PointType>;
+
+DEFINE_string(pcd_path, "./data/ch5/map_example.pcd", "点云文件路径");
+DEFINE_double(image_resolution, 0.1, "俯视图分辨率");
+DEFINE_double(min_z, 0.2, "俯视图最低高度");
+DEFINE_double(max_z, 2.5, "俯视图最高高度");
+
+/// 本节演示如何将一个点云转换为俯视图像
+void GenerateBEVImage(PointCloudType::Ptr cloud) {
+    // 计算点云边界
+    auto minmax_x = std::minmax_element(cloud->points.begin(), cloud->points.end(),
+                                        [](const PointType& p1, const PointType& p2) { return p1.x < p2.x; });
+    auto minmax_y = std::minmax_element(cloud->points.begin(), cloud->points.end(),
+                                        [](const PointType& p1, const PointType& p2) { return p1.y < p2.y; });
+    double min_x = minmax_x.first->x;
+    double max_x = minmax_x.second->x;
+    double min_y = minmax_y.first->y;
+    double max_y = minmax_y.second->y;
+
+    const double inv_r = 1.0 / FLAGS_image_resolution;
+
+    const int image_rows = int((max_y - min_y) * inv_r);
+    const int image_cols = int((max_x - min_x) * inv_r);
+
+    float x_center = 0.5 * (max_x + min_x);
+    float y_center = 0.5 * (max_y + min_y);
+    float x_center_image = image_cols / 2;
+    float y_center_image = image_rows / 2;
+
+    // 生成图像
+    cv::Mat image(image_rows, image_cols, CV_8UC3, cv::Scalar(255, 255, 255));
+
+    for (const auto& pt : cloud->points) {
+        int x = int((pt.x - x_center) * inv_r + x_center_image);
+        int y = int((pt.y - y_center) * inv_r + y_center_image);
+        if (x < 0 || x >= image_cols || y < 0 || y >= image_rows || pt.z < FLAGS_min_z || pt.z > FLAGS_max_z) {
+            continue;
+        }
+
+        image.at<cv::Vec3b>(y, x) = cv::Vec3b(227, 143, 79);
+    }
+
+    cv::imwrite("./bev.png", image);
+}
+
+int main(int argc, char** argv) {
+    google::InitGoogleLogging(argv[0]);
+    FLAGS_stderrthreshold = google::INFO;
+    FLAGS_colorlogtostderr = true;
+    google::ParseCommandLineFlags(&argc, &argv, true);
+
+    if (FLAGS_pcd_path.empty()) {
+        LOG(ERROR) << "pcd path is empty";
+        return -1;
+    }
+
+    // 读取点云
+    PointCloudType::Ptr cloud(new PointCloudType);
+    pcl::io::loadPCDFile(FLAGS_pcd_path, *cloud);
+
+    if (cloud->empty()) {
+        LOG(ERROR) << "cannot load cloud file";
+        return -1;
+    }
+
+    LOG(INFO) << "cloud points: " << cloud->size();
+    GenerateBEVImage(cloud);
+
+    return 0;
+}

workspace/slam_in_autonomous_driving/src/ch5/point_cloud_load_and_vis.cc

@@ -0,0 +1,49 @@
+//
+// Created by xiang on 2021/8/6.
+//
+
+#include <gflags/gflags.h>
+#include <glog/logging.h>
+
+#include <pcl/io/pcd_io.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/visualization/pcl_visualizer.h>
+
+using PointType = pcl::PointXYZI;
+using PointCloudType = pcl::PointCloud<PointType>;
+
+DEFINE_string(pcd_path, "./data/ch5/map_example.pcd", "点云文件路径");
+
+/// 本程序可用于显示单个点云，演示PCL的基本用法
+/// 实际上就是调用了pcl的可视化库，类似于pcl_viewer
+int main(int argc, char** argv) {
+    google::InitGoogleLogging(argv[0]);
+    FLAGS_stderrthreshold = google::INFO;
+    FLAGS_colorlogtostderr = true;
+    google::ParseCommandLineFlags(&argc, &argv, true);
+
+    if (FLAGS_pcd_path.empty()) {
+        LOG(ERROR) << "pcd path is empty";
+        return -1;
+    }
+
+    // 读取点云
+    PointCloudType::Ptr cloud(new PointCloudType);
+    pcl::io::loadPCDFile(FLAGS_pcd_path, *cloud);
+
+    if (cloud->empty()) {
+        LOG(ERROR) << "cannot load cloud file";
+        return -1;
+    }
+
+    LOG(INFO) << "cloud points: " << cloud->size();
+
+    // visualize
+    pcl::visualization::PCLVisualizer viewer("cloud viewer");
+    pcl::visualization::PointCloudColorHandlerGenericField<PointType> handle(cloud, "z");  // 使用高度来着色
+    viewer.addPointCloud<PointType>(cloud, handle);
+    viewer.spin();
+
+    return 0;
+}

workspace/slam_in_autonomous_driving/src/ch5/scan_to_range_image.cc

@@ -0,0 +1,84 @@
+// // Created by xiang on 2021/8/9.
+//
+
+#include <gflags/gflags.h>
+#include <glog/logging.h>
+
+#include <pcl/io/pcd_io.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+#include <opencv2/opencv.hpp>
+
+using PointType = pcl::PointXYZI;
+using PointCloudType = pcl::PointCloud<PointType>;
+
+DEFINE_string(pcd_path, "./data/ch5/scan_example.pcd", "点云文件路径");
+DEFINE_double(azimuth_resolution_deg, 0.3, "方位角分辨率（度）");
+DEFINE_int32(elevation_rows, 16, "俯仰角对应的行数");
+DEFINE_double(elevation_range, 15.0, "俯仰角范围");  // VLP-16 上下各15度范围
+DEFINE_double(lidar_height, 1.128, "雷达安装高度");
+
+void GenerateRangeImage(PointCloudType::Ptr cloud) {
+    int image_cols = int(360 / FLAGS_azimuth_resolution_deg);  // 水平为360度，按分辨率切分即可
+    int image_rows = FLAGS_elevation_rows;                     // 图像行数固定
+    LOG(INFO) << "range image: " << image_rows << "x" << image_cols;
+
+    // 我们生成一个HSV图像以更好地显示图像
+    cv::Mat image(image_rows, image_cols, CV_8UC3, cv::Scalar(0, 0, 0));
+
+    double ele_resolution = FLAGS_elevation_range * 2 / FLAGS_elevation_rows;  // elevation分辨率
+
+    for (const auto& pt : cloud->points) {
+        double azimuth = atan2(pt.y, pt.x) * 180 / M_PI;
+        double range = sqrt(pt.x * pt.x + pt.y * pt.y);
+        double elevation = asin((pt.z - FLAGS_lidar_height) / range) * 180 / M_PI;
+
+        // keep in 0~360
+        if (azimuth < 0) {
+            azimuth += 360;
+        }
+
+        int x = int(azimuth / FLAGS_azimuth_resolution_deg);                      // 行
+        int y = int((elevation + FLAGS_elevation_range) / ele_resolution + 0.5);  // 列
+
+        if (x >= 0 && x < image.cols && y >= 0 && y < image.rows) {
+            image.at<cv::Vec3b>(y, x) = cv::Vec3b(uchar(range / 100 * 255.0), 255, 127);
+        }
+    }
+
+    // 沿Y轴翻转，因为我们希望Z轴朝上时Y朝上
+    cv::Mat image_flipped;
+    cv::flip(image, image_flipped, 0);
+
+    // hsv to rgb
+    cv::Mat image_rgb;
+    cv::cvtColor(image_flipped, image_rgb, cv::COLOR_HSV2BGR);
+    cv::imwrite("./range_image.png", image_rgb);
+}
+
+int main(int argc, char** argv) {
+    google::InitGoogleLogging(argv[0]);
+    FLAGS_stderrthreshold = google::INFO;
+    FLAGS_colorlogtostderr = true;
+    google::ParseCommandLineFlags(&argc, &argv, true);
+
+    if (FLAGS_pcd_path.empty()) {
+        LOG(ERROR) << "pcd path is empty";
+        return -1;
+    }
+
+    // 读取点云
+    PointCloudType::Ptr cloud(new PointCloudType);
+    pcl::io::loadPCDFile(FLAGS_pcd_path, *cloud);
+
+    if (cloud->empty()) {
+        LOG(ERROR) << "cannot load cloud file";
+        return -1;
+    }
+
+    LOG(INFO) << "cloud points: " << cloud->size();
+    GenerateRangeImage(cloud);
+
+    return 0;
+}

workspace/slam_in_autonomous_driving/src/ch5/test_nn.cc

@@ -0,0 +1,348 @@
+//
+// Created by xiang on 2021/8/19.
+//
+#include <gflags/gflags.h>
+#include <glog/logging.h>
+#include <gtest/gtest.h>
+
+#include <pcl/io/pcd_io.h>
+#include <pcl/search/kdtree.h>
+
+#include "ch5/bfnn.h"
+#include "ch5/gridnn.hpp"
+#include "ch5/kdtree.h"
+#include "ch5/octo_tree.h"
+#include "common/point_cloud_utils.h"
+#include "common/point_types.h"
+#include "common/sys_utils.h"
+
+DEFINE_string(first_scan_path, "./data/ch5/first.pcd", "第一个点云路径");
+DEFINE_string(second_scan_path, "./data/ch5/second.pcd", "第二个点云路径");
+DEFINE_double(ANN_alpha, 1.0, "AAN的比例因子");
+
+TEST(CH5_TEST, BFNN) {
+    sad::CloudPtr first(new sad::PointCloudType), second(new sad::PointCloudType);
+    pcl::io::loadPCDFile(FLAGS_first_scan_path, *first);
+    pcl::io::loadPCDFile(FLAGS_second_scan_path, *second);
+
+    if (first->empty() || second->empty()) {
+        LOG(ERROR) << "cannot load cloud";
+        FAIL();
+    }
+
+    // voxel grid 至 0.05
+    sad::VoxelGrid(first);
+    sad::VoxelGrid(second);
+
+    LOG(INFO) << "points: " << first->size() << ", " << second->size();
+
+    // 评价单线程和多线程版本的暴力匹配
+    sad::evaluate_and_call(
+        [&first, &second]() {
+            std::vector<std::pair<size_t, size_t>> matches;
+            sad::bfnn_cloud(first, second, matches);
+        },
+        "暴力匹配（单线程）", 5);
+    sad::evaluate_and_call(
+        [&first, &second]() {
+            std::vector<std::pair<size_t, size_t>> matches;
+            sad::bfnn_cloud_mt(first, second, matches);
+        },
+        "暴力匹配（多线程）", 5);
+
+    SUCCEED();
+}
+
+/**
+ * 评测最近邻的正确性
+ * @param truth 真值
+ * @param esti  估计
+ */
+void EvaluateMatches(const std::vector<std::pair<size_t, size_t>>& truth,
+                     const std::vector<std::pair<size_t, size_t>>& esti) {
+    int fp = 0;  // false-positive，esti存在但truth中不存在
+    int fn = 0;  // false-negative, truth存在但esti不存在
+
+    LOG(INFO) << "truth: " << truth.size() << ", esti: " << esti.size();
+
+    /// 检查某个匹配在另一个容器中存不存在
+    auto exist = [](const std::pair<size_t, size_t>& data, const std::vector<std::pair<size_t, size_t>>& vec) -> bool {
+        return std::find(vec.begin(), vec.end(), data) != vec.end();
+    };
+
+    int effective_esti = 0;
+    for (const auto& d : esti) {
+        if (d.first != sad::math::kINVALID_ID && d.second != sad::math::kINVALID_ID) {
+            effective_esti++;
+
+            if (!exist(d, truth)) {
+                fp++;
+            }
+        }
+    }
+
+    for (const auto& d : truth) {
+        if (!exist(d, esti)) {
+            fn++;
+        }
+    }
+
+    float precision = 1.0 - float(fp) / effective_esti;
+    float recall = 1.0 - float(fn) / truth.size();
+    LOG(INFO) << "precision: " << precision << ", recall: " << recall << ", fp: " << fp << ", fn: " << fn;
+}
+
+TEST(CH5_TEST, GRID_NN) {
+    sad::CloudPtr first(new sad::PointCloudType), second(new sad::PointCloudType);
+    pcl::io::loadPCDFile(FLAGS_first_scan_path, *first);
+    pcl::io::loadPCDFile(FLAGS_second_scan_path, *second);
+
+    if (first->empty() || second->empty()) {
+        LOG(ERROR) << "cannot load cloud";
+        FAIL();
+    }
+
+    // voxel grid 至 0.05
+    sad::VoxelGrid(first);
+    sad::VoxelGrid(second);
+
+    LOG(INFO) << "points: " << first->size() << ", " << second->size();
+
+    std::vector<std::pair<size_t, size_t>> truth_matches;
+    sad::bfnn_cloud(first, second, truth_matches);
+
+    // 对比不同种类的grid
+    sad::GridNN<2> grid0(0.1, sad::GridNN<2>::NearbyType::CENTER), grid4(0.1, sad::GridNN<2>::NearbyType::NEARBY4),
+        grid8(0.1, sad::GridNN<2>::NearbyType::NEARBY8);
+    sad::GridNN<3> grid3(0.1, sad::GridNN<3>::NearbyType::NEARBY6);
+
+    grid0.SetPointCloud(first);
+    grid4.SetPointCloud(first);
+    grid8.SetPointCloud(first);
+    grid3.SetPointCloud(first);
+
+    // 评价各种版本的Grid NN
+    // sorry没有C17的template lambda... 下面必须写的啰嗦一些
+    LOG(INFO) << "===================";
+    std::vector<std::pair<size_t, size_t>> matches;
+    sad::evaluate_and_call(
+        [&first, &second, &grid0, &matches]() { grid0.GetClosestPointForCloud(first, second, matches); },
+        "Grid0 单线程", 10);
+    EvaluateMatches(truth_matches, matches);
+
+    LOG(INFO) << "===================";
+    sad::evaluate_and_call(
+        [&first, &second, &grid0, &matches]() { grid0.GetClosestPointForCloudMT(first, second, matches); },
+        "Grid0 多线程", 10);
+    EvaluateMatches(truth_matches, matches);
+
+    LOG(INFO) << "===================";
+    sad::evaluate_and_call(
+        [&first, &second, &grid4, &matches]() { grid4.GetClosestPointForCloud(first, second, matches); },
+        "Grid4 单线程", 10);
+    EvaluateMatches(truth_matches, matches);
+
+    LOG(INFO) << "===================";
+    sad::evaluate_and_call(
+        [&first, &second, &grid4, &matches]() { grid4.GetClosestPointForCloudMT(first, second, matches); },
+        "Grid4 多线程", 10);
+    EvaluateMatches(truth_matches, matches);
+
+    LOG(INFO) << "===================";
+    sad::evaluate_and_call(
+        [&first, &second, &grid8, &matches]() { grid8.GetClosestPointForCloud(first, second, matches); },
+        "Grid8 单线程", 10);
+    EvaluateMatches(truth_matches, matches);
+
+    LOG(INFO) << "===================";
+    sad::evaluate_and_call(
+        [&first, &second, &grid8, &matches]() { grid8.GetClosestPointForCloudMT(first, second, matches); },
+        "Grid8 多线程", 10);
+    EvaluateMatches(truth_matches, matches);
+
+    LOG(INFO) << "===================";
+    sad::evaluate_and_call(
+        [&first, &second, &grid3, &matches]() { grid3.GetClosestPointForCloud(first, second, matches); },
+        "Grid 3D 单线程", 10);
+    EvaluateMatches(truth_matches, matches);
+
+    LOG(INFO) << "===================";
+    sad::evaluate_and_call(
+        [&first, &second, &grid3, &matches]() { grid3.GetClosestPointForCloudMT(first, second, matches); },
+        "Grid 3D 多线程", 10);
+    EvaluateMatches(truth_matches, matches);
+
+    SUCCEED();
+}
+
+TEST(CH5_TEST, KDTREE_BASICS) {
+    sad::CloudPtr cloud(new sad::PointCloudType);
+    sad::PointType p1, p2, p3, p4;
+    p1.x = 0;
+    p1.y = 0;
+    p1.z = 0;
+
+    p2.x = 1;
+    p2.y = 0;
+    p2.z = 0;
+
+    p3.x = 0;
+    p3.y = 1;
+    p3.z = 0;
+
+    p4.x = 1;
+    p4.y = 1;
+    p4.z = 0;
+
+    cloud->points.push_back(p1);
+    cloud->points.push_back(p2);
+    cloud->points.push_back(p3);
+    cloud->points.push_back(p4);
+
+    sad::KdTree kdtree;
+    kdtree.BuildTree(cloud);
+    kdtree.PrintAll();
+
+    SUCCEED();
+}
+
+TEST(CH5_TEST, KDTREE_KNN) {
+    sad::CloudPtr first(new sad::PointCloudType), second(new sad::PointCloudType);
+    pcl::io::loadPCDFile(FLAGS_first_scan_path, *first);
+    pcl::io::loadPCDFile(FLAGS_second_scan_path, *second);
+
+    if (first->empty() || second->empty()) {
+        LOG(ERROR) << "cannot load cloud";
+        FAIL();
+    }
+
+    // voxel grid 至 0.05
+    sad::VoxelGrid(first);
+    sad::VoxelGrid(second);
+
+    sad::KdTree kdtree;
+    sad::evaluate_and_call([&first, &kdtree]() { kdtree.BuildTree(first); }, "Kd Tree build", 1);
+
+    kdtree.SetEnableANN(true, FLAGS_ANN_alpha);
+
+    LOG(INFO) << "Kd tree leaves: " << kdtree.size() << ", points: " << first->size();
+
+    // 比较 bfnn
+    std::vector<std::pair<size_t, size_t>> true_matches;
+    sad::bfnn_cloud_mt_k(first, second, true_matches);
+
+    // 对第2个点云执行knn
+    std::vector<std::pair<size_t, size_t>> matches;
+    sad::evaluate_and_call([&first, &second, &kdtree, &matches]() { kdtree.GetClosestPointMT(second, matches, 5); },
+                           "Kd Tree 5NN 多线程", 1);
+    EvaluateMatches(true_matches, matches);
+
+    LOG(INFO) << "building kdtree pcl";
+    // 对比PCL
+    pcl::search::KdTree<sad::PointType> kdtree_pcl;
+    sad::evaluate_and_call([&first, &kdtree_pcl]() { kdtree_pcl.setInputCloud(first); }, "Kd Tree build", 1);
+
+    LOG(INFO) << "searching pcl";
+    matches.clear();
+    std::vector<int> search_indices(second->size());
+    for (int i = 0; i < second->points.size(); i++) {
+        search_indices[i] = i;
+    }
+
+    std::vector<std::vector<int>> result_index;
+    std::vector<std::vector<float>> result_distance;
+    sad::evaluate_and_call(
+        [&]() { kdtree_pcl.nearestKSearch(*second, search_indices, 5, result_index, result_distance); },
+        "Kd Tree 5NN in PCL", 1);
+    for (int i = 0; i < second->points.size(); i++) {
+        for (int j = 0; j < result_index[i].size(); ++j) {
+            int m = result_index[i][j];
+            double d = result_distance[i][j];
+            matches.push_back({m, i});
+        }
+    }
+    EvaluateMatches(true_matches, matches);
+
+    LOG(INFO) << "done.";
+
+    SUCCEED();
+}
+
+TEST(CH5_TEST, OCTREE_BASICS) {
+    sad::CloudPtr cloud(new sad::PointCloudType);
+    sad::PointType p1, p2, p3, p4;
+    p1.x = 0;
+    p1.y = 0;
+    p1.z = 0;
+
+    p2.x = 1;
+    p2.y = 0;
+    p2.z = 0;
+
+    p3.x = 0;
+    p3.y = 1;
+    p3.z = 0;
+
+    p4.x = 1;
+    p4.y = 1;
+    p4.z = 0;
+
+    cloud->points.push_back(p1);
+    cloud->points.push_back(p2);
+    cloud->points.push_back(p3);
+    cloud->points.push_back(p4);
+
+    sad::OctoTree octree;
+    octree.BuildTree(cloud);
+    octree.SetApproximate(false);
+    LOG(INFO) << "Octo tree leaves: " << octree.size() << ", points: " << cloud->size();
+
+    SUCCEED();
+}
+
+TEST(CH5_TEST, OCTREE_KNN) {
+    sad::CloudPtr first(new sad::PointCloudType), second(new sad::PointCloudType);
+    pcl::io::loadPCDFile(FLAGS_first_scan_path, *first);
+    pcl::io::loadPCDFile(FLAGS_second_scan_path, *second);
+
+    if (first->empty() || second->empty()) {
+        LOG(ERROR) << "cannot load cloud";
+        FAIL();
+    }
+
+    // voxel grid 至 0.05
+    sad::VoxelGrid(first);
+    sad::VoxelGrid(second);
+
+    sad::OctoTree octree;
+    sad::evaluate_and_call([&first, &octree]() { octree.BuildTree(first); }, "Octo Tree build", 1);
+
+    octree.SetApproximate(true, FLAGS_ANN_alpha);
+    LOG(INFO) << "Octo tree leaves: " << octree.size() << ", points: " << first->size();
+
+    /// 测试KNN
+    LOG(INFO) << "testing knn";
+    std::vector<std::pair<size_t, size_t>> matches;
+    sad::evaluate_and_call([&first, &second, &octree, &matches]() { octree.GetClosestPointMT(second, matches, 5); },
+                           "Octo Tree 5NN 多线程", 1);
+
+    LOG(INFO) << "comparing with bfnn";
+    /// 比较真值
+    std::vector<std::pair<size_t, size_t>> true_matches;
+    sad::bfnn_cloud_mt_k(first, second, true_matches);
+    EvaluateMatches(true_matches, matches);
+
+    LOG(INFO) << "done.";
+
+    SUCCEED();
+}
+
+int main(int argc, char** argv) {
+    google::InitGoogleLogging(argv[0]);
+    FLAGS_stderrthreshold = google::INFO;
+    FLAGS_colorlogtostderr = true;
+
+    testing::InitGoogleTest(&argc, argv);
+    google::ParseCommandLineFlags(&argc, &argv, true);
+    return RUN_ALL_TESTS();
+}

workspace/slam_in_autonomous_driving/src/ch6/CMakeLists.txt

@@ -0,0 +1,93 @@
+add_library(${PROJECT_NAME}.ch6
+        icp_2d.cc
+        lidar_2d_utils.cc
+        likelihood_field.cc
+        occupancy_map.cc
+        submap.cc
+        mapping_2d.cc
+        multi_resolution_likelihood_field.cc
+        loop_closing.cc
+        frame.cc
+        )
+
+target_link_libraries(${PROJECT_NAME}.ch6
+        ${PCL_LIBRARIES}
+        ${PROJECT_NAME}.common
+        )
+
+add_executable(test_2dlidar_io
+        test_2dlidar_io.cc
+        )
+
+target_link_libraries(test_2dlidar_io
+        ${PROJECT_NAME}.ch6
+        ${OpenCV_LIBS}
+        ${catkin_LIBRARIES}
+        glog gflags
+        ${PCL_LIBRARIES}
+        ${third_party_libs}
+        )
+
+add_executable(test_2d_icp_s2s
+        test_2d_icp_s2s.cc
+        )
+
+target_link_libraries(test_2d_icp_s2s
+        ${PROJECT_NAME}.ch6
+        ${OpenCV_LIBS}
+        ${catkin_LIBRARIES}
+        glog gflags
+        ${PCL_LIBRARIES}
+        )
+
+add_executable(test_2d_icp_likelihood
+        test_2d_icp_likelihood.cc
+        )
+
+target_link_libraries(test_2d_icp_likelihood
+        ${PROJECT_NAME}.ch6
+        ${OpenCV_LIBS}
+        ${catkin_LIBRARIES}
+        glog gflags
+        ${PCL_LIBRARIES}
+        ${g2o_libs}
+        )
+
+add_executable(test_occupancy_grid
+        test_occupancy_grid.cc
+        )
+
+target_link_libraries(test_occupancy_grid
+        ${PROJECT_NAME}.ch6
+        ${OpenCV_LIBS}
+        ${catkin_LIBRARIES}
+        glog gflags
+        ${PCL_LIBRARIES}
+        ${g2o_libs}
+        )
+
+add_executable(test_2d_mapping
+        test_2d_mapping.cc
+        )
+
+target_link_libraries(test_2d_mapping
+        ${PROJECT_NAME}.ch6
+        ${OpenCV_LIBS}
+        ${catkin_LIBRARIES}
+        glog gflags
+        ${PCL_LIBRARIES}
+        ${g2o_libs}
+        )
+
+add_executable(test_mr_matching
+        test_mr_matching.cc
+        )
+
+target_link_libraries(test_mr_matching
+        ${PROJECT_NAME}.ch6
+        ${OpenCV_LIBS}
+        ${catkin_LIBRARIES}
+        glog gflags
+        ${PCL_LIBRARIES}
+        ${g2o_libs}
+        )

workspace/slam_in_autonomous_driving/src/ch6/frame.cc

@@ -0,0 +1,47 @@
+//
+// Created by xiang on 2022/4/7.
+//
+
+#include "frame.h"
+
+#include <glog/logging.h>
+#include <fstream>
+
+namespace sad {
+
+void Frame::Dump(const std::string& filename) {
+    std::ofstream fout(filename);
+    fout << id_ << " " << keyframe_id_ << " " << timestamp_ << std::endl;
+    fout << pose_.translation()[0] << " " << pose_.translation()[1] << " " << pose_.so2().log() << std::endl;
+    fout << scan_->angle_min << " " << scan_->angle_max << " " << scan_->angle_increment << " " << scan_->range_min
+         << " " << scan_->range_max << " " << scan_->ranges.size() << std::endl;
+    for (auto& r : scan_->ranges) {
+        fout << r << " ";
+    }
+    fout.close();
+}
+
+void Frame::Load(const std::string& filename) {
+    std::ifstream fin(filename);
+    if (!fin) {
+        LOG(ERROR) << "cannot load from " << filename;
+        return;
+    }
+
+    fin >> id_ >> keyframe_id_ >> timestamp_ >> pose_.translation()[0] >> pose_.translation()[1];
+    double theta = 0;
+    fin >> theta;
+    pose_.so2() = SO2::exp(theta);
+    scan_.reset(new Scan2d);
+    fin >> scan_->angle_min >> scan_->angle_max >> scan_->angle_increment >> scan_->range_min >> scan_->range_max;
+
+    int range_size;
+    fin >> range_size;
+    for (int i = 0; i < range_size; ++i) {
+        double r;
+        fin >> r;
+        scan_->ranges.emplace_back(r);
+    }
+}
+
+}  // namespace sad

workspace/slam_in_autonomous_driving/src/ch6/frame.h

@@ -0,0 +1,36 @@
+//
+// Created by xiang on 2022/3/23.
+//
+
+#ifndef SLAM_IN_AUTO_DRIVING_FRAME_H
+#define SLAM_IN_AUTO_DRIVING_FRAME_H
+
+#include "common/eigen_types.h"
+#include "common/lidar_utils.h"
+
+namespace sad {
+
+/**
+ * 一次2D lidar扫描
+ */
+struct Frame {
+    Frame() {}
+    Frame(Scan2d::Ptr scan) : scan_(scan) {}
+
+    /// 将当前帧存入文本文件以供离线调用
+    void Dump(const std::string& filename);
+
+    /// 从文件读取frame数据
+    void Load(const std::string& filename);
+
+    size_t id_ = 0;               // scan id
+    size_t keyframe_id_ = 0;      // 关键帧 id
+    double timestamp_ = 0;        // 时间戳，一般不用
+    Scan2d::Ptr scan_ = nullptr;  // 激光扫描数据
+    SE2 pose_;                    // 位姿，world to scan, T_w_c
+    SE2 pose_submap_;             // 位姿，submap to scan, T_s_c
+};
+
+}  // namespace sad
+
+#endif  // SLAM_IN_AUTO_DRIVING_FRAME_H

workspace/slam_in_autonomous_driving/src/ch6/g2o_types.h

@@ -0,0 +1,133 @@
+//
+// Created by xiang on 2022/3/22.
+//
+
+#ifndef SLAM_IN_AUTO_DRIVING_G2O_TYPES_H
+#define SLAM_IN_AUTO_DRIVING_G2O_TYPES_H
+
+#include <g2o/core/base_binary_edge.h>
+#include <g2o/core/base_unary_edge.h>
+#include <g2o/core/base_vertex.h>
+
+#include <glog/logging.h>
+#include <opencv2/core.hpp>
+
+#include "common/eigen_types.h"
+#include "common/math_utils.h"
+
+namespace sad {
+
+class VertexSE2 : public g2o::BaseVertex<3, SE2> {
+   public:
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+    void setToOriginImpl() override { _estimate = SE2(); }
+    void oplusImpl(const double* update) override {
+        _estimate.translation()[0] += update[0];
+        _estimate.translation()[1] += update[1];
+        _estimate.so2() = _estimate.so2() * SO2::exp(update[2]);
+    }
+
+    bool read(std::istream& is) override { return true; }
+    bool write(std::ostream& os) const override { return true; }
+};
+
+class EdgeSE2LikelihoodFiled : public g2o::BaseUnaryEdge<1, double, VertexSE2> {
+   public:
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+    EdgeSE2LikelihoodFiled(const cv::Mat& field_image, double range, double angle, float resolution = 10.0)
+        : field_image_(field_image), range_(range), angle_(angle), resolution_(resolution) {}
+
+    /// 判定此条边是否在field image外面
+    bool IsOutSide() {
+        VertexSE2* v = (VertexSE2*)_vertices[0];
+        SE2 pose = v->estimate();
+        Vec2d pw = pose * Vec2d(range_ * std::cos(angle_), range_ * std::sin(angle_));
+        Vec2i pf = (pw * resolution_ + Vec2d(field_image_.rows / 2, field_image_.cols / 2)).cast<int>();  // 图像坐标
+
+        if (pf[0] >= image_boarder_ && pf[0] < field_image_.cols - image_boarder_ && pf[1] >= image_boarder_ &&
+            pf[1] < field_image_.rows - image_boarder_) {
+            return false;
+        } else {
+            return true;
+        }
+    }
+
+    void computeError() override {
+        VertexSE2* v = (VertexSE2*)_vertices[0];
+        SE2 pose = v->estimate();
+        Vec2d pw = pose * Vec2d(range_ * std::cos(angle_), range_ * std::sin(angle_));
+        Vec2d pf =
+            pw * resolution_ + Vec2d(field_image_.rows / 2, field_image_.cols / 2) - Vec2d(0.5, 0.5);  // 图像坐标
+
+        if (pf[0] >= image_boarder_ && pf[0] < field_image_.cols - image_boarder_ && pf[1] >= image_boarder_ &&
+            pf[1] < field_image_.rows - image_boarder_) {
+            _error[0] = math::GetPixelValue<float>(field_image_, pf[0], pf[1]);
+        } else {
+            _error[0] = 0;
+            setLevel(1);
+        }
+    }
+
+    void linearizeOplus() override {
+        VertexSE2* v = (VertexSE2*)_vertices[0];
+        SE2 pose = v->estimate();
+        float theta = pose.so2().log();
+        Vec2d pw = pose * Vec2d(range_ * std::cos(angle_), range_ * std::sin(angle_));
+        Vec2d pf =
+            pw * resolution_ + Vec2d(field_image_.rows / 2, field_image_.cols / 2) - Vec2d(0.5, 0.5);  // 图像坐标
+
+        if (pf[0] >= image_boarder_ && pf[0] < field_image_.cols - image_boarder_ && pf[1] >= image_boarder_ &&
+            pf[1] < field_image_.rows - image_boarder_) {
+            // 图像梯度
+            float dx = 0.5 * (math::GetPixelValue<float>(field_image_, pf[0] + 1, pf[1]) -
+                              math::GetPixelValue<float>(field_image_, pf[0] - 1, pf[1]));
+            float dy = 0.5 * (math::GetPixelValue<float>(field_image_, pf[0], pf[1] + 1) -
+                              math::GetPixelValue<float>(field_image_, pf[0], pf[1] - 1));
+
+            _jacobianOplusXi << resolution_ * dx, resolution_ * dy,
+                -resolution_ * dx * range_ * std::sin(angle_ + theta) +
+                    resolution_ * dy * range_ * std::cos(angle_ + theta);
+        } else {
+            _jacobianOplusXi.setZero();
+            setLevel(1);
+        }
+    }
+
+    bool read(std::istream& is) override { return true; }
+    bool write(std::ostream& os) const override { return true; }
+
+   private:
+    const cv::Mat& field_image_;
+    double range_ = 0;
+    double angle_ = 0;
+    float resolution_ = 10.0;
+    inline static const int image_boarder_ = 10;
+};
+
+/**
+ * SE2 pose graph使用
+ * error = v1.inv * v2 * meas.inv
+ */
+class EdgeSE2 : public g2o::BaseBinaryEdge<3, SE2, VertexSE2, VertexSE2> {
+   public:
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+    EdgeSE2() {}
+
+    void computeError() override {
+        VertexSE2* v1 = (VertexSE2*)_vertices[0];
+        VertexSE2* v2 = (VertexSE2*)_vertices[1];
+        _error = (v1->estimate().inverse() * v2->estimate() * measurement().inverse()).log();
+    }
+
+    // TODO jacobian
+
+    bool read(std::istream& is) override { return true; }
+    bool write(std::ostream& os) const override { return true; }
+
+   private:
+};
+
+}  // namespace sad
+
+#endif  // SLAM_IN_AUTO_DRIVING_G2O_TYPES_H

workspace/slam_in_autonomous_driving/src/ch6/icp_2d.cc

@@ -0,0 +1,204 @@
+//
+// Created by xiang on 2022/3/15.
+//
+
+#include "ch6/icp_2d.h"
+#include "common/math_utils.h"
+
+#include <glog/logging.h>
+#include <pcl/kdtree/kdtree_flann.h>
+#include <pcl/search/impl/kdtree.hpp>
+
+namespace sad {
+
+bool Icp2d::AlignGaussNewton(SE2& init_pose) {
+    int iterations = 10;
+    double cost = 0, lastCost = 0;
+    SE2 current_pose = init_pose;
+    const float max_dis2 = 0.01;    // 最近邻时的最远距离（平方）
+    const int min_effect_pts = 20;  // 最小有效点数
+
+    for (int iter = 0; iter < iterations; ++iter) {
+        Mat3d H = Mat3d::Zero();
+        Vec3d b = Vec3d::Zero();
+        cost = 0;
+
+        int effective_num = 0;  // 有效点数
+
+        // 遍历source
+        for (size_t i = 0; i < source_scan_->ranges.size(); ++i) {
+            float r = source_scan_->ranges[i];
+            if (r < source_scan_->range_min || r > source_scan_->range_max) {
+                continue;
+            }
+
+            float angle = source_scan_->angle_min + i * source_scan_->angle_increment;
+            float theta = current_pose.so2().log();
+            Vec2d pw = current_pose * Vec2d(r * std::cos(angle), r * std::sin(angle));
+            Point2d pt;
+            pt.x = pw.x();
+            pt.y = pw.y();
+
+            // 最近邻
+            std::vector<int> nn_idx;
+            std::vector<float> dis;
+            kdtree_.nearestKSearch(pt, 1, nn_idx, dis);
+
+            if (nn_idx.size() > 0 && dis[0] < max_dis2) {
+                effective_num++;
+                Mat32d J;
+                J << 1, 0, 0, 1, -r * std::sin(angle + theta), r * std::cos(angle + theta);
+                H += J * J.transpose();
+
+                Vec2d e(pt.x - target_cloud_->points[nn_idx[0]].x, pt.y - target_cloud_->points[nn_idx[0]].y);
+                b += -J * e;
+
+                cost += e.dot(e);
+            }
+        }
+
+        if (effective_num < min_effect_pts) {
+            return false;
+        }
+
+        // solve for dx
+        Vec3d dx = H.ldlt().solve(b);
+        if (isnan(dx[0])) {
+            break;
+        }
+
+        cost /= effective_num;
+        if (iter > 0 && cost >= lastCost) {
+            break;
+        }
+
+        LOG(INFO) << "iter " << iter << " cost = " << cost << ", effect num: " << effective_num;
+
+        current_pose.translation() += dx.head<2>();
+        current_pose.so2() = current_pose.so2() * SO2::exp(dx[2]);
+        lastCost = cost;
+    }
+
+    init_pose = current_pose;
+    LOG(INFO) << "estimated pose: " << current_pose.translation().transpose()
+              << ", theta: " << current_pose.so2().log();
+
+    return true;
+}
+
+bool Icp2d::AlignGaussNewtonPoint2Plane(SE2& init_pose) {
+    int iterations = 10;
+    double cost = 0, lastCost = 0;
+    SE2 current_pose = init_pose;
+    const float max_dis = 0.3;      // 最近邻时的最远距离
+    const int min_effect_pts = 20;  // 最小有效点数
+
+    for (int iter = 0; iter < iterations; ++iter) {
+        Mat3d H = Mat3d::Zero();
+        Vec3d b = Vec3d::Zero();
+        cost = 0;
+
+        int effective_num = 0;  // 有效点数
+
+        // 遍历source
+        for (size_t i = 0; i < source_scan_->ranges.size(); ++i) {
+            float r = source_scan_->ranges[i];
+            if (r < source_scan_->range_min || r > source_scan_->range_max) {
+                continue;
+            }
+
+            float angle = source_scan_->angle_min + i * source_scan_->angle_increment;
+            float theta = current_pose.so2().log();
+            Vec2d pw = current_pose * Vec2d(r * std::cos(angle), r * std::sin(angle));
+            Point2d pt;
+            pt.x = pw.x();
+            pt.y = pw.y();
+
+            // 查找5个最近邻
+            std::vector<int> nn_idx;
+            std::vector<float> dis;
+            kdtree_.nearestKSearch(pt, 5, nn_idx, dis);
+
+            std::vector<Vec2d> effective_pts;  // 有效点
+            for (int j = 0; j < nn_idx.size(); ++j) {
+                if (dis[j] < max_dis) {
+                    effective_pts.emplace_back(
+                        Vec2d(target_cloud_->points[nn_idx[j]].x, target_cloud_->points[nn_idx[j]].y));
+                }
+            }
+
+            if (effective_pts.size() < 3) {
+                continue;
+            }
+
+            // 拟合直线，组装J、H和误差
+            Vec3d line_coeffs;
+            if (math::FitLine2D(effective_pts, line_coeffs)) {
+                effective_num++;
+                Vec3d J;
+                J << line_coeffs[0], line_coeffs[1],
+                    -line_coeffs[0] * r * std::sin(angle + theta) + line_coeffs[1] * r * std::cos(angle + theta);
+                H += J * J.transpose();
+
+                double e = line_coeffs[0] * pw[0] + line_coeffs[1] * pw[1] + line_coeffs[2];
+                b += -J * e;
+
+                cost += e * e;
+            }
+        }
+
+        if (effective_num < min_effect_pts) {
+            return false;
+        }
+
+        // solve for dx
+        Vec3d dx = H.ldlt().solve(b);
+        if (isnan(dx[0])) {
+            break;
+        }
+
+        cost /= effective_num;
+        if (iter > 0 && cost >= lastCost) {
+            break;
+        }
+
+        LOG(INFO) << "iter " << iter << " cost = " << cost << ", effect num: " << effective_num;
+
+        current_pose.translation() += dx.head<2>();
+        current_pose.so2() = current_pose.so2() * SO2::exp(dx[2]);
+        lastCost = cost;
+    }
+
+    init_pose = current_pose;
+    LOG(INFO) << "estimated pose: " << current_pose.translation().transpose()
+              << ", theta: " << current_pose.so2().log();
+
+    return true;
+}
+
+void Icp2d::BuildTargetKdTree() {
+    if (target_scan_ == nullptr) {
+        LOG(ERROR) << "target is not set";
+        return;
+    }
+
+    target_cloud_.reset(new Cloud2d);
+    for (size_t i = 0; i < target_scan_->ranges.size(); ++i) {
+        if (target_scan_->ranges[i] < target_scan_->range_min || target_scan_->ranges[i] > target_scan_->range_max) {
+            continue;
+        }
+
+        double real_angle = target_scan_->angle_min + i * target_scan_->angle_increment;
+
+        Point2d p;
+        p.x = target_scan_->ranges[i] * std::cos(real_angle);
+        p.y = target_scan_->ranges[i] * std::sin(real_angle);
+        target_cloud_->points.push_back(p);
+    }
+
+    target_cloud_->width = target_cloud_->points.size();
+    target_cloud_->is_dense = false;
+    kdtree_.setInputCloud(target_cloud_);
+}
+
+}  // namespace sad

workspace/slam_in_autonomous_driving/src/ch6/icp_2d.h

@@ -0,0 +1,53 @@
+//
+// Created by xiang on 2022/3/15.
+//
+
+#ifndef SLAM_IN_AUTO_DRIVING_ICP_2D_H
+#define SLAM_IN_AUTO_DRIVING_ICP_2D_H
+
+#include "common/eigen_types.h"
+#include "common/lidar_utils.h"
+
+#include <pcl/search/kdtree.h>
+
+namespace sad {
+
+/**
+ * 第六章谈到的各种类型的ICP代码实现
+ * 用法：先SetTarget, 此时构建target点云的KD树；再SetSource，然后调用Align*方法
+ */
+class Icp2d {
+   public:
+    using Point2d = pcl::PointXY;
+    using Cloud2d = pcl::PointCloud<Point2d>;
+    Icp2d() {}
+
+    /// 设置目标的Scan
+    void SetTarget(Scan2d::Ptr target) {
+        target_scan_ = target;
+        BuildTargetKdTree();
+    }
+
+    /// 设置被配准的Scan
+    void SetSource(Scan2d::Ptr source) { source_scan_ = source; }
+
+    /// 使用高斯牛顿法进行配准
+    bool AlignGaussNewton(SE2& init_pose);
+
+    /// 使用高斯牛顿法进行配准, Point-to-Plane
+    bool AlignGaussNewtonPoint2Plane(SE2& init_pose);
+
+   private:
+    // 建立目标点云的Kdtree
+    void BuildTargetKdTree();
+
+    pcl::search::KdTree<Point2d> kdtree_;
+    Cloud2d::Ptr target_cloud_;  // PCL 形式的target cloud
+
+    Scan2d::Ptr target_scan_ = nullptr;
+    Scan2d::Ptr source_scan_ = nullptr;
+};
+
+}  // namespace sad
+
+#endif  // SLAM_IN_AUTO_DRIVING_ICP_2D_H

workspace/slam_in_autonomous_driving/src/ch6/lidar_2d_utils.cc

@@ -0,0 +1,44 @@
+//
+// Created by xiang on 2022/3/15.
+//
+
+#include "ch6/lidar_2d_utils.h"
+#include <opencv2/imgproc.hpp>
+
+namespace sad {
+
+void Visualize2DScan(Scan2d::Ptr scan, const SE2& pose, cv::Mat& image, const Vec3b& color, int image_size,
+                     float resolution, const SE2& pose_submap) {
+    if (image.data == nullptr) {
+        image = cv::Mat(image_size, image_size, CV_8UC3, cv::Vec3b(255, 255, 255));
+    }
+
+    for (size_t i = 0; i < scan->ranges.size(); ++i) {
+        if (scan->ranges[i] < scan->range_min || scan->ranges[i] > scan->range_max) {
+            continue;
+        }
+
+        double real_angle = scan->angle_min + i * scan->angle_increment;
+        double x = scan->ranges[i] * std::cos(real_angle);
+        double y = scan->ranges[i] * std::sin(real_angle);
+
+        if (real_angle < scan->angle_min + 30 * M_PI / 180.0 || real_angle > scan->angle_max - 30 * M_PI / 180.0) {
+            continue;
+        }
+
+        Vec2d psubmap = pose_submap.inverse() * (pose * Vec2d(x, y));
+
+        int image_x = int(psubmap[0] * resolution + image_size / 2);
+        int image_y = int(psubmap[1] * resolution + image_size / 2);
+        if (image_x >= 0 && image_x < image.cols && image_y >= 0 && image_y < image.rows) {
+            image.at<cv::Vec3b>(image_y, image_x) = cv::Vec3b(color[0], color[1], color[2]);
+        }
+    }
+
+    // 同时画出pose自身所在位置
+    Vec2d pose_in_image =
+        pose_submap.inverse() * (pose.translation()) * double(resolution) + Vec2d(image_size / 2, image_size / 2);
+    cv::circle(image, cv::Point2f(pose_in_image[0], pose_in_image[1]), 5, cv::Scalar(color[0], color[1], color[2]), 2);
+}
+
+}  // namespace sad

workspace/slam_in_autonomous_driving/src/ch6/lidar_2d_utils.h

@@ -0,0 +1,30 @@
+//
+// Created by xiang on 2022/3/15.
+//
+
+#ifndef SLAM_IN_AUTO_DRIVING_LIDAR_2D_UTILS_H
+#define SLAM_IN_AUTO_DRIVING_LIDAR_2D_UTILS_H
+
+#include "common/eigen_types.h"
+#include "common/lidar_utils.h"
+
+#include <opencv2/core/core.hpp>
+
+/// 为2D lidar的一些辅助函数
+namespace sad {
+
+/**
+ * 在image上绘制一个2D scan
+ * @param scan
+ * @param pose
+ * @param image
+ * @param image_size 图片大小
+ * @param resolution 分辨率，一米多少个像素
+ * @param pose_submap 如果是子地图，提供子地图的pose
+ */
+void Visualize2DScan(Scan2d::Ptr scan, const SE2& pose, cv::Mat& image, const Vec3b& color, int image_size = 800,
+                     float resolution = 20.0, const SE2& pose_submap = SE2());
+
+}  // namespace sad
+
+#endif  // SLAM_IN_AUTO_DRIVING_LIDAR_2D_UTILS_H

workspace/slam_in_autonomous_driving/src/ch6/likelihood_field.cc

@@ -0,0 +1,230 @@
+//
+// Created by xiang on 2022/3/18.
+//
+
+#include "ch6/g2o_types.h"
+#include "ch6/likelihood_filed.h"
+
+#include <glog/logging.h>
+
+#include <g2o/core/base_unary_edge.h>
+#include <g2o/core/block_solver.h>
+#include <g2o/core/optimization_algorithm_levenberg.h>
+#include <g2o/core/robust_kernel.h>
+#include <g2o/core/robust_kernel_impl.h>
+#include <g2o/core/sparse_optimizer.h>
+#include <g2o/solvers/cholmod/linear_solver_cholmod.h>
+#include <g2o/solvers/dense/linear_solver_dense.h>
+
+namespace sad {
+
+void LikelihoodField::SetTargetScan(Scan2d::Ptr scan) {
+    target_ = scan;
+
+    // 在target点上生成场函数
+    field_ = cv::Mat(1000, 1000, CV_32F, 30.0);
+
+    for (size_t i = 0; i < scan->ranges.size(); ++i) {
+        if (scan->ranges[i] < scan->range_min || scan->ranges[i] > scan->range_max) {
+            continue;
+        }
+
+        double real_angle = scan->angle_min + i * scan->angle_increment;
+        double x = scan->ranges[i] * std::cos(real_angle) * resolution_ + 500;
+        double y = scan->ranges[i] * std::sin(real_angle) * resolution_ + 500;
+
+        // 在(x,y)附近填入场函数
+        for (auto& model_pt : model_) {
+            int xx = int(x + model_pt.dx_);
+            int yy = int(y + model_pt.dy_);
+            if (xx >= 0 && xx < field_.cols && yy >= 0 && yy < field_.rows &&
+                field_.at<float>(yy, xx) > model_pt.residual_) {
+                field_.at<float>(yy, xx) = model_pt.residual_;
+            }
+        }
+    }
+}
+
+void LikelihoodField::BuildModel() {
+    const int range = 20;  // 生成多少个像素的模板
+    for (int x = -range; x <= range; ++x) {
+        for (int y = -range; y <= range; ++y) {
+            model_.emplace_back(x, y, std::sqrt((x * x) + (y * y)));
+        }
+    }
+}
+
+void LikelihoodField::SetSourceScan(Scan2d::Ptr scan) { source_ = scan; }
+
+bool LikelihoodField::AlignGaussNewton(SE2& init_pose) {
+    int iterations = 10;
+    double cost = 0, lastCost = 0;
+    SE2 current_pose = init_pose;
+    const int min_effect_pts = 20;  // 最小有效点数
+    const int image_boarder = 20;   // 预留图像边界
+
+    has_outside_pts_ = false;
+    for (int iter = 0; iter < iterations; ++iter) {
+        Mat3d H = Mat3d::Zero();
+        Vec3d b = Vec3d::Zero();
+        cost = 0;
+
+        int effective_num = 0;  // 有效点数
+
+        // 遍历source
+        for (size_t i = 0; i < source_->ranges.size(); ++i) {
+            float r = source_->ranges[i];
+            if (r < source_->range_min || r > source_->range_max) {
+                continue;
+            }
+
+            float angle = source_->angle_min + i * source_->angle_increment;
+            if (angle < source_->angle_min + 30 * M_PI / 180.0 || angle > source_->angle_max - 30 * M_PI / 180.0) {
+                continue;
+            }
+
+            float theta = current_pose.so2().log();
+            Vec2d pw = current_pose * Vec2d(r * std::cos(angle), r * std::sin(angle));
+
+            // 在field中的图像坐标
+            Vec2i pf = (pw * resolution_ + Vec2d(500, 500)).cast<int>();
+
+            if (pf[0] >= image_boarder && pf[0] < field_.cols - image_boarder && pf[1] >= image_boarder &&
+                pf[1] < field_.rows - image_boarder) {
+                effective_num++;
+
+                // 图像梯度
+                float dx = 0.5 * (field_.at<float>(pf[1], pf[0] + 1) - field_.at<float>(pf[1], pf[0] - 1));
+                float dy = 0.5 * (field_.at<float>(pf[1] + 1, pf[0]) - field_.at<float>(pf[1] - 1, pf[0]));
+
+                Vec3d J;
+                J << resolution_ * dx, resolution_ * dy,
+                    -resolution_ * dx * r * std::sin(angle + theta) + resolution_ * dy * r * std::cos(angle + theta);
+                H += J * J.transpose();
+
+                float e = field_.at<float>(pf[1], pf[0]);
+                b += -J * e;
+
+                cost += e * e;
+            } else {
+                has_outside_pts_ = true;
+            }
+        }
+
+        if (effective_num < min_effect_pts) {
+            return false;
+        }
+
+        // solve for dx
+        Vec3d dx = H.ldlt().solve(b);
+        if (isnan(dx[0])) {
+            break;
+        }
+
+        cost /= effective_num;
+        if (iter > 0 && cost >= lastCost) {
+            break;
+        }
+
+        LOG(INFO) << "iter " << iter << " cost = " << cost << ", effect num: " << effective_num;
+
+        current_pose.translation() += dx.head<2>();
+        current_pose.so2() = current_pose.so2() * SO2::exp(dx[2]);
+        lastCost = cost;
+    }
+
+    init_pose = current_pose;
+    return true;
+}
+
+cv::Mat LikelihoodField::GetFieldImage() {
+    cv::Mat image(field_.rows, field_.cols, CV_8UC3);
+    for (int x = 0; x < field_.cols; ++x) {
+        for (int y = 0; y < field_.rows; ++y) {
+            float r = field_.at<float>(y, x) * 255.0 / 30.0;
+            image.at<cv::Vec3b>(y, x) = cv::Vec3b(uchar(r), uchar(r), uchar(r));
+        }
+    }
+
+    return image;
+}
+
+bool LikelihoodField::AlignG2O(SE2& init_pose) {
+    using BlockSolverType = g2o::BlockSolver<g2o::BlockSolverTraits<3, 1>>;
+    using LinearSolverType = g2o::LinearSolverCholmod<BlockSolverType::PoseMatrixType>;
+    auto* solver = new g2o::OptimizationAlgorithmLevenberg(
+        g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
+    g2o::SparseOptimizer optimizer;
+    optimizer.setAlgorithm(solver);
+
+    auto* v = new VertexSE2();
+    v->setId(0);
+    v->setEstimate(init_pose);
+    optimizer.addVertex(v);
+
+    const double range_th = 15.0;  // 不考虑太远的scan，不准
+    const double rk_delta = 0.8;
+
+    has_outside_pts_ = false;
+    // 遍历source
+    for (size_t i = 0; i < source_->ranges.size(); ++i) {
+        float r = source_->ranges[i];
+        if (r < source_->range_min || r > source_->range_max) {
+            continue;
+        }
+
+        if (r > range_th) {
+            continue;
+        }
+
+        float angle = source_->angle_min + i * source_->angle_increment;
+        if (angle < source_->angle_min + 30 * M_PI / 180.0 || angle > source_->angle_max - 30 * M_PI / 180.0) {
+            continue;
+        }
+
+        auto e = new EdgeSE2LikelihoodFiled(field_, r, angle, resolution_);
+        e->setVertex(0, v);
+
+        if (e->IsOutSide()) {
+            has_outside_pts_ = true;
+            delete e;
+            continue;
+        }
+
+        e->setInformation(Eigen::Matrix<double, 1, 1>::Identity());
+        auto rk = new g2o::RobustKernelHuber;
+        rk->setDelta(rk_delta);
+        e->setRobustKernel(rk);
+        optimizer.addEdge(e);
+    }
+
+    optimizer.setVerbose(false);
+    optimizer.initializeOptimization();
+    optimizer.optimize(10);
+
+    init_pose = v->estimate();
+    return true;
+}
+
+void LikelihoodField::SetFieldImageFromOccuMap(const cv::Mat& occu_map) {
+    const int boarder = 25;
+    field_ = cv::Mat(1000, 1000, CV_32F, 30.0);
+
+    for (int x = boarder; x < occu_map.cols - boarder; ++x) {
+        for (int y = boarder; y < occu_map.rows - boarder; ++y) {
+            if (occu_map.at<uchar>(y, x) < 127) {
+                // 在该点生成一个model
+                for (auto& model_pt : model_) {
+                    int xx = int(x + model_pt.dx_);
+                    int yy = int(y + model_pt.dy_);
+                    if (xx >= 0 && xx < field_.cols && yy >= 0 && yy < field_.rows &&
+                        field_.at<float>(yy, xx) > model_pt.residual_) {
+                        field_.at<float>(yy, xx) = model_pt.residual_;
+                    }
+                }
+            }
+        }
+    }
+}
+
+}  // namespace sad

workspace/slam_in_autonomous_driving/src/ch6/likelihood_filed.h

@@ -0,0 +1,70 @@
+//
+// Created by xiang on 2022/3/18.
+//
+
+#ifndef SLAM_IN_AUTO_DRIVING_LIKELIHOOD_FILED_H
+#define SLAM_IN_AUTO_DRIVING_LIKELIHOOD_FILED_H
+
+#include <opencv2/core.hpp>
+
+#include "common/eigen_types.h"
+#include "common/lidar_utils.h"
+
+namespace sad {
+
+class LikelihoodField {
+   public:
+    /// 2D 场的模板，在设置target scan或map的时候生成
+    struct ModelPoint {
+        ModelPoint(int dx, int dy, float res) : dx_(dx), dy_(dy), residual_(res) {}
+        int dx_ = 0;
+        int dy_ = 0;
+        float residual_ = 0;
+    };
+
+    LikelihoodField() { BuildModel(); }
+
+    /// 增加一个2D的目标scan
+    void SetTargetScan(Scan2d::Ptr scan);
+
+    /// 设置被配准的那个scan
+    void SetSourceScan(Scan2d::Ptr scan);
+
+    /// 从占据栅格地图生成一个似然场地图
+    void SetFieldImageFromOccuMap(const cv::Mat& occu_map);
+
+    /// 使用高斯牛顿法配准
+    bool AlignGaussNewton(SE2& init_pose);
+
+    /**
+     * 使用g2o配准
+     * @param init_pose 初始位姿 NOTE 使用submap时，给定相对于该submap的位姿，估计结果也是针对于这个submap的位姿
+     * @return
+     */
+    bool AlignG2O(SE2& init_pose);
+
+    /// 获取场函数，转换为RGB图像
+    cv::Mat GetFieldImage();
+
+    bool HasOutsidePoints() const { return has_outside_pts_; }
+
+    void SetPose(const SE2& pose) { pose_ = pose; }
+
+   private:
+    void BuildModel();
+
+    SE2 pose_;  // T_W_S
+    Scan2d::Ptr target_ = nullptr;
+    Scan2d::Ptr source_ = nullptr;
+
+    std::vector<ModelPoint> model_;  // 2D 模板
+    cv::Mat field_;                  // 场函数
+    bool has_outside_pts_ = false;   // 是否含有出了这个场的点
+
+    // 参数配置
+    inline static const float resolution_ = 20;  // 每米多少个像素
+};
+
+}  // namespace sad
+
+#endif  // SLAM_IN_AUTO_DRIVING_LIKELIHOOD_FILED_H

workspace/slam_in_autonomous_driving/src/ch6/loop_closing.cc

@@ -0,0 +1,223 @@
+//
+// Created by xiang on 2022/4/7.
+//
+
+#include "loop_closing.h"
+#include "ch6/g2o_types.h"
+#include "lidar_2d_utils.h"
+
+#include <glog/logging.h>
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui.hpp>
+
+#include <g2o/core/base_unary_edge.h>
+#include <g2o/core/block_solver.h>
+#include <g2o/core/optimization_algorithm_levenberg.h>
+#include <g2o/core/robust_kernel.h>
+#include <g2o/core/robust_kernel_impl.h>
+#include <g2o/core/sparse_optimizer.h>
+#include <g2o/solvers/cholmod/linear_solver_cholmod.h>
+
+#include <opencv2/opencv.hpp>
+
+namespace sad {
+
+void LoopClosing::AddFinishedSubmap(std::shared_ptr<Submap> submap) {
+    auto mr_field = std::make_shared<MRLikelihoodField>();
+    mr_field->SetPose(submap->GetPose());
+    mr_field->SetFieldImageFromOccuMap(submap->GetOccuMap().GetOccupancyGrid());
+    submap_to_field_.emplace(submap, mr_field);
+}
+
+void LoopClosing::AddNewSubmap(std::shared_ptr<Submap> submap) {
+    submaps_.emplace(submap->GetId(), submap);
+    last_submap_id_ = submap->GetId();
+}
+
+void LoopClosing::AddNewFrame(std::shared_ptr<Frame> frame) {
+    current_frame_ = frame;
+    if (!DetectLoopCandidates()) {
+        return;
+    }
+
+    MatchInHistorySubmaps();
+
+    if (has_new_loops_) {
+        Optimize();
+    }
+}
+
+bool LoopClosing::DetectLoopCandidates() {
+    // 要求当前帧与历史submap有一定间隔
+    has_new_loops_ = false;
+    if (last_submap_id_ < submap_gap_) {
+        return false;
+    }
+
+    current_candidates_.clear();
+
+    for (auto& sp : submaps_) {
+        if ((last_submap_id_ - sp.first) <= submap_gap_) {
+            // 不检查最近的几个submap
+            continue;
+        }
+
+        // 如果这个submap和历史submap已经存在有效的关联，也忽略之
+        auto hist_iter = loop_constraints_.find(std::pair<size_t, size_t>(sp.first, last_submap_id_));
+        if (hist_iter != loop_constraints_.end() && hist_iter->second.valid_) {
+            continue;
+        }
+
+        Vec2d center = sp.second->GetPose().translation();
+        Vec2d frame_pos = current_frame_->pose_.translation();
+        double dis = (center - frame_pos).norm();
+        if (dis < candidate_distance_th_) {
+            /// 如果这个frame离submap中心差距小于阈值，则检查
+            LOG(INFO) << "taking " << current_frame_->keyframe_id_ << " with " << sp.first
+                      << ", last submap id: " << last_submap_id_;
+            current_candidates_.emplace_back(sp.first);
+        }
+    }
+
+    return !current_candidates_.empty();
+}
+
+void LoopClosing::MatchInHistorySubmaps() {
+    // 我们先把要检查的scan, pose和submap存到离线文件, 把mr match调完了再实际上线
+    // current_frame_->Dump("./data/ch6/frame_" + std::to_string(current_frame_->id_) + ".txt");
+
+    for (const size_t& can : current_candidates_) {
+        auto mr = submap_to_field_.at(submaps_[can]);
+        mr->SetSourceScan(current_frame_->scan_);
+
+        auto submap = submaps_[can];
+        SE2 pose_in_target_submap = submap->GetPose().inverse() * current_frame_->pose_;  // T_S1_C
+
+        if (mr->AlignG2O(pose_in_target_submap)) {
+            // set constraints from current submap to target submap
+            // T_S1_S2 = T_S1_C * T_C_W * T_W_S2
+            SE2 T_this_cur =
+                pose_in_target_submap * current_frame_->pose_.inverse() * submaps_[last_submap_id_]->GetPose();
+            loop_constraints_.emplace(std::pair<size_t, size_t>(can, last_submap_id_),
+                                      LoopConstraints(can, last_submap_id_, T_this_cur));
+            LOG(INFO) << "adding loop from submap " << can << " to " << last_submap_id_;
+
+            /// 可视化显示
+            auto occu_image = submap->GetOccuMap().GetOccupancyGridBlackWhite();
+            Visualize2DScan(current_frame_->scan_, pose_in_target_submap, occu_image, Vec3b(0, 0, 255), 1000, 20.0,
+                            SE2());
+            cv::putText(occu_image, "loop submap " + std::to_string(submap->GetId()), cv::Point2f(20, 20),
+                        cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(0, 255, 0));
+            cv::putText(occu_image, "keyframes " + std::to_string(submap->NumFrames()), cv::Point2f(20, 50),
+                        cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(0, 255, 0));
+            cv::imshow("loop closure", occu_image);
+
+            has_new_loops_ = true;
+        }
+
+        debug_fout_ << current_frame_->id_ << " " << can << " " << submaps_[can]->GetPose().translation().x() << " "
+                    << submaps_[can]->GetPose().translation().y() << " " << submaps_[can]->GetPose().so2().log()
+                    << std::endl;
+    }
+
+    current_candidates_.clear();
+}
+
+void LoopClosing::Optimize() {
+    // pose graph optimization
+    using BlockSolverType = g2o::BlockSolver<g2o::BlockSolverTraits<3, 1>>;
+    using LinearSolverType = g2o::LinearSolverCholmod<BlockSolverType::PoseMatrixType>;
+    auto* solver = new g2o::OptimizationAlgorithmLevenberg(
+        g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
+    g2o::SparseOptimizer optimizer;
+    optimizer.setAlgorithm(solver);
+
+    for (auto& sp : submaps_) {
+        auto* v = new VertexSE2();
+        v->setId(sp.first);
+        v->setEstimate(sp.second->GetPose());
+        optimizer.addVertex(v);
+    }
+
+    /// 连续约束
+    for (int i = 0; i < last_submap_id_; ++i) {
+        auto first_submap = submaps_[i];
+        auto next_submap = submaps_[i + 1];
+
+        EdgeSE2* e = new EdgeSE2();
+        e->setVertex(0, optimizer.vertex(i));
+        e->setVertex(1, optimizer.vertex(i + 1));
+        e->setMeasurement(first_submap->GetPose().inverse() * next_submap->GetPose());
+        e->setInformation(Mat3d::Identity() * 1e4);
+
+        optimizer.addEdge(e);
+    }
+
+    /// 回环约束
+    std::map<std::pair<size_t, size_t>, EdgeSE2*> loop_edges;
+    for (auto& c : loop_constraints_) {
+        if (!c.second.valid_) {
+            continue;
+        }
+
+        auto first_submap = submaps_[c.first.first];
+        auto second_submap = submaps_[c.first.second];
+
+        EdgeSE2* e = new EdgeSE2();
+        e->setVertex(0, optimizer.vertex(first_submap->GetId()));
+        e->setVertex(1, optimizer.vertex(second_submap->GetId()));
+        e->setMeasurement(c.second.T12_);
+        e->setInformation(Mat3d::Identity());
+
+        auto rk = new g2o::RobustKernelCauchy;
+        rk->setDelta(loop_rk_delta_);
+        e->setRobustKernel(rk);
+
+        optimizer.addEdge(e);
+        loop_edges.emplace(c.first, e);
+    }
+
+    optimizer.setVerbose(true);
+    optimizer.initializeOptimization();
+    optimizer.optimize(10);
+
+    // validate the loop constraints
+    int inliers = 0;
+    for (auto& ep : loop_edges) {
+        if (ep.second->chi2() < loop_rk_delta_) {
+            LOG(INFO) << "loop from " << ep.first.first << " to " << ep.first.second
+                      << " is correct, chi2: " << ep.second->chi2();
+            ep.second->setRobustKernel(nullptr);
+            loop_constraints_.at(ep.first).valid_ = true;
+            inliers++;
+        } else {
+            ep.second->setLevel(1);
+            LOG(INFO) << "loop from " << ep.first.first << " to " << ep.first.second
+                      << " is invalid, chi2: " << ep.second->chi2();
+            loop_constraints_.at(ep.first).valid_ = false;
+        }
+    }
+
+    optimizer.optimize(5);
+
+    for (auto& sp : submaps_) {
+        VertexSE2* v = (VertexSE2*)optimizer.vertex(sp.first);
+        sp.second->SetPose(v->estimate());
+
+        // 更新所属的frame world pose
+        sp.second->UpdateFramePoseWorld();
+    }
+
+    LOG(INFO) << "loop inliers: " << inliers << "/" << loop_constraints_.size();
+
+    // 移除错误的匹配
+    for (auto iter = loop_constraints_.begin(); iter != loop_constraints_.end();) {
+        if (!iter->second.valid_) {
+            iter = loop_constraints_.erase(iter);
+        } else {
+            iter++;
+        }
+    }
+}
+
+}  // namespace sad

workspace/slam_in_autonomous_driving/src/ch6/loop_closing.h

@@ -0,0 +1,86 @@
+//
+// Created by xiang on 2022/4/7.
+//
+
+#ifndef SLAM_IN_AUTO_DRIVING_LOOP_CLOSING_H
+#define SLAM_IN_AUTO_DRIVING_LOOP_CLOSING_H
+
+#include "common/eigen_types.h"
+#include "multi_resolution_likelihood_field.h"
+#include "submap.h"
+
+#include <fstream>
+#include <map>
+#include <memory>
+
+namespace sad {
+
+/**
+ * 单线程的回环检测模块
+ * 基于里程计估计的位姿进行回环检测，如果检测成功，则更新各submap的pose
+ *
+ * 首先调用 DetectLoopCandidates 检测历史地图中可能的回环
+ * 然后用MultiResolutionMatching进行配准
+ * 如果配准成功，建立一个pose graph进行优化
+ * 图优化也可能认为回环是错误的，进而删掉这个回环
+ */
+class LoopClosing {
+   public:
+    /// 一个回环约束
+    struct LoopConstraints {
+        LoopConstraints(size_t id1, size_t id2, const SE2& T12) : id_submap1_(id1), id_submap2_(id2), T12_(T12) {}
+        size_t id_submap1_ = 0;
+        size_t id_submap2_ = 0;
+        SE2 T12_;  //  相对pose
+        bool valid_ = true;
+    };
+
+    LoopClosing() { debug_fout_.open("./data/ch6/loops.txt"); }
+
+    /// 添加最近的submap，这个submap可能正在构建中
+    void AddNewSubmap(std::shared_ptr<Submap> submap);
+
+    /// 添加一个已经建完的submap，需要在AddNewSubmap函数之后调用
+    void AddFinishedSubmap(std::shared_ptr<Submap> submap);
+
+    /// 为新的frame进行回环检测，更新它的pose和submap的pose
+    void AddNewFrame(std::shared_ptr<Frame> frame);
+
+    /// 获取submap之间的回环检测
+    std::map<std::pair<size_t, size_t>, LoopConstraints> GetLoops() const { return loop_constraints_; }
+
+    bool HasNewLoops() const { return has_new_loops_; }
+
+   private:
+    /// 检测当前帧与历史地图可能存在的回环
+    bool DetectLoopCandidates();
+
+    /// 将当前帧与历史submap进行匹配
+    void MatchInHistorySubmaps();
+
+    /// 进行submap间的pose graph优化
+    void Optimize();
+
+    std::shared_ptr<Frame> current_frame_ = nullptr;
+    size_t last_submap_id_ = 0;  // 最新一个submap的id
+
+    std::map<size_t, std::shared_ptr<Submap>> submaps_;  // 所有的submaps
+
+    // submap到mr field之间的对应关系
+    std::map<std::shared_ptr<Submap>, std::shared_ptr<MRLikelihoodField>> submap_to_field_;
+
+    std::vector<size_t> current_candidates_;                                 // 可能的回环检测点
+    std::map<std::pair<size_t, size_t>, LoopConstraints> loop_constraints_;  // 回环约束, 以被约束的两个submap为索引
+    bool has_new_loops_ = false;
+
+    std::ofstream debug_fout_;  // 调试输出
+
+    /// 参数
+    inline static constexpr float candidate_distance_th_ = 15.0;  // candidate frame与submap中心之间的距离
+    inline static constexpr int submap_gap_ = 1;                  // 当前scan与最近submap编号上的差异
+    inline static constexpr double loop_rk_delta_ = 1.0;          // 回环检测的robust kernel 阈值
+};
+
+}  // namespace sad
+
+#endif  // SLAM_IN_AUTO_DRIVING_LOOP_CLOSING_H

workspace/slam_in_autonomous_driving/src/ch6/mapping_2d.cc

@@ -0,0 +1,290 @@
+//
+// Created by xiang on 2022/3/23.
+//
+
+#include "ch6/mapping_2d.h"
+#include "ch6/lidar_2d_utils.h"
+#include "ch6/loop_closing.h"
+#include "ch6/submap.h"
+
+#include <glog/logging.h>
+#include <execution>
+#include <opencv2/opencv.hpp>
+
+namespace sad {
+
+bool Mapping2D::Init(bool with_loop_closing) {
+    keyframe_id_ = 0;
+    current_submap_ = std::make_shared<Submap>(SE2());
+    all_submaps_.emplace_back(current_submap_);
+
+    if (with_loop_closing) {
+        loop_closing_ = std::make_shared<LoopClosing>();
+        loop_closing_->AddNewSubmap(current_submap_);
+    }
+
+    return true;
+}
+
+bool Mapping2D::ProcessScan(MultiScan2d::Ptr scan) { return ProcessScan(MultiToScan2d(scan)); }
+
+bool Mapping2D::ProcessScan(Scan2d::Ptr scan) {
+    current_frame_ = std::make_shared<Frame>(scan);
+    current_frame_->id_ = frame_id_++;
+
+    if (last_frame_) {
+        // set pose from last frame
+        // current_frame_->pose_ = last_frame_->pose_;
+        current_frame_->pose_ = last_frame_->pose_ * motion_guess_;
+        current_frame_->pose_submap_ = last_frame_->pose_submap_;
+    }
+
+    // 利用scan matching来匹配地图
+    if (!first_scan_) {
+        // 第一帧无法匹配，直接加入到occupancy map
+        current_submap_->MatchScan(current_frame_);
+    }
+
+    // current_submap_->AddScanInOccupancyMap(current_frame_);
+    first_scan_ = false;
+    bool is_kf = IsKeyFrame();
+
+    if (is_kf) {
+        AddKeyFrame();
+        current_submap_->AddScanInOccupancyMap(current_frame_);
+
+        // 处理回环检测
+        if (loop_closing_) {
+            loop_closing_->AddNewFrame(current_frame_);
+        }
+
+        if (current_submap_->HasOutsidePoints() || (current_submap_->NumFrames()) > 50) {
+            /// 走出了submap或者单个submap中的关键帧较多
+            ExpandSubmap();
+        }
+    }
+
+    /// 可视化输出
+    auto occu_image = current_submap_->GetOccuMap().GetOccupancyGridBlackWhite();
+    Visualize2DScan(current_frame_->scan_, current_frame_->pose_, occu_image, Vec3b(0, 0, 255), 1000, 20.0,
+                    current_submap_->GetPose());
+    cv::putText(occu_image, "submap " + std::to_string(current_submap_->GetId()), cv::Point2f(20, 20),
+                cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(0, 255, 0));
+    cv::putText(occu_image, "keyframes " + std::to_string(current_submap_->NumFrames()), cv::Point2f(20, 50),
+                cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(0, 255, 0));
+    cv::imshow("occupancy map", occu_image);
+
+    auto field_image = current_submap_->GetLikelihood().GetFieldImage();
+    Visualize2DScan(current_frame_->scan_, current_frame_->pose_, field_image, Vec3b(0, 0, 255), 1000, 20.0,
+                    current_submap_->GetPose());
+    cv::imshow("likelihood", field_image);
+
+    /// global map
+    if (is_kf) {
+        cv::imshow("global map", ShowGlobalMap());
+    }
+
+    cv::waitKey(10);
+
+    if (last_frame_) {
+        motion_guess_ = last_frame_->pose_.inverse() * current_frame_->pose_;
+    }
+
+    last_frame_ = current_frame_;
+
+    return true;
+}
+
+bool Mapping2D::IsKeyFrame() {
+    if (last_keyframe_ == nullptr) {
+        return true;
+    }
+
+    SE2 delta_pose = last_keyframe_->pose_.inverse() * current_frame_->pose_;
+    if (delta_pose.translation().norm() > keyframe_pos_th_ || fabs(delta_pose.so2().log()) > keyframe_ang_th_) {
+        return true;
+    }
+
+    return false;
+}
+
+void Mapping2D::AddKeyFrame() {
+    LOG(INFO) << "add keyframe " << keyframe_id_;
+    current_frame_->keyframe_id_ = keyframe_id_++;
+    current_submap_->AddKeyFrame(current_frame_);
+    last_keyframe_ = current_frame_;
+}
+
+void Mapping2D::ExpandSubmap() {
+    // 当前submap作为历史地图放入loop closing
+    if (loop_closing_) {
+        loop_closing_->AddFinishedSubmap(current_submap_);
+    }
+
+    // 将当前submap替换成新的
+    auto last_submap = current_submap_;
+
+    // debug
+    cv::imwrite("./data/ch6/submap_" + std::to_string(last_submap->GetId()) + ".png",
+                last_submap->GetOccuMap().GetOccupancyGridBlackWhite());
+
+    current_submap_ = std::make_shared<Submap>(current_frame_->pose_);
+    current_frame_->pose_submap_ = SE2();  // 这个归零
+
+    current_submap_->SetId(++submap_id_);
+    current_submap_->AddKeyFrame(current_frame_);
+    current_submap_->SetOccuFromOtherSubmap(last_submap);  // 把上一帧的数据也放进来，不让一个submap显得太空
+
+    current_submap_->AddScanInOccupancyMap(current_frame_);
+    all_submaps_.emplace_back(current_submap_);
+
+    if (loop_closing_) {
+        loop_closing_->AddNewSubmap(current_submap_);
+    }
+
+    LOG(INFO) << "create submap " << current_submap_->GetId()
+              << " with pose: " << current_submap_->GetPose().translation().transpose() << ", "
+              << current_submap_->GetPose().so2().log();
+}
+
+cv::Mat Mapping2D::ShowGlobalMap(int max_size) {
+    //// TODO 全局地图固定大小，使用动态分辨率
+    Vec2f top_left = Vec2f(999999, 999999);
+    Vec2f bottom_right = Vec2f(-999999, -999999);
+
+    const float submap_resolution = 20.0;  // 子地图分辨率（1米多少个像素）
+    const float submap_size = 50.0;        // 单个submap大小
+
+    /// 计算全局地图物理边界
+    for (auto m : all_submaps_) {
+        Vec2d c = m->GetPose().translation();
+        if (top_left[0] > c[0] - submap_size / 2) {
+            top_left[0] = c[0] - submap_size / 2;
+        }
+        if (top_left[1] > c[1] - submap_size / 2) {
+            top_left[1] = c[1] - submap_size / 2;
+        }
+
+        if (bottom_right[0] < c[0] + submap_size / 2) {
+            bottom_right[0] = c[0] + submap_size / 2;
+        }
+        if (bottom_right[1] < c[1] + submap_size / 2) {
+            bottom_right[1] = c[1] + submap_size / 2;
+        }
+    }
+
+    if (top_left[0] > bottom_right[0] || top_left[1] > bottom_right[1]) {
+        return cv::Mat();
+    }
+
+    /// 全局地图物理中心
+    Vec2f global_center = Vec2f((top_left[0] + bottom_right[0]) / 2.0, (top_left[1] + bottom_right[1]) / 2.0);
+    float phy_width = bottom_right[0] - top_left[0];   // 物理尺寸
+    float phy_height = bottom_right[1] - top_left[1];  // 物理尺寸
+    float global_map_resolution = 0;
+
+    if (phy_width > phy_height) {
+        global_map_resolution = max_size / phy_width;
+    } else {
+        global_map_resolution = max_size / phy_height;
+    }
+
+    Vec2f c = global_center;
+    int c_x = global_center[0] * global_map_resolution;
+    int c_y = global_center[1] * global_map_resolution;
+    global_center = Vec2f(c_x / global_map_resolution, c_y / global_map_resolution);  // 全局地图图像中心
+
+    int width = int((bottom_right[0] - top_left[0]) * global_map_resolution + 0.5);
+    int height = int((bottom_right[1] - top_left[1]) * global_map_resolution + 0.5);
+
+    Vec2f center_image = Vec2f(width / 2, height / 2);
+    cv::Mat output_image(height, width, CV_8UC3, cv::Scalar(127, 127, 127));
+
+    std::vector<Vec2i> render_data;
+    render_data.reserve(width * height);
+    for (int x = 0; x < width; ++x) {
+        for (int y = 0; y < height; ++y) {
+            render_data.emplace_back(Vec2i(x, y));
+        }
+    }
+
+    std::for_each(std::execution::par_unseq, render_data.begin(), render_data.end(), [&](const Vec2i& xy) {
+        int x = xy[0], y = xy[1];
+        Vec2f pw = (Vec2f(x, y) - center_image) / global_map_resolution + c;  // 世界坐标
+
+        for (auto& m : all_submaps_) {
+            Vec2f ps = m->GetPose().inverse().cast<float>() * pw;  // in submap
+            Vec2i pt = (ps * submap_resolution + Vec2f(500, 500)).cast<int>();
+
+            if (pt[0] < 0 || pt[0] >= 1000 || pt[1] < 0 || pt[1] >= 1000) {
+                continue;
+            }
+
+            uchar value = m->GetOccuMap().GetOccupancyGrid().at<uchar>(pt[1], pt[0]);
+            if (value > 127) {
+                if (m == current_submap_) {
+                    output_image.at<cv::Vec3b>(y, x) = cv::Vec3b(235, 250, 230);
+                } else {
+                    output_image.at<cv::Vec3b>(y, x) = cv::Vec3b(255, 255, 255);
+                }
+                break;
+            } else if (value < 127) {
+                if (m == current_submap_) {
+                    output_image.at<cv::Vec3b>(y, x) = cv::Vec3b(230, 20, 30);
+                } else {
+                    output_image.at<cv::Vec3b>(y, x) = cv::Vec3b(0, 0, 0);
+                }
+                break;
+            }
+        }
+    });
+
+    for (auto& m : all_submaps_) {
+        /// submap pose 在全局地图中的投影
+        SE2f submap_pose = m->GetPose().cast<float>();
+        Vec2f submap_center = submap_pose.translation();
+        Vec2f submap_xw = submap_pose * Vec2f(1.0, 0);
+        Vec2f submap_yw = submap_pose * Vec2f(0, 1.0);
+
+        Vec2f center_map = (submap_center - global_center) * global_map_resolution + center_image;
+        Vec2f x_map = (submap_xw - global_center) * global_map_resolution + center_image;
+        Vec2f y_map = (submap_yw - global_center) * global_map_resolution + center_image;
+
+        // x轴和y轴
+        cv::line(output_image, cv::Point2f(center_map.x(), center_map.y()), cv::Point2f(x_map.x(), x_map.y()),
+                 cv::Scalar(0, 0, 255), 2);
+        cv::line(output_image, cv::Point2f(center_map.x(), center_map.y()), cv::Point2f(y_map.x(), y_map.y()),
+                 cv::Scalar(0, 255, 0), 2);
+        cv::putText(output_image, std::to_string(m->GetId()), cv::Point2f(center_map.x() + 10, center_map.y() - 10),
+                    cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(255, 0, 0));
+
+        // 轨迹
+        for (const auto& frame : m->GetFrames()) {
+            Vec2f p_map =
+                (frame->pose_.translation().cast<float>() - global_center) * global_map_resolution + center_image;
+            cv::circle(output_image, cv::Point2f(p_map.x(), p_map.y()), 1, cv::Scalar(0, 0, 255), 1);
+        }
+    }
+
+    if (loop_closing_) {
+        /// 回环检测的pose graph
+        auto loops = loop_closing_->GetLoops();
+        for (auto lc : loops) {
+            auto first_id = lc.first.first;
+            auto second_id = lc.first.second;
+
+            Vec2f c1 = all_submaps_[first_id]->GetPose().translation().cast<float>();
+            Vec2f c2 = all_submaps_[second_id]->GetPose().translation().cast<float>();
+
+            Vec2f c1_map = (c1 - global_center) * global_map_resolution + center_image;
+            Vec2f c2_map = (c2 - global_center) * global_map_resolution + center_image;
+
+            cv::line(output_image, cv::Point2f(c1_map.x(), c1_map.y()), cv::Point2f(c2_map.x(), c2_map.y()),
+                     cv::Scalar(255, 0, 0), 2);
+        }
+    }
+
+    return output_image;
+}
+
+}  // namespace sad

workspace/slam_in_autonomous_driving/src/ch6/mapping_2d.h

@@ -0,0 +1,74 @@
+//
+// Created by xiang on 2022/3/23.
+//
+
+#ifndef SLAM_IN_AUTO_DRIVING_MAPPING_2D_H
+#define SLAM_IN_AUTO_DRIVING_MAPPING_2D_H
+
+#include "ch6/frame.h"
+#include "common/eigen_types.h"
+#include "common/lidar_utils.h"
+
+#include <memory>
+#include <opencv2/core.hpp>
+
+namespace sad {
+
+class Submap;
+class LoopClosing;
+
+/**
+ * 2D 激光建图的主要类
+ */
+class Mapping2D {
+   public:
+    bool Init(bool with_loop_closing = true);
+
+    /// 单回波的scan
+    bool ProcessScan(Scan2d::Ptr scan);
+
+    /// 多回波的scan
+    /// 暂时没用到
+    bool ProcessScan(MultiScan2d::Ptr scan);
+
+    /**
+     * 显示全局地图
+     * @param max_size 全局地图最大长宽
+     * @return 全局地图图像
+     */
+    cv::Mat ShowGlobalMap(int max_size = 500);
+
+   private:
+    /// 判定当前帧是否为关键帧
+    bool IsKeyFrame();
+
+    /// 增加一个关键帧
+    void AddKeyFrame();
+
+    /// 扩展新的submap
+    void ExpandSubmap();
+
+    /// 数据成员
+    size_t frame_id_ = 0;
+    size_t keyframe_id_ = 0;
+    size_t submap_id_ = 0;
+
+    bool first_scan_ = true;
+    std::shared_ptr<Frame> current_frame_ = nullptr;
+    std::shared_ptr<Frame> last_frame_ = nullptr;
+    SE2 motion_guess_;
+    std::shared_ptr<Frame> last_keyframe_ = nullptr;
+    std::shared_ptr<Submap> current_submap_ = nullptr;
+
+    std::vector<std::shared_ptr<Submap>> all_submaps_;
+
+    std::shared_ptr<LoopClosing> loop_closing_ = nullptr;  // 回环检测
+
+    // 参数
+    inline static constexpr double keyframe_pos_th_ = 0.3;              // 关键帧位移量
+    inline static constexpr double keyframe_ang_th_ = 15 * M_PI / 180;  // 关键帧角度量
+};
+
+}  // namespace sad
+
+#endif  // SLAM_IN_AUTO_DRIVING_MAPPING_2D_H

workspace/slam_in_autonomous_driving/src/ch6/multi_resolution_likelihood_field.cc

@@ -0,0 +1,186 @@
+//
+// Created by xiang on 2022/4/7.
+//
+
+#include "multi_resolution_likelihood_field.h"
+#include "ch6/g2o_types.h"
+
+#include <g2o/core/base_unary_edge.h>
+#include <g2o/core/block_solver.h>
+#include <g2o/core/optimization_algorithm_levenberg.h>
+#include <g2o/core/robust_kernel.h>
+#include <g2o/core/robust_kernel_impl.h>
+#include <g2o/core/sparse_optimizer.h>
+#include <g2o/solvers/cholmod/linear_solver_cholmod.h>
+
+#include <glog/logging.h>
+
+namespace sad {
+
+void MRLikelihoodField::BuildModel() {
+    const int range = 20;  // 生成多少个像素的模板
+
+    /// 生成模板金字塔图像
+    field_ = {
+        cv::Mat(125, 125, CV_32F, 30.0),
+        cv::Mat(250, 250, CV_32F, 30.0),
+        cv::Mat(500, 500, CV_32F, 30.0),
+        cv::Mat(1000, 1000, CV_32F, 30.0),
+    };
+
+    for (int x = -range; x <= range; ++x) {
+        for (int y = -range; y <= range; ++y) {
+            model_.emplace_back(x, y, std::sqrt((x * x) + (y * y)));
+        }
+    }
+}
+
+void MRLikelihoodField::SetFieldImageFromOccuMap(const cv::Mat& occu_map) {
+    const int boarder = 25;
+    for (int x = boarder; x < occu_map.cols - boarder; ++x) {
+        for (int y = boarder; y < occu_map.rows - boarder; ++y) {
+            if (occu_map.at<uchar>(y, x) < 127) {
+                // 在该点生成一个model，在每个level中都填入
+                for (int l = 0; l < levels_; ++l) {
+                    for (auto& model_pt : model_) {
+                        int xx = int(x * ratios_[l] + model_pt.dx_);
+                        int yy = int(y * ratios_[l] + model_pt.dy_);
+                        if (xx >= 0 && xx < field_[l].cols && yy >= 0 && yy < field_[l].rows &&
+                            field_[l].at<float>(yy, xx) > model_pt.residual_) {
+                            field_[l].at<float>(yy, xx) = model_pt.residual_;
+                        }
+                    }
+                }
+            }
+        }
+    }
+}
+
+bool MRLikelihoodField::AlignG2O(SE2& init_pose) {
+    num_inliers_.clear();
+    inlier_ratio_.clear();
+
+    for (int l = 0; l < levels_; ++l) {
+        if (!AlignInLevel(l, init_pose)) {
+            return false;
+        }
+    }
+
+    /// 成功匹配的话，打印一些信息
+    for (int l = 0; l < levels_; ++l) {
+        LOG(INFO) << "level " << l << " inliers: " << num_inliers_[l] << ", ratio: " << inlier_ratio_[l];
+    }
+
+    return true;
+}
+
+std::vector<cv::Mat> MRLikelihoodField::GetFieldImage() {
+    std::vector<cv::Mat> images;
+    for (int l = 0; l < levels_; ++l) {
+        cv::Mat img(field_[l].rows, field_[l].cols, CV_8UC3);
+        for (int x = 0; x < field_[l].cols; ++x) {
+            for (int y = 0; y < field_[l].rows; ++y) {
+                float r = field_[l].at<float>(y, x) * 255.0 / 30.0;
+                img.at<cv::Vec3b>(y, x) = cv::Vec3b(uchar(r), uchar(r), uchar(r));
+            }
+        }
+
+        images.emplace_back(img);
+    }
+
+    return images;
+}
+
+bool MRLikelihoodField::AlignInLevel(int level, SE2& init_pose) {
+    using BlockSolverType = g2o::BlockSolver<g2o::BlockSolverTraits<3, 1>>;
+    using LinearSolverType = g2o::LinearSolverCholmod<BlockSolverType::PoseMatrixType>;
+    auto* solver = new g2o::OptimizationAlgorithmLevenberg(
+        g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
+    g2o::SparseOptimizer optimizer;
+    optimizer.setAlgorithm(solver);
+
+    auto* v = new VertexSE2();
+    v->setId(0);
+    v->setEstimate(init_pose);
+    optimizer.addVertex(v);
+
+    const double range_th = 15.0;  // 不考虑太远的scan，不准
+    const double rk_delta[] = {0.2, 0.3, 0.6, 0.8};
+
+    std::vector<EdgeSE2LikelihoodFiled*> edges;
+
+    // 遍历source
+    for (size_t i = 0; i < source_->ranges.size(); ++i) {
+        float r = source_->ranges[i];
+        if (r < source_->range_min || r > source_->range_max) {
+            continue;
+        }
+
+        if (r > range_th) {
+            continue;
+        }
+
+        float angle = source_->angle_min + i * source_->angle_increment;
+        if (angle < source_->angle_min + 30 * M_PI / 180.0 || angle > source_->angle_max - 30 * M_PI / 180.0) {
+            continue;
+        }
+
+        auto e = new EdgeSE2LikelihoodFiled(field_[level], r, angle, resolution_[level]);
+        e->setVertex(0, v);
+
+        if (e->IsOutSide()) {
+            delete e;
+            continue;
+        }
+
+        e->setInformation(Eigen::Matrix<double, 1, 1>::Identity());
+        auto rk = new g2o::RobustKernelHuber;
+        rk->setDelta(rk_delta[level]);
+        e->setRobustKernel(rk);
+        optimizer.addEdge(e);
+
+        edges.emplace_back(e);
+    }
+
+    if (edges.empty()) {
+        return false;
+    }
+
+    optimizer.setVerbose(false);
+    optimizer.initializeOptimization();
+    optimizer.optimize(10);
+
+    /// 计算edges中有多少inlier
+    int num_inliers =
+        std::accumulate(edges.begin(), edges.end(), 0, [&rk_delta, level](int num, EdgeSE2LikelihoodFiled* e) {
+            if (e->level() == 0 && e->chi2() < rk_delta[level]) {
+                return num + 1;
+            }
+            return num;
+        });
+
+    std::vector<double> chi2(edges.size());
+    for (int i = 0; i < edges.size(); ++i) {
+        chi2[i] = edges[i]->chi2();
+    }
+
+    std::sort(chi2.begin(), chi2.end());
+
+    /// 要求inlier比例超过一定值
+    /// 这个比较微妙，因为激光不是360的，这里大多数时候只能部分匹配上
+    const float inlier_ratio_th = 0.4;
+    float inlier_ratio = float(num_inliers) / edges.size();
+
+    num_inliers_.emplace_back(num_inliers);
+    inlier_ratio_.emplace_back(inlier_ratio);
+
+    if (num_inliers > 100 && inlier_ratio > inlier_ratio_th) {
+        init_pose = v->estimate();
+        return true;
+    } else {
+        // LOG(INFO) << "rejected because ratio is not enough: " << inlier_ratio;
+        return false;
+    }
+}
+
+}  // namespace sad

workspace/slam_in_autonomous_driving/src/ch6/multi_resolution_likelihood_field.h

@@ -0,0 +1,76 @@
+//
+// Created by xiang on 2022/4/7.
+//
+
+#ifndef SLAM_IN_AUTO_DRIVING_MULTI_RESOLUTION_LIKELIHOOD_FILED_H
+#define SLAM_IN_AUTO_DRIVING_MULTI_RESOLUTION_LIKELIHOOD_FILED_H
+
+#include <opencv2/core.hpp>
+
+#include "common/eigen_types.h"
+#include "common/lidar_utils.h"
+
+namespace sad {
+
+/// 多分辨率的似然场配准方法
+class MRLikelihoodField {
+   public:
+    /// 2D 场的模板，在设置target scan或map的时候生成
+    struct ModelPoint {
+        ModelPoint(int dx, int dy, float res) : dx_(dx), dy_(dy), residual_(res) {}
+        int dx_ = 0;
+        int dy_ = 0;
+        float residual_ = 0;
+    };
+
+    MRLikelihoodField() { BuildModel(); }
+
+    /// 从占据栅格地图生成一个似然场地图
+    void SetFieldImageFromOccuMap(const cv::Mat& occu_map);
+
+    /// 使用g2o配准，内部调用不同层级的图像进行配准
+    bool AlignG2O(SE2& init_pose);
+
+    /// 获取场函数，转换为RGB图像
+    std::vector<cv::Mat> GetFieldImage();
+
+    /// 设置中心（通常即submap中心）
+    void SetPose(const SE2& pose) { pose_ = pose; }
+
+    /// 设置匹配源
+    void SetSourceScan(Scan2d::Ptr scan) { source_ = scan; }
+
+    float Resolution(int level = 0) const { return resolution_[level]; }
+
+    int Levels() const { return levels_; }
+
+   private:
+    /**
+     * 在某一层图像中配准
+     * @param level
+     * @param init_pose
+     * @return
+     */
+    bool AlignInLevel(int level, SE2& init_pose);
+
+    void BuildModel();
+
+    SE2 pose_;
+
+    Scan2d::Ptr source_ = nullptr;
+
+    std::vector<ModelPoint> model_;  // 2D 模板
+    std::vector<cv::Mat> field_;     // 场函数
+
+    std::vector<int> num_inliers_;
+    std::vector<double> inlier_ratio_;
+
+    // 参数配置
+    inline static const int levels_ = 4;                                       // 金字塔层数
+    inline static const std::vector<float> resolution_ = {2.5, 5, 10, 20};     // 每米多少个像素
+    inline static const std::vector<float> ratios_ = {0.125, 0.25, 0.5, 1.0};  // 相比占据栅格图的比例尺
+};
+
+}  // namespace sad
+
+#endif  // SLAM_IN_AUTO_DRIVING_MULTI_RESOLUTION_LIKELIHOOD_FILED_H

workspace/slam_in_autonomous_driving/src/ch6/occupancy_map.cc

@@ -0,0 +1,221 @@
+//
+// Created by xiang on 2022/3/23.
+//
+
+#include "ch6/occupancy_map.h"
+#include "common/eigen_types.h"
+#include "common/math_utils.h"
+
+#include <glog/logging.h>
+#include <execution>
+
+namespace sad {
+
+OccupancyMap::OccupancyMap() {
+    BuildModel();
+    occupancy_grid_ = cv::Mat(image_size_, image_size_, CV_8U, 127);
+}
+
+void OccupancyMap::BuildModel() {
+    for (int x = -model_size_; x <= model_size_; x++) {
+        for (int y = -model_size_; y <= model_size_; y++) {
+            Model2DPoint pt;
+            pt.dx_ = x;
+            pt.dy_ = y;
+            pt.range_ = sqrt(x * x + y * y) * inv_resolution_;
+            pt.angle_ = std::atan2(y, x);
+            pt.angle_ = pt.angle_ > M_PI ? pt.angle_ - 2 * M_PI : pt.angle_;  // limit in 2pi
+            model_.push_back(pt);
+        }
+    }
+}
+
+double OccupancyMap::FindRangeInAngle(double angle, Scan2d::Ptr scan) {
+    math::KeepAngleInPI(angle);
+    if (angle < scan->angle_min || angle > scan->angle_max) {
+        return 0.0;
+    }
+
+    int angle_index = int((angle - scan->angle_min) / scan->angle_increment);
+    if (angle_index < 0 || angle_index >= scan->ranges.size()) {
+        return 0.0;
+    }
+
+    int angle_index_p = angle_index + 1;
+    double real_angle = angle;
+
+    // take range
+    double range = 0;
+    if (angle_index_p >= scan->ranges.size()) {
+        range = scan->ranges[angle_index];
+    } else {
+        // 插值
+        double s = ((angle - scan->angle_min) / scan->angle_increment) - angle_index;
+        double range1 = scan->ranges[angle_index];
+        double range2 = scan->ranges[angle_index_p];
+
+        double real_angle1 = scan->angle_min + scan->angle_increment * angle_index;
+        double real_angle2 = scan->angle_min + scan->angle_increment * angle_index_p;
+
+        if (range2 < scan->range_min || range2 > scan->range_max) {
+            range = range1;
+            real_angle = real_angle1;
+        } else if (range1 < scan->range_min || range1 > scan->range_max) {
+            range = range2;
+            real_angle = real_angle2;
+        } else if (std::fabs(range1 - range2) > 0.3) {
+            range = s > 0.5 ? range2 : range1;
+            real_angle = s > 0.5 ? real_angle2 : real_angle1;
+        } else {
+            range = range1 * (1 - s) + range2 * s;
+        }
+    }
+    return range;
+}
+
+void OccupancyMap::AddLidarFrame(std::shared_ptr<Frame> frame, GridMethod method) {
+    auto& scan = frame->scan_;
+    
+    // 此处不能直接使用frame->pose_submap_，因为frame可能来自上一个地图
+    // 此时frame->pose_submap_还未更新，依旧是frame在上一个地图中的pose
+    SE2 pose_in_submap = pose_.inverse() * frame->pose_;
+    float theta = pose_in_submap.so2().log();
+    has_outside_pts_ = false;
+
+    // 先计算末端点所在的网格
+    std::set<Vec2i, less_vec<2>> endpoints;
+
+    for (size_t i = 0; i < scan->ranges.size(); ++i) {
+        if (scan->ranges[i] < scan->range_min || scan->ranges[i] > scan->range_max) {
+            continue;
+        }
+
+        double real_angle = scan->angle_min + i * scan->angle_increment;
+        double x = scan->ranges[i] * std::cos(real_angle);
+        double y = scan->ranges[i] * std::sin(real_angle);
+
+        endpoints.emplace(World2Image(frame->pose_ * Vec2d(x, y)));
+    }
+
+    if (method == GridMethod::MODEL_POINTS) {
+        // 遍历模板，生成白色点
+        std::for_each(std::execution::par_unseq, model_.begin(), model_.end(), [&](const Model2DPoint& pt) {
+            Vec2i pos_in_image = World2Image(frame->pose_.translation());
+            Vec2i pw = pos_in_image + Vec2i(pt.dx_, pt.dy_);  // submap下
+
+            if (pt.range_ < closest_th_) {
+                // 小距离内认为无物体
+                SetPoint(pw, false);
+                return;
+            }
+
+            double angle = pt.angle_ - theta;  // 激光系下角度
+            double range = FindRangeInAngle(angle, scan);
+
+            if (range < scan->range_min || range > scan->range_max) {
+                /// 某方向无测量值时，认为无效
+                /// 但离机器比较近时，涂白
+                if (pt.range_ < endpoint_close_th_) {
+                    SetPoint(pw, false);
+                }
+                return;
+            }
+
+            if (range > pt.range_ && endpoints.find(pw) == endpoints.end()) {
+                /// 末端点与车体连线上的点，涂白
+                SetPoint(pw, false);
+            }
+        });
+    } else {
+        Vec2i start = World2Image(frame->pose_.translation());
+        std::for_each(std::execution::par_unseq, endpoints.begin(), endpoints.end(),
+                      [this, &start](const auto& pt) { BresenhamFilling(start, pt); });
+    }
+
+    /// 末端点涂黑
+    std::for_each(endpoints.begin(), endpoints.end(), [this](const auto& pt) { SetPoint(pt, true); });
+}
+
+void OccupancyMap::SetPoint(const Vec2i& pt, bool occupy) {
+    int x = pt[0], y = pt[1];
+    if (x < 0 || y < 0 || x >= occupancy_grid_.cols || y >= occupancy_grid_.rows) {
+        if (occupy) {
+            has_outside_pts_ = true;
+        }
+
+        return;
+    }
+
+    /// 这里设置了一个上下限
+    uchar value = occupancy_grid_.at<uchar>(y, x);
+    if (occupy) {
+        if (value > 117) {
+            occupancy_grid_.ptr<uchar>(y)[x] -= 1;
+        }
+    } else {
+        if (value < 137) {
+            occupancy_grid_.ptr<uchar>(y)[x] += 1;
+        }
+    }
+}
+
+cv::Mat OccupancyMap::GetOccupancyGridBlackWhite() const {
+    cv::Mat image(image_size_, image_size_, CV_8UC3);
+    for (int x = 0; x < occupancy_grid_.cols; ++x) {
+        for (int y = 0; y < occupancy_grid_.rows; ++y) {
+            if (occupancy_grid_.at<uchar>(y, x) == 127) {
+                image.at<cv::Vec3b>(y, x) = cv::Vec3b(127, 127, 127);
+            } else if (occupancy_grid_.at<uchar>(y, x) < 127) {
+                image.at<cv::Vec3b>(y, x) = cv::Vec3b(0, 0, 0);
+            } else if (occupancy_grid_.at<uchar>(y, x) > 127) {
+                image.at<cv::Vec3b>(y, x) = cv::Vec3b(255, 255, 255);
+            }
+        }
+    }
+
+    return image;
+}
+
+void OccupancyMap::BresenhamFilling(const Vec2i& p1, const Vec2i& p2) {
+    int dx = p2.x() - p1.x();
+    int dy = p2.y() - p1.y();
+    int ux = dx > 0 ? 1 : -1;
+    int uy = dy > 0 ? 1 : -1;
+
+    dx = abs(dx);
+    dy = abs(dy);
+    int x = p1.x();
+    int y = p1.y();
+
+    if (dx > dy) {
+        // 以x为增量
+        int e = -dx;
+        for (int i = 0; i < dx; ++i) {
+            x += ux;
+            e += 2 * dy;
+            if (e >= 0) {
+                y += uy;
+                e -= 2 * dx;
+            }
+
+            if (Vec2i(x, y) != p2) {
+                SetPoint(Vec2i(x, y), false);
+            }
+        }
+    } else {
+        int e = -dy;
+        for (int i = 0; i < dy; ++i) {
+            y += uy;
+            e += 2 * dx;
+            if (e >= 0) {
+                x += ux;
+                e -= 2 * dy;
+            }
+            if (Vec2i(x, y) != p2) {
+                SetPoint(Vec2i(x, y), false);
+            }
+        }
+    }
+}
+
+}  // namespace sad

workspace/slam_in_autonomous_driving/src/ch6/occupancy_map.h

@@ -0,0 +1,96 @@
+//
+// Created by xiang on 2022/3/23.
+//
+
+#ifndef SLAM_IN_AUTO_DRIVING_OCCUPANCY_MAP_H
+#define SLAM_IN_AUTO_DRIVING_OCCUPANCY_MAP_H
+
+#include <opencv2/core.hpp>
+
+#include "ch6/frame.h"
+
+namespace sad {
+
+/// 占据栅格地图, 对应书中6.3节内容
+class OccupancyMap {
+   public:
+    /// 栅格模板，预先计算
+    struct Model2DPoint {
+        int dx_ = 0;
+        int dy_ = 0;
+        double angle_ = 0;  // in rad
+        float range_ = 0;   // in meters
+    };
+
+    enum class GridMethod {
+        MODEL_POINTS,  // 模板化算法
+        BRESENHAM,     // 直接栅格化算法
+    };
+
+    OccupancyMap();
+
+    /// 往这个占据栅格地图中增加一个frame
+    void AddLidarFrame(std::shared_ptr<Frame> frame, GridMethod method = GridMethod::BRESENHAM);
+
+    /// 获取原始的占据栅格地图
+    cv::Mat GetOccupancyGrid() const { return occupancy_grid_; }
+
+    /// 获取黑白灰形式的占据栅格，作可视化使用
+    cv::Mat GetOccupancyGridBlackWhite() const;
+
+    /// 设置中心点
+    void SetPose(const SE2& pose) { pose_ = pose; }
+
+    bool HasOutsidePoints() const { return has_outside_pts_; }
+
+    /// 获取分辨率
+    double Resolution() const { return resolution_; }
+
+    /// 在某个点填入占据或者非占据信息
+    void SetPoint(const Vec2i& pt, bool occupy);
+
+   private:
+    /// 生成填充模板
+    void BuildModel();
+
+    /// 从世界坐标系转到图像坐标系
+    template <class T>
+    inline Vec2i World2Image(const Eigen::Matrix<T, 2, 1>& pt) {
+        Vec2d pt_map = (pose_.inverse() * pt) * resolution_ + center_image_;
+        int x = int(pt_map[0]);
+        int y = int(pt_map[1]);
+        return Vec2i(x, y);
+    }
+
+    /// 查找某个角度下的range值
+    double FindRangeInAngle(double angle, Scan2d::Ptr scan);
+
+    /**
+     * Bresenham直线填充，给定起始点和终止点，将中间的区域填充为白色
+     * @param p1
+     * @param p2
+     */
+    void BresenhamFilling(const Vec2i& p1, const Vec2i& p2);
+
+    cv::Mat occupancy_grid_;  // 8bit 占据栅格图像
+
+    SE2 pose_;  // T_W_S
+    Vec2d center_image_ = Vec2d(image_size_ / 2, image_size_ / 2);
+
+    bool has_outside_pts_ = false;  // 标注栅格化过程中是否有落在外部的点
+
+    // 模板
+    std::vector<Model2DPoint> model_;  // 用于填充占据栅格的模板，都是世界系下的点
+
+    // 参数
+    inline static constexpr double closest_th_ = 0.2;         // 近距离阈值
+    inline static constexpr double endpoint_close_th_ = 0.1;  // 末端点障碍物近距离阈值
+    inline static constexpr double resolution_ = 20.0;        // 1m 多少像素
+    inline static constexpr float inv_resolution_ = 0.05;     // 1个像素多少米（栅格分辨率）
+    inline static constexpr int image_size_ = 1000;           // 图像大小
+    inline static constexpr int model_size_ = 400;            // 模板像素大小
+};
+
+}  // namespace sad
+
+#endif  // SLAM_IN_AUTO_DRIVING_OCCUPANCY_MAP_H

workspace/slam_in_autonomous_driving/src/ch6/submap.cc

@@ -0,0 +1,48 @@
+//
+// Created by xiang on 2022/3/23.
+//
+
+#include "ch6/submap.h"
+#include <glog/logging.h>
+
+namespace sad {
+
+void Submap::SetOccuFromOtherSubmap(std::shared_ptr<Submap> other) {
+    auto frames_in_other = other->GetFrames();
+    // 取最近10个帧
+    for (size_t i = frames_in_other.size() - 10; i < frames_in_other.size(); ++i) {
+        if (i > 0) {
+            occu_map_.AddLidarFrame(frames_in_other[i]);
+        }
+    }
+    field_.SetFieldImageFromOccuMap(occu_map_.GetOccupancyGrid());
+}
+
+bool Submap::MatchScan(std::shared_ptr<Frame> frame) {
+    field_.SetSourceScan(frame->scan_);
+    field_.AlignG2O(frame->pose_submap_);
+    frame->pose_ = pose_ * frame->pose_submap_;  // T_w_c = T_w_s * T_s_c
+
+    return true;
+}
+
+void Submap::AddScanInOccupancyMap(std::shared_ptr<Frame> frame) {
+    occu_map_.AddLidarFrame(frame, OccupancyMap::GridMethod::MODEL_POINTS);  // 更新栅格地图中的格子
+    field_.SetFieldImageFromOccuMap(occu_map_.GetOccupancyGrid());           // 更新场函数图像
+}
+
+bool Submap::HasOutsidePoints() const { return occu_map_.HasOutsidePoints(); }
+
+void Submap::SetPose(const SE2& pose) {
+    pose_ = pose;
+    occu_map_.SetPose(pose);
+    field_.SetPose(pose);
+}
+
+void Submap::UpdateFramePoseWorld() {
+    for (auto& frame : frames_) {
+        frame->pose_ = pose_ * frame->pose_submap_;
+    }
+}
+
+}  // namespace sad

workspace/slam_in_autonomous_driving/src/ch6/submap.h

@@ -0,0 +1,69 @@
+//
+// Created by xiang on 2022/3/23.
+//
+
+#ifndef SLAM_IN_AUTO_DRIVING_SUBMAP_H
+#define SLAM_IN_AUTO_DRIVING_SUBMAP_H
+
+#include "ch6/frame.h"
+#include "ch6/likelihood_filed.h"
+#include "ch6/occupancy_map.h"
+
+namespace sad {
+
+/**
+ * 子地图类
+ * 子地图关联到若干个关键帧，也会维护自己的栅格地图与似然场
+ * 往子地图中添加关键帧时，会更新它的栅格地图与似然场
+ * 子地图有自己的pose，记为 Tws，每个frame的世界位姿可以由子地图的pose乘frame在子地图中的相对pose得到
+ */
+class Submap {
+   public:
+    Submap(const SE2& pose) : pose_(pose) {
+        occu_map_.SetPose(pose_);
+        field_.SetPose(pose_);
+    }
+
+    /// 把另一个submap中的占据栅格复制到本地图中
+    void SetOccuFromOtherSubmap(std::shared_ptr<Submap> other);
+
+    /// 将frame与本submap进行匹配，计算frame->pose
+    bool MatchScan(std::shared_ptr<Frame> frame);
+
+    /// 判定当前的scan是否有位于submap外部的点
+    bool HasOutsidePoints() const;
+
+    /// 在栅格地图中增加一个帧
+    void AddScanInOccupancyMap(std::shared_ptr<Frame> frame);
+
+    /// 在子地图中增加一个关键帧
+    void AddKeyFrame(std::shared_ptr<Frame> frame) { frames_.emplace_back(frame); }
+
+    /// 当子地图的位姿更新时，重设每个frame的世界位姿
+    void UpdateFramePoseWorld();
+
+    /// accessors
+    OccupancyMap& GetOccuMap() { return occu_map_; }
+    LikelihoodField& GetLikelihood() { return field_; }
+
+    std::vector<std::shared_ptr<Frame>>& GetFrames() { return frames_; }
+    size_t NumFrames() const { return frames_.size(); }
+
+    void SetId(size_t id) { id_ = id; }
+    size_t GetId() const { return id_; }
+
+    void SetPose(const SE2& pose);
+    SE2 GetPose() const { return pose_; }
+
+   private:
+    SE2 pose_;  // submap的pose, Tws
+    size_t id_ = 0;
+
+    std::vector<std::shared_ptr<Frame>> frames_;  // 一个submap中的关键帧
+    LikelihoodField field_;                       // 用于匹配
+    OccupancyMap occu_map_;                       // 用于生成栅格地图
+};
+
+}  // namespace sad
+
+#endif  // SLAM_IN_AUTO_DRIVING_SUBMAP_H

workspace/slam_in_autonomous_driving/src/ch6/test_2d_icp_likelihood.cc

@@ -0,0 +1,69 @@
+//
+// Created by xiang on 2022/3/15.
+//
+#include <gflags/gflags.h>
+#include <glog/logging.h>
+#include <opencv2/highgui.hpp>
+
+#include "ch6/lidar_2d_utils.h"
+#include "ch6/likelihood_filed.h"
+#include "common/io_utils.h"
+
+DEFINE_string(bag_path, "./dataset/sad/2dmapping/floor1.bag", "数据包路径");
+DEFINE_string(method, "gauss-newton", "gauss-newton/g2o");
+
+/// 测试2D似然场法的ICP
+
+int main(int argc, char** argv) {
+    google::InitGoogleLogging(argv[0]);
+    FLAGS_stderrthreshold = google::INFO;
+    FLAGS_colorlogtostderr = true;
+    google::ParseCommandLineFlags(&argc, &argv, true);
+
+    sad::RosbagIO rosbag_io(fLS::FLAGS_bag_path);
+    Scan2d::Ptr last_scan = nullptr, current_scan = nullptr;
+
+    /// 我们将上一个scan与当前scan进行配准
+    rosbag_io
+        .AddScan2DHandle(
+            "/pavo_scan_bottom",
+            [&](Scan2d::Ptr scan) {
+                sad::LikelihoodField lf;
+                current_scan = scan;
+                SE2 pose;
+
+                if (last_scan == nullptr) {
+                    last_scan = current_scan;
+                    return true;
+                }
+
+                lf.SetTargetScan(last_scan);
+                lf.SetSourceScan(current_scan);
+
+                if (FLAGS_method == "gauss-newton") {
+                    lf.AlignGaussNewton(pose);
+                } else if (FLAGS_method == "g2o") {
+                    lf.AlignG2O(pose);
+                }
+
+                LOG(INFO) << "aligned pose: " << pose.translation().transpose() << ", " << pose.so2().log();
+
+                cv::Mat image;
+                sad::Visualize2DScan(last_scan, SE2(), image, Vec3b(255, 0, 0));    // target是蓝的
+                sad::Visualize2DScan(current_scan, pose, image, Vec3b(0, 0, 255));  // source是红的
+                cv::imshow("scan", image);
+
+                /// 画出target和它的场函数
+                cv::Mat field_image = lf.GetFieldImage();
+                sad::Visualize2DScan(last_scan, SE2(), field_image, Vec3b(255, 0, 0), 1000,
+                                     20.0);  // target是蓝的
+                cv::imshow("field", field_image);
+                cv::waitKey(10);
+
+                last_scan = current_scan;
+                return true;
+            })
+        .Go();
+
+    return 0;
+}

workspace/slam_in_autonomous_driving/src/ch6/test_2d_icp_s2s.cc

@@ -0,0 +1,60 @@
+//
+// Created by xiang on 2022/3/15.
+//
+#include <gflags/gflags.h>
+#include <glog/logging.h>
+#include <opencv2/highgui.hpp>
+
+#include "ch6/icp_2d.h"
+#include "ch6/lidar_2d_utils.h"
+#include "common/io_utils.h"
+
+DEFINE_string(bag_path, "./dataset/sad/2dmapping/floor1.bag", "数据包路径");
+DEFINE_string(method, "point2point", "2d icp方法：point2point/point2plane");
+
+/// 测试从rosbag中读取2d scan并plot的结果
+/// 通过选择method来确定使用点到点或点到面的ICP
+int main(int argc, char** argv) {
+    google::InitGoogleLogging(argv[0]);
+    FLAGS_stderrthreshold = google::INFO;
+    FLAGS_colorlogtostderr = true;
+    google::ParseCommandLineFlags(&argc, &argv, true);
+
+    sad::RosbagIO rosbag_io(fLS::FLAGS_bag_path);
+    Scan2d::Ptr last_scan = nullptr, current_scan = nullptr;
+
+    /// 我们将上一个scan与当前scan进行配准
+    rosbag_io
+        .AddScan2DHandle("/pavo_scan_bottom",
+                         [&](Scan2d::Ptr scan) {
+                             current_scan = scan;
+
+                             if (last_scan == nullptr) {
+                                 last_scan = current_scan;
+                                 return true;
+                             }
+
+                             sad::Icp2d icp;
+                             icp.SetTarget(last_scan);
+                             icp.SetSource(current_scan);
+
+                             SE2 pose;
+                             if (fLS::FLAGS_method == "point2point") {
+                                 icp.AlignGaussNewton(pose);
+                             } else if (fLS::FLAGS_method == "point2plane") {
+                                 icp.AlignGaussNewtonPoint2Plane(pose);
+                             }
+
+                             cv::Mat image;
+                             sad::Visualize2DScan(last_scan, SE2(), image, Vec3b(255, 0, 0));    // target是蓝的
+                             sad::Visualize2DScan(current_scan, pose, image, Vec3b(0, 0, 255));  // source是红的
+                             cv::imshow("scan", image);
+                             cv::waitKey(20);
+
+                             last_scan = current_scan;
+                             return true;
+                         })
+        .Go();
+
+    return 0;
+}

workspace/slam_in_autonomous_driving/src/ch6/test_2d_mapping.cc

@@ -0,0 +1,35 @@
+//
+// Created by xiang on 2022/3/15.
+//
+#include <gflags/gflags.h>
+#include <glog/logging.h>
+#include <opencv2/highgui.hpp>
+
+#include "ch6/lidar_2d_utils.h"
+#include "ch6/mapping_2d.h"
+#include "common/io_utils.h"
+
+DEFINE_string(bag_path, "./dataset/sad/2dmapping/floor1.bag", "数据包路径");
+DEFINE_bool(with_loop_closing, false, "是否使用回环检测");
+
+/// 测试2D lidar SLAM
+
+int main(int argc, char** argv) {
+    google::InitGoogleLogging(argv[0]);
+    FLAGS_stderrthreshold = google::INFO;
+    FLAGS_colorlogtostderr = true;
+    google::ParseCommandLineFlags(&argc, &argv, true);
+
+    sad::RosbagIO rosbag_io(fLS::FLAGS_bag_path);
+    sad::Mapping2D mapping;
+
+    std::system("rm -rf ./data/ch6/*");
+
+    if (mapping.Init(FLAGS_with_loop_closing) == false) {
+        return -1;
+    }
+
+    rosbag_io.AddScan2DHandle("/pavo_scan_bottom", [&](Scan2d::Ptr scan) { return mapping.ProcessScan(scan); }).Go();
+    cv::imwrite("./data/ch6/global_map.png", mapping.ShowGlobalMap(2000));
+    return 0;
+}

workspace/slam_in_autonomous_driving/src/ch6/test_2dlidar_io.cc

@@ -0,0 +1,34 @@
+//
+// Created by xiang on 2022/3/15.
+//
+#include <gflags/gflags.h>
+#include <glog/logging.h>
+#include <opencv2/highgui.hpp>
+
+#include "ch6/lidar_2d_utils.h"
+#include "common/io_utils.h"
+
+DEFINE_string(bag_path, "./dataset/sad/2dmapping/test_2d_lidar.bag", "数据包路径");
+
+/// 测试从rosbag中读取2d scan并plot的结果
+
+int main(int argc, char** argv) {
+    google::InitGoogleLogging(argv[0]);
+    FLAGS_stderrthreshold = google::INFO;
+    FLAGS_colorlogtostderr = true;
+    google::ParseCommandLineFlags(&argc, &argv, true);
+
+    sad::RosbagIO rosbag_io(fLS::FLAGS_bag_path);
+    rosbag_io
+        .AddScan2DHandle("/pavo_scan_bottom",
+                         [](Scan2d::Ptr scan) {
+                             cv::Mat image;
+                             sad::Visualize2DScan(scan, SE2(), image, Vec3b(255, 0, 0));
+                             cv::imshow("scan", image);
+                             cv::waitKey(20);
+                             return true;
+                         })
+        .Go();
+
+    return 0;
+}

workspace/slam_in_autonomous_driving/src/ch6/test_mr_matching.cc

@@ -0,0 +1,77 @@
+//
+// Created by xiang on 2022/3/15.
+//
+#include <gflags/gflags.h>
+#include <glog/logging.h>
+#include <fstream>
+#include <opencv2/highgui.hpp>
+
+#include "ch6/frame.h"
+#include "ch6/lidar_2d_utils.h"
+#include "ch6/multi_resolution_likelihood_field.h"
+
+/// 测试多分辨率的匹配
+
+int main(int argc, char** argv) {
+    google::InitGoogleLogging(argv[0]);
+    FLAGS_stderrthreshold = google::INFO;
+    FLAGS_colorlogtostderr = true;
+    google::ParseCommandLineFlags(&argc, &argv, true);
+
+    std::ifstream fin("./data/ch6/loops.txt");
+    int loop_id = 0;
+
+    while (!fin.eof()) {
+        int frame_id, submap_id;
+        double submap_center_x, submap_center_y, theta;
+        if (fin.peek() == fin.eof()) {
+            break;
+        }
+
+        fin >> frame_id >> submap_id >> submap_center_x >> submap_center_y >> theta;
+        loop_id++;
+
+        sad::MRLikelihoodField mr_field;
+        Vec2d center(submap_center_x, submap_center_y);
+        SE2 pose_submap(SO2::exp(theta), center);
+
+        mr_field.SetPose(pose_submap);
+        cv::Mat occu_map = cv::imread("./data/ch6/submap_" + std::to_string(submap_id) + ".png", cv::IMREAD_GRAYSCALE);
+        cv::Mat occu_map_color =
+            cv::imread("./data/ch6/submap_" + std::to_string(submap_id) + ".png", cv::IMREAD_COLOR);
+        mr_field.SetFieldImageFromOccuMap(occu_map);
+
+        sad::Frame frame;
+        frame.Load("./data/ch6/frame_" + std::to_string(frame_id) + ".txt");
+        mr_field.SetSourceScan(frame.scan_);
+
+        LOG(INFO) << "testing frame " << frame.id_ << " with " << submap_id;
+
+        auto init_pose = frame.pose_;
+        auto frame_pose_in_submap = pose_submap.inverse() * frame.pose_;
+        bool align_success = mr_field.AlignG2O(frame_pose_in_submap);
+
+        if (align_success) {
+            frame.pose_ = pose_submap * frame_pose_in_submap;
+            auto images = mr_field.GetFieldImage();
+            for (int i = 0; i < images.size(); ++i) {
+                /// 初始pose 以红色显示
+                sad::Visualize2DScan(frame.scan_, init_pose, images[i], Vec3b(0, 0, 255), images[i].rows,
+                                     mr_field.Resolution(i), pose_submap);
+                /// 配准后pose 以绿色显示
+                sad::Visualize2DScan(frame.scan_, frame.pose_, images[i], Vec3b(0, 255, 0), images[i].rows,
+                                     mr_field.Resolution(i), pose_submap);
+                cv::imshow("level " + std::to_string(i), images[i]);
+            }
+
+            sad::Visualize2DScan(frame.scan_, init_pose, occu_map_color, Vec3b(0, 0, 255), occu_map_color.rows, 20.0,
+                                 pose_submap);
+            sad::Visualize2DScan(frame.scan_, frame.pose_, occu_map_color, Vec3b(0, 255, 0), occu_map_color.rows, 20.0,
+                                 pose_submap);
+            cv::imshow("occupancy", occu_map_color);
+            cv::waitKey();
+        }
+    }
+
+    return 0;
+}

workspace/slam_in_autonomous_driving/src/ch6/test_occupancy_grid.cc

@@ -0,0 +1,53 @@
+//
+// Created by xiang on 2022/3/15.
+//
+#include <gflags/gflags.h>
+#include <glog/logging.h>
+#include <opencv2/highgui.hpp>
+
+#include "ch6/lidar_2d_utils.h"
+#include "ch6/occupancy_map.h"
+#include "common/io_utils.h"
+#include "common/sys_utils.h"
+
+DEFINE_string(bag_path, "./dataset/sad/2dmapping/floor1.bag", "数据包路径");
+DEFINE_string(method, "model/bresenham", "填充算法：model/bresenham");
+
+/// 测试2D似然场法的ICP
+
+int main(int argc, char** argv) {
+    google::InitGoogleLogging(argv[0]);
+    FLAGS_stderrthreshold = google::INFO;
+    FLAGS_colorlogtostderr = true;
+    google::ParseCommandLineFlags(&argc, &argv, true);
+
+    sad::RosbagIO rosbag_io(fLS::FLAGS_bag_path);
+
+    /// 测试单个scan生成出来的occupancy grid是否正确
+    rosbag_io
+        .AddScan2DHandle("/pavo_scan_bottom",
+                         [&](Scan2d::Ptr scan) {
+                             sad::OccupancyMap oc_map;
+                             if (FLAGS_method == "model") {
+                                 sad::evaluate_and_call(
+                                     [&]() {
+                                         oc_map.AddLidarFrame(std::make_shared<sad::Frame>(scan),
+                                                              sad::OccupancyMap::GridMethod::MODEL_POINTS);
+                                     },
+                                     "Occupancy with model points");
+                             } else {
+                                 sad::evaluate_and_call(
+                                     [&]() {
+                                         oc_map.AddLidarFrame(std::make_shared<sad::Frame>(scan),
+                                                              sad::OccupancyMap::GridMethod::BRESENHAM);
+                                     },
+                                     "Occupancy with bresenham");
+                             }
+                             cv::imshow("occupancy map", oc_map.GetOccupancyGridBlackWhite());
+                             cv::waitKey(10);
+                             return true;
+                         })
+        .Go();
+
+    return 0;
+}

workspace/slam_in_autonomous_driving/src/ch7/CMakeLists.txt

@@ -0,0 +1,98 @@
+add_library(${PROJECT_NAME}.ch7
+        icp_3d.cc
+        ndt_3d.cc
+        gen_simu_data.cc
+        direct_ndt_lo.cc
+        incremental_ndt_lo.cc
+        ndt_inc.cc
+        loam-like/feature_extraction.cc
+        loam-like/loam_like_odom.cc
+
+        loosely_coupled_lio/loosely_lio.cc
+        loosely_coupled_lio/cloud_convert.cc
+        loosely_coupled_lio/measure_sync.cc
+        )
+
+target_link_libraries(${PROJECT_NAME}.ch7
+        ${PROJECT_NAME}.ch3
+        ${PROJECT_NAME}.ch5
+        ${PROJECT_NAME}.common
+        ${third_party_libs}
+        )
+
+add_executable(test_icp
+        test/test_icp.cc
+        )
+
+target_link_libraries(test_icp
+        ${PROJECT_NAME}.ch5
+        ${PROJECT_NAME}.ch7
+        ${PROJECT_NAME}.common
+        ${third_party_libs}
+        )
+
+add_executable(test_ndt_lo
+        test/test_ndt_lo.cc
+        )
+
+target_link_libraries(test_ndt_lo
+        ${PROJECT_NAME}.ch5
+        ${PROJECT_NAME}.ch7
+        ${PROJECT_NAME}.common
+        ${third_party_libs}
+        )
+
+add_executable(test_inc_ndt_lo
+        test/test_inc_ndt_lo.cc
+        )
+
+target_link_libraries(test_inc_ndt_lo
+        ${PROJECT_NAME}.ch5
+        ${PROJECT_NAME}.ch7
+        ${PROJECT_NAME}.common
+        ${third_party_libs}
+        )
+
+add_executable(test_feature_extraction
+        test/test_feature_extraction.cc
+        )
+
+target_link_libraries(test_feature_extraction
+        ${PROJECT_NAME}.ch5
+        ${PROJECT_NAME}.ch7
+        ${PROJECT_NAME}.common
+        ${third_party_libs}
+        )
+
+add_executable(test_loam_odom
+        test/test_loam_odom.cc
+        )
+
+target_link_libraries(test_loam_odom
+        ${PROJECT_NAME}.ch5
+        ${PROJECT_NAME}.ch7
+        ${PROJECT_NAME}.common
+        ${third_party_libs}
+        )
+
+add_executable(test_loosely_lio
+        test/test_loosely_lio.cc
+        )
+
+target_link_libraries(test_loosely_lio
+        ${PROJECT_NAME}.ch5
+        ${PROJECT_NAME}.ch7
+        ${PROJECT_NAME}.common
+        ${third_party_libs}
+        )
+
+add_executable(run_gen_simu_data
+        test/run_gen_simu_data.cc
+        )
+
+target_link_libraries(run_gen_simu_data
+        ${PROJECT_NAME}.ch5
+        ${PROJECT_NAME}.ch7
+        ${PROJECT_NAME}.common
+        ${third_party_libs}
+        )

workspace/slam_in_autonomous_driving/src/ch7/direct_ndt_lo.cc

@@ -0,0 +1,118 @@
+//
+// Created by xiang on 2022/7/18.
+//
+
+#include "ch7/direct_ndt_lo.h"
+#include "common/math_utils.h"
+#include "tools/pcl_map_viewer.h"
+
+#include <pcl/common/transforms.h>
+
+namespace sad {
+
+void DirectNDTLO::AddCloud(CloudPtr scan, SE3& pose) {
+    if (local_map_ == nullptr) {
+        // 第一个帧，直接加入local map
+        local_map_.reset(new PointCloudType);
+        // operator += 用来拼接点云
+        *local_map_ += *scan;
+        pose = SE3();
+        last_kf_pose_ = pose;
+
+        if (options_.use_pcl_ndt_) {
+            ndt_pcl_.setInputTarget(local_map_);
+        } else {
+            ndt_.SetTarget(local_map_);
+        }
+
+        return;
+    }
+
+    // 计算scan相对于local map的位姿
+    pose = AlignWithLocalMap(scan);
+    CloudPtr scan_world(new PointCloudType);
+    pcl::transformPointCloud(*scan, *scan_world, pose.matrix().cast<float>());
+
+    if (IsKeyframe(pose)) {
+        last_kf_pose_ = pose;
+
+        // 重建local map
+        scans_in_local_map_.emplace_back(scan_world);
+        if (scans_in_local_map_.size() > options_.num_kfs_in_local_map_) {
+            scans_in_local_map_.pop_front();
+        }
+
+        local_map_.reset(new PointCloudType);
+        for (auto& scan : scans_in_local_map_) {
+            *local_map_ += *scan;
+        }
+
+        if (options_.use_pcl_ndt_) {
+            ndt_pcl_.setInputTarget(local_map_);
+        } else {
+            ndt_.SetTarget(local_map_);
+        }
+    }
+
+    if (viewer_ != nullptr) {
+        viewer_->SetPoseAndCloud(pose, scan_world);
+    }
+}
+
+bool DirectNDTLO::IsKeyframe(const SE3& current_pose) {
+    // 只要与上一帧相对运动超过一定距离或角度，就记关键帧
+    SE3 delta = last_kf_pose_.inverse() * current_pose;
+    return delta.translation().norm() > options_.kf_distance_ ||
+           delta.so3().log().norm() > options_.kf_angle_deg_ * math::kDEG2RAD;
+}
+
+SE3 DirectNDTLO::AlignWithLocalMap(CloudPtr scan) {
+    if (options_.use_pcl_ndt_) {
+        ndt_pcl_.setInputSource(scan);
+    } else {
+        ndt_.SetSource(scan);
+    }
+
+    CloudPtr output(new PointCloudType());
+
+    SE3 guess;
+    bool align_success = true;
+    if (estimated_poses_.size() < 2) {
+        if (options_.use_pcl_ndt_) {
+            ndt_pcl_.align(*output, guess.matrix().cast<float>());
+            guess = Mat4ToSE3(ndt_pcl_.getFinalTransformation().cast<double>().eval());
+        } else {
+            align_success = ndt_.AlignNdt(guess);
+        }
+    } else {
+        // 从最近两个pose来推断
+        SE3 T1 = estimated_poses_[estimated_poses_.size() - 1];
+        SE3 T2 = estimated_poses_[estimated_poses_.size() - 2];
+        guess = T1 * (T2.inverse() * T1);
+
+        if (options_.use_pcl_ndt_) {
+            ndt_pcl_.align(*output, guess.matrix().cast<float>());
+            guess = Mat4ToSE3(ndt_pcl_.getFinalTransformation().cast<double>().eval());
+        } else {
+            align_success = ndt_.AlignNdt(guess);
+        }
+    }
+
+    LOG(INFO) << "pose: " << guess.translation().transpose() << ", "
+              << guess.so3().unit_quaternion().coeffs().transpose();
+
+    if (options_.use_pcl_ndt_) {
+        LOG(INFO) << "trans prob: " << ndt_pcl_.getTransformationProbability();
+    }
+
+    estimated_poses_.emplace_back(guess);
+    return guess;
+}
+
+void DirectNDTLO::SaveMap(const std::string& map_path) {
+    if (viewer_) {
+        viewer_->SaveMap(map_path);
+    }
+}
+
+}  // namespace sad

workspace/slam_in_autonomous_driving/src/ch7/direct_ndt_lo.h

@@ -0,0 +1,79 @@
+//
+// Created by xiang on 2022/7/18.
+//
+
+#ifndef SLAM_IN_AUTO_DRIVING_DIRECT_NDT_LO_H
+#define SLAM_IN_AUTO_DRIVING_DIRECT_NDT_LO_H
+
+#include <pcl/registration/ndt.h>
+#include <deque>
+
+#include "ch7/ndt_3d.h"
+#include "common/eigen_types.h"
+#include "common/point_types.h"
+#include "tools/pcl_map_viewer.h"
+
+namespace sad {
+
+/**
+ * 使用直接NDT方法进行递推的Lidar Odometry
+ * 使用历史几个关键帧作为local map，进行NDT定位
+ */
+class DirectNDTLO {
+   public:
+    struct Options {
+        Options() {}
+        double kf_distance_ = 0.5;            // 关键帧距离
+        double kf_angle_deg_ = 30;            // 旋转角度
+        int num_kfs_in_local_map_ = 30;       // 局部地图含有多少个关键帧
+        bool use_pcl_ndt_ = true;            // 使用本章的NDT还是PCL的NDT
+        bool display_realtime_cloud_ = true;  // 是否显示实时点云
+
+        Ndt3d::Options ndt3d_options_;  // NDT3D 的配置
+    };
+
+    DirectNDTLO(Options options = Options()) : options_(options) {
+        if (options_.display_realtime_cloud_) {
+            viewer_ = std::make_shared<PCLMapViewer>(0.5);
+        }
+
+        ndt_ = Ndt3d(options_.ndt3d_options_);
+
+        ndt_pcl_.setResolution(1.0);
+        ndt_pcl_.setStepSize(0.1);
+        ndt_pcl_.setTransformationEpsilon(0.01);
+    }
+
+    /**
+     * 往LO中增加一个点云
+     * @param scan  当前帧点云
+     * @param pose 估计pose
+     */
+    void AddCloud(CloudPtr scan, SE3& pose);
+
+    /// 存储地图(viewer里）
+    void SaveMap(const std::string& map_path);
+
+   private:
+    /// 与local map进行配准
+    SE3 AlignWithLocalMap(CloudPtr scan);
+
+    /// 判定是否为关键帧
+    bool IsKeyframe(const SE3& current_pose);
+
+   private:
+    Options options_;
+    CloudPtr local_map_ = nullptr;
+    std::deque<CloudPtr> scans_in_local_map_;
+    std::vector<SE3> estimated_poses_;  // 所有估计出来的pose，用于记录轨迹和预测下一个帧
+    SE3 last_kf_pose_;                  // 上一关键帧的位姿
+
+    pcl::NormalDistributionsTransform<PointType, PointType> ndt_pcl_;
+    Ndt3d ndt_;
+
+    std::shared_ptr<PCLMapViewer> viewer_ = nullptr;
+};
+
+}  // namespace sad
+
+#endif  // SLAM_IN_AUTO_DRIVING_DIRECT_NDT_LO_H

workspace/slam_in_autonomous_driving/src/ch7/gen_simu_data.cc

@@ -0,0 +1,61 @@
+//
+// Created by xiang on 2022/7/12.
+//
+
+#include "gen_simu_data.h"
+#include "common/eigen_types.h"
+
+#include <opencv2/core/core.hpp>
+
+namespace sad {
+
+void GenSimuData::Gen() {
+    GenTarget();
+
+    // 生成 pose 和 source
+    cv::RNG rng;
+    gt_pose_ = SE3(SO3::exp(Vec3d(rng.gaussian(options_.pose_rot_sigma_), rng.gaussian(options_.pose_rot_sigma_),
+                                  rng.gaussian(options_.pose_rot_sigma_))),
+                   Vec3d(rng.gaussian(options_.pose_trans_sigma_), rng.gaussian(options_.pose_trans_sigma_),
+                         rng.gaussian(options_.pose_trans_sigma_)));
+
+    //　生成　source
+    source_.reset(new PointCloudType);
+    for (int i = 0; i < options_.num_points_; ++i) {
+        source_->points.emplace_back(ToPointType(gt_pose_ * ToVec3d(target_->points[i])));
+    }
+    source_->width = source_->size();
+}
+
+void GenSimuData::GenTarget() {
+    cv::RNG rng;
+
+    // 生成taget
+    target_.reset(new PointCloudType());
+
+    for (int i = 0; i < options_.num_points_; ++i) {
+        int num_face = rng.uniform(0, 6);
+        // 先随机生成一点，分配到６个面上
+        Vec3d pt(rng.uniform(-options_.length_, options_.length_), rng.uniform(-options_.width_, options_.width_),
+                 rng.uniform(-options_.height_, options_.height_));
+
+        if (num_face == 0) {
+            pt.z() = -options_.height_;
+        } else if (num_face == 1) {
+            pt.z() = options_.height_;
+        } else if (num_face == 2) {
+            pt.x() = -options_.length_;
+        } else if (num_face == 3) {
+            pt.x() = options_.length_;
+        } else if (num_face == 4) {
+            pt.y() = -options_.width_;
+        } else if (num_face == 5) {
+            pt.y() = options_.width_;
+        }
+        target_->points.emplace_back(ToPointType(pt));
+    }
+
+    target_->width = options_.num_points_;
+}
+
+}  // namespace sad

workspace/slam_in_autonomous_driving/src/ch7/gen_simu_data.h

@@ -0,0 +1,51 @@
+//
+// Created by xiang on 2022/7/12.
+//
+
+#ifndef SLAM_IN_AUTO_DRIVING_GEN_SIMU_DATA_H
+#define SLAM_IN_AUTO_DRIVING_GEN_SIMU_DATA_H
+
+#include "common/eigen_types.h"
+#include "common/point_types.h"
+
+namespace sad {
+
+/**
+ * 生成本章所需要的仿真数据
+ * 仿真数据可以获取真值位姿，也能保证真值的点到点匹配关系，可以检测我们算法是否写对
+ *
+ * 本程序模拟一个简单的长方体，可以给它施加一个随机的6 dof变换，然后输出source和target点云
+ */
+class GenSimuData {
+   public:
+    struct Options {
+        Options() {}
+        int num_points_ = 2000;  // 点数中点的数量
+        // 盒子参数
+        double width_ = 5.0;    // 宽度，y方向
+        double length_ = 10.0;  // 长度，x方向
+        double height_ = 1.0;   // 高度，z方向
+        // pose　参数
+        double pose_rot_sigma_ = 0.05;   // 旋转方向sigma
+        double pose_trans_sigma_ = 0.3;  // 平移方向sigma
+    };
+
+    GenSimuData(Options options = Options()) : options_(options) {}
+
+    /// 生成target和source点云
+    void Gen();
+
+    CloudPtr GetTarget() const { return target_; }
+    CloudPtr GetSource() const { return source_; }
+    SE3 GetPose() const { return gt_pose_; }
+
+   private:
+    void GenTarget();
+
+    CloudPtr target_ = nullptr, source_ = nullptr;
+    Options options_;
+    SE3 gt_pose_;  // 真值，从target转到source
+};
+}  // namespace sad
+
+#endif  // SLAM_IN_AUTO_DRIVING_GEN_SIMU_DATA_H

workspace/slam_in_autonomous_driving/src/ch7/icp_3d.cc

@@ -0,0 +1,344 @@
+//
+// Created by xiang on 2022/7/7.
+//
+
+#include "icp_3d.h"
+#include "common/math_utils.h"
+
+#include <execution>
+
+namespace sad {
+
+bool Icp3d::AlignP2P(SE3& init_pose) {
+    LOG(INFO) << "aligning with point to point";
+    assert(target_ != nullptr && source_ != nullptr);
+
+    SE3 pose = init_pose;
+    if (!options_.use_initial_translation_) {
+        pose.translation() = target_center_ - source_center_;  // 设置平移初始值
+    }
+
+    // 对点的索引，预先生成
+    std::vector<int> index(source_->points.size());
+    for (int i = 0; i < index.size(); ++i) {
+        index[i] = i;
+    }
+
+    // 我们来写一些并发代码
+    std::vector<bool> effect_pts(index.size(), false);
+    std::vector<Eigen::Matrix<double, 3, 6>> jacobians(index.size());
+    std::vector<Vec3d> errors(index.size());
+
+    for (int iter = 0; iter < options_.max_iteration_; ++iter) {
+        // gauss-newton 迭代
+        // 最近邻，可以并发
+        std::for_each(std::execution::par_unseq, index.begin(), index.end(), [&](int idx) {
+            auto q = ToVec3d(source_->points[idx]);
+            Vec3d qs = pose * q;  // 转换之后的q
+            std::vector<int> nn;
+            kdtree_->GetClosestPoint(ToPointType(qs), nn, 1);
+
+            if (!nn.empty()) {
+                Vec3d p = ToVec3d(target_->points[nn[0]]);
+                double dis2 = (p - qs).squaredNorm();
+                if (dis2 > options_.max_nn_distance_) {
+                    // 点离的太远了不要
+                    effect_pts[idx] = false;
+                    return;
+                }
+
+                effect_pts[idx] = true;
+
+                // build residual
+                Vec3d e = p - qs;
+                Eigen::Matrix<double, 3, 6> J;
+                J.block<3, 3>(0, 0) = pose.so3().matrix() * SO3::hat(q);
+                J.block<3, 3>(0, 3) = -Mat3d::Identity();
+
+                jacobians[idx] = J;
+                errors[idx] = e;
+            } else {
+                effect_pts[idx] = false;
+            }
+        });
+
+        // 累加Hessian和error,计算dx
+        // 原则上可以用reduce并发，写起来比较麻烦，这里写成accumulate
+        double total_res = 0;
+        int effective_num = 0;
+        auto H_and_err = std::accumulate(
+            index.begin(), index.end(), std::pair<Mat6d, Vec6d>(Mat6d::Zero(), Vec6d::Zero()),
+            [&jacobians, &errors, &effect_pts, &total_res, &effective_num](const std::pair<Mat6d, Vec6d>& pre,
+                                                                           int idx) -> std::pair<Mat6d, Vec6d> {
+                if (!effect_pts[idx]) {
+                    return pre;
+                } else {
+                    total_res += errors[idx].dot(errors[idx]);
+                    effective_num++;
+                    return std::pair<Mat6d, Vec6d>(pre.first + jacobians[idx].transpose() * jacobians[idx],
+                                                   pre.second - jacobians[idx].transpose() * errors[idx]);
+                }
+            });
+
+        if (effective_num < options_.min_effective_pts_) {
+            LOG(WARNING) << "effective num too small: " << effective_num;
+            return false;
+        }
+
+        Mat6d H = H_and_err.first;
+        Vec6d err = H_and_err.second;
+
+        Vec6d dx = H.inverse() * err;
+        pose.so3() = pose.so3() * SO3::exp(dx.head<3>());
+        pose.translation() += dx.tail<3>();
+
+        // 更新
+        LOG(INFO) << "iter " << iter << " total res: " << total_res << ", eff: " << effective_num
+                  << ", mean res: " << total_res / effective_num << ", dxn: " << dx.norm();
+
+        if (gt_set_) {
+            double pose_error = (gt_pose_.inverse() * pose).log().norm();
+            LOG(INFO) << "iter " << iter << " pose error: " << pose_error;
+        }
+
+        if (dx.norm() < options_.eps_) {
+            LOG(INFO) << "converged, dx = " << dx.transpose();
+            break;
+        }
+    }
+
+    init_pose = pose;
+    return true;
+}
+
+bool Icp3d::AlignP2Plane(SE3& init_pose) {
+    LOG(INFO) << "aligning with point to plane";
+    assert(target_ != nullptr && source_ != nullptr);
+    // 整体流程与p2p一致，读者请关注变化部分
+
+    SE3 pose = init_pose;
+    if (!options_.use_initial_translation_) {
+        pose.translation() = target_center_ - source_center_;  // 设置平移初始值
+    }
+
+    std::vector<int> index(source_->points.size());
+    for (int i = 0; i < index.size(); ++i) {
+        index[i] = i;
+    }
+
+    std::vector<bool> effect_pts(index.size(), false);
+    std::vector<Eigen::Matrix<double, 1, 6>> jacobians(index.size());
+    std::vector<double> errors(index.size());
+
+    for (int iter = 0; iter < options_.max_iteration_; ++iter) {
+        // gauss-newton 迭代
+        // 最近邻，可以并发
+        std::for_each(std::execution::par_unseq, index.begin(), index.end(), [&](int idx) {
+            auto q = ToVec3d(source_->points[idx]);
+            Vec3d qs = pose * q;  // 转换之后的q
+            std::vector<int> nn;
+            kdtree_->GetClosestPoint(ToPointType(qs), nn, 5);  // 这里取5个最近邻
+            if (nn.size() > 3) {
+                // convert to eigen
+                std::vector<Vec3d> nn_eigen;
+                for (int i = 0; i < nn.size(); ++i) {
+                    nn_eigen.emplace_back(ToVec3d(target_->points[nn[i]]));
+                }
+
+                Vec4d n;
+                if (!math::FitPlane(nn_eigen, n)) {
+                    // 失败的不要
+                    effect_pts[idx] = false;
+                    return;
+                }
+
+                double dis = n.head<3>().dot(qs) + n[3];
+                if (fabs(dis) > options_.max_plane_distance_) {
+                    // 点离的太远了不要
+                    effect_pts[idx] = false;
+                    return;
+                }
+
+                effect_pts[idx] = true;
+
+                // build residual
+                Eigen::Matrix<double, 1, 6> J;
+                J.block<1, 3>(0, 0) = -n.head<3>().transpose() * pose.so3().matrix() * SO3::hat(q);
+                J.block<1, 3>(0, 3) = n.head<3>().transpose();
+
+                jacobians[idx] = J;
+                errors[idx] = dis;
+            } else {
+                effect_pts[idx] = false;
+            }
+        });
+
+        // 累加Hessian和error,计算dx
+        // 原则上可以用reduce并发，写起来比较麻烦，这里写成accumulate
+        double total_res = 0;
+        int effective_num = 0;
+        auto H_and_err = std::accumulate(
+            index.begin(), index.end(), std::pair<Mat6d, Vec6d>(Mat6d::Zero(), Vec6d::Zero()),
+            [&jacobians, &errors, &effect_pts, &total_res, &effective_num](const std::pair<Mat6d, Vec6d>& pre,
+                                                                           int idx) -> std::pair<Mat6d, Vec6d> {
+                if (!effect_pts[idx]) {
+                    return pre;
+                } else {
+                    total_res += errors[idx] * errors[idx];
+                    effective_num++;
+                    return std::pair<Mat6d, Vec6d>(pre.first + jacobians[idx].transpose() * jacobians[idx],
+                                                   pre.second - jacobians[idx].transpose() * errors[idx]);
+                }
+            });
+
+        if (effective_num < options_.min_effective_pts_) {
+            LOG(WARNING) << "effective num too small: " << effective_num;
+            return false;
+        }
+
+        Mat6d H = H_and_err.first;
+        Vec6d err = H_and_err.second;
+
+        Vec6d dx = H.inverse() * err;
+        pose.so3() = pose.so3() * SO3::exp(dx.head<3>());
+        pose.translation() += dx.tail<3>();
+
+        // 更新
+        LOG(INFO) << "iter " << iter << " total res: " << total_res << ", eff: " << effective_num
+                  << ", mean res: " << total_res / effective_num << ", dxn: " << dx.norm();
+
+        if (gt_set_) {
+            double pose_error = (gt_pose_.inverse() * pose).log().norm();
+            LOG(INFO) << "iter " << iter << " pose error: " << pose_error;
+        }
+
+        if (dx.norm() < options_.eps_) {
+            LOG(INFO) << "converged, dx = " << dx.transpose();
+            break;
+        }
+    }
+
+    init_pose = pose;
+    return true;
+}
+
+void Icp3d::BuildTargetKdTree() {
+    kdtree_ = std::make_shared<KdTree>();
+    kdtree_->BuildTree(target_);
+    kdtree_->SetEnableANN();
+}
+
+bool Icp3d::AlignP2Line(SE3& init_pose) {
+    LOG(INFO) << "aligning with point to line";
+    assert(target_ != nullptr && source_ != nullptr);
+    // 点线与点面基本是完全一样的
+
+    SE3 pose = init_pose;
+    if (options_.use_initial_translation_) {
+        pose.translation() = target_center_ - source_center_;  // 设置平移初始值
+        LOG(INFO) << "init trans set to " << pose.translation().transpose();
+    }
+
+    std::vector<int> index(source_->points.size());
+    for (int i = 0; i < index.size(); ++i) {
+        index[i] = i;
+    }
+
+    std::vector<bool> effect_pts(index.size(), false);
+    std::vector<Eigen::Matrix<double, 3, 6>> jacobians(index.size());
+    std::vector<Vec3d> errors(index.size());
+
+    for (int iter = 0; iter < options_.max_iteration_; ++iter) {
+        // gauss-newton 迭代
+        // 最近邻，可以并发
+        std::for_each(std::execution::par_unseq, index.begin(), index.end(), [&](int idx) {
+            auto q = ToVec3d(source_->points[idx]);
+            Vec3d qs = pose * q;  // 转换之后的q
+            std::vector<int> nn;
+            kdtree_->GetClosestPoint(ToPointType(qs), nn, 5);  // 这里取5个最近邻
+            if (nn.size() == 5) {
+                // convert to eigen
+                std::vector<Vec3d> nn_eigen;
+                for (int i = 0; i < 5; ++i) {
+                    nn_eigen.emplace_back(ToVec3d(target_->points[nn[i]]));
+                }
+
+                Vec3d d, p0;
+                if (!math::FitLine(nn_eigen, p0, d, options_.max_line_distance_)) {
+                    // 失败的不要
+                    effect_pts[idx] = false;
+                    return;
+                }
+
+                Vec3d err = SO3::hat(d) * (qs - p0);
+
+                if (err.norm() > options_.max_line_distance_) {
+                    // 点离的太远了不要
+                    effect_pts[idx] = false;
+                    return;
+                }
+
+                effect_pts[idx] = true;
+
+                // build residual
+                Eigen::Matrix<double, 3, 6> J;
+                J.block<3, 3>(0, 0) = -SO3::hat(d) * pose.so3().matrix() * SO3::hat(q);
+                J.block<3, 3>(0, 3) = SO3::hat(d);
+
+                jacobians[idx] = J;
+                errors[idx] = err;
+            } else {
+                effect_pts[idx] = false;
+            }
+        });
+
+        // 累加Hessian和error,计算dx
+        // 原则上可以用reduce并发，写起来比较麻烦，这里写成accumulate
+        double total_res = 0;
+        int effective_num = 0;
+        auto H_and_err = std::accumulate(
+            index.begin(), index.end(), std::pair<Mat6d, Vec6d>(Mat6d::Zero(), Vec6d::Zero()),
+            [&jacobians, &errors, &effect_pts, &total_res, &effective_num](const std::pair<Mat6d, Vec6d>& pre,
+                                                                           int idx) -> std::pair<Mat6d, Vec6d> {
+                if (!effect_pts[idx]) {
+                    return pre;
+                } else {
+                    total_res += errors[idx].dot(errors[idx]);
+                    effective_num++;
+                    return std::pair<Mat6d, Vec6d>(pre.first + jacobians[idx].transpose() * jacobians[idx],
+                                                   pre.second - jacobians[idx].transpose() * errors[idx]);
+                }
+            });
+
+        if (effective_num < options_.min_effective_pts_) {
+            LOG(WARNING) << "effective num too small: " << effective_num;
+            return false;
+        }
+
+        Mat6d H = H_and_err.first;
+        Vec6d err = H_and_err.second;
+
+        Vec6d dx = H.inverse() * err;
+        pose.so3() = pose.so3() * SO3::exp(dx.head<3>());
+        pose.translation() += dx.tail<3>();
+
+        if (gt_set_) {
+            double pose_error = (gt_pose_.inverse() * pose).log().norm();
+            LOG(INFO) << "iter " << iter << " pose error: " << pose_error;
+        }
+
+        // 更新
+        LOG(INFO) << "iter " << iter << " total res: " << total_res << ", eff: " << effective_num
+                  << ", mean res: " << total_res / effective_num << ", dxn: " << dx.norm();
+
+        if (dx.norm() < options_.eps_) {
+            LOG(INFO) << "converged, dx = " << dx.transpose();
+            break;
+        }
+    }
+
+    init_pose = pose;
+    return true;
+}
+
+}  // namespace sad

workspace/slam_in_autonomous_driving/src/ch7/icp_3d.h

@@ -0,0 +1,91 @@
+//
+// Created by xiang on 2022/7/7.
+//
+
+#ifndef SLAM_IN_AUTO_DRIVING_ICP_3D_H
+#define SLAM_IN_AUTO_DRIVING_ICP_3D_H
+
+#include "ch5/kdtree.h"
+
+namespace sad {
+
+/**
+ * 3D 形式的ICP
+ * 先SetTarget, 再SetSource, 然后调用Align方法获取位姿
+ *
+ * ICP 求解R,t 将source点云配准到target点云上
+ * 如果 p 是source点云中的点，那么R*p+t就得到target中的配对点
+ *
+ * 使用第5章的Kd树来求取最近邻
+ */
+class Icp3d {
+   public:
+    struct Options {
+        int max_iteration_ = 20;                // 最大迭代次数
+        double max_nn_distance_ = 1.0;          // 点到点最近邻查找时阈值
+        double max_plane_distance_ = 0.05;      // 平面最近邻查找时阈值
+        double max_line_distance_ = 0.5;        // 点线最近邻查找时阈值
+        int min_effective_pts_ = 10;            // 最近邻点数阈值
+        double eps_ = 1e-2;                     // 收敛判定条件
+        bool use_initial_translation_ = false;  // 是否使用初始位姿中的平移估计
+    };
+
+    Icp3d() {}
+    Icp3d(Options options) : options_(options) {}
+
+    /// 设置目标的Scan
+    void SetTarget(CloudPtr target) {
+        target_ = target;
+        BuildTargetKdTree();
+
+        // 计算点云中心
+        target_center_ = std::accumulate(target->points.begin(), target_->points.end(), Vec3d::Zero().eval(),
+                                         [](const Vec3d& c, const PointType& pt) -> Vec3d { return c + ToVec3d(pt); }) /
+                         target_->size();
+        LOG(INFO) << "target center: " << target_center_.transpose();
+    }
+
+    /// 设置被配准的Scan
+    void SetSource(CloudPtr source) {
+        source_ = source;
+        source_center_ = std::accumulate(source_->points.begin(), source_->points.end(), Vec3d::Zero().eval(),
+                                         [](const Vec3d& c, const PointType& pt) -> Vec3d { return c + ToVec3d(pt); }) /
+                         source_->size();
+        LOG(INFO) << "source center: " << source_center_.transpose();
+    }
+
+    void SetGroundTruth(const SE3& gt_pose) {
+        gt_pose_ = gt_pose;
+        gt_set_ = true;
+    }
+
+    /// 使用gauss-newton方法进行配准, 点到点
+    bool AlignP2P(SE3& init_pose);
+
+    /// 基于gauss-newton的点线ICP
+    bool AlignP2Line(SE3& init_pose);
+
+    /// 基于gauss-newton的点面ICP
+    bool AlignP2Plane(SE3& init_pose);
+
+   private:
+    // 建立目标点云的Kdtree
+    void BuildTargetKdTree();
+
+    std::shared_ptr<KdTree> kdtree_ = nullptr;  // 第5章的kd树
+
+    CloudPtr target_ = nullptr;
+    CloudPtr source_ = nullptr;
+
+    Vec3d target_center_ = Vec3d::Zero();
+    Vec3d source_center_ = Vec3d::Zero();
+
+    bool gt_set_ = false;  // 真值是否设置
+    SE3 gt_pose_;
+
+    Options options_;
+};
+
+}  // namespace sad
+
+#endif  // SLAM_IN_AUTO_DRIVING_ICP_3D_H

workspace/slam_in_autonomous_driving/src/ch7/incremental_ndt_lo.cc

@@ -0,0 +1,74 @@
+//
+// Created by xiang on 2022/7/20.
+//
+
+#include "ch7/incremental_ndt_lo.h"
+#include "common/math_utils.h"
+#include "common/timer/timer.h"
+
+namespace sad {
+
+void IncrementalNDTLO::AddCloud(CloudPtr scan, SE3& pose, bool use_guess) {
+    if (first_frame_) {
+        // 第一个帧，直接加入local map
+        pose = SE3();
+        last_kf_pose_ = pose;
+        ndt_.AddCloud(scan);
+        first_frame_ = false;
+        return;
+    }
+
+    // 此时local map位于NDT内部，直接配准即可
+    SE3 guess;
+    ndt_.SetSource(scan);
+    if (estimated_poses_.size() < 2) {
+        ndt_.AlignNdt(guess);
+    } else {
+        if (!use_guess) {
+            // 从最近两个pose来推断
+            SE3 T1 = estimated_poses_[estimated_poses_.size() - 1];
+            SE3 T2 = estimated_poses_[estimated_poses_.size() - 2];
+            guess = T1 * (T2.inverse() * T1);
+        } else {
+            guess = pose;
+        }
+
+        ndt_.AlignNdt(guess);
+    }
+
+    pose = guess;
+    estimated_poses_.emplace_back(pose);
+
+    CloudPtr scan_world(new PointCloudType);
+    pcl::transformPointCloud(*scan, *scan_world, guess.matrix().cast<float>());
+
+    if (IsKeyframe(pose)) {
+        last_kf_pose_ = pose;
+        cnt_frame_ = 0;
+        // 放入ndt内部的local map
+        ndt_.AddCloud(scan_world);
+    }
+
+    if (viewer_ != nullptr) {
+        viewer_->SetPoseAndCloud(pose, scan_world);
+    }
+    cnt_frame_++;
+}
+
+bool IncrementalNDTLO::IsKeyframe(const SE3& current_pose) {
+    if (cnt_frame_ > 10) {
+        return true;
+    }
+
+    SE3 delta = last_kf_pose_.inverse() * current_pose;
+    return delta.translation().norm() > options_.kf_distance_ ||
+           delta.so3().log().norm() > options_.kf_angle_deg_ * math::kDEG2RAD;
+}
+
+void IncrementalNDTLO::SaveMap(const std::string& map_path) {
+    if (viewer_) {
+        viewer_->SaveMap(map_path);
+    }
+}
+
+}  // namespace sad

workspace/slam_in_autonomous_driving/src/ch7/incremental_ndt_lo.h

@@ -0,0 +1,64 @@
+//
+// Created by xiang on 2022/7/20.
+//
+
+#ifndef SLAM_IN_AUTO_DRIVING_INCREMENTAL_NDT_LO_H
+#define SLAM_IN_AUTO_DRIVING_INCREMENTAL_NDT_LO_H
+
+#include "ch7/ndt_inc.h"
+#include "common/eigen_types.h"
+#include "common/point_types.h"
+#include "tools/pcl_map_viewer.h"
+
+namespace sad {
+
+/**
+ * 使用直接NDT方法进行递推的Lidar Odometry
+ * 使用历史几个关键帧作为local map，进行NDT定位
+ */
+class IncrementalNDTLO {
+   public:
+    struct Options {
+        Options() {}
+        double kf_distance_ = 0.5;            // 关键帧距离
+        double kf_angle_deg_ = 30;            // 旋转角度
+        bool display_realtime_cloud_ = true;  // 是否显示实时点云
+        IncNdt3d::Options ndt3d_options_;     // NDT3D 的配置
+    };
+
+    IncrementalNDTLO(Options options = Options()) : options_(options) {
+        if (options_.display_realtime_cloud_) {
+            viewer_ = std::make_shared<PCLMapViewer>(0.5);
+        }
+
+        ndt_ = IncNdt3d(options_.ndt3d_options_);
+    }
+
+    /**
+     * 往LO中增加一个点云
+     * @param scan  当前帧点云
+     * @param pose 估计pose
+     */
+    void AddCloud(CloudPtr scan, SE3& pose, bool use_guess = false);
+
+    /// 存储地图(viewer里）
+    void SaveMap(const std::string& map_path);
+
+   private:
+    /// 判定是否为关键帧
+    bool IsKeyframe(const SE3& current_pose);
+
+   private:
+    Options options_;
+    bool first_frame_ = true;
+    std::vector<SE3> estimated_poses_;  // 所有估计出来的pose，用于记录轨迹和预测下一个帧
+    SE3 last_kf_pose_;                  // 上一关键帧的位姿
+    int cnt_frame_ = 0;
+
+    IncNdt3d ndt_;
+    std::shared_ptr<PCLMapViewer> viewer_ = nullptr;
+};
+
+}  // namespace sad
+
+#endif  // SLAM_IN_AUTO_DRIVING_INCREMENTAL_NDT_LO_H

workspace/slam_in_autonomous_driving/src/ch7/loam-like/feature_extraction.cc

@@ -0,0 +1,134 @@
+//
+// Created by xiang on 2022/7/21.
+//
+#include "ch7/loam-like/feature_extraction.h"
+#include <glog/logging.h>
+
+namespace sad {
+
+void FeatureExtraction::Extract(FullCloudPtr pc_in, CloudPtr pc_out_edge, CloudPtr pc_out_surf) {
+    int num_scans = 16;
+    std::vector<CloudPtr> scans_in_each_line;  // 分线数的点云
+    for (int i = 0; i < num_scans; i++) {
+        scans_in_each_line.emplace_back(new PointCloudType);
+    }
+
+    for (auto &pt : pc_in->points) {
+        assert(pt.ring >= 0 && pt.ring < num_scans);
+        PointType p;
+        p.x = pt.x;
+        p.y = pt.y;
+        p.z = pt.z;
+        p.intensity = pt.intensity;
+
+        scans_in_each_line[pt.ring]->points.emplace_back(p);
+    }
+
+    // 处理曲率
+    for (int i = 0; i < num_scans; i++) {
+        if (scans_in_each_line[i]->points.size() < 131) {
+            continue;
+        }
+
+        std::vector<IdAndValue> cloud_curvature;  // 每条线对应的曲率
+        int total_points = scans_in_each_line[i]->points.size() - 10;
+        for (int j = 5; j < (int)scans_in_each_line[i]->points.size() - 5; j++) {
+            // 两头留一定余量，采样周围10个点取平均值
+            double diffX = scans_in_each_line[i]->points[j - 5].x + scans_in_each_line[i]->points[j - 4].x +
+                           scans_in_each_line[i]->points[j - 3].x + scans_in_each_line[i]->points[j - 2].x +
+                           scans_in_each_line[i]->points[j - 1].x - 10 * scans_in_each_line[i]->points[j].x +
+                           scans_in_each_line[i]->points[j + 1].x + scans_in_each_line[i]->points[j + 2].x +
+                           scans_in_each_line[i]->points[j + 3].x + scans_in_each_line[i]->points[j + 4].x +
+                           scans_in_each_line[i]->points[j + 5].x;
+            double diffY = scans_in_each_line[i]->points[j - 5].y + scans_in_each_line[i]->points[j - 4].y +
+                           scans_in_each_line[i]->points[j - 3].y + scans_in_each_line[i]->points[j - 2].y +
+                           scans_in_each_line[i]->points[j - 1].y - 10 * scans_in_each_line[i]->points[j].y +
+                           scans_in_each_line[i]->points[j + 1].y + scans_in_each_line[i]->points[j + 2].y +
+                           scans_in_each_line[i]->points[j + 3].y + scans_in_each_line[i]->points[j + 4].y +
+                           scans_in_each_line[i]->points[j + 5].y;
+            double diffZ = scans_in_each_line[i]->points[j - 5].z + scans_in_each_line[i]->points[j - 4].z +
+                           scans_in_each_line[i]->points[j - 3].z + scans_in_each_line[i]->points[j - 2].z +
+                           scans_in_each_line[i]->points[j - 1].z - 10 * scans_in_each_line[i]->points[j].z +
+                           scans_in_each_line[i]->points[j + 1].z + scans_in_each_line[i]->points[j + 2].z +
+                           scans_in_each_line[i]->points[j + 3].z + scans_in_each_line[i]->points[j + 4].z +
+                           scans_in_each_line[i]->points[j + 5].z;
+            IdAndValue distance(j, diffX * diffX + diffY * diffY + diffZ * diffZ);
+            cloud_curvature.push_back(distance);
+        }
+
+        // 对每个区间选取特征，把360度分为6个区间
+        for (int j = 0; j < 6; j++) {
+            int sector_length = (int)(total_points / 6);
+            int sector_start = sector_length * j;
+            int sector_end = sector_length * (j + 1) - 1;
+            if (j == 5) {
+                sector_end = total_points - 1;
+            }
+
+            std::vector<IdAndValue> sub_cloud_curvature(cloud_curvature.begin() + sector_start,
+                                                        cloud_curvature.begin() + sector_end);
+
+            ExtractFromSector(scans_in_each_line[i], sub_cloud_curvature, pc_out_edge, pc_out_surf);
+        }
+    }
+}
+
+void FeatureExtraction::ExtractFromSector(const CloudPtr &pc_in, std::vector<IdAndValue> &cloud_curvature,
+                                          CloudPtr &pc_out_edge, CloudPtr &pc_out_surf) {
+    // 按曲率排序
+    std::sort(cloud_curvature.begin(), cloud_curvature.end(),
+              [](const IdAndValue &a, const IdAndValue &b) { return a.value_ < b.value_; });
+
+    int largest_picked_num = 0;
+    int point_info_count = 0;
+
+    /// 按照曲率最大的开始搜，选取曲率最大的角点
+    std::vector<int> picked_points;  // 标记被选中的角点，角点附近的点都不会被选取
+    for (int i = cloud_curvature.size() - 1; i >= 0; i--) {
+        int ind = cloud_curvature[i].id_;
+        if (std::find(picked_points.begin(), picked_points.end(), ind) == picked_points.end()) {
+            if (cloud_curvature[i].value_ <= 0.1) {
+                break;
+            }
+
+            largest_picked_num++;
+            picked_points.push_back(ind);
+
+            if (largest_picked_num <= 20) {
+                pc_out_edge->push_back(pc_in->points[ind]);
+                point_info_count++;
+            } else {
+                break;
+            }
+
+            for (int k = 1; k <= 5; k++) {
+                double diffX = pc_in->points[ind + k].x - pc_in->points[ind + k - 1].x;
+                double diffY = pc_in->points[ind + k].y - pc_in->points[ind + k - 1].y;
+                double diffZ = pc_in->points[ind + k].z - pc_in->points[ind + k - 1].z;
+                if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) {
+                    break;
+                }
+                picked_points.push_back(ind + k);
+            }
+            for (int k = -1; k >= -5; k--) {
+                double diffX = pc_in->points[ind + k].x - pc_in->points[ind + k + 1].x;
+                double diffY = pc_in->points[ind + k].y - pc_in->points[ind + k + 1].y;
+                double diffZ = pc_in->points[ind + k].z - pc_in->points[ind + k + 1].z;
+                if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) {
+                    break;
+                }
+                picked_points.push_back(ind + k);
+            }
+        }
+    }
+
+    /// 选取曲率较小的平面点
+    for (int i = 0; i <= (int)cloud_curvature.size() - 1; i++) {
+        int ind = cloud_curvature[i].id_;
+        if (std::find(picked_points.begin(), picked_points.end(), ind) == picked_points.end()) {
+            pc_out_surf->push_back(pc_in->points[ind]);
+        }
+    }
+}
+
+}  // namespace sad

workspace/slam_in_autonomous_driving/src/ch7/loam-like/feature_extraction.h

@@ -0,0 +1,51 @@
+//
+// Created by xiang on 2022/7/21.
+//
+
+#ifndef SLAM_IN_AUTO_DRIVING_FEATURE_EXTRACTION_H
+#define SLAM_IN_AUTO_DRIVING_FEATURE_EXTRACTION_H
+
+#include "common/point_types.h"
+
+namespace sad {
+
+/**
+ * 根据线束信息来提取特征
+ * 需要知道雷达的线数分布，目前只支持velodyne的雷达
+ */
+class FeatureExtraction {
+    /// 一个线ID+曲率的结构
+    struct IdAndValue {
+        IdAndValue() {}
+        IdAndValue(int id, double value) : id_(id), value_(value) {}
+        int id_ = 0;
+        double value_ = 0;  // 曲率
+    };
+
+   public:
+    FeatureExtraction() {}
+
+    /**
+     * 提取角点和平面点
+     * @param pc_in         输入点云（全量信息）
+     * @param pc_out_edge   输出角点的点云
+     * @param pc_out_surf   输出平面的点云
+     */
+    void Extract(FullCloudPtr pc_in, CloudPtr pc_out_edge, CloudPtr pc_out_surf);
+
+    /**
+     * 对单独一段区域提取角点和面点
+     * @param pc_in
+     * @param cloud_curvature
+     * @param pc_out_edge
+     * @param pc_out_surf
+     */
+    void ExtractFromSector(const CloudPtr& pc_in, std::vector<IdAndValue>& cloud_curvature, CloudPtr& pc_out_edge,
+                           CloudPtr& pc_out_surf);
+
+   private:
+};
+
+}  // namespace sad
+
+#endif  // SLAM_IN_AUTO_DRIVING_FEATURE_EXTRACTION_H

workspace/slam_in_autonomous_driving/src/ch7/loam-like/loam_like_odom.cc

@@ -0,0 +1,285 @@
+//
+// Created by xiang on 2022/7/25.
+//
+
+#include "ch7/loam-like/loam_like_odom.h"
+#include "ch7/loam-like/feature_extraction.h"
+#include "common/lidar_utils.h"
+#include "common/math_utils.h"
+#include "common/point_cloud_utils.h"
+
+#include <execution>
+
+namespace sad {
+
+LoamLikeOdom::LoamLikeOdom(LoamLikeOdom::Options options)
+    : options_(options), feature_extraction_(new FeatureExtraction), global_map_(new PointCloudType()) {
+    if (options_.display_realtime_cloud_) {
+        viewer_ = std::make_shared<PCLMapViewer>(0.1);
+    }
+
+    kdtree_edge_.SetEnableANN();
+    kdtree_surf_.SetEnableANN();
+}
+
+void LoamLikeOdom::ProcessPointCloud(FullCloudPtr cloud) {
+    LOG(INFO) << "processing frame " << cnt_frame_++;
+    // step 1. 提特征
+    CloudPtr current_edge(new PointCloudType), current_surf(new PointCloudType);
+    feature_extraction_->Extract(cloud, current_edge, current_surf);
+
+    if (current_edge->size() < options_.min_edge_pts_ || current_surf->size() < options_.min_surf_pts_) {
+        LOG(ERROR) << "not enough edge/surf pts: " << current_edge->size() << "," << current_surf->size();
+        return;
+    }
+
+    LOG(INFO) << "edge: " << current_edge->size() << ", surf: " << current_surf->size();
+
+    if (local_map_edge_ == nullptr || local_map_surf_ == nullptr) {
+        // 首帧特殊处理
+        local_map_edge_ = current_edge;
+        local_map_surf_ = current_surf;
+
+        kdtree_edge_.BuildTree(local_map_edge_);
+        kdtree_surf_.BuildTree(local_map_surf_);
+
+        edges_.emplace_back(current_edge);
+        surfs_.emplace_back(current_surf);
+        return;
+    }
+
+    /// 与局部地图配准
+    SE3 pose = AlignWithLocalMap(current_edge, current_surf);
+    CloudPtr scan_world(new PointCloudType);
+    pcl::transformPointCloud(*ConvertToCloud<FullPointType>(cloud), *scan_world, pose.matrix());
+
+    CloudPtr edge_world(new PointCloudType), surf_world(new PointCloudType);
+    pcl::transformPointCloud(*current_edge, *edge_world, pose.matrix());
+    pcl::transformPointCloud(*current_surf, *surf_world, pose.matrix());
+
+    if (IsKeyframe(pose)) {
+        LOG(INFO) << "inserting keyframe";
+        last_kf_pose_ = pose;
+        last_kf_id_ = cnt_frame_;
+
+        // 重建local map
+        edges_.emplace_back(edge_world);
+        surfs_.emplace_back(surf_world);
+
+        if (edges_.size() > options_.num_kfs_in_local_map_) {
+            edges_.pop_front();
+        }
+        if (surfs_.size() > options_.num_kfs_in_local_map_) {
+            surfs_.pop_front();
+        }
+
+        local_map_surf_.reset(new PointCloudType);
+        local_map_edge_.reset(new PointCloudType);
+
+        for (auto& s : edges_) {
+            *local_map_edge_ += *s;
+        }
+        for (auto& s : surfs_) {
+            *local_map_surf_ += *s;
+        }
+
+        local_map_surf_ = VoxelCloud(local_map_surf_, 1.0);
+        local_map_edge_ = VoxelCloud(local_map_edge_, 1.0);
+
+        LOG(INFO) << "insert keyframe, surf pts: " << local_map_surf_->size()
+                  << ", edge pts: " << local_map_edge_->size();
+
+        kdtree_surf_.BuildTree(local_map_surf_);
+        kdtree_edge_.BuildTree(local_map_edge_);
+
+        *global_map_ += *scan_world;
+    }
+
+    LOG(INFO) << "current pose: " << pose.translation().transpose() << ", "
+              << pose.so3().unit_quaternion().coeffs().transpose();
+
+    if (viewer_ != nullptr) {
+        viewer_->SetPoseAndCloud(pose, scan_world);
+    }
+}
+
+bool LoamLikeOdom::IsKeyframe(const SE3& current_pose) {
+    if ((cnt_frame_ - last_kf_id_) > 30) {
+        return true;
+    }
+
+    // 只要与上一帧相对运动超过一定距离或角度，就记关键帧
+    SE3 delta = last_kf_pose_.inverse() * current_pose;
+    return delta.translation().norm() > options_.kf_distance_ ||
+           delta.so3().log().norm() > options_.kf_angle_deg_ * math::kDEG2RAD;
+}
+
+SE3 LoamLikeOdom::AlignWithLocalMap(CloudPtr edge, CloudPtr surf) {
+    // 这部分的ICP需要自己写
+    SE3 pose;
+    if (estimated_poses_.size() >= 2) {
+        // 从最近两个pose来推断
+        SE3 T1 = estimated_poses_[estimated_poses_.size() - 1];
+        SE3 T2 = estimated_poses_[estimated_poses_.size() - 2];
+        pose = T1 * (T2.inverse() * T1);
+    }
+
+    int edge_size = edge->size();
+    int surf_size = surf->size();
+
+    // 我们来写一些并发代码
+    for (int iter = 0; iter < options_.max_iteration_; ++iter) {
+        std::vector<bool> effect_surf(surf_size, false);
+        std::vector<Eigen::Matrix<double, 1, 6>> jacob_surf(surf_size);  // 点面的残差是1维的
+        std::vector<double> errors_surf(surf_size);
+
+        std::vector<bool> effect_edge(edge_size, false);
+        std::vector<Eigen::Matrix<double, 3, 6>> jacob_edge(edge_size);  // 点线的残差是3维的
+        std::vector<Vec3d> errors_edge(edge_size);
+
+        std::vector<int> index_surf(surf_size);
+        std::iota(index_surf.begin(), index_surf.end(), 0);  // 填入
+        std::vector<int> index_edge(edge_size);
+        std::iota(index_edge.begin(), index_edge.end(), 0);  // 填入
+
+        // gauss-newton 迭代
+        // 最近邻，角点部分
+        if (options_.use_edge_points_) {
+            std::for_each(std::execution::par_unseq, index_edge.begin(), index_edge.end(), [&](int idx) {
+                Vec3d q = ToVec3d(edge->points[idx]);
+                Vec3d qs = pose * q;
+
+                // 检查最近邻
+                std::vector<int> nn_indices;
+
+                kdtree_edge_.GetClosestPoint(ToPointType(qs), nn_indices, 5);
+                effect_edge[idx] = false;
+
+                if (nn_indices.size() >= 3) {
+                    std::vector<Vec3d> nn_eigen;
+                    for (auto& n : nn_indices) {
+                        nn_eigen.emplace_back(ToVec3d(local_map_edge_->points[n]));
+                    }
+
+                    // point to point residual
+                    Vec3d d, p0;
+                    if (!math::FitLine(nn_eigen, p0, d, options_.max_line_distance_)) {
+                        return;
+                    }
+
+                    Vec3d err = SO3::hat(d) * (qs - p0);
+                    if (err.norm() > options_.max_line_distance_) {
+                        return;
+                    }
+
+                    effect_edge[idx] = true;
+
+                    // build residual
+                    Eigen::Matrix<double, 3, 6> J;
+                    J.block<3, 3>(0, 0) = -SO3::hat(d) * pose.so3().matrix() * SO3::hat(q);
+                    J.block<3, 3>(0, 3) = SO3::hat(d);
+
+                    jacob_edge[idx] = J;
+                    errors_edge[idx] = err;
+                }
+            });
+        }
+
+        /// 最近邻，平面点部分
+        if (options_.use_surf_points_) {
+            std::for_each(std::execution::par_unseq, index_surf.begin(), index_surf.end(), [&](int idx) {
+                Vec3d q = ToVec3d(surf->points[idx]);
+                Vec3d qs = pose * q;
+
+                // 检查最近邻
+                std::vector<int> nn_indices;
+
+                kdtree_surf_.GetClosestPoint(ToPointType(qs), nn_indices, 5);
+                effect_surf[idx] = false;
+
+                if (nn_indices.size() == 5) {
+                    std::vector<Vec3d> nn_eigen;
+                    for (auto& n : nn_indices) {
+                        nn_eigen.emplace_back(ToVec3d(local_map_surf_->points[n]));
+                    }
+
+                    // 点面残差
+                    Vec4d n;
+                    if (!math::FitPlane(nn_eigen, n)) {
+                        return;
+                    }
+
+                    double dis = n.head<3>().dot(qs) + n[3];
+                    if (fabs(dis) > options_.max_plane_distance_) {
+                        return;
+                    }
+
+                    effect_surf[idx] = true;
+
+                    // build residual
+                    Eigen::Matrix<double, 1, 6> J;
+                    J.block<1, 3>(0, 0) = -n.head<3>().transpose() * pose.so3().matrix() * SO3::hat(q);
+                    J.block<1, 3>(0, 3) = n.head<3>().transpose();
+
+                    jacob_surf[idx] = J;
+                    errors_surf[idx] = dis;
+                }
+            });
+        }
+
+        // 累加Hessian和error,计算dx
+        // 原则上可以用reduce并发，写起来比较麻烦，这里写成accumulate
+        double total_res = 0;
+        int effective_num = 0;
+
+        Mat6d H = Mat6d::Zero();
+        Vec6d err = Vec6d::Zero();
+
+        for (const auto& idx : index_surf) {
+            if (effect_surf[idx]) {
+                H += jacob_surf[idx].transpose() * jacob_surf[idx];
+                err += -jacob_surf[idx].transpose() * errors_surf[idx];
+                effective_num++;
+                total_res += errors_surf[idx] * errors_surf[idx];
+            }
+        }
+
+        for (const auto& idx : index_edge) {
+            if (effect_edge[idx]) {
+                H += jacob_edge[idx].transpose() * jacob_edge[idx];
+                err += -jacob_edge[idx].transpose() * errors_edge[idx];
+                effective_num++;
+                total_res += errors_edge[idx].norm();
+            }
+        }
+
+        if (effective_num < options_.min_effective_pts_) {
+            LOG(WARNING) << "effective num too small: " << effective_num;
+            return pose;
+        }
+
+        Vec6d dx = H.inverse() * err;
+        pose.so3() = pose.so3() * SO3::exp(dx.head<3>());
+        pose.translation() += dx.tail<3>();
+
+        // 更新
+        LOG(INFO) << "iter " << iter << " total res: " << total_res << ", eff: " << effective_num
+                  << ", mean res: " << total_res / effective_num << ", dxn: " << dx.norm();
+
+        if (dx.norm() < options_.eps_) {
+            LOG(INFO) << "converged, dx = " << dx.transpose();
+            break;
+        }
+    }
+
+    estimated_poses_.emplace_back(pose);
+    return pose;
+}
+
+void LoamLikeOdom::SaveMap(const std::string& path) {
+    if (global_map_ && global_map_->empty() == false) {
+        sad::SaveCloudToFile(path, *global_map_);
+    }
+}
+
+}  // namespace sad

workspace/slam_in_autonomous_driving/src/ch7/loam-like/loam_like_odom.h

@@ -0,0 +1,84 @@
+//
+// Created by xiang on 2022/7/25.
+//
+
+#ifndef SLAM_IN_AUTO_DRIVING_LOAM_LIKE_ODOM_H
+#define SLAM_IN_AUTO_DRIVING_LOAM_LIKE_ODOM_H
+
+#include "ch5/kdtree.h"
+#include "ch7/icp_3d.h"
+#include "common/eigen_types.h"
+#include "common/point_types.h"
+#include "tools/pcl_map_viewer.h"
+
+#include <deque>
+
+namespace sad {
+class FeatureExtraction;
+
+/**
+ * 类loam的里程计方法
+ * 首先使用feature extraction提出一个点云中的边缘点和平面点
+ * 然后分别对于edge点和surface点，使用不同的ICP方法
+ */
+class LoamLikeOdom {
+   public:
+    struct Options {
+        Options() {}
+
+        int min_edge_pts_ = 20;               // 最小边缘点数
+        int min_surf_pts_ = 20;               // 最小平面点数
+        double kf_distance_ = 1.0;            // 关键帧距离
+        double kf_angle_deg_ = 15;            // 旋转角度
+        int num_kfs_in_local_map_ = 30;       // 局部地图含有多少个关键帧
+        bool display_realtime_cloud_ = true;  // 是否显示实时点云
+
+        // ICP 参数
+        int max_iteration_ = 5;             // 最大迭代次数
+        double max_plane_distance_ = 0.05;  // 平面最近邻查找时阈值
+        double max_line_distance_ = 0.5;    // 点线最近邻查找时阈值
+        int min_effective_pts_ = 10;        // 最近邻点数阈值
+        double eps_ = 1e-3;                 // 收敛判定条件
+
+        bool use_edge_points_ = true;  // 是否使用边缘点
+        bool use_surf_points_ = true;  // 是否使用平面点
+    };
+
+    explicit LoamLikeOdom(Options options = Options());
+
+    /**
+     * 往里程计中添加一个点云，内部会分为角点和平面点
+     * @param full_cloud
+     */
+    void ProcessPointCloud(FullCloudPtr full_cloud);
+
+    void SaveMap(const std::string& path);
+
+   private:
+    /// 与局部地图进行配准
+    SE3 AlignWithLocalMap(CloudPtr edge, CloudPtr surf);
+
+    /// 判定是否为关键帧
+    bool IsKeyframe(const SE3& current_pose);
+
+    Options options_;
+
+    int cnt_frame_ = 0;
+    int last_kf_id_ = 0;
+
+    CloudPtr local_map_edge_ = nullptr, local_map_surf_ = nullptr;  // 局部地图的local map
+    std::vector<SE3> estimated_poses_;    // 所有估计出来的pose，用于记录轨迹和预测下一个帧
+    SE3 last_kf_pose_;                    // 上一关键帧的位姿
+    std::deque<CloudPtr> edges_, surfs_;  // 缓存的角点和平面点
+
+    CloudPtr global_map_ = nullptr;  // 用于保存的全局地图
+
+    std::shared_ptr<FeatureExtraction> feature_extraction_ = nullptr;
+
+    std::shared_ptr<PCLMapViewer> viewer_ = nullptr;
+    KdTree kdtree_edge_, kdtree_surf_;
+};
+
+}  // namespace sad
+
+#endif  // SLAM_IN_AUTO_DRIVING_LOAM_LIKE_ODOM_H

workspace/slam_in_autonomous_driving/src/ch7/loosely_coupled_lio/cloud_convert.cc

@@ -0,0 +1,197 @@
+#include "cloud_convert.h"
+
+#include <glog/logging.h>
+#include <yaml-cpp/yaml.h>
+#include <execution>
+
+namespace sad {
+
+void CloudConvert::Process(const livox_ros_driver::CustomMsg::ConstPtr &msg, FullCloudPtr &pcl_out) {
+    AviaHandler(msg);
+    *pcl_out = cloud_out_;
+}
+
+void CloudConvert::Process(const sensor_msgs::PointCloud2::ConstPtr &msg, FullCloudPtr &pcl_out) {
+    switch (lidar_type_) {
+        case LidarType::OUST64:
+            Oust64Handler(msg);
+            break;
+
+        case LidarType::VELO32:
+            VelodyneHandler(msg);
+            break;
+
+        default:
+            LOG(ERROR) << "Error LiDAR Type: " << int(lidar_type_);
+            break;
+    }
+    *pcl_out = cloud_out_;
+}
+
+void CloudConvert::AviaHandler(const livox_ros_driver::CustomMsg::ConstPtr &msg) {
+    cloud_out_.clear();
+    cloud_full_.clear();
+    int plsize = msg->point_num;
+
+    cloud_out_.reserve(plsize);
+    cloud_full_.resize(plsize);
+
+    std::vector<bool> is_valid_pt(plsize, false);
+    std::vector<uint> index(plsize - 1);
+    for (uint i = 0; i < plsize - 1; ++i) {
+        index[i] = i + 1;  // 从1开始
+    }
+
+    std::for_each(std::execution::par_unseq, index.begin(), index.end(), [&](const uint &i) {
+        if ((msg->points[i].line < num_scans_) &&
+            ((msg->points[i].tag & 0x30) == 0x10 || (msg->points[i].tag & 0x30) == 0x00)) {
+            if (i % point_filter_num_ == 0) {
+                cloud_full_[i].x = msg->points[i].x;
+                cloud_full_[i].y = msg->points[i].y;
+                cloud_full_[i].z = msg->points[i].z;
+                cloud_full_[i].intensity = msg->points[i].reflectivity;
+                cloud_full_[i].time = msg->points[i].offset_time / float(1000000);
+
+                if ((abs(cloud_full_[i].x - cloud_full_[i - 1].x) > 1e-7) ||
+                    (abs(cloud_full_[i].y - cloud_full_[i - 1].y) > 1e-7) ||
+                    (abs(cloud_full_[i].z - cloud_full_[i - 1].z) > 1e-7)) {
+                    is_valid_pt[i] = true;
+                }
+            }
+        }
+    });
+
+    for (uint i = 1; i < plsize; i++) {
+        if (is_valid_pt[i]) {
+            cloud_out_.points.push_back(cloud_full_[i]);
+        }
+    }
+}
+
+void CloudConvert::Oust64Handler(const sensor_msgs::PointCloud2::ConstPtr &msg) {
+    cloud_out_.clear();
+    cloud_full_.clear();
+    pcl::PointCloud<ouster_ros::Point> pl_orig;
+    pcl::fromROSMsg(*msg, pl_orig);
+    int plsize = pl_orig.size();
+    cloud_out_.reserve(plsize);
+
+    for (int i = 0; i < pl_orig.points.size(); i++) {
+        if (i % point_filter_num_ != 0) continue;
+
+        double range = pl_orig.points[i].x * pl_orig.points[i].x + pl_orig.points[i].y * pl_orig.points[i].y +
+                       pl_orig.points[i].z * pl_orig.points[i].z;
+
+        Vec3d pt_vec;
+        FullPointType added_pt;
+        added_pt.x = pl_orig.points[i].x;
+        added_pt.y = pl_orig.points[i].y;
+        added_pt.z = pl_orig.points[i].z;
+        added_pt.intensity = pl_orig.points[i].intensity;
+        added_pt.time = pl_orig.points[i].t / 1e6;  // curvature unit: ms
+
+        cloud_out_.points.push_back(added_pt);
+    }
+}
+
+void CloudConvert::VelodyneHandler(const sensor_msgs::PointCloud2::ConstPtr &msg) {
+    cloud_out_.clear();
+    cloud_full_.clear();
+
+    pcl::PointCloud<velodyne_ros::Point> pl_orig;
+    pcl::fromROSMsg(*msg, pl_orig);
+    int plsize = pl_orig.points.size();
+    cloud_out_.reserve(plsize);
+
+    double omega_l = 3.61;  // scan angular velocity
+    std::vector<bool> is_first(num_scans_, true);
+    std::vector<double> yaw_fp(num_scans_, 0.0);    // yaw of first scan point
+    std::vector<float> yaw_last(num_scans_, 0.0);   // yaw of last scan point
+    std::vector<float> time_last(num_scans_, 0.0);  // last offset time
+
+    bool given_offset_time = false;
+    if (pl_orig.points[plsize - 1].time > 0) {
+        given_offset_time = true;
+    } else {
+        given_offset_time = false;
+
+        double yaw_first = atan2(pl_orig.points[0].y, pl_orig.points[0].x) * 57.29578;
+        double yaw_end = yaw_first;
+        int layer_first = pl_orig.points[0].ring;
+        for (uint i = plsize - 1; i > 0; i--) {
+            if (pl_orig.points[i].ring == layer_first) {
+                yaw_end = atan2(pl_orig.points[i].y, pl_orig.points[i].x) * 57.29578;
+                break;
+            }
+        }
+    }
+
+    for (int i = 0; i < plsize; i++) {
+        FullPointType added_pt;
+        added_pt.x = pl_orig.points[i].x;
+        added_pt.y = pl_orig.points[i].y;
+        added_pt.z = pl_orig.points[i].z;
+        added_pt.intensity = pl_orig.points[i].intensity;
+        added_pt.time = pl_orig.points[i].time * time_scale_;  // curvature unit: ms
+
+        /// 略掉过近的点
+        if (added_pt.getVector3fMap().norm() < 4.0) {
+            continue;
+        }
+
+        if (!given_offset_time) {
+            int layer = pl_orig.points[i].ring;
+            double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.2957;
+
+            if (is_first[layer]) {
+                yaw_fp[layer] = yaw_angle;
+                is_first[layer] = false;
+                added_pt.time = 0.0;
+                yaw_last[layer] = yaw_angle;
+                time_last[layer] = added_pt.time;
+                continue;
+            }
+
+            // compute offset time
+            if (yaw_angle <= yaw_fp[layer]) {
+                added_pt.time = (yaw_fp[layer] - yaw_angle) / omega_l;
+            } else {
+                added_pt.time = (yaw_fp[layer] - yaw_angle + 360.0) / omega_l;
+            }
+
+            if (added_pt.time < time_last[layer]) {
+                added_pt.time += 360.0 / omega_l;
+            }
+
+            yaw_last[layer] = yaw_angle;
+            time_last[layer] = added_pt.time;
+        }
+
+        if (i % point_filter_num_ == 0) {
+            cloud_out_.points.push_back(added_pt);
+        }
+    }
+}
+
+void CloudConvert::LoadFromYAML(const std::string &yaml_file) {
+    auto yaml = YAML::LoadFile(yaml_file);
+    time_scale_ = yaml["preprocess"]["time_scale"].as<double>();
+    int lidar_type = yaml["preprocess"]["lidar_type"].as<int>();
+    num_scans_ = yaml["preprocess"]["scan_line"].as<int>();
+    point_filter_num_ = yaml["point_filter_num"].as<int>();
+
+    if (lidar_type == 1) {
+        lidar_type_ = LidarType::AVIA;
+        LOG(INFO) << "Using AVIA Lidar";
+    } else if (lidar_type == 2) {
+        lidar_type_ = LidarType::VELO32;
+        LOG(INFO) << "Using Velodyne 32 Lidar";
+    } else if (lidar_type == 3) {
+        lidar_type_ = LidarType::OUST64;
+        LOG(INFO) << "Using OUST 64 Lidar";
+    } else {
+        LOG(WARNING) << "unknown lidar_type";
+    }
+}
+
+}  // namespace sad

workspace/slam_in_autonomous_driving/src/ch7/loosely_coupled_lio/cloud_convert.h

@@ -0,0 +1,60 @@
+#pragma once
+
+#include <livox_ros_driver/CustomMsg.h>
+#include <pcl_conversions/pcl_conversions.h>
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+#include "common/point_types.h"
+
+namespace sad {
+
+/**
+ * 预处理雷达点云
+ *
+ * 将Velodyne, ouster, avia等数据转到FullCloud
+ * 该类由MessageSync类持有，负责将收到的雷达消息与IMU同步并预处理后，再交给LO/LIO算法
+ */
+class CloudConvert {
+   public:
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+    enum class LidarType {
+        AVIA = 1,  // 大疆的固态雷达
+        VELO32,    // Velodyne 32线
+        OUST64,    // ouster 64线
+    };
+
+    CloudConvert() = default;
+    ~CloudConvert() = default;
+
+    /**
+     * 处理livox avia 点云
+     * @param msg
+     * @param pcl_out
+     */
+    void Process(const livox_ros_driver::CustomMsg::ConstPtr &msg, FullCloudPtr &pcl_out);
+
+    /**
+     * 处理sensor_msgs::PointCloud2点云
+     * @param msg
+     * @param pcl_out
+     */
+    void Process(const sensor_msgs::PointCloud2::ConstPtr &msg, FullCloudPtr &pcl_out);
+
+    /// 从YAML中读取参数
+    void LoadFromYAML(const std::string &yaml);
+
+   private:
+    void AviaHandler(const livox_ros_driver::CustomMsg::ConstPtr &msg);
+    void Oust64Handler(const sensor_msgs::PointCloud2::ConstPtr &msg);
+    void VelodyneHandler(const sensor_msgs::PointCloud2::ConstPtr &msg);
+
+    FullPointCloudType cloud_full_, cloud_out_;  // 输出点云
+    LidarType lidar_type_ = LidarType::AVIA;     // 雷达类型
+    int point_filter_num_ = 1;                   // 跳点
+    int num_scans_ = 6;                          // 扫描线数
+    float time_scale_ = 1e-3;                    // 雷达点的时间字段与秒的比例
+};
+}  // namespace sad

workspace/slam_in_autonomous_driving/src/ch7/loosely_coupled_lio/loosely_lio.cc

@@ -0,0 +1,189 @@
+#include <yaml-cpp/yaml.h>
+#include <execution>
+
+#include "common/lidar_utils.h"
+#include "common/timer/timer.h"
+#include "common/point_cloud_utils.h"
+#include "loosely_lio.h"
+
+namespace sad {
+
+LooselyLIO::LooselyLIO(Options options) : options_(options) {
+    StaticIMUInit::Options imu_init_options;
+    imu_init_options.use_speed_for_static_checking_ = false;  // 本节数据不需要轮速计
+    imu_init_ = StaticIMUInit(imu_init_options);
+}
+
+bool LooselyLIO::Init(const std::string &config_yaml) {
+    /// 初始化自身的参数
+    if (!LoadFromYAML(config_yaml)) {
+        return false;
+    }
+
+    /// 初始化NDT LO的参数
+    sad::IncrementalNDTLO::Options indt_options;
+    indt_options.display_realtime_cloud_ = false;  // 这个程序自己有UI，不用PCL中的
+    inc_ndt_lo_ = std::make_shared<sad::IncrementalNDTLO>(indt_options);
+
+    /// 初始化UI
+    if (options_.with_ui_) {
+        ui_ = std::make_shared<ui::PangolinWindow>();
+        ui_->Init();
+    }
+
+    return true;
+}
+
+bool LooselyLIO::LoadFromYAML(const std::string &yaml_file) {
+    // get params from yaml
+    sync_ = std::make_shared<MessageSync>([this](const MeasureGroup &m) { ProcessMeasurements(m); });
+    sync_->Init(yaml_file);
+
+    /// 自身参数主要是雷达与IMU外参
+    auto yaml = YAML::LoadFile(yaml_file);
+    std::vector<double> ext_t = yaml["mapping"]["extrinsic_T"].as<std::vector<double>>();
+    std::vector<double> ext_r = yaml["mapping"]["extrinsic_R"].as<std::vector<double>>();
+
+    Vec3d lidar_T_wrt_IMU = math::VecFromArray(ext_t);
+    Mat3d lidar_R_wrt_IMU = math::MatFromArray(ext_r);
+    TIL_ = SE3(lidar_R_wrt_IMU, lidar_T_wrt_IMU);
+    return true;
+}
+
+void LooselyLIO::ProcessMeasurements(const MeasureGroup &meas) {
+    LOG(INFO) << "call meas, imu: " << meas.imu_.size() << ", lidar pts: " << meas.lidar_->size();
+    measures_ = meas;
+
+    if (imu_need_init_) {
+        // 初始化IMU系统
+        TryInitIMU();
+        return;
+    }
+
+    // 利用IMU数据进行状态预测
+    Predict();
+
+    // 对点云去畸变
+    Undistort();
+
+    // 配准
+    Align();
+}
+
+void LooselyLIO::Predict() {
+    imu_states_.clear();
+    imu_states_.emplace_back(eskf_.GetNominalState());
+
+    /// 对IMU状态进行预测
+    for (auto &imu : measures_.imu_) {
+        eskf_.Predict(*imu);
+        imu_states_.emplace_back(eskf_.GetNominalState());
+    }
+}
+
+void LooselyLIO::TryInitIMU() {
+    for (auto imu : measures_.imu_) {
+        imu_init_.AddIMU(*imu);
+    }
+
+    if (imu_init_.InitSuccess()) {
+        // 读取初始零偏，设置ESKF
+        sad::ESKFD::Options options;
+        // 噪声由初始化器估计
+        options.gyro_var_ = sqrt(imu_init_.GetCovGyro()[0]);
+        options.acce_var_ = sqrt(imu_init_.GetCovAcce()[0]);
+        eskf_.SetInitialConditions(options, imu_init_.GetInitBg(), imu_init_.GetInitBa(), imu_init_.GetGravity());
+        imu_need_init_ = false;
+
+        LOG(INFO) << "IMU初始化成功";
+    }
+}
+
+void LooselyLIO::Undistort() {
+    auto cloud = measures_.lidar_;
+    auto imu_state = eskf_.GetNominalState();  // 最后时刻的状态
+    SE3 T_end = SE3(imu_state.R_, imu_state.p_);
+
+    if (options_.save_motion_undistortion_pcd_) {
+        sad::SaveCloudToFile("./data/ch7/before_undist.pcd", *cloud);
+    }
+
+    /// 将所有点转到最后时刻状态上
+    std::for_each(std::execution::par_unseq, cloud->points.begin(), cloud->points.end(), [&](auto &pt) {
+        SE3 Ti = T_end;
+        NavStated match;
+
+        // 根据pt.time查找时间，pt.time是该点打到的时间与雷达开始时间之差，单位为毫秒
+        math::PoseInterp<NavStated>(
+            measures_.lidar_begin_time_ + pt.time * 1e-3, imu_states_, [](const NavStated &s) { return s.timestamp_; },
+            [](const NavStated &s) { return s.GetSE3(); }, Ti, match);
+
+        Vec3d pi = ToVec3d(pt);
+        Vec3d p_compensate = TIL_.inverse() * T_end.inverse() * Ti * TIL_ * pi;
+
+        pt.x = p_compensate(0);
+        pt.y = p_compensate(1);
+        pt.z = p_compensate(2);
+    });
+    scan_undistort_ = cloud;
+
+    if (options_.save_motion_undistortion_pcd_) {
+        sad::SaveCloudToFile("./data/ch7/after_undist.pcd", *cloud);
+    }
+}
+
+void LooselyLIO::Align() {
+    FullCloudPtr scan_undistort_trans(new FullPointCloudType);
+    pcl::transformPointCloud(*scan_undistort_, *scan_undistort_trans, TIL_.matrix());
+    scan_undistort_ = scan_undistort_trans;
+
+    auto current_scan = ConvertToCloud<FullPointType>(scan_undistort_);
+
+    // voxel 之
+    pcl::VoxelGrid<PointType> voxel;
+    voxel.setLeafSize(0.5, 0.5, 0.5);
+    voxel.setInputCloud(current_scan);
+
+    CloudPtr current_scan_filter(new PointCloudType);
+    voxel.filter(*current_scan_filter);
+
+    /// 处理首帧雷达数据
+    if (flg_first_scan_) {
+        SE3 pose;
+        inc_ndt_lo_->AddCloud(current_scan_filter, pose);
+        flg_first_scan_ = false;
+        return;
+    }
+
+    /// 从EKF中获取预测pose，放入LO，获取LO位姿，最后合入EKF
+    SE3 pose_predict = eskf_.GetNominalSE3();
+    inc_ndt_lo_->AddCloud(current_scan_filter, pose_predict, true);
+    pose_of_lo_ = pose_predict;
+    eskf_.ObserveSE3(pose_of_lo_, 1e-2, 1e-2);
+
+    if (options_.with_ui_) {
+        // 放入UI
+        ui_->UpdateScan(current_scan, eskf_.GetNominalSE3());  // 转成Lidar Pose传给UI
+        ui_->UpdateNavState(eskf_.GetNominalState());
+    }
+    frame_num_++;
+}
+
+void LooselyLIO::PCLCallBack(const sensor_msgs::PointCloud2::ConstPtr &msg) { sync_->ProcessCloud(msg); }
+
+void LooselyLIO::LivoxPCLCallBack(const livox_ros_driver::CustomMsg::ConstPtr &msg) { sync_->ProcessCloud(msg); }
+
+void LooselyLIO::IMUCallBack(IMUPtr msg_in) { sync_->ProcessIMU(msg_in); }
+
+void LooselyLIO::Finish() {
+    if (options_.with_ui_) {
+        while (ui_->ShouldQuit() == false) {
+            usleep(1e5);
+        }
+
+        ui_->Quit();
+    }
+    LOG(INFO) << "finish done";
+}
+
+}  // namespace sad

workspace/slam_in_autonomous_driving/src/ch7/loosely_coupled_lio/loosely_lio.h

@@ -0,0 +1,98 @@
+#ifndef FASTER_LIO_LASER_MAPPING_H
+#define FASTER_LIO_LASER_MAPPING_H
+
+#include <livox_ros_driver/CustomMsg.h>
+#include <pcl/filters/voxel_grid.h>
+#include <sensor_msgs/PointCloud2.h>
+
+#include "ch3/eskf.hpp"
+#include "ch3/static_imu_init.h"
+#include "ch7/incremental_ndt_lo.h"
+#include "ch7/loosely_coupled_lio/cloud_convert.h"
+#include "ch7/loosely_coupled_lio/measure_sync.h"
+
+#include "tools/ui/pangolin_window.h"
+
+namespace sad {
+
+/**
+ * 7.5 节实现的松耦合LIO程序
+ * 使用第3章的ekf, 7.3节的增量NDT里程计来实现
+ * 事先从YAML中读取必要参数
+ *
+ * 由于是第一个有IMU和雷达的程序，框架变动较大
+ * 后续紧耦合LIO将继续使用本框架
+ */
+class LooselyLIO {
+   public:
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+    struct Options {
+        Options() {}
+        bool save_motion_undistortion_pcd_ = false;  // 是否保存去畸变前后的点云
+        bool with_ui_ = true;                        // 是否带着UI
+    };
+
+    LooselyLIO(Options options);
+    ~LooselyLIO() = default;
+
+    /// 从配置文件初始化
+    bool Init(const std::string &config_yaml);
+
+    /// 点云回调函数
+    void PCLCallBack(const sensor_msgs::PointCloud2::ConstPtr &msg);
+    void LivoxPCLCallBack(const livox_ros_driver::CustomMsg::ConstPtr &msg);
+
+    /// IMU回调函数
+    void IMUCallBack(IMUPtr msg_in);
+
+    /// 结束程序，退出UI
+    void Finish();
+
+   private:
+    /// 处理同步之后的IMU和雷达数据
+    void ProcessMeasurements(const MeasureGroup &meas);
+
+    bool LoadFromYAML(const std::string &yaml);
+
+    /// 尝试让IMU初始化
+    void TryInitIMU();
+
+    /// 利用IMU预测状态信息
+    /// 这段时间的预测数据会放入imu_states_里
+    void Predict();
+
+    /// 对measures_中的点云去畸变
+    void Undistort();
+
+    /// 执行一次配准和观测
+    void Align();
+
+   private:
+    /// modules
+    std::shared_ptr<MessageSync> sync_ = nullptr;  // 消息同步器
+    StaticIMUInit imu_init_;                       // IMU静止初始化
+    std::shared_ptr<sad::IncrementalNDTLO> inc_ndt_lo_ = nullptr;
+
+    /// point clouds data
+    FullCloudPtr scan_undistort_{new FullPointCloudType()};  // scan after undistortion
+    SE3 pose_of_lo_;
+
+    Options options_;
+
+    // flags
+    bool imu_need_init_ = true;   // 是否需要估计IMU初始零偏
+    bool flg_first_scan_ = true;  // 是否第一个雷达
+    int frame_num_ = 0;           // 帧数计数
+
+    // EKF data
+    MeasureGroup measures_;              // 同步之后的IMU和点云
+    std::vector<NavStated> imu_states_;  // ESKF预测期间的状态
+    ESKFD eskf_;                         // ESKF
+    SE3 TIL_;                            // Lidar与IMU之间外参
+
+    std::shared_ptr<ui::PangolinWindow> ui_ = nullptr;
+};
+
+}  // namespace sad
+
+#endif  // FASTER_LIO_LASER_MAPPING_H

workspace/slam_in_autonomous_driving/src/ch7/loosely_coupled_lio/measure_sync.cc

@@ -0,0 +1,51 @@
+//
+// Created by xiang on 23-2-2.
+//
+#include "measure_sync.h"
+
+namespace sad {
+
+bool MessageSync::Sync() {
+    if (lidar_buffer_.empty() || imu_buffer_.empty()) {
+        return false;
+    }
+
+    if (!lidar_pushed_) {
+        measures_.lidar_ = lidar_buffer_.front();
+        measures_.lidar_begin_time_ = time_buffer_.front();
+
+        lidar_end_time_ = measures_.lidar_begin_time_ + measures_.lidar_->points.back().time / double(1000);
+
+        measures_.lidar_end_time_ = lidar_end_time_;
+        lidar_pushed_ = true;
+    }
+
+    if (last_timestamp_imu_ < lidar_end_time_) {
+        return false;
+    }
+
+    double imu_time = imu_buffer_.front()->timestamp_;
+    measures_.imu_.clear();
+    while ((!imu_buffer_.empty()) && (imu_time < lidar_end_time_)) {
+        imu_time = imu_buffer_.front()->timestamp_;
+        if (imu_time > lidar_end_time_) {
+            break;
+        }
+        measures_.imu_.push_back(imu_buffer_.front());
+        imu_buffer_.pop_front();
+    }
+
+    lidar_buffer_.pop_front();
+    time_buffer_.pop_front();
+    lidar_pushed_ = false;
+
+    if (callback_) {
+        callback_(measures_);
+    }
+
+    return true;
+}
+
+void MessageSync::Init(const std::string& yaml) { conv_->LoadFromYAML(yaml); }
+
+}  // namespace sad

workspace/slam_in_autonomous_driving/src/ch7/loosely_coupled_lio/measure_sync.h

@@ -0,0 +1,109 @@
+//
+// Created by xiang on 22-9-9.
+//
+
+#ifndef SLAM_IN_AUTO_DRIVING_MEASURE_SYNC_H
+#define SLAM_IN_AUTO_DRIVING_MEASURE_SYNC_H
+
+#include "cloud_convert.h"
+#include "common/imu.h"
+#include "common/point_types.h"
+
+#include <glog/logging.h>
+#include <deque>
+
+namespace sad {
+
+/// IMU 数据与雷达同步
+struct MeasureGroup {
+    MeasureGroup() { this->lidar_.reset(new FullPointCloudType()); };
+
+    double lidar_begin_time_ = 0;   // 雷达包的起始时间
+    double lidar_end_time_ = 0;     // 雷达的终止时间
+    FullCloudPtr lidar_ = nullptr;  // 雷达点云
+    std::deque<IMUPtr> imu_;        // 上一时时刻到现在的IMU读数
+};
+
+/**
+ * 将激光数据和IMU数据同步
+ */
+class MessageSync {
+   public:
+    using Callback = std::function<void(const MeasureGroup &)>;
+
+    MessageSync(Callback cb) : callback_(cb), conv_(new CloudConvert) {}
+
+    /// 初始化
+    void Init(const std::string &yaml);
+
+    /// 处理IMU数据
+    void ProcessIMU(IMUPtr imu) {
+        double timestamp = imu->timestamp_;
+        if (timestamp < last_timestamp_imu_) {
+            LOG(WARNING) << "imu loop back, clear buffer";
+            imu_buffer_.clear();
+        }
+
+        last_timestamp_imu_ = timestamp;
+        imu_buffer_.emplace_back(imu);
+    }
+
+    /**
+     * 处理sensor_msgs::PointCloud2点云
+     * @param msg
+     */
+    void ProcessCloud(const sensor_msgs::PointCloud2::ConstPtr &msg) {
+        if (msg->header.stamp.toSec() < last_timestamp_lidar_) {
+            LOG(ERROR) << "lidar loop back, clear buffer";
+            lidar_buffer_.clear();
+        }
+
+        FullCloudPtr cloud(new FullPointCloudType());
+        conv_->Process(msg, cloud);
+        lidar_buffer_.push_back(cloud);
+        time_buffer_.push_back(msg->header.stamp.toSec());
+        last_timestamp_lidar_ = msg->header.stamp.toSec();
+
+        Sync();
+    }
+
+    /// 处理Livox点云
+    void ProcessCloud(const livox_ros_driver::CustomMsg::ConstPtr &msg) {
+        if (msg->header.stamp.toSec() < last_timestamp_lidar_) {
+            LOG(WARNING) << "lidar loop back, clear buffer";
+            lidar_buffer_.clear();
+        }
+
+        last_timestamp_lidar_ = msg->header.stamp.toSec();
+        FullCloudPtr ptr(new FullPointCloudType());
+        conv_->Process(msg, ptr);
+
+        if (ptr->empty()) {
+            return;
+        }
+
+        lidar_buffer_.emplace_back(ptr);
+        time_buffer_.emplace_back(last_timestamp_lidar_);
+
+        Sync();
+    }
+
+   private:
+    /// 尝试同步IMU与激光数据，成功时返回true
+    bool Sync();
+
+    Callback callback_;                             // 同步数据后的回调函数
+    std::shared_ptr<CloudConvert> conv_ = nullptr;  // 点云转换
+    std::deque<FullCloudPtr> lidar_buffer_;         // 雷达数据缓冲
+    std::deque<IMUPtr> imu_buffer_;                 // imu数据缓冲
+    double last_timestamp_imu_ = -1.0;              // 最近imu时间
+    double last_timestamp_lidar_ = 0;               // 最近lidar时间
+    std::deque<double> time_buffer_;
+    bool lidar_pushed_ = false;
+    MeasureGroup measures_;
+    double lidar_end_time_ = 0;
+};
+
+}  // namespace sad
+
+#endif  // SLAM_IN_AUTO_DRIVING_MEASURE_SYNC_H

workspace/slam_in_autonomous_driving/src/ch7/ndt_3d.cc

@@ -0,0 +1,201 @@
+//
+// Created by xiang on 2022/7/14.
+//
+
+#include "ndt_3d.h"
+#include "common/lidar_utils.h"
+#include "common/math_utils.h"
+
+#include <glog/logging.h>
+#include <Eigen/SVD>
+#include <execution>
+
+namespace sad {
+
+void Ndt3d::BuildVoxels() {
+    assert(target_ != nullptr);
+    assert(target_->empty() == false);
+    grids_.clear();
+
+    /// 分配体素
+    std::vector<size_t> index(target_->size());
+    std::for_each(index.begin(), index.end(), [idx = 0](size_t& i) mutable { i = idx++; });
+
+    std::for_each(index.begin(), index.end(), [this](const size_t& idx) {
+        Vec3d pt = ToVec3d(target_->points[idx]) * options_.inv_voxel_size_;
+        auto key = CastToInt(pt);
+        if (grids_.find(key) == grids_.end()) {
+            grids_.insert({key, {idx}});
+        } else {
+            grids_[key].idx_.emplace_back(idx);
+        }
+    });
+
+    /// 计算每个体素中的均值和协方差
+    std::for_each(std::execution::par_unseq, grids_.begin(), grids_.end(), [this](auto& v) {
+        if (v.second.idx_.size() > options_.min_pts_in_voxel_) {
+            // 要求至少有３个点
+            math::ComputeMeanAndCov(v.second.idx_, v.second.mu_, v.second.sigma_,
+                                    [this](const size_t& idx) { return ToVec3d(target_->points[idx]); });
+            // SVD 检查最大与最小奇异值，限制最小奇异值
+
+            Eigen::JacobiSVD svd(v.second.sigma_, Eigen::ComputeFullU | Eigen::ComputeFullV);
+            Vec3d lambda = svd.singularValues();
+            if (lambda[1] < lambda[0] * 1e-3) {
+                lambda[1] = lambda[0] * 1e-3;
+            }
+
+            if (lambda[2] < lambda[0] * 1e-3) {
+                lambda[2] = lambda[0] * 1e-3;
+            }
+
+            Mat3d inv_lambda = Vec3d(1.0 / lambda[0], 1.0 / lambda[1], 1.0 / lambda[2]).asDiagonal();
+
+            // v.second.info_ = (v.second.sigma_ + Mat3d::Identity() * 1e-3).inverse();  // 避免出nan
+            v.second.info_ = svd.matrixV() * inv_lambda * svd.matrixU().transpose();
+        }
+    });
+
+    /// 删除点数不够的
+    for (auto iter = grids_.begin(); iter != grids_.end();) {
+        if (iter->second.idx_.size() > options_.min_pts_in_voxel_) {
+            iter++;
+        } else {
+            iter = grids_.erase(iter);
+        }
+    }
+}
+
+bool Ndt3d::AlignNdt(SE3& init_pose) {
+    LOG(INFO) << "aligning with ndt";
+    assert(grids_.empty() == false);
+
+    SE3 pose = init_pose;
+    if (options_.remove_centroid_) {
+        pose.translation() = target_center_ - source_center_;  // 设置平移初始值
+        LOG(INFO) << "init trans set to " << pose.translation().transpose();
+    }
+
+    // 对点的索引，预先生成
+    int num_residual_per_point = 1;
+    if (options_.nearby_type_ == NearbyType::NEARBY6) {
+        num_residual_per_point = 7;
+    }
+
+    std::vector<int> index(source_->points.size());
+    for (int i = 0; i < index.size(); ++i) {
+        index[i] = i;
+    }
+
+    // 我们来写一些并发代码
+    int total_size = index.size() * num_residual_per_point;
+
+    for (int iter = 0; iter < options_.max_iteration_; ++iter) {
+        std::vector<bool> effect_pts(total_size, false);
+        std::vector<Eigen::Matrix<double, 3, 6>> jacobians(total_size);
+        std::vector<Vec3d> errors(total_size);
+        std::vector<Mat3d> infos(total_size);
+
+        // gauss-newton 迭代
+        // 最近邻，可以并发
+        std::for_each(std::execution::par_unseq, index.begin(), index.end(), [&](int idx) {
+            auto q = ToVec3d(source_->points[idx]);
+            Vec3d qs = pose * q;  // 转换之后的q
+
+            // 计算qs所在的栅格以及它的最近邻栅格
+            Vec3i key = CastToInt(Vec3d(qs * options_.inv_voxel_size_));
+
+            for (int i = 0; i < nearby_grids_.size(); ++i) {
+                auto key_off = key + nearby_grids_[i];
+                auto it = grids_.find(key_off);
+                int real_idx = idx * num_residual_per_point + i;
+                if (it != grids_.end()) {
+                    auto& v = it->second;  // voxel
+                    Vec3d e = qs - v.mu_;
+
+                    // check chi2 th
+                    double res = e.transpose() * v.info_ * e;
+                    if (std::isnan(res) || res > options_.res_outlier_th_) {
+                        effect_pts[real_idx] = false;
+                        continue;
+                    }
+
+                    // build residual
+                    Eigen::Matrix<double, 3, 6> J;
+                    J.block<3, 3>(0, 0) = -pose.so3().matrix() * SO3::hat(q);
+                    J.block<3, 3>(0, 3) = Mat3d::Identity();
+
+                    jacobians[real_idx] = J;
+                    errors[real_idx] = e;
+                    infos[real_idx] = v.info_;
+                    effect_pts[real_idx] = true;
+                } else {
+                    effect_pts[real_idx] = false;
+                }
+            }
+        });
+
+        // 累加Hessian和error,计算dx
+        // 原则上可以用reduce并发，写起来比较麻烦，这里写成accumulate
+        double total_res = 0;
+        int effective_num = 0;
+
+        Mat6d H = Mat6d::Zero();
+        Vec6d err = Vec6d::Zero();
+
+        for (int idx = 0; idx < effect_pts.size(); ++idx) {
+            if (!effect_pts[idx]) {
+                continue;
+            }
+
+            total_res += errors[idx].transpose() * infos[idx] * errors[idx];
+            // chi2.emplace_back(errors[idx].transpose() * infos[idx] * errors[idx]);
+            effective_num++;
+
+            H += jacobians[idx].transpose() * infos[idx] * jacobians[idx];
+            err += -jacobians[idx].transpose() * infos[idx] * errors[idx];
+        }
+
+        if (effective_num < options_.min_effective_pts_) {
+            LOG(WARNING) << "effective num too small: " << effective_num;
+            return false;
+        }
+
+        Vec6d dx = H.inverse() * err;
+        pose.so3() = pose.so3() * SO3::exp(dx.head<3>());
+        pose.translation() += dx.tail<3>();
+
+        // 更新
+        LOG(INFO) << "iter " << iter << " total res: " << total_res << ", eff: " << effective_num
+                  << ", mean res: " << total_res / effective_num << ", dxn: " << dx.norm()
+                  << ", dx: " << dx.transpose();
+
+        // std::sort(chi2.begin(), chi2.end());
+        // LOG(INFO) << "chi2 med: " << chi2[chi2.size() / 2] << ", .7: " << chi2[chi2.size() * 0.7]
+        //           << ", .9: " << chi2[chi2.size() * 0.9] << ", max: " << chi2.back();
+
+        if (gt_set_) {
+            double pose_error = (gt_pose_.inverse() * pose).log().norm();
+            LOG(INFO) << "iter " << iter << " pose error: " << pose_error;
+        }
+
+        if (dx.norm() < options_.eps_) {
+            LOG(INFO) << "converged, dx = " << dx.transpose();
+            break;
+        }
+    }
+
+    init_pose = pose;
+    return true;
+}
+
+void Ndt3d::GenerateNearbyGrids() {
+    if (options_.nearby_type_ == NearbyType::CENTER) {
+        nearby_grids_.emplace_back(KeyType::Zero());
+    } else if (options_.nearby_type_ == NearbyType::NEARBY6) {
+        nearby_grids_ = {KeyType(0, 0, 0),  KeyType(-1, 0, 0), KeyType(1, 0, 0), KeyType(0, 1, 0),
+                         KeyType(0, -1, 0), KeyType(0, 0, -1), KeyType(0, 0, 1)};
+    }
+}
+
+}  // namespace sad

workspace/slam_in_autonomous_driving/src/ch7/ndt_3d.h

@@ -0,0 +1,108 @@
+//
+// Created by xiang on 2022/7/14.
+//
+
+#ifndef SLAM_IN_AUTO_DRIVING_NDT_3D_H
+#define SLAM_IN_AUTO_DRIVING_NDT_3D_H
+
+#include "common/eigen_types.h"
+#include "common/point_types.h"
+
+namespace sad {
+
+/**
+ * 3D 形式的NDT
+ */
+class Ndt3d {
+   public:
+    enum class NearbyType {
+        CENTER,   // 只考虑中心
+        NEARBY6,  // 上下左右前后
+    };
+
+    struct Options {
+        int max_iteration_ = 20;        // 最大迭代次数
+        double voxel_size_ = 1.0;       // 体素大小
+        double inv_voxel_size_ = 1.0;   //
+        int min_effective_pts_ = 10;    // 最近邻点数阈值
+        int min_pts_in_voxel_ = 3;      // 每个栅格中最小点数
+        double eps_ = 1e-2;             // 收敛判定条件
+        double res_outlier_th_ = 20.0;  // 异常值拒绝阈值
+        bool remove_centroid_ = false;  // 是否计算两个点云中心并移除中心？
+
+        NearbyType nearby_type_ = NearbyType::NEARBY6;
+    };
+
+    using KeyType = Eigen::Matrix<int, 3, 1>;  // 体素的索引
+    struct VoxelData {
+        VoxelData() {}
+        VoxelData(size_t id) { idx_.emplace_back(id); }
+
+        std::vector<size_t> idx_;      // 点云中点的索引
+        Vec3d mu_ = Vec3d::Zero();     // 均值
+        Mat3d sigma_ = Mat3d::Zero();  // 协方差
+        Mat3d info_ = Mat3d::Zero();   // 协方差之逆
+    };
+
+    Ndt3d() {
+        options_.inv_voxel_size_ = 1.0 / options_.voxel_size_;
+        GenerateNearbyGrids();
+    }
+
+    Ndt3d(Options options) : options_(options) {
+        options_.inv_voxel_size_ = 1.0 / options_.voxel_size_;
+        GenerateNearbyGrids();
+    }
+
+    /// 设置目标的Scan
+    void SetTarget(CloudPtr target) {
+        target_ = target;
+        BuildVoxels();
+
+        // 计算点云中心
+        target_center_ = std::accumulate(target->points.begin(), target_->points.end(), Vec3d::Zero().eval(),
+                                         [](const Vec3d& c, const PointType& pt) -> Vec3d { return c + ToVec3d(pt); }) /
+                         target_->size();
+    }
+
+    /// 设置被配准的Scan
+    void SetSource(CloudPtr source) {
+        source_ = source;
+
+        source_center_ = std::accumulate(source_->points.begin(), source_->points.end(), Vec3d::Zero().eval(),
+                                         [](const Vec3d& c, const PointType& pt) -> Vec3d { return c + ToVec3d(pt); }) /
+                         source_->size();
+    }
+
+    void SetGtPose(const SE3& gt_pose) {
+        gt_pose_ = gt_pose;
+        gt_set_ = true;
+    }
+
+    /// 使用gauss-newton方法进行ndt配准
+    bool AlignNdt(SE3& init_pose);
+
+   private:
+    void BuildVoxels();
+
+    /// 根据最近邻的类型，生成附近网格
+    void GenerateNearbyGrids();
+
+    CloudPtr target_ = nullptr;
+    CloudPtr source_ = nullptr;
+
+    Vec3d target_center_ = Vec3d::Zero();
+    Vec3d source_center_ = Vec3d::Zero();
+
+    SE3 gt_pose_;
+    bool gt_set_ = false;
+
+    Options options_;
+
+    std::unordered_map<KeyType, VoxelData, hash_vec<3>> grids_;  // 栅格数据
+    std::vector<KeyType> nearby_grids_;                          // 附近的栅格
+};
+
+}  // namespace sad
+
+#endif  // SLAM_IN_AUTO_DRIVING_NDT_3D_H

workspace/slam_in_autonomous_driving/src/ch7/ndt_inc.cc

@@ -0,0 +1,338 @@
+//
+// Created by xiang on 2022/7/20.
+//
+
+#include "ch7/ndt_inc.h"
+#include "common/lidar_utils.h"
+#include "common/math_utils.h"
+#include "common/timer/timer.h"
+
+#include <glog/logging.h>
+#include <execution>
+#include <set>
+
+namespace sad {
+
+void IncNdt3d::AddCloud(CloudPtr cloud_world) {
+    std::set<KeyType, less_vec<3>> active_voxels;  // 记录哪些voxel被更新
+    for (const auto& p : cloud_world->points) {
+        auto pt = ToVec3d(p);
+        auto key = CastToInt(Vec3d(pt * options_.inv_voxel_size_));
+        auto iter = grids_.find(key);
+        if (iter == grids_.end()) {
+            // 栅格不存在
+            data_.push_front({key, {pt}});
+            grids_.insert({key, data_.begin()});
+
+            if (data_.size() >= options_.capacity_) {
+                // 删除一个尾部的数据
+                grids_.erase(data_.back().first);
+                data_.pop_back();
+            }
+        } else {
+            // 栅格存在，添加点，更新缓存
+            iter->second->second.AddPoint(pt);
+            data_.splice(data_.begin(), data_, iter->second);  // 更新的那个放到最前
+            iter->second = data_.begin();                      // grids时也指向最前
+        }
+
+        active_voxels.emplace(key);
+    }
+
+    // 更新active_voxels
+    std::for_each(std::execution::par_unseq, active_voxels.begin(), active_voxels.end(),
+                  [this](const auto& key) { UpdateVoxel(grids_[key]->second); });
+    flag_first_scan_ = false;
+}
+
+void IncNdt3d::GenerateNearbyGrids() {
+    if (options_.nearby_type_ == NearbyType::CENTER) {
+        nearby_grids_.emplace_back(KeyType::Zero());
+    } else if (options_.nearby_type_ == NearbyType::NEARBY6) {
+        nearby_grids_ = {KeyType(0, 0, 0),  KeyType(-1, 0, 0), KeyType(1, 0, 0), KeyType(0, 1, 0),
+                         KeyType(0, -1, 0), KeyType(0, 0, -1), KeyType(0, 0, 1)};
+    }
+}
+
+void IncNdt3d::UpdateVoxel(VoxelData& v) {
+    if (flag_first_scan_) {
+        if (v.pts_.size() > 1) {
+            math::ComputeMeanAndCov(v.pts_, v.mu_, v.sigma_, [this](const Vec3d& p) { return p; });
+            v.info_ = (v.sigma_ + Mat3d::Identity() * 1e-3).inverse();  // 避免出nan
+        } else {
+            v.mu_ = v.pts_[0];
+            v.info_ = Mat3d::Identity() * 1e2;
+        }
+
+        v.ndt_estimated_ = true;
+        v.pts_.clear();
+        return;
+    }
+
+    if (v.ndt_estimated_ && v.num_pts_ > options_.max_pts_in_voxel_) {
+        return;
+    }
+
+    if (!v.ndt_estimated_ && v.pts_.size() > options_.min_pts_in_voxel_) {
+        // 新增的voxel
+        math::ComputeMeanAndCov(v.pts_, v.mu_, v.sigma_, [this](const Vec3d& p) { return p; });
+        v.info_ = (v.sigma_ + Mat3d::Identity() * 1e-3).inverse();  // 避免出nan
+        v.ndt_estimated_ = true;
+        v.pts_.clear();
+    } else if (v.ndt_estimated_ && v.pts_.size() > options_.min_pts_in_voxel_) {
+        // 已经估计，而且还有新来的点
+        Vec3d cur_mu, new_mu;
+        Mat3d cur_var, new_var;
+        math::ComputeMeanAndCov(v.pts_, cur_mu, cur_var, [this](const Vec3d& p) { return p; });
+        math::UpdateMeanAndCov(v.num_pts_, v.pts_.size(), v.mu_, v.sigma_, cur_mu, cur_var, new_mu, new_var);
+
+        v.mu_ = new_mu;
+        v.sigma_ = new_var;
+        v.num_pts_ += v.pts_.size();
+        v.pts_.clear();
+
+        // check info
+        Eigen::JacobiSVD svd(v.sigma_, Eigen::ComputeFullU | Eigen::ComputeFullV);
+        Vec3d lambda = svd.singularValues();
+        if (lambda[1] < lambda[0] * 1e-3) {
+            lambda[1] = lambda[0] * 1e-3;
+        }
+
+        if (lambda[2] < lambda[0] * 1e-3) {
+            lambda[2] = lambda[0] * 1e-3;
+        }
+
+        Mat3d inv_lambda = Vec3d(1.0 / lambda[0], 1.0 / lambda[1], 1.0 / lambda[2]).asDiagonal();
+        v.info_ = svd.matrixV() * inv_lambda * svd.matrixU().transpose();
+    }
+}
+
+bool IncNdt3d::AlignNdt(SE3& init_pose) {
+    LOG(INFO) << "aligning with inc ndt, pts: " << source_->size() << ", grids: " << grids_.size();
+    assert(grids_.empty() == false);
+
+    SE3 pose = init_pose;
+
+    // 对点的索引，预先生成
+    int num_residual_per_point = 1;
+    if (options_.nearby_type_ == NearbyType::NEARBY6) {
+        num_residual_per_point = 7;
+    }
+
+    std::vector<int> index(source_->points.size());
+    for (int i = 0; i < index.size(); ++i) {
+        index[i] = i;
+    }
+
+    // 我们来写一些并发代码
+    int total_size = index.size() * num_residual_per_point;
+
+    for (int iter = 0; iter < options_.max_iteration_; ++iter) {
+        std::vector<bool> effect_pts(total_size, false);
+        std::vector<Eigen::Matrix<double, 3, 6>> jacobians(total_size);
+        std::vector<Vec3d> errors(total_size);
+        std::vector<Mat3d> infos(total_size);
+
+        // gauss-newton 迭代
+        // 最近邻，可以并发
+        std::for_each(std::execution::par_unseq, index.begin(), index.end(), [&](int idx) {
+            auto q = ToVec3d(source_->points[idx]);
+            Vec3d qs = pose * q;  // 转换之后的q
+
+            // 计算qs所在的栅格以及它的最近邻栅格
+            Vec3i key = CastToInt(Vec3d(qs * options_.inv_voxel_size_));
+
+            for (int i = 0; i < nearby_grids_.size(); ++i) {
+                Vec3i real_key = key + nearby_grids_[i];
+                auto it = grids_.find(real_key);
+                int real_idx = idx * num_residual_per_point + i;
+                /// 这里要检查高斯分布是否已经估计
+                if (it != grids_.end() && it->second->second.ndt_estimated_) {
+                    auto& v = it->second->second;  // voxel
+                    Vec3d e = qs - v.mu_;
+
+                    // check chi2 th
+                    double res = e.transpose() * v.info_ * e;
+                    if (std::isnan(res) || res > options_.res_outlier_th_) {
+                        effect_pts[real_idx] = false;
+                        continue;
+                    }
+
+                    // build residual
+                    Eigen::Matrix<double, 3, 6> J;
+                    J.block<3, 3>(0, 0) = -pose.so3().matrix() * SO3::hat(q);
+                    J.block<3, 3>(0, 3) = Mat3d::Identity();
+
+                    jacobians[real_idx] = J;
+                    errors[real_idx] = e;
+                    infos[real_idx] = v.info_;
+                    effect_pts[real_idx] = true;
+                } else {
+                    effect_pts[real_idx] = false;
+                }
+            }
+        });
+
+        // 累加Hessian和error,计算dx
+        double total_res = 0;
+
+        int effective_num = 0;
+
+        Mat6d H = Mat6d::Zero();
+        Vec6d err = Vec6d::Zero();
+
+        for (int idx = 0; idx < effect_pts.size(); ++idx) {
+            if (!effect_pts[idx]) {
+                continue;
+            }
+
+            total_res += errors[idx].transpose() * infos[idx] * errors[idx];
+            effective_num++;
+
+            H += jacobians[idx].transpose() * infos[idx] * jacobians[idx];
+            err += -jacobians[idx].transpose() * infos[idx] * errors[idx];
+        }
+
+        if (effective_num < options_.min_effective_pts_) {
+            LOG(WARNING) << "effective num too small: " << effective_num;
+            init_pose = pose;
+            return false;
+        }
+
+        Vec6d dx = H.inverse() * err;
+        pose.so3() = pose.so3() * SO3::exp(dx.head<3>());
+        pose.translation() += dx.tail<3>();
+
+        // 更新
+        LOG(INFO) << "iter " << iter << " total res: " << total_res << ", eff: " << effective_num
+                  << ", mean res: " << total_res / effective_num << ", dxn: " << dx.norm()
+                  << ", dx: " << dx.transpose();
+
+        if (dx.norm() < options_.eps_) {
+            LOG(INFO) << "converged, dx = " << dx.transpose();
+            break;
+        }
+    }
+
+    init_pose = pose;
+    return true;
+}
+
+void IncNdt3d::ComputeResidualAndJacobians(const SE3& input_pose, Mat18d& HTVH, Vec18d& HTVr) {
+    assert(grids_.empty() == false);
+    SE3 pose = input_pose;
+
+    // 大部分流程和前面的Align是一样的，只是会把z, H, R三者抛出去而非自己处理
+    int num_residual_per_point = 1;
+    if (options_.nearby_type_ == NearbyType::NEARBY6) {
+        num_residual_per_point = 7;
+    }
+
+    std::vector<int> index(source_->points.size());
+    for (int i = 0; i < index.size(); ++i) {
+        index[i] = i;
+    }
+
+    int total_size = index.size() * num_residual_per_point;
+
+    std::vector<bool> effect_pts(total_size, false);
+    std::vector<Eigen::Matrix<double, 3, 18>> jacobians(total_size);
+    std::vector<Vec3d> errors(total_size);
+    std::vector<Mat3d> infos(total_size);
+
+    // gauss-newton 迭代
+    // 最近邻，可以并发
+    std::for_each(std::execution::par_unseq, index.begin(), index.end(), [&](int idx) {
+        auto q = ToVec3d(source_->points[idx]);
+        Vec3d qs = pose * q;  // 转换之后的q
+
+        // 计算qs所在的栅格以及它的最近邻栅格
+        Vec3i key = CastToInt(Vec3d(qs * options_.inv_voxel_size_));
+
+        for (int i = 0; i < nearby_grids_.size(); ++i) {
+            Vec3i real_key = key + nearby_grids_[i];
+            auto it = grids_.find(real_key);
+            int real_idx = idx * num_residual_per_point + i;
+            /// 这里要检查高斯分布是否已经估计
+            if (it != grids_.end() && it->second->second.ndt_estimated_) {
+                auto& v = it->second->second;  // voxel
+                Vec3d e = qs - v.mu_;
+
+                // check chi2 th
+                double res = e.transpose() * v.info_ * e;
+                if (std::isnan(res) || res > options_.res_outlier_th_) {
+                    effect_pts[real_idx] = false;
+                    continue;
+                }
+
+                // build residual
+                Eigen::Matrix<double, 3, 18> J;
+                J.setZero();
+                J.block<3, 3>(0, 0) = Mat3d::Identity();                   // 对p
+                J.block<3, 3>(0, 6) = -pose.so3().matrix() * SO3::hat(q);  // 对R
+
+                jacobians[real_idx] = J;
+                errors[real_idx] = e;
+                infos[real_idx] = v.info_;
+                effect_pts[real_idx] = true;
+            } else {
+                effect_pts[real_idx] = false;
+            }
+        }
+    });
+
+    // 累加Hessian和error,计算dx
+    double total_res = 0;
+    int effective_num = 0;
+
+    HTVH.setZero();
+    HTVr.setZero();
+
+    const double info_ratio = 0.01;  // 每个点反馈的info因子
+
+    for (int idx = 0; idx < effect_pts.size(); ++idx) {
+        if (!effect_pts[idx]) {
+            continue;
+        }
+
+        total_res += errors[idx].transpose() * infos[idx] * errors[idx];
+        effective_num++;
+
+        HTVH += jacobians[idx].transpose() * infos[idx] * jacobians[idx] * info_ratio;
+        HTVr += -jacobians[idx].transpose() * infos[idx] * errors[idx] * info_ratio;
+    }
+
+    LOG(INFO) << "effective: " << effective_num;
+}
+
+void IncNdt3d::BuildNDTEdges(sad::VertexPose* v, std::vector<EdgeNDT*>& edges) {
+    assert(grids_.empty() == false);
+    SE3 pose = v->estimate();
+
+    /// 整体流程和NDT一致，只是把查询函数放到edge内部，建立和v绑定的边
+    for (const auto& pt : source_->points) {
+        Vec3d q = ToVec3d(pt);
+        auto edge = new EdgeNDT(v, q, [this](const Vec3d& qs, Vec3d& mu, Mat3d& info) -> bool {
+            Vec3i key = CastToInt(Vec3d(qs * options_.inv_voxel_size_));
+
+            auto it = grids_.find(key);
+            /// 这里要检查高斯分布是否已经估计
+            if (it != grids_.end() && it->second->second.ndt_estimated_) {
+                auto& v = it->second->second;  // voxel
+                mu = v.mu_;
+                info = v.info_;
+                return true;
+            } else {
+                return false;
+            }
+        });
+
+        if (edge->IsValid()) {
+            edges.emplace_back(edge);
+        } else {
+            delete edge;
+        }
+    }
+}
+
+}  // namespace sad

workspace/slam_in_autonomous_driving/src/ch7/ndt_inc.h

@@ -0,0 +1,123 @@
+//
+// Created by xiang on 2022/7/20.
+//
+
+#ifndef SLAM_IN_AUTO_DRIVING_NDT_INC_H
+#define SLAM_IN_AUTO_DRIVING_NDT_INC_H
+
+#include "common/eigen_types.h"
+#include "common/g2o_types.h"
+#include "common/point_types.h"
+
+#include <list>
+namespace sad {
+
+/**
+ * 增量版本的NDT
+ * 内部会维护增量式的voxels，自动删除较旧的voxel，往voxel里添加点云时，更新其均值和协方差估计
+ */
+class IncNdt3d {
+   public:
+    enum class NearbyType {
+        CENTER,   // 只考虑中心
+        NEARBY6,  // 上下左右前后
+    };
+
+    struct Options {
+        int max_iteration_ = 4;        // 最大迭代次数
+        double voxel_size_ = 1.0;      // 体素大小
+        double inv_voxel_size_ = 1.0;  // 体素大小之逆
+        int min_effective_pts_ = 10;   // 最近邻点数阈值
+        int min_pts_in_voxel_ = 5;     // 每个栅格中最小点数
+        int max_pts_in_voxel_ = 50;    // 每个栅格中最大点数
+        double eps_ = 1e-3;            // 收敛判定条件
+        double res_outlier_th_ = 5.0;  // 异常值拒绝阈值
+        size_t capacity_ = 100000;     // 缓存的体素数量
+
+        NearbyType nearby_type_ = NearbyType::NEARBY6;
+    };
+
+    using KeyType = Eigen::Matrix<int, 3, 1>;  // 体素的索引
+
+    /// 体素内置结构
+    struct VoxelData {
+        VoxelData() {}
+        VoxelData(const Vec3d& pt) {
+            pts_.emplace_back(pt);
+            num_pts_ = 1;
+        }
+
+        void AddPoint(const Vec3d& pt) {
+            pts_.emplace_back(pt);
+            if (!ndt_estimated_) {
+                num_pts_++;
+            }
+        }
+
+        std::vector<Vec3d> pts_;       // 内部点，多于一定数量之后再估计均值和协方差
+        Vec3d mu_ = Vec3d::Zero();     // 均值
+        Mat3d sigma_ = Mat3d::Zero();  // 协方差
+        Mat3d info_ = Mat3d::Zero();   // 协方差之逆
+
+        bool ndt_estimated_ = false;  // NDT是否已经估计
+        int num_pts_ = 0;             // 总共的点数，用于更新估计
+    };
+
+    IncNdt3d() {
+        options_.inv_voxel_size_ = 1.0 / options_.voxel_size_;
+        GenerateNearbyGrids();
+    }
+
+    IncNdt3d(Options options) : options_(options) {
+        options_.inv_voxel_size_ = 1.0 / options_.voxel_size_;
+        GenerateNearbyGrids();
+    }
+
+    /// 获取一些统计信息
+    int NumGrids() const { return grids_.size(); }
+
+    /// 在voxel里添加点云，
+    void AddCloud(CloudPtr cloud_world);
+
+    /// 设置被配准的Scan
+    void SetSource(CloudPtr source) { source_ = source; }
+
+    /// 使用gauss-newton方法进行ndt配准
+    bool AlignNdt(SE3& init_pose);
+
+    /**
+     * 计算给定Pose下的雅可比和残差矩阵，符合IEKF中符号（8.17, 8.19）
+     * @param pose
+     * @param HTVH
+     * @param HTVr
+     */
+    void ComputeResidualAndJacobians(const SE3& pose, Mat18d& HTVH, Vec18d& HTVr);
+
+    /**
+     * 根据估计的NDT建立edges
+     * @param v
+     * @param edges
+     */
+    void BuildNDTEdges(VertexPose* v, std::vector<EdgeNDT*>& edges);
+
+   private:
+    /// 根据最近邻的类型，生成附近网格
+    void GenerateNearbyGrids();
+
+    /// 更新体素内部数据, 根据新加入的pts和历史的估计情况来确定自己的估计
+    void UpdateVoxel(VoxelData& v);
+
+    CloudPtr source_ = nullptr;
+    Options options_;
+
+    using KeyAndData = std::pair<KeyType, VoxelData>;  // 预定义
+    std::list<KeyAndData> data_;                       // 真实数据，会缓存，也会清理
+    std::unordered_map<KeyType, std::list<KeyAndData>::iterator, hash_vec<3>> grids_;  // 栅格数据，存储真实数据的迭代器
+    std::vector<KeyType> nearby_grids_;                                                // 附近的栅格
+
+    bool flag_first_scan_ = true;  // 首帧点云特殊处理
+};
+
+}  // namespace sad
+
+#endif  // SLAM_IN_AUTO_DRIVING_NDT_INC_H

workspace/slam_in_autonomous_driving/src/ch7/test/run_gen_simu_data.cc

@@ -0,0 +1,24 @@
+//
+// Created by xiang on 2022/7/13.
+//
+
+#include <glog/logging.h>
+#include "ch7/gen_simu_data.h"
+
+#include <pcl/io/pcd_io.h>
+
+#include "common/point_cloud_utils.h"
+
+int main(int argc, char** argv) {
+    sad::GenSimuData gen;
+    gen.Gen();
+
+    sad::SaveCloudToFile("./data/ch7/sim_source.pcd", *gen.GetSource());
+    sad::SaveCloudToFile("./data/ch7/sim_target.pcd", *gen.GetTarget());
+
+    SE3 T_target_source = gen.GetPose().inverse();
+    LOG(INFO) << "gt pose: " << T_target_source.translation().transpose() << ", "
+              << T_target_source.so3().unit_quaternion().coeffs().transpose();
+
+    return 0;
+}

workspace/slam_in_autonomous_driving/src/ch7/test/test_feature_extraction.cc

@@ -0,0 +1,48 @@
+//
+// Created by xiang on 2022/7/18.
+//
+
+#include <gflags/gflags.h>
+#include <glog/logging.h>
+
+#include "ch7/loam-like/feature_extraction.h"
+#include "common/io_utils.h"
+
+#include "common/timer/timer.h"
+#include "common/point_cloud_utils.h"
+
+/// 这里需要vlp16的数据，用wxb的
+DEFINE_string(bag_path, "./dataset/sad/wxb/test1.bag", "path to wxb bag");
+
+int main(int argc, char** argv) {
+    google::InitGoogleLogging(argv[0]);
+    FLAGS_stderrthreshold = google::INFO;
+    FLAGS_colorlogtostderr = true;
+    google::ParseCommandLineFlags(&argc, &argv, true);
+
+    // 测试角点和平面点的提取
+    sad::FeatureExtraction feature_extraction;
+
+    system("rm -rf ./data/ch7/*.pcd");
+
+    sad::RosbagIO bag_io(fLS::FLAGS_bag_path);
+    bag_io
+        .AddVelodyneHandle("/velodyne_packets_1",
+                           [&](sad::FullCloudPtr cloud) -> bool {
+                               sad::CloudPtr pcd_corner(new sad::PointCloudType), pcd_surf(new sad::PointCloudType);
+                               sad::common::Timer::Evaluate(
+                                   [&]() { feature_extraction.Extract(cloud, pcd_corner, pcd_surf); },
+                                   "Feature Extraction");
+                               LOG(INFO) << "original pts:" << cloud->size() << ", corners: " << pcd_corner->size()
+                                         << ", surf: " << pcd_surf->size();
+                               sad::SaveCloudToFile("./data/ch7/corner.pcd", *pcd_corner);
+                               sad::SaveCloudToFile("./data/ch7/surf.pcd", *pcd_surf);
+                               return true;
+                           })
+        .Go();
+
+    sad::common::Timer::PrintAll();
+    LOG(INFO) << "done.";
+
+    return 0;
+}

workspace/slam_in_autonomous_driving/src/ch7/test/test_icp.cc

@@ -0,0 +1,174 @@
+//
+// Created by xiang on 2022/7/7.
+//
+
+#include <gflags/gflags.h>
+#include <glog/logging.h>
+
+#include <pcl/common/transforms.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/registration/icp.h>
+#include <pcl/registration/ndt.h>
+
+#include "ch7/icp_3d.h"
+#include "ch7/ndt_3d.h"
+#include "common/point_cloud_utils.h"
+#include "common/sys_utils.h"
+
+DEFINE_string(source, "./data/ch7/EPFL/kneeling_lady_source.pcd", "第1个点云路径");
+DEFINE_string(target, "./data/ch7/EPFL/kneeling_lady_target.pcd", "第2个点云路径");
+DEFINE_string(ground_truth_file, "./data/ch7/EPFL/kneeling_lady_pose.txt", "真值Pose");
+
+int main(int argc, char** argv) {
+    google::InitGoogleLogging(argv[0]);
+    FLAGS_stderrthreshold = google::INFO;
+    FLAGS_colorlogtostderr = true;
+    google::ParseCommandLineFlags(&argc, &argv, true);
+
+    // EPFL 雕像数据集：./ch7/EPFL/aquarius_{sourcd.pcd, target.pcd}，真值在对应目录的_pose.txt中
+    // EPFL 模型比较精细，配准时应该采用较小的栅格
+
+    std::ifstream fin(FLAGS_ground_truth_file);
+    SE3 gt_pose;
+    if (fin) {
+        double tx, ty, tz, qw, qx, qy, qz;
+        fin >> tx >> ty >> tz >> qw >> qx >> qy >> qz;
+        fin.close();
+        gt_pose = SE3(Quatd(qw, qx, qy, qz), Vec3d(tx, ty, tz));
+    }
+
+    sad::CloudPtr source(new sad::PointCloudType), target(new sad::PointCloudType);
+    pcl::io::loadPCDFile(fLS::FLAGS_source, *source);
+    pcl::io::loadPCDFile(fLS::FLAGS_target, *target);
+
+    bool success;
+
+    sad::evaluate_and_call(
+        [&]() {
+            sad::Icp3d icp;
+            icp.SetSource(source);
+            icp.SetTarget(target);
+            icp.SetGroundTruth(gt_pose);
+            SE3 pose;
+            success = icp.AlignP2P(pose);
+            if (success) {
+                LOG(INFO) << "icp p2p align success, pose: " << pose.so3().unit_quaternion().coeffs().transpose()
+                          << ", " << pose.translation().transpose();
+                sad::CloudPtr source_trans(new sad::PointCloudType);
+                pcl::transformPointCloud(*source, *source_trans, pose.matrix().cast<float>());
+                sad::SaveCloudToFile("./data/ch7/icp_trans.pcd", *source_trans);
+            } else {
+                LOG(ERROR) << "align failed.";
+            }
+        },
+        "ICP P2P", 1);
+
+    /// 点到面
+    sad::evaluate_and_call(
+        [&]() {
+            sad::Icp3d icp;
+            icp.SetSource(source);
+            icp.SetTarget(target);
+            icp.SetGroundTruth(gt_pose);
+            SE3 pose;
+            success = icp.AlignP2Plane(pose);
+            if (success) {
+                LOG(INFO) << "icp p2plane align success, pose: " << pose.so3().unit_quaternion().coeffs().transpose()
+                          << ", " << pose.translation().transpose();
+                sad::CloudPtr source_trans(new sad::PointCloudType);
+                pcl::transformPointCloud(*source, *source_trans, pose.matrix().cast<float>());
+                sad::SaveCloudToFile("./data/ch7/icp_plane_trans.pcd", *source_trans);
+            } else {
+                LOG(ERROR) << "align failed.";
+            }
+        },
+        "ICP P2Plane", 1);
+
+    /// 点到线
+    sad::evaluate_and_call(
+        [&]() {
+            sad::Icp3d icp;
+            icp.SetSource(source);
+            icp.SetTarget(target);
+            icp.SetGroundTruth(gt_pose);
+            SE3 pose;
+            success = icp.AlignP2Line(pose);
+            if (success) {
+                LOG(INFO) << "icp p2line align success, pose: " << pose.so3().unit_quaternion().coeffs().transpose()
+                          << ", " << pose.translation().transpose();
+                sad::CloudPtr source_trans(new sad::PointCloudType);
+                pcl::transformPointCloud(*source, *source_trans, pose.matrix().cast<float>());
+                sad::SaveCloudToFile("./data/ch7/icp_line_trans.pcd", *source_trans);
+            } else {
+                LOG(ERROR) << "align failed.";
+            }
+        },
+        "ICP P2Line", 1);
+
+    /// 第７章的NDT
+    sad::evaluate_and_call(
+        [&]() {
+            sad::Ndt3d::Options options;
+            options.voxel_size_ = 0.5;
+            options.remove_centroid_ = true;
+            options.nearby_type_ = sad::Ndt3d::NearbyType::CENTER;
+            sad::Ndt3d ndt(options);
+            ndt.SetSource(source);
+            ndt.SetTarget(target);
+            ndt.SetGtPose(gt_pose);
+            SE3 pose;
+            success = ndt.AlignNdt(pose);
+            if (success) {
+                LOG(INFO) << "ndt align success, pose: " << pose.so3().unit_quaternion().coeffs().transpose() << ", "
+                          << pose.translation().transpose();
+                sad::CloudPtr source_trans(new sad::PointCloudType);
+                pcl::transformPointCloud(*source, *source_trans, pose.matrix().cast<float>());
+                sad::SaveCloudToFile("./data/ch7/ndt_trans.pcd", *source_trans);
+            } else {
+                LOG(ERROR) << "align failed.";
+            }
+        },
+        "NDT", 1);
+
+    /// PCL ICP 作为备选
+    sad::evaluate_and_call(
+        [&]() {
+            pcl::IterativeClosestPoint<sad::PointType, sad::PointType> icp_pcl;
+            icp_pcl.setInputSource(source);
+            icp_pcl.setInputTarget(target);
+            sad::CloudPtr output_pcl(new sad::PointCloudType);
+            icp_pcl.align(*output_pcl);
+            SE3f T = SE3f(icp_pcl.getFinalTransformation());
+            LOG(INFO) << "pose from icp pcl: " << T.so3().unit_quaternion().coeffs().transpose() << ", "
+                      << T.translation().transpose();
+            sad::SaveCloudToFile("./data/ch7/pcl_icp_trans.pcd", *output_pcl);
+
+            // 计算GT pose差异
+            double pose_error = (gt_pose.inverse() * T.cast<double>()).log().norm();
+            LOG(INFO) << "ICP PCL pose error: " << pose_error;
+        },
+        "ICP PCL", 1);
+
+    /// PCL NDT 作为备选
+    sad::evaluate_and_call(
+        [&]() {
+            pcl::NormalDistributionsTransform<sad::PointType, sad::PointType> ndt_pcl;
+            ndt_pcl.setInputSource(source);
+            ndt_pcl.setInputTarget(target);
+            ndt_pcl.setResolution(0.5);
+            sad::CloudPtr output_pcl(new sad::PointCloudType);
+            ndt_pcl.align(*output_pcl);
+            SE3f T = SE3f(ndt_pcl.getFinalTransformation());
+            LOG(INFO) << "pose from ndt pcl: " << T.so3().unit_quaternion().coeffs().transpose() << ", "
+                      << T.translation().transpose() << ', trans: ' << ndt_pcl.getTransformationProbability();
+            sad::SaveCloudToFile("./data/ch7/pcl_ndt_trans.pcd", *output_pcl);
+            LOG(INFO) << "score: " << ndt_pcl.getTransformationProbability();
+
+            // 计算GT pose差异
+            double pose_error = (gt_pose.inverse() * T.cast<double>()).log().norm();
+            LOG(INFO) << "NDT PCL pose error: " << pose_error;
+        },
+        "NDT PCL", 1);
+
+    return 0;
+}

workspace/slam_in_autonomous_driving/src/ch7/test/test_inc_ndt_lo.cc

@@ -0,0 +1,52 @@
+//
+// Created by xiang on 2022/7/18.
+//
+
+#include <gflags/gflags.h>
+#include <glog/logging.h>
+
+#include "ch7/incremental_ndt_lo.h"
+#include "common/io_utils.h"
+#include "common/timer/timer.h"
+
+DEFINE_string(bag_path, "./dataset/sad/ulhk/test2.bag", "path to rosbag");
+DEFINE_string(dataset_type, "ULHK", "NCLT/ULHK/KITTI/WXB3D");  // 数据集类型
+DEFINE_bool(use_ndt_nearby_6, false, "use ndt nearby 6?");
+DEFINE_bool(display_map, true, "display map?");
+
+int main(int argc, char** argv) {
+    google::InitGoogleLogging(argv[0]);
+    FLAGS_stderrthreshold = google::INFO;
+    FLAGS_colorlogtostderr = true;
+    google::ParseCommandLineFlags(&argc, &argv, true);
+
+    sad::RosbagIO rosbag_io(fLS::FLAGS_bag_path, sad::Str2DatasetType(FLAGS_dataset_type));
+
+    sad::IncrementalNDTLO::Options options;
+    options.ndt3d_options_.nearby_type_ =
+        FLAGS_use_ndt_nearby_6 ? sad::IncNdt3d::NearbyType::NEARBY6 : sad::IncNdt3d::NearbyType::CENTER;
+    options.display_realtime_cloud_ = FLAGS_display_map;
+    sad::IncrementalNDTLO ndt_lo(options);
+
+    rosbag_io
+        .AddAutoPointCloudHandle([&ndt_lo](sensor_msgs::PointCloud2::Ptr msg) -> bool {
+            sad::common::Timer::Evaluate(
+                [&]() {
+                    SE3 pose;
+                    ndt_lo.AddCloud(sad::VoxelCloud(sad::PointCloud2ToCloudPtr(msg)), pose);
+                },
+                "NDT registration");
+            return true;
+        })
+        .Go();
+
+    if (FLAGS_display_map) {
+        // 把地图存下来
+        ndt_lo.SaveMap("./data/ch7/map.pcd");
+    }
+
+    sad::common::Timer::PrintAll();
+    LOG(INFO) << "done.";
+
+    return 0;
+}

workspace/slam_in_autonomous_driving/src/ch7/test/test_loam_odom.cc

@@ -0,0 +1,43 @@
+//
+// Created by xiang on 2022/7/18.
+//
+
+#include <gflags/gflags.h>
+#include <glog/logging.h>
+
+#include "ch7/loam-like/loam_like_odom.h"
+#include "common/io_utils.h"
+#include "common/timer/timer.h"
+
+DEFINE_string(bag_path, "./dataset/sad/wxb/test1.bag", "path to wxb bag");
+DEFINE_string(topic, "/velodyne_packets_1", "topic of lidar packets");
+DEFINE_bool(display_map, true, "display map?");
+
+int main(int argc, char** argv) {
+    google::InitGoogleLogging(argv[0]);
+    FLAGS_stderrthreshold = google::INFO;
+    FLAGS_colorlogtostderr = true;
+    google::ParseCommandLineFlags(&argc, &argv, true);
+
+    // 测试loam-like odometry的表现
+    sad::LoamLikeOdom::Options options;
+    options.display_realtime_cloud_ = FLAGS_display_map;
+    sad::LoamLikeOdom lo(options);
+
+    LOG(INFO) << "using topic: " << FLAGS_topic;
+    sad::RosbagIO bag_io(fLS::FLAGS_bag_path);
+    bag_io
+        .AddVelodyneHandle(FLAGS_topic,
+                           [&](sad::FullCloudPtr cloud) -> bool {
+                               sad::common::Timer::Evaluate([&]() { lo.ProcessPointCloud(cloud); }, "Loam-like odom");
+                               return true;
+                           })
+        .Go();
+
+    lo.SaveMap("./data/ch7/loam_map.pcd");
+
+    sad::common::Timer::PrintAll();
+    LOG(INFO) << "done.";
+
+    return 0;
+}

workspace/slam_in_autonomous_driving/src/ch7/test/test_loosely_lio.cc

@@ -0,0 +1,50 @@
+//
+// Created by xiang on 22-8-15.
+//
+#include <gflags/gflags.h>
+#include <glog/logging.h>
+
+#include "ch7/loosely_coupled_lio/loosely_lio.h"
+#include "common/io_utils.h"
+#include "common/sys_utils.h"
+#include "common/timer/timer.h"
+
+DEFINE_string(bag_path, "./dataset/sad/ulhk/test3.bag", "path to rosbag");
+DEFINE_string(dataset_type, "ULHK", "NCLT/ULHK/UTBM/AVIA");                   // 数据集类型
+DEFINE_string(config, "./config/velodyne_ulhk.yaml", "path of config yaml");  // 配置文件类型
+DEFINE_bool(display_map, true, "display map?");
+
+int main(int argc, char** argv) {
+    google::InitGoogleLogging(argv[0]);
+    FLAGS_stderrthreshold = google::INFO;
+    FLAGS_colorlogtostderr = true;
+    google::ParseCommandLineFlags(&argc, &argv, true);
+
+    sad::RosbagIO rosbag_io(fLS::FLAGS_bag_path, sad::Str2DatasetType(FLAGS_dataset_type));
+
+    sad::LooselyLIO::Options options;
+    options.with_ui_ = FLAGS_display_map;
+    sad::LooselyLIO lm(options);
+    lm.Init(FLAGS_config);
+
+    rosbag_io
+        .AddAutoPointCloudHandle([&](sensor_msgs::PointCloud2::Ptr cloud) -> bool {
+            sad::common::Timer::Evaluate([&]() { lm.PCLCallBack(cloud); }, "loosely lio");
+            return true;
+        })
+        .AddLivoxHandle([&](const livox_ros_driver::CustomMsg::ConstPtr& msg) -> bool {
+            sad::common::Timer::Evaluate([&]() { lm.LivoxPCLCallBack(msg); }, "loosely lio");
+            return true;
+        })
+        .AddImuHandle([&](IMUPtr imu) {
+            lm.IMUCallBack(imu);
+            return true;
+        })
+        .Go();
+
+    lm.Finish();
+    sad::common::Timer::PrintAll();
+    LOG(INFO) << "done.";
+
+    return 0;
+}

workspace/slam_in_autonomous_driving/src/ch7/test/test_ndt_lo.cc

@@ -0,0 +1,59 @@
+//
+// Created by xiang on 2022/7/18.
+//
+
+#include <gflags/gflags.h>
+#include <glog/logging.h>
+
+#include "ch7/direct_ndt_lo.h"
+#include "ch7/ndt_3d.h"
+#include "common/dataset_type.h"
+#include "common/io_utils.h"
+#include "common/timer/timer.h"
+
+/// 本程序以ULHK数据集为例
+/// 测试以NDT为主的Lidar Odometry
+/// 若使用PCL NDT的话，会重新建立NDT树
+DEFINE_string(bag_path, "./dataset/sad/ulhk/test2.bag", "path to rosbag");
+DEFINE_string(dataset_type, "ULHK", "NCLT/ULHK/KITTI/WXB_3D");  // 数据集类型
+DEFINE_bool(use_pcl_ndt, false, "use pcl ndt to align?");
+DEFINE_bool(use_ndt_nearby_6, false, "use ndt nearby 6?");
+DEFINE_bool(display_map, true, "display map?");
+
+int main(int argc, char** argv) {
+    google::InitGoogleLogging(argv[0]);
+    FLAGS_stderrthreshold = google::INFO;
+    FLAGS_colorlogtostderr = true;
+    google::ParseCommandLineFlags(&argc, &argv, true);
+
+    sad::RosbagIO rosbag_io(fLS::FLAGS_bag_path, sad::Str2DatasetType(FLAGS_dataset_type));
+
+    sad::DirectNDTLO::Options options;
+    options.use_pcl_ndt_ = fLB::FLAGS_use_pcl_ndt;
+    options.ndt3d_options_.nearby_type_ =
+        FLAGS_use_ndt_nearby_6 ? sad::Ndt3d::NearbyType::NEARBY6 : sad::Ndt3d::NearbyType::CENTER;
+    options.display_realtime_cloud_ = FLAGS_display_map;
+    sad::DirectNDTLO ndt_lo(options);
+
+    rosbag_io
+        .AddAutoPointCloudHandle([&ndt_lo](sensor_msgs::PointCloud2::Ptr msg) -> bool {
+            sad::common::Timer::Evaluate(
+                [&]() {
+                    SE3 pose;
+                    ndt_lo.AddCloud(sad::VoxelCloud(sad::PointCloud2ToCloudPtr(msg)), pose);
+                },
+                "NDT registration");
+            return true;
+        })
+        .Go();
+
+    if (FLAGS_display_map) {
+        // 把地图存下来
+        ndt_lo.SaveMap("./data/ch7/map.pcd");
+    }
+
+    sad::common::Timer::PrintAll();
+    LOG(INFO) << "done.";
+
+    return 0;
+}

workspace/slam_in_autonomous_driving/src/ch8/CMakeLists.txt

@@ -0,0 +1,25 @@
+add_library(${PROJECT_NAME}.ch8
+        lio-iekf/lio_iekf.cc
+        lio-preinteg/lio_preinteg.cc
+        )
+
+target_link_libraries(${PROJECT_NAME}.ch8
+        ${PROJECT_NAME}.ch7
+        ${PROJECT_NAME}.ch3
+        ${PROJECT_NAME}.common
+        ${third_party_libs}
+        )
+
+add_executable(test_lio_iekf test/test_lio_iekf.cc)
+target_link_libraries(test_lio_iekf
+        ${PROJECT_NAME}.ch8
+        ${PROJECT_NAME}.ch7
+        ${PROJECT_NAME}.ch4
+        )
+
+add_executable(test_lio_preinteg test/test_lio_preinteg.cc)
+target_link_libraries(test_lio_preinteg
+        ${PROJECT_NAME}.ch8
+        ${PROJECT_NAME}.ch7
+        ${PROJECT_NAME}.ch4
+        )

workspace/slam_in_autonomous_driving/src/ch8/lio-iekf/iekf.hpp

@@ -0,0 +1,263 @@
+//
+// Created by xiang on 22-9-16.
+//
+
+#ifndef SLAM_IN_AUTO_DRIVING_IESKF_HPP
+#define SLAM_IN_AUTO_DRIVING_IESKF_HPP
+
+#include "common/eigen_types.h"
+#include "common/imu.h"
+#include "common/math_utils.h"
+#include "common/nav_state.h"
+
+namespace sad {
+
+/**
+ * 迭代版的ESKF，运动方程与第3章保持一致
+ *
+ * @tparam S
+ */
+template <typename S>
+class IESKF {
+   public:
+    using SO3 = Sophus::SO3<S>;                     // 旋转变量类型
+    using VecT = Eigen::Matrix<S, 3, 1>;            // 向量类型
+    using Vec18T = Eigen::Matrix<S, 18, 1>;         // 18维向量类型
+    using Mat3T = Eigen::Matrix<S, 3, 3>;           // 3x3矩阵类型
+    using MotionNoiseT = Eigen::Matrix<S, 18, 18>;  // 运动噪声类型
+    using OdomNoiseT = Eigen::Matrix<S, 3, 3>;      // 里程计噪声类型
+    using GnssNoiseT = Eigen::Matrix<S, 6, 6>;      // GNSS噪声类型
+    using Mat18T = Eigen::Matrix<S, 18, 18>;        // 18维方差类型
+    using NavStateT = NavState<S>;                  // 整体名义状态变量类型
+
+    struct Options {
+        Options() = default;
+        /// IEKF配置
+        int num_iterations_ = 3;  // 迭代次数
+        double quit_eps_ = 1e-3;  // 终止迭代的dx大小
+
+        /// IMU 测量与零偏参数
+        double imu_dt_ = 0.01;         // IMU测量间隔
+        double gyro_var_ = 1e-5;       // 陀螺测量标准差
+        double acce_var_ = 1e-2;       // 加计测量标准差
+        double bias_gyro_var_ = 1e-6;  // 陀螺零偏游走标准差
+        double bias_acce_var_ = 1e-4;  // 加计零偏游走标准差
+
+        /// RTK 观测参数
+        double gnss_pos_noise_ = 0.1;                   // GNSS位置噪声
+        double gnss_height_noise_ = 0.1;                // GNSS高度噪声
+        double gnss_ang_noise_ = 1.0 * math::kDEG2RAD;  // GNSS旋转噪声
+
+        /// 其他配置
+        bool update_bias_gyro_ = true;  // 是否更新bias
+        bool update_bias_acce_ = true;  // 是否更新bias
+    };
+
+    /**
+     * 初始零偏取零
+     */
+    IESKF(Options option = Options()) : options_(option) { BuildNoise(option); }
+
+    /**
+     * 由外部指定初始零偏
+     * @param init_bg
+     * @param init_ba
+     * @param gravity
+     */
+    IESKF(Options options, const VecT& init_bg, const VecT& init_ba, const VecT& gravity = VecT(0, 0, -9.8))
+        : options_(options) {
+        BuildNoise(options);
+        bg_ = init_bg;
+        ba_ = init_ba;
+        g_ = gravity;
+    }
+
+    /// 设置初始条件
+    void SetInitialConditions(Options options, const VecT& init_bg, const VecT& init_ba,
+                              const VecT& gravity = VecT(0, 0, -9.8)) {
+        BuildNoise(options);
+        options_ = options;
+        bg_ = init_bg;
+        ba_ = init_ba;
+        g_ = gravity;
+
+        cov_ = 1e-4 * Mat18T::Identity();
+        cov_.template block<3, 3>(6, 6) = 0.1 * math::kDEG2RAD * Mat3T::Identity();
+    }
+
+    /// 使用IMU递推
+    bool Predict(const IMU& imu);
+
+    /**
+     * NDT观测函数，输入一个SE3 Pose, 返回本书(8.10)中的几个项
+     * HT V^{-1} H
+     * H^T V{-1} r
+     * 二者都可以用求和的形式来做
+     */
+    using CustomObsFunc = std::function<void(const SE3& input_pose, Eigen::Matrix<S, 18, 18>& HT_Vinv_H,
+                                             Eigen::Matrix<S, 18, 1>& HT_Vinv_r)>;
+
+    /// 使用自定义观测函数更新滤波器
+    bool UpdateUsingCustomObserve(CustomObsFunc obs);
+
+    /// accessors
+    /// 全量状态
+    NavStateT GetNominalState() const { return NavStateT(current_time_, R_, p_, v_, bg_, ba_); }
+
+    /// SE3 状态
+    SE3 GetNominalSE3() const { return SE3(R_, p_); }
+
+    void SetX(const NavStated& x) {
+        current_time_ = x.timestamp_;
+        R_ = x.R_;
+        p_ = x.p_;
+        v_ = x.v_;
+        bg_ = x.bg_;
+        ba_ = x.ba_;
+    }
+
+    void SetCov(const Mat18T& cov) { cov_ = cov; }
+    Vec3d GetGravity() const { return g_; }
+
+   private:
+    void BuildNoise(const Options& options) {
+        double ev = options.acce_var_;
+        double et = options.gyro_var_;
+        double eg = options.bias_gyro_var_;
+        double ea = options.bias_acce_var_;
+
+        double ev2 = ev;  // * ev;
+        double et2 = et;  // * et;
+        double eg2 = eg;  // * eg;
+        double ea2 = ea;  // * ea;
+
+        // set Q
+        Q_.diagonal() << 0, 0, 0, ev2, ev2, ev2, et2, et2, et2, eg2, eg2, eg2, ea2, ea2, ea2, 0, 0, 0;
+
+        double gp2 = options.gnss_pos_noise_ * options.gnss_pos_noise_;
+        double gh2 = options.gnss_height_noise_ * options.gnss_height_noise_;
+        double ga2 = options.gnss_ang_noise_ * options.gnss_ang_noise_;
+        gnss_noise_.diagonal() << gp2, gp2, gh2, ga2, ga2, ga2;
+    }
+
+    /// 更新名义状态变量
+    void Update() {
+        p_ += dx_.template block<3, 1>(0, 0);
+        v_ += dx_.template block<3, 1>(3, 0);
+        R_ = R_ * SO3::exp(dx_.template block<3, 1>(6, 0));
+
+        if (options_.update_bias_gyro_) {
+            bg_ += dx_.template block<3, 1>(9, 0);
+        }
+
+        if (options_.update_bias_acce_) {
+            ba_ += dx_.template block<3, 1>(12, 0);
+        }
+        g_ += dx_.template block<3, 1>(15, 0);
+    }
+
+    double current_time_ = 0.0;
+
+    // nominal state
+    SO3 R_;
+    VecT p_ = VecT::Zero();
+    VecT v_ = VecT::Zero();
+    VecT bg_ = VecT::Zero();
+    VecT ba_ = VecT::Zero();
+    VecT g_{0, 0, -9.8};
+
+    // error state
+    Vec18T dx_ = Vec18T::Zero();
+
+    // covariance
+    Mat18T cov_ = Mat18T::Identity();
+
+    // noise
+    MotionNoiseT Q_ = MotionNoiseT::Zero();
+    GnssNoiseT gnss_noise_ = GnssNoiseT::Zero();
+
+    Options options_;
+};
+
+using IESKFD = IESKF<double>;
+using IESKFF = IESKF<float>;
+
+template <typename S>
+bool IESKF<S>::Predict(const IMU& imu) {
+    /// Predict 部分与ESKF完全一样，不再解释
+    assert(imu.timestamp_ >= current_time_);
+
+    double dt = imu.timestamp_ - current_time_;
+    if (dt > (5 * options_.imu_dt_) || dt < 0) {
+        LOG(INFO) << "skip this imu because dt_ = " << dt;
+        current_time_ = imu.timestamp_;
+        return false;
+    }
+
+    VecT new_p = p_ + v_ * dt + 0.5 * (R_ * (imu.acce_ - ba_)) * dt * dt + 0.5 * g_ * dt * dt;
+    VecT new_v = v_ + R_ * (imu.acce_ - ba_) * dt + g_ * dt;
+    SO3 new_R = R_ * SO3::exp((imu.gyro_ - bg_) * dt);
+
+    R_ = new_R;
+    v_ = new_v;
+    p_ = new_p;
+
+    Mat18T F = Mat18T::Identity();
+    F.template block<3, 3>(0, 3) = Mat3T::Identity() * dt;
+    F.template block<3, 3>(3, 6) = -R_.matrix() * SO3::hat(imu.acce_ - ba_) * dt;
+    F.template block<3, 3>(3, 12) = -R_.matrix() * dt;
+    F.template block<3, 3>(3, 15) = Mat3T::Identity() * dt;
+    F.template block<3, 3>(6, 6) = SO3::exp(-(imu.gyro_ - bg_) * dt).matrix();
+    F.template block<3, 3>(6, 9) = -Mat3T::Identity() * dt;
+
+    cov_ = F * cov_ * F.transpose() + Q_;
+    current_time_ = imu.timestamp_;
+    return true;
+}
+template <typename S>
+bool IESKF<S>::UpdateUsingCustomObserve(IESKF::CustomObsFunc obs) {
+    // H阵由用户给定
+
+    SO3 start_R = R_;
+    Eigen::Matrix<S, 18, 1> HTVr;
+    Eigen::Matrix<S, 18, 18> HTVH;
+    Eigen::Matrix<S, 18, Eigen::Dynamic> K;
+    Mat18T Pk, Qk;
+
+    for (int iter = 0; iter < options_.num_iterations_; ++iter) {
+        // 调用obs function
+        obs(GetNominalSE3(), HTVH, HTVr);
+
+        // 投影P
+        Mat18T J = Mat18T::Identity();
+        J.template block<3, 3>(6, 6) = Mat3T::Identity() - 0.5 * SO3::hat((R_.inverse() * start_R).log());
+        Pk = J * cov_ * J.transpose();
+
+        // 卡尔曼更新
+        Qk = (Pk.inverse() + HTVH).inverse();  // 这个记作中间变量，最后更新时可以用
+        dx_ = Qk * HTVr;
+        // LOG(INFO) << "iter " << iter << " dx = " << dx_.transpose() << ", dxn: " << dx_.norm();
+
+        // dx合入名义变量
+        Update();
+
+        if (dx_.norm() < options_.quit_eps_) {
+            break;
+        }
+    }
+
+    // update P
+    cov_ = (Mat18T::Identity() - Qk * HTVH) * Pk;
+
+    // project P
+    Mat18T J = Mat18T::Identity();
+    Vec3d dtheta = (R_.inverse() * start_R).log();
+    J.template block<3, 3>(6, 6) = Mat3T::Identity() - 0.5 * SO3::hat(dtheta);
+    cov_ = J * cov_ * J.inverse();
+
+    dx_.setZero();
+    return true;
+}
+
+}  // namespace sad
+#endif  // SLAM_IN_AUTO_DRIVING_IEKF_HPP

workspace/slam_in_autonomous_driving/src/ch8/lio-iekf/lio_iekf.cc

@@ -0,0 +1,209 @@
+#include <pcl/common/transforms.h>
+#include <yaml-cpp/yaml.h>
+#include <execution>
+#include <fstream>
+
+#include <g2o/core/block_solver.h>
+#include <g2o/core/optimization_algorithm_levenberg.h>
+#include <g2o/core/robust_kernel.h>
+#include <g2o/core/sparse_block_matrix.h>
+#include <g2o/solvers/eigen/linear_solver_eigen.h>
+
+// #include "ch4/g2o_types_preinteg.h"
+#include "common/g2o_types.h"
+
+#include "common/lidar_utils.h"
+#include "common/point_cloud_utils.h"
+#include "common/timer/timer.h"
+
+#include "lio_iekf.h"
+
+namespace sad {
+
+LioIEKF::LioIEKF(Options options) : options_(options) {
+    StaticIMUInit::Options imu_init_options;
+    imu_init_options.use_speed_for_static_checking_ = false;  // 本节数据不需要轮速计
+    imu_init_ = StaticIMUInit(imu_init_options);
+}
+
+bool LioIEKF::Init(const std::string &config_yaml) {
+    if (!LoadFromYAML(config_yaml)) {
+        LOG(INFO) << "init failed.";
+        return false;
+    }
+
+    if (options_.with_ui_) {
+        ui_ = std::make_shared<ui::PangolinWindow>();
+        ui_->Init();
+    }
+
+    return true;
+}
+
+void LioIEKF::ProcessMeasurements(const MeasureGroup &meas) {
+    LOG(INFO) << "call meas, imu: " << meas.imu_.size() << ", lidar pts: " << meas.lidar_->size();
+    measures_ = meas;
+
+    if (imu_need_init_) {
+        // 初始化IMU系统
+        TryInitIMU();
+        return;
+    }
+
+    // 利用IMU数据进行状态预测
+    Predict();
+
+    // 对点云去畸变
+    Undistort();
+
+    // 配准
+    Align();
+}
+
+bool LioIEKF::LoadFromYAML(const std::string &yaml_file) {
+    // get params from yaml
+    sync_ = std::make_shared<MessageSync>([this](const MeasureGroup &m) { ProcessMeasurements(m); });
+    sync_->Init(yaml_file);
+
+    /// 自身参数主要是雷达与IMU外参
+    auto yaml = YAML::LoadFile(yaml_file);
+    std::vector<double> ext_t = yaml["mapping"]["extrinsic_T"].as<std::vector<double>>();
+    std::vector<double> ext_r = yaml["mapping"]["extrinsic_R"].as<std::vector<double>>();
+
+    Vec3d lidar_T_wrt_IMU = math::VecFromArray(ext_t);
+    Mat3d lidar_R_wrt_IMU = math::MatFromArray(ext_r);
+    TIL_ = SE3(lidar_R_wrt_IMU, lidar_T_wrt_IMU);
+    return true;
+}
+
+void LioIEKF::Align() {
+    FullCloudPtr scan_undistort_trans(new FullPointCloudType);
+    pcl::transformPointCloud(*scan_undistort_, *scan_undistort_trans, TIL_.matrix().cast<float>());
+    scan_undistort_ = scan_undistort_trans;
+
+    current_scan_ = ConvertToCloud<FullPointType>(scan_undistort_);
+
+    // voxel 之
+    pcl::VoxelGrid<PointType> voxel;
+    voxel.setLeafSize(0.5, 0.5, 0.5);
+    voxel.setInputCloud(current_scan_);
+
+    CloudPtr current_scan_filter(new PointCloudType);
+    voxel.filter(*current_scan_filter);
+
+    /// the first scan
+    if (flg_first_scan_) {
+        ndt_.AddCloud(current_scan_);
+        flg_first_scan_ = false;
+
+        return;
+    }
+
+    // 后续的scan，使用NDT配合pose进行更新
+    LOG(INFO) << "=== frame " << frame_num_;
+
+    ndt_.SetSource(current_scan_filter);
+    ieskf_.UpdateUsingCustomObserve([this](const SE3 &input_pose, Mat18d &HTVH, Vec18d &HTVr) {
+        ndt_.ComputeResidualAndJacobians(input_pose, HTVH, HTVr);
+    });
+
+    auto current_nav_state = ieskf_.GetNominalState();
+
+    // 若运动了一定范围，则把点云放入地图中
+    SE3 current_pose = ieskf_.GetNominalSE3();
+    SE3 delta_pose = last_pose_.inverse() * current_pose;
+
+    if (delta_pose.translation().norm() > 1.0 || delta_pose.so3().log().norm() > math::deg2rad(10.0)) {
+        // 将地图合入NDT中
+        CloudPtr current_scan_world(new PointCloudType);
+        pcl::transformPointCloud(*current_scan_filter, *current_scan_world, current_pose.matrix());
+        ndt_.AddCloud(current_scan_world);
+        last_pose_ = current_pose;
+    }
+
+    // 放入UI
+    if (ui_) {
+        ui_->UpdateScan(current_scan_, current_nav_state.GetSE3());  // 转成Lidar Pose传给UI
+        ui_->UpdateNavState(current_nav_state);
+    }
+
+    frame_num_++;
+    return;
+}
+
+void LioIEKF::TryInitIMU() {
+    for (auto imu : measures_.imu_) {
+        imu_init_.AddIMU(*imu);
+    }
+
+    if (imu_init_.InitSuccess()) {
+        // 读取初始零偏，设置ESKF
+        sad::IESKFD::Options options;
+        // 噪声由初始化器估计
+        options.gyro_var_ = sqrt(imu_init_.GetCovGyro()[0]);
+        options.acce_var_ = sqrt(imu_init_.GetCovAcce()[0]);
+        ieskf_.SetInitialConditions(options, imu_init_.GetInitBg(), imu_init_.GetInitBa(), imu_init_.GetGravity());
+        imu_need_init_ = false;
+
+        LOG(INFO) << "IMU初始化成功";
+    }
+}
+
+void LioIEKF::Undistort() {
+    auto cloud = measures_.lidar_;
+    auto imu_state = ieskf_.GetNominalState();  // 最后时刻的状态
+    SE3 T_end = SE3(imu_state.R_, imu_state.p_);
+
+    if (options_.save_motion_undistortion_pcd_) {
+        sad::SaveCloudToFile("./data/ch7/before_undist.pcd", *cloud);
+    }
+
+    /// 将所有点转到最后时刻状态上
+    std::for_each(std::execution::par_unseq, cloud->points.begin(), cloud->points.end(), [&](auto &pt) {
+        SE3 Ti = T_end;
+        NavStated match;
+
+        // 根据pt.time查找时间，pt.time是该点打到的时间与雷达开始时间之差，单位为毫秒
+        math::PoseInterp<NavStated>(
+            measures_.lidar_begin_time_ + pt.time * 1e-3, imu_states_, [](const NavStated &s) { return s.timestamp_; },
+            [](const NavStated &s) { return s.GetSE3(); }, Ti, match);
+
+        Vec3d pi = ToVec3d(pt);
+        Vec3d p_compensate = TIL_.inverse() * T_end.inverse() * Ti * TIL_ * pi;
+
+        pt.x = p_compensate(0);
+        pt.y = p_compensate(1);
+        pt.z = p_compensate(2);
+    });
+    scan_undistort_ = cloud;
+
+    if (options_.save_motion_undistortion_pcd_) {
+        sad::SaveCloudToFile("./data/ch7/after_undist.pcd", *cloud);
+    }
+}
+
+void LioIEKF::Predict() {
+    imu_states_.clear();
+    imu_states_.emplace_back(ieskf_.GetNominalState());
+
+    /// 对IMU状态进行预测
+    for (auto &imu : measures_.imu_) {
+        ieskf_.Predict(*imu);
+        imu_states_.emplace_back(ieskf_.GetNominalState());
+    }
+}
+
+void LioIEKF::PCLCallBack(const sensor_msgs::PointCloud2::ConstPtr &msg) { sync_->ProcessCloud(msg); }
+
+void LioIEKF::LivoxPCLCallBack(const livox_ros_driver::CustomMsg::ConstPtr &msg) { sync_->ProcessCloud(msg); }
+
+void LioIEKF::IMUCallBack(IMUPtr msg_in) { sync_->ProcessIMU(msg_in); }
+
+void LioIEKF::Finish() {
+    if (ui_) {
+        ui_->Quit();
+    }
+    LOG(INFO) << "finish done";
+}
+
+}  // namespace sad

workspace/slam_in_autonomous_driving/src/ch8/lio-iekf/lio_iekf.h

@@ -0,0 +1,99 @@
+#ifndef SAD_CH8_LASER_MAPPING_H
+#define SAD_CH8_LASER_MAPPING_H
+
+#include <livox_ros_driver/CustomMsg.h>
+#include <pcl/filters/voxel_grid.h>
+#include <sensor_msgs/PointCloud2.h>
+
+/// 部分类直接使用ch7的结果
+#include "ch3/static_imu_init.h"
+#include "ch7/loosely_coupled_lio/cloud_convert.h"
+#include "ch7/loosely_coupled_lio/measure_sync.h"
+#include "ch7/ndt_inc.h"
+#include "ch8/lio-iekf/iekf.hpp"
+
+#include "tools/ui/pangolin_window.h"
+
+namespace sad {
+
+class LioIEKF {
+   public:
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+
+    struct Options {
+        Options() {}
+        bool save_motion_undistortion_pcd_ = false;  // 是否保存去畸变前后的点云
+        bool with_ui_ = true;                        // 是否带着UI
+    };
+
+    LioIEKF(Options options = Options());
+    ~LioIEKF() = default;
+
+    /// init without ros
+    bool Init(const std::string& config_yaml);
+
+    /// 点云回调函数
+    void PCLCallBack(const sensor_msgs::PointCloud2::ConstPtr& msg);
+    void LivoxPCLCallBack(const livox_ros_driver::CustomMsg::ConstPtr& msg);
+
+    /// IMU回调函数
+    void IMUCallBack(IMUPtr msg_in);
+
+    /// 结束程序，退出UI
+    void Finish();
+
+    /// 获取当前姿态
+    NavStated GetCurrentState() const { return ieskf_.GetNominalState(); }
+
+    /// 获取当前扫描
+    CloudPtr GetCurrentScan() const { return current_scan_; }
+
+   private:
+    bool LoadFromYAML(const std::string& yaml_file);
+
+    /// 处理同步之后的IMU和雷达数据
+    void ProcessMeasurements(const MeasureGroup& meas);
+
+    /// 尝试让IMU初始化
+    void TryInitIMU();
+
+    /// 利用IMU预测状态信息
+    /// 这段时间的预测数据会放入imu_states_里
+    void Predict();
+
+    /// 对measures_中的点云去畸变
+    void Undistort();
+
+    /// 执行一次配准和观测
+    void Align();
+
+    /// modules
+    std::shared_ptr<MessageSync> sync_ = nullptr;
+    StaticIMUInit imu_init_;
+
+    /// point clouds data
+    FullCloudPtr scan_undistort_{new FullPointCloudType()};  // scan after undistortion
+    CloudPtr current_scan_ = nullptr;
+
+    /// NDT数据
+    IncNdt3d ndt_;
+    SE3 last_pose_;
+
+    // flags
+    bool imu_need_init_ = true;
+    bool flg_first_scan_ = true;
+    int frame_num_ = 0;
+
+    ///////////////////////// EKF inputs and output ///////////////////////////////////////////////////////
+    MeasureGroup measures_;  // sync IMU and lidar scan
+    std::vector<NavStated> imu_states_;
+    IESKFD ieskf_;  // IESKF
+    SE3 TIL_;       // Lidar与IMU之间外参
+
+    Options options_;
+    std::shared_ptr<ui::PangolinWindow> ui_ = nullptr;
+};
+
+}  // namespace sad
+
+#endif  // FASTER_LIO_LASER_MAPPING_H

workspace/slam_in_autonomous_driving/src/ch8/lio-preinteg/lio_preinteg.cc

@@ -0,0 +1,425 @@
+#include <pcl/common/transforms.h>
+#include <yaml-cpp/yaml.h>
+#include <execution>
+#include <fstream>
+
+#include <g2o/core/block_solver.h>
+#include <g2o/core/optimization_algorithm_levenberg.h>
+#include <g2o/core/robust_kernel.h>
+#include <g2o/core/sparse_block_matrix.h>
+#include <g2o/solvers/eigen/linear_solver_eigen.h>
+
+#include "ch4/g2o_types.h"
+#include "common/g2o_types.h"
+
+#include "common/lidar_utils.h"
+#include "common/math_utils.h"
+#include "common/timer/timer.h"
+#include "lio_preinteg.h"
+
+namespace sad {
+
+LioPreinteg::LioPreinteg(Options options) : options_(options), preinteg_(new IMUPreintegration()) {
+    StaticIMUInit::Options imu_init_options;
+    imu_init_options.use_speed_for_static_checking_ = false;  // 本节数据不需要轮速计
+    imu_init_ = StaticIMUInit(imu_init_options);
+
+    double bg_rw2 = 1.0 / (options_.bias_gyro_var_ * options_.bias_gyro_var_);
+    options_.bg_rw_info_.diagonal() << bg_rw2, bg_rw2, bg_rw2;
+    double ba_rw2 = 1.0 / (options_.bias_acce_var_ * options_.bias_acce_var_);
+    options_.ba_rw_info_.diagonal() << ba_rw2, ba_rw2, ba_rw2;
+
+    double gp2 = options_.ndt_pos_noise_ * options_.ndt_pos_noise_;
+    double ga2 = options_.ndt_ang_noise_ * options_.ndt_ang_noise_;
+
+    options_.ndt_info_.diagonal() << 1.0 / ga2, 1.0 / ga2, 1.0 / ga2, 1.0 / gp2, 1.0 / gp2, 1.0 / gp2;
+
+    options_.ndt_options_.nearby_type_ = IncNdt3d::NearbyType::CENTER;
+    ndt_ = IncNdt3d(options_.ndt_options_);
+}
+
+bool LioPreinteg::Init(const std::string &config_yaml) {
+    if (!LoadFromYAML(config_yaml)) {
+        LOG(INFO) << "init failed.";
+        return false;
+    }
+
+    if (options_.with_ui_) {
+        ui_ = std::make_shared<ui::PangolinWindow>();
+        ui_->Init();
+    }
+
+    return true;
+}
+
+void LioPreinteg::ProcessMeasurements(const MeasureGroup &meas) {
+    LOG(INFO) << "call meas, imu: " << meas.imu_.size() << ", lidar pts: " << meas.lidar_->size();
+    measures_ = meas;
+
+    if (imu_need_init_) {
+        // 初始化IMU系统
+        TryInitIMU();
+        return;
+    }
+
+    // 利用IMU数据进行状态预测
+    Predict();
+
+    // 对点云去畸变
+    Undistort();
+
+    // 配准
+    Align();
+}
+
+bool LioPreinteg::LoadFromYAML(const std::string &yaml_file) {
+    // get params from yaml
+    sync_ = std::make_shared<MessageSync>([this](const MeasureGroup &m) { ProcessMeasurements(m); });
+    sync_->Init(yaml_file);
+
+    /// 自身参数主要是雷达与IMU外参
+    auto yaml = YAML::LoadFile(yaml_file);
+    std::vector<double> ext_t = yaml["mapping"]["extrinsic_T"].as<std::vector<double>>();
+    std::vector<double> ext_r = yaml["mapping"]["extrinsic_R"].as<std::vector<double>>();
+
+    Vec3d lidar_T_wrt_IMU = math::VecFromArray(ext_t);
+    Mat3d lidar_R_wrt_IMU = math::MatFromArray(ext_r);
+    TIL_ = SE3(lidar_R_wrt_IMU, lidar_T_wrt_IMU);
+    return true;
+}
+
+void LioPreinteg::Align() {
+    FullCloudPtr scan_undistort_trans(new FullPointCloudType);
+    pcl::transformPointCloud(*scan_undistort_, *scan_undistort_trans, TIL_.matrix().cast<float>());
+    scan_undistort_ = scan_undistort_trans;
+
+    current_scan_ = ConvertToCloud<FullPointType>(scan_undistort_);
+
+    // voxel 之
+    pcl::VoxelGrid<PointType> voxel;
+    voxel.setLeafSize(0.5, 0.5, 0.5);
+    voxel.setInputCloud(current_scan_);
+
+    CloudPtr current_scan_filter(new PointCloudType);
+    voxel.filter(*current_scan_filter);
+
+    /// the first scan
+    if (flg_first_scan_) {
+        ndt_.AddCloud(current_scan_);
+        preinteg_ = std::make_shared<IMUPreintegration>(options_.preinteg_options_);
+        flg_first_scan_ = false;
+        return;
+    }
+
+    // 后续的scan，使用NDT配合pose进行更新
+    LOG(INFO) << "=== frame " << frame_num_;
+    ndt_.SetSource(current_scan_filter);
+
+    current_nav_state_ = preinteg_->Predict(last_nav_state_, imu_init_.GetGravity());
+    ndt_pose_ = current_nav_state_.GetSE3();
+
+    ndt_.AlignNdt(ndt_pose_);
+
+    Optimize();
+
+    // 若运动了一定范围，则把点云放入地图中
+    SE3 current_pose = current_nav_state_.GetSE3();
+    SE3 delta_pose = last_ndt_pose_.inverse() * current_pose;
+
+    if (delta_pose.translation().norm() > 0.3 || delta_pose.so3().log().norm() > math::deg2rad(5.0)) {
+        // 将地图合入NDT中
+        CloudPtr current_scan_world(new PointCloudType);
+        pcl::transformPointCloud(*current_scan_filter, *current_scan_world, current_pose.matrix());
+        ndt_.AddCloud(current_scan_world);
+        last_ndt_pose_ = current_pose;
+    }
+
+    // 放入UI
+    if (ui_) {
+        ui_->UpdateScan(current_scan_, current_nav_state_.GetSE3());  // 转成Lidar Pose传给UI
+        ui_->UpdateNavState(current_nav_state_);
+    }
+
+    frame_num_++;
+}
+
+void LioPreinteg::TryInitIMU() {
+    for (auto imu : measures_.imu_) {
+        imu_init_.AddIMU(*imu);
+    }
+
+    if (imu_init_.InitSuccess()) {
+        // 读取初始零偏，设置ESKF
+        // 噪声由初始化器估计
+        options_.preinteg_options_.noise_gyro_ = sqrt(imu_init_.GetCovGyro()[0]);
+        options_.preinteg_options_.noise_acce_ = sqrt(imu_init_.GetCovAcce()[0]);
+        options_.preinteg_options_.init_ba_ = imu_init_.GetInitBa();
+        options_.preinteg_options_.init_bg_ = imu_init_.GetInitBg();
+
+        preinteg_ = std::make_shared<IMUPreintegration>(options_.preinteg_options_);
+        imu_need_init_ = false;
+
+        current_nav_state_.v_.setZero();
+        current_nav_state_.bg_ = imu_init_.GetInitBg();
+        current_nav_state_.ba_ = imu_init_.GetInitBa();
+        current_nav_state_.timestamp_ = measures_.imu_.back()->timestamp_;
+        
+        last_nav_state_ = current_nav_state_;
+        last_imu_ = measures_.imu_.back();
+
+        LOG(INFO) << "IMU初始化成功";
+    }
+}
+
+void LioPreinteg::Undistort() {
+    auto cloud = measures_.lidar_;
+    auto imu_state = imu_states_.back();  // 最后时刻的状态
+    SE3 T_end = SE3(imu_state.R_, imu_state.p_);
+
+    /// 将所有点转到最后时刻状态上
+    std::for_each(std::execution::par_unseq, cloud->points.begin(), cloud->points.end(), [&](auto &pt) {
+        SE3 Ti = T_end;
+        NavStated match;
+
+        // 根据pt.time查找时间，pt.time是该点打到的时间与雷达开始时间之差，单位为毫秒
+        math::PoseInterp<NavStated>(
+            measures_.lidar_begin_time_ + pt.time * 1e-3, imu_states_, [](const NavStated &s) { return s.timestamp_; },
+            [](const NavStated &s) { return s.GetSE3(); }, Ti, match);
+
+        Vec3d pi = ToVec3d(pt);
+        Vec3d p_compensate = TIL_.inverse() * T_end.inverse() * Ti * TIL_ * pi;
+
+        pt.x = p_compensate(0);
+        pt.y = p_compensate(1);
+        pt.z = p_compensate(2);
+    });
+    scan_undistort_ = cloud;
+}
+
+void LioPreinteg::Predict() {
+    imu_states_.clear();
+    imu_states_.emplace_back(last_nav_state_);
+
+    /// 对IMU状态进行预测
+    for (auto &imu : measures_.imu_) {
+        if (last_imu_ != nullptr) {
+            preinteg_->Integrate(*imu, imu->timestamp_ - last_imu_->timestamp_);
+        }
+
+        last_imu_ = imu;
+        imu_states_.emplace_back(preinteg_->Predict(last_nav_state_, imu_init_.GetGravity()));
+    }
+}
+
+void LioPreinteg::PCLCallBack(const sensor_msgs::PointCloud2::ConstPtr &msg) { sync_->ProcessCloud(msg); }
+
+void LioPreinteg::LivoxPCLCallBack(const livox_ros_driver::CustomMsg::ConstPtr &msg) { sync_->ProcessCloud(msg); }
+
+void LioPreinteg::IMUCallBack(IMUPtr msg_in) { sync_->ProcessIMU(msg_in); }
+
+void LioPreinteg::Finish() {
+    if (ui_) {
+        ui_->Quit();
+    }
+    LOG(INFO) << "finish done";
+}
+
+void LioPreinteg::Optimize() {
+    // 调用g2o求解优化问题
+    // 上一个state到本时刻state的预积分因子，本时刻的NDT因子
+    LOG(INFO) << " === optimizing frame " << frame_num_ << " === "
+              << ", dt: " << preinteg_->dt_;
+
+    /// NOTE 这些东西是对参数非常敏感的。相差几个数量级的话，容易出现优化不动的情况
+
+    using BlockSolverType = g2o::BlockSolverX;
+    using LinearSolverType = g2o::LinearSolverEigen<BlockSolverType::PoseMatrixType>;
+
+    auto *solver = new g2o::OptimizationAlgorithmLevenberg(
+        g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
+    g2o::SparseOptimizer optimizer;
+    optimizer.setAlgorithm(solver);
+
+    // 上时刻顶点， pose, v, bg, ba
+    auto v0_pose = new VertexPose();
+    v0_pose->setId(0);
+    v0_pose->setEstimate(last_nav_state_.GetSE3());
+    optimizer.addVertex(v0_pose);
+
+    auto v0_vel = new VertexVelocity();
+    v0_vel->setId(1);
+    v0_vel->setEstimate(last_nav_state_.v_);
+    optimizer.addVertex(v0_vel);
+
+    auto v0_bg = new VertexGyroBias();
+    v0_bg->setId(2);
+    v0_bg->setEstimate(last_nav_state_.bg_);
+    optimizer.addVertex(v0_bg);
+
+    auto v0_ba = new VertexAccBias();
+    v0_ba->setId(3);
+    v0_ba->setEstimate(last_nav_state_.ba_);
+    optimizer.addVertex(v0_ba);
+
+    // 本时刻顶点，pose, v, bg, ba
+    auto v1_pose = new VertexPose();
+    v1_pose->setId(4);
+    v1_pose->setEstimate(ndt_pose_);  // NDT pose作为初值
+    // v1_pose->setEstimate(current_nav_state_.GetSE3());  // 预测的pose作为初值
+    optimizer.addVertex(v1_pose);
+
+    auto v1_vel = new VertexVelocity();
+    v1_vel->setId(5);
+    v1_vel->setEstimate(current_nav_state_.v_);
+    optimizer.addVertex(v1_vel);
+
+    auto v1_bg = new VertexGyroBias();
+    v1_bg->setId(6);
+    v1_bg->setEstimate(current_nav_state_.bg_);
+    optimizer.addVertex(v1_bg);
+
+    auto v1_ba = new VertexAccBias();
+    v1_ba->setId(7);
+    v1_ba->setEstimate(current_nav_state_.ba_);
+    optimizer.addVertex(v1_ba);
+
+    // imu factor
+    auto edge_inertial = new EdgeInertial(preinteg_, imu_init_.GetGravity());
+    edge_inertial->setVertex(0, v0_pose);
+    edge_inertial->setVertex(1, v0_vel);
+    edge_inertial->setVertex(2, v0_bg);
+    edge_inertial->setVertex(3, v0_ba);
+    edge_inertial->setVertex(4, v1_pose);
+    edge_inertial->setVertex(5, v1_vel);
+    auto *rk = new g2o::RobustKernelHuber();
+    rk->setDelta(200.0);
+    edge_inertial->setRobustKernel(rk);
+    optimizer.addEdge(edge_inertial);
+
+    // 零偏随机游走
+    auto *edge_gyro_rw = new EdgeGyroRW();
+    edge_gyro_rw->setVertex(0, v0_bg);
+    edge_gyro_rw->setVertex(1, v1_bg);
+    edge_gyro_rw->setInformation(options_.bg_rw_info_);
+    optimizer.addEdge(edge_gyro_rw);
+
+    auto *edge_acc_rw = new EdgeAccRW();
+    edge_acc_rw->setVertex(0, v0_ba);
+    edge_acc_rw->setVertex(1, v1_ba);
+    edge_acc_rw->setInformation(options_.ba_rw_info_);
+    optimizer.addEdge(edge_acc_rw);
+
+    // 上一帧pose, vel, bg, ba的先验
+    auto *edge_prior = new EdgePriorPoseNavState(last_nav_state_, prior_info_);
+    edge_prior->setVertex(0, v0_pose);
+    edge_prior->setVertex(1, v0_vel);
+    edge_prior->setVertex(2, v0_bg);
+    edge_prior->setVertex(3, v0_ba);
+    optimizer.addEdge(edge_prior);
+
+    /// 使用NDT的pose进行观测
+    auto *edge_ndt = new EdgeGNSS(v1_pose, ndt_pose_);
+    edge_ndt->setInformation(options_.ndt_info_);
+    optimizer.addEdge(edge_ndt);
+
+    if (options_.verbose_) {
+        LOG(INFO) << "last: " << last_nav_state_;
+        LOG(INFO) << "pred: " << current_nav_state_;
+        LOG(INFO) << "NDT: " << ndt_pose_.translation().transpose() << ","
+                  << ndt_pose_.so3().unit_quaternion().coeffs().transpose();
+    }
+
+    v0_bg->setFixed(true);
+    v0_ba->setFixed(true);
+
+    // go
+    optimizer.setVerbose(options_.verbose_);
+    optimizer.initializeOptimization();
+    optimizer.optimize(20);
+
+    // get results
+    last_nav_state_.R_ = v0_pose->estimate().so3();
+    last_nav_state_.p_ = v0_pose->estimate().translation();
+    last_nav_state_.v_ = v0_vel->estimate();
+    last_nav_state_.bg_ = v0_bg->estimate();
+    last_nav_state_.ba_ = v0_ba->estimate();
+
+    current_nav_state_.R_ = v1_pose->estimate().so3();
+    current_nav_state_.p_ = v1_pose->estimate().translation();
+    current_nav_state_.v_ = v1_vel->estimate();
+    current_nav_state_.bg_ = v1_bg->estimate();
+    current_nav_state_.ba_ = v1_ba->estimate();
+
+    if (options_.verbose_) {
+        LOG(INFO) << "last changed to: " << last_nav_state_;
+        LOG(INFO) << "curr changed to: " << current_nav_state_;
+        LOG(INFO) << "preinteg chi2: " << edge_inertial->chi2() << ", err: " << edge_inertial->error().transpose();
+        LOG(INFO) << "prior chi2: " << edge_prior->chi2() << ", err: " << edge_prior->error().transpose();
+        LOG(INFO) << "ndt: " << edge_ndt->chi2() << "/" << edge_ndt->error().transpose();
+    }
+
+    /// 重置预积分
+
+    options_.preinteg_options_.init_bg_ = current_nav_state_.bg_;
+    options_.preinteg_options_.init_ba_ = current_nav_state_.ba_;
+    preinteg_ = std::make_shared<IMUPreintegration>(options_.preinteg_options_);
+
+    // 计算当前时刻先验
+    // 构建hessian
+    // 15x2，顺序：v0_pose, v0_vel, v0_bg, v0_ba, v1_pose, v1_vel, v1_bg, v1_ba
+    //            0       6        9     12     15        21      24     27
+    Eigen::Matrix<double, 30, 30> H;
+    H.setZero();
+
+    H.block<24, 24>(0, 0) += edge_inertial->GetHessian();
+
+    Eigen::Matrix<double, 6, 6> Hgr = edge_gyro_rw->GetHessian();
+    H.block<3, 3>(9, 9) += Hgr.block<3, 3>(0, 0);
+    H.block<3, 3>(9, 24) += Hgr.block<3, 3>(0, 3);
+    H.block<3, 3>(24, 9) += Hgr.block<3, 3>(3, 0);
+    H.block<3, 3>(24, 24) += Hgr.block<3, 3>(3, 3);
+
+    Eigen::Matrix<double, 6, 6> Har = edge_acc_rw->GetHessian();
+    H.block<3, 3>(12, 12) += Har.block<3, 3>(0, 0);
+    H.block<3, 3>(12, 27) += Har.block<3, 3>(0, 3);
+    H.block<3, 3>(27, 12) += Har.block<3, 3>(3, 0);
+    H.block<3, 3>(27, 27) += Har.block<3, 3>(3, 3);
+
+    H.block<15, 15>(0, 0) += edge_prior->GetHessian();
+    H.block<6, 6>(15, 15) += edge_ndt->GetHessian();
+
+    H = math::Marginalize(H, 0, 14);
+    prior_info_ = H.block<15, 15>(15, 15);
+
+    if (options_.verbose_) {
+        LOG(INFO) << "info trace: " << prior_info_.trace();
+        LOG(INFO) << "optimization done.";
+    }
+
+    NormalizeVelocity();
+    last_nav_state_ = current_nav_state_;
+}
+
+void LioPreinteg::NormalizeVelocity() {
+    /// 限制v的变化
+    /// 一般是-y 方向速度
+    Vec3d v_body = current_nav_state_.R_.inverse() * current_nav_state_.v_;
+    if (v_body[1] > 0) {
+        v_body[1] = 0;
+    }
+    v_body[2] = 0;
+
+    if (v_body[1] < -2.0) {
+        v_body[1] = -2.0;
+    }
+
+    if (v_body[0] > 0.1) {
+        v_body[0] = 0.1;
+    } else if (v_body[0] < -0.1) {
+        v_body[0] = -0.1;
+    }
+
+    current_nav_state_.v_ = current_nav_state_.R_ * v_body;
+}
+
+}  // namespace sad