How to extract pose/trajectory data from cartographer?

edit

Hello, I am comparing SLAM algorithms based on their poses throughout their trajectory in my thesis and I can't come up with a way to get a trajectory from cartographer. I use .bag data with ground truth from MIT Stata center (link below). Currently I'm obtaining trajectory via hector trajectory server and it worked fine with both Hector SLAM and gMapping but Cartographers trajectory is quite off

This is from run at 0.2 speed. When I run it at realtime and robot is coming back from that part "north" to the main long hall (that part that looks like two curved horns) back that long hall to go "east" it simply skips like this

I believe there is some wild computing when cartographer is closing the loop from that "north" part which is not compatible with hector trajectory server. I also hope that I have right settings for cartographer because map on the outcome is correct.

source code: http://ge.tt/9exBp2k2

used bag: 2012-01-28-11-12-01.bag.noimages (in ground truth) at http://projects.csail.mit.edu/stata/d...

Thanks for any feedback in advance.

cartographer.launch

<launch>
  <node pkg="rosbag" type="play" name="player" output="screen" args="--clock --pause --rate=1 /home/struncp/ros_workspace/test/SLAM_test_MIT_stata_center/2012-01-28-11-12-01.bag"/>

 <param name="/use_sim_time" value="true"/>

  <node name="cartographer_node" pkg="cartographer_ros"
      type="cartographer_node" args="
          -configuration_directory /home/struncp/ros_workspace/test/SLAM_test_MIT_stata_center/cartographer_config
          -configuration_basename config.lua"
      output="screen">
    <remap from="scan" to="/base_scan" />
  </node>

  <node pkg="hector_trajectory_server" type="hector_trajectory_server" name="hector_trajectory_server" output="screen">
    <param name="target_frame_name" type="string" value="/map" />
    <param name="source_frame_name" type="string" value="/base_link" />
    <param name="trajectory_update_rate" type="double" value="4" />
    <param name="trajectory_publish_rate" type="double" value="0.25" />
  </node>

  <node pkg="rviz" type="rviz" name="rviz"
  args="-d /home/struncp/ros_workspace/test/SLAM_test_MIT_stata_center/settings_carto.rviz" />

</launch>

config.lua

include "map_builder.lua"

options = {
  map_builder = MAP_BUILDER,
  map_frame = "map",
  tracking_frame = "base_footprint",
  published_frame = "base_footprint",
  odom_frame = "odom_combined",
  provide_odom_frame = true,
  use_odometry = false,
  use_laser_scan = true,
  use_multi_echo_laser_scan = false,
  num_point_clouds = 0,
  lookup_transform_timeout_sec = 0.2,
  submap_publish_period_sec = 0.3,
  pose_publish_period_sec = 5e-3,
}

MAP_BUILDER.use_trajectory_builder_2d = true

TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
TRAJECTORY_BUILDER_2D.use_imu_data = false
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.15
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.angular_search_window = math.rad(35.)

SPARSE_POSE_GRAPH.optimization_problem.huber_scale = 1e2

return options

trajectory_builder_2d.lua

TRAJECTORY_BUILDER_2D = {
  use_imu_data = true,
  laser_min_range = 0.,
  laser_max_range = 30.,
  laser_min_z = -0.8,
  laser_max_z = 2.,
  laser_missing_echo_ray_length = 5.,
  laser_voxel_filter_size = 0.025,

  use_online_correlative_scan_matching = true,
  adaptive_voxel_filter = {
    max_length = 0.5,
    min_num_points = 200,
    max_range = 50.,
  },

  real_time_correlative_scan_matcher = {
    linear_search_window = 0.1,
    angular_search_window = math.rad(20.),
    translation_delta_cost_weight = 1e-1,
    rotation_delta_cost_weight = 1e-1,
  },

  ceres_scan_matcher = {
    occupied_space_weight = 1e1,
    translation_weight = 1e1,
    rotation_weight = 1e2,
    covariance_scale = 1e-2,
    ceres_solver_options = {
      use_nonmonotonic_steps = false,
      max_num_iterations = 20,
      num_threads = 1,
    },
  },

  motion_filter = {
    max_time_seconds = 5.,
    max_distance_meters = 0.2,
    max_angle_radians = math.rad(1.),
  },

  imu_gravity_time_constant = 10.,
  num_odometry_states = 1000,

  submaps = {
    resolution = 0.05,
    half_length = 200.,
    num_laser_fans = 90,
    output_debug_images = false,
    laser_fan_inserter = {
      insert_free_space = true,
      hit_probability = 0.55,
      miss_probability = 0.49,
    },
  },
}

sparse_pose_graph.lua

SPARSE_POSE_GRAPH = {
  optimize_every_n_scans = 90,
  constraint_builder = {
    sampling_ratio = 0.3,
    max_constraint_distance = 15.,
    adaptive_voxel_filter = {
      max_length = 0.9,
      min_num_points = 100,
      max_range = 50.,
    },
    min_score = 0.55,
    global_localization_min_score = 0.6,
    lower_covariance_eigenvalue_bound = 1e-11,
    log_matches = true,
    fast_correlative_scan_matcher = {
      linear_search_window = 7.,
      angular_search_window = math.rad(30.),
      branch_and_bound_depth = 7,
    },
    ceres_scan_matcher = {
      occupied_space_weight = 20.,
      translation_weight = 10.,
      rotation_weight = 1.,
      covariance_scale = 1e-4,
      ceres_solver_options = {
        use_nonmonotonic_steps = true,
        max_num_iterations = 10 ...