diff --git a/include/objectslam/processor/processor_tracker_landmark_object.h b/include/objectslam/processor/processor_tracker_landmark_object.h
index 700f66a385e5b47d8a2ef09e76cebbf725ebea7d..7dadcb54d808cd2e9d286ecc9e64bf5c9a76702c 100644
--- a/include/objectslam/processor/processor_tracker_landmark_object.h
+++ b/include/objectslam/processor/processor_tracker_landmark_object.h
@@ -157,8 +157,8 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark
FeatureBasePtrList& _features_out_incoming);
- void allMatchesSameType(const FeatureBasePtrList& _features_out_last,
- const FeatureBasePtrList& _features_out_incoming);
+ static std::vector<std::pair<int,int> > allMatchesSameType(const FeatureBasePtrList& _features_out_last,
+ const FeatureBasePtrList& _features_out_incoming);
/** \brief Method for matching feature from last to incoming
@@ -181,8 +181,8 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark
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,
+ std::vector<std::pair<int, int> >& _inliers_idx,
+ std::vector<std::pair<int, int> >& _outliers_idx,
Eigen::Isometry3d& _best_model);
/** \brief Count the number of different matches
@@ -215,9 +215,9 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark
* \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,
+ static void processFeatures(const std::vector<std::pair<int,int> >& _matches,
+ FeatureBasePtrList detections_last,
+ FeatureBasePtrList detections_incoming,
TrackMatrix& _trackMatrix);
/** \brief Filter matches thanks to outliers vector
@@ -227,7 +227,7 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark
* A function which remove from matches the outliers
*
*/
- static void filterMatchesOutliers(const std::vector<int>& _outliers_idx,
+ static void filterMatchesOutliers(const std::vector<std::pair<int, int> >& _outliers_idx,
std::vector<std::pair<int,int> >& _matches);
/** \brief Filter matches thanks to inliers vector
@@ -237,10 +237,10 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark
* A function which create a vector based on _inliers_idx
*
*/
- static void filterMatchesInliers(const std::vector<int>& _inliers_idx,
+ static void filterMatchesInliers(const std::vector<std::pair<int, int> >& _inliers_idx,
std::vector<std::pair<int,int> >& _matches);
- const TrackMatrix& getTrackMatrix() const {return track_matrix_;}
+ TrackMatrix& getTrackMatrix() {return track_matrix_;}
/** \brief Vote for KeyFrame generation
*
@@ -318,4 +318,6 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark
} // namespace wolf
+
+
#endif /* _PROCESSOR_TRACKER_LANDMARK_OBJECT_H_ */
diff --git a/src/processor/processor_tracker_landmark_object.cpp b/src/processor/processor_tracker_landmark_object.cpp
index ef70a59f31993f7fab44f586a9df0394cae7e56f..92f1b743ca6d9c98b7aade60055e86287cb4fd42 100644
--- a/src/processor/processor_tracker_landmark_object.cpp
+++ b/src/processor/processor_tracker_landmark_object.cpp
@@ -479,8 +479,8 @@ unsigned int ProcessorTrackerLandmarkObject::multiviewTypeMatching(const Capture
index_last++;
}
- std::vector<int> inliers_idx;
- std::vector<int> outliers_idx;
+ 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, ratio_inliers_outliers_, inliers_idx, outliers_idx, best_model);
@@ -496,16 +496,37 @@ unsigned int ProcessorTrackerLandmarkObject::multiviewTypeMatching(const Capture
return 1;
}
-void ProcessorTrackerLandmarkObject::allMatchesSameType(const FeatureBasePtrList& _features_out_last,
- const FeatureBasePtrList& _features_out_incoming)
+// std::ostream& operator<<(std::ostream &flux, std::vector<std::pair<int,int> > vector)
+// {
+// for (auto pair : vector)
+// {
+// flux << pair.first << " " << pair.second << "\n";
+// }
+
+// return flux;
+// }
+
+std::ostream& operator<<(std::ostream &flux, std::vector<int> vector)
+{
+ for (auto element : vector)
+ {
+ flux << element << "\n";
+ }
+
+ return flux;
+}
+
+std::vector<std::pair<int,int> > 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;
+ bool pushBack = true;
- for (auto feat_last : detections_last_)
+ for (auto feat_last : _features_out_last)
{
std::string object_type_last = std::static_pointer_cast<FeatureObject>(feat_last)->getObjectType();
@@ -518,52 +539,66 @@ void ProcessorTrackerLandmarkObject::allMatchesSameType(const FeatureBasePtrList
int index_incoming = 0;
- for (auto feat_incoming : detections_incoming_)
+ for (auto feat_incoming : _features_out_incoming)
{
std::string object_type_incoming = std::static_pointer_cast<FeatureObject>(feat_incoming)->getObjectType();
if (object_type_last == object_type_incoming)
{
- //camera to feat
+ std::pair<int, int> pair_indexes = std::make_pair(index_last, index_incoming);
+ matches.push_back(pair_indexes);
+ }
+
+ //camera to feat
+ if (pushBack)
+ {
+
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);
+ ci_M_o_vec.push_back(ci_M_o);
}
+
index_incoming++;
}
+
+ index_last++;
+ pushBack = false;
+
}
- std::vector<int> inliers_idx;
- std::vector<int> outliers_idx;
+ // 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, ratio_inliers_outliers_, inliers_idx, outliers_idx, best_model);
+ 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::filterMatchesInliers(inliers_idx, matches);
+ ProcessorTrackerLandmarkObject::filterMatchesOutliers(outliers_idx, matches);
}
+
+ return matches;
}
-void ProcessorTrackerLandmarkObject::processFeatures(const std::vector<std::pair<int,int> >& _matches_filtered,
- const FeatureBasePtrList& _features_out_last,
- const FeatureBasePtrList& _features_out_incoming,
+void ProcessorTrackerLandmarkObject::processFeatures(const std::vector<std::pair<int,int> >& _matches,
+ FeatureBasePtrList detections_last,
+ FeatureBasePtrList detections_incoming,
TrackMatrix& _trackMatrix)
{
- for (auto match : _matches_filtered)
+ for (auto match : _matches)
{
- auto feat_last_itr = _features_out_last.begin();
- auto feat_incoming_itr = _features_out_incoming.begin();
+ auto feat_last_itr = detections_last.begin();
+ auto feat_incoming_itr = detections_incoming.begin();
std::advance(feat_last_itr, match.first);
std::advance(feat_incoming_itr, match.second);
@@ -590,6 +625,11 @@ void ProcessorTrackerLandmarkObject::processFeatures(const std::vector<std::pair
//New feature or track lost
else
{
+
+ _trackMatrix.newTrack(feat_last);
+ _trackMatrix.add(feat_last, feat_incoming);
+
+
//Check if the feature can be linked to a previous defined lmk
//else we create a new track : _trackMatrix.newTrack(match.first) && _trackMatrix.add(match.first, match.second);
@@ -598,7 +638,7 @@ void ProcessorTrackerLandmarkObject::processFeatures(const std::vector<std::pair
}
}
-void ProcessorTrackerLandmarkObject::filterMatchesOutliers(const std::vector<int>& _outliers_idx,
+void ProcessorTrackerLandmarkObject::filterMatchesOutliers(const std::vector<std::pair<int, int> >& _outliers_idx,
std::vector<std::pair<int,int> >& _matches)
{
//Number of matches
@@ -606,7 +646,7 @@ void ProcessorTrackerLandmarkObject::filterMatchesOutliers(const std::vector<int
for (int i = 0; i < nbMatches; i++)
{
- int index_outlier_occurrence = std::count(_outliers_idx.begin(), _outliers_idx.end(), _matches[i].first);
+ int index_outlier_occurrence = std::count(_outliers_idx.begin(), _outliers_idx.end(), _matches[i]);
//Check if the outlier is in matches
if (index_outlier_occurrence != 0)
@@ -622,7 +662,7 @@ void ProcessorTrackerLandmarkObject::filterMatchesOutliers(const std::vector<int
}
-void ProcessorTrackerLandmarkObject::filterMatchesInliers(const std::vector<int>& _inliers_idx,
+void ProcessorTrackerLandmarkObject::filterMatchesInliers(const std::vector<std::pair<int, int> >& _inliers_idx,
std::vector<std::pair<int,int> >& _matches)
{
//Matches that will store only inliers
@@ -630,7 +670,7 @@ void ProcessorTrackerLandmarkObject::filterMatchesInliers(const std::vector<int>
for (auto pair_indexes : _matches)
{
- int index_inlier_occurrence = std::count(_inliers_idx.begin(), _inliers_idx.end(), pair_indexes.first);
+ int index_inlier_occurrence = std::count(_inliers_idx.begin(), _inliers_idx.end(), pair_indexes);
//Check if the inlier is in matches
if (index_inlier_occurrence != 0)
@@ -649,19 +689,19 @@ bool ProcessorTrackerLandmarkObject::matchingRANSAC(const std::vector<Eigen::Iso
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,
+ std::vector<std::pair<int, int> >& _inliers_idx,
+ std::vector<std::pair<int, int> >& _outliers_idx,
Eigen::Isometry3d& _best_model)
{
//Check if the dataset has a sufficient size
- if (nbOfDifferentMatches(_matches) < 3)
- return false;
+ // 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;
+ std::vector<std::pair<int, int> > inliers_idx_buff;
+ std::vector<std::pair<int, int> > outliers_idx_buff;
//Number of inliers for the best model
int best_nb_inliers = 0;
@@ -688,26 +728,31 @@ bool ProcessorTrackerLandmarkObject::matchingRANSAC(const std::vector<Eigen::Iso
int index_feat_incomming_other = other_match.second;
//Not necessary to test same pair of matches
- if (index_feat_last != index_feat_last_other && index_feat_incoming != index_feat_incomming_other)
- {
//Transformation between camera and object in last/incoming frame for the other match
- Eigen::Isometry3d cl_M_ol = _cl_M_o_vec[index_feat_last_other];
- Eigen::Isometry3d ci_M_oi = _ci_M_o_vec[index_feat_incomming_other];
-
+ Eigen::Isometry3d cl_M_ol = _cl_M_o_vec[index_feat_last_other];
+ Eigen::Isometry3d ci_M_oi = _ci_M_o_vec[index_feat_incomming_other];
+
+ if (index_feat_last != index_feat_last_other || index_feat_incoming != index_feat_incomming_other)
+ {
if(isInliers(cl_M_ol, ci_M_oi, cl_M_ci))
{
nb_inliers++;
- inliers_idx_buff.push_back(index_feat_last_other);
+ auto p = std::make_pair(index_feat_last_other, index_feat_incomming_other);
+ inliers_idx_buff.push_back(p);
}
else
{
- outliers_idx_buff.push_back(index_feat_last_other);
+ auto p = std::make_pair(index_feat_last_other, index_feat_incomming_other);
+ outliers_idx_buff.push_back(p);
}
}
+
+ // std::cout << index_feat_last << " " << index_feat_incoming << "--->" << index_feat_last_other << " " << index_feat_incomming_other << " ---> " << outliers_idx_buff << "\n";
}
//Feature which provide the model is an inlier
- inliers_idx_buff.push_back(index_feat_last);
+ auto p = std::make_pair(index_feat_last, index_feat_incoming);
+ inliers_idx_buff.push_back(p);
//If the model gives better results
if (nb_inliers > best_nb_inliers)
@@ -729,7 +774,7 @@ bool ProcessorTrackerLandmarkObject::matchingRANSAC(const std::vector<Eigen::Iso
if ((double)nb_outliers/nb_inliers > _ratio_outliers_inliers)
return true;
- return false;
+ return true;
}
int ProcessorTrackerLandmarkObject::nbOfDifferentMatches(const std::vector<std::pair<int,int> >& _matches)
diff --git a/test/gtest_processor_tracker_landmark_object.cpp b/test/gtest_processor_tracker_landmark_object.cpp
index 743ac272c0efeae297bba4fdda8b2b95730780bf..2b2b04c8b0941d9f2e92fc1f79b16f111bd7f7e2 100644
--- a/test/gtest_processor_tracker_landmark_object.cpp
+++ b/test/gtest_processor_tracker_landmark_object.cpp
@@ -310,13 +310,24 @@ TEST_F(ProcessorTrackerLandmarkObject_fixture, emplaceFactor)
ASSERT_TRUE(ctr->getType() == "FactorRelativePose3dWithExtrinsics");
}
+std::ostream& operator<<(std::ostream &flux, std::vector<std::pair<int,int> > vector)
+{
+ for (auto pair : vector)
+ {
+ flux << pair.first << " " << pair.second << "\n";
+ }
+
+ 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<Eigen::Isometry3d> cl_M_o_vec;
+ std::vector<Eigen::Isometry3d> ci_M_o_vec;
std::vector<std::pair<int,int> > matches;
- std::vector<int> inliers_idx;
- std::vector<int> outliers_idx;
+ std::vector<std::pair<int, int> > inliers_idx;
+ std::vector<std::pair<int, int> > outliers_idx;
Eigen::Isometry3d best_model;
//Camera poses
@@ -357,7 +368,7 @@ TEST_F(ProcessorTrackerLandmarkObject_fixture, matchingRANSAC)
Eigen::Isometry3d w_M_o3 = posevec_to_isometry(pose_object_3);
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;
@@ -416,8 +427,8 @@ TEST_F(ProcessorTrackerLandmarkObject_fixture, matchingRANSAC)
ASSERT_MATRIX_APPROX(quat_cam.coeffs(), quat_cam_RANSAC.coeffs(), 1e-6)
ASSERT_TRUE(outliers_idx.size() == 2);
- ASSERT_TRUE(outliers_idx[0] == 2);
- ASSERT_TRUE(outliers_idx[1] == 4);
+ ASSERT_TRUE(outliers_idx[0].first == 2);
+ ASSERT_TRUE(outliers_idx[1].first == 4);
}
TEST(ProcessorTrackerLandmarkObject, isInliers)
@@ -429,7 +440,7 @@ TEST(ProcessorTrackerLandmarkObject, isInliers)
//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;
@@ -449,9 +460,9 @@ TEST(ProcessorTrackerLandmarkObject, isInliers)
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_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;
@@ -511,22 +522,22 @@ TEST(ProcessorTrackerLandmarkObject, filterMatchesOutliers)
std::vector<std::pair<int,int> > matches2I;
std::vector<std::pair<int,int> > matches3I;
- std::vector<int> outliers_idx1;
- std::vector<int> outliers_idx2;
- std::vector<int> outliers_idx3;
-
- outliers_idx1.push_back(2);
- outliers_idx1.push_back(5);
- outliers_idx1.push_back(9);
- outliers_idx2.push_back(0);
- outliers_idx2.push_back(9);
- outliers_idx2.push_back(10);
- outliers_idx2.push_back(11);
- outliers_idx3.push_back(4);
- outliers_idx3.push_back(5);
- outliers_idx3.push_back(6);
- outliers_idx3.push_back(7);
- outliers_idx3.push_back(8);
+ 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);
@@ -571,8 +582,8 @@ TEST(ProcessorTrackerLandmarkObject, filterMatchesOutliers)
ProcessorTrackerLandmarkObject::filterMatchesInliers(outliers_idx3, matches3I);
ASSERT_TRUE(matches1.size() == 9);
- ASSERT_TRUE(matches2.size() == 8);
- ASSERT_TRUE(matches3.size() == 7);
+ ASSERT_TRUE(matches2.size() == 8);
+ ASSERT_TRUE(matches3.size() == 7);
ASSERT_TRUE(matches1I.size() == 3);
ASSERT_TRUE(matches2I.size() == 4);
@@ -580,6 +591,168 @@ TEST(ProcessorTrackerLandmarkObject, filterMatchesOutliers)
}
+// std::ostream& operator<<(std::ostream &flux, std::vector<std::pair<int,int> > vector)
+// {
+// for (auto pair : vector)
+// {
+// flux << pair.first << " " << pair.second << "\n";
+// }
+
+// return flux;
+// }
+
+TEST(ProcessorTrackerLandmarkObject, allMatchesSameType)
+{
+ FeatureBasePtrList features_last;
+ FeatureBasePtrList features_incoming;
+
+ 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);
+
+ // 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);
+
+ // 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);
+
+ // 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);
+
+ // 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);
+
+
+ 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_incoming.tail<4>() = pose_cam_incoming.tail<4>().normalized();
+
+
+ Eigen::Isometry3d w_M_ci = posevec_to_isometry(pose_cam_incoming);
+
+
+ // 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);
+
+ // 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);
+
+ // 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);
+
+ // 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);
+
+ // 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);
+
+ // 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);
+
+ // 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));
+
+}
+
int main(int argc, char **argv)