#551 Extended Kalman Filter localization with vector map

G Biggs
Player (393)

This is a new position2d driver "ekfvloc" that implements EKF-based localization on a vectorial map, from book

"Mobile Robot Localization and Map Building: A Multisensor Fusion Approach"
J.A. Castellanos and J.D. Tardós
Kluwer Academic Publishers, Boston, 1999
ISBN 0-7923-7789-3

It's a good alternative when you have a very large map that would require lots of memory for the grid map, since this one uses a vector map.

The driver publishes the localization via a position2d interface and the covariance via an opaque interface. Includes documentation in the ekfvloc_driver.cc file.

The attached patch has been taken from the playerstage/code/player/tags/release-3-0-1/server/drivers/position folder with svn diff.


  • Alejandro R. Mosteo

    Diff to apply in the player/server/drivers/position folder

  • Alejandro R. Mosteo

    • labels: --> Player
  • Alejandro R. Mosteo

    • assigned_to: nobody --> gerkey
  • Alejandro R. Mosteo

    I forgot to set the category, and thus it was unnassigned. I observed while adding another patch that gerkey is the assignee for player patches, so I'm tentatively assigning it to him.

  • Nobody/Anonymous

    Interesting thing! Please add check for STL in CMakeLists.txt before applying it as it uses STL vector.

  • Nobody/Anonymous

    Why can't it use vectormap interface? Fortunately, there's a vec2map driver in Player which translates vectormap to map interface and it is capable to process correctly PLAYER_MAP_REQ_GET_VECTOR request used by this driver.

  • Alejandro R. Mosteo

    The vectormap thing was an oversight on my part. I was aware of mapfile and vmapfile so I thought that was the path to follow.

    I don't know how to add checks for specific libraries in the CMakeLists.txt file. In fact, I'd like to add checks for gsl too. Any pointers on how to do this welcome. Just plain CMake directives?

    I have an updated version that fix some minor bugs and adds a localize interface and the ability to display covariance ellipse and feature matches in a graphics2d interface. Since I foresee that the driver may still evolve a bit in the coming weeks, perhaps the simplest would be if I had SVN write permissions for just the ekfvloc folder, when (if?) it's added. I don't know if this can be done or if all commits must go through someone in the project. Just wondering.

  • Nobody/Anonymous

    Test for STL is simple:
    PLAYERDRIVER_OPTION (ekfvloc build_ekfvloc ON)
    PLAYERDRIVER_OPTION (ekfvloc build_ekfvloc OFF "STL not found")

    Unfortunately, test for GSL is not that simple, see CMakeLists.txt for pmap utility which also uses gsl (utils/pmap/CMakeLists.txt file).

  • Nobody/Anonymous

    Fortunately, gsl provides gsl.pc file for pkg-config infrastructure. Now we can use postgis driver (from vectormap group) as a template for cmake spec file, as it uses both STL and pkg-config to find libpqxx.
    I guess your CMakeLists.txt should look like this:

    PLAYERDRIVER_OPTION (ekfvloc build_ekfvloc ON)
    PLAYERDRIVER_REJECT_OS (ekfvloc build_ekfvloc PLAYER_OS_WIN)
    PLAYERDRIVER_OPTION (ekfvloc build_ekfvloc ON)
    PLAYERDRIVER_OPTION (ekfvloc build_ekfvloc OFF "STL not found")
    PLAYERDRIVER_REQUIRE_PKG (ekfvloc build_ekfvloc gsl ekfvloc_includeDirs ekfvloc_libDirs ekfvloc_linkLibs ekfvloc_linkFlags ekfvloc_cFlags)
    ekfvloc build_ekfvloc

    INCLUDEDIRS ${ekfvloc_includeDirs}

    LIBDIRS ${ekfvloc_libDirs}

    LINKLIBS ${ekfvloc_linkLibs}

    LINKFLAGS ${ekfvloc_linkFlags}



  • Toby Collett

    Toby Collett - 2010-05-03
    • assigned_to: gerkey --> gbiggs
  • Alejandro R. Mosteo

    Thanks for these CMake clues. I've added them and it indeed works.

    The driver has received extensive testing as of late. The up-to-date version is at


  • G Biggs

    G Biggs - 2010-06-02

    Please submit a new patch based on your latest version of the driver.

    Alternatively (and preferably), make the functional part of the driver a library with a nice API, and submit it to a project such as Gearbox, so that it is useful outside of Player. Then submit a patch for a driver that will interface to this library.

    Either approach will be accepted into Player, but we prefer the second.

  • Alejandro R. Mosteo

    Well, indeed my first purpose was to follow the gearbox->player route, and the code is already in the form of library+driver. However, there are always time constraints, and we don't use gearbox and I didn't want to introduce a new dependency in our toolchain.

    This has changed lately, since we now have some Hokuyos and these require gearbox anyway. Thus, I guess the best option is to go the long way around as you suggest. In any case, anyone interested in the driver right now can get it from the git repository until it's properly integrated.

  • Rich Mattes

    Rich Mattes - 2010-07-08

    Have you made any progress integrating the driver into Gearbox?

  • Alejandro R. Mosteo

    Not really; basically I haven't had the time to package and submit it yet.

  • Thimo Langbehn

    Thimo Langbehn - 2010-08-30

    Here is a git patch to adapt the ekvloc driver to the extended localization interface:

    diff --git a/ekfvloc_driver.cc b/ekfvloc_driver.cc
    index df1e4a8..17173f0 100644
    --- a/ekfvloc_driver.cc
    +++ b/ekfvloc_driver.cc
    @@ -947,7 +947,8 @@ void Ekfvloc::PublishInterfaces(double timestamp)
    // Publish localize data
    const Matrix cov = localize_.GetCovariance();
    player_localize_hypoth_t hyp[1] =
    - {{{new_pose.x, new_pose.y, new_pose.th}, {cov(0, 0), cov(1, 1), cov(2, 2)}, 1.0}};
    + {{{new_pose.x, new_pose.y, new_pose.th}, {cov(0, 0), cov(1, 1), cov(2, 2),
    + cov(0,1), cov(1,2), cov(0,2) }, 1.0}};
    player_localize_data_t loc_data = { 0, timestamp, 1, hyp};

  • Alejandro R. Mosteo

    2010.12.10: svn diff taken from the 'drivers' folder against current trunk

  • Alejandro R. Mosteo

    I'm finally going to investigate the gearbox contribution requirements. In the meantime, I have uploaded an up-to-date version that should cleanly compile against player trunk.

  • Rich Mattes

    Rich Mattes - 2010-12-12

    Thanks for updating the patch, I've applied it to SVN trunk.

  • Rich Mattes

    Rich Mattes - 2010-12-12
    • status: open --> closed-accepted
  • Nobody/Anonymous

    M764CY <a href="http://flqrwosbtjcl.com/">flqrwosbtjcl</a>, [url=http://ssxzsgqcdkyu.com/]ssxzsgqcdkyu[/url], [link=http://zzryzyzvnelr.com/]zzryzyzvnelr[/link], http://gamlalqgpzoq.com/


Log in to post a comment.

Get latest updates about Open Source Projects, Conferences and News.

Sign up for the SourceForge newsletter:

JavaScript is required for this form.

No, thanks