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