diff --git a/include/objectslam/processor/processor_tracker_landmark_object.h b/include/objectslam/processor/processor_tracker_landmark_object.h
index c03add82df8ffca2f90743bbac5a4f8cb0674e79..b6d63dad5a12532c0d9434effdcdfb86c288dc50 100644
--- a/include/objectslam/processor/processor_tracker_landmark_object.h
+++ b/include/objectslam/processor/processor_tracker_landmark_object.h
@@ -144,7 +144,14 @@ 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 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);
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 043357ca0095d07eb85f53789d5637ac13d684d2..ed34d3404fe567ba9c1030b86bb46bec26a335a8 100644
--- a/src/processor/processor_tracker_landmark_object.cpp
+++ b/src/processor/processor_tracker_landmark_object.cpp
@@ -405,155 +405,236 @@ unsigned int ProcessorTrackerLandmarkObject::findLandmarks(const LandmarkBasePtr
return ftr_size;
}
-unsigned int ProcessorTrackerLandmarkObject::multiviewTypeMatching(const CaptureBasePtr& _capture,
- FeatureBasePtrList& _features_out_last,
- FeatureBasePtrList& _features_out_incoming)
-{
- if (!last_ptr_){
- return 0;
- }
+// unsigned int ProcessorTrackerLandmarkObject::multiviewTypeMatching(const CaptureBasePtr& _capture,
+// FeatureBasePtrList& _features_out_last,
+// FeatureBasePtrList& _features_out_incoming)
+// {
+// if (!last_ptr_){
+// return 0;
+// }
- //world to rob last frame
- Eigen::Isometry3d w_M_r_last = posevec_to_isometry(getLast()->getFrame()->getStateVector("PO"));
+// //world to rob last frame
+// Eigen::Isometry3d w_M_r_last = posevec_to_isometry(getLast()->getFrame()->getStateVector("PO"));
- //world to rob incoming frame
- Eigen::Isometry3d w_M_r_incoming = posevec_to_isometry(getIncoming()->getFrame()->getStateVector("PO"));
+// //world to rob incoming frame
+// Eigen::Isometry3d w_M_r_incoming = posevec_to_isometry(getIncoming()->getFrame()->getStateVector("PO"));
- //rob to sensor
- Eigen::Isometry3d r_M_c = posevec_to_isometry(getSensor()->getStateVector("PO"));
+// //rob to sensor
+// Eigen::Isometry3d r_M_c = posevec_to_isometry(getSensor()->getStateVector("PO"));
- int index_last = 0;
+// int index_last = 0;
- //matches between last and incoming features
- std::vector<std::tuple<int, int, Eigen::Isometry3d, Eigen::Isometry3d> > last_incoming;
+// //matches between last and incoming features
+// std::vector<std::tuple<int, int, Eigen::Isometry3d, Eigen::Isometry3d> > last_incoming;
- for (auto feat_last : detections_last_)
- {
- std::string object_type_last = std::static_pointer_cast<FeatureObject>(feat_last)->getObjectType();
+// for (auto feat_last : detections_last_)
+// {
+// std::string object_type_last = std::static_pointer_cast<FeatureObject>(feat_last)->getObjectType();
- //camera to feat
- Vector3d pos_obj_last = feat_last->getMeasurement().head(3);
- Quaterniond quat_obj_last (feat_last->getMeasurement().tail(4).data());
- Eigen::Isometry3d c_M_f_last = Eigen::Translation<double,3>(pos_obj_last) * quat_obj_last;
+// //camera to feat
+// Vector3d pos_obj_last = feat_last->getMeasurement().head(3);
+// Quaterniond quat_obj_last (feat_last->getMeasurement().tail(4).data());
+// Eigen::Isometry3d c_M_f_last = Eigen::Translation<double,3>(pos_obj_last) * quat_obj_last;
- Eigen::Isometry3d w_M_f_last = w_M_r_last * r_M_c * c_M_f_last;
- Vector3d pos_feat_last = w_M_f_last.translation();
+// Eigen::Isometry3d w_M_f_last = w_M_r_last * r_M_c * c_M_f_last;
+// Vector3d pos_feat_last = w_M_f_last.translation();
- int index_min_incoming = -1;
- double min_e_pos = 100;
+// int index_min_incoming = -1;
+// double min_e_pos = 100;
- int index_incoming = 0;
+// int index_incoming = 0;
- for (auto feat_incoming : detections_incoming_)
- {
+// for (auto feat_incoming : detections_incoming_)
+// {
- std::string object_type_incoming = std::static_pointer_cast<FeatureObject>(feat_incoming)->getObjectType();
+// std::string object_type_incoming = std::static_pointer_cast<FeatureObject>(feat_incoming)->getObjectType();
- if (object_type_last == object_type_incoming)
- {
+// if (object_type_last == object_type_incoming)
+// {
- Eigen::Isometry3d c_M_f_incoming = posevec_to_isometry(feat_incoming->getMeasurement());
+// Eigen::Isometry3d c_M_f_incoming = posevec_to_isometry(feat_incoming->getMeasurement());
- Eigen::Isometry3d w_M_f_incoming = w_M_r_incoming * r_M_c * c_M_f_incoming;
+// Eigen::Isometry3d w_M_f_incoming = w_M_r_incoming * r_M_c * c_M_f_incoming;
- Vector3d pos_feat_incoming = w_M_f_incoming.translation();
+// Vector3d pos_feat_incoming = w_M_f_incoming.translation();
- // Error computation
- double e_pos = (pos_feat_last - pos_feat_incoming).norm();
+// // Error computation
+// double e_pos = (pos_feat_last - pos_feat_incoming).norm();
- if (e_pos < e_pos_thr_ && e_pos < min_e_pos)
- {
- min_e_pos = e_pos;
- index_min_incoming = index_incoming;
- }
+// if (e_pos < e_pos_thr_ && e_pos < min_e_pos)
+// {
+// min_e_pos = e_pos;
+// index_min_incoming = index_incoming;
+// }
- auto tuple_last_incom = std::make_tuple(index_last, index_incoming, c_M_f_last, c_M_f_incoming);
- last_incoming.push_back(tuple_last_incom);
+// auto tuple_last_incom = std::make_tuple(index_last, index_incoming, c_M_f_last, c_M_f_incoming);
+// last_incoming.push_back(tuple_last_incom);
- }
- index_incoming++;
- }
+// }
+// index_incoming++;
+// }
- std::cout << index_min_incoming;
- index_last++;
+// std::cout << index_min_incoming;
+// index_last++;
- }
- return 1;
-}
+// }
+
+// auto outliers = ProcessorTrackerLandmarkObject::matchingRANSAC(last_incoming);
+// int nb_outliers = outliers.first.size();
+// Eigen::Isometry3d cf_M_ci = outliers.second;
+
+
+// return 1;
+// }
+
+// std::pair<std::vector<int>, Eigen::Isometry3d> ProcessorTrackerLandmarkObject::matchingRANSAC(std::vector<std::tuple<int, int, Eigen::Isometry3d, Eigen::Isometry3d> > last_incoming)
+// {
+// // Vector that will contains index of outliers for each iteration
+// std::vector<std::vector<int> > index_outliers;
+// int i = 0;
+// int best_score = 0;
+// Eigen::Isometry3d best_cl_M_ci = Eigen::Isometry3d::Identity();;
+// int index_best_score = -1;
+
+
+// for (auto last_incoming_tuple : last_incoming)
+// {
+
+// std::vector<int> index_outliers_this_iteration;
+
+// int score = 0;
+// int index_feat_incoming = std::get<1>(last_incoming_tuple);
-std::pair<std::vector<int>, Eigen::Isometry3d> ProcessorTrackerLandmarkObject::matchingRANSAC(std::vector<std::tuple<int, int, Eigen::Isometry3d, Eigen::Isometry3d> > last_incoming)
+// if (index_feat_incoming != -1)
+// {
+
+// Eigen::Isometry3d c_M_f_last = std::get<2>(last_incoming_tuple);
+// Eigen::Isometry3d f_M_c_incoming = std::get<3>(last_incoming_tuple).inverse();
+
+// Eigen::Isometry3d cl_M_ci = c_M_f_last * f_M_c_incoming;
+
+// for (auto last_incoming_tuple_other : last_incoming)
+// {
+
+// int index_feat_last = std::get<0>(last_incoming_tuple);
+// int index_feat_last_other = std::get<0>(last_incoming_tuple_other);
+
+// if (index_feat_incoming != -1 && index_feat_last != index_feat_last_other)
+// {
+// Eigen::Isometry3d cl_M_ol = std::get<2>(last_incoming_tuple_other);
+// Eigen::Isometry3d ci_M_oi = std::get<3>(last_incoming_tuple_other);
+
+// if(isInliers(cl_M_ol, ci_M_oi, cl_M_ci))
+// {
+// score++;
+// }
+// else
+// {
+// index_outliers_this_iteration.push_back(index_feat_last_other);
+// }
+// }
+// }
+
+// if (score > best_score)
+// {
+// best_score = score;
+// index_best_score = i;
+// best_cl_M_ci = cl_M_ci;
+// }
+
+// }
+
+// //Add index of outliers for this iteration of RANSAC
+// index_outliers.push_back(index_outliers_this_iteration);
+// i++;
+// }
+
+// if (index_best_score != -1)
+// {
+// std::pair<std::vector<int>, Eigen::Isometry3d> p{index_outliers[index_best_score], best_cl_M_ci};
+// return p;
+// }
+
+
+// std::vector<int> v;
+// std::pair<std::vector<int>, Eigen::Isometry3d> p{v, best_cl_M_ci};
+
+// return p;
+// }
+
+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,
+ std::vector<int>& inliers_idx,
+ std::vector<int>& outliers_idx,
+ Eigen::Isometry3d& best_model)
{
// Vector that will contains index of outliers for each iteration
- std::vector<std::vector<int> > index_outliers;
- int i = 0;
+ std::vector<int> inliers_idx_buff;
+ std::vector<int> outliers_idx_buff;
+
+ int index_match = 0;
+ int index_other_match = 0;
int best_score = 0;
- Eigen::Isometry3d best_cl_M_ci = Eigen::Isometry3d::Identity();;
- int index_best_score = -1;
-
- for (auto last_incoming_tuple : last_incoming)
+ for (auto match : matches)
{
std::vector<int> index_outliers_this_iteration;
int score = 0;
- int index_feat_incoming = std::get<1>(last_incoming_tuple);
+ int index_feat_incoming = match.second;
if (index_feat_incoming != -1)
{
- Eigen::Isometry3d c_M_f_last = std::get<2>(last_incoming_tuple);
- Eigen::Isometry3d f_M_c_incoming = std::get<3>(last_incoming_tuple).inverse();
+ Eigen::Isometry3d cl_M_o = cl_M_o_vec[index_match];
+ Eigen::Isometry3d o_M_ci = ci_M_o_vec[index_match].inverse();
- Eigen::Isometry3d cl_M_ci = c_M_f_last * f_M_c_incoming;
+ Eigen::Isometry3d cl_M_ci = cl_M_o * o_M_ci;
+
+ index_other_match = 0;
- for (auto last_incoming_tuple_other : last_incoming)
+ for (auto other_match : matches)
{
- int index_feat_last = std::get<0>(last_incoming_tuple);
- int index_feat_last_other = std::get<0>(last_incoming_tuple_other);
+ int index_feat_last = match.first;
+ int index_feat_last_other = other_match.first;
if (index_feat_incoming != -1 && index_feat_last != index_feat_last_other)
{
- Eigen::Isometry3d cl_M_ol = std::get<2>(last_incoming_tuple_other);
- Eigen::Isometry3d ci_M_oi = std::get<3>(last_incoming_tuple_other);
+ Eigen::Isometry3d cl_M_ol = cl_M_o_vec[index_other_match];
+ Eigen::Isometry3d ci_M_oi = ci_M_o_vec[index_other_match];
if(isInliers(cl_M_ol, ci_M_oi, cl_M_ci))
{
score++;
+ inliers_idx_buff.push_back(index_feat_last_other);
}
else
{
- index_outliers_this_iteration.push_back(index_feat_last_other);
+ outliers_idx_buff.push_back(index_feat_last_other);
}
}
+ index_other_match++;
}
if (score > best_score)
{
best_score = score;
- index_best_score = i;
- best_cl_M_ci = cl_M_ci;
+ inliers_idx = inliers_idx_buff;
+ outliers_idx = outliers_idx_buff;
+ best_model = cl_M_ci;
}
- }
-
- //Add index of outliers for this iteration of RANSAC
- index_outliers.push_back(index_outliers_this_iteration);
- i++;
- }
+ inliers_idx_buff.clear();
+ outliers_idx_buff.clear();
- if (index_best_score != -1)
- {
- std::pair<std::vector<int>, Eigen::Isometry3d> p{index_outliers[index_best_score], best_cl_M_ci};
- return p;
- }
-
+ }
+ index_match++;
+ }
- std::vector<int> v;
- std::pair<std::vector<int>, Eigen::Isometry3d> p{v, best_cl_M_ci};
-
- return p;
+ return true;
}
std::ostream &operator<<(std::ostream &flux, Quaterniond quat)
diff --git a/test/gtest_processor_tracker_landmark_object.cpp b/test/gtest_processor_tracker_landmark_object.cpp
index 10fbba51e84be9b1095fd87742676836755f67ab..40563fb9b8a0d130c9dd8ef287b379d2e6a73853 100644
--- a/test/gtest_processor_tracker_landmark_object.cpp
+++ b/test/gtest_processor_tracker_landmark_object.cpp
@@ -314,10 +314,18 @@ TEST_F(ProcessorTrackerLandmarkObject_class, emplaceFactor)
TEST_F(ProcessorTrackerLandmarkObject_class, matchingRANSAC)
{
- std::vector<std::tuple<int, int, Eigen::Isometry3d, Eigen::Isometry3d> > last_incoming;
-
+ 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;
+ Eigen::Isometry3d best_model;
+
+ //Camera poses
Vector7d pose_cam_last;
Vector7d pose_cam_incoming;
+
+ //Create 5 objects
Vector7d pose_object_1;
Vector7d pose_object_2;
Vector7d pose_object_3;
@@ -332,15 +340,16 @@ TEST_F(ProcessorTrackerLandmarkObject_class, matchingRANSAC)
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;
- 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
Eigen::Isometry3d w_M_cl = posevec_to_isometry(pose_cam_last);
Eigen::Isometry3d w_M_ci = posevec_to_isometry(pose_cam_incoming);
@@ -357,29 +366,40 @@ TEST_F(ProcessorTrackerLandmarkObject_class, matchingRANSAC)
Eigen::Isometry3d cl_M_o4 = w_M_cl.inverse() * w_M_o4;
Eigen::Isometry3d cl_M_o5 = w_M_o5;
+ cl_M_o_vec.push_back(cl_M_o1);
+ cl_M_o_vec.push_back(cl_M_o2);
+ cl_M_o_vec.push_back(cl_M_o3);
+ cl_M_o_vec.push_back(cl_M_o4);
+ cl_M_o_vec.push_back(cl_M_o5);
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_o3;
+ 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_o5;
+ Eigen::Isometry3d ci_M_o5 = w_M_o2; //outliers
- Eigen::Isometry3d cl_M_ci = w_M_cl.inverse() * w_M_ci;
+ ci_M_o_vec.push_back(ci_M_o1);
+ ci_M_o_vec.push_back(ci_M_o2);
+ ci_M_o_vec.push_back(ci_M_o3);
+ ci_M_o_vec.push_back(ci_M_o4);
+ ci_M_o_vec.push_back(ci_M_o5);
- auto tuple_o1 = std::make_tuple(0, 0, cl_M_o1, ci_M_o1);
- auto tuple_o2 = std::make_tuple(1, 1, cl_M_o2, ci_M_o2);
- auto tuple_o3 = std::make_tuple(2, 2, cl_M_o3, ci_M_o3);
- auto tuple_o4 = std::make_tuple(3, 3, cl_M_o4, ci_M_o4);
- auto tuple_o5 = std::make_tuple(4, 4, cl_M_o5, ci_M_o5);
+ Eigen::Isometry3d cl_M_ci = w_M_cl.inverse() * w_M_ci;
- last_incoming.push_back(tuple_o1);
- last_incoming.push_back(tuple_o2);
- last_incoming.push_back(tuple_o3);
- last_incoming.push_back(tuple_o4);
- last_incoming.push_back(tuple_o5);
+ 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);
+ 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);
- auto outliers = ProcessorTrackerLandmarkObject::matchingRANSAC(last_incoming);
+ //Detect all outliers of our batch
+ ProcessorTrackerLandmarkObject::matchingRANSAC(cl_M_o_vec, ci_M_o_vec, matches, inliers_idx, outliers_idx, best_model);
Quaterniond quat_cam;
Eigen::Matrix3d wRf = cl_M_ci.linear();
@@ -387,10 +407,11 @@ TEST_F(ProcessorTrackerLandmarkObject_class, matchingRANSAC)
Vector3d pos_cam = cl_M_ci.translation();
Quaterniond quat_cam_RANSAC;
- Eigen::Matrix3d wRf_R = outliers.second.linear();
+ Eigen::Matrix3d wRf_R = best_model.linear();
quat_cam_RANSAC.coeffs() = R2q(wRf_R).coeffs().transpose();
- Vector3d pos_cam_RANSAC = outliers.second.translation();
+ Vector3d pos_cam_RANSAC = best_model.translation();
+ //Check if isometry deduced in matching RANSAC is the same as cl_M_ci given
for (int i = 0; i < pos_cam.size(); i++)
{
ASSERT_TRUE(abs(pos_cam[i] - pos_cam_RANSAC[i]) < 0.0001);
@@ -399,9 +420,11 @@ TEST_F(ProcessorTrackerLandmarkObject_class, matchingRANSAC)
ASSERT_TRUE(abs(quat_cam.y() - quat_cam_RANSAC.y()) < 0.0001);
ASSERT_TRUE(abs(quat_cam.z() - quat_cam_RANSAC.z()) < 0.0001);
ASSERT_TRUE(abs(quat_cam.w() - quat_cam_RANSAC.w()) < 0.0001);
- ASSERT_TRUE(outliers.first.size() == 2);
- ASSERT_TRUE(outliers.first[0] == 2);
- ASSERT_TRUE(outliers.first[1] == 4);
+
+ //Check if detections of outliers is correct
+ ASSERT_TRUE(outliers_idx.size() == 2);
+ ASSERT_TRUE(outliers_idx[0] == 2);
+ ASSERT_TRUE(outliers_idx[1] == 4);
}
TEST(ProcessorTrackerLandmarkObject, isInliers)