diff --git a/include/objectslam/processor/processor_tracker_landmark_object.h b/include/objectslam/processor/processor_tracker_landmark_object.h
index 1ced5fd6dbf6da82d8fbfd15e9fa1be46fd4f927..065de2c5dab9f512e1484df8ae86bb98680f2396 100644
--- a/include/objectslam/processor/processor_tracker_landmark_object.h
+++ b/include/objectslam/processor/processor_tracker_landmark_object.h
@@ -146,15 +146,32 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark
FeatureBasePtrList& _features_out_last,
FeatureBasePtrList& _features_out_incoming);
- // static std::pair<std::vector<int>, Eigen::Isometry3d> matchingRANSAC(std::vector<std::tuple<int, int, Eigen::Isometry3d, Eigen::Isometry3d> > last_incoming);
-
static bool matchingRANSAC(const std::vector<Eigen::Isometry3d>& cl_M_o_vec,
const std::vector<Eigen::Isometry3d>& ci_M_o_vec,
const std::vector<std::pair<int,int> >& matches,
std::vector<int>& inliers_idx,
std::vector<int>& outliers_idx,
- Eigen::Isometry3d& best_model);
+ Eigen::Isometry3d& best_model,
+ double ratio_outliers_inliers);
+
+ /** \brief Count the number of different matches
+ * \param matches A vector of pair of matches between last and incoming
+ *
+ * A function which count number of unique pair (not the same index for last or for incoming)
+ *
+ * \return an int
+ */
+ static int nbOfDifferentMatches(const std::vector<std::pair<int,int> >& matches);
+ /** \brief Check if three transformations correspond to an inlier
+ * \param ci_M_oi camera pose isometry last to incoming
+ * \param cl_M_ol an isometry camera to object (last frame)
+ * \param ci_M_oi an isometry camera to object (incoming frame)
+ *
+ * A function which check if ol_M_cl * cl_M_ci * ci_M_oi is similar to identity (if the object is an inlier)
+ *
+ * \return A bool
+ */
static bool isInliers(Eigen::Isometry3d cl_M_ol, Eigen::Isometry3d ci_M_oi, Eigen::Isometry3d cl_M_ci);
diff --git a/src/processor/processor_tracker_landmark_object.cpp b/src/processor/processor_tracker_landmark_object.cpp
index 5191ec5063a08d22c9c6ab30bdb48fcd64cdb051..106f4a9a0639f7d597f4b960524c0d0a5e22bf0d 100644
--- a/src/processor/processor_tracker_landmark_object.cpp
+++ b/src/processor/processor_tracker_landmark_object.cpp
@@ -568,8 +568,14 @@ bool ProcessorTrackerLandmarkObject::matchingRANSAC(const std::vector<Eigen::Iso
const std::vector<std::pair<int,int> >& matches,
std::vector<int>& inliers_idx,
std::vector<int>& outliers_idx,
- Eigen::Isometry3d& best_model)
+ Eigen::Isometry3d& best_model,
+ double ratio_outliers_inliers)
{
+
+ //Check if the dataset has a sufficient size
+ if (nbOfDifferentMatches(matches) < 3)
+ return false;
+
// Vector that will contains index of inliers/outliers for each iteration
std::vector<int> inliers_idx_buff;
std::vector<int> outliers_idx_buff;
@@ -635,12 +641,34 @@ bool ProcessorTrackerLandmarkObject::matchingRANSAC(const std::vector<Eigen::Iso
int nb_inliers = inliers_idx.size();
int nb_outliers = outliers_idx.size();
- if ((double)nb_outliers/nb_inliers > 0.55)
+ if ((double)nb_outliers/nb_inliers > ratio_outliers_inliers)
return true;
return false;
}
+int ProcessorTrackerLandmarkObject::nbOfDifferentMatches(const std::vector<std::pair<int,int> >& matches)
+{
+ int nb_of_diff_matches = 0;
+
+ //Vectors that will store new indexes
+ std::vector<int> index_last_feat;
+ std::vector<int> index_incoming_feat;
+
+ for (auto match : matches)
+ {
+ //Check if the pair has unique indexes
+ if(!(std::count(index_last_feat.begin(), index_last_feat.end(), match.first)) && !(std::count(index_incoming_feat.begin(), index_incoming_feat.end(), match.second)))
+ {
+ nb_of_diff_matches++;
+ index_last_feat.push_back(match.first);
+ index_incoming_feat.push_back(match.second);
+ }
+ }
+
+ return nb_of_diff_matches;
+}
+
bool ProcessorTrackerLandmarkObject::isInliers(Eigen::Isometry3d cl_M_ol, Eigen::Isometry3d ci_M_oi, Eigen::Isometry3d cl_M_ci)
{
@@ -661,10 +689,7 @@ bool ProcessorTrackerLandmarkObject::isInliers(Eigen::Isometry3d cl_M_ol, Eigen:
Eigen::Matrix3d wRf_i = identity.linear();
quat_feat_identity.coeffs() = R2q(wRf_i).coeffs().transpose();
Vector3d pos_feat_identity = identity.translation();
-
- std::cout << "\n\n\n\n\n" << quat_feat.coeffs().transpose() << " " << quat_feat_identity.coeffs().transpose() << std::endl;
- std::cout << pos_feat << "\n" << pos_feat_identity << "\n\n\n\n\n" << std::endl;
-
+
// Error between identity and ol_M_oi
double e_pos = (pos_feat_identity - pos_feat).norm();
double e_rot = log_q(quat_feat_identity.conjugate() * quat_feat).norm();
diff --git a/test/gtest_processor_tracker_landmark_object.cpp b/test/gtest_processor_tracker_landmark_object.cpp
index 5a2b0593c31ad544946574e0e261e48dc049e72b..9c4e83eebaca448afd804bf49a47bf8e81cfb6ef 100644
--- a/test/gtest_processor_tracker_landmark_object.cpp
+++ b/test/gtest_processor_tracker_landmark_object.cpp
@@ -397,7 +397,7 @@ TEST_F(ProcessorTrackerLandmarkObject_fixture, matchingRANSAC)
matches.push_back(pair_o5);
//Detect all outliers of our batch
- ProcessorTrackerLandmarkObject::matchingRANSAC(cl_M_o_vec, ci_M_o_vec, matches, inliers_idx, outliers_idx, best_model);
+ ProcessorTrackerLandmarkObject::matchingRANSAC(cl_M_o_vec, ci_M_o_vec, matches, inliers_idx, outliers_idx, best_model, 0.55);
Quaterniond quat_cam(cl_M_ci.linear());
Vector3d pos_cam = cl_M_ci.translation();