Cartographer安装测试与模块开发(四)--Cartographer纯定位思路
重定位功能是这个研究方向上很难绕开的一个需求，重定位方法有很多，根据自己的下游任务需求和自己的项目情况来选择合适的重定位方法当然是最好的，我第一次接触重定位时间有些久了细节记不清了，大概是需要将一个开发的视觉模块安置在基于2D雷达的可自动导航平台上进行避障测试，当时选用的组合是使用Cartographer进行地图建立，将pbstream地图转换为pgm地图进行定位，使用PL-ICP输出维护一个激光里程计，并利用ROS导航功能包实现机器人控制等功能。
现在需要完全基于Cartographer来做一个重定位功能，其实Cartographer本身也有定位功能的实现的，但是实际测试重定位效果不是很稳定，而且匹配的比较慢。所以希望实现在给定位姿的情况下能有一个比较快的重定位这样一个功能。
因为重定位部分涉及到的文件有点多，所以不会给出非常详细的步骤，不能像以前一样参考教程做就一定能成功，这里只是给出一个思路，具体的实现需要自己按思路修改源码，所以需要对Cartographer的程序结构有一点了解。
因为是单纯设计纯定位功能，不考虑与其他模块的兼容性与配合，因此这个思路相对来说简单很多，不需要完全按照源码结构去安排代码，省掉了很多无关重定位功能的其他冗余代码。整体上的步骤是这样的：
在ROS层接收初始位姿->通过重定位的接口将信息传递给算法层->在算法层一个函数内实现重定位功能。
重定位接口设计
既然需要输入初始位姿，那么肯定是需要为其设计一个接口来订阅重定位信息并输入算法处理，这部分内容网上也有一些参考，主要分为在node_main.cc文件中编写回调函数接收给的初始位姿，然后一层一层传递到pose_graph_2d.cc文件中，在该文件内编写具体的实现，之所以这样安排是因为算法将ROS与算法进行了隔离，ROS部分只接收发布信息，不负责算法实现，算法层也是同理，两者分别对应src下面的cartographer_ros与cartographer。因此要实现定位功能需要使用的变量以及函数只有在算法层才能够访问到。
在开始搭建接口的时候建议所有的接口函数都设置形如:
void test(){LOG(ERROR) << "\nThe xxx file has been accessed!"
}这样可以在命令行窗口直观看到接口搭建到了哪里，在什么位置会出错，等整个接口搭建完毕，再替换为bool返回值和设置相应的传参内容。
在这里的初始位姿采用Rviz中的initpose给出，这个可以自由替换为自己的信息源，首先在node_main.cc文件中编写回调函数（也可写在node.cc与其他回调写在一起）：
//位于run函数内部trajectory_options_handle = &(trajectory_options);node_handle = &(node);ros::Subscriber initPose_sub = node.node_handle()->subscribe("/initialpose", 1, Reset_InitPose_callback);//外部回调函数
void Reset_InitPose_callback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg) {absl::MutexLock lock(&laser_mutex_);// 关闭当前运行的Trajectoriesnode_handle->FinishAllTrajectories();// 缓存雷达帧*//雷达帧通过读取方法保存至node类中的私有变量内//这样写在node的函数可以直接调用该变量向下传值//也避免了单独书写雷达的回调函数
// 配置参数*// 重定位功能函数 输入：给定的初始位姿、最低匹配得分、匹配位姿（返回值）、最佳匹配得分（返回值）if (!node_handle -> GlobalPositioningTest(Given_initial_pose, Cutoff, &init_pose, &best_score)) {LOG(ERROR) << "Relocalization failed";return;}// 利用重定位功能返回的位姿重启轨迹*// 重新开启Trajectoryif (FLAGS_start_trajectory_with_default_topics) {node_handle->StartTrajectoryWithDefaultTopics(*trajectory_options_handle);}
}为了代码简洁一些，我将部分功能进行了省略，注释部分后面带有*符号的为被省略的功能代码，在后文会贴出来完整代码。
回调主要功能是中断当前活动轨迹，接收需要的数据并传入重定位函数，然后利用重定位函数返回位姿重新启动一个以计算得到的位姿为起点的轨迹。
在这里我需要的信息主要为：给定的初始位姿，当前激光帧，最低匹配得分（固定值，低于此分数的结果为匹配失败）。
重定位得到的位姿与得分一层层返回至init_pose与best_score变量内。
完整代码：
void Reset_InitPose_callback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg) {absl::MutexLock lock(&laser_mutex_);node_handle->FinishAllTrajectories();//雷达帧缓存至node私有变量中sensor_msgs::LaserScan::ConstPtr laser_msg =ros::topic::waitForMessage<sensor_msgs::LaserScan>("/scan", *(node_handle->node_handle()), ros::Duration(0.5));if (!laser_msg) {ROS_WARN("No LaserScan received within 0.5s, skip reset.");return;}node_handle->Save_LaserScan(laser_msg); //在node类内编写该函数，将laser_msg赋给私有变量即可constexpr float Cutoff = 0.5f; cartographer::transform::Rigid2d init_pose;cartographer::transform::Rigid3d Given_initial_pose = cartographer_ros::ToRigid3d(msg->pose.pose);float best_score;if (!node_handle -> GlobalPositioningTest(Given_initial_pose, Cutoff, &init_pose, &best_score)) {LOG(ERROR) << "Relocalization failed";return;}cartographer::transform::Rigid3d init_pose_3d = cartographer::transform::Embed3D(init_pose);auto* proto_pose = trajectory_options_handle->trajectory_builder_options.mutable_initial_trajectory_pose()->mutable_relative_pose();*proto_pose = cartographer::transform::ToProto(init_pose_3d);if (FLAGS_start_trajectory_with_default_topics) {node_handle->StartTrajectoryWithDefaultTopics(*trajectory_options_handle);}
}接下来进入node.h文件，我们需要的数据已经基本传入node类中的GlobalPositioningTest函数，点云数据由于都在类内可以直接进行读取。进入h文件对函数进行声明：
void Save_LaserScan(const sensor_msgs::LaserScan::ConstPtr& msg);
//这里因为GlobalPositioningTest函数只负责向下传值，不负责功能实现，所以直接在h文件中写完，不在cc文件中实现
//注意map_builder_bridge_.GlobalPositioningTest中GlobalPositioningTest函数是map_builder_bridge.cc中的同名函数，与node中的GlobalPositioningTest函数无关bool GlobalPositioningTest(cartographer::transform::Rigid3d Given_initial_pose,float cutoff, ::cartographer::transform::Rigid2d* best_pose_estimate, float* best_score){return map_builder_bridge_.GlobalPositioningTest(Given_initial_pose, pure_point_cloud, cutoff, best_pose_estimate, best_score);}//在node.cc中Save_LaserScan的实现（转格式+保存）：
void Node::Save_LaserScan(const sensor_msgs::LaserScan::ConstPtr& msg){::cartographer::sensor::PointCloudWithIntensities point_cloud;::cartographer::common::Time time;std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg); pure_point_cloud = point_cloud.points; //pure_point_cloud为node类新声明的私有变量
}
数据被向下传递到map_builder_bridge这样一个ros层与算法层交换数据的桥梁文件中，在map_builder_bridge.cc文件中编写函数实现（不要忘了在map_builder_bridge.h文件的类内对函数进行声明）：
bool MapBuilderBridge::GlobalPositioningTest( cartographer::transform::Rigid3d Given_initial_pose,const cartographer::sensor::TimedPointCloud& laser_point_cloud,float cutoff, ::cartographer::transform::Rigid2d* best_pose_estimate, float* best_score){auto* pose_graph = dynamic_cast<cartographer::mapping::PoseGraph2D*>(map_builder_->pose_graph());if (!pose_graph) {LOG(ERROR) << "Only 2D pose graph is supported";return false;}return pose_graph->GlobalPositioningTest(Given_initial_pose, laser_point_cloud, cutoff, best_pose_estimate, best_score);}这样一来数据进一步传递到pose_graph_2d.cc中的GlobalPositioningTest函数内了，这里需要注意：
map_builder_变量调用的pose_graph()函数来自class MapBuilder，继承自MapBuilderInterface，该函数内容为：
mapping::PoseGraphInterface *pose_graph() override {return pose_graph_.get();}因此返回的是指向基类的指针，需要将其动态转换为指向PoseGraph2D的指针才能调用写在pose_graph_2d.cc文件内的函数（因为还有pose_graph_3d的派生类）。
这在声明pose_graph_2d.cc中编写GlobalPositioningTest最终功能函数时有些麻烦，对于该函数的声明不仅仅需要在基类中创建纯虚函数，还需要在2d、3d文件中全部写出具体实现，如下：
//pose_graph_interface.hvirtual bool GlobalPositioningTest(cartographer::transform::Rigid3d Given_initial_pose,const cartographer::sensor::TimedPointCloud& point_cloud, float cutoff, transform::Rigid2d* best_pose_estimate, float* best_score) = 0;//pose_graph_3d.h
//仅供2d使用，3d该函数无效bool GlobalPositioningTest(cartographer::transform::Rigid3d Given_initial_pose,const sensor::TimedPointCloud& point_cloud,float cutoff,transform::Rigid2d* best_pose_estimate,float* best_score) override;//pose_graph_3d.cc
bool PoseGraph3D::GlobalPositioningTest(cartographer::transform::Rigid3d Given_initial_pose,const sensor::TimedPointCloud&,float,transform::Rigid2d*,float*) {LOG(WARNING) << "GlobalPositioningTest not implemented for 3D";return false;
}//pose_graph_2d.hbool GlobalPositioningTest(cartographer::transform::Rigid3d Given_initial_pose,const cartographer::sensor::TimedPointCloud& point_cloud, float cutoff, transform::Rigid2d* best_pose_estimate, float* best_score) override;//pose_graph_2d.cc
bool PoseGraph2D::GlobalPositioningTest(cartographer::transform::Rigid3d Given_initial_pose,const cartographer::sensor::TimedPointCloud& laser_point_cloud,float cutoff, transform::Rigid2d* best_pose_estimate, float* best_score){
//具体实现
}至此接口部分的搭建已经完成，信息可以由node_main传递至pose_graph_2d进行重定位逻辑处理。
定位实现
定位部分的实现就有很多种方案了，参考了下面的代码：https://github.com/zkk123456/cartographer_global_relocation
https://github.com/zkk123456/cartographer_global_relocation进行了测试，代码原理逻辑是行的通的，但是其中部分代码可能存在编译报错，对应修改就好了。
这个方案主要是实现的全局无初始位姿进行的重定位功能，速度取决于子图数量与设置的lua参数。基于这个方法进行优化提高速度的方法主要是三个方面：
传入初始位姿，通过这个初始位姿可以计算每个子图距离该初始位姿的距离，从而筛选出初始位姿附近的子图，减少子图的数量从而加快匹配时间；再一个如果给的初始位姿一般比较准，可以将初始位姿转换到子图坐标系下，将MatchFullSubmap匹配函数替换为需要初值的Match函数，从而直接从给定的初始位姿处开始搜索，而不是从子图中心开始搜索，初值给的准也会提高速度。这里附上有关根据距离筛选子图的部分代码：
cartographer::sensor::PointCloud filtered_point_cloud;//子图扫描范围double search_submap_range = 10.0; //mstd::vector<cartographer::mapping::SubmapId> nearby_submaps;//将cartographer::sensor::TimedPointCloud格式点云转化为需要的cartographer::sensor::PointCloud格式for (const auto& p : cartographer::sensor::VoxelFilter(laser_point_cloud, 0.05))filtered_point_cloud.push_back(cartographer::sensor::RangefinderPoint{p.position});// 基于初始位姿筛选子图for (const auto& submap_id_data : data_.submap_data) {const SubmapId& id = submap_id_data.id;if (id.trajectory_id != 0) {continue;}const auto global_submap_pose = data_.global_submap_poses_2d.at(id).global_pose;const double distance = (global_submap_pose.translation() - Given_initial_pose.translation().head<2>()).norm();if (distance <= search_submap_range) {nearby_submaps.push_back(id);}}最后一个方面就是优化lua参数了，根据自己的环境进行调整，搜索窗大小、角度，分枝定界的树深等等。