diff --git a/include/objectslam/processor/processor_tracker_landmark_object.h b/include/objectslam/processor/processor_tracker_landmark_object.h
index b6d63dad5a12532c0d9434effdcdfb86c288dc50..1ced5fd6dbf6da82d8fbfd15e9fa1be46fd4f927 100644
--- a/include/objectslam/processor/processor_tracker_landmark_object.h
+++ b/include/objectslam/processor/processor_tracker_landmark_object.h
@@ -26,6 +26,7 @@ struct ParamsProcessorTrackerLandmarkObject : public ParamsProcessorTrackerLandm
int fps_;
Matrix3d intrinsic_;
bool first_lmk_matching_;
+ double ratio_inliers_outliers_;
ParamsProcessorTrackerLandmarkObject() = default;
ParamsProcessorTrackerLandmarkObject(std::string _unique_name, const ParamsServer& _server):
@@ -43,6 +44,7 @@ struct ParamsProcessorTrackerLandmarkObject : public ParamsProcessorTrackerLandm
double cx = _server.getParam<double>(prefix + _unique_name + "/intrinsic/cx");
double cy = _server.getParam<double>(prefix + _unique_name + "/intrinsic/cy");
first_lmk_matching_ = _server.getParam<bool> (prefix + _unique_name + "/first_lmk_matching");
+ ratio_inliers_outliers_ = _server.getParam<double>(prefix + _unique_name + "/ratio_inliers_outliers");
intrinsic_ << fx, 0, cx,
0, fy, cy,
0, 0, 1;
@@ -220,6 +222,7 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark
int fps_;
Matrix3d intrinsic_;
bool first_lmk_matching_;
+ double ratio_inliers_outliers_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
diff --git a/src/processor/processor_tracker_landmark_object.cpp b/src/processor/processor_tracker_landmark_object.cpp
index 8ce39e39a5a6aa50c1b74533e739a7c6484f81f4..5191ec5063a08d22c9c6ab30bdb48fcd64cdb051 100644
--- a/src/processor/processor_tracker_landmark_object.cpp
+++ b/src/processor/processor_tracker_landmark_object.cpp
@@ -26,7 +26,8 @@ ProcessorTrackerLandmarkObject::ProcessorTrackerLandmarkObject( ParamsProcessorT
nb_vote_(0),
fps_(_params_tracker_landmark_object->fps_),
intrinsic_(_params_tracker_landmark_object->intrinsic_),
- first_lmk_matching_(_params_tracker_landmark_object->first_lmk_matching_)
+ first_lmk_matching_(_params_tracker_landmark_object->first_lmk_matching_),
+ ratio_inliers_outliers_(_params_tracker_landmark_object->ratio_inliers_outliers_)
{
std::cout << _params_tracker_landmark_object->print() << std::endl;
@@ -563,78 +564,81 @@ unsigned int ProcessorTrackerLandmarkObject::findLandmarks(const LandmarkBasePtr
// }
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)
+ 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
+ // Vector that will contains index of inliers/outliers for each iteration
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;
+ //Number of inliers for the best model
+ int best_nb_inliers = 0;
for (auto match : matches)
{
-
- std::vector<int> index_outliers_this_iteration;
+ //Number of inliers for this model
+ int nb_inliers = 0;
- int score = 0;
+ //Indexes of matching features between last and incoming for this model
+ int index_feat_last = match.first;
int index_feat_incoming = match.second;
- if (index_feat_incoming != -1)
- {
-
- Eigen::Isometry3d cl_M_o = cl_M_o_vec[index_match];
- Eigen::Isometry3d o_M_ci = ci_M_o_vec[index_match].inverse();
+ //Transformation between camera and object in last/incoming frame for this model
+ Eigen::Isometry3d cl_M_o = cl_M_o_vec[index_feat_last];
+ Eigen::Isometry3d o_M_ci = ci_M_o_vec[index_feat_incoming].inverse();
- Eigen::Isometry3d cl_M_ci = cl_M_o * o_M_ci;
-
- index_other_match = 0;
-
- for (auto other_match : matches)
- {
-
- 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 = cl_M_o_vec[index_other_match];
- Eigen::Isometry3d ci_M_oi = ci_M_o_vec[index_other_match];
+ //Camera's transformation between last and incoming for this model
+ Eigen::Isometry3d cl_M_ci = cl_M_o * o_M_ci;
- if(isInliers(cl_M_ol, ci_M_oi, cl_M_ci))
- {
- score++;
- inliers_idx_buff.push_back(index_feat_last_other);
- }
- else
- {
- outliers_idx_buff.push_back(index_feat_last_other);
- }
+ for (auto other_match : matches)
+ {
+
+ int index_feat_last_other = other_match.first;
+ 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];
+
+ if(isInliers(cl_M_ol, ci_M_oi, cl_M_ci))
+ {
+ nb_inliers++;
+ inliers_idx_buff.push_back(index_feat_last_other);
+ }
+ else
+ {
+ outliers_idx_buff.push_back(index_feat_last_other);
}
- index_other_match++;
}
+ }
- if (score > best_score)
- {
- best_score = score;
- inliers_idx = inliers_idx_buff;
- outliers_idx = outliers_idx_buff;
- best_model = cl_M_ci;
- }
+ //If the model gives better results
+ if (nb_inliers > best_nb_inliers)
+ {
+ best_nb_inliers = nb_inliers;
+ inliers_idx = inliers_idx_buff;
+ outliers_idx = outliers_idx_buff;
+ best_model = cl_M_ci;
+ }
- inliers_idx_buff.clear();
- outliers_idx_buff.clear();
+ //Buffers clearing
+ inliers_idx_buff.clear();
+ outliers_idx_buff.clear();
+ }
- }
- index_match++;
- }
+ int nb_inliers = inliers_idx.size();
+ int nb_outliers = outliers_idx.size();
- return true;
+ if ((double)nb_outliers/nb_inliers > 0.55)
+ return true;
+
+ return false;
}
diff --git a/test/gtest_processor_tracker_landmark_object.cpp b/test/gtest_processor_tracker_landmark_object.cpp
index bbc1c4c701c49c5cb42503dc8d63324f53b3163c..5a2b0593c31ad544946574e0e261e48dc049e72b 100644
--- a/test/gtest_processor_tracker_landmark_object.cpp
+++ b/test/gtest_processor_tracker_landmark_object.cpp
@@ -384,11 +384,11 @@ TEST_F(ProcessorTrackerLandmarkObject_fixture, matchingRANSAC)
Eigen::Isometry3d cl_M_ci = w_M_cl.inverse() * w_M_ci;
- 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_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);
@@ -415,28 +415,33 @@ TEST_F(ProcessorTrackerLandmarkObject_fixture, matchingRANSAC)
TEST(ProcessorTrackerLandmarkObject, isInliers)
{
+ //Camera poses
Vector7d pose_cam_last;
Vector7d pose_cam_incoming;
+
+ //Create 2 objects
Vector7d pose_object_1;
Vector7d pose_object_2;
-
+
+
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();
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();
+ //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_ci = w_M_cl.inverse() * w_M_ci;
@@ -451,8 +456,8 @@ TEST(ProcessorTrackerLandmarkObject, isInliers)
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)));
- ASSERT_TRUE(!(ProcessorTrackerLandmarkObject::isInliers(cl_M_o2, ci_M_o1, 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
}