Slam using Intel RealSense tracking camera - T265

classic Classic list List threaded Threaded
10 messages Options
Reply | Threaded
Open this post in threaded view
|

Slam using Intel RealSense tracking camera - T265

nikitha
This post was updated on .
I am trying to construct a point cloud of the scene using T265 camera(having fish-eye lens). However the reconstruction appears as follows when exported and viewed in CloudCompare.


My lauch files are as follows
stereo_t265_m.launch
rs_t265_m.launch

As the /camera/fisheye2/camera_info topic of the right fish eye camera was publishing a 0 in P(0,3) and resulting an error stating that the baseline is zero, I have manually edited that value alone and published the info data via another topic right/info.
Graph looks as follows
rosgraph.svg

The images when extracted from the database appear like this


When the data coming from /points2 topic of stereo_image_proc node is visualised in rviz, i see a single point that moves as the camera moves.

Kindly let me know what changes are required to achieve proper reconstruction of the scene.

Thank You
Reply | Threaded
Open this post in threaded view
|

Re: Slam using Intel RealSense tracking camera - T265

matlabbe
Administrator
Hi,

The images should be rectified so that proper reconstruction can be done. What did you use for calibration? I don't think ROS stereo calibration tool supports fisheye stereo. In rtabmap standalone, it is possible to do fisheye stereo calibration for T265. See Preferences->Source, select Stereo camera, select RealSense2 driver, then click on Calibrate. It may require a couple tries to get the calibration right. Here are my calibration files for T265:

905312112011_left.yaml:
%YAML:1.0
---
camera_name: "905312112011_left"
image_width: 848
image_height: 800
camera_matrix:
   rows: 3
   cols: 3
   data: [ 2.8845847407434218e+02, 0., 4.2366155337303530e+02, 0.,
       2.8732351128054398e+02, 4.0774252018099634e+02, 0., 0., 1. ]
distortion_coefficients:
   rows: 1
   cols: 4
   data: [ 1.6108401660058620e-02, -1.5462775664278158e-02,
       9.5501371240367117e-03, -6.7432882743460659e-03 ]
distortion_model: equidistant
rectification_matrix:
   rows: 3
   cols: 3
   data: [ 9.9996361896819130e-01, 1.0752140026281053e-03,
       -8.4619533729723070e-03, -1.0848458428058135e-03,
       9.9999876886529271e-01, -1.1337448549999169e-03,
       8.4607239368243194e-03, 1.1428835231309651e-03,
       9.9996355431971484e-01 ]
projection_matrix:
   rows: 3
   cols: 4
   data: [ 2.8845847407434218e+02, 0., 4.2228301048278809e+02, 0., 0.,
       2.8845847407434218e+02, 4.0177149677276611e+02, 0., 0., 0., 1.,
       0. ]

905312112011_right.yaml:
%YAML:1.0
---
camera_name: "905312112011_right"
image_width: 848
image_height: 800
camera_matrix:
   rows: 3
   cols: 3
   data: [ 2.8971586612342838e+02, 0., 4.3282951572869268e+02, 0.,
       2.8842800650095052e+02, 3.9647983507794498e+02, 0., 0., 1. ]
distortion_coefficients:
   rows: 1
   cols: 4
   data: [ 1.1755170565796828e-03, 2.7439249292868447e-02,
       -3.1282234368493568e-02, 5.7239868467015087e-03 ]
distortion_model: equidistant
rectification_matrix:
   rows: 3
   cols: 3
   data: [ 9.9999049872412404e-01, 1.9012821940872346e-03,
       -3.9227015558338232e-03, -1.8968518482397822e-03,
       9.9999755932917211e-01, 1.1328233598542061e-03,
       3.9248457986937090e-03, -1.1253718128906738e-03,
       9.9999166452712951e-01 ]
projection_matrix:
   rows: 3
   cols: 4
   data: [ 2.8845847407434218e+02, 0., 4.2228301048278809e+02,
       -1.8501290547465036e+01, 0., 2.8845847407434218e+02,
       4.0177149677276611e+02, 0., 0., 0., 1., 0. ]


905312112011_pose.yaml:
%YAML:1.0
---
camera_name: "905312112011"
rotation_matrix:
   rows: 3
   cols: 3
   data: [ 9.9998938286662098e-01, -8.1716008464192697e-04,
       -4.5350196725749909e-03, 8.0684836818960455e-04,
       9.9999708631380535e-01, -2.2751614471256884e-03,
       4.5368656300714865e-03, 2.2714782182106175e-03,
       9.9998712853564209e-01 ]
translation_matrix:
   rows: 3
   cols: 1
   data: [ -6.4137879190303601e-02, -1.2194536630761036e-04,
       2.5159614897210655e-04 ]

Before calibration:


After calibration (you may have to click "Unlock" to enable the calibration, it is difficult with fisheye cameras to fill the size bar completly):


Reconstruction example:

Reply | Threaded
Open this post in threaded view
|

Re: Slam using Intel RealSense tracking camera - T265

nikitha
In reply to this post by nikitha
i have carried out the calibration using this method and the reconstruction result in the camera viewer were similar to your post. However when i launch the session using the aforementioned launch files, P(0,3) in /camera/fisheye2/camera_info remained to be zero. To fix that i republished the changed info using a different topic again. The reconstruction remains to be distorted. Does stereo_image_proc node produce rectified images for images produced by fisheye lens as well? i suppose the images are not rectified and hence resulting in distorted reconstruction.
Reply | Threaded
Open this post in threaded view
|

Re: Slam using Intel RealSense tracking camera - T265

matlabbe
Administrator
This post was updated on .
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')
    rospy.Subscriber("image", Image, callback)
    info = loadCalibrationFile(url, '')
    print info
    pub = rospy.Publisher('camera_info', CameraInfo, queue_size=1)
    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.
Reply | Threaded
Open this post in threaded view
|

Re: Slam using Intel RealSense tracking camera - T265

Lucy
In reply to this post by matlabbe
Hello,

I followed your steps to do the T265 calibration on rtabmap standalone, I clicked the calibrate, it shows the "camera initialization failed". Do you know why would it happens and how could i solve it? Thank you!

Best,
Lucy
Reply | Threaded
Open this post in threaded view
|

Re: Slam using Intel RealSense tracking camera - T265

matlabbe
Administrator
Are there warnings/errors in the terminal for more info?
Reply | Threaded
Open this post in threaded view
|

Re: Slam using Intel RealSense tracking camera - T265

Lucy
Hello,

The error shown on my terminal is : [ERROR] (2020-03-10 23:03:59.692) CameraRealSense2.cpp:789::init() CameraRealSense: RTAB-Map is not built with RealSense2 support!

Do you know how could i solve it? Thank you!

Best,
Lucy
Reply | Threaded
Open this post in threaded view
|

Re: Slam using Intel RealSense tracking camera - T265

matlabbe
Administrator
Hi,

This means RTAB-Map has not been built with RealSense2 support. When you build rtabmap from source, make sure in the cmake status, it is said "With RealSense2 = YES". If not, it means cmake cannot find realsense2 library. If so, which realsense2 version you are using?

cheers,
Mathieu
Reply | Threaded
Open this post in threaded view
|

Re: Slam using Intel RealSense tracking camera - T265

tnolev
In reply to this post by matlabbe
Hi, I know this topic hasn't been active recently but I ran in a similar issue as the one you are addressing in this current discussion.

I have a T265 which I have calibrated using the standalone rtabmap app, I got the left, right and pose yaml files, looking mostly like yours to the exception that I had 5 distortion coefficients instead of 4. I tried applying all the different commands you suggested (roscore -> roslaunch for rs_t265.launch-> use your python script to modify the camera_info_calib and publish it as a topic-> roslaunch for rtabmap.launch), exactly as you described.

However, I still get the error messages:
[ERROR] (2020-07-28 17:07:41.726) Memory.cpp:4092::createSignature() Camera calibration not valid, calibrate your camera!
[ERROR] (2020-07-28 17:07:41.726) Memory.cpp:797::update() Failed to create a signature...

I do have realsense2 library installed, and I have a somewhat working system when I use rtabmap as standalone (although the mapping doesn't seem to undistort the image since the point cloud resembles the fisheye view), but I can't get it to work properly because of this error message.

Would you be able to point me towards the cause of my problem and eventually how to fix it ?

Here are my yaml files (I removed the %YAML:1.0 as suggested):
ros_left.yaml
"---
camera_name: "11622110037_left"
image_width: 848
image_height: 800
camera_matrix:
   rows: 3
   cols: 3
   data: [ 3.1569942836672891e+02, 0., 4.5320798209351716e+02, 0.,
       3.1773021152099881e+02, 4.0881417193276985e+02, 0., 0., 1. ]
distortion_coefficients:
   rows: 1
   cols: 4
   data: [ -2.1014365596886836e-01, 2.9166548420571422e-02,
       -1.5599954390082896e-03, -2.5452459956943787e-03 ]
distortion_model: equidistant
rectification_matrix:
   rows: 3
   cols: 3
   data: [ 9.9642963318458755e-01, -2.6703329778918736e-02,
       8.0093185043090112e-02, 2.6082786373330547e-02,
       9.9962119145758999e-01, 8.7841814593771789e-03,
       -8.0297411954754594e-02, -6.6637652739555088e-03,
       9.9674867437370696e-01 ]
projection_matrix:
   rows: 3
   cols: 4
   data: [ 1.0405870922780880e+03, 0., 4.0563197374343872e+02, 0., 0.,
       1.0405870922780880e+03, 4.0222117090225220e+02, 0., 0., 0., 1.,
       0. ]
"


ros_right.yaml
"---
camera_name: "11622110037_right"
image_width: 848
image_height: 800
camera_matrix:
   rows: 3
   cols: 3
   data: [ 3.2127677411738898e+02, 0., 3.9013096480351504e+02, 0.,
       3.1595412249994331e+02, 4.0465561119173321e+02, 0., 0., 1. ]
distortion_coefficients:
   rows: 1
   cols: 4
   data: [ -1.8301520791637593e-01, -9.1158020279482974e-03,
       -3.9227771314327318e-04, 4.1384203001368618e-03 ]
distortion_model: equidistant
rectification_matrix:
   rows: 3
   cols: 3
   data: [ 9.9216630444535503e-01, 3.5218729732007084e-02,
       -1.1985685378530113e-01, -3.6146091724685657e-02,
       9.9933098618040250e-01, -5.5713654282353491e-03,
       1.1958045148049003e-01, 9.8600778784026042e-03,
       9.9277555091166203e-01 ]
projection_matrix:
   rows: 3
   cols: 4
   data: [ 1.0405870922780880e+03, 0., 4.0563197374343872e+02,
       -8.5034056783566655e+01, 0., 1.0405870922780880e+03,
       4.0222117090225220e+02, 0., 0., 0., 1., 0. ]
"

ros_pose.yaml
"---
camera_name: "11622110037"
rotation_matrix:
   rows: 3
   cols: 3
   data: [ 9.7807911523364222e-01, -6.3423399359521043e-02,
       1.9833990208089289e-01, 6.0366583841576442e-02,
       9.9794626846714929e-01, 2.1427104512514738e-02,
       -1.9929154497638227e-01, -8.9843010955228883e-03,
       9.7989905726801851e-01 ]
translation_matrix:
   rows: 3
   cols: 1
   data: [ -8.1077236587902266e-02, -2.8779825216937944e-03,
       9.7943887506485791e-03 ]
essential_matrix:
   rows: 3
   cols: 3
   data: [ -1.7696206529639732e-05, -9.7484170441038236e-03,
       -3.0299977512379295e-03, -6.5783206585300086e-03,
       -1.3496157347134439e-03, 8.1390125804127370e-02,
       -2.0794612016507784e-03, -8.1093257145348402e-02,
       -1.1664316504115973e-03 ]
fundamental_matrix:
   rows: 3
   cols: 3
   data: [ 7.2751081474616635e-09, 3.9820678844846108e-06,
       -1.2379669040568661e-03, 2.7499805288348509e-06,
       5.6058309390091584e-07, -1.2216867783501957e-02,
       -8.4097708360577142e-04, 8.8620057840063833e-03, 1. ]
"
Reply | Threaded
Open this post in threaded view
|

Re: Slam using Intel RealSense tracking camera - T265

tnolev
Hello everyone, I fixed my problem, just to let the next people that might run into the same issue.

Turns out my calibration wasn't correct and the reason for it is that I was using a 4x4 chessboard instead of the usual 8x6. Due to the fact that my chessboard was square instead of rectangular. This led the algorithm, during the calibration process, to flip some of my images from the right camera by 90° (to correct the skewness and rotation of the chessboard) but it did not do so on the left image. Because of that the calibration process could not compute properly extrinsic parameters of my stereo t265 (rotation and transformation). For some reason it still let me finish my calibration with these improper parameters and then when I went on to use rtabmap with its ROS wrapper everything failed and I got the error message as mentiionned above.

TL;DR: my solution was to switch from 4x4 chessboard to 8x6 chessboard and recalibrate