Ask Your Question
0

Rtab-map : unexpected occupancy grid map

asked 2020-10-15 13:58:11 -0500

mirella melo gravatar image

updated 2020-10-20 15:16:04 -0500

Hi, I just started using Rtabmap and I'm having an unexpected 2D occupancy grid map. First, here are the parameters I'm setting in the launch file:

parameters=[{
      'frame_id':'camera_link',
      'subscribe_depth':False,
      'subscribe_rgbd':False,
      'subscribe_stereo':True,
      'map_empty_ray_tracing':True,
      'map_always_update':True,
      'approx_sync':False}]

Here is my output in the 'Rtarviz' viewer. There is the 3D map, which honestly, I don't need it - the 2D grid map is all I'm interested in. By the way, the 3D map generated is duplicated, any idea why this is happening?

image description

I subscribed to the topic '/map' in Rviz, then the map generated starts detecting the wall pretty well, but then all became black. . image description

This is a virtual scene, the simple as possible, and with lots of features to be detected (sample of the frames can be observed on the left side). Besides the strange behavior of the grid map became all 'black', it is been built on the opposite side of the walk direction of the camera (points in red are the odometry). By the way, the ray tracer option is set to true, but no ray is forming... =/

Really appreciate any help with that. Thanks!

UPDATE: I was publishing only the odometry to the topic /odom, so I just added this piece of code in order to link odom->base_link:

std::shared_ptr<tf2_ros::TransformBroadcaster> odom_broadcaster;
odom_broadcaster = std::make_shared<tf2_ros::TransformBroadcaster>(node);
odom_trans.header.stamp = node->now();
odom_trans.header.frame_id = "odom";
odom_trans.child_frame_id = "base_link";
odom_trans.transform.rotation.x = transform_tf.getRotation().getX();
odom_trans.transform.rotation.y = transform_tf.getRotation().getY();
odom_trans.transform.rotation.z = transform_tf.getRotation().getZ();
odom_trans.transform.rotation.w = transform_tf.getRotation().getW();
odom_trans.transform.translation.x =  transform_tf.getOrigin().getX();
odom_trans.transform.translation.y =  transform_tf.getOrigin().getY();
odom_trans.transform.translation.z =  0.0;
odom_broadcaster->sendTransform(odom_trans);

And regarding de odometry ROS msg, I have this:

odom_msg_.header.stamp = odom_trans.header.stamp;
odom_msg_.header.frame_id = "odom";
odom_msg_.child_frame_id = "base_link";
odom_msg_.pose.pose.orientation.x = transform_tf.getRotation().getX();
odom_msg_.pose.pose.orientation.y = transform_tf.getRotation().getY();
odom_msg_.pose.pose.orientation.z = transform_tf.getRotation().getZ();
odom_msg_.pose.pose.orientation.w = transform_tf.getRotation().getW();

odom_msg_.pose.pose.position.x = transform_tf.getOrigin().getX();
odom_msg_.pose.pose.position.y = -transform_tf.getOrigin().getZ();
odom_msg_.pose.pose.position.z = 0.0;
odometry_pub_->publish(odom_msg_);

New output:

image description

edit retag flag offensive close merge delete

Comments

can you share the resulting database (~/.ros/rtabmap.db)? It could be that there is a missing optical transformation for the camera. If rtabmap thinks the camera is looking up, it may add only obstacles.

matlabbe gravatar image matlabbe  ( 2020-10-17 20:35:43 -0500 )edit

Sure! i just added to this link. Let me know anything else. Thanks for your time!

mirella melo gravatar image mirella melo  ( 2020-10-17 21:00:12 -0500 )edit

The quaternion looks ok. The Tf tree seems odd to me though, you have map -> base_link -> camera_link, but it should be more like map -> odom -> base_link -> camera_link. Make sure you set map fixed frame in global options of rviz to correctly visualize in map referential. Which odometry node are you using?

matlabbe gravatar image matlabbe  ( 2020-10-20 09:18:03 -0500 )edit

Yes, the fixed frame is set to map. Regarding my node: I was publishing the odometry in the topic /odom, but just now I also added a TransformBroadcaster. I updated the main question again due to space in here. Thanks!

