diff --git a/include/objectslam/processor/processor_tracker_landmark_object.h b/include/objectslam/processor/processor_tracker_landmark_object.h
index aee02d389e14e1fa3c7b7fae18ba54f2e6a87db0..fff207e2fe5e2f87ca691968992234a47de51edd 100644
--- a/include/objectslam/processor/processor_tracker_landmark_object.h
+++ b/include/objectslam/processor/processor_tracker_landmark_object.h
@@ -144,6 +144,11 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark
FeatureBasePtrList& _features_out_last,
FeatureBasePtrList& _features_out_incoming);
+ std::vector<int> matchingRANSAC(std::vector<std::tuple<int, int, Eigen::Isometry3d, Eigen::Isometry3d> > last_incoming);
+
+ bool isInliers(Eigen::Isometry3d cl_M_ol, Eigen::Isometry3d ci_M_oi, Eigen::Isometry3d cl_M_ci);
+
+
/** \brief Vote for KeyFrame generation
*
* If a KeyFrame criterion is validated, this function returns true,
diff --git a/src/processor/processor_tracker_landmark_object.cpp b/src/processor/processor_tracker_landmark_object.cpp
index 0b99ceef5af59889331f75b5d70dfa0cbdfdd408..93b2411ff3993f5670f957829e4a6f72b19f3f59 100644
--- a/src/processor/processor_tracker_landmark_object.cpp
+++ b/src/processor/processor_tracker_landmark_object.cpp
@@ -110,8 +110,9 @@ LandmarkBasePtr ProcessorTrackerLandmarkObject::emplaceLandmark(FeatureBasePtr _
Eigen::Isometry3d w_M_t = w_M_r * r_M_c * c_M_t;
// make 7-vector for lmk pose
- pos = w_M_t.translation();
+ auto pos = w_M_t.translation();
Eigen::Matrix3d wRt = w_M_t.linear();
+ Quaterniond quat;
quat.coeffs() = R2q(wRt).coeffs().transpose();
Vector7d w_pose_t;
w_pose_t << pos, quat.coeffs();
@@ -333,20 +334,23 @@ unsigned int ProcessorTrackerLandmarkObject::bestLmkMatching(const LandmarkBaseP
//Create a pair that stock both position and rotation error
auto error = errorsComputations(w_M_f, lmk_obj);
-
//Test if the error is below the thresholds
if (error.first < e_pos_thr_ && error.second < e_rot_thr_)
{
//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 << std::endl << std::endl << std::endl << std::endl << error.first << " " << error.second << 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;
- match = true;
+ match = true;
}
}
}
@@ -406,28 +410,19 @@ unsigned int ProcessorTrackerLandmarkObject::multiviewTypeMatching(const Capture
return 0;
}
- //world to rob
+ //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"));
-
- std::vector<std::pair<std::string, Eigen::Isometry3d> > feats_incoming;
+ int index_last = 0;
- for (auto feat_incoming : detections_incoming_)
- {
- std::string object_type_incoming = std::static_pointer_cast<FeatureObject>(feat_incoming)->getObjectType();
-
- 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;
-
- std::pair<std::string, Eigen::Isometry3d> p(object_type_incoming, w_M_f_incoming);
- feats_incoming.push_back(p);
- }
+ //matches between last and incoming features
+ std::vector<std::tuple<int, int, Eigen::Isometry3d, Eigen::Isometry3d> > last_incoming;
for (auto feat_last : detections_last_)
{
@@ -439,20 +434,141 @@ unsigned int ProcessorTrackerLandmarkObject::multiviewTypeMatching(const Capture
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();
- std::cout << w_M_f_last(0,0) << std::endl;
+ int index_min_incoming = -1;
+ double min_e_pos = 100;
- for (auto feat_pair_incoming : feats_incoming)
+ for (auto feat_incoming : detections_incoming_)
{
- if (object_type_last == feat_pair_incoming.first)
+ int index_incoming = 0;
+
+ std::string object_type_incoming = std::static_pointer_cast<FeatureObject>(feat_incoming)->getObjectType();
+
+ if (object_type_last == object_type_incoming)
+ {
+
+ 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;
+
+ Vector3d pos_feat_incoming = w_M_f_incoming.translation();
+
+ // Error computation
+ double e_pos = (pos_feat_last - pos_feat_incoming).norm();
+
+ 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);
+
+ }
+ }
+
+ std::cout << index_min_incoming;
+
+ }
+ return 1;
+}
+
+std::vector<int> 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;
+ int index_best_score = -1;
+
+
+ for (auto last_incoming_tuple : last_incoming)
+ {
+
+ std::vector<int> index_outliers_this_iteration;
+
+ int score = 0;
+ int index_feat_incoming = std::get<1>(last_incoming_tuple);
+
+ if (index_feat_incoming != -1)
+ {
+
+ 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();
+
+ Eigen::Isometry3d cl_M_ci = c_M_f_last * f_M_c_incoming;
+
+ 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);
+
+ //Check if there is a match and if
+ 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, cl_M_ci, ci_M_oi))
+ {
+ score++;
+ }
+ else
+ {
+ index_outliers_this_iteration.push_back(index_feat_last_other);
+ }
+ }
+ }
+
+ if (score > best_score)
+ {
+ best_score = score;
+ index_best_score = i;
}
+
}
+
+ //Add index of outliers for this iteration of RANSAC
+ index_outliers.push_back(index_outliers_this_iteration);
+ i++;
}
- return 1;
-}
+ if (index_best_score != -1)
+ return index_outliers[index_best_score];
+
+ return index_outliers[0];
+}
+
+bool ProcessorTrackerLandmarkObject::isInliers(Eigen::Isometry3d cl_M_ol, Eigen::Isometry3d ci_M_oi, Eigen::Isometry3d cl_M_ci)
+{
+ double e_pos_th = 0.5;
+ double e_rot_th = 0.5;
+
+
+ Eigen::Isometry3d ol_M_cl = cl_M_ol.inverse();
+ Eigen::Isometry3d ol_M_oi = ol_M_cl * cl_M_ci * ci_M_oi;
+
+ Quaterniond quat_feat;
+ Eigen::Matrix3d wRf = ol_M_oi.linear();
+ quat_feat.coeffs() = R2q(wRf).coeffs().transpose();
+ Vector3d pos_feat = ol_M_oi.translation();
+
+ Eigen::Isometry3d identity = Eigen::Isometry3d::Identity();
+ Quaterniond quat_feat_identity;
+ Eigen::Matrix3d wRf_i = identity.linear();
+ quat_feat_identity.coeffs() = R2q(wRf_i).coeffs().transpose();
+ Vector3d pos_feat_identity = identity.translation();
+
+
+ // Error computation
+ double e_pos = (pos_feat_identity - pos_feat).norm();
+ double e_rot = log_q(quat_feat_identity.conjugate() * quat_feat).norm();
+
+ return (e_pos < e_pos_th && e_rot < e_rot_th);
+}