Understanding the Landmark Observation Concept

[ghost]

Dear all, I am about to evaluate cartographers ability to integrate landmark observations into the optimized graph. There is for now no documentation about how to integrate landmarks. I only found this github question/issue: #1046

So my questions are:

• Does cartographer expect the landmarks pose in world frame to be known? Or is it sufficient to only provide relative observations relative to the robot?

• I am currently replaying #1046 but do not know how to check if this works at all. How can I see that the published landmarks are processed or ignored etc?

Thanks in advance!

[ojura]

You provide observations relative to the robot. Check out LandmarkEntry.msg and LandmarkList.msg. If you enable landmarks in the .lua file, you have to provide a list of landmarks (LandmarkList message) that your robot is currently observing (can be empty), at a sample rate comparable to your other sensors (lidar/odometry).

[ghost]

@ojura , thank you! So I cannot publish the rostopic pub /landmark cartographer_ros_msgs/LandmarkList from time to time? And are there any entries in the log files to see the landmarks being processed? If I publish them via

rostopic pub -r 10 /landmark cartographer_ros_msgs/LandmarkList ' { header: { frame_id: "base_laser_link" }, landmarks :[ { id: "0", tracking_from_landmark_transform : { position: { x: 2.97, y: 0.53, z: 0 }, orientation: { x: 0, y: 0, z: 0.999, w: -0.0023 } }, translation_weight : 0.0, rotation_weight : 0.0 } ] } '

the pose of the robot and the map does not change

[ojura]

I don’t see the point of publishing a constant landmark observation. That would mean that the robot is not moving relative to the landmark.

Also, the translation and rotation weights of zero mean that effectively your landmark observations are not used. Try using values like 1e1 or 1e5 for the weights.

You can visualize the inferred global positions of landmarks, check out the landmark nodes topic published by the Cartographer node.

Edit: note that your robot has to keep moving in order for trajectory optimization to happen, which is when your landmark observations are used. If your robot is standing still, then the trajectory is not being built, and optimization is not triggered.

[MichaelGrupp]

Linking #909 here for reference.

[MichaelGrupp]

If I publish them via

rostopic pub -r 10 /landmark cartographer_ros_msgs/LandmarkList ' { header: { frame_id: "base_laser_link" }, landmarks :[ { id: "0", tracking_from_landmark_transform : { position: { x: 2.97, y: 0.53, z: 0 }, orientation: { x: 0, y: 0, z: 0.999, w: -0.0023 } }, translation_weight : 0.0, rotation_weight : 0.0 } ] } '

the pose of the robot and the map does not change

Try the following to see an impressive effect (only for testing):

• set the landmark weights to a very high value (e.g. 1e8)
• publish your landmark with id "0"
• move the robot to another place
• again publish the exact same landmark observation with id "0"

The second relative observation will of course be wrong because the robot has moved. Due to the high weights of the landmark, the robot pose should jump because the pose graph optimization tries to keep the global pose of the landmark consistent.

An exaggerated example where I sent 4 fake landmarks repeatedly with random transformations and high weights to completely trash the pose graph (the robot was actually driving a straight line):

In reality, your observations should of course only have small errors ;)

[ghost]

Hi everyone, thank you very much for your comments! @MichaelGrupp thanks for the step-by-step instructions. However, it does not look like cartographer accepts my published landmarks.

@ojura , thank you for the hint with the /landmark_poses_list topic published by cartographer for debugging purpose. It remains empty in my case.

Ok, so I will try to approach the possible error on my (user) side: Here is the lua config file I use for cartographer: https://gist.github.com/mojovski/bb14bdeb6ccb117ece0263caa6b747a9

And this is the new publishing command with new frame_id and the weights:

rostopic pub  /landmark cartographer_ros_msgs/LandmarkList ' { header: { stamp: now, frame_id: "imu_link" }, landmarks :[ { id: "0", tracking_from_landmark_transform : { position: { x: 1, y: 0, z: 0 }, orientation: { x: 0, y: 0, z: 0, w: 1 } }, translation_weight : 99999999.0, rotation_weight : 999999999.0 } ] } '

Then I follow the instructions from @MichaelGrupp

• publish the landmark with the above command
• move the robot to another place
• again publish the exact same landmark with the command above

