From 2ccbe83e53124d298eb1b209592b921f2cd4fef0 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?M=C3=A9d=C3=A9ric=20Fourmy?= <mfourmy@laas.fr>
Date: Tue, 9 Jun 2020 12:11:37 +0200
Subject: [PATCH] Proposal to deal with integrateOneStep() before handling KF
pack
---
include/core/processor/processor_motion.h | 3 ++-
src/processor/processor_motion.cpp | 33 ++++++++++++++++-------
2 files changed, 25 insertions(+), 11 deletions(-)
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index 8533e66ca..41787e38e 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -484,7 +484,8 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
CaptureMotionPtr origin_ptr_;
CaptureMotionPtr last_ptr_;
CaptureMotionPtr incoming_ptr_;
-
+ bool last_nullptr_;
+
protected:
// helpers to avoid allocation
double dt_; ///< Time step
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index ead3fc9e5..5cb8b4290 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -33,6 +33,7 @@ ProcessorMotion::ProcessorMotion(const std::string& _type,
origin_ptr_(),
last_ptr_(),
incoming_ptr_(),
+ last_nullptr_(false),
dt_(0.0),
x_(_state_size),
delta_(_delta_size),
@@ -95,10 +96,19 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
preProcess(); // Derived class operations
+ // integrate data
+ if (last_ptr_ != nullptr)
+ {
+ integrateOneStep();
+ }
+ else {
+ last_nullptr_ = true;
+ }
+
PackKeyFramePtr pack = computeProcessingStep();
if (pack)
buffer_pack_kf_.removeUpTo( pack->key_frame->getTimeStamp() );
-
+
switch(processing_step_)
{
case FIRST_TIME_WITHOUT_KF :
@@ -161,13 +171,15 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// extract pack elements
FrameBasePtr keyframe_from_callback = pack->key_frame;
+ TimeStamp ts_from_callback = keyframe_from_callback->getTimeStamp();
+ keyframe_from_callback->setState(getState(ts_from_callback));
// find the capture whose buffer is affected by the new keyframe
- auto capture_existing = findCaptureContainingTimeStamp(keyframe_from_callback->getTimeStamp()); // k
+ auto capture_existing = findCaptureContainingTimeStamp(ts_from_callback); // k
if (!capture_existing)
{
- WOLF_WARN("A KF before first motion capture (TS = ", keyframe_from_callback->getTimeStamp(), "). ProcessorMotion cannot do anything.");
+ WOLF_WARN("A KF before first motion capture (TS = ", ts_from_callback, "). ProcessorMotion cannot do anything.");
break;
}
@@ -178,7 +190,6 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
VectorXd calib_origin = getCalibration(capture_origin);
// emplace a new motion capture to the new keyframe
- TimeStamp ts_from_callback = keyframe_from_callback->getTimeStamp();
auto capture_for_keyframe_callback = emplaceCapture(keyframe_from_callback, // j
getSensor(),
ts_from_callback,
@@ -260,6 +271,8 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// extract pack elements
FrameBasePtr keyframe_from_callback = pack->key_frame;
+ TimeStamp ts_from_callback = keyframe_from_callback->getTimeStamp();
+ keyframe_from_callback->setState(getState(ts_from_callback));
auto & capture_existing = last_ptr_;
@@ -270,7 +283,6 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
VectorXd calib_origin = getCalibration(capture_origin);
// emplace a new motion capture to the new keyframe
- TimeStamp ts_from_callback = keyframe_from_callback->getTimeStamp();
auto capture_for_keyframe_callback = emplaceCapture(keyframe_from_callback,
getSensor(),
ts_from_callback,
@@ -300,17 +312,18 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
break;
}
- ////////////////////////////////////////////////////
- // NOW on with the received data
+ if (last_nullptr_){
+ integrateOneStep();
+ last_nullptr_ = false;
+ }
- // integrate data
- integrateOneStep();
// Update state and time stamps
const auto& ts = getTimeStamp();
last_ptr_->setTimeStamp( ts );
last_ptr_->getFrame()->setTimeStamp( ts );
- last_ptr_->getFrame()->setState( getProblem()->getState( ts ) );
+ VectorComposite state_propa = getState( ts );
+ last_ptr_->getFrame()->setState( state_propa );
if (permittedKeyFrame() && voteForKeyFrame())
{
--
GitLab