ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange

Position in point cloud

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 ...
edit retag close merge delete

Sort by ยป oldest newest most voted

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

more

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.

( 2016-04-18 11:37:05 -0500 )edit

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.

( 2016-04-18 12:38:26 -0500 )edit

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.

( 2016-04-18 14:25:07 -0500 )edit

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

( 2016-04-18 14:25:59 -0500 )edit

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

( 2016-04-18 14:48:10 -0500 )edit

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

( 2016-04-18 16:12:43 -0500 )edit

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?

( 2016-04-19 02:05:18 -0500 )edit

If you are using a rosbag, then time should be simulated so that TF stamps and stamps in rosbag's topics match together.

( 2016-04-20 07:51:47 -0500 )edit