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

1.代码框架

论文解读:无人驾驶算法学习（六）：多传感器融合MSF算法

1.1代码调用关系图:

1.2主要函数调用关系图:

2.posemsf阅读

2.1程序入口：

ethzasl_msf\msf_updates\src\pose_msf\main.cpp

#include "pose_sensormanager.h"

int main(int argc, char** argv)
{
  ros::init(argc, argv, "msf_pose_sensor");
  msf_pose_sensor::PoseSensorManager manager;
  ros::spin();
  return 0;
}

2.2PoseSensorManager类

ethzasl_msf\msf_updates\src\pose_msf\pose_sensormanager.h
查看构造函数: PoseSensorManager继承自msf_core::MSF_SensorManagerROS，继承自msf_core::MSF_SensorManager<EKFState_T>

typedef msf_updates::SinglePoseSensorConfig Config_T;
typedef dynamic_reconfigure::Server<Config_T> ReconfigureServer;
typedef shared_ptr<ReconfigureServer> ReconfigureServerPtr;//动态参数

typedef PoseSensorHandler<msf_updates::pose_measurement::PoseMeasurement<>,
      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<msf_updates::EKFState>(*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);//触发回调函数的话题"initialize_msf_scale"
    init_height_srv_ = pnh.advertiseService("initialize_msf_height",
                                            &PoseSensorManager::InitHeight, this);
  }

注意其中的imu_handler_和pose_handler_，分别是IMUHandler_ROS对象和PoseSensorHandler模板类对象。IMUHandler_ROS继承自IMUHandler。
msf_updates::pose_measurement::PoseMeasurement<>是模型类PoseSensorHandler的类参数，这是一个观测量参数。
这两个类对象构造的时候都传入了*this指针，即PoseSensorManager对象自身。这很重要，两个类中都调用了PoseSensorManager的成员函数。
可以进入这两个类进行查看构造函数，这2个构造函数都进行了一些主题的订阅。

  shared_ptr<msf_core::IMUHandler_ROS<msf_updates::EKFState> > imu_handler_;
  shared_ptr<PoseSensorHandler_T> pose_handler_;

reconf_server_->setCallback(f)绑定了一个回调，回调函数PoseSensorManager::Config(xx).
这是动态参数中的常规操作

virtual void Config(Config_T &config, uint32_t level)
{
    config_ = config;
    pose_handler_->SetNoises(config.pose_noise_meas_p,
                             config.pose_noise_meas_q);
    pose_handler_->SetDelay(config.pose_delay);
    if ((level & msf_updates::SinglePoseSensor_INIT_FILTER)
        && config.core_init_filter == true)
    {
      Init(config.pose_initial_scale);
      config.core_init_filter = false;
    }
    // Init call with "set height" checkbox.
    if ((level & msf_updates::SinglePoseSensor_SET_HEIGHT)
        && config.core_set_height == true)
     {
      Eigen::Matrix<double, 3, 1> p = pose_handler_->GetPositionMeasurement();
      if (p.norm() == 0)
      {
        MSF_WARN_STREAM(
            "No measurements received yet to initialize position. Height init "
            "not allowed.");
        return;
      }
      double scale = p[2] / config.core_height;
      Init(scale);
      config.core_set_height = false;
    }
}

预测:msf_core::IMUHandler_ROS

IMUHandler_ROS(MSF_SensorManager<EKFState_T>& mng,
                 const std::string& topic_namespace, const std::string& parameternamespace)
       : IMUHandler<EKFState_T>(mng, topic_namespace, parameternamespace)
{
    ros::NodeHandle nh(topic_namespace);
    subImu_ = nh.subscribe("imu_state_input", 100, &IMUHandler_ROS::IMUCallback, this);
    subState_ = nh.subscribe("hl_state_input", 10,   &IMUHandler_ROS::StateCallback, this);
}

测量 : PoseSensorHandler构造，同时查看PoseSensorHandler::MeasurementCallback回调函数。
pose_sensorhander.hpp

