判断amcl位置与地图匹配程度

      navigation启动时，amcl为机器人新建并维护一个粒子滤波器，通过匹配分值判断当前机器人位置与地图的匹配程度。为amcl添加一个输出匹配分值的功能。为了滤除噪声，取1s内5个分值（0.2s间隔）作为该时间段的分值。新建线程，每秒统计一次分值量化判断机器人位置的丢失程度。

      取出当前时刻分值（pf.c）。

1.  
   #include <assert.h>
2.  
   #include <math.h>
3.  
   #include <stdlib.h>
4.  
   #include <time.h>
5.  
    
6.  
   #include "amcl/pf/pf.h"
7.  
   #include "amcl/pf/pf_pdf.h"
8.  
   #include "amcl/pf/pf_kdtree.h"
9.  
    
10.  
    // match_score declare by cabin avoiding error of multiple definition
11.  
    double match_score_;
12.  
     
13.  
    ...
14.  
     
15.  
    void pf_update_sensor(pf_t *pf, pf_sensor_model_fn_t sensor_fn, void *sensor_data){
16.  
    ...
17.  
    // Compute the sample weights
18.  
    total = (*sensor_fn) (sensor_data, set);
19.  
    //get score by cabin
20.  
    match_score_ = total;
21.  
    ...
22.  
    }
23.  
     
24.  
    ...
25.  
     
26.  
    // Delive the score by cabin
27.  
    double get_match_score(){
28.  
    return match_score_;
29.  
    }

 

      统计1s时间段内分值（amcl_node.cpp）。

1.  
   ...
2.  
   class AmclNode
3.  
   {
4.  
   ...
5.  
   std::thread *computeCurrentScoreThread;
6.  
   void computeScoreThread();
7.  
   }
8.  
    
9.  
   ...
10.  
     
11.  
    AmclNode::AmclNode() :
12.  
    sent_first_transform_(false),
13.  
    latest_tf_valid_(false),
14.  
    map_(NULL),
15.  
    pf_(NULL),
16.  
    resample_count_(0),
17.  
    odom_(NULL),
18.  
    laser_(NULL),
19.  
    private_nh_("~"),
20.  
    initial_pose_hyp_(NULL),
21.  
    first_map_received_(false),
22.  
    first_reconfigure_call_(true)
23.  
    {
24.  
    ...
25.  
    computeCurrentScoreThread = new std::thread(&AmclNode::computeScoreThread,this);
26.  
    }
27.  
     
28.  
    ...
29.  
     
30.  
    //=================compute the matching score by cabin============================
31.  
    void AmclNode::computeScoreThread(){
32.  
    double sum_score;
33.  
    double sub_score;
34.  
    int count;
35.  
    while(1){
36.  
    sub_score = get_match_score();
37.  
    sum_score += sub_score;
38.  
    count++;
39.  
    sleep(0.2);
40.  
    if(count>5){
41.  
    std::cout<<"========"<<"pf filter score:"<<sum_score<<"======"<<&std::endl;
42.  
    count =0;
43.  
    sum_score = 0.0;
44.  
    }
45.  
    }
46.  
    }

 

      则在navigation运行时，每秒都会输出机器人当前位置与地图的匹配度作为参考。
---------------------
作者：cabinx
来源：CSDN
原文：https://blog.csdn.net/xiekaikaibing/article/details/80568192
版权声明：本文为作者原创文章，转载请附上博文链接！