这里有个感觉,就是百川汇海。即IMU数据和相机的消息数据都汇集到msf_core进行处理。接上一篇,

1. 查看IMUHandler_ROS::IMUCallback和IMUHandler_ROS::StateCallback回调函数。

MUHandler_ROS::IMUCallback,传入的消息sensor_msgs::ImuConstPtr。

 void IMUCallback(const sensor_msgs::ImuConstPtr & msg)
{
static int lastseq = constants::INVALID_SEQUENCE;
if (static_cast<int>(msg->header.seq) != lastseq +
&& lastseq != constants::INVALID_SEQUENCE) {
MSF_WARN_STREAM(
"msf_core: imu message drop curr seq:" << msg->header.seq
<< " expected: " << lastseq + );
}
lastseq = msg->header.seq; msf_core::Vector3 linacc;
linacc << msg->linear_acceleration.x, msg->linear_acceleration.y, msg
->linear_acceleration.z; msf_core::Vector3 angvel;
angvel << msg->angular_velocity.x, msg->angular_velocity.y, msg
->angular_velocity.z; this->ProcessIMU(linacc, angvel, msg->header.stamp.toSec(),
msg->header.seq);
}

IMUHandler_ROS::StateCallback,传入的参数sensor_fusion_comm::ExtEkfConstPtr,这个需要理解一下。

 void StateCallback(const sensor_fusion_comm::ExtEkfConstPtr & msg)
{
static_cast<MSF_SensorManagerROS<EKFState_T>&>(this->manager_)
.SetHLControllerStateBuffer(*msg); // Get the imu values.
msf_core::Vector3 linacc;
linacc << msg->linear_acceleration.x, msg->linear_acceleration.y, msg
->linear_acceleration.z; msf_core::Vector3 angvel;
angvel << msg->angular_velocity.x, msg->angular_velocity.y, msg
->angular_velocity.z; int32_t flag = msg->flag;
// Make sure we tell the HL to ignore if data playback is on.
if (this->manager_.GetDataPlaybackStatus())
flag = sensor_fusion_comm::ExtEkf::ignore_state; bool isnumeric = true;
if (flag == sensor_fusion_comm::ExtEkf::current_state) {
isnumeric = CheckForNumeric(
Eigen::Map<const Eigen::Matrix<float, , > >(msg->state.data()),
"before prediction p,v,q");
} // Get the propagated states.
msf_core::Vector3 p, v;
msf_core::Quaternion q; p = Eigen::Matrix<double, , >(msg->state[], msg->state[],
msg->state[]);
v = Eigen::Matrix<double, , >(msg->state[], msg->state[],
msg->state[]);
q = Eigen::Quaternion<double>(msg->state[], msg->state[], msg->state[],
msg->state[]);
q.normalize(); bool is_already_propagated = false;
if (flag == sensor_fusion_comm::ExtEkf::current_state && isnumeric) {
is_already_propagated = true;
} this->ProcessState(linacc, angvel, p, v, q, is_already_propagated,
msg->header.stamp.toSec(), msg->header.seq);
}

查看IMUHandler_ROS类的父类IMUHandler的ProcessIMU和ProcessState方法,如下:

 void ProcessIMU(const msf_core::Vector3& linear_acceleration,
const msf_core::Vector3& angular_velocity,
const double& msg_stamp, size_t msg_seq) {
core_->ProcessIMU(linear_acceleration, angular_velocity, msg_stamp,
msg_seq);
}
void ProcessState(const msf_core::Vector3& linear_acceleration,
const msf_core::Vector3& angular_velocity,
const msf_core::Vector3& p, const msf_core::Vector3& v,
const msf_core::Quaternion& q, bool is_already_propagated,
const double& msg_stamp, size_t msg_seq) {
core_->ProcessExternallyPropagatedState(linear_acceleration,
angular_velocity, p, v, q,
is_already_propagated,
msg_stamp, msg_seq);
}

可以发现对应了msf_core_的ProcessIMU和ProcessExternallyPropagatedState方法。

2. 查看PoseSensorHandler::MeasurementCallback回调函数。注意,在构造函数中挂载了三个不同的MeasurementCallback函数。

geometry_msgs::PoseWithCovarianceStamped,geometry_msgs::TransformStamped,geometry_msgs::PoseStamped三种消息类型。

 template<typename MEASUREMENT_TYPE, typename MANAGER_TYPE>
