search

MIT-SPARK / Kimera-VIO

Visual Inertial Odometry with SLAM capabilities and 3D Mesh generation.
BSD 2-Clause "Simplified" License
1.53k stars 416 forks source link

Running Kimera VIO on custom data #177

Closed vguzov closed 1 year ago

vguzov commented 2 years ago

Description: Hello, I'm have a body tracking system consisting of a head-mounted camera and a set of IMU sensor placed on the head (which is a part of Xsens Awinda system). I want to use Kimera-VIO to track the position of the camera. I converted the data to the Euroc format, but unfortunately Kimera gives me an error when I try to run it. The original Euroc data works fine. Could you please tell me what could potentially cause a crash here?

Thank you very much for your great work!

Best, Vladimir

Command: (script is included in the attached files)

bash ./scripts/mpi_kimera_launch_script.sh

Console output:


Using dataset at path: /home/user/localization/data/mpi_kimera
 Launching:

            ██╗  ██╗██╗███╗   ███╗███████╗██████╗  █████╗
            ██║ ██╔╝██║████╗ ████║██╔════╝██╔══██╗██╔══██╗
            █████╔╝ ██║██╔████╔██║█████╗  ██████╔╝███████║
            ██╔═██╗ ██║██║╚██╔╝██║██╔══╝  ██╔══██╗██╔══██║
            ██║  ██╗██║██║ ╚═╝ ██║███████╗██║  ██║██║  ██║
            ╚═╝  ╚═╝╚═╝╚═╝     ╚═╝╚══════╝╚═╝  ╚═╝╚═╝  ╚═╝

I1005 12:09:23.808378  9971 CameraParams.cpp:29] Parsing camera parameters for: left_cam
I1005 12:09:23.808487  9971 CameraParams.cpp:29] Parsing camera parameters for: right_cam
I1005 12:09:23.808732  9971 Logger.cpp:59] Opening output file: traj_gt.csv
I1005 12:09:23.808785  9971 EurocDataProvider.cpp:89] Parsing Euroc dataset...
I1005 12:09:23.817466  9971 EurocDataProvider.cpp:289] IMU rate: 0.0333333
IMU rate std: 0.033335
IMU rate max mismatch: 0.0333333
Maximum measured rotation rate (norm):9.10357
Maximum measured acceleration (norm): 56.0218
I1005 12:09:23.819177  9971 DataProviderInterface-definitions.cpp:104] ------------ CameraImageLists::print -------------
image_folder_path: /home/user/localization/data/mpi_kimera/mav0/cam0
img_lists size: 10883
I1005 12:09:23.820847  9971 DataProviderInterface-definitions.cpp:104] ------------ CameraImageLists::print -------------
image_folder_path: /home/user/localization/data/mpi_kimera/mav0/cam1
img_lists size: 10883
I1005 12:09:23.821765  9971 EurocDataProvider.cpp:428] Maximum ground truth velocity: 0.019551
W1005 12:09:23.821782  9971 EurocDataProvider.cpp:741] Value for final_k, 100000 is larger than total number of frames in dataset 10883
W1005 12:09:23.821791  9971 EurocDataProvider.cpp:744] Using final_k = 10883
I1005 12:09:23.822145  9971 EurocDataProvider.cpp:750] ------------------ ETHDatasetParser::print ------------------
Displaying info for dataset: /home/user/localization/data/mpi_kimera
I1005 12:09:23.822154  9971 EurocDataProvider.cpp:755] 
Left camera name: cam0, with params:
I1005 12:09:23.822173  9971 CameraParams.cpp:209]                                                            
******************** Camera Parameters ********************
Name                                                  Value
-----------------------------------------------------------
Camera ID ..........................................left_cam
Intrinsics: 
- fx......................................871.8
- fy..................................................885.58
- cu.................................................961.544
- cv.................................................550.686
frame_rate_: ......................................0.0333333
image_size_: 
 - width..................................1920
- height................................................1080
-----------------------------------------------------------
I1005 12:09:23.822177  9971 CameraParams.cpp:210] - body_Pose_cam_: R: [
    -0.422087, 0.152125, 0.8937;
    -0.890669, 0.114158, -0.440087;
    -0.168971, -0.981746, 0.0873085
]
t: -0.334006 0.0187823 0002.1773
- K: [871.8002191615716, 0, 961.5442456700675;
 0, 885.5799798967853, 550.6864687995145;
 0, 0, 1]
- Distortion Model:1
- Distortion Coeff:[-0.25462584991295, 0.08039095012756001, 0.0001458329036, -1.397345667e-05]
I1005 12:09:23.822240  9971 DataProviderInterface-definitions.cpp:104] ------------ CameraImageLists::print -------------
image_folder_path: /home/user/localization/data/mpi_kimera/mav0/cam0
img_lists size: 10883
I1005 12:09:23.822243  9971 EurocDataProvider.cpp:755] 
Right camera name: cam1, with params:
I1005 12:09:23.822250  9971 CameraParams.cpp:209]                                                            
******************** Camera Parameters ********************
Name                                                  Value
-----------------------------------------------------------
Camera ID .........................................right_cam
Intrinsics: 
- fx......................................871.8
- fy..................................................885.58
- cu.................................................961.544
- cv.................................................550.686
frame_rate_: ......................................0.0333333
image_size_: 
 - width..................................1920
- height................................................1080
-----------------------------------------------------------
I1005 12:09:23.822258  9971 CameraParams.cpp:210] - body_Pose_cam_: R: [
    -0.515325, -0.0135281, 0.845141;
    -0.840325, 0.114886, -0.510549;
    -0.0911035, -0.983171, -0.0712879
]
t: -0.01 00000 00000
- K: [871.8002191615716, 0, 961.5442456700675;
 0, 885.5799798967853, 550.6864687995145;
 0, 0, 1]
- Distortion Model:1
- Distortion Coeff:[-0.25462584991295, 0.08039095012756001, 0.0001458329036, -1.397345667e-05]
I1005 12:09:23.822274  9971 DataProviderInterface-definitions.cpp:104] ------------ CameraImageLists::print -------------
image_folder_path: /home/user/localization/data/mpi_kimera/mav0/cam1
img_lists size: 10883
I1005 12:09:23.822278  9971 DataProviderInterface-definitions.cpp:110] ------------ GroundTruthData::print -------------
body_Pose_cam_: 
 R: [
    1, 0, 0;
    0, 1, 0;
    0, 0, 1
]
t: 0 0 0
I1005 12:09:23.822297  9971 DataProviderInterface-definitions.cpp:112] 
 gt_rate: 0.00500006
nr of gtStates: 3
I1005 12:09:23.822300  9971 EurocDataProvider.cpp:764] -------------------------------------------------------------
E1005 12:09:23.825014  9971 ImuFrontend.cpp:71] IMU PREINTEGRATION PARAMS GIVEN TO IMU Frontend.
I1005 12:09:23.825155  9971 ImuFrontendParams.cpp:90]                                                            
************************ IMU params ************************
Name                                                  Value
-----------------------------------------------------------
gyroscope_noise_density: .........................0.00016968
gyroscope_random_walk: ................................5e-06
accelerometer_noise_density: ..........................0.002
accelerometer_random_walk: ............................0.003
imu_integration_sigma: ................................1e-08
imu_time_shift: ...........................................0
n_gravity: ....................................................................0
...................0
...............-9.81
-----------------------------------------------------------
E1005 12:09:23.825183  9971 ImuFrontend.cpp:73] IMU BIAS GIVEN TO IMU Frontend AT CONSTRUCTION:
 acc = 0
0
0 gyro = 0
0
0
E1005 12:09:23.825268  9971 ImuFrontend.cpp:75] IMU PREINTEGRATION COVARIANCE: 
MONOEUROC
MONOMONOMONO2222222
PIM type: 1
    deltaTij = 0
    deltaRij.ypr = ( 0 -0  0)
    deltaPij = 0 0 0
    deltaVij = 0 0 0
    gyrobias = 0 0 0
    acc_bias = 0 0 0

    preintMeasCov 
[0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0]
I1005 12:09:23.825691  9971 Logger.cpp:59] Opening output file: output_frontend_stats.csv
I1005 12:09:23.825748  9971 Logger.cpp:59] Opening output file: output_frontend_ransac_mono.csv
I1005 12:09:23.825810  9971 Logger.cpp:59] Opening output file: output_frontend_ransac_stereo.csv
I1005 12:09:23.825884  9971 VisionImuFrontendParams.cpp:76]                                                            
******************* Frontend Parameters *******************
Name                                                  Value
-----------------------------------------------------------
klt_win_size_: ...........................................24
klt_max_iter_: ...........................................30
klt_max_level_: ...........................................4
klt_eps_: ...............................................0.1
maxFeatureAge_: ..........................................25
Optical Flow Predictor Type................................1
useRANSAC_: ...............................................1
minNrMonoInliers_: .......................................10
minNrStereoInliers_: ......................................5
ransac_threshold_mono_: ...............................1e-06
ransac_threshold_stereo_: .................................1
ransac_use_1point_stereo_: ................................1
ransac_use_2point_mono_: ..................................1
ransac_max_iterations_: .................................100
ransac_probability_: ..................................0.995
ransac_randomize_: ........................................0
intra_keyframe_time_: .................................2e+08
minNumberFeatures_: .......................................0
useStereoTracking_: .......................................0
disparityThreshold_: ....................................0.5
-----------------------------------------------------------
I1005 12:09:23.825908  9971 FeatureDetectorParams.cpp:92]                                                            
**************** FeatureDetector Parameters ****************
Name                                                  Value
-----------------------------------------------------------
Feature Detector Type: ....................................3
Max features per frame...................................300
Enable Subpixel corner refinement..........................1
Enable Non-maximum suppression.............................1
Non-maximum suppression type...............................4
Min dist btw tracked/detected features....................20
quality_level_: .......................................0.001
block_size_: ..............................................3
use_harris_corner_detector.................................0
k_: ....................................................0.04
Fast Threshold............................................10
-----------------------------------------------------------
I1005 12:09:23.825917  9971 FeatureDetectorParams.cpp:39]                                                            
************* SubPixelCornerFinder Parameters *************
Name                                                  Value
-----------------------------------------------------------
Termination criteria type..................................3
Termination criteria maximum iters........................40
Termination criteria epsilon...........................0.001
Window size................................................[10 x 10]
Zero zone..................................................[-1 x -1]
-----------------------------------------------------------
I1005 12:09:23.833042  9971 Logger.cpp:59] Opening output file: traj_vio.csv
I1005 12:09:23.833127  9971 Logger.cpp:59] Opening output file: output_smartFactors.csv
I1005 12:09:23.833163  9971 Logger.cpp:59] Opening output file: output_pim_navstates.csv
I1005 12:09:23.833194  9971 Logger.cpp:59] Opening output file: output_backendFactors.csv
I1005 12:09:23.833227  9971 Logger.cpp:59] Opening output file: output_backendTiming.csv
I1005 12:09:23.833331  9971 VioBackendParams.cpp:250]                                                            
******************** Backend Parameters ********************
Name                                                  Value
-----------------------------------------------------------
..............................** Initialization parameters **....................
Automatic Initialization...................................0
Round on autoinit..........................................0
Sigmas:.....................................................
Initial Position Sigma.................................1e-05
Initiail Roll Pitch Sigma...........................0.174533
Initial Yaw Sigma.................................0.00174533
Initial Velocity Sigma.................................0.001
Initial Acc Bias Sigma...................................0.1
Initial Gyro Bias Sigma.................................0.01
..............................** Vision parameters **....................
Linearization Mode: hessian, implicit_schur, jacobian_q, jacobian_svd...................0
Degeneracy Mode: ignore_degeneracy, zero_on_degeneracy, handle_infinity...................1
Rank Tolerance.............................................1
Landmark Distance Threshold...............................10
Outlier Rejection..........................................3
Retriangulation Threshold..............................0.001
Add Btw Stereo Factors.....................................0
Btw Rotation Precision.....................................0
Btw Translation Precision................................100
..............................** Optimization parameters **....................
Relinearize Threshold...................................0.01
Relinearize Skip...........................................1
Zero Velocity Sigma....................................0.001
No Motion Position Sigma...............................0.001
No Motion Rotation Sigma..............................0.0001
Constant Velocity Sigma.................................0.01
Optimization Iterations....................................1
Horizon....................................................6
Isam Wildfire Threshold................................0.001
Use Dog Leg................................................0
-----------------------------------------------------------
I1005 12:09:23.833360  9971 VioBackendParams.cpp:251] ** Backend Iinitialization Parameters **
initial_ground_truth_state_: 
I1005 12:09:23.833381  9971 VioNavState.cpp:38] ---  --- 

 pose: 
 R: [
    0.380012, 0.164691, 0.910202;
    0.0306792, -0.985725, 0.165548;
    0.924473, -0.0349858, -0.37964
]
t: 0.878612  2.14247 0.947262
I1005 12:09:23.833436  9971 VioNavState.cpp:40] 
 velocity: 
00.009474 -0.014009 -0.002145

 imuBias: 
acc = -0.012492
 0.547666
 0.069073 gyro = -0.002229
   0.0207
  0.07635
..........** ISAM2 Parameters **..........
type:              ISAM2GaussNewtonParams
wildfireThreshold: 0.001
relinearizeThreshold:              0.01
relinearizeSkip:                   1
enableRelinearization:             1
evaluateNonlinearError:            0
factorization:                     CHOLESKY
cacheLinearizedFactors:            1
enableDetailedResults:             0
enablePartialRelinearizationCheck: 0
findUnusedFactorSlots:             1
I1005 12:09:23.833539  9971 VioBackend.cpp:1584] Used stereo calibration in Backend: 

 stereoCal_
 K: 1025.55       0 956.939
      0 1025.55 380.154
      0       0       1
Baseline: 2.17924
I1005 12:09:23.833616  9971 VioBackend.cpp:1589] ** Backend Initial Members: 
B_Pose_leftCam_: R: [
    -0.422087, 0.152125, 0.8937;
    -0.890669, 0.114158, -0.440087;
    -0.168971, -0.981746, 0.0873085
]
t: -0.334006 0.0187823 0002.1773
W_Pose_B_lkf_: R: [
    1, 0, 0;
    0, 1, 0;
    0, 0, 1
]
t: 0 0 0
W_Vel_B_lkf_ (transpose): 0 0 0
imu_bias_lkf_acc = 0
0
0 gyro = 0
0
0
imu_bias_prev_kf_acc = 0
0
0 gyro = 0
0
0
last_id_ -1
cur_id_ 0
landmark_count_ 0
I1005 12:09:23.833675  9971 RegularVioBackend.cpp:114] Using Regular VIO Backend.
I1005 12:09:23.833696  9971 RegularVioBackend.cpp:1677] Using Tukey norm, with parameter value: 4.6851
I1005 12:09:23.833720  9971 RegularVioBackend.cpp:1677] Using Tukey norm, with parameter value: 4.6851
I1005 12:09:23.833741  9971 RegularVioBackend.cpp:1666] Using Huber norm, with parameter value: 0.04
I1005 12:09:23.835232  9971 PipelineModule.h:449] MISO Pipeline Module: Display has no output queue registered.
I1005 12:09:23.835369  9971 Pipeline.cpp:296] Pipeline Modules launched (parallel_run set to 1).
I1005 12:09:23.837239  9983 Pipeline.cpp:89] Spinning Kimera-VIO.
I1005 12:09:23.837256  9984 Pipeline.cpp:146] Shutting down VIO pipeline once processing has finished.
I1005 12:09:23.837275  9984 Pipeline.cpp:159] Statistics
-----------                                 #   Log Hz  {avg     +- std    }    [min,max]
Display [ms]                                0   
Mono Data Provider [ms]                     0   
VioBackend [ms]                             0   
VioFrontend [ms]                            0   
Visualizer [ms]                             0   
backend_input_queue Size [#]                0   
data_provider_left_frame_queue Size [#]     0   
display_input_queue Size [#]                0   
frontend_input_queue Size [#]               0   
visualizer_backend_queue Size [#]           0   
visualizer_frontend_queue Size [#]          0   
I1005 12:09:23.839005  9982 EurocDataProvider.cpp:803] Running dataset between frame 1 and frame 10883
I1005 12:09:23.839025  9982 EurocDataProvider.cpp:832] Sending left frame k= 1 with timestamp: 33333333
I1005 12:09:23.843333  9982 EurocDataProvider.cpp:858] Finished VIO processing for frame k = 1
I1005 12:09:23.843343  9982 EurocDataProvider.cpp:832] Sending left frame k= 2 with timestamp: 66666667
I1005 12:09:23.843415  9983 DataProviderModule.cpp:47] Skipping first frame, because we do not have a concept of a previous frame timestamp otherwise.
I1005 12:09:23.847003  9982 EurocDataProvider.cpp:858] Finished VIO processing for frame k = 2
I1005 12:09:23.847013  9982 EurocDataProvider.cpp:832] Sending left frame k= 3 with timestamp: 100000000
I1005 12:09:23.847092  9983 DataProviderModule.cpp:104] ////////////////////////////////////////// Creating packet!
STAMPS IMU rows : 
1
STAMPS IMU cols : 
3
STAMPS IMU: 
33333333 66666666 66666667
ACCGYR IMU rows : 
6
ACCGYR IMU cols : 
3
ACCGYR IMU: 
001.84071 001.48795 001.48795
-0.564322 -0.295651 -0.295651
0-10.3051 0-10.9404 0-10.9404
-0.165422 00.142093 00.142093
-0.120027 00.135662 00.135662
00.169713 0-0.10461 0-0.10461
I1005 12:09:23.847206  9979 MonoVisionImuFrontend.cpp:186] Processing first mono frame 
I1005 12:09:23.850306  9982 EurocDataProvider.cpp:858] Finished VIO processing for frame k = 3
I1005 12:09:23.850317  9982 EurocDataProvider.cpp:832] Sending left frame k= 4 with timestamp: 133333333
I1005 12:09:23.850401  9983 DataProviderModule.cpp:104] ////////////////////////////////////////// Creating packet!
STAMPS IMU rows : 
1
STAMPS IMU cols : 
2
STAMPS IMU: 
099999999 100000000
ACCGYR IMU rows : 
6
ACCGYR IMU cols : 
2
ACCGYR IMU: 
001.55601 001.55601
-0.373854 -0.373854
0-9.36083 0-9.36083
-0.074392 -0.074392
00.291428 00.291428
00.122797 00.122797
I1005 12:09:23.854002  9982 EurocDataProvider.cpp:858] Finished VIO processing for frame k = 4
I1005 12:09:23.854012  9982 EurocDataProvider.cpp:832] Sending left frame k= 5 with timestamp: 166666667
I1005 12:09:23.854106  9983 DataProviderModule.cpp:104] ////////////////////////////////////////// Creating packet!
STAMPS IMU rows : 
1
STAMPS IMU cols : 
2
STAMPS IMU: 
133333332 133333333
ACCGYR IMU rows : 
6
ACCGYR IMU cols : 
2
ACCGYR IMU: 
0001.71988 0001.71988
00-1.12168 00-1.12168
00-9.74741 00-9.74741
00.0701897 00.0701896
000.271615 000.271615
-0.0502331 -0.0502331
I1005 12:09:23.854138  9983 ThreadsafeQueue.h:308] Queue with id: frontend_input_queue is getting full, size: 2
I1005 12:09:23.857734  9982 EurocDataProvider.cpp:858] Finished VIO processing for frame k = 5
I1005 12:09:23.857746  9982 EurocDataProvider.cpp:832] Sending left frame k= 6 with timestamp: 200000000
I1005 12:09:23.857831  9983 DataProviderModule.cpp:104] ////////////////////////////////////////// Creating packet!
STAMPS IMU rows : 
1
STAMPS IMU cols : 
2
STAMPS IMU: 
166666665 166666667
ACCGYR IMU rows : 
6
ACCGYR IMU cols : 
2
ACCGYR IMU: 
0001.47492 0001.47492
0-0.456431 0-0.456431
00-9.68793 00-9.68793
0-0.019174 0-0.019174
000.340769 000.340769
-0.0524348 -0.0524348
I1005 12:09:23.857862  9983 ThreadsafeQueue.h:308] Queue with id: frontend_input_queue is getting full, size: 3
I1005 12:09:23.861543  9982 EurocDataProvider.cpp:858] Finished VIO processing for frame k = 6
I1005 12:09:23.861554  9982 EurocDataProvider.cpp:832] Sending left frame k= 7 with timestamp: 233333333
I1005 12:09:23.861641  9983 DataProviderModule.cpp:104] ////////////////////////////////////////// Creating packet!
STAMPS IMU rows : 
1
STAMPS IMU cols : 
2
STAMPS IMU: 
199999998 200000000
ACCGYR IMU rows : 
6
ACCGYR IMU cols : 
2
ACCGYR IMU: 
0002.21834 0002.21834
0-0.588088 0-0.588088
00-8.89667 00-8.89667
-0.0121265 -0.0121265
000.157021 000.157021
000.048875 000.048875
I1005 12:09:23.861683  9983 ThreadsafeQueue.h:308] Queue with id: frontend_input_queue is getting full, size: 4
I1005 12:09:23.865381  9982 EurocDataProvider.cpp:858] Finished VIO processing for frame k = 7
I1005 12:09:23.865394  9982 EurocDataProvider.cpp:832] Sending left frame k= 8 with timestamp: 266666667
I1005 12:09:23.865481  9983 DataProviderModule.cpp:104] ////////////////////////////////////////// Creating packet!
STAMPS IMU rows : 
1
STAMPS IMU cols : 
2
STAMPS IMU: 
233333331 233333333
ACCGYR IMU rows : 
6
ACCGYR IMU cols : 
2
ACCGYR IMU: 
001.69772 001.69772
-0.522906 -0.522906
0-9.96023 0-9.96023
0.0224787 0.0224787
00.209103 00.209103
00.040384 00.040384
I1005 12:09:23.865512  9983 ThreadsafeQueue.h:308] Queue with id: frontend_input_queue is getting full, size: 5
I1005 12:09:23.869285  9982 EurocDataProvider.cpp:858] Finished VIO processing for frame k = 8
I1005 12:09:23.869297  9982 EurocDataProvider.cpp:832] Sending left frame k= 9 with timestamp: 300000000
I1005 12:09:23.869381  9983 DataProviderModule.cpp:104] ////////////////////////////////////////// Creating packet!
STAMPS IMU rows : 
1
STAMPS IMU cols : 
2
STAMPS IMU: 
266666664 266666667
ACCGYR IMU rows : 
6
ACCGYR IMU cols : 
2
ACCGYR IMU: 
001.98266 001.98266
-0.749429 -0.749429
0-9.12915 0-9.12915
0.0178053 0.0178053
00.197365 00.197365
0.0266787 0.0266787
I1005 12:09:23.873102  9982 EurocDataProvider.cpp:858] Finished VIO processing for frame k = 9
I1005 12:09:23.873116  9982 EurocDataProvider.cpp:832] Sending left frame k= 10 with timestamp: 333333333
I1005 12:09:23.877198  9982 ThreadsafeQueue.h:286] Queue with id: data_provider_left_frame_queue is getting full, size: 2
I1005 12:09:23.877221  9982 EurocDataProvider.cpp:858] Finished VIO processing for frame k = 10
I1005 12:09:23.877228  9982 EurocDataProvider.cpp:832] Sending left frame k= 11 with timestamp: 366666667
I1005 12:09:23.881057  9982 ThreadsafeQueue.h:286] Queue with id: data_provider_left_frame_queue is getting full, size: 3
I1005 12:09:23.881074  9982 EurocDataProvider.cpp:858] Finished VIO processing for frame k = 11
I1005 12:09:23.881080  9982 EurocDataProvider.cpp:832] Sending left frame k= 12 with timestamp: 400000000
I1005 12:09:23.881243  9979 FeatureDetector.cpp:198] Number of points detected : 300
I1005 12:09:23.881259  9979 FeatureDetector.cpp:210] Need n corners: 300
I1005 12:09:23.881265  9979 NonMaximumSuppression.cpp:47] Sorting keypoints in decreasing order of strength.
I1005 12:09:23.881297  9979 NonMaximumSuppression.cpp:61] Starting Adaptive Non-Maximum Suppression.
I1005 12:09:23.881302  9979 NonMaximumSuppression.cpp:87] Running RangeTree: 4
I1005 12:09:23.881573  9979 NonMaximumSuppression.cpp:105] Non Maximum Suppression Timing [ms]: 0
I1005 12:09:23.884810  9982 ThreadsafeQueue.h:286] Queue with id: data_provider_left_frame_queue is getting full, size: 4
I1005 12:09:23.884821  9982 EurocDataProvider.cpp:858] Finished VIO processing for frame k = 12
I1005 12:09:23.884825  9982 EurocDataProvider.cpp:832] Sending left frame k= 13 with timestamp: 433333333
I1005 12:09:23.888532  9982 ThreadsafeQueue.h:286] Queue with id: data_provider_left_frame_queue is getting full, size: 5
I1005 12:09:23.888542  9982 EurocDataProvider.cpp:858] Finished VIO processing for frame k = 13
I1005 12:09:23.888547  9982 EurocDataProvider.cpp:832] Sending left frame k= 14 with timestamp: 466666667
I1005 12:09:23.889472  9979 FeatureDetector.cpp:250] Corner Sub Pixel Refinement Timing [ms]: 7
I1005 12:09:23.889667  9979 FeatureDetector.cpp:147] featureExtraction: frame 2,  Nr tracked keypoints: 0,  Nr extracted keypoints: 300,  total: 300  (max: 300)
I1005 12:09:23.889709  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[2006.48, 29.7065]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.889724  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[2053.2, 718.043]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.889740  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-22.76, 930.405]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.889748  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-57.7193, 456.355]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.889757  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-128.721, 924.532]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.889765  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[2056.37, 402.439]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.889775  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-181.463, 901.575]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.889782  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-201.575, 906.537]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.889791  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-82.5848, -48.2374]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.889798  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[1933.01, 853.036]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.889807  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-83.1941, 770.332]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.889817  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-112.705, 571.325]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.889827  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-162.581, 365.209]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.889834  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-224.065, 333.129]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.889842  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-65.4438, 826.301]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.889851  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-183.815, -103.087]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.889859  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[2048.7, 764.592]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.889868  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-224.842, 292.756]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.889876  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-197.145, 968.653]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.889884  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-148.474, 825.086]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.889892  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-123.857, 469.269]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.889901  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-162.452, 963.147]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.889909  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-217.614, 772.567]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.889917  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-111.683, 952.494]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.889927  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-121.421, 890.291]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.889935  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-186.685, 426.878]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.889943  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-117.491, 527.264]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.889951  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-119.179, 494.696]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.889959  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-184.57, 467.096]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.889967  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-155.248, 761.093]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.889974  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-196.386, 835.647]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.889982  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-21.8221, -109.869]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.889991  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[1965.27, 883.898]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.889998  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-210.516, 941.799]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.890007  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-42.3056, 740.708]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.890014  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[2055.7, 661.551]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.890022  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-196.754, 797.385]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.890030  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-151.465, 788.202]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.890038  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-76.3215, 566.304]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.890046  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-50.0091, 346.88]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.890054  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-59.5471, 412.938]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.890062  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-95.3633, -61.4112]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.890069  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-140.15, 365.669]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.890077  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-140.703, 181.341]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.890086  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[1966.76, 857.683]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.890094  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[384.257, 1136.76]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.890101  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[2117.85, 951.64]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.890111  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-0.924862, 1081.66]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.890120  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-80.5057, 526.42]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.890127  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[1545.91, 1100.1]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.890136  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-198.503, 531.92]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.890144  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[1985.78, 6.64815]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.890151  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[297.366, -18.1144]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.890159  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[1.84836, -32.1091]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.890167  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-176.069, 579.394]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.890177  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-185.256, 248.247]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.890184  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-125.492, 425.827]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.890192  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[2014.23, 70.6816]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.890200  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[1763.4, -10.7305]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.890209  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-188.175, 301.018]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.890218  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[1969.67, 826.616]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.890225  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[9.56459, -96.8391]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.890233  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-164.409, 661.317]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.890242  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-43.0434, 583.484]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.890249  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-68.2869, 744.186]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.890256  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[294.887, -56.54]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.890264  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[2141.69, 583.506]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.890272  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[2007.89, 1026.44]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.890280  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-57.6533, -66.0145]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.890290  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-222.983, 385.185]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.890298  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[2121.63, 821.923]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.890307  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-2.2369, 817.826]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.890314  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[2147.61, 708.423]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.890323  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[2058.32, 627.885]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.890332  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-27.8598, 681.105]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.890341  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[2031.3, 910.188]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.890349  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-38.7491, 620.62]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.890357  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-173.118, 332.236]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.890364  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-172.933, 533.096]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.890372  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-57.7572, 880.63]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.890379  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[1710.86, -3.90995]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.890388  9979 UndistorterRectifier.cpp:186] undistortRectifyPoints: unable to match 81 keypoints of 300 total.
E1005 12:09:23.890398  9979 ImuFrontend.h:143] Reset preintegration with new bias:
=====MONOMONOMONO======
PARALLEL
acc = 0
0
0 gyro = 0
0
0
I1005 12:09:23.890450  9979 MonoVisionImuFrontend.cpp:91] ------------------- Processing frame k = 3--------------------
I1005 12:09:23.890461  9980 VioBackend-definitions.h:261] VioBackend Input Payload print:
Timestamp: 66666667
I1005 12:09:23.890457  9979 MonoImuSyncPacket.cpp:35] Mono Frame timestamp: 100000000
STAMPS IMU rows : 
1
STAMPS IMU cols : 
2
STAMPS IMU: 
099999999 100000000
ACCGYR IMU rows : 
6
ACCGYR IMU cols : 
2
ACCGYR IMU: 
001.55601 001.55601
-0.373854 -0.373854
0-9.36083 0-9.36083
-0.074392 -0.074392
00.291428 00.291428
00.122797 00.122797
I1005 12:09:23.890492  9980 VioBackend.cpp:236] Initial state seed: 
 - Initial timestamp: 66666667
 - Initial pose: R: [
    0.380012, 0.164691, 0.910202;
    0.0306792, -0.985725, 0.165548;
    0.924473, -0.0349858, -0.37964
]
t: 0.878612 02.14247 0.947262
 - Initial vel: 00.009474 -0.014009 -0.002145
 - Initial IMU bias: acc = -0.012492
