AMCL代码详解（二）位姿初始化

AMCL的初始位姿估计可以来自于两个地方：

1.用粒子滤波算法估计出的机器人位姿作为最新的位姿

在AmclNode::AmclNode() 中参数设置完后首先调用了这么一个函数：

updatePoseFromServer();

跳到该函数看一下：

void AmclNode::updatePoseFromServer()
{
  init_pose_[0] = 0.0;
  init_pose_[1] = 0.0;
  init_pose_[2] = 0.0;
  init_cov_[0] = 0.5 * 0.5;
  init_cov_[1] = 0.5 * 0.5;
  init_cov_[2] = (M_PI/12.0) * (M_PI/12.0);
  // Check for NAN on input from param server, #5239
  double tmp_pos;
  //设置初始位姿，该位姿来自于上一次退出时存储的位姿，如果没有参数则默认为0.
  //将initial_pose_x赋值给tmp_pos，没有时使用默认值init_pose_[0]。
  private_nh_.param("initial_pose_x", tmp_pos, init_pose_[0]);
  if(!std::isnan(tmp_pos))
    init_pose_[0] = tmp_pos;
  else 
    ROS_WARN("ignoring NAN in initial pose X position");
  private_nh_.param("initial_pose_y", tmp_pos, init_pose_[1]);
  if(!std::isnan(tmp_pos))
    init_pose_[1] = tmp_pos;
  else
    ROS_WARN("ignoring NAN in initial pose Y position");
  private_nh_.param("initial_pose_a", tmp_pos, init_pose_[2]);
  if(!std::isnan(tmp_pos))
    init_pose_[2] = tmp_pos;
  else
    ROS_WARN("ignoring NAN in initial pose Yaw");
  private_nh_.param("initial_cov_xx", tmp_pos, init_cov_[0]);
  if(!std::isnan(tmp_pos))
    init_cov_[0] =tmp_pos;
  else
    ROS_WARN("ignoring NAN in initial covariance XX");
  private_nh_.param("initial_cov_yy", tmp_pos, init_cov_[1]);
  if(!std::isnan(tmp_pos))
    init_cov_[1] = tmp_pos;
  else
    ROS_WARN("ignoring NAN in initial covariance YY");
  private_nh_.param("initial_cov_aa", tmp_pos, init_cov_[2]);
  if(!std::isnan(tmp_pos))
    init_cov_[2] = tmp_pos;
  else
    ROS_WARN("ignoring NAN in initial covariance AA");	
}

这个函数设置了一个位姿以及一个方差，位姿参数来自于："initial_pose_x"以及"initial_pose_y"等，这个参数在另外一个函数：

//这里只是把最新的odom位姿转换成最新的地图位姿去存储，即初始位姿map_pose,我们也对last_published_pose取其协方差,
//作为初始位姿map_pose的协方差。记住last_published_pose很重要的哦！它一般来源于上一次粒子滤波算法估计出的机器人位置。
void AmclNode::savePoseToServer()
{
  // We need to apply the last transform to the latest odom pose to get
  // the latest map pose to store.  We'll take the covariance from
  // last_published_pose.
  //这里的tf2有点不太好理解，似乎最后存储了一个世界坐标系下的坐标，但是它初始应该是从odom变换过去的还是怎么得到的？？？
  tf2::Transform odom_pose_tf2;
  tf2::convert(latest_odom_pose_.pose, odom_pose_tf2);
  tf2::Transform map_pose = latest_tf_.inverse() * odom_pose_tf2;
  //获取坐标的角度
  double yaw = tf2::getYaw(map_pose.getRotation());

  ROS_DEBUG("Saving pose to server. x: %.3f, y: %.3f", map_pose.getOrigin().x(), map_pose.getOrigin().y() );
  //获取坐标的xy值
  private_nh_.setParam("initial_pose_x", map_pose.getOrigin().x());
  private_nh_.setParam("initial_pose_y", map_pose.getOrigin().y());
  private_nh_.setParam("initial_pose_a", yaw);
  //获取协方差值？
  private_nh_.setParam("initial_cov_xx", 
                                  last_published_pose.pose.covariance[6*0+0]);
  private_nh_.setParam("initial_cov_yy", 
                                  last_published_pose.pose.covariance[6*1+1]);
  private_nh_.setParam("initial_cov_aa", 
                                  last_published_pose.pose.covariance[6*5+5]);
}

