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 handler
signal(SIGINT, sigintHandler);
// Make our node available to sigintHandler
amcl_node_ptr.reset(new AmclNode());
if (argc == 1)
{
// run using ROS input
ros::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 nicely
amcl_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 diagnostics
private_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 RPC
nav_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 space
free_space_indices.resize(0);
//遍歷地圖所有柵格，將狀態爲空閒的柵格的行列號記錄到free_space_indices
for(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 filter
pf_ = 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 filter
updatePoseFromServer();
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
// Odometry
delete odom_;
odom_ = new AMCLOdom();
ROS_ASSERT(odom_);
odom_->SetModel( odom_model_type_, alpha1_, alpha2_, alpha3_, alpha4_, alpha5_ );
// Laser
delete 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 format
map->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 = occ
if(map_msg.data[i] == 0)
map->cells[i].occ_state = -1;
else if(map_msg.data[i] == 100)
map->cells[i].occ_state = +1;
else
map->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 0
pf_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 sampling
pf_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 pdf
pf_pdf_gaussian_free(pdf);
// Re-compute cluster statistics
pf_cluster_stats(pf, set);
//set converged to 0
pf_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;
// Workspace
double m[4], c[2][2];
size_t count;
double weight;
// Cluster the samples
pf_kdtree_cluster(set->kdtree);
// Initialize cluster stats
set->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 stats
count = 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 stats
for (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;
//該集羣粒子數+1
cluster->count += 1;
cluster->weight += sample->weight;
count += 1;
weight += sample->weight;
// Compute mean
cluster->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 components
for (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 components
for (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 components
for (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;
}