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
parent
978544eca4
commit
8854aaae9e
|
@ -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) {
|
||||||
|
|
|
@ -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();
|
||||||
|
|
Loading…
Reference in New Issue