open_vins Estimation diverges on Xavier NX with EuRoc dataset

Estimation diverges on Xavier NX with EuRoc dataset

Open mzahana opened this issue 1 year ago • 16 comments

I have Jetson Xavier NX with a ROS2 hubmle docker container. I cloned this repo and built it using colcon build. the build took some time, but it was successful.

I downloaded one of the EuROC datasets , V1_01_easy. Then, I ran the estimator using ros2 launch ov_msckf subscribe.launch.py config:=euroc_mav. Then, I played the bag using ros2 bag play V1_01_easy.

I visulaized the odomimu in RViz2, and I see the estimator diverges quickly.

The same process works fine on my desktop PC.

What could be the issue? Xavier NX is not powerful enough ?

@goldbattle any hints?

Thanks in advance.

Jun 25 '23 00:06 mzahana

I am not sure, and it is difficult for me to debug for you. I can recommend increasing the verbosity and seeing if there are any issues, and also inspecting the feature tracking by looking at the published images. Do you see that the estimator was able to successfully estimate, are features being extracted, and is the system publishing at full rate (use ros2 topic hz I believe to see this)?

I know that on the Jetson Nano with 5W mode the system struggles / fails, but on non-low power modes code works as expected. You also might see performance issues by running it inside docker, so you might want to run it directly on the system itself. Hope this helps.

EDIT: also I have not tested on humble myself, but this typically isn't an issue unless the linked libraries have had large changes.

Jun 25 '23 01:06 goldbattle

Thanks @goldbattle for the feedback. I am not sure if this helps, but here is some screen output

run_subscribe_msckf-1] subscribing to IMU: /imu0
[run_subscribe_msckf-1] subscribing to cam (stereo): /cam0/image_raw
[run_subscribe_msckf-1] subscribing to cam (stereo): /cam1/image_raw
[run_subscribe_msckf-1] [TIME]: 0.0581 seconds total (17.2 hz, 2.50 ms behind)
[run_subscribe_msckf-1] [TIME]: 0.0488 seconds total (20.5 hz, 2.50 ms behind)
[run_subscribe_msckf-1] [init]: not enough feats to compute disp: 0,0 < 15
[run_subscribe_msckf-1] [TIME]: 0.0367 seconds total (27.2 hz, 2.50 ms behind)
[run_subscribe_msckf-1] [init]: not enough feats to compute disp: 0,0 < 15
[run_subscribe_msckf-1] [TIME]: 0.0268 seconds total (37.4 hz, 1.50 ms behind)
[run_subscribe_msckf-1] [init]: not enough feats to compute disp: 0,81 < 15
[run_subscribe_msckf-1] [TIME]: 0.0145 seconds total (69.0 hz, 1.00 ms behind)
[run_subscribe_msckf-1] [init]: not enough feats to compute disp: 0,81 < 15
[run_subscribe_msckf-1] [TIME]: 0.0380 seconds total (26.3 hz, 2.50 ms behind)
[run_subscribe_msckf-1] [init]: not enough feats to compute disp: 0,81 < 15
[run_subscribe_msckf-1] [TIME]: 0.0189 seconds total (53.0 hz, 2.00 ms behind)
[run_subscribe_msckf-1] [init]: not enough feats to compute disp: 0,81 < 15
[run_subscribe_msckf-1] [TIME]: 0.0111 seconds total (89.9 hz, 3.50 ms behind)
[run_subscribe_msckf-1] [init]: not enough feats to compute disp: 0,81 < 15
[run_subscribe_msckf-1] [TIME]: 0.0376 seconds total (26.6 hz, 1.50 ms behind)
[run_subscribe_msckf-1] [init]: not enough feats to compute disp: 0,81 < 15
[run_subscribe_msckf-1] [TIME]: 0.0379 seconds total (26.4 hz, 0.50 ms behind)
[run_subscribe_msckf-1] [init]: not enough feats to compute disp: 0,81 < 15
[run_subscribe_msckf-1] [TIME]: 0.0158 seconds total (63.4 hz, 0.50 ms behind)
[run_subscribe_msckf-1] [init]: not enough feats to compute disp: 0,81 < 15
[run_subscribe_msckf-1] [TIME]: 0.0126 seconds total (79.4 hz, 1.50 ms behind)
[run_subscribe_msckf-1] [init]: not enough feats to compute disp: 0,81 < 15
[run_subscribe_msckf-1] [TIME]: 0.0118 seconds total (84.7 hz, 0.50 ms behind)
[run_subscribe_msckf-1] [init]: not enough feats to compute disp: 0,81 < 15
[run_subscribe_msckf-1] [TIME]: 0.0146 seconds total (68.4 hz, 0.50 ms behind)
[run_subscribe_msckf-1] [init]: not enough feats to compute disp: 0,81 < 15
[run_subscribe_msckf-1] [TIME]: 0.0132 seconds total (75.8 hz, 0.50 ms behind)
[run_subscribe_msckf-1] [init]: not enough feats to compute disp: 0,81 < 15
[run_subscribe_msckf-1] [TIME]: 0.0094 seconds total (105.9 hz, 2.50 ms behind)
[run_subscribe_msckf-1] [init]: not enough feats to compute disp: 0,81 < 15
[run_subscribe_msckf-1] [TIME]: 0.0271 seconds total (36.9 hz, 0.50 ms behind)
[run_subscribe_msckf-1] [init]: not enough feats to compute disp: 0,81 < 15
[run_subscribe_msckf-1] [TIME]: 0.0238 seconds total (42.1 hz, 2.00 ms behind)
[run_subscribe_msckf-1] [init]: not enough feats to compute disp: 0,81 < 15
[run_subscribe_msckf-1] [TIME]: 0.0194 seconds total (51.5 hz, 2.00 ms behind)
[run_subscribe_msckf-1] [init]: not enough feats to compute disp: 0,81 < 15
[run_subscribe_msckf-1] [TIME]: 0.0113 seconds total (88.2 hz, 0.50 ms behind)
[run_subscribe_msckf-1] [init]: not enough feats to compute disp: 0,81 < 15
[run_subscribe_msckf-1] [TIME]: 0.0131 seconds total (76.1 hz, 0.50 ms behind)
[run_subscribe_msckf-1] [init]: not enough feats to compute disp: 0,81 < 15
[run_subscribe_msckf-1] [TIME]: 0.0107 seconds total (93.5 hz, 0.50 ms behind)
[run_subscribe_msckf-1] [init]: not enough feats to compute disp: 0,81 < 15
[run_subscribe_msckf-1] [TIME]: 0.0172 seconds total (58.3 hz, 1.00 ms behind)
[run_subscribe_msckf-1] [init]: not enough feats to compute disp: 0,81 < 15
[run_subscribe_msckf-1] [TIME]: 0.0108 seconds total (92.5 hz, 0.50 ms behind)
[run_subscribe_msckf-1] [init]: disparity is 0.108,0.138 (10.00 thresh)
[run_subscribe_msckf-1] [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] [TIME]: 0.0125 seconds total (80.1 hz, 0.50 ms behind)
[run_subscribe_msckf-1] [init]: disparity is 0.225,0.057 (10.00 thresh)
[run_subscribe_msckf-1] [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] [TIME]: 0.0263 seconds total (38.0 hz, 0.50 ms behind)
[run_subscribe_msckf-1] [init]: disparity is 0.315,0.089 (10.00 thresh)
[run_subscribe_msckf-1] [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] [TIME]: 0.0213 seconds total (47.0 hz, 3.00 ms behind)
[run_subscribe_msckf-1] [init]: disparity is 0.374,0.143 (10.00 thresh)
[run_subscribe_msckf-1] [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] [TIME]: 0.0232 seconds total (43.1 hz, 0.50 ms behind)
[run_subscribe_msckf-1] [init]: disparity is 0.410,0.138 (10.00 thresh)
[run_subscribe_msckf-1] [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] [TIME]: 0.0214 seconds total (46.7 hz, 0.50 ms behind)
[run_subscribe_msckf-1] [init]: disparity is 0.458,0.199 (10.00 thresh)
[run_subscribe_msckf-1] [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] [TIME]: 0.0107 seconds total (93.7 hz, 1.00 ms behind)
[run_subscribe_msckf-1] [init]: disparity is 0.430,0.266 (10.00 thresh)
[run_subscribe_msckf-1] [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] [TIME]: 0.0106 seconds total (94.2 hz, 0.50 ms behind)
[run_subscribe_msckf-1] [init]: disparity is 0.379,0.763 (10.00 thresh)
[run_subscribe_msckf-1] [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] [TIME]: 0.0218 seconds total (45.9 hz, 0.50 ms behind)
[run_subscribe_msckf-1] [init]: disparity is 0.421,0.884 (10.00 thresh)
[run_subscribe_msckf-1] [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] [TIME]: 0.0096 seconds total (104.2 hz, 9.50 ms behind)
[run_subscribe_msckf-1] [init]: disparity is 0.405,0.717 (10.00 thresh)
[run_subscribe_msckf-1] [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] [TIME]: 0.0116 seconds total (85.9 hz, 4.50 ms behind)
[run_subscribe_msckf-1] [init]: disparity is 0.368,0.387 (10.00 thresh)
[run_subscribe_msckf-1] [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] [TIME]: 0.0224 seconds total (44.7 hz, 5.00 ms behind)
[run_subscribe_msckf-1] [init]: disparity is 0.358,0.473 (10.00 thresh)
[run_subscribe_msckf-1] [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] [TIME]: 0.0196 seconds total (51.1 hz, 6.50 ms behind)
[run_subscribe_msckf-1] [init]: disparity is 0.397,0.412 (10.00 thresh)
[run_subscribe_msckf-1] [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] [TIME]: 0.0190 seconds total (52.7 hz, 1.50 ms behind)
[run_subscribe_msckf-1] [init]: disparity is 0.376,0.397 (10.00 thresh)
[run_subscribe_msckf-1] [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] [TIME]: 0.0245 seconds total (40.8 hz, 0.50 ms behind)
[run_subscribe_msckf-1] [init]: disparity is 0.379,0.350 (10.00 thresh)
[run_subscribe_msckf-1] [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] [TIME]: 0.0130 seconds total (76.7 hz, 5.00 ms behind)
[run_subscribe_msckf-1] [init]: disparity is 0.353,0.304 (10.00 thresh)
[run_subscribe_msckf-1] [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] [TIME]: 0.0089 seconds total (111.9 hz, 5.00 ms behind)
[run_subscribe_msckf-1] [init]: disparity is 0.334,0.294 (10.00 thresh)
[run_subscribe_msckf-1] [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] [TIME]: 0.0124 seconds total (80.6 hz, 5.00 ms behind)
[run_subscribe_msckf-1] [init]: disparity is 0.347,0.307 (10.00 thresh)
[run_subscribe_msckf-1] [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] [TIME]: 0.0170 seconds total (58.8 hz, 5.00 ms behind)
[run_subscribe_msckf-1] [init]: disparity is 0.345,0.672 (10.00 thresh)
[run_subscribe_msckf-1] [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] [TIME]: 0.0177 seconds total (56.4 hz, 5.00 ms behind)
[run_subscribe_msckf-1] [init]: disparity is 0.377,0.620 (10.00 thresh)
[run_subscribe_msckf-1] [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] [TIME]: 0.0103 seconds total (97.0 hz, 5.00 ms behind)
[run_subscribe_msckf-1] [init]: disparity is 0.374,0.458 (10.00 thresh)
[run_subscribe_msckf-1] [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] [TIME]: 0.0138 seconds total (72.6 hz, 4.00 ms behind)

I run the Xaviern NX at the max MODE with 20W and 6 CORES
The frequency of topic /ov_msckf/trackhist is around 6Hz
The frequency of /ov_msckf/odomimu is at max of ~120Hz
The frequency of /ov_msckf/poseimu is ~ 20Hz
OpenCV version is 4.5.0
As I mentioned, I ran the same system inside a docker container with ROS humble on normal PC, and it worked fine.

I can record a ros 2 bag for debugging. What is the minimum set of topics to record ?

Thanks again for your assistance.

Jun 25 '23 08:06 mzahana

Here is a ros 2 bag

Jun 25 '23 09:06 mzahana

What you have printed looks ok. Can you try the DEBUG verbosity level? Publishing of trackhist seems to be a bit slow. Are you recording this over a network? If so, check the /compressed topic instead as sending the full image over will give lower frequency / not full rate.

Looking at ros2 topic hz /ov_msckf/poseimu -w 100 it seems things are only being processed at 10Hz, so likely the feature tracking or something is too slow on the platform. You can try just running 1 camera, or reducing the number of features to see if this provides any insight.

Jun 25 '23 20:06 goldbattle

Thank you very much @goldbattle for taking the time to check my ros bag.

I recorded the bag on the same platform, not over a network.

If it worked OK with you on a jetson nano, then it makes sense that it should work on Xavier NX as it has more compute power. Also, I doubt that running the system inside docker is the main bottleneck. I have used more complex software inside docker containers with no issues.

I think feature tracking has some issues as it seems to be running at low speed, but I am not sure why. I will try to play with the number of features and number of cameras.

What number of features did you use on the Nano?

Jun 25 '23 22:06 mzahana

I set verbosity:=DEBUG and I get the following screen output

