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

ExtrapolatePose uses fewer transforms. (#694) 

ExtrapolatePose avoids conversion to Rigid3d and computes translation
and rotation directly, which is faster.
Per-point unwarping will call this function for every point, so we
optimize it.
master
gaschler 2017-11-23 12:27:26 +01:00 committed by Wally B. Feed
parent 41bb8be0fb
commit a8bd98680e
1 changed files with 6 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
cartographer/mapping/pose_extrapolator.cc
View File

@ -133,10 +133,12 @@ transform::Rigid3d PoseExtrapolator::ExtrapolatePose(const common::Time time) {
// TODO(whess): Keep the last extrapolated pose.
const TimedPose& newest_timed_pose = timed_pose_queue_.back();
CHECK_GE(time, newest_timed_pose.time);
return transform::Rigid3d::Translation(ExtrapolateTranslation(time)) *
newest_timed_pose.pose *
transform::Rigid3d::Rotation(
ExtrapolateRotation(time, extrapolation_imu_tracker_.get()));
const Eigen::Vector3d translation =
ExtrapolateTranslation(time) + newest_timed_pose.pose.translation();
const Eigen::Quaterniond rotation =
newest_timed_pose.pose.rotation() *
ExtrapolateRotation(time, extrapolation_imu_tracker_.get());
return transform::Rigid3d(translation, rotation);
}
Eigen::Quaterniond PoseExtrapolator::EstimateGravityOrientation(