00.547666
00.069073 gyro = -0.002229
0000.0207
000.07635
I1005 12:09:23.890523  9979 ImuFrontend.cpp:108] Finished preintegration: 
PIM type: 1
    deltaTij = 1e-09
I1005 12:09:23.890545  9983 ThreadsafeQueue.h:308] Queue with id: frontend_input_queue is getting full, size: 5
    deltaRij.ypr = (1.22797e-10 2.91428e-10 -7.4392e-11)
    deltaPij =  7.78005e-19 -1.86927e-19 -4.68041e-18
    deltaVij =  1.55601e-09 -3.73854e-10 -9.36083e-09
I1005 12:09:23.890568  9983 DataProviderModule.cpp:104] ////////////////////////////////////////// Creating packet!
STAMPS IMU rows : 
1
STAMPS IMU cols : 
2
STAMPS IMU: 
299999997 300000000
ACCGYR IMU rows : 
6
ACCGYR IMU cols : 
2
ACCGYR IMU: 
0002.17286 0002.17286
0-0.871902 0-0.871902
00-9.38828 00-9.38828
00.0289793 00.0289793
000.122968 000.122968
-0.0884932 -0.0884932
    gyrobias = 0 0 0
    acc_bias = 0 0 0

I1005 12:09:23.890606  9980 VioBackend.cpp:1143] Imu bias for Backend prior:
    preintMeasCov 
[acc = -0.012492
 0.547666
 0.069073 gyro = -0.002229
   0.0207
  0.07635
I1005 12:09:23.890635  9980 VioBackend.cpp:1152] Added initial priors for frame 0
I1005 12:09:23.890655  9980 VioBackend.cpp:254] Start optimize with initial state and priors!
I1005 12:09:23.890677  9980 VioBackend.cpp:1613]  =============== START:Smoother status before update: =============== 
I1005 12:09:23.890683  9980 VioBackend.cpp:1640] Nr of factors in graph (smoother getFactors): 0, with factors:
I1005 12:09:23.890694  9980 VioBackend.cpp:1642] [
 2.87913e-17  1.56048e-37  6.57526e-38            0            0            0            0            0            0
 1.56049e-37  2.87913e-17 -2.57586e-37            0            0            0            0            0            0
 6.57525e-38 -2.57585e-37  2.87913e-17            0            0            0            0            0            0
           0            0            0        1e-25    2.168e-53  9.13512e-54        2e-24  4.33599e-44  1.82701e-44
           0            0            0  2.16799e-53        1e-25 -3.57866e-53  4.33599e-44        2e-24 -7.15731e-44
           0            0            0  9.13519e-54 -3.57866e-53        1e-25  1.82703e-44 -7.15731e-44        2e-24

I1005 12:09:23.890703  9980 VioBackend.cpp:1649]  ]
I1005 12:09:23.890708  9980 VioBackend.cpp:1652] Nr of new factors to add: 3 with factors:
I1005 12:09:23.890712  9980 VioBackend.cpp:1654] [
 (slot # wrt to new_factors_tmp graph)  
           0            0            0        2e-24  4.33599e-44  1.82703e-44        4e-15  8.67199e-35  3.65406e-35
           0            0            0  4.33599e-44        2e-24 -7.15731e-44  8.67198e-35        4e-15 -1.43146e-34
           0            0            0  1.82702e-44 -7.15731e-44         2e-24      Slot # 1: 3.65406e-35 Point Prior: point key    -1.43146e-34 Factor  4e-15]
body_R_cam [
    -0.422087, 0.152125, 0.8937;
    -0.890669, 0.114158, -0.440087;
    -0.168971, -0.981746, 0.0873085
]
camLrectLkf_R_camLrectK_imu [
    1, -1.05884e-07, 5.66842e-08;
    -1.06252e-07, 1, -1.82254e-07;
    5.68814e-08, -1.82752e-07, 1
]
I1005 12:09:23.890755  9979 MonoVisionImuFrontend.cpp:112] Starting processFrame...
v0

I1005 12:09:23.890763  9979 MonoVisionImuFrontend.cpp:215] ===================================================
processing frame: 3 at time 100000000 empirical framerate (sec): 0.0333333 (timestamp diff: 33333333)
I1005 12:09:23.890769  9980 VioBackend.cpp:1661]  ]
I1005 12:09:23.890771  9979 MonoVisionImuFrontend.cpp:226] Starting feature tracking...
I1005 12:09:23.890779  9980 VioBackend.cpp:1664] Nr deleted slots: 0, with slots:
I1005 12:09:23.890794  9979 Tracker.cpp:113] Starting Optical Flow Pyr LK tracking...
I1005 12:09:23.890797  9980 VioBackend.cpp:1666] [

I1005 12:09:23.890816  9980 VioBackend.cpp:1693]  ]
I1005 12:09:23.890821  9980 VioBackend.cpp:1696] Nr of values in state_ : 0, with keys:
[

I1005 12:09:23.890830  9980 VioBackend.cpp:1702]  ]
I1005 12:09:23.890833  9980 VioBackend.cpp:1705] Nr values in new_values_ : 3, with keys:
[
     b0  v0  x0 
I1005 12:09:23.890848  9980 VioBackend.cpp:1713]  ]
isam2 graph:
size: 0

new_factors_tmp:
size: 3

Factor 0: PriorFactor on x0
  prior mean:  R: [
    0.380012, 0.164691, 0.910202;
    0.0306792, -0.985725, 0.165548;
    0.924473, -0.0349858, -0.37964
]
t: 0.878612  2.14247 0.947262
  noise model: Gaussian[
    529.688, -20.0431, -217.493, 0, 0, 0;
    0, 5.73367, 0.0889689, 0, 0, 0;
    0, 0, 6.19318, 0, 0, 0;
    0, 0, 0, 100000, 0, 0;
    0, 0, 0, 0, 100000, 0;
    0, 0, 0, 0, 0, 100000
]

Factor 1: PriorFactor on v0
  prior mean: [
    0.009474;
    -0.014009;
    -0.002145
]
isotropic dim=3 sigma=0.001

Factor 2: PriorFactor on b0
  prior mean: acc = -0.012492
 0.547666
 0.069073 gyro = -0.002229
   0.0207
  0.07635
  noise model: diagonal sigmas[0.1; 0.1; 0.1; 0.01; 0.01; 0.01];

new values:

Values with 3 values:
Value b0: (gtsam::imuBias::ConstantBias)
acc = -0.012492
 0.547666
 0.069073 gyro = -0.002229
   0.0207
  0.07635

Value v0: (Eigen::Matrix<double, 3, 1, 0, 3, 1>)
[
    0.009474;
    -0.014009;
    -0.002145
]

Value x0: (gtsam::Pose3)
R: [
    0.380011666, 0.164691321, 0.910202412;
    0.0306791947, -0.985725343, 0.165547653;
    0.924473065, -0.0349858183, -0.379640249
]
t: 0.878612  2.14247 0.947262
I1005 12:09:23.891018  9980 VioBackend.cpp:1724]  =============== END: Smoother status before update: =============== 
I1005 12:09:23.891026  9980 VioBackend.cpp:1010] iSAM2 update with 3 new factors , 3 new values , and 0 deleted factors.
I1005 12:09:23.891032  9980 VioBackend.cpp:1014] Starting first update.
I1005 12:09:23.891062  9980 VioBackend.cpp:1211] Starting update of smoother_...
I1005 12:09:23.891973  9980 VioBackend.cpp:1214] Finished update of smoother_.
I1005 12:09:23.892002  9980 VioBackend.cpp:1017] Finished first update.
I1005 12:09:23.892007  9980 VioBackend.cpp:1030] Clearing new_smart_factors_!
I1005 12:09:23.892015  9980 VioBackend.cpp:1042] Starting to find smart factors slots.
I1005 12:09:23.892021  9980 VioBackend.cpp:1045] Finished to find smart factors slots.
I1005 12:09:23.892030  9980 VioBackend.cpp:1172] Starting to calculate estimate.
I1005 12:09:23.892073  9980 VioBackend.cpp:1174] Finished to calculate estimate.
I1005 12:09:23.892081  9980 VioBackend.cpp:1187] Backend: Update IMU Bias.
I1005 12:09:23.892086  9980 ImuFrontend.h:127] Updating Preintegration imu bias (needs to reset integration for the bias to have effect):

acc = -0.012492
 0.547666
 0.069073 gyro = -0.002229
   0.0207
  0.07635
I1005 12:09:23.892138  9980 VioBackend.cpp:1920] Hessian stats: ===========
rows: 15
nrElementsInMatrix_: 225
nrZeroElementsInMatrix_: 204
I1005 12:09:23.892149  9980 VioBackend.cpp:1938] Landmarks in old_smart_factors_: 0
I1005 12:09:23.892154  9980 VioBackend-definitions.h:215] -------- DebugVioInfo: --------
 numSF: 0
 numValid: 0
 numDegenerate: 0
 numOutliers: 0
 numFarPoints: 0
 numCheirality: 0
 numNonInitialized: 0
 meanPixelError: 0
 maxPixelError: 0
 meanTrackLength: 0
 maxTrackLength: 0
I1005 12:09:23.892163  9980 VioBackend-definitions.h:205] -------- VIO Backend Timing --------
Find delete time      :0
preUpdate time        :0
Update Time time      :0
Update slot time      :0
Extra iterations time :0
I1005 12:09:23.892172  9980 VioBackend.cpp:1962] Optimization Errors:
 - Error before :4.63043e-30
 - Error after  :4.63043e-30
I1005 12:09:23.892184  9980 VioBackend.cpp:158] Latest Backend IMU bias is: 
acc = -0.012492
 0.547666
 0.069073 gyro = -0.002229
   0.0207
  0.07635
I1005 12:09:23.892203  9980 VioBackend.cpp:160] Prev kf Backend IMU bias is: 
acc = -0.012492
 0.547666
 0.069073 gyro = -0.002229
   0.0207
  0.07635
I1005 12:09:23.892243  9980 VioBackend.cpp:632] Landmark typology to be used for the mesh:
Number of valid smart factors 0 out of 0
Number of landmarks (not involved in a smart factor) 0.
 Total number of landmarks: 0
I1005 12:09:23.892349  9981 QueueSynchronizer.h:126] Popping from: visualizer_frontend_queue
W1005 12:09:23.892367  9981 OpenCvVisualizer3D.cpp:1130] No landmark information for Visualizer. Not displaying 3D points.
I1005 12:09:23.892380  9981 OpenCvVisualizer3D.cpp:368] Starting trajectory visualization...
I1005 12:09:23.893447  9982 ThreadsafeQueue.h:286] Queue with id: data_provider_left_frame_queue is getting full, size: 5
I1005 12:09:23.893504  9982 EurocDataProvider.cpp:858] Finished VIO processing for frame k = 14
I1005 12:09:23.893517  9982 EurocDataProvider.cpp:832] Sending left frame k= 15 with timestamp: 500000000
I1005 12:09:23.894587  9979 Tracker.cpp:128] Optical Flow Timing [ms]: 3
I1005 12:09:23.894616  9979 Tracker.cpp:130] Finished Optical Flow Pyr LK tracking.
I1005 12:09:23.894800  9981 OpenCvVisualizer3D.cpp:395] Finished trajectory visualization.
I1005 12:09:23.894903  9979 Tracker.cpp:170] featureTracking: frame 3,  Nr tracked keypoints: 299 (max: 300) (max observed age of tracked features: 1 vs. maxFeatureAge_: 25)
I1005 12:09:23.895607  9979 MonoVisionImuFrontend.cpp:236] Finished feature tracking.
I1005 12:09:23.895622  9979 MonoVisionImuFrontend.cpp:117] Finished processStereoFrame.
I1005 12:09:23.895627  9979 MonoVisionImuFrontend.cpp:368] SmartMonoMeasurements with status: 
I1005 12:09:23.895632  9979 VisionImuFrontend.cpp:90] Status mono: INVALID
I1005 12:09:23.895637  9979 MonoVisionImuFrontend.cpp:371]  stereo points:

I1005 12:09:23.895648  9979 MonoVisionImuFrontend.cpp:170] Frontend output is not a keyframe. Skipping output queue push.
I1005 12:09:23.895665  9979 MonoImuPipeline.cpp:81] Frontend did not output a keyframe, skipping Backend input.
I1005 12:09:23.895681  9979 MonoVisionImuFrontend.cpp:91] ------------------- Processing frame k = 4--------------------
I1005 12:09:23.895687  9979 MonoImuSyncPacket.cpp:35] Mono Frame timestamp: 133333333
STAMPS IMU rows : 
1
STAMPS IMU cols : 
2
STAMPS IMU: 
133333332 133333333
ACCGYR IMU rows : 
6
ACCGYR IMU cols : 
2
ACCGYR IMU: 
0001.71988 0001.71988
00-1.12168 00-1.12168
00-9.74741 00-9.74741
00.0701897 00.0701896
000.271615 000.271615
-0.0502331 -0.0502331
I1005 12:09:23.895735  9979 ImuFrontend.cpp:108] Finished preintegration: 
PIM type: 1
    deltaTij = 2e-09
    deltaRij.ypr = ( 7.25640157e-11   5.6304326e-10 -4.20239707e-12)
    deltaPij =  3.19395223e-18 -1.12162215e-18 -1.89149473e-17
    deltaVij =  3.27588561e-09 -1.49553605e-09 -1.91082374e-08
    gyrobias = 0 0 0
    acc_bias = 0 0 0

    preintMeasCov 
[ 5.75826048e-17  -5.3006848e-37  1.92645017e-37  7.91561182e-54  1.40320296e-34 -1.61473417e-35  4.34193857e-45  2.80640592e-25 -3.22946834e-26
-5.30067956e-37  5.75826048e-17  2.33452648e-37 -1.40320296e-34 -5.85683395e-54 -2.47587377e-35 -2.80640592e-25 -3.23416249e-45 -4.95174755e-26
 1.92645724e-37  2.33452417e-37  5.75826048e-17  1.61473417e-35  2.47587377e-35 -2.05878718e-54  3.22946834e-26  4.95174755e-26 -1.10778413e-45
 7.91560882e-54 -1.40320296e-34  1.61473417e-35   2.0000001e-25 -1.90645556e-45  3.52585855e-46           8e-24  5.33479881e-45  3.24351239e-43
 1.40320296e-34 -5.85683608e-54  2.47587377e-35 -1.90645568e-45   2.0000001e-25  1.36440617e-45  5.33466397e-45           8e-24 -2.62961625e-43
-1.61473417e-35 -2.47587377e-35 -2.05878659e-54  3.52583868e-46  1.36440556e-45   2.0000001e-25  3.24351199e-43 -2.62961568e-43           8e-24
 4.34193534e-45 -2.80640592e-25  3.22946834e-26           8e-24  5.33476089e-45  3.24351137e-43           8e-15 -1.02537336e-35  5.47414546e-34
 2.80640592e-25 -3.23416749e-45  4.95174755e-26  5.33467964e-45           8e-24 -2.62961608e-43 -1.02537728e-35           8e-15 -3.48783275e-34
-3.22946834e-26 -4.95174755e-26 -1.10778213e-45  3.24350826e-43 -2.62961701e-43           8e-24  5.47414445e-34 -3.48783234e-34           8e-15]
body_R_cam [
    -0.422086895, 0.152124643, 0.893700361;
    -0.890668988, 0.114157535, -0.440086991;
    -0.168970615, -0.981746376, 0.0873084813
]
camLrectLkf_R_camLrectK_imu [
    0.999999662, -1.05822671e-07, 5.6775229e-08;
    -1.06313087e-07, 0.999999796, -1.81991046e-07;
    5.67904354e-08, -1.83014991e-07, 0.999999666
]
I1005 12:09:23.895972  9979 MonoVisionImuFrontend.cpp:112] Starting processFrame...
I1005 12:09:23.895977  9979 MonoVisionImuFrontend.cpp:215] ===================================================
processing frame: 4 at time 133333333 empirical framerate (sec): 0.0333333 (timestamp diff: 33333333)
I1005 12:09:23.895984  9979 MonoVisionImuFrontend.cpp:226] Starting feature tracking...
I1005 12:09:23.895993  9979 Tracker.cpp:113] Starting Optical Flow Pyr LK tracking...
I1005 12:09:23.896160  9983 ThreadsafeQueue.h:308] Queue with id: frontend_input_queue is getting full, size: 5
I1005 12:09:23.896206  9983 DataProviderModule.cpp:104] ////////////////////////////////////////// Creating packet!
STAMPS IMU rows : 
1
STAMPS IMU cols : 
2
STAMPS IMU: 
333333330 333333333
ACCGYR IMU rows : 
6
ACCGYR IMU cols : 
2
ACCGYR IMU: 
0002.17469 0002.17469
0-0.753787 0-0.753787
00-9.64591 00-9.64591
-0.0395967 -0.0395967
00.0917065 00.0917065
0-0.153597 0-0.153597
I1005 12:09:23.898252  9971 OpenCvDisplay.cpp:117] Spin Visualize 2D output.
I1005 12:09:23.899572  9979 Tracker.cpp:128] Optical Flow Timing [ms]: 3
W1005 12:09:23.899595  9971 OpenCvDisplay.cpp:143] Missing Mesh in visualization's 3D widgets.
I1005 12:09:23.900491  9979 Tracker.cpp:130] Finished Optical Flow Pyr LK tracking.
I1005 12:09:23.900791  9979 Tracker.cpp:170] featureTracking: frame 4,  Nr tracked keypoints: 299 (max: 300) (max observed age of tracked features: 1 vs. maxFeatureAge_: 25)
I1005 12:09:23.901844  9982 ThreadsafeQueue.h:286] Queue with id: data_provider_left_frame_queue is getting full, size: 5
I1005 12:09:23.901880  9982 EurocDataProvider.cpp:858] Finished VIO processing for frame k = 15
I1005 12:09:23.901896  9982 EurocDataProvider.cpp:832] Sending left frame k= 16 with timestamp: 533333333
I1005 12:09:23.902328  9979 MonoVisionImuFrontend.cpp:236] Finished feature tracking.
I1005 12:09:23.902344  9979 MonoVisionImuFrontend.cpp:117] Finished processStereoFrame.
I1005 12:09:23.902349  9979 MonoVisionImuFrontend.cpp:368] SmartMonoMeasurements with status: 
I1005 12:09:23.902354  9979 VisionImuFrontend.cpp:90] Status mono: INVALID
I1005 12:09:23.902359  9979 MonoVisionImuFrontend.cpp:371]  stereo points:

I1005 12:09:23.902369  9979 MonoVisionImuFrontend.cpp:170] Frontend output is not a keyframe. Skipping output queue push.
I1005 12:09:23.902385  9979 MonoImuPipeline.cpp:81] Frontend did not output a keyframe, skipping Backend input.
I1005 12:09:23.902393  9979 ThreadsafeQueue.h:286] Queue with id: visualizer_frontend_queue is getting full, size: 2
I1005 12:09:23.902406  9979 MonoVisionImuFrontend.cpp:91] ------------------- Processing frame k = 5--------------------
I1005 12:09:23.902412  9979 MonoImuSyncPacket.cpp:35] Mono Frame timestamp: 166666667
STAMPS IMU rows : 
1
STAMPS IMU cols : 
2
STAMPS IMU: 
166666665 166666667
ACCGYR IMU rows : 
6
ACCGYR IMU cols : 
2
ACCGYR IMU: 
0001.47492 0001.47492
0-0.456431 0-0.456431
00-9.68793 00-9.68793
0-0.019174 0-0.019174
000.340769 000.340769
-0.0524348 -0.0524348
I1005 12:09:23.902459  9979 ImuFrontend.cpp:108] Finished preintegration: 
PIM type: 1
    deltaTij = 4e-09
    deltaRij.ypr = (-3.23055568e-11  1.24458044e-09 -4.25503367e-11)
    deltaPij =  1.26955645e-17 -5.02555556e-18 -7.65072787e-17
    deltaVij =  6.22572662e-09 -2.40839736e-09  -3.8484094e-08
    gyrobias = 0 0 0
    acc_bias = 0 0 0

    preintMeasCov 
[  1.1516521e-16  1.35112323e-36 -9.68206551e-38   3.2873193e-53  1.81731377e-33 -1.33301641e-34  1.70408334e-44  1.39635289e-24 -8.48596156e-26
 1.35112294e-36   1.1516521e-16  5.37791926e-36 -1.81731377e-33 -1.84657063e-53  -2.9365322e-34 -1.39635289e-24 -1.19729838e-44 -2.19377006e-25
-9.68158975e-38  5.37791901e-36   1.1516521e-16  1.33301641e-34   2.9365322e-34 -1.44074646e-53  8.48596156e-26  2.19377006e-25 -5.06787728e-45
 3.28731658e-53 -1.81731377e-33  1.33301641e-34  4.00000082e-25  3.32065516e-45 -4.51735468e-46         3.2e-23  1.25958598e-42  7.61071927e-42
 1.81731377e-33 -1.84657651e-53   2.9365322e-34  3.32065505e-45  4.00000082e-25  1.56589087e-44  1.18449344e-42         3.2e-23 -9.58100185e-43
-1.33301641e-34  -2.9365322e-34 -1.44074652e-53 -4.51697113e-46  1.56589088e-44  4.00000082e-25  7.80812232e-42 -1.51242698e-42         3.2e-23
 1.70408267e-44 -1.39635289e-24  8.48596156e-26         3.2e-23  1.18449336e-42  7.80811772e-42         1.6e-14  7.03439828e-34  5.56153213e-33
 1.39635289e-24 -1.19730012e-44  2.19377006e-25  1.25958594e-42         3.2e-23 -1.51242714e-42  7.03439772e-34         1.6e-14 -1.10563676e-33
-8.48596156e-26 -2.19377006e-25 -5.06788262e-45  7.61072036e-42 -9.58100457e-43         3.2e-23  5.56153216e-33 -1.10563681e-33         1.6e-14]
body_R_cam [
    -0.422086895, 0.152124643, 0.893700361;
    -0.890668988, 0.114157535, -0.440086991;
    -0.168970615, -0.981746376, 0.0873084813
]
camLrectLkf_R_camLrectK_imu [
    0.999999662, -1.05479308e-07, 5.6950153e-08;
    -1.0665645e-07, 0.999999796, -1.81417928e-07;
    5.66155113e-08, -1.83588109e-07, 0.999999666
]
I1005 12:09:23.902709  9979 MonoVisionImuFrontend.cpp:112] Starting processFrame...
I1005 12:09:23.902724  9979 MonoVisionImuFrontend.cpp:215] ===================================================
processing frame: 5 at time 166666667 empirical framerate (sec): 0.0333333 (timestamp diff: 33333334)
I1005 12:09:23.902731  9979 MonoVisionImuFrontend.cpp:226] Starting feature tracking...
I1005 12:09:23.902740  9979 Tracker.cpp:113] Starting Optical Flow Pyr LK tracking...
I1005 12:09:23.902894  9983 ThreadsafeQueue.h:308] Queue with id: frontend_input_queue is getting full, size: 5
I1005 12:09:23.902930  9983 DataProviderModule.cpp:104] ////////////////////////////////////////// Creating packet!
STAMPS IMU rows : 
1
STAMPS IMU cols : 
2
STAMPS IMU: 
366666663 366666667
ACCGYR IMU rows : 
6
ACCGYR IMU cols : 
2
ACCGYR IMU: 
0002.38175 0002.38175
00-0.34342 00-0.34342
00-9.53592 00-9.53592
-0.0811654 -0.0811654
00.0286742 00.0286742
0-0.119962 0-0.119962
I1005 12:09:23.906527  9979 Tracker.cpp:128] Optical Flow Timing [ms]: 3
I1005 12:09:23.906559  9979 Tracker.cpp:130] Finished Optical Flow Pyr LK tracking.
I1005 12:09:23.906847  9979 Tracker.cpp:170] featureTracking: frame 5,  Nr tracked keypoints: 296 (max: 300) (max observed age of tracked features: 1 vs. maxFeatureAge_: 25)
I1005 12:09:23.907025  9982 ThreadsafeQueue.h:286] Queue with id: data_provider_left_frame_queue is getting full, size: 5
I1005 12:09:23.907065  9982 EurocDataProvider.cpp:858] Finished VIO processing for frame k = 16
I1005 12:09:23.907079  9982 EurocDataProvider.cpp:832] Sending left frame k= 17 with timestamp: 566666667
I1005 12:09:23.907675  9979 MonoVisionImuFrontend.cpp:236] Finished feature tracking.
I1005 12:09:23.907692  9979 MonoVisionImuFrontend.cpp:117] Finished processStereoFrame.
I1005 12:09:23.907699  9979 MonoVisionImuFrontend.cpp:368] SmartMonoMeasurements with status: 
I1005 12:09:23.907702  9979 VisionImuFrontend.cpp:90] Status mono: INVALID
I1005 12:09:23.907709  9979 MonoVisionImuFrontend.cpp:371]  stereo points:

I1005 12:09:23.907718  9979 MonoVisionImuFrontend.cpp:170] Frontend output is not a keyframe. Skipping output queue push.
I1005 12:09:23.907733  9979 MonoImuPipeline.cpp:81] Frontend did not output a keyframe, skipping Backend input.
I1005 12:09:23.907742  9979 ThreadsafeQueue.h:286] Queue with id: visualizer_frontend_queue is getting full, size: 3
I1005 12:09:23.907757  9979 MonoVisionImuFrontend.cpp:91] ------------------- Processing frame k = 6--------------------
I1005 12:09:23.907759  9983 ThreadsafeQueue.h:308] Queue with id: frontend_input_queue is getting full, size: 5
I1005 12:09:23.907764  9979 MonoImuSyncPacket.cpp:35] Mono Frame timestamp: 200000000
STAMPS IMU rows : 
1
STAMPS IMU cols : 
2
STAMPS IMU: 
199999998 200000000
ACCGYR IMU rows : 
6
ACCGYR IMU cols : 
2
ACCGYR IMU: 
0002.21834 0002.21834
0-0.588088 0-0.588088
00-8.89667 00-8.89667
-0.0121265 -0.0121265
000.157021 000.157021
000.048875 000.048875
I1005 12:09:23.907819  9979 ImuFrontend.cpp:108] Finished preintegration: 
PIM type: 1
    deltaTij = 6e-09
    deltaRij.ypr = ( 6.54444378e-11  1.55862231e-09 -6.68033034e-11)
    deltaPij =   2.9583694e-17 -1.10185256e-17 -1.71268805e-16
    deltaVij =  1.06624029e-08 -3.58457272e-09 -5.62774322e-08
    gyrobias = 0 0 0
    acc_bias = 0 0 0

I1005 12:09:23.907819  9983 DataProviderModule.cpp:104] ////////////////////////////////////////// Creating packet!
STAMPS IMU rows : 
1
STAMPS IMU cols : 
2
STAMPS IMU: 
399999996 400000000
ACCGYR IMU rows : 
6
ACCGYR IMU cols : 
2
ACCGYR IMU: 
002.26286 002.26286
-0.190085 -0.190086
00-10.172 00-10.172
-0.142948 -0.142948
0.0505096 0.0505096
0.0419614 0.0419614
    preintMeasCov 
