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
parent
41bb8be0fb
commit
a8bd98680e
|
@ -133,10 +133,12 @@ transform::Rigid3d PoseExtrapolator::ExtrapolatePose(const common::Time time) {
|
||||||
// TODO(whess): Keep the last extrapolated pose.
|
// TODO(whess): Keep the last extrapolated pose.
|
||||||
const TimedPose& newest_timed_pose = timed_pose_queue_.back();
|
const TimedPose& newest_timed_pose = timed_pose_queue_.back();
|
||||||
CHECK_GE(time, newest_timed_pose.time);
|
CHECK_GE(time, newest_timed_pose.time);
|
||||||
return transform::Rigid3d::Translation(ExtrapolateTranslation(time)) *
|
const Eigen::Vector3d translation =
|
||||||
newest_timed_pose.pose *
|
ExtrapolateTranslation(time) + newest_timed_pose.pose.translation();
|
||||||
transform::Rigid3d::Rotation(
|
const Eigen::Quaterniond rotation =
|
||||||
ExtrapolateRotation(time, extrapolation_imu_tracker_.get()));
|
newest_timed_pose.pose.rotation() *
|
||||||
|
ExtrapolateRotation(time, extrapolation_imu_tracker_.get());
|
||||||
|
return transform::Rigid3d(translation, rotation);
|
||||||
}
|
}
|
||||||
|
|
||||||
Eigen::Quaterniond PoseExtrapolator::EstimateGravityOrientation(
|
Eigen::Quaterniond PoseExtrapolator::EstimateGravityOrientation(
|
||||||
|
|
Loading…
Reference in New Issue