《自動駕駛與機器人中的SLAM技術》ch9：自動駕駛車輛的離線地圖構建

????????1 點云建圖的流程

? ? ? ? 2 前端實現

? ? ? ? 2.1 前端流程?

? ? ? ? 2.2 前端結果?

? ? ? ? ?3?后端位姿圖優化與異常值剔除

? ? ? ? 3.1 兩階段優化流程

? ? ? ? 3.2 優化結果? ? ? ?

? ? ? ? ① 第一階段優化結果

? ? ? ? ?② 第二階段優化結果

? ? ? ? 4 回環檢測

? ? ? ? 4.1 回環檢測流程?

① 遍歷第一階段優化軌跡中的關鍵幀。

② 并發計算候選回環對是否成立

③ 記錄所有成功經過篩選的回環候選對，并在第二階段優化時讀取回環檢測的結果

? ? ? ? 4.2 回環檢測結果

? ? ? ? 5 軌跡可視化

????????6 地圖導出

? ? ? ? 7 地圖分塊

?????????8 本章小結

????????完整的點云建圖可以看成是一個 RTK、IMU、輪速計、激光的綜合優化問題。大部分 L4 級別的自動駕駛任務都需要一張完整的、與 RTK 對準的點云地圖來進行地圖標注、高精定位等任務。

????????1 點云建圖的流程

????????與在線 SLAM 系統不一樣，地圖構建系統完全可以工作在離線模式下。離線系統的一大好處是有很強的確定性。模塊與模塊之間也沒有線程、資源上的調度問題，而在線系統往往要考慮線程間的等待關系。

1. 首先，我們從給定的 ROS 包出解出 IMU、RTK 和激光數據。由于數據集的差異性，并非所有的數據集都含有輪速信息（而且往往輪速信息的格式與車相關，各有不同），因此我們主要使用 IMU 和激光數據來組成 LIO 系統。這部分內容會直接使用第8章的 IESKF LIO 代碼。
2. 大部分自動駕駛車輛還會攜帶 RTK 設備或者組合導航設備。這些設備能給出車輛在物理世界中的位置，但可能受到信號影響。我們在 LIO 系統中按照一定距離來收集點云關鍵幀，然后按照 RTK 或組合導航的位姿給每個關鍵幀賦一個 RTK 位姿，作為觀測。本書主要以NCLT 數據集為例，而 NCLT 數據集的 RTK 信號屬于單天線方案，僅有平移信息而不含位姿信息。
3. 接下來，我們使用 LIO 作為相鄰幀運動觀測，使用 RTK 位姿作為絕對坐標觀測，優化整條軌跡并判定各關鍵幀的 RTK 有效性。這稱為第一階段優化。如果 RTK 正常，此時我們會得到一條和 RTK 大致符合的軌跡。然而在實際環境中，RTK 會存在許多無效觀測，我們也需要在算法中加以判定。
4. 我們在上一步的基礎上對地圖進行回環檢測。檢測算法可以簡單地使用基于位置（歐氏距離）的回環檢測，并使用 NDT 或常見的配準算法計算它們的相對位姿。本例使用多分辨率的 NDT 匹配作為回環檢測方法。
5. 最后，我們再把這些信息放到統一的位姿圖中進行優化，消除累計誤差帶來的重影，同時也移除 RTK 失效區域。這稱為第二階段優化。在確定了位姿以后，我們就按照這些位姿來導出點云地圖。為了方便地圖加載與查看，我們還會對點云地圖進行切片處理。處理完之后的點云就可以用來進行高精定位或者地圖標注了。

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 frontend";sad::Frontend frontend(FLAGS_config_yaml);if (!frontend.Init()) {LOG(ERROR) << "failed to init frontend.";return -1;}frontend.Run();sad::Optimization opti(FLAGS_config_yaml);if (!opti.Init(1)) {LOG(ERROR) << "failed to init opti1.";return -1;}opti.Run();sad::LoopClosure lc(FLAGS_config_yaml);if (!lc.Init()) {LOG(ERROR) << "failed to init loop closure.";return -1;}lc.Run();sad::Optimization opti2(FLAGS_config_yaml);if (!opti2.Init(2)) {LOG(ERROR) << "failed to init opti2.";return -1;}opti2.Run();LOG(INFO) << "done.";return 0;
}

? ? ? ? 2 前端實現

? ? ? ? 前端代碼運行后會得到每個關鍵幀對應的 LIO 位姿、RTK 位姿以及掃描到的點云 scan。數據都會被存儲在 data/ch9/keyframes.txt?

? ? ? ? 存儲順序為：id_、timestamp_、rtk_heading_valid_、rtk_valid_、rtk_inlier_、lidar_pose_、rtk_pose_、opti_pose_1_、opti_pose_2_

0 1357847247.39686394 0 1 1 -1.46139387880958999e-06 -4.83414494836381997e-05 -5.27170121118259501e-06 -0.00234364570459886113 -7.70544167884409974e-05 4.34271791134374974e-05 0.999997249746972239 0.00277232000311427923 0.00764066664148633015 -0.00289528565243002802 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 
1 1357847253.14514446 0 1 1 -0.0670883829896546102 0.126191846878685315 0.00866291020077865175 0.0426005749366538539 0.000238472378827936458 0.084511669858249247 0.995511382056358696 -0.000874104033612869979 0.102182349318781368 0.00650536902844988177 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 
2 1357847253.64538193 0 1 1 -0.00427907445498453259 0.0132525322323276649 0.000541220831102592155 0.0943471987625134206 0.0112016335156973779 0.157637486857287762 0.982915841885542818 -0.0758422918343611557 0.333011746302124156 0.0181280469104047465 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1

? ? ? ? 2.1 前端流程?

1. 將 ROS 包中的 RTK 數據提取出來，放在 RTK 消息隊列中，按采集時間排序。
2. 用第一個有效的 RTK 數據作為地圖原點，將其他 RTK 讀數減去地圖原點后，作為 RTK 的位置觀測。
3. 用 IMU 和激光數據運行 IESKF LIO，得到當前?State (current_time_, R_, p_, v_, bg_, ba_)。然后根據當前?State 通過 Frontend::ExtractKeyFrame() 函數按照距離和角度閾值判斷當前雷達 scan 是否為關鍵幀。如果是關鍵幀的話，通過插值獲取當前?State 的 RTK 位姿。

? ? ? ? ?時間同步部分：

? ? ? ? 2.2 前端結果?

? ? ? ? IMU 靜止初始化結果：

I0113 22:19:50.345458 409430 static_imu_init.cc:86] mean acce: -0.133286 0.0166332 009.80364
I0113 22:19:50.345479 409430 static_imu_init.cc:109] IMU 初始化成功，初始化時間= 9.9985, bg = 00.00159973 0000.003007 -0.00239681, ba = 07.40016e-05 -9.23492e-06 0-0.00544309, gyro sq = 00.00067222 3.29092e-06 3.63085e-05, acce sq = 7.68385e-06 00.00197847 1.57072e-06, grav = 0000.13336 -0.0166424 00-9.80908, norm: 9.81
I0113 22:19:50.345501 409430 static_imu_init.cc:113] mean gyro: 00.00159973 0000.003007 -0.00239681 acce: 07.40016e-05 -9.23492e-06 0-0.00544309
imu try init true time:1357847247.28515291
I0113 22:19:50.345526 409430 lio_iekf.cc:149] IMU初始化成功

? ? ? ? 前端實現全流程代碼：

void Frontend::Run() {sad::RosbagIO rosbag_io(bag_path_, DatasetType::NCLT);// 先提取RTK pose，注意NCLT只有平移部分rosbag_io.AddAutoRTKHandle([this](GNSSPtr gnss) {gnss_.emplace(gnss->unix_time_, gnss);return true;}).Go();rosbag_io.CleanProcessFunc();  // 不再需要處理RTKRemoveMapOrigin();// 再運行LIOrosbag_io.AddAutoPointCloudHandle([&](sensor_msgs::PointCloud2::Ptr cloud) -> bool {lio_->PCLCallBack(cloud);ExtractKeyFrame(lio_->GetCurrentState());return true;}).AddImuHandle([&](IMUPtr imu) {lio_->IMUCallBack(imu);return true;}).Go();lio_->Finish();// 保存運行結果SaveKeyframes();LOG(INFO) << "done.";
}

? ? ? ? 判斷當前雷達 scan 是否為關鍵幀代碼：