[ 1.72747814e-16  2.33791614e-36   2.1033276e-37  3.50602827e-52  6.65919307e-33 -4.38475353e-34  5.06314168e-44  3.44552641e-24 -2.20314097e-25
 2.33791568e-36  1.72747814e-16  1.40071285e-36 -6.65919307e-33 -4.04219765e-52 -1.24335799e-33 -3.44552641e-24 -4.81928167e-44 -7.30327761e-25
 2.10336116e-37  1.40071262e-36  1.72747814e-16  4.38475353e-34  1.24335799e-33  5.36167361e-53  2.20314097e-25  7.30327761e-25 -2.43860379e-45
 3.50602847e-52 -6.65919307e-33  4.38475353e-34  6.00000282e-25  6.36724046e-45  4.96666662e-46         7.2e-23  5.41824935e-42   5.2352608e-41
 6.65919307e-33 -4.04219899e-52  1.24335799e-33   6.3672399e-45  6.00000282e-25  3.37984044e-45  4.97865788e-42         7.2e-23 -1.47219152e-41
-4.38475353e-34 -1.24335799e-33   5.3616723e-53  4.96696245e-46  3.37984278e-45  6.00000282e-25  4.74205355e-41 -1.53782231e-41         7.2e-23
 5.06313572e-44 -3.44552641e-24  2.20314097e-25         7.2e-23  4.97865904e-42  4.74205277e-41         2.4e-14   2.1217227e-33  2.48085644e-32
 3.44552641e-24 -4.81927456e-44  7.30327761e-25  5.41824871e-42         7.2e-23 -1.53782233e-41  2.12172261e-33         2.4e-14 -7.40485809e-33
-2.20314097e-25 -7.30327761e-25 -2.43860914e-45  5.23526106e-41 -1.47219155e-41         7.2e-23  2.48085644e-32 -7.40485808e-33         2.4e-14]
body_R_cam [
    -0.422086895, 0.152124643, 0.893700361;
    -0.890668988, 0.114157535, -0.440086991;
    -0.168970615, -0.981746376, 0.0873084813
]
camLrectLkf_R_camLrectK_imu [
    0.999999662, -1.05327961e-07, 5.68863482e-08;
    -1.06807796e-07, 0.999999796, -1.8113194e-07;
    5.66793163e-08, -1.83874096e-07, 0.999999666
]
I1005 12:09:23.908072  9979 MonoVisionImuFrontend.cpp:112] Starting processFrame...
I1005 12:09:23.908077  9979 MonoVisionImuFrontend.cpp:215] ===================================================
processing frame: 6 at time 200000000 empirical framerate (sec): 0.0333333 (timestamp diff: 33333333)
I1005 12:09:23.908083  9979 MonoVisionImuFrontend.cpp:226] Starting feature tracking...
I1005 12:09:23.908092  9979 Tracker.cpp:113] Starting Optical Flow Pyr LK tracking...
I1005 12:09:23.912127  9982 ThreadsafeQueue.h:286] Queue with id: data_provider_left_frame_queue is getting full, size: 5
I1005 12:09:23.912179  9982 EurocDataProvider.cpp:858] Finished VIO processing for frame k = 17
I1005 12:09:23.912186  9982 EurocDataProvider.cpp:832] Sending left frame k= 18 with timestamp: 600000000
I1005 12:09:23.912225  9979 Tracker.cpp:128] Optical Flow Timing [ms]: 4
I1005 12:09:23.912240  9979 Tracker.cpp:130] Finished Optical Flow Pyr LK tracking.
I1005 12:09:23.912521  9979 Tracker.cpp:170] featureTracking: frame 6,  Nr tracked keypoints: 294 (max: 300) (max observed age of tracked features: 1 vs. maxFeatureAge_: 25)
I1005 12:09:23.913265  9979 MonoVisionImuFrontend.cpp:236] Finished feature tracking.
I1005 12:09:23.913281  9979 MonoVisionImuFrontend.cpp:117] Finished processStereoFrame.
I1005 12:09:23.913285  9979 MonoVisionImuFrontend.cpp:368] SmartMonoMeasurements with status: 
I1005 12:09:23.913288  9979 VisionImuFrontend.cpp:90] Status mono: INVALID
I1005 12:09:23.913292  9979 MonoVisionImuFrontend.cpp:371]  stereo points:

I1005 12:09:23.913301  9979 MonoVisionImuFrontend.cpp:170] Frontend output is not a keyframe. Skipping output queue push.
I1005 12:09:23.913317  9979 MonoImuPipeline.cpp:81] Frontend did not output a keyframe, skipping Backend input.
I1005 12:09:23.913326  9979 ThreadsafeQueue.h:286] Queue with id: visualizer_frontend_queue is getting full, size: 4
I1005 12:09:23.913343  9983 ThreadsafeQueue.h:308] Queue with id: frontend_input_queue is getting full, size: 5
I1005 12:09:23.913359  9983 DataProviderModule.cpp:104] ////////////////////////////////////////// Creating packet!
STAMPS IMU rows : 
1
STAMPS IMU cols : 
2
STAMPS IMU: 
433333329 433333333
ACCGYR IMU rows : 
6
ACCGYR IMU cols : 
2
ACCGYR IMU: 
002.34168 002.34168
-0.670061 -0.670061
0-9.94952 0-9.94952
-0.125412 -0.125412
0.0690582 0.0690582
00.130418 00.130418
I1005 12:09:23.913400  9979 MonoVisionImuFrontend.cpp:91] ------------------- Processing frame k = 7--------------------
I1005 12:09:23.913408  9979 MonoImuSyncPacket.cpp:35] Mono Frame timestamp: 233333333
STAMPS IMU rows : 
1
STAMPS IMU cols : 
2
STAMPS IMU: 
233333331 233333333
ACCGYR IMU rows : 
6
ACCGYR IMU cols : 
2
ACCGYR IMU: 
001.69772 001.69772
-0.522906 -0.522906
0-9.96023 0-9.96023
0.0224787 0.0224787
00.209103 00.209103
00.040384 00.040384
I1005 12:09:23.913447  9979 ImuFrontend.cpp:108] Finished preintegration: 
PIM type: 1
    deltaTij = 8e-09
    deltaRij.ypr = ( 1.46212428e-10   1.9768286e-09 -2.18459268e-11)
    deltaPij =  5.43039444e-17 -1.92334836e-17 -3.03744132e-16
    deltaVij =  1.40578475e-08 -4.63038525e-09 -7.61978954e-08
    gyrobias = 0 0 0
    acc_bias = 0 0 0

    preintMeasCov 
[ 2.30330419e-16 -1.18065363e-36 -4.69216997e-37  1.22214992e-51  1.69914624e-32 -1.05976538e-33   1.2743793e-43  6.88674289e-24 -4.00975925e-25
-1.18065302e-36  2.30330419e-16 -4.92055871e-36 -1.69914624e-32 -1.36110621e-51 -3.29056917e-33 -6.88674289e-24 -1.32941616e-43 -1.31688341e-24
-4.69207353e-37 -4.92055988e-36  2.30330419e-16  1.05976538e-33  3.29056917e-33  1.38956422e-52  4.00975925e-25  1.31688341e-24  5.50353258e-45
 1.22214938e-51 -1.69914624e-32  1.05976538e-33  8.00000674e-25 -4.91361389e-45 -1.68164248e-45        1.28e-22  1.16172691e-41  1.73746981e-40
 1.69914624e-32 -1.36110608e-51  3.29056917e-33 -4.91361633e-45  8.00000674e-25 -1.68869137e-44  1.10048996e-41        1.28e-22 -5.60138176e-41
-1.05976538e-33 -3.29056917e-33  1.38956327e-52 -1.68152616e-45 -1.68869125e-44  8.00000674e-25  1.73821639e-40 -5.92258736e-41        1.28e-22
 1.27437746e-43 -6.88674289e-24  4.00975925e-25        1.28e-22  1.10049007e-41  1.73821627e-40         3.2e-14   3.6453538e-33  6.26243904e-32
 6.88674289e-24 -1.32941511e-43  1.31688341e-24   1.1617267e-41        1.28e-22 -5.92258747e-41  3.64535342e-33         3.2e-14 -2.00767447e-32
-4.00975925e-25 -1.31688341e-24  5.50354327e-45  1.73746983e-40 -5.60138181e-41        1.28e-22  6.26243899e-32 -2.00767447e-32         3.2e-14]
body_R_cam [
    -0.422086895, 0.152124643, 0.893700361;
    -0.890668988, 0.114157535, -0.440086991;
    -0.168970615, -0.981746376, 0.0873084813
]
camLrectLkf_R_camLrectK_imu [
    0.999999662, -1.05191144e-07, 5.68616349e-08;
    -1.06944613e-07, 0.999999796, -1.80726834e-07;
    5.67040295e-08, -1.84279203e-07, 0.999999666
]
I1005 12:09:23.913691  9979 MonoVisionImuFrontend.cpp:112] Starting processFrame...
I1005 12:09:23.913695  9979 MonoVisionImuFrontend.cpp:215] ===================================================
processing frame: 7 at time 233333333 empirical framerate (sec): 0.0333333 (timestamp diff: 33333333)
I1005 12:09:23.913700  9979 MonoVisionImuFrontend.cpp:226] Starting feature tracking...
I1005 12:09:23.913709  9979 Tracker.cpp:113] Starting Optical Flow Pyr LK tracking...
I1005 12:09:23.917017  9982 ThreadsafeQueue.h:286] Queue with id: data_provider_left_frame_queue is getting full, size: 5
I1005 12:09:23.917100  9982 EurocDataProvider.cpp:858] Finished VIO processing for frame k = 18
I1005 12:09:23.917111  9982 EurocDataProvider.cpp:832] Sending left frame k= 19 with timestamp: 633333333
I1005 12:09:23.917266  9979 Tracker.cpp:128] Optical Flow Timing [ms]: 3
I1005 12:09:23.917285  9979 Tracker.cpp:130] Finished Optical Flow Pyr LK tracking.
I1005 12:09:23.917567  9979 Tracker.cpp:170] featureTracking: frame 7,  Nr tracked keypoints: 293 (max: 300) (max observed age of tracked features: 1 vs. maxFeatureAge_: 25)
I1005 12:09:23.918354  9979 MonoVisionImuFrontend.cpp:236] Finished feature tracking.
I1005 12:09:23.918371  9979 MonoVisionImuFrontend.cpp:117] Finished processStereoFrame.
I1005 12:09:23.918376  9979 MonoVisionImuFrontend.cpp:368] SmartMonoMeasurements with status: 
I1005 12:09:23.918380  9979 VisionImuFrontend.cpp:90] Status mono: INVALID
I1005 12:09:23.918383  9979 MonoVisionImuFrontend.cpp:371]  stereo points:

I1005 12:09:23.918395  9979 MonoVisionImuFrontend.cpp:170] Frontend output is not a keyframe. Skipping output queue push.
I1005 12:09:23.918411  9979 MonoImuPipeline.cpp:81] Frontend did not output a keyframe, skipping Backend input.
I1005 12:09:23.918418  9979 ThreadsafeQueue.h:286] Queue with id: visualizer_frontend_queue is getting full, size: 5
I1005 12:09:23.918435  9983 ThreadsafeQueue.h:308] Queue with id: frontend_input_queue is getting full, size: 5
I1005 12:09:23.918453  9983 DataProviderModule.cpp:104] ////////////////////////////////////////// Creating packet!
STAMPS IMU rows : 
1
STAMPS IMU cols : 
2
STAMPS IMU: 
466666662 466666667
ACCGYR IMU rows : 
6
ACCGYR IMU cols : 
2
ACCGYR IMU: 
0002.27056 0002.27056
00-1.12828 00-1.12828
00-9.83843 00-9.83843
-0.0520898 -0.0520898
00.0791121 00.0791121
00.0795574 00.0795574
I1005 12:09:23.918519  9979 MonoVisionImuFrontend.cpp:91] ------------------- Processing frame k = 8--------------------
I1005 12:09:23.918526  9979 MonoImuSyncPacket.cpp:35] Mono Frame timestamp: 266666667
STAMPS IMU rows : 
1
STAMPS IMU cols : 
2
STAMPS IMU: 
266666664 266666667
ACCGYR IMU rows : 
6
ACCGYR IMU cols : 
2
ACCGYR IMU: 
001.98266 001.98266
-0.749429 -0.749429
0-9.12915 0-9.12915
0.0178053 0.0178053
00.197365 00.197365
0.0266787 0.0266787
I1005 12:09:23.918567  9979 ImuFrontend.cpp:108] Finished preintegration: 
PIM type: 1
    deltaTij = 1.1e-08
    deltaRij.ypr = (2.26248436e-10 2.56892331e-09 3.15700786e-11)
    deltaPij =  1.05399456e-16 -3.64970699e-17 -5.73418984e-16
    deltaVij =  2.00058271e-08 -6.87867231e-09 -1.03585339e-07
    gyrobias = 0 0 0
    acc_bias = 0 0 0

    preintMeasCov 
[ 3.16704326e-16 -9.14833583e-36 -1.54623409e-36  2.71313844e-51   4.7113933e-32 -3.03946651e-33  2.65705408e-43  1.31949042e-23 -9.18824828e-25
-9.14833684e-36  3.16704326e-16 -1.68589547e-35  -4.7113933e-32 -2.76518837e-51 -9.29622041e-33 -1.31949042e-23 -2.84603868e-43 -2.68688407e-24
-1.54622378e-36 -1.68589562e-35  3.16704326e-16  3.03946651e-33  9.29622041e-33  5.20510305e-53  9.18824828e-25  2.68688407e-24  1.88979005e-44
 2.71313816e-51  -4.7113933e-32  3.03946651e-33  1.10000176e-24 -3.02153883e-44  -5.0995883e-45        2.42e-22  3.74193008e-41  6.94917362e-40
  4.7113933e-32 -2.76518717e-51  9.29622041e-33 -3.02153971e-44  1.10000176e-24 -5.47988967e-44  3.87652091e-41        2.42e-22 -2.50110369e-40
-3.03946651e-33 -9.29622041e-33  5.20507734e-53 -5.09942396e-45 -5.47988867e-44  1.10000176e-24  6.76703239e-40 -2.37392501e-40        2.42e-22
 2.65705258e-43 -1.31949042e-23  9.18824828e-25        2.42e-22  3.87652152e-41  6.76703223e-40         4.4e-14  1.06796343e-32  1.76985373e-31
 1.31949042e-23 -2.84603441e-43  2.68688407e-24  3.74192953e-41        2.42e-22 -2.37392501e-40  1.06796343e-32         4.4e-14 -6.28094944e-32
-9.18824828e-25 -2.68688407e-24  1.88981143e-44  6.94917375e-40 -2.50110371e-40        2.42e-22  1.76985375e-31 -6.28094944e-32         4.4e-14]
body_R_cam [
    -0.422086895, 0.152124643, 0.893700361;
    -0.890668988, 0.114157535, -0.440086991;
    -0.168970615, -0.981746376, 0.0873084813
]
camLrectLkf_R_camLrectK_imu [
    0.999999662, -1.04985297e-07, 5.68587777e-08;
    -1.07150461e-07, 0.999999796, -1.80163404e-07;
    5.67068867e-08, -1.84842633e-07, 0.999999666
]
I1005 12:09:23.918807  9979 MonoVisionImuFrontend.cpp:112] Starting processFrame...
I1005 12:09:23.918812  9979 MonoVisionImuFrontend.cpp:215] ===================================================
processing frame: 8 at time 266666667 empirical framerate (sec): 0.0333333 (timestamp diff: 33333334)
I1005 12:09:23.918819  9979 MonoVisionImuFrontend.cpp:226] Starting feature tracking...
I1005 12:09:23.918829  9979 Tracker.cpp:113] Starting Optical Flow Pyr LK tracking...
I1005 12:09:23.921981  9982 ThreadsafeQueue.h:286] Queue with id: data_provider_left_frame_queue is getting full, size: 5
I1005 12:09:23.922063  9982 EurocDataProvider.cpp:858] Finished VIO processing for frame k = 19
I1005 12:09:23.922075  9982 EurocDataProvider.cpp:832] Sending left frame k= 20 with timestamp: 666666667
I1005 12:09:23.922382  9979 Tracker.cpp:128] Optical Flow Timing [ms]: 3
I1005 12:09:23.922400  9979 Tracker.cpp:130] Finished Optical Flow Pyr LK tracking.
I1005 12:09:23.922686  9979 Tracker.cpp:170] featureTracking: frame 8,  Nr tracked keypoints: 291 (max: 300) (max observed age of tracked features: 1 vs. maxFeatureAge_: 25)
I1005 12:09:23.923480  9979 MonoVisionImuFrontend.cpp:236] Finished feature tracking.
I1005 12:09:23.923493  9979 MonoVisionImuFrontend.cpp:254] Keframe after [s]: 0.2
I1005 12:09:23.923508  9979 MonoVisionImuFrontend.cpp:259] Keyframe reason: max time elapsed.
I1005 12:09:23.923591  9979 Tracker.cpp:313] geometricOutlierRejectionMonoGivenRot: starting 2-point RANSAC
I1005 12:09:23.924252  9979 Tracker.cpp:320] geometricOutlierRejectionMonoGivenRot: RANSAC complete
I1005 12:09:23.924266  9979 Tracker.cpp:322] RANSAC (MONO): #iter = 32
 #inliers = 114
 #outliers = 177
 Total = 291
