search

UZ-SLAMLab / ORB_SLAM3

ORB-SLAM3: An Accurate Open-Source Library for Visual, Visual-Inertial and Multi-Map SLAM
GNU General Public License v3.0
6.41k stars 2.52k forks source link

t265 .yaml settings #297

Closed ryleymcc closed 2 years ago

ryleymcc commented 3 years ago

I am having a trouble getting my system calibrated. For simplicity I am leaving out the IMU for now and i am trying to run Stereo with Realsense t265.

After running rs-sensor-control I get the the extrinsic transformation matrix from right camera to left camera(fisheye2 to fisheye1):

Translation Vector : [-0.0640212,-0.000119843,-8.24535e-05]
Rotation Matrix    : [0.999973,0.00361533,-0.00632303]
                       : [-0.00360718,0.999993,0.00130074]
                           : [0.00632768,-0.0012779,0.999979]

And the intrinsics for fisheye1:

Principal Point         : 429.849, 393.509
Focal Length            : 286.641, 286.546
Distortion Model        : Kannala Brandt4
Distortion Coefficients : [-0.0112001,0.0501633,-0.04698,0.009193,0]

And the intrinsics for fisheye2:

Principal Point         : 423.534, 401.768
Focal Length            : 286.157, 286.008
Distortion Model        : Kannala Brandt4
Distortion Coefficients : [-0.00927463,0.0461914,-0.043395,0.00802249,0]

I based the Camera.bf parameter off of the .065m distance between fisheye1 and fisheye2 multiplied by Camera2.fx: 286.1571960449219

This is my yaml file. Notice that I haven't changed the lapping parameters because I don't know where to get them.

%YAML:1.0

#--------------------------------------------------------------------------------------------
# Camera Parameters. Adjust them!
#--------------------------------------------------------------------------------------------
Camera.type: "KannalaBrandt8"

# Left Camera calibration and distortion parameters (OpenCV)
Camera.fx: 286.6408996582031
Camera.fy: 286.5458068847656
Camera.cx: 429.8493957519531
Camera.cy: 393.5089111328125

# Kannala-Brandt distortion parameters
Camera.k1: -0.011200119741261005
Camera.k2: 0.05016331002116203
Camera.k3: -0.04697997868061066
Camera.k4: 0.0091929966583848

# Right Camera calibration and distortion parameters (OpenCV)
Camera2.fx: 286.1571960449219
Camera2.fy: 286.00799560546875
Camera2.cx: 423.5339050292969
Camera2.cy: 401.7677917480469

# Kannala-Brandt distortion parameters
Camera2.k1: -0.009274626150727272
Camera2.k2: 0.04619137942790985
Camera2.k3: -0.043395038694143295
Camera2.k4: 0.00802248902618885

# Transformation matrix from right camera to left camera

Tlr: !!opencv-matrix
  rows: 3
  cols: 4
  dt: f
  data: [ 0.999973,0.00361533,-0.00632303,-0.0640212,
            -0.00360718,0.999993,0.00130074,-0.000119843,
            0.00632768,-0.0012779,0.999979,-0.0000824535]
# Camera resolution
Camera.width: 848
Camera.height: 800

# Lapping area between images (T265 cameras have nearly 180 FOV so i use the full width)
Camera.lappingBegin: 0
Camera.lappingEnd: 848

Camera2.lappingBegin: 0
Camera2.lappingEnd: 848

# Camera frames per second
Camera.fps: 30.0

# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)
Camera.RGB: 1

# Close/Far threshold. Baseline times.
ThDepth: 40.0
# I based this number off of the .065m distance between fisheye1 and fisheye2 multiplied by Camera2.fx: 286.1571960449219
Camera.bf: 18.600217743 

#--------------------------------------------------------------------------------------------
# ORB Parameters
#--------------------------------------------------------------------------------------------

# ORB Extractor: Number of features per image
ORBextractor.nFeatures: 1000

# ORB Extractor: Scale factor between levels in the scale pyramid
ORBextractor.scaleFactor: 1.2

# ORB Extractor: Number of levels in the scale pyramid
ORBextractor.nLevels: 8

# ORB Extractor: Fast threshold
# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response.
# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST
# You can lower these values if your images have low contrast
ORBextractor.iniThFAST: 20
ORBextractor.minThFAST: 7

#--------------------------------------------------------------------------------------------
# Viewer Parameters
#--------------------------------------------------------------------------------------------
Viewer.KeyFrameSize: 0.05
Viewer.KeyFrameLineWidth: 1
Viewer.GraphLineWidth: 1
Viewer.PointSize: 2
Viewer.CameraSize: 0.05
Viewer.CameraLineWidth: 2
Viewer.ViewpointX: 0
Viewer.ViewpointY: -10
Viewer.ViewpointZ: -0.1
Viewer.ViewpointF: 2000

I run the command rosrun ORB_SLAM3 Stereo /path/to/vocab/ORBvoc.txt /path/to/settings/t265_new.yaml false

my terminal output looks like this:

New Map created with 0 points
TRACK_REF_KF: Less than 15 matches!!
Fail to track local map!
LM: Active map reset recieved
LM: Active map reset, waiting...
LM: Reseting current map in Local Mapping...
LM: End reseting Local Mapping...
LM: Reset free the mutex
LM: Active map reset, Done!!!
mnFirstFrameId = 262
mnInitialFrameId = 261
132 Frames set to lost
^CFirst KF:132; Map init KF:131
New Map created with 0 points

SaveKeyFrameTrajectoryTUM to KeyFrameTrajectory_TUM_Format.txt ...

SaveTrajectoryTUM to FrameTrajectory_TUM_Format.txt ...

SaveTrajectoryKITTI to FrameTrajectory_KITTI_Format.txt ...

I get no tracked points using this method. The other ways I've i get some points but the tracking is not usable.

sumitsarkar1 commented 3 years ago

I am facing same problem with Stereo and a TUM yaml file. All I can think of is convert T265.yaml (presently looks like a TUM format yaml) in EuRoC.yaml format and check. This is because all Stereo and Stereo-Inertial for EuRoC data sets are running fine for me. Stereo for TUM is not working AT ALL...Stereo-Inertial for TUM is working but not all cases.

aravindSolteq commented 3 years ago

Do we need rectification of Fisheye images? Because their instruction says that we may not need to rectify the images for fisheye lens

sumitsarkar1 commented 3 years ago

Can you point where it says that rectification not required. I guess we have to rectify...thats why it requires the yaml file.

aravindSolteq commented 3 years ago

https://github.com/UZ-SLAMLab/ORB_SLAM3#running-stereo-node

sumitsarkar1 commented 3 years ago

What you have shared in the launch file is the TUM format where Camera.type: "KannalaBrandt8" . Some how the TUM Stereo Examples are not working even for TUM bench mark data sets . So the way round is ...provide the t265 settings in EuRoC format (EuRoC.yaml in /Examples/Stereo ). And if you notice in the EuRoC.yaml format Camera.type: "PinHole" for which images should be rectified. Hence yaml file is required for PinHole camera type or should be pre rectified if you set ONLINE_RECTIFICATION as False

aravindSolteq commented 3 years ago

But for EuRoC.yaml has this Calibration and distortion parameters for only one camera and it says ("equal for both cameras after stereo rectification"). How to do this stereo rectification?

Also for pre-rectify were we need to give the rectification matrices, How to get the matrices P ? Is there any tool to obtain this directly. I used kalibr to get the Distortion coefficients and K matrices.

sumitsarkar1 commented 3 years ago

1.) I have not used Kalibr but is good.... Realsense itself provides these K and P matrices....or you can use opencv... I use the calibration application provided with Rtabmap (which in turn uses OpenCV at the backend)

2.) Stereo Rectification is the joint rectification of the pair of images to produce the camera parameters. Its there in OpenCV here

3.) I (long time back) used this to rectify individual T265 images and then feed it to the algorithm as two pin hole cams. Or you can use this from ROS

aravindSolteq commented 3 years ago

@sumitsarkar1 : I have calibrated the camera with rtabmap. But the calibration is done using "equidistant" model(I suppose this isnt KannalaBrandt8 model). But can you share me your yaml file that you have used for ORB SLAM 3. Especially if you have tried the "Stereo-Inertial" mode. My map gets resets in between mapping when there is a movement of camera. Have you faced this kind of issue

sumitsarkar1 commented 3 years ago

I have faced all the same issues which you have mentioned till now..I am able to only run Stereo_Inertial on EuRoC data set ...even simple Mono mode on benchmark data is not functional ...i.e. after initial struggle of map initialisation the track abruptly stops. These were not the case with ORB_SLAM2. With ORB_SLAM2 I wrote a ROS node with OpenCV to rectify the T265 single camera (treating the T265 as mono fisheye ) then feed it to ORB_SLAM2 node and it worked fine. I hope the owners come up with some solution. Till then I found an alternative here. It allows map saving too unlike ORB_SLAM. You can try .

jdtardos commented 2 years ago

Version v1.0 now includes a calibration tutorial and examples with camera T265