diff --git a/.gitignore b/.gitignore index 5b1eba01c9bdefe79c1ce1dea4c82eb893e53ff8..234d8709d04e4578c066a395001bc29c18664178 100644 --- a/.gitignore +++ b/.gitignore @@ -27,6 +27,8 @@ src/examples/map_polyline_example_write.yaml *.csv *.csv# +results_* + src/CMakeCache.txt src/CMakeFiles/cmake.check_cache diff --git a/demos/cosyslam.cpp b/demos/cosyslam.cpp index 13eedb5557e34d09c1c65febfc8f9ef49966354b..0f67e0962df6dc8542928b8b88cec776d3d04b0a 100644 --- a/demos/cosyslam.cpp +++ b/demos/cosyslam.cpp @@ -137,7 +137,7 @@ int main() ///////////////////////// std::string wolf_root = _WOLF_OBJECTSLAM_ROOT_DIR; - std::string filename = wolf_root + "/demos/input_demoMulti.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); @@ -145,11 +145,6 @@ int main() // setup the wolf problem // //////////////////////////// - // retrieve parameters from yaml file - // YAML::Node config = YAML::LoadFile(wolf_root + "/demos/processor_tracker_landmark_object.yaml"); - // std::cout << config['vote']['max_time_vote'].as<double>(); - double kf_dt = 0.1; - ProblemPtr problem = Problem::create("POV", 3); SolverCeresPtr solver = std::make_shared<SolverCeres>(problem); solver->getSolverOptions().max_num_iterations = 200; @@ -177,7 +172,8 @@ int main() /////////////// double t_cur = 0; - double t_since_last_kf = 0; + int number_of_kf = 0; + int counter = 0; for (int i = 0; i < len; i++) { @@ -206,16 +202,16 @@ int main() CaptureObjectPtr cap = std::make_shared<CaptureObject>(t_cur, sen, dets); cap->process(); - t_since_last_kf += t-t_cur; dets.clear(); dets.push_back(det); } t_cur = t; - // Solve if a KF should be voted - if (t_since_last_kf > kf_dt){ + // Solve if a KF was voted + number_of_kf = problem->getTrajectory()->getFrameMap().size(); + if (number_of_kf > counter){ std::string report = solver->solve(SolverCeres::ReportVerbosity::FULL); - t_since_last_kf = 0; + counter++; } } @@ -240,7 +236,7 @@ int main() VectorComposite state_est; CaptureBasePtr cap_cosy; - int counter = 0; + counter = 0; for (auto& elt: problem->getTrajectory()->getFrameMap()){ TimeStamp t = elt.first; FrameBasePtr kf = elt.second; @@ -253,15 +249,13 @@ int main() << state_est['O'](0) << "," << state_est['O'](1) << "," << state_est['O'](2) << "," - << state_est['O'](3) << "," - << "\n"; + << state_est['O'](3) << "\n"; counter++; } counter = 0; for (auto& elt: problem->getMap()->getLandmarkList()){ state_est = elt->getState(); - if (elt->getConstrainedByList().size() < 15){ continue; } @@ -272,8 +266,7 @@ int main() << state_est['O'](0) << "," << state_est['O'](1) << "," << state_est['O'](2) << "," - << state_est['O'](3) << "," - << "\n"; + << state_est['O'](3) << "\n"; counter++; } file_res.close(); diff --git a/demos/map.csv b/demos/map.csv deleted file mode 100644 index ed18c9e7d31c0679baab56ffb3d6b127655161a0..0000000000000000000000000000000000000000 --- a/demos/map.csv +++ /dev/null @@ -1,7 +0,0 @@ -id,px,py,pz,qx,qy,qz,qw, -0,-0.0586016,-0.103967,0.490275,-0.0429536,0.420095,0.905714,0.0368407, -1,-0.160589,0.0243347,0.339189,-0.169076,0.90165,-0.397773,-0.0147582, -2,0.133311,-0.0155249,0.4244,0.195321,0.903983,-0.354899,0.136787, -3,0.148708,-0.0284635,0.414177,0.90161,-0.207589,0.0839353,0.370082, -4,-0.0559744,-0.0953355,0.488029,0.905121,-0.0368711,0.0457529,0.421075, -5,-0.146218,0.0292149,0.339253,0.908714,0.15218,-0.0469421,0.385845, diff --git a/demos/result.csv b/demos/result.csv deleted file mode 100644 index 400ccbadc376d3c76e2d63cc2274d181d47d6ec1..0000000000000000000000000000000000000000 --- a/demos/result.csv +++ /dev/null @@ -1,160 +0,0 @@ -id,t,px,py,pz,qx,qy,qz,qw, -0,0.000000000,0.00053724,0.000209711,-0.000341171,2.71308e-05,0.000158285,3.95115e-05,1, -1,0.099518000,-0.00513182,0.000146188,0.000370784,-0.00361063,-0.00156841,-0.000563434,0.999992, -2,0.199035000,-0.0136749,0.00163525,0.00248326,-0.00542409,-0.000310748,-0.00170426,0.999984, -3,0.298553000,-0.0170197,0.00576942,0.00122761,-0.0013344,0.00191335,0.00293387,0.999993, -4,0.398070000,-0.0169183,0.00302827,2.26597e-05,-0.00852854,-0.000791014,0.0137308,0.999869, -5,0.497588000,-0.0220316,0.00292576,-0.000118058,-0.0104321,0.00339222,0.0180385,0.999777, -6,0.597106000,-0.0257956,0.00810196,0.000230124,-0.0054016,0.00604,0.0184232,0.999797, -7,0.696623000,-0.0308121,0.0106795,0.00125538,-0.00272841,0.00758898,0.0182663,0.999801, -8,0.796141000,-0.0333355,0.00919541,0.00245237,-0.00446331,0.00459052,0.0186461,0.999806, -9,0.895658000,-0.0455784,0.010945,0.00270817,-0.00174839,0.0144349,0.0204059,0.999686, -10,0.995176000,-0.0531691,0.00475043,0.00439845,-0.00959216,0.0256361,0.0218899,0.999386, -11,1.094694000,-0.0559504,0.00337196,0.00379001,-0.0104416,0.0356446,0.029455,0.998876, -12,1.194211000,-0.0675741,4.70758e-05,0.00444209,-0.0118591,0.0556111,0.0302859,0.997923, -13,1.293729000,-0.0757968,-0.00430823,0.00637308,-0.0192157,0.0704855,0.0307826,0.996853, -14,1.393246000,-0.0884369,-0.00416602,0.008664,-0.0223372,0.0861481,0.0373211,0.995332, -15,1.492764000,-0.0951528,-0.00768993,0.00901654,-0.0278844,0.0951243,0.0444495,0.994082, -16,1.592281000,-0.1023,-0.0157616,0.010213,-0.0397695,0.105373,0.0514284,0.992305, -17,1.691799000,-0.114308,-0.0230877,0.00574492,-0.0423539,0.120594,0.0593172,0.990023, -18,1.791317000,-0.124822,-0.0221476,0.00693936,-0.0404716,0.130474,0.0677933,0.988303, -19,1.890834000,-0.144722,-0.02576,0.00876419,-0.037595,0.149435,0.0798066,0.984828, -20,1.990352000,-0.16076,-0.018139,0.0113476,-0.0178859,0.160199,0.0917667,0.982647, -21,2.089869000,-0.167309,-0.0317906,0.0192576,-0.042076,0.154969,0.0995104,0.981994, -22,2.189387000,-0.190313,-0.00984323,0.038503,-0.0144043,0.172705,0.0981956,0.979961, -23,2.288905000,-0.188884,-0.0346684,0.0362775,-0.0333967,0.16933,0.118916,0.977789, -24,2.388422000,-0.195873,-0.0338648,0.0426711,-0.0193701,0.187357,0.141709,0.971823, -25,2.487940000,-0.215185,-0.020852,0.0695072,-0.00952479,0.217175,0.143714,0.965448, -26,2.587457000,-0.244234,-0.0571019,0.0831014,-0.0353462,0.256494,0.146986,0.95465, -27,2.686975000,-0.263654,-0.105975,0.0967354,-0.0630118,0.284834,0.182512,0.938929, -28,2.786492000,-0.305312,-0.150361,0.160575,-0.122352,0.363786,0.175758,0.906531, -29,2.886010000,-0.281142,-0.0443163,0.141909,0.0223431,0.289803,0.200921,0.935492, -30,2.985528000,-0.281858,-0.0835571,0.144254,-0.0237221,0.296934,0.227303,0.927147, -31,3.085045000,-0.281035,-0.0988749,0.13929,-0.0382468,0.294899,0.245,0.922793, -32,3.184563000,-0.296326,-0.120541,0.153224,-0.0480498,0.320811,0.272211,0.90591, -33,3.284080000,-0.325929,-0.117246,0.16646,-0.0232597,0.350321,0.267959,0.89718, -34,3.383598000,-0.322726,-0.14956,0.197416,-0.0643757,0.382737,0.270669,0.880969, -35,3.483116000,-0.329372,-0.174791,0.209607,-0.0747588,0.40633,0.291665,0.862693, -36,3.582633000,-0.331003,-0.18536,0.210608,-0.0741639,0.419548,0.305316,0.851623, -37,3.682151000,-0.358013,-0.172272,0.223072,-0.0442215,0.457274,0.2929,0.838543, -38,3.781668000,-0.348984,-0.192005,0.233119,-0.0649382,0.478234,0.32604,0.81288, -39,3.881186000,-0.346531,-0.20274,0.230983,-0.0798857,0.491917,0.361802,0.787867, -40,3.980703000,-0.3646,-0.203694,0.240081,-0.0682764,0.514797,0.381632,0.764644, -41,4.080221000,-0.367784,-0.217434,0.246491,-0.0768209,0.527132,0.384989,0.753667, -42,4.179738999,-0.369313,-0.230965,0.247703,-0.0739353,0.5292,0.400791,0.74421, -43,4.279256000,-0.367806,-0.236146,0.257058,-0.0681718,0.532719,0.403053,0.741021, -44,4.378774000,-0.360966,-0.253817,0.267727,-0.0736677,0.536983,0.411186,0.732904, -45,4.478291000,-0.356194,-0.260566,0.2859,-0.0674436,0.547445,0.41782,0.721929, -46,4.577809000,-0.347382,-0.279067,0.297418,-0.0665367,0.55691,0.425552,0.710161, -47,4.677326000,-0.355274,-0.289215,0.316212,-0.0656573,0.573027,0.426231,0.69689, -48,4.776844000,-0.35172,-0.289637,0.325677,-0.0607747,0.586768,0.445959,0.673149, -49,4.876362000,-0.343831,-0.299602,0.3306,-0.0680288,0.590151,0.465662,0.655937, -50,4.975879000,-0.335976,-0.316221,0.346386,-0.070636,0.608964,0.464976,0.638726, -51,5.075397000,-0.3291,-0.328522,0.354674,-0.0736795,0.61037,0.472567,0.631427, -52,5.174914000,-0.32419,-0.34236,0.368759,-0.0811295,0.615796,0.485484,0.615239, -53,5.274432000,-0.319914,-0.3488,0.381846,-0.0730846,0.620746,0.501603,0.598103, -54,5.373950000,-0.316748,-0.360631,0.390259,-0.0740133,0.624262,0.515846,0.581998, -55,5.473467000,-0.302488,-0.37625,0.403158,-0.0830555,0.634653,0.535983,0.55049, -56,5.572985000,-0.297309,-0.381951,0.409943,-0.0726007,0.652202,0.559952,0.505782, -57,5.672502000,-0.295171,-0.389542,0.415622,-0.0648161,0.658975,0.578528,0.476293, -58,5.772020000,-0.285562,-0.394289,0.432467,-0.0633248,0.670888,0.586663,0.449139, -59,5.871537000,-0.272964,-0.398304,0.430232,-0.0523442,0.666572,0.611187,0.423548, -60,5.971055000,-0.256074,-0.413853,0.456628,-0.0672501,0.688057,0.604012,0.396517, -61,6.070573000,-0.233999,-0.428105,0.471917,-0.0821157,0.697747,0.604187,0.375984, -62,6.170090000,-0.233279,-0.426539,0.462655,-0.0618571,0.693073,0.610285,0.378651, -63,6.269608000,-0.225918,-0.420778,0.45728,-0.0512414,0.693629,0.613488,0.374013, -64,6.369125000,-0.209131,-0.438509,0.49849,-0.0570172,0.726989,0.594969,0.338007, -65,6.468643000,-0.198549,-0.434239,0.49566,-0.0553898,0.725616,0.602805,0.327169, -66,6.568161000,-0.174487,-0.440533,0.527176,-0.0434587,0.751118,0.60029,0.271264, -67,6.667678000,-0.178558,-0.432371,0.513209,-0.0501925,0.732739,0.61147,0.294413, -68,6.767196000,-0.11477,-0.452976,0.524859,-0.0588536,0.745566,0.633698,0.197723, -69,6.866713000,-0.0925751,-0.454144,0.524687,-0.0736815,0.751255,0.630304,0.181393, -70,6.966231000,-0.0797948,-0.444726,0.541369,-0.07235,0.77739,0.603674,0.161273, -71,7.065748000,-0.0617634,-0.444076,0.524442,-0.0703327,0.768783,0.62015,0.139428, -72,7.165266000,-0.0620171,-0.450712,0.564789,-0.071648,0.795986,0.586134,0.133115, -73,7.264784000,-0.0423108,-0.445309,0.552545,-0.0871132,0.787158,0.600611,0.109823, -74,7.364301000,-0.0313327,-0.441424,0.54626,-0.0885707,0.780991,0.612026,0.0873678, -75,7.463819000,-0.0151469,-0.442475,0.541488,-0.0857192,0.772269,0.626373,0.0625323, -76,7.563336000,0.00791469,-0.443128,0.534479,-0.0850347,0.772246,0.628766,0.0325514, -77,7.662854000,0.0222245,-0.441228,0.525508,-0.0782168,0.770235,0.632832,0.0119975, -78,7.762371000,0.0414622,-0.435652,0.51772,-0.0845202,0.766707,0.6364,-0.00336365, -79,7.861889000,0.0715281,-0.424536,0.503782,-0.0860437,0.755994,0.6476,-0.0410279, -80,7.961407000,0.0875989,-0.424919,0.496218,-0.0906963,0.745021,0.659567,-0.0411147, -81,8.060924000,0.111291,-0.421712,0.514032,-0.0989483,0.758298,0.641755,-0.0578245, -82,8.160442000,0.142384,-0.420487,0.516491,-0.105782,0.758455,0.636587,-0.0911775, -83,8.259959000,0.17282,-0.405416,0.514498,-0.117039,0.756466,0.629796,-0.131977, -84,8.359477000,0.186347,-0.408414,0.498507,-0.10894,0.750337,0.633957,-0.152398, -85,8.458995000,0.191987,-0.405845,0.498041,-0.0930809,0.75735,0.625033,-0.164593, -86,8.558512000,0.197281,-0.411771,0.493381,-0.0633621,0.758721,0.623634,-0.177223, -87,8.658030000,0.235887,-0.382443,0.487885,-0.0929626,0.74381,0.61754,-0.23822, -88,8.757547000,0.244544,-0.372894,0.480291,-0.0987982,0.732153,0.611786,-0.28268, -89,8.857065000,0.246133,-0.371769,0.482454,-0.085307,0.72882,0.606672,-0.305767, -90,8.956582000,0.254636,-0.36007,0.471634,-0.0812659,0.715708,0.607466,-0.334877, -91,9.056100000,0.259927,-0.357378,0.460385,-0.0715034,0.708474,0.608693,-0.349921, -92,9.155618000,0.264225,-0.350058,0.446032,-0.0635573,0.699277,0.610003,-0.367244, -93,9.255135000,0.278787,-0.337986,0.439601,-0.0692556,0.697733,0.602708,-0.380939, -94,9.354653000,0.282679,-0.333253,0.434037,-0.0618443,0.695012,0.600181,-0.391046, -95,9.454170000,0.302421,-0.313787,0.431038,-0.0957795,0.696181,0.586792,-0.402287, -96,9.534272000,0.307587,-0.312697,0.419763,-0.0989341,0.687702,0.589669,-0.411786, -97,9.614374000,0.309009,-0.305344,0.408049,-0.10155,0.675579,0.598963,-0.417761, -98,9.694475000,0.313153,-0.299877,0.401903,-0.099585,0.671185,0.589352,-0.438472, -99,9.793993000,0.318715,-0.289822,0.389317,-0.105391,0.655225,0.59299,-0.456, -100,9.893510000,0.328497,-0.278472,0.377695,-0.108832,0.646006,0.587285,-0.475318, -101,9.966734000,0.333911,-0.275506,0.372267,-0.115129,0.639816,0.589411,-0.479558, -102,10.046835000,0.340372,-0.26912,0.352898,-0.106729,0.629294,0.585418,-0.499883, -103,10.126937000,0.352964,-0.24651,0.360058,-0.124206,0.633221,0.564675,-0.514535, -104,10.207039000,0.354465,-0.242069,0.342471,-0.121569,0.619321,0.567849,-0.528403, -105,10.287140000,0.361269,-0.222954,0.355402,-0.148262,0.621017,0.558419,-0.529646, -106,10.367242000,0.370239,-0.212172,0.352741,-0.162896,0.613106,0.546366,-0.546854, -107,10.447344000,0.366843,-0.21104,0.340371,-0.151271,0.601272,0.535249,-0.57367, -108,10.527445000,0.367253,-0.210533,0.323129,-0.14551,0.578636,0.539014,-0.594534, -109,10.607547000,0.370835,-0.21114,0.313465,-0.146911,0.570172,0.536581,-0.604485, -110,10.680770000,0.37336,-0.206557,0.304971,-0.147166,0.566285,0.532523,-0.611623, -111,10.780288000,0.385639,-0.182848,0.301978,-0.164182,0.553883,0.518656,-0.63028, -112,10.879805000,0.396248,-0.167864,0.303512,-0.174553,0.542291,0.504787,-0.64857, -113,10.979323000,0.390668,-0.174836,0.273754,-0.144152,0.518943,0.516742,-0.665504, -114,11.078841000,0.399807,-0.128458,0.28486,-0.180334,0.513166,0.495903,-0.67692, -115,11.178358000,0.362802,-0.186564,0.241628,-0.108874,0.500228,0.531132,-0.675142, -116,11.277876000,0.370536,-0.158307,0.240674,-0.118139,0.493286,0.49325,-0.706694, -117,11.377393000,0.3662,-0.129537,0.216101,-0.125884,0.465843,0.473506,-0.736842, -118,11.476911000,0.354674,-0.109265,0.197637,-0.125136,0.449809,0.454537,-0.758557, -119,11.576428000,0.351461,-0.0876258,0.186179,-0.136454,0.443403,0.443339,-0.766959, -120,11.675946000,0.349712,-0.0906173,0.174561,-0.127459,0.433306,0.451075,-0.769761, -121,11.775464000,0.339908,-0.0770361,0.16775,-0.118123,0.421714,0.43693,-0.785682, -122,11.874981000,0.331897,-0.0674407,0.146953,-0.119993,0.396888,0.434451,-0.799584, -123,11.974499000,0.327593,-0.0728665,0.142326,-0.110591,0.387182,0.434154,-0.805835, -124,12.074016000,0.320493,-0.0573235,0.143426,-0.113263,0.379207,0.412173,-0.820662, -125,12.173534000,0.300325,-0.0311347,0.117809,-0.120444,0.356964,0.399103,-0.835935, -126,12.273052000,0.295236,-0.0274303,0.106137,-0.107994,0.338417,0.38864,-0.850159, -127,12.372569000,0.292703,-0.0160189,0.0991891,-0.106862,0.330102,0.368537,-0.862435, -128,12.472087000,0.297351,-0.00325865,0.0995824,-0.114586,0.321743,0.361028,-0.867761, -129,12.571604000,0.285288,0.0161259,0.0802313,-0.104694,0.304944,0.325505,-0.888873, -130,12.671122000,0.273087,0.0451235,0.0625359,-0.12053,0.279589,0.310807,-0.90039, -131,12.770639000,0.259335,0.0500595,0.0492792,-0.112016,0.259303,0.296264,-0.912383, -132,12.870157000,0.250561,0.0782485,0.0473737,-0.137625,0.247894,0.281768,-0.916632, -133,12.969675000,0.23298,0.0842046,0.0416104,-0.13673,0.238294,0.273846,-0.921699, -134,13.069192000,0.212872,0.0880998,0.0279946,-0.130655,0.221755,0.263926,-0.929568, -135,13.168710000,0.201315,0.0889376,0.0203345,-0.122773,0.212092,0.240466,-0.939212, -136,13.268227000,0.186568,0.0968321,0.0182541,-0.122097,0.202363,0.225688,-0.945096, -137,13.367745000,0.151608,0.0910352,0.00259141,-0.109513,0.172151,0.215212,-0.955015, -138,13.467263000,0.134273,0.0930459,0.00106309,-0.107889,0.159724,0.190332,-0.962612, -139,13.566780000,0.107539,0.0998676,-0.00019556,-0.115031,0.143752,0.16819,-0.968409, -140,13.666298000,0.091495,0.0943717,-0.00433301,-0.107877,0.137265,0.166663,-0.970435, -141,13.765815000,0.085218,0.0914941,-0.00171569,-0.105149,0.12945,0.156403,-0.973511, -142,13.865333000,0.073545,0.0939805,-0.00648879,-0.112687,0.104288,0.150981,-0.97654, -143,13.964850000,0.0569779,0.0932171,-0.00469085,-0.10951,0.0849358,0.144972,-0.979682, -144,14.064368000,0.0387171,0.0864107,-0.00659201,-0.110438,0.0622237,0.145616,-0.981187, -145,14.163886000,0.0342748,0.077406,-0.0125135,-0.112285,0.0486108,0.118764,-0.985355, -146,14.263403000,0.0185535,0.0733347,-0.013482,-0.11425,0.0249912,0.107869,-0.987262, -147,14.362921000,0.00850812,0.068326,-0.0161953,-0.113613,0.0133747,0.0895612,-0.98939, -148,14.462438000,-0.0014272,0.0661054,-0.0190308,-0.110454,0.00481952,0.082744,-0.990419, -149,14.561956000,-0.00990889,0.0708279,-0.0212333,-0.111373,-0.0016078,0.0750589,-0.990939, -150,14.661473000,-0.01871,0.0656756,-0.0234535,-0.0992291,-0.00870898,0.0633712,-0.993006, -151,14.760991000,-0.0240307,0.0701009,-0.0240498,-0.105473,-0.00458173,0.0537971,-0.992955, -152,14.860509000,-0.0310144,0.0686863,-0.0248023,-0.10936,-0.00122301,0.0431027,-0.993066, -153,14.960026000,-0.0445534,0.0522794,-0.0237591,-0.0924506,-0.00884615,0.0469911,-0.994568, -154,15.059544000,-0.0465194,0.0635562,-0.0224284,-0.108586,-0.00857273,0.0426772,-0.993134, -155,15.159061000,-0.0508195,0.0641719,-0.0169765,-0.107172,-0.012611,0.0416502,-0.993288, -156,15.258579000,-0.0497201,0.0644475,-0.0195437,-0.10655,-0.00953684,0.0422558,-0.993363, -157,15.358097000,-0.0503418,0.0638039,-0.0208627,-0.108181,-0.0087465,0.0399299,-0.99329, -158,15.457614000,-0.0525259,0.0610764,-0.0209713,-0.10829,-0.0084292,0.0449566,-0.993067, diff --git a/include/objectslam/landmark/landmark_object.h b/include/objectslam/landmark/landmark_object.h index c4113b221274fa1f428cd8e34a42e5b36960ca40..b36d05c8d58dfebe7aea1b72da8939e21959633d 100644 --- a/include/objectslam/landmark/landmark_object.h +++ b/include/objectslam/landmark/landmark_object.h @@ -23,9 +23,10 @@ class LandmarkObject : public LandmarkBase * Constructor with type, and state pointer * \param _pose: Concatenation of landmark position and quaternion vectors * \param _object_type : type of the object represented by the landmark + * \param _t_init : timestamp of the landmark's initialization * **/ - LandmarkObject(Eigen::Vector7d& _pose, const std::string& _object_type); + LandmarkObject(Eigen::Vector7d& _pose, const std::string& _object_type, const TimeStamp _t_init); ~LandmarkObject() override; @@ -34,6 +35,11 @@ class LandmarkObject : public LandmarkBase **/ const std::string& getObjectType() const; + /** \brief Returns the initialisation timestamp + * + **/ + const TimeStamp getInitTimestamp() const; + /** Factory method to create new landmarks from YAML nodes */ static LandmarkBasePtr create(const YAML::Node& _lmk_node); @@ -43,6 +49,7 @@ class LandmarkObject : public LandmarkBase private: const std::string object_type_; + const TimeStamp t_init_; }; diff --git a/include/objectslam/processor/processor_tracker_landmark_object.h b/include/objectslam/processor/processor_tracker_landmark_object.h index 9b32a5505971170ad0d43ee71f7764acc89670e6..fb34c7ee7d9e6c9cc277a6d7f593047edf1f224a 100644 --- a/include/objectslam/processor/processor_tracker_landmark_object.h +++ b/include/objectslam/processor/processor_tracker_landmark_object.h @@ -134,9 +134,6 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark // To be able to access them in unit tests protected: - Vector3d last_pos; - double dt_kf; - int lmks_matched; double min_time_vote_; double max_time_vote_; unsigned int min_features_for_keyframe_; diff --git a/src/landmark/landmark_object.cpp b/src/landmark/landmark_object.cpp index 77c737b32f7aa25ace05e086f99e60418b5a585f..8fccfb9364652d6a51b87de80ae02badd2e93113 100644 --- a/src/landmark/landmark_object.cpp +++ b/src/landmark/landmark_object.cpp @@ -5,12 +5,12 @@ namespace wolf { -LandmarkObject::LandmarkObject(Eigen::Vector7d& _pose, const std::string& _object_type) : +LandmarkObject::LandmarkObject(Eigen::Vector7d& _pose, const std::string& _object_type, const TimeStamp _t_init) : LandmarkBase("LandmarkObject", std::make_shared<StateBlock>(_pose.head(3)), std::make_shared<StateQuaternion>(_pose.tail(4))), - object_type_(_object_type) + object_type_(_object_type), + t_init_(_t_init) { - // setDescriptor(Eigen::VectorXd::Constant(1,_tagid)); // not used in the end in apriltag - // setId(_tagid); // overwrite lmk ID to match tag_id. // doubfull apriltag choice + // } LandmarkObject::~LandmarkObject() @@ -18,6 +18,10 @@ LandmarkObject::~LandmarkObject() // } +const TimeStamp LandmarkObject::getInitTimestamp() const +{ + return t_init_; +} const std::string& LandmarkObject::getObjectType() const { @@ -34,8 +38,10 @@ LandmarkBasePtr LandmarkObject::create(const YAML::Node& _lmk_node) unsigned int id = _lmk_node["id"] .as<unsigned int>(); std::string object_type = _lmk_node["object type"] .as<std::string>(); Eigen::Vector3d pos = _lmk_node["position"] .as<Eigen::Vector3d>(); + double ts_double = _lmk_node["timestamp"] .as<double>(); bool pos_fixed = _lmk_node["position fixed"] .as<bool>(); Eigen::Vector4d vquat; + if (_lmk_node["orientation"].size() == 3) { // we have been given 3 Euler angles in degrees @@ -54,7 +60,8 @@ LandmarkBasePtr LandmarkObject::create(const YAML::Node& _lmk_node) Eigen::Vector7d pose; pose << pos, vquat; // Create a new landmark - LandmarkObjectPtr lmk_ptr = std::make_shared<LandmarkObject>(pose, object_type); + TimeStamp t_init(ts_double); + LandmarkObjectPtr lmk_ptr = std::make_shared<LandmarkObject>(pose, object_type, t_init); lmk_ptr->setId(id); lmk_ptr->getP()->setFixed(pos_fixed); lmk_ptr->getO()->setFixed(ori_fixed); diff --git a/src/processor/processor_tracker_landmark_object.cpp b/src/processor/processor_tracker_landmark_object.cpp index 85b08e3aee3472fff2c8ffcdba790a1fdbe4e033..9de7f27191c46d881b1840990aa52aad40b98b9e 100644 --- a/src/processor/processor_tracker_landmark_object.cpp +++ b/src/processor/processor_tracker_landmark_object.cpp @@ -50,12 +50,52 @@ void ProcessorTrackerLandmarkObject::preProcess() void ProcessorTrackerLandmarkObject::postProcess() { nb_vote_++; + + // Prediction with constant speed hypothesis + double map_size = getProblem()->getTrajectory()->getFrameMap().size(); + auto iter = getProblem()->getTrajectory()->getFrameMap().end(); + + // Check if we have enough kf + if (map_size > 2){ + // Compute linear velocity + iter--; + FrameBasePtr current_pose = iter->second; + TimeStamp current_ts = iter->first; + iter--; + FrameBasePtr last_pose = iter->second; + TimeStamp last_ts = iter->first; + double dt_kf = current_ts.get()-last_ts.get(); + Vector3d linear_velocity = (current_pose->getP()->getState()-last_pose->getP()->getState())/dt_kf; + + // Compute angular velocity with log map + Quaterniond current_quat(current_pose->getO()->getState().data()); + Quaterniond last_quat(last_pose->getO()->getState().data()); + Vector3d angular_velocity = log_q(current_quat.conjugate() * last_quat)/dt_kf; + + // Integrate speed on current pose + double dt_frame = 0.033333; + current_pose = getLast()->getFrame(); + current_quat.coeffs() = current_pose->getO()->getState(); + Vector3d new_pose = current_pose->getP()->getState() + linear_velocity*dt_frame; + Quaterniond new_quat = exp_q(log_q(current_quat) + angular_velocity*dt_frame); + getLast()->getFrame()->getP()->setState(new_pose); + getLast()->getFrame()->getO()->setState(new_quat.coeffs()); + } + + // Clear the landmark map + LandmarkBasePtrList lmk_list = getProblem()->getMap()->getLandmarkList(); + for (auto iter_map = lmk_list.begin(); iter_map != lmk_list.end(); iter_map++){ + auto lmk_obj = std::dynamic_pointer_cast<LandmarkObject>(*iter_map); + double dt_lmk = getProblem()->getTimeStamp().get() - lmk_obj->getInitTimestamp().get(); + int number_of_factors = lmk_obj->getConstrainedByList().size(); + double lmk_score = number_of_factors / dt_lmk; + + if (lmk_score < 3){ + iter_map->get()->remove(); + } + + } - // Test velocity - Vector3d pos_k = getOrigin()->getFrame()->getP()->getState(); - Vector3d vel = (pos_k-last_pos)/0.1; - WOLF_INFO("======SPEEDO========"); - std::cout << vel << std::endl; } FactorBasePtr ProcessorTrackerLandmarkObject::emplaceFactor(FeatureBasePtr _feature_ptr, @@ -100,21 +140,15 @@ LandmarkBasePtr ProcessorTrackerLandmarkObject::emplaceLandmark(FeatureBasePtr _ FeatureObjectPtr feat_obj = std::static_pointer_cast<FeatureObject>(_feature_ptr); std::string object_type = feat_obj->getObjectType(); + TimeStamp t_init = getProblem()->getTimeStamp(); - return LandmarkBase::emplace<LandmarkObject>(getProblem()->getMap(), w_pose_t, object_type); + return LandmarkBase::emplace<LandmarkObject>(getProblem()->getMap(), w_pose_t, object_type, t_init); } unsigned int ProcessorTrackerLandmarkObject::detectNewFeatures(const int& _max_new_features, const CaptureBasePtr& _capture, FeatureBasePtrList& _features_out) { - // Set the last pos for velocity computation - if (getLast() != nullptr){ - last_pos = getLast()->getFrame()->getP()->getState(); - } else { - last_pos << 0,0,0; - } - // list of landmarks in the map auto lmk_lst = getProblem()->getMap()->getLandmarkList(); for (auto feat : detections_last_) @@ -163,9 +197,9 @@ unsigned int ProcessorTrackerLandmarkObject::detectNewFeatures(const int& _max_n // Error computation double e_pos = (pos_lmk - pos_feat).norm(); - double e_rot = (quat_lmk.vec() - quat_feat.vec()).norm(); + double e_rot = log_q(quat_lmk.conjugate() * quat_feat).norm(); - if (e_pos < 0.15 && e_rot < 0.2){ + if (e_pos < 0.15 && e_rot < 0.4){ feature_already_found = true; break; } @@ -192,11 +226,6 @@ bool ProcessorTrackerLandmarkObject::voteForKeyFrame() const if (detections_last_.empty()) return false; - // Impossible to make a keyframe if no landmarks have been matched - if (lmks_matched == 0){ - return false; - } - auto origin = getOrigin(); auto incoming = getIncoming(); std::cout << "voteForKeyFrame" << std::endl; @@ -264,9 +293,9 @@ unsigned int ProcessorTrackerLandmarkObject::findLandmarks(const LandmarkBasePtr // Error computation double e_pos = (pos_lmk - pos_feat).norm(); - double e_rot = (quat_lmk.vec() - quat_feat.vec()).norm(); + double e_rot = log_q(quat_lmk.conjugate() * quat_feat).norm(); - if (e_rot < 0.15 && e_pos < 0.1){ + if (e_rot < 0.3 && e_pos < 0.1){ _features_out.push_back(feat); double score(1.0); LandmarkMatchPtr matched_landmark = std::make_shared<LandmarkMatch>(lmk, score); @@ -277,7 +306,6 @@ unsigned int ProcessorTrackerLandmarkObject::findLandmarks(const LandmarkBasePtr } } } - lmks_matched = _features_out.size(); return _features_out.size(); } @@ -323,7 +351,7 @@ void ProcessorTrackerLandmarkObject::resetDerived() auto cstr = FactorBase::emplace<FactorDistance3d>(feat_dist, feat_dist, ori_frame, shared_from_this(), params_->apply_loss_function, FAC_ACTIVE); } } - + if (getProblem()->getProcessorIsMotionMap().empty() && reestimate_last_frame_){ reestimateLastFrame(); } diff --git a/test/gtest_landmark_object.cpp b/test/gtest_landmark_object.cpp index 1dee33ab2e09d8bfc69b3a5a7cc56b43b5311cd3..19eda41f1b5e1166585dcd892467c622fceb4f28 100644 --- a/test/gtest_landmark_object.cpp +++ b/test/gtest_landmark_object.cpp @@ -27,18 +27,29 @@ class LandmarkObject_class : public testing::Test{ TEST(LandmarkObject, getObjectType) { Vector7d p; - LandmarkObjectPtr l = std::make_shared<LandmarkObject>(p, "thetype"); // pose, tag_id, tag_width + TimeStamp t; + LandmarkObjectPtr l = std::make_shared<LandmarkObject>(p, "thetype", t); // pose, object_type, t ASSERT_EQ(l->getObjectType(), "thetype"); } TEST(LandmarkObject, getPose) { Vector7d p; + TimeStamp t; p << 0,0,0, 0,0,0,1; - LandmarkObjectPtr l = std::make_shared<LandmarkObject>(p, "thetype"); // pose, tag_id, tag_width + LandmarkObjectPtr l = std::make_shared<LandmarkObject>(p, "thetype", t); // pose, object_type, t ASSERT_MATRIX_APPROX(l->getState().vector("PO"), p, 1e-6); } +TEST(LandmarkObject, getInitTimestamp) +{ + Vector7d p; + TimeStamp t; + t.set(0.0); + LandmarkObjectPtr l = std::make_shared<LandmarkObject>(p, "thetype", t); // pose, object_type, t + ASSERT_EQ(l->getInitTimestamp(), 0.0); +} + TEST_F(LandmarkObject_class, create) { // load original hand-written map diff --git a/test/map_objects.yaml b/test/map_objects.yaml index 846c8f0a2b8eb355948aa21d4dd2f441583e02b2..6d728b1093aa1159a1fc0597b34364238208669a 100644 --- a/test/map_objects.yaml +++ b/test/map_objects.yaml @@ -9,6 +9,7 @@ landmarks: object type: "obj1" position: [0, 0, 0] orientation: [0, 0, 0] # roll pitch yaw in degrees + timestamp: 0.0 position fixed: true orientation fixed: true @@ -17,6 +18,7 @@ landmarks: object type: "obj2" position: [0, 0, 0] orientation: [0, 0, 0] # roll pitch yaw in degrees + timestamp: 0.0 position fixed: true orientation fixed: true @@ -25,6 +27,7 @@ landmarks: object type: "obj3" position: [0, 0, 0] orientation: [0, 0, 0] # roll pitch yaw in degrees + timestamp: 0.0 position fixed: true orientation fixed: true @@ -33,6 +36,7 @@ landmarks: object type: "obj4" position: [0, 0, 0] orientation: [0, 0, 0] # roll pitch yaw in degrees + timestamp: 0.0 position fixed: true orientation fixed: true