Fix 420

src/processor/processor_inertial_kinematics.cpp

@@ -53,11 +53,11 @@ inline void ProcessorInertialKinematics::processCapture(CaptureBasePtr _capture)
     }
 
     // nothing to do if any of the two buffer is empty
-    if(buffer_pack_kf_.empty()){
+    if(buffer_frame_.empty()){
         WOLF_DEBUG("PInertialKinematic: KF pack buffer empty, time ",  _capture->getTimeStamp());
         return;
     }
-    if(buffer_pack_kf_.empty()){
+    if(buffer_frame_.empty()){
         WOLF_DEBUG("PInertialKinematics: Capture buffer empty, time ",  _capture->getTimeStamp());
         return;
     }
@@ -69,21 +69,21 @@ inline void ProcessorInertialKinematics::processCapture(CaptureBasePtr _capture)
 
     // 1. get corresponding KF
     FrameBasePtr kf;
-    auto buffer_pack_kf_it = buffer_pack_kf_.getContainer().begin();
+    auto buffer_frame_it = buffer_frame_.getContainer().begin();
     auto buffer_capture_it = buffer_capture_.getContainer().begin();
 
     auto sensor_angvel = getProblem()->getSensor(params_ikin_->sensor_angvel_name);
-    while ((buffer_pack_kf_it != buffer_pack_kf_.getContainer().end())
+    while ((buffer_frame_it != buffer_frame_.getContainer().end())
         && (buffer_capture_it != buffer_capture_.getContainer().end()))
     {
 
-        bool time_ok = buffer_capture_.simpleCheckTimeTolerance(buffer_pack_kf_it->first, buffer_capture_it->first, buffer_pack_kf_it->second->time_tolerance);
+        bool time_ok = buffer_capture_.simpleCheckTimeTolerance(buffer_frame_it->first, buffer_capture_it->first, buffer_frame_it->second->getTimeTolerance());
         if (time_ok) {
-            CaptureBasePtr cap_angvel = buffer_pack_kf_it->second->key_frame->getCaptureOf(sensor_angvel);
-            auto min_ts = (buffer_pack_kf_it->first < buffer_capture_it->first) ? buffer_pack_kf_it->first : buffer_capture_it->first;
+            CaptureBasePtr cap_angvel = buffer_frame_it->second->getCaptureOf(sensor_angvel);
+            auto min_ts = (buffer_frame_it->first < buffer_capture_it->first) ? buffer_frame_it->first : buffer_capture_it->first;
             if (cap_angvel && cap_angvel->getStateBlock('I')){  // TODO: or only cap_angvel?
                 // cast incoming capture to the InertialKinematics type, add it to the keyframe
-                auto kf = buffer_pack_kf_it->second->key_frame;
+                auto kf = buffer_frame_it->second;
                 auto cap_ikin = std::static_pointer_cast<CaptureInertialKinematics>(buffer_capture_it->second);
                 cap_ikin->link(kf);
                 createInertialKinematicsFactor(cap_ikin,
@@ -92,27 +92,27 @@ inline void ProcessorInertialKinematics::processCapture(CaptureBasePtr _capture)
                 // update pointer to origin capture (the previous one attached to a KF) if we have created a new factor
                 cap_origin_ptr_ = buffer_capture_it->second;
                 buffer_capture_it++;
-                buffer_pack_kf_it++;
+                buffer_frame_it++;
             }
             else {
                 // if time ok but no capture angvel yet, there is not gonna be any in the next KF of the buffer
                 break;
                 buffer_capture_it++;
-                buffer_pack_kf_it++;
+                buffer_frame_it++;
             }
 
             ////////////////
             // remove everything before (Inclusive if equal) this timestamp -> the cap_angvel is yet to come
-            buffer_pack_kf_.removeUpTo(min_ts);
+            buffer_frame_.removeUpTo(min_ts);
             buffer_capture_.removeUpTo(min_ts);
         }
         else {
             // if a time difference between captures and KF pack, we increment the oldest iterator
-            if (buffer_capture_it->first < buffer_pack_kf_it->first){
+            if (buffer_capture_it->first < buffer_frame_it->first){
                 buffer_capture_it++;
             }
             else {
-                buffer_pack_kf_it++;
+                buffer_frame_it++;
             }
         }
     }

src/processor/processor_point_feet_nomove.cpp

@@ -46,14 +46,14 @@ void ProcessorPointFeetNomove::configure(SensorBasePtr _sensor)
 void ProcessorPointFeetNomove::createFactorIfNecessary(){
     auto sensor_pfnm = std::static_pointer_cast<SensorPointFeetNomove>(getSensor());
 
-    while (buffer_pack_kf_.size() >= 2)
+    while (buffer_frame_.size() >= 2)
     {
-        auto kf1_it = buffer_pack_kf_.getContainer().begin();
+        auto kf1_it = buffer_frame_.getContainer().begin();
         auto kf2_it = std::next(kf1_it);
         TimeStamp t1 = kf1_it->first;
         TimeStamp t2 = kf2_it->first;
-        auto cap1_it = buffer_capture_.selectIterator(t1, kf1_it->second->time_tolerance);
-        auto cap2_it = buffer_capture_.selectIterator(t2, kf1_it->second->time_tolerance);
+        auto cap1_it = buffer_capture_.selectIterator(t1, kf1_it->second->getTimeTolerance());
+        auto cap2_it = buffer_capture_.selectIterator(t2, kf1_it->second->getTimeTolerance());
 
         // check that the first 2 KF have corresponding captures in the capture buffer  
         // just quit and assume that you will someday have matching captures
@@ -101,7 +101,7 @@ void ProcessorPointFeetNomove::createFactorIfNecessary(){
                 auto cap2 = std::static_pointer_cast<CapturePointFeetNomove>(cap2_it->second);
                 // link (kind of arbitrarily) factor from KF1 to KF2 with a feature and capture on KF2 to mimic capture motion
                 // however, this capture solely does not contain enough information to recreate the factor
-                cap2->link(kf2_it->second->key_frame);
+                cap2->link(kf2_it->second);
                 auto kin_incontact_t2 = cap2->kin_incontact_;
 
                 for (auto kin_pair1: kin_incontact_from_t1){
@@ -109,13 +109,13 @@ void ProcessorPointFeetNomove::createFactorIfNecessary(){
 
                     Vector6d meas; meas << kin_pair1.second, kin_pair2_it->second;
                     FeatureBasePtr feat = FeatureBase::emplace<FeatureBase>(cap2, "PointFeet", meas, sensor_pfnm->getCovFootNomove());
-                    FactorPointFeetNomovePtr fac = FactorBase::emplace<FactorPointFeetNomove>(feat, feat, kf1_it->second->key_frame, nullptr, true);
+                    FactorPointFeetNomovePtr fac = FactorBase::emplace<FactorPointFeetNomove>(feat, feat, kf1_it->second, nullptr, true);
                 }
             }
 
             // Once the factors are created, remove kf1 and all the captures until ts 2 NOT INCLUDING the one at ts 2 since we need it for the next
             // Note: erase by range does not include the end range iterator
-            buffer_pack_kf_.getContainer().erase(buffer_pack_kf_.getContainer().begin(), kf2_it);  // !! works only if getContainer returns a non const reference
+            buffer_frame_.getContainer().erase(buffer_frame_.getContainer().begin(), kf2_it);  // !! works only if getContainer returns a non const reference
             buffer_capture_.getContainer().erase(buffer_capture_.getContainer().begin(), cap2_it);
         }
     }
@@ -130,11 +130,11 @@ inline void ProcessorPointFeetNomove::processCapture(CaptureBasePtr _capture)
         return;
     }
     // nothing to do if any of the two buffer is empty
-    if(buffer_pack_kf_.empty()){
+    if(buffer_frame_.empty()){
         WOLF_DEBUG("PInertialKinematic: KF pack buffer empty, time ",  _capture->getTimeStamp());
         return;
     }
-    if(buffer_pack_kf_.empty()){
+    if(buffer_frame_.empty()){
         WOLF_DEBUG("PInertialKinematics: Capture buffer empty, time ",  _capture->getTimeStamp());
         return;
     }
@@ -151,11 +151,11 @@ inline void ProcessorPointFeetNomove::processKeyFrame(FrameBasePtr _keyframe_ptr
         return;
     }
     // nothing to do if any of the two buffer is empty
-    if(buffer_pack_kf_.empty()){
+    if(buffer_frame_.empty()){
         WOLF_DEBUG("ProcessorPointFeetNomove: KF pack buffer empty, time ",  _keyframe_ptr->getTimeStamp());
         return;
     }
-    if(buffer_pack_kf_.empty()){
+    if(buffer_frame_.empty()){
         WOLF_DEBUG("ProcessorPointFeetNomove: Capture buffer empty, time ",  _keyframe_ptr->getTimeStamp());
         return;
     }