下面就详细讲解一下外推器是怎么推算位姿的。
先看一下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算出来的角速度。
首先再回忆一下前端的整个过程，首先来了一帧激光，就用这一帧激光建立一个子图，然后就是运动，运动到哪个位置就用外推器计算出来大概的位置，大概是用里程计去计算大概的位置。计算出来之后，我们拿着当前的激光去跟之前建立的子图做匹配，进一步匹配精细化我跟刚刚匹配的位姿。匹配得到了位姿之后，就将位姿放到外推器里面，同时在这个位姿上将新的激光插入到子图上，这就是我们的前端了
标签：cartographer,const,Eigen,pose,推测,tracker,imu,time,位姿 From： https://www.cnblogs.com/gary-guo/p/16608604.html