diff --git a/.gitignore b/.gitignore index 73cb12494a98358d6706d14652b82f0bc4d76025..5b1eba01c9bdefe79c1ce1dea4c82eb893e53ff8 100644 --- a/.gitignore +++ b/.gitignore @@ -24,6 +24,9 @@ src/examples/map_polyline_example_write.yaml *.graffle /Default/ +*.csv +*.csv# + src/CMakeCache.txt src/CMakeFiles/cmake.check_cache diff --git a/demos/cosyslam.cpp b/demos/cosyslam.cpp index 275bf6561a88265b51ea547abc1b231de770ec0f..21600e87a6547f8261632d50e6ae704029d8ad24 100644 --- a/demos/cosyslam.cpp +++ b/demos/cosyslam.cpp @@ -132,15 +132,18 @@ Matrix6d str_to_cov(std::string cov) int main() { + ///////////////////////// + // read the input data // + ///////////////////////// std::string wolf_root = _WOLF_OBJECTSLAM_ROOT_DIR; - std::string filename = wolf_root + "/demos/test.csv"; + std::string filename = wolf_root + "/demos/input_demo2.csv"; std::vector<std::pair<std::string, std::vector<std::string>>> csv_values; csv_values = read_csv(filename); - ///////////////////////////// - // setup the wolf problem - ///////////////////////////// + //////////////////////////// + // setup the wolf problem // + //////////////////////////// ProblemPtr problem = Problem::create("PO", 3); SolverCeresPtr solver = std::make_shared<SolverCeres>(problem); @@ -159,31 +162,51 @@ int main() // Parsing variables int len = csv_values.at(0).second.size(); - int idx_cov = 9; + int idx_cov = 10; int idx_pose = 4; int idx_obj = 3; - int idx_t = 8; + int idx_t = 9; + /////////////// + // SLAM loop // + /////////////// + + + double t_cur = 999; for (int i = 0; i < len; i++) { std::cout << i << std::endl; // Measurement data - Isometry3d c_M_o; - c_M_o = str_to_isometry(csv_values.at(idx_pose).second.at(i)); + Isometry3d c_M_o = str_to_isometry(csv_values.at(idx_pose).second.at(i)); Matrix6d cov; cov = str_to_cov(csv_values.at(idx_cov).second.at(i)); std::string object_name; object_name = csv_values.at(idx_obj).second.at(i); double t; t = std::stod(csv_values.at(idx_t).second.at(i)); - - // Capture processing - dets.clear(); - ObjectDetection det = {.measurement = isometry_to_posevec(c_M_o), .meas_cov = cov, .object_type = object_name}; - dets.push_back(det); - CaptureObjectPtr cap = std::make_shared<CaptureObject>(t, sen, dets); - cap->process(); + ObjectDetection det = {.measurement = isometry_to_posevec(*c_M_o), .meas_cov = cov, .object_type = object_name}; + + if (t == t_cur){ + // Capture on the current frame + dets.push_back(det); + } else if (t_cur == 999){ + // Capture on the first frame + dets.push_back(det); + } else if (i == len-1){ + // Capture on the last frame + dets.push_back(det); + CaptureObjectPtr cap = std::make_shared<CaptureObject>(t_cur, sen, dets); + cap->process(); + } else { + // Capture on a new frame + CaptureObjectPtr cap = std::make_shared<CaptureObject>(t_cur, sen, dets); + cap->process(); + + dets.clear(); + dets.push_back(det); + } + t_cur = t; } std::string report = solver->solve(SolverCeres::ReportVerbosity::FULL); @@ -200,9 +223,9 @@ int main() file_map.open(map_filename, std::fstream::out); // Write headers for csv files - std::string header_res = "id,t,px,py,pz,qx,qy,qz,qw\n"; + std::string header_res = "id,t,px,py,pz,qx,qy,qz,qw,\n"; file_res << header_res; - std::string header_map = "id,px,py,pz,qx,qy,qz,qw\n"; + std::string header_map = "id,px,py,pz,qx,qy,qz,qw,\n"; file_map << header_map; VectorComposite state_est; diff --git a/demos/map.csv b/demos/map.csv index 9d3e694e3fe61c15222404642ae77f81c9569379..f1e18713b7920802213c69978a746b6162d4dc71 100644 --- a/demos/map.csv +++ b/demos/map.csv @@ -1,2 +1,10 @@ -id,px,py,pz,qx,qy,qz,qw -0,0.0118101,-0.0103998,0.42091,-0.00461907,0.771858,-0.628642,0.0949934, +id,px,py,pz,qx,qy,qz,qw, +0,-0.0596441,-0.10401,0.493655,-0.066251,0.429956,0.900086,0.0243665, +1,-0.0596441,-0.10401,0.493655,-0.066251,0.429956,0.900086,0.0243665, +2,-0.0596441,-0.10401,0.493655,-0.066251,0.429956,0.900086,0.0243665, +3,0.138246,-0.0498245,0.450293,0.278378,0.883805,-0.308363,0.215192, +4,0.138246,-0.0498245,0.450293,0.278378,0.883805,-0.308363,0.215192, +5,0.138246,-0.0498245,0.450293,0.278378,0.883805,-0.308363,0.215192, +6,-0.0947857,0.041793,0.310745,-0.0216406,0.915369,-0.39597,0.0695633, +7,-0.0947857,0.041793,0.310745,-0.0216406,0.915369,-0.39597,0.0695633, +8,-0.0947857,0.041793,0.310745,-0.0216406,0.915369,-0.39597,0.0695633, diff --git a/demos/processor_tracker_landmark_object.yaml b/demos/processor_tracker_landmark_object.yaml index f95f2422744379c4e658feb8d41e734a99da12a4..d80bfcf56611b66c1dee75f6ffceab45070bcee8 100644 --- a/demos/processor_tracker_landmark_object.yaml +++ b/demos/processor_tracker_landmark_object.yaml @@ -5,7 +5,7 @@ time_tolerance: 0.1222 vote: voting_active: true min_time_vote: 0 # s - max_time_vote: 0.1 # s + max_time_vote: 0.5 # s min_features_for_keyframe: 1 nb_vote_for_every_first: 0 diff --git a/demos/result.csv b/demos/result.csv index aacb66784956486e690447764e07455294ca58c9..b08d731ad443e69e5eff0c60a3936a0284b2ed92 100644 --- a/demos/result.csv +++ b/demos/result.csv @@ -1,164 +1,33 @@ -id,t,px,py,pz,qx,qy,qz,qw -0,0.000000000,1.84381e-13,-2.798e-11,-1.70113e-12,6.32843e-10,3.91335e-11,7.99363e-11,1, -1,0.099518000,-0.0104137,0.0077851,0.00181263,0.012509,0.00531949,0.00100668,0.999907, -2,0.199035000,-0.0150023,0.00500016,0.00324575,0.0109784,0.0036419,-0.00238622,0.99993, -3,0.298553000,-0.022212,0.000700687,0.00358512,0.00702993,0.00392236,0.00168232,0.999966, -4,0.398070000,-0.043199,-0.00203051,0.00757717,0.00688483,0.0165713,0.00568339,0.999823, -5,0.497588000,-0.0460949,-0.00298447,0.0056032,0.00580757,0.00856018,0.00211528,0.999944, -6,0.597106000,-0.0498825,-0.00230925,0.0059157,0.0083041,0.00695186,0.0006663,0.999941, -7,0.696623000,-0.056499,-0.00266882,0.00454185,0.009491,0.00548888,0.0055494,0.999924, -8,0.796141000,-0.0875173,-0.021327,0.0180811,-0.0102778,0.0388762,0.0188157,0.999014, -9,0.895658000,-0.0784256,-0.0213351,0.0145278,-0.0111415,0.030505,0.0135028,0.999381, -10,0.995176000,-0.0933745,-0.0336061,0.0249545,-0.0261619,0.0606001,0.0267275,0.997461, -11,1.094694000,-0.1056,-0.033298,0.0261718,-0.0239909,0.0825309,0.0372965,0.995601, -12,1.194211000,-0.114915,-0.0265249,0.034208,-0.0172216,0.0956748,0.0446871,0.99426, -13,1.293729000,-0.120425,-0.0362641,0.042266,-0.0238592,0.100829,0.0534247,0.993182, -14,1.393246000,-0.139274,-0.0487197,0.0550595,-0.0270022,0.121781,0.0730903,0.989494, -15,1.492764000,-0.148259,-0.0450491,0.0579103,-0.0184896,0.128445,0.0712859,0.988978, -16,1.592281000,-0.143472,-0.0414073,0.0573522,-0.0171456,0.122076,0.0652742,0.990224, -17,1.691799000,-0.137973,-0.0432384,0.0556763,-0.0227657,0.115468,0.0665541,0.990818, -18,1.791317000,-0.121846,-0.0433565,0.0480688,-0.0251673,0.094104,0.059017,0.993493, -19,1.890834000,-0.120699,-0.0420157,0.0497147,-0.0242278,0.0955087,0.0511068,0.993821, -20,1.990352000,-0.11225,-0.0407377,0.0476949,-0.0237189,0.0901472,0.047366,0.994519, -21,2.089869000,-0.0896885,-0.0431687,0.0457076,-0.0332902,0.0737242,0.0410016,0.995879, -22,2.189387000,-0.0916551,-0.0425754,0.0545092,-0.0343731,0.0951581,0.0446645,0.993865, -23,2.288905000,-0.0473993,-0.0139747,0.0420146,-0.0011778,0.0459912,0.0279583,0.99855, -24,2.388422000,-0.0266703,-0.0259846,0.0467664,-0.0246655,0.0347434,0.0307871,0.998617, -25,2.454767000,-0.0158515,-0.0183473,0.0454522,-0.0187268,0.029791,0.0266378,0.999026, -26,2.554285000,0.0168385,-0.00714708,0.0500795,-0.00987754,-0.00299172,0.0229145,0.999684, -27,2.653802000,0.0323923,-0.00174456,0.0462748,-0.00618191,-0.0126582,0.0187626,0.999725, -28,2.753320000,0.0587395,-0.00156211,0.0519053,-0.0110108,-0.0357037,0.0100864,0.999251, -29,2.852837000,0.0736532,0.00130476,0.051088,-0.0275864,-0.0449859,0.00827483,0.998572, -30,2.952355000,0.103807,0.0131061,0.0595308,-0.0228591,-0.0827264,0.00852907,0.996274, -31,3.051873000,0.123449,0.019038,0.0606726,-0.0147712,-0.107694,0.000944441,0.994074, -32,3.151390000,0.14723,0.030637,0.0689698,-0.00947022,-0.144728,-0.00303976,0.989422, -33,3.250908000,0.171337,0.0251339,0.0783293,-0.0232564,-0.176322,-0.00718808,0.984032, -34,3.350425000,0.182818,0.0106731,0.0887363,-0.0390438,-0.191863,-0.00832991,0.980609, -35,3.449943000,0.195935,0.0117309,0.096873,-0.0330681,-0.200225,-0.00886433,0.979152, -36,3.549461000,0.224604,0.00575342,0.113806,-0.036768,-0.218022,-0.0211271,0.975022, -37,3.648978000,0.238399,0.0162188,0.121392,-0.0255938,-0.21069,-0.0144261,0.977111, -38,3.748496000,0.248664,0.0159724,0.12947,-0.0324751,-0.206609,-0.0127105,0.977802, -39,3.848013000,0.26324,0.0166943,0.137962,-0.0306332,-0.221492,-0.0177966,0.974518, -40,3.947531000,0.258683,0.0158601,0.141042,-0.0376487,-0.212692,-0.0146157,0.976284, -41,4.047048000,0.251891,0.0153758,0.138987,-0.0379931,-0.201472,-0.0117588,0.978686, -42,4.146566000,0.251266,0.00827269,0.147562,-0.0434442,-0.199653,-0.0218317,0.97866, -43,4.246083999,0.240591,0.0179148,0.146538,-0.0253465,-0.18316,-0.0089846,0.982715, -44,4.345601000,0.217858,0.0172101,0.126079,-0.0233371,-0.152513,0.00630314,0.988006, -45,4.445119000,0.211473,0.0181028,0.127235,-0.0173805,-0.154801,0.00703134,0.987768, -46,4.544636000,0.194919,0.0120887,0.123016,-0.0238574,-0.136024,0.00767788,0.990388, -47,4.644154000,0.17808,0.00930125,0.118699,-0.0350668,-0.112572,0.0168418,0.992882, -48,4.743672000,0.164653,0.000632572,0.114512,-0.0441494,-0.0993138,0.0134491,0.993985, -49,4.843189000,0.149283,0.0124682,0.105175,-0.0258574,-0.0887002,0.0249288,0.995411, -50,4.942707000,0.120361,0.0113026,0.0938186,-0.0238952,-0.0649532,0.0316011,0.997102, -51,5.042224000,0.091644,0.0020601,0.0857252,-0.0326401,-0.0446763,0.0373065,0.997771, -52,5.141742000,0.0565977,-0.0104995,0.0827468,-0.0392545,-0.0180505,0.0400642,0.998263, -53,5.241259000,0.0375933,-0.0261576,0.0810137,-0.0460884,-0.00750037,0.0391917,0.99814, -54,5.340777000,0.0303904,-0.0354298,0.0674925,-0.0439438,-0.00208782,0.0453312,0.998003, -55,5.440295000,0.0163903,-0.0400611,0.0535455,-0.0465724,0.0192031,0.0500442,0.997476, -56,5.539812000,0.0112069,-0.0443687,0.035413,-0.0499705,0.0266689,0.0487605,0.997203, -57,5.639330000,0.0125292,-0.0419698,0.0161972,-0.0490445,0.0323669,0.0402546,0.99746, -58,5.738847000,0.0134146,-0.0450088,-0.00116133,-0.0526253,0.0396234,0.038302,0.997093, -59,5.838365000,0.0149326,-0.0483298,-0.0216591,-0.0526733,0.0444989,0.0324132,0.997093, -60,5.937882000,0.0152858,-0.053366,-0.0406393,-0.0529505,0.046936,0.0347396,0.996888, -61,6.037400000,0.0162815,-0.062939,-0.065322,-0.0569219,0.0447831,0.0329997,0.996828, -62,6.136918000,0.00414485,-0.0690722,-0.0866392,-0.055911,0.0554775,0.033223,0.99634, -63,6.236435000,0.00472297,-0.0686868,-0.110617,-0.0541613,0.0504789,0.0306583,0.996784, -64,6.335953000,0.00398202,-0.0752438,-0.131449,-0.0600321,0.0459694,0.0270537,0.99677, -65,6.435470000,0.0172541,-0.0724002,-0.155408,-0.0551584,0.0327342,0.0210449,0.997719, -66,6.534988000,0.0171284,-0.0823973,-0.185068,-0.061622,0.0245121,0.0211813,0.997574, -67,6.634506000,0.0152082,-0.0775584,-0.209378,-0.0563504,0.00859359,0.0140341,0.998275, -68,6.734023000,0.0150379,-0.0703369,-0.248667,-0.0513236,-0.0102172,0.00688141,0.998606, -69,6.833541000,0.0162264,-0.0740766,-0.27287,-0.0575273,-0.0296271,0.00960728,0.997858, -70,6.899886000,0.0178112,-0.0752163,-0.282423,-0.0715167,-0.0408183,0.0020104,0.996602, -71,7.032576000,0.0346229,-0.0621397,-0.290854,-0.067904,-0.0653595,-0.00767216,0.995519, -72,7.132093000,0.0386856,-0.0683244,-0.294024,-0.0763505,-0.063426,-0.0145615,0.994955, -73,7.231611000,0.0425411,-0.0479403,-0.289069,-0.0630441,-0.0632099,-0.0163591,0.995873, -74,7.331129000,0.0525976,-0.00951206,-0.288911,-0.0490016,-0.0703121,-0.0135914,0.996228, -75,7.430646000,0.0723648,0.0273674,-0.278411,-0.0366176,-0.0876624,-0.0159621,0.995349, -76,7.530164000,0.0705168,0.0427728,-0.270447,-0.0402731,-0.0879527,-0.0193786,0.995122, -77,7.629681000,0.0925511,0.04559,-0.249387,-0.0498127,-0.102857,-0.0256313,0.993117, -78,7.729199000,0.089142,0.0537832,-0.230176,-0.0564244,-0.0925481,-0.0275528,0.993726, -79,7.828717000,0.0821574,0.0693259,-0.206654,-0.0511938,-0.0792254,-0.0183612,0.995372, -80,7.928234000,0.0845874,0.0797422,-0.184352,-0.0399376,-0.0712882,-0.015193,0.99654, -81,8.027752000,0.0815232,0.0538593,-0.162747,-0.056871,-0.0603787,-0.0130122,0.996469, -82,8.127269000,0.0791333,0.0578464,-0.13617,-0.0481003,-0.0535571,-0.00277099,0.997402, -83,8.226787000,0.0785472,0.0431487,-0.11077,-0.0520426,-0.0549184,-0.000718456,0.997133, -84,8.326304000,0.0724949,0.0525543,-0.0889835,-0.0334881,-0.0567562,-0.00390391,0.997819, -85,8.425822000,0.0706863,0.0645771,-0.0584816,-0.0228621,-0.0561139,-0.00659697,0.998141, -86,8.525340000,0.072803,0.046609,-0.0332548,-0.0241582,-0.0583373,-0.00348289,0.997999, -87,8.624857000,0.069845,0.029253,-0.00308634,-0.0348911,-0.0498759,0.00177635,0.998144, -88,8.724375000,0.0692812,0.0193106,0.0171899,-0.0355564,-0.0466686,0.00576222,0.998261, -89,8.823892000,0.0697556,0.0192878,0.0346468,-0.0220945,-0.0443491,0.0107119,0.998714, -90,8.923410000,0.0769873,0.00953147,0.0571878,-0.0197131,-0.0494594,0.0119115,0.998511, -91,9.022927000,0.0697975,0.000874978,0.0746585,-0.0122585,-0.0318222,0.0160514,0.999289, -92,9.122445000,0.0573281,-0.0206904,0.0874244,-0.0273264,-0.00317054,0.023698,0.999341, -93,9.221963000,0.0603567,-0.0392483,0.0869072,-0.0449545,0.00192528,0.0225709,0.998732, -94,9.321480000,0.056046,-0.0497229,0.0857417,-0.0675955,0.0106935,0.0198066,0.997459, -95,9.420998000,0.0561238,-0.059022,0.0757563,-0.101091,0.00633783,0.00813291,0.994824, -96,9.520515000,0.058809,-0.0559871,0.0657321,-0.108285,-0.000745311,0.000813119,0.994119, -97,9.620033000,0.0419056,-0.0707626,0.0517127,-0.148834,0.0207255,0.0134448,0.988554, -98,9.719551000,0.0272557,-0.0902503,0.0489875,-0.198014,0.0349692,0.0208193,0.979354, -99,9.819068000,0.0309949,-0.101266,0.0402726,-0.209646,0.0183186,0.0148517,0.977493, -100,9.918586000,0.0394371,-0.110608,0.0288683,-0.225181,-0.000799872,0.0125256,0.974236, -101,10.018103000,0.040246,-0.123661,0.0251319,-0.241906,-0.00314017,0.0143593,0.970188, -102,10.117621000,0.0540018,-0.127667,0.0226253,-0.25238,-0.0132895,0.00485339,0.967525, -103,10.217138000,0.0631226,-0.130795,0.0252264,-0.261089,-0.0139497,0.00357404,0.965207, -104,10.316656000,0.0703676,-0.155488,0.0362531,-0.281187,-0.0138255,0.0103031,0.959498, -105,10.416174000,0.0752088,-0.162294,0.0426866,-0.293234,-0.0158885,0.0170967,0.955756, -106,10.515691000,0.0889143,-0.177128,0.0442056,-0.297123,-0.0303522,0.0145315,0.954246, -107,10.615209000,0.0843939,-0.182431,0.0418268,-0.293906,-0.0248731,0.0239809,0.95521, -108,10.714726000,0.0881223,-0.181943,0.0378129,-0.289595,-0.0337823,0.0165374,0.95641, -109,10.814244000,0.068785,-0.17998,0.0321479,-0.286489,-0.0111194,0.0238578,0.957722, -110,10.913762000,0.0769703,-0.169494,0.0296451,-0.268371,-0.0219404,0.0137906,0.962967, -111,11.013279000,0.069634,-0.148636,0.0225558,-0.233637,-0.00744512,0.0190131,0.97211, -112,11.112797000,0.0632018,-0.14079,0.0226302,-0.212582,0.00352945,0.0238063,0.976847, -113,11.212314000,0.0531655,-0.125949,0.0216365,-0.184704,0.0174035,0.023767,0.982353, -114,11.311832000,0.0399421,-0.110837,0.0297894,-0.165153,0.0408712,0.0292283,0.984987, -115,11.411349000,0.0381459,-0.0954268,0.0427067,-0.13699,0.0530509,0.0291966,0.98872, -116,11.510867000,0.0396894,-0.0826852,0.0521846,-0.109137,0.0619422,0.0305627,0.991624, -117,11.610385000,0.0497736,-0.0611696,0.0646781,-0.0730433,0.0497281,0.0233725,0.995814, -118,11.709902000,0.0533534,-0.0569946,0.0731508,-0.0583674,0.0493631,0.0132993,0.996985, -119,11.809420000,0.0491284,-0.0531697,0.0820769,-0.0556825,0.0547627,0.00542945,0.996931, -120,11.908937000,0.0448327,-0.0548328,0.0820293,-0.0444708,0.0542148,-0.0142108,0.997437, -121,12.008455000,0.0458415,-0.0639888,0.0767052,-0.0491098,0.0473662,-0.0457252,0.996621, -122,12.107973000,0.050727,-0.070487,0.0784987,-0.0666401,0.0294514,-0.0782115,0.994271, -123,12.207490000,0.0572982,-0.0671847,0.0764452,-0.0667752,0.0117672,-0.106679,0.991979, -124,12.307008000,0.0619866,-0.0567163,0.0702281,-0.0475529,0.000751524,-0.136666,0.989475, -125,12.406525000,0.0755938,-0.0410174,0.066231,-0.0214464,-0.0205741,-0.169173,0.985138, -126,12.506043000,0.081834,-0.0376735,0.0629567,-0.0132537,-0.0304331,-0.205027,0.978193, -127,12.605560000,0.0654946,-0.030638,0.0581242,-0.0061305,-0.00770281,-0.229276,0.973312, -128,12.705078000,0.0576659,-0.0299603,0.0556941,-0.00502055,0.00559095,-0.249172,0.96843, -129,12.804596000,0.0543494,-0.033653,0.0553541,-0.0155178,0.00772726,-0.266656,0.963636, -130,12.904113000,0.0713449,-0.043482,0.057064,-0.0301959,-0.0202825,-0.252863,0.966818, -131,13.003631000,0.0680846,-0.040269,0.052959,-0.0252205,-0.0106395,-0.221969,0.974669, -132,13.103148000,0.0718916,-0.0409655,0.0552942,-0.0292326,-0.0168455,-0.190047,0.981195, -133,13.202666000,0.0648447,-0.0534973,0.0590963,-0.0542405,-0.0033763,-0.148762,0.987379, -134,13.302183000,0.0539831,-0.0727274,0.0587357,-0.0899558,0.0121011,-0.106478,0.990164, -135,13.401701000,0.0431021,-0.0733317,0.053925,-0.0920229,0.0275593,-0.0484777,0.994194, -136,13.501219000,0.0321343,-0.0693872,0.0549765,-0.0955572,0.0323155,-0.0150932,0.994785, -137,13.600736000,0.0320299,-0.0763657,0.0563521,-0.100079,0.0285241,0.0149607,0.994458, -138,13.700254000,0.0297575,-0.0939197,0.0524466,-0.103565,0.029246,0.0382136,0.993458, -139,13.799771000,0.0320771,-0.0937671,0.0497685,-0.0863712,0.0155977,0.0391974,0.995369, -140,13.899289000,0.0316049,-0.103048,0.0462776,-0.0856385,0.010246,0.0402937,0.995458, -141,13.998807000,0.0299156,-0.101265,0.0419338,-0.0895835,0.0153219,0.0404324,0.99504, -142,14.098324000,0.032881,-0.0949276,0.0422738,-0.0916909,0.0118868,0.0379333,0.994994, -143,14.197842000,0.0330255,-0.0918447,0.0412266,-0.0894177,0.0122519,0.0399016,0.995119, -144,14.297359000,0.0338196,-0.0932731,0.0424361,-0.0917032,0.00997358,0.0334223,0.995175, -145,14.396877000,0.0306872,-0.0954926,0.0437848,-0.0958261,0.0150253,0.0387382,0.994531, -146,14.496394000,0.0313544,-0.0918321,0.0469432,-0.0943184,0.0135618,0.0411557,0.994599, -147,14.595912000,0.0342644,-0.0906378,0.048435,-0.0933246,0.00878221,0.034582,0.994996, -148,14.695430000,0.0285105,-0.0891108,0.0481628,-0.0907282,0.0160101,0.0344015,0.995153, -149,14.794947000,0.0304786,-0.0898752,0.0498181,-0.0918774,0.0142336,0.0309056,0.995189, -150,14.894465000,0.0314291,-0.0904777,0.0523856,-0.091754,0.0120071,0.0309784,0.995227, -151,14.993982000,0.030663,-0.0927115,0.0534406,-0.0948109,0.0122253,0.033287,0.994864, -152,15.093500000,0.0312085,-0.0939924,0.0539139,-0.096421,0.00975606,0.0308862,0.994813, -153,15.193018000,0.0239234,-0.0929377,0.0537589,-0.0972926,0.018603,0.0304691,0.994615, -154,15.292535000,0.0235872,-0.0931819,0.0559615,-0.101282,0.0188743,0.0287564,0.994263, -155,15.392053000,0.0248763,-0.091923,0.0552244,-0.098166,0.0148479,0.0277217,0.994673, -156,15.491570000,0.0266008,-0.091795,0.0562652,-0.0985591,0.0114943,0.0239646,0.994776, -157,15.591088000,0.0268559,-0.0906483,0.0566647,-0.0985346,0.0123805,0.0207945,0.994839, -158,15.690605000,0.0247102,-0.0886304,0.055933,-0.0970209,0.0165746,0.0198804,0.994946, -159,15.790123000,0.0231996,-0.0890532,0.0576433,-0.0992022,0.0189015,0.0200766,0.994685, -160,15.889641000,0.0255563,-0.0849885,0.0578528,-0.094851,0.0168522,0.0194006,0.99516, -161,15.989158000,0.0277467,-0.0848212,0.0593826,-0.0953652,0.0136328,0.0173066,0.995199, -162,16.088676000,0.0277667,-0.0833906,0.0598822,-0.0931765,0.0134077,0.0174467,0.995406, +id,t,px,py,pz,qx,qy,qz,qw, +0,0.000000000,2.51663e-11,-8.71181e-09,-4.19875e-09,2.05988e-09,-1.58398e-10,2.92981e-10,1, +1,0.497588000,-0.0264847,-8.46493e-05,-0.00399219,-0.0117422,0.00778784,0.0182128,0.999735, +2,0.995176000,-0.0499237,-0.00899706,-0.00119966,-0.0221522,0.0230906,0.0293954,0.999056, +3,1.492764000,-0.00546354,-0.00219766,0.00125746,-0.000762948,0.0011901,-0.000294146,0.999999, +4,1.990352000,1.67905e-05,0.00358173,-0.00627669,0.000321343,-0.00010852,-0.000128037,1, +5,2.487940000,-0.374216,-0.148556,0.151291,0.0157886,0.647296,0.397281,0.650328, +6,2.985528000,-0.266949,-0.113775,0.126278,-0.0640574,0.276114,0.209147,0.935903, +7,3.483116000,-0.299931,-0.174855,0.177958,-0.0802549,0.350739,0.315318,0.878132, +8,3.980703000,-0.309088,-0.14069,0.159908,-0.0569284,0.426073,0.279001,0.858708, +9,4.478291000,-0.316165,-0.196705,0.20063,-0.0472104,0.452595,0.342629,0.821909, +10,4.975879000,0.195235,0.0574257,0.729687,0.827567,0.415299,-0.37767,0.00496301, +11,5.473467000,0.153329,0.0181056,0.785504,-0.88573,-0.350887,0.301308,-0.0396724, +12,5.971055000,0.0903288,-0.0429045,0.833297,0.952284,0.245414,-0.179256,0.0281784, +13,6.468643000,-0.0148607,-0.078459,0.864282,0.978617,0.189864,-0.0784434,0.0103701, +14,6.966231000,-0.102861,-0.0923376,0.846282,-0.994335,-0.102617,-0.0166198,0.0221761, +15,7.463819000,-0.180509,-0.0665181,0.825259,-0.991245,-0.0445852,-0.121824,-0.0245684, +16,7.961407000,-0.267635,-0.0598841,0.787735,0.981775,-0.0100056,0.189779,0.00133286, +17,8.458995000,0.575689,-0.133409,0.355518,-0.45691,-0.381146,0.334623,0.730745, +18,8.956582000,0.148425,0.0273143,0.107987,0.075892,0.130381,0.185701,0.970956, +19,9.454170000,0.0749393,-0.0205812,0.133996,0.0693465,0.205411,0.227354,0.949372, +20,9.926683000,-0.469961,0.0547173,0.549326,0.804822,-0.242192,0.541524,0.0188678, +21,10.407293000,-0.46782,0.0899827,0.503899,0.737044,-0.303739,0.603665,0.00988252, +22,10.879805000,0.413893,-0.215398,0.369712,0.22761,-0.594826,-0.533465,0.556589, +23,11.377393000,0.421733,-0.0537895,0.280106,0.338641,-0.441323,-0.496279,0.666531, +24,11.874981000,0.325699,0.0936254,0.220561,0.373795,-0.261175,-0.414722,0.787445, +25,12.372569000,-0.512935,-0.15546,0.116831,-0.0978231,0.597053,0.478813,0.636158, +26,12.870157000,0.373667,-0.0455879,0.130669,0.13608,-0.377683,-0.40757,0.820198, +27,13.367745000,0.202168,0.0942178,0.0352605,0.121426,-0.229017,-0.190458,0.946854, +28,13.865333000,0.108157,0.0604562,-0.0121661,0.0839108,-0.138218,-0.15068,0.975269, +29,14.362921000,0.0124425,0.0516145,-0.0236974,0.0974592,-0.0172597,-0.0820719,0.9917, +30,14.860509000,0.120984,0.0598476,-0.00225833,0.127159,-0.135539,-0.128828,0.974096, +31,15.358097000,-0.0483837,0.0282599,-0.0341755,0.0736267,0.00758949,-0.0435516,0.996306,