本文主要目的有二:
- 弄清extrapolator的涉及到的运动学变量;
- 弄清odom、imu、scan match对应的数据在pose extrapolator中的对应作用。
PoseExtrapolator::ExtrapolatePose
推算当前给定时间 time
对应的位姿(平移+旋转)。包括调用PoseExtrapolator::ExtrapolateTranslation
来计算平移增量以及调用PoseExtrapolator::ExtrapolateRotation
来计算旋转增量
transform::Rigid3d PoseExtrapolator::ExtrapolatePose(const common::Time time) {const TimedPose& newest_timed_pose = timed_pose_queue_.back();CHECK_GE(time, newest_timed_pose.time);if (cached_extrapolated_pose_.time != time) {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());cached_extrapolated_pose_ =TimedPose{time, transform::Rigid3d{translation, rotation}};}return cached_extrapolated_pose_.pose;
}
PoseExtrapolator::ExtrapolateTranslation
主要就是odometry_data_
队列的值足够的时候,使用odom
推算的linear_velocity_
线性速度;否则根据历史节点的位姿来推算linear_velocity_
线性速度。然后根据当前时间和位姿队列中最新位姿的时间差,计算这段时间内的平移增量。
Eigen::Vector3d PoseExtrapolator::ExtrapolateTranslation(common::Time time) {const TimedPose& newest_timed_pose &#61; timed_pose_queue_.back();const double extrapolation_delta &#61;common::ToSeconds(time - newest_timed_pose.time);if (odometry_data_.size() < 2) {return extrapolation_delta * linear_velocity_from_poses_;}return extrapolation_delta * linear_velocity_from_odometry_;
}
PoseExtrapolator::ExtrapolateRotation
Eigen::Quaterniond PoseExtrapolator::ExtrapolateRotation(const common::Time time, ImuTracker* const imu_tracker) const {CHECK_GE(time, imu_tracker->time());AdvanceImuTracker(time, imu_tracker);const Eigen::Quaterniond last_orientation &#61; imu_tracker_->orientation();return last_orientation.inverse() * imu_tracker->orientation();
}
姿态外推&#xff0c;需要调用函数AdvanceImuTracker
&#xff08;更新imu跟踪器&#xff09;。
void PoseExtrapolator::AdvanceImuTracker(const common::Time time,ImuTracker* const imu_tracker) const {CHECK_GE(time, imu_tracker->time());if (imu_data_.empty() || time < imu_data_.front().time) {imu_tracker->Advance(time);imu_tracker->AddImuLinearAccelerationObservation(Eigen::Vector3d::UnitZ());imu_tracker->AddImuAngularVelocityObservation(odometry_data_.size() < 2 ? angular_velocity_from_poses_: angular_velocity_from_odometry_);LOG(INFO) << "There is no IMU data until &#39;time&#39;: "<< time << " s" << "odometry_data_.size() is: "<<odometry_data_.size();return;}if (imu_tracker->time() < imu_data_.front().time) {imu_tracker->Advance(imu_data_.front().time);}auto it &#61; std::lower_bound(imu_data_.begin(), imu_data_.end(), imu_tracker->time(),[](const sensor::ImuData& imu_data, const common::Time& time) {return imu_data.time < time;});while (it !&#61; imu_data_.end() && it->time < time) { imu_tracker->Advance(it->time);imu_tracker->AddImuLinearAccelerationObservation(it->linear_acceleration);imu_tracker->AddImuAngularVelocityObservation(it->angular_velocity);&#43;&#43;it;}imu_tracker->Advance(time);
}