绑架估计：修改cartographer

一、把kidnap_pose2d做为cartographer的初始位姿

如何把kidnap_pose2d应用到cartographer？想到两种方法。

1. 当前会有个cartographer_node节点正在运行，告诉这个node，把kidnap_pose2d设为当前位姿。只要想，这自然可以做到，但需要修改较多cartographer源码。举个例子，PoseGraph2D有两个变量：data_.submap_data、data_.global_submap_poses_2d，如果要用一个外界强加的位姿，这两个变量必须清空，目前没有清空代码。换个思路，把PoseGraph2D销毁再创建？——PoseGraph2D存在于MapBuilder，它们可说有着一样的生存期。一运行cartographer_node节点就会创建个MapBuilder，节点运行期间MapBuilder必须一直有效。换句话说，节点运行期间PoseGraph2D必须一直有效，所以也不可能通过运行时重创建PoseGraph2D方法来设置特定位姿。
2. 销毁cartographer_node节点，然后以kidnap_pose2d为初始位姿，重新运行该节点。

相对来说，方法1更自然，效率也更高，但它要改的源码过多。第二种方法则要看销毁、运行cartographer_node节点开销。目前用的是第二种。

cartographer存在两个tf：odom-->map的tf、base_footprint-->odom的tf，kidnap_pose2d是作为odom-->map这个tf的初始值。

向cartographer设置个非0初始位姿，涉及到两处。

1.1 MapBuilder::AddTrajectoryBuilder

图1显示了MapBuilder::AddTrajectoryBuilder调用时机。1）它运行在cartographer_node主线程。2）在主线程进入“cbqueue.callAvailable(timeout)”while循环前被调用。

<cartographer>/cartographer/mapping/map_builder.cc
------
int MapBuilder::AddTrajectoryBuilder(
    const std::set<SensorId>& expected_sensor_ids,
    const proto::TrajectoryBuilderOptions& trajectory_options,
    LocalSlamResultCallback local_slam_result_callback) {
  const int trajectory_id = trajectory_builders_.size();
  ...
  if (trajectory_options.has_initial_trajectory_pose()) {
    const auto& initial_trajectory_pose =
        trajectory_options.initial_trajectory_pose();
    pose_graph_->SetInitialTrajectoryPose(
        trajectory_id, initial_trajectory_pose.to_trajectory_id(),
        transform::ToRigid3(initial_trajectory_pose.relative_pose()),
        common::FromUniversal(initial_trajectory_pose.timestamp()));

cartographer支持设置初始位姿，用的应该是修改lua配置。可我还没能正确用起来。

  }

  if (ros::kidnap_pose2d.valid) {
    const tpose2d& pose2d = ros::kidnap_pose2d;
    int to_trajectory_id = 0;
    const cartographer::transform::Rigid3d relative_pose(Eigen::Vector3d(pose2d.x, pose2d.y, 0),
            Eigen::AngleAxisd(pose2d.yaw, Eigen::Vector3d(0, 0, 1)));
    int64_t timestamp = 0;
    pose_graph_->SetInitialTrajectoryPose(
        trajectory_id, to_trajectory_id,
        relative_pose,
        common::FromUniversal(timestamp));

这是现在在用的设置初始位姿方法。同样是调用pose_graph_->SetInitialTrajectoryPose，然后按要求把kidnap_pose2d转换为它须要的参数。

    SDL_Log("use kidnap_pose: (%.5f, %.5f)-deg(%.5f)", pose2d.x, pose2d.y, RAD2DEG(pose2d.yaw));
  }

  ...
}

cartographer支持设置初始位姿，用的应该是修改lua配置。可我还没能正确用起来，也就用了种替代方法。将来等会了，应该会回到lua配置。

1.2 PoseGraph2D::ComputeLocalToGlobalTransform

图2显示了MapBuilder::AddTrajectoryBuilder调用时机：AddSensorData --> PoseGraph2D::AddNode --> PoseGraph2D::GetLocalToGlobalTransform --> 本函数

示例中是第一次调用ComputeLocalToGlobalTransform，因而此时data_.trajectory_nodes、data_.global_submap_poses_2d(即参数global_submap_poses)都是空。

<cartographer>/cartographer/mapping/internal/2d/pose_graph_2d.cc
------
transform::Rigid3d PoseGraph2D::ComputeLocalToGlobalTransform(
    const MapById<SubmapId, optimization::SubmapSpec2D>& global_submap_poses,
    const int trajectory_id) const {
  auto begin_it = global_submap_poses.BeginOfTrajectory(trajectory_id);
  auto end_it = global_submap_poses.EndOfTrajectory(trajectory_id);
  if (begin_it == end_it) {
    const auto it = data_.initial_trajectory_poses.find(trajectory_id);
    if (it != data_.initial_trajectory_poses.end()) {
      return it->second.relative_pose;

上面是我改成的代码，下面注释掉是cartographer官方源码。当在data_.trajectory_nodes是空时，也就是图2显示那种情况，GetInterpolatedGlobalTrajectoryPose会抛出异常。

/*
      return GetInterpolatedGlobalTrajectoryPose(it->second.to_trajectory_id,
                                                 it->second.time) *
             it->second.relative_pose;
*/
    } else {
      return transform::Rigid3d::Identity();
    }
  }
  ,,,
}

经过修改后，time字段就失去了作用。