欧美bbbwbbbw肥妇,免费乱码人妻系列日韩,一级黄片

C++?Cartographer的入口node main詳細(xì)講解

 更新時(shí)間:2023年03月16日 11:46:11   作者:蝦眠不覺曉,  
這篇文章主要介紹了C++Node類Cartographer的入口node main,文中通過示例代碼介紹的非常詳細(xì),對大家的學(xué)習(xí)或者工作具有一定的參考學(xué)習(xí)價(jià)值,需要的朋友們下面隨著小編來一起學(xué)習(xí)吧

啃一下谷歌優(yōu)秀的激光SLAM開源框架-Cartographer. 這個(gè)框架算法簡單,但是程序部分太多需要學(xué)習(xí)的地方了.不論是整體框架的結(jié)構(gòu),還是數(shù)據(jù)的使用,都是非常優(yōu)美的.不愧是大公司啊.接下來記錄一下每天學(xué)習(xí)的內(nèi)容和心得,督促自己堅(jiān)持下去!

node_main.cc是整個(gè)Cartographer程序的入口,用來調(diào)用整個(gè)Cartographer進(jìn)程。以最基礎(chǔ)的單線雷達(dá)和輪速計(jì)為例。

整體的代碼開始是在Run函數(shù)中實(shí)現(xiàn)的。

Run函數(shù)

void Run() {
  constexpr double kTfBufferCacheTimeInSeconds = 10.;
  tf2_ros::Buffer tf_buffer{::ros::Duration(kTfBufferCacheTimeInSeconds)};
  // 開啟監(jiān)聽tf的獨(dú)立線程
  tf2_ros::TransformListener tf(tf_buffer);
  NodeOptions node_options;
  TrajectoryOptions trajectory_options;
  // c++11: std::tie()函數(shù)可以將變量連接到一個(gè)給定的tuple上,生成一個(gè)元素類型全是引用的tuple
  // 讀取Lua文件內(nèi)容,把Lua文件內(nèi)容給到node_options和trajectory_options
  std::tie(node_options, trajectory_options) =
      LoadOptions(FLAGS_configuration_directory, FLAGS_configuration_basename);
  // MapBuilder類是完整的SLAM算法類
  // 包含前端(TrajectoryBuilders,scan to submap) 與 后端(用于查找回環(huán)的PoseGraph) 
  auto map_builder =
      cartographer::mapping::CreateMapBuilder(node_options.map_builder_options);//在map_builder.cc中實(shí)現(xiàn),工廠函數(shù)
                                                                                //在這里,實(shí)例化一個(gè)MapBuilder, 而MapBuilder是MapBuilderInterface的子類                                                                             //MapBuilder的AddTrajectoryBuilder實(shí)例化了CollatedTrajectoryBuilder 
  // c++11: std::move 是將對象的狀態(tài)或者所有權(quán)從一個(gè)對象轉(zhuǎn)移到另一個(gè)對象, 
  // 只是轉(zhuǎn)移, 沒有內(nèi)存的搬遷或者內(nèi)存拷貝所以可以提高利用效率,改善性能..
  // 右值引用是用來支持轉(zhuǎn)移語義的.轉(zhuǎn)移語義可以將資源 ( 堆, 系統(tǒng)對象等 ) 從一個(gè)對象轉(zhuǎn)移到另一個(gè)對象, 
  // 這樣能夠減少不必要的臨時(shí)對象的創(chuàng)建、拷貝以及銷毀, 能夠大幅度提高 C++ 應(yīng)用程序的性能.
  // 臨時(shí)對象的維護(hù) ( 創(chuàng)建和銷毀 ) 對性能有嚴(yán)重影響.
  // Node類的初始化, 開啟訂閱,發(fā)布topic和service,將ROS的topic傳入SLAM, 也就是MapBuilder
  Node node(node_options, std::move(map_builder), &tf_buffer,
            FLAGS_collect_metrics);
  // 如果加載了pbstream文件, 就執(zhí)行這個(gè)函數(shù),為定位
  if (!FLAGS_load_state_filename.empty()) {
    node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state);
  }
  // 使用默認(rèn)topic 開始軌跡
  if (FLAGS_start_trajectory_with_default_topics) {
    node.StartTrajectoryWithDefaultTopics(trajectory_options);
  }
  ::ros::spin();
  // 結(jié)束所有處于活動狀態(tài)的軌跡
  node.FinishAllTrajectories();
  // 當(dāng)所有的軌跡結(jié)束時(shí), 再執(zhí)行一次全局優(yōu)化
  node.RunFinalOptimization();
  // 如果save_state_filename非空, 就保存pbstream文件
  if (!FLAGS_save_state_filename.empty()) {
    node.SerializeState(FLAGS_save_state_filename,
                        true /* include_unfinished_submaps */);
  }
}
}  // namespace
}  // namespace cartographer_ros

Run函數(shù)主要做了一下幾件事:

  • 讀取Lua配置文件中的內(nèi)容,確定節(jié)點(diǎn)構(gòu)造的方式和軌跡構(gòu)造的方式與參數(shù)。
  • 實(shí)例化map_builder,map_builder是完整的SLAM算法類,包含了前端和后端。具體時(shí)間方式是通過工廠模式。
  • 初始化Node,通過初始化Node,開啟訂閱,發(fā)布topic與service,還將topic帶的傳感器數(shù)據(jù)傳入MapBuilder。
  • 判斷是否為定位還是建圖,并開啟軌跡
  • 死循環(huán),不停地接受topic并運(yùn)行Cartographer
  • 結(jié)束時(shí)停止所用傳感器數(shù)據(jù)的訂閱,并且執(zhí)行一次全局優(yōu)化,保存pbstream地圖文件

讀取配置參數(shù)

其中std::tie很有意思,可以實(shí)現(xiàn)多個(gè)不同類型的返回值. 很多時(shí)候我們想通過一個(gè)函數(shù)丟出去多個(gè)結(jié)果,但一個(gè)函數(shù)只能有一個(gè)返回值,于是我們可以用std::make_tuple把多個(gè)返回值打包成std::tuple類型的數(shù)據(jù),這時(shí)候返回值只是tuple類型了,所以沒有違反只能返回一個(gè)返回值的規(guī)定.這點(diǎn)很類似Python中的pickle和tuple,啥都可以裝在一起丟出去. 實(shí)現(xiàn)文件在node_options.cc

/**
 * @brief 加載lua配置文件中的參數(shù)
 * 
 * @param[in] configuration_directory 配置文件所在目錄
 * @param[in] configuration_basename 配置文件的名字
 * @return std::tuple<NodeOptions, TrajectoryOptions> 返回節(jié)點(diǎn)的配置與軌跡的配置
 */
std::tuple<NodeOptions, TrajectoryOptions> LoadOptions(
    const std::string& configuration_directory,
    const std::string& configuration_basename) {
  // 獲取配置文件所在的目錄
  auto file_resolver =
      absl::make_unique<cartographer::common::ConfigurationFileResolver>(
          std::vector<std::string>{configuration_directory});
  // 讀取配置文件內(nèi)容到code中
  const std::string code =
      file_resolver->GetFileContentOrDie(configuration_basename);
  // 根據(jù)給定的字符串, 生成一個(gè)lua字典
  cartographer::common::LuaParameterDictionary lua_parameter_dictionary(
      code, std::move(file_resolver));
  // 創(chuàng)建元組tuple,元組定義了一個(gè)有固定數(shù)目元素的容器, 其中的每個(gè)元素類型都可以不相同
  // 將配置文件的內(nèi)容填充進(jìn)NodeOptions與TrajectoryOptions, 并返回
  return std::make_tuple(CreateNodeOptions(&lua_parameter_dictionary),
                         CreateTrajectoryOptions(&lua_parameter_dictionary));
}

構(gòu)建地圖構(gòu)建器

Cartographer_ros和Cartographer是兩個(gè)部分,一個(gè)是數(shù)據(jù)處理與分配,一個(gè)才是真正的Cartographer算法代碼的部分,代碼上把ros和算法庫分得很開,讓我們移植和開發(fā)很容易.那么如何讓ros數(shù)據(jù)和Cartographer算法建立聯(lián)系呢?第一步就是地圖構(gòu)建器.

地圖構(gòu)建器的大致作用是調(diào)用Cartographer的算法.

地圖構(gòu)建器通過配置文件中node_options中map_builder_options部分去初始化一個(gè)地圖.這個(gè)地圖構(gòu)建器的作用以后再說.先來看看他是怎么實(shí)現(xiàn)的.

由node_main.cc調(diào)用map_builder中的CreateMapBuilder函數(shù),這個(gè)函數(shù)只有一個(gè)參數(shù),就是上一行從lua中讀取的配置文件內(nèi)容. 進(jìn)入map_builder.cc中:

// 工廠函數(shù),生成接口API
std::unique_ptr<MapBuilderInterface> CreateMapBuilder(
    const proto::MapBuilderOptions& options) {
  return absl::make_unique<MapBuilder>(options);
}

發(fā)現(xiàn)這個(gè)就是一個(gè)接口函數(shù). 但這個(gè)函數(shù)也有用到一些cpp的技巧,值得學(xué)習(xí):

返回值是一個(gè)unique_ptr的MapBuilder類型的類,而返回類型卻定于為MapBuilder的父類MapBuilderInterface類,這在cpp中是允許的,而且這樣做更能讓返回值類型更加有包容性,實(shí)現(xiàn)工廠模式.

MapBuilder這個(gè)類是SLAM算法的入口類十分重要,用來初始化pose_graph,創(chuàng)建軌跡等.會在另一篇中詳細(xì)介紹.

Node類的初始化

Node類的作用主要是傳感器數(shù)據(jù)的獲取和處理,讓數(shù)據(jù)與MapBuilder構(gòu)建聯(lián)系,從而使獲取的raw sensor data能夠灌入Cartographer算法庫,實(shí)現(xiàn)定位建圖等功能.

在node_main.cc中初始化方式如下:

  // Node類的初始化, 開啟訂閱,發(fā)布topic和service,將ROS的topic傳入SLAM, 也就是MapBuilder
  Node node(node_options, std::move(map_builder), &tf_buffer,
            FLAGS_collect_metrics);

這一行代碼也有值得學(xué)習(xí)的地方,就是std::move這個(gè)函數(shù),他通過把某個(gè)實(shí)例化的類變?yōu)橛抑狄萌缓笾苯愚D(zhuǎn)移給某個(gè)對象,從而實(shí)現(xiàn)高效的"轉(zhuǎn)移".

舉個(gè)簡單的不太恰當(dāng)?shù)睦?你想要我的西瓜,有兩種方式,一個(gè)是我不遠(yuǎn)千里坐車給你,還有一種是給西瓜貼上你的名字,別人問我就說我說了不算,問你去. std::move就是后者(如有錯(cuò)請指出哈).所以這樣可以直接從一個(gè)對象轉(zhuǎn)移到另一對象(貼名字),取消了不必要的臨時(shí)對象的創(chuàng)建拷貝與銷毀(運(yùn)輸西瓜需要位子還要搬上搬下). 對于占用很大的類的轉(zhuǎn)移就很節(jié)約開銷(一億噸西瓜咋運(yùn)啊).大致就這個(gè)意思.

Node類的內(nèi)容在node.cc中,主要作用是實(shí)現(xiàn)傳感器數(shù)據(jù)的訂閱發(fā)布以及初始處理, 以及傳遞給mapbuilder.具體內(nèi)容在后面會詳細(xì)介紹.

開始軌跡與結(jié)束軌跡

在上面實(shí)例化了Node類之后,我們就可以調(diào)用node中的方法去建圖. 建圖就不用加載地圖了,畢竟是建圖,所以直接調(diào)用node開始軌跡,然后在進(jìn)入ros中的死循環(huán),不停地接受新的數(shù)據(jù),處理并運(yùn)算,輸出結(jié)果, 直到按下ctrl+c去終止程序,跳出死循環(huán),執(zhí)行結(jié)束輸入數(shù)據(jù)和進(jìn)行最終優(yōu)化.

