diff --git a/src/hello_wolf/processor_range_bearing.cpp b/src/hello_wolf/processor_range_bearing.cpp
index 0974d8b0d99d4e6d883ab238427f42dfc2fad390..3991741d59036ae0fb476e0edbca531866ff5d78 100644
--- a/src/hello_wolf/processor_range_bearing.cpp
+++ b/src/hello_wolf/processor_range_bearing.cpp
@@ -22,6 +22,18 @@ ProcessorRangeBearing::ProcessorRangeBearing(const SensorRangeBearingPtr _sensor
void ProcessorRangeBearing::process(CaptureBasePtr _capture)
{
+
+ if ( !kf_pack_buffer_.empty() )
+ {
+ // Select using incoming_ptr
+ KFPackPtr pack = kf_pack_buffer_.selectPack( _capture->getTimeStamp(), time_tolerance_ );
+
+ if (pack!=nullptr)
+ keyFrameCallback(pack->key_frame,pack->time_tolerance);
+
+ kf_pack_buffer_.removeUpTo( _capture->getTimeStamp() );
+ }
+
CaptureRangeBearingPtr capture = std::static_pointer_cast<CaptureRangeBearing>(_capture);
// 1. get KF -- we assume a KF is available to hold this _capture (checked in assert below)
diff --git a/src/problem.cpp b/src/problem.cpp
index e801e145080c655d08a7db25feb8c2649881b9d3..fa7ef55ef2c4590a07c429cd2829dbd072243535 100644
--- a/src/problem.cpp
+++ b/src/problem.cpp
@@ -334,7 +334,7 @@ void Problem::keyFrameCallback(FrameBasePtr _keyframe_ptr, ProcessorBasePtr _pro
for (auto sensor : hardware_ptr_->getSensorList())
for (auto processor : sensor->getProcessorList())
if (processor && (processor != _processor_ptr) )
- processor->keyFrameCallback(_keyframe_ptr, _time_tolerance);
+ processor->keyFrameCallbackNew(_keyframe_ptr, _time_tolerance);
}
LandmarkBasePtr Problem::addLandmark(LandmarkBasePtr _lmk_ptr)
diff --git a/src/processor_base.cpp b/src/processor_base.cpp
index 430ed8ad9d4cda1b154c9e4ab331e1863a30d702..18701229dab299956def1c10bddfac5030ae0e3c 100644
--- a/src/processor_base.cpp
+++ b/src/processor_base.cpp
@@ -45,6 +45,13 @@ FrameBasePtr ProcessorBase::emplaceFrame(FrameType _type, CaptureBasePtr _captur
return new_frame_ptr;
}
+void ProcessorBase::keyFrameCallbackNew(FrameBasePtr _keyframe_ptr, const Scalar& _time_tol_other)
+{
+ if (_keyframe_ptr != nullptr)
+ kf_pack_buffer_.add(_keyframe_ptr,_time_tol_other);
+
+}
+
void ProcessorBase::remove()
{
if (!is_removing_)
@@ -67,9 +74,9 @@ void ProcessorBase::remove()
}
}
-void KFPackBuffer::removeUpTo(const KFPackPtr& _pack)
+void KFPackBuffer::removeUpTo(const TimeStamp& _time_stamp)
{
- KFPackBuffer::Iterator post = container_.upper_bound(_pack->key_frame->getTimeStamp());
+ KFPackBuffer::Iterator post = container_.upper_bound(_time_stamp);
container_.erase(container_.begin(), post); // erasing by range
}
diff --git a/src/processor_base.h b/src/processor_base.h
index dc6eaee20c1e01d6b9af17b3a290f959cbc6d24c..50c78c5a6687f913dea780d0d2cae5123fda8117 100644
--- a/src/processor_base.h
+++ b/src/processor_base.h
@@ -43,7 +43,7 @@ class KFPackBuffer
void add(const FrameBasePtr& _key_frame, const Scalar& _time_tolerance);
- void removeUpTo(const KFPackPtr& _pack);
+ void removeUpTo(const TimeStamp& _time_stamp);
bool checkTimeTolerance(const TimeStamp& _time_stamp1, const Scalar& _time_tolerance1, const TimeStamp& _time_stamp2, const Scalar& _time_tolerance2);
@@ -121,6 +121,8 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
virtual bool keyFrameCallback(FrameBasePtr _keyframe_ptr, const Scalar& _time_tolerance) = 0;
+ void keyFrameCallbackNew(FrameBasePtr _keyframe_ptr, const Scalar& _time_tol_other);
+
SensorBasePtr getSensorPtr();
const SensorBasePtr getSensorPtr() const;
void setSensorPtr(SensorBasePtr _sen_ptr){sensor_ptr_ = _sen_ptr;}
@@ -131,7 +133,10 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
protected:
unsigned int processor_id_;
+
Scalar time_tolerance_; ///< self time tolerance for adding a capture into a frame
+
+ KFPackBuffer kf_pack_buffer_;
};
}
diff --git a/src/processor_motion.cpp b/src/processor_motion.cpp
index a8e94f2432955dc03d0068f8f9f46a4079d7cb78..168f96b7120a50668d3307d24c5da06ed32345f1 100644
--- a/src/processor_motion.cpp
+++ b/src/processor_motion.cpp
@@ -42,103 +42,128 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
{
if (_incoming_ptr == nullptr)
{
- WOLF_ERROR("Process got a nullptr !");
+ WOLF_ERROR("Received capture is nullptr.");
return;
}
- if (status_ == IDLE)
- {
- TimeStamp t0 = _incoming_ptr->getTimeStamp();
+ if ( !kf_pack_buffer_.empty() )
+ {
+ KFPackPtr pack;
+
+ // Select using last_ptr
+ if (last_ptr_ != nullptr)
+ {
+ pack = kf_pack_buffer_.selectPack( last_ptr_->getTimeStamp(), time_tolerance_ );
+ if (pack!=nullptr)
+ {
+ keyFrameCallback(pack->key_frame,pack->time_tolerance);
+ kf_pack_buffer_.removeUpTo( last_ptr_->getTimeStamp() );
+ }
+ }
+
+ // Select using incoming_ptr
+ pack = kf_pack_buffer_.selectPack( incoming_ptr_->getTimeStamp(), time_tolerance_ );
+ if (pack!=nullptr)
+ {
+ keyFrameCallback(pack->key_frame,pack->time_tolerance);
+ kf_pack_buffer_.removeUpTo( incoming_ptr_->getTimeStamp() );
+ }
+ }
- if (origin_ptr_ == nullptr)
- {
- auto frm = getProblem()->getTrajectoryPtr()->closestKeyFrameToTimeStamp(t0);
- if (frm && fabs(frm->getTimeStamp() - t0) < time_tolerance_)
- {
- std::cout << "PM: join KF" << std::endl;
- // Join existing KF
- setOrigin(frm);
- }
- else
- {
- // Create new KF for origin
- std::cout << "PM: make KF" << std::endl;
- VectorXs x0 = getProblem()->zeroState();
- setOrigin(x0, t0);
- }
- }
- status_ = RUNNING;
- }
- incoming_ptr_ = std::static_pointer_cast<CaptureMotion>(_incoming_ptr);
+ if (status_ == IDLE)
+ {
+ TimeStamp t0 = _incoming_ptr->getTimeStamp();
+
+ if (origin_ptr_ == nullptr)
+ {
+ auto frm = getProblem()->getTrajectoryPtr()->closestKeyFrameToTimeStamp(t0);
+ if (frm && fabs(frm->getTimeStamp() - t0) < time_tolerance_)
+ {
+ std::cout << "PM: join KF" << std::endl;
+ // Join existing KF
+ setOrigin(frm);
+ }
+ else
+ {
+ // Create new KF for origin
+ std::cout << "PM: make KF" << std::endl;
+ VectorXs x0 = getProblem()->zeroState();
+ setOrigin(x0, t0);
+ }
+ }
+ status_ = RUNNING;
+ }
- /// @todo Anything else to do ?
- if (incoming_ptr_ == nullptr) return;
+ incoming_ptr_ = std::static_pointer_cast<CaptureMotion>(_incoming_ptr);
- preProcess();
+ /// @todo Anything else to do ?
+ if (incoming_ptr_ == nullptr) return;
- // integrate data
- integrateOneStep();
+ preProcess();
- // Update state and time stamps
- last_ptr_->setTimeStamp(incoming_ptr_->getTimeStamp());
- last_ptr_->getFramePtr()->setTimeStamp(last_ptr_->getTimeStamp());
- last_ptr_->getFramePtr()->setState(getCurrentState());
+ // integrate data
+ integrateOneStep();
- if (voteForKeyFrame() && permittedKeyFrame())
- {
- // Set the frame of last_ptr as key
- auto key_frame_ptr = last_ptr_->getFramePtr();
- key_frame_ptr->setState(getCurrentState());
- key_frame_ptr->setTimeStamp(getCurrentTimeStamp());
- key_frame_ptr->setKey();
-
- // create motion feature and add it to the key_capture
- auto key_feature_ptr = emplaceFeature(last_ptr_);
-
- // create motion constraint and link it to parent feature and other frame (which is origin's frame)
- auto ctr_ptr = emplaceConstraint(key_feature_ptr, origin_ptr_);
-
- // create a new frame
- auto new_frame_ptr = getProblem()->emplaceFrame(NON_KEY_FRAME,
- getCurrentState(),
- getCurrentTimeStamp());
- // create a new capture
- auto new_capture_ptr = emplaceCapture(new_frame_ptr,
- getSensorPtr(),
- key_frame_ptr->getTimeStamp(),
- Eigen::VectorXs::Zero(data_size_),
- Eigen::MatrixXs::Zero(data_size_, data_size_),
- last_ptr_->getCalibration(),
- last_ptr_->getCalibration(),
- key_frame_ptr);
- // reset the new buffer
- new_capture_ptr->getBuffer().get().clear();
- new_capture_ptr->getBuffer().get().push_back( motionZero(key_frame_ptr->getTimeStamp()) ) ;
-
- // reset integrals
- delta_ = deltaZero();
- delta_cov_ . setZero();
- delta_integrated_ = deltaZero();
- delta_integrated_cov_ . setZero();
- jacobian_calib_ . setZero();
-
- // reset processor origin to the new keyframe's capture
- origin_ptr_ = last_ptr_;
- last_ptr_ = new_capture_ptr;
-
- // reset derived things
- resetDerived();
-
- // callback to other processors
- getProblem()->keyFrameCallback(key_frame_ptr, shared_from_this(), time_tolerance_);
- }
+ // Update state and time stamps
+ last_ptr_->setTimeStamp(incoming_ptr_->getTimeStamp());
+ last_ptr_->getFramePtr()->setTimeStamp(last_ptr_->getTimeStamp());
+ last_ptr_->getFramePtr()->setState(getCurrentState());
+
+ if (voteForKeyFrame() && permittedKeyFrame())
+ {
+ // Set the frame of last_ptr as key
+ auto key_frame_ptr = last_ptr_->getFramePtr();
+ key_frame_ptr->setState(getCurrentState());
+ key_frame_ptr->setTimeStamp(getCurrentTimeStamp());
+ key_frame_ptr->setKey();
+
+ // create motion feature and add it to the key_capture
+ auto key_feature_ptr = emplaceFeature(last_ptr_);
+
+ // create motion constraint and link it to parent feature and other frame (which is origin's frame)
+ auto ctr_ptr = emplaceConstraint(key_feature_ptr, origin_ptr_);
+
+ // create a new frame
+ auto new_frame_ptr = getProblem()->emplaceFrame(NON_KEY_FRAME,
+ getCurrentState(),
+ getCurrentTimeStamp());
+ // create a new capture
+ auto new_capture_ptr = emplaceCapture(new_frame_ptr,
+ getSensorPtr(),
+ key_frame_ptr->getTimeStamp(),
+ Eigen::VectorXs::Zero(data_size_),
+ Eigen::MatrixXs::Zero(data_size_, data_size_),
+ last_ptr_->getCalibration(),
+ last_ptr_->getCalibration(),
+ key_frame_ptr);
+ // reset the new buffer
+ new_capture_ptr->getBuffer().get().clear();
+ new_capture_ptr->getBuffer().get().push_back( motionZero(key_frame_ptr->getTimeStamp()) ) ;
+
+ // reset integrals
+ delta_ = deltaZero();
+ delta_cov_ . setZero();
+ delta_integrated_ = deltaZero();
+ delta_integrated_cov_ . setZero();
+ jacobian_calib_ . setZero();
+
+ // reset processor origin to the new keyframe's capture
+ origin_ptr_ = last_ptr_;
+ last_ptr_ = new_capture_ptr;
+
+ // reset derived things
+ resetDerived();
+
+ // callback to other processors
+ getProblem()->keyFrameCallback(key_frame_ptr, shared_from_this(), time_tolerance_);
+ }
- postProcess();
+ postProcess();
- // clear incoming just in case
- incoming_ptr_ = nullptr; // This line is not really needed, but it makes things clearer.
+ // clear incoming just in case
+ incoming_ptr_ = nullptr; // This line is not really needed, but it makes things clearer.
}
void ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXs& _x)
diff --git a/src/processor_tracker.cpp b/src/processor_tracker.cpp
index cc2b764e11126f549d8910ae60f838eacc0fb5fe..2a8de624180b28be6568ab3bc364d5bd7f533155 100644
--- a/src/processor_tracker.cpp
+++ b/src/processor_tracker.cpp
@@ -28,6 +28,60 @@ ProcessorTracker::~ProcessorTracker()
void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
{
+ if (_incoming_ptr == nullptr)
+ {
+ WOLF_ERROR("Received capture is nullptr.");
+ return;
+ }
+
+ WOLF_TRACE("");
+
+ if ( !kf_pack_buffer_.empty() )
+ {
+ KFPackPtr pack;
+
+ WOLF_TRACE("");
+
+ // Select using last_ptr
+ if (last_ptr_ != nullptr)
+ {
+
+ WOLF_TRACE("");
+
+ pack = kf_pack_buffer_.selectPack( last_ptr_->getTimeStamp(), time_tolerance_ );
+ if (pack!=nullptr)
+ {
+
+ WOLF_TRACE("");
+
+ keyFrameCallback(pack->key_frame,pack->time_tolerance);
+
+ WOLF_TRACE("");
+
+ kf_pack_buffer_.removeUpTo( last_ptr_->getTimeStamp() );
+ }
+ }
+
+ WOLF_TRACE("");
+
+ // Select using incoming_ptr
+ pack = kf_pack_buffer_.selectPack( incoming_ptr_->getTimeStamp(), time_tolerance_ );
+
+ WOLF_TRACE("");
+
+ if (pack!=nullptr)
+ {
+ WOLF_TRACE("");
+
+ keyFrameCallback(pack->key_frame,pack->time_tolerance);
+
+ WOLF_TRACE("");
+
+ kf_pack_buffer_.removeUpTo( incoming_ptr_->getTimeStamp() );
+ }
+ }
+
+ WOLF_TRACE("");
using std::abs;
@@ -35,6 +89,8 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
preProcess();
+ WOLF_TRACE("");
+
// FIRST TIME
if (origin_ptr_ == nullptr && last_ptr_ == nullptr)
{
diff --git a/src/test/gtest_processor_base.cpp b/src/test/gtest_processor_base.cpp
index 3750689edb419bec46bbcd7a05f6ab6829dd030d..7f6633eafc224d2ad78e5748ab2d104d6cac7c5d 100644
--- a/src/test/gtest_processor_base.cpp
+++ b/src/test/gtest_processor_base.cpp
@@ -5,13 +5,20 @@
* Author: asantamaria
*/
-
+//Wolf
#include "utils_gtest.h"
-#include "processor_base.h"
+#include "processor_odom_2D.h"
+#include "sensor_odom_2D.h"
+
+#include "processor_tracker_feature_dummy.h"
+#include "capture_void.h"
+
+#include "problem.h"
// STL
#include <iterator>
+#include <iostream>
using namespace wolf;
using namespace Eigen;
@@ -148,7 +155,7 @@ TEST_F(KFPackBufferTest, removeUpTo)
// it should remove f20 and f10, thus size should be 1 after removal
// Specifically, only f21 should remain
KFPackPtr pack20 = std::make_shared<KFPack>(f20,tt20);
- kfpackbuffer.removeUpTo( pack20 );
+ kfpackbuffer.removeUpTo( pack20->key_frame->getTimeStamp() );
ASSERT_EQ(kfpackbuffer.size(),1);
ASSERT_TRUE(kfpackbuffer.selectPack(f10->getTimeStamp(),tt10)==nullptr);
ASSERT_TRUE(kfpackbuffer.selectPack(f20->getTimeStamp(),tt20)==nullptr);
@@ -160,12 +167,85 @@ TEST_F(KFPackBufferTest, removeUpTo)
ASSERT_EQ(kfpackbuffer.size(),2);
FrameBasePtr f22 = std::make_shared<FrameBase>(TimeStamp(22),nullptr,nullptr,nullptr);
KFPackPtr pack22 = std::make_shared<KFPack>(f22,5);
- kfpackbuffer.removeUpTo( pack22 );
+ kfpackbuffer.removeUpTo( pack22->key_frame->getTimeStamp() );
ASSERT_EQ(kfpackbuffer.size(),1);
ASSERT_TRUE(kfpackbuffer.selectPack(f21->getTimeStamp(),tt21)==nullptr);
ASSERT_TRUE(kfpackbuffer.selectPack(f28->getTimeStamp(),tt28)!=nullptr);
}
+
+TEST(ProcessorBase, KeyFrameCallback)
+{
+
+ using namespace wolf;
+ using std::shared_ptr;
+ using std::make_shared;
+ using std::static_pointer_cast;
+ using Eigen::Vector2s;
+
+ // Wolf problem
+ ProblemPtr problem = Problem::create("PO 2D");
+
+ // Install tracker (sensor and processor)
+ SensorBasePtr sen_tracker = make_shared<SensorBase>("FEATURE", std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)),
+ std::make_shared<StateBlock>(Eigen::VectorXs::Zero(1)),
+ std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), 2);
+ shared_ptr<ProcessorTrackerFeatureDummy> proc_tracker = make_shared<ProcessorTrackerFeatureDummy>(7, 4);
+
+ problem->addSensor(sen_tracker);
+ sen_tracker->addProcessor(proc_tracker);
+
+ // Install odometer (sensor and processor)
+ SensorBasePtr sen_odo = problem->installSensor("ODOM 2D", "odometer", Vector3s(0,0,0), "");
+ ProcessorParamsOdom2DPtr proc_odo_params = make_shared<ProcessorParamsOdom2D>();
+ ProcessorBasePtr prc_odo = problem->installProcessor("ODOM 2D", "odometer", sen_odo, proc_odo_params);
+ prc_odo->setTimeTolerance(0.01);
+
+ std::cout << "sensor & processor created and added to wolf problem" << std::endl;
+
+ // Sequence to test KeyFrame creations (callback calls)
+
+ // initialize
+ TimeStamp t(0.0);
+ Vector3s x(0,0,0);
+ Matrix3s P = Matrix3s::Identity() * 0.1;
+ problem->setPrior(x, P, t); // KF1
+
+ CaptureOdom2DPtr capture_odo = make_shared<CaptureOdom2D>(t, sen_odo, Vector2s(0.5,0));
+
+ for (size_t ii=0; ii<10; ii++ )
+ {
+ WOLF_DEBUG("iter:",ii," ts: ", t);
+
+ // Move
+ t = t+0.01;
+ capture_odo->setTimeStamp(t);
+ sen_odo->process(capture_odo);
+
+ WOLF_DEBUG("iter:",ii," ts: ", t);
+
+ t = t+0.01;
+ capture_odo->setTimeStamp(t);
+ sen_odo->process(capture_odo);
+
+ WOLF_DEBUG("iter:",ii," ts: ", t);
+
+ t = t+0.01;
+ capture_odo->setTimeStamp(t);
+ sen_odo->process(capture_odo);
+
+ WOLF_DEBUG("iter:",ii," ts: ", t);
+
+ // Track
+ proc_tracker->process(make_shared<CaptureVoid>(t, sen_tracker));
+
+ WOLF_DEBUG("iter:",ii," ts: ", t);
+ }
+
+ // Print WOLF info
+ problem->print(2);
+}
+
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);