Merge remote-tracking branch 'origin/devel' into...

Merge remote-tracking branch 'origin/devel' into 478-subscriber-processor-for-landmark-external-detections

include/core/factor/factor_base.h

@@ -123,6 +123,31 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
             return topology_;
         }
 
+        std::string getTopologyString() const
+        {
+            switch (topology_)
+            {
+            case TOP_GEOM:
+                return "GEOM";
+                break;
+            case TOP_ABS:
+                return "ABS";
+                break;
+            case TOP_LMK:
+                return "LMK";
+                break;
+            case TOP_LOOP:
+                return "LOOP";
+                break;
+            case TOP_OTHER:
+                return "OTHER";
+                break;
+            default:
+                return "UNDEFINED";
+            }
+        }
+
+
         /** \brief Evaluate the factor given the input parameters and returning the residuals and jacobians
         **/
         virtual bool evaluate(double const* const* _parameters, double* _residuals, double** _jacobians) const = 0;

include/core/processor/processor_motion.h

@@ -30,7 +30,7 @@
 #define PROCESSOR_MOTION_H_
 
 // Wolf
-#include <core/processor/motion_provider.h>
+#include "core/processor/motion_provider.h"
 #include "core/capture/capture_motion.h"
 #include "core/processor/processor_base.h"
 #include "core/common/time_stamp.h"
@@ -271,6 +271,10 @@ class ProcessorMotion : public ProcessorBase, public MotionProvider
         bool voteForKeyFrame() const override;
 
         double updateDt();
+        /** \brief Make one step of motion integration
+         * 
+         * Integrate motiondata in incoming_ptr_ and store all results in the MotionBuffer in last_ptr_
+         */
         void integrateOneStep();
         void reintegrateBuffer(CaptureMotionPtr _capture_ptr) const;
         void splitBuffer(const wolf::CaptureMotionPtr& capture_source,

src/capture/capture_base.cpp

@@ -158,13 +158,15 @@ FactorBasePtrList CaptureBase::getFactorList()
 void CaptureBase::getFactorList(FactorBaseConstPtrList& _fac_list) const
 {
     for (auto f_ptr : getFeatureList())
-        f_ptr->getFactorList(_fac_list);
+        if (not f_ptr->isRemoving())
+            f_ptr->getFactorList(_fac_list);
 }
 
 void CaptureBase::getFactorList(FactorBasePtrList& _fac_list)
 {
     for (auto f_ptr : getFeatureList())
-        f_ptr->getFactorList(_fac_list);
+        if (not f_ptr->isRemoving())
+            f_ptr->getFactorList(_fac_list);
 }
 
 FactorBasePtr CaptureBase::addConstrainedBy(FactorBasePtr _fac_ptr)

src/frame/frame_base.cpp

@@ -351,13 +351,15 @@ FactorBasePtrList FrameBase::getFactorList()
 void FrameBase::getFactorList(FactorBaseConstPtrList& _fac_list) const
 {
     for (auto c_ptr : getCaptureList())
-        c_ptr->getFactorList(_fac_list);
+        if (not c_ptr->isRemoving())
+            c_ptr->getFactorList(_fac_list);
 }
 
 void FrameBase::getFactorList(FactorBasePtrList& _fac_list)
 {
     for (auto c_ptr : getCaptureList())
-        c_ptr->getFactorList(_fac_list);
+        if (not c_ptr->isRemoving())
+            c_ptr->getFactorList(_fac_list);
 }
 
 bool FrameBase::hasCapture(const CaptureBaseConstPtr& _capture) const

src/processor/processor_motion.cpp

@@ -194,7 +194,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
     if (keyframe_from_callback)
         buffer_frame_.removeUpTo( keyframe_from_callback->getTimeStamp() );
     
-    switch(processing_step_)
+    switch(processing_step_) // Things to do before integrating motion data
     {
         case FIRST_TIME_WITHOUT_KF :
         {
@@ -229,14 +229,14 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
     }
 
 
-    // integrate data
+    // integrate motion data
     // Done at this place because setPrior() needs 
     integrateOneStep();
 
     // perform bootstrap steps (usually only IMU requires this)
     if (bootstrapping_) bootstrap();
 
-    switch (processing_step_)
+    switch (processing_step_) // Things to do after integrating motion data
     {
         case RUNNING_WITH_KF_BEFORE_ORIGIN :
         {
@@ -272,11 +272,11 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
             TimeStamp timestamp_from_callback = keyframe_from_callback->getTimeStamp();
 
             // find the capture whose buffer is affected by the new keyframe
-            auto capture_existing   = findCaptureContainingTimeStamp(timestamp_from_callback); // k
+            auto capture_existing   = findCaptureContainingTimeStamp(timestamp_from_callback); 
 
             if (!capture_existing)
             {
-                WOLF_WARN("A KF before first motion capture (TS = ", timestamp_from_callback, "). ProcessorMotion cannot do anything.");
+                WOLF_WARN(getName(), ": Cannot join KF. The received KF (TS = ", timestamp_from_callback, ") is older than the first motion capture.");
                 break;
             }
 
@@ -438,7 +438,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
          *    x : any capture
          *    o : origin capture
          *    l : last capture      -> we'll make a KF here
-         *    i : incoming capture
+         *    i : incoming capture  -> data has already been integrated into last capture
          *    n : new capture       ->
          *  --- : buffer history
          *
@@ -560,8 +560,6 @@ VectorComposite ProcessorMotion::getState(const StateStructure& _structure) cons
     auto calib_preint = last_ptr_->getCalibrationPreint();
 
     VectorComposite state;
-    //WOLF_INFO("processorMotion last timestamp: ", last_ptr_->getTimeStamp());
-    //WOLF_INFO("processorMotion origin timestamp: ", origin_ptr_->getTimeStamp());
     if ( hasCalibration())
     {
         // Get current calibration -- from origin capture
@@ -606,7 +604,6 @@ VectorComposite ProcessorMotion::getState(const StateStructure& _structure) cons
 
 
 
-// _x needs to have the size of the processor state
 VectorComposite ProcessorMotion::getState(const TimeStamp& _ts, const StateStructure& _structure) const
 {
     assert(_ts.ok());
@@ -759,6 +756,9 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
 
 void ProcessorMotion::integrateOneStep()
 {
+    /* Integrate motiondata in incoming_ptr_ and store all results in the MotionBuffer in last_ptr_
+     */
+
     // Set dt
     dt_ = updateDt();
     assert(dt_ >= 0 && "Time interval _dt is negative!");

src/trajectory/trajectory_base.cpp

@@ -54,13 +54,15 @@ void TrajectoryBase::removeFrame(FrameBasePtr _frame_ptr)
 void TrajectoryBase::getFactorList(FactorBaseConstPtrList & _fac_list) const
 {
 	for(auto fr_pair: frame_map_)
-		fr_pair.second->getFactorList(_fac_list);
+        if (not fr_pair.second->isRemoving())
+	    	fr_pair.second->getFactorList(_fac_list);
 }
 
 void TrajectoryBase::getFactorList(FactorBasePtrList & _fac_list)
 {
 	for(auto fr_pair: frame_map_)
-		fr_pair.second->getFactorList(_fac_list);
+        if (not fr_pair.second->isRemoving())
+    		fr_pair.second->getFactorList(_fac_list);
 }
 
 TimeStamp TrajectoryBase::closestTimeStampToTimeStamp(const TimeStamp& _ts) const