1. 程式人生 > >Ethzasl MSF原始碼閱讀(3):MSF_Core和PoseMeasurement

Ethzasl MSF原始碼閱讀(3):MSF_Core和PoseMeasurement

1.MSF_Core的三個函式:ProcessIMU、ProcessExternallyPropagatedState和AddMeasurement

MSF_Core維護了狀態佇列和觀測值佇列,這裡需要結合論文思考這個狀態佇列的作用。

ProcessIMU方法:

  1 template<typename EKFState_T>
  2 void MSF_Core<EKFState_T>::ProcessIMU(
  3     const msf_core::Vector3& linear_acceleration,
  4     const
msf_core::Vector3& angular_velocity, const double& msg_stamp, 5 size_t /*msg_seq*/) { 6 7 if (!initialized_) 8 return; 9 10 msf_timing::DebugTimer timer_PropGetClosestState("PropGetClosestState"); 11 if (it_last_IMU == stateBuffer_.GetIteratorEnd()) { 12 it_last_IMU = stateBuffer_.GetIteratorClosestBefore(msg_stamp);
13 } 14 15 shared_ptr<EKFState_T> lastState = it_last_IMU->second; 16 timer_PropGetClosestState.Stop(); 17 18 msf_timing::DebugTimer timer_PropPrepare("PropPrepare"); 19 if (lastState->time == constants::INVALID_TIME) { 20 MSF_WARN_STREAM_THROTTLE( 21 2
, __FUNCTION__<<"ImuCallback: closest state is invalid\n"); 22 return; // Early abort. 23 } 24 25 shared_ptr<EKFState_T> currentState(new EKFState_T); 26 currentState->time = msg_stamp; 27 28 // Check if this IMU message is really after the last one (caused by restarting 29 // a bag file). 30 if (currentState->time - lastState->time < -0.01 && predictionMade_) { 31 initialized_ = false; 32 predictionMade_ = false; 33 MSF_ERROR_STREAM( 34 __FUNCTION__<<"latest IMU message was out of order by a too large amount, " 35 "resetting EKF: last-state-time: " << msf_core::timehuman(lastState->time) 36 << " "<< "current-imu-time: "<< msf_core::timehuman(currentState->time)); 37 return; 38 } 39 40 static int seq = 0; 41 // Get inputs. 42 currentState->a_m = linear_acceleration; 43 currentState->w_m = angular_velocity; 44 currentState->noise_gyr = Vector3::Constant(usercalc_.GetParamNoiseGyr()); 45 currentState->noise_acc = Vector3::Constant(usercalc_.GetParamNoiseAcc()); 46 47 48 // Remove acc spikes (TODO (slynen): find a cleaner way to do this). 49 static Eigen::Matrix<double, 3, 1> last_am = 50 Eigen::Matrix<double, 3, 1>(0, 0, 0); 51 if (currentState->a_m.norm() > 50) 52 currentState->a_m = last_am; 53 else { 54 // Try to get the state before the current time. 55 if (lastState->time == constants::INVALID_TIME) { 56 MSF_WARN_STREAM( 57 "Accelerometer readings had a spike, but no prior state was in the " 58 "buffer to take cleaner measurements from"); 59 return; 60 } 61 last_am = lastState->a_m; 62 } 63 if (!predictionMade_) { 64 if (lastState->time == constants::INVALID_TIME) { 65 MSF_WARN_STREAM("Wanted to compare prediction time offset to last state, " 66 "but no prior state was in the buffer to take cleaner measurements from"); 67 return; 68 } 69 if (fabs(currentState->time - lastState->time) > 0.1) { 70 MSF_WARN_STREAM_THROTTLE( 71 2, "large time-gap re-initializing to last state\n"); 72 typename StateBuffer_T::Ptr_T tmp = stateBuffer_.UpdateTime( 73 lastState->time, currentState->time); 74 time_P_propagated = currentState->time; 75 return; // // early abort // // (if timegap too big) 76 } 77 } 78 79 if (lastState->time == constants::INVALID_TIME) { 80 MSF_WARN_STREAM( 81 "Wanted to propagate state, but no valid prior state could be found in " 82 "the buffer"); 83 return; 84 } 85 timer_PropPrepare.Stop(); 86 87 msf_timing::DebugTimer timer_PropState("PropState"); 88 //propagate state and covariance 89 PropagateState(lastState, currentState); 90 timer_PropState.Stop(); 91 92 msf_timing::DebugTimer timer_PropInsertState("PropInsertState"); 93 it_last_IMU = stateBuffer_.Insert(currentState); 94 timer_PropInsertState.Stop(); 95 96 msf_timing::DebugTimer timer_PropCov("PropCov"); 97 PropagatePOneStep(); 98 timer_PropCov.Stop(); 99 usercalc_.PublishStateAfterPropagation(currentState); 100 101 // Making sure we have sufficient states to apply measurements to. 102 if (stateBuffer_.Size() > 3) 103 predictionMade_ = true; 104 105 if (predictionMade_) { 106 // Check if we can apply some pending measurement. 107 HandlePendingMeasurements(); 108 } 109 seq++; 110 111 }
ProcessIMU

ProcessExternallyPropagatedState方法:

  1 template<typename EKFState_T>
  2 void MSF_Core<EKFState_T>::ProcessExternallyPropagatedState(
  3     const msf_core::Vector3& linear_acceleration,
  4     const msf_core::Vector3& angular_velocity, const msf_core::Vector3& p,
  5     const msf_core::Vector3& v, const msf_core::Quaternion& q,
  6     bool is_already_propagated, const double& msg_stamp, size_t /*msg_seq*/) {
  7 
  8   if (!initialized_)
  9     return;
 10 
 11   // fast method to get last_IMU is broken
 12   // TODO(slynen): fix iterator setting for state callback
 13 
 14 //  // Get the closest state and check validity.
 15 //  if (it_last_IMU == stateBuffer_.getIteratorEnd()) {
 16 //    it_last_IMU = stateBuffer_.getIteratorClosestBefore(msg_stamp);
 17 //    assert(!(it_last_IMU == stateBuffer_.getIteratorEnd()));
 18 //  }
 19 
 20   // TODO(slynen): not broken, revert back when it_last_IMU->second from above is fixed
 21   shared_ptr<EKFState_T> lastState = stateBuffer_.GetLast();//it_last_IMU->second;
 22   if (lastState->time == constants::INVALID_TIME) {
 23     MSF_WARN_STREAM_THROTTLE(2, "StateCallback: closest state is invalid\n");
 24     return;  // Early abort.
 25   }
 26 
 27   // Create a new state.
 28   shared_ptr<EKFState_T> currentState(new EKFState_T);
 29   currentState->time = msg_stamp;
 30 
 31   // Get inputs.
 32   currentState->a_m = linear_acceleration;
 33   currentState->w_m = angular_velocity;
 34   currentState->noise_gyr = Vector3::Constant(usercalc_.GetParamNoiseGyr());
 35   currentState->noise_acc = Vector3::Constant(usercalc_.GetParamNoiseAcc());
 36 
 37   // Remove acc spikes (TODO (slynen): Find a cleaner way to do this).
 38   static Eigen::Matrix<double, 3, 1> last_am =
 39       Eigen::Matrix<double, 3, 1>(0, 0, 0);
 40   if (currentState->a_m.norm() > 50)
 41     currentState->a_m = last_am;
 42   else
 43     last_am = currentState->a_m;
 44 
 45   if (!predictionMade_) {
 46     if (fabs(currentState->time - lastState->time) > 5) {
 47       typename StateBuffer_T::Ptr_T tmp = stateBuffer_.UpdateTime(
 48           lastState->time, currentState->time);
 49       MSF_WARN_STREAM_THROTTLE(
 50           2, "large time-gap re-initializing to last state: "
 51           << msf_core::timehuman(tmp->time));
 52       return;  // Early abort (if timegap too big).
 53     }
 54   }
 55 
 56   bool isnumeric = CheckForNumeric(
 57       currentState->template Get<StateDefinition_T::p>(),
 58       "before prediction p");
 59 
 60   // State propagation is made externally, so we read the actual state.
 61   if (is_already_propagated && isnumeric) {
 62     currentState->template Get<StateDefinition_T::p>() = p;
 63     currentState->template Get<StateDefinition_T::v>() = v;
 64     currentState->template Get<StateDefinition_T::q>() = q;
 65 
 66     // Zero props: copy non propagation states from last state.
 67     boost::fusion::for_each(
 68         currentState->statevars,
 69         msf_tmp::CopyNonPropagationStates<EKFState_T>(*lastState));
 70 
 71   } else if (!is_already_propagated || !isnumeric) {
 72     // Otherwise let's do the state prop. here.
 73     PropagateState(lastState, currentState);
 74   }
 75 
 76   // Clean reset of state and measurement buffer, before we start propagation.
 77   if (!predictionMade_) {
 78 
 79     // Make sure we keep the covariance for the first state.
 80     currentState->P = stateBuffer_.GetLast()->P;
 81     time_P_propagated = currentState->time;
 82 
 83     stateBuffer_.Clear();
 84     MeasurementBuffer_.Clear();
 85     while (!queueFutureMeasurements_.empty()) {
 86       queueFutureMeasurements_.pop();
 87     }
 88   }
 89 
 90   stateBuffer_.Insert(currentState);
 91   PropagatePOneStep();
 92   predictionMade_ = true;
 93   usercalc_.PublishStateAfterPropagation(currentState);
 94 
 95   isnumeric = CheckForNumeric(
 96       currentState->template Get<StateDefinition_T::p>(), "prediction p");
 97   isnumeric = CheckForNumeric(currentState->P, "prediction done P");
 98 
 99   // Check if we can apply some pending measurement.
100   HandlePendingMeasurements();
101 }
ProcessExternallyPropagatedState

AddMeasurement方法:

  1 template<typename EKFState_T>
  2 void MSF_Core<EKFState_T>::AddMeasurement(
  3     shared_ptr<MSF_MeasurementBase<EKFState_T> > measurement) {
  4 
  5   // Return if not initialized of no imu data available.
  6   if (!initialized_ || !predictionMade_)
  7     return;
  8 
  9   // Check if the measurement is in the future where we don't have imu
 10   // measurements yet.
 11   if (measurement->time > stateBuffer_.GetLast()->time) {
 12     queueFutureMeasurements_.push(measurement);
 13     return;
 14 
 15   }
 16   // Check if there is still a state in the buffer for this message (too old).
 17   if (measurement->time < stateBuffer_.GetFirst()->time) {
 18     MSF_WARN_STREAM(
 19         "You tried to give me a measurement which is too far in the past. Are "
 20         "you sure your clocks are synced and delays compensated correctly? "
 21         "[measurement: "<<timehuman(measurement->time)<<" (s) first state in "
 22             "buffer: "<<timehuman(stateBuffer_.GetFirst()->time)<<" (s)]");
 23     return;  // Reject measurements too far in the past.
 24   }
 25 
 26   // Add this measurement to the buffer and get an iterator to it.
 27   typename measurementBufferT::iterator_T it_meas =
 28       MeasurementBuffer_.Insert(measurement);
 29   // Get an iterator the the end of the measurement buffer.
 30   typename measurementBufferT::iterator_T it_meas_end = MeasurementBuffer_
 31       .GetIteratorEnd();
 32 
 33   // No propagation if no update is applied.
 34   typename StateBuffer_T::iterator_T it_curr = stateBuffer_.GetIteratorEnd();
 35 
 36   bool appliedOne = false;
 37 
 38   isfuzzyState_ = false;
 39 
 40   // Now go through all the measurements and apply them one by one.
 41   for (; it_meas != it_meas_end; ++it_meas) {
 42 
 43     if (it_meas->second->time <= 0)  // Valid?
 44       continue;
 45     msf_timing::DebugTimer timer_meas_get_state("Get state for measurement");
 46     // Propagates covariance to state.
 47     shared_ptr<EKFState_T> state = GetClosestState(it_meas->second->time);
 48     timer_meas_get_state.Stop();
 49     if (state->time <= 0) {
 50       MSF_ERROR_STREAM_THROTTLE(
 51           1, __FUNCTION__<< " GetClosestState returned an invalid state");
 52       continue;
 53     }
 54 
 55     msf_timing::DebugTimer timer_meas_apply("Apply measurement");
 56     // Calls back core::ApplyCorrection(), which sets time_P_propagated to meas
 57     // time.
 58     it_meas->second->Apply(state, *this);
 59     timer_meas_apply.Stop();
 60     // Make sure to propagate to next measurement or up to now if no more
 61     // measurements. Propagate from current state.
 62     it_curr = stateBuffer_.GetIteratorAtValue(state);
 63 
 64     typename StateBuffer_T::iterator_T it_end;
 65     typename measurementBufferT::iterator_T it_nextmeas = it_meas;
 66     ++it_nextmeas;  // The next measurement in the list.
 67     // That was the last measurement, so propagate state to now.
 68     if (it_nextmeas == it_meas_end) {
 69       it_end = stateBuffer_.GetIteratorEnd();
 70     } else {
 71       it_end = stateBuffer_.GetIteratorClosestAfter(it_nextmeas->second->time);
 72       if (it_end != stateBuffer_.GetIteratorEnd()) {
 73         // Propagate to state closest after the measurement so we can interpolate.
 74         ++it_end;
 75       }
 76     }
 77 
 78     typename StateBuffer_T::iterator_T it_next = it_curr;
 79     ++it_next;
 80 
 81     msf_timing::DebugTimer timer_prop_state_after_meas(
 82         "Repropagate state to now");
 83     // Propagate to selected state.
 84     for (; it_curr != it_end && it_next != it_end &&
 85            it_curr->second->time != constants::INVALID_TIME &&
 86            it_next->second->time != constants::INVALID_TIME; ++it_curr,
 87            ++it_next) {
 88       if (it_curr->second == it_next->second) {
 89         MSF_ERROR_STREAM(__FUNCTION__<< " propagation : it_curr points to same "
 90         "state as it_next. This must not happen.");
 91         continue;
 92       }
 93       // Break loop if EKF reset in the meantime.
 94       if (!initialized_ || !predictionMade_)
 95         return;
 96       PropagateState(it_curr->second, it_next->second);
 97     }
 98     timer_prop_state_after_meas.Stop();
 99     appliedOne = true;
100   }
101 
102   if (!appliedOne) {
103     MSF_ERROR_STREAM("No measurement was applied, this should not happen.");
104     return;
105   }
106 
107   // Now publish the best current estimate.
108   shared_ptr<EKFState_T>& latestState = stateBuffer_.GetLast();
109 
110   PropPToState(latestState);  // Get the latest covariance.
111 
112   usercalc_.PublishStateAfterUpdate(latestState);
113 }
AddMeasurement

注意AddMeasurement方法中的 it_meas->second->Apply(state, *this); 這句程式碼,呼叫了PoseMeasurement的Apply方法。注意Apply方法要求*this引數,將MSF_Core作為引數傳入,這在Apply執行過程中有一個呼叫core.ApplyCorrection()的步驟。

2.PoseMeasurement繼承自PoseMeasurementBase,即msf_core::MSF_Measurement,這個又繼承自MSF_MeasurementBase<EKFState_T>。

1 template<typename T, typename RMAT_T, typename EKFState_T>
2 class MSF_Measurement : public MSF_MeasurementBase<EKFState_T>
1 typedef msf_core::MSF_Measurement<geometry_msgs::PoseWithCovarianceStamped,
2     Eigen::Matrix<double, nMeasurements, nMeasurements>, msf_updates::EKFState> PoseMeasurementBase;
struct PoseMeasurement : public PoseMeasurementBase 

 檢視PoseMeasurement類Apply方法的程式碼:

  1 virtual void Apply(shared_ptr<EKFState_T> state_nonconst_new,
  2                      msf_core::MSF_Core<EKFState_T>& core) {
  3 
  4     if (isabsolute_) {  // Does this measurement refer to an absolute measurement,
  5       // or is is just relative to the last measurement.
  6       // Get a const ref, so we can read core states
  7       const EKFState_T& state = *state_nonconst_new;
  8       // init variables
  9       Eigen::Matrix<double, nMeasurements,
 10           msf_core::MSF_Core<EKFState_T>::nErrorStatesAtCompileTime> H_new;
 11       Eigen::Matrix<double, nMeasurements, 1> r_old;
 12 
 13       CalculateH(state_nonconst_new, H_new);
 14 
 15       // Get rotation matrices.
 16       Eigen::Matrix<double, 3, 3> C_wv = state.Get<StateQwvIdx>()
 17 
 18           .conjugate().toRotationMatrix();
 19       Eigen::Matrix<double, 3, 3> C_q = state.Get<StateDefinition_T::q>()
 20           .conjugate().toRotationMatrix();
 21 
 22       // Construct residuals.
 23       // Position.
 24       r_old.block<3, 1>(0, 0) = z_p_
 25           - (C_wv.transpose()
 26               * (-state.Get<StatePwvIdx>()
 27                   + state.Get<StateDefinition_T::p>()
 28                   + C_q.transpose() * state.Get<StatePicIdx>()))
 29               * state.Get<StateLIdx>();
 30 
 31       // Attitude.
 32       Eigen::Quaternion<double> q_err;
 33       q_err = (state.Get<StateQwvIdx>()
 34           * state.Get<StateDefinition_T::q>()
 35           * state.Get<StateQicIdx>()).conjugate() * z_q_;
 36       r_old.block<3, 1>(3, 0) = q_err.vec() / q_err.w() * 2;
 37       // Vision world yaw drift.
 38       q_err = state.Get<StateQwvIdx>();
 39 
 40       r_old(6, 0) = -2 * (q_err.w() * q_err.z() + q_err.x() * q_err.y())
 41           / (1 - 2 * (q_err.y() * q_err.y() + q_err.z() * q_err.z()));
 42 
 43       if (!CheckForNumeric(r_old, "r_old")) {
 44         MSF_ERROR_STREAM("r_old: "<<r_old);
 45         MSF_WARN_STREAM(
 46             "state: "<<const_cast<EKFState_T&>(state). ToEigenVector().transpose());
 47       }
 48       if (!CheckForNumeric(H_new, "H_old")) {
 49         MSF_ERROR_STREAM("H_old: "<<H_new);
 50         MSF_WARN_STREAM(
 51             "state: "<<const_cast<EKFState_T&>(state). ToEigenVector().transpose());
 52       }
 53       if (!CheckForNumeric(R_, "R_")) {
 54         MSF_ERROR_STREAM("R_: "<<R_);
 55         MSF_WARN_STREAM(
 56             "state: "<<const_cast<EKFState_T&>(state). ToEigenVector().transpose());
 57       }
 58 
 59       // Call update step in base class.
 60       this->CalculateAndApplyCorrection(state_nonconst_new, core, H_new, r_old,
 61                                         R_);
 62     } else {
 63       // Init variables: Get previous measurement.
 64       shared_ptr < msf_core::MSF_MeasurementBase<EKFState_T> > prevmeas_base =
 65           core.GetPreviousMeasurement(this->time, this->sensorID_);
 66 
 67       if (prevmeas_base->time == msf_core::constants::INVALID_TIME) {
 68         MSF_WARN_STREAM(
 69             "The previous measurement is invalid. Could not apply measurement! " "time:"<<this->time<<" sensorID: "<<this->sensorID_);
 70         return;
 71       }
 72 
 73       // Make this a pose measurement.
 74       shared_ptr<PoseMeasurement> prevmeas = dynamic_pointer_cast
 75           < PoseMeasurement > (prevmeas_base);
 76       if (!prevmeas) {
 77         MSF_WARN_STREAM(
 78             "The dynamic cast of the previous measurement has failed. "
 79             "Could not apply measurement");
 80         return;
 81       }
 82 
 83       // Get state at previous measurement.
 84       shared_ptr<EKFState_T> state_nonconst_old = core.GetClosestState(
 85           prevmeas->time);
 86 
 87       if (state_nonconst_old->time == msf_core::constants::INVALID_TIME) {
 88         MSF_WARN_STREAM(
 89             "The state at the previous measurement is invalid. Could "
 90             "not apply measurement");
 91         return;
 92       }
 93 
 94       // Get a const ref, so we can read core states.
 95       const EKFState_T& state_new = *state_nonconst_new;
 96       const EKFState_T& state_old = *