diff --git a/include/objectslam/processor/processor_tracker_landmark_object.h b/include/objectslam/processor/processor_tracker_landmark_object.h
index 6bfd5139fa3b9cb10b7c258410e728fe04bfede7..181e4bde1d16f26851c0803d48686b856b1fba52 100644
--- a/include/objectslam/processor/processor_tracker_landmark_object.h
+++ b/include/objectslam/processor/processor_tracker_landmark_object.h
@@ -111,9 +111,9 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark
* \return the number of landmarks found
*/
unsigned int firstLmkMatching(const LandmarkBasePtrList& _landmarks_in,
- const CaptureBasePtr& _capture,
- FeatureBasePtrList& _features_out,
- LandmarkMatchMap& _feature_landmark_correspondences);
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out,
+ LandmarkMatchMap& _feature_landmark_correspondences);
/** \brief Second method for matching lmk and feat
* \param _landmarks_in input list of landmarks to be found
@@ -127,9 +127,9 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark
* \return the number of landmarks found
*/
unsigned int bestLmkMatching(const LandmarkBasePtrList& _landmarks_in,
- const CaptureBasePtr& _capture,
- FeatureBasePtrList& _features_out,
- LandmarkMatchMap& _feature_landmark_correspondences);
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out,
+ LandmarkMatchMap& _feature_landmark_correspondences);
/** \brief Compute errors
* \param _w_M_f an isometry world to feature
* \param _lmk_obj a pointer of lmk
@@ -138,21 +138,21 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark
*
* \return A pair of errors (position and orientation)
*/
- std::pair<double,double> errorsComputations(Eigen::Isometry3d _w_M_f,
+ std::pair<double,double> errorsComputations(const Eigen::Isometry3d& _w_M_f,
std::shared_ptr<wolf::LandmarkObject> _lmk_obj);
unsigned int multiviewTypeMatching(const CaptureBasePtr& _capture,
- FeatureBasePtrList& _features_out_last,
- FeatureBasePtrList& _features_out_incoming);
+ FeatureBasePtrList& _features_out_last,
+ FeatureBasePtrList& _features_out_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,
- double _ratio_outliers_inliers);
+ const std::vector<Eigen::Isometry3d>& _ci_M_o_vec,
+ const std::vector<std::pair<int,int> >& _matches,
+ const double _ratio_outliers_inliers,
+ std::vector<int>& _inliers_idx,
+ std::vector<int>& _outliers_idx,
+ Eigen::Isometry3d& _best_model);
/** \brief Count the number of different matches
* \param matches A vector of pair of matches between last and incoming
@@ -172,16 +172,20 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark
*
* \return A bool
*/
- static bool isInliers(Eigen::Isometry3d _cl_M_ol, Eigen::Isometry3d _ci_M_oi, Eigen::Isometry3d _cl_M_ci);
+ static bool isInliers(const Eigen::Isometry3d& _cl_M_ol,
+ const Eigen::Isometry3d& _ci_M_oi,
+ const Eigen::Isometry3d& _cl_M_ci);
static void processFeatures(const std::vector<std::pair<int,int> >& _matches_filtered,
- TrackMatrix& _trackMatrix,
- const FeatureBasePtrList& _features_out_last,
- const FeatureBasePtrList& _features_out_incoming);
+ const FeatureBasePtrList& _features_out_last,
+ const FeatureBasePtrList& _features_out_incoming,
+ TrackMatrix& _trackMatrix);
- static void filterMatchesOutliers(std::vector<std::pair<int,int> >& matches, const std::vector<int>& outliers_idx);
+ static void filterMatchesOutliers(const std::vector<int>& outliers_idx,
+ std::vector<std::pair<int,int> >& matches);
- static void filterMatchesInliers(std::vector<std::pair<int,int> >& _matches, const std::vector<int>& _inliers_idx);
+ static void filterMatchesInliers(const std::vector<int>& _inliers_idx,
+ std::vector<std::pair<int,int> >& _matches);
const TrackMatrix& getTrackMatrix() const {return track_matrix_;}
diff --git a/src/processor/processor_tracker_landmark_object.cpp b/src/processor/processor_tracker_landmark_object.cpp
index d1345bf6eb043123f5f079b1928472a6e10405d0..32c1141ea0aa8413d87f4b6bf950cba782a2fc5f 100644
--- a/src/processor/processor_tracker_landmark_object.cpp
+++ b/src/processor/processor_tracker_landmark_object.cpp
@@ -269,7 +269,8 @@ unsigned int ProcessorTrackerLandmarkObject::firstLmkMatching(const LandmarkBase
return _features_out.size();
}
-std::pair<double,double> ProcessorTrackerLandmarkObject::errorsComputations(Eigen::Isometry3d _w_M_f, std::shared_ptr<wolf::LandmarkObject> _lmk_obj)
+std::pair<double,double> ProcessorTrackerLandmarkObject::errorsComputations(const Eigen::Isometry3d& _w_M_f,
+ std::shared_ptr<wolf::LandmarkObject> _lmk_obj)
{
Quaterniond quat_feat;
@@ -400,8 +401,8 @@ unsigned int ProcessorTrackerLandmarkObject::findLandmarks(const LandmarkBasePtr
}
unsigned int ProcessorTrackerLandmarkObject::multiviewTypeMatching(const CaptureBasePtr& _capture,
- FeatureBasePtrList& _features_out_last,
- FeatureBasePtrList& _features_out_incoming)
+ FeatureBasePtrList& _features_out_last,
+ FeatureBasePtrList& _features_out_incoming)
{
if (!last_ptr_){
return 0;
@@ -481,24 +482,25 @@ unsigned int ProcessorTrackerLandmarkObject::multiviewTypeMatching(const Capture
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_);
+ bool RANSACWorks = ProcessorTrackerLandmarkObject::matchingRANSAC(cl_M_o_vec, ci_M_o_vec, matches, ratio_inliers_outliers_, inliers_idx, outliers_idx, best_model);
if (RANSACWorks)
{
std::cout << "RANSAC has worked" << std::endl;
//Keep only inliers
- ProcessorTrackerLandmarkObject::filterMatchesInliers(matches, inliers_idx);
+ ProcessorTrackerLandmarkObject::filterMatchesInliers(inliers_idx, matches);
}
return 1;
}
+
void ProcessorTrackerLandmarkObject::processFeatures(const std::vector<std::pair<int,int> >& _matches_filtered,
- TrackMatrix& _trackMatrix,
const FeatureBasePtrList& _features_out_last,
- const FeatureBasePtrList& _features_out_incoming)
+ const FeatureBasePtrList& _features_out_incoming,
+ TrackMatrix& _trackMatrix)
{
for (auto match : _matches_filtered)
{
@@ -538,7 +540,8 @@ void ProcessorTrackerLandmarkObject::processFeatures(const std::vector<std::pair
}
}
-void ProcessorTrackerLandmarkObject::filterMatchesOutliers(std::vector<std::pair<int,int> >& _matches, const std::vector<int>& _outliers_idx)
+void ProcessorTrackerLandmarkObject::filterMatchesOutliers(const std::vector<int>& _outliers_idx,
+ std::vector<std::pair<int,int> >& _matches)
{
auto size = _matches.size();
@@ -556,8 +559,8 @@ void ProcessorTrackerLandmarkObject::filterMatchesOutliers(std::vector<std::pair
}
-void ProcessorTrackerLandmarkObject::filterMatchesInliers(std::vector<std::pair<int,int> >& _matches,
- const std::vector<int>& _inliers_idx)
+void ProcessorTrackerLandmarkObject::filterMatchesInliers(const std::vector<int>& _inliers_idx,
+ std::vector<std::pair<int,int> >& _matches)
{
std::vector<std::pair<int,int> > matches_temp;
@@ -578,10 +581,11 @@ void ProcessorTrackerLandmarkObject::filterMatchesInliers(std::vector<std::pair<
bool ProcessorTrackerLandmarkObject::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,
+ const double _ratio_outliers_inliers,
std::vector<int>& _inliers_idx,
std::vector<int>& _outliers_idx,
- Eigen::Isometry3d& _best_model,
- double _ratio_outliers_inliers)
+ Eigen::Isometry3d& _best_model)
+
{
//Check if the dataset has a sufficient size
@@ -689,7 +693,9 @@ int ProcessorTrackerLandmarkObject::nbOfDifferentMatches(const std::vector<std::
}
-bool ProcessorTrackerLandmarkObject::isInliers(Eigen::Isometry3d _cl_M_ol, Eigen::Isometry3d _ci_M_oi, Eigen::Isometry3d _cl_M_ci)
+bool ProcessorTrackerLandmarkObject::isInliers(const Eigen::Isometry3d& _cl_M_ol,
+ const Eigen::Isometry3d& _ci_M_oi,
+ const Eigen::Isometry3d& _cl_M_ci)
{
//Thresholds
double e_pos_th = 1;
diff --git a/test/gtest_processor_tracker_landmark_object.cpp b/test/gtest_processor_tracker_landmark_object.cpp
index 95b4277a4f2daf0de68f573dcb52ec7456293209..ea6e980ed3e318c0cdf2d00f86eb07ae6e154093 100644
--- a/test/gtest_processor_tracker_landmark_object.cpp
+++ b/test/gtest_processor_tracker_landmark_object.cpp
@@ -400,7 +400,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, 0.55);
+ ProcessorTrackerLandmarkObject::matchingRANSAC(cl_M_o_vec, ci_M_o_vec, matches, 0.55, inliers_idx, outliers_idx, best_model);
Quaterniond quat_cam(cl_M_ci.linear());
Vector3d pos_cam = cl_M_ci.translation();
@@ -409,7 +409,7 @@ TEST_F(ProcessorTrackerLandmarkObject_fixture, matchingRANSAC)
Vector3d pos_cam_RANSAC = best_model.translation();
ASSERT_TRUE(matches.size() == 5);
- ProcessorTrackerLandmarkObject::filterMatchesOutliers(matches, outliers_idx);
+ ProcessorTrackerLandmarkObject::filterMatchesOutliers(outliers_idx, matches);
ASSERT_TRUE(matches.size() == 3);
ASSERT_MATRIX_APPROX(pos_cam, pos_cam_RANSAC, 1e-6)
@@ -538,7 +538,7 @@ TEST(ProcessorTrackerLandmarkObject, filterMatchesOutliers)
auto pair_o7 = std::make_pair(6, 6);
auto pair_o8 = std::make_pair(7, 7);
auto pair_o9 = std::make_pair(8, 8);
- auto pair_o10 = std::make_pair(9, 9);
+ auto pair_o10 = std::make_pair(9, 9);
auto pair_o11 = std::make_pair(10, 10);
auto pair_o12 = std::make_pair(11, 11);
@@ -562,13 +562,13 @@ TEST(ProcessorTrackerLandmarkObject, filterMatchesOutliers)
matches2I = matches1;
matches3I = matches1;
- ProcessorTrackerLandmarkObject::filterMatchesOutliers(matches1, outliers_idx1);
- ProcessorTrackerLandmarkObject::filterMatchesOutliers(matches2, outliers_idx2);
- ProcessorTrackerLandmarkObject::filterMatchesOutliers(matches3, outliers_idx3);
+ ProcessorTrackerLandmarkObject::filterMatchesOutliers(outliers_idx1, matches1);
+ ProcessorTrackerLandmarkObject::filterMatchesOutliers(outliers_idx2, matches2);
+ ProcessorTrackerLandmarkObject::filterMatchesOutliers(outliers_idx3, matches3);
- ProcessorTrackerLandmarkObject::filterMatchesInliers(matches1I, outliers_idx1);
- ProcessorTrackerLandmarkObject::filterMatchesInliers(matches2I, outliers_idx2);
- ProcessorTrackerLandmarkObject::filterMatchesInliers(matches3I, outliers_idx3);
+ ProcessorTrackerLandmarkObject::filterMatchesInliers(outliers_idx1, matches1I);
+ ProcessorTrackerLandmarkObject::filterMatchesInliers(outliers_idx2, matches2I);
+ ProcessorTrackerLandmarkObject::filterMatchesInliers(outliers_idx3, matches3I);
ASSERT_TRUE(matches1.size() == 9);
ASSERT_TRUE(matches2.size() == 8);