纯定位时载入的地图在submap中的trajectory为0，启动定位后，系统默认从起点开始建立子图进行匹配，该子图在submap中的trajectory为 1。
一条trajectory可以理解为一次建图过程。如果只让机器人跑一圈，那么trajectory数就是1. 但是，建好图后又需要在图中走，这时候可以增加一条trajectory，把pure_localization设为true，那么机器人再重新跑的过程中就会跟已经建好的图进行匹配，估计机器人在地图中的路径。所以，一次运行就代表了一条trajectory。
cartographer定位时，它的地图是变化的
纯定位和建图不同，local和global SLAM之间的延迟要尽量低. 其次global SLAM通常会在作为地图的frozen trajectory和当前trajectory之间，找到大量的相互约束(inter constraints)。
纯定位模式默认只保存 3 个子图，只有这些子图内的节点参与优化。所以地图大小并不会影响后端优化的速度。而建图结束的时候全图优化应该是所有节点参与。
我们需要显著减小POSE_GRAPH.optimize_every_n_nodes以接收频繁的结果， global SLAM通常会太慢，所以再显著减小global_sampling_ratio和constraint_builder.sampling_ratio以补偿大量的约束。
submaps.resolution应当和运行的.pbstream中的子图resolution一致
建图时，cartographer使用 fast correlative scan matcher找到回环，search window默认为7m×7m×30°. 但纯定位模式中，search window变得非常大，所以纯定位消耗CPU更大
constraint_builder_2d.cc中的ComputeConstraint()函数，在纯定位模式下，会频繁地调用MaybeAddGlobalConstraint()，这个函数会搜索整个子图，造成的开销特别大。建议将global_sampling_ratio调小，这样就可以从一定程度上减少开销。
数据集的纯定位测试
程序运行时只在局部建图（保留固定数量的子图），随着机器人的运动和观测， 新的子图会添加进来，旧的子图会被删除，就是只保留局部的地图数据。我的理解是保留局部数据越多，与原有地图进行回环检测就增多，这样计算量增加的同时也增加了误匹配的概率，使得整个系统不稳定 ，所以这里只进行定位，不用于建图
需要用到数据集b2-2016-04-05-14-44-52.bag和b2-2016-04-27-12-31-41.bag，前者用于建图，后者用于定位
先运行建图：1
roslaunch cartographer_ros offline_backpack_2d.launch bag_filenames:=${HOME}/path/b2-2016-04-05-14-44-52.bag
等一会，完成后会生成pbstream文件，如果运行建图，实际地图是这样的
这里是先用第一个数据集建图，建完图后用第二个数据集进行定位测试。1
roslaunch cartographer_ros demo_backpack_2d_localization.launch load_state_filename:=${BAG}/b2-2016-04-05-14-44-52.bag.pbstream bag_filename:=${BAG}/b2-2016-04-27-12-31-41.bag
后面两个参数一个是上一步建好的地图，pbstream格式，另一个是数据集。你会看到机器人很快实现了定位
在机器人的旧版本纯定位
用于定位时的home_no_odom_localization.launch1
2
3
4
5
6
7
8
9
10
11
12<launch>
<node name="cartographer_node" pkg="cartographer_ros"
type="cartographer_node" args="
-configuration_directory $(find cartographer_ros)/configuration_files
-configuration_basename home_no_odom_localization.lua
-load_state_filename /home/xiaoqiang/Documents/ros/maps/home.pbstream"
output="screen">
</node>
<node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
</launch>
使用cartographer建图和定位时，launch文件的不同之处在于cartographer_node节点的参数-configuration_basename和-load_state_filename，前者就是lua文件的不同，后者只在定位时才有，毕竟没有地图就不能定位
建图使用no_odom_and_imu.lua，定位使用home_no_odom_localization.lua:1
2
3
4
5
6
7include "home.lua"
TRAJECTORY_BUILDER.pure_localization = true
--- 每10个有效帧一个子图，子图构建完成要闭环检测一次，这个数越小，闭环检测越频繁，当然CPU增大
POSE_GRAPH.optimize_every_n_nodes = 5
POSE_GRAPH.constraint_builder.sampling_ratio = 0.1
POSE_GRAPH.global_sampling_ratio = 0.002
启动roslaunch cartographer_ros home_localization.launch后应该就可以了，必须有map坐标系，否则就是没成功。
optimize_every_n_nodes会影响定位时的CPU占用，设置为5时对应50%，但只有定位时的一秒左右，所以无所谓。
添加轨迹实际调用的源码是node.cc中的Node::HandleStartTrajectory——AddTrajectory
新版本的配置
这是因为我的 cartographer版本旧，不支持pure_localization_trimmer，最好还是更新到最新。在18年7月增加了pure_localization_trimmer，删掉了pure_localization，如果是新一点的版本，做对应修改。commit历史在这里
相应的源码在 MapBuilder::AddTrajectoryBuilder——MaybeAddPureLocalizationTrimmer：1
2
3
4
5
6
7
8
9
10
11
12
13
14
15if (trajectory_options.pure_localization())
{
LOG(WARNING) << "'TrajectoryBuilderOptions::pure_localization' field is deprecated"
"Use 'TrajectoryBuilderOptions::pure_localization_trimmer' instead.";
pose_graph->AddTrimmer(absl::make_unique<PureLocalizationTrimmer>(
trajectory_id, 3 /* max_submaps_to_keep */) );
return;
}
if (trajectory_options.has_pure_localization_trimmer())
{
// 添加一个PureLocalizationTrimmer类型修饰器
pose_graph->AddTrimmer(absl::make_unique<PureLocalizationTrimmer>(
trajectory_id,
trajectory_options.pure_localization_trimmer().max_submaps_to_keep() ) );
}
新版本的配置:1
2
3
4TRAJECTORY_BUILDER.pure_localization_trimmer = {
--- 最大保存子图数，存定位模式通过子图进行定位，但只需要当前和上一个子图即可
max_submaps_to_keep = 3,
}
如果使用了机器人的odom，但无法定位成功或定位误差太大，可能的原因是odom不够精确，尝试关闭odom，并由cartographer代为提供odom。既然定位误差大，可能在建图时的原始submap之间误差较大，图优化后map的边缘较为模糊
重定位注释掉Node::GrawAndPublish函数的后三行，否则会继续建图。但是建图的时候还得加回来。
由于Cartographer定位是当前帧和子图进行匹配，所以对于机器人周围被新物体遮挡的情况完全没有影响。
唯一影响就是没法重定位了，因为地图上没有这些新物体。但是就算长时间被遮挡，存在累积定位误差，只要机器人离开了遮挡区域，回到了之前的环境，就可以重定位回来。
获得机器人在map坐标系中的坐标
cartographer是使用tf转换来发布位姿的，如果想发布类似amcl_pose那样的话题，只能修改源码或自己写个程序。
修改源码的方法可以参考这里
我还是自己写了个程序，其实就是tf报警：Lookup would require extrapolation into the past中的程序改了坐标系和话题名称，以获得base_footprint在map中的坐标系
问题
某次运行得到这样结果：
追查到node.cc里的Node::FinishTrajectoryUnderLock函数，检查了一下是否可以关掉，指定id是否存在，是否已经被Finished了等情况后，如果一切正常，则停止订阅Topic、清除id及其他与该trajectory相关的量。最后调用map_builder_bridge_中的FinishTrajectory函数。发现code对应cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT
