stereo-rosnode has too high latency and the program died

[Shame-fight]

Thank you for your work.I have test the stereo_rosnode,and have some problems: 1.The latency is very low at first, and the program runs for 2-3 seconds and then gets higher and higher, with a delay of about 4-5 seconds. And the CPU utilization gradually increases, and the program crashes when the camera moves too fast or the map is rebuilt..My hardware devices are as follows: Intel® Core™ i7-7700K CPU @ 4.20GHz × 8
GeForce GTX 1080/PCIe/SSE2 Is my hardware device not able to run in real time? But can run ORBSLAM2 in real time, I am confused about this.

[Yourbee]

have the same problem with my realsense t265 Mono/Mono_Inertial, #37 say it may be a calibration problem, but i have not figure it out.

[ccamposm]

Hi @Shame-fight, thanks for sharing that problem. For stereo-rosnode, I suggest you to reduce the queue size to one. Some frames may be skipped if there is not enough time to process them, but system is prepared to deal with that situation. You should change to:

message_filters::Subscriber<sensor_msgs::Image> left_sub(nh, "/camera/left/image_raw", 1);
message_filters::Subscriber<sensor_msgs::Image> right_sub(nh, "/camera/right/image_raw", 1);

@Yourbee For inertial modes (mono and stereo), similar idea applies. In those cases you have to guarantee that only most recent frame is kept (queue size is one). You should change to:

void ImageGrabber::GrabImage(const sensor_msgs::ImageConstPtr &img_msg)
{
  mBufMutex.lock();
  if (!img0Buf.empty())
    img0Buf.pop();
  img0Buf.push(img_msg);
  mBufMutex.unlock();
}

ImuGrabber does not need to be modified. I have tested these changes in euroc dataset (V102, V202 and V203) for stereo, mono-inertial and stereo-inertial and it worked fine. Let us know if problems persist or these changes solve them.

[Shame-fight]

Hi,thank you for your advice,it worked.and I have another question:My zed stereo camera can run stereo_rosnode, but I want better results, so I want to use mynteye stereo camera (including imu) to run ros_stereo-inertial. There is a problem: the map has been restarted, and the detected feature points are less than 100 .Is the camera not calibrated accurately, or is it caused by other problems? The output information is as follows: not IMU meas First KF:0; Map init KF:0 New Map created with 75 points build optimization graph start optimization end optimization update Keyframes velocities and biases IMU in Map 0 is initialized Not enough motion for initializing. Reseting... TRACK: Reset map because local mapper set the bad imu flag LM: Reseting current map in Local Mapping... LM: End reseting Local Mapping... LM: Reset free the mutex 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 39 Frames set to lost not IMU meas First KF:11; Map init KF:0 New Map created with 60 points Fail to track local map! IMU is not or recently initialized. Reseting active 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 = 42 mnInitialFrameId = 40 41 Frames set to lost not IMU meas First KF:12; Map init KF:11 New Map created with 53 points Fail to track local map! IMU is not or recently initialized. Reseting active 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 = 45 mnInitialFrameId = 43 43 Frames set to lost thakns a lot @ccamposm

[ccamposm]

It seems that sometimes it successfully initialized vision, but there is not enough motion to initialize IMU. And other times, vision initialized map with few points and it immediately lost. I would suggest you to:

1. run the same sequence in stereo, without IMU measurements. If problem persists, you should check your cameras calibration (intrinsics, distortion coefficients and Tlr). Are you using fisheye camera model or are you rectifying the stereo pair?
2. If stereo node runs successfully, problem may be that not enough motion exists. Then, try with a new sequence with more movement during the first seconds when initializing. Finally, you should check also your IMU calibration.

[surfii3z]

@ccamposm Thank you for your kind suggestion.

I can confirm that changing the queue_size to 1 does help mitigating the problem on Mono.

[ccamposm]

Reopen if problem persists or more details are required.

[ihamieh]

@Shame-fight @ccamposm I'm having a similar problem! I'm running my data with fisheye and I modified the yaml file for my camera. I posted a new issue https://github.com/UZ-SLAMLab/ORB_SLAM3/issues/117 and the following are my output information:

7H:~/ORB_SLAM3$ rosrun ORB_SLAM3 Mono Vocabulary/ORBvoc.txt Examples/ROS/ORB_SLAM3/TUM_512_IH.yaml

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: Monocular

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: K1 = 0.00348239 K2 = 0.000715035 K3 = -0.00205324 K4 = 0.000202937

Camera Parameters:

fx: 190.978 fy: 190.973 cx: 254.932 cy: 256.897 bf: 0 k1: 0 k2: 0 p1: 0 p2: 0 k3: -0.00205324 fps: 20 color order: RGB (ignored if grayscale) ORB Extractor Parameters:

Number of Features: 1500 Scale Levels: 8 Scale Factor: 1.2 Initial Fast Threshold: 20 Minimum Fast Threshold: 7 First KF:0; Map init KF:0 New Map created with 53 points Point distribution in KeyFrame: left-> 53 --- right-> 0 TRACK_REF_KF: Less than 15 matches!! Fail to track local map! SYSTEM-> Reseting active map in monocular case 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 = 118 mnInitialFrameId = 0 1 Frames set to lost First KF:2; Map init KF:0 New Map created with 71 points Point distribution in KeyFrame: left-> 71 --- right-> 0 TRACK_REF_KF: Less than 15 matches!! Fail to track local map! SYSTEM-> Reseting active map in monocular case 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 = 139 mnInitialFrameId = 119

[whuer-mspace]

It seems that sometimes it successfully initialized vision, but there is not enough motion to initialize IMU. And other times, vision initialized map with few points and it immediately lost. I would suggest you to:

1. run the same sequence in stereo, without IMU measurements. If problem persists, you should check your cameras calibration (intrinsics, distortion coefficients and Tlr). Are you using fisheye camera model or are you rectifying the stereo pair?
2. If stereo node runs successfully, problem may be that not enough motion exists. Then, try with a new sequence with more movement during the first seconds when initializing. Finally, you should check also your IMU calibration. @ccamposm Hello, Can you help me look at this problem? Thanks in advance！ https://github.com/UZ-SLAMLab/ORB_SLAM3/issues/245

[Ador2]

It seems that sometimes it successfully initialized vision, but there is not enough motion to initialize IMU. And other times, vision initialized map with few points and it immediately lost. I would suggest you to:

1. run the same sequence in stereo, without IMU measurements. If problem persists, you should check your cameras calibration (intrinsics, distortion coefficients and Tlr). Are you using fisheye camera model or are you rectifying the stereo pair?

2. If stereo node runs successfully, problem may be that not enough motion exists. Then, try with a new sequence with more movement during the first seconds when initializing. Finally, you should check also your IMU calibration.

what about fisheye lenses?