search

introlab / rtabmap

RTAB-Map library and standalone application
https://introlab.github.io/rtabmap
Other
2.71k stars 775 forks source link

Calibration of t265 #716

Open aravindSolteq opened 3 years ago

aravindSolteq commented 3 years ago

Hi , I'm probably here to seek some help for using the calibration tool for t265.

I could not run the calibration tool with realsense camera. I dont find a explicit driver for the "rtabmap-calibration" tool. And even if I try to calibrate with providing source. I get error messages in the terminal 

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

These are the specifications that I have OS : Ubuntu 18.04 Kernel : 5.4.0-72-generic
Camera : T265 Firmware: 0.2.0.951 librealsense version: 2.44.0

Thanks in advance.

sumitsarkar1 commented 3 years ago

If you need the calibration yaml files let me know...will be little different from yours

matlabbe commented 3 years ago

Hi, rtabmap-calibration tool didn't support realsense2, this has been added yesterday: https://github.com/introlab/rtabmap/commit/4390cb64284b7c029ca091693d2f83713ff22846

rtabmap-calibration --driver 11

I tested with my T265. It won't work for other realsense cameras.

aravindSolteq commented 3 years ago

If you need the calibration yaml files let me know...will be little different from yours

Yes this would be better.. I will take it as a reference

aravindSolteq commented 3 years ago

Hi, rtabmap-calibration tool didn't support realsense2, this has been added yesterday: 4390cb6

rtabmap-calibration --driver 11

I tested with my T265. It won't work for other realsense cameras.

Sure I will check this. Thanks @matlabbe

aravindSolteq commented 3 years ago

@matlabbe : I tried this, the tool is getting connected to fisheye streams but it reports that there is some missing frames.

[ INFO] Using driver 11
[ INFO] Using device 0
[ INFO] Stereo: true
[ INFO] setupDevice...
[ INFO] Device with serial number 908412110271 was found with product ID=2871.
[ INFO] Device Name: Intel RealSense T265
[ INFO] Device Serial No: 908412110271
[ INFO] Device FW version: 0.2.0.951
[ INFO] Device Product ID: 0x0B37
[ INFO] Device Sensors: 
[ INFO] Tracking Module was found.
[ INFO] Sensor 0 "Tracking Module"
[ INFO] profiles=5
[ INFO] Y8 848 800 30 1 Fisheye 1 type=4
[ INFO] Y8 848 800 30 2 Fisheye 2 type=4
[ INFO] MOTION_XYZ32F 0 0 200 0 Gyro type=5
[ INFO] MOTION_XYZ32F 0 0 62 0 Accel type=6
[ INFO] 6DOF 0 0 200 0 Pose type=8
[ INFO] leftToIMU = xyz=0,010700,0,000000,-0,000000 rpy=0,003424,-0,007314,3,137116
[ INFO] imu local transform = xyz=-0,000000,-0,010700,-0,000000 rpy=1,574188,0,004477,1,563482
[ INFO] Starting sensor 0 with 5 profiles
[ INFO] Opening: Y8 848 800 30 1 Fisheye 1 type=4
[ INFO] Opening: Y8 848 800 30 2 Fisheye 2 type=4
[ INFO] Opening: MOTION_XYZ32F 0 0 200 0 Gyro type=5
[ INFO] Opening: MOTION_XYZ32F 0 0 62 0 Accel type=6
[ INFO] Opening: 6DOF 0 0 200 0 Pose type=8
[ INFO] Enabling streams...done!
[ERROR] Missing frames in capture image (received 1, needed=2)
[ WARN] no more images...

I see this error being reported from corelib/src/ CameraRealSense2.cpp where the syncer_.wait_for_frames(100) is not able to get the 2 frames.

But I'm able to receive two streams when I connect realsense2_camera and launch the rs_t265.launch file. Can you please help on this?

sumitsarkar1 commented 3 years ago

I got this yaml from Rtabmap standalone : Screenshot from 2021-05-14 13-17-59

For t265 : Left camera :

%YAML:1.0
---
camera_name: t265_left
image_width: 848
image_height: 800
camera_matrix:
   rows: 3
   cols: 3
   data: [ 2.8727184647822833e+02, 0., 4.2168020613614948e+02, 0.,
       2.8673497080036134e+02, 4.0840727335335356e+02, 0., 0., 1. ]
