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;
}