12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
cartographer/mapping_3d/kalman_local_trajectory_builder.cc
View File

@ -151,12 +151,14 @@ 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,
{{&filtered_point_cloud_in_tracking,
&matching_submap->high_resolution_hybrid_grid()},
{&low_resolution_point_cloud_in_tracking,
&matching_submap->low_resolution_hybrid_grid()}},
&pose_observation_in_submap, &summary);
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,
&matching_submap->low_resolution_hybrid_grid()}},
&pose_observation_in_submap, &summary);
const transform::Rigid3d pose_observation =
matching_submap->local_pose() * pose_observation_in_submap;
pose_tracker_->AddPoseObservation(