diff --git a/.clang-format b/.clang-format
index ea2a2257babe01c1814f5764875509ada2d8b465..bc951c7539c78c4fa00efde0030dcf12b126686a 100644
--- a/.clang-format
+++ b/.clang-format
@@ -1,7 +1,8 @@
---
Language: Cpp
BasedOnStyle: Google
-AccessModifierOffset: 0
+IndentAccessModifiers: false
+AccessModifierOffset: -2
AlignAfterOpenBracket: Align
AlignConsecutiveAssignments: true
AlignConsecutiveDeclarations: true
@@ -11,7 +12,7 @@ AlignTrailingComments: true
AllowAllParametersOfDeclarationOnNextLine: false
AllowShortBlocksOnASingleLine: false
AllowShortCaseLabelsOnASingleLine: false
-AllowShortFunctionsOnASingleLine: All
+AllowShortFunctionsOnASingleLine: Empty
AllowShortIfStatementsOnASingleLine: true
AllowShortLoopsOnASingleLine: true
AlwaysBreakAfterDefinitionReturnType: None
@@ -20,24 +21,24 @@ AlwaysBreakBeforeMultilineStrings: true
AlwaysBreakTemplateDeclarations: true
BinPackArguments: true
BinPackParameters: false
+BreakBeforeBraces: Custom
BraceWrapping:
- AfterClass: false
- AfterControlStatement: false
- AfterEnum: false
- AfterFunction: false
- AfterNamespace: false
- AfterObjCDeclaration: false
- AfterStruct: false
- AfterUnion: false
+ AfterClass: true
+ AfterControlStatement: Always
+ AfterEnum: true
+ AfterFunction: true
+ AfterNamespace: true
+ AfterObjCDeclaration: true
+ AfterStruct: true
+ AfterUnion: true
AfterExternBlock: false
- BeforeCatch: false
- BeforeElse: false
+ BeforeCatch: true
+ BeforeElse: true
IndentBraces: false
SplitEmptyFunction: true
SplitEmptyRecord: true
SplitEmptyNamespace: true
BreakBeforeBinaryOperators: None
-BreakBeforeBraces: Linux
BreakBeforeInheritanceComma: false
BreakBeforeTernaryOperators: true
BreakConstructorInitializersBeforeComma: false
@@ -45,7 +46,8 @@ BreakConstructorInitializers: BeforeColon
BreakAfterJavaFieldAnnotations: false
BreakStringLiterals: true
ColumnLimit: 119
-CommentPragmas: "^ IWYU pragma:"
+# CommentPragmas: "^ IWYU pragma: ^\\.+"
+CommentPragmas: '^\\.+'
CompactNamespaces: false
ConstructorInitializerAllOnOneLineOrOnePerLine: true
ConstructorInitializerIndentWidth: 4
@@ -60,17 +62,17 @@ ForEachMacros:
- Q_FOREACH
- BOOST_FOREACH
IncludeBlocks: Preserve
-IncludeCategories:
- - Regex: '^<pinocchio/fwd\.hpp>'
- Priority: 1
- - Regex: '^<ext/.*\.h>'
- Priority: 3
- - Regex: '^<.*\.h>'
- Priority: 2
- - Regex: "^<.*"
- Priority: 3
- - Regex: ".*"
- Priority: 4
+# IncludeCategories:
+# - Regex: '^<pinocchio/fwd\.hpp>'
+# Priority: 1
+# - Regex: '^<ext/.*\.h>'
+# Priority: 3
+# - Regex: '^<.*\.h>'
+# Priority: 2
+# - Regex: "^<.*"
+# Priority: 3
+# - Regex: ".*"
+# Priority: 4
IncludeIsMainRegex: "([-_](test|unittest))?$"
IndentCaseLabels: true
IndentPPDirectives: None
diff --git a/include/core/processor/track_matrix.h b/include/core/processor/track_matrix.h
index f609dc8efefbbb6fb7c74b7dac46ec8f052b0403..3b06c46381d24a2191ec271b7a741b121b26fbbb 100644
--- a/include/core/processor/track_matrix.h
+++ b/include/core/processor/track_matrix.h
@@ -45,8 +45,8 @@ using std::list;
using std::pair;
using std::shared_ptr;
-typedef map<TimeStamp, FeatureBasePtr> Track;
-typedef map<TimeStamp, FeatureBaseConstPtr> TrackConst;
+typedef map<TimeStamp, FeatureBasePtr> Track;
+typedef map<TimeStamp, FeatureBaseConstPtr> TrackConst;
typedef map<SizeStd, FeatureBasePtr > Snapshot;
typedef map<SizeStd, FeatureBaseConstPtr > SnapshotConst;
typedef map<SizeStd, pair<FeatureBasePtr, FeatureBasePtr> > TrackMatches; // matched feature pairs indexed by track_id
@@ -102,41 +102,40 @@ class TrackMatrix
TrackMatrix();
virtual ~TrackMatrix();
- void newTrack (FeatureBasePtr _ftr);
- void add (const SizeStd& _track_id, const FeatureBasePtr& _ftr);
- void add (const FeatureBasePtr& _ftr_existing, const FeatureBasePtr& _ftr_new);
- void remove (FeatureBasePtr _ftr);
- void remove (const SizeStd& _track_id);
- void remove (CaptureBasePtr _cap);
- SizeStd numTracks () const;
- SizeStd trackSize (const SizeStd& _track_id) const;
- TrackConst track (const SizeStd& _track_id) const;
- Track track (const SizeStd& _track_id);
- SnapshotConst snapshot (CaptureBaseConstPtr _capture) const;
- Snapshot snapshot (CaptureBasePtr _capture);
+ void newTrack (FeatureBasePtr _ftr);
+ void add (const SizeStd& _track_id, const FeatureBasePtr& _ftr);
+ void add (const FeatureBasePtr& _ftr_existing, const FeatureBasePtr& _ftr_new);
+ void remove (FeatureBasePtr _ftr);
+ void remove (const SizeStd& _track_id);
+ void remove (CaptureBasePtr _cap);
+
+ SizeStd numTracks () const;
+ SizeStd trackSize (const SizeStd& _track_id) const;
+ TrackConst track (const SizeStd& _track_id) const;
+ Track track (const SizeStd& _track_id);
+ SnapshotConst snapshot (CaptureBaseConstPtr _capture) const;
+ Snapshot snapshot (CaptureBasePtr _capture);
vector<FeatureBaseConstPtr> trackAsVector(const SizeStd& _track_id) const;
- vector<FeatureBasePtr> trackAsVector(const SizeStd& _track_id);
- FeatureBaseConstPtrList snapshotAsList(CaptureBaseConstPtr _cap) const;
- FeatureBasePtrList snapshotAsList(CaptureBasePtr _cap);
- TrackMatchesConst matches (CaptureBaseConstPtr _cap_1, CaptureBaseConstPtr _cap_2) const;
- TrackMatches matches (CaptureBasePtr _cap_1, CaptureBasePtr _cap_2);
- FeatureBaseConstPtr firstFeature(const SizeStd& _track_id) const;
- FeatureBasePtr firstFeature(const SizeStd& _track_id);
- FeatureBaseConstPtr lastFeature (const SizeStd& _track_id) const;
- FeatureBasePtr lastFeature (const SizeStd& _track_id);
- FeatureBaseConstPtr feature (const SizeStd& _track_id, CaptureBaseConstPtr _cap) const;
- FeatureBasePtr feature (const SizeStd& _track_id, CaptureBasePtr _cap);
- CaptureBaseConstPtr firstCapture(const SizeStd& _track_id) const;
- CaptureBasePtr firstCapture(const SizeStd& _track_id);
-
- list<SizeStd> trackIds() const;
+ vector<FeatureBasePtr> trackAsVector(const SizeStd& _track_id);
+ FeatureBaseConstPtrList snapshotAsList(CaptureBaseConstPtr _cap) const;
+ FeatureBasePtrList snapshotAsList(CaptureBasePtr _cap);
+ TrackMatchesConst matches (CaptureBaseConstPtr _cap_1, CaptureBaseConstPtr _cap_2) const;
+ TrackMatches matches (CaptureBasePtr _cap_1, CaptureBasePtr _cap_2);
+
+ FeatureBaseConstPtr firstFeature(const SizeStd& _track_id) const;
+ FeatureBasePtr firstFeature(const SizeStd& _track_id);
+ FeatureBaseConstPtr lastFeature (const SizeStd& _track_id) const;
+ FeatureBasePtr lastFeature (const SizeStd& _track_id);
+ FeatureBaseConstPtr feature (const SizeStd& _track_id, CaptureBaseConstPtr _cap) const;
+ FeatureBasePtr feature (const SizeStd& _track_id, CaptureBasePtr _cap);
+ CaptureBaseConstPtr firstCapture(const SizeStd& _track_id) const;
+ CaptureBasePtr firstCapture(const SizeStd& _track_id);
+
+ list<SizeStd> trackIds(CaptureBaseConstPtr _capture = nullptr) const;
// tracks across captures that belong to keyframe
- TrackConst trackAtKeyframes(const SizeStd& _track_id) const;
- Track trackAtKeyframes(const SizeStd& _track_id);
-
- const map<SizeStd, Track>& getTracks() {return tracks_;}
- const map<CaptureBasePtr, Snapshot >& getSnapshots() {return snapshots_;}
+ TrackConst trackAtKeyframes(const SizeStd& _track_id) const;
+ Track trackAtKeyframes(const SizeStd& _track_id);
private:
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index 3d3794e75b1a87461832695c26341b8f40fc11f5..b0e08b4b59240b359457565c95071d2b7c1385e6 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -112,15 +112,15 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
incoming_ptr_->link(keyframe);
// Process info
+ // TrackerFeature: We only process new features in Last, here last = nullptr, so we do not have anything to do.
+ // TrackerLandmark: If we have been given a map, all landmarks in the map are known. Process them.
processKnown();
- // We only process new features in Last, here last = nullptr, so we do not have anything to do.
// Issue KF callback with new KF
getProblem()->keyFrameCallback(keyframe, shared_from_this());
- resetDerived();
-
// Update pointers
+ resetDerived();
origin_ptr_ = incoming_ptr_;
last_ptr_ = incoming_ptr_;
incoming_ptr_ = nullptr;
@@ -130,9 +130,17 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
case SECOND_TIME_WITH_KEYFRAME :
{
// No-break case only for debug. Next case will be executed too.
- FrameBasePtr keyframe_from_callback = buffer_frame_.select( incoming_ptr_->getTimeStamp(),
+ FrameBasePtr keyframe_from_callback = buffer_frame_.select( last_ptr_->getTimeStamp(),
params_tracker_->time_tolerance);
-
+ // This received KF is discarded since it is most likely the same KF we createed in FIRST_TIME, ...
+ // ... only that in FIRST_TIME we checked for incominig, and now we checked for last.
+ // Such KF however should have been removed from the buffer of keyframes with the call to buffer_frame_.removeUpTo()
+
+ // The debug line is here to check if this is really the same KF
+ // or it is rather a new KF created by some other processor,
+ // which happens to be within tolerance of timestamps.
+ // In this case we discard it anyway because we already have a KF in last
+ // and we can't link a capture to two KFs.
WOLF_DEBUG( "PT ", getName(), " SECOND_TIME_WITH_KEYFRAME: KF" , keyframe_from_callback->id() , " callback unpacked with ts= " , keyframe_from_callback->getTimeStamp() );
}
// Fall through
@@ -140,14 +148,16 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
{
WOLF_DEBUG( "PT ", getName(), " SECOND_TIME_WITHOUT_KEYFRAME" );
+ // Make a NON KEY Frame to hold incoming capture
FrameBasePtr keyframe = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
getProblem()->getFrameStructure(),
getProblem()->getState());
incoming_ptr_->link(keyframe);
- // We have a last_ Capture with no features, so we do not process known features, and we do not vote for KF.
// Process info
+ // TrackerLandmark: If we have been given a map, all landmarks in the map are known. Process them.
processKnown();
+ // Both Trackers: We have a last_ Capture with not enough features, so populate it.
processNew(params_tracker_->max_new_features);
// Establish factors
@@ -177,13 +187,13 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
last_ptr_->move(keyframe_from_callback);
last_old_frame->remove();
- // Create new frame for incoming
+ // Create new NON KEY frame for incoming
FrameBasePtr frame = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
getProblem()->getFrameStructure(),
getProblem()->getState());
incoming_ptr_->link(frame);
- // Detect new Features, initialize Landmarks, create Factors, ...
+ // Detect new Features, initialize Landmarks, ...
processNew(params_tracker_->max_new_features);
// Establish factors
@@ -209,8 +219,6 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
// process
processNew(params_tracker_->max_new_features);
- //TODO abort KF if known_features_last_.size() < params_tracker_->min_features_for_keyframe
-
// We create a KF
// set KF on last
last_ptr_->getFrame()->setState(getProblem()->getState(last_ptr_->getTimeStamp()));
@@ -222,14 +230,15 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
// Call the new keyframe callback in order to let the other processors to establish their factors
getProblem()->keyFrameCallback(last_ptr_->getFrame(), shared_from_this());
- // Update pointers
- resetDerived();
- // make F; append incoming to new F
+ // make NON KEY frame; append incoming to new frame
FrameBasePtr frame = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
getProblem()->getFrameStructure(),
getProblem()->getState(incoming_ptr_->getTimeStamp()));
incoming_ptr_ ->link(frame);
+
+ // Update pointers
+ resetDerived();
origin_ptr_ = last_ptr_;
last_ptr_ = incoming_ptr_;
last_frame_ptr_ = frame;
@@ -243,7 +252,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
// Advance this
advanceDerived();
- // Replace last frame for a new one in incoming
+ // Replace last frame for a new NON KEY frame in incoming
FrameBasePtr frame = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
getProblem()->getFrameStructure(),
getProblem()->getState(incoming_ptr_->getTimeStamp()));
@@ -278,7 +287,7 @@ void ProcessorTracker::computeProcessingStep()
// Then combine with the existence (or not) of a keyframe callback pack
switch (step)
{
- case FIRST_TIME :
+ case FIRST_TIME : // We check for KF in incoming
if (buffer_frame_.select(incoming_ptr_->getTimeStamp(), params_tracker_->time_tolerance))
processing_step_ = FIRST_TIME_WITH_KEYFRAME;
@@ -286,7 +295,7 @@ void ProcessorTracker::computeProcessingStep()
processing_step_ = FIRST_TIME_WITHOUT_KEYFRAME;
break;
- case SECOND_TIME :
+ case SECOND_TIME : // We check for KF in last
if (buffer_frame_.select(last_ptr_->getTimeStamp(), params_tracker_->time_tolerance))
processing_step_ = SECOND_TIME_WITH_KEYFRAME;
@@ -294,7 +303,7 @@ void ProcessorTracker::computeProcessingStep()
processing_step_ = SECOND_TIME_WITHOUT_KEYFRAME;
break;
- case RUNNING :
+ case RUNNING : // We check for KF in last
default :
if (buffer_frame_.select(last_ptr_->getTimeStamp(), params_tracker_->time_tolerance))
diff --git a/src/processor/track_matrix.cpp b/src/processor/track_matrix.cpp
index de101750edfe66eec72fae0c3be295164e8b7d45..ed4215d12e13403c135b433c440f7f1493612bca 100644
--- a/src/processor/track_matrix.cpp
+++ b/src/processor/track_matrix.cpp
@@ -369,12 +369,26 @@ Track TrackMatrix::trackAtKeyframes(const SizeStd& _track_id)
return Track();
}
-list<SizeStd> TrackMatrix::trackIds() const
+list<SizeStd> TrackMatrix::trackIds(CaptureBaseConstPtr _capture) const
{
list<SizeStd> track_ids;
- for (auto track : tracks_)
+
+ if (not _capture)
+ for (auto track_pair : tracks_)
+ track_ids.push_back(track_pair.first);
+ else
{
- track_ids.push_back(track.first);
+ auto it = std::find_if(snapshots_.begin(),
+ snapshots_.end(),
+ [_capture](const std::pair<CaptureBasePtr, Snapshot>& pair)
+ {
+ return pair.first == _capture;
+ }
+ );
+
+ if (it != snapshots_.end())
+ for (auto track_pair : it->second)
+ track_ids.push_back(track_pair.first);
}
return track_ids;
}
diff --git a/test/gtest_track_matrix.cpp b/test/gtest_track_matrix.cpp
index 130a3495c8469a0b4dbcecb8ebbc68b55412835c..aabfada74ff97166ff7468ffe48cd8c46db4034f 100644
--- a/test/gtest_track_matrix.cpp
+++ b/test/gtest_track_matrix.cpp
@@ -42,7 +42,7 @@ class TrackMatrixTest : public testing::Test
FrameBasePtr F0, F1, F2, F3, F4;
CaptureBasePtr C0, C1, C2, C3, C4;
- FeatureBasePtr f0, f1, f2, f3, f4;
+ FeatureBasePtr f0, f1, f2, f3, f4, f5;
ProblemPtr problem;
void SetUp() override
@@ -71,6 +71,7 @@ class TrackMatrixTest : public testing::Test
f2 = FeatureBase::emplace<FeatureBase>(nullptr, "FeatureBase", m, m_cov);
f3 = FeatureBase::emplace<FeatureBase>(nullptr, "FeatureBase", m, m_cov);
f4 = FeatureBase::emplace<FeatureBase>(nullptr, "FeatureBase", m, m_cov);
+ f5 = FeatureBase::emplace<FeatureBase>(nullptr, "FeatureBase", m, m_cov);
// F0 and F4 are keyframes
F0->link(problem);
@@ -507,6 +508,37 @@ TEST_F(TrackMatrixTest, trackAtKeyframes)
ASSERT_EQ(trk_kf_1.size(), 0);
}
+TEST_F(TrackMatrixTest, trackIds)
+{
+ f0->link(C0);
+ f1->link(C1);
+ f2->link(C1);
+ f3->link(C1);
+ f4->link(C2);
+ f5->link(C2);
+
+ track_matrix.newTrack(f0);
+ track_matrix.add(f0->trackId(), f1);
+ track_matrix.newTrack(f2);
+ track_matrix.newTrack(f3);
+ track_matrix.add(f0->trackId(), f4);
+ track_matrix.add(f2->trackId(), f5);
+
+ /* KC0 C1 C2
+ *
+ * f0---f1---f4 trk 0
+ * | |
+ * f2---f5 trk 1
+ * |
+ * f3 trk 2
+ */
+
+ ASSERT_EQ(track_matrix.trackIds().size(), 3);
+ ASSERT_EQ(track_matrix.trackIds(C0).size(), 1);
+ ASSERT_EQ(track_matrix.trackIds(C1).size(), 3);
+ ASSERT_EQ(track_matrix.trackIds(C2).size(), 2);
+}
+
int main(int argc, char **argv)
{