cartographer-位姿推测部分
下面就详细讲解一下外推器是怎么推算位姿的。
先看一下PoseExtrapolator的实现：
解释一下它的功能：保持某个时间段内的位姿以估计线速度和角速度；用速度来推测运动；如果使用了IMU或者odometry则可以改善推测效果。
pose_extraplator.h
class PoseExtrapolator : public PoseExtrapolatorInterface { public: explicit PoseExtrapolator(common::Duration pose_queue_duration, double imu_gravity_time_constant); PoseExtrapolator(const PoseExtrapolator&) = delete; PoseExtrapolator& operator=(const PoseExtrapolator&) = delete; static std::unique_ptr<PoseExtrapolator> InitializeWithImu( common::Duration pose_queue_duration, double imu_gravity_time_constant, const sensor::ImuData& imu_data); // Returns the time of the last added pose or Time::min() if no pose was added // yet. common::Time GetLastPoseTime() const override; common::Time GetLastExtrapolatedTime() const override; void AddPose(common::Time time, const transform::Rigid3d& pose) override; void AddImuData(const sensor::ImuData& imu_data) override; void AddOdometryData(const sensor::OdometryData& odometry_data) override; transform::Rigid3d ExtrapolatePose(common::Time time) override; ExtrapolationResult ExtrapolatePosesWithGravity( const std::vector<common::Time>& times) override; // Returns the current gravity alignment estimate as a rotation from // the tracking frame into a gravity aligned frame. Eigen::Quaterniond EstimateGravityOrientation(common::Time time) override; private: void UpdateVelocitiesFromPoses(); void TrimImuData(); void TrimOdometryData(); void AdvanceImuTracker(common::Time time, ImuTracker* imu_tracker) const; Eigen::Quaterniond ExtrapolateRotation(common::Time time, ImuTracker* imu_tracker) const; Eigen::Vector3d ExtrapolateTranslation(common::Time time); // 保存一定时间内的pose const common::Duration pose_queue_duration_; struct TimedPose { common::Time time; transform::Rigid3d pose; }; std::deque<TimedPose> timed_pose_queue_; // 线速度有2种计算途径 Eigen::Vector3d linear_velocity_from_poses_ = Eigen::Vector3d::Zero(); Eigen::Vector3d angular_velocity_from_poses_ = Eigen::Vector3d::Zero(); const double gravity_time_constant_; std::deque<sensor::ImuData> imu_data_; std::unique_ptr<ImuTracker> imu_tracker_; // 保存与预测当前姿态 std::unique_ptr<ImuTracker> odometry_imu_tracker_; // 用于计算里程计的姿态的ImuTracker std::unique_ptr<ImuTracker> extrapolation_imu_tracker_; // 用于预测姿态的ImuTracker TimedPose cached_extrapolated_pose_; std::deque<sensor::OdometryData> odometry_data_; Eigen::Vector3d linear_velocity_from_odometry_ = Eigen::Vector3d::Zero(); Eigen::Vector3d angular_velocity_from_odometry_ = Eigen::Vector3d::Zero(); };
pose_extrapolator.cc
先看函数ExtraploatePose,其目的是预测得到time时刻 tracking_frame在local坐标系下的位姿
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) { // 预测tracking frame在local坐标系下time时刻的位置 const Eigen::Vector3d translation = ExtrapolateTranslation(time) + newest_timed_pose.pose.translation(); // 预测tracking frame在local坐标系下time时刻的姿态 const Eigen::Quaterniond rotation = newest_timed_pose.pose.rotation() * ExtrapolateRotation(time, extrapolation_imu_tracker_.get());//R(local->tracking_frame) * R(tracking_frame->tracing_frame) cached_extrapolated_pose_ = TimedPose{time, transform::Rigid3d{translation, rotation}}; } return cached_extrapolated_pose_.pose; }
下面进行逐行分析：
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();
预测tracking_frame在local坐标系下time时刻的位置；函数ExtapolateTranslation(time)，如下面很好理解
// 返回从最后一个位姿的时间 到time时刻 的tracking frame在local坐标系下的平移量 Eigen::Vector3d PoseExtrapolator::ExtrapolateTranslation(common::Time time) { const TimedPose& newest_timed_pose = timed_pose_queue_.back(); const double extrapolation_delta = common::ToSeconds(time - newest_timed_pose.time); // 使用tracking frame 在 local坐标系下的线速度 乘以时间 得到平移量的预测 if (odometry_data_.size() < 2) { return extrapolation_delta * linear_velocity_from_poses_; } // 如果不使用里程计就使用通过pose计算出的线速度 return extrapolation_delta * linear_velocity_from_odometry_; }
接着看姿态的预测，预测tracking frame在local坐标系下time时刻的姿态
const Eigen::Quaterniond rotation = newest_timed_pose.pose.rotation() * ExtrapolateRotation(time, extrapolation_imu_tracker_.get());//R(local->tracking_frame) * R(tracking_frame->tracing_frame)
进一步看函数： ExtrapolateRotation(time, extrapolation_imu_tracker_.get())；
// 计算从imu_tracker到time时刻的姿态变化量 Eigen::Quaterniond PoseExtrapolator::ExtrapolateRotation( const common::Time time, ImuTracker* const imu_tracker) const { CHECK_GE(time, imu_tracker->time()); // 更新imu_tracker的状态到time时刻 AdvanceImuTracker(time, imu_tracker); // 通过imu_tracker_获取上一次位姿校准时的姿态，这里的imu_tracker_是带下划线的，不是局部的那个imu_tracker const Eigen::Quaterniond last_orientation = imu_tracker_->orientation(); // 求取上一帧到当前时刻预测出的姿态变化量：上一帧姿态四元数的逆 乘以 当前时刻预测出来的姿态四元数 return last_orientation.inverse() * imu_tracker->orientation();//R(local->before).inverse() * R(local->current)=R(before->current) }
这里展开函数AdvanceImuTracker(time, imu_tracker);
/** * @brief 更新imu_tracker的状态, 并将imu_tracker的状态预测到time时刻 * * @param[in] time 要预测到的时刻 * @param[in] imu_tracker 给定的先验状态 */ void PoseExtrapolator::AdvanceImuTracker(const common::Time time, ImuTracker* const imu_tracker) const { // 检查指定时间是否大于等于 ImuTracker 的时间 CHECK_GE(time, imu_tracker->time()); // 不使用imu 或者 预测时间之前没有imu数据的情况 if (imu_data_.empty() || time < imu_data_.front().time) { // There is no IMU data until 'time', so we advance the ImuTracker and use // the angular velocities from poses and fake gravity to help 2D stability. // 在time之前没有IMU数据, 因此我们推进ImuTracker, 并使用姿势和假重力产生的角速度来帮助2D稳定 // 预测当前时刻的姿态与重力方向 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_); return; } // imu_tracker的时间比imu数据队列中第一个数据的时间早, 就先预测到imu数据队列中第一个数据的时间 if (imu_tracker->time() < imu_data_.front().time) { // Advance to the beginning of 'imu_data_'. imu_tracker->Advance(imu_data_.front().time); } // c++11: std::lower_bound() 是在区间内找到第一个大于等于 value 的值的位置并返回, 如果没找到就返回 end() 位置 // 在第四个参数位置可以自定义比较规则,在区域内查找第一个 **不符合** comp 规则的元素 // 在imu数据队列中找到第一个时间上 大于等于 imu_tracker->time() 的数据的索引 auto it = 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; }); // 然后依次对imu数据进行预测, 以及添加观测, 直到imu_data_的时间大于等于time截止 while (it != imu_data_.end() && it->time < time) { // 预测出当前时刻的姿态与重力方向 imu_tracker->Advance(it->time);//应该对传感器数据进行去噪声，检查跳变等等 // 根据线速度的观测,更新重力的方向,并根据重力的方向对上一时刻预测的姿态进行校准 imu_tracker->AddImuLinearAccelerationObservation(it->linear_acceleration); // 更新角速度观测 imu_tracker->AddImuAngularVelocityObservation(it->angular_velocity); ++it; } // 最后将imu_tracker的状态预测到time时刻 imu_tracker->Advance(time); }
下面重点解释：
先看函数
// 预测当前时刻的姿态与重力方向 imu_tracker->Advance(time);
展开之
/** * @brief 预测出time时刻的姿态与重力方向 * * @param[in] time 要预测的时刻 */ void ImuTracker::Advance(const common::Time time) { CHECK_LE(time_, time); const double delta_t = common::ToSeconds(time - time_); // 上一时刻的角速度乘以时间,得到当前时刻相对于上一时刻的预测的姿态变化量,再转换成四元数 const Eigen::Quaterniond rotation = transform::AngleAxisVectorToRotationQuaternion( Eigen::Vector3d(imu_angular_velocity_ * delta_t)); // 使用上一时刻的姿态 orientation_ 乘以姿态变化量, 得到当前时刻的预测出的姿态 orientation_ = (orientation_ * rotation).normalized(); // 根据预测出的姿态变化量,预测旋转后的线性加速度的值，车体的旋转方向，跟重力向量的旋转方向是反向的，比如车体向上仰a角度，重力向量是往反方向即俯方向旋转a角度，所以这里是共轭也就是逆，反方向 gravity_vector_ = rotation.conjugate() * gravity_vector_;//conjugate可以理解为求逆，R（before->current）的逆 * gravity_vector_(before) // 更新时间 time_ = time; }
time_为上一次我们advance到的时间；
重点关注gravity_vector_，它就是考虑重力对齐的关系；主要考虑的是imu航向角是否与水平面水平，如果不是，如果不是，就要将重力划分到imu坐标系中。
这个函数imu_tracker_Advance(time)就是更新朝向角，再更新一下重力向量的表示。
这里的gravity_vector_后面再进一步解释
回头再看函数
imu_tracker->AddImuLinearAccelerationObservation(Eigen::Vector3d::UnitZ());
由于这里没有imu数据，再进一步假设是在2D平面上运行，那么只有一个z方向上的重力向量Eigen::Vector3d::UnitZ().
进入到该函数
重点关注
// Change the 'orientation_' so that it agrees with the current // 'gravity_vector_'. // Step: 4 求得 线性加速度的值 与 由上一时刻姿态求出的线性加速度 间的旋转量 const Eigen::Quaterniond rotation = FromTwoVectors( gravity_vector_, orientation_.conjugate() * Eigen::Vector3d::UnitZ());//不理解？？？？ // Step: 5 使用这个旋转量来校准当前的姿态 orientation_ = (orientation_ * rotation).normalized();
FromTwoVectors就是从一个向量转到另一个向量。第一个向量是重力向量gravity_vector_。第二个向量是什么呢？是orientation_（四元数）共轭 也就是逆，。orientation_.conjugate() * Eigen::Vector3d::UnitZ()就是把这个四元数转换成向量的形式。orientation_是当前tracking_frame在local frame下的姿态，在这里我们只关注重力方向上的值，即z轴上的值，由于我们的tracking_frame的姿态在重力方向上的变化量与重力向量的方向是相反的（比如小车向上仰a角度，那么其实重力向量是向俯方向旋转a角度，所以这里要使用逆）；即求出当前姿态下对应的重力向量；而这个旋转就是指我们测量到的重力向量与我们计算的orientation_中得出的重力向量的一个变化量，有点类似于orientation通过计算（运动更新）得来，而gravity_vector_通过观测得来（观测更新）；通过二者的融合来进一步纠正航向的变化；即orientation_ = (orientation_ * rotation).normalized();
下面通过画图来解释
使用重力加速度来纠正我们估计的朝向。
怎么纠正呢，理论上来说，我们车体坐标系上的Z轴上的单位向量，乘以航向角的逆，也就是逆航向角进行旋转，应该是跟重量向量方向是重合的，如果不重合，那么就对航向角进行矫正，矫正的大小就是旋转后的Z轴与重力向量之间的旋转向量rotation。
这就是使用重力加速度来纠正估计航向的原理
CHECK_GT((orientation_ * gravity_vector_).z(), 0.);
CHECK_GT((orientation_ * gravity_vector_).normalized().z(), 0.99);
对于代码个人的理解是：由于航向的变化与重力向量的变化是反向的，而且变化量都是一样的，那二者的乘积，按理论值应该在z上为1.
imu器件测量的原理：重力向量为什么朝上，重力和器件之间作用力和反作用力，重力向下，测量值朝上。
当前函数的名字是ImuTracker::AddImuLinearAccelerationObservation 这个是IMU加入线性加速度观测，加入观测的目的是什么呢？纠正当前的状态。我们之前利用角速度，求出朝向角。然后求出重力朝向，重力朝向去矫正航向角。
前端：我们看完了从pose里面算出来的速度，再看从odom算出来的角速度。
首先再回忆一下前端的整个过程，首先来了一帧激光，就用这一帧激光建立一个子图，然后就是运动，运动到哪个位置就用外推器计算出来大概的位置，大概是用里程计去计算大概的位置。计算出来之后，我们拿着当前的激光去跟之前建立的子图做匹配，进一步匹配精细化我跟刚刚匹配的位姿。匹配得到了位姿之后，就将位姿放到外推器里面，同时在这个位姿上将新的激光插入到子图上，这就是我们的前端了