// 主代碼
lio_->PCLCallBack(cloud);
ExtractKeyFrame(lio_->GetCurrentState());// 當前 state 是否能提取到關鍵幀。如果提取到關鍵幀，插值獲取當前 state 的 RTK 位姿
void Frontend::ExtractKeyFrame(const sad::NavStated& state) {if (last_kf_ == nullptr) {if (!lio_->GetCurrentScan()) {// LIO沒完成初始化return;}// 第一個幀auto kf = std::make_shared<Keyframe>(state.timestamp_, kf_id_++, state.GetSE3(), lio_->GetCurrentScan());FindGPSPose(kf);kf->SaveAndUnloadScan("/home/wu/slam_in_autonomous_driving/data/ch9/");keyframes_.emplace(kf->id_, kf);last_kf_ = kf;} else {// 計算當前state與kf之間的相對運動閾值SE3 delta = last_kf_->lidar_pose_.inverse() * state.GetSE3();if (delta.translation().norm() > kf_dis_th_ || delta.so3().log().norm() > kf_ang_th_deg_ * math::kDEG2RAD) {auto kf = std::make_shared<Keyframe>(state.timestamp_, kf_id_++, state.GetSE3(), lio_->GetCurrentScan());FindGPSPose(kf);keyframes_.emplace(kf->id_, kf);kf->SaveAndUnloadScan("/home/wu/slam_in_autonomous_driving/data/ch9/");LOG(INFO) << "生成關鍵幀" << kf->id_;last_kf_ = kf;}}
}

? ? ? ? RTK 位姿插值代碼：

? ? ? ? 插值詳見?《自動駕駛與機器人中的SLAM技術》ch7：基于 ESKF 的松耦合 LIO 系統插值部分。

假設比例? $s$ ?計算公式如下，其中 $t$ ?為待插值的時刻，? $t_0$ ?為起始時刻， $t_s$ ?為結束時刻：

$s_t=\frac{t - t_0}{t_s-t_0}$

旋轉部分插值：四元數球面線性插值 (SLERP)，確保旋轉路徑在旋轉空間中的弧長最短。

$q_t = q_{t_0}(q_{t_0}^{-1}q_{t_s})^{s_t}$

平移部分插值：平移向量線性插值 (LERP)

$p_t=(1-s_t)p_{t_0} + s_tp_{t_s}$

? ? ? ? 注意：這種去畸變的方法前提是濾波器本身有效。如果濾波器失效或位姿發散，去畸變算法也就隨之發散了。

void Frontend::FindGPSPose(std::shared_ptr<Keyframe> kf) {SE3 pose;GNSSPtr match;// pose 為插值結果if (math::PoseInterp<GNSSPtr>(kf->timestamp_, gnss_, [](const GNSSPtr& gnss) -> SE3 { return gnss->utm_pose_; }, pose, match)) {kf->rtk_pose_ = pose;kf->rtk_valid_ = true;} else {kf->rtk_valid_ = false;}
}/*** pose 插值算法* @tparam T    數據類型* @param query_time 查找時間* @param data  數據* @param take_pose_func 從數據中取pose的謂詞* @param result 查詢結果* @param best_match_iter 查找到的最近匹配** NOTE 要求query_time必須在data最大時間和最小時間之間，不會外推* data的map按時間排序* @return 插值是否成功*/
template <typename T>
bool PoseInterp(double query_time, const std::map<double, T>& data, const std::function<SE3(const T&)>& take_pose_func,SE3& result, T& best_match) {if (data.empty()) {LOG(INFO) << "data is empty";return false;}if (query_time > data.rbegin()->first) {LOG(INFO) << "query time is later than last, " << std::setprecision(18) << ", query: " << query_time<< ", end time: " << data.rbegin()->first;return false;}auto match_iter = data.begin();for (auto iter = data.begin(); iter != data.end(); ++iter) {auto next_iter = iter;next_iter++;if (iter->first < query_time && next_iter->first >= query_time) {match_iter = iter;break;}}auto match_iter_n = match_iter;match_iter_n++;assert(match_iter_n != data.end());double dt = match_iter_n->first - match_iter->first;double s = (query_time - match_iter->first) / dt;  // s=0 時為第一幀，s=1時為nextSE3 pose_first = take_pose_func(match_iter->second);SE3 pose_next = take_pose_func(match_iter_n->second);result = {pose_first.unit_quaternion().slerp(s, pose_next.unit_quaternion()),pose_first.translation() * (1 - s) + pose_next.translation() * s};best_match = s < 0.5 ? match_iter->second : match_iter_n->second;return true;
}

? ? ? ? ?3?后端位姿圖優化與異常值剔除

? ? ? ? 后端圖優化需要包含以下因子：

RTK 因子：3 維的單元邊（稱為絕對位姿約束邊），連接到每個關鍵幀，即頂點。誤差為GNSS 估計位姿（由雷達位姿轉換而來）與 RTK 位姿的平移差。當 RTK 存在外參（RTK 不在車輛中心）時，它的平移觀測需要經過轉換之后才能正確作用于車體的位移。
雷達里程計因子：6 維的二元邊（稱為相對運動約束邊），對每個關鍵幀，連接當前關鍵幀和后續 5 個關鍵幀。誤差為兩幀之間的相對運動差，即? $T_{w1}^{-1}T_{w2}$ 。
回環因子：和雷達里程計因子是同一種邊，對所有的回環候選關鍵幀對添加相對運動約束邊。?

/*** 只有平移的GNSS* 此時需要提供RTK外參 TBG，才能正確施加約束*/
class EdgeGNSSTransOnly : public g2o::BaseUnaryEdge<3, Vec3d, VertexPose> {public:EIGEN_MAKE_ALIGNED_OPERATOR_NEW;/*** 指定位姿頂點、RTK觀測 t_WG、外參TGB* @param v* @param obs*/EdgeGNSSTransOnly(VertexPose* v, const Vec3d& obs, const SE3& TBG) : TBG_(TBG) {setVertex(0, v);setMeasurement(obs);}void computeError() override {VertexPose* v = (VertexPose*)_vertices[0];// RTK 讀數為 T_WG_error = (v->estimate() * TBG_).translation() - _measurement;};// void linearizeOplus() override {//     VertexPose* v = (VertexPose*)_vertices[0];//     // jacobian 6x6//     _jacobianOplusXi.setZero();//     _jacobianOplusXi.block<3, 3>(0, 0) = (_measurement.so3().inverse() * v->estimate().so3()).jr_inv();  // dR/dR//     _jacobianOplusXi.block<3, 3>(3, 3) = Mat3d::Identity();                                              // dp/dp// }virtual bool read(std::istream& in) { return true; }virtual bool write(std::ostream& out) const { return true; }private:SE3 TBG_;
};/*** 6 自由度相對運動* 誤差的平移在前，角度在后* 觀測：T12*/
class EdgeRelativeMotion : public g2o::BaseBinaryEdge<6, SE3, VertexPose, VertexPose> {public:EIGEN_MAKE_ALIGNED_OPERATOR_NEW;EdgeRelativeMotion() = default;EdgeRelativeMotion(VertexPose* v1, VertexPose* v2, const SE3& obs) {setVertex(0, v1);setVertex(1, v2);setMeasurement(obs);}void computeError() override {VertexPose* v1 = (VertexPose*)_vertices[0];VertexPose* v2 = (VertexPose*)_vertices[1];SE3 T12 = v1->estimate().inverse() * v2->estimate();_error = (_measurement.inverse() * v1->estimate().inverse() * v2->estimate()).log();};virtual bool read(std::istream& is) override {double data[7];for (int i = 0; i < 7; i++) {is >> data[i];}Quatd q(data[6], data[3], data[4], data[5]);q.normalize();setMeasurement(SE3(q, Vec3d(data[0], data[1], data[2])));for (int i = 0; i < information().rows() && is.good(); i++) {for (int j = i; j < information().cols() && is.good(); j++) {is >> information()(i, j);if (i != j) information()(j, i) = information()(i, j);}}return true;}virtual bool write(std::ostream& os) const override {os << "EDGE_SE3:QUAT ";auto* v1 = static_cast<VertexPose*>(_vertices[0]);auto* v2 = static_cast<VertexPose*>(_vertices[1]);os << v1->id() << " " << v2->id() << " ";SE3 m = _measurement;Eigen::Quaterniond q = m.unit_quaternion();os << m.translation().transpose() << " ";os << q.coeffs()[0] << " " << q.coeffs()[1] << " " << q.coeffs()[2] << " " << q.coeffs()[3] << " ";// information matrixfor (int i = 0; i < information().rows(); i++) {for (int j = i; j < information().cols(); j++) {os << information()(i, j) << " ";}}os << std::endl;return true;}private:
};

? ? ? ? 3.1 兩階段優化流程

????????優化分為兩階段，順序為第一階段優化 $\rightarrow$ 回環檢測 $\rightarrow$ 第二階段優化。在每階段的優化中，又分別進行了兩次優化。

1. 首先使用 Optimization::Init() 函數初始化優化問題，加載關鍵幀參數及配置參數。若為第二階段，則加載篩選后的回環檢測。隨后進入 Optimization::Run() 函數進行優化處理。
2.?如果是單天線 RTK 方案（沒有姿態信息），且處于第一階段，就調用一次 ICP 將雷達位姿和 RTK 位姿對齊。
3. 建立優化問題，包括優化器的創建、頂點（第一階段使用 LIO 位姿作為頂點位姿估計值，這也是第一階段第一次優化前 lidar 誤差為 0 的原因；第二階段使用第一階段優化后的位姿作為估計值）創建、RTK 邊創建（①帶有魯棒核函數②第二階段的信息矩陣添加了0.01的乘積因子，降低了權重）、LIO 邊的創建（沒有魯棒核函數）。若為第二階段，則進行回環檢測邊的創建（帶有魯棒核函數）。
4. 進行第一次優化，此時 RTK 邊和回環檢測邊帶有魯棒核函數。
5. 遍歷回環檢測邊和 RTK 邊，通過閾值篩選異常邊，將異常邊的等級設置為 1（表示不優化）；對于非異常邊，去掉其魯棒核函數。
6. 進行第二次優化，得到某一階段的最終優化結果。
7. 保存結果。第一階段優化結果保存到 opti_pose_1_，第二階段優化結果保存到 opti_pose_2_。

? ? ? ? 總體來說就是。使用

? ? ? ? 接下來是代碼部分：

1. 首先使用 Optimization::Init() 函數初始化優化問題，加載關鍵幀參數及配置參數。若為第二階段，則加載篩選后的回環檢測。隨后進入 Optimization::Run() 函數進行優化處理。

bool Optimization::Init(int stage) {stage_ = stage;if (!LoadKeyFrames("/home/wu/slam_in_autonomous_driving/data/ch9/keyframes.txt", keyframes_)) {LOG(ERROR) << "cannot load keyframes.txt";return false;}LOG(INFO) << "keyframes: " << keyframes_.size();// 讀參數auto yaml = YAML::LoadFile(yaml_);rtk_outlier_th_ = yaml["rtk_outlier_th"].as<double>();lidar_continuous_num_ = yaml["lidar_continuous_num"].as<int>();rtk_has_rot_ = yaml["rtk_has_rot"].as<bool>();rtk_pos_noise_ = yaml["rtk_pos_noise"].as<double>();rtk_ang_noise_ = yaml["rtk_ang_noise"].as<double>() * math::kDEG2RAD;rtk_height_noise_ratio_ = yaml["rtk_height_noise_ratio"].as<double>();std::vector<double> rtk_ext_t = yaml["rtk_ext"]["t"].as<std::vector<double>>();TBG_ = SE3(SO3(), Vec3d(rtk_ext_t[0], rtk_ext_t[1], rtk_ext_t[2]));LOG(INFO) << "TBG = \n" << TBG_.matrix();// my 兩階段不同之處if (stage_ == 2) {LoadLoopCandidates();}return true;
}

2.?如果是單天線 RTK 方案（沒有姿態信息），且處于第一階段，就調用一次 ICP 將雷達位姿和 RTK 位姿對齊。

void Optimization::Run() {LOG(INFO) << "running optimization on stage " << stage_;// my 兩階段不同之處if (!rtk_has_rot_ && stage_ == 1) {InitialAlign();}BuildProblem();  // 建立問題SaveG2O("/home/wu/slam_in_autonomous_driving/data/ch9/before.g2o");LOG(INFO) << "RTK 誤差：" << print_info(gnss_edge_, rtk_outlier_th_);LOG(INFO) << "RTK 平移誤差：" << print_info(gnss_trans_edge_, rtk_outlier_th_);LOG(INFO) << "lidar 誤差：" << print_info(lidar_edge_, 0);Solve();           // 帶著RK求解一遍RemoveOutliers();  // 移除異常值Solve();           // 再求解一遍SaveG2O("/home/wu/slam_in_autonomous_driving/data/ch9/after.g2o");SaveResults();  // 保存結果LOG(INFO) << "done";
}

3. 建立優化問題，包括優化器的創建、頂點（第一階段使用 LIO 位姿作為頂點位姿估計值，第二階段使用第一階段優化后的位姿作為估計值）創建、RTK 邊創建（①帶有魯棒核函數②第二階段的信息矩陣添加了0.01的乘積因子，降低了權重）、LIO 邊的創建（沒有魯棒核函數）。若為第二階段，則進行回環檢測邊的創建（帶有魯棒核函數）。

void Optimization::BuildProblem() {using BlockSolverType = g2o::BlockSolverX;using LinearSolverType = g2o::LinearSolverEigen<BlockSolverType::PoseMatrixType>;auto* solver = new g2o::OptimizationAlgorithmLevenberg(g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));optimizer_.setAlgorithm(solver);AddVertices();AddRTKEdges();AddLidarEdges();AddLoopEdges();
}void Optimization::AddVertices() {for (auto& kfp : keyframes_) {auto kf = kfp.second;// make g2o vertex for this kfauto v = new VertexPose();v->setId(kf->id_);// my 兩階段不同之處if (stage_ == 1) {v->setEstimate(kf->lidar_pose_);} else {v->setEstimate(kf->opti_pose_1_);}optimizer_.addVertex(v);vertices_.emplace(kf->id_, v);}LOG(INFO) << "vertex: " << vertices_.size();
}void Optimization::AddRTKEdges() {/// RTK 噪聲設置Mat3d info_pos = Mat3d::Identity() * 1.0 / (rtk_pos_noise_ * rtk_pos_noise_);info_pos(2, 2) = 1.0 / (rtk_height_noise_ratio_ * rtk_pos_noise_ * rtk_height_noise_ratio_ * rtk_pos_noise_);Mat3d info_ang = Mat3d::Identity() * 1.0 / (rtk_ang_noise_ * rtk_ang_noise_);Mat6d info_all = Mat6d::Identity();info_all.block<3, 3>(0, 0) = info_ang;info_all.block<3, 3>(3, 3) = info_pos;// 信息矩陣：分配觀測值權重LOG(INFO) << "Info of rtk trans: " << info_pos.diagonal().transpose();// my 兩階段不同之處if (stage_ == 2) {info_pos *= 0.01;info_all *= 0.01;}for (auto& kfp : keyframes_) {auto kf = kfp.second;if (!kf->rtk_valid_) {continue;}if (kf->rtk_heading_valid_) {auto edge = new EdgeGNSS(vertices_.at(kf->id_), kf->rtk_pose_);edge->setInformation(info_all);auto rk = new g2o::RobustKernelHuber();rk->setDelta(rtk_outlier_th_);edge->setRobustKernel(rk);optimizer_.addEdge(edge);gnss_edge_.emplace_back(edge);} else {auto edge = new EdgeGNSSTransOnly(vertices_.at(kf->id_), kf->rtk_pose_.translation(), TBG_);edge->setInformation(info_pos);auto rk = new g2o::RobustKernelCauchy();rk->setDelta(rtk_outlier_th_);edge->setRobustKernel(rk);optimizer_.addEdge(edge);gnss_trans_edge_.emplace_back(edge);}}LOG(INFO) << "gnss edges: " << gnss_edge_.size() << ", " << gnss_trans_edge_.size();
}void Optimization::AddLidarEdges() {const double lidar_pos_noise = 0.01, lidar_ang_noise = 0.1 * math::kDEG2RAD;  // RTK 觀測的噪聲Mat3d info_pos = Mat3d::Identity() * 1.0 / (lidar_pos_noise * lidar_pos_noise);Mat3d info_ang = Mat3d::Identity() * 1.0 / (lidar_ang_noise * lidar_ang_noise);Mat6d info_all = Mat6d::Identity();info_all.block<3, 3>(0, 0) = info_pos;info_all.block<3, 3>(3, 3) = info_ang;for (auto iter = keyframes_.begin(); iter != keyframes_.end(); ++iter) {auto iter_next = iter;for (int i = 0; i < lidar_continuous_num_; ++i) {iter_next++;if (iter_next == keyframes_.end()) {break;}// 添加iter和iter_next之間的相鄰運動// 注意：這里觀測用的是 lidar_pose_，而在第二階段時 v->setEstimate(kf->opti_pose_1_) 添加的是 opti_pose_1_，所以 LIDAR 誤差不為 0。auto edge = new EdgeRelativeMotion(vertices_.at(iter->second->id_), vertices_.at(iter_next->second->id_),iter->second->lidar_pose_.inverse() * iter_next->second->lidar_pose_);edge->setInformation(info_all);optimizer_.addEdge(edge);lidar_edge_.emplace_back(edge);}}LOG(INFO) << "lidar edges: " << lidar_edge_.size();
}void Optimization::AddLoopEdges() {// my 兩階段不同之處if (stage_ == 1) {return;}const double loop_pos_noise = 0.1, loop_ang_noise = 0.5 * math::kDEG2RAD;  // RTK 觀測的噪聲Mat3d info_pos = Mat3d::Identity() * 1.0 / (loop_pos_noise * loop_pos_noise);Mat3d info_ang = Mat3d::Identity() * 1.0 / (loop_ang_noise * loop_ang_noise);Mat6d info_all = Mat6d::Identity();info_all.block<3, 3>(0, 0) = info_pos;info_all.block<3, 3>(3, 3) = info_ang;const double loop_rk_th = 5.2;for (const auto& lc : loop_candidates_) {auto edge = new EdgeRelativeMotion(vertices_.at(lc.idx1_), vertices_.at(lc.idx2_), lc.Tij_);edge->setInformation(info_all);auto rk = new g2o::RobustKernelCauchy();rk->setDelta(loop_rk_th);edge->setRobustKernel(rk);optimizer_.addEdge(edge);loop_edge_.emplace_back(edge);}
}void Optimization::Solve() {optimizer_.setVerbose(true);optimizer_.initializeOptimization(0);optimizer_.optimize(100);LOG(INFO) << "RTK 誤差：" << print_info(gnss_edge_, rtk_outlier_th_);LOG(INFO) << "RTK 平移誤差：" << print_info(gnss_trans_edge_, rtk_outlier_th_);LOG(INFO) << "lidar 誤差：" << print_info(lidar_edge_, 0);LOG(INFO) << "loop 誤差：" << print_info(loop_edge_, 0);
}

4. 進行第一次優化，此時 RTK 邊和回環檢測邊帶有魯棒核函數。

Solve();           // 帶著RK求解一遍

5. 遍歷回環檢測邊和 RTK 邊，通過閾值篩選異常邊，將異常邊的等級設置為 1（表示不優化）；對于非異常邊，去掉其魯棒核函數。

void Optimization::RemoveOutliers() {// 主要用于移除GNSS的異常值int cnt_outlier_removed = 0; // ：統計當前移除的異常值數量。auto remove_outlier = [&cnt_outlier_removed](g2o::OptimizableGraph::Edge* e) {if (e->chi2() > e->robustKernel()->delta()) {// level 等級設置成 1，表示不優化（默認情況下，g2o 只處理 level=0 的邊）e->setLevel(1);cnt_outlier_removed++;} else {// 剩下的邊的誤差均小于魯棒核函數的默認值，也就不需要該函數了，所以設置魯棒核函數為空（nullptr），這樣優化過程中直接按照原二次誤差處理。e->setRobustKernel(nullptr);}};// 移除GNSS的異常值std::for_each(gnss_edge_.begin(), gnss_edge_.end(), remove_outlier);std::for_each(gnss_trans_edge_.begin(), gnss_trans_edge_.end(), remove_outlier);LOG(INFO) << "gnss outlier: " << cnt_outlier_removed << "/" << gnss_edge_.size() + gnss_trans_edge_.size();// 移除回環檢測的異常值cnt_outlier_removed = 0;std::for_each(loop_edge_.begin(), loop_edge_.end(), remove_outlier);LOG(INFO) << "loop outlier: " << cnt_outlier_removed << "/" << loop_edge_.size();
}

6. 進行第二次優化，得到某一階段的最終優化結果。

Solve();           // 再求解一遍

7. 保存結果。第一階段優化結果保存到 opti_pose_1_，第二階段優化結果保存到 opti_pose_2_。

void Optimization::SaveResults() {for (auto& v : vertices_) {if (stage_ == 1) {keyframes_.at(v.first)->opti_pose_1_ = v.second->estimate();} else {keyframes_.at(v.first)->opti_pose_2_ = v.second->estimate();}}// 比較優化pose和rtk posestd::vector<double> rtk_trans_error;for (auto& kfp : keyframes_) {auto kf = kfp.second;Vec3d tWG = kf->rtk_pose_.translation();Vec3d t_opti = (kf->opti_pose_1_ * TBG_).translation();double n = (tWG - t_opti).head<2>().norm();rtk_trans_error.emplace_back(n);}std::sort(rtk_trans_error.begin(), rtk_trans_error.end());LOG(INFO) << "med error: " << rtk_trans_error[rtk_trans_error.size() / 2];// 寫入文件system("rm /home/wu/slam_in_autonomous_driving/data/ch9/keyframes.txt");std::ofstream fout("/home/wu/slam_in_autonomous_driving/data/ch9/keyframes.txt");for (auto& kfp : keyframes_) {kfp.second->Save(fout);}fout.close();
}

? ? ? ? 3.2 優化結果? ? ? ?

? ? ? ? ① 第一階段優化結果

wu@WP:~/slam_in_autonomous_driving/bin$ ./run_optimization --stage=1I0114 13:20:35.330343 421782 run_optimization.cc:23] testing optimization
I0114 13:20:35.335601 421782 keyframe.cc:78] Loaded kfs: 1143
I0114 13:20:35.335613 421782 optimization.cc:52] keyframes: 1143
I0114 13:20:35.346560 421782 optimization.cc:66] TBG = 
000001 000000 000000 000000
000000 000001 000000 000.24
000000 000000 000001 -0.283
000000 000000 000000 000001
I0114 13:20:35.346596 421782 optimization.cc:76] running optimization on stage 1
I0114 13:20:35.346643 421782 optimization.cc:335] p1: -215.594 -47.5573 -5.71106, p2: 218.772 17.5467 1.19319
I0114 13:20:35.346962 421782 optimization.cc:364] initial trans: 
0-0.989348 000.141275 0-0.035093 00-1.58926
0-0.141523 0-0.989924 0.00467225 000.768323
-0.0340794 0.00958895 000.999373 000.383868
0000000000 0000000000 0000000000 0000000001
I0114 13:20:35.347182 421782 optimization.cc:146] vertex: 1143
I0114 13:20:35.347186 421782 optimization.cc:158] Info of rtk trans: 000.25 000.25 0.0025
I0114 13:20:35.347465 421782 optimization.cc:191] gnss edges: 0, 1143
I0114 13:20:35.349598 421782 optimization.cc:221] lidar edges: 5700
I0114 13:20:35.402307 421782 optimization.cc:85] RTK 誤差：
I0114 13:20:35.402323 421782 optimization.cc:86] RTK 平移誤差：數量: 1143, 均值: 5.418336, 中位數: 0.338018, 0.1分位: 0.099467, 0.9分位: 1.155777, 0.95分位：29.841078, 最大值: 166.835762, 閾值: 1.000000
I0114 13:20:35.402400 421782 optimization.cc:87] lidar 誤差：數量: 5700, 均值: 0.000000, 中位數: 0.000000, 0.1分位: 0.000000, 0.9分位: 0.000000, 0.95分位：0.000000, 最大值: 0.000000, 閾值: 0.000000
iteration= 0	 chi2= 645.444572	 time= 0.045737	 cumTime= 0.045737	 edges= 6843	 schur= 0	 lambda= 13.285494	 levenbergIter= 1
iteration= 1	 chi2= 640.432056	 time= 0.035216	 cumTime= 0.080953	 edges= 6843	 schur= 0	 lambda= 4.428498	 levenbergIter= 1
iteration= 2	 chi2= 627.241357	 time= 0.0353586	 cumTime= 0.116312	 edges= 6843	 schur= 0	 lambda= 1.476166	 levenbergIter= 1
iteration= 3	 chi2= 596.229379	 time= 0.0352488	 cumTime= 0.15156	 edges= 6843	 schur= 0	 lambda= 0.492055	 levenbergIter= 1
iteration= 4	 chi2= 541.185154	 time= 0.0350486	 cumTime= 0.186609	 edges= 6843	 schur= 0	 lambda= 0.164018	 levenbergIter= 1
iteration= 5	 chi2= 490.442325	 time= 0.0349324	 cumTime= 0.221541	 edges= 6843	 schur= 0	 lambda= 0.054673	 levenbergIter= 1
iteration= 6	 chi2= 475.227538	 time= 0.0350432	 cumTime= 0.256585	 edges= 6843	 schur= 0	 lambda= 0.018224	 levenbergIter= 1
iteration= 7	 chi2= 472.835370	 time= 0.0350574	 cumTime= 0.291642	 edges= 6843	 schur= 0	 lambda= 0.006075	 levenbergIter= 1
iteration= 8	 chi2= 470.709964	 time= 0.0349512	 cumTime= 0.326593	 edges= 6843	 schur= 0	 lambda= 0.002025	 levenbergIter= 1
iteration= 9	 chi2= 468.734946	 time= 0.0353598	 cumTime= 0.361953	 edges= 6843	 schur= 0	 lambda= 0.000675	 levenbergIter= 1
iteration= 10	 chi2= 468.114726	 time= 0.0350006	 cumTime= 0.396954	 edges= 6843	 schur= 0	 lambda= 0.000225	 levenbergIter= 1
iteration= 11	 chi2= 468.066097	 time= 0.0350694	 cumTime= 0.432023	 edges= 6843	 schur= 0	 lambda= 0.000075	 levenbergIter= 1
iteration= 12	 chi2= 468.065040	 time= 0.0349778	 cumTime= 0.467001	 edges= 6843	 schur= 0	 lambda= 0.000050	 levenbergIter= 1
iteration= 13	 chi2= 468.065030	 time= 0.0349623	 cumTime= 0.501963	 edges= 6843	 schur= 0	 lambda= 0.000033	 levenbergIter= 1
iteration= 14	 chi2= 468.065030	 time= 0.0553328	 cumTime= 0.557296	 edges= 6843	 schur= 0	 lambda= 0.728149	 levenbergIter= 6
iteration= 15	 chi2= 468.065030	 time= 0.0351333	 cumTime= 0.592429	 edges= 6843	 schur= 0	 lambda= 0.485432	 levenbergIter= 1
iteration= 16	 chi2= 468.065030	 time= 0.0351449	 cumTime= 0.627574	 edges= 6843	 schur= 0	 lambda= 0.323622	 levenbergIter= 1
iteration= 17	 chi2= 468.065029	 time= 0.0471594	 cumTime= 0.674733	 edges= 6843	 schur= 0	 lambda= 13.807856	 levenbergIter= 4
iteration= 18	 chi2= 468.065029	 time= 0.035124	 cumTime= 0.709857	 edges= 6843	 schur= 0	 lambda= 9.205237	 levenbergIter= 1
iteration= 19	 chi2= 468.065029	 time= 0.0351963	 cumTime= 0.745054	 edges= 6843	 schur= 0	 lambda= 6.136825	 levenbergIter= 1
iteration= 20	 chi2= 468.065029	 time= 0.0350088	 cumTime= 0.780063	 edges= 6843	 schur= 0	 lambda= 4.091217	 levenbergIter= 1
iteration= 21	 chi2= 468.065029	 time= 0.0350847	 cumTime= 0.815147	 edges= 6843	 schur= 0	 lambda= 2.727478	 levenbergIter= 1
iteration= 22	 chi2= 468.065029	 time= 0.0352051	 cumTime= 0.850352	 edges= 6843	 schur= 0	 lambda= 1.818318	 levenbergIter= 1
iteration= 23	 chi2= 468.065029	 time= 0.0349688	 cumTime= 0.885321	 edges= 6843	 schur= 0	 lambda= 1.212212	 levenbergIter= 1
iteration= 24	 chi2= 468.065029	 time= 0.035229	 cumTime= 0.92055	 edges= 6843	 schur= 0	 lambda= 0.808142	 levenbergIter= 1
iteration= 25	 chi2= 468.065029	 time= 0.0353063	 cumTime= 0.955856	 edges= 6843	 schur= 0	 lambda= 0.538761	 levenbergIter= 1
iteration= 26	 chi2= 468.065029	 time= 0.0350386	 cumTime= 0.990895	 edges= 6843	 schur= 0	 lambda= 0.359174	 levenbergIter= 1
iteration= 27	 chi2= 468.065029	 time= 0.0351134	 cumTime= 1.02601	 edges= 6843	 schur= 0	 lambda= 0.239449	 levenbergIter= 1
iteration= 28	 chi2= 468.065029	 time= 0.0351127	 cumTime= 1.06112	 edges= 6843	 schur= 0	 lambda= 0.159633	 levenbergIter= 1
iteration= 29	 chi2= 468.065029	 time= 0.034973	 cumTime= 1.09609	 edges= 6843	 schur= 0	 lambda= 0.106422	 levenbergIter= 1
iteration= 30	 chi2= 468.065029	 time= 0.0390458	 cumTime= 1.13514	 edges= 6843	 schur= 0	 lambda= 0.141896	 levenbergIter= 2
iteration= 31	 chi2= 468.065028	 time= 0.0350559	 cumTime= 1.1702	 edges= 6843	 schur= 0	 lambda= 0.094597	 levenbergIter= 1
iteration= 32	 chi2= 468.065028	 time= 0.0349438	 cumTime= 1.20514	 edges= 6843	 schur= 0	 lambda= 0.063065	 levenbergIter= 1
iteration= 33	 chi2= 468.065028	 time= 0.0472793	 cumTime= 1.25242	 edges= 6843	 schur= 0	 lambda= 2.690767	 levenbergIter= 4
iteration= 34	 chi2= 468.065028	 time= 0.0349359	 cumTime= 1.28735	 edges= 6843	 schur= 0	 lambda= 1.793845	 levenbergIter= 1
iteration= 35	 chi2= 468.065028	 time= 0.0350663	 cumTime= 1.32242	 edges= 6843	 schur= 0	 lambda= 1.195896	 levenbergIter= 1
iteration= 36	 chi2= 468.065028	 time= 0.0349947	 cumTime= 1.35742	 edges= 6843	 schur= 0	 lambda= 0.797264	 levenbergIter= 1
iteration= 37	 chi2= 468.065028	 time= 0.0350037	 cumTime= 1.39242	 edges= 6843	 schur= 0	 lambda= 0.531510	 levenbergIter= 1
iteration= 38	 chi2= 468.065028	 time= 0.0471563	 cumTime= 1.43958	 edges= 6843	 schur= 0	 lambda= 22.677739	 levenbergIter= 4
iteration= 39	 chi2= 468.065028	 time= 0.0349358	 cumTime= 1.47451	 edges= 6843	 schur= 0	 lambda= 15.118493	 levenbergIter= 1
iteration= 40	 chi2= 468.065028	 time= 0.0351167	 cumTime= 1.50963	 edges= 6843	 schur= 0	 lambda= 10.078995	 levenbergIter= 1
iteration= 41	 chi2= 468.065028	 time= 0.0350639	 cumTime= 1.54469	 edges= 6843	 schur= 0	 lambda= 6.719330	 levenbergIter= 1
iteration= 42	 chi2= 468.065028	 time= 0.0350967	 cumTime= 1.57979	 edges= 6843	 schur= 0	 lambda= 4.479553	 levenbergIter= 1
iteration= 43	 chi2= 468.065028	 time= 0.0350343	 cumTime= 1.61482	 edges= 6843	 schur= 0	 lambda= 2.986369	 levenbergIter= 1
iteration= 44	 chi2= 468.065028	 time= 0.0349	 cumTime= 1.64972	 edges= 6843	 schur= 0	 lambda= 1.990913	 levenbergIter= 1
iteration= 45	 chi2= 468.065028	 time= 0.0471932	 cumTime= 1.69692	 edges= 6843	 schur= 0	 lambda= 84.945606	 levenbergIter= 4
iteration= 46	 chi2= 468.065028	 time= 0.0350559	 cumTime= 1.73197	 edges= 6843	 schur= 0	 lambda= 56.630404	 levenbergIter= 1
iteration= 47	 chi2= 468.065028	 time= 0.035382	 cumTime= 1.76735	 edges= 6843	 schur= 0	 lambda= 37.753602	 levenbergIter= 1
iteration= 48	 chi2= 468.065028	 time= 0.0349866	 cumTime= 1.80234	 edges= 6843	 schur= 0	 lambda= 25.169068	 levenbergIter= 1
iteration= 49	 chi2= 468.065028	 time= 0.0351412	 cumTime= 1.83748	 edges= 6843	 schur= 0	 lambda= 16.779379	 levenbergIter= 1
iteration= 50	 chi2= 468.065028	 time= 0.0351712	 cumTime= 1.87265	 edges= 6843	 schur= 0	 lambda= 11.186253	 levenbergIter= 1
iteration= 51	 chi2= 468.065028	 time= 0.0349284	 cumTime= 1.90758	 edges= 6843	 schur= 0	 lambda= 7.457502	 levenbergIter= 1
iteration= 52	 chi2= 468.065028	 time= 0.0354512	 cumTime= 1.94303	 edges= 6843	 schur= 0	 lambda= 4.971668	 levenbergIter= 1
iteration= 53	 chi2= 468.065028	 time= 0.0350837	 cumTime= 1.97812	 edges= 6843	 schur= 0	 lambda= 3.314445	 levenbergIter= 1
iteration= 54	 chi2= 468.065028	 time= 0.0352127	 cumTime= 2.01333	 edges= 6843	 schur= 0	 lambda= 2.209630	 levenbergIter= 1
iteration= 55	 chi2= 468.065028	 time= 0.0349936	 cumTime= 2.04832	 edges= 6843	 schur= 0	 lambda= 1.473087	 levenbergIter= 1
iteration= 56	 chi2= 468.065028	 time= 0.0349023	 cumTime= 2.08323	 edges= 6843	 schur= 0	 lambda= 0.982058	 levenbergIter= 1
iteration= 57	 chi2= 468.065028	 time= 0.0470382	 cumTime= 2.13026	 edges= 6843	 schur= 0	 lambda= 41.901135	 levenbergIter= 4
iteration= 58	 chi2= 468.065028	 time= 0.0349818	 cumTime= 2.16525	 edges= 6843	 schur= 0	 lambda= 27.934090	 levenbergIter= 1
iteration= 59	 chi2= 468.065028	 time= 0.0349831	 cumTime= 2.20023	 edges= 6843	 schur= 0	 lambda= 18.622726	 levenbergIter= 1
iteration= 60	 chi2= 468.065028	 time= 0.0351551	 cumTime= 2.23538	 edges= 6843	 schur= 0	 lambda= 12.415151	 levenbergIter= 1
iteration= 61	 chi2= 468.065028	 time= 0.035053	 cumTime= 2.27044	 edges= 6843	 schur= 0	 lambda= 8.276767	 levenbergIter= 1
iteration= 62	 chi2= 468.065028	 time= 0.0350606	 cumTime= 2.3055	 edges= 6843	 schur= 0	 lambda= 5.517845	 levenbergIter= 1
iteration= 63	 chi2= 468.065028	 time= 0.0432361	 cumTime= 2.34873	 edges= 6843	 schur= 0	 lambda= 29.428506	 levenbergIter= 3
iteration= 64	 chi2= 468.065028	 time= 0.035078	 cumTime= 2.38381	 edges= 6843	 schur= 0	 lambda= 19.619004	 levenbergIter= 1
iteration= 65	 chi2= 468.065028	 time= 0.0431555	 cumTime= 2.42697	 edges= 6843	 schur= 0	 lambda= 104.634688	 levenbergIter= 3
iteration= 66	 chi2= 468.065028	 time= 0.0352503	 cumTime= 2.46222	 edges= 6843	 schur= 0	 lambda= 69.756459	 levenbergIter= 1
iteration= 67	 chi2= 468.065028	 time= 0.0350938	 cumTime= 2.49731	 edges= 6843	 schur= 0	 lambda= 46.504306	 levenbergIter= 1
iteration= 68	 chi2= 468.065028	 time= 0.0349346	 cumTime= 2.53225	 edges= 6843	 schur= 0	 lambda= 31.002871	 levenbergIter= 1
iteration= 69	 chi2= 468.065028	 time= 0.0350919	 cumTime= 2.56734	 edges= 6843	 schur= 0	 lambda= 20.668580	 levenbergIter= 1
iteration= 70	 chi2= 468.065028	 time= 0.0391162	 cumTime= 2.60645	 edges= 6843	 schur= 0	 lambda= 27.558107	 levenbergIter= 2
iteration= 71	 chi2= 468.065028	 time= 0.0349068	 cumTime= 2.64136	 edges= 6843	 schur= 0	 lambda= 18.372071	 levenbergIter= 1
iteration= 72	 chi2= 468.065028	 time= 0.0350233	 cumTime= 2.67638	 edges= 6843	 schur= 0	 lambda= 12.248048	 levenbergIter= 1
iteration= 73	 chi2= 468.065028	 time= 0.034933	 cumTime= 2.71132	 edges= 6843	 schur= 0	 lambda= 8.165365	 levenbergIter= 1
iteration= 74	 chi2= 468.065028	 time= 0.0390611	 cumTime= 2.75038	 edges= 6843	 schur= 0	 lambda= 10.887153	 levenbergIter= 2
iteration= 75	 chi2= 468.065028	 time= 0.0350384	 cumTime= 2.78542	 edges= 6843	 schur= 0	 lambda= 7.258102	 levenbergIter= 1
iteration= 76	 chi2= 468.065028	 time= 0.0349391	 cumTime= 2.82036	 edges= 6843	 schur= 0	 lambda= 4.838735	 levenbergIter= 1
iteration= 77	 chi2= 468.065028	 time= 0.0349793	 cumTime= 2.85533	 edges= 6843	 schur= 0	 lambda= 3.225823	 levenbergIter= 1
iteration= 78	 chi2= 468.065028	 time= 0.0350315	 cumTime= 2.89037	 edges= 6843	 schur= 0	 lambda= 2.150549	 levenbergIter= 1
iteration= 79	 chi2= 468.065028	 time= 0.0354239	 cumTime= 2.92579	 edges= 6843	 schur= 0	 lambda= 1.433699	 levenbergIter= 1
iteration= 80	 chi2= 468.065028	 time= 0.0470712	 cumTime= 2.97286	 edges= 6843	 schur= 0	 lambda= 61.171167	 levenbergIter= 4
iteration= 81	 chi2= 468.065028	 time= 0.0350588	 cumTime= 3.00792	 edges= 6843	 schur= 0	 lambda= 40.780778	 levenbergIter= 1
iteration= 82	 chi2= 468.065028	 time= 0.0349826	 cumTime= 3.0429	 edges= 6843	 schur= 0	 lambda= 27.187185	 levenbergIter= 1
iteration= 83	 chi2= 468.065028	 time= 0.0349392	 cumTime= 3.07784	 edges= 6843	 schur= 0	 lambda= 18.124790	 levenbergIter= 1
iteration= 84	 chi2= 468.065028	 time= 0.0349629	 cumTime= 3.1128	 edges= 6843	 schur= 0	 lambda= 12.083193	 levenbergIter= 1
iteration= 85	 chi2= 468.065028	 time= 0.0352133	 cumTime= 3.14802	 edges= 6843	 schur= 0	 lambda= 8.055462	 levenbergIter= 1
iteration= 86	 chi2= 468.065028	 time= 0.0351204	 cumTime= 3.18314	 edges= 6843	 schur= 0	 lambda= 5.370308	 levenbergIter= 1
iteration= 87	 chi2= 468.065028	 time= 0.0430952	 cumTime= 3.22623	 edges= 6843	 schur= 0	 lambda= 28.641644	 levenbergIter= 3
iteration= 88	 chi2= 468.065028	 time= 0.0352413	 cumTime= 3.26147	 edges= 6843	 schur= 0	 lambda= 19.094429	 levenbergIter= 1
iteration= 89	 chi2= 468.065028	 time= 0.0351148	 cumTime= 3.29659	 edges= 6843	 schur= 0	 lambda= 12.729619	 levenbergIter= 1
iteration= 90	 chi2= 468.065028	 time= 0.0433217	 cumTime= 3.33991	 edges= 6843	 schur= 0	 lambda= 67.891303	 levenbergIter= 3
iteration= 91	 chi2= 468.065028	 time= 0.0350147	 cumTime= 3.37493	 edges= 6843	 schur= 0	 lambda= 45.260869	 levenbergIter= 1
iteration= 92	 chi2= 468.065028	 time= 0.0431807	 cumTime= 3.41811	 edges= 6843	 schur= 0	 lambda= 241.391301	 levenbergIter= 3
iteration= 93	 chi2= 468.065028	 time= 0.0349405	 cumTime= 3.45305	 edges= 6843	 schur= 0	 lambda= 160.927534	 levenbergIter= 1
iteration= 94	 chi2= 468.065028	 time= 0.0349453	 cumTime= 3.48799	 edges= 6843	 schur= 0	 lambda= 107.285023	 levenbergIter= 1
iteration= 95	 chi2= 468.065028	 time= 0.0390688	 cumTime= 3.52706	 edges= 6843	 schur= 0	 lambda= 143.046697	 levenbergIter= 2
iteration= 96	 chi2= 468.065028	 time= 0.0350537	 cumTime= 3.56212	 edges= 6843	 schur= 0	 lambda= 95.364465	 levenbergIter= 1
iteration= 97	 chi2= 468.065028	 time= 0.0429644	 cumTime= 3.60508	 edges= 6843	 schur= 0	 lambda= 508.610478	 levenbergIter= 3
iteration= 98	 chi2= 468.065028	 time= 0.0350958	 cumTime= 3.64018	 edges= 6843	 schur= 0	 lambda= 339.073652	 levenbergIter= 1
iteration= 99	 chi2= 468.065028	 time= 0.0350706	 cumTime= 3.67525	 edges= 6843	 schur= 0	 lambda= 226.049101	 levenbergIter= 1
I0114 13:20:39.189853 421782 optimization.cc:255] RTK 誤差：
I0114 13:20:39.189865 421782 optimization.cc:256] RTK 平移誤差：數量: 1143, 均值: 5.423604, 中位數: 0.064771, 0.1分位: 0.008310, 0.9分位: 0.498921, 0.95分位：32.240261, 最大值: 173.001502, 閾值: 1.000000
I0114 13:20:39.189934 421782 optimization.cc:257] lidar 誤差：數量: 5700, 均值: 0.002259, 中位數: 0.000477, 0.1分位: 0.000013, 0.9分位: 0.007259, 0.95分位：0.011946, 最大值: 0.028087, 閾值: 0.000000
I0114 13:20:39.191107 421782 optimization.cc:258] loop 誤差：
I0114 13:20:39.191128 421782 optimization.cc:278] gnss outlier: 103/1143
I0114 13:20:39.191130 421782 optimization.cc:283] loop outlier: 0/0
iteration= 0	 chi2= 110.934465	 time= 0.0470475	 cumTime= 0.0470475	 edges= 6740	 schur= 0	 lambda= 13.285428	 levenbergIter= 1
iteration= 1	 chi2= 110.909744	 time= 0.0351188	 cumTime= 0.0821663	 edges= 6740	 schur= 0	 lambda= 4.428476	 levenbergIter= 1
iteration= 2	 chi2= 110.870717	 time= 0.0348684	 cumTime= 0.117035	 edges= 6740	 schur= 0	 lambda= 1.476159	 levenbergIter= 1
iteration= 3	 chi2= 110.825776	 time= 0.0349312	 cumTime= 0.151966	 edges= 6740	 schur= 0	 lambda= 0.492053	 levenbergIter= 1
iteration= 4	 chi2= 110.792967	 time= 0.0351164	 cumTime= 0.187082	 edges= 6740	 schur= 0	 lambda= 0.164018	 levenbergIter= 1
iteration= 5	 chi2= 110.779861	 time= 0.0354022	 cumTime= 0.222484	 edges= 6740	 schur= 0	 lambda= 0.061012	 levenbergIter= 1
iteration= 6	 chi2= 110.777612	 time= 0.0354614	 cumTime= 0.257946	 edges= 6740	 schur= 0	 lambda= 0.040675	 levenbergIter= 1
iteration= 7	 chi2= 110.777317	 time= 0.0350879	 cumTime= 0.293034	 edges= 6740	 schur= 0	 lambda= 0.027116	 levenbergIter= 1
iteration= 8	 chi2= 110.777119	 time= 0.0350583	 cumTime= 0.328092	 edges= 6740	 schur= 0	 lambda= 0.018078	 levenbergIter= 1
iteration= 9	 chi2= 110.776952	 time= 0.0351737	 cumTime= 0.363266	 edges= 6740	 schur= 0	 lambda= 0.012052	 levenbergIter= 1
iteration= 10	 chi2= 110.776786	 time= 0.035232	 cumTime= 0.398498	 edges= 6740	 schur= 0	 lambda= 0.008034	 levenbergIter= 1
iteration= 11	 chi2= 110.776656	 time= 0.0352243	 cumTime= 0.433722	 edges= 6740	 schur= 0	 lambda= 0.005356	 levenbergIter= 1
iteration= 12	 chi2= 110.776583	 time= 0.035121	 cumTime= 0.468843	 edges= 6740	 schur= 0	 lambda= 0.003571	 levenbergIter= 1
iteration= 13	 chi2= 110.776518	 time= 0.0351649	 cumTime= 0.504008	 edges= 6740	 schur= 0	 lambda= 0.002381	 levenbergIter= 1
iteration= 14	 chi2= 110.776499	 time= 0.035041	 cumTime= 0.539049	 edges= 6740	 schur= 0	 lambda= 0.001587	 levenbergIter= 1
iteration= 15	 chi2= 110.776499	 time= 0.0470622	 cumTime= 0.586111	 edges= 6740	 schur= 0	 lambda= 0.067715	 levenbergIter= 4
iteration= 16	 chi2= 110.776499	 time= 0.0351206	 cumTime= 0.621232	 edges= 6740	 schur= 0	 lambda= 0.045143	 levenbergIter= 1
iteration= 17	 chi2= 110.776499	 time= 0.0349234	 cumTime= 0.656155	 edges= 6740	 schur= 0	 lambda= 0.030095	 levenbergIter= 1
iteration= 18	 chi2= 110.776499	 time= 0.0351566	 cumTime= 0.691312	 edges= 6740	 schur= 0	 lambda= 0.020064	 levenbergIter= 1
iteration= 19	 chi2= 110.776499	 time= 0.0391649	 cumTime= 0.730476	 edges= 6740	 schur= 0	 lambda= 0.026751	 levenbergIter= 2
iteration= 20	 chi2= 110.776499	 time= 0.0431754	 cumTime= 0.773652	 edges= 6740	 schur= 0	 lambda= 0.142674	 levenbergIter= 3
iteration= 21	 chi2= 110.776499	 time= 0.0388819	 cumTime= 0.812534	 edges= 6740	 schur= 0	 lambda= 0.190232	 levenbergIter= 2
iteration= 22	 chi2= 110.776498	 time= 0.0355803	 cumTime= 0.848114	 edges= 6740	 schur= 0	 lambda= 0.126822	 levenbergIter= 1
iteration= 23	 chi2= 110.776498	 time= 0.0352904	 cumTime= 0.883404	 edges= 6740	 schur= 0	 lambda= 0.084548	 levenbergIter= 1
iteration= 24	 chi2= 110.776498	 time= 0.0350743	 cumTime= 0.918479	 edges= 6740	 schur= 0	 lambda= 0.056365	 levenbergIter= 1
iteration= 25	 chi2= 110.776498	 time= 0.0432064	 cumTime= 0.961685	 edges= 6740	 schur= 0	 lambda= 0.300614	 levenbergIter= 3
iteration= 26	 chi2= 110.776498	 time= 0.0350856	 cumTime= 0.996771	 edges= 6740	 schur= 0	 lambda= 0.200409	 levenbergIter= 1
iteration= 27	 chi2= 110.776498	 time= 0.0511913	 cumTime= 1.04796	 edges= 6740	 schur= 0	 lambda= 136.812830	 levenbergIter= 5
iteration= 28	 chi2= 110.776498	 time= 0.0352956	 cumTime= 1.08326	 edges= 6740	 schur= 0	 lambda= 91.208553	 levenbergIter= 1
iteration= 29	 chi2= 110.776498	 time= 0.0350566	 cumTime= 1.11831	 edges= 6740	 schur= 0	 lambda= 60.805702	 levenbergIter= 1
iteration= 30	 chi2= 110.776498	 time= 0.0393775	 cumTime= 1.15769	 edges= 6740	 schur= 0	 lambda= 81.074269	 levenbergIter= 2
iteration= 31	 chi2= 110.776498	 time= 0.0350615	 cumTime= 1.19275	 edges= 6740	 schur= 0	 lambda= 54.049513	 levenbergIter= 1
iteration= 32	 chi2= 110.776498	 time= 0.0349912	 cumTime= 1.22774	 edges= 6740	 schur= 0	 lambda= 36.033009	 levenbergIter= 1
iteration= 33	 chi2= 110.776498	 time= 0.0349487	 cumTime= 1.26269	 edges= 6740	 schur= 0	 lambda= 24.022006	 levenbergIter= 1
iteration= 34	 chi2= 110.776498	 time= 0.0350243	 cumTime= 1.29772	 edges= 6740	 schur= 0	 lambda= 16.014671	 levenbergIter= 1
iteration= 35	 chi2= 110.776498	 time= 0.0429543	 cumTime= 1.34067	 edges= 6740	 schur= 0	 lambda= 85.411576	 levenbergIter= 3
iteration= 36	 chi2= 110.776498	 time= 0.0351099	 cumTime= 1.37578	 edges= 6740	 schur= 0	 lambda= 56.941051	 levenbergIter= 1
iteration= 37	 chi2= 110.776498	 time= 0.0350871	 cumTime= 1.41087	 edges= 6740	 schur= 0	 lambda= 37.960700	 levenbergIter= 1
iteration= 38	 chi2= 110.776498	 time= 0.0352381	 cumTime= 1.44611	 edges= 6740	 schur= 0	 lambda= 25.307134	 levenbergIter= 1
iteration= 39	 chi2= 110.776498	 time= 0.043321	 cumTime= 1.48943	 edges= 6740	 schur= 0	 lambda= 134.971379	 levenbergIter= 3
iteration= 40	 chi2= 110.776498	 time= 0.0352493	 cumTime= 1.52468	 edges= 6740	 schur= 0	 lambda= 89.980920	 levenbergIter= 1
iteration= 41	 chi2= 110.776498	 time= 0.0353187	 cumTime= 1.56	 edges= 6740	 schur= 0	 lambda= 59.987280	 levenbergIter= 1
iteration= 42	 chi2= 110.776498	 time= 0.0394095	 cumTime= 1.59941	 edges= 6740	 schur= 0	 lambda= 79.983040	 levenbergIter= 2
iteration= 43	 chi2= 110.776498	 time= 0.0350835	 cumTime= 1.63449	 edges= 6740	 schur= 0	 lambda= 53.322026	 levenbergIter= 1
iteration= 44	 chi2= 110.776498	 time= 0.0397917	 cumTime= 1.67428	 edges= 6740	 schur= 0	 lambda= 71.096035	 levenbergIter= 2
iteration= 45	 chi2= 110.776498	 time= 0.0351542	 cumTime= 1.70943	 edges= 6740	 schur= 0	 lambda= 47.397357	 levenbergIter= 1
iteration= 46	 chi2= 110.776498	 time= 0.0349756	 cumTime= 1.74441	 edges= 6740	 schur= 0	 lambda= 31.598238	 levenbergIter= 1
iteration= 47	 chi2= 110.776498	 time= 0.0350925	 cumTime= 1.7795	 edges= 6740	 schur= 0	 lambda= 21.065492	 levenbergIter= 1
iteration= 48	 chi2= 110.776498	 time= 0.0351852	 cumTime= 1.81469	 edges= 6740	 schur= 0	 lambda= 14.043661	 levenbergIter= 1
iteration= 49	 chi2= 110.776498	 time= 0.0350873	 cumTime= 1.84978	 edges= 6740	 schur= 0	 lambda= 9.362441	 levenbergIter= 1
iteration= 50	 chi2= 110.776498	 time= 0.0352412	 cumTime= 1.88502	 edges= 6740	 schur= 0	 lambda= 6.241627	 levenbergIter= 1
iteration= 51	 chi2= 110.776498	 time= 0.0431349	 cumTime= 1.92815	 edges= 6740	 schur= 0	 lambda= 33.288679	 levenbergIter= 3
iteration= 52	 chi2= 110.776498	 time= 0.0351016	 cumTime= 1.96325	 edges= 6740	 schur= 0	 lambda= 22.192452	 levenbergIter= 1
iteration= 53	 chi2= 110.776498	 time= 0.0353896	 cumTime= 1.99864	 edges= 6740	 schur= 0	 lambda= 14.794968	 levenbergIter= 1
iteration= 54	 chi2= 110.776498	 time= 0.0431881	 cumTime= 2.04183	 edges= 6740	 schur= 0	 lambda= 78.906497	 levenbergIter= 3
iteration= 55	 chi2= 110.776498	 time= 0.0359193	 cumTime= 2.07775	 edges= 6740	 schur= 0	 lambda= 52.604332	 levenbergIter= 1
iteration= 56	 chi2= 110.776498	 time= 0.0432479	 cumTime= 2.121	 edges= 6740	 schur= 0	 lambda= 280.556435	 levenbergIter= 3
iteration= 57	 chi2= 110.776498	 time= 0.0352556	 cumTime= 2.15625	 edges= 6740	 schur= 0	 lambda= 187.037624	 levenbergIter= 1
iteration= 58	 chi2= 110.776498	 time= 0.0352096	 cumTime= 2.19146	 edges= 6740	 schur= 0	 lambda= 124.691749	 levenbergIter= 1
iteration= 59	 chi2= 110.776498	 time= 0.0357311	 cumTime= 2.22719	 edges= 6740	 schur= 0	 lambda= 83.127833	 levenbergIter= 1
iteration= 60	 chi2= 110.776498	 time= 0.0352255	 cumTime= 2.26242	 edges= 6740	 schur= 0	 lambda= 55.418555	 levenbergIter= 1
iteration= 61	 chi2= 110.776498	 time= 0.0356248	 cumTime= 2.29804	 edges= 6740	 schur= 0	 lambda= 36.945703	 levenbergIter= 1
iteration= 62	 chi2= 110.776498	 time= 0.0355421	 cumTime= 2.33359	 edges= 6740	 schur= 0	 lambda= 24.630469	 levenbergIter= 1
iteration= 63	 chi2= 110.776498	 time= 0.0437838	 cumTime= 2.37737	 edges= 6740	 schur= 0	 lambda= 131.362501	 levenbergIter= 3
iteration= 64	 chi2= 110.776498	 time= 0.0355388	 cumTime= 2.41291	 edges= 6740	 schur= 0	 lambda= 87.575001	 levenbergIter= 1
iteration= 65	 chi2= 110.776498	 time= 0.0438357	 cumTime= 2.45674	 edges= 6740	 schur= 0	 lambda= 467.066670	 levenbergIter= 3
iteration= 66	 chi2= 110.776498	 time= 0.0354844	 cumTime= 2.49223	 edges= 6740	 schur= 0	 lambda= 311.377780	 levenbergIter= 1
iteration= 67	 chi2= 110.776498	 time= 0.0351674	 cumTime= 2.5274	 edges= 6740	 schur= 0	 lambda= 207.585187	 levenbergIter= 1
iteration= 68	 chi2= 110.776498	 time= 0.035127	 cumTime= 2.56252	 edges= 6740	 schur= 0	 lambda= 138.390125	 levenbergIter= 1
iteration= 69	 chi2= 110.776498	 time= 0.0393006	 cumTime= 2.60182	 edges= 6740	 schur= 0	 lambda= 184.520166	 levenbergIter= 2
iteration= 70	 chi2= 110.776498	 time= 0.0391779	 cumTime= 2.641	 edges= 6740	 schur= 0	 lambda= 246.026888	 levenbergIter= 2
iteration= 71	 chi2= 110.776498	 time= 0.0433099	 cumTime= 2.68431	 edges= 6740	 schur= 0	 lambda= 1312.143403	 levenbergIter= 3
iteration= 72	 chi2= 110.776498	 time= 0.035241	 cumTime= 2.71955	 edges= 6740	 schur= 0	 lambda= 874.762269	 levenbergIter= 1
iteration= 73	 chi2= 110.776498	 time= 0.035148	 cumTime= 2.7547	 edges= 6740	 schur= 0	 lambda= 583.174846	 levenbergIter= 1
iteration= 74	 chi2= 110.776498	 time= 0.0355041	 cumTime= 2.79021	 edges= 6740	 schur= 0	 lambda= 388.783231	 levenbergIter= 1
iteration= 75	 chi2= 110.776498	 time= 0.0353253	 cumTime= 2.82553	 edges= 6740	 schur= 0	 lambda= 259.188820	 levenbergIter= 1
iteration= 76	 chi2= 110.776498	 time= 0.0353136	 cumTime= 2.86084	 edges= 6740	 schur= 0	 lambda= 172.792547	 levenbergIter= 1
iteration= 77	 chi2= 110.776498	 time= 0.0435316	 cumTime= 2.90438	 edges= 6740	 schur= 0	 lambda= 921.560250	 levenbergIter= 3
iteration= 78	 chi2= 110.776498	 time= 0.0465395	 cumTime= 2.95092	 edges= 6740	 schur= 0	 lambda= 4914.988002	 levenbergIter= 3
iteration= 79	 chi2= 110.776498	 time= 0.0358473	 cumTime= 2.98676	 edges= 6740	 schur= 0	 lambda= 3276.658668	 levenbergIter= 1
iteration= 80	 chi2= 110.776498	 time= 0.0396393	 cumTime= 3.0264	 edges= 6740	 schur= 0	 lambda= 4368.878224	 levenbergIter= 2
iteration= 81	 chi2= 110.776498	 time= 0.0353733	 cumTime= 3.06178	 edges= 6740	 schur= 0	 lambda= 2912.585483	 levenbergIter= 1
iteration= 82	 chi2= 110.776498	 time= 0.0397982	 cumTime= 3.10157	 edges= 6740	 schur= 0	 lambda= 3883.447310	 levenbergIter= 2
iteration= 83	 chi2= 110.776498	 time= 0.039892	 cumTime= 3.14147	 edges= 6740	 schur= 0	 lambda= 5177.929747	 levenbergIter= 2
iteration= 84	 chi2= 110.776498	 time= 0.0355231	 cumTime= 3.17699	 edges= 6740	 schur= 0	 lambda= 3451.953165	 levenbergIter= 1
iteration= 85	 chi2= 110.776498	 time= 0.0438638	 cumTime= 3.22085	 edges= 6740	 schur= 0	 lambda= 18410.416878	 levenbergIter= 3
iteration= 86	 chi2= 110.776498	 time= 0.0437571	 cumTime= 3.26461	 edges= 6740	 schur= 0	 lambda= 98188.890014	 levenbergIter= 3
iteration= 87	 chi2= 110.776498	 time= 0.0354665	 cumTime= 3.30008	 edges= 6740	 schur= 0	 lambda= 65459.260010	 levenbergIter= 1
iteration= 88	 chi2= 110.776498	 time= 0.0390789	 cumTime= 3.33915	 edges= 6740	 schur= 0	 lambda= 87279.013346	 levenbergIter= 2
iteration= 89	 chi2= 110.776498	 time= 0.0391281	 cumTime= 3.37828	 edges= 6740	 schur= 0	 lambda= 116372.017795	 levenbergIter= 2
iteration= 90	 chi2= 110.776498	 time= 0.0392514	 cumTime= 3.41753	 edges= 6740	 schur= 0	 lambda= 155162.690393	 levenbergIter= 2
iteration= 91	 chi2= 110.776498	 time= 0.0388398	 cumTime= 3.45637	 edges= 6740	 schur= 0	 lambda= 206883.587191	 levenbergIter= 2
iteration= 92	 chi2= 110.776498	 time= 0.0432031	 cumTime= 3.49958	 edges= 6740	 schur= 0	 lambda= 1103379.131683	 levenbergIter= 3
iteration= 93	 chi2= 110.776498	 time= 0.0431585	 cumTime= 3.54274	 edges= 6740	 schur= 0	 lambda= 5884688.702312	 levenbergIter= 3
iteration= 94	 chi2= 110.776498	 time= 0.0554472	 cumTime= 3.59818	 edges= 6740	 schur= 0	 lambda= 128552986264.900513	 levenbergIter= 6
iteration= 95	 chi2= 110.776498	 time= 0.0509725	 cumTime= 3.64916	 edges= 6740	 schur= 0	 lambda= 87758838623505.406250	 levenbergIter= 5
iteration= 96	 chi2= 110.776498	 time= 0.0730046	 cumTime= 3.72216	 edges= 6740	 schur= 0	 lambda= 3161845383386291517557101821952.000000	 levenbergIter= 10
I0114 13:20:43.021499 421782 optimization.cc:255] RTK 誤差：
I0114 13:20:43.021515 421782 optimization.cc:256] RTK 平移誤差：數量: 1040, 均值: 0.091952, 中位數: 0.046066, 0.1分位: 0.007426, 0.9分位: 0.227438, 0.95分位：0.305799, 最大值: 0.964747, 閾值: 1.000000
I0114 13:20:43.021579 421782 optimization.cc:257] lidar 誤差：數量: 5700, 均值: 0.002657, 中位數: 0.000573, 0.1分位: 0.000015, 0.9分位: 0.008337, 0.95分位：0.013616, 最大值: 0.032128, 閾值: 0.000000
I0114 13:20:43.022814 421782 optimization.cc:258] loop 誤差：
I0114 13:20:43.061812 421782 optimization.cc:306] med error: 0.496898
I0114 13:20:43.108528 421782 optimization.cc:96] done