void PoseSensorHandler<MEASUREMENT_TYPE, MANAGER_TYPE>::MeasurementCallback(
const geometry_msgs::PoseWithCovarianceStampedConstPtr & msg)
{
this->SequenceWatchDog(msg->header.seq,
subPoseWithCovarianceStamped_.getTopic());
MSF_INFO_STREAM_ONCE(
"*** pose sensor got first measurement from topic "
<< this->topic_namespace_ << "/"
<< subPoseWithCovarianceStamped_.getTopic() << " ***");
ProcessPoseMeasurement(msg);//注意
}

查看 ProcessPoseMeasurement(msg)函数:

 template<typename MEASUREMENT_TYPE, typename MANAGER_TYPE>
void PoseSensorHandler<MEASUREMENT_TYPE, MANAGER_TYPE>::ProcessPoseMeasurement(const geometry_msgs::PoseWithCovarianceStampedConstPtr & msg)
{
received_first_measurement_ = true; // Get the fixed states.
int fixedstates = ;
static_assert(msf_updates::EKFState::nStateVarsAtCompileTime < , "Your state "
"has more than 32 variables. The code needs to be changed here to have a "
"larger variable to mark the fixed_states");
// Do not exceed the 32 bits of int. // Get all the fixed states and set flag bits.
MANAGER_TYPE* mngr = dynamic_cast<MANAGER_TYPE*>(&manager_); // TODO(acmarkus): if we have multiple sensor handlers, they all share the same dynparams,
// which me maybe don't want. E.g. if we have this for multiple AR Markers, we
// may want to keep one fix --> move this to fixed parameters? Could be handled
// with parameter namespace then.
if (mngr) {
if (mngr->Getcfg().pose_fixed_scale) {
fixedstates |= << MEASUREMENT_TYPE::AuxState::L;
}
if (mngr->Getcfg().pose_fixed_p_ic) {
fixedstates |= << MEASUREMENT_TYPE::AuxState::p_ic;
}
if (mngr->Getcfg().pose_fixed_q_ic) {
fixedstates |= << MEASUREMENT_TYPE::AuxState::q_ic;
}
if (mngr->Getcfg().pose_fixed_p_wv) {
fixedstates |= << MEASUREMENT_TYPE::AuxState::p_wv;
}
if (mngr->Getcfg().pose_fixed_q_wv) {
fixedstates |= << MEASUREMENT_TYPE::AuxState::q_wv;
}
} shared_ptr<MEASUREMENT_TYPE> meas(new MEASUREMENT_TYPE(
n_zp_, n_zq_, measurement_world_sensor_, use_fixed_covariance_,
provides_absolute_measurements_, this->sensorID,
enable_mah_outlier_rejection_, mah_threshold_, fixedstates, distorter_)); meas->MakeFromSensorReading(msg, msg->header.stamp.toSec() - delay_); z_p_ = meas->z_p_; //store this for the init procedure
z_q_ = meas->z_q_; this->manager_.msf_core_->AddMeasurement(meas);
}

这里调用了this->manager_.msf_core_->AddMeasurement(meas),查看AddMeasurement方法。

3.以上,最终对应于MSF_Core类的三个函数,即

ProcessIMU、ProcessExternallyPropagatedState、AddMeasurement。

4.MSF_Core类,MSF_core类负责汇集IMU消息和位姿观测值,同时实现了状态预测,而msf_updates::pose_measurement::PoseMeasurement<>实现了状态的更新。

这个在分析MSF_Core三个方法的时候再说明。

