Slam using Intel RealSense tracking camera - T265

[nikitha]

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

[matlabbe]

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:


EDIT: Fisheye calibration is little more sensitive than normal calibration. In the following video, see what kind of motions that will make calibration converge correctly:
1) zoom in and zoom out for both cameras
2) Make the skew from one corner
3) Do horizontal motion while keeping same camera angle, in both ways
4) Do vertical motion while keeping same camera angle, in both ways
https://youtu.be/m-jSfEPUSCU

[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.

[matlabbe]

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.

[Lucy]

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

[matlabbe]

Are there warnings/errors in the terminal for more info?

[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

[matlabbe]

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

[tnolev]

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. ]"

[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

[matlabbe]

Updating example of this post for realsense ros V 2.40.0.

Using T265 odometry

# Start realsense2 (in odometry-only mode):
$ roscore
$ rosparam set /camera/tracking_module/enable_mapping false
$ roslaunch realsense2_camera rs_t265.launch enable_fisheye1:=true enable_fisheye2:=true

# 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 --Mem/UseOdomGravity true --Optimizer/GravitySigma 0.3" \
   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

Using F2M (RTAB-Map) odometry

You should have this commit.

# Start realsense2 (disable pose estimation):
$ roscore
$ roslaunch realsense2_camera rs_t265.launch enable_pose:=false enable_fisheye1:=true enable_fisheye2:=true unite_imu_method:=linear_interpolation

# Compute IMU quaternion
$ rosrun imu_filter_madgwick imu_filter_node \
    _use_mag:=false \
    _publish_tf:=false \
    _world_frame:="enu" \
    /imu/data_raw:=/camera/imu \
    /imu/data:=/rtabmap/imu

# 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, with stereo rectification done by stereo_odometry:
$ roslaunch rtabmap_ros rtabmap.launch \
   args:="-d" \
   odom_args:="--Rtabmap/ImagesAlreadyRectified false --Optimizer/GravitySigma 0.3" \
   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 \
   use_odom_features:=true \
   wait_imu_to_init:=true \
   imu_topic:=/rtabmap/imu

Using F2M (RTAB-Map) odometry but with T265 odometry as guess

You should have this commit.

# Start realsense2 (in odometry-only mode):
$ roscore
$ rosparam set /camera/tracking_module/enable_mapping false
$ roslaunch realsense2_camera rs_t265.launch enable_fisheye1:=true enable_fisheye2:=true unite_imu_method:=linear_interpolation

# Compute IMU quaternion
$ rosrun imu_filter_madgwick imu_filter_node \
    _use_mag:=false \
    _publish_tf:=false \
    _world_frame:="enu" \
    /imu/data_raw:=/camera/imu \
    /imu/data:=/rtabmap/imu

# 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, with stereo rectification done by stereo_odometry:
$ roslaunch rtabmap_ros rtabmap.launch \
   args:="-d" \
   odom_args:="--Rtabmap/ImagesAlreadyRectified false --Optimizer/GravitySigma 0.3" \
   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 \
   odom_guess_frame_id:=camera_odom_frame \
   use_odom_features:=true \
   wait_imu_to_init:=true \
   imu_topic:=/rtabmap/imu

[aravindSolteq]

Hi I'm trying to use the rtabmap calibration tool to calibrate t265. But somehow the camera isnt detected in my PC. I get this error after I press the calibrate in standalone application

[ERROR] (2021-05-06 11:42:27.799) CameraRealSense2.cpp:1453::captureImage() Missing frames (received 1, needed=2)
[ WARN] (2021-05-06 11:42:27.800) CameraThread.cpp:189::mainLoop() no more images...


OS : Ubuntu 18.04
Kernel :  5.4.0-72-generic  
Camera : T265
Firmware: 0.2.0.951


Can you please help me out?

[matlabbe]

which realsense sdk version is it?

[aravindSolteq]

My librealsense version is v2.44.0


Thanks

[matlabbe]

Hi, there is an issue with realsense sdk >=2.43. The synchronization interface used by rtabmap is not working anymore for T265. See this issue: https://github.com/introlab/rtabmap/issues/716

[aravindSolteq]

Hi. The imu from realsesnse (camera/imu) is in camera_imu_optical_frame. which the imu_filter_madgwick will use. The ouput of /rtabmap/imu will also be in same frame.

When rtabmap subscribes to this imu topic, it doesn't do the necessary transformation. The output of rtabmap odom is inverted . Cany you please let me know what change has to be done for this?

[aravindSolteq]

I tried the rtabmap stereo calibration tool recently and found that my Stereo Rectification is failing. It shows black screen after calibration . Although individual cameras can be rectified properly, the stereo rectification is failing.

While debugging, I noticed this wierd values(of order 10^4) in projection matrix
projection_matrix:
   rows: 3
   cols: 4
   data: [ 2.8881755373135707e+02, 0., -2.3571985992431641e+04, 0., 0.,
       2.8881755373135707e+02, 1.8684832443237305e+04, 0., 0., 0., 1.,
       0. ]

Can you please let me know what mistake Im doing?

EDIT: I have solved this. I didnt update the Stereo baseline parameter(which was 0.0). When changed to 0.062, it was proper my stereo-rectification was as expected

[matlabbe]

Hi,

rtabmap will save the transformation between base frame and imu frame (see here) to add gravity constraints in right direction (see here). Can you share a database showing the problem? What do you mean by inverted? Is it that the map is upside down?

[aravindSolteq]

I think the problem is in stereo_odometry, and not the rtabmap node. We used to have forward(x), left (y) and up(z). But the output /rtabmap/imu has a different mode forward(y) , right (x) and z(up).

I feel this is because of the change in orientation from camera_link and camera_imu_optical_frame.

I did a temporary change with world_frame="nwu" in the imu_filter_madgwick node. And the odometry seems to be proper now . But is this the proper way of doing it.

[matlabbe]

Hi,

It depends on the orientation of the IMU. Here some examples where the rtabmap's odometry is correctly initializing the odometry with x forward.

L515/D435i with IMU set at 90 deg on right (with z down):
 

Drone with IMU set at 90 deg on left:



Those above examples start the odometry correctly. However for T265, it is starting 90deg towards left (-y axis). Here is the IMU frame on T265:


Not sure exactly why this happens. Maybe it is related on how magdwick or complementary filters are initializing. I was able to make the odometry with T265 starts towards +x axis, but I had to remove the theta() fix here. However, doing so, all the above examples (D435i,L515, drone) above starts at 90 deg in yaw. Your fix ("nwu") may be the cleanest way to do it.