本次阅读的源码为 release-1.0 版本的代码

https://github.com/googlecartographer/cartographer_ros/tree/release-1.0

https://github.com/googlecartographer/cartographer/tree/release-1.0

也可以下载我上传的全套工作空间的代码，包括 protobuf, cartographer, cartographer_ros, ceres,

https://download.csdn.net/download/tiancailx/11224156，现在上传资源自己不能选下载需要的积分。。。完全靠系统。

pose_extrapolator.h

std::unique_ptr<PoseExtrapolator> PoseExtrapolator::InitializeWithImu(const common::Duration pose_queue_duration,const double imu_gravity_time_constant, const sensor::ImuData& imu_data) {auto extrapolator = common::make_unique<PoseExtrapolator>(pose_queue_duration, imu_gravity_time_constant);extrapolator->AddImuData(imu_data);extrapolator->imu_tracker_ =common::make_unique<ImuTracker>(imu_gravity_time_constant, imu_data.time);extrapolator->imu_tracker_->AddImuLinearAccelerationObservation(imu_data.linear_acceleration);extrapolator->imu_tracker_->AddImuAngularVelocityObservation(imu_data.angular_velocity);extrapolator->imu_tracker_->Advance(imu_data.time);extrapolator->AddPose(imu_data.time,transform::Rigid3d::Rotation(extrapolator->imu_tracker_->orientation()));return extrapolator;
}void PoseExtrapolator::AddImuData(const sensor::ImuData& imu_data) {CHECK(timed_pose_queue_.empty() ||imu_data.time >= timed_pose_queue_.back().time);imu_data_.push_back(imu_data);TrimImuData();
}// 删除第一个数据（老数据），直到第一个imu数据的时间大于timed_pose_queue_的最后一个时间
void PoseExtrapolator::TrimImuData() {while (imu_data_.size() > 1 && !timed_pose_queue_.empty() &&imu_data_[1].time <= timed_pose_queue_.back().time) {imu_data_.pop_front();}
}// 转到imu_tracker.hvoid PoseExtrapolator::AddPose(const common::Time time,const transform::Rigid3d& pose) {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_ =common::make_unique<ImuTracker>(gravity_time_constant_, tracker_start);}timed_pose_queue_.push_back(TimedPose{time, pose});while (timed_pose_queue_.size() > 2 &&timed_pose_queue_[1].time <= time - pose_queue_duration_) {timed_pose_queue_.pop_front();}UpdateVelocitiesFromPoses();AdvanceImuTracker(time, imu_tracker_.get());TrimImuData();TrimOdometryData();odometry_imu_tracker_ = common::make_unique<ImuTracker>(*imu_tracker_);extrapolation_imu_tracker_ = common::make_unique<ImuTracker>(*imu_tracker_);
}

imu_tracker.h

ImuTracker::ImuTracker(const double imu_gravity_time_constant,const common::Time time): imu_gravity_time_constant_(imu_gravity_time_constant),time_(time),last_linear_acceleration_time_(common::Time::min()),orientation_(Eigen::Quaterniond::Identity()),gravity_vector_(Eigen::Vector3d::UnitZ()),imu_angular_velocity_(Eigen::Vector3d::Zero()) {}void ImuTracker::AddImuLinearAccelerationObservation(const Eigen::Vector3d& imu_linear_acceleration) {// Update the 'gravity_vector_' with an exponential moving average using the// 'imu_gravity_time_constant'.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_;const double alpha = 1. - std::exp(-delta_t / imu_gravity_time_constant_);gravity_vector_ =(1. - alpha) * gravity_vector_ + alpha * imu_linear_acceleration;// Change the 'orientation_' so that it agrees with the current// 'gravity_vector_'.const Eigen::Quaterniond rotation = Eigen::Quaterniond::FromTwoVectors(gravity_vector_, orientation_.conjugate() * Eigen::Vector3d::UnitZ());orientation_ = (orientation_ * rotation).normalized();CHECK_GT((orientation_ * gravity_vector_).z(), 0.);CHECK_GT((orientation_ * gravity_vector_).normalized().z(), 0.99);
}void ImuTracker::AddImuAngularVelocityObservation(const Eigen::Vector3d& imu_angular_velocity) {imu_angular_velocity_ = imu_angular_velocity;
}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_ * rotation).normalized();gravity_vector_ = rotation.conjugate() * gravity_vector_;time_ = time;
}

cartographer探秘第四章之代码解析（七）--- pose_extrapolator.h 及 imu_tracker.h相关推荐

学习笔记-第十四章恶意代码分析实战
第十四章恶意代码的网络特征 1.网络应对措施. 网络行为的基本属性包括IP地址,TCP端口,以及流量内容等,网络和安全设备可以利用它们,来提供网络应对措施.根据IP地址和端口,防火墙和路由器可以限 ...
第四章 PX4-Pixhawk-MPU6000传感器驱动解析
第四章MPU6000传感器驱动解析 Mpu6000是一个3轴加速度和3轴陀螺仪传感器,这一章节我们将对MPU6000这个传感器进行解析,依照这个解析步骤同样可以对L3GD20(3轴陀螺仪).HMC58 ...
第四章MPU6000传感器驱动解析
版权声明:本文为博主原创文章,未经博主允许不得转载. 第四章MPU6000传感器驱动解析 Mpu6000是一个3轴加速度和3轴陀螺仪传感器,这一章节我们将对MPU6000这个传感器进行解析,依照这个解 ...
CodeCombat代码全记录（Python学习利器）--安息之云山峰（第四章）代码1
我们已经有了前三章的基础了,到了第四章,你会发现提示少之又少了.这时,我们大都要靠自己了!!!加油!!如果你没有思路,请仔细看下装备的说明,毕竟到了这关,你需要购买好多其他的新装备,而新装备中存在了新 ...
Javascript第四章匿名函数第七课
匿名函数的作用: 1.用于回调 2.一次性执行函数 Javascript第四章定义函数的形式.回调函数第五课 https://blog.csdn.net/qq_30225725/article/det ...
Paxos算法之旅（四）zookeeper代码解析--转载
ZooKeeper是近期比较热门的一个类Paxos实现.也是一个逐渐得到广泛应用的开源的分布式锁服务实现.被认为是Chubby的开源版,虽然具体实现有很多差异.ZooKeeper概要的介绍可以看官方文 ...
一个基于JRTPLIB的轻量级RTSP客户端(myRTSPClient)——收流篇：（四）example代码解析...
--------------------更新2018.08.20------------------- 添加http_tunnel_example.cpp作为RtspOverHttp示例程序. --- ...
视觉SLAM十四讲CH10代码解析及课后习题详解
g2o_viewer问题解决在进行位姿图优化时候,如果出现g2o_viewer: command not found,说明你的g2o_viewer并没有安装上,打开你之前安装的g2o文件夹,打开bi ...
视觉SLAM十四讲CH8代码解析及课后习题详解
第一版的代码: direct_semidense.cpp #include <iostream> #include <fstream> #include <list> ...
CE教程第四章《代码寻找》
步骤 5: 代码寻找 (密码=888899) 有时一些东西的保存位置在你重新开始游戏时会改变,甚至是在你玩的时候也会变,在这种情况下,你用2步仍然能做出可以用的内存列表. 在这一步会说明怎样用寻找代 ...

cartographer探秘第四章之代码解析（七）--- pose_extrapolator.h 及 imu_tracker.h

pose_extrapolator.h

imu_tracker.h

cartographer探秘第四章之代码解析（七）--- pose_extrapolator.h 及 imu_tracker.h相关推荐

最新文章

热门文章