diff --git a/demos/cosyslam.cpp b/demos/cosyslam.cpp
index 6fb919cea902b06f2a25b1e9521da25c96ae0825..edb4194887380f717dca583ec91b648c21b4b721 100644
--- a/demos/cosyslam.cpp
+++ b/demos/cosyslam.cpp
@@ -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;
diff --git a/demos/map.csv b/demos/map.csv
index f1e18713b7920802213c69978a746b6162d4dc71..d27dfe0cbbbc2e37b18ef06d85cd8702f0e31c57 100644
--- a/demos/map.csv
+++ b/demos/map.csv
@@ -1,10 +1,4 @@
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,
diff --git a/demos/processor_tracker_landmark_object.yaml b/demos/processor_tracker_landmark_object.yaml
index d80bfcf56611b66c1dee75f6ffceab45070bcee8..6e36010be4d2aedd07a28aad85d1893e7f2cb6cd 100644
--- a/demos/processor_tracker_landmark_object.yaml
+++ b/demos/processor_tracker_landmark_object.yaml
@@ -5,7 +5,7 @@ time_tolerance: 0.1222
vote:
voting_active: true
min_time_vote: 0 # s
- max_time_vote: 0.5 # s
+ max_time_vote: 0.2 # s
min_features_for_keyframe: 1
nb_vote_for_every_first: 0
diff --git a/demos/result.csv b/demos/result.csv
index b08d731ad443e69e5eff0c60a3936a0284b2ed92..018218898e4042dd04fbd1a93395aa10f7e492b5 100644
--- a/demos/result.csv
+++ b/demos/result.csv
@@ -1,33 +1,24 @@
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,
diff --git a/include/objectslam/capture/capture_object.h b/include/objectslam/capture/capture_object.h
index 8e984107ceb8a7f85407be838fdd9613b492ab76..c5b0b2b83412fb953acc28f46d77fdf7c65e1bd3 100644
--- a/include/objectslam/capture/capture_object.h
+++ b/include/objectslam/capture/capture_object.h
@@ -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;
diff --git a/include/objectslam/processor/processor_tracker_landmark_object.h b/include/objectslam/processor/processor_tracker_landmark_object.h
index fb34c7ee7d9e6c9cc277a6d7f593047edf1f224a..cc6658bf7ce0b07c0816ed5175e497b8634ed8d7 100644
--- a/include/objectslam/processor/processor_tracker_landmark_object.h
+++ b/include/objectslam/processor/processor_tracker_landmark_object.h
@@ -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_;
diff --git a/src/processor/processor_tracker_landmark_object.cpp b/src/processor/processor_tracker_landmark_object.cpp
index 5cd2ec519d4b6e4219b99ff465dc0eb2f23eb5e3..c30f0d39f84f50262c2244d4afd79fd0f23df326 100644
--- a/src/processor/processor_tracker_landmark_object.cpp
+++ b/src/processor/processor_tracker_landmark_object.cpp
@@ -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();
}