/SLAM-Hive/slam_hive_results/mapping_results/2832/2040_vins-fusion-mono-imu+MH_02_easy+Image_comb_test-0.yamlERROR: Unable to communicate with master!
[31m[ERROR] [1720468908.277254715]: [registerPublisher] Failed to contact master at [localhost:11311].  Retrying...[0m
[0m[ INFO] [1720468908.279367336]: init begins[0m
[31m[ERROR] [1720468908.280447919]: [registerPublisher] Failed to contact master at [localhost:11311].  Retrying...[0m
... logging to /root/.ros/log/e8c89d82-3d64-11ef-8382-0242ac110005/roslaunch-6a58f75b5de2-118.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.
]2;roscore
[1mstarted roslaunch server http://6a58f75b5de2:46761/[0m
ros_comm version 1.12.17


SUMMARY
========

PARAMETERS
 * /rosdistro: kinetic
 * /rosversion: 1.12.17

NODES

auto-starting new master
[1mprocess[master]: started with pid [155][0m
[1mROS_MASTER_URI=http://6a58f75b5de2:11311/[0m
]2;roscore http://6a58f75b5de2:11311/
[1msetting /run_id to e8c89d82-3d64-11ef-8382-0242ac110005[0m
[0m[ INFO] [1720468909.432876755]: Connected to master at [localhost:11311][0m
[0m[ INFO] [1720468909.435693631]: Connected to master at [localhost:11311][0m
config_file: /slamhive/mappingtask.yaml
USE_IMU: 1
IMU_TOPIC: /imu0
result path /home/output/vio.csv
[33m[ WARN] [1720468909.442906591]:  fix extrinsic param [0m
camera number 1
[0m[ INFO] [1720468909.443125615]: Synchronized sensors, fix time offset: 0[0m
[0m[ INFO] [1720468909.443154443]: ROW: 480 COL: 752 [0m
 exitrinsic cam 0
 0.0148655  -0.999881  0.0041403
  0.999557  0.0149672  0.0257155
-0.0257744 0.00375619   0.999661
-0.0216401  -0.064677 0.00981073
set g       0       0 9.81007
[0m[ INFO] [1720468909.443297077]: reading paramerter of camera /slamhive/cam0_pinhole.yaml[0m
MULTIPLE_THREAD is 1
[33m[ WARN] [1720468909.443561519]: waiting for image and imu...[0m
config_file: /slamhive/mappingtask.yaml
vocabulary_file/home/catkin_ws/src/VINS-Fusion/loop_fusion/../support_files/brief_k10L6.bin
loop start load vocabulary
BRIEF_PATTERN_FILE/home/catkin_ws/src/VINS-Fusion/loop_fusion/../support_files/brief_pattern.yml
cam calib path: /slamhive/cam0_pinhole.yaml
VIO input, perfrom 4 DoF (x, y, z, yaw) pose graph optimization
no previous pose graph
[1mprocess[rosout-1]: started with pid [211][0m
started core service [/rosout]
rosbag play /slamhive/dataset/MH_02_easy.bag  --clock 
[0m[ INFO] [1720468918.428417274]: Opening /slamhive/dataset/MH_02_easy.bag[0m

Waiting 0.2 seconds after advertising topics... done.

Hit space to toggle paused, or 's' to step.

 [DELAYED]  Bag Time: 1403636856.854208   Duration: 0.000000 / 153.782490   Delay: 316832061.81 
init first imu pose
averge acc 9.439445 -0.460368 -3.138128
init R0 
  0.318508   0.137589   0.937882
         0   -0.98941   0.145148
   0.94792 -0.0462307  -0.315135
[33m[ WARN] [1720468921.887896677]: gyroscope bias initial calibration 0.00132595  0.0080178  0.0885517[0m
[0m[ INFO] [1720468921.955092655]: Initialization finish![0m
time: 1403636859.701667, t: 0.010694 -0.066735 0.228004 q: -0.034346 0.812546 0.043890 0.580226 
time: 1403636859.801666, t: 0.014216 -0.068584 0.277785 q: -0.025490 0.805992 0.043507 0.589775 
time: 1403636859.901666, t: 0.014907 -0.069392 0.323274 q: -0.018283 0.802117 0.044408 0.595233 
time: 1403636860.001667, t: 0.013836 -0.070242 0.358400 q: -0.015820 0.801160 0.043009 0.596693 
time: 1403636860.101666, t: 0.015103 -0.074213 0.368920 q: -0.014423 0.803684 0.039460 0.593572 
time: 1403636860.201667, t: 0.016765 -0.080103 0.346766 q: -0.016564 0.808535 0.037679 0.587007 
time: 1403636860.301666, t: 0.016667 -0.083927 0.297796 q: -0.022747 0.816980 0.035522 0.575121 
time: 1403636860.401666, t: 0.013862 -0.087864 0.239806 q: -0.025455 0.829763 0.031997 0.556616 
time: 1403636860.501667, t: 0.009167 -0.091130 0.183801 q: -0.028847 0.842953 0.028504 0.536456 
time: 1403636860.601666, t: 0.004872 -0.093146 0.138697 q: -0.033949 0.852877 0.027019 0.520306 
time: 1403636860.701667, t: -0.000238 -0.094935 0.108742 q: -0.038313 0.856595 0.029865 0.513698 
time: 1403636860.801666, t: -0.003584 -0.096244 0.100685 q: -0.034775 0.856318 0.036358 0.513992 
time: 1403636860.901666, t: -0.004851 -0.097032 0.120484 q: -0.032178 0.855613 0.042582 0.514857 
wait for imu ... 
time: 1403636861.001667, t: -0.003583 -0.097617 0.164560 q: -0.031837 0.853383 0.046881 0.518195 
time: 1403636861.101666, t: -0.000095 -0.098372 0.222850 q: -0.032041 0.846020 0.046827 0.530123 
time: 1403636861.201667, t: 0.004795 -0.099480 0.285782 q: -0.031626 0.833948 0.046351 0.548982 
time: 1403636861.301666, t: 0.008526 -0.101399 0.348066 q: -0.032970 0.820219 0.045628 0.569274 
time: 1403636861.401666, t: 0.010915 -0.104301 0.398998 q: -0.034363 0.808980 0.042004 0.585326 
time: 1403636861.501667, t: 0.012726 -0.107327 0.423335 q: -0.033828 0.801942 0.039365 0.595143 
time: 1403636861.701667, t: 0.014872 -0.111024 0.376858 q: -0.033478 0.809222 0.039203 0.585236 
time: 1403636861.801666, t: 0.014224 -0.110552 0.323931 q: -0.032740 0.816141 0.041709 0.575415 
time: 1403636861.901666, t: 0.010792 -0.108278 0.266571 q: -0.036175 0.823974 0.043552 0.563792 
time: 1403636862.001667, t: 0.006711 -0.104412 0.208343 q: -0.040120 0.835571 0.043078 0.546220 
time: 1403636862.101666, t: 0.002628 -0.099655 0.155490 q: -0.043550 0.844759 0.040871 0.531804 
time: 1403636862.201667, t: -0.000230 -0.096334 0.116830 q: -0.046477 0.849445 0.041847 0.523959 
time: 1403636862.301666, t: -0.002609 -0.096019 0.103979 q: -0.041058 0.853087 0.047335 0.517993 
time: 1403636862.401666, t: -0.002454 -0.096361 0.124248 q: -0.039586 0.856129 0.050550 0.512758 
time: 1403636862.501667, t: -0.000663 -0.097207 0.171698 q: -0.038481 0.854045 0.051014 0.516260 
time: 1403636862.601666, t: 0.005138 -0.100136 0.233608 q: -0.027584 0.848081 0.049285 0.526848 
time: 1403636862.701667, t: 0.010984 -0.101787 0.301450 q: -0.019610 0.840268 0.046875 0.539786 
wait for imu ... 
time: 1403636862.801666, t: 0.015494 -0.101665 0.370031 q: -0.021384 0.835441 0.039906 0.547712 
time: 1403636862.901666, t: 0.017577 -0.101615 0.434274 q: -0.024509 0.832297 0.031308 0.552902 
time: 1403636863.001667, t: 0.016143 -0.101741 0.478215 q: -0.029275 0.826715 0.027740 0.561174 
time: 1403636863.101666, t: 0.015075 -0.103783 0.487335 q: -0.023364 0.820667 0.032259 0.570017 
time: 1403636863.201667, t: 0.014314 -0.105147 0.462054 q: -0.015303 0.818164 0.037778 0.573539 
time: 1403636863.301666, t: 0.013812 -0.104588 0.410538 q: -0.009526 0.819530 0.041383 0.571460 
time: 1403636863.401666, t: 0.013519 -0.104529 0.346943 q: -0.003639 0.821763 0.040930 0.568345 
time: 1403636863.501667, t: 0.011597 -0.104333 0.278024 q: -0.004113 0.826554 0.036359 0.561667 
time: 1403636863.601666, t: 0.006469 -0.103740 0.210167 q: -0.016187 0.834513 0.033582 0.549726 
time: 1403636863.701667, t: 0.000992 -0.103951 0.151462 q: -0.026156 0.843011 0.031031 0.536363 
time: 1403636863.801666, t: -0.001996 -0.104981 0.113233 q: -0.030381 0.850884 0.027956 0.523729 
wait for imu ... 
time: 1403636863.901666, t: -0.004469 -0.106489 0.108122 q: -0.031017 0.856472 0.029321 0.514426 
time: 1403636864.001667, t: -0.005223 -0.107864 0.139760 q: -0.033580 0.857461 0.032321 0.512433 
time: 1403636864.101666, t: -0.004436 -0.108556 0.198998 q: -0.031885 0.853843 0.037348 0.518209 
time: 1403636864.201667, t: -0.001291 -0.109177 0.271899 q: -0.023703 0.849218 0.039358 0.526039 
time: 1403636864.301666, t: 0.002335 -0.107339 0.342300 q: -0.022605 0.846186 0.039258 0.530959 
time: 1403636864.401666, t: 0.005089 -0.104496 0.403923 q: -0.024630 0.842305 0.036777 0.537180 
time: 1403636864.501667, t: 0.007733 -0.102670 0.455175 q: -0.026598 0.832430 0.029165 0.552723 
time: 1403636864.601666, t: 0.010401 -0.099835 0.483756 q: -0.031058 0.820892 0.023386 0.569759 
time: 1403636864.801666, t: 0.010747 -0.100175 0.445697 q: -0.019788 0.818151 0.031745 0.573785 
time: 1403636864.901666, t: 0.011038 -0.099446 0.391070 q: -0.018089 0.823504 0.032456 0.566093 
time: 1403636865.001667, t: 0.010887 -0.100318 0.330257 q: -0.011471 0.829772 0.031798 0.557077 
time: 1403636865.101666, t: 0.011286 -0.100783 0.268557 q: -0.010867 0.836944 0.031209 0.546289 
time: 1403636865.201667, t: 0.011440 -0.099881 0.213016 q: -0.026084 0.842394 0.022620 0.537754 
time: 1403636865.301666, t: 0.010508 -0.099222 0.170196 q: -0.039759 0.844774 0.016930 0.533376 
time: 1403636865.401666, t: 0.010528 -0.099216 0.143320 q: -0.039012 0.845831 0.016656 0.531762 
time: 1403636865.501667, t: 0.010813 -0.099041 0.137567 q: -0.031649 0.850399 0.022873 0.524688 
wait for imu ... 
time: 1403636865.601666, t: 0.011744 -0.097468 0.160520 q: -0.028651 0.855873 0.028875 0.515583 
time: 1403636865.701667, t: 0.011699 -0.093129 0.207799 q: -0.032930 0.855287 0.031937 0.516120 
time: 1403636865.801666, t: 0.013139 -0.089087 0.269385 q: -0.035949 0.848597 0.031414 0.526882 
time: 1403636865.901666, t: 0.015834 -0.087105 0.337465 q: -0.038925 0.838725 0.028854 0.542395 
time: 1403636866.001667, t: 0.017417 -0.085789 0.406041 q: -0.042716 0.824810 0.025471 0.563219 
time: 1403636866.101666, t: 0.016718 -0.085404 0.467661 q: -0.041536 0.809104 0.021806 0.585790 
time: 1403636866.201667, t: 0.013590 -0.085768 0.510519 q: -0.037838 0.798977 0.022602 0.599744 
time: 1403636866.301666, t: 0.011208 -0.085961 0.522881 q: -0.031697 0.798352 0.024320 0.600865 
time: 1403636866.401666, t: 0.011335 -0.086504 0.504300 q: -0.023634 0.804569 0.025745 0.592831 
time: 1403636866.501667, t: 0.011808 -0.087139 0.461230 q: -0.020385 0.811298 0.027312 0.583638 
time: 1403636866.601666, t: 0.012776 -0.089659 0.404615 q: -0.017546 0.817798 0.027656 0.574572 
time: 1403636866.701667, t: 0.012907 -0.095041 0.351338 q: -0.013184 0.825128 0.027901 0.564103 
time: 1403636866.801666, t: 0.008194 -0.099656 0.309010 q: -0.016663 0.830920 0.031847 0.555230 
time: 1403636866.901666, t: 0.003844 -0.101822 0.273719 q: -0.028512 0.837418 0.034227 0.544745 
time: 1403636867.001667, t: -0.003478 -0.103008 0.246172 q: -0.037791 0.845978 0.034094 0.530783 
time: 1403636867.101666, t: -0.009374 -0.103228 0.229483 q: -0.038362 0.852207 0.034079 0.520682 
time: 1403636867.201667, t: -0.018515 -0.103134 0.222581 q: -0.045398 0.855356 0.036590 0.514748 
time: 1403636867.301666, t: -0.029046 -0.102479 0.222408 q: -0.054580 0.857430 0.043045 0.509884 
time: 1403636867.401666, t: -0.036664 -0.102965 0.226673 q: -0.054852 0.860845 0.045621 0.503841 
time: 1403636867.501667, t: -0.044590 -0.102842 0.232745 q: -0.050670 0.867615 0.046497 0.492457 
time: 1403636867.601666, t: -0.053680 -0.103500 0.234482 q: -0.044748 0.882766 0.050387 0.464954 
time: 1403636867.701667, t: -0.064372 -0.104572 0.230561 q: -0.043480 0.906660 0.051077 0.416494 
optimize pose graph 
time: 1403636867.801666, t: -0.074562 -0.106903 0.224825 q: -0.039105 0.930504 0.049172 0.360853 
wait for imu ... 
time: 1403636867.901666, t: -0.080126 -0.108960 0.222851 q: -0.035883 0.943214 0.043704 0.327339 
time: 1403636868.001667, t: -0.080135 -0.108042 0.226755 q: -0.036155 0.943910 0.039117 0.325878 
time: 1403636868.101666, t: -0.080149 -0.106655 0.232543 q: -0.037204 0.938750 0.038159 0.340454 
time: 1403636868.201667, t: -0.083231 -0.105515 0.235913 q: -0.039820 0.934971 0.038401 0.350385 
time: 1403636868.301666, t: -0.089256 -0.103537 0.237069 q: -0.042044 0.934986 0.036964 0.350238 
time: 1403636868.401666, t: -0.097139 -0.098997 0.237225 q: -0.043780 0.936298 0.033692 0.346833 
time: 1403636868.501667, t: -0.105097 -0.092523 0.239022 q: -0.041931 0.934009 0.031169 0.353409 
time: 1403636868.601666, t: -0.112300 -0.086601 0.244044 q: -0.038267 0.930561 0.026438 0.363170 
time: 1403636868.701667, t: -0.119801 -0.082537 0.248733 q: -0.036661 0.930798 0.024007 0.362897 
time: 1403636868.801666, t: -0.128332 -0.078461 0.249953 q: -0.036363 0.933530 0.029049 0.355465 
time: 1403636868.901666, t: -0.133387 -0.074866 0.250724 q: -0.032540 0.934348 0.033733 0.353267 
time: 1403636869.001667, t: -0.135067 -0.070070 0.252083 q: -0.034830 0.932029 0.035705 0.358935 
time: 1403636869.101666, t: -0.135555 -0.066349 0.253313 q: -0.040901 0.930106 0.037115 0.363116 
time: 1403636869.201667, t: -0.136066 -0.063316 0.252271 q: -0.044118 0.929605 0.039514 0.363768 
time: 1403636869.301666, t: -0.135181 -0.062026 0.250600 q: -0.044252 0.929688 0.041547 0.363312 
time: 1403636869.401666, t: -0.133535 -0.061397 0.249440 q: -0.045743 0.929495 0.042447 0.363517 
time: 1403636869.501667, t: -0.131752 -0.063201 0.248621 q: -0.046162 0.929431 0.042410 0.363632 
time: 1403636869.601666, t: -0.128690 -0.071744 0.247742 q: -0.045712 0.929591 0.041654 0.363367 
time: 1403636869.701667, t: -0.123205 -0.090136 0.246754 q: -0.043971 0.931139 0.040613 0.359719 
optimize pose graph 
time: 1403636869.801666, t: -0.114269 -0.117639 0.246485 q: -0.041610 0.932269 0.037074 0.357447 
time: 1403636869.901666, t: -0.100551 -0.153025 0.247366 q: -0.038175 0.930819 0.030139 0.362229 
time: 1403636870.001667, t: -0.084282 -0.194695 0.247580 q: -0.035161 0.929018 0.022430 0.367676 
time: 1403636870.101666, t: -0.067058 -0.241212 0.246840 q: -0.031297 0.929983 0.013789 0.366008 
time: 1403636870.201667, t: -0.048870 -0.290950 0.247217 q: -0.024277 0.932376 0.009181 0.360558 
time: 1403636870.301666, t: -0.030008 -0.340504 0.248347 q: -0.021691 0.932074 0.005056 0.361583 
time: 1403636870.401666, t: -0.011912 -0.389956 0.248615 q: -0.016985 0.929314 0.000496 0.368900 
time: 1403636870.501667, t: 0.003033 -0.434152 0.245450 q: -0.014375 0.928808 -0.004817 0.370251 
time: 1403636870.601666, t: 0.013702 -0.470353 0.240550 q: -0.011373 0.931652 -0.011604 0.362989 
time: 1403636870.801666, t: 0.030318 -0.517668 0.233081 q: -0.006313 0.929374 -0.020510 0.368516 
time: 1403636870.901666, t: 0.034133 -0.528835 0.228246 q: -0.009906 0.925376 -0.022067 0.378278 
time: 1403636871.001667, t: 0.034839 -0.533418 0.223700 q: -0.014918 0.923381 -0.024021 0.382841 
time: 1403636871.101666, t: 0.033954 -0.533197 0.218358 q: -0.016370 0.926160 -0.027842 0.375746 
time: 1403636871.201667, t: 0.032183 -0.529089 0.217477 q: -0.009570 0.929988 -0.030317 0.366213 
time: 1403636871.301666, t: 0.029454 -0.517228 0.221045 q: -0.009099 0.929901 -0.031559 0.366341 
time: 1403636871.401666, t: 0.024103 -0.496536 0.224934 q: -0.008343 0.929428 -0.027346 0.367894 
time: 1403636871.501667, t: 0.017023 -0.465227 0.228040 q: -0.010042 0.929088 -0.023246 0.368991 
time: 1403636871.601666, t: 0.007890 -0.424835 0.230933 q: -0.012390 0.929606 -0.021136 0.367740 
time: 1403636871.801666, t: -0.014106 -0.323038 0.238439 q: -0.027477 0.930761 -0.013990 0.364326 
time: 1403636871.901666, t: -0.023800 -0.267658 0.239402 q: -0.031043 0.930177 -0.006962 0.365731 
time: 1403636872.001667, t: -0.032126 -0.208572 0.239043 q: -0.039695 0.927800 -0.001261 0.370959 
time: 1403636872.101666, t: -0.039439 -0.149957 0.236795 q: -0.049281 0.925103 0.005350 0.376466 
time: 1403636872.201667, t: -0.047255 -0.097028 0.233288 q: -0.053456 0.924556 0.011754 0.377095 
time: 1403636872.301666, t: -0.056102 -0.049548 0.228173 q: -0.058346 0.926667 0.019539 0.370813 
time: 1403636872.401666, t: -0.064030 -0.011576 0.223241 q: -0.062384 0.929106 0.027186 0.363498 
time: 1403636872.501667, t: -0.068219 0.012907 0.220790 q: -0.064941 0.929745 0.031764 0.361038 
time: 1403636872.601666, t: -0.070467 0.022159 0.221768 q: -0.066539 0.928371 0.036464 0.363826 
time: 1403636872.701667, t: -0.070670 0.019363 0.224052 q: -0.065521 0.926505 0.039342 0.368440 
time: 1403636872.901666, t: -0.062407 -0.019470 0.225253 q: -0.059072 0.927841 0.033677 0.366724 
time: 1403636873.001667, t: -0.052800 -0.058890 0.223775 q: -0.053844 0.928654 0.027970 0.365951 
time: 1403636873.101666, t: -0.040529 -0.107920 0.221769 q: -0.048825 0.928820 0.022500 0.366610 
time: 1403636873.201667, t: -0.026323 -0.163216 0.220352 q: -0.045016 0.929761 0.015027 0.365092 
time: 1403636873.301666, t: -0.010930 -0.222433 0.219526 q: -0.040560 0.930566 0.006490 0.363811 
time: 1403636873.401666, t: 0.003022 -0.282513 0.218276 q: -0.036181 0.931124 -0.000658 0.362904 
time: 1403636873.501667, t: 0.014727 -0.340186 0.215390 q: -0.032845 0.931970 -0.006966 0.360977 
time: 1403636873.601666, t: 0.022557 -0.392887 0.211438 q: -0.025490 0.933651 -0.014022 0.357001 
time: 1403636873.701667, t: 0.028720 -0.436527 0.205626 q: -0.019887 0.935013 -0.020054 0.353488 
time: 1403636873.801666, t: 0.031404 -0.468305 0.200441 q: -0.019293 0.936081 -0.021403 0.350601 
time: 1403636873.901666, t: 0.031964 -0.486654 0.196665 q: -0.019449 0.936004 -0.023768 0.350646 
time: 1403636874.001667, t: 0.032166 -0.493345 0.193914 q: -0.017479 0.935363 -0.026807 0.352238 
time: 1403636874.101666, t: 0.032214 -0.491425 0.192581 q: -0.016755 0.935368 -0.027917 0.352173 
time: 1403636874.201667, t: 0.031479 -0.481974 0.193385 q: -0.017373 0.936024 -0.026911 0.350476 
time: 1403636874.301666, t: 0.028842 -0.461720 0.195547 q: -0.018775 0.936948 -0.022160 0.348260 
time: 1403636874.401666, t: 0.025390 -0.427340 0.197002 q: -0.024352 0.937614 -0.016554 0.346430 
time: 1403636874.501667, t: 0.020780 -0.381597 0.198866 q: -0.029640 0.936913 -0.011255 0.348123 
time: 1403636874.601666, t: 0.014394 -0.328779 0.201135 q: -0.032477 0.935300 -0.005613 0.352317 
time: 1403636874.701667, t: 0.005997 -0.268995 0.201100 q: -0.038774 0.934090 0.001759 0.354921 
time: 1403636874.801666, t: -0.001656 -0.208953 0.198268 q: -0.044681 0.933658 0.008144 0.355275 
time: 1403636874.901666, t: -0.008773 -0.152869 0.193303 q: -0.053442 0.934056 0.012212 0.352894 
time: 1403636875.001667, t: -0.013326 -0.107234 0.189604 q: -0.058867 0.933398 0.015818 0.353628 
time: 1403636875.101666, t: -0.012929 -0.073164 0.186795 q: -0.057080 0.928726 0.010797 0.366187 
time: 1403636875.201667, t: -0.014218 -0.046836 0.184306 q: -0.049074 0.921911 -0.005752 0.384238 
time: 1403636875.301666, t: -0.020875 -0.025118 0.181103 q: -0.041912 0.917296 -0.030063 0.394852 
time: 1403636875.401666, t: -0.033297 -0.007564 0.177494 q: -0.041140 0.915965 -0.057045 0.395045 
time: 1403636875.501667, t: -0.056334 0.002615 0.175200 q: -0.044356 0.916801 -0.076923 0.389346 
time: 1403636875.601666, t: -0.089436 0.001193 0.173848 q: -0.047686 0.918012 -0.091387 0.382920 
time: 1403636875.701667, t: -0.126639 -0.013087 0.173815 q: -0.044060 0.917492 -0.104108 0.381352 
optimize pose graph 
time: 1403636875.801666, t: -0.164227 -0.037682 0.173103 q: -0.033276 0.915577 -0.119262 0.382608 
time: 1403636875.901666, t: -0.201022 -0.068647 0.172004 q: -0.020826 0.914911 -0.137243 0.379038 
time: 1403636876.001667, t: -0.233332 -0.106738 0.173691 q: -0.005960 0.912418 -0.159566 0.376825 
time: 1403636876.101666, t: -0.255785 -0.152920 0.178527 q: 0.015009 0.906608 -0.181779 0.380518 
time: 1403636876.201667, t: -0.265678 -0.202236 0.182815 q: 0.032353 0.899938 -0.201947 0.385075 
time: 1403636876.301666, t: -0.260540 -0.253529 0.183766 q: 0.043836 0.896504 -0.218264 0.383040 
time: 1403636876.401666, t: -0.242071 -0.307834 0.182798 q: 0.052373 0.895753 -0.230708 0.376375 
time: 1403636876.501667, t: -0.207705 -0.362704 0.181749 q: 0.064363 0.894511 -0.235489 0.374503 
time: 1403636876.601666, t: -0.160805 -0.411989 0.183522 q: 0.073981 0.891692 -0.229667 0.382969 
time: 1403636876.701667, t: -0.103916 -0.449639 0.186620 q: 0.076125 0.889929 -0.216389 0.394218 
time: 1403636876.801666, t: -0.043735 -0.469393 0.190497 q: 0.071831 0.891252 -0.196011 0.402603 
time: 1403636876.901666, t: 0.014900 -0.467890 0.193449 q: 0.064076 0.893650 -0.169201 0.410676 
time: 1403636877.001667, t: 0.070388 -0.444150 0.194058 q: 0.049011 0.895279 -0.138647 0.420536 
time: 1403636877.101666, t: 0.118346 -0.401364 0.193425 q: 0.028247 0.897713 -0.102111 0.427653 
time: 1403636877.201667, t: 0.155675 -0.342701 0.192528 q: 0.004054 0.900385 -0.062172 0.430611 
time: 1403636877.301666, t: 0.180380 -0.274123 0.192729 q: -0.020159 0.901910 -0.025429 0.430704 
time: 1403636877.401666, t: 0.186608 -0.201434 0.192761 q: -0.041582 0.902585 0.010984 0.428357 
time: 1403636877.501667, t: 0.176722 -0.131200 0.190729 q: -0.061861 0.903589 0.041475 0.421876 
time: 1403636877.601666, t: 0.150763 -0.070942 0.186269 q: -0.079593 0.906268 0.061096 0.410623 
time: 1403636877.701667, t: 0.112181 -0.023970 0.181615 q: -0.093341 0.909471 0.071695 0.398760 
time: 1403636877.801666, t: 0.063856 0.007371 0.178961 q: -0.100206 0.913092 0.072041 0.388628 
time: 1403636877.901666, t: 0.012116 0.024433 0.174888 q: -0.098865 0.917696 0.063077 0.379579 
time: 1403636878.001667, t: -0.040533 0.026090 0.170119 q: -0.089952 0.922748 0.049007 0.371542 
time: 1403636878.101666, t: -0.089232 0.013105 0.164681 q: -0.075416 0.926053 0.029015 0.368641 
time: 1403636878.201667, t: -0.131200 -0.015109 0.159659 q: -0.060142 0.926603 0.004104 0.371177 
time: 1403636878.301666, t: -0.165845 -0.056496 0.154781 q: -0.048060 0.925463 -0.022336 0.375113 
time: 1403636878.501667, t: -0.202851 -0.166568 0.150073 q: -0.021818 0.921462 -0.076081 0.380320 
time: 1403636878.601666, t: -0.201857 -0.229061 0.154343 q: -0.003995 0.918747 -0.099127 0.382181 
time: 1403636878.701667, t: -0.186406 -0.291279 0.161826 q: 0.014682 0.915177 -0.116729 0.385500 
time: 1403636878.801666, t: -0.160962 -0.349967 0.171269 q: 0.029971 0.910794 -0.128064 0.391351 
time: 1403636878.901666, t: -0.125141 -0.399342 0.177861 q: 0.036686 0.908326 -0.134739 0.394262 
time: 1403636879.001667, t: -0.085493 -0.440771 0.182321 q: 0.037352 0.908616 -0.137190 0.392685 
time: 1403636879.101666, t: -0.040862 -0.466835 0.187226 q: 0.035615 0.907490 -0.137413 0.395361 
time: 1403636879.201667, t: 0.007034 -0.479799 0.194461 q: 0.032531 0.902602 -0.132705 0.408216 
time: 1403636879.301666, t: 0.053095 -0.477213 0.202138 q: 0.029761 0.897903 -0.121687 0.421991 
time: 1403636879.401666, t: 0.093050 -0.458378 0.209123 q: 0.022855 0.895651 -0.101863 0.432331 
time: 1403636879.501667, t: 0.125134 -0.424235 0.214743 q: 0.009088 0.895821 -0.074727 0.437993 
time: 1403636879.601666, t: 0.144010 -0.377127 0.220455 q: -0.007134 0.897734 -0.047563 0.437904 
time: 1403636879.701667, t: 0.150532 -0.320825 0.226579 q: -0.026707 0.898909 -0.026616 0.436510 
time: 1403636879.801666, t: 0.142668 -0.261238 0.231488 q: -0.044426 0.900867 -0.008207 0.431739 
time: 1403636879.901666, t: 0.121693 -0.206007 0.233390 q: -0.055912 0.904294 0.005981 0.423190 
time: 1403636880.001667, t: 0.088052 -0.161914 0.234605 q: -0.062558 0.908061 0.014393 0.413890 
time: 1403636880.101666, t: 0.047003 -0.137798 0.235641 q: -0.064933 0.910713 0.018687 0.407476 
time: 1403636880.201667, t: 0.002381 -0.136783 0.237396 q: -0.060189 0.911419 0.014486 0.406796 
time: 1403636880.301666, t: -0.043468 -0.153776 0.239331 q: -0.047022 0.910111 -0.001591 0.411685 
time: 1403636880.401666, t: -0.088615 -0.179225 0.242739 q: -0.028239 0.907832 -0.023150 0.417740 
time: 1403636880.501667, t: -0.135770 -0.201749 0.250595 q: -0.008524 0.905259 -0.044770 0.422409 
time: 1403636880.601666, t: -0.183629 -0.212564 0.259837 q: 0.007376 0.904610 -0.062776 0.421529 
time: 1403636880.701667, t: -0.229576 -0.207155 0.267079 q: 0.014498 0.907199 -0.074172 0.413857 
time: 1403636880.801666, t: -0.268070 -0.186247 0.271725 q: 0.013389 0.910313 -0.079056 0.406081 
time: 1403636880.901666, t: -0.294689 -0.154614 0.274517 q: 0.004795 0.911359 -0.074690 0.404750 
time: 1403636881.001667, t: -0.306218 -0.117783 0.276348 q: -0.003939 0.908661 -0.063756 0.412619 
time: 1403636881.101666, t: -0.303062 -0.079572 0.278684 q: -0.011960 0.903993 -0.046962 0.424791 
time: 1403636881.201667, t: -0.287911 -0.041899 0.280204 q: -0.027305 0.900356 -0.026089 0.433513 
time: 1403636881.301666, t: -0.262012 -0.011400 0.282944 q: -0.043993 0.898064 -0.003029 0.437649 
time: 1403636881.401666, t: -0.224871 0.006070 0.285837 q: -0.052491 0.896500 0.016869 0.439599 
time: 1403636881.501667, t: -0.177833 0.008483 0.287771 q: -0.052688 0.893986 0.032219 0.443818 
time: 1403636881.601666, t: -0.125355 -0.004206 0.288440 q: -0.049222 0.890514 0.044856 0.450056 
time: 1403636881.701667, t: -0.071328 -0.031341 0.286390 q: -0.045398 0.888462 0.056231 0.453224 
time: 1403636881.801666, t: -0.018893 -0.069470 0.280923 q: -0.039479 0.889266 0.060193 0.451691 
time: 1403636881.901666, t: 0.026007 -0.113883 0.273436 q: -0.034701 0.891162 0.052371 0.449314 
time: 1403636882.001667, t: 0.059096 -0.162714 0.264520 q: -0.031820 0.892820 0.040039 0.447500 
time: 1403636882.101666, t: 0.077720 -0.212112 0.253718 q: -0.028155 0.892724 0.027805 0.448864 
time: 1403636882.201667, t: 0.080167 -0.255770 0.240170 q: -0.022095 0.891478 0.017741 0.452177 
time: 1403636882.301666, t: 0.068995 -0.286907 0.225590 q: -0.015774 0.888906 0.010799 0.457690 
time: 1403636882.401666, t: 0.049071 -0.300753 0.210158 q: -0.017725 0.886638 0.010410 0.462008 
time: 1403636882.501667, t: 0.029343 -0.297169 0.194421 q: -0.027168 0.884918 0.013580 0.464756 
time: 1403636882.601666, t: 0.019588 -0.282293 0.181525 q: -0.038237 0.881371 0.020364 0.470434 
time: 1403636882.701667, t: 0.024447 -0.261851 0.170909 q: -0.050777 0.871010 0.029878 0.487720 
time: 1403636882.901666, t: 0.060713 -0.218476 0.137990 q: -0.086712 0.838479 0.065770 0.533955 
time: 1403636883.001667, t: 0.080090 -0.198453 0.113982 q: -0.104724 0.831094 0.084950 0.539536 
time: 1403636883.101666, t: 0.098427 -0.180625 0.089221 q: -0.116852 0.828385 0.103376 0.537994 
time: 1403636883.201667, t: 0.115340 -0.165336 0.066060 q: -0.119516 0.822579 0.118319 0.543214 
time: 1403636883.301666, t: 0.128637 -0.152025 0.045046 q: -0.123438 0.816004 0.128098 0.549993 
time: 1403636883.401666, t: 0.138492 -0.142531 0.028572 q: -0.128883 0.812180 0.132632 0.553319 
time: 1403636883.501667, t: 0.145597 -0.137065 0.018119 q: -0.134545 0.806790 0.137639 0.558608 
time: 1403636883.601666, t: 0.150797 -0.133235 0.011876 q: -0.140113 0.799278 0.144138 0.566346 
time: 1403636883.701667, t: 0.154991 -0.130453 0.006704 q: -0.143357 0.795516 0.152284 0.568694 
optimize pose graph 
time: 1403636883.801666, t: 0.157855 -0.129065 0.003093 q: -0.145871 0.794523 0.156198 0.568381 
time: 1403636883.901666, t: 0.159939 -0.128202 -0.001257 q: -0.153723 0.793210 0.157122 0.567890 
time: 1403636884.001667, t: 0.161767 -0.127841 -0.003495 q: -0.155348 0.790353 0.160243 0.570553 
time: 1403636884.101666, t: 0.162417 -0.128515 -0.004744 q: -0.161274 0.785916 0.161453 0.574682 
time: 1403636884.201667, t: 0.162299 -0.129596 -0.004693 q: -0.159056 0.782213 0.163729 0.579687 
time: 1403636884.301666, t: 0.161033 -0.129319 -0.004284 q: -0.158436 0.779815 0.163024 0.583275 
wait for imu ... 
time: 1403636884.401666, t: 0.160506 -0.129041 -0.004217 q: -0.159568 0.779430 0.162368 0.583663 
time: 1403636884.501667, t: 0.160525 -0.129294 -0.004170 q: -0.158661 0.779339 0.162921 0.583879 
time: 1403636884.601666, t: 0.160434 -0.129148 -0.004036 q: -0.158994 0.779319 0.162631 0.583896 
time: 1403636884.701667, t: 0.160601 -0.129196 -0.004128 q: -0.158918 0.779572 0.162571 0.583595 
time: 1403636884.801666, t: 0.160668 -0.129279 -0.004113 q: -0.158677 0.779627 0.162565 0.583589 
time: 1403636884.901666, t: 0.160687 -0.129293 -0.004107 q: -0.158674 0.779670 0.162480 0.583556 
wait for imu ... 
time: 1403636885.001667, t: 0.160676 -0.129291 -0.004081 q: -0.158787 0.779762 0.162365 0.583434 
wait for imu ... 
time: 1403636885.101666, t: 0.160635 -0.129274 -0.004066 q: -0.158846 0.779790 0.162186 0.583430 
wait for imu ... 
time: 1403636885.201667, t: 0.160633 -0.129269 -0.004048 q: -0.158863 0.779865 0.162086 0.583354 
time: 1403636885.301666, t: 0.160646 -0.129303 -0.004039 q: -0.158781 0.779885 0.162096 0.583347 
time: 1403636885.401666, t: 0.160647 -0.129314 -0.004047 q: -0.158658 0.779893 0.162027 0.583388 
wait for imu ... 
time: 1403636885.501667, t: 0.160658 -0.129319 -0.004049 q: -0.158612 0.779908 0.162003 0.583387 
time: 1403636885.601666, t: 0.160619 -0.129340 -0.004025 q: -0.158531 0.779926 0.161970 0.583394 
time: 1403636885.701667, t: 0.160691 -0.129317 -0.004046 q: -0.158597 0.780045 0.161896 0.583238 
wait for imu ... 
time: 1403636885.801666, t: 0.160708 -0.129322 -0.004039 q: -0.158513 0.780048 0.161894 0.583258 
time: 1403636885.901666, t: 0.160687 -0.129300 -0.004046 q: -0.158560 0.780122 0.161834 0.583163 
wait for imu ... 
time: 1403636886.001667, t: 0.160682 -0.129295 -0.004041 q: -0.158601 0.780105 0.161796 0.583184 
time: 1403636886.101666, t: 0.160670 -0.129326 -0.004042 q: -0.158422 0.780119 0.161841 0.583201 
time: 1403636886.201667, t: 0.160679 -0.129341 -0.004045 q: -0.158298 0.780138 0.161889 0.583197 
time: 1403636886.301666, t: 0.160678 -0.129345 -0.004037 q: -0.158250 0.780130 0.161921 0.583212 
wait for imu ... 
time: 1403636886.401666, t: 0.160672 -0.129325 -0.004043 q: -0.158363 0.780139 0.161857 0.583186 
time: 1403636886.501667, t: 0.160658 -0.129325 -0.004045 q: -0.158364 0.780131 0.161852 0.583198 
time: 1403636886.601666, t: 0.160668 -0.129329 -0.004059 q: -0.158329 0.780146 0.161856 0.583186 
time: 1403636886.701667, t: 0.160673 -0.129338 -0.004057 q: -0.158297 0.780157 0.161861 0.583179 
time: 1403636886.801666, t: 0.160670 -0.129345 -0.004059 q: -0.158293 0.780143 0.161879 0.583194 
time: 1403636886.901666, t: 0.160684 -0.129339 -0.004063 q: -0.158343 0.780184 0.161858 0.583131 
time: 1403636887.001667, t: 0.160675 -0.129334 -0.004062 q: -0.158207 0.780174 0.161903 0.583170 
time: 1403636887.101666, t: 0.160687 -0.129349 -0.004074 q: -0.158253 0.780185 0.161891 0.583146 
time: 1403636887.201667, t: 0.160679 -0.129352 -0.004079 q: -0.158275 0.780208 0.161856 0.583119 
wait for imu ... 
time: 1403636887.301666, t: 0.160679 -0.129362 -0.004092 q: -0.158259 0.780222 0.161808 0.583118 
wait for imu ... 
time: 1403636887.401666, t: 0.160696 -0.129355 -0.004080 q: -0.158150 0.780245 0.161868 0.583099 
time: 1403636887.501667, t: 0.160698 -0.129360 -0.004092 q: -0.158130 0.780263 0.161886 0.583076 
wait for imu ... 
time: 1403636887.601666, t: 0.160682 -0.129340 -0.004086 q: -0.158197 0.780242 0.161830 0.583101 
time: 1403636887.701667, t: 0.160676 -0.129326 -0.004103 q: -0.158239 0.780257 0.161818 0.583073 
wait for imu ... 
time: 1403636887.801666, t: 0.160677 -0.129327 -0.004093 q: -0.158253 0.780288 0.161816 0.583029 
wait for imu ... 
time: 1403636888.001667, t: 0.160689 -0.129334 -0.004092 q: -0.158161 0.780368 0.161802 0.582951 
time: 1403636888.101666, t: 0.160690 -0.129347 -0.004093 q: -0.158181 0.780348 0.161803 0.582971 
time: 1403636888.201667, t: 0.160689 -0.129360 -0.004121 q: -0.158207 0.780350 0.161782 0.582967 
wait for imu ... 
wait for imu ... 
time: 1403636888.301666, t: 0.160700 -0.129362 -0.004122 q: -0.158212 0.780341 0.161794 0.582975 
time: 1403636888.401666, t: 0.160706 -0.129377 -0.004125 q: -0.158135 0.780377 0.161821 0.582940 
wait for imu ... 
time: 1403636888.501667, t: 0.160706 -0.129382 -0.004116 q: -0.158116 0.780403 0.161791 0.582919 
time: 1403636888.601666, t: 0.160677 -0.129368 -0.004078 q: -0.158114 0.780369 0.161709 0.582987 
time: 1403636888.701667, t: 0.160638 -0.129298 -0.004037 q: -0.158153 0.780215 0.161817 0.583152 
time: 1403636888.901666, t: 0.160543 -0.129188 -0.004097 q: -0.158196 0.780369 0.161786 0.582944 
time: 1403636889.001667, t: 0.160600 -0.129179 -0.004028 q: -0.158502 0.780218 0.161935 0.583022 
time: 1403636889.101666, t: 0.160628 -0.129243 -0.004039 q: -0.158194 0.780325 0.161664 0.583037 
time: 1403636889.201667, t: 0.160666 -0.129274 -0.004043 q: -0.158222 0.780380 0.161587 0.582977 
time: 1403636889.301666, t: 0.160738 -0.129302 -0.004120 q: -0.158192 0.780395 0.161455 0.583002 
time: 1403636889.401666, t: 0.160810 -0.129306 -0.004115 q: -0.158348 0.780311 0.161771 0.582984 
time: 1403636889.501667, t: 0.160762 -0.129327 -0.004099 q: -0.158008 0.780350 0.161694 0.583046 
time: 1403636889.601666, t: 0.160710 -0.129418 -0.004069 q: -0.157800 0.780328 0.161555 0.583170 
time: 1403636889.701667, t: 0.160705 -0.129406 -0.004014 q: -0.157860 0.780184 0.161726 0.583299 
time: 1403636889.801666, t: 0.160662 -0.129378 -0.004019 q: -0.157942 0.780162 0.161836 0.583276 
time: 1403636889.901666, t: 0.160671 -0.129402 -0.004014 q: -0.157835 0.780290 0.161486 0.583231 
time: 1403636890.001667, t: 0.160680 -0.129401 -0.004043 q: -0.157928 0.780323 0.161721 0.583096 
time: 1403636890.101666, t: 0.160699 -0.129532 -0.003991 q: -0.157670 0.780382 0.161000 0.583287 
time: 1403636890.201667, t: 0.160641 -0.129393 -0.003888 q: -0.158065 0.779988 0.161523 0.583562 
time: 1403636890.301666, t: 0.160595 -0.129318 -0.003959 q: -0.158084 0.780298 0.161941 0.583026 
time: 1403636890.501667, t: 0.160614 -0.129446 -0.003931 q: -0.157628 0.780090 0.161124 0.583654 
time: 1403636890.601666, t: 0.160603 -0.129334 -0.003908 q: -0.158027 0.779956 0.161967 0.583492 
time: 1403636890.701667, t: 0.160575 -0.129348 -0.004017 q: -0.157970 0.780407 0.161958 0.582907 
time: 1403636890.801666, t: 0.160622 -0.129485 -0.003971 q: -0.157687 0.780404 0.161185 0.583202 
time: 1403636890.901666, t: 0.160633 -0.129467 -0.003855 q: -0.157795 0.780190 0.161284 0.583431 
wait for imu ... 
time: 1403636891.001667, t: 0.160592 -0.129445 -0.003929 q: -0.157937 0.780238 0.161330 0.583315 
time: 1403636891.101666, t: 0.160598 -0.129427 -0.003941 q: -0.157777 0.780263 0.161366 0.583315 
wait for imu ... 
time: 1403636891.201667, t: 0.160608 -0.129414 -0.003945 q: -0.157742 0.780228 0.161436 0.583352 
time: 1403636891.301666, t: 0.160622 -0.129419 -0.003965 q: -0.157767 0.780248 0.161495 0.583302 
time: 1403636891.401666, t: 0.160617 -0.129415 -0.003964 q: -0.157736 0.780201 0.161470 0.583380 
time: 1403636891.501667, t: 0.160632 -0.129412 -0.003967 q: -0.157794 0.780222 0.161495 0.583330 
time: 1403636891.601666, t: 0.160615 -0.129424 -0.003944 q: -0.157772 0.780277 0.161487 0.583265 
wait for imu ... 
time: 1403636891.701667, t: 0.160601 -0.129435 -0.003953 q: -0.157723 0.780261 0.161419 0.583319 
time: 1403636891.801666, t: 0.160609 -0.129408 -0.003968 q: -0.157762 0.780259 0.161435 0.583306 
time: 1403636891.901666, t: 0.160606 -0.129424 -0.003969 q: -0.157799 0.780254 0.161458 0.583296 
wait for imu ... 
time: 1403636892.001667, t: 0.160603 -0.129423 -0.003970 q: -0.157714 0.780257 0.161449 0.583317 
time: 1403636892.101666, t: 0.160639 -0.129432 -0.003990 q: -0.157737 0.780261 0.161477 0.583299 
time: 1403636892.201667, t: 0.160621 -0.129431 -0.003982 q: -0.157689 0.780266 0.161484 0.583302 
time: 1403636892.301666, t: 0.160708 -0.129511 -0.003993 q: -0.157378 0.780283 0.161606 0.583330 
time: 1403636892.401666, t: 0.160673 -0.129482 -0.003973 q: -0.157544 0.780290 0.161299 0.583361 
time: 1403636892.501667, t: 0.160632 -0.129455 -0.003954 q: -0.157576 0.780222 0.161379 0.583421 
time: 1403636892.601666, t: 0.160609 -0.129447 -0.003952 q: -0.157605 0.780224 0.161421 0.583399 
wait for imu ... 
time: 1403636892.701667, t: 0.160619 -0.129444 -0.003962 q: -0.157589 0.780213 0.161459 0.583408 
time: 1403636892.801666, t: 0.160629 -0.129440 -0.003944 q: -0.157581 0.780192 0.161571 0.583407 
time: 1403636892.901666, t: 0.160642 -0.129452 -0.003969 q: -0.157557 0.780309 0.161510 0.583274 
wait for imu ... 
time: 1403636893.001667, t: 0.160657 -0.129465 -0.003974 q: -0.157502 0.780222 0.161546 0.583395 
time: 1403636893.101666, t: 0.160646 -0.129450 -0.003982 q: -0.157515 0.780208 0.161524 0.583416 
wait for imu ... 
time: 1403636893.201667, t: 0.160630 -0.129438 -0.003991 q: -0.157557 0.780233 0.161486 0.583382 
time: 1403636893.301666, t: 0.160635 -0.129440 -0.004002 q: -0.157591 0.780233 0.161481 0.583374 
time: 1403636893.401666, t: 0.160644 -0.129448 -0.003990 q: -0.157573 0.780267 0.161490 0.583331 
wait for imu ... 
time: 1403636893.501667, t: 0.160656 -0.129456 -0.003996 q: -0.157543 0.780264 0.161481 0.583345 
time: 1403636893.601666, t: 0.160652 -0.129454 -0.003994 q: -0.157491 0.780266 0.161472 0.583359 
wait for imu ... 
time: 1403636893.701667, t: 0.160663 -0.129454 -0.003990 q: -0.157499 0.780250 0.161553 0.583356 
wait for imu ... 
time: 1403636893.801666, t: 0.160651 -0.129437 -0.003987 q: -0.157522 0.780298 0.161498 0.583301 
wait for imu ... 
time: 1403636893.901666, t: 0.160645 -0.129448 -0.003995 q: -0.157558 0.780307 0.161459 0.583290 
wait for imu ... 
time: 1403636894.001667, t: 0.160628 -0.129445 -0.003993 q: -0.157571 0.780321 0.161411 0.583282 
wait for imu ... 
time: 1403636894.101666, t: 0.160605 -0.129444 -0.003986 q: -0.157512 0.780344 0.161393 0.583272 
wait for imu ... 
time: 1403636894.201667, t: 0.160632 -0.129470 -0.004017 q: -0.157543 0.780305 0.161410 0.583310 
wait for imu ... 
time: 1403636894.301666, t: 0.160624 -0.129444 -0.004018 q: -0.157535 0.780278 0.161387 0.583355 
wait for imu ... 
time: 1403636894.401666, t: 0.160635 -0.129459 -0.004011 q: -0.157518 0.780330 0.161415 0.583283 
wait for imu ... 
time: 1403636894.501667, t: 0.160646 -0.129456 -0.004018 q: -0.157574 0.780303 0.161435 0.583298 
wait for imu ... 
time: 1403636894.701667, t: 0.160351 -0.129120 -0.004166 q: -0.158880 0.779080 0.162616 0.584250 
time: 1403636894.801666, t: 0.160357 -0.129110 -0.004168 q: -0.158902 0.779103 0.162654 0.584202 
time: 1403636894.901666, t: 0.160407 -0.129147 -0.004148 q: -0.158713 0.779091 0.162605 0.584284 
wait for imu ... 
time: 1403636895.001667, t: 0.160420 -0.129148 -0.004154 q: -0.158790 0.779093 0.162672 0.584242 
time: 1403636895.101666, t: 0.160417 -0.129136 -0.004141 q: -0.158841 0.779108 0.162671 0.584207 
time: 1403636895.201667, t: 0.160409 -0.129133 -0.004145 q: -0.158830 0.779067 0.162589 0.584288 
time: 1403636895.301666, t: 0.160391 -0.129108 -0.004136 q: -0.158866 0.779054 0.162596 0.584293 
time: 1403636895.401666, t: 0.160378 -0.129100 -0.004133 q: -0.158817 0.779079 0.162635 0.584263 
time: 1403636895.501667, t: 0.160393 -0.129117 -0.004138 q: -0.158849 0.779029 0.162681 0.584307 
wait for imu ... 
time: 1403636895.601666, t: 0.160392 -0.129117 -0.004108 q: -0.158840 0.779090 0.162661 0.584235 
time: 1403636895.701667, t: 0.160411 -0.129130 -0.004130 q: -0.158820 0.779023 0.162707 0.584316 
time: 1403636895.801666, t: 0.160410 -0.129125 -0.004095 q: -0.158720 0.778967 0.162752 0.584406 
wait for imu ... 
time: 1403636895.901666, t: 0.160432 -0.129129 -0.004091 q: -0.158774 0.778989 0.162858 0.584333 
time: 1403636896.001667, t: 0.160409 -0.129119 -0.004065 q: -0.158752 0.779017 0.162906 0.584288 
wait for imu ... 
time: 1403636896.101666, t: 0.160612 -0.129163 -0.004146 q: -0.158860 0.779456 0.162945 0.583662 
time: 1403636896.201667, t: 0.160882 -0.129227 -0.004336 q: -0.158959 0.780129 0.162921 0.582741 
wait for imu ... 
time: 1403636896.301666, t: 0.160850 -0.129259 -0.004293 q: -0.158870 0.780008 0.163112 0.582874 
time: 1403636896.401666, t: 0.160689 -0.129222 -0.004188 q: -0.158817 0.779630 0.163176 0.583376 
time: 1403636896.501667, t: 0.160622 -0.129209 -0.004126 q: -0.158826 0.779443 0.163191 0.583620 
time: 1403636896.601666, t: 0.160629 -0.129217 -0.004128 q: -0.158749 0.779459 0.163201 0.583616 
time: 1403636896.701667, t: 0.160777 -0.129274 -0.004149 q: -0.158799 0.779735 0.163333 0.583197 
wait for imu ... 
time: 1403636896.801666, t: 0.160844 -0.129295 -0.004231 q: -0.158926 0.779965 0.163484 0.582813 
time: 1403636896.901666, t: 0.160857 -0.129350 -0.004134 q: -0.158756 0.779967 0.163724 0.582788 
time: 1403636897.001667, t: 0.160619 -0.129281 -0.003983 q: -0.159031 0.779431 0.163730 0.583429 
time: 1403636897.101666, t: 0.160068 -0.129467 -0.003661 q: -0.159907 0.778798 0.165840 0.583438 
time: 1403636897.201667, t: 0.158868 -0.129921 -0.003894 q: -0.161625 0.778595 0.168623 0.582437 
time: 1403636897.301666, t: 0.156879 -0.133019 -0.004485 q: -0.160798 0.779581 0.167557 0.581654 
wait for imu ... 
time: 1403636897.501667, t: 0.144226 -0.153072 -0.005351 q: -0.147292 0.788677 0.163395 0.574105 
time: 1403636897.601666, t: 0.134369 -0.167461 -0.005296 q: -0.137710 0.796793 0.162906 0.565349 
time: 1403636897.701667, t: 0.122709 -0.183900 -0.000495 q: -0.124606 0.806859 0.166715 0.552864 
time: 1403636897.801666, t: 0.111586 -0.200212 0.011423 q: -0.115757 0.814570 0.171586 0.541881 
time: 1403636897.901666, t: 0.103607 -0.214380 0.030007 q: -0.112010 0.819544 0.174949 0.534035 
time: 1403636898.001667, t: 0.101703 -0.224427 0.052480 q: -0.113700 0.814204 0.176324 0.541344 
time: 1403636898.101666, t: 0.103118 -0.230863 0.067368 q: -0.118729 0.808044 0.176574 0.549355 
time: 1403636898.201667, t: 0.106481 -0.235707 0.079401 q: -0.122012 0.804890 0.177820 0.552852 
time: 1403636898.301666, t: 0.111437 -0.238551 0.093957 q: -0.123797 0.803840 0.180196 0.553213 
time: 1403636898.401666, t: 0.117842 -0.239244 0.111885 q: -0.126071 0.803253 0.183135 0.552587 
time: 1403636898.501667, t: 0.124910 -0.238316 0.132082 q: -0.129476 0.803271 0.185732 0.550905 
time: 1403636898.601666, t: 0.133251 -0.236351 0.153276 q: -0.130956 0.802968 0.188376 0.550097 
time: 1403636898.701667, t: 0.142636 -0.233133 0.173193 q: -0.131295 0.802929 0.190874 0.549211 
time: 1403636898.801666, t: 0.154145 -0.228096 0.191221 q: -0.133740 0.802749 0.191614 0.548627 
time: 1403636898.901666, t: 0.167287 -0.221382 0.206939 q: -0.137039 0.803780 0.190724 0.546610 
time: 1403636899.001667, t: 0.183136 -0.213874 0.220104 q: -0.142981 0.805125 0.186638 0.544515 
time: 1403636899.101666, t: 0.202711 -0.207728 0.231390 q: -0.143886 0.805553 0.178157 0.546480 
time: 1403636899.201667, t: 0.225646 -0.204417 0.242228 q: -0.136000 0.804306 0.167556 0.553643 
time: 1403636899.301666, t: 0.250039 -0.204070 0.252965 q: -0.126261 0.801204 0.150508 0.565223 
time: 1403636899.401666, t: 0.274364 -0.206825 0.261995 q: -0.110521 0.799790 0.130586 0.575386 
time: 1403636899.501667, t: 0.295622 -0.212092 0.269162 q: -0.099992 0.803806 0.108963 0.576215 
time: 1403636899.601666, t: 0.314851 -0.218839 0.273668 q: -0.093738 0.811179 0.092253 0.569816 
time: 1403636899.701667, t: 0.334573 -0.228764 0.279170 q: -0.088785 0.815428 0.080790 0.566275 
optimize pose graph 
time: 1403636899.801666, t: 0.355886 -0.241855 0.284229 q: -0.081845 0.818489 0.074657 0.563741 
time: 1403636899.901666, t: 0.379024 -0.257776 0.287245 q: -0.072424 0.818632 0.074369 0.564859 
time: 1403636900.001667, t: 0.403714 -0.274934 0.288011 q: -0.065994 0.815107 0.074967 0.570636 
time: 1403636900.101666, t: 0.429023 -0.292200 0.288245 q: -0.064178 0.809993 0.074896 0.578085 
time: 1403636900.201667, t: 0.452154 -0.310492 0.295303 q: -0.063308 0.806118 0.076010 0.583428 
time: 1403636900.301666, t: 0.471866 -0.329001 0.311151 q: -0.062427 0.806714 0.078207 0.582408 
time: 1403636900.401666, t: 0.488572 -0.347566 0.335005 q: -0.064087 0.811364 0.080330 0.575437 
time: 1403636900.501667, t: 0.505179 -0.365917 0.360033 q: -0.065040 0.814986 0.083879 0.569677 
time: 1403636900.601666, t: 0.521189 -0.384227 0.381639 q: -0.067002 0.816825 0.085900 0.566507 
time: 1403636900.701667, t: 0.539169 -0.401197 0.394915 q: -0.068614 0.821467 0.087340 0.559335 
time: 1403636900.801666, t: 0.559895 -0.418438 0.403566 q: -0.072101 0.825379 0.087762 0.553036 
time: 1403636900.901666, t: 0.582626 -0.436565 0.407654 q: -0.080302 0.823907 0.083787 0.554715 
time: 1403636901.001667, t: 0.608067 -0.457235 0.409608 q: -0.085486 0.821060 0.080615 0.558618 
time: 1403636901.101666, t: 0.637122 -0.481600 0.407017 q: -0.089665 0.819677 0.078155 0.560340 
time: 1403636901.201667, t: 0.670034 -0.508830 0.400940 q: -0.090645 0.818797 0.077060 0.561620 
time: 1403636901.301666, t: 0.705334 -0.540978 0.399100 q: -0.086773 0.817962 0.079645 0.563085 
time: 1403636901.401666, t: 0.741914 -0.575766 0.402622 q: -0.082953 0.817474 0.081868 0.564051 
time: 1403636901.501667, t: 0.784342 -0.611291 0.412735 q: -0.077621 0.816657 0.085165 0.565502 
time: 1403636901.601666, t: 0.826320 -0.649660 0.422186 q: -0.074196 0.815911 0.087271 0.566717 
time: 1403636901.701667, t: 0.870271 -0.684925 0.423492 q: -0.073709 0.814061 0.087289 0.569432 
time: 1403636901.801666, t: 0.910056 -0.724581 0.419949 q: -0.074907 0.813542 0.086551 0.570129 
optimize pose graph 
time: 1403636901.901666, t: 0.952213 -0.762540 0.410896 q: -0.074395 0.813473 0.087377 0.570168 
time: 1403636902.001667, t: 0.992330 -0.800942 0.397098 q: -0.073119 0.813124 0.088756 0.570618 
time: 1403636902.101666, t: 1.034125 -0.838236 0.386314 q: -0.072982 0.812381 0.089930 0.571510 
time: 1403636902.201667, t: 1.071209 -0.878914 0.382035 q: -0.072601 0.812433 0.091152 0.571290 
time: 1403636902.301666, t: 1.109482 -0.919338 0.381254 q: -0.074921 0.812787 0.088772 0.570862 
time: 1403636902.401666, t: 1.145228 -0.961149 0.368634 q: -0.075539 0.813488 0.086846 0.570077 
time: 1403636902.601666, t: 1.214063 -1.040006 0.316676 q: -0.065859 0.814247 0.087597 0.570080 
time: 1403636902.701667, t: 1.247411 -1.079148 0.293975 q: -0.060762 0.813226 0.087915 0.572050 
time: 1403636902.801666, t: 1.279359 -1.115130 0.279872 q: -0.060232 0.812807 0.086072 0.572982 
time: 1403636902.901666, t: 1.309833 -1.148260 0.274213 q: -0.062283 0.810599 0.085766 0.575929 
time: 1403636903.001667, t: 1.342146 -1.182964 0.272936 q: -0.063919 0.810553 0.086641 0.575684 
time: 1403636903.101666, t: 1.372251 -1.216351 0.279661 q: -0.066168 0.810950 0.087716 0.574706 
time: 1403636903.201667, t: 1.405492 -1.249879 0.296669 q: -0.068159 0.811680 0.088993 0.573246 
time: 1403636903.301666, t: 1.434198 -1.283436 0.318614 q: -0.068326 0.813014 0.091616 0.570917 
time: 1403636903.401666, t: 1.464395 -1.315506 0.339878 q: -0.066559 0.813615 0.094733 0.569760 
time: 1403636903.501667, t: 1.490584 -1.348232 0.353369 q: -0.066151 0.813827 0.096422 0.569221 
time: 1403636903.601666, t: 1.516539 -1.376725 0.362564 q: -0.066737 0.814175 0.097234 0.568516 
time: 1403636903.701667, t: 1.543245 -1.404926 0.362840 q: -0.070167 0.817267 0.095633 0.563920 
time: 1403636903.801666, t: 1.571747 -1.432028 0.360382 q: -0.074183 0.819959 0.093363 0.559864 
time: 1403636903.901666, t: 1.603982 -1.458440 0.363180 q: -0.078415 0.820399 0.090542 0.559105 
time: 1403636904.001667, t: 1.638537 -1.486769 0.373140 q: -0.082207 0.818723 0.088019 0.561415 
time: 1403636904.101666, t: 1.673630 -1.516158 0.390534 q: -0.084835 0.816812 0.086061 0.564106 
time: 1403636904.201667, t: 1.710374 -1.547191 0.414471 q: -0.087616 0.816428 0.083815 0.564575 
time: 1403636904.301666, t: 1.747325 -1.580572 0.439514 q: -0.087315 0.815662 0.082845 0.565870 
time: 1403636904.401666, t: 1.786322 -1.616111 0.462631 q: -0.085245 0.814536 0.082747 0.567818 
time: 1403636904.501667, t: 1.823055 -1.653187 0.479910 q: -0.082137 0.814121 0.083633 0.568741 
time: 1403636904.601666, t: 1.862266 -1.693093 0.493812 q: -0.077953 0.813965 0.084937 0.569359 
time: 1403636904.701667, t: 1.900846 -1.733868 0.505496 q: -0.073529 0.814230 0.085788 0.569441 
time: 1403636904.801666, t: 1.941685 -1.777282 0.515341 q: -0.069831 0.814069 0.086361 0.570050 
time: 1403636904.901666, t: 1.982762 -1.819876 0.523492 q: -0.065485 0.814471 0.087289 0.569851 
time: 1403636905.001667, t: 2.023173 -1.862557 0.525809 q: -0.062290 0.814491 0.087243 0.570187 
time: 1403636905.101666, t: 2.062092 -1.902120 0.525747 q: -0.059489 0.814059 0.086794 0.571170 
time: 1403636905.201667, t: 2.099313 -1.940236 0.528263 q: -0.057069 0.813651 0.085986 0.572120 
time: 1403636905.301666, t: 2.141417 -1.977967 0.536967 q: -0.054051 0.814720 0.085854 0.570911 
time: 1403636905.401666, t: 2.178141 -2.009939 0.550360 q: -0.052224 0.817446 0.085413 0.567238 
time: 1403636905.601666, t: 2.250270 -2.063139 0.586410 q: -0.052105 0.817193 0.097338 0.565691 
time: 1403636905.701667, t: 2.286810 -2.084819 0.596220 q: -0.056702 0.817473 0.099844 0.564406 
time: 1403636905.801666, t: 2.324388 -2.104635 0.597602 q: -0.062178 0.817449 0.100663 0.563718 
wait for imu ... 
time: 1403636905.901666, t: 2.361895 -2.121844 0.594118 q: -0.065570 0.818178 0.101851 0.562060 
time: 1403636906.001667, t: 2.401257 -2.135705 0.584349 q: -0.067404 0.820366 0.103430 0.558353 
time: 1403636906.101666, t: 2.443314 -2.147714 0.570855 q: -0.065997 0.822080 0.106634 0.555391 
time: 1403636906.201667, t: 2.488067 -2.156369 0.560778 q: -0.066869 0.821100 0.107824 0.556505 
time: 1403636906.301666, t: 2.534029 -2.161224 0.559363 q: -0.070529 0.815731 0.106839 0.564087 
time: 1403636906.401666, t: 2.580038 -2.164029 0.564209 q: -0.076591 0.812185 0.102980 0.569108 
time: 1403636906.501667, t: 2.624845 -2.166153 0.573116 q: -0.088344 0.812034 0.094299 0.569126 
time: 1403636906.601666, t: 2.668827 -2.171207 0.580071 q: -0.093335 0.815259 0.090350 0.564338 
time: 1403636906.701667, t: 2.712617 -2.179855 0.583286 q: -0.088088 0.815282 0.093623 0.564615 
time: 1403636906.801666, t: 2.758134 -2.192257 0.580864 q: -0.079835 0.814521 0.099298 0.565969 
time: 1403636906.901666, t: 2.804938 -2.204699 0.574279 q: -0.075247 0.814627 0.103765 0.565644 
time: 1403636907.001667, t: 2.852789 -2.215558 0.572979 q: -0.073065 0.814418 0.107349 0.565563 
time: 1403636907.101666, t: 2.899154 -2.225060 0.576300 q: -0.073349 0.814876 0.109038 0.564543 
time: 1403636907.201667, t: 2.948370 -2.232944 0.580466 q: -0.072680 0.817723 0.111104 0.560092 
time: 1403636907.301666, t: 2.996980 -2.239103 0.576108 q: -0.074279 0.820918 0.111636 0.555081 
time: 1403636907.401666, t: 3.047864 -2.242959 0.570828 q: -0.079951 0.822275 0.109086 0.552785 
time: 1403636907.501667, t: 3.105824 -2.245420 0.572681 q: -0.085810 0.822826 0.106070 0.551674 
time: 1403636907.601666, t: 3.162068 -2.249585 0.582882 q: -0.090818 0.823159 0.102986 0.550958 
time: 1403636907.701667, t: 3.227154 -2.254922 0.599045 q: -0.093809 0.823168 0.100816 0.550845 
time: 1403636907.801666, t: 3.292509 -2.263514 0.620618 q: -0.095619 0.822914 0.099586 0.551136 
time: 1403636907.901666, t: 3.365615 -2.273545 0.634649 q: -0.095973 0.821693 0.099442 0.552920 
time: 1403636908.001667, t: 3.440283 -2.286128 0.638497 q: -0.090609 0.817738 0.104077 0.558805 
time: 1403636908.101666, t: 3.521083 -2.298212 0.634510 q: -0.085468 0.813981 0.109785 0.563984 
time: 1403636908.201667, t: 3.597355 -2.311781 0.635180 q: -0.084696 0.811299 0.113142 0.567291 
time: 1403636908.301666, t: 3.671778 -2.326381 0.638977 q: -0.085047 0.810461 0.115891 0.567881 
time: 1403636908.401666, t: 3.742778 -2.341027 0.639506 q: -0.086094 0.810824 0.118062 0.566757 
time: 1403636908.501667, t: 3.813974 -2.353950 0.634542 q: -0.087528 0.811740 0.119937 0.564829 
time: 1403636908.601666, t: 3.882916 -2.368578 0.627475 q: -0.088919 0.811972 0.121918 0.563854 
wait for imu ... 
wait for imu ... 
wait for imu ... 
wait for imu ... 
wait for imu ... 
wait for imu ... 
time: 1403636908.701667, t: 3.953540 -2.380181 0.623664 q: -0.089677 0.813157 0.123905 0.561588 
wait for imu ... 
time: 1403636908.801666, t: 4.022045 -2.394540 0.628915 q: -0.092419 0.815777 0.124055 0.557294 
time: 1403636908.901666, t: 4.094012 -2.404557 0.642330 q: -0.093666 0.818760 0.123982 0.552710 
time: 1403636909.001667, t: 4.168938 -2.415770 0.662719 q: -0.095704 0.819354 0.123770 0.551526 
time: 1403636909.101666, t: 4.245730 -2.425266 0.683656 q: -0.098199 0.820059 0.121844 0.550467 
time: 1403636909.201667, t: 4.323606 -2.436284 0.699956 q: -0.098829 0.820260 0.120958 0.550250 
time: 1403636909.301666, t: 4.405322 -2.447135 0.710861 q: -0.098828 0.820849 0.119979 0.549586 
time: 1403636909.401666, t: 4.489095 -2.460103 0.718257 q: -0.097492 0.819492 0.119806 0.551883 
time: 1403636909.501667, t: 4.576685 -2.471816 0.719939 q: -0.094391 0.816377 0.120397 0.556887 
time: 1403636909.601666, t: 4.668091 -2.483933 0.715130 q: -0.095065 0.815358 0.118230 0.558727 
time: 1403636909.701667, t: 4.757616 -2.496810 0.704738 q: -0.098533 0.813471 0.114177 0.561711 
time: 1403636909.801666, t: 4.847085 -2.511463 0.688823 q: -0.094066 0.808517 0.116017 0.569203 
time: 1403636909.901666, t: 4.932492 -2.525351 0.668535 q: -0.080911 0.804214 0.124132 0.575573 
time: 1403636910.001667, t: 5.010909 -2.537830 0.643635 q: -0.069403 0.798797 0.131390 0.582961 
time: 1403636910.101666, t: 5.087803 -2.545091 0.616962 q: -0.070664 0.797950 0.130706 0.584122 
time: 1403636910.201667, t: 5.158119 -2.549491 0.592617 q: -0.078932 0.803263 0.126002 0.576769 
time: 1403636910.301666, t: 5.229005 -2.550659 0.578873 q: -0.084001 0.807405 0.123428 0.570795 
time: 1403636910.401666, t: 5.298496 -2.550481 0.573002 q: -0.086494 0.810098 0.122901 0.566706 
time: 1403636910.501667, t: 5.364538 -2.550749 0.573874 q: -0.087564 0.810638 0.124122 0.565502 
time: 1403636910.601666, t: 5.435336 -2.551309 0.572547 q: -0.086688 0.810801 0.126060 0.564975 
time: 1403636910.701667, t: 5.499485 -2.551013 0.567244 q: -0.086949 0.810837 0.126760 0.564725 
time: 1403636910.801666, t: 5.566787 -2.549401 0.554146 q: -0.088393 0.811260 0.126075 0.564047 
time: 1403636910.901666, t: 5.633057 -2.546695 0.538226 q: -0.091406 0.812009 0.124428 0.562853 
time: 1403636911.001667, t: 5.700918 -2.542104 0.521427 q: -0.097405 0.806163 0.120793 0.570984 
time: 1403636911.101666, t: 5.765726 -2.540047 0.509765 q: -0.100243 0.798870 0.119142 0.581003 
time: 1403636911.201667, t: 5.828372 -2.541080 0.501865 q: -0.097431 0.802298 0.121093 0.576335 
time: 1403636911.301666, t: 5.885882 -2.543968 0.501114 q: -0.093981 0.807707 0.123413 0.568810 
time: 1403636911.401666, t: 5.938877 -2.547427 0.509847 q: -0.091042 0.809121 0.125172 0.566893 
time: 1403636911.501667, t: 5.988218 -2.550383 0.525854 q: -0.089202 0.809734 0.125543 0.566227 
time: 1403636911.601666, t: 6.040031 -2.551734 0.539075 q: -0.088515 0.810232 0.125724 0.565582 
time: 1403636911.701667, t: 6.092420 -2.554057 0.548930 q: -0.088311 0.810247 0.125556 0.565630 
time: 1403636911.801666, t: 6.145062 -2.555477 0.551704 q: -0.092198 0.810909 0.121843 0.564873 
time: 1403636911.901666, t: 6.205717 -2.556691 0.547878 q: -0.099293 0.813511 0.116092 0.561128 
time: 1403636912.001667, t: 6.260843 -2.559258 0.539098 q: -0.101797 0.815457 0.113119 0.558454 
time: 1403636912.101666, t: 6.330026 -2.561237 0.526983 q: -0.099029 0.815533 0.113689 0.558725 
time: 1403636912.201667, t: 6.387995 -2.566352 0.518470 q: -0.096160 0.814061 0.115460 0.561005 
time: 1403636912.301666, t: 6.450433 -2.571275 0.517607 q: -0.092187 0.812241 0.117651 0.563847 
time: 1403636912.401666, t: 6.508157 -2.578891 0.521145 q: -0.091225 0.812307 0.117369 0.563968 
time: 1403636912.501667, t: 6.568700 -2.585739 0.522000 q: -0.096378 0.812776 0.113101 0.563307 
time: 1403636912.601666, t: 6.629691 -2.595963 0.518681 q: -0.097146 0.813170 0.111227 0.562980 
time: 1403636912.701667, t: 6.689869 -2.607314 0.509059 q: -0.101784 0.813524 0.106878 0.562490 
time: 1403636912.801666, t: 6.755524 -2.620595 0.492290 q: -0.106107 0.813407 0.103270 0.562535 
time: 1403636912.901666, t: 6.816690 -2.639615 0.474538 q: -0.102879 0.812521 0.104912 0.564109 
time: 1403636913.001667, t: 6.878548 -2.660753 0.450440 q: -0.097062 0.808603 0.108823 0.569998 
time: 1403636913.101666, t: 6.941139 -2.683624 0.427506 q: -0.093014 0.805479 0.111473 0.574566 
time: 1403636913.201667, t: 6.997897 -2.706793 0.404085 q: -0.090172 0.802867 0.113530 0.578259 
time: 1403636913.301666, t: 7.054260 -2.732773 0.392219 q: -0.089004 0.803439 0.114695 0.577416 
time: 1403636913.401666, t: 7.104157 -2.758999 0.385454 q: -0.092815 0.807142 0.112552 0.572047 
time: 1403636913.501667, t: 7.157035 -2.787145 0.385119 q: -0.096812 0.809844 0.109569 0.568133 
time: 1403636913.601666, t: 7.211368 -2.817049 0.382832 q: -0.093517 0.810264 0.111316 0.567746 
time: 1403636913.701667, t: 7.262595 -2.847563 0.374955 q: -0.090442 0.810527 0.112548 0.567626 
time: 1403636913.801666, t: 7.314615 -2.878482 0.363346 q: -0.089128 0.810014 0.113300 0.568415 
time: 1403636913.901666, t: 7.363256 -2.911348 0.356200 q: -0.088015 0.809348 0.114156 0.569366 
time: 1403636914.001667, t: 7.413356 -2.942830 0.357682 q: -0.087377 0.808809 0.115176 0.570024 
time: 1403636914.101666, t: 7.461063 -2.976645 0.362353 q: -0.090419 0.808775 0.113457 0.569943 
time: 1403636914.201667, t: 7.509791 -3.009813 0.367107 q: -0.092381 0.809185 0.111921 0.569350 
time: 1403636914.301666, t: 7.556725 -3.043935 0.367910 q: -0.090626 0.809819 0.112650 0.568586 
time: 1403636914.401666, t: 7.604645 -3.080987 0.362750 q: -0.087054 0.808799 0.114853 0.570153 
time: 1403636914.501667, t: 7.650399 -3.113716 0.363251 q: -0.084285 0.808581 0.116420 0.570560 
wait for imu ... 
wait for imu ... 
wait for imu ... 
time: 1403636914.601666, t: 7.697720 -3.148983 0.369866 q: -0.083413 0.808384 0.116874 0.570874 
time: 1403636914.701667, t: 7.742398 -3.181785 0.385445 q: -0.083244 0.807291 0.116566 0.572507 
time: 1403636914.801666, t: 7.791898 -3.216506 0.401548 q: -0.081972 0.806697 0.116392 0.573562 
time: 1403636914.901666, t: 7.831461 -3.247081 0.412976 q: -0.080875 0.807744 0.115875 0.572347 
time: 1403636915.001667, t: 7.872699 -3.278936 0.416338 q: -0.079844 0.809879 0.115288 0.569587 
time: 1403636915.101666, t: 7.909513 -3.306593 0.415937 q: -0.078570 0.811506 0.115266 0.567450 
time: 1403636915.201667, t: 7.945323 -3.333017 0.409238 q: -0.077225 0.813918 0.115166 0.564191 
time: 1403636915.301666, t: 7.982987 -3.357865 0.397259 q: -0.075709 0.818732 0.115422 0.557337 
time: 1403636915.401666, t: 8.027317 -3.383382 0.381904 q: -0.074768 0.822305 0.116160 0.552024 
time: 1403636915.501667, t: 8.071724 -3.404605 0.369444 q: -0.078287 0.820614 0.113780 0.554543 
time: 1403636915.701667, t: 8.167160 -3.443110 0.366865 q: -0.096272 0.811861 0.102211 0.566715 
time: 1403636915.801666, t: 8.215097 -3.467129 0.376552 q: -0.098146 0.810206 0.101593 0.568870 
time: 1403636915.901666, t: 8.262839 -3.494210 0.395045 q: -0.098506 0.809239 0.102178 0.570077 
time: 1403636916.001667, t: 8.309616 -3.523829 0.415104 q: -0.092539 0.808276 0.106915 0.571574 
time: 1403636916.101666, t: 8.354079 -3.554056 0.430743 q: -0.084099 0.807554 0.113267 0.572673 
time: 1403636916.201667, t: 8.399643 -3.585133 0.439894 q: -0.079976 0.807660 0.116948 0.572375 
time: 1403636916.301666, t: 8.443668 -3.615666 0.450640 q: -0.081679 0.807883 0.116228 0.571965 
time: 1403636916.401666, t: 8.484308 -3.645461 0.466301 q: -0.080659 0.808552 0.116735 0.571061 
time: 1403636916.501667, t: 8.525870 -3.674649 0.480808 q: -0.073861 0.808742 0.112416 0.572576 
time: 1403636916.601666, t: 8.565953 -3.703841 0.490101 q: -0.063702 0.809883 0.102702 0.574007 
time: 1403636916.701667, t: 8.602735 -3.728661 0.493863 q: -0.055110 0.811131 0.092297 0.574901 
time: 1403636916.801666, t: 8.639800 -3.751418 0.492712 q: -0.050003 0.811614 0.083169 0.576078 
time: 1403636916.901666, t: 8.673807 -3.771332 0.486706 q: -0.044487 0.812157 0.077623 0.576538 
time: 1403636917.001667, t: 8.708215 -3.787720 0.475317 q: -0.041405 0.812147 0.073647 0.577303 
time: 1403636917.101666, t: 8.739844 -3.801740 0.458541 q: -0.042139 0.812699 0.069178 0.577026 
time: 1403636917.201667, t: 8.768970 -3.814648 0.438481 q: -0.043737 0.813142 0.063533 0.576933 
time: 1403636917.301666, t: 8.796683 -3.827414 0.411682 q: -0.038128 0.813137 0.054235 0.578285 
time: 1403636917.401666, t: 8.822795 -3.842191 0.381671 q: -0.025294 0.813324 0.040406 0.579855 
time: 1403636917.501667, t: 8.850363 -3.855489 0.346398 q: -0.013065 0.813886 0.023654 0.580395 
time: 1403636917.601666, t: 8.875765 -3.866766 0.315044 q: -0.005741 0.813745 0.009342 0.581119 
time: 1403636917.701667, t: 8.898028 -3.876279 0.290362 q: 0.000280 0.813016 -0.001009 0.582240 
time: 1403636917.801666, t: 8.920378 -3.884314 0.273601 q: 0.006736 0.812512 -0.009048 0.582836 
time: 1403636917.901666, t: 8.939563 -3.889983 0.266946 q: 0.012665 0.811813 -0.014813 0.583593 
time: 1403636918.001667, t: 8.954289 -3.893622 0.265054 q: 0.016678 0.812442 -0.018645 0.582505 
wait for imu ... 
time: 1403636918.101666, t: 8.965973 -3.896213 0.259037 q: 0.020557 0.813453 -0.020047 0.580921 
time: 1403636918.201667, t: 8.978096 -3.897415 0.248027 q: 0.024080 0.814808 -0.019275 0.578910 
wait for imu ... 
time: 1403636918.301666, t: 8.987333 -3.896438 0.241486 q: 0.026427 0.815652 -0.017865 0.577663 
time: 1403636918.401666, t: 8.995743 -3.892080 0.240652 q: 0.026805 0.821642 -0.016913 0.569122 
time: 1403636918.501667, t: 9.005948 -3.884464 0.247293 q: 0.026923 0.827536 -0.015789 0.560545 
time: 1403636918.601666, t: 9.019857 -3.873902 0.262630 q: 0.026580 0.829332 -0.014192 0.557944 
time: 1403636918.701667, t: 9.039584 -3.859584 0.287693 q: 0.023753 0.827910 -0.013288 0.560201 
time: 1403636918.801666, t: 9.063232 -3.842358 0.319645 q: 0.010898 0.824941 -0.019718 0.564770 
time: 1403636918.901666, t: 9.088794 -3.826076 0.356809 q: -0.001830 0.819079 -0.028522 0.572968 
time: 1403636919.001667, t: 9.115294 -3.813266 0.396230 q: -0.007218 0.814288 -0.040051 0.579032 
time: 1403636919.101666, t: 9.142168 -3.806411 0.438077 q: -0.008423 0.811474 -0.048737 0.582292 
time: 1403636919.201667, t: 9.165114 -3.806583 0.482123 q: -0.005704 0.809755 -0.053954 0.584255 
time: 1403636919.301666, t: 9.184306 -3.813605 0.526772 q: -0.001372 0.809152 -0.057069 0.584820 
time: 1403636919.401666, t: 9.201161 -3.827531 0.566411 q: 0.005405 0.808545 -0.059533 0.585390 
time: 1403636919.501667, t: 9.213582 -3.846252 0.598173 q: 0.017371 0.808368 -0.056792 0.585674 
time: 1403636919.601666, t: 9.223162 -3.868139 0.623568 q: 0.029190 0.808327 -0.052624 0.585651 
time: 1403636919.701667, t: 9.228770 -3.890006 0.641344 q: 0.035617 0.807368 -0.050676 0.586789 
time: 1403636919.801666, t: 9.231954 -3.910655 0.652596 q: 0.035577 0.806500 -0.052015 0.587865 
time: 1403636919.901666, t: 9.233498 -3.929601 0.657859 q: 0.033872 0.805922 -0.053624 0.588615 
time: 1403636920.001667, t: 9.232722 -3.949237 0.655843 q: 0.029133 0.804635 -0.056290 0.590377 
time: 1403636920.101666, t: 9.227930 -3.969032 0.649044 q: 0.023078 0.803263 -0.059421 0.592204 
wait for imu ... 
time: 1403636920.201667, t: 9.218688 -3.991214 0.637223 q: 0.017620 0.802462 -0.061851 0.593227 
time: 1403636920.301666, t: 9.205435 -4.016544 0.621491 q: 0.022861 0.802834 -0.057010 0.593030 
time: 1403636920.401666, t: 9.188482 -4.044078 0.602469 q: 0.031292 0.803238 -0.050165 0.592716 
time: 1403636920.501667, t: 9.166499 -4.069222 0.586614 q: 0.036370 0.805872 -0.045710 0.589202 
time: 1403636920.601666, t: 9.140662 -4.095037 0.578471 q: 0.037507 0.809652 -0.044334 0.584031 
time: 1403636920.701667, t: 9.112581 -4.116939 0.579241 q: 0.037713 0.814146 -0.044612 0.577714 
time: 1403636920.801666, t: 9.084415 -4.138511 0.586844 q: 0.037431 0.818025 -0.045450 0.572161 
time: 1403636920.901666, t: 9.056904 -4.155742 0.603310 q: 0.036538 0.820091 -0.046488 0.569171 
wait for imu ... 
time: 1403636921.101666, t: 9.005062 -4.186580 0.639302 q: 0.036976 0.820081 -0.048011 0.569029 
time: 1403636921.201667, t: 8.984362 -4.199139 0.649780 q: 0.037850 0.818607 -0.048122 0.571081 
time: 1403636921.301666, t: 8.964871 -4.210498 0.654964 q: 0.038306 0.812734 -0.048026 0.579387 
wait for imu ... 
time: 1403636921.401666, t: 8.943969 -4.220468 0.653935 q: 0.033900 0.808426 -0.051437 0.585365 
time: 1403636921.501667, t: 8.920265 -4.229645 0.647402 q: 0.027235 0.807017 -0.056298 0.587207 
time: 1403636921.601666, t: 8.893559 -4.240056 0.634444 q: 0.021437 0.810071 -0.060974 0.582759 
time: 1403636921.701667, t: 8.865477 -4.252402 0.615098 q: 0.020341 0.819289 -0.062651 0.569585 
time: 1403636921.801666, t: 8.839467 -4.266765 0.593256 q: 0.023264 0.828217 -0.061649 0.556520 
time: 1403636921.901666, t: 8.818491 -4.283212 0.572741 q: 0.027597 0.828777 -0.060013 0.555666 
time: 1403636922.001667, t: 8.802930 -4.301594 0.552844 q: 0.033468 0.822677 -0.058842 0.564465 
time: 1403636922.101666, t: 8.788426 -4.320293 0.532736 q: 0.040353 0.816403 -0.060054 0.572933 
time: 1403636922.201667, t: 8.774241 -4.337671 0.515563 q: 0.046120 0.813471 -0.062872 0.576355 
time: 1403636922.301666, t: 8.760211 -4.354463 0.505037 q: 0.051513 0.817400 -0.065630 0.569996 
time: 1403636922.401666, t: 8.747038 -4.368520 0.499958 q: 0.054310 0.820909 -0.069486 0.564207 
time: 1403636922.501667, t: 8.735087 -4.381865 0.490433 q: 0.058583 0.824079 -0.075453 0.558363 
time: 1403636922.601666, t: 8.725555 -4.395577 0.475548 q: 0.064851 0.824809 -0.084419 0.555299 
time: 1403636922.701667, t: 8.721148 -4.407977 0.459267 q: 0.072534 0.819029 -0.091522 0.561742 
time: 1403636922.801666, t: 8.718394 -4.417718 0.442758 q: 0.074499 0.809325 -0.099827 0.574001 
time: 1403636922.901666, t: 8.713902 -4.425142 0.429888 q: 0.070418 0.802411 -0.109226 0.582450 
time: 1403636923.001667, t: 8.705678 -4.432399 0.423927 q: 0.064696 0.798036 -0.117688 0.587455 
time: 1403636923.101666, t: 8.695186 -4.441384 0.420865 q: 0.066261 0.797269 -0.122994 0.587235 
time: 1403636923.201667, t: 8.681312 -4.452025 0.411289 q: 0.072887 0.798458 -0.130778 0.583138 
time: 1403636923.301666, t: 8.662062 -4.465122 0.399091 q: 0.084436 0.802750 -0.137105 0.574165 
time: 1403636923.401666, t: 8.641500 -4.478487 0.380530 q: 0.093673 0.808502 -0.142139 0.563335 
time: 1403636923.501667, t: 8.621765 -4.492140 0.357383 q: 0.098726 0.814640 -0.147326 0.552187 
time: 1403636923.601666, t: 8.605988 -4.505660 0.336161 q: 0.100342 0.820742 -0.151728 0.541564 
time: 1403636923.701667, t: 8.597630 -4.520612 0.324753 q: 0.102516 0.823626 -0.153660 0.536208 
time: 1403636923.801666, t: 8.597400 -4.536908 0.325276 q: 0.104336 0.823072 -0.154150 0.536567 
time: 1403636923.901666, t: 8.604931 -4.551793 0.328446 q: 0.105183 0.823251 -0.153833 0.536218 
time: 1403636924.001667, t: 8.620679 -4.566605 0.332098 q: 0.106487 0.823487 -0.152238 0.536054 
time: 1403636924.101666, t: 8.642941 -4.581644 0.327273 q: 0.107466 0.822998 -0.151129 0.536922 
time: 1403636924.201667, t: 8.670839 -4.597515 0.317523 q: 0.108268 0.821920 -0.149122 0.538970 
time: 1403636924.301666, t: 8.704358 -4.613112 0.305900 q: 0.107489 0.820698 -0.148318 0.541205 
time: 1403636924.401666, t: 8.742950 -4.628639 0.300627 q: 0.100603 0.818639 -0.151900 0.544642 
time: 1403636924.501667, t: 8.784524 -4.645497 0.306743 q: 0.093562 0.813063 -0.155668 0.553121 
time: 1403636924.601666, t: 8.830366 -4.665096 0.316601 q: 0.085566 0.806817 -0.159672 0.562343 
time: 1403636924.701667, t: 8.875331 -4.688510 0.325464 q: 0.079894 0.802110 -0.162285 0.569123 
time: 1403636924.801666, t: 8.919358 -4.715233 0.328020 q: 0.073982 0.797135 -0.163841 0.576419 
time: 1403636924.901666, t: 8.961105 -4.747422 0.323221 q: 0.072939 0.793070 -0.164710 0.581885 
time: 1403636925.001667, t: 8.996895 -4.785021 0.309760 q: 0.077684 0.791733 -0.170365 0.581463 
time: 1403636925.101666, t: 9.025216 -4.827858 0.288188 q: 0.089865 0.793045 -0.171867 0.577465 
time: 1403636925.201667, t: 9.048978 -4.872194 0.262165 q: 0.105900 0.797161 -0.168845 0.569922 
time: 1403636925.301666, t: 9.070388 -4.916779 0.231975 q: 0.116189 0.803803 -0.167927 0.558749 
time: 1403636925.401666, t: 9.093086 -4.961025 0.197680 q: 0.121197 0.811985 -0.169509 0.545214 
time: 1403636925.501667, t: 9.119382 -5.004068 0.165354 q: 0.122926 0.817629 -0.172164 0.535474 
time: 1403636925.601666, t: 9.149766 -5.045924 0.138688 q: 0.123272 0.822685 -0.174662 0.526769 
wait for imu ... 
wait for imu ... 
time: 1403636925.701667, t: 9.189969 -5.087462 0.121600 q: 0.123015 0.825721 -0.176807 0.521336 
time: 1403636925.801666, t: 9.240462 -5.129555 0.115731 q: 0.124933 0.826284 -0.176623 0.520049 
time: 1403636925.901666, t: 9.301720 -5.172659 0.118662 q: 0.127144 0.820168 -0.175334 0.529543 
time: 1403636926.001667, t: 9.370851 -5.214307 0.126932 q: 0.131595 0.809565 -0.171929 0.545644 
time: 1403636926.101666, t: 9.443834 -5.253420 0.131251 q: 0.129637 0.797860 -0.172628 0.562862 
time: 1403636926.201667, t: 9.514177 -5.289757 0.127853 q: 0.122949 0.789635 -0.175926 0.574813 
time: 1403636926.401666, t: 9.642747 -5.357947 0.099952 q: 0.109423 0.782891 -0.181933 0.584815 
time: 1403636926.501667, t: 9.696258 -5.392058 0.077777 q: 0.105650 0.783160 -0.182613 0.584937 
time: 1403636926.601666, t: 9.743006 -5.425421 0.058638 q: 0.108086 0.786764 -0.178962 0.580768 
time: 1403636926.701667, t: 9.782740 -5.461275 0.046060 q: 0.111182 0.789854 -0.175458 0.577047 
time: 1403636926.801666, t: 9.816403 -5.495859 0.041370 q: 0.110931 0.793254 -0.179948 0.571018 
time: 1403636926.901666, t: 9.846497 -5.530679 0.043314 q: 0.111365 0.794345 -0.191115 0.565764 
time: 1403636927.001667, t: 9.876654 -5.572097 0.045048 q: 0.116239 0.791878 -0.205858 0.563064 
time: 1403636927.101666, t: 9.902665 -5.614746 0.040612 q: 0.126935 0.787871 -0.221365 0.560486 
time: 1403636927.201667, t: 9.925282 -5.660278 0.031285 q: 0.139781 0.782444 -0.239542 0.557550 
time: 1403636927.301666, t: 9.946254 -5.708452 0.015783 q: 0.155856 0.777039 -0.257212 0.552957 
time: 1403636927.401666, t: 9.963154 -5.759727 -0.004641 q: 0.170579 0.777481 -0.269102 0.542227 
time: 1403636927.501667, t: 9.978209 -5.810503 -0.029276 q: 0.178990 0.782656 -0.278614 0.527054 
time: 1403636927.601666, t: 9.997576 -5.862501 -0.054258 q: 0.183478 0.787654 -0.286675 0.513571 
time: 1403636927.701667, t: 10.024608 -5.917000 -0.071758 q: 0.188253 0.789437 -0.291056 0.506592 
time: 1403636927.801666, t: 10.059548 -5.975221 -0.079427 q: 0.189297 0.788704 -0.294882 0.505131 
time: 1403636927.901666, t: 10.100537 -6.035230 -0.079723 q: 0.189962 0.787237 -0.296449 0.506252 
time: 1403636928.001667, t: 10.147578 -6.099159 -0.083761 q: 0.190760 0.785571 -0.296634 0.508427 
time: 1403636928.101666, t: 10.200534 -6.165654 -0.089995 q: 0.193124 0.782285 -0.294570 0.513773 
time: 1403636928.201667, t: 10.257019 -6.235171 -0.100453 q: 0.194911 0.775624 -0.292139 0.524474 
time: 1403636928.301666, t: 10.316934 -6.305167 -0.113555 q: 0.193744 0.767801 -0.291161 0.536814 
time: 1403636928.401666, t: 10.375327 -6.373525 -0.131643 q: 0.194100 0.762260 -0.288902 0.545729 
time: 1403636928.501667, t: 10.429825 -6.440020 -0.151525 q: 0.196311 0.760061 -0.286140 0.549448 
time: 1403636928.701667, t: 10.531109 -6.567676 -0.176924 q: 0.196859 0.768370 -0.283724 0.538847 
time: 1403636928.801666, t: 10.581686 -6.628949 -0.179195 q: 0.196569 0.773965 -0.283354 0.531083 
time: 1403636928.901666, t: 10.632156 -6.688755 -0.172364 q: 0.196708 0.776205 -0.283131 0.527872 
time: 1403636929.001667, t: 10.686918 -6.747960 -0.155127 q: 0.197668 0.770298 -0.282290 0.536545 
time: 1403636929.101666, t: 10.739853 -6.805568 -0.129881 q: 0.193136 0.761245 -0.285218 0.549414 
time: 1403636929.201667, t: 10.789978 -6.859069 -0.099770 q: 0.187890 0.756588 -0.288626 0.555847 
time: 1403636929.301666, t: 10.834342 -6.911299 -0.071476 q: 0.181772 0.756163 -0.292301 0.556540 
time: 1403636929.401666, t: 10.873548 -6.964737 -0.049197 q: 0.180591 0.756866 -0.291949 0.556154 
time: 1403636929.501667, t: 10.909026 -7.022101 -0.032997 q: 0.183485 0.759402 -0.288661 0.553459 
time: 1403636929.601666, t: 10.940192 -7.079053 -0.022595 q: 0.189966 0.762291 -0.283013 0.550208 
time: 1403636929.701667, t: 10.966739 -7.131936 -0.018544 q: 0.196247 0.764405 -0.277714 0.547765 
time: 1403636929.801666, t: 10.992396 -7.183037 -0.019138 q: 0.199331 0.765874 -0.274963 0.545984 
time: 1403636929.901666, t: 11.015413 -7.227833 -0.025541 q: 0.198157 0.764790 -0.275358 0.547730 
time: 1403636930.001667, t: 11.038589 -7.272420 -0.026632 q: 0.194038 0.763035 -0.277955 0.550335 
time: 1403636930.101666, t: 11.059786 -7.315579 -0.020174 q: 0.191281 0.763166 -0.279654 0.550257 
time: 1403636930.201667, t: 11.076968 -7.353287 -0.008700 q: 0.188128 0.762761 -0.282389 0.550509 
time: 1403636930.301666, t: 11.094105 -7.395858 0.009350 q: 0.185161 0.763917 -0.284525 0.548809 
time: 1403636930.401666, t: 11.107678 -7.438446 0.034548 q: 0.187672 0.763865 -0.283092 0.548769 
time: 1403636930.501667, t: 11.119671 -7.481384 0.055734 q: 0.193471 0.764649 -0.279446 0.547531 
time: 1403636930.601666, t: 11.129458 -7.521020 0.067997 q: 0.195786 0.764548 -0.277572 0.547802 
time: 1403636930.701667, t: 11.137138 -7.559804 0.075711 q: 0.191557 0.763062 -0.280325 0.549964 
time: 1403636930.801666, t: 11.141244 -7.593633 0.075920 q: 0.183732 0.761186 -0.285558 0.552535 
time: 1403636930.901666, t: 11.142273 -7.631041 0.072497 q: 0.179310 0.758831 -0.290045 0.554885 
time: 1403636931.001667, t: 11.141031 -7.672070 0.064400 q: 0.175586 0.754637 -0.300778 0.556081 
time: 1403636931.201667, t: 11.121816 -7.764517 0.050487 q: 0.199075 0.746799 -0.307345 0.555157 
time: 1403636931.301666, t: 11.103591 -7.810062 0.054579 q: 0.212162 0.741722 -0.306322 0.557676 
time: 1403636931.401666, t: 11.080316 -7.851634 0.063336 q: 0.222249 0.740384 -0.305200 0.556139 
time: 1403636931.501667, t: 11.052012 -7.887767 0.077539 q: 0.233390 0.742749 -0.301568 0.550372 
time: 1403636931.601666, t: 11.021037 -7.915845 0.096122 q: 0.244048 0.745595 -0.297011 0.544347 
time: 1403636931.701667, t: 10.990032 -7.934658 0.112380 q: 0.249696 0.746131 -0.294745 0.542279 
time: 1403636931.801666, t: 10.959840 -7.950110 0.119992 q: 0.239384 0.747447 -0.302932 0.540602 
time: 1403636931.901666, t: 10.928302 -7.959338 0.120779 q: 0.230184 0.749424 -0.310062 0.537810 
time: 1403636932.001667, t: 10.898018 -7.966639 0.116693 q: 0.225128 0.749955 -0.314193 0.536813 
time: 1403636932.101666, t: 10.865665 -7.970866 0.107168 q: 0.224826 0.747302 -0.314849 0.540243 
time: 1403636932.201667, t: 10.834335 -7.975231 0.095104 q: 0.227301 0.744344 -0.314139 0.543694 
time: 1403636932.301666, t: 10.798607 -7.974109 0.088503 q: 0.237862 0.742433 -0.308635 0.544940 
time: 1403636932.401666, t: 10.763093 -7.966573 0.088806 q: 0.247707 0.742042 -0.304294 0.543525 
time: 1403636932.501667, t: 10.728861 -7.953221 0.095900 q: 0.252746 0.740297 -0.306664 0.542251 
time: 1403636932.601666, t: 10.690530 -7.931952 0.109571 q: 0.255552 0.737302 -0.318031 0.538456 
time: 1403636932.701667, t: 10.651215 -7.905676 0.130926 q: 0.256759 0.731193 -0.334341 0.536328 
time: 1403636932.801666, t: 10.613340 -7.874839 0.159112 q: 0.261011 0.725748 -0.346259 0.534104 
time: 1403636932.901666, t: 10.574254 -7.837803 0.190197 q: 0.265035 0.722923 -0.356503 0.529192 
wait for imu ... 
time: 1403636933.001667, t: 10.535154 -7.797140 0.211405 q: 0.268004 0.721693 -0.364815 0.523681 
time: 1403636933.101666, t: 10.494724 -7.753503 0.217325 q: 0.273124 0.718498 -0.374300 0.518714 
time: 1403636933.201667, t: 10.453690 -7.707020 0.211513 q: 0.273158 0.719339 -0.384178 0.510239 
time: 1403636933.301666, t: 10.412843 -7.658615 0.198180 q: 0.268752 0.727627 -0.394108 0.492961 
time: 1403636933.401666, t: 10.375234 -7.611361 0.183291 q: 0.263771 0.731882 -0.400933 0.483763 
time: 1403636933.501667, t: 10.341672 -7.569295 0.168194 q: 0.265353 0.729008 -0.401236 0.486975 
time: 1403636933.601666, t: 10.311591 -7.530830 0.154558 q: 0.268926 0.724434 -0.398625 0.493936 
time: 1403636933.701667, t: 10.284700 -7.495038 0.145958 q: 0.272352 0.720564 -0.394972 0.500608 
time: 1403636933.801666, t: 10.259781 -7.462405 0.146179 q: 0.275259 0.717301 -0.390943 0.506829 
time: 1403636933.901666, t: 10.235207 -7.427251 0.152235 q: 0.274822 0.713506 -0.390686 0.512588 
time: 1403636934.001667, t: 10.211311 -7.395348 0.162953 q: 0.275575 0.710006 -0.396407 0.512651 
time: 1403636934.101666, t: 10.182810 -7.359287 0.167247 q: 0.281774 0.703967 -0.407115 0.509206 
time: 1403636934.201667, t: 10.151374 -7.323509 0.157528 q: 0.293902 0.695267 -0.421215 0.502796 
time: 1403636934.301666, t: 10.120118 -7.288638 0.138077 q: 0.307069 0.684370 -0.438226 0.495282 
time: 1403636934.401666, t: 10.086109 -7.249980 0.115883 q: 0.316704 0.669493 -0.460067 0.489710 
time: 1403636934.501667, t: 10.051215 -7.210976 0.098038 q: 0.324953 0.654092 -0.483249 0.482741 
time: 1403636934.601666, t: 10.012374 -7.171678 0.087957 q: 0.337304 0.639896 -0.502997 0.473026 
time: 1403636934.701667, t: 9.969336 -7.128075 0.090909 q: 0.347686 0.629075 -0.519982 0.461516 
time: 1403636934.801666, t: 9.923996 -7.085636 0.101389 q: 0.356873 0.626024 -0.530084 0.446929 
time: 1403636934.901666, t: 9.878385 -7.044164 0.119112 q: 0.362697 0.625918 -0.536486 0.434581 
time: 1403636935.001667, t: 9.833899 -7.003597 0.144734 q: 0.365750 0.625666 -0.539814 0.428217 
time: 1403636935.101666, t: 9.788205 -6.965922 0.177032 q: 0.370116 0.623008 -0.537900 0.430743 
time: 1403636935.201667, t: 9.744675 -6.931628 0.213514 q: 0.370913 0.618326 -0.535688 0.439471 
time: 1403636935.301666, t: 9.705962 -6.899050 0.246046 q: 0.368805 0.616553 -0.534431 0.445228 
time: 1403636935.401666, t: 9.662260 -6.868110 0.270212 q: 0.365088 0.616856 -0.533575 0.448884 
time: 1403636935.501667, t: 9.620251 -6.835574 0.288059 q: 0.363067 0.617932 -0.531837 0.451101 
time: 1403636935.601666, t: 9.573331 -6.806076 0.297688 q: 0.361136 0.619218 -0.530356 0.452629 
time: 1403636935.701667, t: 9.527243 -6.772525 0.300467 q: 0.360422 0.620484 -0.528507 0.453626 
time: 1403636935.801666, t: 9.479599 -6.740344 0.294931 q: 0.357147 0.626252 -0.529057 0.447607 
time: 1403636935.901666, t: 9.430788 -6.708715 0.281991 q: 0.352967 0.632234 -0.530907 0.440264 
time: 1403636936.001667, t: 9.383972 -6.685504 0.259937 q: 0.343692 0.639045 -0.536566 0.430806 
time: 1403636936.101666, t: 9.335400 -6.664517 0.237800 q: 0.339123 0.643036 -0.539250 0.425099 
time: 1403636936.201667, t: 9.290107 -6.652021 0.216003 q: 0.342040 0.643523 -0.537687 0.424004 
wait for imu ... 
time: 1403636936.301666, t: 9.246961 -6.647372 0.199373 q: 0.348382 0.640528 -0.534101 0.427890 
time: 1403636936.401666, t: 9.199765 -6.643060 0.189166 q: 0.354473 0.635642 -0.531160 0.433795 
time: 1403636936.501667, t: 9.159798 -6.645586 0.188069 q: 0.360098 0.630416 -0.528445 0.440058 
time: 1403636936.601666, t: 9.122540 -6.652128 0.191737 q: 0.363265 0.626901 -0.528224 0.442733 
time: 1403636936.701667, t: 9.085476 -6.658599 0.190642 q: 0.364334 0.625168 -0.529664 0.442585 
time: 1403636936.801666, t: 9.041819 -6.660753 0.182990 q: 0.366527 0.624584 -0.529815 0.441417 
time: 1403636936.901666, t: 9.005787 -6.667783 0.176055 q: 0.369277 0.625351 -0.529411 0.438514 
time: 1403636937.001667, t: 8.965070 -6.671351 0.173906 q: 0.371327 0.626147 -0.528873 0.436292 
time: 1403636937.101666, t: 8.928657 -6.678252 0.179448 q: 0.371716 0.626129 -0.529614 0.435084 
time: 1403636937.201667, t: 8.892251 -6.683147 0.190637 q: 0.369059 0.623675 -0.532504 0.437338 
time: 1403636937.301666, t: 8.856716 -6.690118 0.199350 q: 0.368477 0.622924 -0.533797 0.437322 
time: 1403636937.401666, t: 8.821776 -6.695566 0.202984 q: 0.369017 0.621916 -0.534364 0.437609 
time: 1403636937.501667, t: 8.787317 -6.701072 0.200154 q: 0.369411 0.621290 -0.534899 0.437512 
wait for imu ... 
time: 1403636937.601666, t: 8.754139 -6.708701 0.190242 q: 0.370990 0.620829 -0.534844 0.436897 
time: 1403636937.701667, t: 8.721629 -6.713397 0.176761 q: 0.376416 0.623124 -0.532166 0.432234 
time: 1403636937.801666, t: 8.691615 -6.718304 0.157897 q: 0.382653 0.625114 -0.528645 0.428186 
time: 1403636937.901666, t: 8.666804 -6.723042 0.133422 q: 0.390162 0.621697 -0.524527 0.431438 
time: 1403636938.001667, t: 8.644449 -6.725474 0.106912 q: 0.398862 0.618078 -0.519727 0.434481 
time: 1403636938.101666, t: 8.626152 -6.722383 0.075445 q: 0.404685 0.612188 -0.518309 0.439103 
time: 1403636938.201667, t: 8.613131 -6.713510 0.042540 q: 0.404422 0.603391 -0.524028 0.444698 
time: 1403636938.301666, t: 8.604452 -6.694877 0.012904 q: 0.399662 0.594695 -0.532432 0.450693 
time: 1403636938.401666, t: 8.593575 -6.669590 -0.010441 q: 0.383562 0.585415 -0.547635 0.458547 
time: 1403636938.501667, t: 8.576018 -6.637195 -0.029055 q: 0.368390 0.580157 -0.561551 0.460833 
wait for imu ... 
time: 1403636938.601666, t: 8.551608 -6.602947 -0.038101 q: 0.361773 0.580600 -0.569029 0.456322 
time: 1403636938.701667, t: 8.517169 -6.569892 -0.039795 q: 0.360498 0.580893 -0.571753 0.453546 
time: 1403636938.801666, t: 8.474484 -6.537786 -0.035527 q: 0.364829 0.583709 -0.570465 0.448055 
time: 1403636938.901666, t: 8.424631 -6.507143 -0.029311 q: 0.372277 0.590273 -0.566197 0.438644 
time: 1403636939.001667, t: 8.372331 -6.473095 -0.023539 q: 0.377759 0.596617 -0.562964 0.429440 
time: 1403636939.101666, t: 8.316780 -6.440241 -0.022677 q: 0.380241 0.599312 -0.560846 0.426255 
time: 1403636939.201667, t: 8.260512 -6.410240 -0.027450 q: 0.382095 0.598839 -0.558266 0.428643 
time: 1403636939.301666, t: 8.205872 -6.384221 -0.037279 q: 0.382865 0.598397 -0.556197 0.431254 
time: 1403636939.401666, t: 8.147009 -6.356191 -0.049023 q: 0.382699 0.599484 -0.553761 0.433023 
time: 1403636939.501667, t: 8.091266 -6.328791 -0.055449 q: 0.384351 0.599553 -0.549886 0.436390 
time: 1403636939.601666, t: 8.035552 -6.299519 -0.053124 q: 0.388891 0.597848 -0.544275 0.441709 
time: 1403636939.701667, t: 7.976103 -6.265967 -0.044872 q: 0.389777 0.594636 -0.542044 0.447962 
time: 1403636939.801666, t: 7.919119 -6.230402 -0.037961 q: 0.391107 0.594423 -0.540657 0.448760 
time: 1403636939.901666, t: 7.859331 -6.187200 -0.037582 q: 0.388540 0.594697 -0.542626 0.448251 
time: 1403636940.001667, t: 7.799438 -6.141972 -0.044739 q: 0.387173 0.591138 -0.548336 0.447193 
time: 1403636940.101666, t: 7.736009 -6.091800 -0.057834 q: 0.388116 0.587680 -0.555354 0.442245 
time: 1403636940.201667, t: 7.670190 -6.038320 -0.075670 q: 0.386512 0.584486 -0.562957 0.438251 
time: 1403636940.301666, t: 7.602361 -5.984205 -0.096578 q: 0.387458 0.581366 -0.567444 0.435773 
time: 1403636940.401666, t: 7.532873 -5.929616 -0.120273 q: 0.389467 0.578023 -0.570462 0.434486 
time: 1403636940.501667, t: 7.461629 -5.872944 -0.144051 q: 0.390823 0.575635 -0.573143 0.432907 
time: 1403636940.601666, t: 7.388375 -5.815719 -0.162086 q: -0.388094 -0.576885 0.578015 -0.427183 
time: 1403636940.701667, t: 7.312503 -5.759977 -0.173281 q: 0.387707 0.581047 -0.580520 0.418406 
time: 1403636940.801666, t: 7.236337 -5.706034 -0.175666 q: 0.386665 0.584750 -0.582315 0.411663 
time: 1403636940.901666, t: 7.161230 -5.654969 -0.169460 q: 0.387550 0.587680 -0.582246 0.406727 
time: 1403636941.001667, t: 7.086618 -5.607934 -0.156111 q: 0.389584 0.587666 -0.580753 0.406937 
time: 1403636941.101666, t: 7.013081 -5.565882 -0.136927 q: 0.392674 0.586536 -0.578454 0.408869 
time: 1403636941.201667, t: 6.941210 -5.525166 -0.114491 q: 0.394548 0.585566 -0.576353 0.411414 
time: 1403636941.301666, t: 6.869475 -5.485658 -0.093051 q: 0.396765 0.583923 -0.573372 0.415759 
time: 1403636941.401666, t: 6.799792 -5.448173 -0.076876 q: 0.398072 0.584463 -0.569978 0.418410 
time: 1403636941.501667, t: 6.730343 -5.409784 -0.066471 q: 0.398440 0.585460 -0.566844 0.420916 
time: 1403636941.601666, t: 6.661610 -5.370320 -0.060952 q: 0.397426 0.587901 -0.564906 0.421078 
time: 1403636941.701667, t: 6.595164 -5.332213 -0.061121 q: 0.396319 0.589984 -0.563555 0.421018 
time: 1403636941.801666, t: 6.530156 -5.291902 -0.061074 q: 0.394493 0.591468 -0.562962 0.421443 
time: 1403636941.901666, t: 6.467151 -5.255054 -0.056194 q: 0.391856 0.592944 -0.563382 0.421268 
time: 1403636942.101666, t: 6.342104 -5.177082 -0.028143 q: 0.386994 0.591907 -0.565516 0.424351 
time: 1403636942.201667, t: 6.278364 -5.136496 -0.011481 q: 0.386304 0.590042 -0.566175 0.426691 
time: 1403636942.301666, t: 6.215265 -5.098527 -0.002312 q: 0.385182 0.587890 -0.567899 0.428382 
time: 1403636942.401666, t: 6.151109 -5.058484 0.002044 q: 0.383210 0.584603 -0.570590 0.431064 
time: 1403636942.501667, t: 6.087655 -5.019805 -0.001018 q: 0.380963 0.582569 -0.574076 0.431180 
time: 1403636942.601666, t: 6.020157 -4.980367 -0.009143 q: 0.378677 0.582935 -0.577411 0.428237 
time: 1403636942.701667, t: 5.947601 -4.940632 -0.020886 q: 0.378651 0.585723 -0.578976 0.422301 
time: 1403636942.801666, t: 5.874164 -4.902215 -0.034308 q: 0.380423 0.587255 -0.579164 0.418306 
time: 1403636942.901666, t: 5.800200 -4.867020 -0.040836 q: 0.386935 0.586173 -0.576188 0.417959 
time: 1403636943.001667, t: 5.727065 -4.834623 -0.039108 q: 0.391648 0.585109 -0.574188 0.417813 
time: 1403636943.101666, t: 5.653242 -4.801825 -0.028914 q: 0.396401 0.583364 -0.572176 0.418530 
time: 1403636943.201667, t: 5.579207 -4.769630 -0.017027 q: 0.398119 0.580706 -0.572356 0.420346 
time: 1403636943.301666, t: 5.507172 -4.737056 -0.010798 q: 0.400236 0.579822 -0.572539 0.419304 
time: 1403636943.401666, t: 5.435973 -4.703177 -0.010339 q: 0.402757 0.578154 -0.572850 0.418769 
time: 1403636943.501667, t: 5.366545 -4.667685 -0.015654 q: 0.404655 0.577175 -0.573149 0.417880 
time: 1403636943.601666, t: 5.297831 -4.631255 -0.026871 q: 0.407053 0.576602 -0.572778 0.416850 
time: 1403636943.701667, t: 5.228943 -4.593583 -0.042710 q: 0.409009 0.575704 -0.572264 0.416882 
time: 1403636943.801666, t: 5.163286 -4.553658 -0.065017 q: 0.410277 0.575922 -0.572226 0.415384 
time: 1403636943.901666, t: 5.099532 -4.511699 -0.093364 q: 0.404005 0.580428 -0.572374 0.415055 
time: 1403636944.001667, t: 5.036879 -4.468748 -0.127762 q: 0.389973 0.592202 -0.573608 0.410112 
time: 1403636944.101666, t: 4.976600 -4.428827 -0.161307 q: 0.376451 0.604543 -0.575227 0.402401 
time: 1403636944.201667, t: 4.918540 -4.395091 -0.186865 q: 0.368800 0.612945 -0.574218 0.398195 
time: 1403636944.301666, t: 4.863200 -4.370768 -0.202774 q: 0.365926 0.615769 -0.571612 0.400233 
time: 1403636944.401666, t: 4.810663 -4.355517 -0.209366 q: 0.368817 0.613794 -0.566340 0.408031 
time: 1403636944.501667, t: 4.759750 -4.343536 -0.208300 q: 0.379216 0.611628 -0.556692 0.414972 
time: 1403636944.601666, t: 4.712896 -4.334279 -0.201342 q: 0.393167 0.611347 -0.545004 0.417906 
time: 1403636944.701667, t: 4.672927 -4.326603 -0.189996 q: 0.400987 0.614448 -0.535370 0.418380 
time: 1403636944.801666, t: 4.639245 -4.316403 -0.176061 q: 0.395747 0.614621 -0.533698 0.425197 
time: 1403636945.001667, t: 4.582547 -4.288256 -0.136402 q: 0.386503 0.614148 -0.532694 0.435516 
time: 1403636945.101666, t: 4.555721 -4.269264 -0.128400 q: 0.385356 0.614878 -0.531030 0.437532 
time: 1403636945.201667, t: 4.536010 -4.248654 -0.131655 q: 0.384491 0.615867 -0.530025 0.438119 
time: 1403636945.301666, t: 4.515138 -4.224022 -0.137580 q: 0.383843 0.616128 -0.529475 0.438985 
time: 1403636945.401666, t: 4.497789 -4.197902 -0.136775 q: 0.384099 0.616755 -0.529301 0.438089 
time: 1403636945.501667, t: 4.483209 -4.169797 -0.130059 q: 0.384442 0.616090 -0.529901 0.437998 
time: 1403636945.601666, t: 4.470947 -4.140029 -0.126358 q: 0.380734 0.617161 -0.534158 0.434543 
time: 1403636945.701667, t: 4.460321 -4.110121 -0.130280 q: 0.373886 0.622993 -0.540856 0.423749 
time: 1403636945.801666, t: 4.453025 -4.081937 -0.140887 q: 0.367379 0.629402 -0.546872 0.412089 
time: 1403636945.901666, t: 4.449225 -4.056496 -0.154469 q: 0.365116 0.629810 -0.550253 0.408964 
time: 1403636946.001667, t: 4.447108 -4.036276 -0.169320 q: 0.372999 0.624575 -0.546124 0.415363 
time: 1403636946.101666, t: 4.448500 -4.020059 -0.189626 q: 0.379737 0.619386 -0.543022 0.421056 
time: 1403636946.201667, t: 4.450902 -4.004432 -0.217146 q: 0.381958 0.616192 -0.543327 0.423333 
time: 1403636946.301666, t: 4.456797 -3.988127 -0.249763 q: 0.386739 0.613018 -0.541562 0.425856 
time: 1403636946.401666, t: 4.463988 -3.973169 -0.289837 q: 0.389869 0.609731 -0.541237 0.428127 
time: 1403636946.501667, t: 4.473180 -3.956121 -0.335574 q: 0.390871 0.607763 -0.542608 0.428277 
time: 1403636946.601666, t: 4.483650 -3.936247 -0.380918 q: 0.392470 0.606285 -0.543359 0.427956 
time: 1403636946.701667, t: 4.496234 -3.914397 -0.422646 q: 0.386287 0.603727 -0.549165 0.429783 
time: 1403636946.801666, t: 4.509130 -3.890137 -0.457986 q: 0.371806 0.595611 -0.560347 0.439339 
time: 1403636946.901666, t: 4.518440 -3.866954 -0.488265 q: 0.362169 0.590847 -0.567736 0.444308 
time: 1403636947.001667, t: 4.520001 -3.844272 -0.520924 q: 0.350215 0.584791 -0.576018 0.451191 
time: 1403636947.101666, t: 4.513118 -3.825136 -0.556017 q: 0.347808 0.585220 -0.577976 0.449990 
time: 1403636947.201667, t: 4.496402 -3.807487 -0.587739 q: 0.355531 0.596562 -0.573575 0.434423 
time: 1403636947.301666, t: 4.475181 -3.790802 -0.614280 q: 0.360430 0.611671 -0.570754 0.412539 
time: 1403636947.401666, t: 4.453513 -3.782646 -0.631729 q: 0.359677 0.621290 -0.571123 0.398056 
time: 1403636947.501667, t: 4.432537 -3.784360 -0.640687 q: 0.359627 0.621460 -0.571668 0.397054 
time: 1403636947.601666, t: 4.413698 -3.795510 -0.641421 q: 0.363419 0.614273 -0.570147 0.406851 
time: 1403636947.801666, t: 4.377446 -3.837299 -0.632262 q: 0.377082 0.614646 -0.560462 0.407310 
time: 1403636947.901666, t: 4.363257 -3.866005 -0.633859 q: 0.386950 0.619189 -0.552726 0.401708 
time: 1403636948.001667, t: 4.352104 -3.894082 -0.641927 q: 0.387305 0.620514 -0.551557 0.400927 
time: 1403636948.101666, t: 4.347706 -3.926700 -0.655507 q: 0.379765 0.621358 -0.555841 0.400916 
time: 1403636948.201667, t: 4.345747 -3.959582 -0.671514 q: 0.380478 0.623099 -0.553820 0.400333 
time: 1403636948.301666, t: 4.348962 -3.996548 -0.690199 q: 0.382419 0.624392 -0.550775 0.400671 
time: 1403636948.401666, t: 4.357166 -4.036958 -0.711926 q: 0.384965 0.626476 -0.546945 0.400227 
time: 1403636948.501667, t: 4.370968 -4.078522 -0.735263 q: 0.391452 0.630261 -0.539887 0.397566 
time: 1403636948.601666, t: 4.391678 -4.121993 -0.759552 q: 0.404156 0.634932 -0.528241 0.393041 
time: 1403636948.701667, t: 4.424311 -4.163892 -0.780421 q: 0.410839 0.635317 -0.521260 0.394806 
time: 1403636948.801666, t: 4.468723 -4.204895 -0.794856 q: 0.401967 0.628384 -0.526418 0.407970 
time: 1403636948.901666, t: 4.521179 -4.245975 -0.801447 q: 0.391605 0.619716 -0.532284 0.423405 
time: 1403636949.001667, t: 4.577493 -4.283795 -0.804344 q: 0.381653 0.609970 -0.537473 0.439773 
time: 1403636949.101666, t: 4.632170 -4.321046 -0.811185 q: 0.379247 0.607489 -0.537485 0.445240 
time: 1403636949.201667, t: 4.684695 -4.355180 -0.823103 q: 0.380290 0.609205 -0.535487 0.444413 
time: 1403636949.301666, t: 4.735567 -4.384794 -0.840007 q: 0.380608 0.610918 -0.534438 0.443049 
time: 1403636949.401666, t: 4.785214 -4.411834 -0.860714 q: 0.377417 0.609151 -0.535640 0.446745 
time: 1403636949.501667, t: 4.832861 -4.434498 -0.885559 q: 0.374858 0.607023 -0.536938 0.450224 
time: 1403636949.601666, t: 4.877809 -4.456097 -0.907897 q: 0.371259 0.605304 -0.538753 0.453342 
time: 1403636949.701667, t: 4.919636 -4.473646 -0.922812 q: 0.367893 0.608205 -0.541269 0.449187 
time: 1403636949.801666, t: 4.955241 -4.490517 -0.932360 q: 0.365909 0.613971 -0.542401 0.441534 
time: 1403636949.901666, t: 4.989625 -4.504737 -0.936457 q: 0.360731 0.618418 -0.546153 0.434913 
time: 1403636950.001667, t: 5.022046 -4.521322 -0.935762 q: 0.356408 0.620792 -0.549302 0.431112 
time: 1403636950.101666, t: 5.052912 -4.543470 -0.931489 q: 0.352550 0.622443 -0.552458 0.427859 
time: 1403636950.201667, t: 5.081307 -4.569603 -0.929847 q: 0.354787 0.624583 -0.551660 0.423902 
time: 1403636950.301666, t: 5.108326 -4.598269 -0.932432 q: 0.359420 0.625353 -0.548967 0.422356 
time: 1403636950.401666, t: 5.136071 -4.632864 -0.939665 q: 0.366210 0.624494 -0.544685 0.423338 
time: 1403636950.501667, t: 5.164151 -4.668672 -0.950368 q: 0.375570 0.623724 -0.538404 0.424307 
time: 1403636950.601666, t: 5.192844 -4.702072 -0.961075 q: 0.385091 0.624443 -0.531395 0.423551 
time: 1403636950.701667, t: 5.225625 -4.735806 -0.968019 q: 0.386419 0.623219 -0.529918 0.425988 
time: 1403636950.801666, t: 5.262964 -4.770560 -0.970622 q: 0.383763 0.619222 -0.531038 0.432770 
time: 1403636950.901666, t: 5.301289 -4.801927 -0.965641 q: 0.380591 0.617125 -0.532226 0.437084 
time: 1403636951.001667, t: 5.342753 -4.834014 -0.956258 q: 0.379067 0.616208 -0.532446 0.439429 
time: 1403636951.101666, t: 5.385161 -4.863944 -0.940143 q: 0.373158 0.613342 -0.536170 0.443944 
time: 1403636951.201667, t: 5.425638 -4.889860 -0.926603 q: 0.364740 0.614556 -0.542062 0.442102 
time: 1403636951.301666, t: 5.462017 -4.919734 -0.923010 q: 0.361004 0.620084 -0.544985 0.433777 
time: 1403636951.401666, t: 5.495558 -4.944943 -0.920507 q: 0.357707 0.625716 -0.547387 0.425314 
time: 1403636951.501667, t: 5.531350 -4.981963 -0.929098 q: 0.357567 0.628570 -0.547815 0.420647 
time: 1403636951.601666, t: 5.566780 -5.021655 -0.940722 q: 0.359720 0.630001 -0.546616 0.418223 
time: 1403636951.701667, t: 5.600494 -5.061553 -0.948952 q: 0.364335 0.629273 -0.543831 0.418955 
time: 1403636951.801666, t: 5.637843 -5.111293 -0.953169 q: 0.369120 0.626611 -0.540723 0.422762 
time: 1403636951.901666, t: 5.674220 -5.159799 -0.949169 q: 0.375914 0.626134 -0.535829 0.423713 
time: 1403636952.001667, t: 5.713840 -5.213490 -0.945357 q: 0.381585 0.627150 -0.531774 0.422247 
time: 1403636952.101666, t: 5.760034 -5.267206 -0.945181 q: 0.383764 0.625577 -0.530506 0.424195 
time: 1403636952.201667, t: 5.806578 -5.315133 -0.947712 q: 0.382827 0.621794 -0.531220 0.429676 
time: 1403636952.301666, t: 5.856824 -5.364408 -0.958671 q: 0.379292 0.616212 -0.533428 0.438034 
time: 1403636952.401666, t: 5.907673 -5.411553 -0.975801 q: 0.376528 0.611372 -0.535202 0.444983 
time: 1403636952.501667, t: 5.952226 -5.452398 -0.996525 q: 0.379048 0.611322 -0.532812 0.445780 
time: 1403636952.601666, t: 5.996806 -5.491560 -1.019938 q: 0.382343 0.613285 -0.529918 0.443714 
time: 1403636952.701667, t: 6.041183 -5.525417 -1.038245 q: 0.383746 0.614105 -0.528557 0.442991 
time: 1403636952.801666, t: 6.087007 -5.556543 -1.053547 q: 0.380640 0.617650 -0.530731 0.438116 
time: 1403636952.901666, t: 6.131873 -5.584221 -1.065847 q: 0.375124 0.620285 -0.534828 0.434151 
time: 1403636953.001667, t: 6.179601 -5.614541 -1.081998 q: 0.365614 0.620504 -0.542211 0.432791 
time: 1403636953.101666, t: 6.223737 -5.643968 -1.103192 q: 0.357808 0.618279 -0.548061 0.435125 
time: 1403636953.201667, t: 6.271020 -5.680346 -1.130458 q: 0.355030 0.617757 -0.550345 0.435259 
time: 1403636953.301666, t: 6.308872 -5.713217 -1.157886 q: 0.355468 0.618587 -0.549809 0.434399 
time: 1403636953.401666, t: 6.344797 -5.752257 -1.178732 q: 0.359430 0.619906 -0.546522 0.433405 
time: 1403636953.501667, t: 6.377801 -5.790200 -1.193017 q: 0.366655 0.621715 -0.540786 0.431955 
time: 1403636953.601666, t: 6.408702 -5.829569 -1.203346 q: 0.370852 0.623781 -0.536987 0.430129 
time: 1403636953.701667, t: 6.442192 -5.870610 -1.205604 q: 0.372299 0.629358 -0.534590 0.423693 
time: 1403636953.801666, t: 6.474686 -5.909964 -1.198118 q: 0.375724 0.634497 -0.530750 0.417791 
time: 1403636953.901666, t: 6.518050 -5.955213 -1.183913 q: 0.376163 0.635209 -0.528997 0.418537 
time: 1403636954.001667, t: 6.564779 -6.001560 -1.168302 q: 0.372858 0.633704 -0.530560 0.421784 
time: 1403636954.101666, t: 6.612939 -6.051348 -1.154468 q: 0.371573 0.632520 -0.531173 0.423919 
time: 1403636954.201667, t: 6.664246 -6.101129 -1.145968 q: 0.370713 0.631562 -0.531636 0.425517 
time: 1403636954.301666, t: 6.717611 -6.150106 -1.138807 q: 0.369405 0.632471 -0.532804 0.423839 
time: 1403636954.401666, t: 6.776846 -6.207139 -1.130627 q: 0.367156 0.633288 -0.535233 0.421507 
time: 1403636954.501667, t: 6.836677 -6.259982 -1.113804 q: 0.365736 0.632732 -0.537815 0.420288 
time: 1403636954.601666, t: 6.899109 -6.320013 -1.096672 q: 0.368276 0.632531 -0.538128 0.417965 
time: 1403636954.701667, t: 6.962420 -6.376891 -1.088819 q: 0.372430 0.629321 -0.538269 0.418949 
time: 1403636954.801666, t: 7.030253 -6.441884 -1.100603 q: 0.377044 0.623512 -0.538295 0.423448 
time: 1403636954.901666, t: 7.095073 -6.500800 -1.117819 q: 0.381135 0.617804 -0.538953 0.427299 
time: 1403636955.001667, t: 7.165704 -6.563008 -1.145488 q: 0.385063 0.613499 -0.539840 0.428857 
time: 1403636955.101666, t: 7.229704 -6.615751 -1.171701 q: 0.387787 0.612551 -0.541658 0.425452 
time: 1403636955.201667, t: 7.299258 -6.670391 -1.189656 q: 0.390863 0.611469 -0.543398 0.421960 
time: 1403636955.301666, t: 7.371886 -6.725475 -1.201219 q: 0.392817 0.608039 -0.546217 0.421463 
time: 1403636955.401666, t: 7.445002 -6.776920 -1.205152 q: 0.395492 0.605446 -0.548329 0.419949 
time: 1403636955.501667, t: 7.521028 -6.827822 -1.209581 q: 0.397459 0.604206 -0.550003 0.417682 
time: 1403636955.601666, t: 7.599495 -6.877458 -1.218986 q: 0.398605 0.602656 -0.551602 0.416719 
time: 1403636955.701667, t: 7.677562 -6.924637 -1.233650 q: 0.399715 0.601743 -0.552751 0.415450 
time: 1403636955.801666, t: 7.758139 -6.971783 -1.244014 q: 0.400533 0.601314 -0.553398 0.414423 
time: 1403636955.901666, t: 7.835885 -7.014101 -1.245760 q: 0.397257 0.598833 -0.556699 0.416740 
time: 1403636956.001667, t: 7.917055 -7.058089 -1.241302 q: 0.389702 0.592974 -0.563370 0.423235 
time: 1403636956.101666, t: 7.995587 -7.100436 -1.227488 q: 0.381857 0.585595 -0.570153 0.431496 
time: 1403636956.201667, t: 8.064288 -7.137579 -1.212596 q: 0.377828 0.581966 -0.574233 0.434532 
time: 1403636956.301666, t: 8.130075 -7.174980 -1.203934 q: 0.379662 0.582413 -0.574516 0.431953 
time: 1403636956.401666, t: 8.188937 -7.210418 -1.203530 q: 0.389444 0.588156 -0.569009 0.422653 
time: 1403636956.501667, t: 8.247968 -7.245295 -1.207276 q: 0.397502 0.592394 -0.562644 0.417724 
time: 1403636956.601666, t: 8.308119 -7.278524 -1.215886 q: 0.397471 0.593115 -0.555124 0.426695 
time: 1403636956.701667, t: 8.365086 -7.306564 -1.229324 q: 0.385507 0.594437 -0.549455 0.442864 
time: 1403636956.801666, t: 8.421760 -7.331644 -1.243747 q: 0.367004 0.602615 -0.542023 0.456480 
time: 1403636956.901666, t: 8.478987 -7.356580 -1.251984 q: 0.349544 0.618189 -0.528251 0.465417 
time: 1403636957.001667, t: 8.527904 -7.379292 -1.251092 q: 0.339494 0.636917 -0.506268 0.471989 
time: 1403636957.101666, t: 8.572150 -7.409443 -1.248373 q: 0.331161 0.656663 -0.480485 0.477765 
time: 1403636957.201667, t: 8.618698 -7.421512 -1.228630 q: 0.321956 0.674545 -0.457219 0.481959 
time: 1403636957.301666, t: 8.660933 -7.439243 -1.212520 q: 0.311467 0.686383 -0.441007 0.487216 
time: 1403636957.401666, t: 8.700765 -7.456384 -1.201888 q: 0.302648 0.694046 -0.429799 0.491911 
time: 1403636957.501667, t: 8.739765 -7.471140 -1.199139 q: 0.296093 0.698151 -0.423223 0.495779 
time: 1403636957.601666, t: 8.778496 -7.484713 -1.203614 q: 0.289346 0.701125 -0.417954 0.500018 
time: 1403636957.701667, t: 8.816182 -7.497904 -1.216656 q: 0.280949 0.705582 -0.410014 0.505084 
time: 1403636957.801666, t: 8.852537 -7.507975 -1.234264 q: 0.267016 0.713408 -0.398109 0.511137 
time: 1403636957.901666, t: 8.888244 -7.516750 -1.255223 q: 0.249110 0.724229 -0.379276 0.519217 
time: 1403636958.001667, t: 8.921962 -7.525649 -1.273826 q: 0.230220 0.736572 -0.355029 0.527650 
time: 1403636958.101666, t: 8.952454 -7.534576 -1.284705 q: 0.212109 0.748841 -0.329675 0.534380 
time: 1403636958.201667, t: 8.981434 -7.544676 -1.286729 q: 0.196865 0.758792 -0.308460 0.538823 
time: 1403636958.301666, t: 9.005869 -7.556901 -1.276862 q: 0.184847 0.765912 -0.288662 0.543952 
time: 1403636958.401666, t: 9.026559 -7.568411 -1.260638 q: 0.176686 0.773573 -0.270539 0.545137 
time: 1403636958.501667, t: 9.045113 -7.582126 -1.240649 q: 0.171252 0.778714 -0.258780 0.545261 
time: 1403636958.601666, t: 9.064189 -7.597410 -1.226254 q: 0.168564 0.781014 -0.252135 0.545922 
time: 1403636958.701667, t: 9.083589 -7.614493 -1.215665 q: 0.168529 0.781894 -0.248535 0.546324 
time: 1403636958.801666, t: 9.103215 -7.632023 -1.208579 q: 0.169003 0.782091 -0.247895 0.546186 
time: 1403636958.901666, t: 9.128117 -7.651600 -1.208056 q: 0.171047 0.782667 -0.248957 0.544238 
time: 1403636959.001667, t: 9.148663 -7.669205 -1.211565 q: 0.173955 0.782275 -0.250307 0.543260 
time: 1403636959.101666, t: 9.171924 -7.685857 -1.221230 q: 0.174991 0.782367 -0.253233 0.541436 
time: 1403636959.201667, t: 9.196211 -7.702570 -1.235902 q: 0.176172 0.781717 -0.255667 0.540848 
time: 1403636959.301666, t: 9.225955 -7.720948 -1.257170 q: 0.182127 0.781433 -0.255038 0.539581 
time: 1403636959.401666, t: 9.251268 -7.735358 -1.283031 q: 0.188257 0.781708 -0.253023 0.538025 
time: 1403636959.501667, t: 9.279598 -7.747209 -1.314393 q: 0.192470 0.779915 -0.251729 0.539740 
time: 1403636959.601666, t: 9.313815 -7.758097 -1.346421 q: 0.188982 0.774221 -0.255663 0.547270 
time: 1403636959.701667, t: 9.343776 -7.766140 -1.371577 q: 0.186830 0.773804 -0.258803 0.547123 
time: 1403636959.801666, t: 9.373512 -7.772235 -1.387820 q: 0.185235 0.774855 -0.261478 0.544901 
time: 1403636959.901666, t: 9.402156 -7.775840 -1.396153 q: 0.184666 0.775671 -0.263613 0.542901 
time: 1403636960.001667, t: 9.430616 -7.779916 -1.397078 q: 0.183926 0.775919 -0.264695 0.542271 
time: 1403636960.101666, t: 9.460162 -7.784548 -1.392490 q: 0.184246 0.775635 -0.264760 0.542536 
time: 1403636960.301666, t: 9.517442 -7.789539 -1.379809 q: 0.195509 0.778509 -0.256999 0.538192 
time: 1403636960.401666, t: 9.549834 -7.789832 -1.381484 q: 0.200372 0.777851 -0.253152 0.539178 
time: 1403636960.501667, t: 9.582219 -7.785457 -1.386494 q: 0.201558 0.776202 -0.246901 0.543990 
time: 1403636960.601666, t: 9.620101 -7.777663 -1.397562 q: 0.201800 0.776076 -0.238819 0.547675 
time: 1403636960.701667, t: 9.654328 -7.762961 -1.408877 q: 0.194105 0.775629 -0.236794 0.551953 
time: 1403636960.801666, t: 9.692000 -7.744131 -1.426961 q: 0.179835 0.777587 -0.238706 0.553207 
time: 1403636960.901666, t: 9.727853 -7.721763 -1.446537 q: 0.165332 0.781883 -0.231492 0.554740 
time: 1403636961.001667, t: 9.766156 -7.697730 -1.462859 q: 0.151747 0.787312 -0.214217 0.557875 
time: 1403636961.101666, t: 9.798201 -7.672014 -1.472911 q: 0.135752 0.792416 -0.193324 0.562382 
time: 1403636961.201667, t: 9.829767 -7.645480 -1.477265 q: 0.118449 0.797663 -0.170685 0.566189 
time: 1403636961.301666, t: 9.858825 -7.619005 -1.473195 q: 0.102917 0.802382 -0.148126 0.568902 
time: 1403636961.401666, t: 9.889769 -7.592442 -1.462860 q: 0.091720 0.805067 -0.129245 0.571620 
wait for imu ... 
time: 1403636961.501667, t: 9.916857 -7.566653 -1.445403 q: 0.081889 0.807188 -0.116459 0.572869 
time: 1403636961.601666, t: 9.944662 -7.540488 -1.420898 q: 0.071734 0.808841 -0.103602 0.574367 
time: 1403636961.701667, t: 9.971325 -7.513710 -1.389374 q: 0.060636 0.809919 -0.089331 0.576519 
time: 1403636961.801666, t: 9.997982 -7.486795 -1.354911 q: 0.050304 0.811068 -0.073256 0.578162 
time: 1403636961.901666, t: 10.021339 -7.458790 -1.325351 q: 0.040466 0.812988 -0.055235 0.578241 
time: 1403636962.001667, t: 10.045060 -7.430408 -1.302857 q: 0.027210 0.813975 -0.037208 0.579068 
time: 1403636962.101666, t: 10.067688 -7.401575 -1.285622 q: 0.014023 0.815635 -0.018216 0.578110 
time: 1403636962.201667, t: 10.088938 -7.371456 -1.266768 q: 0.000179 0.815834 0.001371 0.578284 
time: 1403636962.301666, t: 10.110108 -7.340564 -1.241904 q: -0.012446 0.814583 0.023117 0.579453 
time: 1403636962.401666, t: 10.126862 -7.308864 -1.211720 q: -0.027150 0.813303 0.044404 0.579507 
time: 1403636962.501667, t: 10.142604 -7.275977 -1.174617 q: -0.038737 0.811693 0.067872 0.578833 
time: 1403636962.601666, t: 10.156343 -7.241416 -1.134181 q: -0.047776 0.809549 0.094019 0.577502 
time: 1403636962.701667, t: 10.167590 -7.205957 -1.097370 q: -0.053452 0.806467 0.118413 0.576830 
time: 1403636962.801666, t: 10.174566 -7.167948 -1.066928 q: -0.058564 0.803922 0.135148 0.576207 
time: 1403636962.901666, t: 10.178942 -7.124724 -1.043464 q: -0.065385 0.801943 0.142767 0.576394 
time: 1403636963.001667, t: 10.180308 -7.078018 -1.024935 q: -0.076231 0.798518 0.141692 0.580069 
time: 1403636963.101666, t: 10.179106 -7.028292 -1.010591 q: -0.083497 0.793156 0.138601 0.587129 
time: 1403636963.201667, t: 10.174616 -6.977852 -1.001939 q: -0.085362 0.792944 0.135694 0.587827 
time: 1403636963.301666, t: 10.164163 -6.927872 -0.999550 q: -0.089099 0.797577 0.129926 0.582281 
time: 1403636963.401666, t: 10.149849 -6.878574 -1.001330 q: -0.091479 0.801037 0.123650 0.578517 
time: 1403636963.501667, t: 10.134922 -6.829494 -1.004677 q: -0.087306 0.802598 0.121687 0.577414 
time: 1403636963.601666, t: 10.114478 -6.783887 -1.004372 q: -0.083132 0.803097 0.119914 0.577707 
time: 1403636963.701667, t: 10.095421 -6.736787 -0.997755 q: -0.078677 0.803836 0.117754 0.577746 
wait for imu ... 
time: 1403636963.801666, t: 10.075870 -6.690048 -0.982509 q: -0.075889 0.804524 0.115278 0.577661 
time: 1403636963.901666, t: 10.053175 -6.643378 -0.961369 q: -0.073512 0.804092 0.113538 0.578914 
time: 1403636964.001667, t: 10.027947 -6.596037 -0.933358 q: -0.072607 0.803899 0.112194 0.579557 
time: 1403636964.101666, t: 10.002096 -6.549220 -0.899918 q: -0.071736 0.803588 0.112169 0.580102 
time: 1403636964.201667, t: 9.971433 -6.502379 -0.867303 q: -0.071310 0.802067 0.112808 0.582132 
time: 1403636964.301666, t: 9.939202 -6.455276 -0.841726 q: -0.069964 0.802180 0.114705 0.581769 
time: 1403636964.401666, t: 9.903003 -6.407212 -0.821469 q: -0.069652 0.801545 0.116325 0.582360 
time: 1403636964.501667, t: 9.865742 -6.357330 -0.806935 q: -0.068436 0.800871 0.119710 0.582744 
time: 1403636964.601666, t: 9.824900 -6.306433 -0.796828 q: -0.066113 0.799085 0.128968 0.583489 
time: 1403636964.701667, t: 9.780196 -6.250076 -0.793510 q: -0.068817 0.797350 0.141427 0.582663 
time: 1403636964.801666, t: 9.731626 -6.192022 -0.795067 q: -0.073381 0.793950 0.158160 0.582447 
time: 1403636964.901666, t: 9.677141 -6.127819 -0.795674 q: -0.090384 0.789164 0.172877 0.582379 
time: 1403636965.001667, t: 9.618249 -6.061226 -0.789794 q: -0.106746 0.782685 0.191880 0.582402 
time: 1403636965.101666, t: 9.556866 -5.992854 -0.776826 q: -0.122934 0.779414 0.209035 0.577672 
time: 1403636965.201667, t: 9.489994 -5.920682 -0.757318 q: -0.134155 0.780240 0.223025 0.568760 
time: 1403636965.301666, t: 9.421045 -5.848251 -0.732776 q: -0.138623 0.787363 0.234825 0.552902 
time: 1403636965.401666, t: 9.350612 -5.771700 -0.703722 q: -0.138378 0.798381 0.243718 0.532954 
time: 1403636965.501667, t: 9.285022 -5.691437 -0.666214 q: -0.142529 0.806371 0.244662 0.519221 
time: 1403636965.601666, t: 9.228414 -5.605150 -0.615360 q: -0.154174 0.802253 0.237313 0.525645 
time: 1403636965.701667, t: 9.180046 -5.515290 -0.556013 q: -0.169081 0.792244 0.226078 0.540972 
time: 1403636965.801666, t: 9.135242 -5.427033 -0.501534 q: -0.175017 0.785317 0.219626 0.551735 
time: 1403636965.901666, t: 9.087228 -5.342681 -0.455421 q: -0.166361 0.780743 0.222776 0.559585 
time: 1403636966.001667, t: 9.034511 -5.259543 -0.417393 q: -0.157642 0.778482 0.226217 0.563862 
time: 1403636966.101666, t: 8.981398 -5.179159 -0.387006 q: -0.152898 0.777044 0.227300 0.566710 
time: 1403636966.201667, t: 8.931355 -5.101455 -0.364462 q: -0.150155 0.776676 0.227010 0.568062 
time: 1403636966.301666, t: 8.876034 -5.027195 -0.350662 q: -0.149497 0.775908 0.225602 0.569843 
time: 1403636966.401666, t: 8.820862 -4.951512 -0.342902 q: -0.159118 0.777110 0.217154 0.568881 
time: 1403636966.501667, t: 8.761615 -4.883100 -0.343998 q: -0.174662 0.779305 0.204313 0.566069 
time: 1403636966.601666, t: 8.702951 -4.820630 -0.348716 q: -0.178341 0.779868 0.199325 0.565925 
time: 1403636966.701667, t: 8.646218 -4.764881 -0.359436 q: -0.167194 0.779656 0.204102 0.567913 
time: 1403636966.801666, t: 8.588020 -4.711652 -0.366482 q: -0.159152 0.778324 0.207272 0.570895 
time: 1403636966.901666, t: 8.528260 -4.662517 -0.366854 q: -0.159810 0.778851 0.205008 0.570809 
time: 1403636967.001667, t: 8.467283 -4.618155 -0.361767 q: -0.162033 0.779416 0.202142 0.570433 
time: 1403636967.101666, t: 8.403782 -4.575912 -0.358586 q: -0.156895 0.778967 0.204359 0.571692 
time: 1403636967.201667, t: 8.340525 -4.537134 -0.362997 q: -0.152333 0.778668 0.206327 0.572626 
time: 1403636967.301666, t: 8.274023 -4.500649 -0.371582 q: -0.150321 0.778758 0.206625 0.572927 
time: 1403636967.401666, t: 8.205601 -4.465687 -0.379568 q: -0.147651 0.777792 0.207571 0.574589 
time: 1403636967.501667, t: 8.134622 -4.430825 -0.381210 q: -0.147859 0.777634 0.206458 0.575151 
time: 1403636967.601666, t: 8.058177 -4.398804 -0.374130 q: -0.145664 0.777183 0.206632 0.576256 
time: 1403636967.701667, t: 7.978146 -4.364534 -0.370023 q: -0.143272 0.777500 0.206791 0.576372 
time: 1403636967.801666, t: 7.896896 -4.333527 -0.379925 q: -0.143387 0.777349 0.206098 0.576795 
time: 1403636967.901666, t: 7.814033 -4.303414 -0.405937 q: -0.143279 0.776711 0.205986 0.577721 
time: 1403636968.001667, t: 7.726780 -4.275831 -0.439068 q: -0.143654 0.776378 0.205957 0.578086 
time: 1403636968.201667, t: 7.544906 -4.223878 -0.513967 q: -0.148661 0.776755 0.203611 0.577143 
time: 1403636968.301666, t: 7.449393 -4.200823 -0.542917 q: -0.148321 0.777623 0.203905 0.575957 
time: 1403636968.401666, t: 7.351419 -4.180268 -0.564393 q: -0.147791 0.778673 0.204178 0.574576 
time: 1403636968.501667, t: 7.247382 -4.161361 -0.577952 q: -0.148244 0.779645 0.203451 0.573398 
time: 1403636968.601666, t: 7.143233 -4.142587 -0.586061 q: -0.146390 0.779273 0.204474 0.574015 
time: 1403636968.701667, t: 7.036077 -4.125107 -0.585952 q: -0.140327 0.778592 0.208281 0.575084 
time: 1403636968.801666, t: 6.931758 -4.104789 -0.589732 q: -0.136037 0.778245 0.210915 0.575625 
time: 1403636968.901666, t: 6.820847 -4.086072 -0.592650 q: -0.138516 0.778896 0.208630 0.574985 
time: 1403636969.001667, t: 6.710216 -4.061670 -0.590136 q: -0.140580 0.779715 0.206971 0.573973 
time: 1403636969.101666, t: 6.592525 -4.041985 -0.579484 q: -0.145208 0.780816 0.203749 0.572475 
time: 1403636969.201667, t: 6.472965 -4.020995 -0.567762 q: -0.145132 0.785723 0.203574 0.565804 
time: 1403636969.301666, t: 6.351343 -4.002603 -0.549399 q: -0.144285 0.792403 0.203653 0.556601 
time: 1403636969.401666, t: 6.237798 -3.985861 -0.522413 q: -0.141019 0.796579 0.205153 0.550897 
time: 1403636969.501667, t: 6.129615 -3.968140 -0.493505 q: -0.138539 0.799135 0.205802 0.547572 
time: 1403636969.601666, t: 6.022076 -3.947318 -0.469895 q: -0.137106 0.800847 0.205436 0.545565 
time: 1403636969.701667, t: 5.916523 -3.924991 -0.453519 q: -0.136720 0.800820 0.204669 0.545990 
time: 1403636969.801666, t: 5.814530 -3.900651 -0.439974 q: -0.135807 0.800929 0.204607 0.546081 
time: 1403636969.901666, t: 5.716503 -3.874357 -0.433357 q: -0.136201 0.801313 0.203804 0.545721 
time: 1403636970.001667, t: 5.623550 -3.848167 -0.430216 q: -0.138598 0.798841 0.202461 0.549229 
time: 1403636970.101666, t: 5.532686 -3.818771 -0.434046 q: -0.140152 0.792578 0.202233 0.557924 
time: 1403636970.201667, t: 5.443468 -3.790262 -0.440702 q: -0.141594 0.787898 0.201670 0.564355 
time: 1403636970.301666, t: 5.361050 -3.761417 -0.448077 q: -0.142242 0.786470 0.201925 0.566091 
time: 1403636970.401666, t: 5.277496 -3.732794 -0.456171 q: -0.141079 0.790940 0.202971 0.559744 
time: 1403636970.501667, t: 5.190243 -3.703246 -0.462877 q: -0.140442 0.796610 0.204026 0.551418 
time: 1403636970.601666, t: 5.107715 -3.672083 -0.466611 q: -0.138989 0.801105 0.205897 0.544535 
time: 1403636970.701667, t: 5.026788 -3.638018 -0.462047 q: -0.137218 0.804709 0.208277 0.538735 
time: 1403636970.801666, t: 4.949290 -3.601481 -0.450274 q: -0.136446 0.808339 0.209908 0.532831 
time: 1403636970.901666, t: 4.882277 -3.561646 -0.439378 q: -0.135005 0.813254 0.211769 0.524924 
time: 1403636971.001667, t: 4.819199 -3.517827 -0.434448 q: -0.138804 0.815427 0.209667 0.521395 
time: 1403636971.101666, t: 4.766047 -3.470508 -0.434433 q: -0.149016 0.813281 0.202627 0.524700 
time: 1403636971.201667, t: 4.719513 -3.422142 -0.439259 q: -0.161854 0.808819 0.193894 0.531055 
time: 1403636971.301666, t: 4.678202 -3.376464 -0.449166 q: -0.174300 0.804364 0.184906 0.537054 
time: 1403636971.401666, t: 4.643988 -3.336732 -0.463960 q: -0.182502 0.802294 0.179491 0.539259 
time: 1403636971.501667, t: 4.617830 -3.303918 -0.483668 q: -0.179744 0.801248 0.181913 0.540926 
time: 1403636971.601666, t: 4.592509 -3.275278 -0.506341 q: -0.172653 0.797622 0.187615 0.546618 
time: 1403636971.701667, t: 4.575819 -3.253286 -0.533742 q: -0.163107 0.791480 0.195274 0.555719 
time: 1403636971.801666, t: 4.558620 -3.233424 -0.564971 q: -0.161243 0.786499 0.197577 0.562479 
time: 1403636971.901666, t: 4.541581 -3.217118 -0.596163 q: -0.161852 0.784175 0.198304 0.565287 
time: 1403636972.001667, t: 4.524941 -3.205818 -0.622632 q: -0.161883 0.784178 0.199295 0.564925 
time: 1403636972.101666, t: 4.507058 -3.196996 -0.639919 q: -0.160452 0.783988 0.200362 0.565219 
time: 1403636972.201667, t: 4.487076 -3.192411 -0.653805 q: -0.161751 0.787563 0.199260 0.560248 
time: 1403636972.301666, t: 4.466224 -3.190955 -0.657411 q: -0.163185 0.793276 0.197892 0.552199 
time: 1403636972.401666, t: 4.451719 -3.190193 -0.655510 q: -0.160148 0.795429 0.199525 0.549395 
time: 1403636972.501667, t: 4.439786 -3.194734 -0.637307 q: -0.156917 0.794269 0.201328 0.551345 
time: 1403636972.601666, t: 4.431477 -3.198041 -0.619317 q: -0.154574 0.792431 0.202222 0.554316 
time: 1403636972.701667, t: 4.423126 -3.202804 -0.599733 q: -0.151521 0.790212 0.203546 0.557832 
time: 1403636972.801666, t: 4.413136 -3.208534 -0.584834 q: -0.150811 0.790161 0.203453 0.558129 
time: 1403636972.901666, t: 4.401819 -3.215026 -0.574322 q: -0.149466 0.790597 0.202981 0.558045 
time: 1403636973.001667, t: 4.392257 -3.221307 -0.573834 q: -0.144131 0.794081 0.197647 0.556414 
time: 1403636973.101666, t: 4.382572 -3.228834 -0.578671 q: -0.137575 0.798638 0.187842 0.554947 
time: 1403636973.201667, t: 4.372722 -3.237226 -0.586272 q: -0.131818 0.801502 0.179521 0.554969 
time: 1403636973.301666, t: 4.364454 -3.244072 -0.599878 q: -0.126298 0.801977 0.173676 0.557421 
time: 1403636973.401666, t: 4.357587 -3.249857 -0.618370 q: -0.122098 0.802626 0.169425 0.558729 
time: 1403636973.501667, t: 4.352310 -3.253735 -0.641487 q: -0.119797 0.803410 0.165878 0.559166 
time: 1403636973.601666, t: 4.347645 -3.256891 -0.667095 q: -0.117502 0.801826 0.163929 0.562491 
time: 1403636973.701667, t: 4.344785 -3.258364 -0.694556 q: -0.116406 0.800171 0.162833 0.565387 
time: 1403636973.801666, t: 4.340305 -3.259712 -0.715470 q: -0.115209 0.797149 0.163160 0.569789 
time: 1403636973.901666, t: 4.334648 -3.260266 -0.729995 q: -0.115256 0.796029 0.163541 0.571234 
time: 1403636974.001667, t: 4.328922 -3.260210 -0.737032 q: -0.117339 0.797777 0.161196 0.569033 
time: 1403636974.101666, t: 4.320569 -3.261571 -0.740480 q: -0.116152 0.802178 0.153357 0.565244 
time: 1403636974.201667, t: 4.313339 -3.265131 -0.744759 q: -0.106945 0.807532 0.139865 0.562932 
time: 1403636974.301666, t: 4.309500 -3.270281 -0.753131 q: -0.090329 0.811008 0.122526 0.564885 
time: 1403636974.401666, t: 4.307940 -3.274181 -0.768908 q: -0.072559 0.813228 0.100686 0.568558 
time: 1403636974.501667, t: 4.304934 -3.278268 -0.788556 q: -0.054598 0.813577 0.075562 0.573935 
time: 1403636974.601666, t: 4.300160 -3.283296 -0.812026 q: -0.035512 0.813412 0.049736 0.578469 
time: 1403636974.701667, t: 4.293825 -3.287174 -0.836578 q: -0.018108 0.815748 0.023812 0.577633 
time: 1403636974.801666, t: 4.283932 -3.290918 -0.855038 q: -0.004438 0.819363 -0.001829 0.573256 
time: 1403636974.901666, t: 4.273811 -3.293961 -0.865103 q: 0.003237 0.823439 -0.028517 0.566679 
time: 1403636975.001667, t: 4.265895 -3.297374 -0.867488 q: 0.011615 0.825837 -0.052299 0.561358 
time: 1403636975.101666, t: 4.261815 -3.304199 -0.862410 q: 0.024206 0.824064 -0.070104 0.561621 
time: 1403636975.201667, t: 4.263143 -3.312309 -0.856568 q: 0.038115 0.818922 -0.079897 0.567037 
time: 1403636975.301666, t: 4.267993 -3.320721 -0.856274 q: 0.049521 0.813417 -0.084306 0.573405 
time: 1403636975.401666, t: 4.273548 -3.328227 -0.862485 q: 0.056125 0.810377 -0.086256 0.576800 
time: 1403636975.501667, t: 4.278870 -3.334449 -0.875174 q: 0.058485 0.811468 -0.087151 0.574895 
time: 1403636975.601666, t: 4.281553 -3.340668 -0.886197 q: 0.059427 0.815501 -0.086268 0.569196 
time: 1403636975.701667, t: 4.289949 -3.344044 -0.889892 q: 0.058549 0.818699 -0.084158 0.564997 
wait for imu ... 
time: 1403636975.801666, t: 4.296242 -3.347862 -0.884233 q: 0.057109 0.820973 -0.082122 0.562136 
time: 1403636975.901666, t: 4.310526 -3.350632 -0.874485 q: 0.060657 0.822190 -0.085973 0.559404 
time: 1403636976.001667, t: 4.322411 -3.354820 -0.867838 q: 0.064852 0.819800 -0.098638 0.560351 
time: 1403636976.101666, t: 4.335389 -3.360115 -0.865735 q: 0.067835 0.812157 -0.119220 0.567085 
time: 1403636976.201667, t: 4.348712 -3.367158 -0.868275 q: 0.069496 0.803326 -0.145198 0.573372 
time: 1403636976.301666, t: 4.358950 -3.378019 -0.876279 q: 0.076658 0.795978 -0.171420 0.575463 
time: 1403636976.401666, t: 4.366016 -3.394558 -0.888792 q: 0.089953 0.789579 -0.196108 0.574470 
time: 1403636976.501667, t: 4.370709 -3.416895 -0.906085 q: 0.110261 0.785326 -0.215396 0.569834 
time: 1403636976.601666, t: 4.370753 -3.442477 -0.928054 q: 0.133222 0.783623 -0.225212 0.563442 
time: 1403636976.701667, t: 4.371205 -3.467888 -0.955948 q: 0.149828 0.782194 -0.231604 0.558644 
time: 1403636976.901666, t: 4.368697 -3.516720 -1.026126 q: 0.156530 0.783927 -0.246520 0.547891 
time: 1403636977.001667, t: 4.367795 -3.541925 -1.054212 q: 0.154099 0.784836 -0.251542 0.544988 
time: 1403636977.101666, t: 4.368133 -3.568936 -1.072142 q: 0.151914 0.785959 -0.253667 0.542995 
time: 1403636977.201667, t: 4.369695 -3.599312 -1.081604 q: 0.148944 0.787210 -0.253933 0.541881 
time: 1403636977.301666, t: 4.371453 -3.632267 -1.085616 q: 0.147311 0.787844 -0.251977 0.542318 
time: 1403636977.401666, t: 4.376330 -3.669804 -1.091295 q: 0.147983 0.785773 -0.247259 0.547289 
time: 1403636977.501667, t: 4.381124 -3.709228 -1.101348 q: 0.150587 0.782741 -0.241197 0.553591 
time: 1403636977.601666, t: 4.385127 -3.749463 -1.117292 q: 0.151114 0.779860 -0.237719 0.558992 
time: 1403636977.701667, t: 4.386138 -3.789436 -1.138544 q: 0.151717 0.778881 -0.237042 0.560480 
time: 1403636977.801666, t: 4.387707 -3.829932 -1.164692 q: 0.147792 0.779062 -0.244416 0.558105 
time: 1403636977.901666, t: 4.386887 -3.871851 -1.193678 q: 0.145564 0.778999 -0.250395 0.556123 
time: 1403636978.001667, t: 4.386369 -3.915207 -1.217193 q: 0.150237 0.777080 -0.251020 0.557283 
time: 1403636978.101666, t: 4.381991 -3.959661 -1.232371 q: 0.157420 0.776946 -0.249189 0.556308 
time: 1403636978.201667, t: 4.376647 -4.005994 -1.240564 q: 0.160974 0.778395 -0.248796 0.553434 
time: 1403636978.301666, t: 4.368329 -4.051255 -1.241702 q: 0.163450 0.780818 -0.248615 0.549361 
time: 1403636978.401666, t: 4.362482 -4.099845 -1.238242 q: 0.164225 0.783929 -0.249422 0.544310 
time: 1403636978.501667, t: 4.355420 -4.145130 -1.227167 q: 0.164255 0.789358 -0.250681 0.535811 
time: 1403636978.601666, t: 4.353680 -4.193849 -1.222457 q: 0.162968 0.794576 -0.252989 0.527339 
time: 1403636978.701667, t: 4.356739 -4.244806 -1.222888 q: 0.162656 0.797833 -0.254225 0.521895 
time: 1403636978.801666, t: 4.366393 -4.295706 -1.228041 q: 0.162618 0.799847 -0.255088 0.518392 
time: 1403636978.901666, t: 4.381558 -4.346974 -1.233630 q: 0.163841 0.796615 -0.254406 0.523296 
time: 1403636979.001667, t: 4.404978 -4.402029 -1.245650 q: 0.168581 0.788842 -0.251271 0.534950 
time: 1403636979.101666, t: 4.427874 -4.452750 -1.260702 q: 0.172461 0.781026 -0.248479 0.546363 
time: 1403636979.201667, t: 4.452798 -4.504846 -1.285900 q: 0.173660 0.778108 -0.247433 0.550606 
time: 1403636979.301666, t: 4.477364 -4.550412 -1.314693 q: 0.171666 0.783317 -0.248696 0.543227 
time: 1403636979.401666, t: 4.503140 -4.596113 -1.336639 q: 0.169899 0.789403 -0.249675 0.534452 
time: 1403636979.501667, t: 4.533848 -4.642715 -1.351356 q: 0.166800 0.793421 -0.251753 0.528471 
time: 1403636979.601666, t: 4.569158 -4.691050 -1.359344 q: 0.166096 0.795355 -0.252873 0.525240 
time: 1403636979.701667, t: 4.608575 -4.737798 -1.357307 q: 0.168233 0.796493 -0.253038 0.522751 
time: 1403636979.801666, t: 4.654718 -4.783945 -1.351009 q: 0.169936 0.795114 -0.253933 0.523863 
time: 1403636979.901666, t: 4.704144 -4.829474 -1.339243 q: 0.173517 0.789693 -0.254048 0.530789 
time: 1403636980.001667, t: 4.755920 -4.872834 -1.330535 q: 0.177768 0.785729 -0.253792 0.535368 
time: 1403636980.101666, t: 4.808278 -4.917141 -1.327700 q: 0.180084 0.783678 -0.254360 0.537326 
time: 1403636980.201667, t: 4.862097 -4.959882 -1.329952 q: 0.180695 0.782098 -0.256000 0.538642 
time: 1403636980.301666, t: 4.919609 -5.004427 -1.338747 q: 0.180044 0.779362 -0.257955 0.541883 
time: 1403636980.401666, t: 4.976430 -5.045517 -1.350249 q: 0.179595 0.774810 -0.258833 0.548107 
time: 1403636980.501667, t: 5.034544 -5.085690 -1.368834 q: 0.178244 0.771098 -0.259795 0.553302 
time: 1403636980.601666, t: 5.092653 -5.125040 -1.388824 q: 0.174094 0.768675 -0.261608 0.557128 
time: 1403636980.701667, t: 5.145978 -5.163553 -1.407639 q: 0.172921 0.767899 -0.260890 0.558896 
time: 1403636980.801666, t: 5.197341 -5.200878 -1.418315 q: 0.175358 0.771600 -0.257803 0.554456 
time: 1403636980.901666, t: 5.245471 -5.234340 -1.423104 q: 0.176776 0.777349 -0.255035 0.547207 
time: 1403636981.001667, t: 5.293994 -5.267208 -1.422850 q: 0.178706 0.780712 -0.252300 0.543045 
time: 1403636981.101666, t: 5.341200 -5.296086 -1.424656 q: 0.178118 0.779872 -0.252379 0.544407 
time: 1403636981.201667, t: 5.392452 -5.325110 -1.432418 q: 0.172660 0.777042 -0.256896 0.548086 
time: 1403636981.301666, t: 5.440724 -5.351590 -1.443614 q: 0.163027 0.771391 -0.265318 0.554964 
time: 1403636981.401666, t: 5.482315 -5.375165 -1.459184 q: 0.157317 0.767609 -0.271213 0.558991 
time: 1403636981.501667, t: 5.520570 -5.402287 -1.477898 q: 0.164417 0.769856 -0.268920 0.554951 
time: 1403636981.601666, t: 5.560747 -5.434813 -1.494290 q: 0.176321 0.775314 -0.263327 0.546313 
time: 1403636981.701667, t: 5.598267 -5.464672 -1.502882 q: 0.184267 0.779291 -0.260653 0.539269 
time: 1403636981.801666, t: 5.630319 -5.488203 -1.503025 q: 0.188017 0.782147 -0.261266 0.533513 
time: 1403636981.901666, t: 5.675274 -5.516622 -1.495960 q: 0.188875 0.786279 -0.263486 0.525991 
time: 1403636982.001667, t: 5.714970 -5.538773 -1.484572 q: 0.185784 0.791021 -0.268249 0.517505 
time: 1403636982.101666, t: 5.768578 -5.569284 -1.479527 q: 0.181129 0.795253 -0.273095 0.510083 
time: 1403636982.201667, t: 5.820753 -5.597266 -1.477159 q: 0.176505 0.796300 -0.277383 0.507752 
time: 1403636982.301666, t: 5.885923 -5.633155 -1.478693 q: 0.170611 0.793384 -0.281621 0.511981 
time: 1403636982.401666, t: 5.951173 -5.671940 -1.483136 q: 0.168973 0.786716 -0.282475 0.522239 
time: 1403636982.501667, t: 6.020883 -5.716730 -1.483795 q: 0.169820 0.777923 -0.280418 0.536062 
time: 1403636982.601666, t: 6.093542 -5.767070 -1.479799 q: 0.172687 0.773691 -0.276776 0.543118 
time: 1403636982.701667, t: 6.166872 -5.820516 -1.471376 q: 0.170094 0.774344 -0.276498 0.543146 
time: 1403636982.801666, t: 6.239388 -5.875654 -1.463227 q: 0.169927 0.778440 -0.274291 0.538442 
time: 1403636982.901666, t: 6.307076 -5.930852 -1.457565 q: 0.171634 0.781897 -0.272245 0.533911 
time: 1403636983.001667, t: 6.379404 -5.989711 -1.457939 q: 0.174356 0.781574 -0.269787 0.534749 
time: 1403636983.101666, t: 6.449174 -6.049317 -1.463811 q: 0.177194 0.777981 -0.267286 0.540283 
time: 1403636983.201667, t: 6.522888 -6.110174 -1.473523 q: 0.181822 0.776249 -0.264120 0.542789 
time: 1403636983.301666, t: 6.594424 -6.169922 -1.488639 q: 0.185800 0.775044 -0.261820 0.544276 
time: 1403636983.401666, t: 6.662875 -6.228056 -1.509267 q: 0.188225 0.774217 -0.260770 0.545122 
time: 1403636983.601666, t: 6.801692 -6.338170 -1.540885 q: 0.189145 0.776114 -0.262884 0.541077 
time: 1403636983.701667, t: 6.873715 -6.388962 -1.545783 q: 0.188883 0.779616 -0.265071 0.535033 
time: 1403636983.801666, t: 6.942366 -6.435665 -1.543411 q: 0.190066 0.782397 -0.266102 0.530018 
time: 1403636983.901666, t: 7.014948 -6.480772 -1.533061 q: 0.190183 0.784593 -0.267811 0.525853 
time: 1403636984.001667, t: 7.088312 -6.526867 -1.519411 q: 0.189677 0.785612 -0.270245 0.523262 
time: 1403636984.101666, t: 7.165645 -6.571142 -1.499497 q: 0.189724 0.786294 -0.272310 0.521147 
time: 1403636984.201667, t: 7.246397 -6.617844 -1.485399 q: 0.192122 0.783689 -0.272385 0.524145 
time: 1403636984.301666, t: 7.332241 -6.664430 -1.476874 q: 0.195125 0.779484 -0.272019 0.529468 
time: 1403636984.401666, t: 7.423607 -6.712899 -1.477794 q: 0.196506 0.776571 -0.272591 0.532933 
time: 1403636984.501667, t: 7.513318 -6.757641 -1.485255 q: 0.198721 0.774143 -0.272245 0.535813 
time: 1403636984.601666, t: 7.605311 -6.801220 -1.496795 q: 0.199467 0.773314 -0.271945 0.536883 
time: 1403636984.701667, t: 7.694222 -6.840538 -1.504151 q: 0.199838 0.773072 -0.271138 0.537503 
time: 1403636984.801666, t: 7.783708 -6.877463 -1.506295 q: 0.196867 0.771810 -0.272325 0.539808 
time: 1403636984.901666, t: 7.878489 -6.916950 -1.505661 q: 0.191460 0.770925 -0.275501 0.541403 
time: 1403636985.001667, t: 7.965486 -6.949598 -1.497197 q: 0.189953 0.770277 -0.276423 0.542386 
time: 1403636985.101666, t: 8.055525 -6.985861 -1.487662 q: 0.191120 0.770876 -0.275873 0.541403 
time: 1403636985.201667, t: 8.139580 -7.014452 -1.485949 q: 0.193863 0.772059 -0.274563 0.539405 
time: 1403636985.301666, t: 8.232691 -7.048104 -1.495681 q: 0.196174 0.771970 -0.273658 0.539155 
time: 1403636985.401666, t: 8.316561 -7.073828 -1.511114 q: 0.195745 0.773423 -0.270023 0.539063 
time: 1403636985.501667, t: 8.410909 -7.101679 -1.519822 q: 0.193236 0.777419 -0.260632 0.538841 
time: 1403636985.601666, t: 8.497694 -7.122727 -1.522135 q: 0.190561 0.779633 -0.251342 0.541005 
time: 1403636985.701667, t: 8.594183 -7.144651 -1.514689 q: 0.190687 0.781023 -0.242606 0.542940 
time: 1403636985.801666, t: 8.689450 -7.161616 -1.502639 q: 0.190208 0.780108 -0.236036 0.547302 
optimize pose graph 
time: 1403636985.901666, t: 8.784625 -7.173590 -1.488553 q: 0.191681 0.770815 -0.229357 0.562582 
time: 1403636986.001667, t: 8.875626 -7.178511 -1.481045 q: 0.194180 0.759971 -0.223334 0.578671 
time: 1403636986.101666, t: 8.955351 -7.174028 -1.483599 q: 0.194489 0.759541 -0.221126 0.579978 
time: 1403636986.201667, t: 9.033130 -7.162281 -1.495860 q: 0.189789 0.767726 -0.224070 0.569535 
time: 1403636986.301666, t: 9.103960 -7.143133 -1.511542 q: 0.185873 0.777170 -0.227066 0.556686 
time: 1403636986.401666, t: 9.177116 -7.119629 -1.529846 q: 0.181781 0.781946 -0.229919 0.550139 
time: 1403636986.501667, t: 9.247823 -7.090850 -1.547200 q: 0.174310 0.782961 -0.234966 0.548980 
time: 1403636986.601666, t: 9.320023 -7.059621 -1.557154 q: 0.167469 0.782061 -0.239256 0.550537 
time: 1403636986.701667, t: 9.389396 -7.027904 -1.558411 q: 0.162470 0.781467 -0.242494 0.551462 
time: 1403636986.801666, t: 9.457441 -6.998212 -1.549542 q: 0.156951 0.780400 -0.246010 0.553011 
time: 1403636986.901666, t: 9.522795 -6.970216 -1.531389 q: 0.155938 0.779184 -0.247102 0.554523 
time: 1403636987.001667, t: 9.584360 -6.944528 -1.504638 q: 0.158643 0.778639 -0.245874 0.555067 
time: 1403636987.101666, t: 9.640909 -6.918845 -1.471708 q: 0.164321 0.778299 -0.243604 0.554894 
time: 1403636987.201667, t: 9.694514 -6.892822 -1.442157 q: 0.167295 0.777205 -0.243447 0.555606 
time: 1403636987.301666, t: 9.747394 -6.867353 -1.420477 q: 0.168710 0.775480 -0.244399 0.557169 
time: 1403636987.401666, t: 9.798608 -6.839823 -1.404249 q: 0.168497 0.774197 -0.243757 0.559294 
time: 1403636987.501667, t: 9.846337 -6.812370 -1.393114 q: 0.169653 0.772385 -0.238252 0.563804 
time: 1403636987.601666, t: 9.892344 -6.781327 -1.385156 q: 0.172338 0.770233 -0.232355 0.568377 
time: 1403636987.701667, t: 9.934017 -6.746330 -1.382124 q: 0.172393 0.769430 -0.228998 0.570805 
time: 1403636987.801666, t: 9.971582 -6.708095 -1.376125 q: 0.169684 0.769629 -0.227348 0.572007 
time: 1403636987.901666, t: 10.003689 -6.667203 -1.363397 q: 0.164536 0.770468 -0.228247 0.572023 
optimize pose graph 
time: 1403636988.001667, t: 10.031785 -6.624265 -1.345443 q: 0.155454 0.772651 -0.232588 0.569866 
time: 1403636988.101666, t: 10.056680 -6.580445 -1.330115 q: 0.146523 0.775178 -0.238506 0.566343 
time: 1403636988.201667, t: 10.076039 -6.538828 -1.319916 q: 0.140710 0.776023 -0.243066 0.564719 
time: 1403636988.301666, t: 10.089985 -6.501813 -1.312821 q: 0.137810 0.772266 -0.245948 0.569318 
time: 1403636988.401666, t: 10.098027 -6.469630 -1.303546 q: 0.138835 0.765767 -0.245931 0.577792 
time: 1403636988.501667, t: 10.098091 -6.438298 -1.286352 q: 0.142235 0.761522 -0.243647 0.583515 
time: 1403636988.601666, t: 10.089983 -6.407627 -1.262681 q: 0.142851 0.759229 -0.243031 0.586601 
time: 1403636988.701667, t: 10.073265 -6.377171 -1.239100 q: 0.142641 0.758164 -0.243236 0.587943 
time: 1403636988.801666, t: 10.047042 -6.348476 -1.220380 q: 0.143965 0.756467 -0.242198 0.590230 
time: 1403636988.901666, t: 10.013934 -6.319695 -1.207917 q: 0.143569 0.753890 -0.242429 0.593520 
time: 1403636989.001667, t: 9.968609 -6.293166 -1.200680 q: 0.142849 0.751253 -0.243193 0.596716 
time: 1403636989.101666, t: 9.913692 -6.265706 -1.198222 q: 0.142476 0.749438 -0.244219 0.598665 
time: 1403636989.201667, t: 9.845795 -6.240999 -1.198152 q: 0.142594 0.749412 -0.245101 0.598309 
time: 1403636989.301666, t: 9.767863 -6.216863 -1.195731 q: 0.148477 0.755687 -0.242948 0.589804 
time: 1403636989.401666, t: 9.683377 -6.192242 -1.186335 q: 0.155166 0.765557 -0.240422 0.576231 
time: 1403636989.501667, t: 9.593733 -6.170251 -1.170095 q: 0.156981 0.773257 -0.240565 0.565296 
time: 1403636989.601666, t: 9.501414 -6.148512 -1.148349 q: 0.159932 0.777041 -0.239470 0.559717 
time: 1403636989.701667, t: 9.407213 -6.126726 -1.128598 q: 0.164425 0.780092 -0.236627 0.555364 
time: 1403636989.801666, t: 9.313788 -6.103264 -1.115088 q: 0.166276 0.781451 -0.235514 0.553371 
time: 1403636989.901666, t: 9.223691 -6.079891 -1.104675 q: 0.165789 0.779924 -0.235630 0.555618 
time: 1403636990.001667, t: 9.130145 -6.053736 -1.099717 q: 0.165676 0.775122 -0.235438 0.562412 
time: 1403636990.101666, t: 9.039383 -6.028028 -1.091415 q: 0.166808 0.771129 -0.234422 0.567963 
time: 1403636990.201667, t: 8.942168 -5.999219 -1.078192 q: 0.168067 0.770874 -0.233085 0.568488 
time: 1403636990.301666, t: 8.847190 -5.969526 -1.058327 q: 0.168348 0.774364 -0.231802 0.564169 
time: 1403636990.401666, t: 8.745934 -5.935998 -1.033552 q: 0.168265 0.777763 -0.230289 0.560124 
time: 1403636990.501667, t: 8.649188 -5.901599 -1.011553 q: 0.164929 0.780658 -0.230490 0.556997 
time: 1403636990.601666, t: 8.551050 -5.865582 -0.996037 q: 0.163375 0.782389 -0.229640 0.555375 
wait for imu ... 
wait for imu ... 
time: 1403636990.701667, t: 8.456493 -5.829584 -0.986859 q: 0.162559 0.784640 -0.228353 0.552964 
time: 1403636990.801666, t: 8.361352 -5.791904 -0.984839 q: 0.162541 0.785946 -0.226829 0.551740 
time: 1403636991.001667, t: 8.177186 -5.716300 -0.996511 q: 0.162086 0.786389 -0.225493 0.551792 
time: 1403636991.101666, t: 8.088143 -5.678229 -1.007816 q: 0.162923 0.785946 -0.225115 0.552330 
time: 1403636991.201667, t: 8.001341 -5.640798 -1.021081 q: 0.164052 0.786001 -0.225200 0.551882 
time: 1403636991.301666, t: 7.914280 -5.600421 -1.028892 q: 0.163859 0.786359 -0.225976 0.551113 
time: 1403636991.401666, t: 7.829606 -5.559688 -1.028451 q: 0.163883 0.786831 -0.227303 0.549884 
time: 1403636991.501667, t: 7.747501 -5.516588 -1.017342 q: 0.165181 0.787030 -0.228257 0.548815 
time: 1403636991.601666, t: 7.671118 -5.477595 -0.998122 q: 0.163200 0.786201 -0.231667 0.549167 
time: 1403636991.701667, t: 7.591378 -5.435207 -0.973800 q: 0.157217 0.784601 -0.238617 0.550224 
time: 1403636991.801666, t: 7.515473 -5.394840 -0.946064 q: 0.152544 0.782991 -0.245090 0.550987 
time: 1403636991.901666, t: 7.431880 -5.356099 -0.917478 q: 0.149896 0.782115 -0.249996 0.550753 
time: 1403636992.001667, t: 7.353457 -5.321780 -0.897497 q: 0.152594 0.781853 -0.251093 0.549884 
time: 1403636992.101666, t: 7.274826 -5.290949 -0.880363 q: 0.162025 0.780371 -0.247185 0.551062 
time: 1403636992.201667, t: 7.195387 -5.261530 -0.875330 q: 0.168817 0.777381 -0.244443 0.554460 
time: 1403636992.301666, t: 7.121568 -5.234773 -0.870055 q: 0.172325 0.776275 -0.243832 0.555200 
time: 1403636992.401666, t: 7.049627 -5.208970 -0.873450 q: 0.173965 0.778325 -0.244947 0.551315 
time: 1403636992.501667, t: 6.978104 -5.180274 -0.872143 q: 0.174554 0.781179 -0.247021 0.546141 
time: 1403636992.601666, t: 6.908849 -5.149677 -0.866668 q: 0.176493 0.782770 -0.248185 0.542703 
time: 1403636992.701667, t: 6.838636 -5.117923 -0.851638 q: 0.177946 0.783201 -0.249442 0.541027 
time: 1403636992.801666, t: 6.772777 -5.087042 -0.834989 q: 0.179796 0.783812 -0.250300 0.539130 
time: 1403636992.901666, t: 6.706714 -5.053716 -0.826450 q: 0.182881 0.783461 -0.250273 0.538616 
time: 1403636993.001667, t: 6.650360 -5.022475 -0.821967 q: 0.184733 0.781638 -0.250728 0.540417 
time: 1403636993.101666, t: 6.593713 -4.988097 -0.823977 q: 0.182125 0.777705 -0.252672 0.546043 
time: 1403636993.201667, t: 6.540713 -4.953658 -0.821620 q: 0.174877 0.775868 -0.250121 0.552165 
time: 1403636993.301666, t: 6.489196 -4.917460 -0.812719 q: 0.166373 0.777217 -0.241613 0.556666 
time: 1403636993.401666, t: 6.437801 -4.880195 -0.796931 q: 0.156295 0.780069 -0.228491 0.561120 
time: 1403636993.501667, t: 6.385011 -4.841348 -0.773628 q: 0.144459 0.784186 -0.212652 0.564768 
time: 1403636993.601666, t: 6.332066 -4.803622 -0.745026 q: 0.132957 0.788898 -0.195521 0.567216 
time: 1403636993.701667, t: 6.280094 -4.766740 -0.717113 q: 0.122719 0.793808 -0.182239 0.567096 
time: 1403636993.801666, t: 6.227132 -4.730105 -0.693991 q: 0.116168 0.797369 -0.172410 0.566553 
time: 1403636993.901666, t: 6.173901 -4.692737 -0.677857 q: 0.113618 0.799470 -0.163843 0.566651 
time: 1403636994.001667, t: 6.119989 -4.655822 -0.666145 q: 0.111517 0.797482 -0.158544 0.571358 
optimize pose graph 
time: 1403636994.101666, t: 6.064599 -4.617496 -0.659534 q: 0.113515 0.792793 -0.153503 0.578818 
time: 1403636994.201667, t: 6.007228 -4.577334 -0.655738 q: 0.114785 0.787813 -0.150247 0.586175 
time: 1403636994.301666, t: 5.946850 -4.535529 -0.657960 q: 0.115411 0.783251 -0.149030 0.592443 
time: 1403636994.401666, t: 5.884693 -4.491335 -0.663324 q: 0.116636 0.780666 -0.148153 0.595825 
time: 1403636994.501667, t: 5.814558 -4.440682 -0.670519 q: 0.116790 0.781261 -0.148834 0.594844 
time: 1403636994.601666, t: 5.744188 -4.391056 -0.673241 q: 0.112552 0.784467 -0.152356 0.590535 
time: 1403636994.701667, t: 5.665747 -4.335367 -0.669461 q: 0.105670 0.789835 -0.158334 0.583030 
time: 1403636994.801666, t: 5.588914 -4.281521 -0.660402 q: 0.096105 0.795223 -0.166248 0.575106 
time: 1403636994.901666, t: 5.509378 -4.228622 -0.648710 q: 0.088998 0.797118 -0.172294 0.571836 
time: 1403636995.001667, t: 5.428353 -4.179864 -0.640812 q: 0.089024 0.795464 -0.173099 0.573889 
time: 1403636995.101666, t: 5.345921 -4.135029 -0.637535 q: 0.096128 0.792438 -0.167802 0.578484 
time: 1403636995.201667, t: 5.259403 -4.089492 -0.633468 q: 0.102265 0.789801 -0.162570 0.582518 
time: 1403636995.301666, t: 5.173121 -4.047494 -0.622981 q: 0.105596 0.788413 -0.158823 0.584833 
time: 1403636995.401666, t: 5.080492 -4.001644 -0.605330 q: 0.107402 0.790836 -0.155649 0.582080 
time: 1403636995.501667, t: 4.988663 -3.958176 -0.581753 q: 0.106371 0.795534 -0.155073 0.575989 
time: 1403636995.601666, t: 4.890471 -3.909848 -0.551853 q: 0.105477 0.797296 -0.154861 0.573770 
time: 1403636995.701667, t: 4.798054 -3.865514 -0.522914 q: 0.103105 0.797355 -0.156503 0.573673 
time: 1403636995.801666, t: 4.702279 -3.817714 -0.498436 q: 0.098435 0.797369 -0.160190 0.573456 
time: 1403636995.901666, t: 4.608239 -3.774914 -0.483124 q: 0.091015 0.798486 -0.165854 0.571515 
time: 1403636996.001667, t: 4.510471 -3.731801 -0.472094 q: 0.087445 0.799544 -0.169120 0.569633 
time: 1403636996.101666, t: 4.415235 -3.694702 -0.466003 q: 0.092543 0.801450 -0.166053 0.567045 
time: 1403636996.201667, t: 4.318173 -3.658944 -0.459215 q: 0.097911 0.802634 -0.162484 0.565501 
time: 1403636996.301666, t: 4.226050 -3.627327 -0.447068 q: 0.101705 0.803369 -0.159022 0.564771 
time: 1403636996.401666, t: 4.133024 -3.595611 -0.425463 q: 0.099361 0.804462 -0.154366 0.564925 
time: 1403636996.501667, t: 4.045539 -3.564366 -0.395893 q: 0.095005 0.806279 -0.145083 0.565544 
time: 1403636996.601666, t: 3.958494 -3.532599 -0.361270 q: 0.088033 0.809017 -0.132156 0.565929 
time: 1403636996.701667, t: 3.876653 -3.501995 -0.329749 q: 0.077950 0.812313 -0.116011 0.566226 
time: 1403636996.801666, t: 3.791761 -3.468424 -0.313021 q: 0.065335 0.815601 -0.098128 0.566479 
time: 1403636996.901666, t: 3.708716 -3.435052 -0.308740 q: 0.049764 0.818370 -0.079876 0.566934 
time: 1403636997.001667, t: 3.628011 -3.402004 -0.310736 q: 0.034492 0.820605 -0.060246 0.567264 
time: 1403636997.101666, t: 3.548748 -3.369336 -0.317509 q: 0.021036 0.822472 -0.042226 0.566846 
time: 1403636997.201667, t: 3.472700 -3.338648 -0.329016 q: 0.010957 0.823825 -0.029210 0.565985 
time: 1403636997.301666, t: 3.400075 -3.309841 -0.343376 q: 0.005121 0.824551 -0.019330 0.565435 
time: 1403636997.401666, t: 3.329386 -3.282541 -0.352367 q: 0.002275 0.825245 -0.013139 0.564617 
time: 1403636997.501667, t: 3.267347 -3.258049 -0.353795 q: 0.001204 0.824923 -0.010201 0.565152 
time: 1403636997.601666, t: 3.207821 -3.232299 -0.347772 q: 0.001050 0.825675 -0.009960 0.564057 
time: 1403636997.701667, t: 3.153875 -3.208699 -0.333094 q: 0.002763 0.826085 -0.010208 0.563446 
time: 1403636997.801666, t: 3.105568 -3.185237 -0.311316 q: 0.005712 0.827178 -0.011039 0.561802 
time: 1403636997.901666, t: 3.065690 -3.161191 -0.282224 q: 0.007943 0.827437 -0.012664 0.561359 
time: 1403636998.001667, t: 3.029277 -3.139916 -0.245814 q: 0.008965 0.827396 -0.014845 0.561351 
time: 1403636998.101666, t: 3.000385 -3.119665 -0.204387 q: 0.009824 0.827628 -0.017035 0.560933 
time: 1403636998.201667, t: 2.983247 -3.102169 -0.164338 q: 0.008910 0.826395 -0.020172 0.562659 
time: 1403636998.301666, t: 2.961721 -3.082864 -0.131380 q: 0.009958 0.826914 -0.021609 0.561825 
optimize pose graph 
time: 1403636998.401666, t: 2.943025 -3.064285 -0.104704 q: 0.009704 0.826567 -0.023181 0.562277 
time: 1403636998.501667, t: 2.929541 -3.046197 -0.079098 q: 0.009186 0.826329 -0.025118 0.562553 
time: 1403636998.601666, t: 2.917301 -3.029187 -0.049112 q: 0.010761 0.823121 -0.024925 0.567216 
time: 1403636998.701667, t: 2.906897 -3.013506 -0.011758 q: 0.014055 0.814952 -0.023375 0.578887 
time: 1403636998.801666, t: 2.895810 -2.998801 0.030310 q: 0.017148 0.809803 -0.022300 0.586026 
time: 1403636998.901666, t: 2.883752 -2.983149 0.069785 q: 0.022721 0.808261 -0.018898 0.588082 
time: 1403636999.001667, t: 2.867707 -2.964387 0.102303 q: 0.026543 0.808640 -0.017125 0.587456 
time: 1403636999.101666, t: 2.847163 -2.942987 0.128294 q: 0.029421 0.808850 -0.015621 0.587071 
time: 1403636999.201667, t: 2.825716 -2.918263 0.148859 q: 0.031018 0.810534 -0.015294 0.584669 
time: 1403636999.301666, t: 2.805731 -2.890310 0.164124 q: 0.029836 0.812238 -0.016659 0.582325 
time: 1403636999.401666, t: 2.779474 -2.859523 0.173096 q: 0.025108 0.812784 -0.020349 0.581668 
time: 1403636999.501667, t: 2.754434 -2.827504 0.177620 q: 0.022216 0.812430 -0.022631 0.582195 
time: 1403636999.601666, t: 2.729793 -2.794716 0.178271 q: 0.020598 0.810247 -0.024263 0.585224 
time: 1403636999.701667, t: 2.701352 -2.761390 0.175634 q: 0.020077 0.808334 -0.025254 0.587839 
time: 1403636999.801666, t: 2.670932 -2.728120 0.169496 q: 0.020089 0.807926 -0.026119 0.588362 
time: 1403636999.901666, t: 2.637796 -2.695479 0.161211 q: 0.021704 0.808127 -0.025438 0.588059 
time: 1403637000.001667, t: 2.600817 -2.659657 0.155479 q: 0.025888 0.808055 -0.022923 0.588092 
time: 1403637000.101666, t: 2.560447 -2.622075 0.155241 q: 0.028115 0.809061 -0.021162 0.586670 
time: 1403637000.201667, t: 2.517244 -2.582817 0.162075 q: 0.027186 0.810734 -0.021309 0.584394 
time: 1403637000.301666, t: 2.478532 -2.544257 0.172956 q: 0.025907 0.812275 -0.022215 0.582276 
time: 1403637000.401666, t: 2.434662 -2.502510 0.179977 q: 0.023693 0.811683 -0.023631 0.583138 
time: 1403637000.501667, t: 2.393325 -2.459439 0.182818 q: 0.025821 0.811718 -0.022400 0.583048 
time: 1403637000.601666, t: 2.346611 -2.414737 0.184198 q: 0.026508 0.811871 -0.021980 0.582821 
wait for imu ... 
time: 1403637000.701667, t: 2.299475 -2.368019 0.191614 q: 0.024408 0.810986 -0.023604 0.584079 
time: 1403637000.801666, t: 2.256674 -2.321232 0.207812 q: 0.024430 0.810410 -0.024310 0.584848 
time: 1403637000.901666, t: 2.208151 -2.273579 0.223358 q: 0.023847 0.810454 -0.025295 0.584770 
time: 1403637001.001667, t: 2.159638 -2.223774 0.234656 q: 0.024162 0.810141 -0.026209 0.585150 
time: 1403637001.201667, t: 2.051144 -2.127175 0.240478 q: 0.022754 0.809253 -0.028093 0.586346 
time: 1403637001.401666, t: 1.930808 -2.027975 0.262749 q: 0.019556 0.810682 -0.031039 0.584337 
time: 1403637001.501667, t: 1.866444 -1.979188 0.284746 q: 0.019660 0.813702 -0.032172 0.580059 
time: 1403637001.601666, t: 1.804682 -1.932091 0.305099 q: 0.020009 0.815185 -0.033176 0.577903 
time: 1403637001.701667, t: 1.739316 -1.886721 0.323459 q: 0.020374 0.815322 -0.033768 0.577663 
time: 1403637001.801666, t: 1.677764 -1.841680 0.333239 q: 0.019753 0.814406 -0.035121 0.578895 
time: 1403637001.901666, t: 1.615563 -1.798087 0.335788 q: 0.020731 0.815921 -0.035035 0.576728 
time: 1403637002.001667, t: 1.555915 -1.756274 0.334271 q: 0.021283 0.816793 -0.034983 0.575476 
time: 1403637002.101666, t: 1.496109 -1.714335 0.328804 q: 0.021691 0.817401 -0.035155 0.574586 
time: 1403637002.201667, t: 1.440387 -1.674698 0.317395 q: 0.021457 0.817576 -0.035290 0.574338 
time: 1403637002.301666, t: 1.383605 -1.635178 0.304029 q: 0.022801 0.817575 -0.034441 0.574339 
time: 1403637002.401666, t: 1.325815 -1.595610 0.294261 q: 0.024787 0.818072 -0.032734 0.573649 
time: 1403637002.501667, t: 1.271221 -1.556494 0.292412 q: 0.026293 0.817534 -0.031464 0.574419 
time: 1403637002.601666, t: 1.215688 -1.515902 0.300378 q: 0.028903 0.817161 -0.029470 0.574930 
time: 1403637002.701667, t: 1.157216 -1.473434 0.315450 q: 0.030195 0.817471 -0.029404 0.574425 
time: 1403637002.801666, t: 1.101122 -1.428814 0.329093 q: 0.030370 0.817224 -0.030330 0.574720 
time: 1403637002.901666, t: 1.045540 -1.384550 0.336883 q: 0.027264 0.816684 -0.033744 0.575452 
time: 1403637003.001667, t: 0.990673 -1.340056 0.336277 q: 0.027017 0.816937 -0.035332 0.575010 
time: 1403637003.101666, t: 0.938880 -1.294506 0.331227 q: 0.026933 0.817269 -0.037120 0.574429 
time: 1403637003.201667, t: 0.887614 -1.250611 0.321905 q: 0.027705 0.816826 -0.038240 0.574948 
time: 1403637003.301666, t: 0.840043 -1.208564 0.309262 q: 0.027693 0.815732 -0.039786 0.576395 
time: 1403637003.401666, t: 0.787580 -1.163764 0.301978 q: 0.027504 0.814404 -0.040846 0.578205 
time: 1403637003.501667, t: 0.737941 -1.120137 0.303076 q: 0.026955 0.813158 -0.042456 0.579866 
time: 1403637003.601666, t: 0.687360 -1.076998 0.310075 q: 0.026399 0.813404 -0.043684 0.579456 
time: 1403637003.701667, t: 0.635604 -1.033682 0.323470 q: 0.026249 0.813595 -0.044473 0.579135 
time: 1403637003.801666, t: 0.578019 -0.990223 0.332679 q: 0.024669 0.814053 -0.044827 0.578532 
time: 1403637003.901666, t: 0.528171 -0.949532 0.338255 q: 0.026014 0.815887 -0.042639 0.576051 
time: 1403637004.001667, t: 0.477052 -0.909808 0.335824 q: 0.026566 0.818513 -0.039935 0.572482 
time: 1403637004.101666, t: 0.421007 -0.874027 0.326334 q: 0.027127 0.820548 -0.037467 0.569703 
time: 1403637004.201667, t: 0.378261 -0.836380 0.314863 q: 0.027413 0.821751 -0.035659 0.568069 
time: 1403637004.301666, t: 0.329752 -0.798252 0.307836 q: 0.025136 0.821879 -0.035207 0.568018 
time: 1403637004.401666, t: 0.288130 -0.758348 0.310999 q: 0.022810 0.820753 -0.036950 0.569631 
time: 1403637004.501667, t: 0.246563 -0.720685 0.324661 q: 0.022775 0.816444 -0.038379 0.575697 
time: 1403637004.601666, t: 0.204881 -0.682703 0.348503 q: 0.022371 0.814019 -0.041290 0.578937 
optimize pose graph 
time: 1403637004.701667, t: 0.161053 -0.646877 0.376015 q: 0.014111 0.813470 -0.050349 0.579251 
time: 1403637004.801666, t: 0.123774 -0.614610 0.395827 q: 0.007558 0.813030 -0.058263 0.579251 
time: 1403637004.901666, t: 0.083693 -0.584213 0.401135 q: 0.006220 0.815791 -0.062083 0.574971 
time: 1403637005.001667, t: 0.042611 -0.558222 0.391319 q: 0.009825 0.820021 -0.061983 0.568883 
time: 1403637005.101666, t: 0.003569 -0.536315 0.375115 q: 0.018115 0.819960 -0.057985 0.569188 
time: 1403637005.201667, t: -0.031094 -0.519266 0.356652 q: 0.025293 0.818831 -0.054150 0.570915 
time: 1403637005.301666, t: -0.064605 -0.503178 0.334851 q: 0.028951 0.820759 -0.051804 0.568184 
time: 1403637005.401666, t: -0.099014 -0.486875 0.311049 q: 0.029893 0.821751 -0.049994 0.566862 
time: 1403637005.501667, t: -0.133132 -0.470674 0.289038 q: 0.031388 0.818752 -0.046124 0.571431 
time: 1403637005.601666, t: -0.160717 -0.454643 0.271874 q: 0.029360 0.817907 -0.044922 0.572841 
time: 1403637005.701667, t: -0.192633 -0.438877 0.265782 q: 0.022726 0.822066 -0.047179 0.566979 
time: 1403637005.801666, t: -0.216975 -0.424547 0.272058 q: 0.019549 0.823739 -0.048105 0.564586 
time: 1403637005.901666, t: -0.238475 -0.410454 0.284052 q: 0.022833 0.822098 -0.044864 0.567116 
time: 1403637006.001667, t: -0.256909 -0.398428 0.293449 q: 0.025329 0.819640 -0.042844 0.570713 
time: 1403637006.101666, t: -0.275166 -0.384303 0.299437 q: 0.023567 0.817705 -0.044208 0.573454 
time: 1403637006.201667, t: -0.293808 -0.370857 0.303246 q: 0.022646 0.817715 -0.044538 0.573451 
time: 1403637006.301666, t: -0.313207 -0.356359 0.304903 q: 0.020113 0.821951 -0.046509 0.567300 
time: 1403637006.401666, t: -0.328956 -0.344535 0.303778 q: 0.017808 0.828438 -0.047946 0.557740 
time: 1403637006.501667, t: -0.343080 -0.332478 0.302146 q: 0.011820 0.831091 -0.052058 0.553569 
time: 1403637006.601666, t: -0.351677 -0.323953 0.301051 q: 0.008176 0.826625 -0.049918 0.560475 
time: 1403637006.701667, t: -0.356001 -0.318483 0.298156 q: 0.006365 0.821813 -0.039512 0.568349 
optimize pose graph 
time: 1403637006.801666, t: -0.358591 -0.314924 0.292222 q: 0.000530 0.817751 -0.025439 0.575009 
time: 1403637006.901666, t: -0.359756 -0.313304 0.283305 q: -0.011225 0.817828 -0.009411 0.575277 
time: 1403637007.001667, t: -0.361348 -0.312487 0.274324 q: -0.021101 0.819058 0.012756 0.573180 
time: 1403637007.101666, t: -0.362821 -0.313023 0.265371 q: -0.029065 0.817628 0.031814 0.574131 
time: 1403637007.201667, t: -0.364503 -0.314028 0.255724 q: -0.035656 0.815361 0.046131 0.576010 
time: 1403637007.301666, t: -0.366755 -0.315254 0.246780 q: -0.037014 0.810122 0.057853 0.582224 
time: 1403637007.401666, t: -0.371961 -0.315466 0.237387 q: -0.037927 0.806681 0.064619 0.586218 
time: 1403637007.501667, t: -0.380694 -0.314263 0.226321 q: -0.039794 0.808039 0.067310 0.583917 
time: 1403637007.601666, t: -0.391290 -0.311932 0.213937 q: -0.040989 0.812591 0.067828 0.577421 
time: 1403637007.701667, t: -0.403816 -0.307856 0.199554 q: -0.040561 0.820789 0.067239 0.565808 
time: 1403637007.801666, t: -0.414587 -0.302271 0.187027 q: -0.038473 0.824377 0.066936 0.560750 
time: 1403637007.901666, t: -0.422763 -0.294545 0.177267 q: -0.042054 0.819544 0.062380 0.568056 
time: 1403637008.001667, t: -0.429794 -0.285891 0.168673 q: -0.048118 0.813108 0.055505 0.577459 
time: 1403637008.101666, t: -0.438383 -0.278374 0.160592 q: -0.044616 0.814863 0.054596 0.575349 
time: 1403637008.201667, t: -0.447918 -0.271385 0.152272 q: -0.039853 0.824005 0.054502 0.562546 
time: 1403637008.301666, t: -0.454640 -0.263630 0.147437 q: -0.034572 0.830177 0.055166 0.553686 
time: 1403637008.401666, t: -0.457447 -0.254463 0.147388 q: -0.031828 0.830357 0.054406 0.553655 
time: 1403637008.501667, t: -0.455783 -0.243144 0.152349 q: -0.034989 0.823001 0.049912 0.564759 
time: 1403637008.601666, t: -0.451620 -0.231619 0.158004 q: -0.037803 0.816909 0.045508 0.573724 
time: 1403637008.701667, t: -0.447657 -0.220608 0.159548 q: -0.042628 0.814899 0.040049 0.576644 
time: 1403637008.801666, t: -0.443644 -0.211915 0.156189 q: -0.054866 0.818616 0.029929 0.570930 
optimize pose graph 
time: 1403637008.901666, t: -0.437555 -0.208779 0.148335 q: -0.062803 0.822860 0.023597 0.564270 
time: 1403637009.001667, t: -0.429826 -0.211201 0.136826 q: -0.059406 0.823291 0.024446 0.563972 
time: 1403637009.101666, t: -0.420034 -0.218218 0.122564 q: -0.047218 0.822045 0.031394 0.566592 
time: 1403637009.201667, t: -0.409264 -0.228678 0.106899 q: -0.035352 0.820941 0.038708 0.568602 
time: 1403637009.301666, t: -0.396247 -0.239819 0.088905 q: -0.019117 0.820829 0.049265 0.568725 
time: 1403637009.401666, t: -0.383581 -0.248185 0.074667 q: -0.033075 0.816746 0.050558 0.573826 
wait for imu ... 
time: 1403637009.501667, t: -0.380801 -0.256794 0.071012 q: -0.046925 0.807097 0.064615 0.584993 
time: 1403637009.601666, t: -0.380640 -0.261500 0.063972 q: -0.089436 0.785079 0.053638 0.610553 
time: 1403637009.701667, t: -0.389905 -0.267817 0.062335 q: -0.078339 0.790845 0.040545 0.605627 
time: 1403637009.801666, t: -0.391405 -0.266944 0.062235 q: -0.078037 0.790165 0.037323 0.606759 
time: 1403637009.901666, t: -0.393416 -0.267311 0.061957 q: -0.078576 0.789915 0.038223 0.606959 
time: 1403637010.001667, t: -0.392249 -0.265767 0.061071 q: -0.078762 0.789813 0.038164 0.607071 
time: 1403637010.101666, t: -0.393184 -0.266373 0.060632 q: -0.079753 0.789982 0.037881 0.606740 
time: 1403637010.201667, t: -0.394605 -0.267725 0.060558 q: -0.080059 0.789920 0.037850 0.606782 
time: 1403637010.301666, t: -0.394459 -0.267968 0.060593 q: -0.080435 0.790154 0.037735 0.606435 
wait for imu ... 
time: 1403637010.401666, t: -0.394350 -0.268187 0.060741 q: -0.080406 0.790135 0.037716 0.606464 
time: 1403637010.501667, t: -0.394281 -0.268327 0.060768 q: -0.080417 0.790245 0.037642 0.606324 

Done.
time: 1403637010.601666, t: -0.394360 -0.268451 0.060819 q: -0.080528 0.790297 0.037555 0.606247 
optimize pose graph 
killing:
 * /loop_fusion
 * /rosout
 * /vins_estimator
[33m[ WARN] [1720469073.142959296]: Shutdown request received.[0m
[33m[ WARN] [1720469073.142999949]: Reason given for shutdown: [user request][0m
[33m[ WARN] [1720469073.143821637]: Shutdown request received.[0m
[33m[ WARN] [1720469073.143847756]: Reason given for shutdown: [user request][0m
killed
