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

这里有个感觉，就是百川汇海。即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三个方法的时候再说明。