Login  Register

Re: Slam using Intel RealSense tracking camera - T265

Posted by matlabbe on Oct 15, 2019; 7:18pm
URL: http://official-rtab-map-forum.206.s1.nabble.com/Slam-using-Intel-RealSense-tracking-camera-T265-tp6333p6343.html

Hi, yes stereo_img_proc may not support fisheye distortion model. For convenience, rectification can be done on rtabmap side if parameter Rtabmap/ImagesAlreadyRectified is false. However, camera_info topic should contain the calibration done in rtabmap's calibration tool. To do so, I created a small python script (camera_info_pub.py) to publish the new camera_info:

#!/usr/bin/env python
import rospy
from sensor_msgs.msg import Image
from sensor_msgs.msg import CameraInfo
import yaml

def loadCalibrationFile(filename, cname):
    ci = CameraInfo()

    f = open(filename)
    calib = yaml.load(f)
    if calib is not None:
        ci.width = calib['image_width']
        ci.height = calib['image_height']
        ci.distortion_model = calib['distortion_model']
        ci.D = calib['distortion_coefficients']['data']
        ci.K = calib['camera_matrix']['data']
        ci.R = calib['rectification_matrix']['data']
        ci.P = calib['projection_matrix']['data']

    return ci

def callback(data):
    info.header = data.header
    pub.publish(info)
    
if __name__ == '__main__':
    rospy.init_node('camera_info_pub', anonymous=True)
    url = rospy.get_param('~url')
    info = loadCalibrationFile(url, '')
    print(info)
    pub = rospy.Publisher('camera_info', CameraInfo, queue_size=1)
    rospy.Subscriber("image", Image, callback)
    rospy.spin()


To read rtabmap's calibration files, remove the "%YAML:1.0" at the beginning of the YAML files, otherwise the script will throw an error. Here is an usage example:
# Start realsense2 (in odometry-only mode):
$ roscore
$ rosparam set /camera/tracking_module/enable_mapping false
$ roslaunch realsense2_camera rs_t265.launch

# Start the camera_info publishers with the stereo fisheye calibration files:
$ python camera_info_pub.py \
   _url:=/home/mathieu/Documents/RTAB-Map/camera_info/905312112011_left.yaml \
   image:=/camera/fisheye1/image_raw \
   camera_info:=/camera/fisheye1/camera_info_calib
$ python camera_info_pub.py \
   _url:=/home/mathieu/Documents/RTAB-Map/camera_info/905312112011_right.yaml \
   image:=/camera/fisheye2/image_raw \
   camera_info:=/camera/fisheye2/camera_info_calib

# Start RTAB-Map in stereo mode, use T265 odometry and do rectification:
$ roslaunch rtabmap_ros rtabmap.launch \
   args:="-d --Rtabmap/ImagesAlreadyRectified false" \
   stereo:=true \
   left_image_topic:=/camera/fisheye1/image_raw \
   right_image_topic:=/camera/fisheye2/image_raw \
   left_camera_info_topic:=/camera/fisheye1/camera_info_calib \
   right_camera_info_topic:=/camera/fisheye2/camera_info_calib \
   visual_odometry:=false \
   odom_frame_id:=camera_odom_frame

When starting rtabmap, you should see that warning telling that rectification will be done:
[ WARN] (2019-10-15 15:14:49.764) Memory.cpp:4115::createSignature() Initializing rectification maps (only done for the first image received)...
[ WARN] (2019-10-15 15:14:49.802) Memory.cpp:4117::createSignature() Initializing rectification maps (only done for the first image received)...done!



If you are not satisfied with the disparity computation, you can tune cv::StereoBM with those parameters:
$ rtabmap --params | grep StereoBM
Param: Stereo/DenseStrategy = "0"                          [0=cv::StereoBM, 1=cv::StereoSGBM]
Param: StereoBM/BlockSize = "15"                           [See cv::StereoBM]
Param: StereoBM/Disp12MaxDiff = "-1"                       [See cv::StereoBM]
Param: StereoBM/MinDisparity = "0"                         [See cv::StereoBM]
Param: StereoBM/NumDisparities = "128"                     [See cv::StereoBM]
Param: StereoBM/PreFilterCap = "31"                        [See cv::StereoBM]
Param: StereoBM/PreFilterSize = "9"                        [See cv::StereoBM]
Param: StereoBM/SpeckleRange = "4"                         [See cv::StereoBM]
Param: StereoBM/SpeckleWindowSize = "100"                  [See cv::StereoBM]
Param: StereoBM/TextureThreshold = "10"                    [See cv::StereoBM]
Param: StereoBM/UniquenessRatio = "15"                     [See cv::StereoBM]

EDIT Monocular mode
It is possible to use realsense calibration directly in monocular mode, but we need to modify realsense package to provide the right distortion model. To do so, change this line from "plumb_bob" to "equidistant"  and rebuild realsense2_camera package.
# Start realsense2 (in odometry-only mode):
$ roscore
$ rosparam set /camera/tracking_module/enable_mapping false
$ roslaunch realsense2_camera rs_t265.launch

# Start RTAB-Map in monocular mode, use T265 odometry and do rectification:
$ rosrun rtabmap_ros rtabmap -d\
   --Rtabmap/ImagesAlreadyRectified false \
   --Rtabmap/DetectionRate 2 \
   _subscribe_depth:=false \
   rgb/image:=/camera/fisheye1/image_raw \
   rgb/camera_info:=/camera/fisheye1/camera_info \
   _odom_frame_id:=camera_odom_frame \
   _frame_id:=camera_pose_frame

$ rosrun rtabmap_ros rtabmapviz _odom_frame_id:=camera_odom_frame
Note that more accurate loop closures would be accepted using stereo mode version.