Ethzasl MSF源码阅读(2)：百川汇海

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

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

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

 1 void IMUCallback(const sensor_msgs::ImuConstPtr & msg) 
 2 {
 3     static int lastseq = constants::INVALID_SEQUENCE;
 4     if (static_cast<int>(msg->header.seq) != lastseq + 1
 5         && lastseq != constants::INVALID_SEQUENCE) {
 6       MSF_WARN_STREAM(
 7           "msf_core: imu message drop curr seq:" << msg->header.seq
 8               << " expected: " << lastseq + 1);
 9     }
10     lastseq = msg->header.seq;
11 
12     msf_core::Vector3 linacc;
13     linacc << msg->linear_acceleration.x, msg->linear_acceleration.y, msg
14         ->linear_acceleration.z;
15 
16     msf_core::Vector3 angvel;
17     angvel << msg->angular_velocity.x, msg->angular_velocity.y, msg
18         ->angular_velocity.z;
19 
20     this->ProcessIMU(linacc, angvel, msg->header.stamp.toSec(),
21                       msg->header.seq);
22   }

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

 1 void StateCallback(const sensor_fusion_comm::ExtEkfConstPtr & msg) 
 2 {
 3     static_cast<MSF_SensorManagerROS<EKFState_T>&>(this->manager_)
 4         .SetHLControllerStateBuffer(*msg);
 5 
 6     // Get the imu values.
 7     msf_core::Vector3 linacc;
 8     linacc << msg->linear_acceleration.x, msg->linear_acceleration.y, msg
 9         ->linear_acceleration.z;
10 
11     msf_core::Vector3 angvel;
12     angvel << msg->angular_velocity.x, msg->angular_velocity.y, msg
13         ->angular_velocity.z;
14 
15     int32_t flag = msg->flag;
16     // Make sure we tell the HL to ignore if data playback is on.
17     if (this->manager_.GetDataPlaybackStatus())
18       flag = sensor_fusion_comm::ExtEkf::ignore_state;
19 
20     bool isnumeric = true;
21     if (flag == sensor_fusion_comm::ExtEkf::current_state) {
22       isnumeric = CheckForNumeric(
23           Eigen::Map<const Eigen::Matrix<float, 10, 1> >(msg->state.data()),
24           "before prediction p,v,q");
25     }
26 
27     // Get the propagated states.
28     msf_core::Vector3 p, v;
29     msf_core::Quaternion q;
30 
31     p = Eigen::Matrix<double, 3, 1>(msg->state[0], msg->state[1],
32                                     msg->state[2]);
33     v = Eigen::Matrix<double, 3, 1>(msg->state[3], msg->state[4],
34                                     msg->state[5]);
35     q = Eigen::Quaternion<double>(msg->state[6], msg->state[7], msg->state[8],
36                                   msg->state[9]);
37     q.normalize();
38 
39     bool is_already_propagated = false;
40     if (flag == sensor_fusion_comm::ExtEkf::current_state && isnumeric) {
41       is_already_propagated = true;
42     }
43 
44     this->ProcessState(linacc, angvel, p, v, q, is_already_propagated,
45                         msg->header.stamp.toSec(), msg->header.seq);
46   }

查看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三种消息类型。

 1 template<typename MEASUREMENT_TYPE, typename MANAGER_TYPE>
 2 void PoseSensorHandler<MEASUREMENT_TYPE, MANAGER_TYPE>::MeasurementCallback(
 3     const geometry_msgs::PoseWithCovarianceStampedConstPtr & msg) 
 4 {
 5   this->SequenceWatchDog(msg->header.seq,
 6                          subPoseWithCovarianceStamped_.getTopic());
 7   MSF_INFO_STREAM_ONCE(
 8       "*** pose sensor got first measurement from topic "
 9           << this->topic_namespace_ << "/"
10           << subPoseWithCovarianceStamped_.getTopic() << " ***");
11   ProcessPoseMeasurement(msg);//注意
12 }

查看 ProcessPoseMeasurement(msg)函数：

 1 template<typename MEASUREMENT_TYPE, typename MANAGER_TYPE>
 2 void PoseSensorHandler<MEASUREMENT_TYPE, MANAGER_TYPE>::ProcessPoseMeasurement(const geometry_msgs::PoseWithCovarianceStampedConstPtr & msg) 
 3 {
 4   received_first_measurement_ = true;
 5 
 6   // Get the fixed states.
 7   int fixedstates = 0;
 8   static_assert(msf_updates::EKFState::nStateVarsAtCompileTime < 32, "Your state "
 9       "has more than 32 variables. The code needs to be changed here to have a "
10       "larger variable to mark the fixed_states");
11   // Do not exceed the 32 bits of int.
12 
13   // Get all the fixed states and set flag bits.
14   MANAGER_TYPE* mngr = dynamic_cast<MANAGER_TYPE*>(&manager_);
15 
16   // TODO(acmarkus): if we have multiple sensor handlers, they all share the same dynparams,
17   // which me maybe don't want. E.g. if we have this for multiple AR Markers, we
18   // may want to keep one fix --> move this to fixed parameters? Could be handled
19   // with parameter namespace then.
20   if (mngr) {
21     if (mngr->Getcfg().pose_fixed_scale) {
22       fixedstates |= 1 << MEASUREMENT_TYPE::AuxState::L;
23     }
24     if (mngr->Getcfg().pose_fixed_p_ic) {
25       fixedstates |= 1 << MEASUREMENT_TYPE::AuxState::p_ic;
26     }
27     if (mngr->Getcfg().pose_fixed_q_ic) {
28       fixedstates |= 1 << MEASUREMENT_TYPE::AuxState::q_ic;
29     }
30     if (mngr->Getcfg().pose_fixed_p_wv) {
31       fixedstates |= 1 << MEASUREMENT_TYPE::AuxState::p_wv;
32     }
33     if (mngr->Getcfg().pose_fixed_q_wv) {
34       fixedstates |= 1 << MEASUREMENT_TYPE::AuxState::q_wv;
35     }
36   }
37 
38   shared_ptr<MEASUREMENT_TYPE> meas(new MEASUREMENT_TYPE(
39       n_zp_, n_zq_, measurement_world_sensor_, use_fixed_covariance_,
40       provides_absolute_measurements_, this->sensorID,
41       enable_mah_outlier_rejection_, mah_threshold_, fixedstates, distorter_));
42 
43   meas->MakeFromSensorReading(msg, msg->header.stamp.toSec() - delay_);
44 
45   z_p_ = meas->z_p_;  //store this for the init procedure
46   z_q_ = meas->z_q_;
47 
48   this->manager_.msf_core_->AddMeasurement(meas);
49 }

这里调用了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三个方法的时候再说明。