cartographer_pose_extrapolator
cartographer_pose_extrapolator
- 0.引言
- 1.添加观测
- 1.1. AddImuData
- 1.2. AddOdometryData
- 2.状态量的更新与校准AddPose
- 2.1. UpdateVelocitiesFromPoses
- 2.3.AdvanceImuTracker
- 2.3.1.不使用imu 或者 预测时间之前没有imu数据的情况
- 2.3.2.有 imu 数据的情况
- 2.4.修剪数据
- 3.位姿预测
- 3.1.ExtrapolatePose
- 3.2.EstimateGravityOrientation
- 3.3.ExtrapolatePosesWithGravity
0.引言
- 传感器数据处理:从LaserScan到TimedPointCloudOriginData
PoseExtrapolator 主要用于前端里程计:
- 进行畸变去除
- 点云处理时估计重力 EstimateGravityOrientation()
- 提供scanMatcher位姿的预测值 extrapolator_->ExtrapolatePose()
PoseExtrapolator主要思想: 采用匀速运动学模型预测未来时间time处的pose. 而匀速运动学模型的平移速度(线速度)是由
- 1.里程计或者
- 2.两次scanmatch结果的微分得到,并优先采用里程计的数据.
角速度是由
- 1.采用两次scan-match的航向进行微分获得
- 2.里程计
- 3.直接用IMU的角速度测量,优先级最高
私有变量:
- 姿态预测相关
imu_tracker_ 只在添加位姿时更新, 用于保存添加校位姿准时的姿态
odometry_imu_tracker_ 只在添加位姿时更新, 用于根据里程计数据计算线速度时姿态的预测
extrapolation_imu_tracker_ 只在添加位姿时更新, 用于位姿预测时的姿态预测 - 通过里程计计算的线速度与角速度
linear_velocity_from_odometry_ 只在添加里程计数据时更新, 用于位姿预测时的平移量预测
angular_velocity_from_odometry_ 只在添加里程计数据时更新, 用于不使用imu数据时的imu_tracker_的角速度的更新 - 通过pose计算的线速度与角速度
linear_velocity_from_poses_ 只在添加位姿时更新, 用于位姿预测时 不使用里程计数据时 平移量的预测
angular_velocity_from_poses_ 只在添加位姿时更新, 用于 不使用里程计数据时 的imu_tracker_的角速度的更新 - 传感器数据队列的个数
imu_date_ 队列数据的个数最少是1个
odometry_data_ 队列数据的个数最少是2个, 所以, odometry_data_.size() < 2 就意味着不使用里程计
timed_pose_queue_ 队列数据的个数最少是2个
数据结构:
// 保存一定时间内的pose
const common::Duration pose_queue_duration_; // 时间差 0.001s
struct TimedPose {common::Time time;transform::Rigid3d pose;
};
std::deque<TimedPose> timed_pose_queue_;
std::deque<sensor::ImuData> imu_data_;
struct OdometryData {common::Time time;transform::Rigid3d pose;
};
std::deque<sensor::OdometryData> odometry_data_;
1.添加观测
1.1. AddImuData
向imu数据队列中添加imu数据,并进行数据队列的修剪 TrimImuData();
1.2. AddOdometryData
向odom数据队列中添加odom数据,并进行数据队列的修剪 TrimOdometryData();
,并计算角速度与线速度.
从 odometry_data_ 里面取出最老的与最新的两帧的数据。
// 根据位姿队列中最后一个位姿 乘以 上次添加位姿时的姿态预测到time时刻的姿态变化量// 得到预测的 最新里程计数据时刻 tracking frame 在 local 坐标系下的姿态const Eigen::Quaterniond orientation_at_newest_odometry_time =timed_pose_queue_.back().pose.rotation() *ExtrapolateRotation(odometry_data_newest.time,odometry_imu_tracker_.get());// 将tracking frame的线速度进行旋转, 得到 local 坐标系下 tracking frame 的线速度linear_velocity_from_odometry_ =orientation_at_newest_odometry_time *linear_velocity_in_tracking_frame_at_newest_odometry_time;
2.状态量的更新与校准AddPose
将扫描匹配后的pose加入到pose队列中,计算线速度与角速度,并将imu_tracker_的状态更新到time时刻.
void PoseExtrapolator::AddPose(const comm主要[参考文章](https://blog.csdn.net/qq_42700518/article/details/110913070)
on::Time time,const transform::Rigid3d& pose)
- step 1 如果imu_tracker_没有初始化就先进行初始化
if (imu_tracker_ == nullptr) {common::Time tracker_start = time; if (!imu_data_.empty()) {tracker_start = std::min(tracker_start, imu_data_.front().time); } // imu_tracker_的初始化 imu_tracker_ =absl::make_unique<ImuTracker>(gravity_time_constant_, tracker_start); }
- step 2 在timed_pose_queue_中保存pose 并 修剪数据
timed_pose_queue_.push_back(TimedPose{time, pose});// 保持pose队列中第二个pose的时间要大于 time - pose_queue_duration_while (timed_pose_queue_.size() > 2 && // timed_pose_queue_最少是2个数据timed_pose_queue_[1].time <= time - pose_queue_duration_) {timed_pose_queue_.pop_front();}
2.1. UpdateVelocitiesFromPoses
根据timed_pose_queue_中的数据(包含刚新加入的pose)计算线速度与角速度.
2.3.AdvanceImuTracker
/*** @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 {// 将imu_tracker_更新到time时刻
AdvanceImuTracker(time, imu_tracker_.get());
2.3.1.不使用imu 或者 预测时间之前没有imu数据的情况
在time之前没有IMU数据, 因此我们推进ImuTracker, 并使用姿势和假重力产生的角速度来帮助2D稳定
step 1 预测当前时刻的姿态与重力方向
imu_tracker->Advance(time);
// 根据预测出的姿态变化量,预测旋转后的线性加速度的值gravity_vector_ = rotation.conjugate() * gravity_vector_;
为什么要对旋转求逆??
step 2 使用 假的重力数据对加速度的测量进行更新。
imu_tracker->AddImuLinearAccelerationObservation(Eigen::Vector3d::UnitZ());
// 指数滑动平均法 exponential moving average// Step: 1 求delta_t, delta_t初始时刻为infinity, 之后为time_-last_linear_acceleration_time_ const double delta_t =last_linear_acceleration_time_ > common::Time::min()? common::ToSeconds(time_ - last_linear_acceleration_time_): std::numeric_limits<double>::infinity(); last_linear_acceleration_time_ = time_;// Step: 2 求alpha, alpha=1-e^(-delta_t/10) // delta_t越大, alpha越大 const double alpha = 1. - std::exp(-delta_t / imu_gravity_time_constant_);// Step: 3 将之前的线加速度与当前传入的线加速度进行融合, 这里采用指数滑动平均法// 指数来确定权重, 因为有噪声的存在, 时间差越大, 当前的线性加速度的权重越大 // 这里的gravity_vector_改成线性加速度更清晰一些 gravity_vector_ =(1. - alpha) * gravity_vector_ + alpha * imu_linear_acceleration;// 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();
step 3 使用从pose 或 从odom 计算得到的角速度进行测量值的更新
imu_angular_velocity_ = imu_angular_velocity;
2.3.2.有 imu 数据的情况
- step 1 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); }
- step 2 利用 imu数据依次更新状态直至 当前时刻。
// 在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);
2.4.修剪数据
// pose队列更新了,之前imu及里程计数据已经过时了// 因为pose是匹配的结果,之前的imu及里程计数据是用于预测的,现在结果都有了,之前的用于预测的数据肯定不需要了TrimImuData();TrimOdometryData();// 用于根据里程计数据计算线速度时姿态的预测odometry_imu_tracker_ = absl::make_unique<ImuTracker>(*imu_tracker_);// 用于位姿预测时的姿态预测extrapolation_imu_tracker_ = absl::make_unique<ImuTracker>(*imu_tracker_);
3.位姿预测
3.1.ExtrapolatePose
预测得到time时刻 tracking frame 在 local 坐标系下的位姿
- ExtrapolateTranslation预测平移
- ExtrapolateRotation预测旋转
3.2.EstimateGravityOrientation
预测得到time时刻 tracking frame 在 local 坐标系下的姿态,利用 imu_tracker 进行预测:
ImuTracker imu_tracker = *imu_tracker_;// 使得 imu_tracker 预测到time时刻AdvanceImuTracker(time, &imu_tracker);
3.3.ExtrapolatePosesWithGravity
获取一段时间内的预测位姿的结果.
参考文章
cartographer_pose_extrapolator相关推荐
最新文章
- java baseentity_如何在JPA的BaseEntity中实现equals()和hashcode()方法?
- 相机成像原理_【亲子科学小实验】相机原理和小孔成像的秘密
- 虚拟机ubuntu/pytorch/python出现killed已杀死
- burpsuite csrf攻击_WEB安全之CSRF
- 重磅!新增一个假期!此地已正式发文!
- insertAdjacentHTML方法:在指定的地方插入html标签语句
- Ado.Net 实体类、数据连接类、数据访问类
- Flask之Flask实例有哪些参数
- Oracle使用IN 不能超过1000问题
- 磁盘过热导致 3D32B80D NIM thread blocked故障
- 《虚拟化与云计算》作者介绍
- 终端如何粘贴快捷键_linux shell实用快捷键
- GAMES101-现代计算机图形学入门-闫令琪——Lecture 22 Animation Cont 学习笔记【完结】
- java 短链跳转原理_短连接跳转的原理
- 众数和中位数的应用题_什么是众数和中位数
- 解决word文件由于扩展名不匹配问题
- E. Vowels(SOSdp的简单转化)
- 伟大的数学家们是在多大的时候改变的世界?
- 霍尼韦尔携手昕诺飞部署集成照明解决方案以改善住户体验
- oracle导入导出DMP文件【整个步骤】