#551 Extended Kalman Filter localization with vector map

closed-accepted
G Biggs
Player (393)
5
2010-12-12
2010-02-09
No

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.

Discussion

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

     
    Attachments
    • labels: --> Player
     
    • assigned_to: nobody --> gerkey
     
  • 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.

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

     
  • 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.

     
  • 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.

     
  • Test for STL is simple:
    IF (HAVE_STL)
    PLAYERDRIVER_OPTION (ekfvloc build_ekfvloc ON)
    ELSE (HAVE_STL)
    PLAYERDRIVER_OPTION (ekfvloc build_ekfvloc OFF "STL not found")
    ENDIF (HAVE_STL)

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

     
  • 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)
    IF (HAVE_STL)
    PLAYERDRIVER_OPTION (ekfvloc build_ekfvloc ON)
    ELSE (HAVE_STL)
    PLAYERDRIVER_OPTION (ekfvloc build_ekfvloc OFF "STL not found")
    ENDIF (HAVE_STL)
    PLAYERDRIVER_REQUIRE_PKG (ekfvloc build_ekfvloc gsl ekfvloc_includeDirs ekfvloc_libDirs ekfvloc_linkLibs ekfvloc_linkFlags ekfvloc_cFlags)
    PLAYERDRIVER_ADD_DRIVER(
    ekfvloc build_ekfvloc

    INCLUDEDIRS ${ekfvloc_includeDirs}

    LIBDIRS ${ekfvloc_libDirs}

    LINKLIBS ${ekfvloc_linkLibs}

    LINKFLAGS ${ekfvloc_linkFlags}

    CFLAGS
    -Wall
    -pedantic
    -g
    ${ekfvloc_cFlags}

    SOURCES
    ekfvloc_driver.cc
    feature.cc
    hregions.cc
    localize.cc
    params.cc
    robot_location.cc
    scan.cc
    segment_map.cc
    sub_opt.cc
    transf.cc
    uloc.cc
    )

     
  • Toby Collett
    Toby Collett
    2010-05-03

    • assigned_to: gerkey --> gbiggs
     
  • 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

    git://github.com/mosteo/ekfvloc.git

     
  • 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.

     
  • 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?

     
  • 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};
    Publish(loc_addr_, PLAYER_MSGTYPE_DATA, PLAYER_LOCALIZE_DATA_HYPOTHS,
    static_cast<void*>(&loc_data));

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

     
    Attachments
  • 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
     
  • M764CY <a href="http://flqrwosbtjcl.com/">flqrwosbtjcl</a>, [url=http://ssxzsgqcdkyu.com/]ssxzsgqcdkyu[/url], [link=http://zzryzyzvnelr.com/]zzryzyzvnelr[/link], http://gamlalqgpzoq.com/