判斷amcl位置與地圖匹配程度

      navigation啓動時，amcl爲機器人新建並維護一個粒子濾波器，通過匹配分值判斷當前機器人位置與地圖的匹配程度。爲amcl添加一個輸出匹配分值的功能。爲了濾除噪聲，取1s內5個分值（0.2s間隔）作爲該時間段的分值。新建線程，每秒統計一次分值量化判斷機器人位置的丟失程度。

      取出當前時刻分值（pf.c）。

#include <assert.h>
#include <math.h>
#include <stdlib.h>
#include <time.h>

#include "amcl/pf/pf.h"
#include "amcl/pf/pf_pdf.h"
#include "amcl/pf/pf_kdtree.h"

// match_score declare by cabin avoiding error of multiple definition
double match_score_;

...

void pf_update_sensor(pf_t *pf, pf_sensor_model_fn_t sensor_fn, void *sensor_data){
...
  // Compute the sample weights
  total = (*sensor_fn) (sensor_data, set);
 //get score by cabin
  match_score_ = total; 
... 
}

...

// Delive the score by cabin
double get_match_score(){
	return match_score_;
}

      統計1s時間段內分值（amcl_node.cpp）。

...
class AmclNode
{
...
    std::thread *computeCurrentScoreThread;
    void computeScoreThread();
}

...

AmclNode::AmclNode() :
        sent_first_transform_(false),
        latest_tf_valid_(false),
        map_(NULL),
        pf_(NULL),
        resample_count_(0),
        odom_(NULL),
        laser_(NULL),
	private_nh_("~"),
        initial_pose_hyp_(NULL),
        first_map_received_(false),
        first_reconfigure_call_(true)
{
...
   computeCurrentScoreThread = new std::thread(&AmclNode::computeScoreThread,this);
}

...

//=================compute the matching score by cabin============================
void AmclNode::computeScoreThread(){
	double sum_score;
	double sub_score;
	int count;
	while(1){
		sub_score = get_match_score();
		sum_score += sub_score;
		count++;
		sleep(0.2);
		if(count>5){
			std::cout<<"========"<<"pf filter score:"<<sum_score<<"======"<<&std::endl;
			count =0;
			sum_score = 0.0;
			}
		}
}

      則在navigation運行時，每秒都會輸出機器人當前位置與地圖的匹配度作爲參考。