search

lukasvst / dm-vio-ros

ROS wrapper for DM-VIO: Delayed Marginalization Visual-Inertial Odometry
GNU General Public License v3.0
111 stars 21 forks source link

Repeated resets when running from a rosbag #7

Closed martinakos closed 1 year ago

martinakos commented 1 year ago

I got dm-vio working with a realsense t265 camera. I've also got this dm-vio-ros wrapper to work well with a live t265 camera. However, when running from a rosbag I keep getting repeated resets and the /dmvio/system_status only oscillates between 0 and 1. Curiously, the same rosbag with useimu=0 tracks well.

The output I get looks like this: 

Enabling IMU integration!
QUIET MODE, I'll shut up!
PHOTOMETRIC MODE WITH CALIBRATION, BUT NO OR INACCURATE EXPOSURE!
Loading settings from yaml file: /home/user/develop/thirdparty/dm-vio/configs/t265_noise_tumvi.yaml!
Enabling IMU integration!
Settings:
accelerometer_noise_density: 0.224
accelerometer_random_walk: 0.043
addVisualToCoarseGraphIfTrackingBad: 0
alwaysCanBreakIMU: 0
baToCoarseAccBiasVariance: 1000
baToCoarseGyrBiasVariance: 0.05
baToCoarsePoseVariance: 0.1
baToCoarseRotVariance: 1
baToCoarseVelVariance: 0.1
calib: //mnt/data/data/dm-vio/FACTORY_CALIBRATION_WILL_BE_SAVED_HERE.txt
dynamicWeightRMSEThresh: 8
fixKeyframeDuringCoarseTracking: 1
gamma: /home/user/develop/thirdparty/dm-vio/configs/pcalib_linear_8bit.txt
generalScaleIntervalSize: 60
gravityDirectionFixZ: 1
gyroscope_noise_density: 0.0128
gyroscope_random_walk: 0.0011
imuCalib: //mnt/data/data/dm-vio/t265_factory_camchain3.yaml
init_coarseScaleUncertaintyThresh: 5
init_conversionType: 0
init_disableVIOUntilFirstInit: 1
init_fixPoses: 1
init_initDSOParams: 1
init_lambdaLowerBound: 1e-16
init_maxNumPoses: 100
init_multipleBiases: 0
init_multithreadedInitDespiteNonRT: 0
init_onlyKFs: 1
init_pgba_conversionType: 0
init_pgba_delay: 100
init_pgba_prepareGraphAddDelValues: 0
init_pgba_prepareGraphAddFactors: 0
init_pgba_priorExtrinsicsRot: 0.01
init_pgba_priorExtrinsicsTrans: 0.1
init_pgba_priorGravityDirection: 0.4
init_pgba_priorGravityDirectionZ: 0.0001
init_pgba_reinitScaleUncertaintyThresh: 1
init_pgba_scaleUncertaintyThresh: 5
init_pgba_skipFirstKFs: 1
init_priorExtrinsicsRot: 0.01
init_priorExtrinsicsTrans: 0.1
init_priorGravityDirection: 0.4
init_priorGravityDirectionZ: 0.0001
init_priorRotSigma: 1e-05
init_priorTransSigma: 1e-05
init_requestFullResetErrorThreshold: -1
init_requestFullResetNormalizedErrorThreshold: 0.1
init_scalePriorAfterInit: 0
init_secondthreshGravdir: 1000
init_secondthreshScale: 1e+07
init_threshGravdir: 1000
init_threshScale: 1.02
init_transitionModel: 2
init_updatePoses: 1
integration_sigma: 0.316227
maxFrameEnergyThreshold: 5000
maxSkipFramesFullReset: -1
maxSkipFramesVisualInertial: 2
maxSkipFramesVisualInit: 0
maxSkipFramesVisualOnlyMode: 1
maxTimeBetweenInitFrames: 1
minQueueSizeForSkipping: 2
normalizeCamSize: 0.2
numMeasurementsGravityInit: 40
preload: 0
priorExtrinsicsRot: 0.01
priorExtrinsicsTrans: 0.1
priorGravityDirection: 0.4
priorGravityDirectionZ: 0.0001
resultsPrefix: //mnt/data/data/dm-vio/
setting_forceNoKFTranslationThresh: 0
setting_maxOptIterations: 6
setting_minFramesBetweenKeyframes: -0.5
setting_minIdepth: 0.02
setting_minOptIterations: 1
setting_optGravity: 1
setting_optIMUExtrinsics: 0
setting_optScaleBA: 1
setting_prior_bias: 0
setting_prior_velocity: 0
setting_scaleFixTH: 0
setting_solverMode: 2048
setting_transferCovToCoarse: 1
setting_visualOnlyAfterScaleFixing: 0
setting_weightDSOCoarse: 0.001
setting_weightDSOToGTSAM: 1.66667e-05
setting_weightZeroPriorDSOInitX: 0
setting_weightZeroPriorDSOInitY: 0
skipFirstKeyframe: 0
skipFramesVisualOnlyDelay: 30
speed: 0
start: 2
transferCovToCoarseMultiplier: 1
updateDynamicWeightDuringOptimization: 1
useScaleDiagonalHack: 0
vignette: /home/user/develop/thirdparty/dm-vio/configs/realsense/vignette_t265.png
Reading Calibration from file //mnt/data/data/dm-vio/FACTORY_CALIBRATION_WILL_BE_SAVED_HERE.txt ... found!
Creating KannalaBrandt undistorter
Input resolution: 848 800
In: KannalaBrandt 285.468000 285.555000 414.625000 408.867000 -0.004671 0.041731 -0.038895 0.006624
Out: 0.200000 0.200000 0.499000 0.499000 0.000000
Output resolution: 848 800