I1005 12:09:23.924285  9979 Tracker.cpp:351] median disparity 40.2716
I1005 12:09:23.924296  9979 VisionImuFrontend.cpp:90] Status mono: VALID
I1005 12:09:23.926326  9982 ThreadsafeQueue.h:286] Queue with id: data_provider_left_frame_queue is getting full, size: 6
I1005 12:09:23.926345  9982 EurocDataProvider.cpp:858] Finished VIO processing for frame k = 20
I1005 12:09:23.926352  9982 EurocDataProvider.cpp:832] Sending left frame k= 21 with timestamp: 700000000
I1005 12:09:23.930338  9982 ThreadsafeQueue.h:286] Queue with id: data_provider_left_frame_queue is getting full, size: 7
I1005 12:09:23.930354  9982 EurocDataProvider.cpp:858] Finished VIO processing for frame k = 21
I1005 12:09:23.930361  9982 EurocDataProvider.cpp:832] Sending left frame k= 22 with timestamp: 733333333
I1005 12:09:23.934161  9982 ThreadsafeQueue.h:286] Queue with id: data_provider_left_frame_queue is getting full, size: 8
I1005 12:09:23.934178  9982 EurocDataProvider.cpp:858] Finished VIO processing for frame k = 22
I1005 12:09:23.934185  9982 EurocDataProvider.cpp:832] Sending left frame k= 23 with timestamp: 766666667
I1005 12:09:23.938024  9982 ThreadsafeQueue.h:286] Queue with id: data_provider_left_frame_queue is getting full, size: 9
I1005 12:09:23.938041  9982 EurocDataProvider.cpp:858] Finished VIO processing for frame k = 23
I1005 12:09:23.938046  9982 EurocDataProvider.cpp:832] Sending left frame k= 24 with timestamp: 800000000
I1005 12:09:23.941962  9982 ThreadsafeQueue.h:286] Queue with id: data_provider_left_frame_queue is getting full, size: 10
I1005 12:09:23.941980  9982 EurocDataProvider.cpp:858] Finished VIO processing for frame k = 24
I1005 12:09:23.941985  9982 EurocDataProvider.cpp:832] Sending left frame k= 25 with timestamp: 833333333
I1005 12:09:23.945662  9982 ThreadsafeQueue.h:286] Queue with id: data_provider_left_frame_queue is getting full, size: 11
I1005 12:09:23.945674  9982 EurocDataProvider.cpp:858] Finished VIO processing for frame k = 25
I1005 12:09:23.945679  9982 EurocDataProvider.cpp:832] Sending left frame k= 26 with timestamp: 866666667
I1005 12:09:23.949409  9982 ThreadsafeQueue.h:286] Queue with id: data_provider_left_frame_queue is getting full, size: 12
I1005 12:09:23.949422  9982 EurocDataProvider.cpp:858] Finished VIO processing for frame k = 26
I1005 12:09:23.949427  9982 EurocDataProvider.cpp:832] Sending left frame k= 27 with timestamp: 900000000
I1005 12:09:23.953680  9982 ThreadsafeQueue.h:286] Queue with id: data_provider_left_frame_queue is getting full, size: 13
I1005 12:09:23.953723  9982 EurocDataProvider.cpp:858] Finished VIO processing for frame k = 27
I1005 12:09:23.953732  9982 EurocDataProvider.cpp:832] Sending left frame k= 28 with timestamp: 933333333
I1005 12:09:23.958228  9982 ThreadsafeQueue.h:286] Queue with id: data_provider_left_frame_queue is getting full, size: 14
I1005 12:09:23.958336  9982 EurocDataProvider.cpp:858] Finished VIO processing for frame k = 28
I1005 12:09:23.958353  9982 EurocDataProvider.cpp:832] Sending left frame k= 29 with timestamp: 966666667
I1005 12:09:23.962600  9979 FeatureDetector.cpp:198] Number of points detected : 300
I1005 12:09:23.962641  9979 FeatureDetector.cpp:210] Need n corners: 186
I1005 12:09:23.962648  9979 NonMaximumSuppression.cpp:47] Sorting keypoints in decreasing order of strength.
I1005 12:09:23.962667  9979 NonMaximumSuppression.cpp:61] Starting Adaptive Non-Maximum Suppression.
I1005 12:09:23.962672  9979 NonMaximumSuppression.cpp:87] Running RangeTree: 4
I1005 12:09:23.963114  9979 NonMaximumSuppression.cpp:105] Non Maximum Suppression Timing [ms]: 0
I1005 12:09:23.963214  9982 ThreadsafeQueue.h:286] Queue with id: data_provider_left_frame_queue is getting full, size: 15
I1005 12:09:23.963232  9982 EurocDataProvider.cpp:858] Finished VIO processing for frame k = 29
I1005 12:09:23.963239  9982 EurocDataProvider.cpp:832] Sending left frame k= 30 with timestamp: 1000000000
I1005 12:09:23.967255  9982 ThreadsafeQueue.h:286] Queue with id: data_provider_left_frame_queue is getting full, size: 16
I1005 12:09:23.967281  9982 EurocDataProvider.cpp:858] Finished VIO processing for frame k = 30
I1005 12:09:23.967288  9982 EurocDataProvider.cpp:832] Sending left frame k= 31 with timestamp: 1033333333
I1005 12:09:23.970885  9979 FeatureDetector.cpp:250] Corner Sub Pixel Refinement Timing [ms]: 7
I1005 12:09:23.971114  9979 FeatureDetector.cpp:147] featureExtraction: frame 8,  Nr tracked keypoints: 291,  Nr extracted keypoints: 190,  total: 481  (max: 300)
I1005 12:09:23.971176  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[2055.68, -14.1966]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971208  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[2096.11, 712.146]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971215  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-22.0985, 891.735]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971220  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-68.5194, 414.555]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971227  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[1920.67, 795.371]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971233  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-129.815, 888.592]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971238  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[2109.24, 365.774]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971246  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-181.923, 866.651]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971256  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-203.03, 872.151]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971264  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-89.7364, -93.2238]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971271  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[1979.56, 821.299]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971282  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-90.4968, 732.233]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971292  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-123.639, 532.583]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971303  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-175.025, 320.283]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971312  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-70.2169, 788.533]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971323  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[2093.14, 734.918]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971333  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-197.487, 935.928]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971340  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-151.849, 788.745]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971349  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-135.225, 428.525]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971359  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-164.041, 929.197]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971379  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-220.18, 737.24]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971388  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-113.351, 916.564]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971397  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-123.843, 853.793]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971405  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-198.426, 388.867]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971412  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-129.42, 487.838]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971421  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-126.659, 455.266]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971428  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-197.644, 410.956]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971436  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-158.956, 725.119]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971444  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-199.729, 801.239]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971452  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[2007.72, 852.761]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971462  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-213.05, 909.324]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971467  9982 ThreadsafeQueue.h:286] Queue with id: data_provider_left_frame_queue is getting full, size: 17
I1005 12:09:23.971470  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-43.8053, 700.804]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971485  9982 EurocDataProvider.cpp:858] Finished VIO processing for frame k = 31
I1005 12:09:23.971498  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[2101.99, 642.119]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971505  9982 EurocDataProvider.cpp:832] Sending left frame k= 32 with timestamp: 1066666667
I1005 12:09:23.971513  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-200.474, 761.797]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971529  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-156.767, 751.246]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971537  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-81.2369, 525.888]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971546  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-63.2048, 298.633]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971554  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-71.2805, 366.446]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971565  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-101.386, -104.44]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971572  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-152.897, 320.346]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971585  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-152.3, 133.065]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971596  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[2011.01, 826.018]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971603  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[392.183, 1105.78]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971612  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[2134.92, 910.002]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971619  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-1.42819, 1046.38]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971627  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-85.9462, 486.119]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971637  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-202.826, 494.687]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971645  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[2036.01, -38.8869]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971653  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[279.393, -85.4444]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971662  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-7.19736, -81.1484]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971670  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-186.883, 512.469]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971680  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[1943.68, -11.3977]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971689  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-195.765, 204.591]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971697  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-138.497, 381.863]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971706  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[2065.33, 31.4836]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971714  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[1822.93, -74.7254]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971724  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-198.57, 257.959]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971732  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[2014.86, 794.651]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971741  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-179.131, 575.497]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971751  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-50.555, 543.305]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971760  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-70.0452, 704.708]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971768  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[2040.8, 1004.61]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971776  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-64.96, -111.301]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971787  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-231.201, 343.762]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971796  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-4.66351, 779.228]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971805  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[2106.25, 602.694]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971813  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-33.8022, 641.769]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971822  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[2068.62, 887.997]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971829  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-45.4757, 580.161]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971837  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-185.3, 287.477]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971844  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-183.553, 494.34]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971849  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[37.8966, -53.2026]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971854  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-61.4414, 843.158]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971860  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[1770.45, -71.1594]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971868  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-68.5952, 416.348]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971874  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[2109.61, 366.966]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971880  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-51.3152, 546.322]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971889  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-184.49, 289.472]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971897  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-178.106, 497.446]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971905  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[2056.38, -15.0647]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971913  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[1770.3, -60.5534]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971920  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[14.9294, -134.071]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971930  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-225.091, 340.824]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971937  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-88.5416, 484.274]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971946  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-187.115, 402.553]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971956  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-124.161, 531.121]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971963  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[1627.17, 1195.13]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971971  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[393.009, 1104.47]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971980  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[2069.67, 893.5]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971988  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-100.587, -104.519]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.971997  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-89.8168, 732.613]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.972004  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-138.142, 426.311]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.972012  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[1978.68, 821.818]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.972019  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-63.617, 298.6]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.972028  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[2095.52, 720.625]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.972035  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[2042.11, 1002.35]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.972043  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-72.0598, 366.048]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.972052  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[2019.18, 862.672]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.972059  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-225.9, 806.792]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.972067  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[1286.2, -40.6221]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.972074  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-215.732, 252.333]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.972081  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[1861.97, -136.285]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.972088  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[2114.7, 45.7807]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.972097  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-196.692, 542.243]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.972105  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-145.56, 913.787]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.972113  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[2111.7, 928.74]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.972121  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-34.1772, 640.004]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.972128  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-174.864, 679.784]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.972138  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-181.419, 454.268]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.972146  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-216.585, 688.375]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.972154  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[26.0745, 1085.58]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.972162  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-142.757, -117.005]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.972170  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-116.178, 588.599]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.972178  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-189.05, 602.842]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.972185  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[278.977, -135.862]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.972193  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[475.68, 1171.28]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.972199  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-114.161, 1109.8]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.972208  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-78.7725, 181.932]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.972214  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[2070.29, 33.1312]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.972221  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[1942.57, -10.7096]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.972229  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[1161.8, -36.3641]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.972236  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[639.684, 1140.61]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.972244  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-158.51, 813.157]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.972250  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[2002.91, 1129.57]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.972259  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[2115.78, 881.146]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.972265  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-14.2338, -84.7242]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.972272  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[279.294, -85.2695]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.972280  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[2055.5, 956.649]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.972285  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-22.6365, 857.302]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.972291  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-61.4685, -108.946]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.972301  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-152.409, 132.344]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.972309  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[1898.03, -21.734]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.972316  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[2102.34, 647.999]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.972321  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[1822.83, -74.1791]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.972328  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[-195.595, 203.715]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.972334  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[38.217, -54.6529]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.972339  9979 UndistorterRectifier.cpp:154] Undistorted Rectified keypoint out of image!
Keypoint undistorted: 
[2119.92, 1112.38]
Image Size (map x size): 1080 x 1920
I1005 12:09:23.972347  9979 UndistorterRectifier.cpp:186] undistortRectifyPoints: unable to match 137 keypoints of 481 total.
I1005 12:09:23.974233  9979 MonoVisionImuFrontend.cpp:313] timeGetMeasurements: 0
I1005 12:09:23.974269  9979 MonoVisionImuFrontend.cpp:117] Finished processStereoFrame.
I1005 12:09:23.974275  9979 MonoVisionImuFrontend.cpp:368] SmartMonoMeasurements with status: 
I1005 12:09:23.974280  9979 VisionImuFrontend.cpp:90] Status mono: VALID
I1005 12:09:23.974287  9979 MonoVisionImuFrontend.cpp:371]  stereo points:
 (601.622925, nan, 384.61499)  (877.300354, nan, 457.48584)  (0, nan, 891.735413)  (0, nan, 888.592224)  (909.010925, nan, 362.85321)  (956.027222, nan, 494.577454)  (961.931519, nan, 529.753479)  (0, nan, 866.651428)  (0, nan, 872.151489)  (691.96759, nan, 423.528229)  (764.96283, nan, 371.561554)  (895.368591, nan, 381.085724)  (934.739014, nan, 455.635132)  (867.504395, nan, 533.816406)  (860.961609, nan, 515.760864)  (601.237305, nan, 480.40921)  (884.331299, nan, 413.433014)  (796.178345, nan, 521.367188)  (666.769226, nan, 482.344849)  (787.869324, nan, 367.697632)  (619.199158, nan, 385.254456)  (878.932312, nan, 474.21521)  (580.058289, nan, 464.607208)  (0, nan, 788.532654)  (708.020569, nan, 472.631287)  (925.063171, nan, 512.731567)  (811.24231, nan, 551.596985)  (654.654846, nan, 545.852966)  (724.574402, nan, 529.126526)  (578.138184, nan, 593.456116)  (969.807983, nan, 452.311615)  (0, nan, 935.928101)  (0, nan, 788.744995)  (949.614075, nan, 445.364685)  (729.338196, nan, 502.490692)  (0, nan, 929.197449)  (0, nan, 737.239563)  (721.390198, nan, 374.225525)  (543.229553, nan, 598.556091)  (0, nan, 916.563843)  (0, nan, 853.793396)  (940.801331, nan, 407.795502)  (0, nan, 725.119141)  (0, nan, 801.238831)  (842.994446, nan, 481.4086)  (633.80603, nan, 475.799591)  (723.775269, nan, 443.727051)  (525.495056, nan, 504.421509)  (931.477234, nan, 483.060333)  (826.946106, nan, 526.508057)  (568.416565, nan, 638.637817)  (0, nan, 909.323547)  (0, nan, 700.803772)  (562.960876, nan, 481.193054)  (0, nan, 761.796631)  (838.067261, nan, 556.199219)  (0, nan, 751.246277)  (0, nan, 525.887634)  (907.210388, nan, 469.28363)  (942.09613, nan, 463.584015)  (917.53186, nan, 339.405701)  (666.035217, nan, 624.860352)  (910.074341, nan, 428.055573)  (517.09552, nan, 461.137146)  (935.162598, nan, 538.297668)  (666.495483, nan, 424.723358)  (603.507935, nan, 454.455017)  (948.359863, nan, 351.492035)  (603.678528, nan, 347.90686)  (790.661987, nan, 539.907104)  (0, nan, 1046.37988)  (350.459167, nan, 800.854675)  (577.357056, nan, 443.491211)  (636.922424, nan, 527.418945)  (0, nan, 494.687073)  (877.158875, nan, 517.789978)  (904.670471, nan, 323.905365)  (693.617065, nan, 593.937195)  (941.093628, nan, 560.775452)  (709.612732, nan, 510.272644)  (962.108032, nan, 559.369751)  (621.999329, nan, 406.148376)  (743.259766, nan, 577.23291)  (798.503723, nan, 492.105957)  (715.76355, nan, 326.500275)  (617.901306, nan, 620.900818)  (723.913513, nan, 408.749237)  (761.610413, nan, 338.626831)  (0, nan, 704.708374)  (322.252014, nan, 855.766174)  (756.739014, nan, 291.201202)  (900.728577, nan, 339.566315)  (534.896973, nan, 575.453003)  (591.239136, nan, 499.73822)  (659.128296, nan, 458.556335)  (536.697937, nan, 478.888306)  (0, nan, 779.228149)  (647.681458, nan, 615.972717)  (929.457153, nan, 374.8078)  (735.232361, nan, 552.710449)  (564.598145, nan, 610.387085)  (547.716187, nan, 526.719421)  (772.008057, nan, 439.993774)  (647.738708, nan, 402.288757)  (957.646301, nan, 276.63266)  (681.750977, nan, 509.848663)  (917.505554, nan, 396.026672)  (684.763977, nan, 468.236237)  (658.201538, nan, 526.108948)  (778.40686, nan, 492.69455)  (629.740906, nan, 494.453156)  (0, nan, 843.157715)  (797.07135, nan, 679.799255)  (532.614685, nan, 557.322937)  (1120.16431, nan, 721.251099)  (1417.69385, nan, 936.181091)  (577.163086, nan, 416.021271)  (0, nan, 416.348175)  (1919, nan, 366.966217)  (1563.49683, nan, 958.782776)  (1240.13708, nan, 164.514435)  (695.47467, nan, 315.680634)  (1155.33911, nan, 736.956726)  (1144.30408, nan, 261.053009)  (0, nan, 546.321899)  (1563.45874, nan, 269.021759)  (1041.75208, nan, 701.407593)  (1128.79749, nan, 967.09259)  (1036.42163, nan, 778.706055)  (658.841614, nan, 501.495483)  (1082.63513, nan, 703.639465)  (863.571533, nan, 490.735474)  (1565.25293, nan, 885.960938)  (1029.12561, nan, 810.093079)  (347.648468, nan, 413.166809)  (1038.11267, nan, 917.501709)  (0, nan, 289.472443)  (407.890076, nan, 417.759277)  (1329.4928, nan, 877.485962)  (1357.9657, nan, 487.806091)  (573.193054, nan, 349.12912)  (954.949097, nan, 244.283386)  (0, nan, 497.445618)  (1243.17786, nan, 792.736877)  (1919, nan, 0)  (988.585144, nan, 334.812653)  (890.1698, nan, 429.458038)  (1770.29602, nan, 0)  (1782.63745, nan, 90.8095245)  (14.9294348, nan, 0)  (1095.32068, nan, 797.101318)  (991.861206, nan, 422.576294)  (294.858521, nan, 940.606506)  (0, nan, 340.824402)  (552.978027, nan, 701.123108)  (0, nan, 484.273682)  (0, nan, 402.552826)  (844.134399, nan, 578.801025)  (1733.36511, nan, 118.155609)  (84.2361603, nan, 537.115601)  (541.464966, nan, 436.285339)  (0, nan, 531.121338)  (1401.97021, nan, 302.272064)  (1627.17432, nan, 1079)  (393.008728, nan, 1079)  (960.999023, nan, 473.850159)  (1309.96155, nan, 62.5706291)  (1118.82092, nan, 323.193024)  (1179.12476, nan, 788.777161)  (1561.07971, nan, 1063.04651)  (1134.021, nan, 906.719421)  (412.785675, nan, 217.104004)  (1217.73572, nan, 610.536499)  (1223.71399, nan, 350.537292)  (642.018921, nan, 424.237885)  (1577.48291, nan, 139.553482)  (1919, nan, 893.499512)  (1637.8252, nan, 173.58313)  (1074.13171, nan, 661.0755)  (1068.66455, nan, 851.309265)  (1204.18494, nan, 311.046234)  (0, nan, 0)  (1153.88257, nan, 852.852966)  (1168.1864, nan, 661.34259)  (907.963928, nan, 205.502502)  (0, nan, 732.613159)  (0, nan, 426.311432)  (1919, nan, 821.817566)  (702.43396, nan, 444.290039)  (1124.65125, nan, 852.569458)  (587.112854, nan, 320.850464)  (1005.8139, nan, 274.303436)  (1215.61072, nan, 536.230652)  (931.492676, nan, 440.989471)  (507.08252, nan, 397.962097)  (1217.15332, nan, 570.352295)  (0, nan, 298.600494)  (1919, nan, 720.625)  (1023.00128, nan, 585.573914)  (986.750305, nan, 199.389114)  (1243.32605, nan, 243.165665)  (1619.55652, nan, 112.473518)  (991.351196, nan, 242.351944)  (912.110779, nan, 247.647583)  (1374.76221, nan, 921.975952)  (1562.41113, nan, 307.5383)  (1919, nan, 1002.35126)  (1558.72034, nan, 227.92659)  (1172.66882, nan, 924.378784)  (997.015076, nan, 548.999084)  (1222.03552, nan, 670.318298)  (0, nan, 366.047974)  (1127.81384, nan, 659.600952)  (915.502625, nan, 280.810944)  (1224.13501, nan, 704.673523)  (493.637817, nan, 310.654327)  (1210.55847, nan, 434.617584)  (1919, nan, 862.671753)  (0, nan, 806.79187)  (1286.19629, nan, 0)  (0, nan, 252.333374)  (1861.97095, nan, 0)  (1919, nan, 45.7807159)  (633.146423, nan, 325.06778)  (872.836243, nan, 389.407623)  (291.714172, nan, 722.356689)  (1488.10352, nan, 352.136597)  (0, nan, 542.242798)  (975.008667, nan, 508.973022)  (803.167358, nan, 288.834045)  (668.622009, nan, 357.472992)  (0, nan, 913.786987)  (857.421326, nan, 169.13353)  (724.963318, nan, 590.894531)  (400.166168, nan, 463.640015)  (34.4196434, nan, 33.6221237)  (1919, nan, 928.740417)  (0, nan, 640.00415)  (0, nan, 679.783508)  (0, nan, 454.26828)  (316.300415, nan, 579.960999)  (0, nan, 688.375183)  (994.464905, nan, 131.898285)  (26.0745144, nan, 1079)  (1812.38757, nan, 39.6256371)  (1489.47314, nan, 414.123322)  (0, nan, 0)  (656.085815, nan, 832.436096)  (839.644165, nan, 623.525024)  (0, nan, 588.59906)  (0, nan, 602.842041)  (278.976685, nan, 0)  (475.680145, nan, 1079)  (1011.57214, nan, 633.986572)  (0, nan, 1079)  (0, nan, 181.93187)  (665.589355, nan, 580.017273)  (699.984436, nan, 539.432373)  (1919, nan, 33.1312332)  (610.045349, nan, 590.562073)  (1919, nan, 0)  (1161.79785, nan, 0)  (639.684448, nan, 1079)  (874.605835, nan, 344.686401)  (0, nan, 813.157227)  (1268.68591, nan, 1042.5282)  (792.723328, nan, 628.51947)  (1919, nan, 1079)  (588.800476, nan, 548.329346)  (1919, nan, 881.14624)  (1067.60815, nan, 459.074524)  (0, nan, 0)  (441.547607, nan, 986.507324)  (1202.60071, nan, 260.677216)  (279.293823, nan, 0)  (1919, nan, 956.649109)  (957.71106, nan, 135.675644)  (1244.81165, nan, 479.009216)  (0, nan, 857.301514)  (939.855469, nan, 70.7048645)  (1225.05847, nan, 743.077393)  (250.481171, nan, 673.428772)  (0, nan, 0)  (425.390411, nan, 303.174805)  (795.341797, nan, 439.68222)  (1918.58923, nan, 792.359741)  (1002.66675, nan, 64.7218246)  (965.0271, nan, 396.816132)  (0, nan, 132.343597)  (1898.02893, nan, 0)  (1919, nan, 647.999084)  (54.2473259, nan, 348.026642)  (813.511719, nan, 393.760345)  (299.257294, nan, 419.10022)  (54.3809166, nan, 999.42981)  (1822.82898, nan, 0)  (0, nan, 203.714661)  (759.069519, nan, 685.067688)  (34.6647339, nan, 491.057465)  (214.372635, nan, 356.323669)  (38.2170372, nan, 0)  (239.146957, nan, 725.140259)  (1919, nan, 1079)  (284.808136, nan, 1050.64282) 
