[ZUPT] Problems with zero velocity update #89
Description
I can run openvins without enabling the zero velocity update feature. Once i enable it, the program seems to get stuck when the program remains in zero velocity state for a long time (< 2 mins). And sometimes the program just dies (see below). And the program does not publish the pose of IMU when it is in zero velocity state. Is this expected or not? I think IMU pose should still be published with a different timestamp when the device is not moving.
[ZUPT]: accepted zero velocity |v_IinG| = 0.025 (chi2 0.547 < 126.574)
[ZUPT]: chi2_check over the residual limit - 4614
[ZUPT]: rejected zero velocity |v_IinG| = 0.025 (chi2 26.072 > 0.000)
[TIME]: 34.2291 seconds for tracking
[TIME]: 0.0053 seconds for propagation
[TIME]: 0.0096 seconds for MSCKF update (7 features)
[TIME]: 0.0012 seconds for SLAM update (9 feats)
[TIME]: 0.0000 seconds for SLAM delayed init (0 feats)
[TIME]: 0.0029 seconds for marginalization (15 clones in state)
[TIME]: 34.2481 seconds for total
q_GtoI = 0.002,0.002,0.677,0.736 | p_IinG = 2.164,-1.111,-0.190 | dist = 3.39 (meters)
bg = 0.0014,0.0015,0.0095 | ba = 0.1782,-0.0449,0.0023
camera-imu timeoffset = -0.01607
cam0 intrinsics = 550.765,573.616,715.611,548.810 | 0.410,0.076,-0.274,0.292
cam1 intrinsics = 548.473,552.193,723.017,546.064 | 0.421,0.081,-0.297,0.334
cam0 extrinsics = -0.001,-0.712,0.702,0.005 | -0.008,-0.065,-0.219
cam1 extrinsics = -0.716,0.003,-0.002,0.698 | -0.036,-0.098,-0.749
[ZUPT]: accepted zero velocity |v_IinG| = 0.023 (chi2 22.560 < 437.047)
[ZUPT]: accepted zero velocity |v_IinG| = 0.021 (chi2 1.863 < 126.574)
terminate called after throwing an instance of 'std::bad_alloc'
what(): std::bad_alloc
```