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

Estimate angular velocities from odometry. (#458) 

If no IMU is available we will now use odometry to estimate
angular velocities if available instead of the last poses.

Fixes #453.
master
Wolfgang Hess 2017-08-16 16:55:39 +02:00 committed by GitHub
parent 0c6d6979c4
commit 9498cc90ca
2 changed files with 17 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
cartographer/mapping/pose_extrapolator.cc
View File

@ -90,22 +90,29 @@ void PoseExtrapolator::AddOdometryData(
odometry_data.time >= timed_pose_queue_.back().time);
odometry_data_.push_back(odometry_data);
TrimOdometryData();
if (odometry_data_.size() < 2 || timed_pose_queue_.empty()) {
if (odometry_data_.size() < 2) {
return;
}
// TODO(whess): Improve by using more than just the last two odometry poses.
// TODO(whess): Use odometry to predict orientation if there is no IMU.
// 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 double odometry_time_delta =
common::ToSeconds(odometry_data_older.time - odometry_data_newer.time);
const transform::Rigid3d odometry_pose_delta =
odometry_data_newer.pose.inverse() * odometry_data_older.pose;
angular_velocity_from_odometry_ =
transform::RotationQuaternionToAngleAxisVector(
odometry_pose_delta.rotation()) /
odometry_time_delta;
if (timed_pose_queue_.empty()) {
return;
}
const Eigen::Vector3d
linear_velocity_in_tracking_frame_at_newer_odometry_time =
(odometry_data_newer.pose.inverse() * odometry_data_older.pose)
.translation() /
common::ToSeconds(odometry_data_older.time -
odometry_data_newer.time);
odometry_pose_delta.translation() / odometry_time_delta;
const Eigen::Quaterniond orientation_at_newer_odometry_time =
timed_pose_queue_.back().pose.rotation() *
ExtrapolateRotation(odometry_data_newer.time);
@ -171,7 +178,9 @@ void PoseExtrapolator::AdvanceImuTracker(const common::Time time,
// the angular velocities from poses and fake gravity to help 2D stability.
imu_tracker->Advance(time);
imu_tracker->AddImuLinearAccelerationObservation(Eigen::Vector3d::UnitZ());
imu_tracker->AddImuAngularVelocityObservation(angular_velocity_from_poses_);
imu_tracker->AddImuAngularVelocityObservation(
odometry_data_.size() < 2 ? angular_velocity_from_poses_
: angular_velocity_from_odometry_);
return;
}
if (imu_tracker->time() < imu_data_.front().time) {

1
cartographer/mapping/pose_extrapolator.h
View File

@ -81,6 +81,7 @@ class PoseExtrapolator {
std::deque<sensor::OdometryData> odometry_data_;
Eigen::Vector3d linear_velocity_from_odometry_ = Eigen::Vector3d::Zero();
Eigen::Vector3d angular_velocity_from_odometry_ = Eigen::Vector3d::Zero();
};
} // namespace mapping