How to properly use landmarks during mapping and localization

Recently I've been trying to get cartographer to use landmarks in order to improve localization in challenging environments, e.g. long passageways with not many features for the laser scanners. The perfect application for landmarks - so I thought.

I quickly pieced together a gazebo simulation with a very mean environment, where the robot is between two long walls and has no other features to localize against.

With a dummy script I frequently publish three landmarks at a location constant in the odom frame (odom->base_link is provided by an ekf. Landmarks get published in the robot's base_link frame). I've set the translation weight very large in order to make sure to see an effect, as proposed in https://github.com/cartographer-project/cartographer_ros/issues/1067#issuecomment-430693289.

e.g. the message looks like this:

---
header:
  seq: 4087
  stamp:
    secs: 420
    nsecs:  96000000
  frame_id: "base_link"
landmarks:
  -
    id: "0"
    tracking_from_landmark_transform:
      position:
        x: 0.21412764582658875
        y: 2.2409702483619682
        z: 1.0
      orientation:
        x: 0.0
        y: 0.0
        z: 0.4858772003659604
        w: 0.8740270854867921
    translation_weight: 100000000.0
    rotation_weight: 0.0
  -
    id: "1"
    tracking_from_landmark_transform:
      position:
        x: 1.9128069791879587
        y: 1.1852768637038231
        z: 1.0
      orientation:
        x: 0.0
        y: 0.0
        z: 0.4858772003659604
        w: 0.8740270854867921
    translation_weight: 100000000.0
    rotation_weight: 0.0
  -
    id: "2"
    tracking_from_landmark_transform:
      position:
        x: -1.0479194568090173
        y: -1.6842351106898439
        z: 1.0
      orientation:
        x: 0.0
        y: 0.0
        z: 0.4858772003659604
        w: 0.8740270854867921
    translation_weight: 100000000.0
    rotation_weight: 0.0

From the video in https://google-cartographer-ros.readthedocs.io/en/latest/demos.html#static-landmarks I would expect to see the landmarks immediately with constraints to the landmarks growing while the robot moves around. However, the landmarks take a very long time before they even get published (in the /landmark_poses_list topic). Then, they get added once and I don't see any constraints connected. When I run the rosbag-demo as noted in the documentation related to that video, behavior seems similar. While in the video the landmark poses get updated continuously, in my execution they hardly move ever.

To take things to a test, I disabled using odometry information in cartographer. With that, landmarks where obviously the only localization source for the robot to get longitudinal information along the two side walls. As soon as I move the robot along the walls, its pose gets wrong inside rviz and the raw poses published in the odom frame wander off from the published landmark poses. Apparently, cartographer does not use the landmark information to continuously localize itself. So, as base_link wanders away from odom, map does not wander away from base_link. If I only use this longitudinal motion after gazebo startup, cartographer doesn't provide any landmark_poses ever.

I would expect that the landmarks would help the robot to continuously know its location which could help overcoming passages with little sensor information. While my example is certainly over-exaggerated such a feature would be very handy in some real-life applications.

My full demo-setup is available at https://github.com/fmauch/mir_cartographer_test with just two launch files away in case someone wants to reproduce it.

Is this expected behavior, do I misunderstand anything or am I doing something wrong?

cartographer-project / cartographer_ros

How to properly use landmarks during mapping and localization #1680