Ask Your Question
1

How to extract pose/trajectory data from cartographer?

asked 2017-05-09 16:16:24 -0500

struncp gravatar image

updated 2017-05-10 11:47:50 -0500

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

image description

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

image description

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 ...
(more)
edit retag flag offensive close merge delete

Comments

Could I ask you to please attach all images to your question? I've given you enough karma to do so. Also: it would be nice if you could include your code in your question too: just use the Preformatted Text button for that. If it's too much, then consider using Github. Dropbox does not work ..

gvdhoorn gravatar imagegvdhoorn ( 2017-05-10 00:58:51 -0500 )edit

.. very well for these things, as it completely depends on you maintaining your account and not deleting the files.

gvdhoorn gravatar imagegvdhoorn ( 2017-05-10 00:59:15 -0500 )edit

About images: when I use the provided tag"alt text(/path/img.jpg "title")" (with ! and []) I can't see the image and I don't have any restricted characters in the path. Strange.

Anyway looks like the drift is caused by not using imu in the algorithm.

struncp gravatar imagestruncp ( 2017-05-10 05:37:50 -0500 )edit
1

You should be able to attach the images directly to your post. Just use the image button and browse for your image.

gvdhoorn gravatar imagegvdhoorn ( 2017-05-10 05:52:51 -0500 )edit

Hi! can I ask you how you have plotted the ground truth vs the hector_trajectory output? Excuse me if this is unrelated to your original question. Thanks

fj138696 gravatar imagefj138696 ( 2017-06-30 01:31:22 -0500 )edit

I extracted timestamps, positions and bearings from each implementation to .csv format and plotted them via matlab

struncp gravatar imagestruncp ( 2017-07-09 14:13:34 -0500 )edit

what do you mean by bearing?

fj138696 gravatar imagefj138696 ( 2017-07-12 04:45:14 -0500 )edit

angle of orientation of the robot

struncp gravatar imagestruncp ( 2017-07-12 10:24:44 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2017-05-18 18:14:33 -0500

struncp gravatar image
edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2017-05-09 16:13:58 -0500

Seen: 2,401 times

Last updated: May 18 '17