From d230ff89c3640bd39a6b894c67169df1fa6758c8 Mon Sep 17 00:00:00 2001
From: ydepledt <yanndepledt360@gmail.com>
Date: Mon, 20 Jun 2022 10:17:38 +0200
Subject: [PATCH] Comments and make more readable nbOfDifferentMatches function
---
src/processor/processor_tracker_landmark_object.cpp | 10 ++++++++--
1 file changed, 8 insertions(+), 2 deletions(-)
diff --git a/src/processor/processor_tracker_landmark_object.cpp b/src/processor/processor_tracker_landmark_object.cpp
index 106f4a9..f507e3d 100644
--- a/src/processor/processor_tracker_landmark_object.cpp
+++ b/src/processor/processor_tracker_landmark_object.cpp
@@ -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();
--
GitLab