distortion_coefficients:
   rows: 1
   cols: 4
   data: [ -4.2255265071390342e-04, 2.3531973133527690e-02,
       -1.4654243614093544e-02, -4.3611467599366316e-03 ]
distortion_model: equidistant
rectification_matrix:
   rows: 3
   cols: 3
   data: [ 9.9999504305576770e-01, -2.8619178553628338e-03,
       1.3127414378555557e-03, 2.8618846361930942e-03,
       9.9999590443032127e-01, 2.7182952606179453e-05,
       -1.3128138568089518e-03, -2.3425903309501875e-05,
       9.9999913798513074e-01 ]
projection_matrix:
   rows: 3
   cols: 4
   data: [ 2.8727184647822833e+02, 0., 2.8237225341796875e+02, 0., 0.,
       2.8727184647822833e+02, 4.3397737121582031e+02, 0., 0., 0., 1.,
       0. ]

Right Camera :

%YAML:1.0
---
camera_name: t265_right
image_width: 848
image_height: 800
camera_matrix:
   rows: 3
   cols: 3
   data: [ 2.8601176673060849e+02, 0., 4.2690380827722919e+02, 0.,
       2.8532768388032918e+02, 4.0803558177499212e+02, 0., 0., 1. ]
distortion_coefficients:
   rows: 1
   cols: 4
   data: [ -4.7362977705872498e-03, 4.8894650703196173e-02,
       -5.6998538782791201e-02, 1.9026345800852582e-02 ]
distortion_model: equidistant
rectification_matrix:
   rows: 3
   cols: 3
   data: [ 9.9995006920047491e-01, -1.7551567526819791e-03,
       9.8376079785193228e-03, 1.7555323140681356e-03,
       9.9999845861562731e-01, -2.9540882900355133e-05,
       -9.8375409661040168e-03, 4.6809646599883010e-05,
       9.9995160912746039e-01 ]
projection_matrix:
   rows: 3
   cols: 4
   data: [ 2.8727184647822833e+02, 0., 2.8237225341796875e+02,
       -1.6693138104046195e+01, 0., 2.8727184647822833e+02,
       4.3397737121582031e+02, 0., 0., 0., 1., 0. ]
aravindSolteq commented 3 years ago

@sumitsarkar1 @matlabbe : This is cool . But I dont know why Im getting a problem with rtabmap standalone tool while calibrating. Screenshot from 2021-05-14 13-49-17

As you can see the error message in the rtabmap console.

aravindSolteq commented 3 years ago

@sumitsarkar1 @matlabbe Update The calibration tool works with librealsense v2.40 but it doesnt work with librealsense v2.44.

sumitsarkar1 commented 3 years ago

Exactly...I tried with old versions of Rtabmap (like 2020 Dec, when I was able to calibrate t265 with RTabmap) but was of no luck ..even with latest commit 4390cb6. Then I realised it must be librealsense ! rtabmap-calibration --driver 11 gives [ERROR] Missing frames (received 1, needed=2)

matlabbe commented 3 years ago

I reproduced the error with realsense v2.44 and v2.45 (was working with 2.42 before I upgraded). The rs2::syncer seems not working anymore with fisheye streams, don't know why. I tested 2 setups with T265 and D435i in IR stereo mode. I changed this: https://github.com/introlab/rtabmap/blob/4390cb64284b7c029ca091693d2f83713ff22846/corelib/src/camera/CameraRealSense2.cpp#L192-L196 to this:

void CameraRealSense2::frame_callback(rs2::frame frame)
{
    auto video_profile = frame.get_profile().as<rs2::video_stream_profile>();
     UWARN("Sync: %s %d %d %d %d %s type=%d %f", rs2_format_to_string(
             video_profile.format()),
             video_profile.width(),
             video_profile.height(),
             video_profile.fps(),
             video_profile.stream_index(),
             video_profile.stream_name().c_str(),
             video_profile.stream_type(),
             frame.get_timestamp());
    syncer_(frame);
}

I added also ERROR logs after the synchronization wait to see which frames are in the frameset.

T265 log (frameset=1, failing, note also that is random which fisheye 1 or 2 is in the final frameset):

