Ethzasl MSF原始碼閱讀(1):程式入口和主題訂閱
Ethz的Stephen Weiss的工作,是一個IMU鬆耦合的方法。
1.程式入口:ethzasl_msf\msf_updates\src\pose_msf\main.cpp
1 #include "pose_sensormanager.h" 2 3 int main(int argc, char** argv) 4 { 5 ros::init(argc, argv, "msf_pose_sensor"); 6 msf_pose_sensor::PoseSensorManager manager; 7 ros::spin(); 8 return 0;9 }
PoseSensorManager類,檢視建構函式。PoseSensorManager繼承自msf_core::MSF_SensorManagerROS,繼承自msf_core::MSF_SensorManager<EKFState_T>
1 PoseSensorManager(ros::NodeHandle pnh = ros::NodeHandle("~/pose_sensor")) 2 { 3 bool distortmeas = false; //< Distort the pose measurements. 4 imu_handler_.reset(newmsf_core::IMUHandler_ROS<msf_updates::EKFState>(*this, "msf_core", "imu_handler")); 5 pose_handler_.reset(new PoseSensorHandler_T(*this, "", "pose_sensor", distortmeas)); AddHandler(pose_handler_); 6 7 reconf_server_.reset(new ReconfigureServer(pnh)); 8 ReconfigureServer::CallbackType f = boost::bind(&PoseSensorManager::Config, this, _1, _2);//回撥 9 reconf_server_->setCallback(f); 10 11 init_scale_srv_ = pnh.advertiseService("initialize_msf_scale", 12 &PoseSensorManager::InitScale, this); 13 init_height_srv_ = pnh.advertiseService("initialize_msf_height", 14 &PoseSensorManager::InitHeight, this); 15 }
2. 注意其中的imu_handler_和pose_handler_,分別是IMUHandler_ROS物件和PoseSensorHandler模板類物件。IMUHandler_ROS繼承自IMUHandler。
typedef PoseSensorHandler<msf_updates::pose_measurement::PoseMeasurement<>,
PoseSensorManager> PoseSensorHandler_T;
(msf_updates::pose_measurement::PoseMeasurement<>是模型類PoseSensorHandler的類引數,這是一個觀測量引數。)
注意IMUHandler和PoseSensorHandler最終都繼承自msf_core::SensorHandler<typename msf_updates::EKFState>類。
這兩個類物件構造的時候都傳入了*this指標,即PoseSensorManager物件自身。這很重要,兩個類中都呼叫了PoseSensorManager的成員函式。
可以進入這兩個類進行檢視建構函式,這2個建構函式都進行了一些主題的訂閱。
IMUHandler_ROS構造,同時檢視IMUHandler_ROS::IMUCallback回撥函式。
1 IMUHandler_ROS(MSF_SensorManager<EKFState_T>& mng, 2 const std::string& topic_namespace, const std::string& parameternamespace) 3 : IMUHandler<EKFState_T>(mng, topic_namespace, parameternamespace) 4 { 5 ros::NodeHandle nh(topic_namespace); 6 subImu_ = nh.subscribe("imu_state_input", 100, &IMUHandler_ROS::IMUCallback, this); 7 subState_ = nh.subscribe("hl_state_input", 10, &IMUHandler_ROS::StateCallback, this); 8 }
PoseSensorHandler構造,同時檢視PoseSensorHandler::MeasurementCallback回撥函式。
1 template<typename MEASUREMENT_TYPE, typename MANAGER_TYPE> 2 PoseSensorHandler<MEASUREMENT_TYPE, MANAGER_TYPE>::PoseSensorHandler( 3 MANAGER_TYPE& meas, std::string topic_namespace, 4 std::string parameternamespace, bool distortmeas) 5 : SensorHandler<msf_updates::EKFState>(meas, topic_namespace, parameternamespace), 6 n_zp_(1e-6), 7 n_zq_(1e-6), 8 delay_(0), 9 timestamp_previous_pose_(0) 10 { 11 ros::NodeHandle pnh("~/" + parameternamespace); 12 13 MSF_INFO_STREAM( 14 "Loading parameters for pose sensor from namespace: " 15 << pnh.getNamespace()); 16 17 pnh.param("pose_absolute_measurements", provides_absolute_measurements_, 18 true); 19 pnh.param("pose_measurement_world_sensor", measurement_world_sensor_, true); 20 pnh.param("pose_use_fixed_covariance", use_fixed_covariance_, false); 21 pnh.param("pose_measurement_minimum_dt", pose_measurement_minimum_dt_, 0.05); 22 pnh.param("enable_mah_outlier_rejection", enable_mah_outlier_rejection_, false); 23 pnh.param("mah_threshold", mah_threshold_, msf_core::kDefaultMahThreshold_); 24 25 MSF_INFO_STREAM_COND(measurement_world_sensor_, "Pose sensor is interpreting " 26 "measurement as sensor w.r.t. world"); 27 MSF_INFO_STREAM_COND( 28 !measurement_world_sensor_, 29 "Pose sensor is interpreting measurement as world w.r.t. " 30 "sensor (e.g. ethzasl_ptam)"); 31 32 MSF_INFO_STREAM_COND(use_fixed_covariance_, "Pose sensor is using fixed " 33 "covariance"); 34 MSF_INFO_STREAM_COND(!use_fixed_covariance_, 35 "Pose sensor is using covariance " 36 "from sensor"); 37 38 MSF_INFO_STREAM_COND(provides_absolute_measurements_, 39 "Pose sensor is handling " 40 "measurements as absolute values"); 41 MSF_INFO_STREAM_COND(!provides_absolute_measurements_, "Pose sensor is " 42 "handling measurements as relative values"); 43 44 ros::NodeHandle nh("msf_updates/" + topic_namespace); 45 subPoseWithCovarianceStamped_ = 46 nh.subscribe < geometry_msgs::PoseWithCovarianceStamped 47 > ("pose_with_covariance_input", 20, &PoseSensorHandler::MeasurementCallback, this); 48 subTransformStamped_ = nh.subscribe < geometry_msgs::TransformStamped 49 > ("transform_input", 20, &PoseSensorHandler::MeasurementCallback, this); 50 subPoseStamped_ = nh.subscribe < geometry_msgs::PoseStamped 51 > ("pose_input", 20, &PoseSensorHandler::MeasurementCallback, this); 52 53 z_p_.setZero(); 54 z_q_.setIdentity(); 55 56 if (distortmeas) 57 { 58 Eigen::Vector3d meanpos; 59 double distortpos_mean; 60 pnh.param("distortpos_mean", distortpos_mean, 0.0); 61 meanpos.setConstant(distortpos_mean); 62 63 Eigen::Vector3d stddevpos; 64 double distortpos_stddev; 65 pnh.param("distortpos_stddev", distortpos_stddev, 0.0); 66 stddevpos.setConstant(distortpos_stddev); 67 68 Eigen::Vector3d meanatt; 69 double distortatt_mean; 70 pnh.param("distortatt_mean", distortatt_mean, 0.0); 71 meanatt.setConstant(distortatt_mean); 72 73 Eigen::Vector3d stddevatt; 74 double distortatt_stddev; 75 pnh.param("distortatt_stddev", distortatt_stddev, 0.0); 76 stddevatt.setConstant(distortatt_stddev); 77 78 double distortscale_mean; 79 pnh.param("distortscale_mean", distortscale_mean, 0.0); 80 double distortscale_stddev; 81 pnh.param("distortscale_stddev", distortscale_stddev, 0.0); 82 83 distorter_.reset( new msf_updates::PoseDistorter(meanpos, stddevpos, meanatt, stddevatt, distortscale_mean, distortscale_stddev)); 84 } 85 }
3.reconf_server_->setCallback(f)綁定了一個回撥,回撥函式PoseSensorManager::Config(xx).
1 virtual void Config(Config_T &config, uint32_t level) 2 { 3 config_ = config; 4 pose_handler_->SetNoises(config.pose_noise_meas_p, 5 config.pose_noise_meas_q); 6 pose_handler_->SetDelay(config.pose_delay); 7 if ((level & msf_updates::SinglePoseSensor_INIT_FILTER) 8 && config.core_init_filter == true) 9 { 10 Init(config.pose_initial_scale); 11 config.core_init_filter = false; 12 } 13 // Init call with "set height" checkbox. 14 if ((level & msf_updates::SinglePoseSensor_SET_HEIGHT) 15 && config.core_set_height == true) 16 { 17 Eigen::Matrix<double, 3, 1> p = pose_handler_->GetPositionMeasurement(); 18 if (p.norm() == 0) 19 { 20 MSF_WARN_STREAM( 21 "No measurements received yet to initialize position. Height init " 22 "not allowed."); 23 return; 24 } 25 double scale = p[2] / config.core_height; 26 Init(scale); 27 config.core_set_height = false; 28 } 29 }
參考文獻:
相關推薦
Ethzasl MSF原始碼閱讀(1):程式入口和主題訂閱
Ethz的Stephen Weiss的工作,是一個IMU鬆耦合的方法。 1.程式入口:ethzasl_msf\msf_updates\src\pose_msf\main.cpp 1 #include "pose_sensormanager.h" 2 3 int main(int argc,
Ethzasl MSF源碼閱讀(1):程序入口和主題訂閱
turn war lB void true matrix emp ati spin 1.程序入口:ethzasl_msf\msf_updates\src\pose_msf\main.cpp 1 #include "pose_sensormanager.h" 2
Cartographer原始碼閱讀(1):程式入口
1 [email protected]:~$ sudo apt-get install kdevelop 2 [sudo] password for yhexie: 3 Reading package lists... Done 4 Building dependency
Ethzasl MSF原始碼閱讀(2):百川匯海
這裡有個感覺,就是百川匯海。即IMU資料和相機的訊息資料都彙集到msf_core進行處理。接上一篇, 1. 檢視IMUHandler_ROS::IMUCallback和IMUHandler_ROS::StateCallback回撥函式。 MUHandler_ROS::IMUCallback,傳入的訊息s
Ethzasl MSF原始碼閱讀(3):MSF_Core和PoseMeasurement
1.MSF_Core的三個函式:ProcessIMU、ProcessExternallyPropagatedState和AddMeasurement MSF_Core維護了狀態佇列和觀測值佇列,這裡需要結合論文思考這個狀態佇列的作用。 ProcessIMU方法: 1 template<
Cartographer原始碼閱讀(3):程式邏輯結構
Cartographer早期的程式碼在進行3d製圖的時候使用了UKF方法,檢視現有的tag版本,可以轉到0.1.0和0.2.0檢視,包含kalman_filter資料夾。 資料夾中的pose_tracker類檔案在mapping_3d的檔案加下有kalman_local_trajectory_builder
Cartographer原始碼閱讀(7):軌跡推算和位姿推算的原理
其實也就是包括兩個方面的內容:類似於運動模型的位姿估計和掃描匹配,因為需要計算速度,所以時間就有必要了! 1. PoseExtrapolator解決了IMU資料、里程計和位姿資訊進行融合的問題。 該類定義了三個佇列。 1 std::deque<TimedPose> timed_pose_
轉:Django 原始碼閱讀(一):概覽從入口到請求到響應
轉載:Django 原始碼閱讀(一):概覽從入口到請求到響應————作者:hongweipeng 起步 在我研究完 django 的自動載入機制後,有了閱讀 django 原始碼的想法。那就看看吧,也不知道能堅持到什麼地方。我閱讀的版本也是我正在使用的 1.10.5 版本,算是比較新的了。
Spring4.3.12原始碼閱讀系列:1-環境搭建
學習任務 近期想增加部分原始碼閱讀經驗,提高自己在造輪子方面的實力,增長些在設計模式應用方面的編碼能力,以及懷著向大佬們膜拜的心情,開始有計劃地閱讀Spring原始碼 前期準備 以下幾項準備事項,算是基本的日常開發環境,就算沒有,也是動動手很快安
spark2.1原始碼分析1:Win10下IDEA原始碼閱讀環境的搭建
環境:win10、IDEA2016.3、maven3.3.9、git、scala 2.11.8、java1.8.0_101、sbt0.13.12 下載: #git bash中執行: git clone https://github.com/apache/
比特幣原始碼閱讀(1)--雜記與加密部分(爽之小刀)
比特幣原始碼閱讀(1)–雜記與加密部分(爽之小刀) 歡迎轉載和引用 若有問題請聯絡請聯絡 Email : [email protected] QQ:2279557541 最近從成都來到了杭州拼一槍 由於老婆為了圓自己的一個大公司夢來到了杭州
SpringMVC原始碼分析1:SpringMVC概述
轉載自:https://blog.csdn.net/a724888/article/details/76014532 Web MVC簡介 1.1、Web開發中的請求-響應模型: 在Web世界裡,具體步驟如下: 1、 Web瀏覽器(如IE)發起請求,如訪問http:/
Caffe原始碼理解1:Blob儲存結構與設計
Blob作用 A Blob is a wrapper over the actual data being processed and passed along by Caffe, and also under the hood provides synchronization capability b
OpenCV原始碼閱讀——1.2 Mat的記憶體管理
1.2 Mat的記憶體管理 影象資料量大,不妥善管理好記憶體會產生很大的問題。OpenCV1.X中多采用C的結構,需要使用者自己管理記憶體,在影象不再使用時呼叫CvRelease。OpenCV2.X中採用C++面向物件的方式,記憶體可以由自動申請和釋放。 1.2.1 影象
Tensorflow object detection API 原始碼閱讀筆記:RPN
Update: 建議先看從程式設計實現角度學習Faster R-CNN,比較直觀。這裡由於原始碼抽象程度較高,顯得比較混亂。 faster_rcnn_meta_arch.py中這兩個對應知乎文章中RPN包含的3*3和1*1卷積: rpn_box_pred
原始碼閱讀系列:為什麼要閱讀原始碼?
一.為什麼要閱讀程式碼 養成閱讀高品質程式碼的習慣,可以提高編寫程式碼的能力。 電腦科學是一門實踐性很強的學科,很多內容在書本上根本學不到。就拿專案的組織來說,沒有什麼書籍專門論述應該如何組織與管理專案的目錄結構,因為這本身就是一種見仁見智的活動,要
XBMC原始碼分析 1:整體結構以及編譯方法
XBMC(全稱是XBOX Media Center)是一個開源的媒體中心軟體。XBMC最初為Xbox而開發,可以執行在Linux、OSX、Windows、Android4.0系統。我自己下載了一個然後體驗了一下,感覺確實不錯,和Windows自帶的媒體中心差不多。 XB
Tensorflow object detection API 原始碼閱讀筆記:架構
在之前的博文中介紹過用tf提供的預訓練模型進行inference,非常簡單。這裡我們深入原始碼,瞭解檢測API的程式碼架構,每個部分的深入閱讀留待後續。 '''構建自己模型的介面是虛基類DetectionModel,具體有5個抽象函式需要實現。 ''' o
Tensorflow object detection API 原始碼閱讀筆記:RFCN
有了前面Faster R-CNN的基礎,RFCN就比較容易了。 """object_detection/meta_architectures/rfcn_meta_arch.py The R-FCN
Cartographer原始碼閱讀(8):imu_tracker
1 /* 2 * Copyright 2016 The Cartographer Authors 3 * 4 * Licensed under the Apache License, Version 2.0 (the "License"); 5 * you may not use