diff --git a/src/processor_image_feature.cpp b/src/processor_image_feature.cpp
index 0fe5993ca0076ad5cb750ee4f2fa6865e3c63d6a..c728686d73a4d9ce85ef5c61cf6dfd381522a1b6 100644
--- a/src/processor_image_feature.cpp
+++ b/src/processor_image_feature.cpp
@@ -214,8 +214,6 @@ unsigned int ProcessorImageFeature::trackFeatures(const FeatureBaseList& _featur
incoming_point_ptr->setIsKnown(feature_ptr->isKnown());
_feature_list_out.push_back(incoming_point_ptr);
- incoming_point_ptr->setTrackId(feature_ptr->trackId());
-
_feature_matches[incoming_point_ptr] = std::make_shared<FeatureMatch>(FeatureMatch({feature_base_ptr, normalized_score}));
}
else
@@ -321,7 +319,6 @@ unsigned int ProcessorImageFeature::detectNewFeatures(const unsigned int& _max_n
new_descriptors.row(index),
Eigen::Matrix2s::Identity()*params_.noise.pixel_noise_var);
point_ptr->setIsKnown(false);
- point_ptr->setTrackId(point_ptr->id());
addNewFeatureLast(point_ptr);
active_search_grid_.hitCell(new_keypoints[0]);
@@ -369,10 +366,10 @@ unsigned int ProcessorImageFeature::detect(cv::Mat _image, cv::Rect& _roi, std::
detector_descriptor_ptr_->detect(_image_roi, _new_keypoints);
detector_descriptor_ptr_->compute(_image_roi, _new_keypoints, new_descriptors);
- for (unsigned int i = 0; i < _new_keypoints.size(); i++)
+ for (auto point : _new_keypoints)
{
- _new_keypoints[i].pt.x = _new_keypoints[i].pt.x + _roi.x;
- _new_keypoints[i].pt.y = _new_keypoints[i].pt.y + _roi.y;
+ point.pt.x += _roi.x;
+ point.pt.y += _roi.y;
}
return _new_keypoints.size();
}
diff --git a/src/processor_tracker_feature.cpp b/src/processor_tracker_feature.cpp
index a073aa5f4183ece803b673ce360684d558a59246..59ee59659227c8ca889d68f3a776744e30b95223 100644
--- a/src/processor_tracker_feature.cpp
+++ b/src/processor_tracker_feature.cpp
@@ -19,6 +19,41 @@ ProcessorTrackerFeature::~ProcessorTrackerFeature()
{
}
+unsigned int ProcessorTrackerFeature::processNew(const unsigned int& _max_new_features)
+{
+ /* Rationale: A keyFrame will be created using the last Capture.
+ * First, we work on the last Capture to detect new Features,
+ * When done, we need to track these new Features to the incoming Capture.
+ * At the end, all new Features are appended to the lists of known Features in
+ * the last and incoming Captures.
+ */
+
+ // Populate the last Capture with new Features. The result is in new_features_last_.
+ unsigned int n = detectNewFeatures(_max_new_features);
+ for (auto ftr : new_features_last_)
+ {
+ ftr->setTrackId( ftr->id() );
+ WOLF_INFO("New ftr track: ", ftr->trackId(), ", last: ", ftr->id());
+ }
+
+ // Track new features from last to incoming. This will append new correspondences to matches_last_incoming
+ trackFeatures(new_features_last_, new_features_incoming_, matches_last_from_incoming_);
+ for (auto match : matches_last_from_incoming_)
+ {
+ match.first->setTrackId( match.second->feature_ptr_->trackId() );
+ WOLF_INFO("New ftr track: ", match.first->trackId(), ", last: ", match.second->feature_ptr_->id(), ", inc: ", match.first->id());
+ }
+
+ // Append all new Features to the incoming Captures' list of Features
+ incoming_ptr_->addFeatureList(new_features_incoming_);
+
+ // Append all new Features to the last Captures' list of Features
+ last_ptr_->addFeatureList(new_features_last_);
+
+ // return the number of new features detected in \b last
+ return n;
+}
+
unsigned int ProcessorTrackerFeature::processKnown()
{
@@ -31,6 +66,11 @@ unsigned int ProcessorTrackerFeature::processKnown()
// Track features from last_ptr_ to incoming_ptr_
trackFeatures(last_ptr_->getFeatureList(), known_features_incoming_, matches_last_from_incoming_);
+ for (auto match : matches_last_from_incoming_)
+ {
+ match.first->setTrackId( match.second->feature_ptr_->trackId() );
+ WOLF_INFO("Known ftr track: ", match.first->trackId(), ", last: ", match.second->feature_ptr_->id(), ", inc: ", match.first->id());
+ }
// std::cout << "Tracked: " << known_features_incoming_.size() << std::endl;
@@ -53,10 +93,10 @@ unsigned int ProcessorTrackerFeature::processKnown()
known_incoming_feature_it++;
}
}
- std::cout << "known features incoming: ";
- for (auto ftr:known_features_incoming_)
- std::cout << ftr->id() << ", ";
- std::cout << std::endl;
+ // std::cout << "known features incoming: ";
+ // for (auto ftr:known_features_incoming_)
+ // std::cout << ftr->id() << ", ";
+ // std::cout << std::endl;
// Append remaining incoming features -> this empties known_features_incoming_
incoming_ptr_->addFeatureList(known_features_incoming_);
known_features_incoming_.clear();
@@ -92,7 +132,6 @@ void ProcessorTrackerFeature::advanceDerived()
void ProcessorTrackerFeature::resetDerived()
{
- // std::cout << "ProcessorTrackerFeature::reset()" << std::endl;
// We also reset here the list of correspondences, which passes from last--incoming to origin--last.
std::cout << "\tincoming 2 last: " << matches_last_from_incoming_.size() << std::endl;
@@ -110,31 +149,6 @@ void ProcessorTrackerFeature::resetDerived()
}
-unsigned int ProcessorTrackerFeature::processNew(const unsigned int& _max_new_features)
-{
- /* Rationale: A keyFrame will be created using the last Capture.
- * First, we work on the last Capture to detect new Features,
- * When done, we need to track these new Features to the incoming Capture.
- * At the end, all new Features are appended to the lists of known Features in
- * the last and incoming Captures.
- */
-
- // Populate the last Capture with new Features. The result is in new_features_last_.
- unsigned int n = detectNewFeatures(_max_new_features);
-
- // Track new features from last to incoming. This will append new correspondences to matches_last_incoming
- trackFeatures(new_features_last_, new_features_incoming_, matches_last_from_incoming_);
-
- // Append all new Features to the incoming Captures' list of Features
- incoming_ptr_->addFeatureList(new_features_incoming_);
-
- // Append all new Features to the last Captures' list of Features
- last_ptr_->addFeatureList(new_features_last_);
-
- // return the number of new features detected in \b last
- return n;
-}
-
void ProcessorTrackerFeature::establishConstraints()
{
for (auto match : matches_origin_from_last_)
diff --git a/src/processor_tracker_feature_dummy.cpp b/src/processor_tracker_feature_dummy.cpp
index 5fe2ba1d18efbb24977c003a2d8de792e46ef46b..4dc8bd1199d35639bbd49b26b42c3c34da8a7dc5 100644
--- a/src/processor_tracker_feature_dummy.cpp
+++ b/src/processor_tracker_feature_dummy.cpp
@@ -17,8 +17,6 @@ unsigned int ProcessorTrackerFeatureDummy::trackFeatures(const FeatureBaseList&
{
std::cout << "tracking " << _feature_list_in.size() << " features..." << std::endl;
- WOLF_TRACE("List sizes: in: ", _feature_list_in.size(), "; out: ", _feature_list_out.size(), "; correspondences: ", _feature_correspondences.size());
-
// loosing the track of the first 2 features
auto features_lost = 0;
for (auto feat_in_ptr : _feature_list_in)
@@ -32,7 +30,6 @@ unsigned int ProcessorTrackerFeatureDummy::trackFeatures(const FeatureBaseList&
else
{
FeatureBasePtr ftr(std::make_shared<FeatureBase>("POINT IMAGE", feat_in_ptr->getMeasurement(), feat_in_ptr->getMeasurementCovariance()));
- ftr->setTrackId(feat_in_ptr->id());
_feature_list_out.push_back(ftr);
_feature_correspondences[_feature_list_out.back()] = std::make_shared<FeatureMatch>(FeatureMatch({feat_in_ptr,0}));
std::cout << "feature " << feat_in_ptr->id() << " tracked to feature " << ftr->id() << " !" << std::endl;
@@ -62,7 +59,6 @@ unsigned int ProcessorTrackerFeatureDummy::detectNewFeatures(const unsigned int&
FeatureBasePtr ftr(
std::make_shared<FeatureBase>("POINT IMAGE", n_feature_* Eigen::Vector1s::Ones(), Eigen::MatrixXs::Ones(1, 1)));
new_features_last_.push_back(ftr);
- ftr->setTrackId(ftr->id());
std::cout << "feature " << ftr->id() << " detected!" << std::endl;
}
std::cout << new_features_last_.size() << " features detected!" << std::endl;