From a8bd98680eefb493d7450fdae238a76dfe10b330 Mon Sep 17 00:00:00 2001 From: gaschler Date: Thu, 23 Nov 2017 12:27:26 +0100 Subject: [PATCH] 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. --- cartographer/mapping/pose_extrapolator.cc | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/cartographer/mapping/pose_extrapolator.cc b/cartographer/mapping/pose_extrapolator.cc index 82ca57b..4519c07 100644 --- a/cartographer/mapping/pose_extrapolator.cc +++ b/cartographer/mapping/pose_extrapolator.cc @@ -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(