Resetting camera

[Ezio46]

For some reason, a restart occurs every few seconds and all information received from the camera is reset

console output:

InitTimeBetweenFrames: 1.03 RESETTING! destroyed ThreadReduce destroyed ThreadReduce Switching to initializer state: RealtimeCoarseIMUInitState PixelSelector: Using block sizes: 16, 16 PixelSelector: Using block sizes: 16, 16 InitTimeBetweenFrames: 0.032 InitTimeBetweenFrames: 0.066 InitTimeBetweenFrames: 0.1 InitTimeBetweenFrames: 0.132 InitTimeBetweenFrames: 0.166 InitTimeBetweenFrames: 0.2 InitTimeBetweenFrames: 0.232 InitTimeBetweenFrames: 0.266 InitTimeBetweenFrames: 0.3 InitTimeBetweenFrames: 0.332 InitTimeBetweenFrames: 0.366 InitTimeBetweenFrames: 0.4 InitTimeBetweenFrames: 0.432 InitTimeBetweenFrames: 0.466 InitTimeBetweenFrames: 0.5 InitTimeBetweenFrames: 0.532 InitTimeBetweenFrames: 0.566 InitTimeBetweenFrames: 0.6 InitTimeBetweenFrames: 0.632 InitTimeBetweenFrames: 0.666 InitTimeBetweenFrames: 0.7 InitTimeBetweenFrames: 0.732 InitTimeBetweenFrames: 0.766 InitTimeBetweenFrames: 0.8 InitTimeBetweenFrames: 0.832 InitTimeBetweenFrames: 0.866 InitTimeBetweenFrames: 0.9 InitTimeBetweenFrames: 0.932 InitTimeBetweenFrames: 0.966 InitTimeBetweenFrames: 1 InitTimeBetweenFrames: 1.032 RESETTING! destroyed ThreadReduce destroyed ThreadReduce Switching to initializer state: RealtimeCoarseIMUInitState PixelSelector: Using block sizes: 16, 16 PixelSelector: Using block sizes: 16, 16

[Ezio46]

This is what it looks like when the drone is in motion

InitTimeBetweenFrames: 0.834 Scaling with rescaleFactor: 0.995611 INITIALIZE FROM INITIALIZER (1995 pts)! CoarseIMUInit normalized error: 0.0653244 variance: 0 scale: 2.1551e-08 Large CoarseIMUInitializer error! Requesting full reset! 0.139622 RESETTING! MAPPING FINISHED! destroyed ThreadReduce destroyed ThreadReduce Switching to initializer state: RealtimeCoarseIMUInitState PixelSelector: Using block sizes: 16, 16 PixelSelector: Using block sizes: 16, 16 InitTimeBetweenFrames: 0.034 InitTimeBetweenFrames: 0.066 InitTimeBetweenFrames: 0.1 InitTimeBetweenFrames: 0.134 InitTimeBetweenFrames: 0.166 InitTimeBetweenFrames: 0.2 InitTimeBetweenFrames: 0.234 InitTimeBetweenFrames: 0.266 InitTimeBetweenFrames: 0.3 InitTimeBetweenFrames: 0.334 InitTimeBetweenFrames: 0.366 InitTimeBetweenFrames: 0.4 InitTimeBetweenFrames: 0.434 InitTimeBetweenFrames: 0.466 InitTimeBetweenFrames: 0.5 InitTimeBetweenFrames: 0.534 InitTimeBetweenFrames: 0.566 InitTimeBetweenFrames: 0.6 InitTimeBetweenFrames: 0.634 InitTimeBetweenFrames: 0.666 InitTimeBetweenFrames: 0.7 InitTimeBetweenFrames: 0.734 InitTimeBetweenFrames: 0.766 InitTimeBetweenFrames: 0.8 InitTimeBetweenFrames: 0.834 InitTimeBetweenFrames: 0.866 Scaling with rescaleFactor: 1.01431 INITIALIZE FROM INITIALIZER (1995 pts)! Large CoarseIMUInitializer error! Requesting full reset! 0.110187 RESETTING! MAPPING FINISHED! destroyed ThreadReduce destroyed ThreadReduce Switching to initializer state: RealtimeCoarseIMUInitState PixelSelector: Using block sizes: 16, 16 PixelSelector: Using block sizes: 16, 16 InitTimeBetweenFrames: 0.034 InitTimeBetweenFrames: 0.066 InitTimeBetweenFrames: 0.1 InitTimeBetweenFrames: 0.134 InitTimeBetweenFrames: 0.166 InitTimeBetweenFrames: 0.2 InitTimeBetweenFrames: 0.234 InitTimeBetweenFrames: 0.266 InitTimeBetweenFrames: 0.3 InitTimeBetweenFrames: 0.334 InitTimeBetweenFrames: 0.366 InitTimeBetweenFrames: 0.4 InitTimeBetweenFrames: 0.434 InitTimeBetweenFrames: 0.466 InitTimeBetweenFrames: 0.5 InitTimeBetweenFrames: 0.534 ^CInitTimeBetweenFrames: 0.566 InitTimeBetweenFrames: 0.6