Ethzasl MSF源码阅读(2):百川汇海的更多相关文章

  1. Ethzasl MSF源码阅读(1):程序入口和主题订阅

    关于IMU融合知乎上的一篇问答:有哪些开源项目是关于单目+imu做slam的? Ethz的Stephen Weiss的工作,是一个IMU松耦合的方法. 1.程序入口:ethzasl_msf\msf_u ...

  2. Ethzasl MSF源码阅读(3):MSF_Core和PoseMeasurement

    1.MSF_Core的三个函数:ProcessIMU.ProcessExternallyPropagatedState和AddMeasurement MSF_Core维护了状态队列和观测值队列,这里需 ...

  3. 【原】FMDB源码阅读(三)

    [原]FMDB源码阅读(三) 本文转载请注明出处 —— polobymulberry-博客园 1. 前言 FMDB比较优秀的地方就在于对多线程的处理.所以这一篇主要是研究FMDB的多线程处理的实现.而 ...

  4. 【原】FMDB源码阅读(二)

    [原]FMDB源码阅读(二) 本文转载请注明出处 -- polobymulberry-博客园 1. 前言 上一篇只是简单地过了一下FMDB一个简单例子的基本流程,并没有涉及到FMDB的所有方方面面,比 ...

  5. 【原】FMDB源码阅读(一)

    [原]FMDB源码阅读(一) 本文转载请注明出处 —— polobymulberry-博客园 1. 前言 说实话,之前的SDWebImage和AFNetworking这两个组件我还是使用过的,但是对于 ...

  6. 【原】AFNetworking源码阅读(六)

    [原]AFNetworking源码阅读(六) 本文转载请注明出处 —— polobymulberry-博客园 1. 前言 这一篇的想讲的,一个就是分析一下AFSecurityPolicy文件,看看AF ...

  7. 【原】AFNetworking源码阅读(五)

    [原]AFNetworking源码阅读(五) 本文转载请注明出处 —— polobymulberry-博客园 1. 前言 上一篇中提及到了Multipart Request的构建方法- [AFHTTP ...

  8. 【原】AFNetworking源码阅读(四)

    [原]AFNetworking源码阅读(四) 本文转载请注明出处 —— polobymulberry-博客园 1. 前言 上一篇还遗留了很多问题,包括AFURLSessionManagerTaskDe ...

  9. 【原】AFNetworking源码阅读(三)

    [原]AFNetworking源码阅读(三) 本文转载请注明出处 —— polobymulberry-博客园 1. 前言 上一篇的话,主要是讲了如何通过构建一个request来生成一个data tas ...

随机推荐

  1. 基于Vue element-ui实现支持多级纵向动态表头的仿表格布局

    [本文出自天外归云的博客园] 需求图示如下,多级纵向动态表头表格: 我的思路是用element-ui的layout实现,做出一个仿造表格,能够支持动态的.多级的.纵向的表头: <template ...

  2. Linux 安装Zookeeper集群

    1.解压,配置环境变量 export ZOOKEEPER_HOME=/usr/local/zkexport PATH=.:$HADOOP_HOME/bin:$ZOOKEEPER_HOME/bin:$J ...

  3. Java多线程系列——从菜鸟到入门

    持续更新系列. 参考自Java多线程系列目录(共43篇).<Java并发编程实战>.<实战Java高并发程序设计>.<Java并发编程的艺术>. 基础 Java多线 ...

  4. Zookeeper —— 一致性协议

    一致性协议 为了解决分布式系统中存在的一致性问题,提出了一些经典的一致性协议和算法. 其中著名的有:二阶段提交协议.三阶段提交协议和 Paxos 算法. 2PC 与 3PC 2PC 2pc(Two-P ...

  5. mongoDB用法整理

    1. mongoDB UI工具, Studio 3T,用Non_Commercial的版本就足够. 2. 查询某字段长度大于特定值的 db.test.find({ F_DAQDATA: { $type ...

  6. go语言调用append之后是否重新分配内存?

    查看tidb源代码:::util/charset/charset.go,下面有段代码: // GetAllCharsets gets all charset descriptions in the l ...

  7. kafka性能测试

    参考阿里中间件团队博客的博文 Kafka vs RocketMQ——多Topic对性能稳定性的影响 使用的测试工具为Jmeter

  8. HAWQ配置之HDFS HA

    一.在ambari管理界面启用HDFS HA 在ambari中这步很简单,在所有安装的服务都正常之后,在HDFS的服务界面中,点击下拉菜单“Actions”,选择启用HDFS HA项 “Enable ...

  9. 3D Object Classification With Point Convolution —— 点云卷积网络

    今天刚刚得到消息,之前投给IROS 2017的文章收录了.很久很久没有写过博客,今天正好借这个机会来谈谈点云卷积网络的一些细节. 1.点云与三维表达 三维数据后者说空间数据有很多种表达方式,比如:RG ...

  10. Ubuntu 14.04 LTS 系统空间不足,输入密码后,无法进入桌面的解决办法

    问题场景:系统空间不足,在没有清理空间的情况下,重启机器,可能会出现,输入密码后,无法进入桌面的现象. 如何解决? 解决方法:以访客身份登录系统,可以看到你的ip地址,然后以ssh的方式登录你的主机, ...