From 758a86dc1f6bdd9df1c5c736b5f3fb2d8fcc4f22 Mon Sep 17 00:00:00 2001
From: andreucm <acoromin@iri.upc.edu>
Date: Wed, 26 Oct 2016 14:59:35 +0200
Subject: [PATCH] FeatureMatchMap is now a pair of Ptr. Updated files using
 that. Check test_processor_tracker_landmark and
 test_processor_tracker_feature successfully, also Valgrind passed both tests
 with 0 errors

---
 src/feature_match.h                      | 3 ++-
 src/processor_image_feature.cpp          | 2 +-
 src/processor_tracker_feature.cpp        | 3 +--
 src/processor_tracker_feature.h          | 8 ++++----
 src/processor_tracker_feature_corner.cpp | 3 ++-
 src/processor_tracker_feature_dummy.cpp  | 4 +++-
 6 files changed, 13 insertions(+), 10 deletions(-)

diff --git a/src/feature_match.h b/src/feature_match.h
index 87119851d..b447adbb4 100644
--- a/src/feature_match.h
+++ b/src/feature_match.h
@@ -18,7 +18,8 @@ struct FeatureMatch
         Scalar normalized_score_;
 };
 
-typedef std::map<FeatureBasePtr, FeatureMatch> FeatureMatchMap;
+typedef std::shared_ptr<FeatureMatch> FeatureMatchPtr;
+typedef std::map<FeatureBasePtr, FeatureMatchPtr> FeatureMatchMap;
 
 }//end namespace
 
diff --git a/src/processor_image_feature.cpp b/src/processor_image_feature.cpp
index 18112eaca..68d2ac08e 100644
--- a/src/processor_image_feature.cpp
+++ b/src/processor_image_feature.cpp
@@ -144,7 +144,7 @@ unsigned int ProcessorImageFeature::trackFeatures(const FeatureBaseList& _featur
 
                 incoming_point_ptr->setTrackId(feature_ptr->trackId());
 
-                _feature_matches[incoming_point_ptr] = FeatureMatch({feature_base_ptr, normalized_score});
+                _feature_matches[incoming_point_ptr] = std::make_shared<FeatureMatch>(FeatureMatch({feature_base_ptr, normalized_score}));
             }
             else
             {
diff --git a/src/processor_tracker_feature.cpp b/src/processor_tracker_feature.cpp
index 953b6c1c6..235d6688a 100644
--- a/src/processor_tracker_feature.cpp
+++ b/src/processor_tracker_feature.cpp
@@ -39,8 +39,7 @@ unsigned int ProcessorTrackerFeature::processKnown()
         auto known_incoming_feature_it = known_features_incoming_.begin();
         while (known_incoming_feature_it != known_features_incoming_.end())
         {
-            if (!correctFeatureDrift(matches_origin_from_last_[matches_last_from_incoming_[*known_incoming_feature_it].feature_ptr_].feature_ptr_,
-                                     matches_last_from_incoming_[*known_incoming_feature_it].feature_ptr_,*known_incoming_feature_it))
+            if (!correctFeatureDrift(matches_origin_from_last_[matches_last_from_incoming_[*known_incoming_feature_it]->feature_ptr_]->feature_ptr_,                                   matches_last_from_incoming_[*known_incoming_feature_it]->feature_ptr_,*known_incoming_feature_it))
             {
                 // Correspondence not confirmed -> Remove correspondence and destruct incoming feature
                 matches_last_from_incoming_.erase(*known_incoming_feature_it);
diff --git a/src/processor_tracker_feature.h b/src/processor_tracker_feature.h
index a4614f104..84b252bde 100644
--- a/src/processor_tracker_feature.h
+++ b/src/processor_tracker_feature.h
@@ -174,10 +174,10 @@ inline void ProcessorTrackerFeature::establishConstraints()
 {
     for (auto match : matches_origin_from_last_)
     {
-        auto ctr = createConstraint(match.first, match.second.feature_ptr_);
-        ctr->setFeatureOtherPtr(match.second.feature_ptr_);
+        auto ctr = createConstraint(match.first, match.second->feature_ptr_);
+        ctr->setFeatureOtherPtr(match.second->feature_ptr_);
         match.first->addConstraint(ctr);
-        match.second.feature_ptr_->addConstrainedBy(ctr);
+        match.second->feature_ptr_->addConstrainedBy(ctr);
     }
 }
 
@@ -189,7 +189,7 @@ inline void ProcessorTrackerFeature::advance()
     for (auto match : matches_last_from_incoming_)
     {
         matches_last_from_incoming_[match.first] =
-                matches_origin_from_last_[matches_last_from_incoming_[match.first].feature_ptr_];
+                matches_origin_from_last_[matches_last_from_incoming_[match.first]->feature_ptr_];
     }
     matches_origin_from_last_ = std::move(matches_last_from_incoming_);
 
diff --git a/src/processor_tracker_feature_corner.cpp b/src/processor_tracker_feature_corner.cpp
index 9c80d497e..e7889ad63 100644
--- a/src/processor_tracker_feature_corner.cpp
+++ b/src/processor_tracker_feature_corner.cpp
@@ -64,7 +64,8 @@ unsigned int ProcessorTrackerFeatureCorner::trackFeatures(const FeatureBaseList&
             if (((*feat_out_it)->getMeasurement().head<3>() - expected_feature_pose).squaredNorm() > position_error_th_*position_error_th_)
             {
                 // match
-                _feature_correspondences[*feat_out_it] = FeatureMatch({feat_in_ptr,0});
+                //_feature_correspondences[*feat_out_it] = FeatureMatch({feat_in_ptr,0});
+                _feature_correspondences[*feat_out_it] = std::make_shared<FeatureMatch>(FeatureMatch({feat_in_ptr,0}));
                 
                 // move matched feature to list
                 _feature_list_out.splice(_feature_list_out.end(), corners_incoming_, feat_out_it);
diff --git a/src/processor_tracker_feature_dummy.cpp b/src/processor_tracker_feature_dummy.cpp
index 71decb6aa..0bcf1fd0f 100644
--- a/src/processor_tracker_feature_dummy.cpp
+++ b/src/processor_tracker_feature_dummy.cpp
@@ -29,7 +29,9 @@ unsigned int ProcessorTrackerFeatureDummy::trackFeatures(const FeatureBaseList&
         else
         {
             _feature_list_out.push_back(std::make_shared<FeatureBase>(FEATURE_POINT_IMAGE, "POINT IMAGE", feat_in_ptr->getMeasurement(), feat_in_ptr->getMeasurementCovariance()));
-            _feature_correspondences[_feature_list_out.back()] = FeatureMatch({feat_in_ptr,0});
+            //_feature_correspondences[_feature_list_out.back()] = FeatureMatch({feat_in_ptr,0});
+            //_feature_correspondences[_feature_list_out.back()] = std::make_shared<FeatureMatch>({feat_in_ptr,0});
+            _feature_correspondences[_feature_list_out.back()] = std::make_shared<FeatureMatch>(FeatureMatch({feat_in_ptr,0}));
             std::cout << "feature " << feat_in_ptr->getMeasurement() << " tracked!" << std::endl;
         }
     }
-- 
GitLab