其實(shí)看程序就可以知道,Cartographer的建圖和定位是一樣的,只是建圖的時(shí)候不加載地圖并且在結(jié)束的時(shí)候保存地圖,定位的時(shí)候加載地圖,可以不保存地圖,也可不進(jìn)行最終優(yōu)化.其實(shí)我測試的不進(jìn)行最終優(yōu)化也是可以的,畢竟定位是實(shí)時(shí)的,就算最終優(yōu)化使之前的定位結(jié)果有變化,機(jī)器人也回不去了.所以我認(rèn)為是可以去掉的.

  // 如果加載了pbstream文件, 就執(zhí)行這個(gè)函數(shù),為定位
  if (!FLAGS_load_state_filename.empty()) {
    node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state);
  }
  // 使用默認(rèn)topic 開始軌跡
  if (FLAGS_start_trajectory_with_default_topics) {
    node.StartTrajectoryWithDefaultTopics(trajectory_options);
  }
  ::ros::spin();
  // 結(jié)束所有處于活動狀態(tài)的軌跡
  node.FinishAllTrajectories();
  // 當(dāng)所有的軌跡結(jié)束時(shí), 再執(zhí)行一次全局優(yōu)化
  node.RunFinalOptimization();
  // 如果save_state_filename非空, 就保存pbstream文件
  if (!FLAGS_save_state_filename.empty()) {
    node.SerializeState(FLAGS_save_state_filename,
                        true /* include_unfinished_submaps */);
  }

LoadState作用是加載地圖文件.這個(gè)地圖不同于可以可視化的地圖,這個(gè)地圖里面包含了位姿圖pose_graph,傳感器數(shù)據(jù)和landmark_pose等其他信息,不單單是一個(gè)地形圖一樣的地圖.調(diào)用的最終函數(shù)是Cartographer算法部分的map_builder.cc中的同名函數(shù),調(diào)用流程一環(huán)套一環(huán)(Cartographer整體框架就是這樣,復(fù)雜但都是必要的).調(diào)用的流程如下:

只有最后一層的map_builder.cc才是Cartographer算法部分的內(nèi)容,才是真正實(shí)現(xiàn)加載地圖的功能. 這部分程序又臭又長,大家可以自己看看,實(shí)現(xiàn)功能加載posegraph和舊地圖的傳感器數(shù)據(jù)與landmark.

StartTrajectoryWithDefaultTopics實(shí)際上是調(diào)用了node.cc的AddTrajectory,去讓map_builder創(chuàng)建一個(gè)軌跡,并且新增位姿估計(jì)器,傳感器數(shù)據(jù)采樣器,訂閱topic以及調(diào)用回調(diào)函數(shù)的功能. 這個(gè)函數(shù)建立了數(shù)據(jù)與算法的統(tǒng)一. 詳細(xì)會在Node中解析.

FinishAllTrajectories調(diào)用node.cc中的FinishTrajectoryUnderLock去結(jié)束傳感器訂閱,然后調(diào)用map_builder的FinishTrajectory()進(jìn)行軌跡的結(jié)束

node::RunFinalOptimization調(diào)用map_builder的pose_graph的RunFinalOptimization實(shí)現(xiàn)結(jié)束建圖后所有位姿圖的最終優(yōu)化.

由此可見, Node類通過類方法,實(shí)現(xiàn)了傳感器數(shù)據(jù)的處理與使用.具體的方式是用了sensor_bridge和map_builder_bridge,把傳感器數(shù)據(jù)轉(zhuǎn)換并且給了Cartographer的算法部分, 實(shí)現(xiàn)了建圖與定位.

到此這篇關(guān)于C++ Cartographer的入口node_main詳細(xì)講解的文章就介紹到這了,更多相關(guān)C++ node_main內(nèi)容請搜索腳本之家以前的文章或繼續(xù)瀏覽下面的相關(guān)文章希望大家以后多多支持腳本之家!

