Build 3D map with RTAB and filtered odometry with Husky A200

asked 2016-08-26 15:03:13 -0500

Marcus Barnet
277 ●63 ●74 ●83

updated 2016-08-28 04:30:22 -0500

I'm continuing the discussion of my previous topic by focusing on the RTAB results. I'm using robot_localization to obtain filtered odometry from wheel encoders, IMU (xsens Mti-10) and visual odometry (RealSense R200). The IMU sensor is mounting on the center of the robot while the RSense is mounted on a vertical axis 116 centimeters above the IMU.

I launch the Husky base.launch node in order to start the teleoperation and the locomotion node; in addition, the base node also launch control.launch file which launch robot_localization. Control.launch file loads parameters from localization.yaml for IMU and wheel_encoders and I added also the odom1 which should be generated by rgbd_odometry from rtabmap_ros package. At the bottom of control.launch file, I also added the transformation for my sensors:

<node pkg="tf" type="static_transform_publisher"  name="base_to_realsense"
      args="0.3 1.16 0 1.5707963 0 -1.5707963 /base_link /realsense_frame 100" />
 <node pkg="tf" type="static_transform_publisher"  name="base_to_laser"
      args="0.3 0.6 0.1 0 0 0 /base_link /laser 100" />
<node pkg="tf" type="static_transform_publisher"  name="base_to_color"
      args="0 0 0.6 1.5707963 0 -1.5707963 /base_link /camera_color_optical_frame 100" />
<node pkg="tf" type="static_transform_publisher"  name="color_to_depth"
      args="0 0 0 0 0 0 /camera_color_optical_frame /camera_depth_optical_frame 100" />

This is my TF tree: frames.pdf. Should the camera_color_optical_frame be a child of realsense_frame?

EDIT (@matlabbe 28/08/2016): This is a very light bag file without /tf and with compressed image topics: /scan /camera/color/image_raw/compressed camera/depth/image_raw/compressed /camera/color/camera_info /imu/data /odometry/filtered /tf /husky_velocity_controller/odom END_EDIT

With this configuration, I recorded a bag file, in particular, these topics:

rosbag record /scan /camera/color/image_raw camera/depth/image_raw /camera/color/camera_info /imu/data /odometry/filtered /tf /husky_velocity_controller/odom

Then, I try to run the rgbd_mapping.launch file in order to obtain the 3D map by playing back the ros bag file (I added the same transformations at the end of this file, too).

$ rosparam set use_sim_time true
$ roslaunch rtabmap_ros rgbd_mapping.launch rgb_topic:=/camera/color/image_raw depth_registered_topic:=/camera/depth/image_raw camera_info_topic:=/camera/color/camera_info compressed:=true frame_id:=base_link rtabmap_args:="--delete_db_on_start" estimation:=1
$ rosbag play --clock file.bag

The problem is that the map is not built correctly since I can only see few frames without any connection. I uplaoded a short video on YouTube.

Do you think the problem is related to wrong transformations set for the sensors?
The problem can come from a wrong /odometry/filtered?
May be the rgb/image, depth/image, rgb/camera_info are set to the wrong published topics? (both in control.launch and in rgbd_mapping.launch files)
Control.launch is OK?
Should the camera_color_optical_frame be a child of realsense_frame?

Comments

Can you link on a rosbag file? Don't record TF if you are republishing them along replaying the bag.

matlabbe ( 2016-08-27 09:07:30 -0500 )edit

Yes, I can upload a ros bag file, do you have problems to download large files? Unfortunately, few seconds of recording are about 2-3 gb since realsense frames are very large files.

Marcus Barnet ( 2016-08-27 09:41:54 -0500 )edit

These topics are enough? /scan /camera/color/image_raw camera/depth/image_raw /camera/color/camera_info /imu/data /odometry/filtered /tf /husky_velocity_controller/odom Any topics is missing?

Marcus Barnet ( 2016-08-27 09:43:32 -0500 )edit

You should not record /tf if you are republishing them when replaying the bag. You can use $ rosbag compress to compress a bag. Note that 10 sec rosbag is enough, like moving 1 or 2 meters with the robot.

matlabbe ( 2016-08-27 18:31:17 -0500 )edit

This is the first recorded file which contains only few topics. Later, I will rosbag compress another bag file and I will post it. It will include all the published topics without /tf.

Marcus Barnet ( 2016-08-27 19:04:25 -0500 )edit

Since it takes few hours to upload the new file without the /tf, I uploaded also this other file which contains all the topics (unfortunately, it includes also /tf). I will upload the other bag file during next hours.

Marcus Barnet ( 2016-08-27 19:46:49 -0500 )edit

I would like to obtain something like your demo Robot Mapping in RVIZ in order to display a 2D map with its projected 3D view. It would be very interesting for outdoor applications.

Marcus Barnet ( 2016-08-27 19:52:41 -0500 )edit

Final very lightbag file without /tf and with compressed image topics: /scan /camera/color/image_raw/compressed camera/depth/image_raw/compressed /camera/color/camera_info /imu/data /odometry/filtered /tf /husky_velocity_controller/odom

Marcus Barnet ( 2016-08-28 04:28:14 -0500 )edit

add a comment

answered 2016-08-28 10:57:25 -0500

matlabbe

6409 ●11 ●96 ●80

updated 2016-08-29 10:25:57 -0500

