Build 3D map with RTAB and filtered odometry with Husky A200
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 ofrealsense_frame
?
Can you link on a rosbag file? Don't record TF if you are republishing them along replaying the bag.
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.
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?
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.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
.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.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.
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