wu@WP:~/slam_in_autonomous_driving/data/ch9$ g2o_viewer before.g2o

wu@WP:~/slam_in_autonomous_driving/data/ch9$ g2o_viewer after.g2o

? ? ? ? ?② 第二階段優化結果

? ? ? ? 注意：完成回環檢測后才進行第二階段優化。

wu@WP:~/slam_in_autonomous_driving/bin$ ./run_optimization --stage=2I0114 14:47:06.440253 425119 run_optimization.cc:23] testing optimization
I0114 14:47:06.444443 425119 keyframe.cc:78] Loaded kfs: 1143
I0114 14:47:06.444453 425119 optimization.cc:52] keyframes: 1143
I0114 14:47:06.444612 425119 optimization.cc:66] TBG = 
000001 000000 000000 000000
000000 000001 000000 000.24
000000 000000 000001 -0.283
000000 000000 000000 000001
I0114 14:47:06.444928 425119 optimization.cc:401] loaded loops: 249
I0114 14:47:06.444943 425119 optimization.cc:76] running optimization on stage 2
I0114 14:47:06.445127 425119 optimization.cc:146] vertex: 1143
I0114 14:47:06.445129 425119 optimization.cc:158] Info of rtk trans: 000.25 000.25 0.0025
I0114 14:47:06.445425 425119 optimization.cc:191] gnss edges: 0, 1143
I0114 14:47:06.447690 425119 optimization.cc:221] lidar edges: 5700
I0114 14:47:06.498154 425119 optimization.cc:85] RTK 誤差：
I0114 14:47:06.498178 425119 optimization.cc:86] RTK 平移誤差：數量: 1143, 均值: 0.054123, 中位數: 0.000632, 0.1分位: 0.000078, 0.9分位: 0.004924, 0.95分位：0.321209, 最大值: 1.727218, 閾值: 1.000000
I0114 14:47:06.498303 425119 optimization.cc:87] lidar 誤差：數量: 5700, 均值: 0.002657, 中位數: 0.000573, 0.1分位: 0.000015, 0.9分位: 0.008335, 0.95分位：0.013617, 最大值: 0.032125, 閾值: 0.000000
iteration= 0	 chi2= 732.273825	 time= 0.0669518	 cumTime= 0.0669518	 edges= 7092	 schur= 0	 lambda= 14.600698	 levenbergIter= 1
iteration= 1	 chi2= 692.103366	 time= 0.0427196	 cumTime= 0.109671	 edges= 7092	 schur= 0	 lambda= 4.866899	 levenbergIter= 1
iteration= 2	 chi2= 683.780205	 time= 0.044161	 cumTime= 0.153832	 edges= 7092	 schur= 0	 lambda= 1.622300	 levenbergIter= 1
iteration= 3	 chi2= 676.140472	 time= 0.042287	 cumTime= 0.196119	 edges= 7092	 schur= 0	 lambda= 0.540767	 levenbergIter= 1
iteration= 4	 chi2= 668.287330	 time= 0.0447868	 cumTime= 0.240906	 edges= 7092	 schur= 0	 lambda= 0.180256	 levenbergIter= 1
iteration= 5	 chi2= 663.433612	 time= 0.042204	 cumTime= 0.28311	 edges= 7092	 schur= 0	 lambda= 0.060085	 levenbergIter= 1
iteration= 6	 chi2= 661.863390	 time= 0.0435874	 cumTime= 0.326697	 edges= 7092	 schur= 0	 lambda= 0.020028	 levenbergIter= 1
iteration= 7	 chi2= 661.592309	 time= 0.0476941	 cumTime= 0.374392	 edges= 7092	 schur= 0	 lambda= 0.006676	 levenbergIter= 1
iteration= 8	 chi2= 661.365199	 time= 0.0439028	 cumTime= 0.418294	 edges= 7092	 schur= 0	 lambda= 0.002225	 levenbergIter= 1
iteration= 9	 chi2= 661.154271	 time= 0.0424439	 cumTime= 0.460738	 edges= 7092	 schur= 0	 lambda= 0.000742	 levenbergIter= 1
iteration= 10	 chi2= 661.091033	 time= 0.044019	 cumTime= 0.504757	 edges= 7092	 schur= 0	 lambda= 0.000257	 levenbergIter= 1
iteration= 11	 chi2= 661.085559	 time= 0.0424286	 cumTime= 0.547186	 edges= 7092	 schur= 0	 lambda= 0.000171	 levenbergIter= 1
iteration= 12	 chi2= 661.084967	 time= 0.0433414	 cumTime= 0.590527	 edges= 7092	 schur= 0	 lambda= 0.000114	 levenbergIter= 1
iteration= 13	 chi2= 661.082050	 time= 0.0438021	 cumTime= 0.634329	 edges= 7092	 schur= 0	 lambda= 0.000076	 levenbergIter= 1
iteration= 14	 chi2= 661.081452	 time= 0.0514515	 cumTime= 0.685781	 edges= 7092	 schur= 0	 lambda= 0.000102	 levenbergIter= 2
iteration= 15	 chi2= 661.080845	 time= 0.059497	 cumTime= 0.745278	 edges= 7092	 schur= 0	 lambda= 0.000541	 levenbergIter= 3
iteration= 16	 chi2= 661.080815	 time= 0.0684689	 cumTime= 0.813747	 edges= 7092	 schur= 0	 lambda= 0.023099	 levenbergIter= 4
iteration= 17	 chi2= 661.080765	 time= 0.0422402	 cumTime= 0.855987	 edges= 7092	 schur= 0	 lambda= 0.015399	 levenbergIter= 1
iteration= 18	 chi2= 661.080750	 time= 0.0428142	 cumTime= 0.898801	 edges= 7092	 schur= 0	 lambda= 0.010266	 levenbergIter= 1
iteration= 19	 chi2= 661.080717	 time= 0.0429474	 cumTime= 0.941749	 edges= 7092	 schur= 0	 lambda= 0.006844	 levenbergIter= 1
iteration= 20	 chi2= 661.080716	 time= 0.0430636	 cumTime= 0.984812	 edges= 7092	 schur= 0	 lambda= 0.004563	 levenbergIter= 1
iteration= 21	 chi2= 661.080708	 time= 0.0417634	 cumTime= 1.02658	 edges= 7092	 schur= 0	 lambda= 0.003042	 levenbergIter= 1
iteration= 22	 chi2= 661.080707	 time= 0.0790561	 cumTime= 1.10563	 edges= 7092	 schur= 0	 lambda= 2.076534	 levenbergIter= 5
iteration= 23	 chi2= 661.080707	 time= 0.0417019	 cumTime= 1.14733	 edges= 7092	 schur= 0	 lambda= 1.384356	 levenbergIter= 1
iteration= 24	 chi2= 661.080707	 time= 0.0437479	 cumTime= 1.19108	 edges= 7092	 schur= 0	 lambda= 0.922904	 levenbergIter= 1
iteration= 25	 chi2= 661.080707	 time= 0.0679209	 cumTime= 1.259	 edges= 7092	 schur= 0	 lambda= 39.377243	 levenbergIter= 4
iteration= 26	 chi2= 661.080707	 time= 0.0683598	 cumTime= 1.32736	 edges= 7092	 schur= 0	 lambda= 1680.095684	 levenbergIter= 4
iteration= 27	 chi2= 661.080707	 time= 0.0412889	 cumTime= 1.36865	 edges= 7092	 schur= 0	 lambda= 1120.063789	 levenbergIter= 1
iteration= 28	 chi2= 661.080707	 time= 0.0605974	 cumTime= 1.42925	 edges= 7092	 schur= 0	 lambda= 5973.673543	 levenbergIter= 3
iteration= 29	 chi2= 661.080707	 time= 0.0507893	 cumTime= 1.48004	 edges= 7092	 schur= 0	 lambda= 7964.898058	 levenbergIter= 2
iteration= 30	 chi2= 661.080707	 time= 0.0432925	 cumTime= 1.52333	 edges= 7092	 schur= 0	 lambda= 5309.932038	 levenbergIter= 1
iteration= 31	 chi2= 661.080707	 time= 0.0418494	 cumTime= 1.56518	 edges= 7092	 schur= 0	 lambda= 3539.954692	 levenbergIter= 1
iteration= 32	 chi2= 661.080707	 time= 0.0520767	 cumTime= 1.61726	 edges= 7092	 schur= 0	 lambda= 4719.939590	 levenbergIter= 2
iteration= 33	 chi2= 661.080707	 time= 0.0429237	 cumTime= 1.66018	 edges= 7092	 schur= 0	 lambda= 3146.626393	 levenbergIter= 1
iteration= 34	 chi2= 661.080707	 time= 0.0599151	 cumTime= 1.7201	 edges= 7092	 schur= 0	 lambda= 16782.007430	 levenbergIter= 3
iteration= 35	 chi2= 661.080707	 time= 0.0419952	 cumTime= 1.76209	 edges= 7092	 schur= 0	 lambda= 11188.004953	 levenbergIter= 1
iteration= 36	 chi2= 661.080707	 time= 0.0519325	 cumTime= 1.81402	 edges= 7092	 schur= 0	 lambda= 14917.339938	 levenbergIter= 2
iteration= 37	 chi2= 661.080707	 time= 0.0427233	 cumTime= 1.85675	 edges= 7092	 schur= 0	 lambda= 9944.893292	 levenbergIter= 1
iteration= 38	 chi2= 661.080707	 time= 0.0515514	 cumTime= 1.9083	 edges= 7092	 schur= 0	 lambda= 13259.857723	 levenbergIter= 2
iteration= 39	 chi2= 661.080707	 time= 0.0514173	 cumTime= 1.95971	 edges= 7092	 schur= 0	 lambda= 17679.810297	 levenbergIter= 2
iteration= 40	 chi2= 661.080707	 time= 0.0605415	 cumTime= 2.02026	 edges= 7092	 schur= 0	 lambda= 94292.321583	 levenbergIter= 3
iteration= 41	 chi2= 661.080707	 time= 0.0431446	 cumTime= 2.0634	 edges= 7092	 schur= 0	 lambda= 62861.547722	 levenbergIter= 1
iteration= 42	 chi2= 661.080707	 time= 0.0520464	 cumTime= 2.11545	 edges= 7092	 schur= 0	 lambda= 83815.396963	 levenbergIter= 2
iteration= 43	 chi2= 661.080707	 time= 0.0529544	 cumTime= 2.1684	 edges= 7092	 schur= 0	 lambda= 111753.862617	 levenbergIter= 2
iteration= 44	 chi2= 661.080707	 time= 0.0520558	 cumTime= 2.22046	 edges= 7092	 schur= 0	 lambda= 149005.150156	 levenbergIter= 2
iteration= 45	 chi2= 661.080707	 time= 0.0427795	 cumTime= 2.26324	 edges= 7092	 schur= 0	 lambda= 99336.766771	 levenbergIter= 1
iteration= 46	 chi2= 661.080707	 time= 0.0538824	 cumTime= 2.31712	 edges= 7092	 schur= 0	 lambda= 132449.022361	 levenbergIter= 2
iteration= 47	 chi2= 661.080707	 time= 0.0512105	 cumTime= 2.36833	 edges= 7092	 schur= 0	 lambda= 176598.696481	 levenbergIter= 2
iteration= 48	 chi2= 661.080707	 time= 0.0517327	 cumTime= 2.42006	 edges= 7092	 schur= 0	 lambda= 235464.928641	 levenbergIter= 2
iteration= 49	 chi2= 661.080707	 time= 0.0512654	 cumTime= 2.47133	 edges= 7092	 schur= 0	 lambda= 313953.238189	 levenbergIter= 2
iteration= 50	 chi2= 661.080707	 time= 0.0433526	 cumTime= 2.51468	 edges= 7092	 schur= 0	 lambda= 209302.158792	 levenbergIter= 1
iteration= 51	 chi2= 661.080707	 time= 0.0484793	 cumTime= 2.56316	 edges= 7092	 schur= 0	 lambda= 279069.545056	 levenbergIter= 2
iteration= 52	 chi2= 661.080707	 time= 0.0483123	 cumTime= 2.61147	 edges= 7092	 schur= 0	 lambda= 372092.726742	 levenbergIter= 2
iteration= 53	 chi2= 661.080707	 time= 0.0404669	 cumTime= 2.65194	 edges= 7092	 schur= 0	 lambda= 248061.817828	 levenbergIter= 1
iteration= 54	 chi2= 661.080707	 time= 0.048849	 cumTime= 2.70079	 edges= 7092	 schur= 0	 lambda= 330749.090437	 levenbergIter= 2
iteration= 55	 chi2= 661.080707	 time= 0.0485885	 cumTime= 2.74938	 edges= 7092	 schur= 0	 lambda= 440998.787250	 levenbergIter= 2
iteration= 56	 chi2= 661.080707	 time= 0.0402083	 cumTime= 2.78959	 edges= 7092	 schur= 0	 lambda= 293999.191500	 levenbergIter= 1
iteration= 57	 chi2= 661.080707	 time= 0.0484342	 cumTime= 2.83802	 edges= 7092	 schur= 0	 lambda= 391998.922000	 levenbergIter= 2
iteration= 58	 chi2= 661.080707	 time= 0.0481979	 cumTime= 2.88622	 edges= 7092	 schur= 0	 lambda= 522665.229333	 levenbergIter= 2
iteration= 59	 chi2= 661.080707	 time= 0.0565311	 cumTime= 2.94275	 edges= 7092	 schur= 0	 lambda= 2787547.889776	 levenbergIter= 3
iteration= 60	 chi2= 661.080707	 time= 0.0483864	 cumTime= 2.99113	 edges= 7092	 schur= 0	 lambda= 3716730.519702	 levenbergIter= 2
iteration= 61	 chi2= 661.080707	 time= 0.080179	 cumTime= 3.07131	 edges= 7092	 schur= 0	 lambda= 81193217113.057663	 levenbergIter= 6
iteration= 62	 chi2= 661.080707	 time= 0.0563714	 cumTime= 3.12768	 edges= 7092	 schur= 0	 lambda= 433030491269.640869	 levenbergIter= 3
iteration= 63	 chi2= 661.080707	 time= 0.0646127	 cumTime= 3.1923	 edges= 7092	 schur= 0	 lambda= 18475967627504.675781	 levenbergIter= 4
iteration= 64	 chi2= 661.080707	 time= 0.0658315	 cumTime= 3.25813	 edges= 7092	 schur= 0	 lambda= 788307952106866.125000	 levenbergIter= 4
iteration= 65	 chi2= 661.080707	 time= 0.122538	 cumTime= 3.38067	 edges= 7092	 schur= 0	 lambda= 28401787194893448701701766774784.000000	 levenbergIter= 10
I0114 14:47:09.966687 425119 optimization.cc:255] RTK 誤差：
I0114 14:47:09.966703 425119 optimization.cc:256] RTK 平移誤差：數量: 1143, 均值: 0.054377, 中位數: 0.002057, 0.1分位: 0.000644, 0.9分位: 0.008998, 0.95分位：0.311381, 最大值: 1.703702, 閾值: 1.000000
I0114 14:47:09.966771 425119 optimization.cc:257] lidar 誤差：數量: 5700, 均值: 0.009403, 中位數: 0.000008, 0.1分位: 0.000000, 0.9分位: 0.033417, 0.95分位：0.055681, 最大值: 0.286353, 閾值: 0.000000
I0114 14:47:09.967931 425119 optimization.cc:258] loop 誤差：數量: 249, 均值: 2.421480, 中位數: 1.883298, 0.1分位: 0.652742, 0.9分位: 4.821146, 0.95分位：6.357756, 最大值: 16.565907, 閾值: 0.000000
I0114 14:47:09.968003 425119 optimization.cc:278] gnss outlier: 25/1143
I0114 14:47:09.968009 425119 optimization.cc:283] loop outlier: 21/249
iteration= 0	 chi2= 524.481274	 time= 0.0534861	 cumTime= 0.0534861	 edges= 7046	 schur= 0	 lambda= 14.659429	 levenbergIter= 1
iteration= 1	 chi2= 524.396160	 time= 0.0416237	 cumTime= 0.0951098	 edges= 7046	 schur= 0	 lambda= 4.886476	 levenbergIter= 1
iteration= 2	 chi2= 524.311806	 time= 0.0407585	 cumTime= 0.135868	 edges= 7046	 schur= 0	 lambda= 1.628825	 levenbergIter= 1
iteration= 3	 chi2= 524.201346	 time= 0.0401856	 cumTime= 0.176054	 edges= 7046	 schur= 0	 lambda= 0.542942	 levenbergIter= 1
iteration= 4	 chi2= 524.070422	 time= 0.0399105	 cumTime= 0.215964	 edges= 7046	 schur= 0	 lambda= 0.180981	 levenbergIter= 1
iteration= 5	 chi2= 523.976912	 time= 0.0406078	 cumTime= 0.256572	 edges= 7046	 schur= 0	 lambda= 0.060327	 levenbergIter= 1
iteration= 6	 chi2= 523.943490	 time= 0.0401059	 cumTime= 0.296678	 edges= 7046	 schur= 0	 lambda= 0.020109	 levenbergIter= 1
iteration= 7	 chi2= 523.931502	 time= 0.0400695	 cumTime= 0.336748	 edges= 7046	 schur= 0	 lambda= 0.006703	 levenbergIter= 1
iteration= 8	 chi2= 523.921460	 time= 0.0401983	 cumTime= 0.376946	 edges= 7046	 schur= 0	 lambda= 0.002234	 levenbergIter= 1
iteration= 9	 chi2= 523.911708	 time= 0.039986	 cumTime= 0.416932	 edges= 7046	 schur= 0	 lambda= 0.001490	 levenbergIter= 1
iteration= 10	 chi2= 523.909097	 time= 0.0400748	 cumTime= 0.457007	 edges= 7046	 schur= 0	 lambda= 0.000993	 levenbergIter= 1
iteration= 11	 chi2= 523.908841	 time= 0.0401001	 cumTime= 0.497107	 edges= 7046	 schur= 0	 lambda= 0.000662	 levenbergIter= 1
iteration= 12	 chi2= 523.908475	 time= 0.0402014	 cumTime= 0.537308	 edges= 7046	 schur= 0	 lambda= 0.000441	 levenbergIter= 1
iteration= 13	 chi2= 523.908462	 time= 0.0558095	 cumTime= 0.593118	 edges= 7046	 schur= 0	 lambda= 0.002354	 levenbergIter= 3
iteration= 14	 chi2= 523.908461	 time= 0.0712249	 cumTime= 0.664343	 edges= 7046	 schur= 0	 lambda= 1.606902	 levenbergIter= 5
iteration= 15	 chi2= 523.908459	 time= 0.0398896	 cumTime= 0.704232	 edges= 7046	 schur= 0	 lambda= 1.071268	 levenbergIter= 1
iteration= 16	 chi2= 523.908457	 time= 0.0400018	 cumTime= 0.744234	 edges= 7046	 schur= 0	 lambda= 0.714179	 levenbergIter= 1
iteration= 17	 chi2= 523.908456	 time= 0.0398928	 cumTime= 0.784127	 edges= 7046	 schur= 0	 lambda= 0.476119	 levenbergIter= 1
iteration= 18	 chi2= 523.908456	 time= 0.055664	 cumTime= 0.839791	 edges= 7046	 schur= 0	 lambda= 2.539302	 levenbergIter= 3
iteration= 19	 chi2= 523.908456	 time= 0.0401968	 cumTime= 0.879988	 edges= 7046	 schur= 0	 lambda= 1.692868	 levenbergIter= 1
iteration= 20	 chi2= 523.908456	 time= 0.0634764	 cumTime= 0.943464	 edges= 7046	 schur= 0	 lambda= 72.229036	 levenbergIter= 4
iteration= 21	 chi2= 523.908456	 time= 0.0401159	 cumTime= 0.98358	 edges= 7046	 schur= 0	 lambda= 48.152691	 levenbergIter= 1
iteration= 22	 chi2= 523.908456	 time= 0.040062	 cumTime= 1.02364	 edges= 7046	 schur= 0	 lambda= 32.101794	 levenbergIter= 1
iteration= 23	 chi2= 523.908456	 time= 0.0399811	 cumTime= 1.06362	 edges= 7046	 schur= 0	 lambda= 21.401196	 levenbergIter= 1
iteration= 24	 chi2= 523.908455	 time= 0.0634087	 cumTime= 1.12703	 edges= 7046	 schur= 0	 lambda= 913.117694	 levenbergIter= 4
iteration= 25	 chi2= 523.908455	 time= 0.0399583	 cumTime= 1.16699	 edges= 7046	 schur= 0	 lambda= 608.745129	 levenbergIter= 1
iteration= 26	 chi2= 523.908455	 time= 0.0401255	 cumTime= 1.20712	 edges= 7046	 schur= 0	 lambda= 405.830086	 levenbergIter= 1
iteration= 27	 chi2= 523.908455	 time= 0.0477113	 cumTime= 1.25483	 edges= 7046	 schur= 0	 lambda= 541.106782	 levenbergIter= 2
iteration= 28	 chi2= 523.908455	 time= 0.0399891	 cumTime= 1.29482	 edges= 7046	 schur= 0	 lambda= 360.737854	 levenbergIter= 1
iteration= 29	 chi2= 523.908455	 time= 0.0400652	 cumTime= 1.33488	 edges= 7046	 schur= 0	 lambda= 240.491903	 levenbergIter= 1
iteration= 30	 chi2= 523.908455	 time= 0.0556648	 cumTime= 1.39055	 edges= 7046	 schur= 0	 lambda= 1282.623482	 levenbergIter= 3
iteration= 31	 chi2= 523.908455	 time= 0.0399304	 cumTime= 1.43048	 edges= 7046	 schur= 0	 lambda= 855.082322	 levenbergIter= 1
iteration= 32	 chi2= 523.908455	 time= 0.048053	 cumTime= 1.47853	 edges= 7046	 schur= 0	 lambda= 1140.109762	 levenbergIter= 2
iteration= 33	 chi2= 523.908455	 time= 0.0399793	 cumTime= 1.51851	 edges= 7046	 schur= 0	 lambda= 760.073175	 levenbergIter= 1
iteration= 34	 chi2= 523.908455	 time= 0.0557636	 cumTime= 1.57427	 edges= 7046	 schur= 0	 lambda= 4053.723599	 levenbergIter= 3
iteration= 35	 chi2= 523.908455	 time= 0.0401256	 cumTime= 1.6144	 edges= 7046	 schur= 0	 lambda= 2702.482399	 levenbergIter= 1
iteration= 36	 chi2= 523.908455	 time= 0.0399855	 cumTime= 1.65438	 edges= 7046	 schur= 0	 lambda= 1801.654933	 levenbergIter= 1
iteration= 37	 chi2= 523.908455	 time= 0.0477881	 cumTime= 1.70217	 edges= 7046	 schur= 0	 lambda= 2402.206577	 levenbergIter= 2
iteration= 38	 chi2= 523.908455	 time= 0.0558878	 cumTime= 1.75806	 edges= 7046	 schur= 0	 lambda= 12811.768410	 levenbergIter= 3
iteration= 39	 chi2= 523.908455	 time= 0.0398665	 cumTime= 1.79793	 edges= 7046	 schur= 0	 lambda= 8541.178940	 levenbergIter= 1
iteration= 40	 chi2= 523.908455	 time= 0.0477839	 cumTime= 1.84571	 edges= 7046	 schur= 0	 lambda= 11388.238587	 levenbergIter= 2
iteration= 41	 chi2= 523.908455	 time= 0.0398962	 cumTime= 1.88561	 edges= 7046	 schur= 0	 lambda= 7592.159058	 levenbergIter= 1
iteration= 42	 chi2= 523.908455	 time= 0.0478328	 cumTime= 1.93344	 edges= 7046	 schur= 0	 lambda= 10122.878744	 levenbergIter= 2
iteration= 43	 chi2= 523.908455	 time= 0.0476778	 cumTime= 1.98112	 edges= 7046	 schur= 0	 lambda= 13497.171659	 levenbergIter= 2
iteration= 44	 chi2= 523.908455	 time= 0.0478666	 cumTime= 2.02898	 edges= 7046	 schur= 0	 lambda= 17996.228878	 levenbergIter= 2
iteration= 45	 chi2= 523.908455	 time= 0.0478792	 cumTime= 2.07686	 edges= 7046	 schur= 0	 lambda= 23994.971837	 levenbergIter= 2
iteration= 46	 chi2= 523.908455	 time= 0.048001	 cumTime= 2.12486	 edges= 7046	 schur= 0	 lambda= 31993.295783	 levenbergIter= 2
iteration= 47	 chi2= 523.908455	 time= 0.0478022	 cumTime= 2.17267	 edges= 7046	 schur= 0	 lambda= 42657.727711	 levenbergIter= 2
iteration= 48	 chi2= 523.908455	 time= 0.0476888	 cumTime= 2.22035	 edges= 7046	 schur= 0	 lambda= 56876.970281	 levenbergIter= 2
iteration= 49	 chi2= 523.908455	 time= 0.0480185	 cumTime= 2.26837	 edges= 7046	 schur= 0	 lambda= 75835.960375	 levenbergIter= 2
iteration= 50	 chi2= 523.908455	 time= 0.0478004	 cumTime= 2.31617	 edges= 7046	 schur= 0	 lambda= 101114.613833	 levenbergIter= 2
iteration= 51	 chi2= 523.908455	 time= 0.0479061	 cumTime= 2.36408	 edges= 7046	 schur= 0	 lambda= 134819.485111	 levenbergIter= 2
iteration= 52	 chi2= 523.908455	 time= 0.0477509	 cumTime= 2.41183	 edges= 7046	 schur= 0	 lambda= 179759.313481	 levenbergIter= 2
iteration= 53	 chi2= 523.908455	 time= 0.0398897	 cumTime= 2.45172	 edges= 7046	 schur= 0	 lambda= 119839.542321	 levenbergIter= 1
iteration= 54	 chi2= 523.908455	 time= 0.0477834	 cumTime= 2.4995	 edges= 7046	 schur= 0	 lambda= 159786.056428	 levenbergIter= 2
iteration= 55	 chi2= 523.908455	 time= 0.0478571	 cumTime= 2.54736	 edges= 7046	 schur= 0	 lambda= 213048.075237	 levenbergIter= 2
iteration= 56	 chi2= 523.908455	 time= 0.0477106	 cumTime= 2.59507	 edges= 7046	 schur= 0	 lambda= 284064.100316	 levenbergIter= 2
iteration= 57	 chi2= 523.908455	 time= 0.047834	 cumTime= 2.6429	 edges= 7046	 schur= 0	 lambda= 378752.133755	 levenbergIter= 2
iteration= 58	 chi2= 523.908455	 time= 0.0400486	 cumTime= 2.68295	 edges= 7046	 schur= 0	 lambda= 252501.422503	 levenbergIter= 1
iteration= 59	 chi2= 523.908455	 time= 0.0558823	 cumTime= 2.73884	 edges= 7046	 schur= 0	 lambda= 1346674.253352	 levenbergIter= 3
iteration= 60	 chi2= 523.908455	 time= 0.0399954	 cumTime= 2.77883	 edges= 7046	 schur= 0	 lambda= 897782.835568	 levenbergIter= 1
iteration= 61	 chi2= 523.908455	 time= 0.103178	 cumTime= 2.88201	 edges= 7046	 schur= 0	 lambda= 31587925341583663104.000000	 levenbergIter= 9
I0114 14:47:12.922147 425119 optimization.cc:255] RTK 誤差：
I0114 14:47:12.922160 425119 optimization.cc:256] RTK 平移誤差：數量: 1118, 均值: 0.024533, 中位數: 0.001735, 0.1分位: 0.000555, 0.9分位: 0.005800, 0.95分位：0.185730, 最大值: 0.970918, 閾值: 1.000000
I0114 14:47:12.922228 425119 optimization.cc:257] lidar 誤差：數量: 5700, 均值: 0.009159, 中位數: 0.000007, 0.1分位: 0.000000, 0.9分位: 0.032959, 0.95分位：0.055079, 最大值: 0.265268, 閾值: 0.000000
I0114 14:47:12.923386 425119 optimization.cc:258] loop 誤差：數量: 228, 均值: 1.948570, 中位數: 1.779741, 0.1分位: 0.619406, 0.9分位: 3.609942, 0.95分位：4.147956, 最大值: 5.030160, 閾值: 0.000000
I0114 14:47:12.963697 425119 optimization.cc:306] med error: 0.496892
I0114 14:47:12.973387 425119 optimization.cc:96] done