yes, see the following sceenshots. /camera_color_optical_frame is not a child of /realsense_frame. /realsense_frame and /laser don't seem to be on the robot. The camera is also looking backward in RVIZ (second screenshot). It seems also that you have /camera_color_optical_frame published by 2 nodes with 2 different transforms.
Printing /odometry/filterered on terminal, the values jump a lot. Please use default husky's robot_localization config when recording (without /vo)
What are all your camera topics? Make sure you are subscribing to depth image registered with color image.
TF should be fixed first.
yes

Please, don't record a bag with rtabmap running. Just bringup the robot and record. It seems that multiple nodes are publishing /odom -> /base_footprint transform. At least before recording, make sure in RVIZ that laser and depth cloud are correctly aligned together and with the robot. You should be able to answer yes to this question: "Are you able to teleoperate the robot only looking at RVIZ?"

Depth images should not be recorded with /camera/depth/compressed but with /camera/depth/compressedDepth instead.

EDIT

With the bag with corrected TF, here is an example launch file to create a map similar to Robot mapping with RVIZ demo:

<launch>
  <param name="use_sim_time" type="bool" value="True"/>
  <group ns="rtabmap">
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
      <param name="frame_id"           type="string" value="base_link"/>

      <param name="subscribe_depth" type="bool" value="true"/>
      <param name="subscribe_scan"  type="bool" value="true"/>

      <remap from="odom" to="/odometry/filtered"/>
      <remap from="scan" to="/scan"/>

      <remap from="rgb/image"       to="/camera/color/image_raw"/>
      <remap from="depth/image"     to="/camera/depth/image_raw"/>
      <remap from="rgb/camera_info" to="/camera/color/camera_info"/>

      <param name="rgb/image_transport"   type="string" value="compressed"/>
      <param name="depth/image_transport" type="string" value="compressedDepth"/>

      <param name="Reg/Strategy"              type="string" value="1"/>     <!-- 0=Visual, 1=ICP, 2=Visual+ICP -->
      <param name="Optimizer/Slam2D"          type="string" value="true"/>
      <param name="Reg/Force3DoF"             type="string" value="true"/>
   </node>
  </group>

  <!-- Visualisation RVIZ -->
  <node pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/demo_robot_mapping.rviz" output="screen"/>
</launch>

Using it and resulting map:

$ roslaunch test.launch
$ rosbag play --clock light-tf-ok.bag

image description

At first look, /odometry/filtered seems not very accurate so I gave a try with /husky_velocity_controller/odom alone. I removed from the bag all /odom tf using this filterBagTF.py script (change map for odom), /odom -> /base_link is republished from the /husky_velocity_controller/odom topic using odom_msg_to_tf node:

<launch>
  <param name="use_sim_time" type="bool" value="True"/>
  <group ns="rtabmap">
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
      <param name="frame_id"           type="string" value="base_link"/>

      <param name="subscribe_depth" type="bool" value="true"/>
      <param name="subscribe_scan"  type="bool" value="true"/>

      <remap from="odom" to="/husky_velocity_controller/odom"/>
      <remap from="scan" to="/scan"/>

      <remap from="rgb/image"       to="/camera/color/image_raw"/>
      <remap from="depth/image"     to="/camera/depth/image_raw"/>
      <remap from="rgb/camera_info" to="/camera/color/camera_info"/>

      <param name="rgb/image_transport"   type="string" value="compressed"/>
      <param name="depth/image_transport" type="string" value ...

(more)

edit flag offensive delete link

Comments

@matlabbe, thanks for your suggestions. I fixed the /tf problem and everything is OK since I checked all the frames under RVIZ and /laser and realsense_frame are aligned. This is a compressed bag file.

Marcus Barnet ( 2016-08-28 15:40:02 -0500 )edit

This is the control.launch file without /vo. The bag file includes the /tf topic since it is more easy for me to display it under RVIZ. Can you tell me if it is OK now and if it can be used with RTAB to obtain a 2D map and the projected 3d view, please?

Marcus Barnet ( 2016-08-28 15:43:42 -0500 )edit

I deleted the RTAB from control.launch file, but now the /odometry/filtered will not contain also the visual odometry. This can still be OK for what I would like to do? I mean, something like your demo.

Marcus Barnet ( 2016-08-28 15:46:55 -0500 )edit

The compressed bag file will be available in 30 minutes starting from now. It is in upload right now.

Marcus Barnet ( 2016-08-28 15:52:50 -0500 )edit

Now the file has been correctly uploaded.

Marcus Barnet ( 2016-08-28 16:33:32 -0500 )edit

@matlabbe: I tried to run your launch file, but I'm not able to build a map since RVIZ says me that: no map is received. If I set base_footprint or /odom as fixed frame, I can see the robot moving around, but no map. Do I have to run some other launch file, too?

Marcus Barnet ( 2016-08-29 03:23:21 -0500 )edit

I can select grid_map, cloud_map, mapData and mapGraph in RVIZ, but it always says that no map is received. In my TF frames, there is no map reference. It seems that no map is created. How this can be possible? Do you launch some other file in addition to your test.launch?

Marcus Barnet ( 2016-08-29 04:25:26 -0500 )edit

Make sure light-tf-ok.bag is decompressed: $ rosbag decompress light-tf-ok.bag. It is only test.launch running.

matlabbe ( 2016-08-29 08:32:28 -0500 )edit

see more comments

Build 3D map with RTAB and filtered odometry with Husky A200

Comments

1 Answer

Comments

Question Tools

Stats

Related questions

Build 3D map with RTAB and filtered odometry with Husky A200 edit

Comments

1 Answer

Comments

Question Tools

Stats

Related questions

Build 3D map with RTAB and filtered odometry with Husky A200