AMCL代码review

主要针对一下几个方面进行review

1. 初始map_odom如何是初始化
2. map_odom如何进行更新
3. 总结

---

1.AMCL的初始化

初始的MAP_ODOM位态在setInitialPose直接指定：

void Node::setInitialPose(const tf::Transform& init_pose, const float x_dev, const float y_dev, const float z_dev,
                          const float a_dev)
{
  initodom_2_world_tf_ = init_pose;

  const tf::Vector3 t = init_pose.getOrigin();

  const float x_init = t.x();
  const float y_init = t.y();
  const float z_init = t.z();
  const float a_init = static_cast<float>(getYawFromTf(init_pose));

  pf_.init(parameters_.num_particles_, x_init, y_init, z_init, a_init, x_dev, y_dev, z_dev, a_dev);

  mean_p_ = pf_.getMean();
  lastmean_p_ = mean_p_;

  /* Extract TFs for future updates */
  /* Reset lastupdatebase_2_odom_tf_ */
  lastupdatebase_2_odom_tf_ = base_2_odom_tf_;

  /* Publish particles */
  publishParticles();
}

但map-odom会在程序运行中实时更新

2.如何更新map_odom坐标

1. 读取读取odom的callback，得到base_2_odom_tf_
2. 根据粒子滤波器和地图的估计结果，得到base_2_world
3. 因为odom的频率较快，那么在domcallback更新滤波器的结果已经落后了，最新时刻的车体的位置应该是原先的位置+odom在这段时间记录的位置差。
4. 最新base_2_world_tf = base_2_world_tf * base_2_odom_tf_.inverse();

