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