Skip to content
Snippets Groups Projects
Commit e965ba57 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

working in emplace-based processor tracker api

parent cd85660d
No related branches found
No related tags found
1 merge request!274Resolve "Emplace API inconsistent with ProcessorTrackerFeature/Landmark functions"
This commit is part of merge request !274. Comments created here will be created in the context of that merge request.
...@@ -113,19 +113,29 @@ class ProcessorTrackerFeature : public ProcessorTracker ...@@ -113,19 +113,29 @@ class ProcessorTrackerFeature : public ProcessorTracker
*/ */
virtual unsigned int processKnown(); virtual unsigned int processKnown();
/** \brief Track provided features from \b last to \b incoming /** \brief Track provided features in \b _capture
* \param _features_last_in input list of features in \b last to track * \param _features_in input list of features in \b last to track
* \param _features_incoming_out returned list of features found in \b incoming * \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 * \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. /** \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 _origin_feature input feature in origin capture tracked
* \param _incoming_feature input/output feature in incoming capture to be corrected * \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 * \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 /** \brief Vote for KeyFrame generation
* *
...@@ -147,25 +157,30 @@ class ProcessorTrackerFeature : public ProcessorTracker ...@@ -147,25 +157,30 @@ class ProcessorTrackerFeature : public ProcessorTracker
/** \brief Detect new Features /** \brief Detect new Features
* \param _max_features maximum number of features detected (-1: unlimited. 0: none) * \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. * \return The number of detected Features.
* *
* This function detects Features that do not correspond to known Features/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 ProcessorTrackerFeature::processNew() to set the member new_features_last_, * 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_. * 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_ptr pointer to the parent Feature
* \param _feature_other_ptr pointer to the other feature constrained. * \param _feature_other_ptr pointer to the other feature constrained.
* *
* Implement this method in derived classes. * 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 /** \brief Establish factors between features in Captures \b last and \b origin
*/ */
......
...@@ -105,14 +105,19 @@ class ProcessorTrackerLandmark : public ProcessorTracker ...@@ -105,14 +105,19 @@ class ProcessorTrackerLandmark : public ProcessorTracker
virtual unsigned int processKnown(); virtual unsigned int processKnown();
/** \brief Find provided landmarks in the incoming capture /** \brief Find provided landmarks in the incoming capture
* \param _landmarks_in input list of landmarks to be found in incoming * \param _landmarks_in input list of landmarks to be found
* \param _features_incoming_out returned list of incoming features corresponding to a landmark of _landmarks_in * \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 * \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 * \return the number of landmarks found
*/ */
virtual unsigned int findLandmarks(const LandmarkBasePtrList& _landmarks_in, virtual unsigned int findLandmarks(const LandmarkBasePtrList& _landmarks_in,
FeatureBasePtrList& _features_incoming_out, const CaptureBasePtr& _capture,
LandmarkMatchMap& _feature_landmark_correspondences) = 0; FeatureBasePtrList& _features_out,
LandmarkMatchMap& _feature_landmark_correspondences) = 0;
/** \brief Vote for KeyFrame generation /** \brief Vote for KeyFrame generation
* *
...@@ -134,33 +139,38 @@ class ProcessorTrackerLandmark : public ProcessorTracker ...@@ -134,33 +139,38 @@ class ProcessorTrackerLandmark : public ProcessorTracker
/** \brief Detect new Features /** \brief Detect new Features
* \param _max_features maximum number of features detected (-1: unlimited. 0: none) * \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. * \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 function is called in ProcessorTrackerLandmark::processNew() to set the member new_features_last_,
* the list of newly detected features of the capture last_ptr_. * 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. * 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 _feature_ptr pointer to the Feature to constrain
* \param _landmark_ptr LandmarkBase pointer to the Landmark constrained. * \param _landmark_ptr LandmarkBase pointer to the Landmark constrained.
* *
* Implement this method in derived classes. * 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 /** \brief Establish factors between features in Capture \b last and landmarks
*/ */
......
...@@ -36,25 +36,18 @@ unsigned int ProcessorTrackerFeature::processNew(const int& _max_new_features) ...@@ -36,25 +36,18 @@ unsigned int ProcessorTrackerFeature::processNew(const int& _max_new_features)
matches_last_from_incoming_.clear(); matches_last_from_incoming_.clear();
// Populate the last Capture with new Features. The result is in new_features_last_. // 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_) 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 // 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_) for (auto ftr : new_features_incoming_)
{ {
ftr->setProblem(this->getProblem()); assert(matches_last_from_incoming_.count(ftr) != 0);
SizeStd trk_id_from_last = matches_last_from_incoming_[ftr]->feature_ptr_->trackId(); track_matrix_.add(matches_last_from_incoming_[ftr]->feature_ptr_, ftr);
track_matrix_.add(trk_id_from_last, incoming_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 the number of new features detected in \b last
return n; return n;
} }
...@@ -70,16 +63,18 @@ unsigned int ProcessorTrackerFeature::processKnown() ...@@ -70,16 +63,18 @@ unsigned int ProcessorTrackerFeature::processKnown()
known_features_incoming_.clear(); known_features_incoming_.clear();
if (!last_ptr_ || last_ptr_->getFeatureList().empty()) if (!last_ptr_ || last_ptr_->getFeatureList().empty())
{
return 0; return 0;
}
// Track features from last_ptr_ to incoming_ptr_ // Track features from last_ptr_ to incoming_ptr_
trackFeatures(last_ptr_->getFeatureList(), known_features_incoming_, matches_last_from_incoming_); trackFeatures(last_ptr_->getFeatureList(),
for (auto match : matches_last_from_incoming_) 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(); assert(matches_last_from_incoming_.count(ftr) != 0);
track_matrix_.add(trk_id_from_last, incoming_ptr_, match.first); track_matrix_.add(matches_last_from_incoming_[ftr]->feature_ptr_, ftr);
} }
// Check/correct incoming-origin correspondences // Check/correct incoming-origin correspondences
...@@ -87,14 +82,10 @@ unsigned int ProcessorTrackerFeature::processKnown() ...@@ -87,14 +82,10 @@ unsigned int ProcessorTrackerFeature::processKnown()
{ {
for (auto feature_in_incoming : known_features_incoming_) 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_last = track_matrix_.feature(track_id, last_ptr_);
FeatureBasePtr feature_in_origin = track_matrix_.feature(track_id, origin_ptr_); FeatureBasePtr feature_in_origin = track_matrix_.feature(track_id, origin_ptr_);
if (correctFeatureDrift(feature_in_origin, feature_in_last, feature_in_incoming)) if (!correctFeatureDrift(feature_in_origin, feature_in_last, feature_in_incoming))
{
feature_in_incoming->setProblem(this->getProblem());
}
else
{ {
// Remove this feature from many places: // Remove this feature from many places:
matches_last_from_incoming_ .erase (feature_in_incoming); // remove match matches_last_from_incoming_ .erase (feature_in_incoming); // remove match
...@@ -105,9 +96,6 @@ unsigned int ProcessorTrackerFeature::processKnown() ...@@ -105,9 +96,6 @@ unsigned int ProcessorTrackerFeature::processKnown()
} }
} }
// Add to wolf tree
incoming_ptr_->addFeatureList(known_features_incoming_);
return matches_last_from_incoming_.size(); return matches_last_from_incoming_.size();
} }
...@@ -116,10 +104,6 @@ void ProcessorTrackerFeature::advanceDerived() ...@@ -116,10 +104,6 @@ void ProcessorTrackerFeature::advanceDerived()
// Reset here the list of correspondences. // Reset here the list of correspondences.
matches_last_from_incoming_.clear(); 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 // // remove last from track matrix in case you want to have only KF in the track matrix
// track_matrix_.remove(last_ptr_); // track_matrix_.remove(last_ptr_);
} }
...@@ -129,16 +113,10 @@ void ProcessorTrackerFeature::resetDerived() ...@@ -129,16 +113,10 @@ void ProcessorTrackerFeature::resetDerived()
// Reset here the list of correspondences. // Reset here the list of correspondences.
matches_last_from_incoming_.clear(); matches_last_from_incoming_.clear();
// Update features according to the move above. // Debug
TrackMatches matches_origin_last = track_matrix_.matches(origin_ptr_, last_ptr_); for (auto const & pair_trkid_pair : track_matrix_.matches(origin_ptr_, last_ptr_))
for (auto const & pair_trkid_pair : matches_origin_last)
{ {
FeatureBasePtr feature_in_origin = pair_trkid_pair.second.first; WOLF_DEBUG("Matches reset: track: ", pair_trkid_pair.first, " origin: ", pair_trkid_pair.second.first->id(), " last: ", pair_trkid_pair.second.second->id());
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());
} }
} }
...@@ -152,12 +130,11 @@ void ProcessorTrackerFeature::establishFactors() ...@@ -152,12 +130,11 @@ void ProcessorTrackerFeature::establishFactors()
FeatureBasePtr feature_in_last = pair_trkid_pair.second.second; FeatureBasePtr feature_in_last = pair_trkid_pair.second.second;
auto fac_ptr = createFactor(feature_in_last, feature_in_origin); auto fac_ptr = emplaceFactor(feature_in_last, feature_in_origin);
fac_ptr->link(feature_in_last);
WOLF_DEBUG( "Factor: track: " , feature_in_last->trackId(), WOLF_DEBUG( "Factor: track: " , feature_in_last->trackId(),
" origin: " , feature_in_origin->id() , " origin: " , feature_in_origin->id() ,
" from last: " , feature_in_last->id() ); " from last: " , feature_in_last->id() );
} }
} }
......
...@@ -63,9 +63,9 @@ unsigned int ProcessorTrackerLandmark::processKnown() ...@@ -63,9 +63,9 @@ unsigned int ProcessorTrackerLandmark::processKnown()
// Find landmarks in incoming_ptr_ // Find landmarks in incoming_ptr_
FeatureBasePtrList known_features_list_incoming; FeatureBasePtrList known_features_list_incoming;
unsigned int n = findLandmarks(getProblem()->getMap()->getLandmarkList(), unsigned int n = findLandmarks(getProblem()->getMap()->getLandmarkList(),
known_features_list_incoming, matches_landmark_from_incoming_); incoming_ptr_,
// Append found incoming features known_features_list_incoming,
incoming_ptr_->addFeatureList(known_features_list_incoming); matches_landmark_from_incoming_);
return n; return n;
} }
...@@ -89,31 +89,27 @@ unsigned int ProcessorTrackerLandmark::processNew(const int& _max_features) ...@@ -89,31 +89,27 @@ unsigned int ProcessorTrackerLandmark::processNew(const int& _max_features)
new_landmarks_.clear(); new_landmarks_.clear();
// We first need to populate the \b last Capture with new Features // 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 // create new landmarks with the new features discovered
createNewLandmarks(); emplaceNewLandmarks();
// Find the new landmarks in incoming_ptr_ (if it's not nullptr) // Find the new landmarks in incoming_ptr_ (if it's not nullptr)
if (incoming_ptr_ != nullptr) if (incoming_ptr_ != nullptr)
{ {
findLandmarks(new_landmarks_, new_features_incoming_, matches_landmark_from_incoming_); findLandmarks(new_landmarks_,
incoming_ptr_,
// Append all new Features to the incoming Capture's list of Features new_features_incoming_,
incoming_ptr_->addFeatureList(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 the number of new features detected in \b last
return n; return n;
} }
void ProcessorTrackerLandmark::createNewLandmarks() void ProcessorTrackerLandmark::emplaceNewLandmarks()
{ {
// First, make sure the list is empty and will only contain new lmks // First, make sure the list is empty and will only contain new lmks
new_landmarks_.clear(); new_landmarks_.clear();
...@@ -122,7 +118,7 @@ void ProcessorTrackerLandmark::createNewLandmarks() ...@@ -122,7 +118,7 @@ void ProcessorTrackerLandmark::createNewLandmarks()
for (auto new_feature_ptr : new_features_last_) for (auto new_feature_ptr : new_features_last_)
{ {
// create new landmark // 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); new_landmarks_.push_back(new_lmk_ptr);
...@@ -131,30 +127,13 @@ void ProcessorTrackerLandmark::createNewLandmarks() ...@@ -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() void ProcessorTrackerLandmark::establishFactors()
{ {
// Loop all features in last_ptr_ // Loop all features in last_ptr_
for (auto last_feature : last_ptr_->getFeatureList()) for (auto last_feature : last_ptr_->getFeatureList())
{ {
auto lmk = matches_landmark_from_last_[last_feature]->landmark_ptr_; assert(matches_landmark_from_last_.count(last_feature) == 1);
FactorBasePtr fac_ptr = createFactor(last_feature, FactorBasePtr fac_ptr = emplaceFactor(last_feature, matches_landmark_from_last_[last_feature]->landmark_ptr_);
lmk);
if (fac_ptr != nullptr) // factor links
{
fac_ptr->link(last_feature);
}
} }
} }
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment