diff --git a/src/feature_match.h b/src/feature_match.h index 87119851d38dd8658364b241bf162c3fdf8730ff..b447adbb4d73eb3deec2feb676cbb47b287dde81 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 18112eaca27cfc58c4f211237ac8abedd4abe28c..68d2ac08e8fe68c5ed0f4321a51b43ff70494f39 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 953b6c1c6ce911152e5b55fd386d191d1995ad46..235d6688a91c8ce734b551dd2c2f47c5e078cf7a 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 a4614f10455406d84df5a7f711010bc8abca6c78..84b252bde3ab147b66b2969a3d0e8c6fcddf926e 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 9c80d497e6a017775a9f74cb00eb76476bb90f95..e7889ad636537c0158a83bc0f67015c9ef6f3398 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 71decb6aa29225cba6aeec4851d6990c03e5ba32..0bcf1fd0f63c0edb43730015c8ac4a3673b347d8 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; } }