Skip to content
Snippets Groups Projects
Commit c3261010 authored by Cesar Debeunne's avatar Cesar Debeunne
Browse files

multi object

parent bb324f62
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
demos/cosyslam.cpp
+ 19
− 2
View file @ c3261010
...@@ -137,7 +137,7 @@ int main() ...@@ -137,7 +137,7 @@ int main()
///////////////////////// /////////////////////////
std::string wolf_root = _WOLF_OBJECTSLAM_ROOT_DIR; std::string wolf_root = _WOLF_OBJECTSLAM_ROOT_DIR;
std::string filename = wolf_root + "/demos/input_demo2.csv"; std::string filename = wolf_root + "/demos/input_demoMulti.csv";
std::vector<std::pair<std::string, std::vector<std::string>>> csv_values; std::vector<std::pair<std::string, std::vector<std::string>>> csv_values;
csv_values = read_csv(filename); csv_values = read_csv(filename);
...@@ -145,6 +145,11 @@ int main() ...@@ -145,6 +145,11 @@ int main()
// setup the wolf problem // // 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); ProblemPtr problem = Problem::create("POV", 3);
SolverCeresPtr solver = std::make_shared<SolverCeres>(problem); SolverCeresPtr solver = std::make_shared<SolverCeres>(problem);
solver->getSolverOptions().max_num_iterations = 200; solver->getSolverOptions().max_num_iterations = 200;
...@@ -171,8 +176,9 @@ int main() ...@@ -171,8 +176,9 @@ int main()
// SLAM loop // // SLAM loop //
/////////////// ///////////////
double t_cur = 0; double t_cur = 0;
double t_since_last_kf = 0;
for (int i = 0; i < len; i++) for (int i = 0; i < len; i++)
{ {
std::cout << i << std::endl; std::cout << i << std::endl;
...@@ -200,10 +206,17 @@ int main() ...@@ -200,10 +206,17 @@ int main()
CaptureObjectPtr cap = std::make_shared<CaptureObject>(t_cur, sen, dets); CaptureObjectPtr cap = std::make_shared<CaptureObject>(t_cur, sen, dets);
cap->process(); cap->process();
t_since_last_kf += t-t_cur;
dets.clear(); dets.clear();
dets.push_back(det); dets.push_back(det);
} }
t_cur = t; t_cur = t;
// Solve if a KF should be voted
if (t_since_last_kf > kf_dt){
std::string report = solver->solve(SolverCeres::ReportVerbosity::FULL);
t_since_last_kf = 0;
}
} }
std::string report = solver->solve(SolverCeres::ReportVerbosity::FULL); std::string report = solver->solve(SolverCeres::ReportVerbosity::FULL);
...@@ -248,6 +261,10 @@ int main() ...@@ -248,6 +261,10 @@ int main()
counter = 0; counter = 0;
for (auto& elt: problem->getMap()->getLandmarkList()){ for (auto& elt: problem->getMap()->getLandmarkList()){
state_est = elt->getState(); state_est = elt->getState();
if (elt->getConstrainedByList().size() < 15){
continue;
}
file_map << counter << "," file_map << counter << ","
<< state_est['P'](0) << "," << state_est['P'](0) << ","
<< state_est['P'](1) << "," << state_est['P'](1) << ","
......
demos/map.csv
+ 6
− 3
View file @ c3261010
id,px,py,pz,qx,qy,qz,qw, id,px,py,pz,qx,qy,qz,qw,
0,-0.0614247,-0.102457,0.495463,-0.0662311,0.421016,0.904218,0.0273766, 0,-0.0586016,-0.103967,0.490275,-0.0429536,0.420095,0.905714,0.0368407,
1,-0.160675,0.0237989,0.338653,-0.167039,0.900001,-0.402082,-0.0206261, 1,-0.160589,0.0243347,0.339189,-0.169076,0.90165,-0.397773,-0.0147582,
2,0.132295,-0.0152859,0.425448,0.194154,0.906178,-0.348613,0.140051, 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,
demos/processor_tracker_landmark_object.yaml
+ 1
− 1
View file @ c3261010
...@@ -5,7 +5,7 @@ time_tolerance: 0.1222 ...@@ -5,7 +5,7 @@ time_tolerance: 0.1222
vote: vote:
voting_active: true voting_active: true
min_time_vote: 0 # s min_time_vote: 0 # s
max_time_vote: 0.2 # s max_time_vote: 0.1 # s
min_features_for_keyframe: 1 min_features_for_keyframe: 1
nb_vote_for_every_first: 0 nb_vote_for_every_first: 0
......
demos/result.csv
+ 159
− 23
View file @ c3261010
id,t,px,py,pz,qx,qy,qz,qw, id,t,px,py,pz,qx,qy,qz,qw,
0,0.000000000,-7.13745e-09,-3.17056e-09,2.67137e-09,-1.08901e-09,3.88643e-09,4.77464e-10,1, 0,0.000000000,0.00053724,0.000209711,-0.000341171,2.71308e-05,0.000158285,3.95115e-05,1,
1,0.199035000,-0.0147153,0.000985145,0.00321811,-0.00562051,-0.000470111,-0.00193335,0.999982, 1,0.099518000,-0.00513182,0.000146188,0.000370784,-0.00361063,-0.00156841,-0.000563434,0.999992,
2,0.398070000,-0.0179304,0.00258154,0.000629218,-0.00857347,-0.00101809,0.013546,0.999871, 2,0.199035000,-0.0136749,0.00163525,0.00248326,-0.00542409,-0.000310748,-0.00170426,0.999984,
3,0.597106000,-0.0267357,0.00768433,0.000805707,-0.00544569,0.00583282,0.018268,0.999801, 3,0.298553000,-0.0170197,0.00576942,0.00122761,-0.0013344,0.00191335,0.00293387,0.999993,
4,0.796141000,-0.034288,0.00881549,0.00312212,-0.00439498,0.0043273,0.0186002,0.999808, 4,0.398070000,-0.0169183,0.00302827,2.26597e-05,-0.00852854,-0.000791014,0.0137308,0.999869,
5,0.995176000,-0.054172,0.00412001,0.00517675,-0.00973791,0.0254734,0.0217271,0.999392, 5,0.497588000,-0.0220316,0.00292576,-0.000118058,-0.0104321,0.00339222,0.0180385,0.999777,
6,1.194211000,-0.0706687,-0.00257071,0.00593923,-0.0144656,0.0582125,0.0284753,0.997793, 6,0.597106000,-0.0257956,0.00810196,0.000230124,-0.0054016,0.00604,0.0184232,0.999797,
7,1.393246000,-0.0898303,-0.00531453,0.00976512,-0.0230286,0.0866555,0.0367664,0.995293, 7,0.696623000,-0.0308121,0.0106795,0.00125538,-0.00272841,0.00758898,0.0182663,0.999801,
8,1.592281000,-0.103203,-0.0164662,0.0111209,-0.0398646,0.105225,0.0513253,0.992323, 8,0.796141000,-0.0333355,0.00919541,0.00245237,-0.00446331,0.00459052,0.0186461,0.999806,
9,1.791317000,-0.125482,-0.0229034,0.00803872,-0.040498,0.130044,0.0678853,0.988352, 9,0.895658000,-0.0455784,0.010945,0.00270817,-0.00174839,0.0144349,0.0204059,0.999686,
10,1.990352000,-0.153863,-0.024897,0.0100727,-0.025176,0.151829,0.0926269,0.983735, 10,0.995176000,-0.0531691,0.00475043,0.00439845,-0.00959216,0.0256361,0.0218899,0.999386,
11,2.189387000,-0.200826,-0.0311449,0.0416477,-0.0355259,0.180014,0.109197,0.976939, 11,1.094694000,-0.0559504,0.00337196,0.00379001,-0.0104416,0.0356446,0.029455,0.998876,
12,2.388422000,-0.215623,-0.0467369,0.0541434,-0.0353748,0.207558,0.140889,0.967377, 12,1.194211000,-0.0675741,4.70758e-05,0.00444209,-0.0118591,0.0556111,0.0302859,0.997923,
13,2.653802000,0,0,0,0,0,0,1, 13,1.293729000,-0.0757968,-0.00430823,0.00637308,-0.0192157,0.0704855,0.0307826,0.996853,
14,13.898505000,0,0,0,0,0,0,1, 14,1.393246000,-0.0884369,-0.00416602,0.008664,-0.0223372,0.0861481,0.0373211,0.995332,
15,14.097541000,0.0882042,0.093087,0.00755886,0.129889,-0.104404,-0.153225,0.974038, 15,1.492764000,-0.0951528,-0.00768993,0.00901654,-0.0278844,0.0951243,0.0444495,0.994082,
16,14.296576000,-0.000340052,0.0563682,-0.0220339,0.0960243,-0.00648276,-0.105164,0.989787, 16,1.592281000,-0.1023,-0.0157616,0.010213,-0.0397695,0.105373,0.0514284,0.992305,
17,14.495611000,-0.00608663,0.066942,-0.0199413,0.110239,-0.00262628,-0.0804219,0.990643, 17,1.691799000,-0.114308,-0.0230877,0.00574492,-0.0423539,0.120594,0.0593172,0.990023,
18,14.694646000,-0.023845,0.0595486,-0.0238599,0.0927894,0.00941896,-0.0622909,0.993691, 18,1.791317000,-0.124822,-0.0221476,0.00693936,-0.0404716,0.130474,0.0677933,0.988303,
19,14.893681000,-0.0447324,0.0587233,-0.0228003,0.0974111,0.0113943,-0.0464065,0.994096, 19,1.890834000,-0.144722,-0.02576,0.00876419,-0.037595,0.149435,0.0798066,0.984828,
20,15.092716000,-0.0452005,0.0576207,-0.0213797,0.0998969,0.00601765,-0.0470362,0.993867, 20,1.990352000,-0.16076,-0.018139,0.0113476,-0.0178859,0.160199,0.0917667,0.982647,
21,15.291752000,-0.0503393,0.0628617,-0.019674,0.104966,0.00829545,-0.0414138,0.993578, 21,2.089869000,-0.167309,-0.0317906,0.0192576,-0.042076,0.154969,0.0995104,0.981994,
22,15.490787000,-0.0570391,0.0705774,-0.0207946,0.120835,0.0115654,-0.0464367,0.991518, 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,
demos/test.csv deleted 100644 → 0
+ 0
− 483
View file @ bb324f62
source diff could not be displayed: it is too large. Options to address this: view the blob.
include/objectslam/processor/processor_tracker_landmark_object.h
+ 1
− 0
View file @ c3261010
...@@ -135,6 +135,7 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark ...@@ -135,6 +135,7 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark
// To be able to access them in unit tests // To be able to access them in unit tests
protected: protected:
Vector3d last_pos; Vector3d last_pos;
double dt_kf;
int lmks_matched; int lmks_matched;
double min_time_vote_; double min_time_vote_;
double max_time_vote_; double max_time_vote_;
......
src/processor/processor_tracker_landmark_object.cpp
+ 48
− 53
View file @ c3261010
...@@ -36,13 +36,6 @@ void ProcessorTrackerLandmarkObject::preProcess() ...@@ -36,13 +36,6 @@ void ProcessorTrackerLandmarkObject::preProcess()
//clear wolf detections so that new ones will be stored inside //clear wolf detections so that new ones will be stored inside
detections_incoming_.clear(); detections_incoming_.clear();
// Set the last pos for velocity computation
if (getOrigin() != nullptr){
last_pos = getOrigin()->getFrame()->getP()->getState();
} else {
last_pos << 0,0,0;
}
auto incoming_ptr = std::dynamic_pointer_cast<CaptureObject>(incoming_ptr_); auto incoming_ptr = std::dynamic_pointer_cast<CaptureObject>(incoming_ptr_);
assert(incoming_ptr != nullptr && "Capture type mismatch. ProcessorTrackerLandmarkObject can only process captures of type CaptureObject"); assert(incoming_ptr != nullptr && "Capture type mismatch. ProcessorTrackerLandmarkObject can only process captures of type CaptureObject");
...@@ -60,7 +53,7 @@ void ProcessorTrackerLandmarkObject::postProcess() ...@@ -60,7 +53,7 @@ void ProcessorTrackerLandmarkObject::postProcess()
// Test velocity // Test velocity
Vector3d pos_k = getOrigin()->getFrame()->getP()->getState(); Vector3d pos_k = getOrigin()->getFrame()->getP()->getState();
Vector3d vel = (pos_k-last_pos); Vector3d vel = (pos_k-last_pos)/0.1;
WOLF_INFO("======SPEEDO========"); WOLF_INFO("======SPEEDO========");
std::cout << vel << std::endl; std::cout << vel << std::endl;
} }
...@@ -115,6 +108,13 @@ unsigned int ProcessorTrackerLandmarkObject::detectNewFeatures(const int& _max_n ...@@ -115,6 +108,13 @@ unsigned int ProcessorTrackerLandmarkObject::detectNewFeatures(const int& _max_n
const CaptureBasePtr& _capture, const CaptureBasePtr& _capture,
FeatureBasePtrList& _features_out) 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 // list of landmarks in the map
auto lmk_lst = getProblem()->getMap()->getLandmarkList(); auto lmk_lst = getProblem()->getMap()->getLandmarkList();
for (auto feat : detections_last_) for (auto feat : detections_last_)
...@@ -127,9 +127,6 @@ unsigned int ProcessorTrackerLandmarkObject::detectNewFeatures(const int& _max_n ...@@ -127,9 +127,6 @@ unsigned int ProcessorTrackerLandmarkObject::detectNewFeatures(const int& _max_n
auto feat_obj = std::static_pointer_cast<FeatureObject>(feat); auto feat_obj = std::static_pointer_cast<FeatureObject>(feat);
// Loop over the landmark to find if one is associated to feat // Loop over the landmark to find if one is associated to feat
// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
// !! Assume that the object type is unique -> to be adapted if necessary
// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
for(auto lmk: lmk_lst){ for(auto lmk: lmk_lst){
LandmarkObjectPtr lmk_obj = std::dynamic_pointer_cast<LandmarkObject>(lmk); LandmarkObjectPtr lmk_obj = std::dynamic_pointer_cast<LandmarkObject>(lmk);
...@@ -137,49 +134,46 @@ unsigned int ProcessorTrackerLandmarkObject::detectNewFeatures(const int& _max_n ...@@ -137,49 +134,46 @@ unsigned int ProcessorTrackerLandmarkObject::detectNewFeatures(const int& _max_n
if(lmk_obj != nullptr and lmk_obj->getObjectType() == feat_obj->getObjectType()){ if(lmk_obj != nullptr and lmk_obj->getObjectType() == feat_obj->getObjectType()){
// Then check if the world pose is similar // Then check if the world pose is similar
// world to rob // world to rob
// Vector3d pos = getLast()->getFrame()->getP()->getState(); Vector3d pos = getLast()->getFrame()->getP()->getState();
// Quaterniond quat (getLast()->getFrame()->getO()->getState().data()); Quaterniond quat (getLast()->getFrame()->getO()->getState().data());
// Eigen::Isometry3d w_M_r = Eigen::Translation<double,3>(pos.head(3)) * quat; Eigen::Isometry3d w_M_r = Eigen::Translation<double,3>(pos.head(3)) * quat;
// // rob to sensor // rob to sensor
// pos = getSensor()->getP()->getState(); pos = getSensor()->getP()->getState();
// quat.coeffs() = getSensor()->getO()->getState(); quat.coeffs() = getSensor()->getO()->getState();
// Eigen::Isometry3d r_M_c = Eigen::Translation<double,3>(pos.head(3)) * quat; Eigen::Isometry3d r_M_c = Eigen::Translation<double,3>(pos.head(3)) * quat;
// // camera to feat // camera to feat
// pos = feat->getMeasurement().head(3); pos = feat->getMeasurement().head(3);
// quat.coeffs() = feat->getMeasurement().tail(4); quat.coeffs() = feat->getMeasurement().tail(4);
// Eigen::Isometry3d c_M_f = Eigen::Translation<double,3>(pos) * quat; Eigen::Isometry3d c_M_f = Eigen::Translation<double,3>(pos) * quat;
// // world to feat // world to feat
// Eigen::Isometry3d w_M_f = w_M_r * r_M_c * c_M_f; Eigen::Isometry3d w_M_f = w_M_r * r_M_c * c_M_f;
// Quaterniond quat_feat; Quaterniond quat_feat;
// Eigen::Matrix3d wRf = w_M_f.linear(); Eigen::Matrix3d wRf = w_M_f.linear();
// quat_feat.coeffs() = R2q(wRf).coeffs().transpose(); quat_feat.coeffs() = R2q(wRf).coeffs().transpose();
// Vector3d pos_feat = w_M_f.translation(); Vector3d pos_feat = w_M_f.translation();
// // world to lmk // world to lmk
// Vector3d pos_lmk = lmk_obj->getP()->getState(); Vector3d pos_lmk = lmk_obj->getP()->getState();
// Quaterniond quat_lmk(lmk_obj->getO()->getState().data()); Quaterniond quat_lmk(lmk_obj->getO()->getState().data());
// // Error computation // Error computation
// double e_pos = (pos_lmk - pos_feat).norm(); double e_pos = (pos_lmk - pos_feat).norm();
// double e_rot = (quat_lmk.vec() - quat_feat.vec()).norm(); double e_rot = (quat_lmk.vec() - quat_feat.vec()).norm();
// if (e_pos < 0.2 && e_rot < 0.25){ if (e_pos < 0.15 && e_rot < 0.2){
// feature_already_found = true; feature_already_found = true;
// break; break;
// } }
feature_already_found = true;
break;
} }
} }
if (!feature_already_found) if (!feature_already_found)
{ {
std::cout << feat_obj->getObjectType() << std::endl;
// If the feature is not in the map & not in the list of newly detected features yet, then we add it. // If the feature is not in the map & not in the list of newly detected features yet, then we add it.
_features_out.push_back(feat); _features_out.push_back(feat);
...@@ -198,6 +192,11 @@ bool ProcessorTrackerLandmarkObject::voteForKeyFrame() const ...@@ -198,6 +192,11 @@ bool ProcessorTrackerLandmarkObject::voteForKeyFrame() const
if (detections_last_.empty()) if (detections_last_.empty())
return false; return false;
// Impossible to make a keyframe if no landmarks have been matched
if (lmks_matched == 0){
return false;
}
auto origin = getOrigin(); auto origin = getOrigin();
auto incoming = getIncoming(); auto incoming = getIncoming();
std::cout << "voteForKeyFrame" << std::endl; std::cout << "voteForKeyFrame" << std::endl;
...@@ -208,10 +207,6 @@ bool ProcessorTrackerLandmarkObject::voteForKeyFrame() const ...@@ -208,10 +207,6 @@ bool ProcessorTrackerLandmarkObject::voteForKeyFrame() const
bool enough_time_vote = dt_incoming_origin > min_time_vote_; bool enough_time_vote = dt_incoming_origin > min_time_vote_;
bool too_long_since_last_KF = dt_incoming_origin > max_time_vote_; bool too_long_since_last_KF = dt_incoming_origin > max_time_vote_;
// Impossible to make a keyframe if no landmarks have been matched
if (lmks_matched == 0){
return false;
}
// the elapsed time since last KF is too long // the elapsed time since last KF is too long
if (too_long_since_last_KF){ if (too_long_since_last_KF){
return true; return true;
...@@ -232,7 +227,7 @@ unsigned int ProcessorTrackerLandmarkObject::findLandmarks(const LandmarkBasePtr ...@@ -232,7 +227,7 @@ unsigned int ProcessorTrackerLandmarkObject::findLandmarks(const LandmarkBasePtr
const CaptureBasePtr& _capture, const CaptureBasePtr& _capture,
FeatureBasePtrList& _features_out, FeatureBasePtrList& _features_out,
LandmarkMatchMap& _feature_landmark_correspondences) LandmarkMatchMap& _feature_landmark_correspondences)
{ {
for (auto feat : detections_incoming_) for (auto feat : detections_incoming_)
{ {
std::string object_type = std::static_pointer_cast<FeatureObject>(feat)->getObjectType(); std::string object_type = std::static_pointer_cast<FeatureObject>(feat)->getObjectType();
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment