diff --git a/include/objectslam/processor/processor_tracker_landmark_object.h b/include/objectslam/processor/processor_tracker_landmark_object.h
index 181e4bde1d16f26851c0803d48686b856b1fba52..700f66a385e5b47d8a2ef09e76cebbf725ebea7d 100644
--- a/include/objectslam/processor/processor_tracker_landmark_object.h
+++ b/include/objectslam/processor/processor_tracker_landmark_object.h
@@ -141,11 +141,42 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark
std::pair<double,double> errorsComputations(const Eigen::Isometry3d& _w_M_f,
std::shared_ptr<wolf::LandmarkObject> _lmk_obj);
-
+ /** \brief Method for matching feature from last to incoming
+ * \param _capture the capture in which the _landmarks_in should be searched
+ * \param _features_out_last returned list of features found in \b _capture corresponding to a landmark of _landmarks_in
+ * \param _features_out_incoming returned list of features found in incoming capture corresponding to a landmark of _landmarks_in
+ *
+
+ *
+ * Match the best(smallest error) feature of the list that respect the given threshold
+ *
+ * \return the number of landmarks found
+ */
unsigned int multiviewTypeMatching(const CaptureBasePtr& _capture,
FeatureBasePtrList& _features_out_last,
FeatureBasePtrList& _features_out_incoming);
+
+ void allMatchesSameType(const FeatureBasePtrList& _features_out_last,
+ const FeatureBasePtrList& _features_out_incoming);
+
+
+ /** \brief Method for matching feature from last to incoming
+ * \param _cl_M_o_vec vector of camera to object isometry for last capture
+ * \param _ci_M_o_vec vector of camera to object isometry for incoming capture
+ * \param _matches vector of indexes of matching feature from last to incoming
+ * \param _ratio_outliers_inliers threshold
+ * \param _inliers_idx vector of inliers
+ * \param _outliers_idx vector of outliers
+ * \param _best_model isometry of best model
+ *
+ *
+
+ *
+ * Match the best(smallest error) features between last and incoming of the list that respect the given threshold
+ *
+ * \return the number of landmarks found
+ */
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,
@@ -176,14 +207,36 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark
const Eigen::Isometry3d& _ci_M_oi,
const Eigen::Isometry3d& _cl_M_ci);
+
+ /** \brief Method for matching feature from last to incoming
+ * \param _matches_filtered vector of indexes of matching feature from last to incoming "without" outliers
+ * \param _features_out_last returned list of features found in \b _capture corresponding to a landmark of _landmarks_in
+ * \param _features_out_incoming returned list of features found in incoming capture corresponding to a landmark of _landmarks_in
+ * \param _trackMatrix matrix of all tracks of features along captures
+ *
+ */
static void processFeatures(const std::vector<std::pair<int,int> >& _matches_filtered,
const FeatureBasePtrList& _features_out_last,
const FeatureBasePtrList& _features_out_incoming,
TrackMatrix& _trackMatrix);
- static void filterMatchesOutliers(const std::vector<int>& outliers_idx,
- std::vector<std::pair<int,int> >& matches);
+ /** \brief Filter matches thanks to outliers vector
+ * \param _outliers_idx vector of outliers
+ * \param _matches vector of indexes of matching feature from last to incoming
+ *
+ * A function which remove from matches the outliers
+ *
+ */
+ static void filterMatchesOutliers(const std::vector<int>& _outliers_idx,
+ std::vector<std::pair<int,int> >& _matches);
+ /** \brief Filter matches thanks to inliers vector
+ * \param _inliers_idx vector of inliers
+ * \param matches vector of indexes of matching feature from last to incoming
+ *
+ * A function which create a vector based on _inliers_idx
+ *
+ */
static void filterMatchesInliers(const std::vector<int>& _inliers_idx,
std::vector<std::pair<int,int> >& _matches);
diff --git a/src/processor/processor_tracker_landmark_object.cpp b/src/processor/processor_tracker_landmark_object.cpp
index 54f2f30077d546bee2378fb559293f671c7369f4..15729a5f463d746ea30c90edeba857fb221e4dd4 100644
--- a/src/processor/processor_tracker_landmark_object.cpp
+++ b/src/processor/processor_tracker_landmark_object.cpp
@@ -496,6 +496,50 @@ unsigned int ProcessorTrackerLandmarkObject::multiviewTypeMatching(const Capture
return 1;
}
+void ProcessorTrackerLandmarkObject::allMatchesSameType(const FeatureBasePtrList& _features_out_last,
+ const FeatureBasePtrList& _features_out_incoming)
+{
+ std::vector<Eigen::Isometry3d> cl_M_o_vec;
+ std::vector<Eigen::Isometry3d> ci_M_o_vec;
+ std::vector<std::pair<int,int> > matches;
+
+ int index_last = 0;
+
+ 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());
+
+ Eigen::Isometry3d cl_M_o = Eigen::Translation<double,3>(pos_obj_last) * quat_obj_last;
+ cl_M_o_vec.push_back(cl_M_o);
+
+ int index_incoming = 0;
+
+ 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());
+
+ Eigen::Isometry3d ci_M_o = Eigen::Translation<double,3>(pos_obj_incoming) * quat_obj_incoming;
+ ci_M_o_vec.push_back(ci_M_o);
+
+ std::pair<int, int> pair_indexes = std::make_pair(index_last, index_incoming);
+ matches.push_back(pair_indexes);
+
+ }
+ index_incoming++;
+ }
+ }
+}
+
void ProcessorTrackerLandmarkObject::processFeatures(const std::vector<std::pair<int,int> >& _matches_filtered,
const FeatureBasePtrList& _features_out_last,
@@ -543,16 +587,21 @@ void ProcessorTrackerLandmarkObject::processFeatures(const std::vector<std::pair
void ProcessorTrackerLandmarkObject::filterMatchesOutliers(const std::vector<int>& _outliers_idx,
std::vector<std::pair<int,int> >& _matches)
{
- auto size = _matches.size();
+ //Number of matches
+ auto nbMatches = _matches.size();
- for (int i = 0; i < size; i++)
+ for (int i = 0; i < nbMatches; i++)
{
int index_outlier_occurrence = std::count(_outliers_idx.begin(), _outliers_idx.end(), _matches[i].first);
+ //Check if the outlier is in matches
if (index_outlier_occurrence != 0)
- {
+ {
+ //Delete outiler from matches
_matches.erase(std::remove(_matches.begin(), _matches.end(), _matches[i]));
- size--;
+ //Matches has lost an element
+ nbMatches--;
+ //Decrement counter to avoid skipping elements
i--;
}
}
@@ -562,19 +611,23 @@ void ProcessorTrackerLandmarkObject::filterMatchesOutliers(const std::vector<int
void ProcessorTrackerLandmarkObject::filterMatchesInliers(const std::vector<int>& _inliers_idx,
std::vector<std::pair<int,int> >& _matches)
{
+ //Matches that will store only inliers
std::vector<std::pair<int,int> > matches_temp;
for (auto pair_indexes : _matches)
{
int index_inlier_occurrence = std::count(_inliers_idx.begin(), _inliers_idx.end(), pair_indexes.first);
+ //Check if the inlier is in matches
if (index_inlier_occurrence != 0)
- {
+ {
+ //Append inlier to matches
matches_temp.push_back(pair_indexes);
}
}
_matches.clear();
+ //Modify matches given in parameters
_matches = matches_temp;
}
@@ -639,6 +692,7 @@ bool ProcessorTrackerLandmarkObject::matchingRANSAC(const std::vector<Eigen::Iso
}
}
+ //Feature which provide the model is an inlier
inliers_idx_buff.push_back(index_feat_last);
//If the model gives better results
diff --git a/test/gtest_processor_tracker_landmark_object.cpp b/test/gtest_processor_tracker_landmark_object.cpp
index ea6e980ed3e318c0cdf2d00f86eb07ae6e154093..743ac272c0efeae297bba4fdda8b2b95730780bf 100644
--- a/test/gtest_processor_tracker_landmark_object.cpp
+++ b/test/gtest_processor_tracker_landmark_object.cpp
@@ -478,11 +478,11 @@ TEST(ProcessorTrackerLandmarkObject, nbOfDifferentMatches)
auto pair_o4 = std::make_pair(3, 3);
auto pair_o5 = std::make_pair(4, 4);
auto pair_o6 = std::make_pair(8, 9);
- auto pair_o7 = std::make_pair(1, 4);
- auto pair_o8 = std::make_pair(3, 2);
- auto pair_o9 = std::make_pair(1, 6);
- auto pair_o10 = std::make_pair(4, 1);
- auto pair_o11 = std::make_pair(3, 7);
+ auto pair_o7 = std::make_pair(1, 4); //Not unique
+ auto pair_o8 = std::make_pair(3, 2); //Not unique
+ auto pair_o9 = std::make_pair(1, 6); //Not unique
+ auto pair_o10 = std::make_pair(4, 1); //Not unique
+ auto pair_o11 = std::make_pair(3, 7); //Not unique
auto pair_o12 = std::make_pair(6, 7);
//Append pairs in matches object
@@ -573,6 +573,7 @@ TEST(ProcessorTrackerLandmarkObject, filterMatchesOutliers)
ASSERT_TRUE(matches1.size() == 9);
ASSERT_TRUE(matches2.size() == 8);
ASSERT_TRUE(matches3.size() == 7);
+
ASSERT_TRUE(matches1I.size() == 3);
ASSERT_TRUE(matches2I.size() == 4);
ASSERT_TRUE(matches3I.size() == 5);