void PFLocalizationCore::updateFilter(CActionCollectionPtr _action, CSensoryFramePtr _sf) {
    if(state_ == INIT) initializeFilter();
    tictac_.Tic();
    pf_.executeOn( pdf_, _action.pointer(), _sf.pointer(), &pf_stats_ );
    timeLastUpdate_ = _sf->getObservationByIndex(0)->timestamp;
    update_counter_++;
}
void PFLocalizationCore::observation(CSensoryFramePtr _sf, CObservationOdometryPtr _odometry) {

	CActionCollectionPtr action = CActionCollection::Create();
	CActionRobotMovement2D odom_move;
    odom_move.timestamp = _sf->getObservationByIndex(0)->timestamp;
    if(_odometry) {
        if(odomLastObservation_.empty()) {
            odomLastObservation_ = _odometry->odometry;
        }
        mrpt::poses::CPose2D incOdoPose = _odometry->odometry - odomLastObservation_;
        odomLastObservation_ = _odometry->odometry;
        odom_move.computeFromOdometry(incOdoPose, motion_model_options_);
        action->insert(odom_move);
        updateFilter(action, _sf);
    } else {
      if(use_motion_model_default_options_){
        log_info("No odometry at update %4i -> using dummy", update_counter_);
		odom_move.computeFromOdometry(mrpt::poses::CPose2D(0,0,0), motion_model_default_options_);
        action->insert(odom_move);
        updateFilter(action, _sf);
      } else {
        log_info("No odometry at update %4i -> skipping observation", update_counter_);
      }
    }
}
Example #3
0
void RBPFSlamCore::updateFilter(CActionCollectionPtr _action, CSensoryFramePtr _sf) {
    if(state_ == INIT) initializeFilter();
    tictac_.Tic();
    timeLastUpdate_ = _sf->getObservationByIndex(0)->timestamp;
    update_counter_++;
}