SLAM中的非線性優化-2D圖優化之零空間實戰（十六）

? ? ? ?終于有時間更新實戰篇了，本節實戰幾乎包含了SLAM后端的所有技巧，其中包括：舒爾補/先驗Factor/魯棒核函數/FEJ/BA優化等滑動窗口法的相關技巧，其中構建2D輪式里程計預積分以及絕對位姿觀測的10幀滑動窗口，并邊緣化最老幀，其中所有雅可比及其殘差都可以在本系列博客之前章節找到對應的數學原理，完整版代碼可在如下地址找到

slam_optimizer: 個人CSDN博客《SLAM中非線性優化的從古至今》對應的源碼，該系列博客地址：https://blog.csdn.net/zl_vslam/category_12872677.html - Gitee.comhttps://gitee.com/zl_vslam/slam_optimizer/tree/master/2d_optimize/ch16

接下來請看實戰。

一. 主函數

int main() {  double acc_noise_std = 0.1;double init_bg = 0.0;std::vector<double> timestamps;std::vector<Eigen::Vector3d> imu_data;  std::vector<Eigen::Vector3d> odometry_data;std::vector<Eigen::Vector3d> vel_data;  std::vector<Eigen::Vector3d> pose_data;std::vector<Eigen::Vector3d> gps_data;read_simulate(init_bg, timestamps, imu_data, odometry_data, vel_data, pose_data, gps_data);// initial wheel WheelOptions wheel_options;wheel_options.sigma_x = 1e-8;wheel_options.sigma_y = 1e-8;wheel_options.sigma_t = 1e-8;std::shared_ptr<WheelPreIntegration> wheel_preintegration = std::make_shared<WheelPreIntegration>(wheel_options);Optimizer optimizer(wheel_preintegration);// initial stateState state;  memset(&state, 0.0, sizeof(state));std::vector<Eigen::Vector3d> pose_gt = pose_data;state.timestamp = timestamps[0];state.p = pose_gt[0].head<2>();state.R = SO2(pose_gt[0][2]);state.type = "s";State last_state = state; Eigen::Vector3d last_pose;int id = optimizer.AddVertex(state, true);int last_id = id; for(unsigned int i = 1; i < timestamps.size(); i++) {cout << "\ncurr index ==== : " << i << std::endl;double delta_time = timestamps[i]-timestamps[i-1];// cout << "delta_time ==== : " << delta_time << std::endl;Eigen::Vector3d last_odometry_o = odometry_data[i-1];Eigen::Vector3d curr_odometry_o = odometry_data[i];Eigen::Vector3d delta_odometry = curr_odometry_o - last_odometry_o;std::cout << " last_odometry_o =======: " << last_odometry_o.transpose() << std::endl;std::cout << " curr_odometry_o =======: " << curr_odometry_o.transpose() << std::endl;std::cout << " delta_odometry =======: " << delta_odometry.transpose() << std::endl;Eigen::Vector3d last_pose = pose_data[i-1];Eigen::Vector3d curr_pose = pose_data[i];SE2 last_odometry = SE2(last_odometry_o[0], last_odometry_o[1], last_odometry_o[2]);SE2 current_odometry = SE2(curr_odometry_o[0], curr_odometry_o[1], curr_odometry_o[2]);wheel_preintegration->Integrate(last_odometry, current_odometry);State& last_state_t = state;wheel_preintegration->Predict(last_state_t);state.type = "s";id = optimizer.AddVertex(state);State last_pose_state;last_pose_state.timestamp = timestamps[i-1];last_pose_state.p = last_pose.head<2>();last_pose_state.R = SO2(last_pose[2]);last_pose_state.type = "p";State pose_state;pose_state.timestamp = timestamps[i];pose_state.p = curr_pose.head<2>();pose_state.R = SO2(curr_pose[2]);	pose_state.type = "p";if(i > 0) {Matrix3d info = Matrix3d::Identity();State wheel_state;wheel_state.timestamp = timestamps[i];wheel_state.p = wheel_preintegration->GetTranslation();wheel_state.R = wheel_preintegration->GetSO2();wheel_state.type = "w";info = wheel_preintegration->GetCov().inverse();optimizer.AddEdge(PoseGraphEdge(id-1, id, wheel_state, info));Matrix3d pose_info = 100 * Matrix3d::Identity();double last_pose_sigma = 1e-15;double curr_pose_sigma = 1e-15;Eigen::Matrix<double, 3, 3> last_pose_covariance = Eigen::Matrix<double, 3, 3>::Identity();last_pose_covariance(0,0) = std::pow(last_pose_sigma,2);last_pose_covariance(1,1) = std::pow(last_pose_sigma,2);last_pose_covariance(2,2) = std::pow(last_pose_sigma,2);Eigen::Matrix<double, 3, 3> last_information_matrix = last_pose_covariance.inverse();Eigen::Matrix<double, 3, 3> curr_pose_covariance = Eigen::Matrix<double, 3, 3>::Identity();curr_pose_covariance(0,0) = std::pow(curr_pose_sigma,2);curr_pose_covariance(1,1) = std::pow(curr_pose_sigma,2);curr_pose_covariance(2,2) = std::pow(curr_pose_sigma,2);Eigen::Matrix<double, 3, 3> curr_information_matrix = curr_pose_covariance.inverse();optimizer.AddEdge(PoseGraphEdge(id-1, id-1, last_pose_state, last_information_matrix));optimizer.AddEdge(PoseGraphEdge(id, id, pose_state, curr_information_matrix));std::cout << " wheel dp_ =======: " << wheel_preintegration->dp_.transpose() << std::endl;std::cout << " wheel dR_ =======: " << wheel_preintegration->dR_.log() << std::endl;optimizer.Optimize(20);optimizer.UpdateState(state, id);wheel_preintegration = std::make_shared<WheelPreIntegration>(wheel_options);cout << "After step " << i << ":" << endl;optimizer.PrintPoses();cout << "----------------------" << endl;// cout << "curr_odometry ==== : " << curr_odometry_o.transpose() << std::endl;// cout << "delta_odometry ==== : " << delta_odometry.transpose() << std::endl;// cout << "curr_velocity ==== : " << curr_velocity.transpose() << std::endl;cout << "gt_pose ==== : " << curr_pose.transpose() << std::endl;// std::cout << "optimize state.R.log() ============= : " << state.R.log() << std::endl;std::cout << "optimize state.pose ============= : " << state.p.transpose() << " " << state.R.log() << std::endl;// std::cout << "optimize state.cov ============= : \n" << state.cov << std::endl;last_id = id;last_state = state;usleep(150000);		}}return 0;
}

與之前相似，相信大家能夠明白

二. 輪式里程計預積分

void WheelPreIntegration::Integrate(const SE2& last_odometry, const SE2& current_odometry) {// preintegrationSE2 odok = current_odometry - last_odometry;Eigen::Vector2d odork = odok.translation();Eigen::Matrix2d Phi_ik = dR_.matrix();dp_ += Phi_ik * odork;dR_ = dR_ * odok.so2();// std::cout << " Phi_ik =======: " << Phi_ik << std::endl;// std::cout << " odork =======: " << odork << std::endl;// std::cout << " wheel dp_0 =======: " << dp_.transpose() << std::endl;// std::cout << " wheel dR_0 =======: " << dR_.log() << std::endl;Eigen::Matrix3d Ak = Eigen::Matrix3d::Identity();Eigen::Matrix3d Bk = Eigen::Matrix3d::Identity();Ak.block<2,1>(1,0) = Phi_ik * Eigen::Vector2d(-odork[1], odork[0]);Bk.block<2,2>(1,1) = Phi_ik;// Ak.block<2,1>(0,2) = Phi_ik * Eigen::Vector2d(-odork[1], odork[0]);// Bk.block<2,2>(0,0) = Phi_ik;Eigen::Matrix3d Sigma_vk = Eigen::Matrix3d::Identity();Sigma_vk(0,0) = options_.sigma_t * options_.sigma_t;Sigma_vk(1,1) = options_.sigma_x * options_.sigma_x;Sigma_vk(2,2) = options_.sigma_y * options_.sigma_y;cov_ =  Ak * cov_ * Ak.transpose() + Bk * Sigma_vk * Bk.transpose();
}

