-
Notifications
You must be signed in to change notification settings - Fork 3
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
Showing
10 changed files
with
336 additions
and
58 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
105 changes: 105 additions & 0 deletions
105
src/DeltaPoseReconstruction/include/Vocpp_DeltaPoseReconstruction/FeatureTracker.h
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,105 @@ | ||
| /* This file is part of the Visual-Odometry-Cpp project. | ||
| * It is subject to the license terms in the LICENSE file | ||
| * found in the top-level directory of this distribution. | ||
| * | ||
| * Copyright (C) 2020 Manuel Kraus | ||
| */ | ||
|
|
||
| #ifndef VOCPP_FEATURE_TRACKER_H | ||
| #define VOCPP_FEATURE_TRACKER_H | ||
|
|
||
| #include <Vocpp_FeatureHandling/LshMatcher.h> | ||
|
|
||
| #include <map> | ||
|
|
||
| namespace VOCPP | ||
| { | ||
| namespace DeltaPoseReconstruction | ||
| { | ||
|
|
||
| class TrackedFeature | ||
| { | ||
| public: | ||
| TrackedFeature(unsigned int in_currentFrameId, unsigned int in_currentFeatureId, const FeatureHandling::BinaryFeatureDescription& in_currentDesc) : | ||
| m_frameIdVsFeatureId(), | ||
| m_frameIdVsDescription() | ||
| { | ||
| Update(in_currentFrameId, in_currentFeatureId, in_currentDesc); | ||
| } | ||
|
|
||
| bool IsPresentInFrame(unsigned int in_frameId) const | ||
| { | ||
| return m_frameIdVsFeatureId.count(in_frameId) > 0 ? true : false; | ||
| } | ||
|
|
||
| bool IsPresentInFrameWithFeatureId(const unsigned int& in_frameId, const unsigned int& in_featureId) const | ||
| { | ||
| bool ret = m_frameIdVsFeatureId.count(in_frameId) > 0 ? true : false; | ||
|
|
||
| if (ret) | ||
| { | ||
| ret = m_frameIdVsFeatureId.at(in_frameId) == in_featureId ? true : false; | ||
| } | ||
|
|
||
| return ret; | ||
| } | ||
|
|
||
| void Update(unsigned int in_currentFrameId, unsigned int in_currentFeatureId, const FeatureHandling::BinaryFeatureDescription& in_currentDesc) | ||
| { | ||
| // Only add occurence in current frame (should be present in last one already) | ||
| m_frameIdVsFeatureId.insert(std::pair<int, int>(in_currentFrameId, in_currentFeatureId)); | ||
| m_frameIdVsDescription.insert(std::pair<int, FeatureHandling::BinaryFeatureDescription>(in_currentFrameId, in_currentDesc)); | ||
| m_lastSeenFrameId = in_currentFrameId; | ||
| } | ||
|
|
||
| unsigned int m_lastSeenFrameId; | ||
| std::map<int, int> m_frameIdVsFeatureId; | ||
| std::map<int, FeatureHandling::BinaryFeatureDescription> m_frameIdVsDescription; | ||
| }; | ||
|
|
||
| struct TrackedFeatureSet | ||
| { | ||
| std::vector<TrackedFeature> features; | ||
| bool isKeySet; | ||
| }; | ||
|
|
||
| class FeatureTracker | ||
| { | ||
| public: | ||
|
|
||
| FeatureTracker(unsigned int in_minTrackedTwoFrames=100U, unsigned int in_minTrackedThreeFrames=10U) | ||
| : m_matcher(), | ||
| m_currentKeyFeatureSet(), | ||
| m_lastKeyFrameId(s_invalidFrameId), | ||
| m_KeyFrameIdBeforeLast(s_invalidFrameId), | ||
| m_lastKeyFrameDescriptions(), | ||
| m_minTrackedTwoFrames(in_minTrackedTwoFrames), | ||
| m_minTrackedThreeFrames(in_minTrackedThreeFrames), | ||
| m_trackingStatus(TrackingStatus::Lost) | ||
| { | ||
| } | ||
|
|
||
| TrackedFeatureSet ProvideDescriptionSet(unsigned int in_currentFrameId, std::vector<VOCPP::FeatureHandling::BinaryFeatureDescription>& in_currentDesc); | ||
|
|
||
| private: | ||
|
|
||
| FeatureHandling::LshMatcher m_matcher; | ||
| TrackedFeatureSet m_currentKeyFeatureSet; | ||
|
|
||
| unsigned int m_lastKeyFrameId; | ||
| unsigned int m_KeyFrameIdBeforeLast; | ||
| std::vector<FeatureHandling::BinaryFeatureDescription> m_lastKeyFrameDescriptions; | ||
| const unsigned int m_minTrackedTwoFrames; | ||
| const unsigned int m_minTrackedThreeFrames; | ||
|
|
||
| enum TrackingStatus | ||
| { | ||
| Active = 0U, | ||
| Lost = 1U | ||
| } m_trackingStatus; | ||
| }; | ||
|
|
||
| } //namespace DeltaPoseReconstruction | ||
| } //namespace VOCPP | ||
|
|
||
| #endif /* VOCPP_FEATURE_TRACKER_H */ |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,115 @@ | ||
| /* This file is part of the Visual-Odometry-Cpp project. | ||
| * It is subject to the license terms in the LICENSE file | ||
| * found in the top-level directory of this distribution. | ||
| * | ||
| * Copyright (C) 2020 Manuel Kraus | ||
| */ | ||
|
|
||
| #include <Vocpp_DeltaPoseReconstruction/FeatureTracker.h> | ||
|
|
||
| namespace | ||
| { | ||
|
|
||
| void RemoveUntrackedFeatures(VOCPP::DeltaPoseReconstruction::TrackedFeatureSet& featSet, const unsigned int& in_currentFrameId) | ||
| { | ||
| auto newEnd = std::remove_if(featSet.features.begin(), featSet.features.end(), | ||
| [&in_currentFrameId](const VOCPP::DeltaPoseReconstruction::TrackedFeature& i) {return i.m_lastSeenFrameId < in_currentFrameId; }); | ||
| featSet.features.erase(newEnd, featSet.features.end()); | ||
| }; | ||
|
|
||
| } | ||
|
|
||
| namespace VOCPP | ||
| { | ||
| namespace DeltaPoseReconstruction | ||
| { | ||
|
|
||
| TrackedFeatureSet FeatureTracker::ProvideDescriptionSet(unsigned int in_currentFrameId, std::vector<VOCPP::FeatureHandling::BinaryFeatureDescription>& in_currentDesc) | ||
| { | ||
| TrackedFeatureSet featureSet{ std::vector<TrackedFeature>(), false }; | ||
|
|
||
| if (m_trackingStatus == TrackingStatus::Lost) | ||
| { | ||
| for (auto desc : in_currentDesc) | ||
| { | ||
| featureSet.features.push_back(TrackedFeature(desc.GetFeature().frameId, desc.GetFeature().id, desc)); | ||
| } | ||
|
|
||
| // No valid last key set | ||
| featureSet.isKeySet = true; | ||
| m_currentKeyFeatureSet = featureSet; | ||
| m_lastKeyFrameId = in_currentDesc.size() > 0U ? in_currentDesc[0].GetFeature().frameId : s_invalidFrameId; | ||
| m_lastKeyFrameDescriptions = in_currentDesc; | ||
| m_trackingStatus = in_currentDesc.size() > 0U ? TrackingStatus::Active : TrackingStatus::Lost; | ||
| std::cout << "Lost" << std::endl; | ||
| } | ||
| else | ||
| { | ||
| std::cout << "Active" << std::endl; | ||
| // Here we should have at least descriptions for the last keyFrame | ||
| std::vector<FeatureHandling::BinaryDescriptionMatch> matches; | ||
| bool ret = m_matcher.MatchDesriptions(in_currentDesc, m_lastKeyFrameDescriptions, matches); | ||
|
|
||
| // First copy the key set history to the new feature set | ||
| featureSet = m_currentKeyFeatureSet; | ||
|
|
||
| TrackedFeatureSet missingFeatureSet{ std::vector<TrackedFeature>(), false }; | ||
|
|
||
| for (unsigned int it = 0U; it < matches.size(); it++) | ||
| { | ||
| // Check whether this feature has been observed before | ||
| bool found = false; | ||
| for (auto& feat : featureSet.features) | ||
| { | ||
| // We assume here that first frame == current frame and second frame == last frame | ||
| if (feat.IsPresentInFrameWithFeatureId(matches[it].GetSecondFeature().frameId, matches[it].GetSecondFeature().id)) | ||
| { | ||
| feat.Update(matches[it].GetFirstFeature().frameId, matches[it].GetFirstFeature().id, matches[it].GetFirstDescription()); | ||
| found = true; | ||
| } | ||
| } | ||
|
|
||
| // If feature has not been found --> store them for later when we decide whether this will be a key set | ||
| if (!found) | ||
| { | ||
| missingFeatureSet.features.push_back(TrackedFeature(matches[it].GetFirstFeature().frameId, matches[it].GetFirstFeature().id, matches[it].GetFirstDescription())); | ||
| } | ||
| } | ||
| RemoveUntrackedFeatures(featureSet, in_currentFrameId); | ||
|
|
||
| const unsigned int numMatchesTrackedTwoKeyFrames = matches.size(); | ||
| unsigned int numMatchesTrackedThreeKeyFrames = 0U; | ||
| for (auto feat : m_currentKeyFeatureSet.features) | ||
| { | ||
| if (feat.IsPresentInFrame(m_KeyFrameIdBeforeLast)) | ||
| { | ||
| numMatchesTrackedThreeKeyFrames++; | ||
| } | ||
| } | ||
| std::cout << numMatchesTrackedTwoKeyFrames << " " << numMatchesTrackedThreeKeyFrames << " "<<in_currentFrameId<<" "<< m_KeyFrameIdBeforeLast<< std::endl; | ||
| if (numMatchesTrackedTwoKeyFrames < m_minTrackedTwoFrames || numMatchesTrackedThreeKeyFrames < m_minTrackedThreeFrames) | ||
| { | ||
| featureSet.isKeySet = true; | ||
| for (auto feat : missingFeatureSet.features) | ||
| { | ||
| featureSet.features.push_back(feat); | ||
| } | ||
|
|
||
| m_currentKeyFeatureSet = featureSet; | ||
| m_lastKeyFrameDescriptions = in_currentDesc; | ||
| m_KeyFrameIdBeforeLast = m_lastKeyFrameId; | ||
| m_lastKeyFrameId = in_currentFrameId; | ||
| std::cout << "Keyframe " << std::endl; | ||
| } | ||
| else | ||
| { | ||
| featureSet.isKeySet = false; | ||
| } | ||
| } | ||
|
|
||
| return featureSet; | ||
| } | ||
|
|
||
|
|
||
| } //namespace DeltaPoseReconstruction | ||
| } //namespace VOCPP |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.