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

Use oldest and newest instead of last two odometry data. (#530)

master
Jihoon Lee 2017-09-14 16:10:03 +02:00 committed by Wolfgang Hess
parent 5896ead32e
commit 259e22a5fc
1 changed files with 11 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
cartographer/mapping/pose_extrapolator.cc
View File

@ -95,14 +95,14 @@ void PoseExtrapolator::AddOdometryData(
} }
// TODO(whess): Improve by using more than just the last two odometry poses. // TODO(whess): Improve by using more than just the last two odometry poses.
// Compute extrapolation in the tracking frame. // Compute extrapolation in the tracking frame.
const sensor::OdometryData& odometry_data_older = const sensor::OdometryData& odometry_data_oldest =
odometry_data_[odometry_data_.size() - 2]; odometry_data_.front();
const sensor::OdometryData& odometry_data_newer = const sensor::OdometryData& odometry_data_newest =
odometry_data_[odometry_data_.size() - 1]; odometry_data_.back();
const double odometry_time_delta = const double odometry_time_delta =
common::ToSeconds(odometry_data_older.time - odometry_data_newer.time); common::ToSeconds(odometry_data_oldest.time - odometry_data_newest.time);
const transform::Rigid3d odometry_pose_delta = const transform::Rigid3d odometry_pose_delta =
odometry_data_newer.pose.inverse() * odometry_data_older.pose; odometry_data_newest.pose.inverse() * odometry_data_oldest.pose;
angular_velocity_from_odometry_ = angular_velocity_from_odometry_ =
transform::RotationQuaternionToAngleAxisVector( transform::RotationQuaternionToAngleAxisVector(
odometry_pose_delta.rotation()) / odometry_pose_delta.rotation()) /
@ -111,14 +111,14 @@ void PoseExtrapolator::AddOdometryData(
return; return;
} }
const Eigen::Vector3d const Eigen::Vector3d
linear_velocity_in_tracking_frame_at_newer_odometry_time = linear_velocity_in_tracking_frame_at_newest_odometry_time =
odometry_pose_delta.translation() / odometry_time_delta; odometry_pose_delta.translation() / odometry_time_delta;
const Eigen::Quaterniond orientation_at_newer_odometry_time = const Eigen::Quaterniond orientation_at_newest_odometry_time =
timed_pose_queue_.back().pose.rotation() * timed_pose_queue_.back().pose.rotation() *
ExtrapolateRotation(odometry_data_newer.time); ExtrapolateRotation(odometry_data_newest.time);
linear_velocity_from_odometry_ = linear_velocity_from_odometry_ =
orientation_at_newer_odometry_time * orientation_at_newest_odometry_time *
linear_velocity_in_tracking_frame_at_newer_odometry_time; linear_velocity_in_tracking_frame_at_newest_odometry_time;
} }
transform::Rigid3d PoseExtrapolator::ExtrapolatePose(const common::Time time) { transform::Rigid3d PoseExtrapolator::ExtrapolatePose(const common::Time time) {