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_);
}
}
}
void RBPFSlamCore::updateFilter(CActionCollectionPtr _action, CSensoryFramePtr _sf) {
if(state_ == INIT) initializeFilter();
tictac_.Tic();
timeLastUpdate_ = _sf->getObservationByIndex(0)->timestamp;
update_counter_++;
}