Resetting camera about dm-vio HOT 19 CLOSED

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

from dm-vio.

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

from dm-vio.

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.

from dm-vio.

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](https://github.com/lukasvst/dm-vio/files/14769880/pcalib_l
t265_noise_tumvi.txt
inear_8bit.txt)
camchain.txt

from dm-vio.

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): 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): Assertion `std::isfinite(p->data->step)' failed.
Aborted (core dumped)

from dm-vio.

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?

from dm-vio.

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

from dm-vio.

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)

from dm-vio.

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

from dm-vio.

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.

from dm-vio.

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

from dm-vio.

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)

from dm-vio.

Or is it still a bad result?

from dm-vio.

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

from dm-vio.

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.

from dm-vio.

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

from dm-vio.

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.

from dm-vio.

I tried 640x480
Does it look more correct now?

from dm-vio.

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

from dm-vio.