wu@WP:~/slam_in_autonomous_driving/data/ch9$ g2o_viewer before.g2o

?????????g2o_viewer after.g2o 結果類似，基本沒有區別。

? ? ? ? 4 回環檢測

????????系統是離線運行的，可以一次性把所有待檢查的區域檢查完，不必像實時系統那樣需要等待前端的結果。執行匹配時，可以充分利用并行機制，加速回環檢測的過程。回環檢測結果以候選回環對的形式描述：

/*** 回環檢測候選幀*/
struct LoopCandidate {LoopCandidate() {}LoopCandidate(IdType id1, IdType id2, SE3 Tij) : idx1_(id1), idx2_(id2), Tij_(Tij) {}IdType idx1_ = 0;IdType idx2_ = 0;SE3 Tij_;double ndt_score_ = 0.0;
};

? ? ? ? 4.1 回環檢測流程?

? ? ? ? 使用基于位置（歐氏距離）的回環檢測，回環檢測流程如下：

① 遍歷第一階段優化軌跡中的關鍵幀。

????????對每兩個在空間上相隔較近，但時間上存在一定距離的關鍵幀，進行一次回環檢測。我們稱這樣一對關鍵幀為檢查點。為了防止檢查點數量太大，我們設置一個 ID 間隔，即每隔多少個關鍵幀取一次檢查。實驗當中這個間隔取 5。