void Node::odomCallback(const geometry_msgs::TransformStampedConstPtr& msg)
{
  ROS_DEBUG("odomCallback open");
//1. 读取读取odom的callback，得到base_2_odom_tf_
  base_2_odom_tf_.setOrigin(
      tf::Vector3(msg->transform.translation.x, msg->transform.translation.y, msg->transform.translation.z));
  base_2_odom_tf_.setRotation(tf::Quaternion(msg->transform.rotation.x, msg->transform.rotation.y,
                                             msg->transform.rotation.z, msg->transform.rotation.w));

  /* If the filter is not initialized then exit */
  if (!pf_.isInitialized())
  {
    ROS_WARN("Filter not initialized yet, waiting for initial pose.");
    if (parameters_.set_initial_pose_)
    {
      tf::Transform init_pose;
      init_pose.setOrigin(tf::Vector3(parameters_.init_x_, parameters_.init_y_, parameters_.init_z_));
      init_pose.setRotation(tf::Quaternion(0.0, 0.0, sin(parameters_.init_a_ * 0.5), cos(parameters_.init_a_ * 0.5)));
      setInitialPose(init_pose, parameters_.init_x_dev_, parameters_.init_y_dev_, parameters_.init_z_dev_,
                     parameters_.init_a_dev_);
    }
    return;
  }

  /* Update roll and pitch from odometry */
  double yaw;
  base_2_odom_tf_.getBasis().getRPY(roll_, pitch_, yaw);

  static tf::TransformBroadcaster tf_br;
  tf_br.sendTransform(
      tf::StampedTransform(base_2_odom_tf_, ros::Time::now(), parameters_.odom_frame_id_, parameters_.base_frame_id_));

  if (!is_odom_)
  {
    is_odom_ = true;
    lastbase_2_world_tf_ = initodom_2_world_tf_ * base_2_odom_tf_;
    lastodom_2_world_tf_ = initodom_2_world_tf_;
  }

  static bool has_takenoff = false;
  if (!has_takenoff)
  {
    ROS_WARN("Not <<taken off>> yet");

    /* Check takeoff height */
    has_takenoff = base_2_odom_tf_.getOrigin().getZ() > parameters_.take_off_height_;

    lastbase_2_world_tf_ = initodom_2_world_tf_ * base_2_odom_tf_;
    lastodom_2_world_tf_ = initodom_2_world_tf_;

    lastmean_p_ = mean_p_;  // for not 'jumping' whenever has_takenoff is true */
  }
  else
  {
    /* Check if AMCL went wrong (nan, inf) */
    if (std::isnan(mean_p_.x) || std::isnan(mean_p_.y) || std::isnan(mean_p_.z) || std::isnan(mean_p_.a))
    {
      ROS_WARN("AMCL NaN detected");
      amcl_out_ = true;
    }
    if (std::isinf(mean_p_.x) || std::isinf(mean_p_.y) || std::isinf(mean_p_.z) || std::isinf(mean_p_.a))
    {
      ROS_WARN("AMCL Inf detected");
      amcl_out_ = true;
    }

    /* Check jumps */
    if (fabs(mean_p_.x - lastmean_p_.x) > 1.)
    {
      ROS_WARN("AMCL Jump detected in X");
      amcl_out_ = true;
    }
    if (fabs(mean_p_.y - lastmean_p_.y) > 1.)
    {
      ROS_WARN("AMCL Jump detected in Y");
      amcl_out_ = true;
    }
    if (fabs(mean_p_.z - lastmean_p_.z) > 1.)
    {
      ROS_WARN("AMCL Jump detected in Z");
      amcl_out_ = true;
    }
    if (fabs(mean_p_.a - lastmean_p_.a) > 1.)
    {
      ROS_WARN("AMCL Jump detected in Yaw");
      amcl_out_ = true;
    }

    if (!amcl_out_)
    {
    	//2. 根据粒子滤波器和地图的估计结果，得到base_2_world
      tf::Transform base_2_world_tf;
      base_2_world_tf.setOrigin(tf::Vector3(mean_p_.x, mean_p_.y, mean_p_.z));
      tf::Quaternion q;
      q.setRPY(roll_, pitch_, mean_p_.a);
      base_2_world_tf.setRotation(q);
		//3. 因为odom的频率较快，那么在domcallback更新滤波器的结果已经落后了，最新时刻的车体的位置应该是原先的位置+odom在这段时间记录的位置差。
		//lastupdatebase_2_odom_tf_是在lidarcallback和initialcallback更新的
      base_2_world_tf = base_2_world_tf * lastupdatebase_2_odom_tf_.inverse() * base_2_odom_tf_;

      lastmean_p_ = mean_p_;

      lastbase_2_world_tf_ = base_2_world_tf;
      //4.最新base_2_world_tf  =  base_2_world_tf * base_2_odom_tf_.inverse();
      lastodom_2_world_tf_ = base_2_world_tf * base_2_odom_tf_.inverse();

      amcl_out_lastbase_2_odom_tf_ = lastupdatebase_2_odom_tf_;
    }
    else
    {
      lastbase_2_world_tf_ = lastbase_2_world_tf_ * amcl_out_lastbase_2_odom_tf_.inverse() * base_2_odom_tf_;
      amcl_out_lastbase_2_odom_tf_ = base_2_odom_tf_;
    }
  }

  /* Publish transform */
  geometry_msgs::TransformStamped odom_2_base_tf;
  odom_2_base_tf.header.stamp = msg->header.stamp;
  odom_2_base_tf.header.frame_id = parameters_.global_frame_id_;
  odom_2_base_tf.child_frame_id = parameters_.base_frame_id_;
  odom_2_base_tf.transform.translation.x = lastbase_2_world_tf_.getOrigin().getX();
  odom_2_base_tf.transform.translation.y = lastbase_2_world_tf_.getOrigin().getY();
  odom_2_base_tf.transform.translation.z = lastbase_2_world_tf_.getOrigin().getZ();
  odom_2_base_tf.transform.rotation.x = lastbase_2_world_tf_.getRotation().getX();
  odom_2_base_tf.transform.rotation.y = lastbase_2_world_tf_.getRotation().getY();
  odom_2_base_tf.transform.rotation.z = lastbase_2_world_tf_.getRotation().getZ();
  odom_2_base_tf.transform.rotation.w = lastbase_2_world_tf_.getRotation().getW();
  odom_base_pub_.publish(odom_2_base_tf);

  tf_br.sendTransform(tf::StampedTransform(lastodom_2_world_tf_, ros::Time::now(), parameters_.global_frame_id_,parameters_.odom_frame_id_));

  ROS_DEBUG("odomCallback close");
}

3.总结

其实整个流程和用odom预测，与雷达ndt配准更新odom-map类似。在AMCL中base-map由滤波器给出，在ndtt匹配中则是由匹配给出。base-odom可以直接由odom读取，而map-odom的初始值有init给定，即使不准也会在后续持续不断更新，初始化的步骤也将极大地简化。