Could not get transform from odom to camera_depth_optical_frame, RTAB Realsense R200 Issue

[Gabe]

Hi,

I am trying to run RTAB on outdoor bag files we took with the Realsense R200 camera on a Clearpath Jackal. I ran the following code to record the bag files:

rosbag record -O justjackal.bag --split 1024 /bluetooth_teleop/joy /camera/camera_nodelet_manager/bond /camera/depth/camera_info /camera/depth/image /camera/depth/image_raw /camera/depth/points /camera/driver/parameter_descriptions /camera/driver/parameter_updates /camera/rectify_color/parameter_descriptions /camera/rectify_color/parameter_updates /camera/rgb/camera_info /camera/rgb/image_raw /cmd_drive /cmd_vel front/scan /joint_states /odometry/filtered /rosout /rosout_agg /status /tf /twist_marker_server/update /twist_marker_server/update_full /wifi_connected

I then run "rosparam set use_sim_time true" and "rosbag play --clock justjackal_0.bag", and then run the following RTAB launch code:

roslaunch rtabmap_ros rgbd_mapping.launch rtabmap_args:="--delete_db_on_start" odom:=/odometry/filtered rgb_topic:=/camera/rgb/image_raw depth_registered_topic:=/camera/depth/image_raw camera_info_topic:=/camera/rgb/camera_info queue_size:=10 RGBD/AngularUpdate:=0.01 RGBD/LinearUpdate:=0.01 Rtabmap/TimeThr:=700 Mem/RehearsalSimilarity:=0.45 RGBD/OptimizeFromGraphEnd:=true visual_odometry:=false odom_topic:=/odometry/filtered rtabmapviz:=true rviz:=false subscribe_scan:=true scan_topic:=/front/scan compressed:=true config_path:=~/august_2_megido_jackal19/justjackal frame_id:=odom

But I get the following warnings and messed up RTAB map:

[ WARN] [1470216120.608702263, 1470155297.373358939]: rtabmapviz: Could not get transform from odom to camera_depth_optical_frame after 0.200000 seconds (for stamp=1470155297.118691)!
[ WARN] [1470216120.608702612, 1470155297.373358939]: rtabmap: Could not get transform from odom to camera_depth_optical_frame after 0.200000 seconds (for stamp=1470155297.118691)!
[ERROR] [1470216120.608762425, 1470155297.373358939]: TF of received depth image 0 at time 1470155297.118691s is not set, aborting rtabmap update.
[ WARN] [1470216120.812503232, 1470155297.576390646]: rtabmapviz: Could not get transform from odom to camera_depth_optical_frame after 0.200000 seconds (for stamp=1470155297.151934)!

^ The top and bottom right terminals are me running "rosrun tf tf_echo /camera_link /camera_depth_optical_frame" and "rosrun tf tf_echo /odom /camera_link" respectively, showing that the tfs are actually being published, but the RTAB seems to not be recognizing them.


I then try to run my RTAB with a modified launch file according to this thread: http://answers.ros.org/question/224250/rtabmap_ros-with-realsense-odometry-could-not-get-transform-from-base_link-to-camera_color_optical_frame/ ,

but even adding this line:

"
<node pkg="tf" type="static_transform_publisher"  name="base_to_realsense"
      args="0 0 0 -1.5707963 0 -1.5707963 /base_link /realsense_frame 100" />"

Does not help, I get the following (slightly different) tf errors:

[ WARN] [1470216364.095011744, 1470155298.225915932]: rtabmapviz: Could not get transform from odom to front_laser after 1.000000 seconds (for stamp=1470155297.144756)!
[ WARN] [1470216365.346593488, 1470155299.477246454]: rtabmapviz: Could not get transform from odom to camera_depth_optical_frame after 1.000000 seconds (for stamp=1470155298.418318)!

Let me know what I might be doing wrong, thanks for all the help!!

Gabe

[matlabbe]

When running the bag alone, show tf tree with "$ rosrun tf view_frames"

When running with rtabmap, show again the TF tree. Show also the rqt_graph.

As you are recording /odometry/filtered from the robot, is there also a TF /odom recorded in /tf? Well, we should see that in the first TF tree above.

To start rtabmap:
 - frame_id should be the fixed frame on the robot (like base_link or base_footprint), not odom
 - Is "/camera/depth/image_raw" registered with the color image?
 - Don't need "compressed:=true" if you recorded not compressed

cheers

[Gabe]

Hi Matlabbe,

Thanks for the response. Here is what I did:

Started playing the bag file (with rosparam set use_sim_time true and the --clock option in the rosbag play), and this is the resilt of tf view_frames:

http://docdro.id/RGFT6Np

Tf tree seems normal.

Ran the following code:

roslaunch rtabmap_ros rgbd_mapping.launch rtabmap_args:="--delete_db_on_start" odom:=/odometry/filtered rgb_topic:=/camera/rgb/image_raw depth_registered_topic:=/camera/depth/image_raw camera_info_topic:=/camera/rgb/camera_info queue_size:=10 RGBD/AngularUpdate:=0.01 RGBD/LinearUpdate:=0.01 Rtabmap/TimeThr:=700 Mem/RehearsalSimilarity:=0.45 RGBD/OptimizeFromGraphEnd:=true visual_odometry:=false odom_topic:=/odometry/filtered rtabmapviz:=true rviz:=false subscribe_scan:=true scan_topic:=/front/scan

Then ran tf view_frames again:

http://docdro.id/zt6WUNr

Also seems normal.

Here is the rqt_graph:



- I did not change the fixed frame parameter on my line above, so the fixed frame should be base_link and it does seem like it when viewing the RTAB map being generated.

- My /camera/depth/image_raw is not registered with the color image. Does it need to be?

- Ok, removed the compressed:=true.

Let me know how I might be able to fix this, the main problem is that RTAB thinks that the robot is moving all over the place but it is just moving in a straight line. Seems like an issue with the tf. I checked the /odometry/filtered topic, and the poses of the robot seem normal, no sudden jumps like those seen in RTAB.

Let me know if there is any solution to this, thanks!

Best,

Gabriel

And

[matlabbe]

Hi,

Your TF frame rate for the camera topics is quite low (~4-5 Hz). Try to increase it at least at 10-15 Hz.

Can you share the resulting database? (~/.ros/rtabmap.db)

[Gabe]

Hi Matlabbe,

Attached is my rtabmap.db file, the one where the robot seems to be very lost and jump around. This data was recorded with the robot driving forward on a level surface orchard.

rtabmap.db

Thanks for the help.

Gabe

[matlabbe]

Hi Gabe,

There is something wrong with the odometry. If you open the database in rtabmap-databaseViewer, you can look at the neighbor links in the Constraints view. These are odometry transforms, and they have a lot of errors. Example:


If I refine manually the transform by recomputing visual correspndences, I can get something better (you can see the next frame referential is just in front of the previous one):


cheers

[Gabe]

Hi Matlabbe,

Thank you very much for looking into this, yes it seems like the error is just in the odometry. I will try to fix that. Thanks!

Best,

Gabe