[Ezio46]

I realized that the RESETTING is happening due to a large InitTimeBetweenFrames But I don't really understand why this is happening. My topics: cam0/image_raw:=/iris/c920/image_raw imu0:=/mavros/imu/data_raw

[lukasvst]

Actually there are two different things / resets going on here:

• InitTimeBetweenFrames RESETTING! When the camera is not in motion, the system cannot be initialized. In order to not have too much time between the first frame and the initialized frame, the system will regularly reset when it does not initialize for some time. This is normal and should stop once you start moving the camera.
• Large CoarseIMUInitializer error! Requesting full reset! 0.110187 This is the actual problem in your case. Background: After the visual system is initialized, it will try to initialize the visual-inertial system (IMU + camera), by aligning the camera trajectory with the prediction from the IMU. If this optimization reports a large error (0.110187 in your case), the system will assume that the visual system failed and reset.

If this (the second point) happens repeatedly, it generally means that your vision and IMU data don't agree for which there might be a number of reasons:

• The IMU-camera calibration is wrong.
• The IMU-camera time synchronization is off
• The IMU data is wrong in some other way
• The visual calibration is wrong. You can test this easily by setting useimu=0 and see if the pointcloud looks nice for an easy-to-track trajectory.

You can also try to just increase the threshold (e.g. pass init_requestFullResetNormalizedErrorThreshold=10) and see if it works.

[Ezio46]

Regarding resetting: when moving, it also happens (or should the movement be long?). At the same time, the frames are not smooth at all. I'm taking footage and imu from the drone topic in my Gazebo stimulation and would like a smoother rendering. Is there a problem with calibration? I have a c920 camera model taken from PX4 Autopilot. My camera.txt , calcchain.xml and the settings File (took t265_noise_tami.yami) I attach camera.txt pcalib_linear_8bit.txt camchain.txt

[Ezio46]

I made the drone spin in place. It seems that at some point resetting is really eliminated and pointcloud starts to appear, but it crashes pretty quickly with the next set of messages in the console

PixelSelector: Using block sizes: 16, 16 InitTimeBetweenFrames: 0.038 InitTimeBetweenFrames: 0.084 InitTimeBetweenFrames: 0.126 InitTimeBetweenFrames: 0.166 InitTimeBetweenFrames: 0.204 InitTimeBetweenFrames: 0.24 Scaling with rescaleFactor: 2.01496 INITIALIZE FROM INITIALIZER (1994 pts)! I THINK INITIALIZATINO FAILED! Resetting. RESETTING! destroyed ThreadReduce destroyed ThreadReduce Switching to initializer state: RealtimeCoarseIMUInitState PixelSelector: Using block sizes: 16, 16 PixelSelector: Using block sizes: 16, 16 InitTimeBetweenFrames: 0.044 InitTimeBetweenFrames: 0.094 InitTimeBetweenFrames: 0.144 InitTimeBetweenFrames: 0.188 InitTimeBetweenFrames: 0.232 InitTimeBetweenFrames: 0.276 Scaling with rescaleFactor: 1.15152 INITIALIZE FROM INITIALIZER (1991 pts)! WARNING: Coarse tracker thinks that tracking was not good! Forcing NO KF! WARNING: Coarse tracker thinks that tracking was not good! Forcing NO KF! WARNING: Coarse tracker thinks that tracking was not good! Forcing NO KF! REPEAT LEVEL! WARNING: Coarse tracker thinks that tracking was not good! Forcing NO KF! node: /home/igor/dm-vio/src/dso/OptimizationBackend/EnergyFunctional.cpp:319: void dso::EnergyFunctional::resubstituteFPt(const VecCf&, dso::Mat18f, int, int, dso::Vec10, int): Assertion std::isfinite(p->data->step)' failed. node: /home/igor/dm-vio/src/dso/OptimizationBackend/EnergyFunctional.cpp:319: void dso::EnergyFunctional::resubstituteFPt(const VecCf&, dso::Mat18f*, int, int, dso::Vec10*, int): Assertionstd::isfinite(p->data->step)' failed. node: /home/igor/dm-vio/src/dso/OptimizationBackend/EnergyFunctional.cpp:319: void dso::EnergyFunctional::resubstituteFPt(const VecCf&, dso::Mat18f, int, int, dso::Vec10, int): Assertion `std::isfinite(p->data->step)' failed. Aborted (core dumped)

[Ezio46]

With minor intervention, the dm-vio still started to work stably (with fast movement, it constantly crashes), but it is not clear that this is a problem with the mood of going to camchain.yaml (I chose cam1 because I was afraid that she was here because of me): cam0: T_cam_imu:

• [-0.9995250378696743, 0.029615343885863205, -0.008522328211654736, 0.04727988224914392]
• [0.0075019185074052044, -0.03439736061393144, -0.9993800792498829, -0.047443232143367084]
• [-0.02989013031643309, -0.998969345370175, 0.03415885127385616, -0.0681999605066297]
• [0.0, 0.0, 0.0, 1.0] cam_overlaps: [1] camera_model: pinhole distortion of the coffers: [0.0034823894022493434, 0.0007150348452162257, -0.0020532361418706202, 0.00020293673591811182] Distortion model: equidistant internal components: [1394.6027293299926, 1394.6027293299926, 995.588675691456, 599.3212928484164] Resolution: [1920, 1080] rostopic: /cam0/image_raw

After a much more impressive time, even with slow movement, the Large CoarseIMUInitializer error crashes! Requesting full reset! 0.977979 At the same time, during the flight, he constantly writes similar messages to CoarseIMUInit normalized error: 0.0275383 variation: 14.3817 scale: 1.74535 Maybe the problem is in T_cam_imu, which I did not change?

[Ezio46]

The launch command looks like this now (since the camera is not fisheye, how did I understand that vignette and gamma are not needed?)

rosrun dmvio_ros node nogui=0 useimu=1 quiet=1 mode=3 calib=/home/igor/RealSense/camera.txt imuCalib=/home/igor/RealSense/camchain.yaml settingsFile=/home/igor/RealSense/t265_noise_tumvi.yaml resultsPrefix=/home/igor/RealSense/Results.txt cam0/image_raw:=/iris/c920/image_raw imu0:=/mavros/imu/data_raw

[Ezio46]

At the same time, without imu (useimu=0), only WARNING is written to the console: Not sending frame, because it does not have ANY data yet. And somehow draws a route in dm-vio (which is not particularly similar to rotation around its axis)

[Ezio46]

If you set init_requestFullResetNormalizedErrorThreshold: 10, then the error in the console is as follows:

CoarseIMUInit normalized error: 0.283067 variance: 4.49743 scale: 7.31841 Switching to initializer state: RealtimePGBAState Updating transform with index 0 to val: 7.31841 PGBA: Built graph with 9 IMU factors and 11 poses. PGBA Error: 51.7286 PGBA: 7.31595 var: 24.4113 3.5552 7.44438 2.75631 5.01826e-05 5.02694e-05 4.99076e-05 Switching to initializer state: RealtimeCoarseIMUInitState CoarseIMUInit normalized error: 0.401977 variance: 5.36444 scale: 4.97382 CoarseIMUInit normalized error: 0.481794 variance: 7.38771 scale: 4.23474 CoarseIMUInit normalized error: 0.502064 variance: 4.32981 scale: 5.50059 Switching to initializer state: RealtimePGBAState Updating transform with index 0 to val: 5.50059 PGBA: Built graph with 11 IMU factors and 13 poses. PGBA Error: 91.9033 PGBA: 5.46979 var: 27.7567 1.54394 2.23269 1.90679 4.11845e-05 4.12577e-05 4.1019e-05 Switching to initializer state: RealtimeCoarseIMUInitState CoarseIMUInit normalized error: 2.72618 variance: 2.97104 scale: 5.50207 Switching to initializer state: RealtimePGBAState Updating transform with index 0 to val: 5.50207 PGBA: Built graph with 12 IMU factors and 14 poses. PGBA Error: 150.242 PGBA: 5.5036 var: 26.3831 1.04906 1.61089 1.43823 3.79578e-05 3.79701e-05 3.77647e-05 Switching to initializer state: RealtimeCoarseIMUInitState CoarseIMUInit normalized error: 4.2548 variance: 2.86894 scale: 5.50242 Switching to initializer state: RealtimePGBAState Updating transform with index 0 to val: 5.50242 PGBA: Built graph with 13 IMU factors and 15 poses. terminate called after throwing an instance of 'Sophus::ScaleNotPositive' what(): Sophus exception: Scale factor is not positive Aborted (core dumped)

I found the data for camera.txt in the c920 model, and there were even some necessary ones for the cam chain.yaml. After that, I calculated the T_cam_imu using the initial camera and imu positions in the Gazebo. Current files camera.txt and camchain.yaml look like this:

camera.txt: Pinhole 1360.4704964995865 1360.4704964995865 960.5 540.5 0 1920 1080 crop 1920 1072

camchain.yaml: cam0: T_cam_imu:

• [0.99999964, -0.000002, 0.000852, -0.06997401]
• [0.00000202, 1.0, -0.000022, -0.00000157]
• [-0.000852, 0.000022, 0.99999964, -0.02580739]
• [0.0, 0.0, 0.0, 1.0] cam_overlaps: [1] camera_model: Pinhole distortion_coeffs: [0.0, 0.0, 0.0, 0.0, 0.0] distortion_model: plumb_bob intrinsics: [1360.4704964995865, 1360.4704964995865, 960.5, 540.5] resolution: [1920, 1072] rostopic: /cam0/image_raw

Nevertheless, the problem remains and I no longer know what else needs to be calibrated and how

[lukasvst]

This looks like your camera.txt does not include the distortion coefficients of the camera model which would explain that it does not work.

Just to clarify: Does the system work well with useimu=0 when slowly moving? You should be able to recognize your environment in the reconstructed pointcloud. Once this works you can try enabling the IMU again.

[Ezio46]

I set it up as a Pinhole, is it necessary to specify these coefficients there? Or is it better not to specify it as a Pinhole? Regarding useimu=0, I'll check again, but it seems like pointcloud turned out to be normal. Thanks for your help

[Ezio46]

In general, his surroundings are quite clear. However, the shelves on the left are more massive (is this normal?). The coefficients in the camera model itself are set to zero, and in general, everything works without imu (although only at low speeds)

[Ezio46]

Or is it still a bad result?

[Ezio46]

As a result, I even changed the imu topic to 200 Hz (the frequency of publications from the camera is 50 Hz), the result is the same - almost instantaneous Large CoarseIMUInitializer error! Requesting full reset! 340.953

camera.txt: Pinhole 1360.4704964995865 1360.4704964995865 960.5 540.5 0 1920 1080 crop 1920 1072

camchain.yaml: cam0: T_cam_imu:

• [1.0, 0.0, 0.0, -0.07]
• [0.0, 1.0, 0.0, 0.0]
• [0.0, 0.0, 1.0, -0.046]
• [0.0, 0.0, 0.0, 1.0] cam_overlaps: [1] camera_model: Pinhole distortion_coeffs: [0.0, 0.0, 0.0, 0.0, 0.0] distortion_model: plumb_bob intrinsics: [1360.4704964995865, 1360.4704964995865, 960.5, 540.5] resolution: [1920, 1072] rostopic: /cam0/image_raw

t_265_noise_tumvi.yaml:

accelerometer_noise_density: 0.00186 gyroscope_noise_density: 0.00018665 accelerometer_random_walk: 0.006 gyroscope_random_walk: 0.000038785 integration_sigma: 0.316227 maxSkipFramesVisualOnlyMode: 1 # This many frames can be skipped at a time while in visual-only mode. maxSkipFramesVisualInertial: 2 # This many frames can be skipped at a time while in visual-inertial mode. skipFramesVisualOnlyDelay: 30 # After visual initializer finished, wait this amount of frames before switching to the visualOnly threshold. This is useful because during the first frames the system might be less stable than later. minQueueSizeForSkipping: 2 # Don't skip frames if the image queue is smaller than this. maxTimeBetweenInitFrames: 1.0 # Reset the visual initializer if this time has passed between the first and last frame. init_pgba_skipFirstKFs: 1 init_requestFullResetNormalizedErrorThreshold: 10 # If the error after the CoarseIMUInit is larger than this we assume the visual system failed and reset the full system. normalizeCamSize: 0.2 # Sets the size of the displayed camera (normalized with the scale obtained from IMU data). init_coarseScaleUncertaintyThresh: 5 # 1.0 init_pgba_scaleUncertaintyThresh: 5 # 1.0 init_pgba_reinitScaleUncertaintyThresh: 1 # 0.5 setting_minIdepth: 0.0

console command:

rosrun dmvio_ros node nogui=0 useimu=1 quiet=1 mode=3 calib=/home/igor/RealSense/camera.txt imuCalib=/home/igor/RealSense/camchain.yaml settingsFile=/home/igor/RealSense/t265_noise_tumvi.yaml resultsPrefix=/home/igor/RealSense/Results.txt cam0/image_raw:=/iris/c920/image_raw imu0:=/raw_imu

[lukasvst]

The pointclouds in the images you posted don't look clean / sharp enough.

Looking at what you posted earlier, you need to specify the Equidistant distortion model and the distortion coefficients in your camera.txt, as explained in https://github.com/lukasvst/dm-vio/blob/master/src/dso/README.md#calibration-file-for-equidistant-camera-model

Pinhole (what you specified at the moment in your camera.txt) should only be used for pre-rectified images without any distortion which seems to be not the case here.

[Ezio46]

For the c920, the coefficients are set to zero in the description of the sdf model. I tried equidistant and it only got worse

[lukasvst]

Another thing you can try is to decrease the resolution of the output image (last line of the camera.txt). Many parts of the visual system are optimized for ~VGA resolution (e.g. 640x480 / 512x512, etc). When the resolution is much larger like in your case it might be less stable.

[Ezio46]

I tried 640x480 Does it look more correct now?

[Ezio46]

As a result, everything worked. The camera simply did not have enough visibility distance (it had only 10 exposed)