? ? ? ? 具體實現為：

????????對每個關鍵幀 kf_first，遍歷其之后的關鍵幀 kf_second，檢查是否滿足回環條件（距離小于 30 米）。若滿足回環條件，則將其添加到候選回環對中。
? ? ? ? 在整個過程中，使用 check_first 和 check_second 跟蹤之前已添加為候選對的關鍵幀對，如果正在檢查的關鍵幀 kf_first 離 check_first 的距離太近（ID 間隔小于 5），則跳過此關鍵幀進行下一個 kf_first。
? ? ? ? 當 kf_first 滿足距離條件，進而判斷 kf_second 是否同時滿足與 check_second （ID 間隔小于 5）以及 kf_first （ID 間隔小于 100）的距離條件，如果都滿足則使用第一階段優化位姿來計算并判定 kf_first 和 kf_second 之間的平移距離是否滿足回環條件（距離小于 30 米），滿足則添加到回環候選對當中。

void LoopClosure::DetectLoopCandidates() {KFPtr check_first = nullptr;KFPtr check_second = nullptr;LOG(INFO) << "detecting loop candidates from pose in stage 1";// 本質上是兩重循環for (auto iter_first = keyframes_.begin(); iter_first != keyframes_.end(); ++iter_first) {auto kf_first = iter_first->second;if (check_first != nullptr && abs(int(kf_first->id_) - int(check_first->id_)) <= skip_id_) {// 兩個關鍵幀之前ID太近continue;}for (auto iter_second = iter_first; iter_second != keyframes_.end(); ++iter_second) {auto kf_second = iter_second->second;if (check_second != nullptr && abs(int(kf_second->id_) - int(check_second->id_)) <= skip_id_) {// 兩個關鍵幀之前ID太近continue;}if (abs(int(kf_first->id_) - int(kf_second->id_)) < min_id_interval_) {/// 在同一條軌跡中，如果間隔太近，就不考慮回環continue;}Vec3d dt = kf_first->opti_pose_1_.translation() - kf_second->opti_pose_1_.translation();double t2d = dt.head<2>().norm();  // x-y distancedouble range_th = min_distance_;if (t2d < range_th) {LoopCandidate c(kf_first->id_, kf_second->id_,kf_first->opti_pose_1_.inverse() * kf_second->opti_pose_1_);loop_candiates_.emplace_back(c);check_first = kf_first;check_second = kf_second;}}}LOG(INFO) << "detected candidates: " << loop_candiates_.size();
}

