IMU align with camera's Coordinates

[Abner0907]

I have met a issue when run realsense D455. When I run the ros Mono node, it can detetced the ORB feature and local the map. How ever, when I run ros Mono-inertial node, I found that it can not local the map with the D455 module.

I differ the offical Euor data bag, and monitor the imu topic, I found the imu has different coordinate.

so I guess it must be my bad input imu coordinates. I am very appreciated that someone can give me some references or anything related. many thanks again.

[Sherlock-hh]

hi, I met the same problem, I refer to the Euor datasets, turn the coordinate like this, cv::Point3f acc = {-accel_data.y, accel_data.x, accel_data.z}; but my code still have some problems

[Abner0907]

@Sherlock-hh hi, When you do like this, Can it detect the orb features

[Sherlock-hh]

you mean orbextractor? yes, it can detect the features

[Sherlock-hh]

here's my ORB_slam3 Info, I use D455 either:

ORB-SLAM3 Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
ORB-SLAM2 Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
This program comes with ABSOLUTELY NO WARRANTY;
This is free software, and you are welcome to redistribute it
under certain conditions. See LICENSE.txt.

Input sensor was set to: Stereo-Inertial

Loading ORB Vocabulary. This could take a while...
Vocabulary loaded!

Creation of new map with id: 0
Creation of new map with last KF id: 0
Seq. Name: 

Camera Parameters: 
- Camera: Pinhole
- fx: 642.176
- fy: 642.176
- cx: 639.825
- cy: 356.422
- k1: 0
- k2: 0
- p1: 0
- p2: 0
- fps: 30
- color order: BGR (ignored if grayscale)

Depth Threshold (Close/Far Points): 38.0073

ORB Extractor Parameters: 
- Number of Features: 2000
- Scale Levels: 8
- Scale Factor: 1.2
- Initial Fast Threshold: 20
- Minimum Fast Threshold: 7

Left camera to Imu Transform (Tbc): 
[1, 0, 0, 0.06479822844266891;
 0, 1, 0, 0.007400000002235174;
 0, 0, 1, 0.01601999998092651;
 0, 0, 0, 1]

IMU frequency: 60 Hz
IMU gyro noise: 0 rad/s/sqrt(Hz)
IMU gyro walk: 0 rad/s^2/sqrt(Hz)
IMU accelerometer noise: 0 m/s^2/sqrt(Hz)
IMU accelerometer walk: 0 m/s^3/sqrt(Hz)
not IMU meas
not enough acceleration
not enough acceleration
not enough acceleration
not enough acceleration
not enough acceleration
not enough acceleration
not enough acceleration
not enough acceleration
not enough acceleration
not enough acceleration
not enough acceleration
not enough acceleration
not enough acceleration
not enough acceleration
not enough acceleration
not enough acceleration
not enough acceleration
not enough acceleration
not enough acceleration
not enough acceleration
not enough acceleration
not enough acceleration
not enough acceleration
not enough acceleration
not enough acceleration