template<typename MEASUREMENT_TYPE, typename MANAGER_TYPE>
PoseSensorHandler<MEASUREMENT_TYPE, MANAGER_TYPE>::PoseSensorHandler(
    MANAGER_TYPE& meas, std::string topic_namespace,
    std::string parameternamespace, bool distortmeas)
    : SensorHandler<msf_updates::EKFState>(meas, topic_namespace, parameternamespace),
      n_zp_(1e-6),
      n_zq_(1e-6),
      delay_(0),
      timestamp_previous_pose_(0)
{
  ros::NodeHandle pnh("~/" + parameternamespace);

  MSF_INFO_STREAM(
      "Loading parameters for pose sensor from namespace: "
          << pnh.getNamespace());

  pnh.param("pose_absolute_measurements", provides_absolute_measurements_,
            true);
  pnh.param("pose_measurement_world_sensor", measurement_world_sensor_, true);
  pnh.param("pose_use_fixed_covariance", use_fixed_covariance_, false);
  pnh.param("pose_measurement_minimum_dt", pose_measurement_minimum_dt_, 0.05);
  pnh.param("enable_mah_outlier_rejection", enable_mah_outlier_rejection_, false);
  pnh.param("mah_threshold", mah_threshold_, msf_core::kDefaultMahThreshold_);

  MSF_INFO_STREAM_COND(measurement_world_sensor_, "Pose sensor is interpreting "
                       "measurement as sensor w.r.t. world");
  MSF_INFO_STREAM_COND(
      !measurement_world_sensor_,
      "Pose sensor is interpreting measurement as world w.r.t. "
      "sensor (e.g. ethzasl_ptam)");

  MSF_INFO_STREAM_COND(use_fixed_covariance_, "Pose sensor is using fixed "
                       "covariance");
  MSF_INFO_STREAM_COND(!use_fixed_covariance_,
                       "Pose sensor is using covariance "
                       "from sensor");

  MSF_INFO_STREAM_COND(provides_absolute_measurements_,
                       "Pose sensor is handling "
                       "measurements as absolute values");
  MSF_INFO_STREAM_COND(!provides_absolute_measurements_, "Pose sensor is "
                       "handling measurements as relative values");

  ros::NodeHandle nh("msf_updates/" + topic_namespace);
  subPoseWithCovarianceStamped_ =
      nh.subscribe < geometry_msgs::PoseWithCovarianceStamped
          > ("pose_with_covariance_input", 20, &PoseSensorHandler::MeasurementCallback, this);
  subTransformStamped_ = nh.subscribe < geometry_msgs::TransformStamped
      > ("transform_input", 20, &PoseSensorHandler::MeasurementCallback, this);
  subPoseStamped_ = nh.subscribe < geometry_msgs::PoseStamped
      > ("pose_input", 20, &PoseSensorHandler::MeasurementCallback, this);

  z_p_.setZero();
  z_q_.setIdentity();

  if (distortmeas)
 {
    Eigen::Vector3d meanpos;
    double distortpos_mean;
    pnh.param("distortpos_mean", distortpos_mean, 0.0);
    meanpos.setConstant(distortpos_mean);

    Eigen::Vector3d stddevpos;
    double distortpos_stddev;
    pnh.param("distortpos_stddev", distortpos_stddev, 0.0);
    stddevpos.setConstant(distortpos_stddev);

    Eigen::Vector3d meanatt;
    double distortatt_mean;
    pnh.param("distortatt_mean", distortatt_mean, 0.0);
    meanatt.setConstant(distortatt_mean);

    Eigen::Vector3d stddevatt;
    double distortatt_stddev;
    pnh.param("distortatt_stddev", distortatt_stddev, 0.0);
    stddevatt.setConstant(distortatt_stddev);

    double distortscale_mean;
    pnh.param("distortscale_mean", distortscale_mean, 0.0);
    double distortscale_stddev;
    pnh.param("distortscale_stddev", distortscale_stddev, 0.0);

    distorter_.reset( new msf_updates::PoseDistorter(meanpos, stddevpos, meanatt, stddevatt, distortscale_mean, distortscale_stddev));
  }
}