Skip to content
Snippets Groups Projects
Commit 31565cc0 authored by Angel Santamaria-Navarro's avatar Angel Santamaria-Navarro
Browse files

debugging callback

parent afe8269b
No related branches found
No related tags found
1 merge request!157Kfpackmanager
Pipeline #
......@@ -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)
......
......@@ -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)
......
......@@ -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
}
......
......@@ -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_;
};
}
......
......@@ -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)
......
......@@ -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)
{
......
......@@ -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);
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment