diff --git a/src/processor/processor_visual_odometry.cpp b/src/processor/processor_visual_odometry.cpp index 0f50386f9c9fd8868bb5fb886c60750e74077134..b0d02ec63273b32ef61e26aa7059917a89fee82b 100644 --- a/src/processor/processor_visual_odometry.cpp +++ b/src/processor/processor_visual_odometry.cpp @@ -498,6 +498,19 @@ LandmarkBasePtr ProcessorVisualOdometry::emplaceLandmark(FeatureBasePtr _feat) void ProcessorVisualOdometry::postProcess() { frame_count_ ++; + + // delete tracks with no keyframes + for (auto track_it = track_matrix_.getTracks().begin(); track_it != track_matrix_.getTracks().end(); /* do not increment iterator yet... */) + { + auto track_id = track_it->first; + if (track_matrix_.trackAtKeyframes(track_id).empty()) + { + ++track_it; // ... increment iterator **before** erasing the element!!! + track_matrix_.remove(track_id); + } + else + ++track_it; + } } bool ProcessorVisualOdometry::voteForKeyFrame() const