/SLAM-Hive/slam_hive_results/mapping_results/3418/2040_vins-fusion-mono-imu+MH_02_easy+Image_comb_test-0.yamlERROR: Unable to communicate with master!
... logging to /root/.ros/log/61d344ec-9d7e-11ef-819e-0242ac110002/roslaunch-600af4e00347-119.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
[0m[ INFO] [1731035160.799091413]: init begins[0m
[31m[ERROR] [1731035160.799477780]: [registerPublisher] Failed to contact master at [localhost:11311].  Retrying...[0m
[31m[ERROR] [1731035160.800366094]: [registerPublisher] Failed to contact master at [localhost:11311].  Retrying...[0m
[1mstarted roslaunch server http://600af4e00347:42051/[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 [156][0m
[0m[ INFO] [1731035161.655116177]: Connected to master at [localhost:11311][0m
[0m[ INFO] [1731035161.655754368]: Connected to master at [localhost:11311][0m
config_file: /slamhive/mappingtask.yaml
config_file: /slamhive/mappingtask.yaml
[1mROS_MASTER_URI=http://600af4e00347:11311/[0m
]2;roscore http://600af4e00347:11311/
[1msetting /run_id to 61d344ec-9d7e-11ef-819e-0242ac110002[0m
USE_IMU: 1
IMU_TOPIC: /imu0
result path /home/output/vio.csv
[33m[ WARN] [1731035161.676475703]:  fix extrinsic param [0m
camera number 1
[0m[ INFO] [1731035161.678466616]: Synchronized sensors, fix time offset: 0[0m
[0m[ INFO] [1731035161.678502347]: 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] [1731035161.678681393]: reading paramerter of camera /slamhive/cam0_pinhole.yaml[0m
vocabulary_file/home/catkin_ws/src/VINS-Fusion/loop_fusion/../support_files/brief_k10L6.bin
MULTIPLE_THREAD is 1
[33m[ WARN] [1731035161.688548700]: waiting for image and imu...[0m
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 [243][0m
started core service [/rosout]
rosbag play /slamhive/dataset/MH_02_easy.bag  --clock 
[0m[ INFO] [1731035170.521711276]: 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: 327398313.90 
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] [1731035173.965531423]: gyroscope bias initial calibration 0.00132595  0.0080178  0.0885517[0m
[0m[ INFO] [1731035174.035900584]: Initialization finish![0m
time: 1403636859.801666, t: 0.014047 -0.068251 0.276616 q: -0.025496 0.805934 0.043590 0.589847 
time: 1403636859.901666, t: 0.014755 -0.069161 0.322594 q: -0.018266 0.802068 0.044465 0.595295 
time: 1403636860.001667, t: 0.013702 -0.070069 0.358107 q: -0.015797 0.801116 0.043061 0.596749 
time: 1403636860.101666, t: 0.014983 -0.074064 0.368813 q: -0.014404 0.803644 0.039514 0.593622 
time: 1403636860.301666, t: 0.016586 -0.083777 0.297643 q: -0.022733 0.816953 0.035577 0.575157 
time: 1403636860.401666, t: 0.013809 -0.087723 0.239656 q: -0.025445 0.829745 0.032052 0.556640 
time: 1403636860.501667, t: 0.009167 -0.091078 0.183709 q: -0.028915 0.842961 0.028543 0.536438 
time: 1403636860.601666, t: 0.004853 -0.093070 0.138577 q: -0.033989 0.852871 0.027052 0.520312 
time: 1403636860.701667, t: -0.000257 -0.094844 0.108616 q: -0.038331 0.856584 0.029895 0.513712 
time: 1403636860.801666, t: -0.003596 -0.096124 0.100585 q: -0.034754 0.856312 0.036413 0.514000 
time: 1403636860.901666, t: -0.004869 -0.096925 0.120371 q: -0.032173 0.855602 0.042622 0.514872 
wait for imu ... 
time: 1403636861.001667, t: -0.003603 -0.097513 0.164440 q: -0.031831 0.853369 0.046909 0.518215 
time: 1403636861.101666, t: -0.000120 -0.098269 0.222727 q: -0.032027 0.846005 0.046844 0.530147 
time: 1403636861.201667, t: 0.004765 -0.099376 0.285657 q: -0.031602 0.833931 0.046358 0.549009 
time: 1403636861.301666, t: 0.008494 -0.101290 0.347937 q: -0.032934 0.820200 0.045626 0.569302 
time: 1403636861.401666, t: 0.010878 -0.104188 0.398868 q: -0.034317 0.808960 0.041995 0.585357 
time: 1403636861.501667, t: 0.012693 -0.107204 0.423208 q: -0.033761 0.801927 0.039347 0.595168 
time: 1403636861.601666, t: 0.013555 -0.109977 0.414393 q: -0.031914 0.802734 0.038359 0.594247 
time: 1403636861.701667, t: 0.014840 -0.110902 0.376730 q: -0.033405 0.809207 0.039179 0.585263 
time: 1403636861.801666, t: 0.014195 -0.110436 0.323805 q: -0.032663 0.816126 0.041681 0.575442 
time: 1403636861.901666, t: 0.010771 -0.108160 0.266450 q: -0.036074 0.823962 0.043512 0.563819 
time: 1403636862.001667, t: 0.006695 -0.104301 0.208220 q: -0.040013 0.835559 0.043032 0.546250 
time: 1403636862.101666, t: 0.002617 -0.099550 0.155369 q: -0.043437 0.844747 0.040818 0.531836 
time: 1403636862.201667, t: -0.000239 -0.096236 0.116711 q: -0.046357 0.849432 0.041788 0.523994 
time: 1403636862.301666, t: -0.002619 -0.095925 0.103860 q: -0.040931 0.853074 0.047269 0.518030 
time: 1403636862.401666, t: -0.002467 -0.096265 0.124127 q: -0.039450 0.856117 0.050477 0.512795 
time: 1403636862.501667, t: -0.000677 -0.097103 0.171577 q: -0.038349 0.854028 0.050945 0.516304 
time: 1403636862.601666, t: 0.005117 -0.100026 0.233484 q: -0.027439 0.848062 0.049206 0.526893 
time: 1403636862.701667, t: 0.010957 -0.101668 0.301324 q: -0.019453 0.840247 0.046789 0.539830 
time: 1403636862.801666, t: 0.015460 -0.101535 0.369901 q: -0.021217 0.835419 0.039813 0.547759 
time: 1403636862.901666, t: 0.017536 -0.101474 0.434139 q: -0.024331 0.832274 0.031208 0.552950 
wait for imu ... 
time: 1403636863.001667, t: 0.016097 -0.101589 0.478077 q: -0.029085 0.826692 0.027636 0.561223 
time: 1403636863.101666, t: 0.015026 -0.103627 0.487194 q: -0.023162 0.820643 0.032150 0.570065 
time: 1403636863.201667, t: 0.014269 -0.104995 0.461915 q: -0.015094 0.818140 0.037666 0.573585 
time: 1403636863.301666, t: 0.013774 -0.104446 0.410404 q: -0.009309 0.819507 0.041266 0.571505 
time: 1403636863.401666, t: 0.013490 -0.104401 0.346812 q: -0.003415 0.821740 0.040808 0.568389 
time: 1403636863.501667, t: 0.011576 -0.104218 0.277898 q: -0.003883 0.826530 0.036231 0.561712 
time: 1403636863.601666, t: 0.006455 -0.103637 0.210045 q: -0.015951 0.834490 0.033448 0.549775 
time: 1403636863.701667, t: 0.000984 -0.103862 0.151342 q: -0.025948 0.842986 0.030906 0.536420 
time: 1403636863.801666, t: -0.001995 -0.104897 0.113112 q: -0.030183 0.850859 0.027834 0.523788 
time: 1403636863.901666, t: -0.004473 -0.106404 0.108003 q: -0.030808 0.856447 0.029189 0.514487 
time: 1403636864.001667, t: -0.005232 -0.107774 0.139639 q: -0.033363 0.857438 0.032183 0.512496 
time: 1403636864.101666, t: -0.004452 -0.108455 0.198874 q: -0.031659 0.853819 0.037205 0.518273 
wait for imu ... 
time: 1403636864.201667, t: -0.001318 -0.109061 0.271771 q: -0.023466 0.849193 0.039210 0.526102 
time: 1403636864.301666, t: 0.002301 -0.107209 0.342168 q: -0.022358 0.846160 0.039103 0.531022 
time: 1403636864.401666, t: 0.005047 -0.104350 0.403787 q: -0.024374 0.842278 0.036618 0.537245 
time: 1403636864.501667, t: 0.007685 -0.102511 0.455034 q: -0.026331 0.832399 0.029005 0.552790 
time: 1403636864.601666, t: 0.010355 -0.099667 0.483609 q: -0.030778 0.820861 0.023225 0.569826 
time: 1403636864.701667, t: 0.010776 -0.099530 0.480055 q: -0.025197 0.815430 0.026355 0.577706 
time: 1403636864.801666, t: 0.010706 -0.100014 0.445553 q: -0.019491 0.818120 0.031576 0.573849 
time: 1403636864.901666, t: 0.011004 -0.099299 0.390931 q: -0.017786 0.823474 0.032279 0.566156 
time: 1403636865.001667, t: 0.010861 -0.100188 0.330123 q: -0.011162 0.829742 0.031615 0.557139 
time: 1403636865.101666, t: 0.011270 -0.100671 0.268429 q: -0.010554 0.836914 0.031018 0.546353 
time: 1403636865.201667, t: 0.011432 -0.099787 0.212892 q: -0.025769 0.842362 0.022424 0.537828 
time: 1403636865.301666, t: 0.010508 -0.099143 0.170074 q: -0.039439 0.844742 0.016729 0.533457 
time: 1403636865.401666, t: 0.010533 -0.099147 0.143200 q: -0.038688 0.845798 0.016452 0.531845 
time: 1403636865.501667, t: 0.010810 -0.098962 0.137444 q: -0.031365 0.850372 0.022693 0.524756 
time: 1403636865.601666, t: 0.011739 -0.097385 0.160397 q: -0.028363 0.855848 0.028688 0.515652 
time: 1403636865.701667, t: 0.011692 -0.093034 0.207672 q: -0.032635 0.855262 0.031745 0.516191 
time: 1403636865.801666, t: 0.013132 -0.088986 0.269255 q: -0.035574 0.848569 0.031165 0.526967 
wait for imu ... 
time: 1403636865.901666, t: 0.015818 -0.086981 0.337328 q: -0.038539 0.838696 0.028607 0.542480 
time: 1403636866.001667, t: 0.017392 -0.085640 0.405897 q: -0.042319 0.824778 0.025230 0.563306 
time: 1403636866.101666, t: 0.016685 -0.085230 0.467510 q: -0.041125 0.809069 0.021572 0.585876 
time: 1403636866.201667, t: 0.013554 -0.085579 0.510363 q: -0.037441 0.798944 0.022384 0.599821 
time: 1403636866.301666, t: 0.011170 -0.085762 0.522722 q: -0.031293 0.798317 0.024099 0.600941 
time: 1403636866.401666, t: 0.011296 -0.086312 0.504144 q: -0.023227 0.804533 0.025517 0.592905 
time: 1403636866.501667, t: 0.011775 -0.086962 0.461080 q: -0.019975 0.811263 0.027076 0.583712 
time: 1403636866.601666, t: 0.012754 -0.089516 0.404475 q: -0.017178 0.817769 0.027436 0.574635 
time: 1403636866.701667, t: 0.012882 -0.094914 0.351204 q: -0.012795 0.825095 0.027664 0.564172 
time: 1403636866.801666, t: 0.008170 -0.099539 0.308882 q: -0.016251 0.830888 0.031592 0.555305 
time: 1403636866.901666, t: 0.003827 -0.101711 0.273597 q: -0.028151 0.837391 0.033997 0.544819 
time: 1403636867.001667, t: -0.003493 -0.102903 0.246053 q: -0.037429 0.845954 0.033856 0.530863 
time: 1403636867.101666, t: -0.009384 -0.103124 0.229365 q: -0.037996 0.852183 0.033832 0.520764 
time: 1403636867.201667, t: -0.018521 -0.103024 0.222459 q: -0.045027 0.855335 0.036336 0.514834 
time: 1403636867.301666, t: -0.029049 -0.102361 0.222283 q: -0.054204 0.857414 0.042784 0.509974 
time: 1403636867.501667, t: -0.044590 -0.102708 0.232616 q: -0.050288 0.867602 0.046217 0.492547 
time: 1403636867.601666, t: -0.053678 -0.103356 0.234352 q: -0.044369 0.882755 0.050090 0.465044 
time: 1403636867.701667, t: -0.064376 -0.104424 0.230426 q: -0.043183 0.906655 0.050827 0.416568 
time: 1403636867.801666, t: -0.074569 -0.106750 0.224687 q: -0.038820 0.930501 0.048900 0.360928 
wait for imu ... 
time: 1403636867.901666, t: -0.080140 -0.108804 0.222710 q: -0.035605 0.943211 0.043418 0.327413 
time: 1403636868.001667, t: -0.080151 -0.107884 0.226614 q: -0.035873 0.943908 0.038827 0.325950 
time: 1403636868.101666, t: -0.080165 -0.106496 0.232404 q: -0.036915 0.938748 0.037870 0.340521 
optimize pose graph 
time: 1403636868.201667, t: -0.083247 -0.105351 0.235774 q: -0.039524 0.934972 0.038112 0.350446 
time: 1403636868.301666, t: -0.089272 -0.103368 0.236932 q: -0.041745 0.934990 0.036672 0.350294 
time: 1403636868.401666, t: -0.097152 -0.098821 0.237088 q: -0.043479 0.936304 0.033396 0.346884 
time: 1403636868.501667, t: -0.105104 -0.092340 0.238884 q: -0.041624 0.934016 0.030871 0.353453 
time: 1403636868.601666, t: -0.112301 -0.086409 0.243903 q: -0.037953 0.930568 0.026141 0.363208 
time: 1403636868.701667, t: -0.119793 -0.082330 0.248582 q: -0.036304 0.930796 0.023674 0.362959 
time: 1403636868.801666, t: -0.128323 -0.078244 0.249803 q: -0.036007 0.933536 0.028712 0.355513 
time: 1403636868.901666, t: -0.133375 -0.074644 0.250575 q: -0.032184 0.934359 0.033395 0.353303 
time: 1403636869.001667, t: -0.135051 -0.069846 0.251935 q: -0.034470 0.932043 0.035369 0.358965 
time: 1403636869.101666, t: -0.135535 -0.066123 0.253164 q: -0.040539 0.930124 0.036780 0.363145 
time: 1403636869.201667, t: -0.136042 -0.063086 0.252121 q: -0.043735 0.929628 0.039167 0.363793 
time: 1403636869.301666, t: -0.135158 -0.061797 0.250450 q: -0.043870 0.929712 0.041199 0.363337 
time: 1403636869.401666, t: -0.133511 -0.061171 0.249290 q: -0.045361 0.929520 0.042099 0.363543 
time: 1403636869.501667, t: -0.131727 -0.062972 0.248471 q: -0.045745 0.929459 0.042045 0.363655 
time: 1403636869.601666, t: -0.128672 -0.071518 0.247595 q: -0.045297 0.929618 0.041289 0.363392 
time: 1403636869.701667, t: -0.123205 -0.089916 0.246616 q: -0.043559 0.931164 0.040247 0.359744 
time: 1403636869.801666, t: -0.114297 -0.117425 0.246361 q: -0.041172 0.932292 0.036697 0.357476 
time: 1403636869.901666, t: -0.100614 -0.152826 0.247261 q: -0.037750 0.930839 0.029768 0.362255 
time: 1403636870.001667, t: -0.084390 -0.194512 0.247502 q: -0.034712 0.929034 0.022054 0.367701 
time: 1403636870.101666, t: -0.067216 -0.241048 0.246788 q: -0.030852 0.929993 0.013412 0.366034 
time: 1403636870.201667, t: -0.049082 -0.290803 0.247195 q: -0.023837 0.932381 0.008803 0.360582 
time: 1403636870.301666, t: -0.030275 -0.340360 0.248352 q: -0.021253 0.932077 0.004679 0.361606 
time: 1403636870.401666, t: -0.012230 -0.389835 0.248648 q: -0.016546 0.929313 0.000124 0.368921 
time: 1403636870.501667, t: 0.002671 -0.434046 0.245507 q: -0.013939 0.928805 -0.005188 0.370271 
time: 1403636870.601666, t: 0.013298 -0.470242 0.240627 q: -0.010942 0.931644 -0.011978 0.363009 
time: 1403636870.701667, t: 0.021836 -0.499459 0.236840 q: -0.003287 0.932667 -0.016278 0.360357 
time: 1403636870.801666, t: 0.029893 -0.517625 0.233177 q: -0.005888 0.929362 -0.020878 0.368532 
time: 1403636870.901666, t: 0.033697 -0.528789 0.228345 q: -0.009481 0.925365 -0.022429 0.378296 
time: 1403636871.001667, t: 0.034405 -0.533379 0.223797 q: -0.014496 0.923371 -0.024379 0.382860 
time: 1403636871.101666, t: 0.033526 -0.533166 0.218450 q: -0.015957 0.926148 -0.028201 0.375766 
time: 1403636871.201667, t: 0.031763 -0.529055 0.217564 q: -0.009166 0.929974 -0.030677 0.366229 
time: 1403636871.301666, t: 0.029050 -0.517185 0.221122 q: -0.008707 0.929886 -0.031914 0.366357 
time: 1403636871.401666, t: 0.023721 -0.496485 0.224998 q: -0.007958 0.929416 -0.027698 0.367907 
time: 1403636871.501667, t: 0.016665 -0.465171 0.228097 q: -0.009636 0.929080 -0.023606 0.368999 
time: 1403636871.601666, t: 0.007570 -0.424763 0.230970 q: -0.011993 0.929599 -0.021493 0.367750 
time: 1403636871.701667, t: -0.003530 -0.376324 0.235117 q: -0.019441 0.930537 -0.018778 0.365200 
time: 1403636871.801666, t: -0.014335 -0.322934 0.238432 q: -0.027100 0.930761 -0.014341 0.364341 
time: 1403636871.901666, t: -0.023986 -0.267536 0.239377 q: -0.030678 0.930183 -0.007308 0.365739 
time: 1403636872.001667, t: -0.032261 -0.208444 0.238996 q: -0.039336 0.927809 -0.001602 0.370973 
time: 1403636872.101666, t: -0.039523 -0.149826 0.236726 q: -0.048928 0.925116 0.005015 0.376484 
time: 1403636872.201667, t: -0.047293 -0.096899 0.233202 q: -0.053111 0.924572 0.011423 0.377114 
time: 1403636872.301666, t: -0.056095 -0.049440 0.228071 q: -0.058011 0.926687 0.019209 0.370832 
time: 1403636872.401666, t: -0.063989 -0.011482 0.223128 q: -0.062058 0.929130 0.026856 0.363518 
time: 1403636872.501667, t: -0.068163 0.013008 0.220674 q: -0.064622 0.929771 0.031436 0.361057 
time: 1403636872.701667, t: -0.070620 0.019481 0.223943 q: -0.065211 0.926533 0.039021 0.368458 
time: 1403636872.801666, t: -0.068580 0.006442 0.225435 q: -0.061008 0.926656 0.038443 0.368929 
time: 1403636872.901666, t: -0.062397 -0.019353 0.225164 q: -0.058774 0.927865 0.033360 0.366740 
time: 1403636873.001667, t: -0.052820 -0.058781 0.223697 q: -0.053547 0.928674 0.027653 0.365967 
time: 1403636873.101666, t: -0.040588 -0.107824 0.221708 q: -0.048534 0.928837 0.022186 0.366624 
time: 1403636873.201667, t: -0.026424 -0.163133 0.220306 q: -0.044720 0.929776 0.014710 0.365103 
time: 1403636873.301666, t: -0.011078 -0.222361 0.219498 q: -0.040270 0.930576 0.006175 0.363824 
time: 1403636873.401666, t: 0.002825 -0.282447 0.218264 q: -0.035897 0.931130 -0.000971 0.362916 
time: 1403636873.501667, t: 0.014483 -0.340116 0.215393 q: -0.032567 0.931973 -0.007277 0.360989 
time: 1403636873.601666, t: 0.022262 -0.392827 0.211468 q: -0.025199 0.933651 -0.014341 0.357009 
time: 1403636873.701667, t: 0.028389 -0.436437 0.205663 q: -0.019608 0.935008 -0.020369 0.353497 
time: 1403636873.801666, t: 0.031055 -0.468227 0.200483 q: -0.019022 0.936077 -0.021716 0.350609 
time: 1403636873.901666, t: 0.031607 -0.486593 0.196708 q: -0.019184 0.935999 -0.024079 0.350653 
time: 1403636874.001667, t: 0.031809 -0.493289 0.193956 q: -0.017219 0.935357 -0.027114 0.352244 
time: 1403636874.101666, t: 0.031861 -0.491369 0.192619 q: -0.016500 0.935362 -0.028222 0.352178 
time: 1403636874.201667, t: 0.031134 -0.481909 0.193419 q: -0.017124 0.936018 -0.027215 0.350481 
time: 1403636874.301666, t: 0.028515 -0.461659 0.195573 q: -0.018531 0.936944 -0.022462 0.348264 
time: 1403636874.401666, t: 0.025091 -0.427278 0.197018 q: -0.024113 0.937613 -0.016855 0.346435 
time: 1403636874.501667, t: 0.020513 -0.381540 0.198874 q: -0.029387 0.936914 -0.011561 0.348131 
time: 1403636874.601666, t: 0.014167 -0.328715 0.201129 q: -0.032235 0.935305 -0.005913 0.352322 
time: 1403636874.701667, t: 0.005814 -0.268929 0.201080 q: -0.038535 0.934098 0.001461 0.354928 
time: 1403636874.801666, t: -0.001795 -0.208892 0.198236 q: -0.044447 0.933668 0.007848 0.355284 
time: 1403636874.901666, t: -0.008871 -0.152811 0.193260 q: -0.053212 0.934069 0.011917 0.352905 
time: 1403636875.001667, t: -0.013389 -0.107196 0.189552 q: -0.058640 0.933412 0.015525 0.353641 
time: 1403636875.101666, t: -0.012969 -0.073152 0.186739 q: -0.056852 0.928738 0.010508 0.366200 
time: 1403636875.201667, t: -0.014239 -0.046845 0.184247 q: -0.048842 0.921916 -0.006036 0.384250 
time: 1403636875.301666, t: -0.020877 -0.025174 0.181044 q: -0.041680 0.917292 -0.030343 0.394865 
time: 1403636875.401666, t: -0.033290 -0.007618 0.177436 q: -0.040910 0.915952 -0.057324 0.395060 
time: 1403636875.501667, t: -0.056324 0.002599 0.175143 q: -0.044130 0.916782 -0.077201 0.389363 
time: 1403636875.601666, t: -0.089418 0.001197 0.173791 q: -0.047462 0.917988 -0.091667 0.382939 
time: 1403636875.701667, t: -0.126628 -0.013049 0.173761 q: -0.043838 0.917463 -0.104387 0.381370 
time: 1403636875.801666, t: -0.164225 -0.037618 0.173056 q: -0.033060 0.915542 -0.119537 0.382622 
time: 1403636875.901666, t: -0.201024 -0.068559 0.171965 q: -0.020620 0.914870 -0.137515 0.379048 
time: 1403636876.001667, t: -0.233343 -0.106619 0.173659 q: -0.005758 0.912370 -0.159837 0.376830 
time: 1403636876.101666, t: -0.255840 -0.152780 0.178500 q: 0.015206 0.906551 -0.182044 0.380518 
optimize pose graph 
time: 1403636876.201667, t: -0.265757 -0.202076 0.182794 q: 0.032548 0.899875 -0.202209 0.385069 
time: 1403636876.301666, t: -0.260632 -0.253349 0.183747 q: 0.044027 0.896435 -0.218523 0.383031 
time: 1403636876.401666, t: -0.242186 -0.307621 0.182786 q: 0.052562 0.895680 -0.230967 0.376364 
time: 1403636876.501667, t: -0.207855 -0.362466 0.181746 q: 0.064552 0.894435 -0.235748 0.374489 
time: 1403636876.601666, t: -0.161007 -0.411735 0.183530 q: 0.074172 0.891617 -0.229924 0.382953 
time: 1403636876.701667, t: -0.104169 -0.449367 0.186633 q: 0.076322 0.889857 -0.216645 0.394200 
time: 1403636876.801666, t: -0.044035 -0.469131 0.190508 q: 0.072031 0.891188 -0.196268 0.402585 
time: 1403636876.901666, t: 0.014574 -0.467679 0.193465 q: 0.064275 0.893595 -0.169458 0.410658 
time: 1403636877.001667, t: 0.070037 -0.444030 0.194077 q: 0.049242 0.895234 -0.138914 0.420517 
time: 1403636877.101666, t: 0.117977 -0.401244 0.193451 q: 0.028467 0.897684 -0.102373 0.427636 
time: 1403636877.201667, t: 0.155311 -0.342615 0.192554 q: 0.004261 0.900371 -0.062432 0.430599 
time: 1403636877.301666, t: 0.180042 -0.274066 0.192753 q: -0.019966 0.901911 -0.025687 0.430695 
time: 1403636877.401666, t: 0.186322 -0.201403 0.192785 q: -0.041403 0.902599 0.010728 0.428351 
time: 1403636877.501667, t: 0.176450 -0.131184 0.190759 q: -0.061696 0.903613 0.041220 0.421874 
time: 1403636877.601666, t: 0.150535 -0.070929 0.186300 q: -0.079439 0.906298 0.060842 0.410624 
time: 1403636877.701667, t: 0.112000 -0.023946 0.181644 q: -0.093194 0.909505 0.071440 0.398763 
time: 1403636877.801666, t: 0.063712 0.007444 0.178983 q: -0.100060 0.913125 0.071785 0.388636 
time: 1403636877.901666, t: 0.012003 0.024548 0.174901 q: -0.098721 0.917725 0.062822 0.379591 
time: 1403636878.001667, t: -0.040634 0.026249 0.170128 q: -0.089811 0.922770 0.048753 0.371555 
time: 1403636878.101666, t: -0.089333 0.013299 0.164689 q: -0.075276 0.926068 0.028761 0.368652 
time: 1403636878.201667, t: -0.131305 -0.014892 0.159668 q: -0.060001 0.926610 0.003851 0.371187 
time: 1403636878.301666, t: -0.165964 -0.056260 0.154791 q: -0.047919 0.925460 -0.022588 0.375122 
time: 1403636878.401666, t: -0.190511 -0.107460 0.150613 q: -0.037225 0.923428 -0.050613 0.378594 
time: 1403636878.501667, t: -0.203011 -0.166298 0.150086 q: -0.021679 0.921442 -0.076329 0.380328 
time: 1403636878.601666, t: -0.202039 -0.228776 0.154355 q: -0.003857 0.918719 -0.099373 0.382186 
time: 1403636878.701667, t: -0.186635 -0.290995 0.161835 q: 0.014818 0.915143 -0.116973 0.385502 
time: 1403636878.801666, t: -0.161206 -0.349655 0.171273 q: 0.030107 0.910756 -0.128306 0.391350 
time: 1403636878.901666, t: -0.125415 -0.399022 0.177866 q: 0.036820 0.908286 -0.134978 0.394260 
time: 1403636879.001667, t: -0.085806 -0.440427 0.182326 q: 0.037482 0.908575 -0.137429 0.392684 
time: 1403636879.101666, t: -0.041203 -0.466519 0.187232 q: 0.035745 0.907450 -0.137652 0.395359 
time: 1403636879.201667, t: 0.006663 -0.479496 0.194471 q: 0.032661 0.902564 -0.132941 0.408213 
time: 1403636879.301666, t: 0.052705 -0.476923 0.202149 q: 0.029894 0.897869 -0.121921 0.421986 
time: 1403636879.401666, t: 0.092652 -0.458103 0.209134 q: 0.022990 0.895624 -0.102096 0.432327 
time: 1403636879.501667, t: 0.124735 -0.423978 0.214752 q: 0.009221 0.895802 -0.074959 0.437990 
time: 1403636879.601666, t: 0.143622 -0.376893 0.220464 q: -0.007002 0.897724 -0.047796 0.437903 
time: 1403636879.701667, t: 0.150155 -0.320612 0.226587 q: -0.026575 0.898905 -0.026850 0.436512 
time: 1403636879.801666, t: 0.142336 -0.261040 0.231498 q: -0.044295 0.900869 -0.008443 0.431743 
time: 1403636879.901666, t: 0.121411 -0.205807 0.233407 q: -0.055781 0.904302 0.005743 0.423195 
time: 1403636880.001667, t: 0.087791 -0.161712 0.234629 q: -0.062427 0.908070 0.014153 0.413897 
time: 1403636880.101666, t: 0.046765 -0.137584 0.235667 q: -0.064800 0.910724 0.018444 0.407484 
time: 1403636880.201667, t: 0.002154 -0.136537 0.237427 q: -0.060054 0.911429 0.014243 0.406803 
time: 1403636880.301666, t: -0.043698 -0.153495 0.239364 q: -0.046882 0.910115 -0.001834 0.411690 
time: 1403636880.401666, t: -0.088853 -0.178907 0.242774 q: -0.028094 0.907830 -0.023392 0.417742 
time: 1403636880.501667, t: -0.136009 -0.201393 0.250626 q: -0.008377 0.905249 -0.045012 0.422408 
time: 1403636880.601666, t: -0.183861 -0.212175 0.259865 q: 0.007523 0.904593 -0.063018 0.421525 
time: 1403636880.701667, t: -0.229789 -0.206739 0.267102 q: 0.014643 0.907179 -0.074414 0.413853 
time: 1403636880.801666, t: -0.268274 -0.185812 0.271748 q: 0.013529 0.910291 -0.079299 0.406078 
time: 1403636880.901666, t: -0.294848 -0.154171 0.274536 q: 0.004933 0.911338 -0.074932 0.404751 
time: 1403636881.001667, t: -0.306328 -0.117344 0.276364 q: -0.003801 0.908643 -0.063996 0.412623 
time: 1403636881.101666, t: -0.303103 -0.079155 0.278694 q: -0.011823 0.903980 -0.047199 0.424797 
time: 1403636881.201667, t: -0.287914 -0.041516 0.280208 q: -0.027168 0.900348 -0.026324 0.433523 
time: 1403636881.301666, t: -0.262011 -0.011049 0.282947 q: -0.043856 0.898063 -0.003262 0.437663 
time: 1403636881.401666, t: -0.224888 0.006398 0.285843 q: -0.052352 0.896505 0.016636 0.439615 
time: 1403636881.501667, t: -0.177873 0.008788 0.287783 q: -0.052547 0.893995 0.031987 0.443835 
time: 1403636881.601666, t: -0.125423 -0.003929 0.288457 q: -0.049075 0.890525 0.044624 0.450073 
time: 1403636881.701667, t: -0.071425 -0.031092 0.286405 q: -0.045228 0.888478 0.055991 0.453240 
time: 1403636881.901666, t: 0.025821 -0.113651 0.273467 q: -0.034541 0.891176 0.052133 0.449327 
time: 1403636882.001667, t: 0.058895 -0.162478 0.264554 q: -0.031665 0.892831 0.039803 0.447511 
time: 1403636882.101666, t: 0.077494 -0.211880 0.253757 q: -0.027994 0.892731 0.027566 0.448873 
optimize pose graph 
time: 1403636882.201667, t: 0.079908 -0.255544 0.240221 q: -0.021911 0.891484 0.017493 0.452183 
time: 1403636882.301666, t: 0.068730 -0.286647 0.225639 q: -0.015619 0.888909 0.010566 0.457696 
time: 1403636882.401666, t: 0.048818 -0.300492 0.210207 q: -0.017573 0.886641 0.010179 0.462013 
time: 1403636882.501667, t: 0.029109 -0.296914 0.194467 q: -0.027019 0.884922 0.013351 0.464763 
time: 1403636882.601666, t: 0.019351 -0.282014 0.181581 q: -0.038081 0.881378 0.020134 0.470443 
time: 1403636882.701667, t: 0.024234 -0.261593 0.170953 q: -0.050628 0.871019 0.029656 0.487733 
time: 1403636882.801666, t: 0.040232 -0.239339 0.157469 q: -0.069038 0.854277 0.046307 0.513128 
time: 1403636882.901666, t: 0.060524 -0.218253 0.138023 q: -0.086550 0.838499 0.065557 0.533977 
time: 1403636883.001667, t: 0.079912 -0.198240 0.114013 q: -0.104561 0.831119 0.084739 0.539562 
time: 1403636883.101666, t: 0.098257 -0.180422 0.089250 q: -0.116690 0.828414 0.103166 0.538024 
time: 1403636883.201667, t: 0.115183 -0.165152 0.066087 q: -0.119323 0.822618 0.118099 0.543244 
time: 1403636883.301666, t: 0.128479 -0.151855 0.045084 q: -0.123254 0.816042 0.127886 0.550026 
time: 1403636883.401666, t: 0.138334 -0.142373 0.028616 q: -0.128704 0.812218 0.132423 0.553354 
time: 1403636883.501667, t: 0.145409 -0.137017 0.018054 q: -0.134344 0.806823 0.137443 0.558657 
time: 1403636883.601666, t: 0.150561 -0.133123 0.011651 q: -0.139903 0.799309 0.143937 0.566405 
time: 1403636883.701667, t: 0.154815 -0.130283 0.006462 q: -0.143153 0.795554 0.152084 0.568746 
time: 1403636883.801666, t: 0.157725 -0.128827 0.002872 q: -0.145679 0.794562 0.156003 0.568430 
time: 1403636883.901666, t: 0.159788 -0.127968 -0.001468 q: -0.153560 0.793239 0.156941 0.567944 
time: 1403636884.001667, t: 0.161621 -0.127599 -0.003710 q: -0.155197 0.790381 0.160069 0.570604 
time: 1403636884.101666, t: 0.162317 -0.128252 -0.004970 q: -0.161097 0.785948 0.161268 0.574740 
optimize pose graph 
time: 1403636884.201667, t: 0.162193 -0.129339 -0.004921 q: -0.158876 0.782246 0.163543 0.579744 
time: 1403636884.301666, t: 0.160916 -0.129070 -0.004503 q: -0.158258 0.779848 0.162841 0.583330 
wait for imu ... 
time: 1403636884.401666, t: 0.160388 -0.128795 -0.004435 q: -0.159375 0.779465 0.162177 0.583723 
time: 1403636884.501667, t: 0.160410 -0.129053 -0.004389 q: -0.158447 0.779378 0.162718 0.583942 
time: 1403636884.601666, t: 0.160321 -0.128907 -0.004256 q: -0.158777 0.779359 0.162427 0.583958 
time: 1403636884.701667, t: 0.160489 -0.128955 -0.004347 q: -0.158702 0.779613 0.162368 0.583656 
time: 1403636884.801666, t: 0.160558 -0.129042 -0.004333 q: -0.158441 0.779671 0.162349 0.583654 
time: 1403636884.901666, t: 0.160575 -0.129052 -0.004326 q: -0.158460 0.779710 0.162277 0.583617 
wait for imu ... 
time: 1403636885.001667, t: 0.160565 -0.129050 -0.004301 q: -0.158570 0.779804 0.162160 0.583494 
time: 1403636885.101666, t: 0.160523 -0.129031 -0.004285 q: -0.158641 0.779827 0.161989 0.583491 
time: 1403636885.201667, t: 0.160522 -0.129024 -0.004266 q: -0.158666 0.779898 0.161893 0.583417 
time: 1403636885.301666, t: 0.160534 -0.129062 -0.004258 q: -0.158567 0.779926 0.161893 0.583406 
time: 1403636885.401666, t: 0.160535 -0.129074 -0.004266 q: -0.158440 0.779936 0.161821 0.583447 
time: 1403636885.501667, t: 0.160547 -0.129079 -0.004268 q: -0.158393 0.779950 0.161796 0.583448 
time: 1403636885.601666, t: 0.160507 -0.129102 -0.004245 q: -0.158304 0.779972 0.161759 0.583454 
time: 1403636885.701667, t: 0.160578 -0.129079 -0.004266 q: -0.158372 0.780091 0.161687 0.583296 
time: 1403636885.801666, t: 0.160595 -0.129084 -0.004257 q: -0.158289 0.780091 0.161685 0.583318 
time: 1403636885.901666, t: 0.160574 -0.129061 -0.004264 q: -0.158337 0.780165 0.161625 0.583223 
wait for imu ... 
time: 1403636886.001667, t: 0.160569 -0.129056 -0.004260 q: -0.158379 0.780149 0.161588 0.583243 
time: 1403636886.101666, t: 0.160558 -0.129087 -0.004261 q: -0.158201 0.780163 0.161633 0.583261 
time: 1403636886.201667, t: 0.160566 -0.129102 -0.004264 q: -0.158076 0.780182 0.161680 0.583256 
wait for imu ... 
time: 1403636886.301666, t: 0.160565 -0.129106 -0.004256 q: -0.158029 0.780174 0.161713 0.583270 
wait for imu ... 
time: 1403636886.401666, t: 0.160560 -0.129086 -0.004262 q: -0.158141 0.780184 0.161648 0.583245 
wait for imu ... 
time: 1403636886.501667, t: 0.160545 -0.129086 -0.004264 q: -0.158142 0.780177 0.161643 0.583254 
time: 1403636886.601666, t: 0.160557 -0.129091 -0.004278 q: -0.158100 0.780190 0.161642 0.583249 
time: 1403636886.701667, t: 0.160560 -0.129099 -0.004277 q: -0.158076 0.780202 0.161652 0.583236 
time: 1403636886.801666, t: 0.160557 -0.129107 -0.004278 q: -0.158069 0.780189 0.161669 0.583251 
time: 1403636886.901666, t: 0.160571 -0.129100 -0.004282 q: -0.158120 0.780230 0.161649 0.583189 
time: 1403636887.001667, t: 0.160562 -0.129096 -0.004281 q: -0.157984 0.780219 0.161694 0.583228 
time: 1403636887.101666, t: 0.160574 -0.129110 -0.004293 q: -0.158029 0.780230 0.161682 0.583205 
time: 1403636887.201667, t: 0.160566 -0.129114 -0.004298 q: -0.158048 0.780255 0.161645 0.583176 
time: 1403636887.301666, t: 0.160566 -0.129124 -0.004311 q: -0.158035 0.780266 0.161597 0.583177 
wait for imu ... 
time: 1403636887.401666, t: 0.160583 -0.129116 -0.004299 q: -0.157927 0.780290 0.161658 0.583158 
time: 1403636887.501667, t: 0.160585 -0.129121 -0.004310 q: -0.157906 0.780309 0.161676 0.583133 
wait for imu ... 
time: 1403636887.601666, t: 0.160569 -0.129102 -0.004304 q: -0.157974 0.780288 0.161620 0.583159 
time: 1403636887.701667, t: 0.160563 -0.129088 -0.004321 q: -0.158015 0.780302 0.161608 0.583132 
wait for imu ... 
time: 1403636887.801666, t: 0.160564 -0.129089 -0.004312 q: -0.158029 0.780333 0.161605 0.583087 
wait for imu ... 
time: 1403636887.901666, t: 0.160562 -0.129086 -0.004302 q: -0.157981 0.780365 0.161600 0.583059 
time: 1403636888.001667, t: 0.160577 -0.129096 -0.004310 q: -0.157938 0.780413 0.161592 0.583009 
time: 1403636888.101666, t: 0.160577 -0.129109 -0.004312 q: -0.157956 0.780394 0.161592 0.583030 
wait for imu ... 
time: 1403636888.201667, t: 0.160577 -0.129121 -0.004340 q: -0.157983 0.780397 0.161571 0.583024 
time: 1403636888.301666, t: 0.160589 -0.129124 -0.004341 q: -0.157985 0.780386 0.161581 0.583035 
time: 1403636888.401666, t: 0.160593 -0.129139 -0.004345 q: -0.157909 0.780424 0.161609 0.582997 
time: 1403636888.501667, t: 0.160593 -0.129143 -0.004335 q: -0.157893 0.780448 0.161580 0.582977 
time: 1403636888.601666, t: 0.160562 -0.129129 -0.004296 q: -0.157891 0.780413 0.161499 0.583047 
wait for imu ... 
time: 1403636888.701667, t: 0.160522 -0.129058 -0.004255 q: -0.157933 0.780261 0.161610 0.583208 
time: 1403636888.801666, t: 0.160488 -0.128960 -0.004255 q: -0.157969 0.780214 0.161851 0.583195 
time: 1403636888.901666, t: 0.160421 -0.128948 -0.004313 q: -0.157971 0.780415 0.161576 0.583002 
time: 1403636889.001667, t: 0.160482 -0.128941 -0.004245 q: -0.158277 0.780264 0.161725 0.583080 
time: 1403636889.101666, t: 0.160510 -0.129007 -0.004259 q: -0.157966 0.780368 0.161451 0.583100 
time: 1403636889.201667, t: 0.160548 -0.129038 -0.004264 q: -0.157993 0.780423 0.161373 0.583041 
time: 1403636889.301666, t: 0.160621 -0.129064 -0.004338 q: -0.157964 0.780440 0.161243 0.583062 
time: 1403636889.401666, t: 0.160693 -0.129067 -0.004333 q: -0.158125 0.780357 0.161562 0.583042 
time: 1403636889.501667, t: 0.160644 -0.129089 -0.004317 q: -0.157784 0.780395 0.161485 0.583104 
time: 1403636889.701667, t: 0.160584 -0.129168 -0.004232 q: -0.157632 0.780231 0.161514 0.583357 
time: 1403636889.801666, t: 0.160542 -0.129141 -0.004238 q: -0.157717 0.780207 0.161625 0.583335 
time: 1403636889.901666, t: 0.160551 -0.129165 -0.004233 q: -0.157611 0.780335 0.161276 0.583289 
wait for imu ... 
time: 1403636890.001667, t: 0.160561 -0.129163 -0.004262 q: -0.157703 0.780368 0.161511 0.583155 
time: 1403636890.101666, t: 0.160579 -0.129292 -0.004210 q: -0.157457 0.780425 0.160796 0.583343 
time: 1403636890.201667, t: 0.160522 -0.129154 -0.004108 q: -0.157844 0.780033 0.161314 0.583620 
time: 1403636890.301666, t: 0.160474 -0.129078 -0.004178 q: -0.157872 0.780342 0.161737 0.583081 
time: 1403636890.401666, t: 0.160454 -0.129184 -0.004218 q: -0.157379 0.780564 0.161121 0.583088 
time: 1403636890.501667, t: 0.160493 -0.129209 -0.004150 q: -0.157405 0.780135 0.160914 0.583712 
time: 1403636890.601666, t: 0.160482 -0.129096 -0.004126 q: -0.157803 0.780001 0.161758 0.583550 
time: 1403636890.701667, t: 0.160453 -0.129111 -0.004236 q: -0.157746 0.780452 0.161748 0.582966 
time: 1403636890.801666, t: 0.160502 -0.129247 -0.004190 q: -0.157463 0.780449 0.160975 0.583260 
time: 1403636890.901666, t: 0.160514 -0.129227 -0.004075 q: -0.157575 0.780235 0.161076 0.583488 
wait for imu ... 
time: 1403636891.001667, t: 0.160473 -0.129207 -0.004149 q: -0.157712 0.780284 0.161119 0.583374 
time: 1403636891.101666, t: 0.160479 -0.129189 -0.004161 q: -0.157553 0.780309 0.161156 0.583373 
wait for imu ... 
time: 1403636891.201667, t: 0.160489 -0.129176 -0.004164 q: -0.157519 0.780273 0.161227 0.583410 
wait for imu ... 
time: 1403636891.301666, t: 0.160503 -0.129180 -0.004185 q: -0.157544 0.780293 0.161286 0.583360 
time: 1403636891.401666, t: 0.160498 -0.129177 -0.004183 q: -0.157513 0.780246 0.161260 0.583439 
time: 1403636891.501667, t: 0.160513 -0.129174 -0.004186 q: -0.157571 0.780267 0.161286 0.583388 
time: 1403636891.601666, t: 0.160496 -0.129186 -0.004164 q: -0.157547 0.780322 0.161276 0.583324 
wait for imu ... 
time: 1403636891.701667, t: 0.160481 -0.129197 -0.004173 q: -0.157499 0.780306 0.161209 0.583377 
time: 1403636891.801666, t: 0.160492 -0.129168 -0.004187 q: -0.157540 0.780303 0.161226 0.583365 
time: 1403636891.901666, t: 0.160487 -0.129186 -0.004188 q: -0.157577 0.780299 0.161249 0.583354 
wait for imu ... 
time: 1403636892.001667, t: 0.160484 -0.129184 -0.004190 q: -0.157491 0.780302 0.161240 0.583375 
time: 1403636892.101666, t: 0.160521 -0.129192 -0.004210 q: -0.157515 0.780305 0.161268 0.583357 
time: 1403636892.201667, t: 0.160503 -0.129193 -0.004202 q: -0.157467 0.780311 0.161275 0.583360 
time: 1403636892.301666, t: 0.160590 -0.129272 -0.004213 q: -0.157155 0.780328 0.161397 0.583388 
time: 1403636892.401666, t: 0.160555 -0.129243 -0.004193 q: -0.157323 0.780334 0.161090 0.583419 
wait for imu ... 
time: 1403636892.501667, t: 0.160512 -0.129217 -0.004174 q: -0.157354 0.780266 0.161170 0.583480 
time: 1403636892.601666, t: 0.160490 -0.129208 -0.004171 q: -0.157383 0.780269 0.161212 0.583457 
wait for imu ... 
wait for imu ... 
time: 1403636892.801666, t: 0.160510 -0.129201 -0.004163 q: -0.157356 0.780237 0.161361 0.583465 
time: 1403636892.901666, t: 0.160522 -0.129213 -0.004188 q: -0.157337 0.780353 0.161302 0.583332 
wait for imu ... 
time: 1403636893.001667, t: 0.160538 -0.129225 -0.004194 q: -0.157280 0.780267 0.161337 0.583453 
time: 1403636893.101666, t: 0.160527 -0.129212 -0.004202 q: -0.157291 0.780253 0.161313 0.583475 
time: 1403636893.201667, t: 0.160511 -0.129199 -0.004210 q: -0.157333 0.780278 0.161276 0.583441 
wait for imu ... 
time: 1403636893.301666, t: 0.160515 -0.129201 -0.004222 q: -0.157369 0.780277 0.161272 0.583433 
time: 1403636893.401666, t: 0.160528 -0.129206 -0.004209 q: -0.157356 0.780309 0.161283 0.583390 
time: 1403636893.501667, t: 0.160538 -0.129215 -0.004216 q: -0.157330 0.780307 0.161276 0.583403 
time: 1403636893.601666, t: 0.160533 -0.129216 -0.004214 q: -0.157268 0.780311 0.161262 0.583418 
wait for imu ... 
time: 1403636893.701667, t: 0.160545 -0.129215 -0.004210 q: -0.157277 0.780295 0.161344 0.583414 
time: 1403636893.801666, t: 0.160532 -0.129198 -0.004207 q: -0.157302 0.780342 0.161290 0.583359 
wait for imu ... 
time: 1403636893.901666, t: 0.160525 -0.129209 -0.004215 q: -0.157337 0.780351 0.161250 0.583348 
time: 1403636894.001667, t: 0.160508 -0.129205 -0.004213 q: -0.157354 0.780364 0.161204 0.583339 
time: 1403636894.101666, t: 0.160486 -0.129203 -0.004206 q: -0.157299 0.780387 0.161189 0.583328 
wait for imu ... 
time: 1403636894.201667, t: 0.160513 -0.129231 -0.004237 q: -0.157322 0.780350 0.161201 0.583368 
time: 1403636894.301666, t: 0.160504 -0.129206 -0.004238 q: -0.157312 0.780323 0.161178 0.583414 
wait for imu ... 
time: 1403636894.401666, t: 0.160517 -0.129219 -0.004231 q: -0.157296 0.780374 0.161206 0.583341 
wait for imu ... 
time: 1403636894.501667, t: 0.160529 -0.129216 -0.004238 q: -0.157353 0.780347 0.161226 0.583357 
wait for imu ... 
time: 1403636894.601666, t: 0.160233 -0.128908 -0.004400 q: -0.158332 0.779266 0.162246 0.584253 
time: 1403636894.701667, t: 0.160230 -0.128878 -0.004387 q: -0.158660 0.779116 0.162418 0.584317 
time: 1403636894.801666, t: 0.160236 -0.128869 -0.004389 q: -0.158684 0.779138 0.162457 0.584269 
time: 1403636894.901666, t: 0.160285 -0.128906 -0.004368 q: -0.158491 0.779126 0.162407 0.584352 
wait for imu ... 
time: 1403636895.001667, t: 0.160298 -0.128906 -0.004375 q: -0.158570 0.779128 0.162475 0.584309 
time: 1403636895.101666, t: 0.160295 -0.128894 -0.004362 q: -0.158622 0.779144 0.162475 0.584274 
wait for imu ... 
wait for imu ... 
time: 1403636895.201667, t: 0.160287 -0.128892 -0.004366 q: -0.158610 0.779103 0.162391 0.584355 
wait for imu ... 
time: 1403636895.301666, t: 0.160269 -0.128867 -0.004357 q: -0.158647 0.779090 0.162399 0.584360 
time: 1403636895.401666, t: 0.160256 -0.128858 -0.004353 q: -0.158599 0.779114 0.162439 0.584330 
time: 1403636895.501667, t: 0.160270 -0.128876 -0.004358 q: -0.158629 0.779065 0.162485 0.584374 
wait for imu ... 
time: 1403636895.601666, t: 0.160269 -0.128875 -0.004328 q: -0.158620 0.779125 0.162464 0.584302 
wait for imu ... 
time: 1403636895.701667, t: 0.160289 -0.128888 -0.004350 q: -0.158599 0.779060 0.162509 0.584383 
time: 1403636895.801666, t: 0.160287 -0.128883 -0.004315 q: -0.158500 0.779003 0.162555 0.584473 
wait for imu ... 
time: 1403636895.901666, t: 0.160309 -0.128888 -0.004311 q: -0.158553 0.779024 0.162661 0.584400 
wait for imu ... 
time: 1403636896.001667, t: 0.160286 -0.128877 -0.004284 q: -0.158532 0.779052 0.162709 0.584355 
wait for imu ... 
time: 1403636896.101666, t: 0.160489 -0.128922 -0.004365 q: -0.158641 0.779492 0.162749 0.583728 
time: 1403636896.201667, t: 0.160758 -0.128986 -0.004556 q: -0.158739 0.780166 0.162724 0.582808 
wait for imu ... 
time: 1403636896.301666, t: 0.160727 -0.129018 -0.004512 q: -0.158651 0.780044 0.162915 0.582940 
time: 1403636896.401666, t: 0.160565 -0.128982 -0.004408 q: -0.158597 0.779666 0.162980 0.583443 
wait for imu ... 
time: 1403636896.501667, t: 0.160499 -0.128968 -0.004346 q: -0.158605 0.779479 0.162995 0.583686 
time: 1403636896.601666, t: 0.160506 -0.128976 -0.004348 q: -0.158532 0.779494 0.163005 0.583683 
time: 1403636896.701667, t: 0.160654 -0.129033 -0.004369 q: -0.158580 0.779771 0.163136 0.583263 
wait for imu ... 
time: 1403636896.801666, t: 0.160722 -0.129053 -0.004452 q: -0.158708 0.780000 0.163288 0.582879 
time: 1403636896.901666, t: 0.160735 -0.129110 -0.004354 q: -0.158541 0.780002 0.163528 0.582855 
time: 1403636897.001667, t: 0.160497 -0.129040 -0.004203 q: -0.158814 0.779467 0.163534 0.583495 
time: 1403636897.101666, t: 0.159945 -0.129228 -0.003882 q: -0.159692 0.778834 0.165643 0.583505 
time: 1403636897.201667, t: 0.158739 -0.129684 -0.004115 q: -0.161413 0.778632 0.168426 0.582503 
time: 1403636897.301666, t: 0.156751 -0.132788 -0.004715 q: -0.160601 0.779617 0.167355 0.581719 
time: 1403636897.401666, t: 0.152046 -0.140913 -0.005172 q: -0.155347 0.783277 0.164810 0.578949 
time: 1403636897.501667, t: 0.144039 -0.152902 -0.005565 q: -0.147090 0.788716 0.163195 0.574159 
time: 1403636897.601666, t: 0.134187 -0.167287 -0.005503 q: -0.137509 0.796832 0.162707 0.565399 
time: 1403636897.701667, t: 0.122506 -0.183733 -0.000698 q: -0.124393 0.806901 0.166504 0.552913 
time: 1403636897.801666, t: 0.111370 -0.200041 0.011227 q: -0.115536 0.814616 0.171362 0.541929 
time: 1403636897.901666, t: 0.103389 -0.214186 0.029814 q: -0.111785 0.819593 0.174721 0.534082 
time: 1403636898.001667, t: 0.101475 -0.224225 0.052290 q: -0.113481 0.814251 0.176093 0.541395 
time: 1403636898.101666, t: 0.102880 -0.230658 0.067210 q: -0.118511 0.808095 0.176355 0.549396 
time: 1403636898.201667, t: 0.106276 -0.235548 0.079297 q: -0.121813 0.804917 0.177617 0.552922 
time: 1403636898.301666, t: 0.111224 -0.238400 0.093867 q: -0.123608 0.803874 0.180000 0.553270 
time: 1403636898.401666, t: 0.117634 -0.239085 0.111801 q: -0.125879 0.803293 0.182935 0.552639 
time: 1403636898.501667, t: 0.124703 -0.238166 0.132029 q: -0.129283 0.803314 0.185531 0.550955 
time: 1403636898.601666, t: 0.133052 -0.236200 0.153249 q: -0.130761 0.803013 0.188172 0.550148 
time: 1403636898.701667, t: 0.142448 -0.232981 0.173192 q: -0.131099 0.802975 0.190669 0.549263 
time: 1403636898.801666, t: 0.153972 -0.227939 0.191233 q: -0.133542 0.802795 0.191407 0.548680 
time: 1403636898.901666, t: 0.167136 -0.221153 0.206983 q: -0.136830 0.803834 0.190513 0.546656 
time: 1403636899.001667, t: 0.182988 -0.213673 0.220129 q: -0.142778 0.805171 0.186430 0.544571 
time: 1403636899.101666, t: 0.202576 -0.207545 0.231423 q: -0.143679 0.805596 0.177947 0.546541 
time: 1403636899.201667, t: 0.225562 -0.204260 0.242282 q: -0.135814 0.804366 0.167349 0.553665 
time: 1403636899.301666, t: 0.249954 -0.203929 0.253025 q: -0.126063 0.801247 0.150301 0.565262 
time: 1403636899.401666, t: 0.274285 -0.206701 0.262061 q: -0.110313 0.799820 0.130376 0.575431 
time: 1403636899.501667, t: 0.295546 -0.211985 0.269232 q: -0.099779 0.803828 0.108749 0.576262 
time: 1403636899.601666, t: 0.314796 -0.218741 0.273744 q: -0.093538 0.811181 0.092036 0.569882 
time: 1403636899.701667, t: 0.334518 -0.228687 0.279254 q: -0.088582 0.815434 0.080573 0.566329 
time: 1403636899.801666, t: 0.355833 -0.241796 0.284315 q: -0.081632 0.818497 0.074436 0.563789 
time: 1403636899.901666, t: 0.378937 -0.257765 0.287344 q: -0.072202 0.818644 0.074139 0.564900 
time: 1403636900.001667, t: 0.403651 -0.274936 0.288109 q: -0.065767 0.815118 0.074738 0.570676 
time: 1403636900.101666, t: 0.428955 -0.292220 0.288349 q: -0.063958 0.810001 0.074674 0.578128 
optimize pose graph 
time: 1403636900.201667, t: 0.452047 -0.310555 0.295398 q: -0.063047 0.806140 0.075763 0.583459 
time: 1403636900.301666, t: 0.471765 -0.329073 0.311266 q: -0.062174 0.806730 0.077967 0.582445 
time: 1403636900.401666, t: 0.488467 -0.347648 0.335131 q: -0.063844 0.811379 0.080096 0.575477 
time: 1403636900.501667, t: 0.505072 -0.366015 0.360168 q: -0.064804 0.815001 0.083647 0.569717 
time: 1403636900.601666, t: 0.521042 -0.384350 0.381778 q: -0.066749 0.816843 0.085654 0.566547 
time: 1403636900.701667, t: 0.539034 -0.401327 0.395087 q: -0.068375 0.821484 0.087102 0.559377 
time: 1403636900.801666, t: 0.559767 -0.418588 0.403741 q: -0.071870 0.825396 0.087526 0.553078 
time: 1403636900.901666, t: 0.582498 -0.436726 0.407853 q: -0.080075 0.823922 0.083555 0.554760 
time: 1403636901.001667, t: 0.607933 -0.457422 0.409805 q: -0.085260 0.821074 0.080385 0.558665 
time: 1403636901.101666, t: 0.636975 -0.481809 0.407216 q: -0.089438 0.819691 0.077926 0.560389 
time: 1403636901.201667, t: 0.669864 -0.509059 0.401151 q: -0.090419 0.818811 0.076832 0.561668 
time: 1403636901.301666, t: 0.705160 -0.541233 0.399301 q: -0.086548 0.817977 0.079418 0.563131 
time: 1403636901.401666, t: 0.741727 -0.576052 0.402822 q: -0.082727 0.817489 0.081641 0.564095 
time: 1403636901.501667, t: 0.784134 -0.611606 0.412944 q: -0.077395 0.816673 0.084939 0.565544 
time: 1403636901.601666, t: 0.826089 -0.650013 0.422401 q: -0.073970 0.815928 0.087045 0.566757 
time: 1403636901.701667, t: 0.870053 -0.685340 0.423724 q: -0.073485 0.814077 0.087065 0.569472 
time: 1403636901.801666, t: 0.909826 -0.725038 0.420185 q: -0.074684 0.813558 0.086328 0.570169 
time: 1403636901.901666, t: 0.951982 -0.763045 0.411144 q: -0.074172 0.813489 0.087155 0.570208 
time: 1403636902.001667, t: 0.992085 -0.801469 0.397355 q: -0.072892 0.813142 0.088532 0.570657 
time: 1403636902.101666, t: 1.033876 -0.838822 0.386571 q: -0.072758 0.812398 0.089708 0.571549 
time: 1403636902.201667, t: 1.070955 -0.879538 0.382294 q: -0.072375 0.812451 0.090929 0.571329 
optimize pose graph 
time: 1403636902.301666, t: 1.109202 -0.919990 0.381522 q: -0.074694 0.812804 0.088548 0.570902 
time: 1403636902.401666, t: 1.144982 -0.961871 0.368901 q: -0.075317 0.813504 0.086625 0.570118 
time: 1403636902.501667, t: 1.178228 -1.002378 0.345635 q: -0.072235 0.814299 0.086250 0.569439 
time: 1403636902.601666, t: 1.213758 -1.040770 0.316949 q: -0.065631 0.814263 0.087372 0.570118 
time: 1403636902.701667, t: 1.247059 -1.079854 0.294250 q: -0.060542 0.813238 0.087695 0.572091 
time: 1403636902.801666, t: 1.278970 -1.115858 0.280146 q: -0.060008 0.812822 0.085850 0.573018 
time: 1403636902.901666, t: 1.309391 -1.148977 0.274493 q: -0.062057 0.810615 0.085543 0.575965 
time: 1403636903.001667, t: 1.341775 -1.183799 0.273207 q: -0.063695 0.810569 0.086420 0.575719 
time: 1403636903.101666, t: 1.371881 -1.217220 0.279930 q: -0.065945 0.810967 0.087496 0.574742 
time: 1403636903.201667, t: 1.405110 -1.250789 0.296948 q: -0.067936 0.811696 0.088772 0.573283 
time: 1403636903.301666, t: 1.433811 -1.284388 0.318904 q: -0.068103 0.813032 0.091394 0.570954 
time: 1403636903.401666, t: 1.463984 -1.316479 0.340182 q: -0.066336 0.813634 0.094511 0.569796 
time: 1403636903.501667, t: 1.490142 -1.349208 0.353683 q: -0.065927 0.813846 0.096199 0.569257 
time: 1403636903.601666, t: 1.516091 -1.377727 0.362887 q: -0.066512 0.814195 0.097010 0.568552 
time: 1403636903.701667, t: 1.542785 -1.405943 0.363171 q: -0.069946 0.817286 0.095410 0.563958 
time: 1403636903.801666, t: 1.571226 -1.433020 0.360723 q: -0.073961 0.819979 0.093137 0.559902 
time: 1403636903.901666, t: 1.603444 -1.459454 0.363527 q: -0.078193 0.820418 0.090316 0.559145 
time: 1403636904.001667, t: 1.637968 -1.487793 0.373491 q: -0.081986 0.818742 0.087795 0.561455 
time: 1403636904.101666, t: 1.673042 -1.517205 0.390892 q: -0.084613 0.816829 0.085837 0.564148 
time: 1403636904.201667, t: 1.709778 -1.548269 0.414837 q: -0.087394 0.816444 0.083591 0.564618 
time: 1403636904.301666, t: 1.746642 -1.581599 0.439882 q: -0.087093 0.815679 0.082622 0.565912 
time: 1403636904.401666, t: 1.785622 -1.617164 0.463003 q: -0.085023 0.814553 0.082524 0.567860 
time: 1403636904.501667, t: 1.822315 -1.654242 0.480288 q: -0.081915 0.814138 0.083411 0.568782 
time: 1403636904.601666, t: 1.861486 -1.694160 0.494189 q: -0.077730 0.813982 0.084715 0.569398 
time: 1403636904.701667, t: 1.900032 -1.734950 0.505884 q: -0.073307 0.814247 0.085566 0.569479 
time: 1403636904.801666, t: 1.940813 -1.778357 0.515718 q: -0.069610 0.814086 0.086140 0.570087 
time: 1403636904.901666, t: 1.981848 -1.820959 0.523881 q: -0.065266 0.814487 0.087070 0.569886 
time: 1403636905.001667, t: 2.022184 -1.863639 0.526192 q: -0.062068 0.814508 0.087021 0.570221 
time: 1403636905.101666, t: 2.061142 -1.903285 0.526167 q: -0.059268 0.814076 0.086573 0.571203 
time: 1403636905.201667, t: 2.098347 -1.941435 0.528684 q: -0.056848 0.813667 0.085765 0.572153 
time: 1403636905.301666, t: 2.140307 -1.979095 0.537381 q: -0.053827 0.814736 0.085631 0.570943 
time: 1403636905.401666, t: 2.177005 -2.011088 0.550774 q: -0.052001 0.817462 0.085189 0.567269 
wait for imu ... 
time: 1403636905.501667, t: 2.212065 -2.039200 0.568825 q: -0.051903 0.817795 0.090537 0.565969 
time: 1403636905.701667, t: 2.285661 -2.086062 0.596669 q: -0.056479 0.817494 0.099619 0.564437 
time: 1403636905.801666, t: 2.323297 -2.105943 0.598089 q: -0.061953 0.817471 0.100438 0.563751 
wait for imu ... 
time: 1403636905.901666, t: 2.360793 -2.123187 0.594602 q: -0.065346 0.818201 0.101626 0.562094 
time: 1403636906.001667, t: 2.400147 -2.137088 0.584862 q: -0.067178 0.820390 0.103202 0.558388 
time: 1403636906.101666, t: 2.442046 -2.149015 0.571329 q: -0.065762 0.822107 0.106398 0.555424 
time: 1403636906.201667, t: 2.486800 -2.157678 0.561244 q: -0.066645 0.821125 0.107596 0.556539 
time: 1403636906.301666, t: 2.532773 -2.162566 0.559831 q: -0.070306 0.815755 0.106615 0.564122 
time: 1403636906.401666, t: 2.578739 -2.165359 0.564681 q: -0.076366 0.812210 0.102758 0.569144 
time: 1403636906.501667, t: 2.623582 -2.167522 0.573598 q: -0.088121 0.812056 0.094078 0.569166 
time: 1403636906.601666, t: 2.667576 -2.172620 0.580561 q: -0.093110 0.815281 0.090125 0.564380 
time: 1403636906.701667, t: 2.711406 -2.181310 0.583781 q: -0.087866 0.815304 0.093400 0.564655 
time: 1403636906.801666, t: 2.756893 -2.193727 0.581357 q: -0.079614 0.814545 0.099077 0.566005 
wait for imu ... 
time: 1403636906.901666, t: 2.803657 -2.206225 0.574781 q: -0.075009 0.814654 0.103532 0.565680 
time: 1403636907.001667, t: 2.851497 -2.217100 0.573485 q: -0.072838 0.814444 0.107123 0.565597 
time: 1403636907.101666, t: 2.897838 -2.226607 0.576802 q: -0.073130 0.814902 0.108818 0.564575 
time: 1403636907.201667, t: 2.946992 -2.234515 0.580976 q: -0.072462 0.817750 0.110882 0.560124 
time: 1403636907.301666, t: 2.995574 -2.240694 0.576617 q: -0.074062 0.820945 0.111412 0.555113 
time: 1403636907.401666, t: 3.046450 -2.244587 0.571342 q: -0.079728 0.822303 0.108859 0.552821 
time: 1403636907.501667, t: 3.104363 -2.247055 0.573200 q: -0.085606 0.822851 0.105854 0.551709 
time: 1403636907.601666, t: 3.160567 -2.251255 0.583404 q: -0.090608 0.823185 0.102766 0.550995 
time: 1403636907.701667, t: 3.225652 -2.256650 0.599571 q: -0.093595 0.823194 0.100594 0.550884 
time: 1403636907.801666, t: 3.290992 -2.265282 0.621145 q: -0.095404 0.822940 0.099362 0.551176 
time: 1403636907.901666, t: 3.364150 -2.275372 0.635179 q: -0.095755 0.821720 0.099217 0.552959 
time: 1403636908.001667, t: 3.438792 -2.288001 0.639024 q: -0.090388 0.817766 0.103853 0.558842 
time: 1403636908.101666, t: 3.519458 -2.300126 0.635044 q: -0.085247 0.814009 0.109564 0.564020 
time: 1403636908.201667, t: 3.595735 -2.313734 0.635724 q: -0.084471 0.811329 0.112920 0.567326 
time: 1403636908.301666, t: 3.670160 -2.328338 0.639708 q: -0.084818 0.810496 0.115665 0.567911 
time: 1403636908.401666, t: 3.741145 -2.343053 0.640077 q: -0.085868 0.810855 0.117838 0.566793 
time: 1403636908.501667, t: 3.812346 -2.356025 0.635111 q: -0.087304 0.811772 0.119713 0.564865 
time: 1403636908.601666, t: 3.881309 -2.370695 0.628050 q: -0.088695 0.812004 0.121695 0.563891 
wait for imu ... 
wait for imu ... 
wait for imu ... 
wait for imu ... 
wait for imu ... 
time: 1403636908.701667, t: 3.951911 -2.382343 0.624242 q: -0.089453 0.813190 0.123681 0.561625 
wait for imu ... 
time: 1403636908.801666, t: 4.020408 -2.396747 0.629498 q: -0.092196 0.815810 0.123830 0.557333 
time: 1403636908.901666, t: 4.092352 -2.406807 0.642912 q: -0.093445 0.818793 0.123756 0.552749 
time: 1403636909.101666, t: 4.243888 -2.427534 0.684294 q: -0.097987 0.820090 0.121623 0.550508 
time: 1403636909.201667, t: 4.321813 -2.438639 0.700556 q: -0.098614 0.820291 0.120735 0.550291 
time: 1403636909.301666, t: 4.403565 -2.449551 0.711453 q: -0.098611 0.820880 0.119754 0.549627 
time: 1403636909.501667, t: 4.574928 -2.474339 0.720549 q: -0.094172 0.816408 0.120174 0.556927 
time: 1403636909.601666, t: 4.666173 -2.486506 0.715744 q: -0.094845 0.815388 0.118007 0.558768 
time: 1403636909.701667, t: 4.755538 -2.499422 0.705354 q: -0.098311 0.813500 0.113954 0.561754 
time: 1403636909.801666, t: 4.844957 -2.514127 0.689443 q: -0.093842 0.808546 0.115796 0.569244 
time: 1403636909.901666, t: 4.930317 -2.528053 0.669163 q: -0.080684 0.804245 0.123912 0.575609 
time: 1403636910.001667, t: 5.008832 -2.540575 0.644275 q: -0.069175 0.798829 0.131172 0.582993 
time: 1403636910.101666, t: 5.085795 -2.547895 0.617608 q: -0.070437 0.797982 0.130489 0.584154 
time: 1403636910.201667, t: 5.156272 -2.552353 0.593263 q: -0.078707 0.803294 0.125783 0.576803 
time: 1403636910.301666, t: 5.227068 -2.553564 0.579527 q: -0.083778 0.807436 0.123208 0.570832 
time: 1403636910.401666, t: 5.296463 -2.553433 0.573668 q: -0.086273 0.810129 0.122679 0.566744 
time: 1403636910.501667, t: 5.362510 -2.553744 0.574550 q: -0.087340 0.810669 0.123899 0.565541 
time: 1403636910.601666, t: 5.433388 -2.554361 0.573207 q: -0.086465 0.810833 0.125837 0.565012 
time: 1403636910.701667, t: 5.497588 -2.554105 0.567906 q: -0.086727 0.810869 0.126538 0.564763 
time: 1403636910.801666, t: 5.564867 -2.552538 0.554807 q: -0.088171 0.811292 0.125853 0.564085 
time: 1403636910.901666, t: 5.631118 -2.549875 0.538900 q: -0.091185 0.812041 0.124206 0.562893 
time: 1403636911.001667, t: 5.698950 -2.545331 0.522092 q: -0.097181 0.806193 0.120572 0.571026 
time: 1403636911.101666, t: 5.763745 -2.543315 0.510444 q: -0.100015 0.798899 0.118924 0.581045 
time: 1403636911.201667, t: 5.826382 -2.544388 0.502530 q: -0.097204 0.802328 0.120873 0.576377 
time: 1403636911.301666, t: 5.883916 -2.547305 0.501767 q: -0.093759 0.807738 0.123193 0.568852 
time: 1403636911.401666, t: 5.936851 -2.550792 0.510519 q: -0.090820 0.809151 0.124951 0.566933 
time: 1403636911.501667, t: 5.986209 -2.553769 0.526517 q: -0.088982 0.809764 0.125323 0.566267 
time: 1403636911.601666, t: 6.038001 -2.555160 0.539749 q: -0.088293 0.810262 0.125502 0.565622 
time: 1403636911.701667, t: 6.090420 -2.557513 0.549597 q: -0.088093 0.810277 0.125337 0.565670 
time: 1403636911.801666, t: 6.143073 -2.558964 0.552373 q: -0.091981 0.810937 0.121625 0.564914 
time: 1403636911.901666, t: 6.203805 -2.560229 0.548545 q: -0.099079 0.813538 0.115874 0.561171 
time: 1403636912.001667, t: 6.258905 -2.562828 0.539770 q: -0.101584 0.815484 0.112901 0.558498 
time: 1403636912.101666, t: 6.327864 -2.564889 0.527665 q: -0.098818 0.815560 0.113471 0.558768 
time: 1403636912.201667, t: 6.385797 -2.570040 0.519157 q: -0.095947 0.814089 0.115242 0.561046 
time: 1403636912.301666, t: 6.448188 -2.574993 0.518293 q: -0.091976 0.812270 0.117436 0.563886 
time: 1403636912.401666, t: 6.505902 -2.582642 0.521833 q: -0.091013 0.812335 0.117153 0.564007 
time: 1403636912.501667, t: 6.566499 -2.589513 0.522683 q: -0.096166 0.812803 0.112885 0.563347 
time: 1403636912.601666, t: 6.627479 -2.599775 0.519368 q: -0.096933 0.813196 0.111010 0.563021 
time: 1403636912.701667, t: 6.687646 -2.611162 0.509749 q: -0.101571 0.813550 0.106662 0.562533 
time: 1403636912.801666, t: 6.753137 -2.624480 0.492980 q: -0.105893 0.813432 0.103052 0.562580 
time: 1403636912.901666, t: 6.814301 -2.643532 0.475232 q: -0.102664 0.812545 0.104695 0.564153 
time: 1403636913.001667, t: 6.875981 -2.664686 0.451145 q: -0.096845 0.808629 0.108607 0.570040 
time: 1403636913.101666, t: 6.938535 -2.687594 0.428223 q: -0.092795 0.805505 0.111258 0.574607 
time: 1403636913.201667, t: 6.995255 -2.710767 0.404808 q: -0.089955 0.802893 0.113318 0.578299 
time: 1403636913.301666, t: 7.051771 -2.736804 0.392945 q: -0.088784 0.803465 0.114480 0.577456 
time: 1403636913.401666, t: 7.101693 -2.763043 0.386180 q: -0.092599 0.807168 0.112337 0.572089 
time: 1403636913.501667, t: 7.154541 -2.791218 0.385857 q: -0.096597 0.809869 0.109352 0.568175 
time: 1403636913.601666, t: 7.208826 -2.821148 0.383570 q: -0.093302 0.810289 0.111100 0.567788 
time: 1403636913.701667, t: 7.260172 -2.851752 0.375693 q: -0.090222 0.810554 0.112327 0.567667 
time: 1403636913.801666, t: 7.312052 -2.882650 0.364101 q: -0.088911 0.810040 0.113082 0.568456 
time: 1403636913.901666, t: 7.360727 -2.915544 0.356940 q: -0.087799 0.809373 0.113939 0.569407 
time: 1403636914.001667, t: 7.410831 -2.947065 0.358432 q: -0.087161 0.808835 0.114959 0.570064 
time: 1403636914.201667, t: 7.507360 -3.014144 0.367844 q: -0.092168 0.809209 0.111706 0.569392 
time: 1403636914.301666, t: 7.554273 -3.048298 0.368651 q: -0.090412 0.809844 0.112434 0.568627 
time: 1403636914.401666, t: 7.602100 -3.085359 0.363498 q: -0.086838 0.808825 0.114636 0.570193 
time: 1403636914.501667, t: 7.647802 -3.118117 0.364009 q: -0.084064 0.808608 0.116200 0.570600 
wait for imu ... 
wait for imu ... 
time: 1403636914.601666, t: 7.695133 -3.153421 0.370616 q: -0.083194 0.808410 0.116656 0.570914 
time: 1403636914.701667, t: 7.739799 -3.186253 0.386202 q: -0.083025 0.807317 0.116348 0.572546 
time: 1403636914.801666, t: 7.789149 -3.220954 0.402318 q: -0.081753 0.806722 0.116174 0.573602 
time: 1403636914.901666, t: 7.828692 -3.251545 0.413750 q: -0.080656 0.807770 0.115658 0.572386 
time: 1403636915.001667, t: 7.869851 -3.283391 0.417112 q: -0.079626 0.809905 0.115070 0.569625 
time: 1403636915.101666, t: 7.906562 -3.311031 0.416713 q: -0.078347 0.811532 0.115043 0.567488 
time: 1403636915.201667, t: 7.942527 -3.337556 0.410016 q: -0.077008 0.813943 0.114947 0.564229 
time: 1403636915.301666, t: 7.980167 -3.362424 0.398040 q: -0.075495 0.818757 0.115201 0.557375 
time: 1403636915.401666, t: 8.024332 -3.387870 0.382680 q: -0.074558 0.822331 0.115940 0.552060 
time: 1403636915.501667, t: 8.068751 -3.409143 0.370232 q: -0.078075 0.820639 0.113559 0.554581 
time: 1403636915.701667, t: 8.164252 -3.447765 0.367651 q: -0.096058 0.811882 0.101994 0.566761 
time: 1403636915.801666, t: 8.212101 -3.471771 0.377350 q: -0.097930 0.810227 0.101377 0.568915 
time: 1403636915.901666, t: 8.259841 -3.498887 0.395850 q: -0.098291 0.809260 0.101962 0.570123 
time: 1403636916.001667, t: 8.306524 -3.528488 0.415907 q: -0.092322 0.808298 0.106699 0.571617 
time: 1403636916.101666, t: 8.351022 -3.558764 0.431555 q: -0.083883 0.807578 0.113052 0.572713 
time: 1403636916.201667, t: 8.396562 -3.589869 0.440715 q: -0.079759 0.807685 0.116733 0.572413 
time: 1403636916.301666, t: 8.440498 -3.620397 0.451466 q: -0.081462 0.807908 0.116012 0.572005 
time: 1403636916.401666, t: 8.481076 -3.650192 0.467126 q: -0.080441 0.808577 0.116518 0.571100 
time: 1403636916.501667, t: 8.522623 -3.679403 0.481643 q: -0.073643 0.808766 0.112199 0.572614 
time: 1403636916.601666, t: 8.562652 -3.708600 0.490933 q: -0.063484 0.809903 0.102484 0.574041 
time: 1403636916.701667, t: 8.599512 -3.733481 0.494714 q: -0.054893 0.811147 0.092080 0.574934 
time: 1403636916.801666, t: 8.636540 -3.756252 0.493568 q: -0.049787 0.811627 0.082952 0.576109 
time: 1403636916.901666, t: 8.670515 -3.776178 0.487564 q: -0.044271 0.812169 0.077406 0.576567 
time: 1403636917.001667, t: 8.704849 -3.792553 0.476172 q: -0.041188 0.812158 0.073430 0.577332 
time: 1403636917.101666, t: 8.736456 -3.806577 0.459405 q: -0.041922 0.812708 0.068961 0.577055 
time: 1403636917.201667, t: 8.765576 -3.819509 0.439349 q: -0.043521 0.813149 0.063316 0.576962 
time: 1403636917.301666, t: 8.793276 -3.832286 0.412556 q: -0.037912 0.813141 0.054019 0.578313 
time: 1403636917.401666, t: 8.819372 -3.847080 0.382553 q: -0.025079 0.813324 0.040189 0.579879 
time: 1403636917.501667, t: 8.846931 -3.860398 0.347286 q: -0.012851 0.813880 0.023438 0.580417 
time: 1403636917.601666, t: 8.872338 -3.871698 0.315934 q: -0.005528 0.813734 0.009127 0.581139 
time: 1403636917.701667, t: 8.894621 -3.881232 0.291246 q: 0.000488 0.813003 -0.001221 0.582258 
time: 1403636917.801666, t: 8.916906 -3.889266 0.274510 q: 0.006949 0.812494 -0.009264 0.582855 
time: 1403636917.901666, t: 8.936088 -3.894946 0.267855 q: 0.012876 0.811794 -0.015027 0.583608 
time: 1403636918.001667, t: 8.950823 -3.898563 0.265962 q: 0.016858 0.812421 -0.018840 0.582523 
time: 1403636918.101666, t: 8.962524 -3.901175 0.259927 q: 0.020752 0.813434 -0.020251 0.580934 
time: 1403636918.201667, t: 8.974639 -3.902384 0.248924 q: 0.024268 0.814790 -0.019475 0.578921 
time: 1403636918.301666, t: 8.983889 -3.901416 0.242368 q: 0.026625 0.815634 -0.018072 0.577672 
time: 1403636918.401666, t: 8.992297 -3.897072 0.241538 q: 0.027005 0.821625 -0.017124 0.569131 
time: 1403636918.501667, t: 9.002506 -3.889464 0.248179 q: 0.027121 0.827519 -0.016004 0.560553 
time: 1403636918.601666, t: 9.016422 -3.878902 0.263509 q: 0.026777 0.829316 -0.014406 0.557952 
time: 1403636918.701667, t: 9.036138 -3.864587 0.288585 q: 0.023925 0.827893 -0.013483 0.560214 
time: 1403636918.801666, t: 9.059808 -3.847371 0.320521 q: 0.011087 0.824925 -0.019923 0.564782 
time: 1403636918.901666, t: 9.085367 -3.831108 0.357685 q: -0.001634 0.819061 -0.028728 0.572984 
time: 1403636919.001667, t: 9.111865 -3.818313 0.397102 q: -0.007020 0.814268 -0.040257 0.579050 
time: 1403636919.101666, t: 9.138732 -3.811475 0.438944 q: -0.008224 0.811451 -0.048942 0.582309 
time: 1403636919.201667, t: 9.161671 -3.811654 0.482990 q: -0.005509 0.809730 -0.054155 0.584273 
time: 1403636919.301666, t: 9.180852 -3.818682 0.527638 q: -0.001173 0.809127 -0.057272 0.584835 
time: 1403636919.401666, t: 9.197696 -3.832613 0.567279 q: 0.005606 0.808519 -0.059738 0.585403 
time: 1403636919.501667, t: 9.210104 -3.851328 0.599048 q: 0.017569 0.808343 -0.056994 0.585683 
time: 1403636919.601666, t: 9.219678 -3.873211 0.624459 q: 0.029392 0.808302 -0.052830 0.585657 
time: 1403636919.701667, t: 9.225256 -3.895088 0.642239 q: 0.035844 0.807339 -0.050898 0.586795 
time: 1403636919.801666, t: 9.228414 -3.915736 0.653486 q: 0.035788 0.806475 -0.052227 0.587869 
time: 1403636920.001667, t: 9.229168 -3.954311 0.656747 q: 0.029337 0.804611 -0.056496 0.590381 
time: 1403636920.101666, t: 9.224369 -3.974112 0.649950 q: 0.023281 0.803238 -0.059626 0.592209 
time: 1403636920.201667, t: 9.215116 -3.996289 0.638132 q: 0.017823 0.802437 -0.062054 0.593234 
time: 1403636920.301666, t: 9.201865 -4.021617 0.622402 q: 0.023050 0.802809 -0.057205 0.593038 
time: 1403636920.401666, t: 9.184878 -4.049131 0.603376 q: 0.031489 0.803216 -0.050366 0.592719 
time: 1403636920.501667, t: 9.162884 -4.074273 0.587524 q: 0.036568 0.805851 -0.045913 0.589203 
time: 1403636920.601666, t: 9.137027 -4.100057 0.579387 q: 0.037703 0.809631 -0.044538 0.584031 
time: 1403636920.701667, t: 9.108923 -4.121948 0.580158 q: 0.037915 0.814127 -0.044823 0.577712 
time: 1403636920.801666, t: 9.080773 -4.143507 0.587762 q: 0.037607 0.818004 -0.045643 0.572164 
time: 1403636920.901666, t: 9.053242 -4.160727 0.604226 q: 0.036718 0.820070 -0.046686 0.569173 
time: 1403636921.001667, t: 9.025279 -4.177406 0.624502 q: 0.037173 0.820453 -0.047286 0.568541 
time: 1403636921.101666, t: 9.001384 -4.191503 0.640224 q: 0.037164 0.820060 -0.048214 0.569031 
time: 1403636921.201667, t: 8.980680 -4.204052 0.650702 q: 0.038038 0.818586 -0.048325 0.571082 
time: 1403636921.301666, t: 8.961188 -4.215395 0.655885 q: 0.038496 0.812713 -0.048226 0.579387 
time: 1403636921.401666, t: 8.940285 -4.225352 0.654856 q: 0.034091 0.808404 -0.051636 0.585367 
time: 1403636921.501667, t: 8.916621 -4.234479 0.648327 q: 0.027426 0.806995 -0.056496 0.587210 
time: 1403636921.601666, t: 8.889911 -4.244874 0.635367 q: 0.021626 0.810047 -0.061172 0.582764 
time: 1403636921.701667, t: 8.861827 -4.257203 0.616023 q: 0.020527 0.819265 -0.062852 0.569590 
time: 1403636921.801666, t: 8.835851 -4.271526 0.594188 q: 0.023445 0.828194 -0.061853 0.556524 
time: 1403636921.901666, t: 8.814883 -4.287956 0.573679 q: 0.027779 0.828754 -0.060217 0.555669 
time: 1403636922.001667, t: 8.799319 -4.306329 0.553786 q: 0.033652 0.822654 -0.059044 0.564466 
time: 1403636922.101666, t: 8.784808 -4.325019 0.533685 q: 0.040537 0.816379 -0.060253 0.572932 
time: 1403636922.201667, t: 8.770645 -4.342381 0.516508 q: 0.046293 0.813446 -0.063061 0.576356 
time: 1403636922.301666, t: 8.756622 -4.359138 0.505987 q: 0.051692 0.817375 -0.065825 0.569993 
time: 1403636922.401666, t: 8.743420 -4.373199 0.500912 q: 0.054490 0.820883 -0.069684 0.564204 
time: 1403636922.501667, t: 8.731412 -4.386563 0.491378 q: 0.058765 0.824051 -0.075656 0.558358 
time: 1403636922.601666, t: 8.721900 -4.400249 0.476496 q: 0.065027 0.824779 -0.084618 0.555293 
time: 1403636922.701667, t: 8.717477 -4.412659 0.460209 q: 0.072709 0.818997 -0.091717 0.561734 
time: 1403636922.801666, t: 8.714715 -4.422397 0.443700 q: 0.074684 0.809290 -0.100025 0.573992 
time: 1403636922.901666, t: 8.710236 -4.429798 0.430840 q: 0.070608 0.802373 -0.109424 0.582442 
time: 1403636923.001667, t: 8.702021 -4.437035 0.424890 q: 0.064888 0.797996 -0.117884 0.587448 
time: 1403636923.101666, t: 8.691532 -4.446003 0.421835 q: 0.066452 0.797228 -0.123191 0.587227 
time: 1403636923.201667, t: 8.677652 -4.456633 0.412266 q: 0.073077 0.798414 -0.130975 0.583129 
time: 1403636923.301666, t: 8.658397 -4.469717 0.400069 q: 0.084623 0.802705 -0.137304 0.574153 
time: 1403636923.401666, t: 8.637823 -4.483075 0.381504 q: 0.093858 0.808455 -0.142339 0.563321 
time: 1403636923.501667, t: 8.618087 -4.496711 0.358361 q: 0.098907 0.814592 -0.147529 0.552170 
time: 1403636923.601666, t: 8.602302 -4.510221 0.337139 q: 0.100521 0.820694 -0.151933 0.541548 
time: 1403636923.701667, t: 8.593932 -4.525170 0.325732 q: 0.102693 0.823577 -0.153865 0.536191 
time: 1403636923.801666, t: 8.593695 -4.541453 0.326263 q: 0.104520 0.823022 -0.154361 0.536547 
wait for imu ... 
time: 1403636923.901666, t: 8.601223 -4.556341 0.329440 q: 0.105361 0.823202 -0.154039 0.536200 
time: 1403636924.001667, t: 8.616976 -4.571156 0.333095 q: 0.106664 0.823439 -0.152443 0.536034 
time: 1403636924.101666, t: 8.639231 -4.586204 0.328276 q: 0.107642 0.822950 -0.151333 0.536902 
time: 1403636924.201667, t: 8.667115 -4.602086 0.318531 q: 0.108443 0.821873 -0.149325 0.538950 
time: 1403636924.301666, t: 8.700621 -4.617705 0.306906 q: 0.107658 0.820653 -0.148516 0.541186 
time: 1403636924.401666, t: 8.739190 -4.633236 0.301648 q: 0.100783 0.818591 -0.152105 0.544624 
time: 1403636924.501667, t: 8.780742 -4.650113 0.307750 q: 0.093741 0.813015 -0.155869 0.553104 
time: 1403636924.601666, t: 8.826567 -4.669733 0.317612 q: 0.085744 0.806769 -0.159868 0.562329 
time: 1403636924.701667, t: 8.871505 -4.693160 0.326470 q: 0.080076 0.802061 -0.162480 0.569111 
time: 1403636924.801666, t: 8.915504 -4.719896 0.329024 q: 0.074165 0.797086 -0.164035 0.576408 
time: 1403636924.901666, t: 8.957204 -4.752082 0.324229 q: 0.073125 0.793021 -0.164904 0.581874 
time: 1403636925.001667, t: 8.992957 -4.789695 0.310769 q: 0.077870 0.791682 -0.170559 0.581452 
time: 1403636925.101666, t: 9.021252 -4.832547 0.289204 q: 0.090049 0.792993 -0.172060 0.577450 
time: 1403636925.201667, t: 9.045000 -4.876898 0.263180 q: 0.106083 0.797110 -0.169040 0.569902 
time: 1403636925.301666, t: 9.066386 -4.921486 0.232991 q: 0.116369 0.803753 -0.168125 0.558725 
time: 1403636925.401666, t: 9.089050 -4.965729 0.198702 q: 0.121372 0.811935 -0.169709 0.545188 
time: 1403636925.501667, t: 9.115293 -5.008759 0.166385 q: 0.123100 0.817578 -0.172367 0.535446 
time: 1403636925.601666, t: 9.145669 -5.050622 0.139723 q: 0.123437 0.822635 -0.174862 0.526743 
wait for imu ... 
wait for imu ... 
time: 1403636925.701667, t: 9.185831 -5.092163 0.122646 q: 0.123183 0.825670 -0.177010 0.521309 
time: 1403636925.801666, t: 9.236287 -5.134267 0.116786 q: 0.125101 0.826232 -0.176828 0.520021 
time: 1403636925.901666, t: 9.297497 -5.177383 0.119730 q: 0.127315 0.820117 -0.175537 0.529514 
time: 1403636926.001667, t: 9.366599 -5.219058 0.127998 q: 0.131767 0.809515 -0.172127 0.545614 
time: 1403636926.101666, t: 9.439547 -5.258199 0.132319 q: 0.129811 0.797810 -0.172820 0.562833 
time: 1403636926.201667, t: 9.509843 -5.294554 0.128922 q: 0.123128 0.789584 -0.176117 0.574787 
time: 1403636926.301666, t: 9.577151 -5.329499 0.118170 q: 0.116657 0.785320 -0.179235 0.580981 
time: 1403636926.401666, t: 9.638349 -5.362800 0.101018 q: 0.109603 0.782840 -0.182122 0.584791 
time: 1403636926.501667, t: 9.691782 -5.396902 0.078861 q: 0.105831 0.783107 -0.182801 0.584916 
time: 1403636926.601666, t: 9.738512 -5.430292 0.059717 q: 0.108266 0.786713 -0.179152 0.580746 
time: 1403636926.701667, t: 9.778212 -5.466158 0.047150 q: 0.111361 0.789804 -0.175648 0.577024 
time: 1403636926.801666, t: 9.811852 -5.500761 0.042464 q: 0.111111 0.793203 -0.180141 0.570993 
time: 1403636926.901666, t: 9.841948 -5.535599 0.044400 q: 0.111542 0.794291 -0.191308 0.565740 
time: 1403636927.001667, t: 9.872061 -5.577017 0.046141 q: 0.116414 0.791820 -0.206050 0.563039 
time: 1403636927.101666, t: 9.898076 -5.619697 0.041695 q: 0.127112 0.787809 -0.221557 0.560457 
time: 1403636927.201667, t: 9.920667 -5.665207 0.032369 q: 0.139947 0.782378 -0.239728 0.557521 
time: 1403636927.301666, t: 9.941580 -5.713376 0.016868 q: 0.156028 0.776968 -0.257400 0.552921 
time: 1403636927.401666, t: 9.958431 -5.764635 -0.003551 q: 0.170750 0.777406 -0.269292 0.542186 
time: 1403636927.501667, t: 9.973463 -5.815449 -0.028178 q: 0.179173 0.782573 -0.278818 0.527008 
time: 1403636927.601666, t: 9.992760 -5.867339 -0.053154 q: 0.183636 0.787576 -0.286863 0.513529 
time: 1403636927.701667, t: 10.019781 -5.921871 -0.070649 q: 0.188414 0.789356 -0.291247 0.506549 
time: 1403636927.801666, t: 10.054678 -5.980089 -0.078314 q: 0.189460 0.788621 -0.295077 0.505086 
time: 1403636927.901666, t: 10.095641 -6.040119 -0.078611 q: 0.190125 0.787154 -0.296642 0.506207 
time: 1403636928.001667, t: 10.142627 -6.104044 -0.082638 q: 0.190924 0.785487 -0.296826 0.508381 
time: 1403636928.101666, t: 10.195521 -6.170530 -0.088862 q: 0.193289 0.782202 -0.294761 0.513727 
time: 1403636928.201667, t: 10.251944 -6.240048 -0.099310 q: 0.195079 0.775541 -0.292328 0.524427 
time: 1403636928.301666, t: 10.311807 -6.310058 -0.112410 q: 0.193914 0.767720 -0.291347 0.536768 
time: 1403636928.401666, t: 10.370158 -6.378442 -0.130496 q: 0.194271 0.762180 -0.289086 0.545684 
time: 1403636928.501667, t: 10.424611 -6.444953 -0.150375 q: 0.196482 0.759982 -0.286323 0.549402 
time: 1403636928.601666, t: 10.476205 -6.509739 -0.166353 q: 0.197399 0.762308 -0.284503 0.546790 
time: 1403636928.701667, t: 10.525711 -6.572540 -0.175745 q: 0.197034 0.768289 -0.283913 0.538798 
time: 1403636928.801666, t: 10.576253 -6.633823 -0.178013 q: 0.196738 0.773886 -0.283543 0.531035 
time: 1403636928.901666, t: 10.626657 -6.693617 -0.171171 q: 0.196876 0.776126 -0.283321 0.527824 
time: 1403636929.001667, t: 10.681293 -6.752756 -0.153926 q: 0.197845 0.770217 -0.282482 0.536495 
time: 1403636929.101666, t: 10.734225 -6.810407 -0.128688 q: 0.193310 0.761165 -0.285403 0.549367 
time: 1403636929.201667, t: 10.784328 -6.863926 -0.098593 q: 0.188064 0.756509 -0.288810 0.555801 
time: 1403636929.301666, t: 10.828685 -6.916187 -0.070310 q: 0.181944 0.756084 -0.292484 0.556495 
time: 1403636929.401666, t: 10.867896 -6.969670 -0.048039 q: 0.180762 0.756787 -0.292132 0.556109 
time: 1403636929.501667, t: 10.903337 -7.027046 -0.031843 q: 0.183655 0.759325 -0.288844 0.553413 
time: 1403636929.601666, t: 10.934461 -7.083995 -0.021444 q: 0.190135 0.762215 -0.283197 0.550161 
time: 1403636929.701667, t: 10.960990 -7.136911 -0.017399 q: 0.196420 0.764329 -0.277901 0.547714 
time: 1403636929.801666, t: 10.986587 -7.187960 -0.017991 q: 0.199500 0.765800 -0.275149 0.545933 
time: 1403636929.901666, t: 11.009559 -7.232718 -0.024392 q: 0.198326 0.764716 -0.275544 0.547678 
time: 1403636930.001667, t: 11.032687 -7.277284 -0.025487 q: 0.194208 0.762960 -0.278140 0.550284 
time: 1403636930.101666, t: 11.053761 -7.320299 -0.019054 q: 0.191449 0.763093 -0.279838 0.550206 
time: 1403636930.201667, t: 11.071028 -7.358133 -0.007548 q: 0.188299 0.762686 -0.282574 0.550459 
time: 1403636930.301666, t: 11.088084 -7.400650 0.010489 q: 0.185334 0.763841 -0.284714 0.548758 
time: 1403636930.401666, t: 11.101690 -7.443295 0.035691 q: 0.187842 0.763790 -0.283278 0.548720 
time: 1403636930.501667, t: 11.113723 -7.486303 0.056885 q: 0.193637 0.764576 -0.279630 0.547481 
time: 1403636930.601666, t: 11.123514 -7.525979 0.069159 q: 0.195954 0.764475 -0.277757 0.547751 
time: 1403636930.701667, t: 11.131149 -7.564728 0.076861 q: 0.191726 0.762988 -0.280510 0.549913 
time: 1403636930.901666, t: 11.136311 -7.636053 0.073673 q: 0.179479 0.758755 -0.290228 0.554838 
time: 1403636931.001667, t: 11.135026 -7.677055 0.065575 q: 0.175758 0.754558 -0.300962 0.556034 
time: 1403636931.101666, t: 11.128714 -7.722367 0.055633 q: 0.183106 0.750338 -0.308326 0.555338 
time: 1403636931.201667, t: 11.115786 -7.769499 0.051677 q: 0.199238 0.746720 -0.307522 0.555107 
time: 1403636931.301666, t: 11.097495 -7.814965 0.055775 q: 0.212332 0.741641 -0.306502 0.557620 
time: 1403636931.401666, t: 11.074181 -7.856470 0.064540 q: 0.222421 0.740302 -0.305381 0.556079 
time: 1403636931.501667, t: 11.045865 -7.892571 0.078752 q: 0.233561 0.742669 -0.301749 0.550309 
time: 1403636931.601666, t: 11.014869 -7.920619 0.097344 q: 0.244224 0.745513 -0.297198 0.544278 
time: 1403636931.701667, t: 10.983906 -7.939534 0.113602 q: 0.249851 0.746056 -0.294918 0.542217 
time: 1403636931.801666, t: 10.953707 -7.954874 0.121220 q: 0.239548 0.747367 -0.303112 0.540538 
time: 1403636931.901666, t: 10.922163 -7.964074 0.122006 q: 0.230350 0.749342 -0.310245 0.537749 
time: 1403636932.001667, t: 10.891884 -7.971364 0.117931 q: 0.225293 0.749872 -0.314375 0.536753 
time: 1403636932.101666, t: 10.859533 -7.975564 0.108406 q: 0.224992 0.747219 -0.315031 0.540183 
time: 1403636932.201667, t: 10.828218 -7.979913 0.096347 q: 0.227467 0.744261 -0.314319 0.543633 
time: 1403636932.301666, t: 10.792496 -7.978764 0.089749 q: 0.238027 0.742352 -0.308814 0.544876 
time: 1403636932.401666, t: 10.756976 -7.971248 0.090054 q: 0.247866 0.741964 -0.304470 0.543462 
time: 1403636932.501667, t: 10.722784 -7.957866 0.097145 q: 0.252909 0.740217 -0.306842 0.542184 
time: 1403636932.601666, t: 10.684483 -7.936579 0.110817 q: 0.255715 0.737219 -0.318209 0.538388 
time: 1403636932.701667, t: 10.645179 -7.910287 0.132169 q: 0.256920 0.731105 -0.334516 0.536261 
time: 1403636932.801666, t: 10.607290 -7.879447 0.160359 q: 0.261171 0.725657 -0.346432 0.534037 
time: 1403636932.901666, t: 10.568256 -7.842393 0.191441 q: 0.265192 0.722830 -0.356674 0.529125 
wait for imu ... 
time: 1403636933.001667, t: 10.529169 -7.801717 0.212650 q: 0.268159 0.721598 -0.364986 0.523614 
time: 1403636933.101666, t: 10.488754 -7.758060 0.218573 q: 0.273278 0.718400 -0.374470 0.518645 
time: 1403636933.201667, t: 10.447743 -7.711555 0.212764 q: 0.273310 0.719239 -0.384348 0.510170 
time: 1403636933.301666, t: 10.406905 -7.663118 0.199434 q: 0.268902 0.727523 -0.394283 0.492893 
time: 1403636933.401666, t: 10.369326 -7.615848 0.184544 q: 0.263919 0.731776 -0.401108 0.483697 
time: 1403636933.501667, t: 10.335804 -7.573774 0.169445 q: 0.265503 0.728902 -0.401412 0.486908 
time: 1403636933.601666, t: 10.305763 -7.535315 0.155807 q: 0.269079 0.724328 -0.398801 0.493866 
time: 1403636933.701667, t: 10.278897 -7.499511 0.147206 q: 0.272508 0.720458 -0.395148 0.500536 
time: 1403636933.801666, t: 10.253983 -7.466875 0.147425 q: 0.275412 0.717197 -0.391114 0.506760 
time: 1403636933.901666, t: 10.229440 -7.431713 0.153482 q: 0.274978 0.713403 -0.390858 0.512517 
time: 1403636934.001667, t: 10.205570 -7.399817 0.164199 q: 0.275731 0.709901 -0.396578 0.512580 
time: 1403636934.101666, t: 10.177088 -7.363737 0.168494 q: 0.281929 0.703859 -0.407285 0.509133 
time: 1403636934.201667, t: 10.145670 -7.327941 0.158775 q: 0.294056 0.695155 -0.421382 0.502720 
time: 1403636934.301666, t: 10.114458 -7.293089 0.139327 q: 0.307221 0.684253 -0.438391 0.495203 
time: 1403636934.401666, t: 10.080477 -7.254419 0.117135 q: 0.316855 0.669370 -0.460229 0.489628 
time: 1403636934.501667, t: 10.045594 -7.215386 0.099286 q: 0.325103 0.653963 -0.483407 0.482658 
time: 1403636934.601666, t: 10.006767 -7.176064 0.089205 q: 0.337451 0.639761 -0.503151 0.472940 
time: 1403636934.701667, t: 9.963799 -7.132475 0.092160 q: 0.347830 0.628935 -0.520134 0.461426 
time: 1403636934.801666, t: 9.918495 -7.090020 0.102642 q: 0.357013 0.625882 -0.530236 0.446837 
time: 1403636934.901666, t: 9.872901 -7.048517 0.120361 q: 0.362834 0.625773 -0.536638 0.434487 
time: 1403636935.001667, t: 9.828429 -7.007913 0.145981 q: 0.365885 0.625521 -0.539966 0.428122 
time: 1403636935.101666, t: 9.782804 -6.970249 0.178285 q: 0.370251 0.622863 -0.538051 0.430646 
time: 1403636935.201667, t: 9.739320 -6.935948 0.214766 q: 0.371051 0.618182 -0.535839 0.439374 
time: 1403636935.301666, t: 9.700628 -6.903347 0.247297 q: 0.368945 0.616410 -0.534581 0.445131 
time: 1403636935.401666, t: 9.656969 -6.872405 0.271458 q: 0.365228 0.616714 -0.533725 0.448789 
time: 1403636935.501667, t: 9.614992 -6.839856 0.289300 q: 0.363207 0.617790 -0.531987 0.451005 
time: 1403636935.601666, t: 9.568081 -6.810390 0.298914 q: 0.361261 0.619085 -0.530496 0.452546 
time: 1403636935.701667, t: 9.522072 -6.776811 0.301692 q: 0.360561 0.620344 -0.528657 0.453532 
time: 1403636935.801666, t: 9.474444 -6.744613 0.296157 q: 0.357282 0.626114 -0.529208 0.447515 
time: 1403636935.901666, t: 9.425660 -6.712962 0.283220 q: 0.353101 0.632095 -0.531060 0.440172 
time: 1403636936.001667, t: 9.378843 -6.689722 0.261181 q: 0.343825 0.638903 -0.536722 0.430716 
time: 1403636936.101666, t: 9.330276 -6.668698 0.239056 q: 0.339256 0.642893 -0.539408 0.425010 
time: 1403636936.201667, t: 9.284943 -6.656157 0.217273 q: 0.342170 0.643382 -0.537843 0.423915 
time: 1403636936.301666, t: 9.241788 -6.651486 0.200651 q: 0.348512 0.640388 -0.534256 0.427800 
time: 1403636936.401666, t: 9.194602 -6.647151 0.190441 q: 0.354604 0.635503 -0.531314 0.433702 
time: 1403636936.501667, t: 9.154670 -6.649671 0.189352 q: 0.360235 0.630276 -0.528600 0.439961 
time: 1403636936.601666, t: 9.117471 -6.656251 0.193016 q: 0.363399 0.626764 -0.528376 0.442637 
time: 1403636936.701667, t: 9.080408 -6.662713 0.191927 q: 0.364466 0.625031 -0.529815 0.442489 
time: 1403636936.801666, t: 9.036766 -6.664845 0.184278 q: 0.366659 0.624446 -0.529966 0.441320 
time: 1403636936.901666, t: 9.000667 -6.671841 0.177341 q: 0.369408 0.625214 -0.529561 0.438418 
time: 1403636937.001667, t: 8.959963 -6.675388 0.175197 q: 0.371457 0.626010 -0.529024 0.436194 
time: 1403636937.101666, t: 8.923558 -6.682274 0.180738 q: 0.371845 0.625992 -0.529765 0.434987 
time: 1403636937.201667, t: 8.887159 -6.687150 0.191933 q: 0.369189 0.623538 -0.532654 0.437242 
time: 1403636937.301666, t: 8.851648 -6.694100 0.200645 q: 0.368605 0.622788 -0.533947 0.437226 
time: 1403636937.401666, t: 8.816714 -6.699527 0.204286 q: 0.369147 0.621778 -0.534514 0.437512 
time: 1403636937.501667, t: 8.782265 -6.705013 0.201464 q: 0.369542 0.621152 -0.535050 0.437414 
time: 1403636937.601666, t: 8.749115 -6.712625 0.191547 q: 0.371120 0.620691 -0.534994 0.436799 
time: 1403636937.701667, t: 8.716599 -6.717303 0.178071 q: 0.376544 0.622987 -0.532317 0.432134 
time: 1403636937.801666, t: 8.686583 -6.722193 0.159213 q: 0.382780 0.624978 -0.528796 0.428085 
time: 1403636938.001667, t: 8.639413 -6.729339 0.108230 q: 0.398991 0.617944 -0.519876 0.434374 
time: 1403636938.101666, t: 8.621194 -6.726227 0.076765 q: 0.404815 0.612055 -0.518457 0.438994 
time: 1403636938.201667, t: 8.608158 -6.717350 0.043868 q: 0.404554 0.603256 -0.524174 0.444589 
time: 1403636938.301666, t: 8.599452 -6.698722 0.014215 q: 0.399795 0.594558 -0.532576 0.450585 
time: 1403636938.401666, t: 8.588571 -6.673436 -0.009124 q: 0.383698 0.585274 -0.547777 0.458444 
time: 1403636938.501667, t: 8.571020 -6.641036 -0.027757 q: 0.368526 0.580013 -0.561692 0.460734 
time: 1403636938.601666, t: 8.546618 -6.606781 -0.036819 q: 0.361908 0.580454 -0.569170 0.456225 
time: 1403636938.701667, t: 8.512221 -6.573712 -0.038513 q: 0.360633 0.580746 -0.571894 0.453448 
time: 1403636938.801666, t: 8.469572 -6.541592 -0.034238 q: 0.364963 0.583562 -0.570607 0.447956 
time: 1403636938.901666, t: 8.419728 -6.510920 -0.028017 q: 0.372409 0.590127 -0.566341 0.438543 
time: 1403636939.001667, t: 8.367437 -6.476836 -0.022251 q: 0.377893 0.596468 -0.563113 0.429332 
time: 1403636939.101666, t: 8.311905 -6.443963 -0.021398 q: 0.380371 0.599166 -0.560994 0.426150 
time: 1403636939.201667, t: 8.255660 -6.413937 -0.026171 q: 0.382225 0.598694 -0.558413 0.428537 
time: 1403636939.301666, t: 8.201036 -6.387891 -0.035995 q: 0.382996 0.598253 -0.556344 0.431148 
time: 1403636939.401666, t: 8.142241 -6.359855 -0.047728 q: 0.382831 0.599340 -0.553910 0.432916 
time: 1403636939.501667, t: 8.086512 -6.332429 -0.054148 q: 0.384484 0.599409 -0.550035 0.436282 
time: 1403636939.601666, t: 8.030814 -6.303128 -0.051821 q: 0.389026 0.597706 -0.544423 0.441600 
time: 1403636939.801666, t: 7.914477 -6.233976 -0.036653 q: 0.391245 0.594282 -0.540805 0.448649 
time: 1403636939.901666, t: 7.854710 -6.190739 -0.036279 q: 0.388679 0.594555 -0.542774 0.448141 
time: 1403636940.001667, t: 7.794856 -6.145485 -0.043431 q: 0.387311 0.590994 -0.548483 0.447083 
time: 1403636940.101666, t: 7.731409 -6.095256 -0.056531 q: 0.388253 0.587534 -0.555499 0.442136 
time: 1403636940.201667, t: 7.665664 -6.041767 -0.074363 q: 0.386647 0.584338 -0.563101 0.438143 
time: 1403636940.301666, t: 7.597908 -5.987635 -0.095267 q: 0.387592 0.581217 -0.567587 0.435665 
time: 1403636940.401666, t: 7.528462 -5.933018 -0.118959 q: 0.389600 0.577874 -0.570605 0.434378 
time: 1403636940.501667, t: 7.457270 -5.876322 -0.142733 q: 0.390957 0.575485 -0.573285 0.432798 
time: 1403636940.601666, t: 7.384073 -5.819075 -0.160764 q: -0.388226 -0.576733 0.578158 -0.427074 
time: 1403636940.701667, t: 7.308293 -5.763337 -0.171950 q: 0.387837 0.580895 -0.580665 0.418297 
time: 1403636940.801666, t: 7.232209 -5.709393 -0.174325 q: 0.386794 0.584597 -0.582461 0.411553 
time: 1403636940.901666, t: 7.157147 -5.658312 -0.168119 q: 0.387677 0.587527 -0.582393 0.406617 
time: 1403636941.001667, t: 7.082600 -5.611265 -0.154770 q: 0.389711 0.587514 -0.580900 0.406827 
time: 1403636941.101666, t: 7.009103 -5.569187 -0.135591 q: 0.392801 0.586384 -0.578600 0.408758 
time: 1403636941.201667, t: 6.937269 -5.528438 -0.113156 q: 0.394676 0.585415 -0.576499 0.411303 
time: 1403636941.301666, t: 6.865585 -5.488908 -0.091722 q: 0.396894 0.583773 -0.573517 0.415647 
time: 1403636941.401666, t: 6.795951 -5.451405 -0.075553 q: 0.398201 0.584314 -0.570123 0.418297 
time: 1403636941.501667, t: 6.726582 -5.413015 -0.065154 q: 0.398569 0.585312 -0.566989 0.420804 
time: 1403636941.601666, t: 6.657912 -5.373532 -0.059638 q: 0.397556 0.587753 -0.565052 0.420965 
time: 1403636941.701667, t: 6.591515 -5.335410 -0.059809 q: 0.396449 0.589837 -0.563701 0.420906 
time: 1403636941.801666, t: 6.526555 -5.295079 -0.059764 q: 0.394623 0.591321 -0.563109 0.421331 
time: 1403636941.901666, t: 6.463577 -5.258207 -0.054880 q: 0.391985 0.592797 -0.563530 0.421158 
time: 1403636942.001667, t: 6.401021 -5.217911 -0.042076 q: 0.389071 0.593064 -0.564614 0.422030 
time: 1403636942.101666, t: 6.338621 -5.180190 -0.026829 q: 0.387125 0.591759 -0.565664 0.424241 
time: 1403636942.201667, t: 6.274925 -5.139582 -0.010174 q: 0.386435 0.589894 -0.566321 0.426583 
time: 1403636942.301666, t: 6.211875 -5.101602 -0.001003 q: 0.385313 0.587742 -0.568045 0.428274 
time: 1403636942.401666, t: 6.147757 -5.061530 0.003351 q: 0.383341 0.584455 -0.570735 0.430956 
time: 1403636942.501667, t: 6.084337 -5.022837 0.000298 q: 0.381095 0.582420 -0.574221 0.431073 
time: 1403636942.601666, t: 6.016890 -4.983390 -0.007820 q: 0.378807 0.582784 -0.577556 0.428131 
time: 1403636942.701667, t: 5.944405 -4.943642 -0.019557 q: 0.378780 0.585572 -0.579123 0.422194 
time: 1403636942.801666, t: 5.871013 -4.905202 -0.032968 q: 0.380551 0.587104 -0.579311 0.418198 
time: 1403636943.001667, t: 5.723932 -4.837531 -0.037752 q: 0.391777 0.584959 -0.574334 0.417702 
time: 1403636943.101666, t: 5.650152 -4.804707 -0.027555 q: 0.396530 0.583215 -0.572322 0.418417 
time: 1403636943.201667, t: 5.576226 -4.772512 -0.015662 q: 0.398248 0.580556 -0.572502 0.420231 
time: 1403636943.301666, t: 5.504232 -4.739913 -0.009430 q: 0.400365 0.579673 -0.572685 0.419190 
time: 1403636943.401666, t: 5.433097 -4.706018 -0.008969 q: 0.402886 0.578004 -0.572995 0.418654 
time: 1403636943.501667, t: 5.363723 -4.670505 -0.014282 q: 0.404782 0.577025 -0.573294 0.417764 
time: 1403636943.601666, t: 5.295077 -4.634060 -0.025497 q: 0.407180 0.576452 -0.572922 0.416734 
time: 1403636943.701667, t: 5.226260 -4.596370 -0.041329 q: 0.409137 0.575554 -0.572409 0.416764 
time: 1403636943.801666, t: 5.160620 -4.556415 -0.063633 q: 0.410403 0.575774 -0.572370 0.415268 
time: 1403636944.001667, t: 5.034286 -4.471450 -0.126363 q: 0.390098 0.592052 -0.573757 0.410000 
time: 1403636944.101666, t: 4.974038 -4.431513 -0.159901 q: 0.376572 0.604394 -0.575378 0.402295 
time: 1403636944.201667, t: 4.915994 -4.397755 -0.185455 q: 0.368919 0.612797 -0.574371 0.398091 
time: 1403636944.301666, t: 4.860663 -4.373410 -0.201365 q: 0.366045 0.615623 -0.571765 0.400131 
time: 1403636944.401666, t: 4.808137 -4.358141 -0.207960 q: 0.368939 0.613649 -0.566492 0.407928 
time: 1403636944.501667, t: 4.757248 -4.346148 -0.206884 q: 0.379339 0.611485 -0.556843 0.414866 
time: 1403636944.601666, t: 4.710407 -4.336875 -0.199924 q: 0.393291 0.611207 -0.545155 0.417797 
time: 1403636944.701667, t: 4.670466 -4.329199 -0.188584 q: 0.401109 0.614312 -0.535521 0.418269 
time: 1403636944.801666, t: 4.636780 -4.318978 -0.174662 q: 0.395872 0.614485 -0.533849 0.425087 
time: 1403636944.901666, t: 4.604581 -4.304410 -0.154425 q: 0.390066 0.614229 -0.533504 0.431212 
time: 1403636945.001667, t: 4.580096 -4.290805 -0.135013 q: 0.386631 0.614013 -0.532845 0.435408 
time: 1403636945.101666, t: 4.553286 -4.271805 -0.127013 q: 0.385484 0.614742 -0.531181 0.437425 
time: 1403636945.201667, t: 4.533561 -4.251171 -0.130274 q: 0.384620 0.615732 -0.530177 0.438012 
time: 1403636945.301666, t: 4.512697 -4.226529 -0.136199 q: 0.383972 0.615993 -0.529627 0.438878 
time: 1403636945.401666, t: 4.495363 -4.200401 -0.135394 q: 0.384228 0.616620 -0.529453 0.437982 
time: 1403636945.501667, t: 4.480798 -4.172290 -0.128678 q: 0.384571 0.615955 -0.530053 0.437891 
time: 1403636945.601666, t: 4.468550 -4.142516 -0.124977 q: 0.380862 0.617025 -0.534311 0.434438 
time: 1403636945.701667, t: 4.457942 -4.112607 -0.128899 q: 0.374011 0.622855 -0.541010 0.423645 
time: 1403636945.801666, t: 4.450662 -4.084419 -0.139506 q: 0.367501 0.629262 -0.547028 0.411987 
time: 1403636945.901666, t: 4.446873 -4.058974 -0.153087 q: 0.365238 0.629669 -0.550410 0.408861 
time: 1403636946.001667, t: 4.444788 -4.038769 -0.167938 q: 0.373123 0.624435 -0.546279 0.415258 
time: 1403636946.101666, t: 4.446180 -4.022551 -0.188242 q: 0.379862 0.619247 -0.543175 0.420949 
time: 1403636946.201667, t: 4.448594 -4.006933 -0.215759 q: 0.382083 0.616053 -0.543480 0.423226 
time: 1403636946.301666, t: 4.454490 -3.990624 -0.248377 q: 0.386865 0.612879 -0.541714 0.425747 
time: 1403636946.401666, t: 4.461682 -3.975673 -0.288446 q: 0.389996 0.609592 -0.541388 0.428018 
time: 1403636946.501667, t: 4.470877 -3.958632 -0.334179 q: 0.390998 0.607624 -0.542759 0.428167 
time: 1403636946.701667, t: 4.493944 -3.916924 -0.421241 q: 0.386414 0.603587 -0.549314 0.429675 
time: 1403636946.801666, t: 4.506842 -3.892655 -0.456602 q: 0.371936 0.595468 -0.560495 0.439235 
time: 1403636946.901666, t: 4.516159 -3.869478 -0.486872 q: 0.362301 0.590702 -0.567882 0.444206 
time: 1403636947.001667, t: 4.517740 -3.846821 -0.519535 q: 0.350349 0.584643 -0.576163 0.451092 
time: 1403636947.101666, t: 4.510865 -3.827687 -0.554615 q: 0.347942 0.585072 -0.578121 0.449892 
time: 1403636947.201667, t: 4.494157 -3.810023 -0.586363 q: 0.355661 0.596414 -0.573724 0.434322 
time: 1403636947.301666, t: 4.472944 -3.793321 -0.612900 q: 0.360554 0.611523 -0.570907 0.412437 
time: 1403636947.401666, t: 4.451281 -3.785152 -0.630374 q: 0.359798 0.621143 -0.571280 0.397954 
time: 1403636947.501667, t: 4.430313 -3.786901 -0.639308 q: 0.359747 0.621312 -0.571823 0.396952 
time: 1403636947.601666, t: 4.411472 -3.798056 -0.640047 q: 0.363543 0.614125 -0.570302 0.406747 
time: 1403636947.701667, t: 4.392778 -3.816442 -0.634138 q: 0.370265 0.611998 -0.565891 0.410035 
time: 1403636947.801666, t: 4.375201 -3.839827 -0.630888 q: 0.377206 0.614500 -0.560616 0.407203 
time: 1403636947.901666, t: 4.360998 -3.868524 -0.632493 q: 0.387073 0.619045 -0.552882 0.401596 
time: 1403636948.001667, t: 4.349829 -3.896593 -0.640560 q: 0.387428 0.620370 -0.551713 0.400816 
time: 1403636948.101666, t: 4.345412 -3.929209 -0.654144 q: 0.379889 0.621213 -0.555997 0.400807 
time: 1403636948.201667, t: 4.343431 -3.962089 -0.670150 q: 0.380601 0.622955 -0.553977 0.400224 
time: 1403636948.301666, t: 4.346640 -3.999046 -0.688842 q: 0.382543 0.624249 -0.550932 0.400560 
time: 1403636948.401666, t: 4.354803 -4.039465 -0.710561 q: 0.385088 0.626333 -0.547103 0.400116 
time: 1403636948.501667, t: 4.368591 -4.081023 -0.733902 q: 0.391573 0.630121 -0.540044 0.397455 
time: 1403636948.601666, t: 4.389270 -4.124509 -0.758187 q: 0.404276 0.634794 -0.528400 0.392925 
time: 1403636948.701667, t: 4.421885 -4.166404 -0.779056 q: 0.410959 0.635182 -0.521419 0.394689 
time: 1403636948.801666, t: 4.466281 -4.207398 -0.793486 q: 0.402090 0.628248 -0.526575 0.407857 
time: 1403636948.901666, t: 4.518708 -4.248508 -0.800072 q: 0.391732 0.619578 -0.532439 0.423294 
time: 1403636949.001667, t: 4.574994 -4.286358 -0.802982 q: 0.381783 0.609832 -0.537626 0.439666 
time: 1403636949.101666, t: 4.629642 -4.323628 -0.809829 q: 0.379379 0.607351 -0.537636 0.445133 
time: 1403636949.201667, t: 4.682139 -4.357781 -0.821750 q: 0.380422 0.609068 -0.535639 0.444305 
time: 1403636949.301666, t: 4.732991 -4.387426 -0.838659 q: 0.380738 0.610781 -0.534591 0.442941 
time: 1403636949.401666, t: 4.782614 -4.414484 -0.859359 q: 0.377549 0.609014 -0.535792 0.446638 
time: 1403636949.501667, t: 4.830255 -4.437188 -0.884218 q: 0.374991 0.606886 -0.537090 0.450117 
time: 1403636949.601666, t: 4.875181 -4.458803 -0.906547 q: 0.371393 0.605166 -0.538905 0.453236 
time: 1403636949.701667, t: 4.916962 -4.476342 -0.921467 q: 0.368027 0.608066 -0.541422 0.449081 
time: 1403636949.801666, t: 4.952555 -4.493235 -0.931007 q: 0.366041 0.613831 -0.542556 0.441427 
time: 1403636949.901666, t: 4.986907 -4.507456 -0.935115 q: 0.360860 0.618278 -0.546309 0.434809 
time: 1403636950.001667, t: 5.019291 -4.524040 -0.934425 q: 0.356540 0.620649 -0.549460 0.431008 
time: 1403636950.101666, t: 5.050117 -4.546194 -0.930154 q: 0.352681 0.622299 -0.552617 0.427755 
time: 1403636950.201667, t: 5.078485 -4.572331 -0.928505 q: 0.354917 0.624440 -0.551820 0.423797 
time: 1403636950.301666, t: 5.105464 -4.600998 -0.931093 q: 0.359551 0.625210 -0.549127 0.422250 
time: 1403636950.401666, t: 5.133179 -4.635602 -0.938320 q: 0.366341 0.624352 -0.544845 0.423229 
time: 1403636950.501667, t: 5.161233 -4.671419 -0.949020 q: 0.375702 0.623583 -0.538563 0.424196 
time: 1403636950.601666, t: 5.189898 -4.704819 -0.959736 q: 0.385223 0.624303 -0.531554 0.423437 
time: 1403636950.701667, t: 5.222693 -4.738582 -0.966698 q: 0.386549 0.623081 -0.530076 0.425876 
time: 1403636950.801666, t: 5.259982 -4.773334 -0.969283 q: 0.383897 0.619082 -0.531196 0.432656 
time: 1403636950.901666, t: 5.298280 -4.804709 -0.964313 q: 0.380727 0.616985 -0.532383 0.436972 
time: 1403636951.001667, t: 5.339675 -4.836778 -0.954929 q: 0.379201 0.616069 -0.532601 0.439320 
time: 1403636951.101666, t: 5.382054 -4.866725 -0.938824 q: 0.373294 0.613202 -0.536324 0.443837 
time: 1403636951.201667, t: 5.422519 -4.892671 -0.925293 q: 0.364875 0.614415 -0.542217 0.441997 
time: 1403636951.401666, t: 5.492405 -4.947779 -0.919198 q: 0.357838 0.625573 -0.547546 0.425211 
time: 1403636951.501667, t: 5.528153 -4.984803 -0.927788 q: 0.357696 0.628427 -0.547974 0.420545 
time: 1403636951.601666, t: 5.563532 -5.024489 -0.939410 q: 0.359847 0.629859 -0.546774 0.418121 
time: 1403636951.701667, t: 5.597118 -5.063772 -0.946743 q: 0.364469 0.629130 -0.543993 0.418843 
time: 1403636951.801666, t: 5.634579 -5.114057 -0.951793 q: 0.369250 0.626470 -0.540880 0.422657 
time: 1403636951.901666, t: 5.670926 -5.162626 -0.947821 q: 0.376044 0.625994 -0.535988 0.423604 
time: 1403636952.001667, t: 5.710501 -5.216347 -0.944044 q: 0.381713 0.627011 -0.531932 0.422138 
time: 1403636952.101666, t: 5.756635 -5.270075 -0.943887 q: 0.383893 0.625439 -0.530664 0.424085 
time: 1403636952.201667, t: 5.803155 -5.318023 -0.946416 q: 0.382957 0.621656 -0.531376 0.429567 
time: 1403636952.301666, t: 5.853346 -5.367300 -0.957383 q: 0.379425 0.616074 -0.533583 0.437926 
time: 1403636952.401666, t: 5.904112 -5.414422 -0.974517 q: 0.376663 0.611233 -0.535355 0.444876 
time: 1403636952.501667, t: 5.948652 -5.455291 -0.995239 q: 0.379182 0.611184 -0.532964 0.445673 
time: 1403636952.601666, t: 5.993197 -5.494459 -1.018646 q: 0.382476 0.613147 -0.530071 0.443607 
wait for imu ... 
time: 1403636952.701667, t: 6.037548 -5.528337 -1.036948 q: 0.383879 0.613969 -0.528710 0.442883 
time: 1403636952.801666, t: 6.083315 -5.559456 -1.052246 q: 0.380771 0.617513 -0.530885 0.438009 
time: 1403636952.901666, t: 6.128160 -5.587157 -1.064539 q: 0.375254 0.620147 -0.534983 0.434046 
time: 1403636953.001667, t: 6.175860 -5.617500 -1.080694 q: 0.365744 0.620364 -0.542367 0.432688 
time: 1403636953.101666, t: 6.219977 -5.646949 -1.101885 q: 0.357938 0.618138 -0.548216 0.435024 
time: 1403636953.201667, t: 6.267167 -5.683307 -1.129141 q: 0.355160 0.617615 -0.550500 0.435159 
time: 1403636953.301666, t: 6.304996 -5.716195 -1.156566 q: 0.355598 0.618445 -0.549964 0.434298 
time: 1403636953.401666, t: 6.340918 -5.755264 -1.177420 q: 0.359559 0.619765 -0.546678 0.433303 
time: 1403636953.501667, t: 6.373899 -5.793222 -1.191698 q: 0.366784 0.621576 -0.540942 0.431851 
time: 1403636953.601666, t: 6.404778 -5.832603 -1.202025 q: 0.370981 0.623643 -0.537143 0.430024 
time: 1403636953.701667, t: 6.438291 -5.873696 -1.204315 q: 0.372425 0.629221 -0.534747 0.423588 
time: 1403636953.801666, t: 6.470782 -5.913065 -1.196843 q: 0.375849 0.634360 -0.530909 0.417685 
time: 1403636953.901666, t: 6.514083 -5.958317 -1.182641 q: 0.376288 0.635072 -0.529156 0.418431 
time: 1403636954.001667, t: 6.560829 -6.004720 -1.167057 q: 0.372984 0.633567 -0.530719 0.421679 
time: 1403636954.201667, t: 6.660201 -6.104316 -1.144718 q: 0.370838 0.631425 -0.531793 0.425414 
time: 1403636954.301666, t: 6.713566 -6.153343 -1.137565 q: 0.369530 0.632335 -0.532962 0.423736 
time: 1403636954.401666, t: 6.772735 -6.210375 -1.129386 q: 0.367280 0.633151 -0.535390 0.421406 
time: 1403636954.501667, t: 6.832532 -6.263246 -1.112564 q: 0.365860 0.632594 -0.537972 0.420187 
time: 1403636954.601666, t: 6.894892 -6.323259 -1.095432 q: 0.368400 0.632393 -0.538285 0.417863 
time: 1403636954.701667, t: 6.958186 -6.380177 -1.087583 q: 0.372554 0.629182 -0.538425 0.418847 
time: 1403636954.801666, t: 7.025854 -6.445085 -1.099365 q: 0.377168 0.623374 -0.538449 0.423346 
time: 1403636954.901666, t: 7.090632 -6.504021 -1.116578 q: 0.381260 0.617666 -0.539105 0.427196 
time: 1403636955.001667, t: 7.161058 -6.566096 -1.144245 q: 0.385189 0.613360 -0.539990 0.428754 
time: 1403636955.101666, t: 7.225013 -6.618828 -1.170458 q: 0.387911 0.612412 -0.541807 0.425349 
time: 1403636955.201667, t: 7.294557 -6.673531 -1.188405 q: 0.390987 0.611329 -0.543547 0.421855 
time: 1403636955.301666, t: 7.367067 -6.728580 -1.199966 q: 0.392940 0.607899 -0.546365 0.421358 
time: 1403636955.401666, t: 7.440144 -6.780057 -1.203894 q: 0.395615 0.605305 -0.548476 0.419843 
time: 1403636955.501667, t: 7.516095 -6.830964 -1.208321 q: 0.397581 0.604065 -0.550151 0.417576 
time: 1403636955.601666, t: 7.594421 -6.880557 -1.217719 q: 0.398728 0.602514 -0.551749 0.416613 
time: 1403636955.701667, t: 7.672465 -6.927786 -1.232377 q: 0.399837 0.601601 -0.552899 0.415343 
time: 1403636955.801666, t: 7.753086 -6.975042 -1.242734 q: 0.400655 0.601171 -0.553545 0.414315 
time: 1403636955.901666, t: 7.830702 -7.017313 -1.244480 q: 0.397380 0.598690 -0.556845 0.416633 
time: 1403636956.001667, t: 7.911840 -7.061346 -1.240020 q: 0.389826 0.592829 -0.563516 0.423130 
time: 1403636956.101666, t: 7.990177 -7.103610 -1.226199 q: 0.381984 0.585449 -0.570296 0.431394 
time: 1403636956.201667, t: 8.058953 -7.140845 -1.211321 q: 0.377955 0.581819 -0.574375 0.434431 
time: 1403636956.301666, t: 8.124674 -7.178255 -1.202661 q: 0.379788 0.582266 -0.574659 0.431851 
time: 1403636956.401666, t: 8.183511 -7.213720 -1.202256 q: 0.389568 0.588009 -0.569153 0.422547 
time: 1403636956.501667, t: 8.242557 -7.248655 -1.206014 q: 0.397625 0.592249 -0.562790 0.417616 
time: 1403636956.601666, t: 8.302638 -7.281878 -1.214617 q: 0.397596 0.592972 -0.555271 0.426587 
time: 1403636956.701667, t: 8.359635 -7.309970 -1.228067 q: 0.385637 0.594295 -0.549602 0.442758 
time: 1403636956.801666, t: 8.416333 -7.335105 -1.242500 q: 0.367138 0.602476 -0.542173 0.456378 
time: 1403636956.901666, t: 8.473532 -7.360080 -1.250741 q: 0.349681 0.618053 -0.528405 0.465319 
time: 1403636957.001667, t: 8.522493 -7.382853 -1.249856 q: 0.339633 0.636787 -0.506428 0.471893 
time: 1403636957.101666, t: 8.566798 -7.413079 -1.247143 q: 0.331301 0.656540 -0.480651 0.477670 
time: 1403636957.201667, t: 8.613273 -7.425143 -1.227400 q: 0.322096 0.674428 -0.457390 0.481866 
time: 1403636957.301666, t: 8.655460 -7.442888 -1.211292 q: 0.311609 0.686271 -0.441181 0.487126 
time: 1403636957.401666, t: 8.695300 -7.460058 -1.200660 q: 0.302790 0.693938 -0.429974 0.491824 
time: 1403636957.501667, t: 8.734305 -7.474840 -1.197913 q: 0.296236 0.698044 -0.423399 0.495693 
time: 1403636957.601666, t: 8.772991 -7.488424 -1.202391 q: 0.289490 0.701020 -0.418131 0.499934 
time: 1403636957.701667, t: 8.810659 -7.501631 -1.215434 q: 0.281094 0.705479 -0.410191 0.505003 
time: 1403636957.801666, t: 8.846991 -7.511709 -1.233042 q: 0.267161 0.713310 -0.398286 0.511060 
time: 1403636957.901666, t: 8.882686 -7.520495 -1.254000 q: 0.249255 0.724136 -0.379455 0.519147 
time: 1403636958.001667, t: 8.916370 -7.529406 -1.272600 q: 0.230369 0.736485 -0.355212 0.527583 
time: 1403636958.101666, t: 8.946857 -7.538348 -1.283474 q: 0.212259 0.748761 -0.329862 0.534317 
time: 1403636958.201667, t: 8.975837 -7.548439 -1.285491 q: 0.197007 0.758719 -0.308640 0.538771 
time: 1403636958.301666, t: 9.000241 -7.560700 -1.275626 q: 0.184998 0.765844 -0.288852 0.543897 
time: 1403636958.401666, t: 9.020926 -7.572221 -1.259403 q: 0.176838 0.773509 -0.270732 0.545084 
time: 1403636958.501667, t: 9.039477 -7.585943 -1.239417 q: 0.171400 0.778653 -0.258970 0.545211 
time: 1403636958.601666, t: 9.058556 -7.601241 -1.225028 q: 0.168716 0.780955 -0.252330 0.545870 
time: 1403636958.701667, t: 9.077968 -7.618359 -1.214444 q: 0.168695 0.781834 -0.248741 0.546265 
time: 1403636958.801666, t: 9.097572 -7.635876 -1.207358 q: 0.169156 0.782033 -0.248090 0.546134 
time: 1403636958.901666, t: 9.122461 -7.655467 -1.206837 q: 0.171197 0.782609 -0.249151 0.544187 
time: 1403636959.001667, t: 9.142997 -7.673077 -1.210348 q: 0.174104 0.782217 -0.250500 0.543208 
time: 1403636959.101666, t: 9.166248 -7.689738 -1.220013 q: 0.175139 0.782308 -0.253425 0.541384 
time: 1403636959.201667, t: 9.190524 -7.706460 -1.234685 q: 0.176319 0.781657 -0.255859 0.540796 
time: 1403636959.301666, t: 9.220270 -7.724858 -1.255953 q: 0.182273 0.781373 -0.255229 0.539528 
time: 1403636959.401666, t: 9.245574 -7.739277 -1.281815 q: 0.188402 0.781649 -0.253214 0.537971 
time: 1403636959.501667, t: 9.273875 -7.751169 -1.313187 q: 0.192624 0.779856 -0.251928 0.539679 
time: 1403636959.601666, t: 9.308088 -7.762039 -1.345204 q: 0.189129 0.774161 -0.255851 0.547216 
time: 1403636959.701667, t: 9.338053 -7.770099 -1.370358 q: 0.186976 0.773743 -0.258990 0.547071 
wait for imu ... 
time: 1403636959.801666, t: 9.367792 -7.776199 -1.386598 q: 0.185375 0.774794 -0.261660 0.544852 
time: 1403636959.901666, t: 9.396389 -7.779748 -1.394871 q: 0.184776 0.775615 -0.263772 0.542866 
time: 1403636960.001667, t: 9.424880 -7.783911 -1.395846 q: 0.184069 0.775858 -0.264879 0.542220 
time: 1403636960.101666, t: 9.454422 -7.788556 -1.391265 q: 0.184390 0.775573 -0.264945 0.542485 
time: 1403636960.201667, t: 9.482471 -7.791821 -1.382332 q: 0.187143 0.776305 -0.263134 0.541377 
time: 1403636960.301666, t: 9.511666 -7.793568 -1.378590 q: 0.195657 0.778448 -0.257189 0.538136 
time: 1403636960.401666, t: 9.544077 -7.793868 -1.380246 q: 0.200510 0.777792 -0.253334 0.539126 
time: 1403636960.501667, t: 9.576421 -7.789494 -1.385274 q: 0.201701 0.776144 -0.247085 0.543936 
time: 1403636960.601666, t: 9.614286 -7.781718 -1.396346 q: 0.201949 0.776019 -0.239007 0.547620 
time: 1403636960.701667, t: 9.648480 -7.767012 -1.407651 q: 0.194249 0.775573 -0.236978 0.551902 
time: 1403636960.801666, t: 9.686117 -7.748191 -1.425725 q: 0.179978 0.777530 -0.238889 0.553161 
time: 1403636960.901666, t: 9.721958 -7.725841 -1.445297 q: 0.165477 0.781828 -0.231678 0.554696 
time: 1403636961.001667, t: 9.760269 -7.701828 -1.461615 q: 0.151889 0.787261 -0.214401 0.557836 
time: 1403636961.101666, t: 9.792363 -7.676142 -1.471686 q: 0.135901 0.792371 -0.193514 0.562344 
time: 1403636961.201667, t: 9.823960 -7.649625 -1.476050 q: 0.118597 0.797624 -0.170875 0.566156 
time: 1403636961.301666, t: 9.853020 -7.623170 -1.471982 q: 0.103066 0.802348 -0.148318 0.568873 
time: 1403636961.401666, t: 9.883978 -7.596626 -1.461646 q: 0.091869 0.805038 -0.129436 0.571595 
wait for imu ... 
time: 1403636961.501667, t: 9.911087 -7.570852 -1.444198 q: 0.082040 0.807162 -0.116652 0.572845 
time: 1403636961.601666, t: 9.938897 -7.544711 -1.419696 q: 0.071885 0.808818 -0.103795 0.574346 
time: 1403636961.701667, t: 9.965562 -7.517956 -1.388176 q: 0.060787 0.809900 -0.089524 0.576500 
time: 1403636961.801666, t: 9.992216 -7.491065 -1.353717 q: 0.050451 0.811053 -0.073446 0.578146 
time: 1403636961.901666, t: 10.015594 -7.463068 -1.324163 q: 0.040616 0.812977 -0.055428 0.578228 
time: 1403636962.001667, t: 10.039320 -7.434707 -1.301674 q: 0.027360 0.813969 -0.037401 0.579058 
time: 1403636962.101666, t: 10.061959 -7.405887 -1.284441 q: 0.014173 0.815633 -0.018409 0.578102 
time: 1403636962.201667, t: 10.083206 -7.375795 -1.265594 q: 0.000327 0.815837 0.001179 0.578281 
time: 1403636962.301666, t: 10.104394 -7.344911 -1.240729 q: -0.012298 0.814591 0.022925 0.579453 
time: 1403636962.401666, t: 10.121158 -7.313228 -1.210550 q: -0.027003 0.813317 0.044214 0.579510 
time: 1403636962.501667, t: 10.136913 -7.280351 -1.173449 q: -0.038591 0.811711 0.067683 0.578839 
time: 1403636962.601666, t: 10.151132 -7.245992 -1.133041 q: -0.047625 0.809584 0.093819 0.577498 
time: 1403636962.701667, t: 10.161939 -7.210367 -1.096208 q: -0.053308 0.806497 0.118226 0.576839 
time: 1403636962.801666, t: 10.168932 -7.172357 -1.065767 q: -0.058422 0.803956 0.134963 0.576218 
time: 1403636962.901666, t: 10.173377 -7.129136 -1.042257 q: -0.065242 0.801980 0.142582 0.576404 
time: 1403636963.001667, t: 10.174735 -7.082458 -1.023774 q: -0.076089 0.798555 0.141509 0.580083 
time: 1403636963.101666, t: 10.173983 -7.032585 -1.009366 q: -0.083332 0.793204 0.138401 0.587136 
time: 1403636963.201667, t: 10.169121 -6.982329 -1.000793 q: -0.085219 0.792979 0.135513 0.587843 
time: 1403636963.301666, t: 10.158680 -6.932389 -0.998433 q: -0.088960 0.797610 0.129746 0.582297 
time: 1403636963.401666, t: 10.144402 -6.883113 -1.000229 q: -0.091341 0.801068 0.123469 0.578534 
time: 1403636963.501667, t: 10.129493 -6.834097 -1.003595 q: -0.087170 0.802628 0.121507 0.577430 
time: 1403636963.601666, t: 10.109085 -6.788503 -1.003295 q: -0.082996 0.803127 0.119734 0.577722 
time: 1403636963.701667, t: 10.090050 -6.741440 -0.996675 q: -0.078542 0.803866 0.117575 0.577760 
time: 1403636963.801666, t: 10.070532 -6.694719 -0.981439 q: -0.075755 0.804553 0.115099 0.577675 
time: 1403636963.901666, t: 10.047871 -6.648073 -0.960313 q: -0.073379 0.804120 0.113359 0.578927 
time: 1403636964.001667, t: 10.022693 -6.600754 -0.932316 q: -0.072473 0.803927 0.112015 0.579570 
time: 1403636964.101666, t: 9.996865 -6.554006 -0.898884 q: -0.071605 0.803616 0.111992 0.580114 
time: 1403636964.201667, t: 9.966700 -6.507329 -0.866131 q: -0.071183 0.802104 0.112635 0.582130 
time: 1403636964.301666, t: 9.934644 -6.460061 -0.840429 q: -0.069837 0.802222 0.114528 0.581761 
time: 1403636964.401666, t: 9.898000 -6.411675 -0.820318 q: -0.069516 0.801576 0.116143 0.582369 
time: 1403636964.501667, t: 9.860724 -6.361879 -0.805809 q: -0.068302 0.800903 0.119529 0.582754 
time: 1403636964.601666, t: 9.819936 -6.310991 -0.795718 q: -0.065979 0.799118 0.128787 0.583498 
time: 1403636964.701667, t: 9.775269 -6.254727 -0.792414 q: -0.068684 0.797386 0.141247 0.582673 
time: 1403636964.801666, t: 9.726751 -6.196734 -0.793976 q: -0.073249 0.793989 0.157982 0.582458 
time: 1403636964.901666, t: 9.672324 -6.132570 -0.794581 q: -0.090252 0.789206 0.172700 0.582394 
time: 1403636965.001667, t: 9.613461 -6.065944 -0.788684 q: -0.106615 0.782732 0.191704 0.582421 
time: 1403636965.101666, t: 9.552067 -5.997473 -0.775690 q: -0.122803 0.779465 0.208860 0.577695 
time: 1403636965.201667, t: 9.485275 -5.925322 -0.756182 q: -0.134027 0.780294 0.222850 0.568785 
time: 1403636965.301666, t: 9.416400 -5.852901 -0.731639 q: -0.138499 0.787419 0.234649 0.552928 
time: 1403636965.401666, t: 9.346085 -5.776409 -0.702595 q: -0.138260 0.798439 0.243541 0.532980 
time: 1403636965.501667, t: 9.280554 -5.696142 -0.665093 q: -0.142414 0.806429 0.244483 0.519248 
time: 1403636965.601666, t: 9.224022 -5.609867 -0.614253 q: -0.154058 0.802309 0.237135 0.525674 
time: 1403636965.701667, t: 9.175695 -5.519987 -0.554916 q: -0.168960 0.792297 0.225901 0.541005 
time: 1403636965.801666, t: 9.130910 -5.431686 -0.500441 q: -0.174892 0.785369 0.219450 0.551770 
time: 1403636965.901666, t: 9.082963 -5.347338 -0.454346 q: -0.166235 0.780795 0.222600 0.559619 
time: 1403636966.001667, t: 9.030329 -5.264217 -0.416340 q: -0.157514 0.778536 0.226041 0.563895 
wait for imu ... 
time: 1403636966.101666, t: 8.977290 -5.183856 -0.385972 q: -0.152769 0.777097 0.227124 0.566742 
time: 1403636966.201667, t: 8.927297 -5.106154 -0.363448 q: -0.150025 0.776729 0.226834 0.568094 
time: 1403636966.301666, t: 8.872017 -5.031885 -0.349647 q: -0.149366 0.775960 0.225425 0.569876 
time: 1403636966.401666, t: 8.816922 -4.956230 -0.341913 q: -0.158987 0.777161 0.216977 0.568916 
time: 1403636966.501667, t: 8.757702 -4.887804 -0.342990 q: -0.174531 0.779352 0.204135 0.566107 
time: 1403636966.601666, t: 8.699067 -4.825321 -0.347693 q: -0.178210 0.779914 0.199147 0.565965 
time: 1403636966.701667, t: 8.642365 -4.769570 -0.358398 q: -0.167062 0.779703 0.203923 0.567951 
time: 1403636966.801666, t: 8.584206 -4.716336 -0.365448 q: -0.159019 0.778372 0.207093 0.570931 
time: 1403636966.901666, t: 8.524480 -4.667181 -0.365817 q: -0.159676 0.778899 0.204830 0.570846 
time: 1403636967.001667, t: 8.463531 -4.622790 -0.360711 q: -0.161899 0.779464 0.201963 0.570470 
time: 1403636967.101666, t: 8.400077 -4.580543 -0.357535 q: -0.156761 0.779015 0.204181 0.571727 
time: 1403636967.201667, t: 8.336838 -4.541715 -0.361929 q: -0.152197 0.778716 0.206148 0.572660 
time: 1403636967.301666, t: 8.270367 -4.505205 -0.370513 q: -0.150186 0.778807 0.206446 0.572961 
time: 1403636967.401666, t: 8.201971 -4.470211 -0.378493 q: -0.147515 0.777841 0.207392 0.574623 
time: 1403636967.501667, t: 8.131036 -4.435322 -0.380128 q: -0.147723 0.777682 0.206279 0.575184 
time: 1403636967.601666, t: 8.054630 -4.403267 -0.373054 q: -0.145529 0.777232 0.206454 0.576289 
time: 1403636967.701667, t: 7.974669 -4.368997 -0.368940 q: -0.143136 0.777549 0.206612 0.576404 
time: 1403636967.801666, t: 7.893453 -4.337958 -0.378844 q: -0.143251 0.777397 0.205919 0.576827 
time: 1403636967.901666, t: 7.810661 -4.307840 -0.404849 q: -0.143142 0.776759 0.205807 0.577753 
time: 1403636968.001667, t: 7.723458 -4.280229 -0.437972 q: -0.143517 0.776426 0.205779 0.578118 
time: 1403636968.101666, t: 7.635611 -4.253246 -0.475823 q: -0.145944 0.775792 0.205027 0.578628 
time: 1403636968.201667, t: 7.541667 -4.228210 -0.512850 q: -0.148525 0.776803 0.203432 0.577177 
time: 1403636968.301666, t: 7.446201 -4.205124 -0.541792 q: -0.148185 0.777670 0.203727 0.575991 
time: 1403636968.401666, t: 7.348253 -4.184526 -0.563264 q: -0.147656 0.778721 0.203999 0.574609 
time: 1403636968.501667, t: 7.244246 -4.165570 -0.576816 q: -0.148109 0.779692 0.203272 0.573431 
time: 1403636968.601666, t: 7.140158 -4.146765 -0.584922 q: -0.146256 0.779321 0.204296 0.574048 
time: 1403636968.701667, t: 7.033001 -4.129224 -0.584814 q: -0.140193 0.778640 0.208103 0.575116 
time: 1403636968.901666, t: 6.817633 -4.090069 -0.591530 q: -0.138385 0.778943 0.208454 0.575017 
time: 1403636969.001667, t: 6.707161 -4.065625 -0.589019 q: -0.140449 0.779762 0.206794 0.574005 
time: 1403636969.101666, t: 6.589494 -4.045883 -0.578366 q: -0.145077 0.780862 0.203572 0.572508 
time: 1403636969.201667, t: 6.470084 -4.024866 -0.566638 q: -0.145003 0.785769 0.203396 0.565836 
time: 1403636969.301666, t: 6.348677 -4.006458 -0.548258 q: -0.144159 0.792450 0.203474 0.556632 
time: 1403636969.401666, t: 6.235112 -3.989664 -0.521289 q: -0.140894 0.796626 0.204973 0.550928 
time: 1403636969.501667, t: 6.126922 -3.971896 -0.492396 q: -0.138413 0.799182 0.205621 0.547604 
time: 1403636969.601666, t: 6.019460 -3.951039 -0.468798 q: -0.136982 0.800894 0.205255 0.545596 
time: 1403636969.701667, t: 5.913951 -3.928671 -0.452429 q: -0.136595 0.800867 0.204488 0.546021 
time: 1403636969.801666, t: 5.812074 -3.904301 -0.438899 q: -0.135683 0.800976 0.204426 0.546112 
time: 1403636969.901666, t: 5.714131 -3.877975 -0.432289 q: -0.136077 0.801359 0.203623 0.545751 
time: 1403636970.001667, t: 5.621260 -3.851759 -0.429157 q: -0.138473 0.798887 0.202281 0.549260 
time: 1403636970.101666, t: 5.530450 -3.822331 -0.432992 q: -0.140025 0.792624 0.202054 0.557955 
time: 1403636970.201667, t: 5.441345 -3.793805 -0.439653 q: -0.141466 0.787944 0.201492 0.564387 
time: 1403636970.301666, t: 5.358911 -3.764933 -0.447015 q: -0.142113 0.786515 0.201747 0.566123 
time: 1403636970.501667, t: 5.188153 -3.706704 -0.461804 q: -0.140316 0.796656 0.203846 0.551451 
time: 1403636970.601666, t: 5.105701 -3.675524 -0.465533 q: -0.138865 0.801152 0.205715 0.544568 
time: 1403636970.701667, t: 5.024843 -3.641441 -0.460966 q: -0.137095 0.804755 0.208095 0.538767 
time: 1403636970.801666, t: 4.947384 -3.604876 -0.449194 q: -0.136325 0.808386 0.209725 0.532863 
time: 1403636970.901666, t: 4.880349 -3.565012 -0.438297 q: -0.134884 0.813301 0.211584 0.524957 
time: 1403636971.001667, t: 4.817261 -3.521161 -0.433368 q: -0.138684 0.815473 0.209481 0.521429 
time: 1403636971.101666, t: 4.764106 -3.473818 -0.433355 q: -0.148895 0.813326 0.202441 0.524736 
time: 1403636971.201667, t: 4.717558 -3.425422 -0.438183 q: -0.161730 0.808862 0.193708 0.531096 
time: 1403636971.301666, t: 4.676284 -3.379732 -0.448095 q: -0.174175 0.804405 0.184721 0.537098 
time: 1403636971.401666, t: 4.642111 -3.340008 -0.462881 q: -0.182377 0.802333 0.179307 0.539305 
time: 1403636971.501667, t: 4.615982 -3.307195 -0.482593 q: -0.179618 0.801288 0.181729 0.540971 
time: 1403636971.601666, t: 4.590652 -3.278530 -0.505261 q: -0.172525 0.797662 0.187432 0.546662 
time: 1403636971.701667, t: 4.573969 -3.256528 -0.532660 q: -0.162977 0.791522 0.195092 0.555761 
time: 1403636971.801666, t: 4.556780 -3.236659 -0.563888 q: -0.161111 0.786542 0.197396 0.562520 
time: 1403636971.901666, t: 4.539749 -3.220344 -0.595080 q: -0.161719 0.784218 0.198124 0.565329 
time: 1403636972.001667, t: 4.523112 -3.209033 -0.621543 q: -0.161750 0.784222 0.199114 0.564966 
time: 1403636972.101666, t: 4.505239 -3.200203 -0.638834 q: -0.160319 0.784032 0.200181 0.565260 
time: 1403636972.201667, t: 4.485265 -3.195609 -0.652712 q: -0.161618 0.787606 0.199078 0.560289 
time: 1403636972.301666, t: 4.464413 -3.194141 -0.656327 q: -0.163054 0.793319 0.197708 0.552240 
time: 1403636972.401666, t: 4.449914 -3.193373 -0.654419 q: -0.160018 0.795474 0.199341 0.549436 
time: 1403636972.501667, t: 4.437978 -3.197912 -0.636216 q: -0.156785 0.794314 0.201143 0.551385 
time: 1403636972.601666, t: 4.429666 -3.201212 -0.618224 q: -0.154441 0.792476 0.202037 0.554356 
time: 1403636972.801666, t: 4.411298 -3.211681 -0.583779 q: -0.150676 0.790206 0.203270 0.558169 
time: 1403636972.901666, t: 4.399987 -3.218174 -0.573244 q: -0.149332 0.790643 0.202796 0.558084 
time: 1403636973.001667, t: 4.390427 -3.224450 -0.572752 q: -0.143997 0.794125 0.197462 0.556452 
time: 1403636973.101666, t: 4.380739 -3.231972 -0.577590 q: -0.137441 0.798680 0.187656 0.554983 
time: 1403636973.201667, t: 4.370885 -3.240360 -0.585194 q: -0.131684 0.801542 0.179334 0.555004 
time: 1403636973.301666, t: 4.362618 -3.247203 -0.598801 q: -0.126164 0.802016 0.173489 0.557454 
time: 1403636973.401666, t: 4.355752 -3.252988 -0.617293 q: -0.121964 0.802664 0.169238 0.558761 
time: 1403636973.501667, t: 4.350483 -3.256862 -0.640406 q: -0.119663 0.803446 0.165691 0.559197 
time: 1403636973.601666, t: 4.345819 -3.260014 -0.666013 q: -0.117367 0.801862 0.163742 0.562522 
time: 1403636973.701667, t: 4.342957 -3.261493 -0.693476 q: -0.116272 0.800206 0.162647 0.565417 
time: 1403636973.801666, t: 4.338481 -3.262835 -0.714385 q: -0.115074 0.797184 0.162975 0.569820 
time: 1403636973.901666, t: 4.332824 -3.263385 -0.728907 q: -0.115121 0.796064 0.163357 0.571265 
time: 1403636974.001667, t: 4.327092 -3.263334 -0.735951 q: -0.117205 0.797812 0.161012 0.569064 
time: 1403636974.101666, t: 4.318742 -3.264688 -0.739395 q: -0.116019 0.802211 0.153172 0.565275 
time: 1403636974.201667, t: 4.311511 -3.268244 -0.743673 q: -0.106813 0.807562 0.139678 0.562960 
time: 1403636974.301666, t: 4.307661 -3.273401 -0.752054 q: -0.090197 0.811033 0.122339 0.564910 
time: 1403636974.401666, t: 4.306105 -3.277299 -0.767832 q: -0.072427 0.813248 0.100499 0.568579 
time: 1403636974.501667, t: 4.303098 -3.281382 -0.787481 q: -0.054464 0.813592 0.075375 0.573952 
time: 1403636974.601666, t: 4.298322 -3.286405 -0.810954 q: -0.035376 0.813421 0.049548 0.578481 
time: 1403636974.701667, t: 4.291987 -3.290276 -0.835508 q: -0.017973 0.815751 0.023623 0.577642 
time: 1403636974.801666, t: 4.282097 -3.294011 -0.853963 q: -0.004303 0.819359 -0.002019 0.573261 
time: 1403636974.901666, t: 4.271975 -3.297048 -0.864029 q: 0.003371 0.823429 -0.028708 0.566682 
time: 1403636975.001667, t: 4.264060 -3.300461 -0.866414 q: 0.011747 0.825822 -0.052491 0.561359 
time: 1403636975.101666, t: 4.259982 -3.307285 -0.861336 q: 0.024338 0.824044 -0.070295 0.561620 
time: 1403636975.201667, t: 4.261309 -3.315397 -0.855494 q: 0.038247 0.818900 -0.080087 0.567033 
time: 1403636975.301666, t: 4.266161 -3.323810 -0.855201 q: 0.049655 0.813394 -0.084494 0.573398 
time: 1403636975.401666, t: 4.271716 -3.331319 -0.861415 q: 0.056258 0.810355 -0.086443 0.576791 
time: 1403636975.501667, t: 4.277039 -3.337540 -0.874105 q: 0.058618 0.811445 -0.087338 0.574885 
time: 1403636975.601666, t: 4.279717 -3.343760 -0.885128 q: 0.059559 0.815479 -0.086456 0.569186 
time: 1403636975.701667, t: 4.288112 -3.347140 -0.888825 q: 0.058679 0.818677 -0.084347 0.564987 
wait for imu ... 
time: 1403636975.801666, t: 4.294403 -3.350960 -0.883167 q: 0.057238 0.820952 -0.082311 0.562127 
time: 1403636975.901666, t: 4.308682 -3.353741 -0.873419 q: 0.060786 0.822168 -0.086162 0.559394 
time: 1403636976.001667, t: 4.320565 -3.357934 -0.866771 q: 0.064981 0.819775 -0.098826 0.560339 
time: 1403636976.101666, t: 4.333539 -3.363235 -0.864666 q: 0.067965 0.812127 -0.119407 0.567073 
time: 1403636976.201667, t: 4.346860 -3.370284 -0.867205 q: 0.069629 0.803290 -0.145383 0.573359 
time: 1403636976.301666, t: 4.357092 -3.381144 -0.875208 q: 0.076792 0.795936 -0.171604 0.575449 
time: 1403636976.401666, t: 4.364148 -3.397684 -0.887720 q: 0.090087 0.789531 -0.196290 0.574452 
time: 1403636976.501667, t: 4.368830 -3.420025 -0.905013 q: 0.110392 0.785274 -0.215577 0.569811 
time: 1403636976.601666, t: 4.368859 -3.445603 -0.926982 q: 0.133353 0.783569 -0.225393 0.563414 
time: 1403636976.701667, t: 4.369299 -3.471015 -0.954876 q: 0.149957 0.782139 -0.231785 0.558612 
time: 1403636976.801666, t: 4.368256 -3.495707 -0.988510 q: 0.157237 0.781330 -0.239251 0.554580 
time: 1403636976.901666, t: 4.366772 -3.519849 -1.025061 q: 0.156657 0.783868 -0.246701 0.547857 
time: 1403636977.001667, t: 4.365858 -3.545053 -1.053148 q: 0.154226 0.784776 -0.251724 0.544955 
time: 1403636977.101666, t: 4.366182 -3.572059 -1.071076 q: 0.152041 0.785898 -0.253850 0.542962 
time: 1403636977.201667, t: 4.367729 -3.602440 -1.080537 q: 0.149071 0.787149 -0.254116 0.541848 
time: 1403636977.301666, t: 4.369471 -3.635382 -1.084547 q: 0.147438 0.787784 -0.252160 0.542285 
time: 1403636977.401666, t: 4.374331 -3.672926 -1.090223 q: 0.148112 0.785714 -0.247442 0.547256 
time: 1403636977.501667, t: 4.379106 -3.712350 -1.100275 q: 0.150717 0.782684 -0.241380 0.553558 
time: 1403636977.601666, t: 4.383088 -3.752589 -1.116219 q: 0.151246 0.779803 -0.237901 0.558959 
time: 1403636977.701667, t: 4.384080 -3.792539 -1.137469 q: 0.151849 0.778824 -0.237224 0.560446 
time: 1403636977.801666, t: 4.385632 -3.833036 -1.163610 q: 0.147924 0.779003 -0.244598 0.558072 
time: 1403636977.901666, t: 4.384781 -3.874947 -1.192586 q: 0.145696 0.778939 -0.250577 0.556091 
time: 1403636978.101666, t: 4.379854 -3.962758 -1.231289 q: 0.157551 0.776887 -0.249369 0.556273 
time: 1403636978.201667, t: 4.374483 -4.009087 -1.239491 q: 0.161105 0.778336 -0.248977 0.553398 
time: 1403636978.301666, t: 4.366143 -4.054347 -1.240624 q: 0.163579 0.780759 -0.248797 0.549324 
time: 1403636978.401666, t: 4.360268 -4.102906 -1.237162 q: 0.164354 0.783869 -0.249605 0.544274 
time: 1403636978.501667, t: 4.353183 -4.148201 -1.226086 q: 0.164381 0.789298 -0.250865 0.535775 
time: 1403636978.601666, t: 4.351419 -4.196920 -1.221385 q: 0.163093 0.794515 -0.253174 0.527303 
time: 1403636978.701667, t: 4.354454 -4.247865 -1.221819 q: 0.162780 0.797772 -0.254411 0.521859 
time: 1403636978.801666, t: 4.364085 -4.298757 -1.226972 q: 0.162742 0.799786 -0.255275 0.518356 
time: 1403636978.901666, t: 4.379226 -4.350033 -1.232557 q: 0.163965 0.796554 -0.254592 0.523260 
time: 1403636979.001667, t: 4.402622 -4.405065 -1.244575 q: 0.168708 0.788782 -0.251455 0.534912 
time: 1403636979.101666, t: 4.425497 -4.455794 -1.259627 q: 0.172591 0.780967 -0.248661 0.546325 
time: 1403636979.201667, t: 4.450396 -4.507893 -1.284827 q: 0.173791 0.778049 -0.247614 0.550567 
time: 1403636979.301666, t: 4.474945 -4.553483 -1.313628 q: 0.171795 0.783257 -0.248879 0.543189 
time: 1403636979.401666, t: 4.500699 -4.599195 -1.335572 q: 0.170025 0.789343 -0.249859 0.534414 
time: 1403636979.501667, t: 4.531380 -4.645795 -1.350284 q: 0.166925 0.793360 -0.251938 0.528434 
time: 1403636979.601666, t: 4.566646 -4.694069 -1.358264 q: 0.166221 0.795294 -0.253059 0.525203 
time: 1403636979.701667, t: 4.606038 -4.740813 -1.356221 q: 0.168358 0.796432 -0.253224 0.522714 
time: 1403636979.801666, t: 4.652129 -4.786906 -1.349916 q: 0.170061 0.795053 -0.254119 0.523826 
time: 1403636979.901666, t: 4.701520 -4.832430 -1.338146 q: 0.173643 0.789631 -0.254232 0.530751 
time: 1403636980.001667, t: 4.753315 -4.875914 -1.329458 q: 0.177895 0.785668 -0.253975 0.535329 
time: 1403636980.101666, t: 4.805648 -4.920245 -1.326626 q: 0.180211 0.783617 -0.254542 0.537287 
time: 1403636980.201667, t: 4.859459 -4.963034 -1.328882 q: 0.180822 0.782037 -0.256182 0.538603 
time: 1403636980.301666, t: 4.916937 -5.007593 -1.337674 q: 0.180172 0.779300 -0.258136 0.541844 
time: 1403636980.401666, t: 4.973752 -5.048733 -1.349181 q: 0.179723 0.774747 -0.259013 0.548068 
time: 1403636980.501667, t: 5.031841 -5.088929 -1.367764 q: 0.178374 0.771036 -0.259974 0.553263 
time: 1403636980.601666, t: 5.089893 -5.128271 -1.387750 q: 0.174225 0.768612 -0.261787 0.557089 
time: 1403636980.701667, t: 5.143189 -5.166799 -1.406560 q: 0.173053 0.767837 -0.261069 0.558859 
time: 1403636980.801666, t: 5.194543 -5.204157 -1.417241 q: 0.175489 0.771538 -0.257982 0.554418 
time: 1403636980.901666, t: 5.242658 -5.237640 -1.422026 q: 0.176905 0.777288 -0.255216 0.547168 
time: 1403636981.001667, t: 5.291147 -5.270509 -1.421769 q: 0.178834 0.780651 -0.252482 0.543005 
time: 1403636981.101666, t: 5.338321 -5.299388 -1.423580 q: 0.178248 0.779811 -0.252561 0.544368 
time: 1403636981.201667, t: 5.389553 -5.328436 -1.431340 q: 0.172790 0.776980 -0.257077 0.548048 
time: 1403636981.301666, t: 5.437805 -5.354934 -1.442532 q: 0.163159 0.771327 -0.265498 0.554928 
time: 1403636981.401666, t: 5.479363 -5.378498 -1.458105 q: 0.157451 0.767544 -0.271392 0.558956 
time: 1403636981.501667, t: 5.517627 -5.405644 -1.476825 q: 0.164550 0.769791 -0.269100 0.554914 
time: 1403636981.601666, t: 5.557778 -5.438184 -1.493212 q: 0.176452 0.775251 -0.263509 0.546273 
time: 1403636981.701667, t: 5.595265 -5.468046 -1.501799 q: 0.184397 0.779228 -0.260836 0.539228 
time: 1403636981.801666, t: 5.627301 -5.491582 -1.501945 q: 0.188145 0.782084 -0.261450 0.533470 
time: 1403636981.901666, t: 5.672216 -5.520010 -1.494872 q: 0.189002 0.786215 -0.263670 0.525948 
time: 1403636982.001667, t: 5.711901 -5.542175 -1.483489 q: 0.185909 0.790956 -0.268436 0.517463 
time: 1403636982.101666, t: 5.765531 -5.572741 -1.478452 q: 0.181252 0.795187 -0.273282 0.510042 
time: 1403636982.201667, t: 5.817760 -5.600789 -1.476102 q: 0.176628 0.796233 -0.277571 0.507711 
time: 1403636982.301666, t: 5.882916 -5.636715 -1.477636 q: 0.170735 0.793316 -0.281808 0.511942 
time: 1403636982.401666, t: 5.948168 -5.675546 -1.482086 q: 0.169100 0.786648 -0.282660 0.522201 
time: 1403636982.501667, t: 6.017860 -5.720371 -1.482742 q: 0.169950 0.777855 -0.280602 0.536023 
time: 1403636982.601666, t: 6.090502 -5.770760 -1.478752 q: 0.172819 0.773624 -0.276958 0.543078 
time: 1403636982.701667, t: 6.163766 -5.824223 -1.470327 q: 0.170226 0.774277 -0.276680 0.543108 
time: 1403636982.801666, t: 6.236194 -5.879368 -1.462175 q: 0.170058 0.778373 -0.274475 0.538404 
time: 1403636982.901666, t: 6.303852 -5.934593 -1.456513 q: 0.171765 0.781831 -0.272430 0.533872 
time: 1403636983.001667, t: 6.376107 -5.993459 -1.456885 q: 0.174487 0.781509 -0.269972 0.534709 
time: 1403636983.101666, t: 6.445840 -6.053092 -1.462757 q: 0.177327 0.777916 -0.267470 0.540242 
time: 1403636983.201667, t: 6.519491 -6.113962 -1.472470 q: 0.181955 0.776184 -0.264303 0.542748 
time: 1403636983.301666, t: 6.591025 -6.173761 -1.487583 q: 0.185934 0.774980 -0.262003 0.544233 
time: 1403636983.401666, t: 6.659468 -6.231939 -1.508209 q: 0.188359 0.774153 -0.260954 0.545079 
time: 1403636983.501667, t: 6.729509 -6.289233 -1.526207 q: 0.188300 0.773972 -0.262138 0.544788 
time: 1403636983.601666, t: 6.798257 -6.342142 -1.539823 q: 0.189278 0.776050 -0.263068 0.541033 
time: 1403636983.701667, t: 6.870207 -6.392941 -1.544718 q: 0.189014 0.779551 -0.265256 0.534989 
time: 1403636983.801666, t: 6.938835 -6.439673 -1.542345 q: 0.190196 0.782332 -0.266288 0.529974 
time: 1403636983.901666, t: 7.011291 -6.484743 -1.531989 q: 0.190312 0.784528 -0.267997 0.525809 
time: 1403636984.001667, t: 7.084633 -6.530872 -1.518341 q: 0.189805 0.785546 -0.270431 0.523219 
time: 1403636984.101666, t: 7.162267 -6.575378 -1.498609 q: 0.189845 0.786228 -0.272509 0.521099 
time: 1403636984.201667, t: 7.242636 -6.621896 -1.484331 q: 0.192250 0.783622 -0.272570 0.524101 
time: 1403636984.301666, t: 7.328454 -6.668525 -1.475809 q: 0.195253 0.779418 -0.272203 0.529424 
time: 1403636984.401666, t: 7.419669 -6.717007 -1.476654 q: 0.196633 0.776505 -0.272772 0.532889 
time: 1403636984.501667, t: 7.509302 -6.761736 -1.484152 q: 0.198849 0.774077 -0.272426 0.535769 
time: 1403636984.601666, t: 7.601268 -6.805352 -1.495693 q: 0.199596 0.773248 -0.272127 0.536839 
time: 1403636984.701667, t: 7.690257 -6.844764 -1.503055 q: 0.199966 0.773006 -0.271319 0.537458 
time: 1403636984.801666, t: 7.779722 -6.881733 -1.505195 q: 0.196996 0.771744 -0.272506 0.539764 
time: 1403636984.901666, t: 7.874467 -6.921259 -1.504570 q: 0.191589 0.770858 -0.275682 0.541360 
time: 1403636985.001667, t: 7.961457 -6.953850 -1.496445 q: 0.190083 0.770206 -0.276610 0.542345 
time: 1403636985.101666, t: 8.051573 -6.990206 -1.486885 q: 0.191248 0.770805 -0.276060 0.541363 
time: 1403636985.201667, t: 8.142464 -7.024285 -1.484406 q: 0.193859 0.772033 -0.274728 0.539358 
time: 1403636985.301666, t: 8.224851 -7.051040 -1.495820 q: 0.196333 0.771877 -0.273801 0.539158 
time: 1403636985.401666, t: 8.315637 -7.080415 -1.509710 q: 0.195804 0.773357 -0.270237 0.539029 
time: 1403636985.501667, t: 8.400692 -7.103195 -1.520420 q: 0.193392 0.777296 -0.260825 0.538870 
time: 1403636985.601666, t: 8.495722 -7.128551 -1.519857 q: 0.190649 0.779552 -0.251571 0.540983 
time: 1403636985.701667, t: 8.588868 -7.149216 -1.513165 q: 0.190765 0.780947 -0.242821 0.542926 
time: 1403636985.801666, t: 8.681644 -7.165356 -1.501165 q: 0.190287 0.780033 -0.236260 0.547285 
time: 1403636985.901666, t: 8.775631 -7.176997 -1.487073 q: 0.191764 0.770740 -0.229575 0.562567 
time: 1403636986.001667, t: 8.862445 -7.180797 -1.479286 q: 0.194245 0.759892 -0.223525 0.578679 
time: 1403636986.101666, t: 8.943487 -7.176990 -1.482735 q: 0.194570 0.759461 -0.221304 0.579988 
time: 1403636986.201667, t: 9.019961 -7.165247 -1.495583 q: 0.189855 0.767647 -0.224262 0.569544 
optimize pose graph 
time: 1403636986.301666, t: 9.090054 -7.145999 -1.510993 q: 0.185941 0.777100 -0.227251 0.556686 
time: 1403636986.401666, t: 9.161229 -7.121960 -1.528354 q: 0.181845 0.781876 -0.230084 0.550149 
time: 1403636986.501667, t: 9.232630 -7.093221 -1.545452 q: 0.174383 0.782891 -0.235143 0.548981 
time: 1403636986.601666, t: 9.303938 -7.062366 -1.555350 q: 0.167526 0.782001 -0.239420 0.550534 
time: 1403636986.701667, t: 9.373526 -7.031048 -1.556779 q: 0.162531 0.781407 -0.242662 0.551455 
time: 1403636986.801666, t: 9.441652 -7.001587 -1.547942 q: 0.157017 0.780338 -0.246185 0.553002 
time: 1403636986.901666, t: 9.506768 -6.973763 -1.529764 q: 0.156008 0.779123 -0.247278 0.554511 
time: 1403636987.001667, t: 9.568057 -6.948226 -1.503001 q: 0.158715 0.778580 -0.246048 0.555053 
time: 1403636987.101666, t: 9.624416 -6.922706 -1.470170 q: 0.164398 0.778239 -0.243782 0.554876 
time: 1403636987.201667, t: 9.677979 -6.896723 -1.440733 q: 0.167379 0.777147 -0.243625 0.555584 
time: 1403636987.301666, t: 9.730674 -6.871390 -1.419126 q: 0.168799 0.775421 -0.244580 0.557144 
time: 1403636987.401666, t: 9.781291 -6.843585 -1.402975 q: 0.168591 0.774140 -0.243924 0.559272 
time: 1403636987.501667, t: 9.829101 -6.816597 -1.391961 q: 0.169746 0.772330 -0.238429 0.563776 
time: 1403636987.601666, t: 9.875393 -6.785691 -1.384126 q: 0.172440 0.770179 -0.232541 0.568342 
time: 1403636987.701667, t: 9.917319 -6.750891 -1.381076 q: 0.172496 0.769380 -0.229191 0.570763 
time: 1403636987.801666, t: 9.954827 -6.712773 -1.375193 q: 0.169795 0.769577 -0.227545 0.571965 
time: 1403636987.901666, t: 9.986361 -6.671962 -1.362698 q: 0.164652 0.770411 -0.228440 0.571989 
time: 1403636988.001667, t: 10.014466 -6.629141 -1.344814 q: 0.155576 0.772591 -0.232784 0.569833 
time: 1403636988.101666, t: 10.039601 -6.585404 -1.329286 q: 0.146646 0.775119 -0.238714 0.566305 
time: 1403636988.201667, t: 10.058944 -6.543869 -1.319077 q: 0.140839 0.775962 -0.243280 0.564679 
time: 1403636988.301666, t: 10.072954 -6.507009 -1.312000 q: 0.137943 0.772206 -0.246163 0.569275 
optimize pose graph 
time: 1403636988.401666, t: 10.080966 -6.474928 -1.302726 q: 0.138973 0.765707 -0.246147 0.577747 
time: 1403636988.501667, t: 10.081161 -6.443733 -1.285542 q: 0.142378 0.761463 -0.243865 0.583465 
time: 1403636988.601666, t: 10.073180 -6.413167 -1.261907 q: 0.143001 0.759172 -0.243252 0.586547 
time: 1403636988.801666, t: 10.030489 -6.354078 -1.219681 q: 0.144127 0.756409 -0.242426 0.590172 
time: 1403636988.901666, t: 9.997495 -6.325138 -1.207133 q: 0.143748 0.753832 -0.242676 0.593450 
time: 1403636989.001667, t: 9.952423 -6.298717 -1.200013 q: 0.143025 0.751194 -0.243428 0.596653 
time: 1403636989.101666, t: 9.897717 -6.271187 -1.197575 q: 0.142659 0.749377 -0.244458 0.598601 
time: 1403636989.201667, t: 9.829952 -6.246431 -1.197482 q: 0.142780 0.749351 -0.245341 0.598243 
time: 1403636989.301666, t: 9.752179 -6.222218 -1.195038 q: 0.148664 0.755625 -0.243192 0.589735 
time: 1403636989.401666, t: 9.667891 -6.197539 -1.185632 q: 0.155352 0.765495 -0.240671 0.576160 
time: 1403636989.601666, t: 9.486240 -6.153659 -1.147615 q: 0.160117 0.776977 -0.239726 0.559643 
time: 1403636989.701667, t: 9.392208 -6.131822 -1.127866 q: 0.164609 0.780029 -0.236885 0.555287 
time: 1403636989.801666, t: 9.298961 -6.108317 -1.114367 q: 0.166461 0.781389 -0.235774 0.553293 
time: 1403636989.901666, t: 9.209042 -6.084928 -1.103956 q: 0.165975 0.779861 -0.235889 0.555540 
time: 1403636990.001667, t: 9.115722 -6.058748 -1.099014 q: 0.165867 0.775060 -0.235697 0.562332 
time: 1403636990.101666, t: 9.024847 -6.033078 -1.090632 q: 0.166997 0.771067 -0.234669 0.567889 
time: 1403636990.201667, t: 8.927784 -6.004219 -1.077404 q: 0.168258 0.770814 -0.233331 0.568412 
time: 1403636990.301666, t: 8.833236 -5.974453 -1.057697 q: 0.168540 0.774302 -0.232063 0.564091 
time: 1403636990.401666, t: 8.732123 -5.940887 -1.032936 q: 0.168455 0.777701 -0.230549 0.560045 
time: 1403636990.501667, t: 8.635569 -5.906478 -1.010965 q: 0.165117 0.780596 -0.230752 0.556920 
time: 1403636990.601666, t: 8.537536 -5.870415 -0.995513 q: 0.163560 0.782326 -0.229900 0.555301 
wait for imu ... 
wait for imu ... 
time: 1403636990.701667, t: 8.443185 -5.834395 -0.986389 q: 0.162744 0.784577 -0.228614 0.552891 
time: 1403636990.801666, t: 8.348150 -5.796717 -0.984369 q: 0.162723 0.785884 -0.227089 0.551668 
time: 1403636991.001667, t: 8.164301 -5.721119 -0.996055 q: 0.162265 0.786326 -0.225751 0.551722 
time: 1403636991.101666, t: 8.075410 -5.683051 -1.007365 q: 0.163102 0.785884 -0.225372 0.552261 
time: 1403636991.201667, t: 7.988630 -5.645598 -1.020584 q: 0.164229 0.785938 -0.225456 0.551815 
time: 1403636991.301666, t: 7.901741 -5.605237 -1.028393 q: 0.164035 0.786295 -0.226231 0.551046 
time: 1403636991.401666, t: 7.817152 -5.564496 -1.027910 q: 0.164058 0.786767 -0.227557 0.549819 
time: 1403636991.501667, t: 7.735218 -5.521417 -1.016816 q: 0.165355 0.786965 -0.228511 0.548750 
time: 1403636991.601666, t: 7.658886 -5.482413 -0.997590 q: 0.163374 0.786134 -0.231919 0.549104 
time: 1403636991.701667, t: 7.579260 -5.440015 -0.973306 q: 0.157391 0.784531 -0.238869 0.550164 
time: 1403636991.801666, t: 7.503310 -5.399600 -0.945580 q: 0.152717 0.782918 -0.245340 0.550931 
time: 1403636991.901666, t: 7.419812 -5.360834 -0.917032 q: 0.150069 0.782041 -0.250246 0.550698 
time: 1403636992.001667, t: 7.341422 -5.326480 -0.897074 q: 0.152767 0.781778 -0.251342 0.549830 
time: 1403636992.101666, t: 7.262870 -5.295663 -0.879956 q: 0.162197 0.780297 -0.247432 0.551005 
time: 1403636992.201667, t: 7.183273 -5.266008 -0.875096 q: 0.168990 0.777301 -0.244700 0.554407 
time: 1403636992.301666, t: 7.109673 -5.239292 -0.869804 q: 0.172501 0.776196 -0.244087 0.555144 
time: 1403636992.401666, t: 7.037752 -5.213442 -0.873154 q: 0.174140 0.778245 -0.245202 0.551259 
time: 1403636992.501667, t: 6.966323 -5.184728 -0.871857 q: 0.174728 0.781099 -0.247277 0.546085 
time: 1403636992.601666, t: 6.897141 -5.154112 -0.866354 q: 0.176666 0.782689 -0.248440 0.542646 
time: 1403636992.701667, t: 6.826996 -5.122329 -0.851321 q: 0.178119 0.783120 -0.249698 0.540969 
time: 1403636992.801666, t: 6.761244 -5.091442 -0.834692 q: 0.179969 0.783731 -0.250556 0.539072 
time: 1403636992.901666, t: 6.695257 -5.058090 -0.826163 q: 0.183053 0.783379 -0.250528 0.538557 
time: 1403636993.001667, t: 6.638963 -5.026839 -0.821691 q: 0.184906 0.781557 -0.250983 0.540358 
time: 1403636993.101666, t: 6.582342 -4.992431 -0.823703 q: 0.182300 0.777622 -0.252926 0.545985 
time: 1403636993.201667, t: 6.529421 -4.957991 -0.821355 q: 0.175054 0.775786 -0.250374 0.552110 
time: 1403636993.301666, t: 6.477962 -4.921788 -0.812460 q: 0.166551 0.777137 -0.241866 0.556614 
time: 1403636993.401666, t: 6.426639 -4.884524 -0.796681 q: 0.156475 0.779994 -0.228745 0.561071 
time: 1403636993.501667, t: 6.373942 -4.845694 -0.773400 q: 0.144640 0.784116 -0.212907 0.564723 
time: 1403636993.601666, t: 6.320994 -4.807932 -0.744790 q: 0.133138 0.788833 -0.195778 0.567175 
time: 1403636993.701667, t: 6.269002 -4.771006 -0.716875 q: 0.122900 0.793747 -0.182497 0.567059 
time: 1403636993.801666, t: 6.216132 -4.734383 -0.693775 q: 0.116349 0.797311 -0.172669 0.566518 
time: 1403636993.901666, t: 6.162934 -4.696997 -0.677654 q: 0.113799 0.799415 -0.164103 0.566617 
time: 1403636994.001667, t: 6.109126 -4.660103 -0.665965 q: 0.111700 0.797429 -0.158804 0.571325 
time: 1403636994.101666, t: 6.053800 -4.621770 -0.659364 q: 0.113701 0.792742 -0.153761 0.578783 
time: 1403636994.201667, t: 5.996514 -4.581608 -0.655577 q: 0.114974 0.787762 -0.150504 0.586140 
time: 1403636994.301666, t: 5.936225 -4.539808 -0.657809 q: 0.115602 0.783201 -0.149285 0.592408 
time: 1403636994.401666, t: 5.874103 -4.495582 -0.663165 q: 0.116829 0.780616 -0.148408 0.595789 
optimize pose graph 
time: 1403636994.501667, t: 5.804059 -4.444926 -0.670365 q: 0.116983 0.781211 -0.149089 0.594809 
time: 1403636994.601666, t: 5.733722 -4.395260 -0.673071 q: 0.112744 0.784415 -0.152612 0.590501 
time: 1403636994.701667, t: 5.655362 -4.339554 -0.669297 q: 0.105859 0.789782 -0.158593 0.582998 
time: 1403636994.801666, t: 5.578585 -4.285672 -0.660224 q: 0.096292 0.795166 -0.166508 0.575077 
time: 1403636994.901666, t: 5.499109 -4.232738 -0.648524 q: 0.089185 0.797060 -0.172555 0.571809 
time: 1403636995.001667, t: 5.418120 -4.183930 -0.640619 q: 0.089212 0.795406 -0.173360 0.573862 
time: 1403636995.101666, t: 5.335787 -4.139084 -0.637343 q: 0.096318 0.792381 -0.168063 0.578455 
time: 1403636995.201667, t: 5.249325 -4.093503 -0.633275 q: 0.102456 0.789746 -0.162830 0.582486 
time: 1403636995.301666, t: 5.163155 -4.051495 -0.622791 q: 0.105788 0.788360 -0.159082 0.584800 
time: 1403636995.401666, t: 5.070626 -4.005625 -0.605146 q: 0.107594 0.790783 -0.155909 0.582046 
time: 1403636995.501667, t: 4.978786 -3.962065 -0.581571 q: 0.106561 0.795481 -0.155335 0.575956 
time: 1403636995.601666, t: 4.880683 -3.913703 -0.551680 q: 0.105667 0.797244 -0.155123 0.573737 
time: 1403636995.701667, t: 4.788265 -3.869283 -0.522749 q: 0.103296 0.797302 -0.156767 0.573641 
time: 1403636995.801666, t: 4.692567 -3.821447 -0.498281 q: 0.098625 0.797315 -0.160454 0.573426 
time: 1403636995.901666, t: 4.598498 -3.778544 -0.482972 q: 0.091205 0.798429 -0.166118 0.571487 
time: 1403636996.001667, t: 4.500832 -3.735405 -0.471953 q: 0.087635 0.799486 -0.169384 0.569607 
time: 1403636996.101666, t: 4.405660 -3.698253 -0.465869 q: 0.092732 0.801393 -0.166318 0.567017 
time: 1403636996.201667, t: 4.308668 -3.662450 -0.459087 q: 0.098099 0.802578 -0.162749 0.565471 
time: 1403636996.301666, t: 4.216672 -3.630815 -0.446953 q: 0.101894 0.803314 -0.159288 0.564740 
time: 1403636996.401666, t: 4.123760 -3.599076 -0.425363 q: 0.099550 0.804409 -0.154632 0.564894 
time: 1403636996.501667, t: 4.036381 -3.567802 -0.395804 q: 0.095194 0.806229 -0.145351 0.565514 
time: 1403636996.601666, t: 3.949383 -3.535986 -0.361194 q: 0.088223 0.808972 -0.132424 0.565902 
optimize pose graph 
time: 1403636996.701667, t: 3.867494 -3.505298 -0.329664 q: 0.078140 0.812273 -0.116280 0.566203 
time: 1403636996.801666, t: 3.782845 -3.471753 -0.312978 q: 0.065525 0.815566 -0.098399 0.566459 
time: 1403636996.901666, t: 3.699760 -3.438299 -0.308686 q: 0.049955 0.818341 -0.080147 0.566921 
time: 1403636997.001667, t: 3.619000 -3.405164 -0.310664 q: 0.034683 0.820582 -0.060518 0.567256 
time: 1403636997.101666, t: 3.539738 -3.372432 -0.317426 q: 0.021227 0.822455 -0.042498 0.566843 
time: 1403636997.201667, t: 3.463669 -3.341675 -0.328920 q: 0.011147 0.823812 -0.029482 0.565986 
time: 1403636997.301666, t: 3.391107 -3.312832 -0.343281 q: 0.005311 0.824541 -0.019602 0.565437 
time: 1403636997.401666, t: 3.320450 -3.285486 -0.352271 q: 0.002465 0.825238 -0.013412 0.564620 
time: 1403636997.501667, t: 3.258432 -3.260963 -0.353691 q: 0.001395 0.824917 -0.010473 0.565156 
time: 1403636997.601666, t: 3.198971 -3.235191 -0.347667 q: 0.001240 0.825669 -0.010233 0.564060 
time: 1403636997.801666, t: 3.096680 -3.188040 -0.311213 q: 0.005901 0.827172 -0.011312 0.561805 
time: 1403636997.901666, t: 3.056904 -3.164003 -0.282126 q: 0.008132 0.827431 -0.012937 0.561360 
time: 1403636998.001667, t: 3.020520 -3.142711 -0.245724 q: 0.009154 0.827388 -0.015118 0.561352 
time: 1403636998.101666, t: 2.991519 -3.122398 -0.204308 q: 0.010013 0.827619 -0.017308 0.560934 
time: 1403636998.201667, t: 2.974465 -3.104929 -0.164255 q: 0.009100 0.826386 -0.020445 0.562660 
time: 1403636998.301666, t: 2.952985 -3.085622 -0.131314 q: 0.010148 0.826904 -0.021883 0.561825 
time: 1403636998.401666, t: 2.934318 -3.067040 -0.104649 q: 0.009894 0.826556 -0.023454 0.562278 
time: 1403636998.501667, t: 2.920767 -3.048915 -0.079028 q: 0.009376 0.826317 -0.025391 0.562554 
time: 1403636998.601666, t: 2.908552 -3.031906 -0.049053 q: 0.010952 0.823110 -0.025197 0.567217 
time: 1403636998.701667, t: 2.898188 -3.016232 -0.011714 q: 0.014250 0.814941 -0.023644 0.578886 
optimize pose graph 
time: 1403636998.801666, t: 2.887101 -3.001520 0.030354 q: 0.017346 0.809793 -0.022567 0.586025 
time: 1403636998.901666, t: 2.874979 -2.985827 0.069865 q: 0.022919 0.808252 -0.019165 0.588078 
time: 1403636999.001667, t: 2.858970 -2.967068 0.102368 q: 0.026741 0.808631 -0.017392 0.587451 
time: 1403636999.101666, t: 2.838427 -2.945647 0.128360 q: 0.029619 0.808842 -0.015889 0.587065 
time: 1403636999.201667, t: 2.817006 -2.920916 0.148916 q: 0.031215 0.810526 -0.015562 0.584663 
time: 1403636999.301666, t: 2.797041 -2.892954 0.164182 q: 0.030031 0.812229 -0.016927 0.582319 
time: 1403636999.401666, t: 2.770802 -2.862147 0.173152 q: 0.025303 0.812774 -0.020618 0.581664 
time: 1403636999.501667, t: 2.745767 -2.830101 0.177688 q: 0.022412 0.812420 -0.022899 0.582192 
time: 1403636999.601666, t: 2.721202 -2.797342 0.178304 q: 0.020794 0.810236 -0.024530 0.585222 
time: 1403636999.701667, t: 2.692776 -2.763993 0.175668 q: 0.020274 0.808323 -0.025520 0.587837 
time: 1403636999.801666, t: 2.662396 -2.730720 0.169517 q: 0.020285 0.807915 -0.026385 0.588359 
time: 1403636999.901666, t: 2.629288 -2.698057 0.161245 q: 0.021901 0.808115 -0.025704 0.588056 
time: 1403637000.001667, t: 2.592332 -2.662206 0.155514 q: 0.026085 0.808044 -0.023190 0.588087 
time: 1403637000.101666, t: 2.551981 -2.624589 0.155285 q: 0.028311 0.809051 -0.021429 0.586664 
time: 1403637000.301666, t: 2.470118 -2.546721 0.173018 q: 0.026102 0.812265 -0.022483 0.582271 
time: 1403637000.401666, t: 2.426277 -2.504940 0.180035 q: 0.023888 0.811673 -0.023899 0.583134 
time: 1403637000.501667, t: 2.384992 -2.461883 0.182881 q: 0.026015 0.811708 -0.022667 0.583043 
time: 1403637000.601666, t: 2.338307 -2.417144 0.184260 q: 0.026702 0.811861 -0.022247 0.582815 
time: 1403637000.701667, t: 2.291231 -2.370433 0.191675 q: 0.024603 0.810976 -0.023870 0.584074 
time: 1403637000.801666, t: 2.248444 -2.323594 0.207878 q: 0.024625 0.810400 -0.024577 0.584843 
time: 1403637000.901666, t: 2.199955 -2.275910 0.223421 q: 0.024042 0.810444 -0.025562 0.584764 
time: 1403637001.001667, t: 2.151501 -2.226093 0.234721 q: 0.024357 0.810131 -0.026476 0.585144 
time: 1403637001.101666, t: 2.097933 -2.178001 0.238717 q: 0.023949 0.809934 -0.027184 0.585401 
time: 1403637001.201667, t: 2.043100 -2.129451 0.240534 q: 0.022950 0.809242 -0.028359 0.586341 
time: 1403637001.301666, t: 1.983257 -2.078857 0.248190 q: 0.022164 0.809369 -0.029303 0.586150 
time: 1403637001.401666, t: 1.922939 -2.030281 0.262788 q: 0.019750 0.810670 -0.031306 0.584332 
time: 1403637001.501667, t: 1.858685 -1.981520 0.284779 q: 0.019853 0.813689 -0.032439 0.580055 
time: 1403637001.601666, t: 1.796984 -1.934409 0.305123 q: 0.020200 0.815173 -0.033444 0.577899 
time: 1403637001.701667, t: 1.731633 -1.888973 0.323485 q: 0.020566 0.815309 -0.034037 0.577659 
time: 1403637001.801666, t: 1.670126 -1.843903 0.333261 q: 0.019946 0.814393 -0.035389 0.578891 
time: 1403637001.901666, t: 1.607986 -1.800296 0.335804 q: 0.020923 0.815908 -0.035304 0.576723 
time: 1403637002.001667, t: 1.548280 -1.758372 0.334301 q: 0.021476 0.816779 -0.035252 0.575472 
time: 1403637002.101666, t: 1.488537 -1.716422 0.328828 q: 0.021884 0.817388 -0.035425 0.574581 
time: 1403637002.201667, t: 1.432835 -1.676742 0.317424 q: 0.021650 0.817562 -0.035560 0.574334 
time: 1403637002.301666, t: 1.376061 -1.637166 0.304061 q: 0.022994 0.817561 -0.034711 0.574334 
time: 1403637002.401666, t: 1.318380 -1.597618 0.294289 q: 0.024980 0.818059 -0.033005 0.573643 
time: 1403637002.501667, t: 1.263851 -1.558493 0.292437 q: 0.026486 0.817521 -0.031735 0.574413 
time: 1403637002.601666, t: 1.208368 -1.517878 0.300400 q: 0.029097 0.817149 -0.029741 0.574923 
time: 1403637002.701667, t: 1.149975 -1.475407 0.315470 q: 0.030388 0.817459 -0.029675 0.574418 
time: 1403637002.801666, t: 1.093964 -1.430786 0.329105 q: 0.030564 0.817212 -0.030601 0.574712 
time: 1403637002.901666, t: 1.038498 -1.386558 0.336888 q: 0.027457 0.816671 -0.034014 0.575446 
time: 1403637003.001667, t: 0.983590 -1.341982 0.336289 q: 0.027210 0.816923 -0.035602 0.575004 
time: 1403637003.101666, t: 0.931867 -1.296426 0.331236 q: 0.027126 0.817254 -0.037390 0.574423 
time: 1403637003.201667, t: 0.880691 -1.252544 0.321911 q: 0.027898 0.816811 -0.038510 0.574942 
time: 1403637003.301666, t: 0.833033 -1.210394 0.309281 q: 0.027887 0.815717 -0.040056 0.576389 
time: 1403637003.401666, t: 0.780655 -1.165594 0.301996 q: 0.027698 0.814388 -0.041115 0.578199 
time: 1403637003.501667, t: 0.731066 -1.121949 0.303093 q: 0.027150 0.813141 -0.042725 0.579861 
time: 1403637003.601666, t: 0.680519 -1.078785 0.310094 q: 0.026594 0.813387 -0.043953 0.579451 
time: 1403637003.701667, t: 0.628818 -1.035452 0.323486 q: 0.026444 0.813577 -0.044743 0.579129 
time: 1403637003.801666, t: 0.571302 -0.991980 0.332693 q: 0.024864 0.814036 -0.045096 0.578528 
time: 1403637004.001667, t: 0.470427 -0.911530 0.335834 q: 0.026759 0.818497 -0.040206 0.572477 
time: 1403637004.101666, t: 0.414346 -0.875665 0.326347 q: 0.027319 0.820533 -0.037739 0.569698 
time: 1403637004.301666, t: 0.323205 -0.799873 0.307850 q: 0.025328 0.821864 -0.035480 0.568013 
time: 1403637004.401666, t: 0.281622 -0.759950 0.311013 q: 0.023003 0.820738 -0.037223 0.569627 
time: 1403637004.501667, t: 0.240088 -0.722264 0.324671 q: 0.022970 0.816429 -0.038649 0.575693 
time: 1403637004.601666, t: 0.198437 -0.684257 0.348509 q: 0.022567 0.814002 -0.041560 0.578933 
time: 1403637004.701667, t: 0.154580 -0.648366 0.376014 q: 0.014307 0.813451 -0.050619 0.579251 
time: 1403637004.801666, t: 0.117322 -0.616076 0.395822 q: 0.007754 0.813007 -0.058532 0.579252 
time: 1403637004.901666, t: 0.077252 -0.585647 0.401129 q: 0.006415 0.815768 -0.062354 0.574973 
time: 1403637005.001667, t: 0.036194 -0.559631 0.391312 q: 0.010018 0.819997 -0.062255 0.568883 
optimize pose graph 
time: 1403637005.101666, t: -0.002834 -0.537693 0.375110 q: 0.018308 0.819938 -0.058258 0.569186 
time: 1403637005.201667, t: -0.037417 -0.520676 0.356641 q: 0.025486 0.818810 -0.054421 0.570910 
time: 1403637005.301666, t: -0.070918 -0.504561 0.334842 q: 0.029144 0.820739 -0.052077 0.568178 
time: 1403637005.401666, t: -0.105313 -0.488230 0.311048 q: 0.030086 0.821732 -0.050267 0.566855 
time: 1403637005.501667, t: -0.139428 -0.471993 0.289043 q: 0.031582 0.818734 -0.046396 0.571424 
time: 1403637005.601666, t: -0.166994 -0.455950 0.271882 q: 0.029555 0.817890 -0.045194 0.572835 
time: 1403637005.701667, t: -0.198929 -0.440145 0.265790 q: 0.022919 0.822048 -0.047452 0.566975 
time: 1403637005.801666, t: -0.223288 -0.425786 0.272062 q: 0.019741 0.823721 -0.048379 0.564583 
time: 1403637005.901666, t: -0.244781 -0.411679 0.284056 q: 0.023026 0.822080 -0.045137 0.567113 
time: 1403637006.001667, t: -0.263205 -0.399643 0.293453 q: 0.025523 0.819622 -0.043117 0.570709 
time: 1403637006.101666, t: -0.281444 -0.385511 0.299441 q: 0.023762 0.817687 -0.044480 0.573450 
time: 1403637006.201667, t: -0.300074 -0.372055 0.303250 q: 0.022841 0.817697 -0.044810 0.573447 
time: 1403637006.301666, t: -0.319443 -0.357553 0.304906 q: 0.020306 0.821933 -0.046782 0.567297 
time: 1403637006.401666, t: -0.335188 -0.345717 0.303780 q: 0.017997 0.828420 -0.048221 0.557738 
time: 1403637006.501667, t: -0.349280 -0.333665 0.302148 q: 0.012008 0.831071 -0.052334 0.553568 
time: 1403637006.601666, t: -0.357857 -0.325143 0.301053 q: 0.008365 0.826606 -0.050192 0.560476 
time: 1403637006.701667, t: -0.362174 -0.319673 0.298157 q: 0.006557 0.821798 -0.039784 0.568351 
time: 1403637006.801666, t: -0.364747 -0.316123 0.292224 q: 0.000724 0.817740 -0.025710 0.575013 
time: 1403637006.901666, t: -0.365931 -0.314490 0.283309 q: -0.011031 0.817822 -0.009682 0.575284 
time: 1403637007.001667, t: -0.367520 -0.313674 0.274330 q: -0.020907 0.819060 0.012484 0.573191 
time: 1403637007.101666, t: -0.368998 -0.314206 0.265378 q: -0.028870 0.817637 0.031543 0.574144 
optimize pose graph 
time: 1403637007.201667, t: -0.370681 -0.315209 0.255728 q: -0.035461 0.815374 0.045860 0.576025 
time: 1403637007.301666, t: -0.372934 -0.316432 0.246788 q: -0.036817 0.810139 0.057584 0.582240 
time: 1403637007.401666, t: -0.378139 -0.316641 0.237396 q: -0.037728 0.806701 0.064351 0.586234 
time: 1403637007.501667, t: -0.386872 -0.315432 0.226327 q: -0.039595 0.808059 0.067042 0.583933 
time: 1403637007.601666, t: -0.397467 -0.313093 0.213941 q: -0.040793 0.812612 0.067558 0.577438 
time: 1403637007.701667, t: -0.409984 -0.309010 0.199564 q: -0.040369 0.820810 0.066966 0.565825 
time: 1403637007.801666, t: -0.420749 -0.303420 0.187040 q: -0.038283 0.824398 0.066662 0.560767 
time: 1403637007.901666, t: -0.428914 -0.295690 0.177285 q: -0.041861 0.819563 0.062108 0.568073 
time: 1403637008.001667, t: -0.435937 -0.287034 0.168692 q: -0.047922 0.813125 0.055235 0.577478 
time: 1403637008.101666, t: -0.444523 -0.279511 0.160611 q: -0.044421 0.814879 0.054325 0.575368 
time: 1403637008.201667, t: -0.454049 -0.272519 0.152292 q: -0.039662 0.824020 0.054228 0.562563 
time: 1403637008.301666, t: -0.460759 -0.264763 0.147462 q: -0.034384 0.830193 0.054890 0.553701 
time: 1403637008.401666, t: -0.463558 -0.255596 0.147412 q: -0.031640 0.830373 0.054130 0.553669 
time: 1403637008.501667, t: -0.461888 -0.244278 0.152368 q: -0.034797 0.823016 0.049638 0.564775 
time: 1403637008.601666, t: -0.457720 -0.232756 0.158022 q: -0.037608 0.816922 0.045236 0.573741 
time: 1403637008.701667, t: -0.453753 -0.221747 0.159561 q: -0.042433 0.814910 0.039777 0.576662 
time: 1403637008.801666, t: -0.449729 -0.213068 0.156203 q: -0.054672 0.818624 0.029657 0.570953 
time: 1403637008.901666, t: -0.443641 -0.209939 0.148352 q: -0.062611 0.822865 0.023323 0.564295 
time: 1403637009.001667, t: -0.435916 -0.212366 0.136845 q: -0.059214 0.823297 0.024171 0.563996 
time: 1403637009.101666, t: -0.426129 -0.219381 0.122584 q: -0.047025 0.822053 0.031120 0.566612 
time: 1403637009.201667, t: -0.415368 -0.229846 0.106921 q: -0.035158 0.820951 0.038433 0.568618 
time: 1403637009.301666, t: -0.402359 -0.240995 0.088928 q: -0.018923 0.820843 0.048990 0.568735 
optimize pose graph 
time: 1403637009.401666, t: -0.389699 -0.249368 0.074690 q: -0.032878 0.816760 0.050285 0.573841 
wait for imu ... 
time: 1403637009.501667, t: -0.386930 -0.257976 0.071037 q: -0.046724 0.807116 0.064344 0.585013 
time: 1403637009.601666, t: -0.386773 -0.262681 0.063995 q: -0.089226 0.785095 0.053375 0.610587 
time: 1403637009.701667, t: -0.396040 -0.268990 0.062354 q: -0.078130 0.790856 0.040279 0.605657 
time: 1403637009.801666, t: -0.397537 -0.268115 0.062258 q: -0.077829 0.790176 0.037058 0.606788 
time: 1403637009.901666, t: -0.399547 -0.268479 0.061980 q: -0.078367 0.789925 0.037958 0.606989 
time: 1403637010.001667, t: -0.398377 -0.266930 0.061096 q: -0.078553 0.789824 0.037898 0.607101 
time: 1403637010.101666, t: -0.399309 -0.267532 0.060658 q: -0.079544 0.789993 0.037616 0.606770 
time: 1403637010.201667, t: -0.400735 -0.268891 0.060582 q: -0.079851 0.789930 0.037585 0.606813 
time: 1403637010.301666, t: -0.400591 -0.269130 0.060616 q: -0.080228 0.790164 0.037470 0.606466 
wait for imu ... 
time: 1403637010.401666, t: -0.400478 -0.269352 0.060766 q: -0.080199 0.790145 0.037451 0.606495 
time: 1403637010.501667, t: -0.400409 -0.269492 0.060793 q: -0.080210 0.790255 0.037377 0.606355 

Done.
time: 1403637010.601666, t: -0.400488 -0.269617 0.060844 q: -0.080321 0.790308 0.037290 0.606277 
killing:
 * /loop_fusion
 * /rosout
 * /vins_estimator
[33m[ WARN] [1731035325.315754909]: Shutdown request received.[0m
[33m[ WARN] [1731035325.315802641]: Reason given for shutdown: [user request][0m
[33m[ WARN] [1731035325.316687824]: Shutdown request received.[0m
[33m[ WARN] [1731035325.316718522]: Reason given for shutdown: [user request][0m
killed