② 并發計算候選回環對是否成立

????????對于每個候選回環對，我們使用 scan to map 的配準方式，對候選回環對中第一個關鍵幀?kf1?建立子地圖（前后各 40 個關鍵幀，每隔 4 個關鍵幀選取一幀，共計 20 幀點云拼接，建立在世界坐標系下），對第二個關鍵幀?kf2?只取其一幀點云。將點云與子地圖進行多分辨率 NDT 配準，得到 kf2 世界坐標系下的位姿。

????????與里程計方法不同，在回環檢測配準過程中，我們經常要面對初值位姿估計很差的情況，希望算法不太依賴于給定的位姿初值。因此使用由粗至精的配準過程，即多分辨率 NDT 配準（體素分辨率?10.0, 5.0, 4.0, 3.0）。

? ? ? ? 最后判斷配準后的候選回環對中的 ndt_score_ 是否滿足閾值（4.5），滿足則視為成功的候選回環對。

void LoopClosure::ComputeLoopCandidates() {// 執行計算std::for_each(std::execution::par_unseq, loop_candiates_.begin(), loop_candiates_.end(),[this](LoopCandidate& c) { ComputeForCandidate(c); });// 保存成功的候選std::vector<LoopCandidate> succ_candidates;for (const auto& lc : loop_candiates_) {if (lc.ndt_score_ > ndt_score_th_) {succ_candidates.emplace_back(lc);}}LOG(INFO) << "success: " << succ_candidates.size() << "/" << loop_candiates_.size();// 將兩個容器的內容互換loop_candiates_.swap(succ_candidates);
}void LoopClosure::ComputeForCandidate(sad::LoopCandidate& c) {LOG(INFO) << "aligning " << c.idx1_ << " with " << c.idx2_;const int submap_idx_range = 40;KFPtr kf1 = keyframes_.at(c.idx1_), kf2 = keyframes_.at(c.idx2_);auto build_submap = [this](int given_id, bool build_in_world) -> CloudPtr {CloudPtr submap(new PointCloudType);for (int idx = -submap_idx_range; idx < submap_idx_range; idx += 4) {int id = idx + given_id;if (id < 0) {continue;}auto iter = keyframes_.find(id);if (iter == keyframes_.end()) {continue;}auto kf = iter->second;CloudPtr cloud(new PointCloudType);pcl::io::loadPCDFile("/home/wu/slam_in_autonomous_driving/data/ch9/" + std::to_string(id) + ".pcd", *cloud);sad::RemoveGround(cloud, 0.1);if (cloud->empty()) {continue;}// 轉到世界系下SE3 Twb = kf->opti_pose_1_;if (!build_in_world) {// 轉到 given_id 所在的關鍵幀坐標系下Twb = keyframes_.at(given_id)->opti_pose_1_.inverse() * Twb;}CloudPtr cloud_trans(new PointCloudType);pcl::transformPointCloud(*cloud, *cloud_trans, Twb.matrix());*submap += *cloud_trans;}return submap;};auto submap_kf1 = build_submap(kf1->id_, true);CloudPtr submap_kf2(new PointCloudType);pcl::io::loadPCDFile("/home/wu/slam_in_autonomous_driving/data/ch9/" + std::to_string(kf2->id_) + ".pcd", *submap_kf2);if (submap_kf1->empty() || submap_kf2->empty()) {c.ndt_score_ = 0;return;}pcl::NormalDistributionsTransform<PointType, PointType> ndt;ndt.setTransformationEpsilon(0.05);ndt.setStepSize(0.7);ndt.setMaximumIterations(40);// T_wjMat4f Tw2 = kf2->opti_pose_1_.matrix().cast<float>();/// 不同分辨率下的匹配// 多分辨率 NDT 配準CloudPtr output(new PointCloudType);std::vector<double> res{10.0, 5.0, 4.0, 3.0};for (auto& r : res) {// 設置/更改體素網格的分辨率。ndt.setResolution(r);// 降采樣尺寸和體素分辨率成比例變化，比例因子為 0.1auto rough_map1 = VoxelCloud(submap_kf1, r * 0.1);auto rough_map2 = VoxelCloud(submap_kf2, r * 0.1);ndt.setInputTarget(rough_map1);ndt.setInputSource(rough_map2);ndt.align(*output, Tw2);Tw2 = ndt.getFinalTransformation();}Mat4d T = Tw2.cast<double>();Quatd q(T.block<3, 3>(0, 0));q.normalize();Vec3d t = T.block<3, 1>(0, 3);// T_ij = T_wi^-1 * T_wjc.Tij_ = kf1->opti_pose_1_.inverse() * SE3(q, t);// 返回 變換概率（Transformation Probability), 是一個 非負浮點數，值越大表示匹配效果越好c.ndt_score_ = ndt.getTransformationProbability();// std::cout << "Transformation Probability: " << c.ndt_score_ << std::endl;
}

③ 記錄所有成功經過篩選的回環候選對，并在第二階段優化時讀取回環檢測的結果

void LoopClosure::SaveResults() {auto save_SE3 = [](std::ostream& f, SE3 pose) {auto q = pose.so3().unit_quaternion();Vec3d t = pose.translation();f << t[0] << " " << t[1] << " " << t[2] << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << " ";};std::ofstream fout("/home/wu/slam_in_autonomous_driving/data/ch9/loops.txt");for (const auto& lc : loop_candiates_) {fout << lc.idx1_ << " " << lc.idx2_ << " " << lc.ndt_score_ << " ";save_SE3(fout, lc.Tij_);fout << std::endl;}
}

? ? ? ? 4.2 回環檢測結果

? ? ? ? 回環檢測數據會被存儲在 data/ch9/keyframes.txt 。

????????存儲順序為：idx1_、idx2_、ndt_score_、Tij_

wu@WP:~/slam_in_autonomous_driving/bin$ ./run_loopclosureI0114 14:39:34.600237 424832 keyframe.cc:78] Loaded kfs: 1143
I0114 14:39:34.600332 424832 loopclosure.cc:26] keyframes: 1143
I0114 14:39:34.600500 424832 loopclosure.cc:47] detecting loop candidates from pose in stage 1
I0114 14:39:34.605368 424832 loopclosure.cc:84] detected candidates: 272
I0114 14:39:34.605371 424832 loopclosure.cc:109] aligning 0 with 100
I0114 14:39:34.660082 424832 loopclosure.cc:109] aligning 0 with 106
I0114 14:39:34.715945 424832 loopclosure.cc:109] aligning 246 with 631
I0114 14:39:34.838471 424832 loopclosure.cc:109] aligning 246 with 637
I0114 14:39:34.961339 424832 loopclosure.cc:109] aligning 252 with 612
I0114 14:39:35.083011 424832 loopclosure.cc:109] aligning 252 with 618
I0114 14:39:35.214619 424832 loopclosure.cc:109] aligning 252 with 624
I0114 14:39:35.336724 424832 loopclosure.cc:109] aligning 252 with 630
I0114 14:39:35.461663 424832 loopclosure.cc:109] aligning 252 with 636
I0114 14:39:35.698182 424832 loopclosure.cc:109] aligning 252 with 642
I0114 14:39:35.822700 424832 loopclosure.cc:109] aligning 252 with 648
I0114 14:39:35.944046 424832 loopclosure.cc:109] aligning 258 with 602
I0114 14:39:36.076938 424832 loopclosure.cc:109] aligning 258 with 608
I0114 14:39:36.218856 424832 loopclosure.cc:109] aligning 258 with 614
I0114 14:39:36.361122 424832 loopclosure.cc:109] aligning 258 with 620
I0114 14:39:36.492717 424832 loopclosure.cc:109] aligning 258 with 626
I0114 14:39:36.618754 424832 loopclosure.cc:109] aligning 258 with 632
I0114 14:39:36.735234 424832 loopclosure.cc:109] aligning 258 with 638
I0114 14:39:36.852849 424832 loopclosure.cc:109] aligning 258 with 644
I0114 14:39:36.974691 424832 loopclosure.cc:109] aligning 258 with 650
I0114 14:39:37.102720 424832 loopclosure.cc:109] aligning 264 with 591
I0114 14:39:37.239533 424832 loopclosure.cc:109] aligning 264 with 597
I0114 14:39:37.377104 424832 loopclosure.cc:109] aligning 264 with 603
I0114 14:39:37.514230 424832 loopclosure.cc:109] aligning 264 with 609
I0114 14:39:37.659909 424832 loopclosure.cc:109] aligning 264 with 615
I0114 14:39:37.802589 424832 loopclosure.cc:109] aligning 264 with 621
I0114 14:39:37.935138 424832 loopclosure.cc:109] aligning 264 with 627
I0114 14:39:38.060462 424832 loopclosure.cc:109] aligning 264 with 633
I0114 14:39:38.178555 424832 loopclosure.cc:109] aligning 264 with 639
I0114 14:39:38.302886 424832 loopclosure.cc:109] aligning 264 with 645
I0114 14:39:38.424055 424832 loopclosure.cc:109] aligning 264 with 651
I0114 14:39:38.555510 424832 loopclosure.cc:109] aligning 270 with 577
I0114 14:39:38.683847 424832 loopclosure.cc:109] aligning 270 with 583
I0114 14:39:38.814306 424832 loopclosure.cc:109] aligning 270 with 589
I0114 14:39:38.945605 424832 loopclosure.cc:109] aligning 270 with 595
I0114 14:39:39.081390 424832 loopclosure.cc:109] aligning 270 with 601
I0114 14:39:39.217231 424832 loopclosure.cc:109] aligning 270 with 607
I0114 14:39:39.361466 424832 loopclosure.cc:109] aligning 270 with 613
I0114 14:39:39.482360 424832 loopclosure.cc:109] aligning 270 with 619
I0114 14:39:39.612998 424832 loopclosure.cc:109] aligning 270 with 625
I0114 14:39:39.735734 424832 loopclosure.cc:109] aligning 270 with 631
I0114 14:39:39.855314 424832 loopclosure.cc:109] aligning 270 with 637
I0114 14:39:39.976063 424832 loopclosure.cc:109] aligning 270 with 643
I0114 14:39:40.100976 424832 loopclosure.cc:109] aligning 270 with 649
I0114 14:39:40.233157 424832 loopclosure.cc:109] aligning 276 with 570
I0114 14:39:40.358598 424832 loopclosure.cc:109] aligning 276 with 576
I0114 14:39:40.482266 424832 loopclosure.cc:109] aligning 276 with 582
I0114 14:39:40.599781 424832 loopclosure.cc:109] aligning 276 with 588
I0114 14:39:40.732409 424832 loopclosure.cc:109] aligning 276 with 594
I0114 14:39:40.862918 424832 loopclosure.cc:109] aligning 276 with 600
I0114 14:39:41.003029 424832 loopclosure.cc:109] aligning 276 with 606
I0114 14:39:41.145427 424832 loopclosure.cc:109] aligning 276 with 612
I0114 14:39:41.264133 424832 loopclosure.cc:109] aligning 276 with 618
I0114 14:39:41.391021 424832 loopclosure.cc:109] aligning 276 with 624
I0114 14:39:41.510094 424832 loopclosure.cc:109] aligning 276 with 630
I0114 14:39:41.628989 424832 loopclosure.cc:109] aligning 276 with 636
I0114 14:39:41.747018 424832 loopclosure.cc:109] aligning 276 with 642
I0114 14:39:41.868511 424832 loopclosure.cc:109] aligning 276 with 648
I0114 14:39:41.986932 424832 loopclosure.cc:109] aligning 282 with 565
I0114 14:39:42.131405 424832 loopclosure.cc:109] aligning 282 with 571
I0114 14:39:42.264037 424832 loopclosure.cc:109] aligning 282 with 577
I0114 14:39:42.397466 424832 loopclosure.cc:109] aligning 282 with 583
I0114 14:39:42.533788 424832 loopclosure.cc:109] aligning 282 with 589
I0114 14:39:42.670135 424832 loopclosure.cc:109] aligning 282 with 595
I0114 14:39:42.810570 424832 loopclosure.cc:109] aligning 282 with 601
I0114 14:39:42.951463 424832 loopclosure.cc:109] aligning 282 with 607
I0114 14:39:43.101276 424832 loopclosure.cc:109] aligning 282 with 613
I0114 14:39:43.226984 424832 loopclosure.cc:109] aligning 282 with 619
I0114 14:39:43.364259 424832 loopclosure.cc:109] aligning 282 with 625
I0114 14:39:43.491612 424832 loopclosure.cc:109] aligning 282 with 631
I0114 14:39:43.615964 424832 loopclosure.cc:109] aligning 282 with 637
I0114 14:39:43.742473 424832 loopclosure.cc:109] aligning 288 with 559
I0114 14:39:43.881448 424832 loopclosure.cc:109] aligning 288 with 565
I0114 14:39:44.026831 424832 loopclosure.cc:109] aligning 288 with 571
I0114 14:39:44.159776 424832 loopclosure.cc:109] aligning 288 with 577
I0114 14:39:44.294018 424832 loopclosure.cc:109] aligning 288 with 583
I0114 14:39:44.437618 424832 loopclosure.cc:109] aligning 288 with 589
I0114 14:39:44.573611 424832 loopclosure.cc:109] aligning 288 with 595
I0114 14:39:44.710968 424832 loopclosure.cc:109] aligning 288 with 601
I0114 14:39:44.857462 424832 loopclosure.cc:109] aligning 288 with 607
I0114 14:39:45.006702 424832 loopclosure.cc:109] aligning 288 with 613
I0114 14:39:45.133204 424832 loopclosure.cc:109] aligning 288 with 619
I0114 14:39:45.275009 424832 loopclosure.cc:109] aligning 288 with 625
I0114 14:39:45.406067 424832 loopclosure.cc:109] aligning 288 with 631
I0114 14:39:45.531435 424832 loopclosure.cc:109] aligning 294 with 555
I0114 14:39:45.834162 424832 loopclosure.cc:109] aligning 294 with 561
I0114 14:39:45.985754 424832 loopclosure.cc:109] aligning 294 with 567
I0114 14:39:46.139230 424832 loopclosure.cc:109] aligning 294 with 573
I0114 14:39:46.285375 424832 loopclosure.cc:109] aligning 294 with 579
I0114 14:39:46.429277 424832 loopclosure.cc:109] aligning 294 with 585
I0114 14:39:46.570621 424832 loopclosure.cc:109] aligning 294 with 591
I0114 14:39:46.722086 424832 loopclosure.cc:109] aligning 294 with 597
I0114 14:39:46.869537 424832 loopclosure.cc:109] aligning 294 with 603
I0114 14:39:47.013934 424832 loopclosure.cc:109] aligning 294 with 609
I0114 14:39:47.166580 424832 loopclosure.cc:109] aligning 294 with 615
I0114 14:39:47.318617 424832 loopclosure.cc:109] aligning 294 with 621
I0114 14:39:47.460216 424832 loopclosure.cc:109] aligning 294 with 627
I0114 14:39:47.594961 424832 loopclosure.cc:109] aligning 300 with 551
I0114 14:39:47.746563 424832 loopclosure.cc:109] aligning 300 with 557
I0114 14:39:47.896881 424832 loopclosure.cc:109] aligning 300 with 563
I0114 14:39:48.044682 424832 loopclosure.cc:109] aligning 300 with 569
I0114 14:39:48.190032 424832 loopclosure.cc:109] aligning 300 with 575
I0114 14:39:48.334643 424832 loopclosure.cc:109] aligning 300 with 581
I0114 14:39:48.486544 424832 loopclosure.cc:109] aligning 300 with 587
I0114 14:39:48.637301 424832 loopclosure.cc:109] aligning 300 with 593
I0114 14:39:48.790048 424832 loopclosure.cc:109] aligning 300 with 599
I0114 14:39:48.942795 424832 loopclosure.cc:109] aligning 300 with 605
I0114 14:39:49.072114 424832 loopclosure.cc:109] aligning 300 with 611
I0114 14:39:49.230252 424832 loopclosure.cc:109] aligning 300 with 617
I0114 14:39:49.377197 424832 loopclosure.cc:109] aligning 300 with 623
I0114 14:39:49.514369 424832 loopclosure.cc:109] aligning 306 with 547
I0114 14:39:49.676714 424832 loopclosure.cc:109] aligning 306 with 553
I0114 14:39:49.829732 424832 loopclosure.cc:109] aligning 306 with 559
I0114 14:39:49.986097 424832 loopclosure.cc:109] aligning 306 with 565
I0114 14:39:50.150573 424832 loopclosure.cc:109] aligning 306 with 571
I0114 14:39:50.298872 424832 loopclosure.cc:109] aligning 306 with 577
I0114 14:39:50.448585 424832 loopclosure.cc:109] aligning 306 with 583
I0114 14:39:50.597843 424832 loopclosure.cc:109] aligning 306 with 589
I0114 14:39:50.747596 424832 loopclosure.cc:109] aligning 306 with 595
I0114 14:39:50.901561 424832 loopclosure.cc:109] aligning 306 with 601
I0114 14:39:51.055693 424832 loopclosure.cc:109] aligning 306 with 607
I0114 14:39:51.217823 424832 loopclosure.cc:109] aligning 306 with 613
I0114 14:39:51.356518 424832 loopclosure.cc:109] aligning 306 with 619
I0114 14:39:51.505082 424832 loopclosure.cc:109] aligning 312 with 541
I0114 14:39:51.665200 424832 loopclosure.cc:109] aligning 312 with 547
I0114 14:39:51.839165 424832 loopclosure.cc:109] aligning 312 with 553
I0114 14:39:51.997599 424832 loopclosure.cc:109] aligning 312 with 559
I0114 14:39:52.156019 424832 loopclosure.cc:109] aligning 312 with 565
I0114 14:39:52.320459 424832 loopclosure.cc:109] aligning 312 with 571
I0114 14:39:52.471503 424832 loopclosure.cc:109] aligning 312 with 577
I0114 14:39:52.624399 424832 loopclosure.cc:109] aligning 312 with 583
I0114 14:39:52.786643 424832 loopclosure.cc:109] aligning 312 with 589
I0114 14:39:52.938331 424832 loopclosure.cc:109] aligning 312 with 595
I0114 14:39:53.094488 424832 loopclosure.cc:109] aligning 312 with 601
I0114 14:39:53.264699 424832 loopclosure.cc:109] aligning 312 with 607
I0114 14:39:53.432278 424832 loopclosure.cc:109] aligning 312 with 613
I0114 14:39:53.575907 424832 loopclosure.cc:109] aligning 312 with 619
I0114 14:39:53.734344 424832 loopclosure.cc:109] aligning 318 with 536
I0114 14:39:53.909785 424832 loopclosure.cc:109] aligning 318 with 542
I0114 14:39:54.069643 424832 loopclosure.cc:109] aligning 318 with 548
I0114 14:39:54.280661 424832 loopclosure.cc:109] aligning 318 with 554
I0114 14:39:54.440966 424832 loopclosure.cc:109] aligning 318 with 560
I0114 14:39:54.928949 424832 loopclosure.cc:109] aligning 318 with 566
I0114 14:39:55.095808 424832 loopclosure.cc:109] aligning 318 with 572
I0114 14:39:55.260769 424832 loopclosure.cc:109] aligning 318 with 578
I0114 14:39:55.419515 424832 loopclosure.cc:109] aligning 318 with 584
I0114 14:39:55.568085 424832 loopclosure.cc:109] aligning 318 with 590
I0114 14:39:55.734709 424832 loopclosure.cc:109] aligning 318 with 596
I0114 14:39:55.890156 424832 loopclosure.cc:109] aligning 318 with 602
I0114 14:39:56.050088 424832 loopclosure.cc:109] aligning 318 with 608
I0114 14:39:56.216840 424832 loopclosure.cc:109] aligning 324 with 530
I0114 14:39:56.392796 424832 loopclosure.cc:109] aligning 324 with 536
I0114 14:39:56.572223 424832 loopclosure.cc:109] aligning 324 with 542
I0114 14:39:56.737180 424832 loopclosure.cc:109] aligning 324 with 548
I0114 14:39:56.915028 424832 loopclosure.cc:109] aligning 324 with 554
I0114 14:39:57.080109 424832 loopclosure.cc:109] aligning 324 with 560
I0114 14:39:57.252213 424832 loopclosure.cc:109] aligning 324 with 566
I0114 14:39:57.423735 424832 loopclosure.cc:109] aligning 324 with 572
I0114 14:39:57.590920 424832 loopclosure.cc:109] aligning 324 with 578
I0114 14:39:57.753762 424832 loopclosure.cc:109] aligning 324 with 584
I0114 14:39:57.914364 424832 loopclosure.cc:109] aligning 324 with 590
I0114 14:39:58.072743 424832 loopclosure.cc:109] aligning 324 with 596
I0114 14:39:58.231325 424832 loopclosure.cc:109] aligning 324 with 602
I0114 14:39:58.398078 424832 loopclosure.cc:109] aligning 330 with 524
I0114 14:39:58.583384 424832 loopclosure.cc:109] aligning 330 with 530
I0114 14:39:58.772176 424832 loopclosure.cc:109] aligning 330 with 536
I0114 14:39:58.960115 424832 loopclosure.cc:109] aligning 330 with 542
I0114 14:39:59.135846 424832 loopclosure.cc:109] aligning 330 with 548
I0114 14:39:59.323920 424832 loopclosure.cc:109] aligning 330 with 554
I0114 14:39:59.499577 424832 loopclosure.cc:109] aligning 330 with 560
I0114 14:39:59.693114 424832 loopclosure.cc:109] aligning 330 with 566
I0114 14:39:59.876761 424832 loopclosure.cc:109] aligning 330 with 572
I0114 14:40:00.057803 424832 loopclosure.cc:109] aligning 330 with 578
I0114 14:40:00.229849 424832 loopclosure.cc:109] aligning 336 with 519
I0114 14:40:00.416292 424832 loopclosure.cc:109] aligning 336 with 525
I0114 14:40:00.634541 424832 loopclosure.cc:109] aligning 336 with 531
I0114 14:40:00.830394 424832 loopclosure.cc:109] aligning 336 with 537
I0114 14:40:01.022815 424832 loopclosure.cc:109] aligning 336 with 543
I0114 14:40:01.239847 424832 loopclosure.cc:109] aligning 336 with 549
I0114 14:40:01.431026 424832 loopclosure.cc:109] aligning 336 with 555
I0114 14:40:01.624220 424832 loopclosure.cc:109] aligning 336 with 561
I0114 14:40:01.808413 424832 loopclosure.cc:109] aligning 336 with 567
I0114 14:40:01.998997 424832 loopclosure.cc:109] aligning 336 with 573
I0114 14:40:02.180449 424832 loopclosure.cc:109] aligning 342 with 513
I0114 14:40:02.370496 424832 loopclosure.cc:109] aligning 342 with 519
I0114 14:40:02.559326 424832 loopclosure.cc:109] aligning 342 with 525
I0114 14:40:02.761710 424832 loopclosure.cc:109] aligning 342 with 531
I0114 14:40:02.960893 424832 loopclosure.cc:109] aligning 342 with 537
I0114 14:40:03.152431 424832 loopclosure.cc:109] aligning 342 with 543
I0114 14:40:03.376098 424832 loopclosure.cc:109] aligning 342 with 549
I0114 14:40:03.569427 424832 loopclosure.cc:109] aligning 342 with 555
I0114 14:40:03.761706 424832 loopclosure.cc:109] aligning 342 with 561
I0114 14:40:03.948815 424832 loopclosure.cc:109] aligning 342 with 567
I0114 14:40:04.140497 424832 loopclosure.cc:109] aligning 348 with 507
I0114 14:40:04.349716 424832 loopclosure.cc:109] aligning 348 with 513
I0114 14:40:04.546634 424832 loopclosure.cc:109] aligning 348 with 519
I0114 14:40:04.740049 424832 loopclosure.cc:109] aligning 348 with 525
I0114 14:40:04.959317 424832 loopclosure.cc:109] aligning 348 with 531
I0114 14:40:05.162758 424832 loopclosure.cc:109] aligning 348 with 537
I0114 14:40:05.361279 424832 loopclosure.cc:109] aligning 348 with 543
I0114 14:40:05.591269 424832 loopclosure.cc:109] aligning 348 with 549
I0114 14:40:05.794329 424832 loopclosure.cc:109] aligning 348 with 555
I0114 14:40:05.995422 424832 loopclosure.cc:109] aligning 348 with 561
I0114 14:40:06.189824 424832 loopclosure.cc:109] aligning 354 with 501
I0114 14:40:06.387213 424832 loopclosure.cc:109] aligning 354 with 507
I0114 14:40:06.746068 424832 loopclosure.cc:109] aligning 354 with 513
I0114 14:40:06.939594 424832 loopclosure.cc:109] aligning 354 with 519
I0114 14:40:07.133322 424832 loopclosure.cc:109] aligning 354 with 525
I0114 14:40:07.339231 424832 loopclosure.cc:109] aligning 354 with 531
I0114 14:40:07.544878 424832 loopclosure.cc:109] aligning 354 with 537
I0114 14:40:07.736769 424832 loopclosure.cc:109] aligning 354 with 543
I0114 14:40:07.930209 424832 loopclosure.cc:109] aligning 354 with 549
I0114 14:40:08.125648 424832 loopclosure.cc:109] aligning 354 with 555
I0114 14:40:08.320142 424832 loopclosure.cc:109] aligning 360 with 495
I0114 14:40:08.540534 424832 loopclosure.cc:109] aligning 360 with 501
I0114 14:40:08.779071 424832 loopclosure.cc:109] aligning 360 with 507
I0114 14:40:08.993053 424832 loopclosure.cc:109] aligning 360 with 513
I0114 14:40:09.191763 424832 loopclosure.cc:109] aligning 360 with 519
I0114 14:40:09.401374 424832 loopclosure.cc:109] aligning 360 with 525
I0114 14:40:09.623682 424832 loopclosure.cc:109] aligning 360 with 531
I0114 14:40:09.826388 424832 loopclosure.cc:109] aligning 360 with 537
I0114 14:40:10.029390 424832 loopclosure.cc:109] aligning 360 with 543
I0114 14:40:10.236105 424832 loopclosure.cc:109] aligning 360 with 549
I0114 14:40:10.442286 424832 loopclosure.cc:109] aligning 366 with 489
I0114 14:40:10.632905 424832 loopclosure.cc:109] aligning 366 with 495
I0114 14:40:10.840906 424832 loopclosure.cc:109] aligning 366 with 501
I0114 14:40:11.037856 424832 loopclosure.cc:109] aligning 366 with 507
I0114 14:40:11.260270 424832 loopclosure.cc:109] aligning 366 with 513
I0114 14:40:11.465464 424832 loopclosure.cc:109] aligning 366 with 519
I0114 14:40:11.659193 424832 loopclosure.cc:109] aligning 366 with 525
I0114 14:40:11.875249 424832 loopclosure.cc:109] aligning 366 with 531
I0114 14:40:12.082633 424832 loopclosure.cc:109] aligning 366 with 537
I0114 14:40:12.279668 424832 loopclosure.cc:109] aligning 366 with 543
I0114 14:40:12.495205 424832 loopclosure.cc:109] aligning 372 with 483
I0114 14:40:12.722119 424832 loopclosure.cc:109] aligning 372 with 489
I0114 14:40:12.916532 424832 loopclosure.cc:109] aligning 372 with 495
I0114 14:40:13.117549 424832 loopclosure.cc:109] aligning 372 with 501
I0114 14:40:13.347931 424832 loopclosure.cc:109] aligning 372 with 507
I0114 14:40:13.552963 424832 loopclosure.cc:109] aligning 372 with 513
I0114 14:40:13.744216 424832 loopclosure.cc:109] aligning 372 with 519
I0114 14:40:13.935374 424832 loopclosure.cc:109] aligning 372 with 525
I0114 14:40:14.144537 424832 loopclosure.cc:109] aligning 372 with 531
I0114 14:40:14.345687 424832 loopclosure.cc:109] aligning 372 with 537
I0114 14:40:14.540819 424832 loopclosure.cc:109] aligning 378 with 478
I0114 14:40:14.721980 424832 loopclosure.cc:109] aligning 378 with 484
I0114 14:40:14.918220 424832 loopclosure.cc:109] aligning 378 with 490
I0114 14:40:15.098886 424832 loopclosure.cc:109] aligning 378 with 496
I0114 14:40:15.302568 424832 loopclosure.cc:109] aligning 378 with 502
I0114 14:40:15.520671 424832 loopclosure.cc:109] aligning 378 with 508
I0114 14:40:15.730944 424832 loopclosure.cc:109] aligning 378 with 514
I0114 14:40:15.933339 424832 loopclosure.cc:109] aligning 378 with 520
I0114 14:40:16.116988 424832 loopclosure.cc:109] aligning 378 with 526
I0114 14:40:16.304324 424832 loopclosure.cc:109] aligning 378 with 532
I0114 14:40:16.513626 424832 loopclosure.cc:109] aligning 384 with 484
I0114 14:40:16.713029 424832 loopclosure.cc:109] aligning 384 with 490
I0114 14:40:16.898360 424832 loopclosure.cc:109] aligning 384 with 496
I0114 14:40:17.177525 424832 loopclosure.cc:109] aligning 384 with 502
I0114 14:40:17.511034 424832 loopclosure.cc:109] aligning 384 with 508
I0114 14:40:17.715927 424832 loopclosure.cc:109] aligning 384 with 514
I0114 14:40:18.106099 424832 loopclosure.cc:109] aligning 384 with 520
I0114 14:40:18.292825 424832 loopclosure.cc:109] aligning 384 with 526
I0114 14:40:18.485371 424832 loopclosure.cc:109] aligning 390 with 490
I0114 14:40:18.665769 424832 loopclosure.cc:109] aligning 390 with 496
I0114 14:40:18.902027 424832 loopclosure.cc:109] aligning 390 with 502
I0114 14:40:19.115675 424832 loopclosure.cc:109] aligning 390 with 508
I0114 14:40:19.313446 424832 loopclosure.cc:109] aligning 390 with 514
I0114 14:40:19.656185 424832 loopclosure.cc:109] aligning 390 with 520
I0114 14:40:19.840093 424832 loopclosure.cc:109] aligning 396 with 496
I0114 14:40:20.085132 424832 loopclosure.cc:109] aligning 396 with 502
I0114 14:40:20.293792 424832 loopclosure.cc:109] aligning 396 with 508
I0114 14:40:20.501314 424832 loopclosure.cc:109] aligning 396 with 514
I0114 14:40:20.699945 424832 loopclosure.cc:109] aligning 402 with 502
I0114 14:40:20.910954 424832 loopclosure.cc:102] success: 249/272

