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话题了。