Temporary fixes & debug info

include/core/trajectory/trajectory_base.h

@@ -96,6 +96,7 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj
                    bool state_blocks,
                    std::ostream& stream = std::cout,
                    std::string _tabs = "") const;
+        void printDebug();
     private:
         FrameBasePtr addFrame(FrameBasePtr _frame_ptr);
         void removeFrame(FrameBasePtr _frame_ptr);

src/processor/processor_motion.cpp

@@ -459,9 +459,10 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
 
     // ---------- LAST ----------
     // Make non-key-frame for last Capture
+    auto new_ts = origin_ts + 1e-9;
     auto new_frame_ptr  = getProblem()->emplaceFrame(NON_ESTIMATED,
                                                      _origin_frame->getState(),
-                                                     origin_ts);
+                                                     new_ts);
                                         
     // emplace (emtpy) last Capture
     last_ptr_ = emplaceCapture(new_frame_ptr,

src/processor/processor_tracker.cpp

@@ -107,8 +107,9 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
         case SECOND_TIME_WITHOUT_PACK :
         {
             WOLF_DEBUG( "PT ", getName(), " SECOND_TIME_WITHOUT_PACK" );
-
-            FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, incoming_ptr_->getTimeStamp());
+auto test_stamp = incoming_ptr_->getTimeStamp()+2e-9;
+WOLF_DEBUG("TIME STAMP ", test_stamp);
+            FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, incoming_ptr_->getTimeStamp()+2e-9);
             incoming_ptr_->link(frm);
 
             // We have a last_ Capture with no features, so we do not process known features, and we do not vote for KF.

src/trajectory/trajectory_base.cpp

@@ -20,8 +20,10 @@ TrajectoryBase::~TrajectoryBase()
 FrameBasePtr TrajectoryBase::addFrame(FrameBasePtr _frame_ptr)
 {
     // add to list
-    frame_list_.insert(std::pair<TimeStamp, FrameBasePtr>(_frame_ptr->getTimeStamp(), _frame_ptr));
-
+    std::cout << "Inserting " << _frame_ptr->getTimeStamp() << " " << _frame_ptr << std::endl;
+    auto success = frame_list_.insert(std::pair<TimeStamp, FrameBasePtr>(_frame_ptr->getTimeStamp(), _frame_ptr));
+    std::cout << "Sucess? " << success.second << std::endl;
+    printDebug();
     if (_frame_ptr->isKeyOrAux())
     {
         // update last_estimated and last_key
@@ -35,7 +37,7 @@ void TrajectoryBase::removeFrame(FrameBasePtr _frame_ptr)
 {
     // add to list
     // frame_list_.erase(_frame_ptr);
-    frame_list_.erase(_frame_ptr->id());
+    frame_list_.erase(_frame_ptr->getTimeStamp());
 
     // update last_estimated and last_key
     if (_frame_ptr->isKeyOrAux())
@@ -124,4 +126,9 @@ void TrajectoryBase::print(int _depth, bool _constr_by, bool _metric, bool _stat
             F->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + "  ");
 
 }
+void TrajectoryBase::printDebug()
+{
+    for( auto it : frame_list_ )
+        WOLF_DEBUG("KEY ", it.first, " VALUE ", it.second);
+}
 } // namespace wolf

test/gtest_processor_base.cpp

@@ -115,7 +115,6 @@ TEST(ProcessorBase, KeyFrameCallback)
 
         capt_odo->setTimeStamp(t);
         std::cout << "2\n";
-        problem->check(1);
 //        auto proc_odo_motion = std::static_pointer_cast<ProcessorMotion>(proc_odo);
 //        auto last_ptr = proc_odo_motion->last_ptr_;
 //        auto last_ptr_frame = last_ptr->getFrame();
@@ -125,6 +124,7 @@ TEST(ProcessorBase, KeyFrameCallback)
         // Track
         capt_trk = make_shared<CaptureVoid>(t, sens_trk);
         std::cout << "4\n";
+        problem->print(4,1,1,1, std::cout);
         proc_trk->captureCallback(capt_trk);
         std::cout << "5\n";
 

test/gtest_processor_tracker_feature_dummy.cpp

@@ -311,6 +311,7 @@ TEST_F(ProcessorTrackerFeatureDummyTest, process)
     //4TH TIME
     WOLF_INFO("Forth time...");
     CaptureBasePtr cap4 = std::make_shared<CaptureVoid>(3, sensor);
+    problem->getTrajectory()->printDebug();
     cap4->process();
 
     ASSERT_EQ(processor->getLast()->getFeatureList().size(), params->max_new_features-3);