I1005 12:09:23.974910  9979 MonoVisionImuFrontend.cpp:128] Keyframe 8 with: 304 smart measurements
I1005 12:09:23.974974  9979 MonoVisionImuFrontend.cpp:147] Reset IMU preintegration with latest IMU bias.
E1005 12:09:23.974983  9979 ImuFrontend.h:143] Reset preintegration with new bias:
acc = -0.012492
 0.547666
 0.069073 gyro = -0.002229
   0.0207
  0.07635
I1005 12:09:23.975021  9979 MonoVisionImuFrontend.cpp:154] Frontend output is a keyframe: pushing to output callbacks.
I1005 12:09:23.975049  9979 ThreadsafeQueue.h:286] Queue with id: visualizer_frontend_queue is getting full, size: 6
I1005 12:09:23.975059  9980 VioBackend-definitions.h:261] VioBackend Input Payload print:
Timestamp: 266666667
I1005 12:09:23.975065  9979 MonoVisionImuFrontend.cpp:91] ------------------- Processing frame k = 9--------------------
I1005 12:09:23.975077  9983 ThreadsafeQueue.h:308] Queue with id: frontend_input_queue is getting full, size: 5
I1005 12:09:23.975087  9980 VioBackend-definitions.h:264] Status smart stereo measurements: 
     Meas size: 304
I1005 12:09:23.975121  9980 VioBackend-definitions.h:268] Mono Tracking Status: VALID
I1005 12:09:23.975096  9979 MonoImuSyncPacket.cpp:35] Mono Frame timestamp: 300000000
STAMPS IMU rows : 
1
STAMPS IMU cols : 
2
STAMPS IMU: 
299999997 300000000
ACCGYR IMU rows : 
6
ACCGYR IMU cols : 
2
ACCGYR IMU: 
0002.17286 0002.17286
0-0.871902 0-0.871902
00-9.38828 00-9.38828
00.0289793 00.0289793
000.122968 000.122968
-0.0884932 -0.0884932
I1005 12:09:23.975123  9983 DataProviderModule.cpp:104] ////////////////////////////////////////// Creating packet!
STAMPS IMU rows : 
1
STAMPS IMU cols : 
2
STAMPS IMU: 
499999995 500000000
ACCGYR IMU rows : 
6
ACCGYR IMU cols : 
2
ACCGYR IMU: 
002.24895 002.24895
-0.824843 -0.824842
0-9.44292 0-9.44292
00.012231 00.012231
0.0225309 0.0225308
0.0271131 0.0271131

     Tracker Pose (mono):  R: [
    0.999999662, -1.04985297e-07, 5.68587777e-08;
    -1.07150461e-07, 0.999999796, -1.80163404e-07;
    5.67068867e-08, -1.84842633e-07, 0.999999666
]
t: -0.0650710792   0.993912699   0.088900509
I1005 12:09:23.975154  9979 ImuFrontend.cpp:108] Finished preintegration: 
I1005 12:09:23.975167  9980 VioBackend-definitions.h:274] Stereo Tracking Status: DISABLED
PIM type: 1
    deltaTij = 3e-09
    deltaRij.ypr = (
     Tracker Pose (stereo):  R: [
    1, 0, 0;
    0, 1, 0;
    0, 0, 1
]
t: -4.94529617e-10               0  3.06804351e-100                0
I1005 12:09:23.975201  9980 VioBackend-definitions.h:281] Stereo Tracking Status: DISABLED
9.36249754e-11)
    deltaPij = PIM : 
    deltaTij = 1.1e-08
    deltaRij.ypr = ( 9.83410544e-18 -6.38805464e-18 -4.25580825e-17
    deltaVij = 2.26248436e-10 2.56892331e-09 3.15700786e-11             ) 
    deltaPij =  6.55607029e-09 -4.25870309e-09  -2.8372055e-08
    gyrobias =  1.05399456e-16 -3.64970699e-17 -5.73418984e-16
    deltaVij =       -0.002229          0.0207         0.07635
    acc_bias =  2.00058271e-08 -6.87867231e-09 -1.03585339e-07
    gyrobias =       -0.012492        0.547666        0.069073

    preintMeasCov 
