Update trackId in ProcessorTrackerFeature

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();
 }

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_)

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;