usually after the first frame It's keep reporting "not enough acceleration", but sometimes I met it stuck at optimizer.optimize(its[it]); ` int nBad=0; for(size_t it=0; it<4; it++) {

    vSE3->setEstimate(Converter::toSE3Quat(pFrame->mTcw));
    optimizer.initializeOptimization(0);
    optimizer.optimize(its[it]);

    nBad=0;
    for(size_t i=0, iend=vpEdgesMono.size(); i<iend; i++)
    {
        ORB_SLAM3::EdgeSE3ProjectXYZOnlyPose* e = vpEdgesMono[i];

It's will throw an error: Thread 51 received signal SIGSEGV, Segmentation fault. 0x00007ffff1e6fead in Eigen::DenseStorage<double, 4, 4, 1, 0>::DenseStorage (other=..., this=0xffffffff00000007) at /usr/include/eigen3/Eigen/src/Core/DenseStorage.h:194 194 DenseStorage(const DenseStorage& other) : m_data(other.m_data) { ` there are many error to solve, if you have some advice I'll be very thankful.

[Sherlock-hh]

hi, I met the same problem, I refer to the Euor datasets, turn the coordinate like this, cv::Point3f acc = {-accel_data.y, accel_data.x, accel_data.z}; but my code still have some problems

sorry, it's wrong, because in the config.yaml, there is a Tbc matrix already transformed the imu coordinate to camera 0 coordinate, it dosen't need to be transformed manully.

[Abner0907]

hi, I met the same problem, I refer to the Euor datasets, turn the coordinate like this, cv::Point3f acc = {-accel_data.y, accel_data.x, accel_data.z}; but my code still have some problems

sorry, it's wrong, because in the config.yaml, there is a Tbc matrix already transformed the imu coordinate to camera 0 coordinate, it dosen't need to be transformed manully.

yes, I have got this. Thank you all the same.

[Abner0907]

@Sherlock-hh hi, There is one question about use the D455. I found that D455 only publish one image topic /camera/color/image_raw if you use Stereo-Inertial, how can you subscribe the another image topic.

I use the different test as yours, It works for me when use mono thread. run command:

rosrun ORB_SLAM3 Mono Vocabulary/ORBvoc.txt Examples/Monocular-Inertial/D455i.yaml

It can not work for me when use below which get in the IMU data.

rosrun ORB_SLAM3 Mono_Inertial Vocabulary/ORBvoc.txt Examples/Monocular-Inertial/D455i.yaml

[Sherlock-hh]

@Sherlock-hh hi, There is one question about use the D455. I found that D455 only publish one image topic /camera/color/image_raw if you use Stereo-Inertial, how can you subscribe the another image topic.

I use the different test as yours, It works for me when use mono thread. run command:

rosrun ORB_SLAM3 Mono Vocabulary/ORBvoc.txt Examples/Monocular-Inertial/D455i.yaml

It can not work for me when use below which get in the IMU data.

rosrun ORB_SLAM3 Mono_Inertial Vocabulary/ORBvoc.txt Examples/Monocular-Inertial/D455i.yaml

sorry, I didn't use it ROSrun to get image, I use [librealsense](https://github.com/Shaxpy/Intel_Realsense_D455) to get two infrared images, have you see this issue ? actually, without imu, most realtime demo, it's can run well, I reinstall the opencv, it's solve the "stuck at optimizer.optimize(its[it])" now my code can run for 30 seconds? actually D455 have 2 infrared image, here is my code: ir_frame_left = data.get_infrared_frame(1)) img_L=cv::Mat(cv::Size(ir_frame_left.get_width(), ir_frame_left.get_height()), CV_8UC1, (void *)ir_frame_left.get_data(), cv::Mat::AUTO_STEP)) ir_frame_right = data.get_infrared_frame(2)) img_r=cv::Mat(cv::Size(ir_frame_right.get_width(), ir_frame_right.get_height()), CV_8UC1, (void *)ir_frame_right.get_data(), cv::Mat::AUTO_STEP))

[Abner0907]

@Sherlock-hh Thank you for your kindly share, i have one T265, I will test it and let you know the result tomorrow. Today I have some flight controller work to finish.

Can you localtion with the stereo demo in the first 30 seconds?

[Abner0907]

I use librealsense to get two infrared images, have you see this issue ?

I will also review your reference. thank you for your share.

[Sherlock-hh]

@Sherlock-hh Thank you for your kindly share, i have one T265, I will test it and let you know the result tomorrow. Today I have some flight controller work to finish.

Can you localtion with the stereo demo in the first 30 seconds?

Thanks for your share! no, it can't, usually it's tracked success at first several frames, and then It's shows faile to track local map,and reset the map, restart the Track, and loop above. As I check the code, when it's start to Intialize, StereoInitialization() need to have enough motion, I don't know is mono same? but when I try to move the camera fast, It's keep report faile to track local map, I'll keep find out the reason.

IMU frequency: 250 Hz
IMU gyro noise: 0.00201078 rad/s/sqrt(Hz)
IMU gyro walk: 1.33364e-05 rad/s^2/sqrt(Hz)
IMU accelerometer noise: 0.0170676 m/s^2/sqrt(Hz)
IMU accelerometer walk: 0.000377325 m/s^3/sqrt(Hz)
not IMU meas
dist:7798639747.243557
First KF:0; Map init KF:0
New Map created with 147 points
Tracked! 
Tracked! 
Tracked! 
Tracked! 
Tracked! 
Tracked! 
Tracked! 
Tracked! 
Tracked! 
Tracked! 
Tracked! 
Tracked! 
Tracked! 
Tracked! 
Tracked! 
Tracked! 
Tracked! 
Tracked! 
Tracked! 
Tracked! 
Tracked! 
Tracked! 
Tracked! 
Tracked! 
Tracked! 
Tracked! 
Tracked! 
Tracked! 
Tracked! 
Tracked! 
Tracked! 
Tracked! 
Tracked! 
Tracked! 
Tracked! 
Tracked! 
Tracked! 
Tracked! 
Tracked! 
Tracked! 
Tracked! 
Tracked! 
Tracked! 
Tracked! 
Tracked! 
Tracked! 
Fail to track local map!
IMU is not or recently initialized. Reseting active map...
Tracked! 
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 = 1
mnInitialFrameId = 0
47 Frames set to lost
not IMU meas
dist:0.65457307804665832
First KF:9; Map init KF:0
New Map created with 199 points
Tracked! 
Tracked! 
Tracked! 
Tracked! 
Tracked! 
Tracked! 
Fail to track local map!
IMU is not or recently initialized. Reseting active map...
Tracked! 
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 = 49
mnInitialFrameId = 47
54 Frames set to lost
not IMU meas
dist:0.53143759665025792
First KF:10; Map init KF:9
New Map created with 124 points
Tracked! 
Fail to track local map!
IMU is not or recently initialized. Reseting active map...
Tracked! 
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 = 57
mnInitialFrameId = 55
'''

[0y8w1x]

@Sherlock-hh @Abner0907 did you resolve the problem ? Because I'm having the same troubles.

[Abner0907]

@0y8w1x nope, I can not test it at last as my camera is used by others.

[Sherlock-hh]

the imu initialization need motion, you can try to move the imu at first till it print the "end VBA2".

[Abner0907]

no issues any more. I close it. If anyone is interested in ORB_SLAM. we can chat in wechat.

[zgxsin]

no issues any more. I close it. If anyone is interested in ORB_SLAM. we can chat in wechat.

Could you tell how you solved the issue? thanks a lot!

[Styazoua]

no issues any more. I close it. If anyone is interested in ORB_SLAM. we can chat in wechat.

How did you solve this problem?

[cmakelabs]

Could you please share how to solve this issue