Improve debug info

src/frame_base.cpp

@@ -26,11 +26,11 @@ FrameBase::FrameBase(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _
 
     if ( isKey() )
     {
-        WOLF_INFO("constructed +KF", this->id() );
+        WOLF_DEBUG("New KF", this->id() );
     }
     else
     {
-        WOLF_INFO("constructed + F", this->id() );
+        WOLF_DEBUG("New F", this->id() );
     }
 }
 
@@ -49,11 +49,11 @@ FrameBase::FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr
 
     if ( isKey() )
     {
-        WOLF_INFO("constructed +KF", this->id() );
+        WOLF_DEBUG("New KF", this->id() );
     }
     else
     {
-        WOLF_INFO("constructed + F", this->id() );
+        WOLF_DEBUG("New F", this->id() );
     }
 }
                 
@@ -132,6 +132,8 @@ void FrameBase::setKey()
             getTrajectoryPtr()->setLastKeyFramePtr(shared_from_this());
 
         getTrajectoryPtr()->sortFrame(shared_from_this());
+
+        WOLF_DEBUG("Set KF", this->id());
     }
 }
 

src/problem.cpp

@@ -330,7 +330,8 @@ bool Problem::permitKeyFrame(ProcessorBasePtr _processor_ptr)
 
 void Problem::keyFrameCallback(FrameBasePtr _keyframe_ptr, ProcessorBasePtr _processor_ptr, const Scalar& _time_tolerance)
 {
-    //std::cout << "Problem::keyFrameCallback: processor " << _processor_ptr->getName() << std::endl;
+    WOLF_DEBUG((_processor_ptr->isMotion() ? "PM " : "PT "), _processor_ptr->getName(), ": KF", _keyframe_ptr->id(), " Callback emitted with ts = ", _keyframe_ptr->getTimeStamp());
+
     for (auto sensor : hardware_ptr_->getSensorList())
     	for (auto processor : sensor->getProcessorList())
     		if (processor && (processor != _processor_ptr) )

src/processor_tracker.cpp

@@ -53,7 +53,7 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
             KFPackPtr pack = selectPack( incoming_ptr_);
             kf_pack_buffer_.removeUpTo( incoming_ptr_->getTimeStamp() );
 
-            WOLF_DEBUG( "PT: KF" , pack->key_frame->id() , " callback received at ts= " , pack->key_frame->getTimeStamp().get() );
+            WOLF_DEBUG( "PT: KF" , pack->key_frame->id() , " callback received with ts= " , pack->key_frame->getTimeStamp().get() );
 
             // Append incoming to KF
             pack->key_frame->addCapture(incoming_ptr_);

src/processor_tracker_feature.cpp

@@ -151,6 +151,12 @@ void ProcessorTrackerFeature::establishConstraints()
         match.first->addConstraint(ctr);
         match.second->feature_ptr_->addConstrainedBy(ctr);
     }
+    for (auto match : matches_origin_from_last_)
+    {
+        WOLF_DEBUG( "Constraint: track: " , match.second->feature_ptr_->trackId() ,
+                    " origin: " , match.second->feature_ptr_->id() ,
+                    " from last: " , match.first->id() );
+    }
 }
 
 } // namespace wolf