Extrapolator reuses result using additional IMU trackers. (#623)

This makes the extrapolator stricter that time arguments
must be monotonously increasing when calling the Extrapolate
methods.
master
gaschler 2017-11-03 16:19:55 +01:00 committed by Wolfgang Hess
parent 978544eca4
commit 8854aaae9e
2 changed files with 20 additions and 10 deletions

View File

@ -57,7 +57,10 @@ common::Time PoseExtrapolator::GetLastPoseTime() const {
} }
common::Time PoseExtrapolator::GetLastExtrapolatedTime() const { common::Time PoseExtrapolator::GetLastExtrapolatedTime() const {
return GetLastPoseTime(); if (!extrapolation_imu_tracker_) {
return common::Time::min();
}
return extrapolation_imu_tracker_->time();
} }
void PoseExtrapolator::AddPose(const common::Time time, void PoseExtrapolator::AddPose(const common::Time time,
@ -79,6 +82,8 @@ void PoseExtrapolator::AddPose(const common::Time time,
AdvanceImuTracker(time, imu_tracker_.get()); AdvanceImuTracker(time, imu_tracker_.get());
TrimImuData(); TrimImuData();
TrimOdometryData(); TrimOdometryData();
odometry_imu_tracker_ = common::make_unique<ImuTracker>(*imu_tracker_);
extrapolation_imu_tracker_ = common::make_unique<ImuTracker>(*imu_tracker_);
} }
void PoseExtrapolator::AddImuData(const sensor::ImuData& imu_data) { void PoseExtrapolator::AddImuData(const sensor::ImuData& imu_data) {
@ -117,7 +122,8 @@ void PoseExtrapolator::AddOdometryData(
odometry_pose_delta.translation() / odometry_time_delta; odometry_pose_delta.translation() / odometry_time_delta;
const Eigen::Quaterniond orientation_at_newest_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_newest.time); ExtrapolateRotation(odometry_data_newest.time,
odometry_imu_tracker_.get());
linear_velocity_from_odometry_ = linear_velocity_from_odometry_ =
orientation_at_newest_odometry_time * orientation_at_newest_odometry_time *
linear_velocity_in_tracking_frame_at_newest_odometry_time; linear_velocity_in_tracking_frame_at_newest_odometry_time;
@ -129,7 +135,8 @@ transform::Rigid3d PoseExtrapolator::ExtrapolatePose(const common::Time time) {
CHECK_GE(time, newest_timed_pose.time); CHECK_GE(time, newest_timed_pose.time);
return transform::Rigid3d::Translation(ExtrapolateTranslation(time)) * return transform::Rigid3d::Translation(ExtrapolateTranslation(time)) *
newest_timed_pose.pose * newest_timed_pose.pose *
transform::Rigid3d::Rotation(ExtrapolateRotation(time)); transform::Rigid3d::Rotation(
ExtrapolateRotation(time, extrapolation_imu_tracker_.get()));
} }
Eigen::Quaterniond PoseExtrapolator::EstimateGravityOrientation( Eigen::Quaterniond PoseExtrapolator::EstimateGravityOrientation(
@ -180,7 +187,7 @@ void PoseExtrapolator::TrimOdometryData() {
} }
void PoseExtrapolator::AdvanceImuTracker(const common::Time time, void PoseExtrapolator::AdvanceImuTracker(const common::Time time,
ImuTracker* const imu_tracker) { ImuTracker* const imu_tracker) const {
CHECK_GE(time, imu_tracker->time()); CHECK_GE(time, imu_tracker->time());
if (imu_data_.empty() || time < imu_data_.front().time) { if (imu_data_.empty() || time < imu_data_.front().time) {
// There is no IMU data until 'time', so we advance the ImuTracker and use // There is no IMU data until 'time', so we advance the ImuTracker and use
@ -211,11 +218,11 @@ void PoseExtrapolator::AdvanceImuTracker(const common::Time time,
} }
Eigen::Quaterniond PoseExtrapolator::ExtrapolateRotation( Eigen::Quaterniond PoseExtrapolator::ExtrapolateRotation(
const common::Time time) { const common::Time time, ImuTracker* const imu_tracker) const {
ImuTracker imu_tracker = *imu_tracker_; CHECK_GE(time, imu_tracker->time());
AdvanceImuTracker(time, &imu_tracker); AdvanceImuTracker(time, imu_tracker);
const Eigen::Quaterniond last_orientation = imu_tracker_->orientation(); const Eigen::Quaterniond last_orientation = imu_tracker_->orientation();
return last_orientation.inverse() * imu_tracker.orientation(); return last_orientation.inverse() * imu_tracker->orientation();
} }
Eigen::Vector3d PoseExtrapolator::ExtrapolateTranslation(common::Time time) { Eigen::Vector3d PoseExtrapolator::ExtrapolateTranslation(common::Time time) {

View File

@ -61,8 +61,9 @@ class PoseExtrapolator {
void UpdateVelocitiesFromPoses(); void UpdateVelocitiesFromPoses();
void TrimImuData(); void TrimImuData();
void TrimOdometryData(); void TrimOdometryData();
void AdvanceImuTracker(common::Time time, ImuTracker* imu_tracker); void AdvanceImuTracker(common::Time time, ImuTracker* imu_tracker) const;
Eigen::Quaterniond ExtrapolateRotation(common::Time time); Eigen::Quaterniond ExtrapolateRotation(common::Time time,
ImuTracker* imu_tracker) const;
Eigen::Vector3d ExtrapolateTranslation(common::Time time); Eigen::Vector3d ExtrapolateTranslation(common::Time time);
const common::Duration pose_queue_duration_; const common::Duration pose_queue_duration_;
@ -77,6 +78,8 @@ class PoseExtrapolator {
const double gravity_time_constant_; const double gravity_time_constant_;
std::deque<sensor::ImuData> imu_data_; std::deque<sensor::ImuData> imu_data_;
std::unique_ptr<ImuTracker> imu_tracker_; std::unique_ptr<ImuTracker> imu_tracker_;
std::unique_ptr<ImuTracker> odometry_imu_tracker_;
std::unique_ptr<ImuTracker> extrapolation_imu_tracker_;
std::deque<sensor::OdometryData> odometry_data_; std::deque<sensor::OdometryData> odometry_data_;
Eigen::Vector3d linear_velocity_from_odometry_ = Eigen::Vector3d::Zero(); Eigen::Vector3d linear_velocity_from_odometry_ = Eigen::Vector3d::Zero();