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 + 1
        && lastseq != constants::INVALID_SEQUENCE) {
      MSF_WARN_STREAM(
          "msf_core: imu message drop curr seq:" << msg->header.seq
              << " expected: " << lastseq + 1);
    }
    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, 10, 1> >(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, 3, 1>(msg->state[0], msg->state[1],
                                    msg->state[2]);
    v = Eigen::Matrix<double, 3, 1>(msg->state[3], msg->state[4],
                                    msg->state[5]);
    q = Eigen::Quaternion<double>(msg->state[6], msg->state[7], msg->state[8],
                                  msg->state[9]);
    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 = 0;
  static_assert(msf_updates::EKFState::nStateVarsAtCompileTime < 32, "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 |= 1 << MEASUREMENT_TYPE::AuxState::L;
    }
    if (mngr->Getcfg().pose_fixed_p_ic) {
      fixedstates |= 1 << MEASUREMENT_TYPE::AuxState::p_ic;
    }
    if (mngr->Getcfg().pose_fixed_q_ic) {
      fixedstates |= 1 << MEASUREMENT_TYPE::AuxState::q_ic;
    }
    if (mngr->Getcfg().pose_fixed_p_wv) {
      fixedstates |= 1 << MEASUREMENT_TYPE::AuxState::p_wv;
    }
    if (mngr->Getcfg().pose_fixed_q_wv) {
      fixedstates |= 1 << 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三个方法的时候再说明。