laser scan matcher 筆記

整體流程

1. LaserScanMatcher類中，先initParams();初始化引數，然後設定ros的釋出節點和接受節點

2. 然後看雷達資料函式LaserScanMatcher::scanCallback，其中呼叫了兩個函式
   laserScanToLDP(scan_msg, curr_ldp_scan);和processScan(curr_ldp_scan, scan_msg->header.stamp);
   ，我們一個一個看

   • laserScanToLDP函式，把ros的laser_scan轉換成本程式定義的雷達格式struct laser_data

     ，typedef struct laser_data* LDP; 繞了個大彎，這個LDP 中包含了里程計資訊 odometry和真是位置true_pose estimate

   • processScan函式，把轉換過的雷達格式傳入

     • 首先預測根據前後兩幀的變化時間預測getPrediction位姿變化
     • 然後 createTfFromXYTheta得到tf轉換，這裡發現個好東西tf::Transform可以直接運算pr_ch_l = laser_to_base_ * f2b_.inverse() * pr_ch * f2b_ * base_to_laser_ ;
       ,他們分別是上一個tf， ，最後得到當前預測的位置在全域性座標系下的座標
     • 這裡才到了最關鍵最重要的地方 ICP,呼叫CSM（ C(anonical) Scan Matcher (CSM) ）的點到線的ICP演算法sm_icp(&input_, &output_),看了一晚上了，好像看錯了庫
     • 後面就是對sm_icp求得的結果進行驗證，並轉換成tf，最後整理髮布就完鳥。

3. 關鍵的關鍵sm_icp

sm_icp一個sm_params結構體，並返回一個sm_result,那麼，如果我只想用這個庫來完成我的匹配工作，我就只需要知道這個輸入的結構，然後呼叫就完事了。
sm_params

接下來就實驗一下吧.
星期三, 30. 五月 2018 07:01下午今天試了一下這個庫，執行不出來，現在在決定要不要放棄自己寫，還是說去改原來的庫檔案。
先自己測試下吧，最後實在是不行的話再改原檔案吧。
先分析一下當前存在的問題，當前的問題聚焦再sm_icp上面：
- 一方面可能是輸入引數的問題，params初始化是不是正確,先檢驗這個吧
- 另一方面就是，輸入的sm_result1的結果的問題
星期三, 30. 五月 2018 08:42下午
c++編碼能力太差了，哎，寫個程式，費死個老勁啊

tf::Transform 的騷操作：
  input_.first_guess[0] = pr_ch_l.getOrigin().getX(); // 得到xy
  input_.first_guess[1] = pr_ch_l.getOrigin().getY();
  input_.first_guess[2] = tf::getYaw(pr_ch_l.getRotation()); //得到水平旋轉角，帥