From 6324bd8d7ed2a7d073f1518e8bab032254f76b58 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Mon, 6 Dec 2021 10:54:00 +0100
Subject: [PATCH] Improve usages of time_tolerance in Frame
---
include/core/processor/processor_base.h | 4 ++--
src/frame/frame_base.cpp | 9 ++++---
src/problem/problem.cpp | 1 +
src/processor/processor_base.cpp | 32 ++++++++++++-------------
src/processor/processor_motion.cpp | 11 +++++----
src/processor/processor_tracker.cpp | 16 ++++++++-----
6 files changed, 41 insertions(+), 32 deletions(-)
diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h
index bc4d52374..55b354b77 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.h
@@ -318,13 +318,13 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
* It stores the new KF in buffer_frame_ and calls triggerInKF()
*
*/
- void keyFrameCallback(FrameBasePtr _keyframe_ptr, const double& _time_tol_other);
+ void keyFrameCallback(FrameBasePtr _keyframe, const double& _time_tol_other);
/**\brief notify a new capture
*
* It stores the new capture in buffer_capture_ and calls triggerInCapture()
*/
- void captureCallback(CaptureBasePtr _capture_ptr);
+ void captureCallback(CaptureBasePtr _capture);
SensorBasePtr getSensor() const;
private:
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index 9511b531f..33bf4ec25 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -40,7 +40,8 @@ FrameBase::FrameBase(const TimeStamp& _ts,
HasStateBlocks(_frame_structure),
trajectory_ptr_(),
frame_id_(++frame_id_count_),
- time_stamp_(_ts)
+ time_stamp_(_ts),
+ time_tolerance_(0)
{
assert(_state.includesStructure(_frame_structure) && "_state does not include all keys of _frame_structure");
@@ -59,7 +60,8 @@ FrameBase::FrameBase(const TimeStamp& _ts,
HasStateBlocks(_frame_structure),
trajectory_ptr_(),
frame_id_(++frame_id_count_),
- time_stamp_(_ts)
+ time_stamp_(_ts),
+ time_tolerance_(0)
{
assert(_frame_structure.size() == _vectors.size() && "Structure size does not match number of provided vectors!");
@@ -85,7 +87,8 @@ FrameBase::FrameBase(const TimeStamp& _ts,
HasStateBlocks(""),
trajectory_ptr_(),
frame_id_(++frame_id_count_),
- time_stamp_(_ts)
+ time_stamp_(_ts),
+ time_tolerance_(0)
{
if (_p_ptr)
{
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 0627215bf..9a3523cc3 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -1086,6 +1086,7 @@ FrameBasePtr Problem::applyPriorOptions(const TimeStamp& _ts)
if (prior_options_->mode != "nothing" and prior_options_->mode != "")
{
prior_keyframe = emplaceFrame(_ts, frame_structure_, prior_options_->state);
+ prior_keyframe->setTimeTolerance(prior_options_->time_tolerance);
// Update origin for odometry processors
for (auto proc_pair : motion_provider_map_)
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index 634a6e0b9..596521ae3 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -54,33 +54,33 @@ bool ProcessorBase::permittedKeyFrame()
return isVotingActive() && getProblem()->permitKeyFrame(shared_from_this());
}
-void ProcessorBase::keyFrameCallback(FrameBasePtr _keyframe_ptr, const double& _time_tol_other)
+void ProcessorBase::keyFrameCallback(FrameBasePtr _keyframe, const double& _time_tol_other)
{
- assert(_keyframe_ptr != nullptr && "keyFrameCallback with a nullptr frame");
- WOLF_DEBUG("P", isMotionProvider() ? "M " : "T ", getName(), ": KF", _keyframe_ptr->id(), " callback received with ts = ", _keyframe_ptr->getTimeStamp());
+ assert(_keyframe != nullptr && "keyFrameCallback with a nullptr frame");
+ WOLF_DEBUG("P", isMotionProvider() ? "M " : "T ", getName(), ": KF", _keyframe->id(), " callback received with ts = ", _keyframe->getTimeStamp());
// profiling
n_kf_callback_++;
startKFProfiling();
- _keyframe_ptr->setTimeTolerance(_time_tol_other);
+ _keyframe->setTimeTolerance(_time_tol_other);
// asking if frame should be stored
- if (storeKeyFrame(_keyframe_ptr))
- buffer_frame_.add(_keyframe_ptr->getTimeStamp(), _keyframe_ptr);
+ if (storeKeyFrame(_keyframe))
+ buffer_frame_.add(_keyframe->getTimeStamp(), _keyframe);
// asking if frame should be processed
- if (triggerInKeyFrame(_keyframe_ptr, _time_tol_other))
- processKeyFrame(_keyframe_ptr, _time_tol_other);
+ if (triggerInKeyFrame(_keyframe, _time_tol_other))
+ processKeyFrame(_keyframe, _time_tol_other);
// profiling
stopKFProfiling();
}
-void ProcessorBase::captureCallback(CaptureBasePtr _capture_ptr)
+void ProcessorBase::captureCallback(CaptureBasePtr _capture)
{
- assert(_capture_ptr != nullptr && "captureCallback with a nullptr capture");
- WOLF_DEBUG("P", isMotionProvider() ? "M " : "T ", getName(), ": Capture ", _capture_ptr->id(), " callback received with ts = ", _capture_ptr->getTimeStamp());
+ assert(_capture != nullptr && "captureCallback with a nullptr capture");
+ WOLF_DEBUG("P", isMotionProvider() ? "M " : "T ", getName(), ": Capture ", _capture->id(), " callback received with ts = ", _capture->getTimeStamp());
// profiling
n_capture_callback_++;
@@ -88,15 +88,15 @@ void ProcessorBase::captureCallback(CaptureBasePtr _capture_ptr)
// apply prior in problem if not done (very first capture)
if (getProblem() && !getProblem()->isPriorSet())
- getProblem()->applyPriorOptions(_capture_ptr->getTimeStamp());
+ getProblem()->applyPriorOptions(_capture->getTimeStamp());
// asking if capture should be stored
- if (storeCapture(_capture_ptr))
- buffer_capture_.add(_capture_ptr->getTimeStamp(), _capture_ptr);
+ if (storeCapture(_capture))
+ buffer_capture_.add(_capture->getTimeStamp(), _capture);
// asking if capture should be processed
- if (triggerInCapture(_capture_ptr))
- processCapture(_capture_ptr);
+ if (triggerInCapture(_capture))
+ processCapture(_capture);
// profiling
stopCaptureProfiling();
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 6f4339a92..0dc8be3a2 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -473,8 +473,9 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
setCalibration(last_ptr_, getCalibration(origin_ptr_));
// Set the frame of last_ptr as key
- auto key_frame = last_ptr_->getFrame();
- key_frame ->link(getProblem());
+ auto keyframe = last_ptr_->getFrame();
+ keyframe ->setTimeTolerance(params_motion_->time_tolerance);
+ keyframe ->link(getProblem());
// create motion feature and add it to the key_capture
auto key_feature = emplaceFeature(last_ptr_);
@@ -489,14 +490,14 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// create a new capture
auto capture_new = emplaceCapture(frame_new,
getSensor(),
- key_frame->getTimeStamp(),
+ keyframe->getTimeStamp(),
Eigen::VectorXd::Zero(data_size_),
getSensor()->getNoiseCov(),
getCalibration(origin_ptr_),
getCalibration(origin_ptr_),
last_ptr_);
// reset the new buffer
- capture_new->getBuffer().push_back( motionZero(key_frame->getTimeStamp()) ) ;
+ capture_new->getBuffer().push_back( motionZero(keyframe->getTimeStamp()) ) ;
// reset derived things
resetDerived();
@@ -507,7 +508,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
last_frame_ptr_ = frame_new;
// callback to other processors
- getProblem()->keyFrameCallback(key_frame, shared_from_this(), params_motion_->time_tolerance);
+ getProblem()->keyFrameCallback(keyframe, shared_from_this(), params_motion_->time_tolerance);
}
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index 49b604fb2..8492ed031 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -102,18 +102,22 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
{
WOLF_DEBUG( "PT ", getName(), " FIRST_TIME_WITHOUT_KEYFRAME" );
- FrameBasePtr kfrm = FrameBase::emplace<FrameBase>(getProblem()->getTrajectory(),
- incoming_ptr_->getTimeStamp(),
- getProblem()->getFrameStructure(),
- getProblem()->getState());
- incoming_ptr_->link(kfrm);
+ // make a new KF at incoming
+ FrameBasePtr keyframe = FrameBase::emplace<FrameBase>(getProblem()->getTrajectory(),
+ incoming_ptr_->getTimeStamp(),
+ getProblem()->getFrameStructure(),
+ getProblem()->getState());
+ keyframe->setTimeTolerance(params_tracker_->time_tolerance);
+
+ // link incoming to the new KF
+ incoming_ptr_->link(keyframe);
// Process info
processKnown();
// We only process new features in Last, here last = nullptr, so we do not have anything to do.
// Issue KF callback with new KF
- getProblem()->keyFrameCallback(kfrm, shared_from_this(), params_tracker_->time_tolerance);
+ getProblem()->keyFrameCallback(keyframe, shared_from_this(), params_tracker_->time_tolerance);
resetDerived();
--
GitLab