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

Uncomment multiviewTypeMatching function and change it

parent d230ff89
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/processor/processor_tracker_landmark_object.cpp
+ 75
− 145
View file @ 49fbcec6
......@@ -317,11 +317,11 @@ unsigned int ProcessorTrackerLandmarkObject::bestLmkMatching(const LandmarkBaseP
break;
}
//A counter for _landmarl_in
//Counter for _landmark_in
int i = 0;
//A variable to store the best lmk match
//Variable to store the best lmk match
int index_min = -1;
//A variable to store store if there was a match or not
//A variable to store if there was a match or not
bool match = false;
//Variables to compare errors of lmks
......@@ -344,13 +344,6 @@ unsigned int ProcessorTrackerLandmarkObject::bestLmkMatching(const LandmarkBaseP
//Test if errors are below the previous min errors
if (error.first < e_pos_min && error.second < e_rot_min)
{
// test_counter++;
// std::cout << "\n\n\n\n\n\n\n\n" << test_counter << std::endl << std::endl;
// std::cout << "\n\n\n\n\n" << error.first << " " << error.second << std::endl << std::endl;
e_pos_min = error.first;
e_rot_min = error.second;
index_min = i;
......@@ -406,162 +399,99 @@ 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;
// }
// //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"));
// //rob to sensor
// Eigen::Isometry3d r_M_c = posevec_to_isometry(getSensor()->getStateVector("PO"));
// int index_last = 0;
// //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();
// //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();
// int index_min_incoming = -1;
// double min_e_pos = 100;
// int index_incoming = 0;
// for (auto feat_incoming : detections_incoming_)
// {
// std::string object_type_incoming = std::static_pointer_cast<FeatureObject>(feat_incoming)->getObjectType();
unsigned int ProcessorTrackerLandmarkObject::multiviewTypeMatching(const CaptureBasePtr& _capture,
FeatureBasePtrList& _features_out_last,
FeatureBasePtrList& _features_out_incoming)
{
if (!last_ptr_){
return 0;
}
// if (object_type_last == object_type_incoming)
// {
//world to rob last frame
Eigen::Isometry3d w_M_r_last = posevec_to_isometry(getLast()->getFrame()->getStateVector("PO"));
// 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;
//world to rob incoming frame
Eigen::Isometry3d w_M_r_incoming = posevec_to_isometry(getIncoming()->getFrame()->getStateVector("PO"));
// Vector3d pos_feat_incoming = w_M_f_incoming.translation();
//rob to sensor
Eigen::Isometry3d r_M_c = posevec_to_isometry(getSensor()->getStateVector("PO"));
// // Error computation
// double e_pos = (pos_feat_last - pos_feat_incoming).norm();
std::vector<Eigen::Isometry3d> cl_M_o_vec;
std::vector<Eigen::Isometry3d> ci_M_o_vec;
std::vector<std::pair<int,int> > matches;
// 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);
int index_last = 0;
// }
// index_incoming++;
// }
// std::cout << index_min_incoming;
// index_last++;
for (auto feat_last : detections_last_)
{
std::string object_type_last = std::static_pointer_cast<FeatureObject>(feat_last)->getObjectType();
// }
//camera to feat (last)
Vector3d pos_obj_last = feat_last->getMeasurement().head(3);
Quaterniond quat_obj_last (feat_last->getMeasurement().tail(4).data());
// auto outliers = ProcessorTrackerLandmarkObject::matchingRANSAC(last_incoming);
// int nb_outliers = outliers.first.size();
// Eigen::Isometry3d cf_M_ci = outliers.second;
Eigen::Isometry3d cl_M_o = Eigen::Translation<double,3>(pos_obj_last) * quat_obj_last;
cl_M_o_vec.push_back(cl_M_o);
Eigen::Isometry3d w_M_f_last = w_M_r_last * r_M_c * cl_M_o;
Vector3d pos_feat_last = w_M_f_last.translation();
int index_best_incoming = -1;
double min_e_pos = 100;
// return 1;
// }
int index_incoming = 0;
// 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 feat_incoming : detections_incoming_)
{
std::string object_type_incoming = std::static_pointer_cast<FeatureObject>(feat_incoming)->getObjectType();
if (object_type_last == object_type_incoming)
{
//camera to feat
Vector3d pos_obj_incoming = feat_incoming->getMeasurement().head(3);
Quaterniond quat_obj_incoming (feat_incoming->getMeasurement().tail(4).data());
// for (auto last_incoming_tuple : last_incoming)
// {
// std::vector<int> index_outliers_this_iteration;
Eigen::Isometry3d ci_M_o = Eigen::Translation<double,3>(pos_obj_incoming) * quat_obj_incoming;
ci_M_o_vec.push_back(ci_M_o);
// int score = 0;
// int index_feat_incoming = std::get<1>(last_incoming_tuple);
Eigen::Isometry3d w_M_f_incoming = w_M_r_incoming * r_M_c * ci_M_o;
Vector3d pos_feat_incoming = w_M_f_incoming.translation();
// if (index_feat_incoming != -1)
// {
// Error computation
double e_pos = (pos_feat_last - pos_feat_incoming).norm();
// 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();
if (e_pos < e_pos_thr_ && e_pos < min_e_pos)
{
min_e_pos = e_pos;
index_best_incoming = index_incoming;
}
}
index_incoming++;
}
// Eigen::Isometry3d cl_M_ci = c_M_f_last * f_M_c_incoming;
if (index_best_incoming != -1)
{
std::pair<int, int> pair_indexes = std::make_pair(index_last, index_best_incoming);
matches.push_back(pair_indexes);
}
// 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++;
// }
index_last++;
}
// 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> inliers_idx;
std::vector<int> outliers_idx;
Eigen::Isometry3d best_model = Eigen::Isometry3d::Identity();
bool RANSACWorks = ProcessorTrackerLandmarkObject::matchingRANSAC(cl_M_o_vec, ci_M_o_vec, matches, inliers_idx, outliers_idx, best_model, ratio_inliers_outliers_);
// std::vector<int> v;
// std::pair<std::vector<int>, Eigen::Isometry3d> p{v, best_cl_M_ci};
if (RANSACWorks)
{
std::cout << "RANSAC has worked" << std::endl;
// int nb_outliers = outliers_idx.size();
// Eigen::Isometry3d cf_M_ci = best_model;
}
// return p;
// }
return 1;
}
bool ProcessorTrackerLandmarkObject::matchingRANSAC(const std::vector<Eigen::Isometry3d>& cl_M_o_vec,
const std::vector<Eigen::Isometry3d>& ci_M_o_vec,
......
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