cartographer发布畸变矫正后的scan数据

实现方式：
模仿源代码，在cartographer_ros写一个函数，以函数指针的方式传入cartographer后端，然后接收矫正后的scan数据，然后按照话题laserScan发布出来。
需要同时发布点云强度信息的，还要自己添加含有强度信息的代码。
核心转换代码如下：

  for (size_t i = 0; i < synchronized_data.ranges.size(); ++i) {

    const sensor::TimedRangefinderPoint& hit =
        synchronized_data.ranges[i].point_time;
    const Eigen::Vector3f origin_in_local =
        range_data_poses[i] *
        synchronized_data.origins.at(synchronized_data.ranges[i].origin_index);

    sensor::RangefinderPoint hit_in_local =
        range_data_poses[i] * sensor::ToRangefinderPoint(hit);
    const Eigen::Vector3f delta = hit_in_local.position - origin_in_local;
    const float range = delta.norm();
    if (range >= options_.min_range()) {
      if (range <= options_.max_range()) {
        hit_in_local.intensity = synchronized_data.ranges[i].intensity; //
        accumulated_range_data_.returns.push_back(hit_in_local);
        //
         transform::Rigid3f sensor_to_tracking = 
         transform::Rigid3f::Translation(Eigen::Vector3f(
           synchronized_data.origins.at(synchronized_data.ranges[i].origin_index)[0],
           synchronized_data.origins.at(synchronized_data.ranges[i].origin_index)[1],
           synchronized_data.origins.at(synchronized_data.ranges[i].origin_index)[2]));
         undistortion_scan_data_.returns.push_back(
             sensor_to_tracking.inverse() *
             (range_data_poses[0].inverse() * hit_in_local));

      } else {
        hit_in_local.position =
            origin_in_local +
            options_.missing_data_ray_length() / range * delta;
        hit_in_local.intensity = synchronized_data.ranges[i].intensity; //    
        accumulated_range_data_.misses.push_back(hit_in_local);
        //
         transform::Rigid3f sensor_to_tracking = 
         transform::Rigid3f::Translation(Eigen::Vector3f(
           synchronized_data.origins.at(synchronized_data.ranges[i].origin_index)[0],
           synchronized_data.origins.at(synchronized_data.ranges[i].origin_index)[1],
           synchronized_data.origins.at(synchronized_data.ranges[i].origin_index)[2]));
         undistortion_scan_data_.returns.push_back(
             sensor_to_tracking.inverse() *
             (range_data_poses[0].inverse() * hit_in_local));
      }
    }
    else{
        //
        hit_in_local.position = Eigen::Vector3f::Zero();
        //origin_in_local + 0.0 / range * delta;
        hit_in_local.intensity = 0; //    
        //accumulated_range_data_.misses.push_back(hit_in_local);
        //
        undistortion_scan_data_.returns.push_back(hit_in_local);
    }
  }

其他：改的地方其实挺多，因为
1.我把强度信息也传进去了。
2.对于注册返回的函数指针，为了不破坏原有的代码结构，都是自己添加新的接口。
3.因为源代码对于一些异常点进行了移除，而我要保留所有矫正后的点，所以也修改了部分代码。
4.最核心的就是数据转换，因为carto处理后的数据的坐标系是local的，所以要要逆变换回来。因为carto传入的数据考虑了激光头的外参数，所以传回来也要外参数逆变换回来。

好了，大家可以开心的使用畸变矫正后的scan话题了。