这里主要用到了tf2的一些知识，将map_pose的位姿赋值给了上述几个参数。具体的tf2变换可以参考ROS官网对于tf2的描述。

这里其实赋值完后就没事了，但是在后面：

if(use_map_topic_) {
    map_sub_ = nh_.subscribe("map", 1, &AmclNode::mapReceived, this);
    ROS_INFO("Subscribed to map topic.");
  } else {
    //所以这里调用了map_server节点
    requestMap();//请求静态地图，调用map_server节点
  }

默认use_map_topic_=false所以调用了requestMap();函数。

AmclNode::requestMap()
{
  boost::recursive_mutex::scoped_lock ml(configuration_mutex_);

  // get map via RPC GetMap服务
  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 );
}

这里转到了：

AmclNode::handleMapMessage(const nav_msgs::OccupancyGrid& msg)

函数处理，注意到在这个函数中有一段代码：

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;

这里跟前面的位姿初始化关联起来了。这个位姿最后是赋值给了pf_init_pose_mean，这是init_pose_唯一被使用到的地方。pf_init_pose_mean的值再次被传参到：

pf_init(pf_, pf_init_pose_mean, pf_init_pose_cov);

函数对粒子进行初始化。也就是说算法通过该方法初始化的粒子的位姿最初是来自于AmclNode::savePoseToServer函数的。

2.订阅话题获得初始位姿

除了从上述方法获得位姿外，算法还可以通过订阅话题的方式获得位姿。函数中订阅了一个位姿话题：

initial_pose_sub_ = nh_.subscribe("initialpose", 2, &AmclNode::initialPoseReceived, this);

回调函数initialPoseReceived调用了另外一个函数handleInitialPoseMessage去处理了这个消息：

AmclNode::initialPoseReceived(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg)
{
  handleInitialPoseMessage(*msg);
}

进入到handleInitialPoseMessage函数，可以看到其主要做了一件事情：

将位姿赋值给变量initial_pose_hyp_并调用applyInitialPose函数进行滤波器的初始化。具体内容包括了以下几个部分：

2.1 frame_id确认

对于传入的消息，首先确认其消息是否是对应frame_id。frame_id需要对应为“map“，否则跳出。

if(msg.header.frame_id == "")
  {
    // This should be removed at some point
    ROS_WARN("Received initial pose with empty frame_id.  You should always supply a frame_id.");
  }
  // We only accept initial pose estimates in the global frame, #5148.
  else if(stripSlash(msg.header.frame_id) != global_frame_id_)//global_frame_id_是指map
  {
    ROS_WARN("Ignoring initial pose in frame \"%s\"; initial poses must be in the global frame, \"%s\"",
             stripSlash(msg.header.frame_id).c_str(),
             global_frame_id_.c_str());
    return;
  }

感觉这里if语句缺少一个return。

2.2 tf2变换

确认消息输入没有问题的情况下，调用tf2变换将位姿变换到指定坐标系下。

geometry_msgs::TransformStamped tx_odom;
  try
  {
    ros::Time now = ros::Time::now();
    // wait a little for the latest tf to become available
    //数据的坐标变换。获得两个坐标系之间转换的关系，包括旋转和平移。
    tx_odom = tf_->lookupTransform(base_frame_id_, msg.header.stamp,
                                   base_frame_id_, ros::Time::now(),//base_link
                                   odom_frame_id_, ros::Duration(0.5));//odom
  }
  catch(tf2::TransformException e)
  {
    // If we've never sent a transform, then this is normal, because the
    // global_frame_id_ frame doesn't exist.  We only care about in-time
    // transformation for on-the-move pose-setting, so ignoring this
    // startup condition doesn't really cost us anything.
    if(sent_first_transform_)
      ROS_WARN("Failed to transform initial pose in time (%s)", e.what());
    tf2::convert(tf2::Transform::getIdentity(), tx_odom.transform);
  }

  tf2::Transform tx_odom_tf2;
  tf2::convert(tx_odom.transform, tx_odom_tf2);
  tf2::Transform pose_old, pose_new;
  tf2::convert(msg.pose.pose, pose_old);
  pose_new = pose_old * tx_odom_tf2;

  // Transform into the global frame
  //变换一个世界坐标
  ROS_INFO("Setting pose (%.6f): %.3f %.3f %.3f",
           ros::Time::now().toSec(),
           pose_new.getOrigin().x(),
           pose_new.getOrigin().y(),
           tf2::getYaw(pose_new.getRotation()));
  // Re-initialize the filter
  pf_vector_t pf_init_pose_mean = pf_vector_zero();
  pf_init_pose_mean.v[0] = pose_new.getOrigin().x();
  pf_init_pose_mean.v[1] = pose_new.getOrigin().y();
  pf_init_pose_mean.v[2] = tf2::getYaw(pose_new.getRotation());
  pf_matrix_t pf_init_pose_cov = pf_matrix_zero();
  // Copy in the covariance, converting from 6-D to 3-D
  for(int i=0; i<2; i++)
  {
    for(int j=0; j<2; j++)
    {
      pf_init_pose_cov.m[i][j] = msg.pose.covariance[6*i+j];
    }
  }
  pf_init_pose_cov.m[2][2] = msg.pose.covariance[6*5+5];

  delete initial_pose_hyp_;
  initial_pose_hyp_ = new amcl_hyp_t();
  initial_pose_hyp_->pf_pose_mean = pf_init_pose_mean;
  initial_pose_hyp_->pf_pose_cov = pf_init_pose_cov;