mirella melo gravatar image mirella melo  ( 2020-10-20 15:05:54 -0500 )edit

odom->base_link should be published by an odometry node, not a static transform unless the robot is not moving.

matlabbe gravatar image matlabbe  ( 2020-10-20 15:38:06 -0500 )edit

Ok. I can see that I actually have two issues, one related to camera orientation (which you already give me the solution, and thank you for that!); The other is related to my odometry node and my non-knowledge regarding frame transformation. So I stopped to study it, and I had some advances in that, but I'm still missing something. Since this is another topic, I'm asking for help with another question. Your answers were fundamental since then. Thank you so much!

mirella melo gravatar image mirella melo  ( 2020-10-21 12:05:32 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
2

answered 2020-10-17 21:18:23 -0500

matlabbe gravatar image

updated 2020-10-17 21:19:04 -0500

The problem is that there is a missing optical rotation. rtabmap thinks the camera is looking up. image description

The frame_id is set to camera_link. I don't know if it is on a robot or it is a single camera in simulation, for a robot, you may add in the URDF the optical rotation. Otherwise if camera_link is your only frame, in ros1 we would add a static transform publisher in the launch file like this:

<node pkg="tf" type="static_transform_publisher" name="camera_optical_rotation"
        args="0 0 0 -1.5707963267948966 0 -1.5707963267948966 base_link camera_link 100" />

Then use frame_id set to base_link for rtabmap node.

edit flag offensive delete link more

Comments

Thank you for answering! I'm using ROS2, and I added to my launch file.:

        Node(
             package='tf2_ros', node_executable='static_transform_publisher',
             arguments=["0.0", "0.0", "0.0", "-1.57.0", "0.0", "-1.57", "base_link", "camera_link"]

Then, the error Could not convert stereo msgs! Aborting rtabmap update... and this warning:

     [ WARN] MsgConversion.cpp:1385::getTransform() (getting transform camera_link -> base_link) Could not find a connection between 'camera_link' and 'base_link' because they are not part of the same tree.Tf has two or more unconnected trees.

Regarding the publish rate, which you suggested 100Hz, from here, I can't input this info. Trying to figure it out, and I`ll let you know the news. Thanks!!

mirella melo gravatar image mirella melo  ( 2020-10-18 14:23:58 -0500 )edit

Make sure base_link and camera_link is published by the static_transform_publisher(https://index.ros.org/doc/ros2/Tutori...):

ros2 run tf2_ros tf2_echo base_link camera_link
matlabbe gravatar image matlabbe  ( 2020-10-18 15:05:56 -0500 )edit

It says:

    Exception thrown: Could not find a connection between 'base_link' and 'camera_link' because they are not part of the same tree.Tf has two or more unconnected trees.
    The current list of frames is:
    Frame camera_link exists with parent map.
mirella melo gravatar image mirella melo  ( 2020-10-18 17:59:26 -0500 )edit

With ros2 run rqt_console rqt_console, you can know from which node that exception is thrown. If camera_link has parent map, then frame_id is not correctly set to base_link. Maybe static_transform_publisher node is not publishing base_link for some reason. You could make a simple launch file with only that node, and check if the TF is published with tf2_echo.

matlabbe gravatar image matlabbe  ( 2020-10-19 11:44:27 -0500 )edit

view_frames tool can also be used to output the Tf Tree for debugging.

ros2 run tf2_tools view_frames.py
matlabbe gravatar image matlabbe  ( 2020-10-19 12:20:45 -0500 )edit

Hi! I could not run tf2_tools for ros2 (neither dashing nor eloquent), but lately, I discovered that it can be done by RVIZ. \o/ The odometry frame was set to camera_link, which was messing up everything since then!! I changed to base_link, just like the frame_id of the images and camera parameters. I just updated the main question to show the new map formed, which is almost there! Not sure if this is a rotation "issue" or the odometry's frame_id (set to base_link). Reeeally thanks for helping with this!

mirella melo gravatar image mirella melo  ( 2020-10-20 00:48:51 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2020-10-15 13:58:11 -0500

Seen: 86 times

Last updated: Oct 20