diff --git a/include/core/processor/processor_tracker_feature.h b/include/core/processor/processor_tracker_feature.h
index 055e2ddf87220a65289c09e20746dd4da990541d..ed87f3ea76c252c8ba9f1a91bf1432f9365f1709 100644
--- a/include/core/processor/processor_tracker_feature.h
+++ b/include/core/processor/processor_tracker_feature.h
@@ -113,19 +113,29 @@ class ProcessorTrackerFeature : public ProcessorTracker
*/
virtual unsigned int processKnown();
- /** \brief Track provided features from \b last to \b incoming
- * \param _features_last_in input list of features in \b last to track
- * \param _features_incoming_out returned list of features found in \b incoming
+ /** \brief Track provided features in \b _capture
+ * \param _features_in input list of features in \b last to track
+ * \param _capture the capture in which the _features_in should be searched
+ * \param _features_out returned list of features found in \b _capture
* \param _feature_correspondences returned map of correspondences: _feature_correspondences[feature_out_ptr] = feature_in_ptr
+ *
+ * IMPORTANT: The features in _features_out should be emplaced. It means, they have to be already linked to the _capture
+ *
+ * \return the number of features tracked
*/
- virtual unsigned int trackFeatures(const FeatureBasePtrList& _features_last_in, FeatureBasePtrList& _features_incoming_out, FeatureMatchMap& _feature_correspondences) = 0;
+ virtual unsigned int trackFeatures(const FeatureBasePtrList& _features_in,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out,
+ FeatureMatchMap& _feature_correspondences) = 0;
/** \brief Correct the drift in incoming feature by re-comparing against the corresponding feature in origin.
* \param _origin_feature input feature in origin capture tracked
* \param _incoming_feature input/output feature in incoming capture to be corrected
* \return false if the the process discards the correspondence with origin's feature
*/
- virtual bool correctFeatureDrift(const FeatureBasePtr _origin_feature, const FeatureBasePtr _last_feature, FeatureBasePtr _incoming_feature) = 0;
+ virtual bool correctFeatureDrift(const FeatureBasePtr _origin_feature,
+ const FeatureBasePtr _last_feature,
+ FeatureBasePtr _incoming_feature) = 0;
/** \brief Vote for KeyFrame generation
*
@@ -147,25 +157,30 @@ class ProcessorTrackerFeature : public ProcessorTracker
/** \brief Detect new Features
* \param _max_features maximum number of features detected (-1: unlimited. 0: none)
- * \param _features_last_out The list of detected Features.
+ * \param _capture The capture in which the new features should be detected.
+ * \param _features_out The list of detected Features in _capture.
* \return The number of detected Features.
*
* This function detects Features that do not correspond to known Features/Landmarks in the system.
*
+ * IMPORTANT: The features in _features_out should be emplaced. It means, they have to be already linked to the _capture
+ *
* The function is called in ProcessorTrackerFeature::processNew() to set the member new_features_last_,
* the list of newly detected features of the capture last_ptr_.
*/
- virtual unsigned int detectNewFeatures(const int& _max_new_features, FeatureBasePtrList& _features_last_out) = 0;
+ virtual unsigned int detectNewFeatures(const int& _max_new_features,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out) = 0;
- /** \brief Create a new factor and link it to the wolf tree
+ /** \brief Emplaces a new factor
* \param _feature_ptr pointer to the parent Feature
* \param _feature_other_ptr pointer to the other feature constrained.
*
* Implement this method in derived classes.
*
- * This function creates a factor of the appropriate type for the derived processor.
+ * This function emplaces a factor of the appropriate type for the derived processor.
*/
- virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr) = 0;
+ virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr) = 0;
/** \brief Establish factors between features in Captures \b last and \b origin
*/
diff --git a/include/core/processor/processor_tracker_landmark.h b/include/core/processor/processor_tracker_landmark.h
index 0e7e2e0a2b7bac5161caa2d7969766a2b57f2581..db19472e0c18b3a7128472dfa00e8d152f6e9625 100644
--- a/include/core/processor/processor_tracker_landmark.h
+++ b/include/core/processor/processor_tracker_landmark.h
@@ -105,14 +105,19 @@ class ProcessorTrackerLandmark : public ProcessorTracker
virtual unsigned int processKnown();
/** \brief Find provided landmarks in the incoming capture
- * \param _landmarks_in input list of landmarks to be found in incoming
- * \param _features_incoming_out returned list of incoming features corresponding to a landmark of _landmarks_in
+ * \param _landmarks_in input list of landmarks to be found
+ * \param _capture the capture in which the _landmarks_in should be searched
+ * \param _features_out returned list of features found in \b _capture corresponding to a landmark of _landmarks_in
* \param _feature_landmark_correspondences returned map of landmark correspondences: _feature_landmark_correspondences[_feature_out_ptr] = landmark_in_ptr
+ *
+ * IMPORTANT: The features in _features_out should be emplaced. It means, they have to be already linked to the _capture
+ *
* \return the number of landmarks found
*/
- virtual unsigned int findLandmarks(const LandmarkBasePtrList& _landmarks_in,
- FeatureBasePtrList& _features_incoming_out,
- LandmarkMatchMap& _feature_landmark_correspondences) = 0;
+ virtual unsigned int findLandmarks(const LandmarkBasePtrList& _landmarks_in,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out,
+ LandmarkMatchMap& _feature_landmark_correspondences) = 0;
/** \brief Vote for KeyFrame generation
*
@@ -134,33 +139,38 @@ class ProcessorTrackerLandmark : public ProcessorTracker
/** \brief Detect new Features
* \param _max_features maximum number of features detected (-1: unlimited. 0: none)
- * \param _features_last_out The list of detected Features.
+ * \param _capture The capture in which the new features should be detected.
+ * \param _features_out The list of detected Features in _capture.
* \return The number of detected Features.
*
- * This function detects Features that do not correspond to known Landmarks in the system.
+ * This function detects Features that do not correspond to known Features/Landmarks in the system.
+ *
+ * IMPORTANT: The features in _features_out should be emplaced. It means, they have to be already linked to the _capture
*
* The function is called in ProcessorTrackerLandmark::processNew() to set the member new_features_last_,
* the list of newly detected features of the capture last_ptr_.
*/
- virtual unsigned int detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_incoming_out) = 0;
+ virtual unsigned int detectNewFeatures(const int& _max_new_features,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out) = 0;
- /** \brief Creates a landmark for each of new_features_last_
+ /** \brief Emplace a landmark for each of new_features_last_
**/
- virtual void createNewLandmarks();
+ virtual void emplaceNewLandmarks();
- /** \brief Create one landmark
+ /** \brief Emplace one landmark
*
* Implement in derived classes to build the type of landmark you need for this tracker.
*/
- virtual LandmarkBasePtr createLandmark(FeatureBasePtr _feature_ptr) = 0;
+ virtual LandmarkBasePtr emplaceLandmark(FeatureBasePtr _feature_ptr) = 0;
- /** \brief Create a new factor
+ /** \brief Emplace a new factor
* \param _feature_ptr pointer to the Feature to constrain
* \param _landmark_ptr LandmarkBase pointer to the Landmark constrained.
*
* Implement this method in derived classes.
*/
- virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr) = 0;
+ virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr) = 0;
/** \brief Establish factors between features in Capture \b last and landmarks
*/
diff --git a/src/processor/processor_tracker_feature.cpp b/src/processor/processor_tracker_feature.cpp
index dbcc3646b0954a4e34da071ba7b34c878d79e265..9dae609e87721ddbf7dfa717b844981d684d98dd 100644
--- a/src/processor/processor_tracker_feature.cpp
+++ b/src/processor/processor_tracker_feature.cpp
@@ -36,25 +36,18 @@ unsigned int ProcessorTrackerFeature::processNew(const int& _max_new_features)
matches_last_from_incoming_.clear();
// Populate the last Capture with new Features. The result is in new_features_last_.
- unsigned int n = detectNewFeatures(_max_new_features, new_features_last_);
+ unsigned int n = detectNewFeatures(_max_new_features, last_ptr_, new_features_last_);
for (auto ftr : new_features_last_)
- track_matrix_.newTrack(last_ptr_, ftr);
+ track_matrix_.newTrack(ftr);
// 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_);
+ trackFeatures(new_features_last_, incoming_ptr_, new_features_incoming_, matches_last_from_incoming_);
for (auto ftr : new_features_incoming_)
{
- ftr->setProblem(this->getProblem());
- SizeStd trk_id_from_last = matches_last_from_incoming_[ftr]->feature_ptr_->trackId();
- track_matrix_.add(trk_id_from_last, incoming_ptr_, ftr);
+ assert(matches_last_from_incoming_.count(ftr) != 0);
+ track_matrix_.add(matches_last_from_incoming_[ftr]->feature_ptr_, ftr);
}
- // 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;
}
@@ -70,16 +63,18 @@ unsigned int ProcessorTrackerFeature::processKnown()
known_features_incoming_.clear();
if (!last_ptr_ || last_ptr_->getFeatureList().empty())
- {
return 0;
- }
// 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_)
+ trackFeatures(last_ptr_->getFeatureList(),
+ incoming_ptr_,
+ known_features_incoming_,
+ matches_last_from_incoming_);
+
+ for (auto ftr : known_features_incoming_)
{
- SizeStd trk_id_from_last = match.second->feature_ptr_->trackId();
- track_matrix_.add(trk_id_from_last, incoming_ptr_, match.first);
+ assert(matches_last_from_incoming_.count(ftr) != 0);
+ track_matrix_.add(matches_last_from_incoming_[ftr]->feature_ptr_, ftr);
}
// Check/correct incoming-origin correspondences
@@ -87,14 +82,10 @@ unsigned int ProcessorTrackerFeature::processKnown()
{
for (auto feature_in_incoming : known_features_incoming_)
{
- SizeStd track_id = feature_in_incoming->trackId();
+ SizeStd track_id = feature_in_incoming->trackId();
FeatureBasePtr feature_in_last = track_matrix_.feature(track_id, last_ptr_);
FeatureBasePtr feature_in_origin = track_matrix_.feature(track_id, origin_ptr_);
- if (correctFeatureDrift(feature_in_origin, feature_in_last, feature_in_incoming))
- {
- feature_in_incoming->setProblem(this->getProblem());
- }
- else
+ if (!correctFeatureDrift(feature_in_origin, feature_in_last, feature_in_incoming))
{
// Remove this feature from many places:
matches_last_from_incoming_ .erase (feature_in_incoming); // remove match
@@ -105,9 +96,6 @@ unsigned int ProcessorTrackerFeature::processKnown()
}
}
- // Add to wolf tree
- incoming_ptr_->addFeatureList(known_features_incoming_);
-
return matches_last_from_incoming_.size();
}
@@ -116,10 +104,6 @@ void ProcessorTrackerFeature::advanceDerived()
// Reset here the list of correspondences.
matches_last_from_incoming_.clear();
- // We set problem here because we could not determine Problem from incoming capture at the time of adding the features to incoming's feature list.
- for (auto ftr : incoming_ptr_->getFeatureList())
- ftr->setProblem(getProblem());
-
// // remove last from track matrix in case you want to have only KF in the track matrix
// track_matrix_.remove(last_ptr_);
}
@@ -129,16 +113,10 @@ void ProcessorTrackerFeature::resetDerived()
// Reset here the list of correspondences.
matches_last_from_incoming_.clear();
- // Update features according to the move above.
- TrackMatches matches_origin_last = track_matrix_.matches(origin_ptr_, last_ptr_);
-
- for (auto const & pair_trkid_pair : matches_origin_last)
+ // Debug
+ for (auto const & pair_trkid_pair : track_matrix_.matches(origin_ptr_, last_ptr_))
{
- FeatureBasePtr feature_in_origin = pair_trkid_pair.second.first;
- FeatureBasePtr feature_in_last = pair_trkid_pair.second.second;
- feature_in_last->setProblem(getProblem()); // Since these features were in incoming_, they had no Problem assigned.
-
- WOLF_DEBUG("Matches reset: track: ", pair_trkid_pair.first, " origin: ", feature_in_origin->id(), " last: ", feature_in_last->id());
+ WOLF_DEBUG("Matches reset: track: ", pair_trkid_pair.first, " origin: ", pair_trkid_pair.second.first->id(), " last: ", pair_trkid_pair.second.second->id());
}
}
@@ -152,12 +130,11 @@ void ProcessorTrackerFeature::establishFactors()
FeatureBasePtr feature_in_last = pair_trkid_pair.second.second;
- auto fac_ptr = createFactor(feature_in_last, feature_in_origin);
- fac_ptr->link(feature_in_last);
+ auto fac_ptr = emplaceFactor(feature_in_last, feature_in_origin);
WOLF_DEBUG( "Factor: track: " , feature_in_last->trackId(),
- " origin: " , feature_in_origin->id() ,
- " from last: " , feature_in_last->id() );
+ " origin: " , feature_in_origin->id() ,
+ " from last: " , feature_in_last->id() );
}
}
diff --git a/src/processor/processor_tracker_landmark.cpp b/src/processor/processor_tracker_landmark.cpp
index 1706985724d3d61d388a2d70e83fb87c12483a4f..377a99c5665869b58c8942ba15c61472fb467fdb 100644
--- a/src/processor/processor_tracker_landmark.cpp
+++ b/src/processor/processor_tracker_landmark.cpp
@@ -63,9 +63,9 @@ unsigned int ProcessorTrackerLandmark::processKnown()
// Find landmarks in incoming_ptr_
FeatureBasePtrList known_features_list_incoming;
unsigned int n = findLandmarks(getProblem()->getMap()->getLandmarkList(),
- known_features_list_incoming, matches_landmark_from_incoming_);
- // Append found incoming features
- incoming_ptr_->addFeatureList(known_features_list_incoming);
+ incoming_ptr_,
+ known_features_list_incoming,
+ matches_landmark_from_incoming_);
return n;
}
@@ -89,31 +89,27 @@ unsigned int ProcessorTrackerLandmark::processNew(const int& _max_features)
new_landmarks_.clear();
// We first need to populate the \b last Capture with new Features
- unsigned int n = detectNewFeatures(_max_features, new_features_last_);
+ unsigned int n = detectNewFeatures(_max_features,
+ last_ptr_,
+ new_features_last_);
// create new landmarks with the new features discovered
- createNewLandmarks();
+ emplaceNewLandmarks();
// Find the new landmarks in incoming_ptr_ (if it's not nullptr)
if (incoming_ptr_ != nullptr)
{
- findLandmarks(new_landmarks_, new_features_incoming_, matches_landmark_from_incoming_);
-
- // Append all new Features to the incoming Capture's list of Features
- incoming_ptr_->addFeatureList(new_features_incoming_);
+ findLandmarks(new_landmarks_,
+ incoming_ptr_,
+ new_features_incoming_,
+ matches_landmark_from_incoming_);
}
- // Append all new Features to the last Capture's list of Features
- last_ptr_->addFeatureList(new_features_last_);
-
- // Append new landmarks to the map
- getProblem()->addLandmarkList(new_landmarks_);
-
// return the number of new features detected in \b last
return n;
}
-void ProcessorTrackerLandmark::createNewLandmarks()
+void ProcessorTrackerLandmark::emplaceNewLandmarks()
{
// First, make sure the list is empty and will only contain new lmks
new_landmarks_.clear();
@@ -122,7 +118,7 @@ void ProcessorTrackerLandmark::createNewLandmarks()
for (auto new_feature_ptr : new_features_last_)
{
// create new landmark
- LandmarkBasePtr new_lmk_ptr = createLandmark(new_feature_ptr);
+ LandmarkBasePtr new_lmk_ptr = emplaceLandmark(new_feature_ptr);
new_landmarks_.push_back(new_lmk_ptr);
@@ -131,30 +127,13 @@ void ProcessorTrackerLandmark::createNewLandmarks()
}
}
-// unsigned int ProcessorTrackerLandmark::processKnown()
-// {
-// // Find landmarks in incoming_ptr_
-// FeatureBasePtrList known_features_list_incoming;
-// unsigned int n = findLandmarks(getProblem()->getMap()->getLandmarkList(),
-// known_features_list_incoming, matches_landmark_from_incoming_);
-// // Append found incoming features
-// incoming_ptr_->addFeatureList(known_features_list_incoming);
-
-// return n;
-// }
-
void ProcessorTrackerLandmark::establishFactors()
{
// Loop all features in last_ptr_
for (auto last_feature : last_ptr_->getFeatureList())
{
- auto lmk = matches_landmark_from_last_[last_feature]->landmark_ptr_;
- FactorBasePtr fac_ptr = createFactor(last_feature,
- lmk);
- if (fac_ptr != nullptr) // factor links
- {
- fac_ptr->link(last_feature);
- }
+ assert(matches_landmark_from_last_.count(last_feature) == 1);
+ FactorBasePtr fac_ptr = emplaceFactor(last_feature, matches_landmark_from_last_[last_feature]->landmark_ptr_);
}
}