ethzasl_msf/msf_updates/src/pose_msf/pose_sensormanager.h中查看
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);
init_height_srv_ = pnh.advertiseService("initialize_msf_height",
&PoseSensorManager::InitHeight,
this);
}
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<typename MEASUREMENT_TYPE, typename MANAGER_TYPE>
void PoseSensorHandler<MEASUREMENT_TYPE, MANAGER_TYPE>::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<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);
}
ethzasl_msf/msf_updates/include/msf_updates/pose_sensor_handler/implementation/pose_sensorhandler.hpp查看:
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方法, 注意AddMeasurement方法中的 it_meas->second->Apply(state, this); 这句代码，调用了PoseMeasurement的Apply方法。Apply方法要求this参数，将MSF_Core作为参数传入，这在Apply执行过程中有一个调用core.ApplyCorrection()的步骤。
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_));中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<EKFState_T>中实现, 这2个方法实现了EKF方法的更新步骤.
/**
* The method called by the msf_core to apply the measurement represented by
* this object
*/
virtual void Apply(shared_ptr<EKFState_T> state_nonconst_new,
msf_core::MSF_Core<EKFState_T>& 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<double, nMeasurements,
msf_core::MSF_Core<EKFState_T>::nErrorStatesAtCompileTime> H_new;
Eigen::Matrix<double, nMeasurements, 1> r_old;
CalculateH(state_nonconst_new, H_new);
// Get rotation matrices.
Eigen::Matrix<double, 3, 3> C_wv = state.Get<StateQwvIdx>()
.conjugate().toRotationMatrix();
Eigen::Matrix<double, 3, 3> C_q = state.Get<StateDefinition_T::q>()
.conjugate().toRotationMatrix();
// Construct residuals.
// Position.
r_old.block<3, 1>(0, 0) = z_p_
- (C_wv.transpose()
* (-state.Get<StatePwvIdx>()
+ state.Get<StateDefinition_T::p>()
+ C_q.transpose() * state.Get<StatePicIdx>()))
* state.Get<StateLIdx>();
// Attitude.
Eigen::Quaternion<double> q_err;
q_err = (state.Get<StateQwvIdx>()
* state.Get<StateDefinition_T::q>()
* state.Get<StateQicIdx>()).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<StateQwvIdx>();
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: "<<r_old);
MSF_WARN_STREAM(
"state: "<<const_cast<EKFState_T&>(state). ToEigenVector().transpose());
}
if (!CheckForNumeric(H_new, "H_old")) {
MSF_ERROR_STREAM("H_old: "<<H_new);
MSF_WARN_STREAM(
"state: "<<const_cast<EKFState_T&>(state). ToEigenVector().transpose());
}
if (!CheckForNumeric(R_, "R_")) {
MSF_ERROR_STREAM("R_: "<<R_);
MSF_WARN_STREAM(
"state: "<<const_cast<EKFState_T&>(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<EKFState_T> > 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:"<<this->time<<" sensorID: "<<this->sensorID_);
return;
}
// Make this a pose measurement.
shared_ptr<PoseMeasurement> 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<EKFState_T> 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<double, nMeasurements,
msf_core::MSF_Core<EKFState_T>::nErrorStatesAtCompileTime> H_new,
H_old;
Eigen::Matrix<double, nMeasurements, 1> 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<double, 3, 3> C_wv_old, C_wv_new;
Eigen::Matrix<double, 3, 3> C_q_old, C_q_new;
C_wv_new = state_new.Get<StateQwvIdx>().conjugate().toRotationMatrix();
C_q_new = state_new.Get<StateDefinition_T::q>().conjugate()
.toRotationMatrix();
C_wv_old = state_old.Get<StateQwvIdx>().conjugate().toRotationMatrix();
C_q_old = state_old.Get<StateDefinition_T::q>().conjugate()
.toRotationMatrix();
// Construct residuals.
// Position:
Eigen::Matrix<double, 3, 1> diffprobpos = (C_wv_new.transpose()
* (-state_new.Get<StatePwvIdx>() + state_new.Get<StateDefinition_T::p>()
+ C_q_new.transpose() * state_new.Get<StatePicIdx>()))
* state_new.Get<StateLIdx>() - (C_wv_old.transpose()
* (-state_old.Get<StatePwvIdx>() + state_old.Get<StateDefinition_T::p>()
+ C_q_old.transpose() * state_old.Get<StatePicIdx>()))
* state_old.Get<StateLIdx>();
Eigen::Matrix<double, 3, 1> diffmeaspos = z_p_ - prevmeas->z_p_;
r_new.block<3, 1>(0, 0) = diffmeaspos - diffprobpos;
// Attitude:
Eigen::Quaternion<double> diffprobatt = (state_new.Get<StateQwvIdx>()
* state_new.Get<StateDefinition_T::q>()
* state_new.Get<StateQicIdx>()).conjugate()
* (state_old.Get<StateQwvIdx>()
* state_old.Get<StateDefinition_T::q>()
* state_old.Get<StateQicIdx>());
Eigen::Quaternion<double> diffmeasatt = z_q_.conjugate() * prevmeas->z_q_;
Eigen::Quaternion<double> 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<StateQwvIdx>();
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: "<<r_old);
MSF_WARN_STREAM(
"state: "<<const_cast<EKFState_T&>(state_new). ToEigenVector().transpose());
}
if (!CheckForNumeric(H_new, "H_old")) {
MSF_ERROR_STREAM("H_old: "<<H_new);
MSF_WARN_STREAM(
"state: "<<const_cast<EKFState_T&>(state_new). ToEigenVector().transpose());
}
if (!CheckForNumeric(R_, "R_")) {
MSF_ERROR_STREAM("R_: "<<R_);
MSF_WARN_STREAM(
"state: "<<const_cast<EKFState_T&>(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_);
}
MSF_Core类负责汇集IMU消息和位姿观测值，同时实现了状态预测
而msf_updates::pose_measurement::PoseMeasurement<>实现了状态的更新。
