diff --git a/src/processor/processor_tracker_landmark_object.cpp b/src/processor/processor_tracker_landmark_object.cpp
index f507e3d839222d5e357c6bfe6a9da54bc853a198..b57729c50322dd12bd7f69df014beb498ae29d91 100644
--- a/src/processor/processor_tracker_landmark_object.cpp
+++ b/src/processor/processor_tracker_landmark_object.cpp
@@ -317,11 +317,11 @@ unsigned int ProcessorTrackerLandmarkObject::bestLmkMatching(const LandmarkBaseP
break;
}
- //A counter for _landmarl_in
+ //Counter for _landmark_in
int i = 0;
- //A variable to store the best lmk match
+ //Variable to store the best lmk match
int index_min = -1;
- //A variable to store store if there was a match or not
+ //A variable to store if there was a match or not
bool match = false;
//Variables to compare errors of lmks
@@ -344,13 +344,6 @@ unsigned int ProcessorTrackerLandmarkObject::bestLmkMatching(const LandmarkBaseP
//Test if errors are below the previous min errors
if (error.first < e_pos_min && error.second < e_rot_min)
{
- // test_counter++;
-
- // std::cout << "\n\n\n\n\n\n\n\n" << test_counter << std::endl << std::endl;
-
-
- // std::cout << "\n\n\n\n\n" << error.first << " " << error.second << std::endl << std::endl;
-
e_pos_min = error.first;
e_rot_min = error.second;
index_min = i;
@@ -406,162 +399,99 @@ unsigned int ProcessorTrackerLandmarkObject::findLandmarks(const LandmarkBasePtr
return ftr_size;
}
-// unsigned int ProcessorTrackerLandmarkObject::multiviewTypeMatching(const CaptureBasePtr& _capture,
-// FeatureBasePtrList& _features_out_last,
-// FeatureBasePtrList& _features_out_incoming)
-// {
-// if (!last_ptr_){
-// return 0;
-// }
-
-// //world to rob last frame
-// Eigen::Isometry3d w_M_r_last = posevec_to_isometry(getLast()->getFrame()->getStateVector("PO"));
-
-// //world to rob incoming frame
-// Eigen::Isometry3d w_M_r_incoming = posevec_to_isometry(getIncoming()->getFrame()->getStateVector("PO"));
-
-// //rob to sensor
-// Eigen::Isometry3d r_M_c = posevec_to_isometry(getSensor()->getStateVector("PO"));
-
-// int index_last = 0;
-
-// //matches between last and incoming features
-// std::vector<std::tuple<int, int, Eigen::Isometry3d, Eigen::Isometry3d> > last_incoming;
-
-// for (auto feat_last : detections_last_)
-// {
-// std::string object_type_last = std::static_pointer_cast<FeatureObject>(feat_last)->getObjectType();
-
-// //camera to feat
-// Vector3d pos_obj_last = feat_last->getMeasurement().head(3);
-// Quaterniond quat_obj_last (feat_last->getMeasurement().tail(4).data());
-// Eigen::Isometry3d c_M_f_last = Eigen::Translation<double,3>(pos_obj_last) * quat_obj_last;
-
-// Eigen::Isometry3d w_M_f_last = w_M_r_last * r_M_c * c_M_f_last;
-// Vector3d pos_feat_last = w_M_f_last.translation();
-
-// int index_min_incoming = -1;
-// double min_e_pos = 100;
-
-// int index_incoming = 0;
-
-// for (auto feat_incoming : detections_incoming_)
-// {
-
-// std::string object_type_incoming = std::static_pointer_cast<FeatureObject>(feat_incoming)->getObjectType();
+unsigned int ProcessorTrackerLandmarkObject::multiviewTypeMatching(const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out_last,
+ FeatureBasePtrList& _features_out_incoming)
+{
+ if (!last_ptr_){
+ return 0;
+ }
-// if (object_type_last == object_type_incoming)
-// {
+ //world to rob last frame
+ Eigen::Isometry3d w_M_r_last = posevec_to_isometry(getLast()->getFrame()->getStateVector("PO"));
-// Eigen::Isometry3d c_M_f_incoming = posevec_to_isometry(feat_incoming->getMeasurement());
-
-// Eigen::Isometry3d w_M_f_incoming = w_M_r_incoming * r_M_c * c_M_f_incoming;
+ //world to rob incoming frame
+ Eigen::Isometry3d w_M_r_incoming = posevec_to_isometry(getIncoming()->getFrame()->getStateVector("PO"));
-// Vector3d pos_feat_incoming = w_M_f_incoming.translation();
+ //rob to sensor
+ Eigen::Isometry3d r_M_c = posevec_to_isometry(getSensor()->getStateVector("PO"));
-// // Error computation
-// double e_pos = (pos_feat_last - pos_feat_incoming).norm();
+ std::vector<Eigen::Isometry3d> cl_M_o_vec;
+ std::vector<Eigen::Isometry3d> ci_M_o_vec;
+ std::vector<std::pair<int,int> > matches;
-// if (e_pos < e_pos_thr_ && e_pos < min_e_pos)
-// {
-// min_e_pos = e_pos;
-// index_min_incoming = index_incoming;
-// }
-
-// auto tuple_last_incom = std::make_tuple(index_last, index_incoming, c_M_f_last, c_M_f_incoming);
-// last_incoming.push_back(tuple_last_incom);
+ int index_last = 0;
-// }
-// index_incoming++;
-// }
-
-// std::cout << index_min_incoming;
-// index_last++;
+ for (auto feat_last : detections_last_)
+ {
+ std::string object_type_last = std::static_pointer_cast<FeatureObject>(feat_last)->getObjectType();
-// }
+ //camera to feat (last)
+ Vector3d pos_obj_last = feat_last->getMeasurement().head(3);
+ Quaterniond quat_obj_last (feat_last->getMeasurement().tail(4).data());
-// auto outliers = ProcessorTrackerLandmarkObject::matchingRANSAC(last_incoming);
-// int nb_outliers = outliers.first.size();
-// Eigen::Isometry3d cf_M_ci = outliers.second;
+ Eigen::Isometry3d cl_M_o = Eigen::Translation<double,3>(pos_obj_last) * quat_obj_last;
+ cl_M_o_vec.push_back(cl_M_o);
+
+ Eigen::Isometry3d w_M_f_last = w_M_r_last * r_M_c * cl_M_o;
+ Vector3d pos_feat_last = w_M_f_last.translation();
+ int index_best_incoming = -1;
+ double min_e_pos = 100;
-// return 1;
-// }
+ int index_incoming = 0;
-// std::pair<std::vector<int>, Eigen::Isometry3d> ProcessorTrackerLandmarkObject::matchingRANSAC(std::vector<std::tuple<int, int, Eigen::Isometry3d, Eigen::Isometry3d> > last_incoming)
-// {
-// // Vector that will contains index of outliers for each iteration
-// std::vector<std::vector<int> > index_outliers;
-// int i = 0;
-// int best_score = 0;
-// Eigen::Isometry3d best_cl_M_ci = Eigen::Isometry3d::Identity();;
-// int index_best_score = -1;
+ for (auto feat_incoming : detections_incoming_)
+ {
+ std::string object_type_incoming = std::static_pointer_cast<FeatureObject>(feat_incoming)->getObjectType();
+ if (object_type_last == object_type_incoming)
+ {
+ //camera to feat
+ Vector3d pos_obj_incoming = feat_incoming->getMeasurement().head(3);
+ Quaterniond quat_obj_incoming (feat_incoming->getMeasurement().tail(4).data());
-// for (auto last_incoming_tuple : last_incoming)
-// {
-
-// std::vector<int> index_outliers_this_iteration;
+ Eigen::Isometry3d ci_M_o = Eigen::Translation<double,3>(pos_obj_incoming) * quat_obj_incoming;
+ ci_M_o_vec.push_back(ci_M_o);
-// int score = 0;
-// int index_feat_incoming = std::get<1>(last_incoming_tuple);
+ Eigen::Isometry3d w_M_f_incoming = w_M_r_incoming * r_M_c * ci_M_o;
+ Vector3d pos_feat_incoming = w_M_f_incoming.translation();
-// if (index_feat_incoming != -1)
-// {
+ // Error computation
+ double e_pos = (pos_feat_last - pos_feat_incoming).norm();
-// Eigen::Isometry3d c_M_f_last = std::get<2>(last_incoming_tuple);
-// Eigen::Isometry3d f_M_c_incoming = std::get<3>(last_incoming_tuple).inverse();
+ if (e_pos < e_pos_thr_ && e_pos < min_e_pos)
+ {
+ min_e_pos = e_pos;
+ index_best_incoming = index_incoming;
+ }
+ }
+ index_incoming++;
+ }
-// Eigen::Isometry3d cl_M_ci = c_M_f_last * f_M_c_incoming;
+ if (index_best_incoming != -1)
+ {
+ std::pair<int, int> pair_indexes = std::make_pair(index_last, index_best_incoming);
+ matches.push_back(pair_indexes);
+ }
-// for (auto last_incoming_tuple_other : last_incoming)
-// {
-
-// int index_feat_last = std::get<0>(last_incoming_tuple);
-// int index_feat_last_other = std::get<0>(last_incoming_tuple_other);
-
-// if (index_feat_incoming != -1 && index_feat_last != index_feat_last_other)
-// {
-// Eigen::Isometry3d cl_M_ol = std::get<2>(last_incoming_tuple_other);
-// Eigen::Isometry3d ci_M_oi = std::get<3>(last_incoming_tuple_other);
-
-// if(isInliers(cl_M_ol, ci_M_oi, cl_M_ci))
-// {
-// score++;
-// }
-// else
-// {
-// index_outliers_this_iteration.push_back(index_feat_last_other);
-// }
-// }
-// }
-
-// if (score > best_score)
-// {
-// best_score = score;
-// index_best_score = i;
-// best_cl_M_ci = cl_M_ci;
-// }
-
-// }
-
-// //Add index of outliers for this iteration of RANSAC
-// index_outliers.push_back(index_outliers_this_iteration);
-// i++;
-// }
+ index_last++;
+ }
-// if (index_best_score != -1)
-// {
-// std::pair<std::vector<int>, Eigen::Isometry3d> p{index_outliers[index_best_score], best_cl_M_ci};
-// return p;
-// }
-
+ std::vector<int> inliers_idx;
+ std::vector<int> outliers_idx;
+ Eigen::Isometry3d best_model = Eigen::Isometry3d::Identity();
+ bool RANSACWorks = ProcessorTrackerLandmarkObject::matchingRANSAC(cl_M_o_vec, ci_M_o_vec, matches, inliers_idx, outliers_idx, best_model, ratio_inliers_outliers_);
-// std::vector<int> v;
-// std::pair<std::vector<int>, Eigen::Isometry3d> p{v, best_cl_M_ci};
+ if (RANSACWorks)
+ {
+ std::cout << "RANSAC has worked" << std::endl;
+ // int nb_outliers = outliers_idx.size();
+ // Eigen::Isometry3d cf_M_ci = best_model;
+ }
-// return p;
-// }
+ return 1;
+}
bool ProcessorTrackerLandmarkObject::matchingRANSAC(const std::vector<Eigen::Isometry3d>& cl_M_o_vec,
const std::vector<Eigen::Isometry3d>& ci_M_o_vec,