Fixes the previous pose in local 3D SLAM. (#367)

This bug was introduced in #294. PAIR=damonkohler
2017-06-27 11:53:57 +02:00 · 2017-06-27 11:53:57 +02:00 · 010fefc204
parent d8eb94b4a9
commit 010fefc204
1 changed files with 8 additions and 6 deletions
--- a/cartographer/mapping_3d/kalman_local_trajectory_builder.cc
+++ b/cartographer/mapping_3d/kalman_local_trajectory_builder.cc
@ -151,7 +151,9 @@ KalmanLocalTrajectoryBuilder::AddAccumulatedRangeData(
      options_.low_resolution_adaptive_voxel_filter_options());
  const sensor::PointCloud low_resolution_point_cloud_in_tracking =
      low_resolution_adaptive_voxel_filter.Filter(filtered_range_data.returns);
-  ceres_scan_matcher_->Match(scan_matcher_pose_estimate_, initial_ceres_pose,
+  ceres_scan_matcher_->Match(
+      matching_submap->local_pose().inverse() * scan_matcher_pose_estimate_,
+      initial_ceres_pose,
      {{&filtered_point_cloud_in_tracking,
        &matching_submap->high_resolution_hybrid_grid()},
       {&low_resolution_point_cloud_in_tracking,