Improve usages of time_tolerance in Frame

include/core/processor/processor_base.h

@@ -318,13 +318,13 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
          * It stores the new KF in buffer_frame_ and calls triggerInKF()
          *
          */
-        void keyFrameCallback(FrameBasePtr _keyframe_ptr, const double& _time_tol_other);
+        void keyFrameCallback(FrameBasePtr _keyframe, const double& _time_tol_other);
 
         /**\brief notify a new capture
          *
          * It stores the new capture in buffer_capture_ and calls triggerInCapture()
          */
-        void captureCallback(CaptureBasePtr _capture_ptr);
+        void captureCallback(CaptureBasePtr _capture);
 
         SensorBasePtr getSensor() const;
     private:

src/frame/frame_base.cpp

@@ -40,7 +40,8 @@ FrameBase::FrameBase(const TimeStamp& _ts,
         HasStateBlocks(_frame_structure),
         trajectory_ptr_(),
         frame_id_(++frame_id_count_),
-        time_stamp_(_ts)
+        time_stamp_(_ts),
+        time_tolerance_(0)
 {
     assert(_state.includesStructure(_frame_structure) && "_state does not include all keys of _frame_structure");
 
@@ -59,7 +60,8 @@ FrameBase::FrameBase(const TimeStamp& _ts,
                 HasStateBlocks(_frame_structure),
                 trajectory_ptr_(),
                 frame_id_(++frame_id_count_),
-                time_stamp_(_ts)
+                time_stamp_(_ts),
+                time_tolerance_(0)
 {
     assert(_frame_structure.size() == _vectors.size() && "Structure size does not match number of provided vectors!");
 
@@ -85,7 +87,8 @@ FrameBase::FrameBase(const TimeStamp& _ts,
             HasStateBlocks(""),
             trajectory_ptr_(),
             frame_id_(++frame_id_count_),
-            time_stamp_(_ts)
+            time_stamp_(_ts),
+            time_tolerance_(0)
 {
     if (_p_ptr)
     {

src/problem/problem.cpp

@@ -1086,6 +1086,7 @@ FrameBasePtr Problem::applyPriorOptions(const TimeStamp& _ts)
     if (prior_options_->mode != "nothing" and prior_options_->mode != "")
     {
         prior_keyframe = emplaceFrame(_ts, frame_structure_, prior_options_->state);
+        prior_keyframe->setTimeTolerance(prior_options_->time_tolerance);
 
         // Update origin for odometry processors
         for (auto proc_pair : motion_provider_map_)

src/processor/processor_base.cpp

@@ -54,33 +54,33 @@ bool ProcessorBase::permittedKeyFrame()
     return isVotingActive() && getProblem()->permitKeyFrame(shared_from_this());
 }
 
-void ProcessorBase::keyFrameCallback(FrameBasePtr _keyframe_ptr, const double& _time_tol_other)
+void ProcessorBase::keyFrameCallback(FrameBasePtr _keyframe, const double& _time_tol_other)
 {
-    assert(_keyframe_ptr != nullptr && "keyFrameCallback with a nullptr frame");
-    WOLF_DEBUG("P", isMotionProvider() ? "M " : "T ", getName(), ": KF", _keyframe_ptr->id(), " callback received with ts = ", _keyframe_ptr->getTimeStamp());
+    assert(_keyframe != nullptr && "keyFrameCallback with a nullptr frame");
+    WOLF_DEBUG("P", isMotionProvider() ? "M " : "T ", getName(), ": KF", _keyframe->id(), " callback received with ts = ", _keyframe->getTimeStamp());
 
     // profiling
     n_kf_callback_++;
     startKFProfiling();
 
-    _keyframe_ptr->setTimeTolerance(_time_tol_other);
+    _keyframe->setTimeTolerance(_time_tol_other);
 
     // asking if frame should be stored
-    if (storeKeyFrame(_keyframe_ptr))
-        buffer_frame_.add(_keyframe_ptr->getTimeStamp(), _keyframe_ptr);
+    if (storeKeyFrame(_keyframe))
+        buffer_frame_.add(_keyframe->getTimeStamp(), _keyframe);
 
     // asking if frame should be processed
-    if (triggerInKeyFrame(_keyframe_ptr, _time_tol_other))
-        processKeyFrame(_keyframe_ptr, _time_tol_other);
+    if (triggerInKeyFrame(_keyframe, _time_tol_other))
+        processKeyFrame(_keyframe, _time_tol_other);
 
     // profiling
     stopKFProfiling();
 }
 
-void ProcessorBase::captureCallback(CaptureBasePtr _capture_ptr)
+void ProcessorBase::captureCallback(CaptureBasePtr _capture)
 {
-    assert(_capture_ptr != nullptr && "captureCallback with a nullptr capture");
-    WOLF_DEBUG("P", isMotionProvider() ? "M " : "T ", getName(), ": Capture ", _capture_ptr->id(), " callback received with ts = ", _capture_ptr->getTimeStamp());
+    assert(_capture != nullptr && "captureCallback with a nullptr capture");
+    WOLF_DEBUG("P", isMotionProvider() ? "M " : "T ", getName(), ": Capture ", _capture->id(), " callback received with ts = ", _capture->getTimeStamp());
 
     // profiling
     n_capture_callback_++;
@@ -88,15 +88,15 @@ void ProcessorBase::captureCallback(CaptureBasePtr _capture_ptr)
 
     // apply prior in problem if not done (very first capture)
     if (getProblem() && !getProblem()->isPriorSet())
-        getProblem()->applyPriorOptions(_capture_ptr->getTimeStamp());
+        getProblem()->applyPriorOptions(_capture->getTimeStamp());
 
     // asking if capture should be stored
-    if (storeCapture(_capture_ptr))
-        buffer_capture_.add(_capture_ptr->getTimeStamp(), _capture_ptr);
+    if (storeCapture(_capture))
+        buffer_capture_.add(_capture->getTimeStamp(), _capture);
 
     // asking if capture should be processed
-    if (triggerInCapture(_capture_ptr))
-        processCapture(_capture_ptr);
+    if (triggerInCapture(_capture))
+        processCapture(_capture);
 
     // profiling
     stopCaptureProfiling();

src/processor/processor_motion.cpp

@@ -473,8 +473,9 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
         setCalibration(last_ptr_, getCalibration(origin_ptr_));
 
         // Set the frame of last_ptr as key
-        auto key_frame      = last_ptr_->getFrame();
-        key_frame           ->link(getProblem());
+        auto keyframe       = last_ptr_->getFrame();
+        keyframe            ->setTimeTolerance(params_motion_->time_tolerance);
+        keyframe            ->link(getProblem());
 
         // create motion feature and add it to the key_capture
         auto key_feature    = emplaceFeature(last_ptr_);
@@ -489,14 +490,14 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
         // create a new capture
         auto capture_new    = emplaceCapture(frame_new,
                                              getSensor(),
-                                             key_frame->getTimeStamp(),
+                                             keyframe->getTimeStamp(),
                                              Eigen::VectorXd::Zero(data_size_),
                                              getSensor()->getNoiseCov(),
                                              getCalibration(origin_ptr_),
                                              getCalibration(origin_ptr_),
                                              last_ptr_);
         // reset the new buffer
-        capture_new->getBuffer().push_back( motionZero(key_frame->getTimeStamp()) ) ;
+        capture_new->getBuffer().push_back( motionZero(keyframe->getTimeStamp()) ) ;
 
         // reset derived things
         resetDerived();
@@ -507,7 +508,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
         last_frame_ptr_ = frame_new;
 
         // callback to other processors
-        getProblem()->keyFrameCallback(key_frame, shared_from_this(), params_motion_->time_tolerance);
+        getProblem()->keyFrameCallback(keyframe, shared_from_this(), params_motion_->time_tolerance);
 
     }
 

src/processor/processor_tracker.cpp

@@ -102,18 +102,22 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
         {
             WOLF_DEBUG( "PT ", getName(), " FIRST_TIME_WITHOUT_KEYFRAME" );
 
-            FrameBasePtr kfrm = FrameBase::emplace<FrameBase>(getProblem()->getTrajectory(),
-                                                              incoming_ptr_->getTimeStamp(),
-                                                              getProblem()->getFrameStructure(),
-                                                              getProblem()->getState());
-            incoming_ptr_->link(kfrm);
+            // make a new KF at incoming
+            FrameBasePtr keyframe = FrameBase::emplace<FrameBase>(getProblem()->getTrajectory(),
+                                                                  incoming_ptr_->getTimeStamp(),
+                                                                  getProblem()->getFrameStructure(),
+                                                                  getProblem()->getState());
+            keyframe->setTimeTolerance(params_tracker_->time_tolerance);
+
+            // link incoming to the new KF
+            incoming_ptr_->link(keyframe);
 
             // Process info
             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(kfrm, shared_from_this(), params_tracker_->time_tolerance);
+            getProblem()->keyFrameCallback(keyframe, shared_from_this(), params_tracker_->time_tolerance);
 
             resetDerived();