diff --git a/include/core/factor/factor_base.h b/include/core/factor/factor_base.h
index 1894148de2ea576614b14d6de81a4f138c4e037c..0ca91c9c6bb5db6577e268e9f159da0f09a89a4e 100644
--- a/include/core/factor/factor_base.h
+++ b/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;
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index 3bf0b996153858d5904181832a463ec4c1854e1c..c286d247e34676cbc687fff1ac8db2872b068980 100644
--- a/include/core/processor/processor_motion.h
+++ b/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,
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index e6d4a1e660bd0664f2fa1196c2ab558840664263..a154cdbb4b62606dc5260310cbfd5b4052bda54c 100644
--- a/src/capture/capture_base.cpp
+++ b/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)
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index aae21b6eb2fa9d5552e8212259299b71e0e0c440..390d6d51019aea6a65208707b1b8671194dc060e 100644
--- a/src/frame/frame_base.cpp
+++ b/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
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 53f6f71589987ef9f04f60e55aea91c3ba6bb981..3c014ca6a55e4b9416b1e39c41ed0b25d4921418 100644
--- a/src/processor/processor_motion.cpp
+++ b/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!");
diff --git a/src/trajectory/trajectory_base.cpp b/src/trajectory/trajectory_base.cpp
index 24de3de294a6e65ea409d345a31a23393904ae50..244d6a505d66143823e6a67456c33972ff86a588 100644
--- a/src/trajectory/trajectory_base.cpp
+++ b/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