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.
// Compute extrapolation in the tracking frame.
const sensor::OdometryData& odometry_data_older =
odometry_data_[odometry_data_.size() - 2];
const sensor::OdometryData& odometry_data_newer =
odometry_data_[odometry_data_.size() - 1];
const sensor::OdometryData& odometry_data_oldest =
odometry_data_.front();
const sensor::OdometryData& odometry_data_newest =
odometry_data_.back();
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 =
odometry_data_newer.pose.inverse() * odometry_data_older.pose;
odometry_data_newest.pose.inverse() * odometry_data_oldest.pose;
angular_velocity_from_odometry_ =
transform::RotationQuaternionToAngleAxisVector(
odometry_pose_delta.rotation()) /
@ -111,14 +111,14 @@ void PoseExtrapolator::AddOdometryData(
return;
}
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;
const Eigen::Quaterniond orientation_at_newer_odometry_time =
const Eigen::Quaterniond orientation_at_newest_odometry_time =
timed_pose_queue_.back().pose.rotation() *
ExtrapolateRotation(odometry_data_newer.time);
ExtrapolateRotation(odometry_data_newest.time);
linear_velocity_from_odometry_ =
orientation_at_newer_odometry_time *
linear_velocity_in_tracking_frame_at_newer_odometry_time;
orientation_at_newest_odometry_time *
linear_velocity_in_tracking_frame_at_newest_odometry_time;
}
transform::Rigid3d PoseExtrapolator::ExtrapolatePose(const common::Time time) {