landmark based map support on all algorithms

[Magnusgaertner]

Hi,
the ekf_slam_2d wrapper already supports landmarks (2d points, no image landmarks, but identified objects with a large distance ~3meters from each other) as input, but other wrappers e.g rbpf_slam do not. How can I modify the wrappers to get landmarks working? I added a subscriber and callback for landmarks:

 void PFslamWrapper::landmarkCallback(
    const mrpt_msgs::ObservationRangeBearing& _msg) {
#if MRPT_VERSION >= 0x130
  using namespace mrpt::maps;
  using namespace mrpt::obs;
#else
  using namespace mrpt::slam;
#endif

  CObservationBearingRange::Ptr landmark = CObservationBearingRange::Create();
  if (landmark_poses_.find(_msg.header.frame_id) == landmark_poses_.end()) {
    updateSensorPose(_msg.header.frame_id);
  } else {
    sf = CSensoryFrame::Create();
    CObservationOdometry::Ptr odometry;
    odometryForCallback(odometry, _msg.header);

    CObservation::Ptr obs = CObservation::Ptr(landmark);
    sf->insert(obs);
    observation(sf, odometry);
    timeLastUpdate_ = sf->getObservationByIndex(0)->timestamp;

    tictac.Tic();
    mapBuilder->processActionObservation(*action, *sf);
    t_exec = tictac.Tac();
    ROS_INFO("Map building executed in %.03fms", 1000.0f * t_exec);

    publishMapPose();
    run3Dwindow();
  }
}

changed the config file to:

# ========================================================
#            MULTIMETRIC MAP CONFIGURATION
# See docs for (Google for) mrpt::maps::CMultiMetricMap
# ========================================================
# Creation of maps:
occupancyGrid_count=0
gasGrid_count=0
landmarksMap_count=1
beaconMap_count=0
pointsMap_count=0

# Selection of map for likelihood: (fuseAll=-1,occGrid=0, points=1,landmarks=2,gasGrid=3)
likelihoodMapSelection=2

but now I am stuck at figuring out which map actually holds the landmarks, I tried:

metric_map_->m_landmarksMap

However m_landmarksMap does not seem to be the right map as it does not process the landmarks at all. So my question is basically, what do I miss/ which map could I use to use the rbpf slam with non identifyable landmarks?

Best Regards
Magnus

[jlblancoc]

Hi Magnus,

No, you didn't miss anything.

It's simply that I never had the motivation/time to implement landmarks-based rbpf-slam. It was on my "wish list" during a long time, but "state of the art" in SLAM quickly moved forward and attention for this kind of solutions dropped.

From what I recall, the optimal approach would be implementing the "FASTSLAM 2.0" algorithm (paper) as the "optimal PF algorithm".
However, a simpler working solution would be possible: just add support for insertObservation() and computeObservationLikelihood() in the landmarks map for the range-bearing observation.

As you can see in the observations/map matrix table, CObservationBearingRange is shamelessly ignored by all metric maps right now (!!).

Implementing both methods above, the particle filter algorithm named "Standard Proposal" will work, despite not being optimal.

Cheers!

[JadBatmobile]

Hey Magnaes Gater,

Did you implement this ?
:)))

[Magnusgaertner]

nope, sorry...