? ? ? ? 5 軌跡可視化

? ? ? ? 將 lidar_pose_、rtk_pose_、opti_pose_1_、opti_pose_2_ 軌跡可視化：

wu@WP:~/slam_in_autonomous_driving/scripts$ python3 all_path.py /home/wu/slam_in_autonomous_driving/data/ch9/keyframes.txt all_path.py:10: MatplotlibDeprecationWarning: Axes3D(fig) adding itself to the figure is deprecated since 3.4. Pass the keyword argument auto_add_to_figure=False and use fig.add_axes(ax) to suppress this warning. The default value of auto_add_to_figure will change to False in mpl3.5 and True values will no longer work in 3.6.  This is consistent with other Axes classes.ax = Axes3D(fig

? ? ? ? 2D 軌跡：

? ? ? ? 3D 軌跡：

????????6 地圖導出

? ? ? ? 根據?pose_source 參數選擇位姿來源，可選擇?lidar/rtk/opti1/opti2。原理是根據每個關鍵幀的位姿，將關鍵幀點云轉到世界坐標系上，再拼接起來。

/*** 將keyframes.txt中的地圖和點云合并為一個pcd*/int main(int argc, char** argv) {google::ParseCommandLineFlags(&argc, &argv, true);google::InitGoogleLogging(argv[0]);FLAGS_stderrthreshold = google::INFO;FLAGS_colorlogtostderr = true;using namespace sad;std::map<IdType, KFPtr> keyframes;if (!LoadKeyFrames("/home/wu/slam_in_autonomous_driving/data/ch9/keyframes.txt", keyframes)) {LOG(ERROR) << "failed to load keyframes.txt";return -1;}if (keyframes.empty()) {LOG(INFO) << "keyframes are empty";return 0;}// dump kf cloud and mergeLOG(INFO) << "merging";CloudPtr global_cloud(new PointCloudType);pcl::VoxelGrid<PointType> voxel_grid_filter;float resolution = FLAGS_voxel_size;voxel_grid_filter.setLeafSize(resolution, resolution, resolution);int cnt = 0;for (auto& kfp : keyframes) {auto kf = kfp.second;SE3 pose;if (FLAGS_pose_source == "rtk") {pose = kf->rtk_pose_;} else if (FLAGS_pose_source == "lidar") {pose = kf->lidar_pose_;} else if (FLAGS_pose_source == "opti1") {pose = kf->opti_pose_1_;} else if (FLAGS_pose_source == "opti2") {pose = kf->opti_pose_2_;}kf->LoadScan("/home/wu/slam_in_autonomous_driving/data/ch9/");CloudPtr cloud_trans(new PointCloudType);pcl::transformPointCloud(*kf->cloud_, *cloud_trans, pose.matrix());// voxel sizeCloudPtr kf_cloud_voxeled(new PointCloudType);voxel_grid_filter.setInputCloud(cloud_trans);voxel_grid_filter.filter(*kf_cloud_voxeled);*global_cloud += *kf_cloud_voxeled;kf->cloud_ = nullptr;LOG(INFO) << "merging " << cnt << " in " << keyframes.size() << ", pts: " << kf_cloud_voxeled->size()<< " global pts: " << global_cloud->size();cnt++;}if (!global_cloud->empty()) {sad::SaveCloudToFile(FLAGS_dump_to + "/map.pcd", *global_cloud);}LOG(INFO) << "done.";return 0;
}

? ? ? ? 運行結果：

wu@WP:~/slam_in_autonomous_driving/bin$ ./dump_map --pose_source=opti2I0114 15:08:02.963590 425971 keyframe.cc:78] Loaded kfs: 1143
I0114 15:08:02.963681 425971 dump_map.cc:42] merging
I0114 15:08:02.967100 425971 dump_map.cc:76] merging 0 in 1143, pts: 1758 global pts: 1758
I0114 15:08:02.970384 425971 dump_map.cc:76] merging 1 in 1143, pts: 1771 global pts: 3529
I0114 15:08:02.973193 425971 dump_map.cc:76] merging 2 in 1143, pts: 1524 global pts: 5053
//* 省略 *//
I0114 15:08:09.278196 425971 dump_map.cc:76] merging 1140 in 1143, pts: 2632 global pts: 3317012
I0114 15:08:09.282899 425971 dump_map.cc:76] merging 1141 in 1143, pts: 2334 global pts: 3319346
I0114 15:08:09.287400 425971 dump_map.cc:76] merging 1142 in 1143, pts: 2246 global pts: 3321592
I0114 15:08:12.711279 425971 dump_map.cc:85] done.

