多传感器融合MSF算法源码阅读(三)

无人驾驶算法学习（六）：多传感器融合MSF算法
多传感器融合MSF算法源码阅读(一)
多传感器融合MSF算法源码阅读(二)

1.触发测量更新回调函数

• 查看主类PoseSensorManager的构造函数。注意，在构造函数中调用了类PoseSensorHandler

ethzasl_msf/msf_updates/src/pose_msf/pose_sensormanager.h中查看

 typedef PoseSensorHandler,
      PoseSensorManager> PoseSensorHandler_T;
      
PoseSensorManager(ros::NodeHandle pnh = ros::NodeHandle("~/pose_sensor")) {
    bool distortmeas = false;  ///< Distort the pose measurements.

    imu_handler_.reset(
        new msf_core::IMUHandler_ROS(*this, "msf_core",
                                                            "imu_handler"));
    pose_handler_.reset(
        new PoseSensorHandler_T(*this, "", "pose_sensor", distortmeas));

    AddHandler(pose_handler_);

    reconf_server_.reset(new ReconfigureServer(pnh));
    ReconfigureServer::CallbackType f = boost::bind(&PoseSensorManager::Config,
                                                    this, _1, _2);
    reconf_server_->setCallback(f);

    init_scale_srv_ = pnh.advertiseService("initialize_msf_scale",
                                           &PoseSensorManager::InitScale, this);
    init_height_srv_ = pnh.advertiseService("initialize_msf_height",
                                            &PoseSensorManager::InitHeight,
                                            this);
  }

• 类PoseSensorHandler中,在构造函数中挂载了三个不同的MeasurementCallback函数:

ethzasl_msf/msf_updates/include/msf_updates/pose_sensor_handler/implementation/pose_sensorhandler.hpp查看:
重载函数的形参:
geometry_msgs::PoseWithCovarianceStamped，geometry_msgs::TransformStamped，geometry_msgs::PoseStamped三种消息类型。

如geometry_msgs::PoseWithCovarianceStamped:
subPoseWithCovarianceStamped_ =
      nh.subscribe < geometry_msgs::PoseWithCovarianceStamped
          > ("pose_with_covariance_input", 20, &PoseSensorHandler::MeasurementCallback, this);

实现:
template
void PoseSensorHandler::MeasurementCallback(
    const geometry_msgs::PoseStampedConstPtr & msg) {
  this->SequenceWatchDog(msg->header.seq, subPoseStamped_.getTopic());
  MSF_INFO_STREAM_ONCE(
      "*** pose sensor got first measurement from topic "
          << this->topic_namespace_ << "/" << subPoseStamped_.getTopic()
          << " ***");

  geometry_msgs::PoseWithCovarianceStampedPtr pose(
      new geometry_msgs::PoseWithCovarianceStamped());

  if (!use_fixed_covariance_)  // Take covariance from sensor.
  {
    MSF_WARN_STREAM_THROTTLE(
        2,
        "Provided message type without covariance but set fixed_covariance =="
        "false at the same time. Discarding message.");
    return;
  }
*重要*
template
void PoseSensorHandler::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);
}

2.测量更新状态量

ethzasl_msf/msf_updates/include/msf_updates/pose_sensor_handler/implementation/pose_sensorhandler.hpp查看:
ProcessPoseMeasurement(msg)函数：

template
void PoseSensorHandler::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_);

  // 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 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方法, 注意AddMeasurement方法中的 it_meas->second->Apply(state, this); 这句代码，调用了PoseMeasurement的Apply方法。Apply方法要求this参数，将MSF_Core作为参数传入，这在Apply执行过程中有一个调用core.ApplyCorrection()的步骤。

shared_ptr 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_));中new MEASUREMENT_TYPE,也就是new了一个struct PoseMeasurement
原因:主类PoseSensorManager的构造函数:
typedef PoseSensorHandler<msf_updates::pose_measurement::PoseMeasurement<>,
PoseSensorManager> PoseSensorHandler_T;
所以MEASUREMENT_TYPE就是msf_updates::pose_measurement::PoseMeasurement<>

ethzasl_msf/msf_updates/include/msf_updates/pose_sensor_handler/pose_measurement.h查看
struct PoseMeasurement : public PoseMeasurementBase 数据结构完成测量更新

分析其中最核心的Apply函数:
里面有2个方法CalculateAndApplyCorrection和CalculateAndApplyCorrectionRelative，这2个方法在父类MSF_MeasurementBase中实现, 这2个方法实现了EKF方法的更新步骤.