Does anyone see an issue in these? Is it correct to publish the landmark in the imu_link frame, which is the tracking_frame defined in the .lua config file?

If this doesn't help, I can also provide a minimal in-docker project for reproduction.

Thank you very much in advance!

PS: The launch file I use can be seen here: https://gist.github.com/mojovski/3f4c145a3759a87d51efea4372829320

Edit: I am using the Dockerfile.indigo from the current master branch

Update: I also added a log file in DEBUG level from cartographer_node after posting a landmark to /landmark. https://gist.github.com/mojovski/05cf30fa3ad5a419c0f7615a9be33463 But I cannot see anything strange. The connection to the landmark publisher is established, but there are no further information about stuff happening with the landmark or the socket (21 in this case)

[MichaelGrupp]

I don't have too much time to test your data atm, but you can check out this script that I have been using to test with fake landmarks: https://github.com/googlecartographer/cartographer_ros/pull/1071

[MichaelGrupp]

It might be that you have to publish to the landmarks topic at a more or less fixed frequency, even if there's no actual observation. See https://github.com/googlecartographer/cartographer_ros/issues/909#issuecomment-400308764

Is it correct to publish the landmark in the imu_link frame, which is the tracking_frame defined in the .lua config file?

The landmark pose is assumed to have been observed from the tracking frame, so this looks fine.

[ghost]

@MichaelGrupp thank you very much for the script to publish fake landmarks! Indeed it started to show some landmarks in the topic /landmark_poses_list For now, I am not sure why it failed with the simple command rostopic pub ... But I assume, that landmarks must be published in a fixed frequency. This is how I managed to "brake" the cartographer navigation accuracy by providing duplicate and wrong landmarks, in order to see its effect: python publish_fake_landmarks.py --allow_duplicate_ids --num_landmarks 5 --id_lenght 1 --translation_weight 1e19 --rotation_weight 1e19

I will evaluate the reason why this effect did not come up when using publishing from command line and come back here.

or anyone, any ideas?

Does cartographer filter landmark messages with similar values?

[MichaelGrupp]

For now, I am not sure why it failed with the simple command rostopic pub ... But I assume, that landmarks must be published in a fixed frequency.

Sounds reasonable. You could test the following: set the --sample_period of the test script to a high value, so it will only publish empty landmark messages at a fixed frequency. Then additionally send your landmarks with rostopic pub .... This should work

[ojura]

Isn't the sensor queue disabled for landmarks by default? https://github.com/googlecartographer/cartographer/blob/d0348634b2bc467da6919adbacc4211c810b8cd3/configuration_files/trajectory_builder.lua#L25

So the sampling rate shouldn't matter, landmark observations can be sparse. If you turn collating landmark observations on, then what I've said earlier is true (you have to send them at a high sample rate, even if they are empty).

[MichaelGrupp]

Oh, didn't know that - this should be documented.

https://github.com/googlecartographer/cartographer/pull/1224

However, if I only send sparse landmarks nothing happens, and I didn't change collate_landmarks.

[MichaelGrupp]

@pifon2a

[pifon2a]

https://github.com/googlecartographer/cartographer_ros/pull/1077 this might help. I uploaded a bag file with landmarks and instructions how to launch it.

https://www.youtube.com/embed/E2-OD-ycivc

[EvanYehh]

Hi all Thanks for the information about landmark issue. I downloaded the bag file in #1077 and follow the steps, it works and I can echo the /landmarks_poses_list with landmark data. But I failed to get the silimar result as #1077 when I tried to reproduce it

The steps as below

1. roslaunch pr2_gazebo pr2_wg_world.launch (mark arg gui part)
2. roslaunch cartographer_ros demo_pr2.launch (mark the bag file part and enable landmark at pr2.lua)
3. publish landmark data through publish_fake_random_landmarks.py
4. try to get /landmark_poses_list data but it always empty
5. modify script to publish fixed landmark data, still can't get /landmark_poses_list data

Here is my questions

1. The position in /landmark is relative position to robot or absolute position? ex: In map coordinate, robot position(x,y,z) is (1,2,0) and landmark is (3,3,0) Then it should publish /landmark data with position(x,y,z) = (2,1,0) or (3,3,0)?
   After robot move to (2,2,0) then /landmark data with position(x,y,z) = (1,1,0) or (3,3,0)? Or what is correct landmark position for this case?
