Position in point cloud

Question

Hello,

I'm currently learning to work with ros and use it to build a mapping tool. I'm trying to get the current pose out of the RTABMAP algoritme, with only RGBD data.

I've mapped the room where I want to localize myself. And made a picture with a ground floor of the area. On the ground floor there is a red dot showing the location of the "camera"/"person" and an arrow with the view direction (yaw). This all works while building a map, until i start the localization.

I suppose that i'm not currently on a known position on the map, so i press reset odomotry from the menu. Then it takes the program some time to match the map with my current view, but it works perfectly. Yet the location of the person is all wrong.

Topic I'm using for the data : /rtabmap/odom

I guess the problem lies with me not using the topics as i should, because i get data in a different scale.

Output of topic before localization and reset odom:

POSITION FROM TOPIC  x: 0.012990 y: -0.394857
POSITION FROM TOPIC  x: 0.014069 y: -0.384452
POSITION FROM TOPIC  x: 0.011949 y: -0.382053
POSITION FROM TOPIC  x: 0.012687 y: -0.372685
POSITION FROM TOPIC  x: 0.013236 y: -0.370237
POSITION FROM TOPIC  x: 0.011633 y: -0.360908
POSITION FROM TOPIC  x: 0.004766 y: -0.361776

Output after reset and localization in map:

POSITION FROM TOPIC  x: 0.000000 y: 0.000000
POSITION FROM TOPIC  x: -0.000545 y: -0.001833
POSITION FROM TOPIC  x: -0.003815 y: 0.006487

As i noticed the position X and Y are 10 times smaller, so when i use the scale factor ( devide by 0.005) i always get a zero, while before the reset i got the result i wished for.

EDIT

Output of rosrun tf tf_echo /map /base_link

Failure at 1461013681.102334178
Exception thrown:"base_link" passed to lookupTransform argument source_frame does not exist. 
The current list of frames is:
Frame camera_depth_optical_frame exists with parent camera_depth_frame.
Frame camera_depth_frame exists with parent camera_link.
Frame camera_link exists with parent odom.
Frame odom exists with parent map.
Frame camera_rgb_frame exists with parent camera_link.
Frame camera_rgb_optical_frame exists with parent camera_rgb_frame.

Output of $ rosrun tf tf_echo /map /camera_link

Failure at 1461013479.628889493
Exception thrown:Lookup would require extrapolation into the past.  Requested time 1459949359.363947774 but the earliest data is at time 1460911947.649899845, when looking up transform from frame [camera_link] to frame [map]
The current list of frames is:
Frame camera_depth_frame exists with parent camera_link.
Frame camera_link exists with parent odom.
Frame camera_rgb_frame exists with parent camera_link.
Frame camera_depth_optical_frame exists with parent camera_depth_frame.
Frame camera_rgb_optical_frame exists with parent camera_rgb_frame.
Frame odom exists with parent map.

Output of $ rosrun tf tf_echo /odom /camera_link

At time 1459949360.228
- Translation: [0.119, -0.752, 0.062]
- Rotation: in Quaternion [0.042, -0.056, -0.297, 0.952]
            in RPY (radian) [0.113, -0.083, -0.610]
            in RPY (degree) [6.500, -4.736, -34.952]

Output of rosrun tf tf_echo /map /odom

At time 1460911972.251
- Translation: [0.000, -0.000, 0.000]
- Rotation: in Quaternion [-0.000, 0.000, 0.000, 1.000]
            in RPY (radian) [-0.000, 0.000, 0.000]
            in RPY (degree) [-0.000, 0.000, 0.000]
At time 1460911973.249
- Translation: [-0.000, -0.000, 0.000]
- Rotation: in Quaternion [-0.000, 0.000, 0.000, 1.000]
            in RPY (radian) [-0.000, 0.000, 0.000]
            in RPY (degree) [-0.000, 0.000, 0.000]

Like you can see the camera_link form /odom gives a good position with a 3 digit value. The /odom from the /map gives me a value smaller then 3 digits, hence the -0.000 and 0.000 (i didn't add the minus sign)

---

Originally posted by anonymous24903 on ROS Answers with karma: 20 on 2016-04-17

Post score: 0

Answer 1

Hi,

You should use TF /map -> /camera_link. When reseting odometry, it restarts to pose (0,0). The poses from /rtabmap/odom are in /odom frame, not /map frame. What you want are the coordinates in /map frame (including localization):

$ rosrun tf tf_echo /map /camera_link

EDIT

The $ rosrun tf tf_echo /map /camera_link should have worked, but it seems you have a problem with the clock. Your requested time is 1459949359.363947774 (06 Apr 2016 13:29:19 GMT) and the earliest TF frame is 1460911947.649899845 (17 Apr 2016 16:52:27 GMT). Do you use a rosbag? If so, don't forget to set use_sim_time to true and do $ rosbag play --clock my_bag.bag.

The output of $ rosrun tf tf_echo /map /odom is ok, in your case it may be Identity transform (0 everywhere) as no loop closures (or relocalization) were detected yet. For example, if the robot didn't close a loop yet (odometry not yet corrected), /map -> /odom will be Identity.

Note that you can show up /TF in RVIZ to help visualize the transforms (set general option "Fixed Frame" to /map). You can see also the TF tree with:

$ rosrun tf view_frames
$ evince frames.pdf

cheers

---

Originally posted by matlabbe with karma: 6409 on 2016-04-17

This answer was ACCEPTED on the original site

Post score: 1

---

Original comments

Comment by anonymous24903 on 2016-04-18:
I'll try to get the position out of the /TF. Is there a rescalling in the information provided by the TF frame? Because the information isn't the same as the odom, before the reset.

Comment by matlabbe on 2016-04-18:
There is no scaling. The information provided in /rtabmap/odom should be the same as TF /odom -> /base_link (well, the same as the frames in the header of the odom topic). On localization, /map->/odom will be updated so that /map -> /base_link will contain the pose in map coordinates.

Comment by anonymous24903 on 2016-04-18:
I just tried it with the base_link with this output: Exception thrown:"base_link" passed to lookupTransform argument source_frame does not exist. The current list of frames is: Frame camera_link exists with parent odom. Frame odom exists with parent map.

Comment by anonymous24903 on 2016-04-18:
When i try to us the /odom from /map i get coordinates in a strange scale ... but it's the only one available by the rtabmap /tf

Comment by matlabbe on 2016-04-18:
Try $ rosrun tf tf_echo /map /camera_link instead to see camera pose in /map frame and $ rosrun tf tf_echo /odom /camera_link to see camera pose in /odom frame. Not sure to understand your scale problem...

Comment by anonymous24903 on 2016-04-18:
I added the output of the commands to the question with the later being the /map /odom where i sense a scale problem because it's zero (but actually really small, compared to the /odom /camera_link). While everything works fine with the /odom /camera_link before i reset odometry ...

Comment by anonymous24903 on 2016-04-19:
Now it works !! The use_sim_time and the --clock made the /map /camera_link work... Thank you very much! and Cheers !

But i don't understand why that was the problem?

Comment by matlabbe on 2016-04-20:
If you are using a rosbag, then time should be simulated so that TF stamps and stamps in rosbag's topics match together.