void PFLocalizationCore::updateFilter(
CActionCollection::Ptr _action, CSensoryFrame::Ptr _sf)
{
if (state_ == INIT) initializeFilter();
tictac_.Tic();
pf_.executeOn(pdf_, _action.get(), _sf.get(), &pf_stats_);
time_last_update_ = _sf->getObservationByIndex(0)->timestamp;
update_counter_++;
}
void PFLocalizationCore::observation(
CSensoryFrame::Ptr _sf, CObservationOdometry::Ptr _odometry)
{
auto action = CActionCollection::Create();
CActionRobotMovement2D odom_move;
odom_move.timestamp = _sf->getObservationByIndex(0)->timestamp;
if (_odometry)
{
if (odom_last_observation_.empty())
{
odom_last_observation_ = _odometry->odometry;
}
mrpt::poses::CPose2D incOdoPose =
_odometry->odometry - odom_last_observation_;
odom_last_observation_ = _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_);
}
}
}