Re: (Bad 3D map) robot_localization sensor fusion imu and odom

Posted by matlabbe on
URL: http://official-rtab-map-forum.206.s1.nabble.com/Bad-3D-map-robot-localization-sensor-fusion-imu-and-odom-tp5456p5538.html

Hi,

I cannot test the imu/fusion stuff as there are launch files missing. I could test the rtabmap vo part. It seems to work correctly, minor the fact there is a huge drift on take off, which makes the whole map tilting.

I tried the stereo mode for rtabmap, but the right camera_info of the zed seems not correct (which makes the clouds very small and vo poses very small). The Tx (baseline) is wrong (note the -0.04010362923145294):

header: 
  seq: 114
  stamp: 
    secs: 1550050594
    nsecs: 135220352
  frame_id: "zed_left_camera"
height: 376
width: 672
distortion_model: "plumb_bob"
D: [0.0, 0.0, 0.0, 0.0, 0.0]
K: [334.4449462890625, 0.0, 350.9258728027344, 0.0, 334.4449462890625, 190.33578491210938, 0.0, 0.0, 1.0]
R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
P: [334.4449462890625, 0.0, 350.9258728027344, -0.04010362923145294, 0.0, 334.4449462890625, 190.33578491210938, 0.0, 0.0, 0.0, 1.0, 0.0]
binning_x: 0
binning_y: 0
roi: 
  x_offset: 0
  y_offset: 0
  height: 0
  width: 0
  do_rectify: False

Here is an example with my zed:

header: 
  seq: 238
  stamp: 
    secs: 1551112002
    nsecs: 133999296
  frame_id: "zed_right_camera_optical_frame"
height: 376
width: 672
distortion_model: "plumb_bob"
D: [0.0, 0.0, 0.0, 0.0, 0.0]
K: [331.7145690917969, 0.0, 352.4257507324219, 0.0, 331.7145690917969, 189.7296142578125, 0.0, 0.0, 1.0]
R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
P: [331.7145690917969, 0.0, 352.4257507324219, -39.805747985839844, 0.0, 331.7145690917969, 189.7296142578125, 0.0, 0.0, 0.0, 1.0, 0.0]
binning_x: 0
binning_y: 0
roi: 
  x_offset: 0
  y_offset: 0
  height: 0
  width: 0
  do_rectify: False

There is maybe a TF missing to rotate the camera in ROS coordinates on stereo mode. Look at this example:

$ export ROS_NAMESPACE=stereo_camera
$ roslaunch zed_wrapper zed_camera.launch publish_tf:=false resolution:=3
$ rosrun tf static_transform_publisher 0 0 0 -1.5707963267948966 0 -1.5707963267948966 camera_link zed_left_camera_optical_ame 100
$ roslaunch rtabmap_ros rtabmap.launch rtabmap_args:="--delete_db_on_start --Vis/CorFlowMaxLevel 5 --Stereo/MaxDisparity 200" right_image_topic:=/stereo_camera/right/image_rect_color stereo:=true

Also when using rtabmap.launch in a launch file, set parameters in rtabmap_args argument like the example above.

cheers,
Mathieu