2D激光SLAM：：ROS-AMCL包源码阅读（三）从main()开始

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("amcl_pose", 2, true);
  //"particlecloud"话题
  particlecloud_pub_ = nh_.advertise("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(nh_, scan_topic_, 100);
  laser_scan_filter_ = 
          new tf2_ros::MessageFilter(*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(ros::NodeHandle("~"));
  dynamic_reconfigure::Server::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 > 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;isize_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;
}