2. Should landmark id be unique? or it is ok to have two landmarks with same id but different positions? And will landmark id be used in the /landmark_poses_list?
3. How to generate /landmark_poses_list? is there any parameter need to be config? or the data generated by script is not enough to output /landmark_poses_list
4. Could you share the /landmark data or anything helpful to reproduce case #1077 for reference?

Thanks

[ghost]

Hi @EvanYehh as far as I understand it now (still doing more tests):

1. The landmark must be in the tracking frame (named inside the lua configuration. usually imu_link).
2. The ID must be unique. You can use python uuid module to generate random unique long hashes for this.
3. The topic /landmark_poses_list is the output from cartographer, which helps to to verify if the cartographer computed the global pose of a landmark on the map frame.
4. The fake landmark script @MichaelGrupp provided is quite helpful with that.

[ghost]

Ok, from my point of view, I think I got the concept and brought also the landmark mapping into running state. If someone has questions on this, feel free to post here or a direct message.

Huge thanks to all the help especially from @pifon2a @ojura and @MichaelGrupp

[EvanYehh]

Hi @mojovski Could you share your environment/cartographer/robot settings? Especially landmark data input to cartographer node.

Many thanks

[pifon2a]

@EvanYehh https://google-cartographer-ros.readthedocs.io/en/latest/demos.html#static-landmarks

[EvanYehh]

Hi @pifon2a @mojovski I want to add a fixed-landmark at position(x,y,z)=(2,2,0), and robot start at (x,y,z)=(0,0,0). Then I modify the publish_fake_random_landmarks.py to publish one landmark only. And its id is "A", its position related to robot's current position => A - robot EX: At beginning robot a (0,0,0) then it publish /landmark id A with position (2,2,0) When robot move to (1,0,0) then it publish /landmark id A with position (1,2,0)

According to the setting, I suppose it should generate a landmark_pose_list close to (2,2,0) But it output (1.66,2.06,0) then continue moving it change to (1.09,2.06,0) as following(through rostopic echo /landmark_pose_list)

markers:

• header: seq: 0 stamp: secs: 810 nsecs: 803000000 frame_id: "map" ns: "Landmarks" id: 0 type: 2 action: 0 pose: position: x: 1.66701743519 y: 2.06956866161 z: 0.0 orientation: x: -9.98553262434e-05 y: 0.00157683814248 z: 0.0644869920552 w: 0.997917296907 scale: x: 0.2 y: 0.2 z: 0.2 color: r: 0.207129895687 g: 0.115499980748 b: 0.769999980927 a: 1.0 lifetime: secs: 0 nsecs: 0 frame_locked: False points: [] colors: [] text: '' mesh_resource: '' mesh_use_embedded_materials: False

  markers:

• header: seq: 0 stamp: secs: 810 nsecs: 806000000 frame_id: "map" ns: "Landmarks" id: 0 type: 2 action: 0 pose: position: x: 1.09779299673 y: 2.060462482 z: 0.0 orientation: x: -0.000266551819487 y: 0.00155735023078 z: 0.169975190686 w: 0.985447075272 scale: x: 0.2 y: 0.2 z: 0.2 color: r: 0.207129895687 g: 0.115499980748 b: 0.769999980927 a: 1.0 lifetime: secs: 0 nsecs: 0 frame_locked: False points: [] colors: [] text: '' mesh_resource: '' mesh_use_embedded_materials: False

This is a strange behavior. Is there anything wrong about /landmark position? Or do I need to config something else for a fixed-landmark in (2,2,0)? And that's why I need your /landmark data for reference.

Thanks

[ghost]

Hi @EvanYehh , I actually set the view values in the lua file to

options = {
  ....
  tracking_frame = "imu_link",
  use_landmarks = true,
  ....
}
TRAJECTORY_BUILDER.collate_landmarks = false  -- if landmarks are used, set this to false

I haven't adopted the fake landmark publisher but rote my own publisher from scratch, since it required some special logic about landmark detection and processing. In any case, you should make sure:

• the landmark observations published on /landmark topic are in the tracking frame (in my case it was imu_link) When you compute the landmark pose in the imu_link frame, make sure you call tf.lookupTransform with correct arguments (target, source). The returned pose transforms from the source frame_id on the target frame_id.