[ WARN] (2021-05-14 10:23:54.199) CameraRealSense2.cpp:195::frame_callback() Sync: Y8 848 800 30 2 Fisheye 2 type=4 1621002234176.003662
[ WARN] (2021-05-14 10:23:54.231) CameraRealSense2.cpp:195::frame_callback() Sync: Y8 848 800 30 1 Fisheye 1 type=4 1621002234209.336914
[ WARN] (2021-05-14 10:23:54.233) CameraRealSense2.cpp:195::frame_callback() Sync: Y8 848 800 30 2 Fisheye 2 type=4 1621002234209.336914
[ WARN] (2021-05-14 10:23:54.264) CameraRealSense2.cpp:195::frame_callback() Sync: Y8 848 800 30 1 Fisheye 1 type=4 1621002234242.670410
[ WARN] (2021-05-14 10:23:54.265) CameraRealSense2.cpp:195::frame_callback() Sync: Y8 848 800 30 2 Fisheye 2 type=4 1621002234242.670410
[ WARN] (2021-05-14 10:23:54.297) CameraRealSense2.cpp:195::frame_callback() Sync: Y8 848 800 30 1 Fisheye 1 type=4 1621002234276.003906
[ WARN] (2021-05-14 10:23:54.299) CameraRealSense2.cpp:195::frame_callback() Sync: Y8 848 800 30 2 Fisheye 2 type=4 1621002234276.003906
[ WARN] (2021-05-14 10:23:54.330) CameraRealSense2.cpp:195::frame_callback() Sync: Y8 848 800 30 1 Fisheye 1 type=4 1621002234309.337158
[ WARN] (2021-05-14 10:23:54.332) CameraRealSense2.cpp:195::frame_callback() Sync: Y8 848 800 30 2 Fisheye 2 type=4 1621002234309.337158
[ WARN] (2021-05-14 10:23:54.364) CameraRealSense2.cpp:195::frame_callback() Sync: Y8 848 800 30 1 Fisheye 1 type=4 1621002234342.525879
[ WARN] (2021-05-14 10:23:54.365) CameraRealSense2.cpp:195::frame_callback() Sync: Y8 848 800 30 2 Fisheye 2 type=4 1621002234342.525879
[ WARN] (2021-05-14 10:23:54.397) CameraRealSense2.cpp:195::frame_callback() Sync: Y8 848 800 30 1 Fisheye 1 type=4 1621002234375.859131
[ERROR] (2021-05-14 10:23:54.398) CameraRealSense2.cpp:1476::captureImage() Missing frames (received 1, needed=2)
[ERROR] (2021-05-14 10:23:54.398) CameraRealSense2.cpp:1480::captureImage() stream type=4
[ERROR] (2021-05-14 10:23:54.398) CameraRealSense2.cpp:1482::captureImage() Received: Y8 848 800 30 1 Fisheye 1 type=4 1621002234375.859131

D435i (frameset=2, working):

[ WARN] (2021-05-14 10:22:48.313) CameraRealSense2.cpp:195::frame_callback() Sync: Y8 1280 720 30 2 Infrared 2 type=3 1621002168302.552979
[ WARN] (2021-05-14 10:22:48.345) CameraRealSense2.cpp:195::frame_callback() Sync: Y8 1280 720 30 1 Infrared 1 type=3 1621002168336.126465
[ WARN] (2021-05-14 10:22:48.345) CameraRealSense2.cpp:195::frame_callback() Sync: Y8 1280 720 30 2 Infrared 2 type=3 1621002168336.126465
[ WARN] (2021-05-14 10:22:48.379) CameraRealSense2.cpp:195::frame_callback() Sync: Y8 1280 720 30 1 Infrared 1 type=3 1621002168369.684082
[ WARN] (2021-05-14 10:22:48.380) CameraRealSense2.cpp:195::frame_callback() Sync: Y8 1280 720 30 2 Infrared 2 type=3 1621002168369.684082
[ERROR] (2021-05-14 10:22:48.383) CameraRealSense2.cpp:1231::captureImage() Received: Y8 1280 720 30 2 Infrared 2 type=3 1621002168369.684082
[ERROR] (2021-05-14 10:22:48.384) CameraRealSense2.cpp:1231::captureImage() Received: Y8 1280 720 30 1 Infrared 1 type=3 1621002168369.684082

I'll continue looking...

matlabbe commented 3 years ago

I tested on Windows by switching easily realsense2.dll from v2.44 and decreasing until v2.42 worked. They changed something inside realsense library between 2.42 and 2.43 that has broken rs2::syncer and T265 fisheye streams compatibility.

Current compatibility for T265:

matlabbe commented 2 years ago

A fix can be found here for the synchronization problem with T265: https://github.com/IntelRealSense/librealsense/issues/9030#issuecomment-962223017