这一部分感觉是位姿初始化中最难理解的地方，tf2的内容网上能查到的不太多，目前也是一知半解，后面再找个时间专门学习一下。

2.3 初始化粒子分布

调用applyInitialPose函数对粒子进行初始化，这里除了需要一个初始位姿还需要一个条件：地图存在。

AmclNode::applyInitialPose()
{
  boost::recursive_mutex::scoped_lock cfl(configuration_mutex_);
  if( initial_pose_hyp_ != NULL && map_ != NULL ) {
    // 用高斯分布来初始化滤波器
    pf_init(pf_, initial_pose_hyp_->pf_pose_mean, initial_pose_hyp_->pf_pose_cov);
    pf_init_ = false;

    delete initial_pose_hyp_;
    initial_pose_hyp_ = NULL;
  }
}//经历如此种种之后，粒子滤波器得到初始化，也就是粒子得到了新的位姿。

上述的两个办法中粒子的初始化使用的都是高斯分布，其实粒子初始化模型有两种，另外一种只在特定条件下使用，后面再详细介绍。

两种方式的对比

这两种方式都用于初始化位姿，但是用处不同。很多时候我们算法中没有对应的话题时我们都会使用第一种方式进行初始化。

注意到第一种方式中的savePoseToServer，其实是用于位姿保存的，该函数会在每次程序结束时执行一次，以及每隔一定频率也会执行一次：

如果程序关闭而ROS master并没有关闭的话，该位姿会被记录下来，下次启动时还会调用该位姿。

例如下图中，机器人处于场景中某一个非原点的位置时，我们关闭amcl节点。如果我们保留ROS master然后重新打开amcl节点，会发现机器人的初始化位姿就是我们上一次节点关闭时的位姿：

而当我们关闭amcl节点时，同时关闭ROS master节点。然后重新打开ROS master以及amcl。会发现机器人的初始化位姿变成了原点而不是机器人真实存在的位姿：

所以第一种方式生效的条件一般是需要机器人启动时在地图原点或者机器人的ROS master节点不能关闭的情况。

而对于第二种情况，我们可以通过一个自己写的节点测试一下。这里简单写了一个读取仿真中odom数据然后以initialpose话题名称重新发布的节点，注意数据格式要跟amcl中的数据格式对应起来，要不然会报错。这里使用的数据格式是<geometry_msgs::PoseWithCovarianceStamped>格式。
这个是在上面一张图的基础上新增了一个initialpose后得到的结果，可以看到initialpose话题对于amcl节点来说其优先级高于第一种初始化方式。它修正了第一种位姿初始化后的错误的位姿估计。

另外，这里由于我的initialpose节点是一直发布的，所以最后得到的位姿始终只有一个，就是只显示一个箭头：
包括我连续运行：

而当我将该节点关闭之后，继续运行之后会回到最初的状态，也就是一堆离散的坐标点：

根据这张消息打印结果大致也能够看出，在程序开始时，算法调用第一种初始化位姿方式进行了一次位姿初始化，然后在订阅到initialpose后采用第二种方式再次进行了一次初始化。程序的主要执行顺序即根据打印消息的顺序执行。由于这里initialpose是一直在接收的，所以打印消息中出现了很多次。如果只执行一次的话后面的顺序跟前面第一张终端图中的消息顺序是一样的。

参考：

https://zhuanlan.zhihu.com/p/434271496