关于IMU融合知乎上的一篇问答：有哪些开源项目是关于单目+imu做slam的？
Ethz的Stephen Weiss的工作，是一个IMU松耦合的方法。
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 ;
}
PoseSensorManager类，查看构造函数。PoseSensorManager继承自msf_core::MSF_SensorManagerROS，继承自msf_core::MSF_SensorManager<EKFState_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);
init_height_srv_ = pnh.advertiseService("initialize_msf_height",
&PoseSensorManager::InitHeight, this);
}
2. 注意其中的imu_handler_和pose_handler_，分别是IMUHandler_ROS对象和PoseSensorHandler模板类对象。IMUHandler_ROS继承自IMUHandler。
typedef PoseSensorHandler<msf_updates::pose_measurement::PoseMeasurement<>,
PoseSensorManager> PoseSensorHandler_T;
（msf_updates::pose_measurement::PoseMeasurement<>是模型类PoseSensorHandler的类参数，这是一个观测量参数。）
注意IMUHandler和PoseSensorHandler最终都继承自msf_core::SensorHandler<typename msf_updates::EKFState>类。
这两个类对象构造的时候都传入了*this指针，即PoseSensorManager对象自身。这很重要，两个类中都调用了PoseSensorManager的成员函数。
可以进入这两个类进行查看构造函数，这2个构造函数都进行了一些主题的订阅。
IMUHandler_ROS构造，同时查看IMUHandler_ROS::IMUCallback回调函数。
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", , &IMUHandler_ROS::IMUCallback, this);
subState_ = nh.subscribe("hl_state_input", , &IMUHandler_ROS::StateCallback, this);
}
PoseSensorHandler构造，同时查看PoseSensorHandler::MeasurementCallback回调函数。
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-),
n_zq_(1e-),
delay_(),
timestamp_previous_pose_()
{
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", , &PoseSensorHandler::MeasurementCallback, this);
subTransformStamped_ = nh.subscribe < geometry_msgs::TransformStamped
> ("transform_input", , &PoseSensorHandler::MeasurementCallback, this);
subPoseStamped_ = nh.subscribe < geometry_msgs::PoseStamped
> ("pose_input", , &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));
}
}
3.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, , > p = pose_handler_->GetPositionMeasurement();
if (p.norm() == )
{
MSF_WARN_STREAM(
"No measurements received yet to initialize position. Height init "
"not allowed.");
return;
}
double scale = p[] / config.core_height;
Init(scale);
config.core_set_height = false;
}
}
参考文献：