2D激光SLAM：：ROS-AMCL包源碼閱讀（三）從main()開始

---

一、初始化主體部分

---

int main(): 最主要的作用是創建了 AmclNode對象

int main(int argc, char** argv) {ros::init(argc, argv, "amcl");ros::NodeHandle nh;// Override default sigint handlersignal(SIGINT, sigintHandler);// Make our node available to sigintHandleramcl_node_ptr.reset(new AmclNode());if (argc == 1){// run using ROS inputros::spin();}else if ((argc == 3) && (std::string(argv[1]) == "--run-from-bag")){amcl_node_ptr->runFromBag(argv[2]);}// Without this, our boost locks are not shut down nicelyamcl_node_ptr.reset();// To quote Morgan, Hooray!return(0); }

---

接下來看類 AmclNode的構造函數

AmclNode::AmclNode()主要內容為：

1、從參數服務器上獲取大量參數

2、updatePoseFromServer() //從參數服務器獲取機器人的初始位置

3、注冊publisher

（1）“amcl-pose"話題

（2）"particlecloud"話題

3、創建服務

（1）“global_localization"服務，注冊回調AmclNode::globalLocalizationCallback():沒有給定初始位姿的情況下在全局范圍內初始化粒子位姿，該Callback調用pf_init_model，然后調用AmclNode::uniformPoseGenerator在地圖的free點隨機生成pf->max_samples個粒子

（2）"request_nomotion_update"服務，注冊回調&AmclNode::nomotionUpdateCallback():運動模型沒有更新的情況下也更新粒子群

（3）"set_map"服務，注冊回調&AmclNode::setMapCallback():

a、handleMapMessage()：進行地圖轉換 ，記錄free space ，以及初始化pf_t 結構體,實例化運動模型(odom)和觀測模型(laser) 【handleMapMessage() 后面會有詳細描述】

b、handleInitialPoseMessage(req.initial_pose);?根據接收的初始位姿消息,在該位姿附近高斯采樣重新生成粒子集

?

4 、話題訂閱

（1）”scan" : 消息類型sensor_msgs::LaserScan， 注冊回調AmclNode::laserReceived() :這個回調是amcl的整個主題流程 【下一篇會有詳細描述】

（2）"initialpose"，注冊回調AmclNode::initialPoseReceived() : 調用handleInitialPoseMessage();?根據接收的初始位姿消息,在該位姿附近高斯采樣重新生成粒子集

（3）（這個話題可選）"map"，注冊回調AmclNode::mapReceived() : 調用handleMapMessage() 進行地圖轉換 ，記錄free space ，以及初始化pf_t 結構體,實例化運動模型(odom)和觀測模型(laser) 【handleMapMessage() 這個后面會有詳細描述】

?

5、若沒有訂閱話題?"map"，則調用requestMap()

（1）調用ros::service::call，請求"static_map"服務，請求獲取地圖

（2）獲取后，調用handleMapMessage( resp.map );【handleMapMessage() 后面會有詳細描述】

?

6、創建定時器

一個15秒的定時器：AmclNode::checkLaserReceived，檢查 15上一次收到激光雷達數據至今是否超過15秒，如超過則報錯?

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) {boost::recursive_mutex::scoped_lock l(configuration_mutex_);// Grab params off the param server//從參數服務器上獲取大量參數private_nh_.param("use_map_topic", use_map_topic_, false);private_nh_.param("first_map_only", first_map_only_, false);double tmp;private_nh_.param("gui_publish_rate", tmp, -1.0);gui_publish_period = ros::Duration(1.0/tmp);private_nh_.param("save_pose_rate", tmp, 0.5);save_pose_period = ros::Duration(1.0/tmp);private_nh_.param("laser_min_range", laser_min_range_, -1.0);private_nh_.param("laser_max_range", laser_max_range_, -1.0);private_nh_.param("laser_max_beams", max_beams_, 30);private_nh_.param("min_particles", min_particles_, 100);private_nh_.param("max_particles", max_particles_, 5000);private_nh_.param("kld_err", pf_err_, 0.01);private_nh_.param("kld_z", pf_z_, 0.99);private_nh_.param("odom_alpha1", alpha1_, 0.2);private_nh_.param("odom_alpha2", alpha2_, 0.2);private_nh_.param("odom_alpha3", alpha3_, 0.2);private_nh_.param("odom_alpha4", alpha4_, 0.2);private_nh_.param("odom_alpha5", alpha5_, 0.2);private_nh_.param("do_beamskip", do_beamskip_, false);private_nh_.param("beam_skip_distance", beam_skip_distance_, 0.5);private_nh_.param("beam_skip_threshold", beam_skip_threshold_, 0.3);if (private_nh_.hasParam("beam_skip_error_threshold_")){private_nh_.param("beam_skip_error_threshold_", beam_skip_error_threshold_);}else{private_nh_.param("beam_skip_error_threshold", beam_skip_error_threshold_, 0.9);}private_nh_.param("laser_z_hit", z_hit_, 0.95);private_nh_.param("laser_z_short", z_short_, 0.1);private_nh_.param("laser_z_max", z_max_, 0.05);private_nh_.param("laser_z_rand", z_rand_, 0.05);private_nh_.param("laser_sigma_hit", sigma_hit_, 0.2);private_nh_.param("laser_lambda_short", lambda_short_, 0.1);private_nh_.param("laser_likelihood_max_dist", laser_likelihood_max_dist_, 2.0);std::string tmp_model_type;private_nh_.param("laser_model_type", tmp_model_type, std::string("likelihood_field"));if(tmp_model_type == "beam")laser_model_type_ = LASER_MODEL_BEAM;else if(tmp_model_type == "likelihood_field")laser_model_type_ = LASER_MODEL_LIKELIHOOD_FIELD;else if(tmp_model_type == "likelihood_field_prob"){laser_model_type_ = LASER_MODEL_LIKELIHOOD_FIELD_PROB;}else{ROS_WARN("Unknown laser model type \"%s\"; defaulting to likelihood_field model",tmp_model_type.c_str());laser_model_type_ = LASER_MODEL_LIKELIHOOD_FIELD;}private_nh_.param("odom_model_type", tmp_model_type, std::string("diff"));if(tmp_model_type == "diff")odom_model_type_ = ODOM_MODEL_DIFF;else if(tmp_model_type == "omni")odom_model_type_ = ODOM_MODEL_OMNI;else if(tmp_model_type == "diff-corrected")odom_model_type_ = ODOM_MODEL_DIFF_CORRECTED;else if(tmp_model_type == "omni-corrected")odom_model_type_ = ODOM_MODEL_OMNI_CORRECTED;else{ROS_WARN("Unknown odom model type \"%s\"; defaulting to diff model",tmp_model_type.c_str());odom_model_type_ = ODOM_MODEL_DIFF;}private_nh_.param("update_min_d", d_thresh_, 0.2);private_nh_.param("update_min_a", a_thresh_, M_PI/6.0);private_nh_.param("odom_frame_id", odom_frame_id_, std::string("odom"));private_nh_.param("base_frame_id", base_frame_id_, std::string("base_link"));private_nh_.param("global_frame_id", global_frame_id_, std::string("map"));private_nh_.param("resample_interval", resample_interval_, 2);double tmp_tol;private_nh_.param("transform_tolerance", tmp_tol, 0.1);private_nh_.param("recovery_alpha_slow", alpha_slow_, 0.001);private_nh_.param("recovery_alpha_fast", alpha_fast_, 0.1);private_nh_.param("tf_broadcast", tf_broadcast_, true);// For diagnosticsprivate_nh_.param("std_warn_level_x", std_warn_level_x_, 0.2);private_nh_.param("std_warn_level_y", std_warn_level_y_, 0.2);private_nh_.param("std_warn_level_yaw", std_warn_level_yaw_, 0.1);transform_tolerance_.fromSec(tmp_tol);{double bag_scan_period;private_nh_.param("bag_scan_period", bag_scan_period, -1.0);bag_scan_period_.fromSec(bag_scan_period);}odom_frame_id_ = stripSlash(odom_frame_id_);base_frame_id_ = stripSlash(base_frame_id_);global_frame_id_ = stripSlash(global_frame_id_);//get initial pose and init particles from Server//從參數服務器獲取機器人的初始位置updatePoseFromServer();cloud_pub_interval.fromSec(1.0);tfb_.reset(new tf2_ros::TransformBroadcaster());tf_.reset(new tf2_ros::Buffer());tfl_.reset(new tf2_ros::TransformListener(*tf_));//注冊publisher//“amcl-pose"話題pose_pub_ = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("amcl_pose", 2, true);//"particlecloud"話題particlecloud_pub_ = nh_.advertise<geometry_msgs::PoseArray>("particlecloud", 2, true);//創建服務//“global_localization"服務，注冊回調AmclNode::globalLocalizationCallback()://沒有給定初始位姿的情況下在全局范圍內初始化粒子位姿，//該Callback調用pf_init_model，//然后調用AmclNode::uniformPoseGenerator在地圖的free點隨機生成pf->max_samples個粒子global_loc_srv_ = nh_.advertiseService("global_localization", &AmclNode::globalLocalizationCallback,this);//"request_nomotion_update"服務，//注冊回調&AmclNode::nomotionUpdateCallback():運動模型沒有更新的情況下也更新粒子群nomotion_update_srv_= nh_.advertiseService("request_nomotion_update", &AmclNode::nomotionUpdateCallback, this);//"set_map"服務，注冊回調&AmclNode::setMapCallback():set_map_srv_= nh_.advertiseService("set_map", &AmclNode::setMapCallback, this);//話題訂閱laser_scan_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(nh_, scan_topic_, 100);laser_scan_filter_ = new tf2_ros::MessageFilter<sensor_msgs::LaserScan>(*laser_scan_sub_,*tf_,odom_frame_id_,100,nh_);laser_scan_filter_->registerCallback(boost::bind(&AmclNode::laserReceived,this, _1));initial_pose_sub_ = nh_.subscribe("initialpose", 2, &AmclNode::initialPoseReceived, this);//若沒有訂閱話題 "map"，則調用requestMap()if(use_map_topic_) {map_sub_ = nh_.subscribe("map", 1, &AmclNode::mapReceived, this);ROS_INFO("Subscribed to map topic.");} else {requestMap();}m_force_update = false;dsrv_ = new dynamic_reconfigure::Server<amcl::AMCLConfig>(ros::NodeHandle("~"));dynamic_reconfigure::Server<amcl::AMCLConfig>::CallbackType cb = boost::bind(&AmclNode::reconfigureCB, this, _1, _2);dsrv_->setCallback(cb);// 15s timer to warn on lack of receipt of laser scans, #5209//創建定時器laser_check_interval_ = ros::Duration(15.0);check_laser_timer_ = nh_.createTimer(laser_check_interval_, boost::bind(&AmclNode::checkLaserReceived, this, _1));diagnosic_updater_.setHardwareID("None");diagnosic_updater_.add("Standard deviation", this, &AmclNode::standardDeviationDiagnostics); }

---

接下來看requestMap()，里面主要調用了handleMapMessage()

AmclNode::requestMap()

1、一直請求服務"static_map"直到成功

2、獲取到地圖數據后，調用handleMapMessage( resp.map );? //處理接收到的地圖數據，初始化粒子濾波器等操作 【后面會詳細描述】

void AmclNode::requestMap() {boost::recursive_mutex::scoped_lock ml(configuration_mutex_);// get map via RPCnav_msgs::GetMap::Request req;nav_msgs::GetMap::Response resp;ROS_INFO("Requesting the map...");while(!ros::service::call("static_map", req, resp)){ROS_WARN("Request for map failed; trying again...");ros::Duration d(0.5);d.sleep();}handleMapMessage( resp.map ); }

---

接下來是主要的初始化部分，handleMapMessage()：

handleMapMessage() 主要內容為：

1、freeMapDependentMemory(); // 清空map_ ,pf_ , laser_ 這些對象

2、map_=convertMap(msg); // 重新初始化map_對象，將map_msg消息類型轉換為map_t結構體，具體操作為對map_msg.data[]數組的內容變成地圖柵格：0->-1(不是障礙);100->+1（障礙）;else->0（不明）【后面給出相關內容】

3、遍歷地圖所有柵格，將狀態為空閑的柵格的行列號記錄到free_space_indices??

static std::vector<std::pair<int,int> > free_space_indices;

4、pf_ = pf_alloc(最小粒子數，最大粒子數，alpha_slow_，alpha_fast_，粒子初始位姿隨機生成器(這是一個函數，在這里作為變量了），map_)? ?//創建粒子濾波器 【后面給出相關內容】

5、updatePoseFromServer(); //載入預設的濾波器均值和方差

6、pf_init(pf_, pf_init_pose_mean, pf_init_pose_cov);? //根據上一步載入的均值和方差、以及粒子初始位姿隨機生成器，對粒子濾波器進行初始化，步驟簡述為：??【后面給出相關內容】

（1）選擇要使用的粒子集合（因為在創建濾波器時，創建了兩份粒子集合，一份使用，另一份用來重采樣緩沖）

（2）pf_kdtree_clear(set->kdtree); //對傳入參數所指向的kdtree進行清空

（3）設置粒子數

（4）pdf = pf_pdf_gaussian_alloc(mean, cov); //使用傳入的均值和方差來初始化高斯分布

（5）對每個粒子的位姿使用高斯分布進行初始化

（6）釋放高斯分布pdf

（7）pf_cluster_stats(pf, set); //對粒子濾波器的粒子集進行聚類

（8）設置聚類收斂為0

7、odom_ = new AMCLOdom(); //創建AMCLOdom對象

8、odom_->SetModel( odom_model_type_, alpha1_, alpha2_, alpha3_, alpha4_, alpha5_ ); //設置里程計模型，傳入參數

9、laser_ = new AMCLLaser(max_beams_, map_); //創建AMCLLaser對象

10、設置激光雷達傳感器模型，默認為LASER_MODEL_LIKELIHOOD_FIELD

laser_->SetModelLikelihoodField(z_hit_, z_rand_, sigma_hit_,laser_likelihood_max_dist_); //根據選擇的激光傳感器模型傳入參數，并設置地圖上障礙物膨脹最大距離

//這里涉及到激光雷達傳感器的概率模型【下一篇會給出詳細內容】

11、?applyInitialPose();? //當map_變量初始化完成，以及接收到了“initialpose”話題消息后，才會執行這個，里面是pf_init()，意思是，如果又收到新的初始位姿信息，則重新初始化一次粒子濾波器的粒子位姿

void AmclNode::handleMapMessage(const nav_msgs::OccupancyGrid& msg) {boost::recursive_mutex::scoped_lock cfl(configuration_mutex_);ROS_INFO("Received a %d X %d map @ %.3f m/pix\n",msg.info.width,msg.info.height,msg.info.resolution);if(msg.header.frame_id != global_frame_id_)ROS_WARN("Frame_id of map received:'%s' doesn't match global_frame_id:'%s'. This could cause issues with reading published topics",msg.header.frame_id.c_str(),global_frame_id_.c_str());freeMapDependentMemory();// Clear queued laser objects because they hold pointers to the existing// map, #5202.lasers_.clear();lasers_update_.clear();frame_to_laser_.clear();map_ = convertMap(msg);#if NEW_UNIFORM_SAMPLING// Index of free spacefree_space_indices.resize(0);//遍歷地圖所有柵格，將狀態為空閑的柵格的行列號記錄到free_space_indicesfor(int i = 0; i < map_->size_x; i++)for(int j = 0; j < map_->size_y; j++)if(map_->cells[MAP_INDEX(map_,i,j)].occ_state == -1)free_space_indices.push_back(std::make_pair(i,j)); #endif// Create the particle filterpf_ = pf_alloc(min_particles_, max_particles_,alpha_slow_, alpha_fast_,(pf_init_model_fn_t)AmclNode::uniformPoseGenerator,(void *)map_);pf_->pop_err = pf_err_;pf_->pop_z = pf_z_;// Initialize the filterupdatePoseFromServer();pf_vector_t pf_init_pose_mean = pf_vector_zero();pf_init_pose_mean.v[0] = init_pose_[0];pf_init_pose_mean.v[1] = init_pose_[1];pf_init_pose_mean.v[2] = init_pose_[2];pf_matrix_t pf_init_pose_cov = pf_matrix_zero();pf_init_pose_cov.m[0][0] = init_cov_[0];pf_init_pose_cov.m[1][1] = init_cov_[1];pf_init_pose_cov.m[2][2] = init_cov_[2];pf_init(pf_, pf_init_pose_mean, pf_init_pose_cov);pf_init_ = false;// Instantiate the sensor objects// Odometrydelete odom_;odom_ = new AMCLOdom();ROS_ASSERT(odom_);odom_->SetModel( odom_model_type_, alpha1_, alpha2_, alpha3_, alpha4_, alpha5_ );// Laserdelete laser_;laser_ = new AMCLLaser(max_beams_, map_);ROS_ASSERT(laser_);if(laser_model_type_ == LASER_MODEL_BEAM)laser_->SetModelBeam(z_hit_, z_short_, z_max_, z_rand_,sigma_hit_, lambda_short_, 0.0);else if(laser_model_type_ == LASER_MODEL_LIKELIHOOD_FIELD_PROB){ROS_INFO("Initializing likelihood field model; this can take some time on large maps...");laser_->SetModelLikelihoodFieldProb(z_hit_, z_rand_, sigma_hit_,laser_likelihood_max_dist_, do_beamskip_, beam_skip_distance_, beam_skip_threshold_, beam_skip_error_threshold_);ROS_INFO("Done initializing likelihood field model.");}else{ROS_INFO("Initializing likelihood field model; this can take some time on large maps...");laser_->SetModelLikelihoodField(z_hit_, z_rand_, sigma_hit_,laser_likelihood_max_dist_);ROS_INFO("Done initializing likelihood field model.");}// In case the initial pose message arrived before the first map,// try to apply the initial pose now that the map has arrived.applyInitialPose();}

---

二、一些函數的具體實現

前文描述了AMCL初始化的大體框架，接下來是一些關于初始化的具體函數實現部分

---

1、convertMap(msg):??

convertMap(msg):??

實現nav_msgs::OccupancyGrid& map_msg 類型數據轉換到代碼定義的 map_t結構體數據類型

主要設置了：

1、地圖的尺寸

2、地圖的尺度（分辨率）

3、地圖原點，注意map_msg.info.origin這個點是地圖的中心點，設置地圖原點時還要加偏移

4、設置地圖的每個柵格的占據狀況

/*** Convert an OccupancyGrid map message into the internal* representation. This allocates a map_t and returns it.*/ map_t* AmclNode::convertMap( const nav_msgs::OccupancyGrid& map_msg ) {map_t* map = map_alloc();ROS_ASSERT(map);map->size_x = map_msg.info.width;map->size_y = map_msg.info.height;map->scale = map_msg.info.resolution;//(map_msg.info.origin.position.x,map_msg.info.origin.position.y) 是柵格地圖(0,0)的世界坐標系坐標map->origin_x = map_msg.info.origin.position.x + (map->size_x / 2) * map->scale;map->origin_y = map_msg.info.origin.position.y + (map->size_y / 2) * map->scale;// Convert to player formatmap->cells = (map_cell_t*)malloc(sizeof(map_cell_t)*map->size_x*map->size_y);ROS_ASSERT(map->cells);for(int i=0;i<map->size_x * map->size_y;i++){//根據map_msg消息來設置地圖對應柵格的狀態occ_state : -1 = free, 0 = unknown, +1 = occif(map_msg.data[i] == 0)map->cells[i].occ_state = -1;else if(map_msg.data[i] == 100)map->cells[i].occ_state = +1;elsemap->cells[i].occ_state = 0;}return map; }

---

2、pf_alloc(int min_samples, int max_samples,double alpha_slow, double alpha_fast,pf_init_model_fn_t random_pose_fn, void *random_pose_data)

pf_ = pf_alloc(最小粒子數，最大粒子數，alpha_slow_，alpha_fast_，粒子初始位姿隨機生成器(這是一個函數，在這里作為變量了），地圖對象)?

主要作用是創建粒子濾波器，分配內存等

具體實現順序為：

（1）設置濾波器的粒子位姿隨機生成函數

（2）設置濾波器的粒子位姿數據（實際上傳入的是柵格地圖數據）

（3）設置粒子數上下限

（4）設置濾波器參數

（5）對濾波器的兩份粒子集合進行零初始化（分配內存、位姿初始化為0）

（6）對每份粒子集合創建kdtree

（7）初始化聚類數目、最大類別數

（8）初始化粒子集合的均值和方差

（9）設置收斂為0

// Create a new filter pf_t *pf_alloc(int min_samples, int max_samples,double alpha_slow, double alpha_fast,pf_init_model_fn_t random_pose_fn, void *random_pose_data) {int i, j;pf_t *pf;pf_sample_set_t *set;pf_sample_t *sample;srand48(time(NULL));pf = calloc(1, sizeof(pf_t));pf->random_pose_fn = random_pose_fn;pf->random_pose_data = random_pose_data;pf->min_samples = min_samples;pf->max_samples = max_samples;// Control parameters for the population size calculation. [err] is// the max error between the true distribution and the estimated// distribution. [z] is the upper standard normal quantile for (1 -// p), where p is the probability that the error on the estimated// distrubition will be less than [err].pf->pop_err = 0.01;pf->pop_z = 3;pf->dist_threshold = 0.5; pf->current_set = 0;//對濾波器的兩份粒子集合進行初始化for (j = 0; j < 2; j++){set = pf->sets + j;set->sample_count = max_samples;set->samples = calloc(max_samples, sizeof(pf_sample_t));//對粒子集合里面的每個粒子進行初始化for (i = 0; i < set->sample_count; i++){sample = set->samples + i;sample->pose.v[0] = 0.0;sample->pose.v[1] = 0.0;sample->pose.v[2] = 0.0;sample->weight = 1.0 / max_samples;}// HACK: is 3 times max_samples enough?set->kdtree = pf_kdtree_alloc(3 * max_samples);set->cluster_count = 0;set->cluster_max_count = max_samples;set->clusters = calloc(set->cluster_max_count, sizeof(pf_cluster_t));//初始化粒子集合的均值和方差set->mean = pf_vector_zero();set->cov = pf_matrix_zero();}pf->w_slow = 0.0;pf->w_fast = 0.0;pf->alpha_slow = alpha_slow;pf->alpha_fast = alpha_fast;//set converged to 0pf_init_converged(pf);return pf; }

?

---

3、void pf_init(pf_t *pf, pf_vector_t mean, pf_matrix_t cov)

pf_init(濾波器對象, 均值, 方差)

主要功能是利用高斯分布來初始化粒子濾波器，主要是用來初始化粒子的初始位姿

具體實現順序為：

（1）選擇要使用的粒子集合（因為在創建濾波器時，創建了兩份粒子集合，一份使用，另一份用來重采樣緩沖）

（2）pf_kdtree_clear(set->kdtree); //對傳入參數所指向的kdtree進行清空

（3）設置粒子數

（4）pdf = pf_pdf_gaussian_alloc(mean, cov); //使用傳入的均值和方差來初始化高斯分布

（5）對每個粒子的位姿使用高斯分布進行初始化，然后將每個粒子插入到kdtree中

（6）釋放高斯分布pdf

（7）pf_cluster_stats(pf, set); //對粒子濾波器的粒子集進行聚類 【后面稍微給出相關，本人也不太了解這個聚類】

（8）設置聚類收斂為0

// Initialize the filter using a guassian void pf_init(pf_t *pf, pf_vector_t mean, pf_matrix_t cov) {int i;pf_sample_set_t *set;pf_sample_t *sample;pf_pdf_gaussian_t *pdf;//選擇要使用的粒子集合set = pf->sets + pf->current_set;// Create the kd tree for adaptive samplingpf_kdtreeshe_clear(set->kdtree);//configure particle counts//設置粒子數set->sample_count = pf->max_samples;// Create a gaussian pdf//使用mean和cov來初始化高斯分布pdf = pf_pdf_gaussian_alloc(mean, cov);// Compute the new sample poses//對每個粒子的位姿使用高斯分布進行初始化for (i = 0; i < set->sample_count; i++){sample = set->samples + i;sample->weight = 1.0 / pf->max_samples;sample->pose = pf_pdf_gaussian_sample(pdf);// Add sample to histogram// Insert a pose into the tree.pf_kdtree_insert(set->kdtree, sample->pose, sample->weight);}pf->w_slow = pf->w_fast = 0.0;//release pdfpf_pdf_gaussian_free(pdf);// Re-compute cluster statisticspf_cluster_stats(pf, set); //set converged to 0pf_init_converged(pf);return; }

---

4、void pf_cluster_stats(pf_t *pf, pf_sample_set_t *set)

pf_cluster_stats(粒子濾波器對象, 粒子集合)

主要功能是對粒子集合進行聚類，將每個粒子歸到所屬類別，并進行統計

實現順序為：

（1）pf_kdtree_cluster(set->kdtree); //對粒子集合進行聚類

（2）對每個類別的統計值進行初始化（初始成0）

（3）對粒子集合的均值和方差進行0值初始化

（4）對每個粒子進行操作：

a、獲取該粒子所屬的集群編號cidx

b、對該集群cidx的粒子數+1，權重+=該粒子權重

c、粒子所屬集群pose均值+=該粒子權重×該粒子pose

d、粒子集的pose均值+=該粒子權重×該粒子pose

（5）對每個集群進行操作：

a、對該集群的pose均值進行歸一化，即pose的每個分量/該集群權重

b、計算方差

（6）對粒子集的全部粒子均值進行歸一化，即粒子集pose均值/粒子集權重

// Re-compute the cluster statistics for a sample set void pf_cluster_stats(pf_t *pf, pf_sample_set_t *set) {int i, j, k, cidx;pf_sample_t *sample;pf_cluster_t *cluster;// Workspacedouble m[4], c[2][2];size_t count;double weight;// Cluster the samplespf_kdtree_cluster(set->kdtree);// Initialize cluster statsset->cluster_count = 0;for (i = 0; i < set->cluster_max_count; i++){cluster = set->clusters + i;cluster->count = 0;cluster->weight = 0;cluster->mean = pf_vector_zero();cluster->cov = pf_matrix_zero();for (j = 0; j < 4; j++)cluster->m[j] = 0.0;for (j = 0; j < 2; j++)for (k = 0; k < 2; k++)cluster->c[j][k] = 0.0;}// Initialize overall filter statscount = 0;weight = 0.0;set->mean = pf_vector_zero();set->cov = pf_matrix_zero();for (j = 0; j < 4; j++)m[j] = 0.0;for (j = 0; j < 2; j++)for (k = 0; k < 2; k++)c[j][k] = 0.0;// Compute cluster statsfor (i = 0; i < set->sample_count; i++){sample = set->samples + i;//printf("%d %f %f %f\n", i, sample->pose.v[0], sample->pose.v[1], sample->pose.v[2]);// Get the cluster label for this sample//獲取該粒子所屬的集群編號cidx = pf_kdtree_get_cluster(set->kdtree, sample->pose);assert(cidx >= 0);if (cidx >= set->cluster_max_count)continue;//如果這個粒子的集群編號大于粒子集的集群數，表示這是一個新的集群if (cidx + 1 > set->cluster_count)set->cluster_count = cidx + 1;//集群選定cluster = set->clusters + cidx;//該集群粒子數+1cluster->count += 1;cluster->weight += sample->weight;count += 1;weight += sample->weight;// Compute meancluster->m[0] += sample->weight * sample->pose.v[0];cluster->m[1] += sample->weight * sample->pose.v[1];cluster->m[2] += sample->weight * cos(sample->pose.v[2]);cluster->m[3] += sample->weight * sin(sample->pose.v[2]);m[0] += sample->weight * sample->pose.v[0];m[1] += sample->weight * sample->pose.v[1];m[2] += sample->weight * cos(sample->pose.v[2]);m[3] += sample->weight * sin(sample->pose.v[2]);// Compute covariance in linear componentsfor (j = 0; j < 2; j++)for (k = 0; k < 2; k++){cluster->c[j][k] += sample->weight * sample->pose.v[j] * sample->pose.v[k];c[j][k] += sample->weight * sample->pose.v[j] * sample->pose.v[k];}}// Normalize//對每個集群的均值進行歸一化for (i = 0; i < set->cluster_count; i++){cluster = set->clusters + i;cluster->mean.v[0] = cluster->m[0] / cluster->weight;cluster->mean.v[1] = cluster->m[1] / cluster->weight;cluster->mean.v[2] = atan2(cluster->m[3], cluster->m[2]);cluster->cov = pf_matrix_zero();// Covariance in linear componentsfor (j = 0; j < 2; j++)for (k = 0; k < 2; k++)cluster->cov.m[j][k] = cluster->c[j][k] / cluster->weight -cluster->mean.v[j] * cluster->mean.v[k];// Covariance in angular components; I think this is the correct// formula for circular statistics.cluster->cov.m[2][2] = -2 * log(sqrt(cluster->m[2] * cluster->m[2] +cluster->m[3] * cluster->m[3]));//printf("cluster %d %d %f (%f %f %f)\n", i, cluster->count, cluster->weight,//cluster->mean.v[0], cluster->mean.v[1], cluster->mean.v[2]);//pf_matrix_fprintf(cluster->cov, stdout, "%e");}// Compute overall filter stats//對粒子集的全部粒子的均值進行歸一化(不分集群)set->mean.v[0] = m[0] / weight;set->mean.v[1] = m[1] / weight;set->mean.v[2] = atan2(m[3], m[2]);// Covariance in linear componentsfor (j = 0; j < 2; j++)for (k = 0; k < 2; k++)set->cov.m[j][k] = c[j][k] / weight - set->mean.v[j] * set->mean.v[k];// Covariance in angular components; I think this is the correct// formula for circular statistics.set->cov.m[2][2] = -2 * log(sqrt(m[2] * m[2] + m[3] * m[3]));return; }

?

?

總結

以上是生活随笔為你收集整理的2D激光SLAM：：ROS-AMCL包源码阅读（三）从main()开始的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。