Skip to content
Snippets Groups Projects
Commit f2081e9b authored by ydepledt's avatar ydepledt
Browse files

Change function matchingRANSAC

parent 476362cd
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
include/objectslam/processor/processor_tracker_landmark_object.h
+ 8
− 1
View file @ f2081e9b
...@@ -144,7 +144,14 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark ...@@ -144,7 +144,14 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark
FeatureBasePtrList& _features_out_last, FeatureBasePtrList& _features_out_last,
FeatureBasePtrList& _features_out_incoming); 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); static bool isInliers(Eigen::Isometry3d cl_M_ol, Eigen::Isometry3d ci_M_oi, Eigen::Isometry3d cl_M_ci);
......
src/processor/processor_tracker_landmark_object.cpp
+ 169
− 88
View file @ f2081e9b
...@@ -405,155 +405,236 @@ unsigned int ProcessorTrackerLandmarkObject::findLandmarks(const LandmarkBasePtr ...@@ -405,155 +405,236 @@ unsigned int ProcessorTrackerLandmarkObject::findLandmarks(const LandmarkBasePtr
return ftr_size; return ftr_size;
} }
unsigned int ProcessorTrackerLandmarkObject::multiviewTypeMatching(const CaptureBasePtr& _capture, // unsigned int ProcessorTrackerLandmarkObject::multiviewTypeMatching(const CaptureBasePtr& _capture,
FeatureBasePtrList& _features_out_last, // FeatureBasePtrList& _features_out_last,
FeatureBasePtrList& _features_out_incoming) // FeatureBasePtrList& _features_out_incoming)
{ // {
if (!last_ptr_){ // if (!last_ptr_){
return 0; // return 0;
} // }
//world to rob last frame // //world to rob last frame
Eigen::Isometry3d w_M_r_last = posevec_to_isometry(getLast()->getFrame()->getStateVector("PO")); // Eigen::Isometry3d w_M_r_last = posevec_to_isometry(getLast()->getFrame()->getStateVector("PO"));
//world to rob incoming frame // //world to rob incoming frame
Eigen::Isometry3d w_M_r_incoming = posevec_to_isometry(getIncoming()->getFrame()->getStateVector("PO")); // Eigen::Isometry3d w_M_r_incoming = posevec_to_isometry(getIncoming()->getFrame()->getStateVector("PO"));
//rob to sensor // //rob to sensor
Eigen::Isometry3d r_M_c = posevec_to_isometry(getSensor()->getStateVector("PO")); // 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 // //matches between last and incoming features
std::vector<std::tuple<int, int, Eigen::Isometry3d, Eigen::Isometry3d> > last_incoming; // std::vector<std::tuple<int, int, Eigen::Isometry3d, Eigen::Isometry3d> > last_incoming;
for (auto feat_last : detections_last_) // for (auto feat_last : detections_last_)
{ // {
std::string object_type_last = std::static_pointer_cast<FeatureObject>(feat_last)->getObjectType(); // std::string object_type_last = std::static_pointer_cast<FeatureObject>(feat_last)->getObjectType();
//camera to feat // //camera to feat
Vector3d pos_obj_last = feat_last->getMeasurement().head(3); // Vector3d pos_obj_last = feat_last->getMeasurement().head(3);
Quaterniond quat_obj_last (feat_last->getMeasurement().tail(4).data()); // 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 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; // 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(); // Vector3d pos_feat_last = w_M_f_last.translation();
int index_min_incoming = -1; // int index_min_incoming = -1;
double min_e_pos = 100; // 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 // // Error computation
double e_pos = (pos_feat_last - pos_feat_incoming).norm(); // double e_pos = (pos_feat_last - pos_feat_incoming).norm();
if (e_pos < e_pos_thr_ && e_pos < min_e_pos) // if (e_pos < e_pos_thr_ && e_pos < min_e_pos)
{ // {
min_e_pos = e_pos; // min_e_pos = e_pos;
index_min_incoming = index_incoming; // index_min_incoming = index_incoming;
} // }
auto tuple_last_incom = std::make_tuple(index_last, index_incoming, c_M_f_last, c_M_f_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); // last_incoming.push_back(tuple_last_incom);
} // }
index_incoming++; // index_incoming++;
} // }
std::cout << index_min_incoming; // std::cout << index_min_incoming;
index_last++; // 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 // Vector that will contains index of outliers for each iteration
std::vector<std::vector<int> > index_outliers; std::vector<int> inliers_idx_buff;
int i = 0; std::vector<int> outliers_idx_buff;
int index_match = 0;
int index_other_match = 0;
int best_score = 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; std::vector<int> index_outliers_this_iteration;
int score = 0; int score = 0;
int index_feat_incoming = std::get<1>(last_incoming_tuple); int index_feat_incoming = match.second;
if (index_feat_incoming != -1) if (index_feat_incoming != -1)
{ {
Eigen::Isometry3d c_M_f_last = std::get<2>(last_incoming_tuple); Eigen::Isometry3d cl_M_o = cl_M_o_vec[index_match];
Eigen::Isometry3d f_M_c_incoming = std::get<3>(last_incoming_tuple).inverse(); 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 = match.first;
int index_feat_last_other = std::get<0>(last_incoming_tuple_other); int index_feat_last_other = other_match.first;
if (index_feat_incoming != -1 && index_feat_last != index_feat_last_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 cl_M_ol = cl_M_o_vec[index_other_match];
Eigen::Isometry3d ci_M_oi = std::get<3>(last_incoming_tuple_other); Eigen::Isometry3d ci_M_oi = ci_M_o_vec[index_other_match];
if(isInliers(cl_M_ol, ci_M_oi, cl_M_ci)) if(isInliers(cl_M_ol, ci_M_oi, cl_M_ci))
{ {
score++; score++;
inliers_idx_buff.push_back(index_feat_last_other);
} }
else 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) if (score > best_score)
{ {
best_score = score; best_score = score;
index_best_score = i; inliers_idx = inliers_idx_buff;
best_cl_M_ci = cl_M_ci; outliers_idx = outliers_idx_buff;
best_model = cl_M_ci;
} }
} inliers_idx_buff.clear();
outliers_idx_buff.clear();
//Add index of outliers for this iteration of RANSAC
index_outliers.push_back(index_outliers_this_iteration);
i++;
}
if (index_best_score != -1) }
{ index_match++;
std::pair<std::vector<int>, Eigen::Isometry3d> p{index_outliers[index_best_score], best_cl_M_ci}; }
return p;
}
std::vector<int> v; return true;
std::pair<std::vector<int>, Eigen::Isometry3d> p{v, best_cl_M_ci};
return p;
} }
std::ostream &operator<<(std::ostream &flux, Quaterniond quat) std::ostream &operator<<(std::ostream &flux, Quaterniond quat)
......
test/gtest_processor_tracker_landmark_object.cpp
+ 51
− 28
View file @ f2081e9b
...@@ -314,10 +314,18 @@ TEST_F(ProcessorTrackerLandmarkObject_class, emplaceFactor) ...@@ -314,10 +314,18 @@ TEST_F(ProcessorTrackerLandmarkObject_class, emplaceFactor)
TEST_F(ProcessorTrackerLandmarkObject_class, matchingRANSAC) 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_last;
Vector7d pose_cam_incoming; Vector7d pose_cam_incoming;
//Create 5 objects
Vector7d pose_object_1; Vector7d pose_object_1;
Vector7d pose_object_2; Vector7d pose_object_2;
Vector7d pose_object_3; Vector7d pose_object_3;
...@@ -332,15 +340,16 @@ TEST_F(ProcessorTrackerLandmarkObject_class, matchingRANSAC) ...@@ -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_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_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_cam_incoming.tail<4>() = pose_cam_incoming.tail<4>().normalized();
pose_object_1.tail<4>() = pose_object_1.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_2.tail<4>() = pose_object_2.tail<4>().normalized();
pose_object_3.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_4.tail<4>() = pose_object_4.tail<4>().normalized();
pose_object_5.tail<4>() = pose_object_5.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_cl = posevec_to_isometry(pose_cam_last);
Eigen::Isometry3d w_M_ci = posevec_to_isometry(pose_cam_incoming); Eigen::Isometry3d w_M_ci = posevec_to_isometry(pose_cam_incoming);
...@@ -357,29 +366,40 @@ TEST_F(ProcessorTrackerLandmarkObject_class, matchingRANSAC) ...@@ -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_o4 = w_M_cl.inverse() * w_M_o4;
Eigen::Isometry3d cl_M_o5 = w_M_o5; 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_o1 = w_M_ci.inverse() * w_M_o1;
Eigen::Isometry3d ci_M_o2 = w_M_ci.inverse() * w_M_o2; 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_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); Eigen::Isometry3d cl_M_ci = w_M_cl.inverse() * w_M_ci;
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);
last_incoming.push_back(tuple_o1); auto pair_o1 = std::make_pair(0, 0);
last_incoming.push_back(tuple_o2); auto pair_o2 = std::make_pair(1, 1);
last_incoming.push_back(tuple_o3); auto pair_o3 = std::make_pair(2, 2);
last_incoming.push_back(tuple_o4); auto pair_o4 = std::make_pair(3, 3);
last_incoming.push_back(tuple_o5); 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; Quaterniond quat_cam;
Eigen::Matrix3d wRf = cl_M_ci.linear(); Eigen::Matrix3d wRf = cl_M_ci.linear();
...@@ -387,10 +407,11 @@ TEST_F(ProcessorTrackerLandmarkObject_class, matchingRANSAC) ...@@ -387,10 +407,11 @@ TEST_F(ProcessorTrackerLandmarkObject_class, matchingRANSAC)
Vector3d pos_cam = cl_M_ci.translation(); Vector3d pos_cam = cl_M_ci.translation();
Quaterniond quat_cam_RANSAC; 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(); 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++) for (int i = 0; i < pos_cam.size(); i++)
{ {
ASSERT_TRUE(abs(pos_cam[i] - pos_cam_RANSAC[i]) < 0.0001); ASSERT_TRUE(abs(pos_cam[i] - pos_cam_RANSAC[i]) < 0.0001);
...@@ -399,9 +420,11 @@ TEST_F(ProcessorTrackerLandmarkObject_class, matchingRANSAC) ...@@ -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.y() - quat_cam_RANSAC.y()) < 0.0001);
ASSERT_TRUE(abs(quat_cam.z() - quat_cam_RANSAC.z()) < 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(abs(quat_cam.w() - quat_cam_RANSAC.w()) < 0.0001);
ASSERT_TRUE(outliers.first.size() == 2);
ASSERT_TRUE(outliers.first[0] == 2); //Check if detections of outliers is correct
ASSERT_TRUE(outliers.first[1] == 4); ASSERT_TRUE(outliers_idx.size() == 2);
ASSERT_TRUE(outliers_idx[0] == 2);
ASSERT_TRUE(outliers_idx[1] == 4);
} }
TEST(ProcessorTrackerLandmarkObject, isInliers) TEST(ProcessorTrackerLandmarkObject, isInliers)
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment