fix: processor_pose.h

include/core/processor/processor_pose.h

@@ -37,14 +37,13 @@ class ProcessorPose : public ProcessorBase{
 
         void configure(SensorBasePtr _sensor) override;
 
-        void processCapture(CaptureBasePtr _incoming) override;
-        void processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance) override;
-        bool triggerInCapture(CaptureBasePtr) const override;
-        bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance) const override;
-        bool storeKeyFrame(FrameBasePtr) override;
-        bool storeCapture(CaptureBasePtr) override;
-        bool voteForKeyFrame() const override;
-
+        void processCapture(CaptureBasePtr) override;
+        void processKeyFrame(FrameBasePtr, const double&) override;
+        bool triggerInCapture(CaptureBasePtr _cap) const override { return true;};
+        bool triggerInKeyFrame(FrameBasePtr _frm, const double& _time_tol) const override { return true;};
+        bool storeKeyFrame(FrameBasePtr _frm) override { return true;};
+        bool storeCapture(CaptureBasePtr _cap) override { return true;};
+        bool voteForKeyFrame() const override { return false;};
         void createFactorIfNecessary();
 
     protected:
@@ -53,31 +52,7 @@ class ProcessorPose : public ProcessorBase{
 
 
 
-inline bool ProcessorPose::triggerInKeyFrame(FrameBasePtr _keyframe_ptr,
-                                             const double& _time_tolerance) const
-{
-    return true;
-}
-
-inline bool ProcessorPose::triggerInCapture(CaptureBasePtr _capture) const
-{   
-    return true;
-}
-
-inline bool ProcessorPose::storeKeyFrame(FrameBasePtr)
-{
-    return true;
-}
-
-inline bool ProcessorPose::storeCapture(CaptureBasePtr)
-{
-    return true;
-}
 
-inline bool ProcessorPose::voteForKeyFrame() const
-{
-    return false;
-}
 
 } /* namespace wolf */
 

src/processor/processor_pose.cpp

@@ -24,10 +24,10 @@ void ProcessorPose::configure(SensorBasePtr _sensor)
 
 void ProcessorPose::createFactorIfNecessary(){
     auto sensor_pose = std::static_pointer_cast<SensorPose>(getSensor());
-
-    while (buffer_pack_kf_.size() >= 1)
+    auto kf_it_last = buffer_pack_kf_.getContainer().end();
+    auto kf_it = buffer_pack_kf_.getContainer().begin();
+    while (kf_it != buffer_pack_kf_.getContainer().end())
     {
-        auto kf_it = buffer_pack_kf_.getContainer().begin();
         TimeStamp t = kf_it->first;
         double time_tolerance = std::min(getTimeTolerance(), kf_it->second->time_tolerance);
         if (getTimeTolerance() == 0.0){
@@ -37,24 +37,33 @@ void ProcessorPose::createFactorIfNecessary(){
         auto cap_it = buffer_capture_.selectIterator(t, time_tolerance);
 
         // if capture with corresponding timestamp is not found, stop and assume you will get it later
-        if (cap_it == buffer_capture_.getContainer().end())
-        {
-            return;
-        }
-        else
+        if (cap_it != buffer_capture_.getContainer().end())
         {
             // if a corresponding capture exists, link it to the KF and create a factor
             auto cap = std::static_pointer_cast<CapturePose>(cap_it->second);
             cap->link(kf_it->second->key_frame);
             FeatureBasePtr feat = FeatureBase::emplace<FeatureBase>(cap, "Pose", cap->getData(), cap->getDataCovariance());
-            FactorPose3dWithExtrinsicsPtr fac = FactorBase::emplace<FactorPose3dWithExtrinsics>(feat, feat, nullptr, false, TOP_MOTION);
+            FactorPose3dWithExtrinsicsPtr fac = FactorBase::emplace<FactorPose3dWithExtrinsics>(feat, feat, shared_from_this(), false, TOP_MOTION);
 
             // erase removes range [first, last): it does not removes last
             // so increment the iterator so that it points to the next element in the container
-            buffer_pack_kf_.getContainer().erase(buffer_pack_kf_.getContainer().begin(), std::next(kf_it));
-            buffer_capture_.getContainer().erase(buffer_capture_.getContainer().begin(), std::next(cap_it));
+            buffer_capture_.getContainer().erase(buffer_capture_.getContainer().begin(), std::next(cap_it));     
+
+            // we cannot erase on the kf buffer since we are looping over it so we store the iterator for later
+            kf_it_last = kf_it;
         }
+
+        kf_it++;
     }
+
+    // whatever happened, remove very old captures
+    buffer_capture_.removeUpTo(buffer_pack_kf_.getContainer().begin()->first - 5);
+
+    // now we erase the kf buffer if there was a match
+    if (kf_it_last != buffer_pack_kf_.getContainer().end()){
+        buffer_pack_kf_.getContainer().erase(buffer_pack_kf_.getContainer().begin(), std::next(kf_it_last));
+    }
+
 }
 
 

test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp

@@ -18,11 +18,6 @@
 
 using namespace Eigen;
 using namespace wolf;
-using std::cout;
-using std::endl;
-
-const Vector3d zero3 = Vector3d::Zero();
-
 
 
 class FactorPose3dWithExtrinsicsBase_Test : public testing::Test