Commit 2777d264 authored by Mederic Fourmy's avatar Mederic Fourmy
Browse files

Merge branch 'devel' into 3-adapt-to-state-derived-in-core

parents fd80859d b9b3d3a7
......@@ -142,8 +142,26 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark
FeatureBasePtrList& _features_out_incoming);
static std::vector<std::pair<int,int> > allMatchesSameType(const FeatureBasePtrList& _features_out_last,
const FeatureBasePtrList& _features_out_incoming);
static void keepInliers(const std::vector<Eigen::Isometry3d>& _cl_M_o_vec,
const std::vector<Eigen::Isometry3d>& _ci_M_o_vec,
std::vector<std::pair<int,int> >& matches,
Eigen::Isometry3d& _best_model);
/** \brief Method for create all pair of features (same type) from last and incoming
* \param _features_out_last returned list of features found in \b _capture corresponding to a landmark of _landmarks_in
* \param _features_out_incoming returned list of features found in incoming capture corresponding to a landmark of _landmarks_in
* \param _cl_M_o_vec vector of camera to object isometry for last capture
* \param _ci_M_o_vec vector of camera to object isometry for incoming capture
*
*
* \return All matches available for those two lists
*/
static std::vector<std::pair<int,int> > createAllMatchesSameType(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);
/** \brief Method for matching feature from last to incoming
......
......@@ -511,14 +511,39 @@ std::ostream& operator<<(std::ostream &flux, std::vector<int> vector)
return flux;
}
std::vector<std::pair<int,int> > ProcessorTrackerLandmarkObject::allMatchesSameType(const FeatureBasePtrList& _features_out_last,
const FeatureBasePtrList& _features_out_incoming)
void ProcessorTrackerLandmarkObject::keepInliers(const std::vector<Eigen::Isometry3d>& _cl_M_o_vec,
const std::vector<Eigen::Isometry3d>& _ci_M_o_vec,
std::vector<std::pair<int,int> >& matches,
Eigen::Isometry3d& _best_model)
{
//Vector that will store inliers/outliers pairs
std::vector<std::pair<int,int> > inliers_idx;
std::vector<std::pair<int,int> > outliers_idx;
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::filterMatchesOutliers(outliers_idx, matches);
}
}
std::vector<std::pair<int,int> > ProcessorTrackerLandmarkObject::createAllMatchesSameType(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<Eigen::Isometry3d> cl_M_o_vec;
std::vector<Eigen::Isometry3d> ci_M_o_vec;
std::vector<std::pair<int,int> > matches;
//Vector that store all matches available
std::vector<std::pair<int,int> > all_matches;
int index_last = 0;
//First pass throught the loop
bool pushBack = true;
for (auto feat_last : _features_out_last)
......@@ -528,60 +553,43 @@ std::vector<std::pair<int,int> > ProcessorTrackerLandmarkObject::allMatchesSameT
//camera to feat (last)
Vector3d pos_obj_last = feat_last->getMeasurement().head(3);
Quaterniond quat_obj_last (feat_last->getMeasurement().tail(4).data());
Eigen::Isometry3d cl_M_o = Eigen::Translation<double,3>(pos_obj_last) * quat_obj_last;
cl_M_o_vec.push_back(cl_M_o);
_cl_M_o_vec.push_back(cl_M_o);
int index_incoming = 0;
for (auto feat_incoming : _features_out_incoming)
{
std::string object_type_incoming = std::static_pointer_cast<FeatureObject>(feat_incoming)->getObjectType();
//Create a pair only if features have same types
if (object_type_last == object_type_incoming)
{
std::pair<int, int> pair_indexes = std::make_pair(index_last, index_incoming);
matches.push_back(pair_indexes);
all_matches.push_back(pair_indexes);
}
//camera to feat
//if first pass throught the loop
if (pushBack)
{
//camera to feat (incoming)
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);
_ci_M_o_vec.push_back(ci_M_o);
}
index_incoming++;
}
index_last++;
pushBack = false;
//Not first pass throught the loop
pushBack = false;
}
// 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, 0.10, inliers_idx, outliers_idx, best_model);
if (RANSACWorks)
{
std::cout << "RANSAC has worked" << std::endl;
//Keep only inliers
ProcessorTrackerLandmarkObject::filterMatchesOutliers(outliers_idx, matches);
}
return matches;
return all_matches;
}
......@@ -766,8 +774,10 @@ bool ProcessorTrackerLandmarkObject::matchingRANSAC(const std::vector<Eigen::Iso
int nb_inliers = _inliers_idx.size();
int nb_outliers = _outliers_idx.size();
if ((double)nb_outliers/nb_inliers > _ratio_outliers_inliers)
if ((double)nb_outliers/nb_inliers > _ratio_outliers_inliers) {
return true;
}
return true;
}
......
Supports Markdown
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