Rectified Kamera Matrix:
  169.6       0 422.652
      0     160   398.7
      0       0       1

Reading Photometric Calibration from file /home/user/develop/thirdparty/dm-vio/configs/pcalib_linear_8bit.txt
Reading Vignette Image from /home/user/develop/thirdparty/dm-vio/configs/realsense/vignette_t265.png
Successfully read photometric calibration!
using pyramid levels 0 to 4. coarsest resolution: 53 x 50!
Loading IMU parameter file at: //mnt/data/data/dm-vio/t265_factory_camchain3.yaml
Used T_cam_imu: 
   -0.999899    0.0005102    0.0141741    0.0106989
  -0.0004956    -0.999999    0.0010342  5.30262e-06
   0.0141746    0.0010271     0.999899 -0.000151668
           0            0            0            1
Used noise values: 0.043 0.0011 0.224 0.0128
START PANGOLIN!
Switching to initializer state: RealtimeCoarseIMUInitState
PixelSelector: Using block sizes: 16, 16
OpenGL Error 502: GL_INVALID_OPERATION: The specified operation is not allowed in the current state.
In: /usr/local/include/pangolin/gl/gl.hpp, line 214
PixelSelector: Using block sizes: 16, 16
InitTimeBetweenFrames: 0.0333216
InitTimeBetweenFrames: 0.0666792
InitTimeBetweenFrames: 0.100001
InitTimeBetweenFrames: 0.133346
InitTimeBetweenFrames: 0.166668
InitTimeBetweenFrames: 0.200013
InitTimeBetweenFrames: 0.233335
InitTimeBetweenFrames: 0.26668
InitTimeBetweenFrames: 0.300002
InitTimeBetweenFrames: 0.333347
InitTimeBetweenFrames: 0.366668
InitTimeBetweenFrames: 0.400013
InitTimeBetweenFrames: 0.433335
InitTimeBetweenFrames: 0.46668
InitTimeBetweenFrames: 0.500002
InitTimeBetweenFrames: 0.533347
InitTimeBetweenFrames: 0.566669
InitTimeBetweenFrames: 0.600013
InitTimeBetweenFrames: 0.633336
InitTimeBetweenFrames: 0.666681
InitTimeBetweenFrames: 0.700002
InitTimeBetweenFrames: 0.733347
InitTimeBetweenFrames: 0.766669
InitTimeBetweenFrames: 0.800014
InitTimeBetweenFrames: 0.833336
InitTimeBetweenFrames: 0.866681
InitTimeBetweenFrames: 0.900002
InitTimeBetweenFrames: 0.933347
InitTimeBetweenFrames: 0.96667
InitTimeBetweenFrames: 1.00001
RESETTING!
destroyed ThreadReduce
Switching to initializer state: RealtimeCoarseIMUInitState
PixelSelector: Using block sizes: 16, 16
PixelSelector: Using block sizes: 16, 16
InitTimeBetweenFrames: 0.033345
InitTimeBetweenFrames: 0.0666668
InitTimeBetweenFrames: 0.100012
InitTimeBetweenFrames: 0.133333
InitTimeBetweenFrames: 0.166678
InitTimeBetweenFrames: 0.200001
InitTimeBetweenFrames: 0.233148
InitTimeBetweenFrames: 0.26647
InitTimeBetweenFrames: 0.299815
InitTimeBetweenFrames: 0.333137
InitTimeBetweenFrames: 0.366482
InitTimeBetweenFrames: 0.399804
InitTimeBetweenFrames: 0.433149
InitTimeBetweenFrames: 0.46647
InitTimeBetweenFrames: 0.499815
InitTimeBetweenFrames: 0.533138
InitTimeBetweenFrames: 0.566482
InitTimeBetweenFrames: 0.599804
InitTimeBetweenFrames: 0.633149
InitTimeBetweenFrames: 0.666471
InitTimeBetweenFrames: 0.699816
InitTimeBetweenFrames: 0.733249
InitTimeBetweenFrames: 0.766594
InitTimeBetweenFrames: 0.799916
InitTimeBetweenFrames: 0.833261
InitTimeBetweenFrames: 0.866583
InitTimeBetweenFrames: 0.899928
InitTimeBetweenFrames: 0.93325
InitTimeBetweenFrames: 0.966595
InitTimeBetweenFrames: 0.999917
InitTimeBetweenFrames: 1.03326
RESETTING!
destroyed ThreadReduce
Switching to initializer state: RealtimeCoarseIMUInitState
PixelSelector: Using block sizes: 16, 16
PixelSelector: Using block sizes: 16, 16
InitTimeBetweenFrames: 0.0333216
InitTimeBetweenFrames: 0.0666668
InitTimeBetweenFrames: 0.0999887
InitTimeBetweenFrames: 0.133334
InitTimeBetweenFrames: 0.166656
InitTimeBetweenFrames: 0.2
InitTimeBetweenFrames: 0.233403
InitTimeBetweenFrames: 0.266679
InitTimeBetweenFrames: 0.300059
InitTimeBetweenFrames: 0.333357
InitTimeBetweenFrames: 0.366714
InitTimeBetweenFrames: 0.400024
InitTimeBetweenFrames: 0.433381
InitTimeBetweenFrames: 0.466702
InitTimeBetweenFrames: 0.499994
InitTimeBetweenFrames: 0.533315
InitTimeBetweenFrames: 0.56666
InitTimeBetweenFrames: 0.599982
Scaling with rescaleFactor: 1.54566
INITIALIZE FROM INITIALIZER (1999 pts)!
Large CoarseIMUInitializer error! Requesting full reset! 0.371034
RESETTING!
MAPPING FINISHED!
destroyed ThreadReduce
Switching to initializer state: RealtimeCoarseIMUInitState
PixelSelector: Using block sizes: 16, 16
PixelSelector: Using block sizes: 16, 16
InitTimeBetweenFrames: 0.0333102
InitTimeBetweenFrames: 0.0666666
InitTimeBetweenFrames: 0.0999887
InitTimeBetweenFrames: 0.133322
InitTimeBetweenFrames: 0.166655
InitTimeBetweenFrames: 0.199989
Scaling with rescaleFactor: 5.72666
INITIALIZE FROM INITIALIZER (2002 pts)!

It seems to start tracking for a few moments but it resets again. I suspected from the camchain but I've checked it a few times and I think it's correct.

Do you have any suggestions to get it to work with this rosbag and useimu=1?

lukasvst commented 1 year ago

You can increase the parameter init_requestFullResetNormalizedErrorThreshold in t265_noise_tumvi.yaml (or simply pass a different value as a commandline parameter, e.g. init_requestFullResetNormalizedErrorThreshold=1.0

A bit of background: when initializing, the CoarseIMUInitializer checks the error of the graph and if it is above a threshold it assumes that the visual system failed. The best setting for it might depend on you application however. You can first set it to a very large value and have a look at the commandline output lines starting with CoarseIMUInit normalized error: and then set the threshold accordingly.

One more thing to note is that the dmvio_t265 executable will usually work a bit better than the ROS version with the Realsense, because the latter does not get the exposure times from the camera.

martinakos commented 1 year ago

Setting init_requestFullResetNormalizedErrorThreshold=1.0 worked! Thanks. I have another question but maybe I'll open another issue as it's not related to this one.