[run_subscribe_msckf-1] ROS2Visualizer.cpp:49 Publishing: /ov_msckf/poseimu
[run_subscribe_msckf-1] ROS2Visualizer.cpp:51 Publishing: /ov_msckf/odomimu
[run_subscribe_msckf-1] ROS2Visualizer.cpp:53 Publishing: /ov_msckf/pathimu
[run_subscribe_msckf-1] ROS2Visualizer.cpp:57 Publishing: /ov_msckf/points_msckf
[run_subscribe_msckf-1] ROS2Visualizer.cpp:59 Publishing: /ov_msckf/points_msckf
[run_subscribe_msckf-1] ROS2Visualizer.cpp:61 Publishing: /ov_msckf/points_aruco
[run_subscribe_msckf-1] ROS2Visualizer.cpp:63 Publishing: /ov_msckf/points_sim
[run_subscribe_msckf-1] ROS2Visualizer.cpp:67 Publishing: /ov_msckf/trackhist
[run_subscribe_msckf-1] ROS2Visualizer.cpp:71 Publishing: /ov_msckf/posegt
[run_subscribe_msckf-1] ROS2Visualizer.cpp:73 Publishing: /ov_msckf/pathgt
[run_subscribe_msckf-1] ROS2Visualizer.cpp:175 subscribing to IMU: /imu0
[run_subscribe_msckf-1] ROS2Visualizer.cpp:200 subscribing to cam (stereo): /cam0/image_raw
[run_subscribe_msckf-1] ROS2Visualizer.cpp:201 subscribing to cam (stereo): /cam1/image_raw
[run_subscribe_msckf-1] run_subscribe_msckf.cpp:102 done...spinning to ros
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0394 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0411 seconds total (24.3 hz, 10.00 ms behind)
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0000 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0250 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0253 seconds total (39.6 hz, 5.00 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:117 [init]: not enough feats to compute disp: 0,0 < 15
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0002 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0339 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0342 seconds total (29.3 hz, 7.50 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:117 [init]: not enough feats to compute disp: 0,0 < 15
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0003 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0212 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0215 seconds total (46.6 hz, 2.50 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:117 [init]: not enough feats to compute disp: 0,76 < 15
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0003 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0174 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0176 seconds total (56.9 hz, 2.50 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:117 [init]: not enough feats to compute disp: 0,76 < 15
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0003 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0112 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0114 seconds total (87.4 hz, 3.50 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:117 [init]: not enough feats to compute disp: 0,76 < 15
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0003 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0202 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0205 seconds total (48.9 hz, 5.00 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:117 [init]: not enough feats to compute disp: 0,76 < 15
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0003 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0147 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0149 seconds total (67.0 hz, 7.50 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:117 [init]: not enough feats to compute disp: 0,76 < 15
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0003 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0116 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0119 seconds total (84.3 hz, 2.50 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:117 [init]: not enough feats to compute disp: 0,76 < 15
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0002 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0184 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0186 seconds total (53.7 hz, 0.50 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:117 [init]: not enough feats to compute disp: 0,76 < 15
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0004 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0166 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0169 seconds total (59.3 hz, 2.50 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:117 [init]: not enough feats to compute disp: 0,76 < 15
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0006 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0178 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0180 seconds total (55.5 hz, 0.50 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:117 [init]: not enough feats to compute disp: 0,76 < 15
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0004 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0139 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0141 seconds total (70.8 hz, 0.50 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:117 [init]: not enough feats to compute disp: 0,76 < 15
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0003 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0166 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0168 seconds total (59.5 hz, 0.50 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:117 [init]: not enough feats to compute disp: 0,76 < 15
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0004 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0100 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0103 seconds total (97.4 hz, 0.50 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:117 [init]: not enough feats to compute disp: 0,76 < 15
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0003 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0174 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0177 seconds total (56.6 hz, 0.50 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:117 [init]: not enough feats to compute disp: 0,76 < 15
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0004 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0103 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0106 seconds total (94.6 hz, 1.00 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:117 [init]: not enough feats to compute disp: 0,76 < 15
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0003 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0151 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0153 seconds total (65.2 hz, 0.50 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:117 [init]: not enough feats to compute disp: 0,76 < 15
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0004 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0110 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0112 seconds total (89.0 hz, 0.50 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:117 [init]: not enough feats to compute disp: 0,76 < 15
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0004 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0230 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0233 seconds total (42.9 hz, 0.50 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:117 [init]: not enough feats to compute disp: 0,76 < 15
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0004 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0169 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0171 seconds total (58.4 hz, 0.50 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:117 [init]: not enough feats to compute disp: 0,76 < 15
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0004 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0214 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0224 seconds total (44.7 hz, 1.50 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:117 [init]: not enough feats to compute disp: 0,76 < 15
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0004 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0101 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0103 seconds total (97.5 hz, 0.50 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:117 [init]: not enough feats to compute disp: 0,76 < 15
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0004 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0179 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0181 seconds total (55.2 hz, 0.50 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:122 [init]: disparity is 0.024,0.332 (10.00 thresh)
[run_subscribe_msckf-1] InertialInitializer.cpp:144 [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0026 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0104 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0107 seconds total (93.3 hz, 1.00 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:122 [init]: disparity is 0.023,0.367 (10.00 thresh)
[run_subscribe_msckf-1] InertialInitializer.cpp:144 [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0005 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0149 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0151 seconds total (66.0 hz, 1.50 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:122 [init]: disparity is 0.026,0.324 (10.00 thresh)
[run_subscribe_msckf-1] InertialInitializer.cpp:144 [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0005 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0112 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0115 seconds total (87.2 hz, 0.50 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:122 [init]: disparity is 0.026,0.443 (10.00 thresh)
[run_subscribe_msckf-1] InertialInitializer.cpp:144 [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0005 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0172 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0175 seconds total (57.1 hz, 0.50 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:122 [init]: disparity is 0.106,0.511 (10.00 thresh)
[run_subscribe_msckf-1] InertialInitializer.cpp:144 [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0005 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0158 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0160 seconds total (62.5 hz, 1.50 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:122 [init]: disparity is 0.255,0.695 (10.00 thresh)
[run_subscribe_msckf-1] InertialInitializer.cpp:144 [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0005 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0163 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0165 seconds total (60.5 hz, 0.50 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:122 [init]: disparity is 0.206,0.435 (10.00 thresh)
[run_subscribe_msckf-1] InertialInitializer.cpp:144 [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0006 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0129 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0132 seconds total (75.9 hz, 0.50 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:122 [init]: disparity is 0.410,0.261 (10.00 thresh)
[run_subscribe_msckf-1] InertialInitializer.cpp:144 [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0005 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0199 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0201 seconds total (49.6 hz, 0.50 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:122 [init]: disparity is 0.301,0.072 (10.00 thresh)
[run_subscribe_msckf-1] InertialInitializer.cpp:144 [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0005 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0214 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0219 seconds total (45.6 hz, 0.50 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:122 [init]: disparity is 0.154,0.108 (10.00 thresh)
[run_subscribe_msckf-1] InertialInitializer.cpp:144 [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0006 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0148 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0150 seconds total (66.6 hz, 0.50 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:122 [init]: disparity is 0.071,0.219 (10.00 thresh)
[run_subscribe_msckf-1] InertialInitializer.cpp:144 [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0005 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0103 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0105 seconds total (95.4 hz, 0.50 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:122 [init]: disparity is 0.083,0.975 (10.00 thresh)
[run_subscribe_msckf-1] InertialInitializer.cpp:144 [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0006 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0220 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0222 seconds total (45.0 hz, 0.50 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:122 [init]: disparity is 0.266,0.420 (10.00 thresh)
[run_subscribe_msckf-1] InertialInitializer.cpp:144 [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0006 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0100 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0102 seconds total (97.7 hz, 0.50 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:122 [init]: disparity is 1.026,0.350 (10.00 thresh)
[run_subscribe_msckf-1] InertialInitializer.cpp:144 [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0007 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0197 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0199 seconds total (50.2 hz, 5.00 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:122 [init]: disparity is 0.444,0.226 (10.00 thresh)
[run_subscribe_msckf-1] InertialInitializer.cpp:144 [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0006 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0124 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0127 seconds total (79.0 hz, 5.00 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:122 [init]: disparity is 0.406,0.512 (10.00 thresh)
[run_subscribe_msckf-1] InertialInitializer.cpp:144 [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0006 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0157 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0159 seconds total (63.0 hz, 5.00 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:122 [init]: disparity is 0.232,0.377 (10.00 thresh)
[run_subscribe_msckf-1] InertialInitializer.cpp:144 [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0007 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0107 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0110 seconds total (90.9 hz, 5.00 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:122 [init]: disparity is 0.513,0.489 (10.00 thresh)
[run_subscribe_msckf-1] InertialInitializer.cpp:144 [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0006 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0194 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0197 seconds total (50.8 hz, 5.00 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:122 [init]: disparity is 0.371,0.354 (10.00 thresh)
[run_subscribe_msckf-1] InertialInitializer.cpp:144 [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0007 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0140 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0143 seconds total (70.1 hz, 5.00 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:122 [init]: disparity is 0.444,0.398 (10.00 thresh)
[run_subscribe_msckf-1] InertialInitializer.cpp:144 [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0007 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0220 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0222 seconds total (45.0 hz, 5.00 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:122 [init]: disparity is 0.337,0.319 (10.00 thresh)
[run_subscribe_msckf-1] InertialInitializer.cpp:144 [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0029 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0227 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0230 seconds total (43.5 hz, 5.00 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:122 [init]: disparity is 0.366,0.256 (10.00 thresh)
[run_subscribe_msckf-1] InertialInitializer.cpp:144 [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0008 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0137 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0139 seconds total (71.7 hz, 5.00 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:122 [init]: disparity is 0.313,0.349 (10.00 thresh)
[run_subscribe_msckf-1] InertialInitializer.cpp:144 [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0008 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0140 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0142 seconds total (70.5 hz, 5.00 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:122 [init]: disparity is 0.266,0.326 (10.00 thresh)
[run_subscribe_msckf-1] InertialInitializer.cpp:144 [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0007 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0101 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0103 seconds total (96.9 hz, 5.00 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:122 [init]: disparity is 0.420,0.213 (10.00 thresh)
[run_subscribe_msckf-1] InertialInitializer.cpp:144 [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0008 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0148 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0150 seconds total (66.6 hz, 5.00 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:122 [init]: disparity is 0.563,0.147 (10.00 thresh)
[run_subscribe_msckf-1] InertialInitializer.cpp:144 [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0010 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0105 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0107 seconds total (93.2 hz, 4.00 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:122 [init]: disparity is 0.348,0.059 (10.00 thresh)
[run_subscribe_msckf-1] InertialInitializer.cpp:144 [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0021 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0203 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0205 seconds total (48.7 hz, 5.00 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:122 [init]: disparity is 0.517,0.090 (10.00 thresh)
[run_subscribe_msckf-1] InertialInitializer.cpp:144 [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0008 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0108 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0110 seconds total (90.9 hz, 5.00 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:122 [init]: disparity is 0.296,0.145 (10.00 thresh)
[run_subscribe_msckf-1] InertialInitializer.cpp:144 [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0008 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0173 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0176 seconds total (56.7 hz, 5.00 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:122 [init]: disparity is 0.105,0.137 (10.00 thresh)
[run_subscribe_msckf-1] InertialInitializer.cpp:144 [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0007 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0136 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0138 seconds total (72.6 hz, 4.00 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:122 [init]: disparity is 0.091,0.203 (10.00 thresh)
[run_subscribe_msckf-1] InertialInitializer.cpp:144 [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0007 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0149 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0152 seconds total (66.0 hz, 0.50 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:122 [init]: disparity is 0.129,0.272 (10.00 thresh)
[run_subscribe_msckf-1] InertialInitializer.cpp:144 [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0007 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0099 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0102 seconds total (98.3 hz, 0.50 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:122 [init]: disparity is 0.241,0.764 (10.00 thresh)
[run_subscribe_msckf-1] InertialInitializer.cpp:144 [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0008 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0243 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0249 seconds total (40.2 hz, 0.50 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:122 [init]: disparity is 0.971,0.875 (10.00 thresh)
[run_subscribe_msckf-1] InertialInitializer.cpp:144 [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0014 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0106 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0109 seconds total (91.9 hz, 0.50 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:122 [init]: disparity is 0.413,0.710 (10.00 thresh)
[run_subscribe_msckf-1] InertialInitializer.cpp:144 [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0007 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0145 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0148 seconds total (67.7 hz, 0.50 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:122 [init]: disparity is 0.402,0.379 (10.00 thresh)
[run_subscribe_msckf-1] InertialInitializer.cpp:144 [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0008 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0106 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0108 seconds total (92.3 hz, 0.50 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:122 [init]: disparity is 0.252,0.462 (10.00 thresh)
[run_subscribe_msckf-1] InertialInitializer.cpp:144 [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0008 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0180 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0183 seconds total (54.8 hz, 0.50 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:122 [init]: disparity is 0.529,0.409 (10.00 thresh)
[run_subscribe_msckf-1] InertialInitializer.cpp:144 [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0009 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0126 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0128 seconds total (78.1 hz, 0.50 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:122 [init]: disparity is 0.373,0.388 (10.00 thresh)
[run_subscribe_msckf-1] InertialInitializer.cpp:144 [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0008 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0265 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0267 seconds total (37.4 hz, 2.50 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:122 [init]: disparity is 0.468,0.332 (10.00 thresh)
[run_subscribe_msckf-1] InertialInitializer.cpp:144 [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0007 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0113 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0115 seconds total (86.8 hz, 2.00 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:122 [init]: disparity is 0.339,0.291 (10.00 thresh)
[run_subscribe_msckf-1] InertialInitializer.cpp:144 [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0007 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0155 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0158 seconds total (63.4 hz, 0.50 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:122 [init]: disparity is 0.359,0.274 (10.00 thresh)
[run_subscribe_msckf-1] InertialInitializer.cpp:144 [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0008 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0098 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0100 seconds total (100.0 hz, 0.50 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:122 [init]: disparity is 0.327,0.301 (10.00 thresh)
[run_subscribe_msckf-1] InertialInitializer.cpp:144 [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0007 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0197 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0199 seconds total (50.2 hz, 0.50 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:122 [init]: disparity is 0.266,0.664 (10.00 thresh)
[run_subscribe_msckf-1] InertialInitializer.cpp:144 [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0008 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0150 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0152 seconds total (65.7 hz, 0.50 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:122 [init]: disparity is 0.385,0.616 (10.00 thresh)
[run_subscribe_msckf-1] InertialInitializer.cpp:144 [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0008 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0336 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0339 seconds total (29.5 hz, 0.50 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:122 [init]: disparity is 0.381,0.456 (10.00 thresh)
[run_subscribe_msckf-1] InertialInitializer.cpp:144 [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0009 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0113 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0128 seconds total (78.3 hz, 0.50 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:122 [init]: disparity is 0.221,0.457 (10.00 thresh)
[run_subscribe_msckf-1] InertialInitializer.cpp:144 [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0018 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0228 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0230 seconds total (43.5 hz, 0.50 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:122 [init]: disparity is 0.116,0.509 (10.00 thresh)
[run_subscribe_msckf-1] InertialInitializer.cpp:144 [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0008 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0133 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0136 seconds total (73.7 hz, 0.50 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:122 [init]: disparity is 0.078,0.597 (10.00 thresh)
[run_subscribe_msckf-1] InertialInitializer.cpp:144 [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0008 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0288 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0290 seconds total (34.5 hz, 0.50 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:122 [init]: disparity is 0.143,0.359 (10.00 thresh)
[run_subscribe_msckf-1] InertialInitializer.cpp:144 [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0016 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0107 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0109 seconds total (91.8 hz, 0.50 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:122 [init]: disparity is 0.153,0.135 (10.00 thresh)
[run_subscribe_msckf-1] InertialInitializer.cpp:144 [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0007 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0181 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0183 seconds total (54.6 hz, 0.50 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:122 [init]: disparity is 0.251,0.181 (10.00 thresh)
[run_subscribe_msckf-1] InertialInitializer.cpp:144 [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0008 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0106 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0108 seconds total (92.6 hz, 1.00 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:122 [init]: disparity is 0.806,0.582 (10.00 thresh)
[run_subscribe_msckf-1] InertialInitializer.cpp:144 [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0008 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0146 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0148 seconds total (67.6 hz, 1.00 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:122 [init]: disparity is 0.862,0.937 (10.00 thresh)
[run_subscribe_msckf-1] InertialInitializer.cpp:144 [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0008 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0127 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0129 seconds total (77.5 hz, 1.00 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:122 [init]: disparity is 0.661,0.656 (10.00 thresh)
[run_subscribe_msckf-1] InertialInitializer.cpp:144 [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0008 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0225 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0228 seconds total (43.9 hz, 1.00 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:122 [init]: disparity is 0.406,0.829 (10.00 thresh)
[run_subscribe_msckf-1] InertialInitializer.cpp:144 [init]: failed static init: no accel jerk detected
[run_subscribe_msckf-1] VioManagerHelper.cpp:170 [init]: failed initialization in 0.0008 seconds
[run_subscribe_msckf-1] VioManager.cpp:314 [TIME]: 0.0163 seconds for tracking
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0166 seconds total (60.3 hz, 6.50 ms behind)
[run_subscribe_msckf-1] InertialInitializer.cpp:122 [init]: disparity is 0.507,0.909 (10.00 thresh)
[run_subscribe_msckf-1] InertialInitializer.cpp:144 [init]: failed static init: no accel jerk detected

.
.
.
.

[run_subscribe_msckf-1] VioManager.cpp:615 [TIME]: 0.0001 seconds for MSCKF update (0 feats)
[run_subscribe_msckf-1] VioManager.cpp:617 [TIME]: 0.0129 seconds for SLAM update (48 feats)
[run_subscribe_msckf-1] VioManager.cpp:618 [TIME]: 0.0000 seconds for SLAM delayed init (0 feats)
[run_subscribe_msckf-1] VioManager.cpp:620 [TIME]: 0.0066 seconds for re-tri & marg (11 clones in state)
[run_subscribe_msckf-1] VioManager.cpp:628 [TIME]: 0.04454 seconds for total (camera 0 1)
[run_subscribe_msckf-1] VioManager.cpp:654 q_GtoI = -0.815,0.007,-0.579,0.000 | p_IinG = 0.083,0.040,0.140 | dist = 0.11 (meters)
[run_subscribe_msckf-1] VioManager.cpp:657 bg = -0.0029,0.0273,0.0733 | ba = -0.0905,-0.0072,0.0453
[run_subscribe_msckf-1] VioManager.cpp:662 camera-imu timeoffset = -0.00974
[run_subscribe_msckf-1] VioManager.cpp:669 cam0 intrinsics = 458.865,456.166,368.421,247.418 | -0.284,0.064,0.007,0.005
[run_subscribe_msckf-1] VioManager.cpp:669 cam1 intrinsics = 458.385,455.713,381.076,255.615 | -0.284,0.063,0.006,0.005
[run_subscribe_msckf-1] VioManager.cpp:678 cam0 extrinsics = -0.002,0.006,0.706,0.708 | 0.066,-0.018,0.057
[run_subscribe_msckf-1] VioManager.cpp:678 cam1 extrinsics = 0.002,0.010,0.707,0.707 | -0.032,-0.016,0.058
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0453 seconds total (22.1 hz, 18.48 ms behind)
[run_subscribe_msckf-1] VioManager.cpp:613 [TIME]: 0.0152 seconds for tracking
[run_subscribe_msckf-1] VioManager.cpp:614 [TIME]: 0.0009 seconds for propagation
[run_subscribe_msckf-1] VioManager.cpp:615 [TIME]: 0.0042 seconds for MSCKF update (0 feats)
[run_subscribe_msckf-1] VioManager.cpp:617 [TIME]: 0.0059 seconds for SLAM update (48 feats)
[run_subscribe_msckf-1] VioManager.cpp:618 [TIME]: 0.0047 seconds for SLAM delayed init (4 feats)
[run_subscribe_msckf-1] VioManager.cpp:620 [TIME]: 0.0070 seconds for re-tri & marg (11 clones in state)
[run_subscribe_msckf-1] VioManager.cpp:628 [TIME]: 0.0379 seconds for total (camera 0 1)
[run_subscribe_msckf-1] VioManager.cpp:654 q_GtoI = -0.816,0.008,-0.578,0.001 | p_IinG = 0.080,0.039,0.142 | dist = 0.11 (meters)
[run_subscribe_msckf-1] VioManager.cpp:657 bg = -0.0028,0.0276,0.0731 | ba = -0.0913,-0.0070,0.0455
[run_subscribe_msckf-1] VioManager.cpp:662 camera-imu timeoffset = -0.00967
[run_subscribe_msckf-1] VioManager.cpp:669 cam0 intrinsics = 458.854,456.200,368.357,247.470 | -0.284,0.064,0.007,0.005
[run_subscribe_msckf-1] VioManager.cpp:669 cam1 intrinsics = 458.369,455.734,381.149,255.626 | -0.284,0.063,0.007,0.005
[run_subscribe_msckf-1] VioManager.cpp:678 cam0 extrinsics = -0.002,0.006,0.706,0.708 | 0.067,-0.018,0.058
[run_subscribe_msckf-1] VioManager.cpp:678 cam1 extrinsics = 0.002,0.009,0.707,0.707 | -0.031,-0.016,0.058
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0387 seconds total (25.9 hz, 18.47 ms behind)
[run_subscribe_msckf-1] VioManager.cpp:613 [TIME]: 0.0123 seconds for tracking
[run_subscribe_msckf-1] VioManager.cpp:614 [TIME]: 0.0009 seconds for propagation
[run_subscribe_msckf-1] VioManager.cpp:615 [TIME]: 0.0087 seconds for MSCKF update (0 feats)
[run_subscribe_msckf-1] VioManager.cpp:617 [TIME]: 0.0031 seconds for SLAM update (46 feats)
[run_subscribe_msckf-1] VioManager.cpp:618 [TIME]: 0.0231 seconds for SLAM delayed init (32 feats)
[run_subscribe_msckf-1] VioManager.cpp:620 [TIME]: 0.0098 seconds for re-tri & marg (11 clones in state)
[run_subscribe_msckf-1] VioManager.cpp:628 [TIME]: 0.0579 seconds for total (camera 0 1)
[run_subscribe_msckf-1] VioManager.cpp:654 q_GtoI = -0.816,0.009,-0.577,0.001 | p_IinG = 0.075,0.037,0.148 | dist = 0.12 (meters)
[run_subscribe_msckf-1] VioManager.cpp:657 bg = -0.0027,0.0282,0.0729 | ba = -0.0930,-0.0067,0.0460
[run_subscribe_msckf-1] VioManager.cpp:662 camera-imu timeoffset = -0.00958
[run_subscribe_msckf-1] VioManager.cpp:669 cam0 intrinsics = 458.816,456.319,368.331,247.533 | -0.284,0.064,0.008,0.006
[run_subscribe_msckf-1] VioManager.cpp:669 cam1 intrinsics = 458.324,455.835,381.255,255.679 | -0.284,0.063,0.008,0.005
[run_subscribe_msckf-1] VioManager.cpp:678 cam0 extrinsics = -0.002,0.006,0.706,0.708 | 0.068,-0.018,0.057
[run_subscribe_msckf-1] VioManager.cpp:678 cam1 extrinsics = 0.002,0.009,0.707,0.708 | -0.030,-0.017,0.057
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0589 seconds total (17.0 hz, 18.47 ms behind)
[run_subscribe_msckf-1] VioManager.cpp:613 [TIME]: 0.0222 seconds for tracking
[run_subscribe_msckf-1] VioManager.cpp:614 [TIME]: 0.0032 seconds for propagation
[run_subscribe_msckf-1] VioManager.cpp:615 [TIME]: 0.0051 seconds for MSCKF update (0 feats)
[run_subscribe_msckf-1] VioManager.cpp:617 [TIME]: 0.0259 seconds for SLAM update (46 feats)
[run_subscribe_msckf-1] VioManager.cpp:618 [TIME]: 0.0065 seconds for SLAM delayed init (5 feats)
[run_subscribe_msckf-1] VioManager.cpp:620 [TIME]: 0.0094 seconds for re-tri & marg (11 clones in state)
[run_subscribe_msckf-1] VioManager.cpp:628 [TIME]: 0.07218 seconds for total (camera 0 1)
[run_subscribe_msckf-1] VioManager.cpp:654 q_GtoI = -0.817,0.010,-0.576,0.001 | p_IinG = 0.071,0.035,0.157 | dist = 0.13 (meters)
[run_subscribe_msckf-1] VioManager.cpp:657 bg = -0.0022,0.0294,0.0730 | ba = -0.0957,-0.0055,0.0470
[run_subscribe_msckf-1] VioManager.cpp:662 camera-imu timeoffset = -0.00969
[run_subscribe_msckf-1] VioManager.cpp:669 cam0 intrinsics = 458.745,456.567,368.402,247.651 | -0.283,0.064,0.009,0.006
[run_subscribe_msckf-1] VioManager.cpp:669 cam1 intrinsics = 458.221,456.034,381.365,255.766 | -0.283,0.063,0.009,0.006
[run_subscribe_msckf-1] VioManager.cpp:678 cam0 extrinsics = -0.001,0.006,0.706,0.708 | 0.069,-0.020,0.054
[run_subscribe_msckf-1] VioManager.cpp:678 cam1 extrinsics = 0.003,0.009,0.707,0.707 | -0.029,-0.020,0.054
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0730 seconds total (13.7 hz, 18.46 ms behind)
[run_subscribe_msckf-1] VioManager.cpp:613 [TIME]: 0.0212 seconds for tracking
[run_subscribe_msckf-1] VioManager.cpp:614 [TIME]: 0.0007 seconds for propagation
[run_subscribe_msckf-1] VioManager.cpp:615 [TIME]: 0.0001 seconds for MSCKF update (0 feats)
[run_subscribe_msckf-1] VioManager.cpp:617 [TIME]: 0.0217 seconds for SLAM update (46 feats)
[run_subscribe_msckf-1] VioManager.cpp:618 [TIME]: 0.0000 seconds for SLAM delayed init (0 feats)
[run_subscribe_msckf-1] VioManager.cpp:620 [TIME]: 0.0087 seconds for re-tri & marg (11 clones in state)
[run_subscribe_msckf-1] VioManager.cpp:628 [TIME]: 0.05237 seconds for total (camera 0 1)
[run_subscribe_msckf-1] VioManager.cpp:654 q_GtoI = -0.819,0.010,-0.574,0.001 | p_IinG = 0.069,0.034,0.167 | dist = 0.13 (meters)
[run_subscribe_msckf-1] VioManager.cpp:657 bg = -0.0017,0.0315,0.0732 | ba = -0.1010,-0.0034,0.0492
[run_subscribe_msckf-1] VioManager.cpp:662 camera-imu timeoffset = -0.01037
[run_subscribe_msckf-1] VioManager.cpp:669 cam0 intrinsics = 458.672,456.978,368.511,247.768 | -0.283,0.064,0.011,0.007
[run_subscribe_msckf-1] VioManager.cpp:669 cam1 intrinsics = 458.099,456.341,381.420,255.842 | -0.282,0.063,0.010,0.006
[run_subscribe_msckf-1] VioManager.cpp:678 cam0 extrinsics = -0.001,0.006,0.707,0.708 | 0.072,-0.023,0.047
[run_subscribe_msckf-1] VioManager.cpp:678 cam1 extrinsics = 0.004,0.009,0.707,0.707 | -0.028,-0.023,0.047
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0532 seconds total (18.8 hz, 18.47 ms behind)
[run_subscribe_msckf-1] VioManager.cpp:613 [TIME]: 0.0167 seconds for tracking
[run_subscribe_msckf-1] VioManager.cpp:614 [TIME]: 0.0006 seconds for propagation
[run_subscribe_msckf-1] VioManager.cpp:615 [TIME]: 0.0001 seconds for MSCKF update (0 feats)
[run_subscribe_msckf-1] VioManager.cpp:617 [TIME]: 0.0251 seconds for SLAM update (46 feats)
[run_subscribe_msckf-1] VioManager.cpp:618 [TIME]: 0.0000 seconds for SLAM delayed init (0 feats)
[run_subscribe_msckf-1] VioManager.cpp:620 [TIME]: 0.0237 seconds for re-tri & marg (11 clones in state)
[run_subscribe_msckf-1] VioManager.cpp:628 [TIME]: 0.06631 seconds for total (camera 0 1)
[run_subscribe_msckf-1] VioManager.cpp:654 q_GtoI = 0.821,-0.010,0.570,0.001 | p_IinG = 0.068,0.037,0.180 | dist = 0.14 (meters)
[run_subscribe_msckf-1] VioManager.cpp:657 bg = -0.0007,0.0332,0.0742 | ba = -0.1029,-0.0006,0.0513
[run_subscribe_msckf-1] VioManager.cpp:662 camera-imu timeoffset = -0.01026
[run_subscribe_msckf-1] VioManager.cpp:669 cam0 intrinsics = 458.582,457.243,368.561,247.996 | -0.282,0.064,0.012,0.007
[run_subscribe_msckf-1] VioManager.cpp:669 cam1 intrinsics = 457.856,456.574,381.600,255.954 | -0.282,0.063,0.012,0.006
[run_subscribe_msckf-1] VioManager.cpp:678 cam0 extrinsics = 0.000,0.006,0.707,0.707 | 0.072,-0.023,0.050
[run_subscribe_msckf-1] VioManager.cpp:678 cam1 extrinsics = 0.005,0.009,0.707,0.707 | -0.029,-0.023,0.049
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0671 seconds total (14.9 hz, 22.04 ms behind)
[run_subscribe_msckf-1] VioManager.cpp:613 [TIME]: 0.0144 seconds for tracking
[run_subscribe_msckf-1] VioManager.cpp:614 [TIME]: 0.0007 seconds for propagation
[run_subscribe_msckf-1] VioManager.cpp:615 [TIME]: 0.0004 seconds for MSCKF update (0 feats)
[run_subscribe_msckf-1] VioManager.cpp:617 [TIME]: 0.0242 seconds for SLAM update (45 feats)
[run_subscribe_msckf-1] VioManager.cpp:618 [TIME]: 0.0001 seconds for SLAM delayed init (0 feats)
[run_subscribe_msckf-1] VioManager.cpp:620 [TIME]: 0.0076 seconds for re-tri & marg (11 clones in state)
[run_subscribe_msckf-1] VioManager.cpp:628 [TIME]: 0.04733 seconds for total (camera 0 1)
[run_subscribe_msckf-1] VioManager.cpp:654 q_GtoI = 0.824,-0.010,0.567,0.002 | p_IinG = 0.067,0.042,0.194 | dist = 0.15 (meters)
[run_subscribe_msckf-1] VioManager.cpp:657 bg = 0.0003,0.0334,0.0752 | ba = -0.0918,-0.0004,0.0482
[run_subscribe_msckf-1] VioManager.cpp:662 camera-imu timeoffset = -0.00835
[run_subscribe_msckf-1] VioManager.cpp:669 cam0 intrinsics = 458.621,457.101,368.599,248.307 | -0.283,0.065,0.013,0.007
[run_subscribe_msckf-1] VioManager.cpp:669 cam1 intrinsics = 457.702,456.479,381.809,256.045 | -0.283,0.064,0.012,0.006
[run_subscribe_msckf-1] VioManager.cpp:678 cam0 extrinsics = 0.001,0.006,0.707,0.707 | 0.068,-0.019,0.048
[run_subscribe_msckf-1] VioManager.cpp:678 cam1 extrinsics = 0.006,0.010,0.707,0.707 | -0.033,-0.018,0.048
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0482 seconds total (20.8 hz, 23.53 ms behind)
[run_subscribe_msckf-1] VioManager.cpp:613 [TIME]: 0.0237 seconds for tracking
[run_subscribe_msckf-1] VioManager.cpp:614 [TIME]: 0.0008 seconds for propagation
[run_subscribe_msckf-1] VioManager.cpp:615 [TIME]: 0.0001 seconds for MSCKF update (0 feats)
[run_subscribe_msckf-1] VioManager.cpp:617 [TIME]: 0.0200 seconds for SLAM update (45 feats)
[run_subscribe_msckf-1] VioManager.cpp:618 [TIME]: 0.0000 seconds for SLAM delayed init (0 feats)
[run_subscribe_msckf-1] VioManager.cpp:620 [TIME]: 0.0091 seconds for re-tri & marg (11 clones in state)
[run_subscribe_msckf-1] VioManager.cpp:628 [TIME]: 0.05381 seconds for total (camera 0 1)
[run_subscribe_msckf-1] VioManager.cpp:654 q_GtoI = 0.828,-0.011,0.560,0.003 | p_IinG = 0.067,0.046,0.203 | dist = 0.15 (meters)
[run_subscribe_msckf-1] VioManager.cpp:657 bg = 0.0007,0.0334,0.0759 | ba = -0.0835,-0.0009,0.0440
[run_subscribe_msckf-1] VioManager.cpp:662 camera-imu timeoffset = -0.00739
[run_subscribe_msckf-1] VioManager.cpp:669 cam0 intrinsics = 458.650,457.136,368.597,248.598 | -0.284,0.066,0.013,0.007
[run_subscribe_msckf-1] VioManager.cpp:669 cam1 intrinsics = 457.626,456.548,381.905,256.107 | -0.283,0.064,0.013,0.007
[run_subscribe_msckf-1] VioManager.cpp:678 cam0 extrinsics = 0.001,0.007,0.706,0.708 | 0.066,-0.012,0.037
[run_subscribe_msckf-1] VioManager.cpp:678 cam1 extrinsics = 0.006,0.011,0.707,0.707 | -0.035,-0.013,0.037
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0546 seconds total (18.3 hz, 23.33 ms behind)
[run_subscribe_msckf-1] VioManager.cpp:613 [TIME]: 0.0134 seconds for tracking
[run_subscribe_msckf-1] VioManager.cpp:614 [TIME]: 0.0006 seconds for propagation
[run_subscribe_msckf-1] VioManager.cpp:615 [TIME]: 0.0073 seconds for MSCKF update (2 feats)
[run_subscribe_msckf-1] VioManager.cpp:617 [TIME]: 0.0246 seconds for SLAM update (45 feats)
[run_subscribe_msckf-1] VioManager.cpp:618 [TIME]: 0.0043 seconds for SLAM delayed init (4 feats)
[run_subscribe_msckf-1] VioManager.cpp:620 [TIME]: 0.0077 seconds for re-tri & marg (11 clones in state)
[run_subscribe_msckf-1] VioManager.cpp:628 [TIME]: 0.05788 seconds for total (camera 0 1)
[run_subscribe_msckf-1] VioManager.cpp:654 q_GtoI = 0.832,-0.009,0.554,0.005 | p_IinG = 0.070,0.046,0.210 | dist = 0.16 (meters)
[run_subscribe_msckf-1] VioManager.cpp:657 bg = 0.0006,0.0337,0.0761 | ba = -0.0824,-0.0010,0.0425
[run_subscribe_msckf-1] VioManager.cpp:662 camera-imu timeoffset = -0.00665
[run_subscribe_msckf-1] VioManager.cpp:669 cam0 intrinsics = 458.474,457.267,368.594,248.919 | -0.284,0.065,0.013,0.007
[run_subscribe_msckf-1] VioManager.cpp:669 cam1 intrinsics = 457.622,456.797,381.815,256.257 | -0.283,0.064,0.013,0.006
[run_subscribe_msckf-1] VioManager.cpp:678 cam0 extrinsics = -0.000,0.009,0.707,0.708 | 0.068,-0.008,0.033
[run_subscribe_msckf-1] VioManager.cpp:678 cam1 extrinsics = 0.005,0.014,0.707,0.707 | -0.034,-0.009,0.033
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0588 seconds total (17.0 hz, 23.24 ms behind)
[run_subscribe_msckf-1] VioManager.cpp:613 [TIME]: 0.0106 seconds for tracking
[run_subscribe_msckf-1] VioManager.cpp:614 [TIME]: 0.0005 seconds for propagation
[run_subscribe_msckf-1] VioManager.cpp:615 [TIME]: 0.0106 seconds for MSCKF update (0 feats)
[run_subscribe_msckf-1] VioManager.cpp:617 [TIME]: 0.0218 seconds for SLAM update (48 feats)
[run_subscribe_msckf-1] VioManager.cpp:618 [TIME]: 0.0165 seconds for SLAM delayed init (3 feats)
[run_subscribe_msckf-1] VioManager.cpp:620 [TIME]: 0.0077 seconds for re-tri & marg (11 clones in state)
[run_subscribe_msckf-1] VioManager.cpp:628 [TIME]: 0.0677 seconds for total (camera 0 1)
[run_subscribe_msckf-1] VioManager.cpp:654 q_GtoI = 0.835,-0.007,0.550,0.009 | p_IinG = 0.075,0.048,0.217 | dist = 0.17 (meters)
[run_subscribe_msckf-1] VioManager.cpp:657 bg = 0.0009,0.0341,0.0767 | ba = -0.0892,0.0021,0.0469
[run_subscribe_msckf-1] VioManager.cpp:662 camera-imu timeoffset = -0.00557
[run_subscribe_msckf-1] VioManager.cpp:669 cam0 intrinsics = 458.378,457.529,368.527,249.371 | -0.282,0.065,0.013,0.007
[run_subscribe_msckf-1] VioManager.cpp:669 cam1 intrinsics = 457.635,457.124,381.552,256.514 | -0.282,0.064,0.013,0.006
[run_subscribe_msckf-1] VioManager.cpp:678 cam0 extrinsics = -0.000,0.012,0.707,0.707 | 0.061,0.001,0.034
[run_subscribe_msckf-1] VioManager.cpp:678 cam1 extrinsics = 0.005,0.016,0.707,0.707 | -0.041,0.000,0.034
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0687 seconds total (14.6 hz, 23.17 ms behind)
[run_subscribe_msckf-1] VioManager.cpp:613 [TIME]: 0.0127 seconds for tracking
[run_subscribe_msckf-1] VioManager.cpp:614 [TIME]: 0.0006 seconds for propagation
[run_subscribe_msckf-1] VioManager.cpp:615 [TIME]: 0.0002 seconds for MSCKF update (0 feats)
[run_subscribe_msckf-1] VioManager.cpp:617 [TIME]: 0.0337 seconds for SLAM update (48 feats)
[run_subscribe_msckf-1] VioManager.cpp:618 [TIME]: 0.0000 seconds for SLAM delayed init (0 feats)
[run_subscribe_msckf-1] VioManager.cpp:620 [TIME]: 0.0150 seconds for re-tri & marg (11 clones in state)
[run_subscribe_msckf-1] VioManager.cpp:628 [TIME]: 0.06221 seconds for total (camera 0 1)
[run_subscribe_msckf-1] VioManager.cpp:654 q_GtoI = 0.837,-0.004,0.547,0.014 | p_IinG = 0.080,0.050,0.225 | dist = 0.17 (meters)
[run_subscribe_msckf-1] VioManager.cpp:657 bg = 0.0015,0.0341,0.0774 | ba = -0.0914,0.0041,0.0525
[run_subscribe_msckf-1] VioManager.cpp:662 camera-imu timeoffset = -0.00405
[run_subscribe_msckf-1] VioManager.cpp:669 cam0 intrinsics = 458.196,457.733,368.164,249.613 | -0.281,0.064,0.013,0.007
[run_subscribe_msckf-1] VioManager.cpp:669 cam1 intrinsics = 457.485,457.402,381.245,256.576 | -0.281,0.063,0.013,0.006
[run_subscribe_msckf-1] VioManager.cpp:678 cam0 extrinsics = -0.001,0.015,0.707,0.707 | 0.046,0.014,0.036
[run_subscribe_msckf-1] VioManager.cpp:678 cam1 extrinsics = 0.005,0.019,0.708,0.706 | -0.056,0.014,0.037
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0630 seconds total (15.9 hz, 28.06 ms behind)
[run_subscribe_msckf-1] VioManager.cpp:613 [TIME]: 0.0200 seconds for tracking
[run_subscribe_msckf-1] VioManager.cpp:614 [TIME]: 0.0007 seconds for propagation
[run_subscribe_msckf-1] VioManager.cpp:615 [TIME]: 0.0001 seconds for MSCKF update (0 feats)
[run_subscribe_msckf-1] VioManager.cpp:617 [TIME]: 0.0249 seconds for SLAM update (48 feats)
[run_subscribe_msckf-1] VioManager.cpp:618 [TIME]: 0.0000 seconds for SLAM delayed init (0 feats)
[run_subscribe_msckf-1] VioManager.cpp:620 [TIME]: 0.0070 seconds for re-tri & marg (11 clones in state)
[run_subscribe_msckf-1] VioManager.cpp:628 [TIME]: 0.05275 seconds for total (camera 0 1)
[run_subscribe_msckf-1] VioManager.cpp:654 q_GtoI = 0.837,-0.002,0.547,0.018 | p_IinG = 0.087,0.052,0.231 | dist = 0.18 (meters)
[run_subscribe_msckf-1] VioManager.cpp:657 bg = 0.0020,0.0338,0.0779 | ba = -0.0955,0.0054,0.0595
[run_subscribe_msckf-1] VioManager.cpp:662 camera-imu timeoffset = -0.00265
[run_subscribe_msckf-1] VioManager.cpp:669 cam0 intrinsics = 458.057,458.148,367.712,249.719 | -0.280,0.064,0.013,0.008
[run_subscribe_msckf-1] VioManager.cpp:669 cam1 intrinsics = 457.363,457.863,380.857,256.536 | -0.280,0.063,0.013,0.007
[run_subscribe_msckf-1] VioManager.cpp:678 cam0 extrinsics = -0.002,0.017,0.708,0.706 | 0.031,0.025,0.030
[run_subscribe_msckf-1] VioManager.cpp:678 cam1 extrinsics = 0.004,0.021,0.709,0.705 | -0.072,0.024,0.031
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0535 seconds total (18.7 hz, 27.91 ms behind)
[run_subscribe_msckf-1] VioManager.cpp:613 [TIME]: 0.0164 seconds for tracking
[run_subscribe_msckf-1] VioManager.cpp:614 [TIME]: 0.0007 seconds for propagation
[run_subscribe_msckf-1] VioManager.cpp:615 [TIME]: 0.0001 seconds for MSCKF update (0 feats)
[run_subscribe_msckf-1] VioManager.cpp:617 [TIME]: 0.0263 seconds for SLAM update (48 feats)
[run_subscribe_msckf-1] VioManager.cpp:618 [TIME]: 0.0000 seconds for SLAM delayed init (0 feats)
[run_subscribe_msckf-1] VioManager.cpp:620 [TIME]: 0.0073 seconds for re-tri & marg (11 clones in state)
[run_subscribe_msckf-1] VioManager.cpp:628 [TIME]: 0.05076 seconds for total (camera 0 1)
[run_subscribe_msckf-1] VioManager.cpp:654 q_GtoI = 0.835,0.000,0.549,0.022 | p_IinG = 0.092,0.053,0.239 | dist = 0.20 (meters)
[run_subscribe_msckf-1] VioManager.cpp:657 bg = 0.0021,0.0328,0.0783 | ba = -0.0958,0.0056,0.0604
[run_subscribe_msckf-1] VioManager.cpp:662 camera-imu timeoffset = -0.00229
[run_subscribe_msckf-1] VioManager.cpp:669 cam0 intrinsics = 457.896,458.300,367.490,249.968 | -0.281,0.064,0.012,0.008
[run_subscribe_msckf-1] VioManager.cpp:669 cam1 intrinsics = 457.195,457.983,380.687,256.764 | -0.280,0.063,0.012,0.007
[run_subscribe_msckf-1] VioManager.cpp:678 cam0 extrinsics = -0.002,0.019,0.709,0.705 | 0.033,0.035,0.033
[run_subscribe_msckf-1] VioManager.cpp:678 cam1 extrinsics = 0.004,0.023,0.709,0.704 | -0.071,0.034,0.034
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0515 seconds total (19.4 hz, 27.77 ms behind)
[run_subscribe_msckf-1] VioManager.cpp:613 [TIME]: 0.0115 seconds for tracking
[run_subscribe_msckf-1] VioManager.cpp:614 [TIME]: 0.0008 seconds for propagation
[run_subscribe_msckf-1] VioManager.cpp:615 [TIME]: 0.0005 seconds for MSCKF update (0 feats)
[run_subscribe_msckf-1] VioManager.cpp:617 [TIME]: 0.0224 seconds for SLAM update (48 feats)
[run_subscribe_msckf-1] VioManager.cpp:618 [TIME]: 0.0012 seconds for SLAM delayed init (1 feats)
[run_subscribe_msckf-1] VioManager.cpp:620 [TIME]: 0.0097 seconds for re-tri & marg (11 clones in state)
[run_subscribe_msckf-1] VioManager.cpp:628 [TIME]: 0.0461 seconds for total (camera 0 1)
[run_subscribe_msckf-1] VioManager.cpp:654 q_GtoI = 0.833,0.002,0.552,0.024 | p_IinG = 0.096,0.053,0.248 | dist = 0.21 (meters)
[run_subscribe_msckf-1] VioManager.cpp:657 bg = 0.0019,0.0316,0.0785 | ba = -0.0918,0.0054,0.0551
[run_subscribe_msckf-1] VioManager.cpp:662 camera-imu timeoffset = -0.00215
[run_subscribe_msckf-1] VioManager.cpp:669 cam0 intrinsics = 457.825,457.889,367.469,250.174 | -0.282,0.065,0.011,0.008
[run_subscribe_msckf-1] VioManager.cpp:669 cam1 intrinsics = 457.146,457.622,380.771,257.087 | -0.281,0.063,0.011,0.007
[run_subscribe_msckf-1] VioManager.cpp:678 cam0 extrinsics = -0.002,0.020,0.709,0.704 | 0.037,0.042,0.053
[run_subscribe_msckf-1] VioManager.cpp:678 cam1 extrinsics = 0.004,0.024,0.710,0.704 | -0.067,0.042,0.053
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0470 seconds total (21.3 hz, 27.73 ms behind)
[run_subscribe_msckf-1] VioManager.cpp:613 [TIME]: 0.0215 seconds for tracking
[run_subscribe_msckf-1] VioManager.cpp:614 [TIME]: 0.0008 seconds for propagation
[run_subscribe_msckf-1] VioManager.cpp:615 [TIME]: 0.0007 seconds for MSCKF update (0 feats)
[run_subscribe_msckf-1] VioManager.cpp:617 [TIME]: 0.0205 seconds for SLAM update (48 feats)
[run_subscribe_msckf-1] VioManager.cpp:618 [TIME]: 0.0015 seconds for SLAM delayed init (1 feats)
[run_subscribe_msckf-1] VioManager.cpp:620 [TIME]: 0.0072 seconds for re-tri & marg (11 clones in state)
[run_subscribe_msckf-1] VioManager.cpp:628 [TIME]: 0.05208 seconds for total (camera 0 1)
[run_subscribe_msckf-1] VioManager.cpp:654 q_GtoI = 0.831,0.002,0.556,0.026 | p_IinG = 0.100,0.053,0.257 | dist = 0.23 (meters)
[run_subscribe_msckf-1] VioManager.cpp:657 bg = 0.0018,0.0306,0.0787 | ba = -0.0865,0.0059,0.0480
[run_subscribe_msckf-1] VioManager.cpp:662 camera-imu timeoffset = -0.00180
[run_subscribe_msckf-1] VioManager.cpp:669 cam0 intrinsics = 457.758,457.528,367.507,250.248 | -0.283,0.065,0.010,0.008
[run_subscribe_msckf-1] VioManager.cpp:669 cam1 intrinsics = 456.987,457.188,380.753,257.350 | -0.281,0.064,0.010,0.007
[run_subscribe_msckf-1] VioManager.cpp:678 cam0 extrinsics = -0.002,0.021,0.710,0.704 | 0.036,0.046,0.076
[run_subscribe_msckf-1] VioManager.cpp:678 cam1 extrinsics = 0.004,0.025,0.710,0.704 | -0.066,0.046,0.076
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0529 seconds total (18.9 hz, 27.72 ms behind)
[run_subscribe_msckf-1] VioManager.cpp:613 [TIME]: 0.0139 seconds for tracking
[run_subscribe_msckf-1] VioManager.cpp:614 [TIME]: 0.0006 seconds for propagation
[run_subscribe_msckf-1] VioManager.cpp:615 [TIME]: 0.0030 seconds for MSCKF update (0 feats)
[run_subscribe_msckf-1] VioManager.cpp:617 [TIME]: 0.0139 seconds for SLAM update (46 feats)
[run_subscribe_msckf-1] VioManager.cpp:618 [TIME]: 0.0054 seconds for SLAM delayed init (4 feats)
[run_subscribe_msckf-1] VioManager.cpp:620 [TIME]: 0.0064 seconds for re-tri & marg (11 clones in state)
[run_subscribe_msckf-1] VioManager.cpp:628 [TIME]: 0.04325 seconds for total (camera 0 1)
[run_subscribe_msckf-1] VioManager.cpp:654 q_GtoI = 0.829,-0.001,0.559,0.027 | p_IinG = 0.110,0.053,0.264 | dist = 0.24 (meters)
[run_subscribe_msckf-1] VioManager.cpp:657 bg = 0.0017,0.0301,0.0787 | ba = -0.0814,0.0068,0.0419
[run_subscribe_msckf-1] VioManager.cpp:662 camera-imu timeoffset = -0.00114
[run_subscribe_msckf-1] VioManager.cpp:669 cam0 intrinsics = 457.787,457.267,367.427,250.265 | -0.284,0.065,0.010,0.008
[run_subscribe_msckf-1] VioManager.cpp:669 cam1 intrinsics = 456.917,456.899,380.599,257.401 | -0.282,0.064,0.010,0.007
[run_subscribe_msckf-1] VioManager.cpp:678 cam0 extrinsics = -0.002,0.021,0.710,0.704 | 0.031,0.044,0.092
[run_subscribe_msckf-1] VioManager.cpp:678 cam1 extrinsics = 0.004,0.026,0.710,0.703 | -0.071,0.044,0.092
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0441 seconds total (22.7 hz, 27.68 ms behind)
[run_subscribe_msckf-1] VioManager.cpp:613 [TIME]: 0.0120 seconds for tracking
[run_subscribe_msckf-1] VioManager.cpp:614 [TIME]: 0.0007 seconds for propagation
[run_subscribe_msckf-1] VioManager.cpp:615 [TIME]: 0.0034 seconds for MSCKF update (1 feats)
[run_subscribe_msckf-1] VioManager.cpp:617 [TIME]: 0.0125 seconds for SLAM update (45 feats)
[run_subscribe_msckf-1] VioManager.cpp:618 [TIME]: 0.0084 seconds for SLAM delayed init (7 feats)
[run_subscribe_msckf-1] VioManager.cpp:620 [TIME]: 0.0066 seconds for re-tri & marg (11 clones in state)
[run_subscribe_msckf-1] VioManager.cpp:628 [TIME]: 0.0435 seconds for total (camera 0 1)
[run_subscribe_msckf-1] VioManager.cpp:654 q_GtoI = 0.827,-0.007,0.562,0.026 | p_IinG = 0.120,0.055,0.266 | dist = 0.25 (meters)
[run_subscribe_msckf-1] VioManager.cpp:657 bg = 0.0018,0.0293,0.0788 | ba = -0.0767,0.0075,0.0368
[run_subscribe_msckf-1] VioManager.cpp:662 camera-imu timeoffset = -0.00052
[run_subscribe_msckf-1] VioManager.cpp:669 cam0 intrinsics = 457.872,457.129,367.082,250.246 | -0.284,0.065,0.009,0.008
[run_subscribe_msckf-1] VioManager.cpp:669 cam1 intrinsics = 456.769,456.670,380.431,257.568 | -0.282,0.064,0.009,0.007
[run_subscribe_msckf-1] VioManager.cpp:678 cam0 extrinsics = -0.003,0.022,0.710,0.703 | 0.020,0.045,0.105
[run_subscribe_msckf-1] VioManager.cpp:678 cam1 extrinsics = 0.003,0.026,0.711,0.703 | -0.082,0.045,0.105
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0443 seconds total (22.6 hz, 27.61 ms behind)
[run_subscribe_msckf-1] VioManager.cpp:613 [TIME]: 0.0160 seconds for tracking
[run_subscribe_msckf-1] VioManager.cpp:614 [TIME]: 0.0007 seconds for propagation
[run_subscribe_msckf-1] VioManager.cpp:615 [TIME]: 0.0046 seconds for MSCKF update (1 feats)
[run_subscribe_msckf-1] VioManager.cpp:617 [TIME]: 0.0140 seconds for SLAM update (45 feats)
[run_subscribe_msckf-1] VioManager.cpp:618 [TIME]: 0.0104 seconds for SLAM delayed init (12 feats)
[run_subscribe_msckf-1] VioManager.cpp:620 [TIME]: 0.0120 seconds for re-tri & marg (11 clones in state)
[run_subscribe_msckf-1] VioManager.cpp:628 [TIME]: 0.05771 seconds for total (camera 0 1)
[run_subscribe_msckf-1] VioManager.cpp:654 q_GtoI = 0.826,-0.016,0.562,0.027 | p_IinG = 0.135,0.061,0.268 | dist = 0.27 (meters)
[run_subscribe_msckf-1] VioManager.cpp:657 bg = 0.0023,0.0292,0.0793 | ba = -0.0780,0.0063,0.0425
[run_subscribe_msckf-1] VioManager.cpp:662 camera-imu timeoffset = -0.00086
[run_subscribe_msckf-1] VioManager.cpp:669 cam0 intrinsics = 458.035,457.130,366.984,250.101 | -0.285,0.065,0.008,0.008
[run_subscribe_msckf-1] VioManager.cpp:669 cam1 intrinsics = 456.766,456.541,380.451,257.498 | -0.282,0.063,0.008,0.008
[run_subscribe_msckf-1] VioManager.cpp:678 cam0 extrinsics = -0.003,0.021,0.711,0.703 | 0.027,0.036,0.103
[run_subscribe_msckf-1] VioManager.cpp:678 cam1 extrinsics = 0.002,0.026,0.711,0.702 | -0.075,0.037,0.102
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0587 seconds total (17.0 hz, 27.55 ms behind)
[run_subscribe_msckf-1] VioManager.cpp:613 [TIME]: 0.0238 seconds for tracking
[run_subscribe_msckf-1] VioManager.cpp:614 [TIME]: 0.0006 seconds for propagation
[run_subscribe_msckf-1] VioManager.cpp:615 [TIME]: 0.0002 seconds for MSCKF update (0 feats)
[run_subscribe_msckf-1] VioManager.cpp:617 [TIME]: 0.0216 seconds for SLAM update (45 feats)
[run_subscribe_msckf-1] VioManager.cpp:618 [TIME]: 0.0000 seconds for SLAM delayed init (0 feats)
[run_subscribe_msckf-1] VioManager.cpp:620 [TIME]: 0.0099 seconds for re-tri & marg (11 clones in state)
[run_subscribe_msckf-1] VioManager.cpp:628 [TIME]: 0.05611 seconds for total (camera 0 1)
[run_subscribe_msckf-1] VioManager.cpp:654 q_GtoI = 0.825,-0.025,0.563,0.029 | p_IinG = 0.150,0.067,0.269 | dist = 0.28 (meters)
[run_subscribe_msckf-1] VioManager.cpp:657 bg = 0.0026,0.0291,0.0796 | ba = -0.0812,0.0029,0.0518
[run_subscribe_msckf-1] VioManager.cpp:662 camera-imu timeoffset = -0.00086
[run_subscribe_msckf-1] VioManager.cpp:669 cam0 intrinsics = 458.192,457.169,366.949,249.999 | -0.284,0.065,0.008,0.009
[run_subscribe_msckf-1] VioManager.cpp:669 cam1 intrinsics = 456.863,456.538,380.529,257.409 | -0.281,0.062,0.008,0.008
[run_subscribe_msckf-1] VioManager.cpp:678 cam0 extrinsics = -0.003,0.020,0.711,0.703 | 0.043,0.035,0.100
[run_subscribe_msckf-1] VioManager.cpp:678 cam1 extrinsics = 0.002,0.025,0.711,0.702 | -0.059,0.035,0.099
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0568 seconds total (17.6 hz, 27.59 ms behind)
[run_subscribe_msckf-1] VioManager.cpp:613 [TIME]: 0.0130 seconds for tracking
[run_subscribe_msckf-1] VioManager.cpp:614 [TIME]: 0.0008 seconds for propagation
[run_subscribe_msckf-1] VioManager.cpp:615 [TIME]: 0.0022 seconds for MSCKF update (0 feats)
[run_subscribe_msckf-1] VioManager.cpp:617 [TIME]: 0.0158 seconds for SLAM update (43 feats)
[run_subscribe_msckf-1] VioManager.cpp:618 [TIME]: 0.0027 seconds for SLAM delayed init (3 feats)
[run_subscribe_msckf-1] VioManager.cpp:620 [TIME]: 0.0119 seconds for re-tri & marg (11 clones in state)
[run_subscribe_msckf-1] VioManager.cpp:628 [TIME]: 0.04643 seconds for total (camera 0 1)
[run_subscribe_msckf-1] VioManager.cpp:654 q_GtoI = 0.825,-0.033,0.564,0.033 | p_IinG = 0.166,0.069,0.273 | dist = 0.30 (meters)
[run_subscribe_msckf-1] VioManager.cpp:657 bg = 0.0025,0.0292,0.0795 | ba = -0.0848,-0.0006,0.0584
[run_subscribe_msckf-1] VioManager.cpp:662 camera-imu timeoffset = -0.00096
[run_subscribe_msckf-1] VioManager.cpp:669 cam0 intrinsics = 458.281,457.309,366.949,249.975 | -0.284,0.064,0.008,0.009
[run_subscribe_msckf-1] VioManager.cpp:669 cam1 intrinsics = 456.929,456.636,380.453,257.458 | -0.281,0.062,0.008,0.008
[run_subscribe_msckf-1] VioManager.cpp:678 cam0 extrinsics = -0.003,0.020,0.711,0.703 | 0.055,0.042,0.096
[run_subscribe_msckf-1] VioManager.cpp:678 cam1 extrinsics = 0.002,0.024,0.712,0.702 | -0.047,0.042,0.095
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0472 seconds total (21.2 hz, 32.59 ms behind)
[run_subscribe_msckf-1] VioManager.cpp:613 [TIME]: 0.0207 seconds for tracking
[run_subscribe_msckf-1] VioManager.cpp:614 [TIME]: 0.0014 seconds for propagation
[run_subscribe_msckf-1] VioManager.cpp:615 [TIME]: 0.0126 seconds for MSCKF update (5 feats)
[run_subscribe_msckf-1] VioManager.cpp:617 [TIME]: 0.0159 seconds for SLAM update (47 feats)
[run_subscribe_msckf-1] VioManager.cpp:618 [TIME]: 0.0196 seconds for SLAM delayed init (8 feats)
[run_subscribe_msckf-1] VioManager.cpp:620 [TIME]: 0.0069 seconds for re-tri & marg (11 clones in state)
[run_subscribe_msckf-1] VioManager.cpp:628 [TIME]: 0.07703 seconds for total (camera 0 1)
[run_subscribe_msckf-1] VioManager.cpp:654 q_GtoI = 0.823,-0.043,0.564,0.039 | p_IinG = 0.176,0.072,0.275 | dist = 0.31 (meters)
[run_subscribe_msckf-1] VioManager.cpp:657 bg = 0.0026,0.0287,0.0797 | ba = -0.0886,-0.0074,0.0702
[run_subscribe_msckf-1] VioManager.cpp:662 camera-imu timeoffset = -0.00077
[run_subscribe_msckf-1] VioManager.cpp:669 cam0 intrinsics = 458.365,457.381,367.040,249.875 | -0.285,0.064,0.008,0.009
[run_subscribe_msckf-1] VioManager.cpp:669 cam1 intrinsics = 456.922,456.648,380.518,257.402 | -0.280,0.061,0.008,0.009
[run_subscribe_msckf-1] VioManager.cpp:678 cam0 extrinsics = -0.003,0.019,0.712,0.702 | 0.068,0.049,0.096
[run_subscribe_msckf-1] VioManager.cpp:678 cam1 extrinsics = 0.002,0.023,0.713,0.701 | -0.033,0.050,0.095
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0779 seconds total (12.8 hz, 30.60 ms behind)
[run_subscribe_msckf-1] VioManager.cpp:613 [TIME]: 0.0125 seconds for tracking
[run_subscribe_msckf-1] VioManager.cpp:614 [TIME]: 0.0007 seconds for propagation
[run_subscribe_msckf-1] VioManager.cpp:615 [TIME]: 0.0313 seconds for MSCKF update (9 feats)
[run_subscribe_msckf-1] VioManager.cpp:617 [TIME]: 0.0190 seconds for SLAM update (50 feats)
[run_subscribe_msckf-1] VioManager.cpp:618 [TIME]: 0.0215 seconds for SLAM delayed init (8 feats)
[run_subscribe_msckf-1] VioManager.cpp:620 [TIME]: 0.0090 seconds for re-tri & marg (11 clones in state)
[run_subscribe_msckf-1] VioManager.cpp:628 [TIME]: 0.09392 seconds for total (camera 0 1)
[run_subscribe_msckf-1] VioManager.cpp:654 q_GtoI = 0.823,-0.054,0.564,0.045 | p_IinG = 0.183,0.072,0.277 | dist = 0.33 (meters)
[run_subscribe_msckf-1] VioManager.cpp:657 bg = 0.0024,0.0280,0.0797 | ba = -0.0916,-0.0181,0.0794
[run_subscribe_msckf-1] VioManager.cpp:662 camera-imu timeoffset = 0.00016
[run_subscribe_msckf-1] VioManager.cpp:669 cam0 intrinsics = 458.899,456.756,367.258,249.932 | -0.281,0.063,0.007,0.011
[run_subscribe_msckf-1] VioManager.cpp:669 cam1 intrinsics = 457.487,455.904,380.425,257.667 | -0.277,0.060,0.007,0.010
[run_subscribe_msckf-1] VioManager.cpp:678 cam0 extrinsics = -0.002,0.019,0.713,0.701 | 0.083,0.057,0.103
[run_subscribe_msckf-1] VioManager.cpp:678 cam1 extrinsics = 0.003,0.023,0.714,0.700 | -0.019,0.058,0.101
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0949 seconds total (10.5 hz, 32.58 ms behind)
[run_subscribe_msckf-1] VioManager.cpp:613 [TIME]: 0.0282 seconds for tracking
[run_subscribe_msckf-1] VioManager.cpp:614 [TIME]: 0.0006 seconds for propagation
[run_subscribe_msckf-1] VioManager.cpp:615 [TIME]: 0.0008 seconds for MSCKF update (0 feats)
[run_subscribe_msckf-1] VioManager.cpp:617 [TIME]: 0.0232 seconds for SLAM update (48 feats)
[run_subscribe_msckf-1] VioManager.cpp:618 [TIME]: 0.0000 seconds for SLAM delayed init (0 feats)
[run_subscribe_msckf-1] VioManager.cpp:620 [TIME]: 0.0070 seconds for re-tri & marg (11 clones in state)
[run_subscribe_msckf-1] VioManager.cpp:628 [TIME]: 0.05994 seconds for total (camera 0 1)
[run_subscribe_msckf-1] VioManager.cpp:654 q_GtoI = 0.822,-0.065,0.564,0.052 | p_IinG = 0.194,0.072,0.281 | dist = 0.34 (meters)
[run_subscribe_msckf-1] VioManager.cpp:657 bg = 0.0022,0.0276,0.0794 | ba = -0.0935,-0.0215,0.0748
[run_subscribe_msckf-1] VioManager.cpp:662 camera-imu timeoffset = -0.00007
[run_subscribe_msckf-1] VioManager.cpp:669 cam0 intrinsics = 459.073,456.639,367.250,249.888 | -0.282,0.063,0.007,0.010
[run_subscribe_msckf-1] VioManager.cpp:669 cam1 intrinsics = 457.748,455.831,380.413,257.655 | -0.278,0.061,0.007,0.010
[run_subscribe_msckf-1] VioManager.cpp:678 cam0 extrinsics = -0.002,0.019,0.713,0.700 | 0.077,0.060,0.096
[run_subscribe_msckf-1] VioManager.cpp:678 cam1 extrinsics = 0.003,0.023,0.714,0.700 | -0.025,0.061,0.094
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0607 seconds total (16.5 hz, 37.48 ms behind)
[run_subscribe_msckf-1] VioManager.cpp:613 [TIME]: 0.0151 seconds for tracking
[run_subscribe_msckf-1] VioManager.cpp:614 [TIME]: 0.0007 seconds for propagation
[run_subscribe_msckf-1] VioManager.cpp:615 [TIME]: 0.0012 seconds for MSCKF update (0 feats)
[run_subscribe_msckf-1] VioManager.cpp:617 [TIME]: 0.0189 seconds for SLAM update (47 feats)
[run_subscribe_msckf-1] VioManager.cpp:618 [TIME]: 0.0011 seconds for SLAM delayed init (1 feats)
[run_subscribe_msckf-1] VioManager.cpp:620 [TIME]: 0.0076 seconds for re-tri & marg (11 clones in state)
[run_subscribe_msckf-1] VioManager.cpp:628 [TIME]: 0.04463 seconds for total (camera 0 1)
[run_subscribe_msckf-1] VioManager.cpp:654 q_GtoI = 0.820,-0.075,0.565,0.058 | p_IinG = 0.204,0.070,0.284 | dist = 0.36 (meters)
[run_subscribe_msckf-1] VioManager.cpp:657 bg = 0.0018,0.0271,0.0790 | ba = -0.0971,-0.0269,0.0635
[run_subscribe_msckf-1] VioManager.cpp:662 camera-imu timeoffset = -0.00065
[run_subscribe_msckf-1] VioManager.cpp:669 cam0 intrinsics = 459.410,456.428,367.218,249.858 | -0.282,0.063,0.008,0.011
[run_subscribe_msckf-1] VioManager.cpp:669 cam1 intrinsics = 458.226,455.640,380.243,257.681 | -0.278,0.061,0.007,0.010
[run_subscribe_msckf-1] VioManager.cpp:678 cam0 extrinsics = -0.002,0.020,0.714,0.700 | 0.070,0.063,0.086
[run_subscribe_msckf-1] VioManager.cpp:678 cam1 extrinsics = 0.002,0.023,0.715,0.699 | -0.032,0.064,0.084
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0455 seconds total (22.0 hz, 37.51 ms behind)
[run_subscribe_msckf-1] VioManager.cpp:613 [TIME]: 0.0223 seconds for tracking
[run_subscribe_msckf-1] VioManager.cpp:614 [TIME]: 0.0006 seconds for propagation
[run_subscribe_msckf-1] VioManager.cpp:615 [TIME]: 0.0021 seconds for MSCKF update (2 feats)
[run_subscribe_msckf-1] VioManager.cpp:617 [TIME]: 0.0096 seconds for SLAM update (47 feats)
[run_subscribe_msckf-1] VioManager.cpp:618 [TIME]: 0.0022 seconds for SLAM delayed init (2 feats)
[run_subscribe_msckf-1] VioManager.cpp:620 [TIME]: 0.0075 seconds for re-tri & marg (11 clones in state)
[run_subscribe_msckf-1] VioManager.cpp:628 [TIME]: 0.04435 seconds for total (camera 0 1)
[run_subscribe_msckf-1] VioManager.cpp:654 q_GtoI = 0.818,-0.084,0.565,0.064 | p_IinG = 0.220,0.073,0.288 | dist = 0.38 (meters)
[run_subscribe_msckf-1] VioManager.cpp:657 bg = 0.0017,0.0272,0.0788 | ba = -0.0986,-0.0264,0.0565
[run_subscribe_msckf-1] VioManager.cpp:662 camera-imu timeoffset = -0.00088
[run_subscribe_msckf-1] VioManager.cpp:669 cam0 intrinsics = 459.602,456.222,367.165,249.830 | -0.281,0.063,0.007,0.010
[run_subscribe_msckf-1] VioManager.cpp:669 cam1 intrinsics = 458.504,455.501,380.313,257.680 | -0.278,0.061,0.007,0.010
[run_subscribe_msckf-1] VioManager.cpp:678 cam0 extrinsics = -0.003,0.020,0.714,0.700 | 0.070,0.062,0.083
[run_subscribe_msckf-1] VioManager.cpp:678 cam1 extrinsics = 0.002,0.024,0.714,0.699 | -0.032,0.064,0.082
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0452 seconds total (22.1 hz, 37.56 ms behind)
[run_subscribe_msckf-1] VioManager.cpp:613 [TIME]: 0.0159 seconds for tracking
[run_subscribe_msckf-1] VioManager.cpp:614 [TIME]: 0.0005 seconds for propagation
[run_subscribe_msckf-1] VioManager.cpp:615 [TIME]: 0.0041 seconds for MSCKF update (2 feats)
[run_subscribe_msckf-1] VioManager.cpp:617 [TIME]: 0.0055 seconds for SLAM update (45 feats)
[run_subscribe_msckf-1] VioManager.cpp:618 [TIME]: 0.0090 seconds for SLAM delayed init (9 feats)
[run_subscribe_msckf-1] VioManager.cpp:620 [TIME]: 0.0081 seconds for re-tri & marg (11 clones in state)
[run_subscribe_msckf-1] VioManager.cpp:628 [TIME]: 0.04308 seconds for total (camera 0 1)
[run_subscribe_msckf-1] VioManager.cpp:654 q_GtoI = 0.817,-0.093,0.565,0.069 | p_IinG = 0.234,0.075,0.294 | dist = 0.39 (meters)
[run_subscribe_msckf-1] VioManager.cpp:657 bg = 0.0015,0.0272,0.0787 | ba = -0.1000,-0.0264,0.0491
[run_subscribe_msckf-1] VioManager.cpp:662 camera-imu timeoffset = -0.00114
[run_subscribe_msckf-1] VioManager.cpp:669 cam0 intrinsics = 459.769,456.014,367.217,249.879 | -0.280,0.063,0.007,0.010
[run_subscribe_msckf-1] VioManager.cpp:669 cam1 intrinsics = 458.692,455.356,380.470,257.708 | -0.277,0.061,0.007,0.009
[run_subscribe_msckf-1] VioManager.cpp:678 cam0 extrinsics = -0.003,0.021,0.714,0.700 | 0.069,0.061,0.082
[run_subscribe_msckf-1] VioManager.cpp:678 cam1 extrinsics = 0.002,0.024,0.714,0.699 | -0.032,0.062,0.081
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0439 seconds total (22.8 hz, 37.59 ms behind)
[run_subscribe_msckf-1] VioManager.cpp:613 [TIME]: 0.0114 seconds for tracking
[run_subscribe_msckf-1] VioManager.cpp:614 [TIME]: 0.0007 seconds for propagation
[run_subscribe_msckf-1] VioManager.cpp:615 [TIME]: 0.0074 seconds for MSCKF update (2 feats)
[run_subscribe_msckf-1] VioManager.cpp:617 [TIME]: 0.0073 seconds for SLAM update (44 feats)
[run_subscribe_msckf-1] VioManager.cpp:618 [TIME]: 0.0155 seconds for SLAM delayed init (21 feats)
[run_subscribe_msckf-1] VioManager.cpp:620 [TIME]: 0.0069 seconds for re-tri & marg (11 clones in state)
[run_subscribe_msckf-1] VioManager.cpp:628 [TIME]: 0.04914 seconds for total (camera 0 1)
[run_subscribe_msckf-1] VioManager.cpp:654 q_GtoI = 0.816,-0.100,0.565,0.073 | p_IinG = 0.245,0.077,0.301 | dist = 0.41 (meters)
[run_subscribe_msckf-1] VioManager.cpp:657 bg = 0.0014,0.0270,0.0786 | ba = -0.1008,-0.0292,0.0442
[run_subscribe_msckf-1] VioManager.cpp:662 camera-imu timeoffset = -0.00133
[run_subscribe_msckf-1] VioManager.cpp:669 cam0 intrinsics = 459.807,455.967,367.368,249.922 | -0.281,0.064,0.007,0.010
[run_subscribe_msckf-1] VioManager.cpp:669 cam1 intrinsics = 458.734,455.320,380.602,257.708 | -0.278,0.062,0.007,0.009
[run_subscribe_msckf-1] VioManager.cpp:678 cam0 extrinsics = -0.003,0.021,0.714,0.700 | 0.069,0.060,0.082
[run_subscribe_msckf-1] VioManager.cpp:678 cam1 extrinsics = 0.002,0.025,0.714,0.699 | -0.033,0.061,0.080
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0500 seconds total (20.0 hz, 37.61 ms behind)
[run_subscribe_msckf-1] VioManager.cpp:613 [TIME]: 0.0208 seconds for tracking
[run_subscribe_msckf-1] VioManager.cpp:614 [TIME]: 0.0006 seconds for propagation
[run_subscribe_msckf-1] VioManager.cpp:615 [TIME]: 0.0009 seconds for MSCKF update (0 feats)
[run_subscribe_msckf-1] VioManager.cpp:617 [TIME]: 0.0137 seconds for SLAM update (43 feats)
[run_subscribe_msckf-1] VioManager.cpp:618 [TIME]: 0.0027 seconds for SLAM delayed init (3 feats)
[run_subscribe_msckf-1] VioManager.cpp:620 [TIME]: 0.0078 seconds for re-tri & marg (11 clones in state)
[run_subscribe_msckf-1] VioManager.cpp:628 [TIME]: 0.04651 seconds for total (camera 0 1)
[run_subscribe_msckf-1] VioManager.cpp:654 q_GtoI = 0.815,-0.106,0.565,0.078 | p_IinG = 0.250,0.077,0.305 | dist = 0.43 (meters)
[run_subscribe_msckf-1] VioManager.cpp:657 bg = 0.0011,0.0264,0.0784 | ba = -0.1035,-0.0377,0.0342
[run_subscribe_msckf-1] VioManager.cpp:662 camera-imu timeoffset = -0.00139
[run_subscribe_msckf-1] VioManager.cpp:669 cam0 intrinsics = 459.715,456.050,367.467,249.881 | -0.281,0.064,0.007,0.010
[run_subscribe_msckf-1] VioManager.cpp:669 cam1 intrinsics = 458.725,455.420,380.589,257.673 | -0.278,0.062,0.007,0.009
[run_subscribe_msckf-1] VioManager.cpp:678 cam0 extrinsics = -0.003,0.021,0.714,0.700 | 0.071,0.058,0.082
[run_subscribe_msckf-1] VioManager.cpp:678 cam1 extrinsics = 0.002,0.025,0.715,0.699 | -0.030,0.059,0.081
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0473 seconds total (21.2 hz, 37.63 ms behind)
[run_subscribe_msckf-1] VioManager.cpp:613 [TIME]: 0.0122 seconds for tracking
[run_subscribe_msckf-1] VioManager.cpp:614 [TIME]: 0.0008 seconds for propagation
[run_subscribe_msckf-1] VioManager.cpp:615 [TIME]: 0.0013 seconds for MSCKF update (0 feats)
[run_subscribe_msckf-1] VioManager.cpp:617 [TIME]: 0.0146 seconds for SLAM update (43 feats)
[run_subscribe_msckf-1] VioManager.cpp:618 [TIME]: 0.0074 seconds for SLAM delayed init (6 feats)
[run_subscribe_msckf-1] VioManager.cpp:620 [TIME]: 0.0098 seconds for re-tri & marg (11 clones in state)
[run_subscribe_msckf-1] VioManager.cpp:628 [TIME]: 0.04599 seconds for total (camera 0 1)
[run_subscribe_msckf-1] VioManager.cpp:654 q_GtoI = 0.814,-0.111,0.565,0.081 | p_IinG = 0.256,0.072,0.308 | dist = 0.44 (meters)
[run_subscribe_msckf-1] VioManager.cpp:657 bg = 0.0007,0.0259,0.0780 | ba = -0.1107,-0.0473,0.0194
[run_subscribe_msckf-1] VioManager.cpp:662 camera-imu timeoffset = -0.00171
[run_subscribe_msckf-1] VioManager.cpp:669 cam0 intrinsics = 459.546,456.136,367.473,249.825 | -0.281,0.064,0.007,0.010
[run_subscribe_msckf-1] VioManager.cpp:669 cam1 intrinsics = 458.663,455.529,380.500,257.629 | -0.278,0.062,0.007,0.009
[run_subscribe_msckf-1] VioManager.cpp:678 cam0 extrinsics = -0.003,0.021,0.715,0.699 | 0.073,0.056,0.085
[run_subscribe_msckf-1] VioManager.cpp:678 cam1 extrinsics = 0.002,0.025,0.715,0.698 | -0.028,0.057,0.084
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0467 seconds total (21.4 hz, 38.14 ms behind)
[run_subscribe_msckf-1] VioManager.cpp:613 [TIME]: 0.0117 seconds for tracking
[run_subscribe_msckf-1] VioManager.cpp:614 [TIME]: 0.0005 seconds for propagation
[run_subscribe_msckf-1] VioManager.cpp:615 [TIME]: 0.0007 seconds for MSCKF update (0 feats)
[run_subscribe_msckf-1] VioManager.cpp:617 [TIME]: 0.0143 seconds for SLAM update (42 feats)
[run_subscribe_msckf-1] VioManager.cpp:618 [TIME]: 0.0020 seconds for SLAM delayed init (2 feats)
[run_subscribe_msckf-1] VioManager.cpp:620 [TIME]: 0.0068 seconds for re-tri & marg (11 clones in state)
[run_subscribe_msckf-1] VioManager.cpp:628 [TIME]: 0.03599 seconds for total (camera 0 1)
[run_subscribe_msckf-1] VioManager.cpp:654 q_GtoI = 0.813,-0.116,0.564,0.083 | p_IinG = 0.271,0.064,0.314 | dist = 0.46 (meters)
[run_subscribe_msckf-1] VioManager.cpp:657 bg = 0.0001,0.0260,0.0774 | ba = -0.1217,-0.0490,0.0044
[run_subscribe_msckf-1] VioManager.cpp:662 camera-imu timeoffset = -0.00210
[run_subscribe_msckf-1] VioManager.cpp:669 cam0 intrinsics = 459.323,456.221,367.355,249.742 | -0.281,0.064,0.007,0.010
[run_subscribe_msckf-1] VioManager.cpp:669 cam1 intrinsics = 458.516,455.570,380.289,257.575 | -0.278,0.062,0.007,0.009
[run_subscribe_msckf-1] VioManager.cpp:678 cam0 extrinsics = -0.003,0.022,0.715,0.699 | 0.071,0.054,0.090
[run_subscribe_msckf-1] VioManager.cpp:678 cam1 extrinsics = 0.001,0.025,0.716,0.698 | -0.030,0.056,0.089
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0368 seconds total (27.2 hz, 33.14 ms behind)
[run_subscribe_msckf-1] VioManager.cpp:613 [TIME]: 0.0124 seconds for tracking
[run_subscribe_msckf-1] VioManager.cpp:614 [TIME]: 0.0008 seconds for propagation
[run_subscribe_msckf-1] VioManager.cpp:615 [TIME]: 0.0040 seconds for MSCKF update (1 feats)
[run_subscribe_msckf-1] VioManager.cpp:617 [TIME]: 0.0176 seconds for SLAM update (41 feats)
[run_subscribe_msckf-1] VioManager.cpp:618 [TIME]: 0.0000 seconds for SLAM delayed init (0 feats)
[run_subscribe_msckf-1] VioManager.cpp:620 [TIME]: 0.0068 seconds for re-tri & marg (11 clones in state)
[run_subscribe_msckf-1] VioManager.cpp:628 [TIME]: 0.04156 seconds for total (camera 0 1)
[run_subscribe_msckf-1] VioManager.cpp:654 q_GtoI = 0.813,-0.120,0.564,0.085 | p_IinG = 0.282,0.054,0.319 | dist = 0.47 (meters)
[run_subscribe_msckf-1] VioManager.cpp:657 bg = -0.0004,0.0258,0.0769 | ba = -0.1329,-0.0534,-0.0165
[run_subscribe_msckf-1] VioManager.cpp:662 camera-imu timeoffset = -0.00225
[run_subscribe_msckf-1] VioManager.cpp:669 cam0 intrinsics = 459.118,456.268,367.285,249.635 | -0.280,0.064,0.007,0.009
[run_subscribe_msckf-1] VioManager.cpp:669 cam1 intrinsics = 458.462,455.659,380.236,257.436 | -0.278,0.062,0.007,0.009
[run_subscribe_msckf-1] VioManager.cpp:678 cam0 extrinsics = -0.003,0.022,0.715,0.699 | 0.072,0.052,0.097
[run_subscribe_msckf-1] VioManager.cpp:678 cam1 extrinsics = 0.001,0.026,0.716,0.698 | -0.028,0.054,0.096
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0437 seconds total (22.9 hz, 37.71 ms behind)
[run_subscribe_msckf-1] VioManager.cpp:613 [TIME]: 0.0147 seconds for tracking
[run_subscribe_msckf-1] VioManager.cpp:614 [TIME]: 0.0007 seconds for propagation
[run_subscribe_msckf-1] VioManager.cpp:615 [TIME]: 0.0016 seconds for MSCKF update (0 feats)
[run_subscribe_msckf-1] VioManager.cpp:617 [TIME]: 0.0158 seconds for SLAM update (41 feats)
[run_subscribe_msckf-1] VioManager.cpp:618 [TIME]: 0.0067 seconds for SLAM delayed init (8 feats)
[run_subscribe_msckf-1] VioManager.cpp:620 [TIME]: 0.0094 seconds for re-tri & marg (11 clones in state)
[run_subscribe_msckf-1] VioManager.cpp:628 [TIME]: 0.04888 seconds for total (camera 0 1)
[run_subscribe_msckf-1] VioManager.cpp:654 q_GtoI = 0.812,-0.124,0.564,0.087 | p_IinG = 0.281,0.048,0.321 | dist = 0.49 (meters)
[run_subscribe_msckf-1] VioManager.cpp:657 bg = -0.0009,0.0250,0.0766 | ba = -0.1402,-0.0701,-0.0334
[run_subscribe_msckf-1] VioManager.cpp:662 camera-imu timeoffset = -0.00219
[run_subscribe_msckf-1] VioManager.cpp:669 cam0 intrinsics = 458.923,456.335,367.434,249.583 | -0.280,0.064,0.007,0.009
[run_subscribe_msckf-1] VioManager.cpp:669 cam1 intrinsics = 458.484,455.837,380.290,257.370 | -0.279,0.062,0.007,0.009
[run_subscribe_msckf-1] VioManager.cpp:678 cam0 extrinsics = -0.003,0.022,0.715,0.698 | 0.077,0.050,0.101
[run_subscribe_msckf-1] VioManager.cpp:678 cam1 extrinsics = 0.001,0.025,0.716,0.698 | -0.022,0.052,0.100
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0497 seconds total (20.1 hz, 37.73 ms behind)
[run_subscribe_msckf-1] VioManager.cpp:613 [TIME]: 0.0127 seconds for tracking
[run_subscribe_msckf-1] VioManager.cpp:614 [TIME]: 0.0008 seconds for propagation
[run_subscribe_msckf-1] VioManager.cpp:615 [TIME]: 0.0003 seconds for MSCKF update (0 feats)
[run_subscribe_msckf-1] VioManager.cpp:617 [TIME]: 0.0091 seconds for SLAM update (45 feats)
[run_subscribe_msckf-1] VioManager.cpp:618 [TIME]: 0.0141 seconds for SLAM delayed init (5 feats)
[run_subscribe_msckf-1] VioManager.cpp:620 [TIME]: 0.0065 seconds for re-tri & marg (11 clones in state)
[run_subscribe_msckf-1] VioManager.cpp:628 [TIME]: 0.04354 seconds for total (camera 0 1)
[run_subscribe_msckf-1] VioManager.cpp:654 q_GtoI = 0.810,-0.129,0.564,0.091 | p_IinG = 0.287,0.045,0.324 | dist = 0.50 (meters)
[run_subscribe_msckf-1] VioManager.cpp:657 bg = -0.0010,0.0245,0.0764 | ba = -0.1413,-0.0783,-0.0435
[run_subscribe_msckf-1] VioManager.cpp:662 camera-imu timeoffset = -0.00246
[run_subscribe_msckf-1] VioManager.cpp:669 cam0 intrinsics = 459.104,456.221,367.500,249.565 | -0.280,0.064,0.007,0.009
[run_subscribe_msckf-1] VioManager.cpp:669 cam1 intrinsics = 458.841,455.785,380.265,257.381 | -0.279,0.062,0.007,0.009
[run_subscribe_msckf-1] VioManager.cpp:678 cam0 extrinsics = -0.003,0.022,0.716,0.698 | 0.079,0.050,0.101
[run_subscribe_msckf-1] VioManager.cpp:678 cam1 extrinsics = 0.001,0.025,0.716,0.697 | -0.020,0.052,0.101
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0444 seconds total (22.5 hz, 37.72 ms behind)
[run_subscribe_msckf-1] VioManager.cpp:613 [TIME]: 0.0113 seconds for tracking
[run_subscribe_msckf-1] VioManager.cpp:614 [TIME]: 0.0008 seconds for propagation
[run_subscribe_msckf-1] VioManager.cpp:615 [TIME]: 0.0317 seconds for MSCKF update (8 feats)
[run_subscribe_msckf-1] VioManager.cpp:617 [TIME]: 0.0100 seconds for SLAM update (47 feats)
[run_subscribe_msckf-1] VioManager.cpp:618 [TIME]: 0.0184 seconds for SLAM delayed init (4 feats)
[run_subscribe_msckf-1] VioManager.cpp:620 [TIME]: 0.0062 seconds for re-tri & marg (11 clones in state)

I set the number of features to 150/100/70, and it still diverges
I set use_stereo:=false and max_cameras:=1, but it also diverges

Jun 26 '23 09:06 mzahana

Wow this is really slow :0.0317 seconds for MSCKF update. Maybe there are some compiler flags that make the eigen operations much slower. You can try to run the ov_core test_profile script and see if that shows anything.

Jun 26 '23 14:06 goldbattle

Here is the output of ros2 run ov_core test_profile

Setting printing level to: INFO
OPENCV: RANDOM BIG IMAGE: 21.280 +- 8.770ms
OPENCV: HISTOGRAM EQUALIZATION: 8.874 +- 7.351ms
OPENCV: BUILD OPTICAL FLOW PYRAMID: 5.000 +- 5.773ms
OPENCV: FAST FEATURE EXTRACTION: 33.970 +- 8.910 ms (90402.12 avg feats)
OPENCV: KLT OPTICAL FLOW: 18.359 +- 3.239 ms (415.51 avg feats)
EIGEN3(double): RANDOM BIG MATRIX: 6.252 +- 0.983ms
EIGEN3(double): MATRIX MULTIPLY: 9.013 +- 1.296ms
EIGEN3(double): MATRIX INVERSION: 17.601 +- 1.774ms
EIGEN3(double): HOUSEHOLDER QR FULL: 36.662 +- 2.900ms
EIGEN3(double): HOUSEHOLDER QR PIV: 44.418 +- 2.914ms
EIGEN3(double): HOUSEHOLDER QR CUSTOM: 43.052 +- 2.203ms
EIGEN3(float): RANDOM BIG MATRIX: 4.720 +- 0.796ms
EIGEN3(float): MATRIX MULTIPLY: 4.409 +- 0.576ms
EIGEN3(float): MATRIX INVERSION: 11.001 +- 1.417ms
EIGEN3(float): HOUSEHOLDER QR FULL: 167.093 +- 5.698ms
EIGEN3(float): HOUSEHOLDER QR PIV: 24.115 +- 1.516ms
EIGEN3(float): HOUSEHOLDER QR CUSTOM: 22.997 +- 1.670ms

Jun 26 '23 16:06 mzahana

We were able to run openvins with a T265 on Jetson NANO in real-time with the following important changes in the default config of EurocMAV:

max_slam: 25
max_slam_in_update: 25
max_msckf_in_update: 20
num_pts: 50
downsample_cameras: true #this reduces the tracking time and CPU workload by a lot

The runtime statistics are

 mean_time = 0.0192 | std = 0.0084 | 99th = 0.0388  | max = 0.0603 (tracking)
mean_time = 0.0006 | std = 0.0002 | 99th = 0.0012  | max = 0.0092 (propagation)
mean_time = 0.0008 | std = 0.0013 | 99th = 0.0039  | max = 0.0217 (msckf update)
mean_time = 0.0030 | std = 0.0022 | 99th = 0.0082  | max = 0.0200 (slam update)
mean_time = 0.0023 | std = 0.0085 | 99th = 0.0222  | max = 0.0915 (slam delayed)
mean_time = 0.0003 | std = 0.0002 | 99th = 0.0008  | max = 0.0088 (re-tri & marg)
mean_time = 0.0263 | std = 0.0121 | 99th = 0.0543  | max = 0.1129 (total)

Jun 26 '23 19:06 HoEmpire

@HoEmpire I tried your configs (on V1_01_easy, EuRoc dataset), but the estimation drifts a lot. Can you share how you setup open_vins on Jetons Nano? Maybe I am missin something related to the setup of the dependencies such as opencv, Eigen, Ceres

Jun 27 '23 14:06 mzahana

The Jetson Nano is not with me at this moment, so I am not sure what versions the opencv and Eigen are. We only tested this config live running openvins with a real sense T265, the trajectory looks reasonably good, but we didn't evaluate the accuracy. And we didn't test this configuration on EuRoc dataset at that time. I just quickly checked this config on my laptop (opencv 4.2.0, eigen 3.3.7, ceres should not be a problem if not using dynamic initialization if I remember correctly) and got the following result

============================================
ATE LATEX TABLE
============================================
 & \textbf{V1\_01\_easy} & \textbf{V1\_02\_medium} & \textbf{V1\_03\_difficult} & \textbf{V2\_02\_medium}  \\\hline
openvins & 0.827 / 0.061 & 1.585 / 0.135 & 3.000 / 0.142 & 1.072 / 0.120  \\
============================================

It failed on V201 and V203. But at least I assume this config should be able to run on V101 if the tracking performs normally. You can try uncommenting these two lines(https://github.com/rpng/open_vins/blob/master/ov_msckf/src/ros1_serial_msckf.cpp#L66-L67) to disable multi-threading and rebuild. Then roslaunch ov_msckf serial.launch to run things in serial to see if it helps.

Jun 27 '23 18:06 HoEmpire

When I ran the euroc MH_01_easy dataset, it drifts far away quickly, but if I set the downsample_cameras: ture, the estimation was just OK.

Jun 29 '23 07:06 WZG3661

You should take a look at the script folder to see how we run all the datasets. There are some non-static periods in the machine hall which require addressing to initialize if I remember correctly.

On Thu, Jun 29, 2023 at 12:07 AM WZG3661 @.***> wrote:

When I ran the euroc MH_01_easy dataset, it drifts far away quickly, but if I set the downsample_cameras: ture, the estimation was just OK.

— Reply to this email directly, view it on GitHub https://github.com/rpng/open_vins/issues/350#issuecomment-1612529161, or unsubscribe https://github.com/notifications/unsubscribe-auth/AAQ6TYVCY3A52OXZFCHK25LXNUST3ANCNFSM6AAAAAAZSZHCXE . You are receiving this because you were mentioned.Message ID: @.***>

Jun 29 '23 16:06 goldbattle

@WZG3661 I set downsample_cameras: ture, but still the estimation diverges.

@goldbattle I saw the scripts folder. For the easy ros bag, I saw the start time is at 0, so there is no difference to my case as I start estimator first and then play the bag.

I am not sure what is missing.

Jun 30 '23 20:06 mzahana

One difference is that it is over the ROS communication instead of reading the bag directly, so this can cause some differences. There might be some issues with limited resources and the current multi-threading implementation, I am not sure. I know one problem is that the KLT tracking from OpenCV will have lag spikes which could also cause a problem. Unfortunately I can't debug much from here for you.

Oct 28 '23 18:10 goldbattle

OpenVINS works fine with some tuning for ROS1. Once we switched to ROS2 and solved intra-process communication between the realsense camera and OpenVINS, the estimation diverge was worse than ROS1. The pose can float around when the stereo camera is almost static and stereo VIO can lose tracking very easily.

I am also suspecting multi-threading implementation for ROS2 has some issues. To be clear, we have made our own composable node wrapper and compose both realsenseu node and openvins node into one container, and launched the container with the multi-threading mode.

Dec 07 '23 17:12 makeecat

open_vins open_vins copied to clipboard

Estimation diverges on Xavier NX with EuRoc dataset

open_vins
open_vins copied to clipboard