diff --git a/src/processor_motion.cpp b/src/processor_motion.cpp
index 0abab214309a12779c0d916ac559ca4afbbd1339..221c7f0a0ec6a47683d118c9052ad21a5ae940b7 100644
--- a/src/processor_motion.cpp
+++ b/src/processor_motion.cpp
@@ -39,6 +39,36 @@ ProcessorMotion::~ProcessorMotion()
// std::cout << "destructed -p-Mot" << id() << std::endl;
}
+void ProcessorMotion::splitBuffer(const wolf::CaptureMotionPtr& _capture_source,
+ TimeStamp _ts_split,
+ const FrameBasePtr& _keyframe_target,
+ const wolf::CaptureMotionPtr& _capture_target)
+{
+ // 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());
+
+ // interpolate individual delta which has been cut by the split timestamp
+ if (!_capture_source->getBuffer().get().empty()
+ && _capture_target->getBuffer().get().back().ts_ != _ts_split)
+ {
+ // interpolate Motion at the new time stamp
+ Motion motion_interpolated = interpolate(_capture_target->getBuffer().get().back(), // last Motion of old buffer
+ _capture_source->getBuffer().get().front(), // first motion of new buffer
+ _ts_split,
+ _capture_source->getBuffer().get().front());
+
+ // add to old buffer
+ _capture_target->getBuffer().get().push_back(motion_interpolated);
+ }
+
+ // Update the existing capture
+ _capture_source->setOriginFramePtr(_keyframe_target);
+
+ // re-integrate existing buffer -- note: the result of re-integration is stored in the same buffer!
+ reintegrateBuffer(_capture_source);
+}
+
void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
{
if (_incoming_ptr == nullptr)
@@ -86,20 +116,7 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
// split the buffer
// and give the part of the buffer before the new keyframe to the capture for the KF callback
- existing_capture->getBuffer().split(ts_from_callback, capture_for_keyframe_callback->getBuffer());
-
-
- // interpolate individual delta
- if (!existing_capture->getBuffer().get().empty() && capture_for_keyframe_callback->getBuffer().get().back().ts_ != ts_from_callback)
- {
- // interpolate Motion at the new time stamp
- Motion motion_interpolated = interpolate(capture_for_keyframe_callback->getBuffer().get().back(), // last Motion of old buffer
- existing_capture->getBuffer().get().front(), // first motion of new buffer
- ts_from_callback);
- // add to old buffer
- capture_for_keyframe_callback->getBuffer().get().push_back(motion_interpolated);
- }
-
+ splitBuffer(existing_capture, ts_from_callback, keyframe_from_callback, capture_for_keyframe_callback);
// create motion feature and add it to the capture
auto new_feature = emplaceFeature(capture_for_keyframe_callback);
@@ -107,12 +124,6 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
// create motion constraint and add it to the feature, and constrain to the other capture (origin)
emplaceConstraint(new_feature, keyframe_origin->getCaptureOf(getSensorPtr()) );
- // Update the existing capture
- existing_capture->setOriginFramePtr(keyframe_from_callback);
-
- // re-integrate existing buffer -- note: the result of re-integration is stored in the same buffer!
- reintegrateBuffer(existing_capture);
-
// modify existing feature and constraint (if they exist in the existing capture)
if (!existing_capture->getFeatureList().empty())
{
@@ -154,18 +165,7 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
// split the buffer
// and give the part of the buffer before the new keyframe to the capture for the KF callback
- last_ptr_->getBuffer().split(ts_from_callback, capture_for_keyframe_callback->getBuffer());
-
- // interpolate individual delta
- if (!last_ptr_->getBuffer().get().empty() && capture_for_keyframe_callback->getBuffer().get().back().ts_ != ts_from_callback)
- {
- // interpolate Motion at the new time stamp
- Motion motion_interpolated = interpolate(capture_for_keyframe_callback->getBuffer().get().back(), // last Motion of old buffer
- last_ptr_->getBuffer().get().front(), // first motion of new buffer
- ts_from_callback);
- // add to old buffer
- capture_for_keyframe_callback->getBuffer().get().push_back(motion_interpolated);
- }
+ splitBuffer(last_ptr_, ts_from_callback, keyframe_from_callback, capture_for_keyframe_callback);
// create motion feature and add it to the capture
auto feature_for_keyframe_callback = emplaceFeature(capture_for_keyframe_callback);
@@ -176,12 +176,6 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
// reset processor origin
origin_ptr_ = capture_for_keyframe_callback;
- // Update the existing capture
- last_ptr_->setOriginFramePtr(keyframe_from_callback);
-
- // re-integrate existing buffer -- note: the result of re-integration is stored in the same buffer!
- reintegrateBuffer(last_ptr_);
-
break;
}
diff --git a/src/processor_motion.h b/src/processor_motion.h
index e399bddcce6562e265d540c9e5699cd8e7f64193..daf7d71880f3408f55e54f7bf6974a59eae16388 100644
--- a/src/processor_motion.h
+++ b/src/processor_motion.h
@@ -207,8 +207,11 @@ class ProcessorMotion : public ProcessorBase
Scalar updateDt();
void integrateOneStep();
- void splitBuffer(const TimeStamp& _t_split, MotionBuffer& _oldest_part);
void reintegrateBuffer(CaptureMotionPtr _capture_ptr);
+ void splitBuffer(const wolf::CaptureMotionPtr& capture_source,
+ TimeStamp ts_split,
+ const FrameBasePtr& keyframe_target,
+ const wolf::CaptureMotionPtr& capture_target);
/** Pre-process incoming Capture
*
@@ -476,7 +479,6 @@ class ProcessorMotion : public ProcessorBase
Eigen::MatrixXs jacobian_delta_; ///< jacobian of delta composition w.r.t current delta
Eigen::MatrixXs jacobian_calib_; ///< jacobian of delta preintegration wrt calibration params
Eigen::MatrixXs jacobian_delta_calib_; ///< jacobian of delta wrt calib params
-
};
}
@@ -485,11 +487,6 @@ class ProcessorMotion : public ProcessorBase
namespace wolf{
-inline void ProcessorMotion::splitBuffer(const TimeStamp& _t_split, MotionBuffer& _oldest_part)
-{
- last_ptr_->getBuffer().split(_t_split, _oldest_part);
-}
-
inline void ProcessorMotion::resetDerived()
{
// Blank function, to be implemented in derived classes