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

Comments and make more readable nbOfDifferentMatches function

parent e1e52e3c
No related branches found
No related tags found
No related merge requests found
......@@ -657,10 +657,16 @@ int ProcessorTrackerLandmarkObject::nbOfDifferentMatches(const std::vector<std::
for (auto match : matches)
{
//Nb of occurences of indexes
int index_last_feat_occurrence = std::count(index_last_feat.begin(), index_last_feat.end(), match.first);
int index_incoming_feat_occurrence = std::count(index_incoming_feat.begin(), index_incoming_feat.end(), match.second);
//Check if the pair has unique indexes
if(!(std::count(index_last_feat.begin(), index_last_feat.end(), match.first)) && !(std::count(index_incoming_feat.begin(), index_incoming_feat.end(), match.second)))
if(index_last_feat_occurrence == 0 && index_incoming_feat_occurrence == 0)
{
nb_of_diff_matches++;
//Add unique indexes in vector
index_last_feat.push_back(match.first);
index_incoming_feat.push_back(match.second);
}
......@@ -689,7 +695,7 @@ bool ProcessorTrackerLandmarkObject::isInliers(Eigen::Isometry3d cl_M_ol, Eigen:
Eigen::Matrix3d wRf_i = identity.linear();
quat_feat_identity.coeffs() = R2q(wRf_i).coeffs().transpose();
Vector3d pos_feat_identity = identity.translation();
// Error between identity and ol_M_oi
double e_pos = (pos_feat_identity - pos_feat).norm();
double e_rot = log_q(quat_feat_identity.conjugate() * quat_feat).norm();
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment