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

This bug was introduced in #294.

PAIR=damonkohler
master
Wolfgang Hess 2017-06-27 11:53:57 +02:00 committed by Damon Kohler
parent d8eb94b4a9
commit 010fefc204
1 changed files with 8 additions and 6 deletions

View File

@ -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,