From b9b3d3a7d15acff3bc8a01bf5650349b890ae1b5 Mon Sep 17 00:00:00 2001
From: ydepledt <yanndepledt360@gmail.com>
Date: Wed, 29 Jun 2022 17:16:21 +0200
Subject: [PATCH] Refactoring gtest
---
.../processor_tracker_landmark_object.h | 22 +-
.../processor_tracker_landmark_object.cpp | 78 ++-
...test_processor_tracker_landmark_object.cpp | 591 ++++++++----------
3 files changed, 332 insertions(+), 359 deletions(-)
diff --git a/include/objectslam/processor/processor_tracker_landmark_object.h b/include/objectslam/processor/processor_tracker_landmark_object.h
index 7dadcb5..6a65a1d 100644
--- a/include/objectslam/processor/processor_tracker_landmark_object.h
+++ b/include/objectslam/processor/processor_tracker_landmark_object.h
@@ -157,8 +157,26 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark
FeatureBasePtrList& _features_out_incoming);
- static std::vector<std::pair<int,int> > allMatchesSameType(const FeatureBasePtrList& _features_out_last,
- const FeatureBasePtrList& _features_out_incoming);
+ static void keepInliers(const std::vector<Eigen::Isometry3d>& _cl_M_o_vec,
+ const std::vector<Eigen::Isometry3d>& _ci_M_o_vec,
+ std::vector<std::pair<int,int> >& matches,
+ Eigen::Isometry3d& _best_model);
+
+
+ /** \brief Method for create all pair of features (same type) from last and incoming
+ * \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 _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
+ *
+
+ *
+ * \return All matches available for those two lists
+ */
+ static std::vector<std::pair<int,int> > createAllMatchesSameType(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);
/** \brief Method for matching feature from last to incoming
diff --git a/src/processor/processor_tracker_landmark_object.cpp b/src/processor/processor_tracker_landmark_object.cpp
index 92f1b74..72242a8 100644
--- a/src/processor/processor_tracker_landmark_object.cpp
+++ b/src/processor/processor_tracker_landmark_object.cpp
@@ -516,14 +516,39 @@ std::ostream& operator<<(std::ostream &flux, std::vector<int> vector)
return flux;
}
-std::vector<std::pair<int,int> > ProcessorTrackerLandmarkObject::allMatchesSameType(const FeatureBasePtrList& _features_out_last,
- const FeatureBasePtrList& _features_out_incoming)
+void ProcessorTrackerLandmarkObject::keepInliers(const std::vector<Eigen::Isometry3d>& _cl_M_o_vec,
+ const std::vector<Eigen::Isometry3d>& _ci_M_o_vec,
+ std::vector<std::pair<int,int> >& matches,
+ Eigen::Isometry3d& _best_model)
+{
+ //Vector that will store inliers/outliers pairs
+ std::vector<std::pair<int,int> > inliers_idx;
+ std::vector<std::pair<int,int> > outliers_idx;
+
+ bool RANSACWorks = ProcessorTrackerLandmarkObject::matchingRANSAC(_cl_M_o_vec, _ci_M_o_vec, matches, 0.10, inliers_idx, outliers_idx, _best_model);
+
+ if (RANSACWorks)
+ {
+ std::cout << "RANSAC has worked" << std::endl;
+
+ //Keep only inliers
+ ProcessorTrackerLandmarkObject::filterMatchesOutliers(outliers_idx, matches);
+
+ }
+}
+
+std::vector<std::pair<int,int> > ProcessorTrackerLandmarkObject::createAllMatchesSameType(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<Eigen::Isometry3d> cl_M_o_vec;
- std::vector<Eigen::Isometry3d> ci_M_o_vec;
- std::vector<std::pair<int,int> > matches;
+ //Vector that store all matches available
+ std::vector<std::pair<int,int> > all_matches;
int index_last = 0;
+
+ //First pass throught the loop
bool pushBack = true;
for (auto feat_last : _features_out_last)
@@ -533,60 +558,43 @@ std::vector<std::pair<int,int> > ProcessorTrackerLandmarkObject::allMatchesSameT
//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);
+ _cl_M_o_vec.push_back(cl_M_o);
int index_incoming = 0;
for (auto feat_incoming : _features_out_incoming)
{
std::string object_type_incoming = std::static_pointer_cast<FeatureObject>(feat_incoming)->getObjectType();
-
+
+ //Create a pair only if features have same types
if (object_type_last == object_type_incoming)
{
std::pair<int, int> pair_indexes = std::make_pair(index_last, index_incoming);
- matches.push_back(pair_indexes);
+ all_matches.push_back(pair_indexes);
}
- //camera to feat
+ //if first pass throught the loop
if (pushBack)
{
-
+
+ //camera to feat (incoming)
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);
+ _ci_M_o_vec.push_back(ci_M_o);
}
index_incoming++;
}
index_last++;
- pushBack = false;
+ //Not first pass throught the loop
+ pushBack = false;
}
- // std::cout << matches << "\n";
-
-
- std::vector<std::pair<int,int> > inliers_idx;
- std::vector<std::pair<int,int> > outliers_idx;
- Eigen::Isometry3d best_model = Eigen::Isometry3d::Identity();
- bool RANSACWorks = ProcessorTrackerLandmarkObject::matchingRANSAC(cl_M_o_vec, ci_M_o_vec, matches, 0.10, inliers_idx, outliers_idx, best_model);
-
- if (RANSACWorks)
- {
- std::cout << "RANSAC has worked" << std::endl;
-
- //Keep only inliers
- ProcessorTrackerLandmarkObject::filterMatchesOutliers(outliers_idx, matches);
-
- }
-
- return matches;
+ return all_matches;
}
@@ -771,8 +779,10 @@ 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 > _ratio_outliers_inliers)
+ if ((double)nb_outliers/nb_inliers > _ratio_outliers_inliers) {
return true;
+ }
+
return true;
}
diff --git a/test/gtest_processor_tracker_landmark_object.cpp b/test/gtest_processor_tracker_landmark_object.cpp
index 2b2b04c..566b8b0 100644
--- a/test/gtest_processor_tracker_landmark_object.cpp
+++ b/test/gtest_processor_tracker_landmark_object.cpp
@@ -16,50 +16,74 @@ using namespace Eigen;
using namespace wolf;
using std::static_pointer_cast;
-
////////////////////////////////////////////////////////////////
/*
* Wrapper class to be able to have setOriginPtr() and setLastPtr() in ProcessorTrackerLandmarkObject
*/
WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmarkObject_Wrapper);
+
+
+
+Eigen::Isometry3d appendFeatureLast(const std::string& _object_type, Eigen::Vector7d& _pose, FeatureBasePtrList& _feature_last)
+
+{
+ _pose.tail<4>() = _pose.tail<4>().normalized();
+ Eigen::Isometry3d cl_M_f = posevec_to_isometry(_pose);
+ FeatureBasePtr f = std::make_shared<FeatureObject>(_pose, Matrix6d::Identity(), _object_type);
+ _feature_last.push_back(f);
+
+ return cl_M_f;
+}
+
+
+void appendFeaturencoming(const std::string& _object_type, const Eigen::Isometry3d& _cl_M_f, const Eigen::Isometry3d& _ci_M_cl, FeatureBasePtrList& _features_incoming)
+{
+ Eigen::Isometry3d ci_M_f = _ci_M_cl * _cl_M_f;
+ Eigen::Quaterniond quat(ci_M_f.linear());
+ Eigen::Vector3d pos = ci_M_f.translation();
+ Eigen::Vector7d pose;
+ pose << pos, quat.coeffs();
+ FeatureBasePtr f = std::make_shared<FeatureObject>(pose, Matrix6d::Identity(), _object_type);
+ _features_incoming.push_back(f);
+
+}
+
+
class ProcessorTrackerLandmarkObject_Wrapper : public ProcessorTrackerLandmarkObject
{
- public:
- ProcessorTrackerLandmarkObject_Wrapper(ParamsProcessorTrackerLandmarkObjectPtr _params_tracker_landmark_object) :
- ProcessorTrackerLandmarkObject(_params_tracker_landmark_object)
- {
- setType("ProcessorTrackerLandmarkObject_Wrapper");
- };
- ~ProcessorTrackerLandmarkObject_Wrapper() override{}
- void setOriginPtr(const CaptureBasePtr _origin_ptr) { origin_ptr_ = _origin_ptr; }
- void setLastPtr (const CaptureBasePtr _last_ptr) { last_ptr_ = _last_ptr; }
- void setIncomingPtr (const CaptureBasePtr _incoming_ptr) { incoming_ptr_ = _incoming_ptr; }
- unsigned int getMinFeaturesForKeyframe (){return min_features_for_keyframe_;}
- double getMinTimeVote (){return min_time_vote_;}
- void setIncomingDetections(const FeatureBasePtrList _incoming_detections) { detections_incoming_ = _incoming_detections; }
- void setLastDetections(const FeatureBasePtrList _last_detections) { detections_last_ = _last_detections; }
-
- // for factory
- static ProcessorBasePtr create(const std::string& _unique_name, const ParamsProcessorBasePtr _params)
- {
- auto prc_object_params_ = std::static_pointer_cast<ParamsProcessorTrackerLandmarkObject>(_params);
-
- auto prc_ptr = std::make_shared<ProcessorTrackerLandmarkObject_Wrapper>(prc_object_params_);
-
- prc_ptr->setName(_unique_name);
-
- return prc_ptr;
- }
+public:
+ ProcessorTrackerLandmarkObject_Wrapper(ParamsProcessorTrackerLandmarkObjectPtr _params_tracker_landmark_object) : ProcessorTrackerLandmarkObject(_params_tracker_landmark_object)
+ {
+ setType("ProcessorTrackerLandmarkObject_Wrapper");
+ };
+ ~ProcessorTrackerLandmarkObject_Wrapper() override {}
+ void setOriginPtr(const CaptureBasePtr _origin_ptr) { origin_ptr_ = _origin_ptr; }
+ void setLastPtr(const CaptureBasePtr _last_ptr) { last_ptr_ = _last_ptr; }
+ void setIncomingPtr(const CaptureBasePtr _incoming_ptr) { incoming_ptr_ = _incoming_ptr; }
+ unsigned int getMinFeaturesForKeyframe() { return min_features_for_keyframe_; }
+ double getMinTimeVote() { return min_time_vote_; }
+ void setIncomingDetections(const FeatureBasePtrList _incoming_detections) { detections_incoming_ = _incoming_detections; }
+ void setLastDetections(const FeatureBasePtrList _last_detections) { detections_last_ = _last_detections; }
+
+ // for factory
+ static ProcessorBasePtr create(const std::string &_unique_name, const ParamsProcessorBasePtr _params)
+ {
+ auto prc_object_params_ = std::static_pointer_cast<ParamsProcessorTrackerLandmarkObject>(_params);
+
+ auto prc_ptr = std::make_shared<ProcessorTrackerLandmarkObject_Wrapper>(prc_object_params_);
+ prc_ptr->setName(_unique_name);
+
+ return prc_ptr;
+ }
};
-namespace wolf{
-// Register in the Factories
-WOLF_REGISTER_PROCESSOR(ProcessorTrackerLandmarkObject_Wrapper);
+namespace wolf
+{
+ // Register in the Factories
+ WOLF_REGISTER_PROCESSOR(ProcessorTrackerLandmarkObject_Wrapper);
}
////////////////////////////////////////////////////////////////
-
-
////////////////////////////////////////////////////////////////
/*
* Test class to prepare a little wolf problem to test the class ProcessorTrackerLandmarkObject
@@ -67,42 +91,44 @@ WOLF_REGISTER_PROCESSOR(ProcessorTrackerLandmarkObject_Wrapper);
* The class ProcessorTrackerLandmarkObject is sometimes tested via the wrapper ProcessorTrackerLandmarkObject_Wrapper
*/
// Use the following in case you want to initialize tests with predefined variables or methods.
-class ProcessorTrackerLandmarkObject_fixture : public testing::Test{
- public:
- void SetUp() override
- {
- wolf_root = _WOLF_OBJECTSLAM_ROOT_DIR;
-
- // configure wolf problem
- problem = Problem::create("PO", 3);
- ParamsSensorPosePtr params_sp = std::make_shared<ParamsSensorPose>();
- sen = problem->installSensor("SensorPose", "sensor_pose", (Vector7d()<<0,0,0,0,0,0,1).finished(), params_sp);
- prc = problem->installProcessor("ProcessorTrackerLandmarkObject_Wrapper", "objects_wrapper", "sensor_pose", wolf_root + "/test/processor_tracker_landmark_object.yaml");
- prc_obj = std::static_pointer_cast<ProcessorTrackerLandmarkObject_Wrapper>(prc);
-
- // set prior
- VectorComposite x0("PO", {Vector3d(0,0,0), Quaterniond::Identity().coeffs()});
- VectorComposite s0("PO", {Vector3d(1,1,1), Vector3d(1,1,1)});
- F1 = problem->setPriorFactor(x0, s0, 0.0);
-
- // minimal config for the processor to be operative
- C1 = CaptureBase::emplace<CapturePose>(F1, 1.0, sen, Vector7d(), Matrix6d());
- prc_obj->setOriginPtr(C1);
- prc_obj->setLastPtr(C1);
- }
-
- public:
- ProcessorTrackerLandmarkObject_WrapperPtr prc_obj;
- std::string wolf_root;
- ProblemPtr problem;
- SensorBasePtr sen;
- ProcessorBasePtr prc;
- FrameBasePtr F1;
- CaptureBasePtr C1;
-};
-////////////////////////////////////////////////////////////////
+class ProcessorTrackerLandmarkObject_fixture : public testing::Test
+{
+public:
+ void SetUp() override
+ {
+ wolf_root = _WOLF_OBJECTSLAM_ROOT_DIR;
+
+ // configure wolf problem
+ problem = Problem::create("PO", 3);
+ ParamsSensorPosePtr params_sp = std::make_shared<ParamsSensorPose>();
+ sen = problem->installSensor("SensorPose", "sensor_pose", (Vector7d() << 0, 0, 0, 0, 0, 0, 1).finished(), params_sp);
+ prc = problem->installProcessor("ProcessorTrackerLandmarkObject_Wrapper", "objects_wrapper", "sensor_pose", wolf_root + "/test/processor_tracker_landmark_object.yaml");
+ prc_obj = std::static_pointer_cast<ProcessorTrackerLandmarkObject_Wrapper>(prc);
+
+ // set prior
+ VectorComposite x0("PO", {Vector3d(0, 0, 0), Quaterniond::Identity().coeffs()});
+ VectorComposite s0("PO", {Vector3d(1, 1, 1), Vector3d(1, 1, 1)});
+ F1 = problem->setPriorFactor(x0, s0, 0.0);
+
+ // minimal config for the processor to be operative
+ C1 = CaptureBase::emplace<CapturePose>(F1, 1.0, sen, Vector7d(), Matrix6d());
+ prc_obj->setOriginPtr(C1);
+ prc_obj->setLastPtr(C1);
+
+ // define camera and object poses
+ }
+public:
+ ProcessorTrackerLandmarkObject_WrapperPtr prc_obj;
+ std::string wolf_root;
+ ProblemPtr problem;
+ SensorBasePtr sen;
+ ProcessorBasePtr prc;
+ FrameBasePtr F1;
+ CaptureBasePtr C1;
+};
+////////////////////////////////////////////////////////////////
/////////////////// TESTS START HERE ///////////////////////////
// //
@@ -226,36 +252,15 @@ TEST_F(ProcessorTrackerLandmarkObject_fixture, detectNewFeatures)
// Some detected features TODO
FeatureBasePtrList features_in;
- Eigen::Vector3d pos;
- Eigen::Vector3d ori; //Euler angles in rad
- Eigen::Quaterniond quat;
- Eigen::Vector7d pose;
+ Eigen::Vector7d pose_dummy = Vector7d::Zero();
Eigen::Matrix6d meas_cov = Matrix6d::Identity();
- std::string object_type;
// feature 0
- pos << 0,2,0;
- ori << M_TORAD * 0, M_TORAD * 0, M_TORAD * 0;
- quat = e2q(ori);
- pose << pos, quat.coeffs();
- object_type = "type0";
- FeatureBasePtr f0 = std::make_shared<FeatureObject>(pose, meas_cov, object_type);
-
+ FeatureBasePtr f0 = std::make_shared<FeatureObject>(pose_dummy, meas_cov, "type0");
// feature 1
- pos << 1,2,0;
- ori << M_TORAD * 0, M_TORAD * 0, M_TORAD * 0;
- quat = e2q(ori);
- pose << pos, quat.coeffs();
- object_type = "type1";
- FeatureBasePtr f1 = std::make_shared<FeatureObject>(pose, meas_cov, object_type);
-
+ FeatureBasePtr f1 = std::make_shared<FeatureObject>(pose_dummy, meas_cov, "type1");
// feature 2
- pos << 0,2,1;
- ori << M_TORAD * 0, M_TORAD * 0, M_TORAD * 0;
- quat = e2q(ori);
- pose << pos, quat.coeffs();
- object_type = "type2";
- FeatureBasePtr f2 = std::make_shared<FeatureObject>(pose, meas_cov, object_type);
+ FeatureBasePtr f2 = std::make_shared<FeatureObject>(pose_dummy, meas_cov, "type2");
// we add 2 features in the detection list
features_in.push_back(f0);
@@ -287,7 +292,7 @@ TEST_F(ProcessorTrackerLandmarkObject_fixture, detectNewFeatures)
TEST_F(ProcessorTrackerLandmarkObject_fixture, emplaceLandmark)
{
- Vector7d pose_landmark((Vector7d()<<0,0,0,0,0,0,1).finished());
+ Vector7d pose_landmark((Vector7d() << 0, 0, 0, 0, 0, 0, 1).finished());
FeatureBasePtr f1 = FeatureBase::emplace<FeatureObject>(C1, pose_landmark, Matrix6d::Identity(), "thetype");
LandmarkBasePtr lmk = prc_obj->emplaceLandmark(f1);
@@ -299,7 +304,7 @@ TEST_F(ProcessorTrackerLandmarkObject_fixture, emplaceLandmark)
TEST_F(ProcessorTrackerLandmarkObject_fixture, emplaceFactor)
{
- FeatureBasePtr f1 = FeatureBase::emplace<FeatureObject>(C1, (Vector7d()<<0,0,0,0,0,0,1).finished(), Matrix6d::Identity(), "thetype");
+ FeatureBasePtr f1 = FeatureBase::emplace<FeatureObject>(C1, (Vector7d() << 0, 0, 0, 0, 0, 0, 1).finished(), Matrix6d::Identity(), "thetype");
LandmarkBasePtr lmk = prc_obj->emplaceLandmark(f1);
LandmarkObjectPtr lmk_object = std::static_pointer_cast<LandmarkObject>(lmk);
@@ -310,7 +315,7 @@ TEST_F(ProcessorTrackerLandmarkObject_fixture, emplaceFactor)
ASSERT_TRUE(ctr->getType() == "FactorRelativePose3dWithExtrinsics");
}
-std::ostream& operator<<(std::ostream &flux, std::vector<std::pair<int,int> > vector)
+std::ostream &operator<<(std::ostream &flux, std::vector<std::pair<int, int>> vector)
{
for (auto pair : vector)
{
@@ -320,46 +325,45 @@ std::ostream& operator<<(std::ostream &flux, std::vector<std::pair<int,int> > ve
return flux;
}
-
TEST_F(ProcessorTrackerLandmarkObject_fixture, matchingRANSAC)
{
std::vector<Eigen::Isometry3d> cl_M_o_vec;
std::vector<Eigen::Isometry3d> ci_M_o_vec;
- std::vector<std::pair<int,int> > matches;
- std::vector<std::pair<int, int> > inliers_idx;
- std::vector<std::pair<int, int> > outliers_idx;
+ std::vector<std::pair<int, int>> matches;
+ std::vector<std::pair<int, int>> inliers_idx;
+ std::vector<std::pair<int, int>> outliers_idx;
Eigen::Isometry3d best_model;
- //Camera poses
+ // Camera poses
Vector7d pose_cam_last;
Vector7d pose_cam_incoming;
- //Create 5 objects
+ // Create 5 objects
Vector7d pose_object_1;
Vector7d pose_object_2;
Vector7d pose_object_3;
Vector7d pose_object_4;
Vector7d pose_object_5;
- //Give random values
- pose_cam_last << 4.1,3.5,0, 0.8,2.7,1.3,1;
- pose_cam_incoming << 5.2,4.8,0.5,0.4, 2.0,2.1,1.2;
- pose_object_1 << 3.2,2.1,4.3,5.5, 3.3,2.4,1;
- pose_object_2 << 2.2,1.1,3.3,4.5, 2.3,1.4,0.8;
- pose_object_3 << 1.2,0.1,3.7,4.5, 2.4,1.4,0.5;
- pose_object_4 << 1.8,1.5,1.3,3.5, 3.2,1.0,0.4;
- pose_object_5 << 0.8,2.5,2.3,2.5, 1.2,2.0,1.1;
+ // Give random values
+ pose_cam_last << 4.1, 3.5, 0, 0.8, 2.7, 1.3, 1;
+ pose_cam_incoming << 5.2, 4.8, 0.5, 0.4, 2.0, 2.1, 1.2;
+ pose_object_1 << 3.2, 2.1, 4.3, 5.5, 3.3, 2.4, 1;
+ pose_object_2 << 2.2, 1.1, 3.3, 4.5, 2.3, 1.4, 0.8;
+ pose_object_3 << 1.2, 0.1, 3.7, 4.5, 2.4, 1.4, 0.5;
+ pose_object_4 << 1.8, 1.5, 1.3, 3.5, 3.2, 1.0, 0.4;
+ pose_object_5 << 0.8, 2.5, 2.3, 2.5, 1.2, 2.0, 1.1;
- //Normalizing quaternions
- pose_cam_last.tail<4>() = pose_cam_last.tail<4>().normalized();
+ // Normalizing quaternions
+ pose_cam_last.tail<4>() = pose_cam_last.tail<4>().normalized();
pose_cam_incoming.tail<4>() = pose_cam_incoming.tail<4>().normalized();
- pose_object_1.tail<4>() = pose_object_1.tail<4>().normalized();
- pose_object_2.tail<4>() = pose_object_2.tail<4>().normalized();
- pose_object_3.tail<4>() = pose_object_2.tail<4>().normalized();
- pose_object_4.tail<4>() = pose_object_4.tail<4>().normalized();
- pose_object_5.tail<4>() = pose_object_5.tail<4>().normalized();
+ pose_object_1.tail<4>() = pose_object_1.tail<4>().normalized();
+ pose_object_2.tail<4>() = pose_object_2.tail<4>().normalized();
+ pose_object_3.tail<4>() = pose_object_2.tail<4>().normalized();
+ pose_object_4.tail<4>() = pose_object_4.tail<4>().normalized();
+ pose_object_5.tail<4>() = pose_object_5.tail<4>().normalized();
- //Transform pose into isometry
+ // Transform pose into isometry
Eigen::Isometry3d w_M_cl = posevec_to_isometry(pose_cam_last);
Eigen::Isometry3d w_M_ci = posevec_to_isometry(pose_cam_incoming);
@@ -369,7 +373,6 @@ TEST_F(ProcessorTrackerLandmarkObject_fixture, matchingRANSAC)
Eigen::Isometry3d w_M_o4 = posevec_to_isometry(pose_object_4);
Eigen::Isometry3d w_M_o5 = posevec_to_isometry(pose_object_5);
-
Eigen::Isometry3d cl_M_o1 = w_M_cl.inverse() * w_M_o1;
Eigen::Isometry3d cl_M_o2 = w_M_cl.inverse() * w_M_o2;
Eigen::Isometry3d cl_M_o3 = w_M_o3;
@@ -384,9 +387,9 @@ TEST_F(ProcessorTrackerLandmarkObject_fixture, matchingRANSAC)
Eigen::Isometry3d ci_M_o1 = w_M_ci.inverse() * w_M_o1;
Eigen::Isometry3d ci_M_o2 = w_M_ci.inverse() * w_M_o2;
- Eigen::Isometry3d ci_M_o3 = w_M_o5; //outliers
+ Eigen::Isometry3d ci_M_o3 = w_M_o5; // outliers
Eigen::Isometry3d ci_M_o4 = w_M_ci.inverse() * w_M_o4;
- Eigen::Isometry3d ci_M_o5 = w_M_o2; //outliers
+ Eigen::Isometry3d ci_M_o5 = w_M_o2; // outliers
ci_M_o_vec.push_back(ci_M_o1);
ci_M_o_vec.push_back(ci_M_o2);
@@ -396,21 +399,21 @@ TEST_F(ProcessorTrackerLandmarkObject_fixture, matchingRANSAC)
Eigen::Isometry3d cl_M_ci = w_M_cl.inverse() * w_M_ci;
- //Create 5 matches
- auto pair_o1 = std::make_pair(0, 0);
- auto pair_o2 = std::make_pair(1, 1);
- auto pair_o3 = std::make_pair(2, 2);
- auto pair_o4 = std::make_pair(3, 3);
- auto pair_o5 = std::make_pair(4, 4);
+ // Create 5 matches
+ auto pair_o1 = std::make_pair(0, 0);
+ auto pair_o2 = std::make_pair(1, 1);
+ auto pair_o3 = std::make_pair(2, 2);
+ auto pair_o4 = std::make_pair(3, 3);
+ auto pair_o5 = std::make_pair(4, 4);
- //Append pairs in matches object
+ // Append pairs in matches object
matches.push_back(pair_o1);
matches.push_back(pair_o2);
matches.push_back(pair_o3);
matches.push_back(pair_o4);
matches.push_back(pair_o5);
- //Detect all outliers of our batch
+ // Detect all outliers of our batch
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());
@@ -433,70 +436,66 @@ TEST_F(ProcessorTrackerLandmarkObject_fixture, matchingRANSAC)
TEST(ProcessorTrackerLandmarkObject, isInliers)
{
- //Camera poses
+ // Camera poses
Vector7d pose_cam_last;
Vector7d pose_cam_incoming;
- //Create 2 objects
+ // Create 2 objects
Vector7d pose_object_1;
Vector7d pose_object_2;
- //Give random values
- pose_cam_last << 4.1,3.5,0,0.8, 2.7,1.3,1;
- pose_cam_incoming << 5.2,4.8,0.5,0.4, 2.0,2.1,1.2;
- pose_object_1 << 3.2,2.1,4.3,5.5, 3.3,2.4,1;
- pose_object_2 << 2.2,1.1,3.3,4.5, 2.3,1.4,0.8;
+ // Give random values
+ pose_cam_last << 4.1, 3.5, 0, 0.8, 2.7, 1.3, 1;
+ pose_cam_incoming << 5.2, 4.8, 0.5, 0.4, 2.0, 2.1, 1.2;
+ pose_object_1 << 3.2, 2.1, 4.3, 5.5, 3.3, 2.4, 1;
+ pose_object_2 << 2.2, 1.1, 3.3, 4.5, 2.3, 1.4, 0.8;
- //Normalizing quaternions
- pose_cam_last.tail<4>() = pose_cam_last.tail<4>().normalized();
+ // Normalizing quaternions
+ pose_cam_last.tail<4>() = pose_cam_last.tail<4>().normalized();
pose_cam_incoming.tail<4>() = pose_cam_incoming.tail<4>().normalized();
- pose_object_1.tail<4>() = pose_object_1.tail<4>().normalized();
- pose_object_2.tail<4>() = pose_object_2.tail<4>().normalized();
+ pose_object_1.tail<4>() = pose_object_1.tail<4>().normalized();
+ pose_object_2.tail<4>() = pose_object_2.tail<4>().normalized();
- //Transform pose into isometry
+ // Transform pose into isometry
Eigen::Isometry3d w_M_cl = posevec_to_isometry(pose_cam_last);
Eigen::Isometry3d w_M_ci = posevec_to_isometry(pose_cam_incoming);
- Eigen::Isometry3d w_M_o1 = posevec_to_isometry(pose_object_1);
- Eigen::Isometry3d w_M_o2 = posevec_to_isometry(pose_object_2);
- Eigen::Isometry3d w_M_o3 = Eigen::Isometry3d::Identity();
+ Eigen::Isometry3d cl_M_o1 = posevec_to_isometry(pose_object_1);
+ Eigen::Isometry3d cl_M_o2 = posevec_to_isometry(pose_object_2);
+ Eigen::Isometry3d cl_M_o3 = Eigen::Isometry3d::Identity();
Eigen::Isometry3d cl_M_ci = w_M_cl.inverse() * w_M_ci;
- Eigen::Isometry3d cl_M_o1 = w_M_cl.inverse() * w_M_o1;
- Eigen::Isometry3d cl_M_o2 = w_M_cl.inverse() * w_M_o2;
- Eigen::Isometry3d cl_M_o3 = w_M_cl.inverse() * w_M_o3;
- Eigen::Isometry3d ci_M_o1 = w_M_ci.inverse() * w_M_o1;
- Eigen::Isometry3d ci_M_o2 = w_M_ci.inverse() * w_M_o2;
- Eigen::Isometry3d ci_M_o3 = w_M_ci.inverse() * w_M_o3;
-
+ Eigen::Isometry3d ci_M_o1 = cl_M_ci.inverse() * cl_M_o1;
+ Eigen::Isometry3d ci_M_o2 = cl_M_ci.inverse() * cl_M_o2;
+ Eigen::Isometry3d ci_M_o3 = cl_M_ci.inverse() * cl_M_o3;
ASSERT_TRUE(ProcessorTrackerLandmarkObject::isInliers(cl_M_o1, ci_M_o1, cl_M_ci));
ASSERT_TRUE(ProcessorTrackerLandmarkObject::isInliers(cl_M_o2, ci_M_o2, cl_M_ci));
ASSERT_TRUE(ProcessorTrackerLandmarkObject::isInliers(cl_M_o3, ci_M_o3, cl_M_ci));
- ASSERT_TRUE(!(ProcessorTrackerLandmarkObject::isInliers(cl_M_o1, ci_M_o2, cl_M_ci))); //outliers
- ASSERT_TRUE(!(ProcessorTrackerLandmarkObject::isInliers(cl_M_o2, ci_M_o1, cl_M_ci))); //outliers
+ ASSERT_TRUE(!(ProcessorTrackerLandmarkObject::isInliers(cl_M_o1, ci_M_o2, cl_M_ci))); // outliers
+ ASSERT_TRUE(!(ProcessorTrackerLandmarkObject::isInliers(cl_M_o2, ci_M_o1, cl_M_ci))); // outliers
}
TEST(ProcessorTrackerLandmarkObject, nbOfDifferentMatches)
{
- std::vector<std::pair<int,int> > matches;
-
- //Create 12 matches
- auto pair_o1 = std::make_pair(0, 0);
- auto pair_o2 = std::make_pair(1, 1);
- auto pair_o3 = std::make_pair(2, 2);
- 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); //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
+ std::vector<std::pair<int, int>> matches;
+
+ // Create 12 matches
+ auto pair_o1 = std::make_pair(0, 0);
+ auto pair_o2 = std::make_pair(1, 1);
+ auto pair_o3 = std::make_pair(2, 2);
+ 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); // 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
matches.push_back(pair_o1);
matches.push_back(pair_o2);
matches.push_back(pair_o3);
@@ -515,45 +514,45 @@ TEST(ProcessorTrackerLandmarkObject, nbOfDifferentMatches)
TEST(ProcessorTrackerLandmarkObject, filterMatchesOutliers)
{
- std::vector<std::pair<int,int> > matches1;
- std::vector<std::pair<int,int> > matches2;
- std::vector<std::pair<int,int> > matches3;
- std::vector<std::pair<int,int> > matches1I;
- std::vector<std::pair<int,int> > matches2I;
- std::vector<std::pair<int,int> > matches3I;
-
- std::vector<std::pair<int,int> > outliers_idx1;
- std::vector<std::pair<int,int> > outliers_idx2;
- std::vector<std::pair<int,int> > outliers_idx3;
-
- outliers_idx1.push_back(std::make_pair(2,2));
- outliers_idx1.push_back(std::make_pair(5,5));
- outliers_idx1.push_back(std::make_pair(9,9));
- outliers_idx2.push_back(std::make_pair(0,0));
- outliers_idx2.push_back(std::make_pair(9,9));
- outliers_idx2.push_back(std::make_pair(10,10));
- outliers_idx2.push_back(std::make_pair(11,11));
- outliers_idx3.push_back(std::make_pair(4,4));
- outliers_idx3.push_back(std::make_pair(5,5));
- outliers_idx3.push_back(std::make_pair(6,6));
- outliers_idx3.push_back(std::make_pair(7,7));
- outliers_idx3.push_back(std::make_pair(8,8));
-
- //Create 12 matches
- auto pair_o1 = std::make_pair(0, 0);
- auto pair_o2 = std::make_pair(1, 1);
- auto pair_o3 = std::make_pair(2, 2);
- auto pair_o4 = std::make_pair(3, 3);
- auto pair_o5 = std::make_pair(4, 4);
- auto pair_o6 = std::make_pair(5, 5);
- 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_o11 = std::make_pair(10, 10);
- auto pair_o12 = std::make_pair(11, 11);
-
- //Append pairs in matches object
+ std::vector<std::pair<int, int>> matches1;
+ std::vector<std::pair<int, int>> matches2;
+ std::vector<std::pair<int, int>> matches3;
+ std::vector<std::pair<int, int>> matches1I;
+ std::vector<std::pair<int, int>> matches2I;
+ std::vector<std::pair<int, int>> matches3I;
+
+ std::vector<std::pair<int, int>> outliers_idx1;
+ std::vector<std::pair<int, int>> outliers_idx2;
+ std::vector<std::pair<int, int>> outliers_idx3;
+
+ outliers_idx1.push_back(std::make_pair(2, 2));
+ outliers_idx1.push_back(std::make_pair(5, 5));
+ outliers_idx1.push_back(std::make_pair(9, 9));
+ outliers_idx2.push_back(std::make_pair(0, 0));
+ outliers_idx2.push_back(std::make_pair(9, 9));
+ outliers_idx2.push_back(std::make_pair(10, 10));
+ outliers_idx2.push_back(std::make_pair(11, 11));
+ outliers_idx3.push_back(std::make_pair(4, 4));
+ outliers_idx3.push_back(std::make_pair(5, 5));
+ outliers_idx3.push_back(std::make_pair(6, 6));
+ outliers_idx3.push_back(std::make_pair(7, 7));
+ outliers_idx3.push_back(std::make_pair(8, 8));
+
+ // Create 12 matches
+ auto pair_o1 = std::make_pair(0, 0);
+ auto pair_o2 = std::make_pair(1, 1);
+ auto pair_o3 = std::make_pair(2, 2);
+ auto pair_o4 = std::make_pair(3, 3);
+ auto pair_o5 = std::make_pair(4, 4);
+ auto pair_o6 = std::make_pair(5, 5);
+ 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_o11 = std::make_pair(10, 10);
+ auto pair_o12 = std::make_pair(11, 11);
+
+ // Append pairs in matches object
matches1.push_back(pair_o1);
matches1.push_back(pair_o2);
matches1.push_back(pair_o3);
@@ -588,7 +587,6 @@ TEST(ProcessorTrackerLandmarkObject, filterMatchesOutliers)
ASSERT_TRUE(matches1I.size() == 3);
ASSERT_TRUE(matches2I.size() == 4);
ASSERT_TRUE(matches3I.size() == 5);
-
}
// std::ostream& operator<<(std::ostream &flux, std::vector<std::pair<int,int> > vector)
@@ -605,159 +603,106 @@ TEST(ProcessorTrackerLandmarkObject, allMatchesSameType)
{
FeatureBasePtrList features_last;
FeatureBasePtrList features_incoming;
+ Eigen::Isometry3d best_model = Eigen::Isometry3d::Identity();
+ std::vector<Eigen::Isometry3d> cl_M_o_vec;
+ std::vector<Eigen::Isometry3d> ci_M_o_vec;
Eigen::Vector7d pose;
Eigen::Vector3d pos;
- Eigen::Matrix6d meas_cov = Matrix6d::Identity();
- std::string object_type;
+
// feature 0
- pose << 0.8,2.5,2.3,2.5, 1.2,2.0,1.1;
- pose.tail<4>() = pose.tail<4>().normalized();
- Eigen::Isometry3d w_M_f0 = posevec_to_isometry(pose);
- object_type = "type0";
- FeatureBasePtr f0 = std::make_shared<FeatureObject>(pose, meas_cov, object_type);
+ pose << 0.8, 2.5, 2.3, 2.5, 1.2, 2.0, 1.1;
+ Eigen::Isometry3d cl_M_f0 = appendFeatureLast("type0", pose, features_last);
// feature 1
- pose << 0.5,3.5,3.2,1.5, 0.2,2.1,1.0;
- pose.tail<4>() = pose.tail<4>().normalized();
- Eigen::Isometry3d w_M_f1 = posevec_to_isometry(pose);
- object_type = "type0";
- FeatureBasePtr f1 = std::make_shared<FeatureObject>(pose, meas_cov, object_type);
+ pose << 0.5, 3.5, 3.2, 1.5, 0.2, 2.1, 1.0;
+ Eigen::Isometry3d cl_M_f1 = appendFeatureLast("type0", pose, features_last);
// feature 2
- pose << 1.5,3.2,1.2,2.0, 2.2,1.0,1.0;
- pose.tail<4>() = pose.tail<4>().normalized();
- Eigen::Isometry3d w_M_f2 = posevec_to_isometry(pose);
- object_type = "type2";
- FeatureBasePtr f2 = std::make_shared<FeatureObject>(pose, meas_cov, object_type);
+ pose << 1.5, 3.2, 1.2, 2.0, 2.2, 1.0, 1.0;
+ Eigen::Isometry3d cl_M_f2 = appendFeatureLast("type2", pose, features_last);
// feature 3
- pose << 2.1,1.1,2.1,1.0, 1.2,1.3,1.1;
- pose.tail<4>() = pose.tail<4>().normalized();
- Eigen::Isometry3d w_M_f3 = posevec_to_isometry(pose);
- object_type = "type1";
- FeatureBasePtr f3 = std::make_shared<FeatureObject>(pose, meas_cov, object_type);
+ pose << 2.1, 1.1, 2.1, 1.0, 1.2, 1.3, 1.1;
+ Eigen::Isometry3d cl_M_f3 = appendFeatureLast("type1", pose, features_last);
// feature 4
- pose << 1.9,4.2,8.2,3.0, 4.2,8.0,1.6;
- pose.tail<4>() = pose.tail<4>().normalized();
- Eigen::Isometry3d w_M_f4 = posevec_to_isometry(pose);
- object_type = "type1";
- FeatureBasePtr f4 = std::make_shared<FeatureObject>(pose, meas_cov, object_type);
-
+ pose << 1.9, 4.2, 8.2, 3.0, 4.2, 8.0, 1.6;
+ Eigen::Isometry3d cl_M_f4 = appendFeatureLast("type1", pose, features_last);
Eigen::Vector7d pose_cam_last;
Eigen::Vector7d pose_cam_incoming;
- pose_cam_last << 4.1,3.5,0,0.8, 2.7,1.3,1;
- pose_cam_incoming << 5.2,4.8,0.5,0.4, 2.0,2.1,1.2;
- pose_cam_last.tail<4>() = pose_cam_last.tail<4>().normalized();
+ pose_cam_last << 4.1, 3.5, 0, 0.8, 2.7, 1.3, 1;
+ pose_cam_incoming << 5.2, 4.8, 0.5, 0.4, 2.0, 2.1, 1.2;
+ pose_cam_last.tail<4>() = pose_cam_last.tail<4>().normalized();
pose_cam_incoming.tail<4>() = pose_cam_incoming.tail<4>().normalized();
-
- Eigen::Isometry3d w_M_ci = posevec_to_isometry(pose_cam_incoming);
-
+ Eigen::Isometry3d w_M_cl = posevec_to_isometry(pose_cam_last);
+ Eigen::Isometry3d w_M_ci = posevec_to_isometry(pose_cam_incoming);
+ Eigen::Isometry3d ci_M_cl = w_M_ci.inverse() * w_M_cl;
// feature 0
- Eigen::Isometry3d ci_M_f0 = w_M_ci.inverse() * w_M_f0;
- Eigen::Isometry3d new_w_M_f0 = w_M_ci * ci_M_f0;
- Eigen::Quaterniond quat0(new_w_M_f0.linear());
- pos = new_w_M_f0.translation();
- pose << pos, quat0.coeffs();
- pose.tail<4>() = pose.tail<4>().normalized();
- object_type = "type0";
- FeatureBasePtr new_f0 = std::make_shared<FeatureObject>(pose, meas_cov, object_type);
+ appendFeaturencoming("type0", cl_M_f0, ci_M_cl, features_incoming); // go with f0
// feature 1
- Eigen::Isometry3d ci_M_f1 = w_M_ci.inverse() * w_M_f1;
- Eigen::Isometry3d new_w_M_f1 = w_M_ci * ci_M_f1;
- Eigen::Quaterniond quat1(new_w_M_f1.linear());
- pos = new_w_M_f1.translation();
- pose << pos, quat1.coeffs();
- pose.tail<4>() = pose.tail<4>().normalized();
- object_type = "type0";
- FeatureBasePtr new_f1 = std::make_shared<FeatureObject>(pose, meas_cov, object_type);
+ appendFeaturencoming("type0", cl_M_f1, ci_M_cl, features_incoming); // go with f1
// feature 2
- Eigen::Isometry3d ci_M_f2 = w_M_ci.inverse() * w_M_f2;
- Eigen::Isometry3d new_w_M_f2 = w_M_ci * ci_M_f2;
- Eigen::Quaterniond quat2(new_w_M_f2.linear());
- pos = new_w_M_f2.translation();
- pose << pos, quat2.coeffs();
- pose.tail<4>() = pose.tail<4>().normalized();
- object_type = "type2";
- FeatureBasePtr new_f2 = std::make_shared<FeatureObject>(pose, meas_cov, object_type);
+ appendFeaturencoming("type2", cl_M_f2, ci_M_cl, features_incoming); // go with f2
// feature 3
- Eigen::Isometry3d new_w_M_f3 = Eigen::Isometry3d::Identity(); //outliers
- Eigen::Quaterniond quat3(new_w_M_f3.linear());
- pos = new_w_M_f3.translation();
- pose << pos, quat3.coeffs();
- pose.tail<4>() = pose.tail<4>().normalized();
- object_type = "type0";
- FeatureBasePtr new_f3 = std::make_shared<FeatureObject>(pose, meas_cov, object_type);
+ appendFeaturencoming("type0", cl_M_f0, Eigen::Isometry3d::Identity(), features_incoming); //outliers
// feature 4
- Eigen::Isometry3d new_w_M_f4 = Eigen::Isometry3d::Identity(); //outliers
- Eigen::Quaterniond quat4(new_w_M_f4.linear());
- pos = new_w_M_f4.translation();
- pose << pos, quat4.coeffs();
- pose.tail<4>() = pose.tail<4>().normalized();
- object_type = "type1";
- FeatureBasePtr new_f4 = std::make_shared<FeatureObject>(pose, meas_cov, object_type);
+ appendFeaturencoming("type1", cl_M_f0, Eigen::Isometry3d::Identity(), features_incoming); //outliers
// feature 5
- Eigen::Isometry3d ci_M_f5 = w_M_ci.inverse() * w_M_f3;
- Eigen::Isometry3d new_w_M_f5 = w_M_ci * ci_M_f5;
- Eigen::Quaterniond quat5(new_w_M_f5.linear());
- pos = new_w_M_f5.translation();
- pose << pos, quat5.coeffs();
- pose.tail<4>() = pose.tail<4>().normalized();
- object_type = "type1";
- FeatureBasePtr new_f5 = std::make_shared<FeatureObject>(pose, meas_cov, object_type);
+ appendFeaturencoming("type1", cl_M_f3, ci_M_cl, features_incoming); // go with f3
// feature 6
- Eigen::Isometry3d ci_M_f6 = w_M_ci.inverse() * w_M_f4;
- Eigen::Isometry3d new_w_M_f6 = w_M_ci * ci_M_f6;
- Eigen::Quaterniond quat6(new_w_M_f6.linear());
- pos = new_w_M_f6.translation();
- pose << pos, quat6.coeffs();
- pose.tail<4>() = pose.tail<4>().normalized();
- object_type = "type1";
- FeatureBasePtr new_f6 = std::make_shared<FeatureObject>(pose, meas_cov, object_type);
-
- features_last.push_back(f0);
- features_last.push_back(f1);
- features_last.push_back(f2);
- features_last.push_back(f3);
- features_last.push_back(f4);
-
- features_incoming.push_back(new_f0); //go with f0
- features_incoming.push_back(new_f1); //go with f1
- features_incoming.push_back(new_f2); //go with f2
- features_incoming.push_back(new_f3); //outliers
- features_incoming.push_back(new_f4); //outliers
- features_incoming.push_back(new_f5); //go with f3
- features_incoming.push_back(new_f6); //go with f4
-
-
- auto matches = ProcessorTrackerLandmarkObject::allMatchesSameType(features_last, features_incoming);
-
- //Matches has only inliers
- ASSERT_TRUE(matches.size() == 5);
- ASSERT_TRUE(matches[0] == std::make_pair(0,0));
- ASSERT_TRUE(matches[1] == std::make_pair(1,1));
- ASSERT_TRUE(matches[2] == std::make_pair(2,2));
- ASSERT_TRUE(matches[3] == std::make_pair(3,5));
- ASSERT_TRUE(matches[4] == std::make_pair(4,6));
+ appendFeaturencoming("type1", cl_M_f4, ci_M_cl, features_incoming); // go with f4
-}
+ auto all_matches = ProcessorTrackerLandmarkObject::createAllMatchesSameType(features_last, features_incoming, cl_M_o_vec, ci_M_o_vec);
+
+ // 2 * 3 (type0) + 2 * 3 (type1) + 1 * 1 (type2) = 13
+ ASSERT_TRUE(all_matches.size() == 13);
+
+ // Type 0
+ ASSERT_TRUE(std::count(all_matches.begin(), all_matches.end(), std::make_pair(0, 0)));
+ ASSERT_TRUE(std::count(all_matches.begin(), all_matches.end(), std::make_pair(0, 1))); // outliers
+ ASSERT_TRUE(std::count(all_matches.begin(), all_matches.end(), std::make_pair(0, 3))); // outliers
+ ASSERT_TRUE(std::count(all_matches.begin(), all_matches.end(), std::make_pair(1, 0))); // outliers
+ ASSERT_TRUE(std::count(all_matches.begin(), all_matches.end(), std::make_pair(1, 1)));
+ ASSERT_TRUE(std::count(all_matches.begin(), all_matches.end(), std::make_pair(1, 3))); // outliers
+
+ // Type 1
+ ASSERT_TRUE(std::count(all_matches.begin(), all_matches.end(), std::make_pair(3, 4))); // outliers
+ ASSERT_TRUE(std::count(all_matches.begin(), all_matches.end(), std::make_pair(3, 5)));
+ ASSERT_TRUE(std::count(all_matches.begin(), all_matches.end(), std::make_pair(3, 6))); // outliers
+ ASSERT_TRUE(std::count(all_matches.begin(), all_matches.end(), std::make_pair(4, 4))); // outliers
+ ASSERT_TRUE(std::count(all_matches.begin(), all_matches.end(), std::make_pair(4, 5))); // outliers
+ ASSERT_TRUE(std::count(all_matches.begin(), all_matches.end(), std::make_pair(4, 6)));
+
+ // Type 2
+ ASSERT_TRUE(std::count(all_matches.begin(), all_matches.end(), std::make_pair(2, 2)));
+
+ ProcessorTrackerLandmarkObject::keepInliers(cl_M_o_vec, ci_M_o_vec, all_matches, best_model);
+
+ // All_matches has now only inliers
+ ASSERT_TRUE(all_matches.size() == 5);
+
+ ASSERT_TRUE(std::count(all_matches.begin(), all_matches.end(), std::make_pair(0, 0)));
+ ASSERT_TRUE(std::count(all_matches.begin(), all_matches.end(), std::make_pair(1, 1)));
+ ASSERT_TRUE(std::count(all_matches.begin(), all_matches.end(), std::make_pair(2, 2)));
+ ASSERT_TRUE(std::count(all_matches.begin(), all_matches.end(), std::make_pair(3, 5)));
+ ASSERT_TRUE(std::count(all_matches.begin(), all_matches.end(), std::make_pair(4, 6)));
+}
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
-
--
GitLab