這段代碼的原理在之前章節也可以找到

三. 預積分Factor

void WheelFactor::ComputeResidualAndJacobianPoseGraph(const State& last_state, const State& state, Eigen::Matrix<double, 3, 1> &residual, Eigen::Matrix<double, 3, 3> &jacobianXi, Eigen::Matrix<double, 3, 3> &jacobianXj) {Eigen::Matrix2d Ri = SO2(last_state.R).matrix();Eigen::Vector2d ri = last_state.p;double ai = SO2(last_state.R).log();double aj = SO2(state.R).log();Eigen::Vector2d rj = state.p;// 公式（24）residual.head<2>() = Ri.transpose() * (rj-ri) - wheel_preintegration_->dp_;residual[2] = SO2(aj - ai - wheel_preintegration_->dR_.log()).log();// compute jacobian matrixEigen::Vector2d rij = rj-ri;Eigen::Vector2d rij_x(-rij[1], rij[0]);// 公式（25）jacobianXi.block<2,2>(0,0) = -Ri.transpose();jacobianXi.block<2,1>(0,2) = -Ri.transpose() * rij_x;jacobianXi.block<1,2>(2,0).setZero();jacobianXi(2,2) = -1;jacobianXj.setIdentity();jacobianXj.block<2,2>(0,0) = Ri.transpose();
}

四.位姿Factor

// predict - obs
void PoseObserve::ComputeResidualAndJacobianPoseGraph(const State& state, const State& pose_state, Eigen::Matrix<double, 3, 1> &residualXi, Eigen::Matrix<double, 3, 3> &jacobianXi) {Eigen::Vector2d res_p = state.p - pose_state.p;double res_theta = math_utils::theta_normalize(SO2(state.R).log() - SO2(pose_state.R).log());residualXi.block<2,1>(0,0) = res_p;residualXi(2,0) = res_theta;// cout << "residualXi ================== : " << residualXi << endl;jacobianXi = Eigen::Matrix<double, 3, 3>::Identity();// cout << "jacobianXi == : \n" << jacobianXi << endl;
}

五. 相關修改位置

        for (size_t edge_idx : related_edges) {const auto& edge = edges_[edge_idx];if (!vertices_.count(edge.id1) || !vertices_.count(edge.id2)) continue;State xi = vertices_.at(edge.id1), xj = vertices_.at(edge.id2);Vector3d e;Eigen::Matrix<double, 3, 3> jacobianXi, jacobianXj;if(edge.measurement.type == "w") {wheel_factor_->ComputeResidualAndJacobianPoseGraph(xi, xj, e, jacobianXi, jacobianXj);MatrixXd weighted_H;VectorXd weighted_e;std::tie(weighted_H, weighted_e) = robust_kernel_->robustify(e);Matrix3d Omega_robust = edge.information * weighted_H;Vector3d e_robust = weighted_e;int idx1 = local_indices[edge.id1], idx2 = local_indices[edge.id2];// cout << "w idx1 ==== : " << idx1 << " , " << idx2 << std::endl;H_local.block<3, 3>(3*idx1, 3*idx1) += jacobianXi.transpose() * Omega_robust * jacobianXi;H_local.block<3, 3>(3*idx1, 3*idx2) += jacobianXi.transpose() * Omega_robust * jacobianXj;H_local.block<3, 3>(3*idx2, 3*idx1) += jacobianXj.transpose() * Omega_robust * jacobianXi;H_local.block<3, 3>(3*idx2, 3*idx2) += jacobianXj.transpose() * Omega_robust * jacobianXj;b_local.segment<3>(3*idx1) += jacobianXi.transpose() * Omega_robust * e_robust;b_local.segment<3>(3*idx2) += jacobianXj.transpose() * Omega_robust * e_robust;}// cout << "edge.id1 ==== : " << edge.id1 << std::endl;// cout << "edge.id2 ==== : " << edge.id2 << std::endl;if(edge.measurement.type == "p") {if(edge.id1 == edge.id2) {PoseObserve::ComputeResidualAndJacobianPoseGraph(xi, edge.measurement, e, jacobianXi);MatrixXd weighted_H;VectorXd weighted_e;std::tie(weighted_H, weighted_e) = robust_kernel_->robustify(e);Matrix3d Omega_robust = edge.information * weighted_H;Vector3d e_robust = weighted_e;int idx1 = local_indices[edge.id1], idx2 = local_indices[edge.id2];// cout << "p idx1 ==== : " << idx1 << " , " << idx2 << std::endl;H_local.block<3, 3>(3*idx1, 3*idx1) += jacobianXi.transpose() * Omega_robust * jacobianXi;// H_local.block<3, 3>(3*idx1, 3*idx2) += jacobianXi.transpose() * Omega_robust * jacobianXj;// H_local.block<3, 3>(3*idx2, 3*idx1) += jacobianXj.transpose() * Omega_robust * jacobianXi;// H_local.block<3, 3>(3*idx2, 3*idx2) += jacobianXj.transpose() * Omega_robust * jacobianXj;b_local.segment<3>(3*idx1) += jacobianXi.transpose() * Omega_robust * e_robust;// b_local.segment<3>(3*idx2) += jacobianXj.transpose() * Omega_robust * e_robust;}}    }

主要是替換上節代碼相應位置的殘差及雅可比，其它基本沒有改動，就不作過多說明了

總結

? ? ? ?經過這樣一個10幀的滑動窗口法，算法即可獲得優化效果，其中需要調節協方差相關參數，即可影響結果，本代碼中，融合的是真值位姿，因此最終結果也收斂到了真值上，大家若想體驗其它效果，可自行調節sigma之類的參數，本節更新較晚，主要還是代碼調試需要時間，本節更新完，2D圖優化也將進入終極篇了，終極篇預告：2D視覺慣性VIO，敬請期待

本文來自互聯網用戶投稿，該文觀點僅代表作者本人，不代表本站立場。本站僅提供信息存儲空間服務，不擁有所有權，不承擔相關法律責任。
如若轉載，請注明出處：http://www.pswp.cn/web/91697.shtml
繁體地址，請注明出處：http://hk.pswp.cn/web/91697.shtml
英文地址，請注明出處：http://en.pswp.cn/web/91697.shtml

如若內容造成侵權/違法違規/事實不符，請聯系多彩編程網進行投訴反饋email:809451989@qq.com，一經查實，立即刪除！

相關文章

知識隨記-----Qt 實戰教程：使用 QNetworkAccessManager 發送 HTTP POST

知識隨記-----Qt 實戰教程：使用 QNetworkAccessManager 發送 HTTP POST

文章目錄Qt 網絡編程&#xff1a;使用 QNetworkAccessManager 實現 HTTP POST 請求概要整體架構流程技術名詞解釋技術細節注意事項&#xff1a;Qt 網絡編程&#xff1a;使用 QNetworkAccessManager 實現 HTTP POST 請求 概要 本文介紹如何使用 Qt 框架的網絡模塊&#xff08;…
閱讀更多...
wordpress批量新建產品分類

wordpress批量新建產品分類

1、下載安裝插件&#xff1a;bulk-category-import-export2、激活插件后&#xff0c;左側點擊插件下的導入&#xff0c;選擇product categories&#xff0c;點擊下一步3、這里可以選擇導入的分類列表文件&#xff0c;可以選擇分隔符&#xff0c;CSV文件默認為‘&#xff0c;’要…
閱讀更多...
CentOS 鏡像源配置與 EOL 后的應對策略

CentOS 鏡像源配置與 EOL 后的應對策略

引言 本文將詳細介紹如何使用 阿里云開源鏡像站 配置 CentOS 的各類軟件源&#xff0c;包括基礎源、歷史歸檔源&#xff08;vault&#xff09;、ARM 架構源、Stream 版本以及調試信息源&#xff08;debuginfo&#xff09;&#xff0c;并重點講解在 CentOS 8 停止維護后&#x…
閱讀更多...
CTF實戰：用Sqlmap破解表單輸入型SQL注入題（輸入賬號密碼/usernamepassword）

CTF實戰：用Sqlmap破解表單輸入型SQL注入題（輸入賬號密碼/usernamepassword）

目錄 引言 步驟1&#xff1a;用Burp Suite捕獲表單請求 步驟2&#xff1a;用Sqlmap獲取數據庫名稱 參數解釋&#xff1a; 輸出示例&#xff08;根據題目環境調整&#xff09;&#xff1a; 步驟3&#xff1a;獲取目標數據庫中的表名 參數解釋&#xff1a; 輸出示例&#…
閱讀更多...
質數時間（二分查找）

質數時間（二分查找）

題目描述如果把一年之中的某個時間寫作 a 月 b 日 c 時 d 分 e 秒的形式&#xff0c;當這五個數都為質數時&#xff0c;我們把這樣的時間叫做質數時間&#xff0c;現已知起始時刻是 2022 年的 a 月 b 日 c 時 d 分 e 秒&#xff0c;終止時刻是 2022 年的 u 月 v 日 w 時 x 分 y…
閱讀更多...
Python訓練Day29

Python訓練Day29

浙大疏錦行 類的裝飾器裝飾器思想的進一步理解&#xff1a;外部修改、動態類方法的定義&#xff1a;內部定義和外部定義
閱讀更多...
新手DBA實戰指南：如何使用gh-ost實現MySQL無鎖表結構變更

新手DBA實戰指南：如何使用gh-ost實現MySQL無鎖表結構變更

新手DBA實戰指南：如何使用gh-ost實現MySQL無鎖表結構變更 作為DBA，大表結構變更(DDL)一直是令人頭疼的問題。傳統的ALTER TABLE操作會鎖表，嚴重影響業務連續性；而常見的pt-online-schema-change工具雖然能實現在線變更，但依賴觸發器機制，在高并發場景下性能表現不佳。本…
閱讀更多...
OSPF綜合

OSPF綜合

一、實驗拓撲二、實驗需求1、R4為ISP&#xff0c;其上只配置IP地址&#xff1b;R4與其他所直連設備間均使用公有IP&#xff1b; 2、R3-R5、R6、R7為MGRE環境&#xff0c;R3為中心站點&#xff1b; 3、整個OSPF環境IP基于172.16.0.0/16劃分&#xff1b;除了R12有兩個環回&#x…
閱讀更多...
技術面試知識點詳解 - 從電路到編程的全棧面經

技術面試知識點詳解 - 從電路到編程的全棧面經

技術面試知識點詳解 - 從電路到編程的全棧面經 目錄 模擬電路基礎數字電路原理電源設計相關編程語言基礎數據庫與并發網絡協議基礎算法與數據結構 模擬電路基礎 1. 放大電路類型判斷 這是模擬電路面試的經典題目&#xff0c;通過電壓放大倍數判斷放大電路類型&#xff1a; …
閱讀更多...
LangGraph認知篇-Command函數

LangGraph認知篇-Command函數

Command簡述 在 LangGraph 中&#xff0c;Command 是一個極具實用性的功能&#xff0c;它能夠將控制流&#xff08;邊&#xff09;和狀態更新&#xff08;節點&#xff09;巧妙地結合起來。這意味著開發者可以在同一個節點中&#xff0c;既執行狀態更新操作&#xff0c;又決定下…
閱讀更多...
【目標檢測】小樣本度量學習

【目標檢測】小樣本度量學習

小樣本度量學習&#xff08;Few-Shot Metric Learning&#xff09;通常用于分類任務?&#xff08;如圖像分類&#xff09;&#xff0c;但它也可以與目標檢測&#xff08;Object Detection&#xff09;結合&#xff0c;解決小樣本目標檢測&#xff08;Few-Shot Object Detectio…
閱讀更多...
cmd怎么取消關機命令

cmd怎么取消關機命令

在 Windows 的命令提示符&#xff08;CMD&#xff09;中取消已計劃的關機操作&#xff0c;可以通過 shutdown 命令的 ?**-a**? 參數實現。以下是具體步驟&#xff1a;?操作方法??打開 CMD?按下 Win R 組合鍵&#xff0c;輸入 cmd 并回車&#xff0c;打開命令提示符窗口。…
閱讀更多...
網易云音樂硬剛騰訊系！起訴SM娛樂濫用市場支配地位

網易云音樂硬剛騰訊系！起訴SM娛樂濫用市場支配地位

企查查APP顯示&#xff0c;近日&#xff0c;法院公開杭州樂讀科技有限公司、杭州網易云音樂科技有限公司起訴SM ENTERTAINMENT CO. 、卡斯夢&#xff08;上海&#xff09;文化傳播有限公司等開庭信息&#xff0c;案由涉及濫用市場支配地位糾紛。公告顯示&#xff0c;該案件計劃…
閱讀更多...
[css]切角

[css]切角

使用css實現一個切角的功能&#xff0c;有以下幾種方案&#xff1a; <div class"box"></div>方案一&#xff1a;linear-gradient linear-gradient配合backgroud-image可以實現背景漸變的效果。linear-gradient的漸變過渡區的占比是總的空間&#xff08;高…
閱讀更多...
分享一個可以測試離線服務器性能的腳本

分享一個可以測試離線服務器性能的腳本

在日常運維工作中&#xff0c;經常會遇到系統性能莫名跟不上業務需求的情況&#xff1a;服務器響應變慢、應用加載卡頓、資源占用異常飆升等問題頻繁出現&#xff0c;卻難以快速問題根源究竟在CPU過載、內存泄漏、磁盤I/O阻塞還是網絡帶寬瓶頸。這種時候&#xff0c;特別需要一…
閱讀更多...
Python Pandas.unique函數解析與實戰教程

Python Pandas.unique函數解析與實戰教程

Python Pandas.unique 函數解析與實戰教程 摘要 本文章旨在全面地解析 pandas 庫中的 unique 函數。pandas.unique 是一個用于從一維數組型（array-like）對象中提取唯一值的高效工具。我們將從其核心功能、函數簽名、參數詳解、返回值類型，到關鍵行為特性（如順序保留、缺失…
閱讀更多...
排序算法入門：直接插入排序詳解

排序算法入門：直接插入排序詳解

這里寫目錄標題介紹原理代碼實現分析介紹 直接插入排序是一種簡單直觀的排序算法&#xff0c;適用于小規模數據或基本有序的數據集。其核心思想是構建有序序列&#xff0c;對于未排序數據&#xff0c;在已排序序列中從后向前掃描&#xff0c;找到相應位置并插入。 原理 我們…
閱讀更多...
ClickHouse MergeTree引擎：從核心架構到三級索引實戰

ClickHouse MergeTree引擎：從核心架構到三級索引實戰

摘要 MergeTree是ClickHouse最核心的存儲引擎&#xff0c;采用列式存儲LSM-Tree架構設計&#xff0c;支持高效的數據寫入、合并和查詢。本文將全面解析MergeTree引擎的基礎概念、數據流、核心架構、索引系統以及常見問題。 基礎篇&#xff1a; 一、MergeTree引擎基礎概念 1. 定…
閱讀更多...
電腦手機熱點方式通信（上）

電腦手機熱點方式通信（上）

電腦連接手機熱點時的無線鏈路情況&#xff1a; 電腦上網時&#xff08;從服務器下載數據&#xff0c;或者上傳指令、數據&#xff09;&#xff0c;首先電腦與手機之間基于WiFi協議在2.4G頻段或者5G頻段通信&#xff0c;然后手機與基站之間再基于4G LTE或者5G NR協議在2412MHz…
閱讀更多...
MySQL CPU占用過高排查指南

MySQL CPU占用過高排查指南

MySQL CPU 占用過高時&#xff0c;排查具體占用資源的表需結合系統監控、數據庫分析工具和 SQL 診斷命令。&#x1f50d; ?一、快速定位問題根源??確認 MySQL 進程占用 CPU?使用 top 或 htop 命令查看系統進程&#xff0c;確認是否為 mysqld 進程導致 CPU 飆升。若 MySQL 進…
閱讀更多...
最新文章

Copyright @ 2022~2023