? ? ? ? 7 地圖分塊

?????????在多數應用中，我們希望控制實時點云的載入規模，例如只加載自身周圍 200 米范圍內的點云，其他范圍的點云則視情況卸載，這樣可以控制實時系統的計算量。因此需要對大地圖進行分塊處理（這里按照 100m 進行切分）。

????????點云的切分實際上是根據每個點的坐標計算其所在的網格，然后把它投到對應的網格中去。如果我們考慮更周密一些的話，也可以把并行化、點云分批讀寫等行為考慮進來。

? ? ? ? 地圖分塊的流程是：遍歷每一個關鍵幀。對于當前關鍵幀，遍歷其每一個點，對每個點的? $x,y$ ?坐標進行處理，判斷其所屬的網格并將其插入到對應的子地圖塊中。

? ? ? ? 這里對每個點的? $x,y$ ?坐標進行如下處理：

            // floor()：向下取整。-50 是為了使包含原 (0,0) 的區域不被分開,形成以 (0,0) 為中心的塊int gx = floor((pt.x - 50.0) / 100);int gy = floor((pt.y - 50.0) / 100);

? ? ? ? ?

int main(int argc, char** argv) {google::InitGoogleLogging(argv[0]);FLAGS_stderrthreshold = google::INFO;FLAGS_colorlogtostderr = true;google::ParseCommandLineFlags(&argc, &argv, true);using namespace sad;std::map<IdType, KFPtr> keyframes;if (!LoadKeyFrames("/home/wu/slam_in_autonomous_driving/data/ch9/keyframes.txt", keyframes)) {LOG(ERROR) << "failed to load keyframes";return 0;}std::map<Vec2i, CloudPtr, less_vec<2>> map_data;  // 以網格ID為索引的地圖數據pcl::VoxelGrid<PointType> voxel_grid_filter;float resolution = FLAGS_voxel_size;voxel_grid_filter.setLeafSize(resolution, resolution, resolution);// 邏輯和dump map差不多，但每個點個查找它的網格ID，沒有的話會創建for (auto& kfp : keyframes) {auto kf = kfp.second;kf->LoadScan("/home/wu/slam_in_autonomous_driving/data/ch9/");CloudPtr cloud_trans(new PointCloudType);pcl::transformPointCloud(*kf->cloud_, *cloud_trans, kf->opti_pose_2_.matrix());// voxel sizeCloudPtr kf_cloud_voxeled(new PointCloudType);voxel_grid_filter.setInputCloud(cloud_trans);voxel_grid_filter.filter(*kf_cloud_voxeled);LOG(INFO) << "building kf " << kf->id_ << " in " << keyframes.size();// add to gridfor (const auto& pt : kf_cloud_voxeled->points) {// floor()：向下取整。-50 是為了使包含原 (0,0) 的區域不被分開,形成以 (0,0) 為中心的塊int gx = floor((pt.x - 50.0) / 100);int gy = floor((pt.y - 50.0) / 100);Vec2i key(gx, gy);auto iter = map_data.find(key);if (iter == map_data.end()) {// create point cloudCloudPtr cloud(new PointCloudType);// 最后插入的是 pt，(gx, gy)只是為了分塊。cloud->points.emplace_back(pt);cloud->is_dense = false;cloud->height = 1;map_data.emplace(key, cloud);} else {iter->second->points.emplace_back(pt);}}}// 存儲點云和索引文件LOG(INFO) << "saving maps, grids: " << map_data.size();std::system("mkdir -p /home/wu/slam_in_autonomous_driving/data/ch9/map_data/");std::system("rm -rf /home/wu/slam_in_autonomous_driving/data/ch9/map_data/*");  // 清理一下文件夾std::ofstream fout("/home/wu/slam_in_autonomous_driving/data/ch9/map_data/map_index.txt");for (auto& dp : map_data) {fout << dp.first[0] << " " << dp.first[1] << std::endl;dp.second->width = dp.second->size();sad::VoxelGrid(dp.second, 0.1);sad::SaveCloudToFile("/home/wu/slam_in_autonomous_driving/data/ch9/map_data/" + std::to_string(dp.first[0]) + "_" + std::to_string(dp.first[1]) + ".pcd",*dp.second);}fout.close();LOG(INFO) << "done.";return 0;
}

? ? ? ? 運行結果：

wu@WP:~/slam_in_autonomous_driving/bin$ ./split_map I0114 15:20:58.483939 426447 keyframe.cc:78] Loaded kfs: 1143
I0114 15:20:58.487437 426447 split_map.cc:52] building kf 0 in 1143
I0114 15:20:58.490739 426447 split_map.cc:52] building kf 1 in 1143
I0114 15:20:58.493566 426447 split_map.cc:52] building kf 2 in 1143
//* 省略 *//
I0114 15:21:04.844982 426447 split_map.cc:52] building kf 1140 in 1143
I0114 15:21:04.849773 426447 split_map.cc:52] building kf 1141 in 1143
I0114 15:21:04.854301 426447 split_map.cc:52] building kf 1142 in 1143
I0114 15:21:04.854329 426447 split_map.cc:76] saving maps, grids: 24
I0114 15:21:06.976038 426447 split_map.cc:91] done.

?????????8 本章小結

????????本章主要向讀者演示了一個完整的點云地圖構建、優化、切分導出的程序，可以看到，整個建圖過程是比較流程化、自動化的。這些地圖可以幫助我們進行激光高精定位，讓車輛和機器人在沒有 RTK 的環境下獲取高精度位姿。

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 frontend";sad::Frontend frontend(FLAGS_config_yaml);if (!frontend.Init()) {LOG(ERROR) << "failed to init frontend.";return -1;}frontend.Run();sad::Optimization opti(FLAGS_config_yaml);if (!opti.Init(1)) {LOG(ERROR) << "failed to init opti1.";return -1;}opti.Run();sad::LoopClosure lc(FLAGS_config_yaml);if (!lc.Init()) {LOG(ERROR) << "failed to init loop closure.";return -1;}lc.Run();sad::Optimization opti2(FLAGS_config_yaml);if (!opti2.Init(2)) {LOG(ERROR) << "failed to init opti2.";return -1;}opti2.Run();LOG(INFO) << "done.";return 0;
}