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

Update 

Outlier rejection for landmark matching
Tests for velocity computation
Tests for multi object association
parent 58709f7b
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
demos/cosyslam.cpp
+ 3
− 3
View file @ bb324f62
......@@ -145,7 +145,7 @@ int main()
// setup the wolf problem //
////////////////////////////
ProblemPtr problem = Problem::create("PO", 3);
ProblemPtr problem = Problem::create("POV", 3);
SolverCeresPtr solver = std::make_shared<SolverCeres>(problem);
solver->getSolverOptions().max_num_iterations = 200;
......@@ -154,8 +154,8 @@ int main()
auto prc = problem->installProcessor("ProcessorTrackerLandmarkObject", "objects_wrapper", "sensor_pose", wolf_root + "/demos/processor_tracker_landmark_object.yaml");
auto prc_obj = std::static_pointer_cast<ProcessorTrackerLandmarkObject>(prc);
VectorComposite x0("PO", {Vector3d(0, 0, 0), Quaterniond::Identity().coeffs()});
VectorComposite sig0("PO", {Vector3d(1, 1, 1), Vector3d(1, 1, 1)});
VectorComposite x0("POV", {Vector3d(0, 0, 0), Quaterniond::Identity().coeffs(), Vector3d(0,0,0)});
VectorComposite sig0("POV", {Vector3d(1, 1, 1), Vector3d(1, 1, 1), Vector3d(1,1,1)});
FrameBasePtr F1 = problem->setPriorFactor(x0, sig0, 0.0, 0.1);
ObjectDetections dets;
......
demos/map.csv
+ 3
− 9
View file @ bb324f62
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,
0,-0.0614247,-0.102457,0.495463,-0.0662311,0.421016,0.904218,0.0273766,
1,-0.160675,0.0237989,0.338653,-0.167039,0.900001,-0.402082,-0.0206261,
2,0.132295,-0.0152859,0.425448,0.194154,0.906178,-0.348613,0.140051,
demos/processor_tracker_landmark_object.yaml
+ 1
− 1
View file @ bb324f62
......@@ -5,7 +5,7 @@ time_tolerance: 0.1222
vote:
voting_active: true
min_time_vote: 0 # s
max_time_vote: 0.5 # s
max_time_vote: 0.2 # s
min_features_for_keyframe: 1
nb_vote_for_every_first: 0
......
demos/result.csv
+ 23
− 32
View file @ bb324f62
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,
0,0.000000000,-7.13745e-09,-3.17056e-09,2.67137e-09,-1.08901e-09,3.88643e-09,4.77464e-10,1,
1,0.199035000,-0.0147153,0.000985145,0.00321811,-0.00562051,-0.000470111,-0.00193335,0.999982,
2,0.398070000,-0.0179304,0.00258154,0.000629218,-0.00857347,-0.00101809,0.013546,0.999871,
3,0.597106000,-0.0267357,0.00768433,0.000805707,-0.00544569,0.00583282,0.018268,0.999801,
4,0.796141000,-0.034288,0.00881549,0.00312212,-0.00439498,0.0043273,0.0186002,0.999808,
5,0.995176000,-0.054172,0.00412001,0.00517675,-0.00973791,0.0254734,0.0217271,0.999392,
6,1.194211000,-0.0706687,-0.00257071,0.00593923,-0.0144656,0.0582125,0.0284753,0.997793,
7,1.393246000,-0.0898303,-0.00531453,0.00976512,-0.0230286,0.0866555,0.0367664,0.995293,
8,1.592281000,-0.103203,-0.0164662,0.0111209,-0.0398646,0.105225,0.0513253,0.992323,
9,1.791317000,-0.125482,-0.0229034,0.00803872,-0.040498,0.130044,0.0678853,0.988352,
10,1.990352000,-0.153863,-0.024897,0.0100727,-0.025176,0.151829,0.0926269,0.983735,
11,2.189387000,-0.200826,-0.0311449,0.0416477,-0.0355259,0.180014,0.109197,0.976939,
12,2.388422000,-0.215623,-0.0467369,0.0541434,-0.0353748,0.207558,0.140889,0.967377,
13,2.653802000,0,0,0,0,0,0,1,
14,13.898505000,0,0,0,0,0,0,1,
15,14.097541000,0.0882042,0.093087,0.00755886,0.129889,-0.104404,-0.153225,0.974038,
16,14.296576000,-0.000340052,0.0563682,-0.0220339,0.0960243,-0.00648276,-0.105164,0.989787,
17,14.495611000,-0.00608663,0.066942,-0.0199413,0.110239,-0.00262628,-0.0804219,0.990643,
18,14.694646000,-0.023845,0.0595486,-0.0238599,0.0927894,0.00941896,-0.0622909,0.993691,
19,14.893681000,-0.0447324,0.0587233,-0.0228003,0.0974111,0.0113943,-0.0464065,0.994096,
20,15.092716000,-0.0452005,0.0576207,-0.0213797,0.0998969,0.00601765,-0.0470362,0.993867,
21,15.291752000,-0.0503393,0.0628617,-0.019674,0.104966,0.00829545,-0.0414138,0.993578,
22,15.490787000,-0.0570391,0.0705774,-0.0207946,0.120835,0.0115654,-0.0464367,0.991518,
include/objectslam/capture/capture_object.h
+ 3
− 3
View file @ bb324f62
......@@ -9,9 +9,9 @@ namespace wolf {
typedef struct ObjectDetection
{
const Eigen::Vector7d & measurement;
const Eigen::Matrix6d & meas_cov;
const std::string & object_type;
const Eigen::Vector7d measurement;
const Eigen::Matrix6d meas_cov;
const std::string object_type;
} ObjectDetection;
......
include/objectslam/processor/processor_tracker_landmark_object.h
+ 2
− 0
View file @ bb324f62
......@@ -134,6 +134,8 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark
// To be able to access them in unit tests
protected:
Vector3d last_pos;
int lmks_matched;
double min_time_vote_;
double max_time_vote_;
unsigned int min_features_for_keyframe_;
......
src/processor/processor_tracker_landmark_object.cpp
+ 99
− 13
View file @ bb324f62
......@@ -8,14 +8,11 @@
#include "objectslam/feature/feature_object.h"
#include "objectslam/landmark/landmark_object.h"
namespace wolf {
// Constructor
ProcessorTrackerLandmarkObject::ProcessorTrackerLandmarkObject( ParamsProcessorTrackerLandmarkObjectPtr _params_tracker_landmark_object) :
ProcessorTrackerLandmark("ProcessorTrackerLandmarkObject", "PO", _params_tracker_landmark_object ),
ProcessorTrackerLandmark("ProcessorTrackerLandmarkObject", "POV", _params_tracker_landmark_object ),
reestimate_last_frame_(_params_tracker_landmark_object->reestimate_last_frame_),
n_reset_(0),
min_time_vote_(_params_tracker_landmark_object->min_time_vote_),
......@@ -39,6 +36,13 @@ void ProcessorTrackerLandmarkObject::preProcess()
//clear wolf detections so that new ones will be stored inside
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_);
assert(incoming_ptr != nullptr && "Capture type mismatch. ProcessorTrackerLandmarkObject can only process captures of type CaptureObject");
......@@ -53,6 +57,12 @@ void ProcessorTrackerLandmarkObject::preProcess()
void ProcessorTrackerLandmarkObject::postProcess()
{
nb_vote_++;
// Test velocity
Vector3d pos_k = getOrigin()->getFrame()->getP()->getState();
Vector3d vel = (pos_k-last_pos);
WOLF_INFO("======SPEEDO========");
std::cout << vel << std::endl;
}
FactorBasePtr ProcessorTrackerLandmarkObject::emplaceFactor(FeatureBasePtr _feature_ptr,
......@@ -122,14 +132,54 @@ unsigned int ProcessorTrackerLandmarkObject::detectNewFeatures(const int& _max_n
// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
for(auto lmk: lmk_lst){
LandmarkObjectPtr lmk_obj = std::dynamic_pointer_cast<LandmarkObject>(lmk);
// Check if the object ID is the same
if(lmk_obj != nullptr and lmk_obj->getObjectType() == feat_obj->getObjectType()){
// Then check if the world pose is similar
// world to rob
// Vector3d pos = getLast()->getFrame()->getP()->getState();
// Quaterniond quat (getLast()->getFrame()->getO()->getState().data());
// Eigen::Isometry3d w_M_r = Eigen::Translation<double,3>(pos.head(3)) * quat;
// // rob to sensor
// pos = getSensor()->getP()->getState();
// quat.coeffs() = getSensor()->getO()->getState();
// Eigen::Isometry3d r_M_c = Eigen::Translation<double,3>(pos.head(3)) * quat;
// // camera to feat
// pos = feat->getMeasurement().head(3);
// quat.coeffs() = feat->getMeasurement().tail(4);
// Eigen::Isometry3d c_M_f = Eigen::Translation<double,3>(pos) * quat;
// // world to feat
// Eigen::Isometry3d w_M_f = w_M_r * r_M_c * c_M_f;
// Quaterniond quat_feat;
// Eigen::Matrix3d wRf = w_M_f.linear();
// quat_feat.coeffs() = R2q(wRf).coeffs().transpose();
// Vector3d pos_feat = w_M_f.translation();
// // world to lmk
// Vector3d pos_lmk = lmk_obj->getP()->getState();
// Quaterniond quat_lmk(lmk_obj->getO()->getState().data());
// // Error computation
// double e_pos = (pos_lmk - pos_feat).norm();
// double e_rot = (quat_lmk.vec() - quat_feat.vec()).norm();
// if (e_pos < 0.2 && e_rot < 0.25){
// feature_already_found = true;
// break;
// }
feature_already_found = true;
break;
break;
}
}
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.
_features_out.push_back(feat);
......@@ -154,10 +204,14 @@ bool ProcessorTrackerLandmarkObject::voteForKeyFrame() const
std::cout << origin->id() << " " << getOrigin()->getTimeStamp().get() << std::endl;
std::cout << incoming->id() << " " <<getIncoming()->getTimeStamp().get() << std::endl;
double dt_incoming_origin = getIncoming()->getTimeStamp().get() - getOrigin()->getTimeStamp().get();
bool enough_time_vote = dt_incoming_origin > min_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
if (too_long_since_last_KF){
return true;
......@@ -187,16 +241,48 @@ unsigned int ProcessorTrackerLandmarkObject::findLandmarks(const LandmarkBasePtr
auto lmk_obj = std::dynamic_pointer_cast<LandmarkObject>(lmk);
if(lmk_obj != nullptr and lmk_obj->getObjectType() == object_type)
{
_features_out.push_back(feat);
double score(1.0);
LandmarkMatchPtr matched_landmark = std::make_shared<LandmarkMatch>(lmk, score);
_feature_landmark_correspondences.emplace ( feat, matched_landmark );
feat->link(_capture); // since all features are created in preProcess() are unlinked
break;
// world to rob
Vector3d pos = getLast()->getFrame()->getP()->getState();
Quaterniond quat (getLast()->getFrame()->getO()->getState().data());
Eigen::Isometry3d w_M_r = Eigen::Translation<double,3>(pos.head(3)) * quat;
// rob to sensor
pos = getSensor()->getP()->getState();
quat.coeffs() = getSensor()->getO()->getState();
Eigen::Isometry3d r_M_c = Eigen::Translation<double,3>(pos.head(3)) * quat;
// camera to feat
pos = feat->getMeasurement().head(3);
quat.coeffs() = feat->getMeasurement().tail(4);
Eigen::Isometry3d c_M_f = Eigen::Translation<double,3>(pos) * quat;
// world to feat
Eigen::Isometry3d w_M_f = w_M_r * r_M_c * c_M_f;
Quaterniond quat_feat;
Eigen::Matrix3d wRf = w_M_f.linear();
quat_feat.coeffs() = R2q(wRf).coeffs().transpose();
Vector3d pos_feat = w_M_f.translation();
// world to lmk
Vector3d pos_lmk = lmk_obj->getP()->getState();
Quaterniond quat_lmk(lmk_obj->getO()->getState().data());
// Error computation
double e_pos = (pos_lmk - pos_feat).norm();
double e_rot = (quat_lmk.vec() - quat_feat.vec()).norm();
if (e_rot < 0.15 && e_pos < 0.1){
_features_out.push_back(feat);
double score(1.0);
LandmarkMatchPtr matched_landmark = std::make_shared<LandmarkMatch>(lmk, score);
_feature_landmark_correspondences.emplace (feat, matched_landmark);
feat->link(_capture); // since all features are created in preProcess() are unlinked
break;
}
}
}
}
lmks_matched = _features_out.size();
return _features_out.size();
}
......
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