• I published them in a fixed period: at 10Hz, even if no landmark is visible an empty LandmarkList message is published.

Concerning the accuracy, I also having some issues with the estimated accuracy. But its far less than in your case. In general, I will open a new issue here about its integration in the optimization core, where not just the two scalar weights are considered but a full 7x7 covariance matrix. (not urgent for now).

[ghost]

One more question to the experts @pifon2a @MichaelGrupp : Are the landmark constraints also solved to the state when I call

rosservice call /write_state /map/agvstate.pbstream true

?

[pifon2a]

@mojovski yes, they are serialized in the form of observations https://github.com/googlecartographer/cartographer/blob/b7fb299d1a462b92b8fcd779a25ddd40b7e72ead/cartographer/mapping/proto/serialization.proto#L86

[ghost]

:+1: @pifon2a , thanks!

[ojura]

@mojovski, if you are providing landmark observations at 10 Hz (and not sparsely), you can turn on TRAJECTORY_BUILDER.collate_landmarks = on. The added benefit is that Cartographer will run deterministically (i.e. for a given bag, you will get identical results each time with the offline node). When collate_landmarks is off, landmark observations skip the sensor queue (so they do not block it if they are sparse) and are injected directly into the pose graph, which is not deterministic.

[EvanYehh]

Hi @mojovski Du you mean the landmark_list.header.frame_id must be the same as tracking_frame in the lua file? And is the idea to compute the landmark position correct? (it' related to robot position and maybe need to check tf to get correct one, not just a easy minus method)

Thanks

[ybkim95]

Ok, from my point of view, I think I got the concept and brought also the landmark mapping into running state. If someone has questions on this, feel free to post here or a direct message.

Huge thanks to all the help especially from @pifon2a @ojura and @MichaelGrupp

Hello @mojovski can you please send me an email to ybkim95@yonsei.ac.kr?

[GeesonCheung]

Hello, I have a new understanding of landmark based on the above information. Recently, I have been studying how to better use landmark.Now I run the script file provided by @MichaelGrupp to publish landmark with an empty landmark with an interval of 0.1s and a sample landmark with an interval of 5s.Althoght landmark was displayed on the map when building the map,landmark‘s constraints were not seen, all constraints were generated by scan and submap, which was inconsistent with video display provided by @pifon2a . All constraints on video were established with landmark.

In my opinion, landmark must have constraints in order for it to work. I beg your help.

Here are my lua files: `include "map_builder.lua" include "trajectory_builder.lua"

options = { map_builder = MAP_BUILDER, trajectory_builder = TRAJECTORY_BUILDER, map_frame = "map", tracking_frame = "i_imu", published_frame = "i_base_link", odom_frame = "odom", provide_odom_frame = false, publish_frame_projected_to_2d = false, use_odometry = true, use_nav_sat = false, use_landmarks = true, num_laser_scans = 1, num_multi_echo_laser_scans = 0, num_subdivisions_per_laser_scan = 1, num_point_clouds = 0, lookup_transform_timeout_sec = 0.2, submap_publish_period_sec = 0.3, pose_publish_period_sec = 5e-3, trajectory_publish_period_sec = 30e-3, rangefinder_sampling_ratio = 1., odometry_sampling_ratio = 1., fixed_frame_pose_sampling_ratio = 1., imu_sampling_ratio = 1., landmarks_sampling_ratio = 1., }

TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 1 MAP_BUILDER.use_trajectory_builder_2d = true

TRAJECTORY_BUILDER_2D.submaps.num_range_data = 35--35 TRAJECTORY_BUILDER_2D.min_range = 0.1 TRAJECTORY_BUILDER_2D.max_range = 30. TRAJECTORY_BUILDER_2D.missing_data_ray_length = 1. TRAJECTORY_BUILDER_2D.use_imu_data = true

TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.1 TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.translation_delta_cost_weight = 10. TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.rotation_delta_cost_weight = 1e-1 POSE_GRAPH.constraint_builder.min_score = 0.4--0.55 0.4 POSE_GRAPH.constraint_builder.global_localization_min_score = 0.5 --0.8 0.5 POSE_GRAPH.optimization_problem.huber_scale = 1e2 POSE_GRAPH.optimize_every_n_nodes = 35--35 POSE_GRAPH.constraint_builder.min_score = 0.65

return options`

[GeesonCheung]