相關(guān)文章

  • Qt(C++)調(diào)用工業(yè)相機(jī)Basler的SDK使用示例

    Qt(C++)調(diào)用工業(yè)相機(jī)Basler的SDK使用示例

    這篇文章主要介紹了Qt(C++)調(diào)用工業(yè)相機(jī)Basler的SDK使用示例,文中通過示例代碼介紹的非常詳細(xì),對大家的學(xué)習(xí)或者工作具有一定的參考學(xué)習(xí)價(jià)值,需要的朋友們下面隨著小編來一起學(xué)習(xí)學(xué)習(xí)吧
    2020-03-03
  • C++實(shí)現(xiàn)圖書管理系統(tǒng)課程設(shè)計(jì)(面向?qū)ο?

    C++實(shí)現(xiàn)圖書管理系統(tǒng)課程設(shè)計(jì)(面向?qū)ο?

    這篇文章主要為大家詳細(xì)介紹了C++實(shí)現(xiàn)圖書管理系統(tǒng)課程設(shè)計(jì),文中示例代碼介紹的非常詳細(xì),具有一定的參考價(jià)值,感興趣的小伙伴們可以參考一下
    2022-03-03
  • C語言中fopen()函數(shù)的使用方法示例詳解

    C語言中fopen()函數(shù)的使用方法示例詳解

    這篇文章主要介紹了C語言中fopen()函數(shù)的使用方法,本文結(jié)合實(shí)例代碼給大家介紹的非常詳細(xì),對大家的學(xué)習(xí)或工作具有一定的參考借鑒價(jià)值,需要的朋友可以參考下
    2023-06-06
  • C++實(shí)現(xiàn)Go的defer功能(示例代碼)

    C++實(shí)現(xiàn)Go的defer功能(示例代碼)

    defer和go一樣都是Go語言提供的關(guān)鍵字。defer用于資源的釋放,會在函數(shù)返回之前進(jìn)行調(diào)用。接下來通過本文給大家介紹C++實(shí)現(xiàn)Go的defer功能,感興趣的朋友跟隨小編一起看看吧
    2021-07-07
  • C/C++ Qt QChart繪圖組件的具體使用

    C/C++ Qt QChart繪圖組件的具體使用

    QtCharts 組件是QT中提供圖表繪制的模塊,用來繪制常規(guī)圖形,本文就詳細(xì)的介紹了QChart的使用,以及柱狀圖,折線圖等常用的圖形的實(shí)現(xiàn),感興趣的可以了解一下
    2021-11-11
  • C語言實(shí)現(xiàn)學(xué)生學(xué)籍管理系統(tǒng)

    C語言實(shí)現(xiàn)學(xué)生學(xué)籍管理系統(tǒng)

    這篇文章主要為大家詳細(xì)介紹了C語言實(shí)現(xiàn)學(xué)生學(xué)籍管理系統(tǒng),具有一定的參考價(jià)值,感興趣的小伙伴們可以參考一下
    2018-01-01
  • 實(shí)戰(zhàn)開發(fā)為單片機(jī)的按鍵加一個(gè)鎖防止多次觸發(fā)的細(xì)節(jié)

    實(shí)戰(zhàn)開發(fā)為單片機(jī)的按鍵加一個(gè)鎖防止多次觸發(fā)的細(xì)節(jié)

    今天小編就為大家分享一篇關(guān)于實(shí)戰(zhàn)開發(fā)為單片機(jī)的按鍵加一個(gè)鎖防止多次觸發(fā)的細(xì)節(jié),小編覺得內(nèi)容挺不錯(cuò)的,現(xiàn)在分享給大家,具有很好的參考價(jià)值,需要的朋友一起跟隨小編來看看吧
    2018-12-12
  • C語言代碼實(shí)現(xiàn)俄羅斯方塊

    C語言代碼實(shí)現(xiàn)俄羅斯方塊

    這篇文章主要為大家詳細(xì)介紹了C語言代碼實(shí)現(xiàn)俄羅斯方塊,文中示例代碼介紹的非常詳細(xì),具有一定的參考價(jià)值,感興趣的小伙伴們可以參考一下
    2021-01-01
  • 基于OpenCV?差分法實(shí)現(xiàn)綠葉識別

    基于OpenCV?差分法實(shí)現(xiàn)綠葉識別

    物體識別是圖像處理學(xué)在現(xiàn)實(shí)生活中較多的應(yīng)用之一,本文提供了一種相對簡單的思路來實(shí)現(xiàn)綠葉識別,適合初學(xué)圖像處理的新人研究參考。感興趣的同學(xué)可以關(guān)注一下
    2021-11-11
  • C語言入門篇--定義宏#define的概述

    C語言入門篇--定義宏#define的概述

    本篇文章是C語言系列基礎(chǔ)篇,適合c語言剛?cè)腴T的朋友,本文對關(guān)于c語言的定義宏#define作了簡要的概述,希望可以幫助大家快速入門c語言的世界,更好的理解c語言
    2021-08-08

最新評論