/**
   * The method called by the msf_core to apply the measurement represented by
   * this object
   */
  virtual void Apply(shared_ptr state_nonconst_new,
                     msf_core::MSF_Core& core) {

    if (isabsolute_) {  // Does this measurement refer to an absolute measurement,
      // or is is just relative to the last measurement.
      // Get a const ref, so we can read core states
      const EKFState_T& state = *state_nonconst_new;
      // init variables
      Eigen::Matrix::nErrorStatesAtCompileTime> H_new;
      Eigen::Matrix r_old;

      CalculateH(state_nonconst_new, H_new);

      // Get rotation matrices.
      Eigen::Matrix C_wv = state.Get()

          .conjugate().toRotationMatrix();
      Eigen::Matrix C_q = state.Get()
          .conjugate().toRotationMatrix();

      // Construct residuals.
      // Position.
      r_old.block<3, 1>(0, 0) = z_p_
          - (C_wv.transpose()
              * (-state.Get()
                  + state.Get()
                  + C_q.transpose() * state.Get()))
              * state.Get();

      // Attitude.
      Eigen::Quaternion q_err;
      q_err = (state.Get()
          * state.Get()
          * state.Get()).conjugate() * z_q_;
      r_old.block<3, 1>(3, 0) = q_err.vec() / q_err.w() * 2;
      // Vision world yaw drift.
      q_err = state.Get();

      r_old(6, 0) = -2 * (q_err.w() * q_err.z() + q_err.x() * q_err.y())
          / (1 - 2 * (q_err.y() * q_err.y() + q_err.z() * q_err.z()));

      if (!CheckForNumeric(r_old, "r_old")) {
        MSF_ERROR_STREAM("r_old: "<(state). ToEigenVector().transpose());
      }
      if (!CheckForNumeric(H_new, "H_old")) {
        MSF_ERROR_STREAM("H_old: "<(state). ToEigenVector().transpose());
      }
      if (!CheckForNumeric(R_, "R_")) {
        MSF_ERROR_STREAM("R_: "<(state). ToEigenVector().transpose());
      }

      // Call update step in base class.
      this->CalculateAndApplyCorrection(state_nonconst_new, core, H_new, r_old,
                                        R_);
    } else {
      // Init variables: Get previous measurement.
      shared_ptr < msf_core::MSF_MeasurementBase > prevmeas_base =
          core.GetPreviousMeasurement(this->time, this->sensorID_);

      if (prevmeas_base->time == msf_core::constants::INVALID_TIME) {
        MSF_WARN_STREAM(
            "The previous measurement is invalid. Could not apply measurement! " "time:"<time<<" sensorID: "<sensorID_);
        return;
      }

      // Make this a pose measurement.
      shared_ptr prevmeas = dynamic_pointer_cast
          < PoseMeasurement > (prevmeas_base);
      if (!prevmeas) {
        MSF_WARN_STREAM(
            "The dynamic cast of the previous measurement has failed. "
            "Could not apply measurement");
        return;
      }

      // Get state at previous measurement.
      shared_ptr state_nonconst_old = core.GetClosestState(
          prevmeas->time);

      if (state_nonconst_old->time == msf_core::constants::INVALID_TIME) {
        MSF_WARN_STREAM(
            "The state at the previous measurement is invalid. Could "
            "not apply measurement");
        return;
      }

      // Get a const ref, so we can read core states.
      const EKFState_T& state_new = *state_nonconst_new;
      const EKFState_T& state_old = *state_nonconst_old;

      Eigen::Matrix::nErrorStatesAtCompileTime> H_new,
          H_old;
      Eigen::Matrix r_new, r_old;

      CalculateH(state_nonconst_old, H_old);

      H_old *= -1;

      CalculateH(state_nonconst_new, H_new);

      //TODO (slynen): check that both measurements have the same states fixed!
      Eigen::Matrix C_wv_old, C_wv_new;
      Eigen::Matrix C_q_old, C_q_new;

      C_wv_new = state_new.Get().conjugate().toRotationMatrix();
      C_q_new = state_new.Get().conjugate()
          .toRotationMatrix();

      C_wv_old = state_old.Get().conjugate().toRotationMatrix();
      C_q_old = state_old.Get().conjugate()
          .toRotationMatrix();

      // Construct residuals.
      // Position:
      Eigen::Matrix diffprobpos = (C_wv_new.transpose()
          * (-state_new.Get() + state_new.Get()
              + C_q_new.transpose() * state_new.Get()))
          * state_new.Get() - (C_wv_old.transpose()
          * (-state_old.Get() + state_old.Get()
              + C_q_old.transpose() * state_old.Get()))
              * state_old.Get();


      Eigen::Matrix diffmeaspos = z_p_ - prevmeas->z_p_;

      r_new.block<3, 1>(0, 0) = diffmeaspos - diffprobpos;

      // Attitude:
      Eigen::Quaternion diffprobatt = (state_new.Get()
          * state_new.Get()
          * state_new.Get()).conjugate()
          * (state_old.Get()
              * state_old.Get()
              * state_old.Get());

      Eigen::Quaternion diffmeasatt = z_q_.conjugate() * prevmeas->z_q_;

      Eigen::Quaternion q_err;
      q_err = diffprobatt.conjugate() * diffmeasatt;

      r_new.block<3, 1>(3, 0) = q_err.vec() / q_err.w() * 2;
      // Vision world yaw drift.
      q_err = state_new.Get();

      r_new(6, 0) = -2 * (q_err.w() * q_err.z() + q_err.x() * q_err.y())
          / (1 - 2 * (q_err.y() * q_err.y() + q_err.z() * q_err.z()));

      if (!CheckForNumeric(r_old, "r_old")) {
        MSF_ERROR_STREAM("r_old: "<(state_new). ToEigenVector().transpose());
      }
      if (!CheckForNumeric(H_new, "H_old")) {
        MSF_ERROR_STREAM("H_old: "<(state_new). ToEigenVector().transpose());
      }
      if (!CheckForNumeric(R_, "R_")) {
        MSF_ERROR_STREAM("R_: "<(state_new). ToEigenVector().transpose());
      }

      // Call update step in base class.
      this->CalculateAndApplyCorrectionRelative(state_nonconst_old,
                                                state_nonconst_new, core, H_old,
                                                H_new, r_new, R_);

    }

3.总结

MSF_Core类负责汇集IMU消息和位姿观测值，同时实现了状态预测

而msf_updates::pose_measurement::PoseMeasurement<>实现了状态的更新。