[              0               0               0
    acc_bias = 0 0 0

    preintMeasCov 
[ 3.16704326e-16 -9.14833583e-36 -1.54623409e-36  2.71313844e-51   4.7113933e-32 -3.03946651e-33  2.65705408e-43  1.31949042e-23 -9.18824828e-25
-9.14833684e-36  3.16704326e-16 -1.68589547e-35  -4.7113933e-32 -2.76518837e-51 -9.29622041e-33 -1.31949042e-23 -2.84603868e-43 -2.68688407e-24
   -1.54622378e-36             -1.68589562e-35  3.16704326e-16  8.63739072e-17 3.03946651e-33                -6.2026169e-379.29622041e-33   5.20510305e-53  9.99782537e-37 9.18824828e-25                         0                   02.68688407e-24                              0                1.88979005e-44
              02.71313816e-51   -4.7113933e-32               0        3.03946651e-33          1.10000176e-240 
-3.02153883e-44  -5.0995883e-45-6.20262762e-37         2.42e-228.63739072e-17   3.74193008e-41  3.27624524e-36 6.94917362e-40                             0
   4.7113933e-320  -2.76518717e-51              0   9.29622041e-33 0         -3.02153971e-44        1.10000176e-240  -5.47988967e-44               0
3.87652091e-41        2.42e-22 9.99786651e-37      -2.50110369e-40    
     -3.03946651e-33 -9.29622041e-333.27624511e-36   5.20507734e-53  8.63739072e-17 -5.09942396e-45               0-5.47988867e-44                01.10000176e-24                06.76703239e-40  -2.37392501e-40              0         2.42e-22
              0  2.65705258e-43               0-1.31949042e-23
  9.18824828e-25              0                   2.42e-22      3.87652152e-41               0  6.76703223e-40               04.4e-14   1.06796343e-32  3.00000027e-25  1.76985373e-31
 1.31949042e-23 -7.7556243e-52  -2.84603441e-43   1.2501083e-512.68688407e-24                        1.8e-23  3.74192953e-41        2.42e-22 -5.17041783e-43           -2.37392501e-40      1.06796343e-32         4.4e-14   8.3340651e-43          -6.28094944e-32
    -9.18824828e-25  
-2.68688407e-24 0                1.88981143e-44                           0    6.94917375e-40               0 -2.50110371e-40 -7.75562751e-52       2.42e-22   1.76985375e-31  3.00000027e-25 -6.28094944e-32         4.4e-14 4.09654389e-51               ]
I1005 12:09:23.975474  9980 VioBackend.cpp:358] Add visual inertial state and optimize.
I1005 12:09:23.975483  9980 RegularVioBackend.cpp:165] Processing keyframe 1 at timestamp: 0.266667 (nsec)
-5.17042203e-43         1.8e-23  2.73102916e-42
              0               0               0  1.25010844e-51  4.09654337e-51  3.00000027e-25  8.33405299e-43  2.73102894e-42         1.8e-23
              0               0               0         1.8e-23 -5.17040745e-43  8.33405237e-43         1.2e-14 -3.44694428e-34  5.55604602e-34
              0               0               0 -5.17042657e-43         1.8e-23  2.73102906e-42 -3.44694927e-34         1.2e-14  1.82068614e-33
              0               0               0  8.33406193e-43  2.73102916e-42         1.8e-23  5.55603635e-34  1.82068614e-33         1.2e-14]
I1005 12:09:23.975507  9980 RegularVioBackend.cpp:174] Adding IMU factor between pose id: 0 and pose id: 1
body_R_cam [
    -0.422086895, 0.152124643, 0.893700361;
    -0.890668988, 0.114157535, -0.440086991;
    -0.168970615, -0.981746376, 0.0873084813
]
camLrectLkf_R_camLrectK_imu [
    0.999999662, -1.05973355e-07, 5.73176014e-08;
    -1.06162403e-07, 0.999999796, -1.822738e-07;
    5.62480631e-08, -1.82732236e-07, 0.999999666
]
I1005 12:09:23.975539  9979 MonoVisionImuFrontend.cpp:112] Starting processFrame...
I1005 12:09:23.975546  9979 MonoVisionImuFrontend.cpp:215] ===================================================
processing frame: 9 at time 300000000 empirical framerate (sec): 0.0333333 (timestamp diff: 33333333)
I1005 12:09:23.975555  9979 MonoVisionImuFrontend.cpp:226] Starting feature tracking...
I1005 12:09:23.975567  9979 Tracker.cpp:113] Starting Optical Flow Pyr LK tracking...
I1005 12:09:23.975602  9980 RegularVioBackend.cpp:228] 
I1005 12:09:23.975598  9980 RegularVioBackend.cpp:221] Tracker has a VALID status.
I1005 12:09:23.975612  9980 RegularVioBackend.cpp:232] Starting extracting lmk ids from set of planes...
I1005 12:09:23.975620  9980 RegularVioBackend.cpp:278] Finished extracting lmk ids from set of planes, total of 0 lmks with regularities.
I1005 12:09:23.975627  9980 RegularVioBackend.cpp:283] Starting adding/updating landmarks to graph...
I1005 12:09:23.975638  9980 RegularVioBackend.cpp:447] Added 0 new landmarks.
Updated 0 landmarks in graph.
I1005 12:09:23.975646  9980 RegularVioBackend.cpp:285] Finished adding/updating landmarks to graph.
I1005 12:09:23.975653  9980 RegularVioBackend.cpp:358] Starting optimize...
I1005 12:09:23.975673  9980 VioBackend.cpp:1613]  =============== START:Smoother status before update: =============== 
I1005 12:09:23.975680  9980 VioBackend.cpp:1640] Nr of factors in graph (smoother getFactors): 3, with factors:
I1005 12:09:23.975689  9980 VioBackend.cpp:1642] [
    Slot # 1: Point Prior: point key    Factor  v0

I1005 12:09:23.975723  9980 VioBackend.cpp:1649]  ]
I1005 12:09:23.975729  9980 VioBackend.cpp:1652] Nr of new factors to add: 2 with factors:
I1005 12:09:23.975735  9980 VioBackend.cpp:1654] [
 (slot # wrt to new_factors_tmp graph)  

I1005 12:09:23.975749  9980 VioBackend.cpp:1661]  ]
I1005 12:09:23.975755  9980 VioBackend.cpp:1664] Nr deleted slots: 0, with slots:
I1005 12:09:23.975761  9980 VioBackend.cpp:1666] [

I1005 12:09:23.975770  9980 VioBackend.cpp:1693]  ]
I1005 12:09:23.975776  9980 VioBackend.cpp:1696] Nr of values in state_ : 3, with keys:
[
    b0 v0 x0 
I1005 12:09:23.975795  9980 VioBackend.cpp:1702]  ]
I1005 12:09:23.975802  9980 VioBackend.cpp:1705] Nr values in new_values_ : 3, with keys:
[
     b1  v1  x1 
I1005 12:09:23.975821  9980 VioBackend.cpp:1713]  ]
isam2 graph:
size: 3

Factor 0: PriorFactor on x0
  prior mean:  R: [
    0.380011666, 0.164691321, 0.910202412;
    0.0306791947, -0.985725343, 0.165547653;
    0.924473065, -0.0349858183, -0.379640249
]
t: 0.878612  2.14247 0.947262
  noise model: Gaussian[
    529.687875, -20.0431479, -217.493478, 0, 0, 0;
    0, 5.73367158, 0.0889688765, 0, 0, 0;
    0, 0, 6.19318078, 0, 0, 0;
    0, 0, 0, 100000, 0, 0;
    0, 0, 0, 0, 100000, 0;
    0, 0, 0, 0, 0, 100000
]

Factor 1: PriorFactor on v0
  prior mean: [
    0.009474;
    -0.014009;
    -0.002145
]
isotropic dim=3 sigma=0.001

Factor 2: PriorFactor on b0
  prior mean: acc = -0.012492
 0.547666
 0.069073 gyro = -0.002229
   0.0207
  0.07635
  noise model: diagonal sigmas[0.1; 0.1; 0.1; 0.01; 0.01; 0.01];

new_factors_tmp:
size: 2

Factor 0: 
ImuFactor(x0,v0,x1,v1,b0)
preintegrated measurements:

    deltaTij = 1.1e-08
    deltaRij.ypr = (2.26248436e-10 2.56892331e-09 3.15700786e-11)
    deltaPij =  1.05399456e-16 -3.64970699e-17 -5.73418984e-16
    deltaVij =  2.00058271e-08 -6.87867231e-09 -1.03585339e-07
    gyrobias = 0 0 0
    acc_bias = 0 0 0

    preintMeasCov 
[ 3.16704326e-16 -9.14833583e-36 -1.54623409e-36  2.71313844e-51   4.7113933e-32 -3.03946651e-33  2.65705408e-43  1.31949042e-23 -9.18824828e-25
-9.14833684e-36  3.16704326e-16 -1.68589547e-35  -4.7113933e-32 -2.76518837e-51 -9.29622041e-33 -1.31949042e-23 -2.84603868e-43 -2.68688407e-24
-1.54622378e-36 -1.68589562e-35  3.16704326e-16  3.03946651e-33  9.29622041e-33  5.20510305e-53  9.18824828e-25  2.68688407e-24  1.88979005e-44
 2.71313816e-51  -4.7113933e-32  3.03946651e-33  1.10000176e-24 -3.02153883e-44  -5.0995883e-45        2.42e-22  3.74193008e-41  6.94917362e-40
  4.7113933e-32 -2.76518717e-51  9.29622041e-33 -3.02153971e-44  1.10000176e-24 -5.47988967e-44  3.87652091e-41        2.42e-22 -2.50110369e-40
-3.03946651e-33 -9.29622041e-33  5.20507734e-53 -5.09942396e-45 -5.47988867e-44  1.10000176e-24  6.76703239e-40 -2.37392501e-40        2.42e-22
 2.65705258e-43 -1.31949042e-23  9.18824828e-25        2.42e-22  3.87652152e-41  6.76703223e-40         4.4e-14  1.06796343e-32  1.76985373e-31
 1.31949042e-23 -2.84603441e-43  2.68688407e-24  3.74192953e-41        2.42e-22 -2.37392501e-40  1.06796343e-32         4.4e-14 -6.28094944e-32
-9.18824828e-25 -2.68688407e-24  1.88981143e-44  6.94917375e-40 -2.50110371e-40        2.42e-22  1.76985375e-31 -6.28094944e-32         4.4e-14]
  noise model sigmas: 1.77961885e-08 1.77961885e-08 1.77961885e-08 1.04880969e-12 1.04880969e-12 1.04880969e-12  2.0976177e-07  2.0976177e-07  2.0976177e-07

Factor 1: BetweenFactor(b0,b1)
  measured: acc = 0
0
0 gyro = 0
0
0
  noise model: constrained sigmas[3.14642654e-07; 3.14642654e-07; 3.14642654e-07; 5.24404424e-10; 5.24404424e-10; 5.24404424e-10];
  noise model: constrained mu[1000; 1000; 1000; 1000; 1000; 1000];

new values:

Values with 3 values:
Value b1: (gtsam::imuBias::ConstantBias)
acc = -0.012492
 0.547666
 0.069073 gyro = -0.002229
   0.0207
  0.07635

Value v1: (Eigen::Matrix<double, 3, 1, 0, 3, 1>)
[
    0.00947391055;
    -0.0140090039;
    -0.00214504922
]

Value x1: (gtsam::Pose3)
R: [
    0.380011664, 0.164691321, 0.910202413;
    0.0306791949, -0.985725343, 0.165547654;
    0.924473066, -0.0349858177, -0.379640247
]
t: 0.878612  2.14247 0.947262
I1005 12:09:23.976219  9980 VioBackend.cpp:1724]  =============== END: Smoother status before update: =============== 
I1005 12:09:23.976228  9980 VioBackend.cpp:1010] iSAM2 update with 2 new factors , 3 new values , and 0 deleted factors.
I1005 12:09:23.976233  9980 VioBackend.cpp:1014] Starting first update.
I1005 12:09:23.976279  9980 VioBackend.cpp:1211] Starting update of smoother_...
I1005 12:09:23.976545  9980 VioBackend.cpp:1214] Finished update of smoother_.
I1005 12:09:23.976564  9980 VioBackend.cpp:1017] Finished first update.
I1005 12:09:23.976572  9980 VioBackend.cpp:1030] Clearing new_smart_factors_!
I1005 12:09:23.976577  9980 VioBackend.cpp:1042] Starting to find smart factors slots.
I1005 12:09:23.976583  9980 VioBackend.cpp:1045] Finished to find smart factors slots.
I1005 12:09:23.976588  9980 VioBackend.cpp:1172] Starting to calculate estimate.
I1005 12:09:23.976619  9980 VioBackend.cpp:1174] Finished to calculate estimate.
I1005 12:09:23.976625  9980 VioBackend.cpp:1187] Backend: Update IMU Bias.
I1005 12:09:23.976629  9980 ImuFrontend.h:127] Updating Preintegration imu bias (needs to reset integration for the bias to have effect):

acc = -0.012492
 0.547666
 0.069073 gyro = -0.002229
   0.0207
  0.07635
I1005 12:09:23.976672  9982 ThreadsafeQueue.h:286] Queue with id: data_provider_left_frame_queue is getting full, size: 17
I1005 12:09:23.976689  9982 EurocDataProvider.cpp:858] Finished VIO processing for frame k = 32
I1005 12:09:23.976697  9982 EurocDataProvider.cpp:832] Sending left frame k= 33 with timestamp: 1100000000
terminate called after throwing an instance of 'std::invalid_argument'
  what():  JacobianFactor::updateHessian: cannot update information with constrained noise model
./scripts/monoVIOEuroc.bash: line 110:  9971 Aborted                 (core dumped) $BUILD_PATH/stereoVIOEuroc --dataset_type="$DATASET_TYPE" --dataset_path="$DATASET_PATH" --initial_k=1 --final_k=100000 --params_folder_path="$PARAMS_PATH" --use_lcd="$USE_LCD" --vocabulary_path="$VOCABULARY_PATH/ORBvoc.yml" --flagfile="$PARAMS_PATH/flags/stereoVIOEuroc.flags" --flagfile="$PARAMS_PATH/flags/Mesher.flags" --flagfile="$PARAMS_PATH/flags/VioBackend.flags" --flagfile="$PARAMS_PATH/flags/RegularVioBackend.flags" --flagfile="$PARAMS_PATH/flags/Visualizer3D.flags" --logtostderr=1 --colorlogtostderr=1 --log_prefix=1 --v=10 --vmodule=Pipeline*=00 --log_output="$LOG_OUTPUT" --log_euroc_gt_data="$LOG_OUTPUT" --save_frontend_images=1 --visualize_frontend_images=1 --output_path="$OUTPUT_PATH"

Additional files: Here is the link to the data folder: https://nextcloud.mpi-klsb.mpg.de/index.php/s/Gzf8rF3j5gNDiAs Here I uploaded the data itself (mpi_kimera.tar.gz), Kimera params (mpi_kimera_params.tar.gz), script that I used for launching (mpi_kimera_launch_script.sh, which was located at Kimera-VIO/scripts folder) and full startup log (kimera_sequential.log)

Please give also the following information:

marcusabate commented 1 year ago

It's unusual to see that many points out-of-frame from the UndistorterRectifier. If you are still working on this, I think it is probably a calibration issue. Intrinsic most likely.