diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index c286d247e34676cbc687fff1ac8db2872b068980..cdc54800b6c700e2ab9e8ca41204a679c3021b79 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -313,7 +313,7 @@ class ProcessorMotion : public ProcessorBase, public MotionProvider
FrameBasePtr computeProcessingStep();
-
+ void assertsCaptureMotion(CaptureMotionPtr _capture, std::string error_prefix) const;
// These are the pure virtual functions doing the mathematics
public:
diff --git a/src/processor/processor_landmark_external.cpp b/src/processor/processor_landmark_external.cpp
index c1a39970d7890adc75d733934fefa9032bf07ba5..075770934d5cc7d87ec21015e95a89cf0a1b3905 100644
--- a/src/processor/processor_landmark_external.cpp
+++ b/src/processor/processor_landmark_external.cpp
@@ -41,7 +41,7 @@ namespace wolf
void ProcessorLandmarkExternal::preProcess()
{
- assert(new_features_incoming_.empty());
+ new_features_incoming_.clear();
auto dim = getProblem()->getDim();
auto cap_landmarks = std::dynamic_pointer_cast<CaptureLandmarksExternal>(incoming_ptr_);
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index e5f7e4af014001df01d5f91ef45431f8c2284581..2f9d13be58d3f6c6b9ebf3e3e7021717acad199c 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -87,10 +87,10 @@ ProcessorMotion::~ProcessorMotion()
void ProcessorMotion::mergeCaptures(CaptureMotionPtr cap_prev,
CaptureMotionPtr cap_target)
{
- assert(cap_prev != nullptr);
- assert(cap_target != nullptr);
+ assertsCaptureMotion(cap_prev, "ProcessorMotion::mergeCaptures: cap_prev");
+ assertsCaptureMotion(cap_target, "ProcessorMotion::mergeCaptures: cap_target");
+
assert(cap_prev == cap_target->getOriginCapture() && "ProcessorMotion::mergeCaptures: merging not consecutive capture motions");
- assert((cap_prev->getBuffer().front().delta_integr_ - deltaZero()).isZero(Constants::EPS_SMALL) && "ProcessorMotion::mergeCaptures: cap_prev buffer's first motion is not zero!");
// add prev buffer (discarding the first zero motion)
cap_target->getBuffer().pop_front();
@@ -145,6 +145,7 @@ void ProcessorMotion::splitBuffer(const CaptureMotionPtr& _capture_source,
*
*/
// CHECKS
+ assertsCaptureMotion(_capture_source, "ProcessorMotion::splitBuffer _capture_source (before split)");
if (checkTimeTolerance(_capture_source, _ts_split))
{
WOLF_ERROR("ProcessorMotion::splitBuffer: _capture_source.ts == ts_split within tolerance, shouldn't split!");
@@ -166,6 +167,11 @@ void ProcessorMotion::splitBuffer(const CaptureMotionPtr& _capture_source,
throw std::runtime_error("");
}
+ // guarantee that capture_target is empty
+ WOLF_WARN_COND(not _capture_target->getBuffer().empty(), "ProcessorMotion::splitBuffer: capture_target buffer not empty! size: ", _capture_target->getBuffer().size());
+ if (not _capture_target->getBuffer().empty())
+ _capture_target->getBuffer().clear();
+
// split the buffer
// and give the part of the buffer before the new keyframe to the capture for the KF callback
_capture_source->getBuffer().split(_ts_split, _capture_target->getBuffer());
@@ -188,6 +194,9 @@ void ProcessorMotion::splitBuffer(const CaptureMotionPtr& _capture_source,
// re-integrate source capture's buffer -- note: the result of re-integration is stored in the same buffer!
reintegrateBuffer(_capture_source);
+
+ assertsCaptureMotion(_capture_source, "ProcessorMotion::splitBuffer _capture_source (after split)");
+ assertsCaptureMotion(_capture_target, "ProcessorMotion::splitBuffer _capture_target (after split)");
}
void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
@@ -757,6 +766,9 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
getCalibration(),
nullptr);
+ // add zero motion (just for consistency)
+ origin_ptr_->getBuffer().push_back(motionZero(_origin_frame->getTimeStamp()));
+
// ---------- LAST ----------
// Make non-key-frame for last Capture
last_frame_ptr_ = std::make_shared<FrameBase>(origin_ts,
@@ -845,6 +857,9 @@ void ProcessorMotion::integrateOneStep()
void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr) const
{
+ // check capture (not null, not empty buffer, first buffer's motion zero)
+ assertsCaptureMotion(_capture_ptr, "ProcessorMotion::reintegrateBuffer");
+
VectorXd calib = _capture_ptr->getCalibrationPreint();
// some temporaries for integration
@@ -852,11 +867,6 @@ void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr) const
delta_integrated_cov_.setZero();
jacobian_calib_ .setZero();
- // check that first motion in buffer is zero!
- assert((_capture_ptr->getBuffer().front().delta_integr_ - delta_integrated_).isZero(Constants::EPS_SMALL) && "Buffer's first motion is not zero!");
- assert(_capture_ptr->getBuffer().front().delta_integr_cov_.isZero(Constants::EPS_SMALL) && "Buffer's first motion covariance is not zero!");
- assert(_capture_ptr->getBuffer().front().jacobian_calib_.isZero(Constants::EPS_SMALL) && "Buffer's first motion jacobian is not zero!");
-
// Iterate all the buffer
auto motion_it = _capture_ptr->getBuffer().begin();
auto prev_motion_it = motion_it;
@@ -1071,6 +1081,15 @@ FrameBasePtr ProcessorMotion::computeProcessingStep()
return nullptr;
}
+void ProcessorMotion::assertsCaptureMotion(CaptureMotionPtr _capture, std::string error_prefix) const
+{
+ assert(_capture != nullptr && (error_prefix + ": null capture").c_str());
+ assert(not _capture->getBuffer().empty() && (error_prefix + ": empty buffer").c_str());
+ assert((_capture->getBuffer().front().delta_integr_ - deltaZero()).isZero(Constants::EPS_SMALL) && (error_prefix + ": buffer's first motion is not zero!").c_str());
+ assert(_capture->getBuffer().front().delta_integr_cov_.isZero(Constants::EPS_SMALL) && (error_prefix + ": Buffer's first motion covariance is not zero!").c_str());
+ assert(_capture->getBuffer().front().jacobian_calib_.isZero(Constants::EPS_SMALL) && (error_prefix + ": Buffer's first motion jacobian is not zero!").c_str());
+}
+
TimeStamp ProcessorMotion::getTimeStamp ( ) const
{
if (not origin_ptr_ or