diff --git a/demos/hello_wolf/hello_wolf.cpp b/demos/hello_wolf/hello_wolf.cpp
index f6f9ae7502283be1438aa3b98ba3b798a1203836..4eebf156ee21c17235b4b33d54c9c71952568fdc 100644
--- a/demos/hello_wolf/hello_wolf.cpp
+++ b/demos/hello_wolf/hello_wolf.cpp
@@ -161,7 +161,7 @@ int main()
VectorComposite x(Vector3d(0,0,0), "PO", {2,1});
// Matrix3d P = Matrix3d::Identity() * 0.1;
VectorComposite P(Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)), "PO", {2,1});
- FrameBasePtr KF1 = problem->setPriorFactor(x, P, t, 0.5); // KF1 : (0,0,0)
+ FrameBasePtr KF1 = problem->setPriorFactor(x, P, t); // KF1 : (0,0,0)
std::static_pointer_cast<ProcessorMotion>(processor)->setOrigin(KF1);
// SELF CALIBRATION ===================================================
diff --git a/demos/hello_wolf/processor_range_bearing.h b/demos/hello_wolf/processor_range_bearing.h
index dd9eaaf0739726fd9b03969cfbb426612294d252..a5d78948a7669de26f5715351d9c3c406f60131e 100644
--- a/demos/hello_wolf/processor_range_bearing.h
+++ b/demos/hello_wolf/processor_range_bearing.h
@@ -76,9 +76,9 @@ class ProcessorRangeBearing : public ProcessorBase
protected:
// Implementation of pure virtuals from ProcessorBase
void processCapture (CaptureBasePtr _capture) override;
- void processKeyFrame (FrameBasePtr _keyframe_ptr, const double& _time_tol_other) override {};
+ void processKeyFrame (FrameBasePtr _keyframe_ptr) override {};
bool triggerInCapture (CaptureBasePtr) const override { return true;};
- bool triggerInKeyFrame (FrameBasePtr _keyframe_ptr, const double& _time_tol_other) const override {return false;}
+ bool triggerInKeyFrame (FrameBasePtr _keyframe_ptr) const override {return false;}
bool voteForKeyFrame () const override {return false;}
/** \brief store key frame
diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h
index 328df7c2dc2cfeebd30cf14f54d3c155a15d4214..6a9488e681e46ec5e775e43f2e7e3d5d236726dc 100644
--- a/include/core/frame/frame_base.h
+++ b/include/core/frame/frame_base.h
@@ -58,7 +58,6 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
protected:
unsigned int frame_id_;
TimeStamp time_stamp_; ///< frame time stamp
- double time_tolerance_;
public:
/** \brief Constructor with type, time stamp and state pointer
@@ -102,8 +101,6 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
void setTimeStamp(const TimeStamp& _ts);
TimeStamp getTimeStamp() const;
void getTimeStamp(TimeStamp& _ts) const;
- double getTimeTolerance() const {return time_tolerance_;}
- void setTimeTolerance(double _time_tolerance) {time_tolerance_ = _time_tolerance;}
// State blocks ----------------------------------------------------
public:
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 5e35324c2ebc2e84838a4ba23ecf0ac208bd84a5..aa13d1ede62ca1610ee9c43d7c7ec2ab97da865a 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -61,7 +61,6 @@ struct PriorOptions
std::string mode = "";
VectorComposite state;
MatrixComposite cov;
- double time_tolerance;
};
WOLF_STRUCT_PTR_TYPEDEFS(PriorOptions);
@@ -208,20 +207,16 @@ class Problem : public std::enable_shared_from_this<Problem>
// Prior
bool isPriorSet() const;
void setPriorOptions(const std::string& _mode,
- const double _time_tolerance = 0,
const VectorComposite& _state = VectorComposite(),
const VectorComposite& _cov = VectorComposite());
FrameBasePtr applyPriorOptions(const TimeStamp& _ts);
FrameBasePtr setPriorFactor(const VectorComposite &_state,
const VectorComposite &_sigma,
- const TimeStamp &_ts,
- const double &_time_tol);
+ const TimeStamp &_ts);
FrameBasePtr setPriorFix(const VectorComposite &_state,
- const TimeStamp &_ts,
- const double &_time_tol);
+ const TimeStamp &_ts);
FrameBasePtr setPriorInitialGuess(const VectorComposite &_state,
- const TimeStamp &_ts,
- const double &_time_tol);
+ const TimeStamp &_ts);
/** \brief Emplace frame from string frame_structure, dimension and vector
* \param _time_stamp Time stamp of the frame
@@ -268,7 +263,7 @@ class Problem : public std::enable_shared_from_this<Problem>
* - If it is key-frame, update state-block lists in Problem
*/
FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, //
- const VectorComposite& _frame_state);
+ const VectorComposite& _frame_state);
/** \brief Emplace frame from string frame_structure and dimension
* \param _time_stamp Time stamp of the frame
@@ -333,8 +328,7 @@ class Problem : public std::enable_shared_from_this<Problem>
* New key frame callback: It should be called by any processor that creates a new key frame. It calls the keyFrameCallback of the rest of processors.
*/
void keyFrameCallback(FrameBasePtr _keyframe_ptr, //
- ProcessorBasePtr _processor_ptr, //
- const double& _time_tolerance);
+ ProcessorBasePtr _processor_ptr);
// State getters
TimeStamp getTimeStamp ( ) const;
diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h
index 8e15ff0ff9ddb659d3391f12b4d63d4b9146eca0..8d6e0894e320caa31882652a254bca8ad885745e 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.h
@@ -273,7 +273,7 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
* Each derived processor should implement this function. It will be called if:
* - A new KF arrived and triggerInKF() returned true.
*/
- virtual void processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance) = 0;
+ virtual void processKeyFrame(FrameBasePtr _keyframe_ptr) = 0;
/** \brief trigger in capture
*
@@ -285,7 +285,7 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
*
* Returns true if processKeyFrame() should be called after the provided KF arrived.
*/
- virtual bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance) const = 0;
+ virtual bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr) const = 0;
/** \brief store key frame
*
@@ -318,7 +318,7 @@ 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, const double& _time_tol_other);
+ void keyFrameCallback(FrameBasePtr _keyframe);
/**\brief notify a new capture
*
diff --git a/include/core/processor/processor_fix_wing_model.h b/include/core/processor/processor_fix_wing_model.h
index ddb74f8b3597f2d82d6ce5ea16b04d7538eb6e16..4d3287ceb3c2b62361526226bdcda9525a8e851a 100644
--- a/include/core/processor/processor_fix_wing_model.h
+++ b/include/core/processor/processor_fix_wing_model.h
@@ -82,7 +82,7 @@ class ProcessorFixWingModel : public ProcessorBase
/** \brief process an incoming key-frame: applies the motion model between consecutive keyframes
*/
- virtual void processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance) override;
+ virtual void processKeyFrame(FrameBasePtr _keyframe_ptr) override;
/** \brief trigger in capture
*/
@@ -90,7 +90,7 @@ class ProcessorFixWingModel : public ProcessorBase
/** \brief trigger in key-frame
*/
- virtual bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance) const override {return true;};
+ virtual bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr) const override {return true;};
/** \brief store key frame
*/
diff --git a/include/core/processor/processor_loop_closure.h b/include/core/processor/processor_loop_closure.h
index 9749a3ca84bcc6891820b1a8aab03d91999e1b09..12d090eef2427d0db195edb2cac5db01b7a4fc25 100644
--- a/include/core/processor/processor_loop_closure.h
+++ b/include/core/processor/processor_loop_closure.h
@@ -115,10 +115,10 @@ class ProcessorLoopClosure : public ProcessorBase
virtual void emplaceFactors(MatchLoopClosurePtr) = 0;
void processCapture(CaptureBasePtr) override;
- void processKeyFrame(FrameBasePtr, const double&) override;
+ void processKeyFrame(FrameBasePtr _frm) override;
bool triggerInCapture(CaptureBasePtr _cap) const override { return true;};
- bool triggerInKeyFrame(FrameBasePtr _frm, const double& _time_tol) const override { return true;};
+ bool triggerInKeyFrame(FrameBasePtr _frm) const override { return true;};
bool storeKeyFrame(FrameBasePtr _frm) override { return false;};
bool storeCapture(CaptureBasePtr _cap) override { return false;};
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index c24c9d727af9e03801d27f56a52d914cd12b5180..847a2fabe1a29fba26f84f512ac29e14050d81aa 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -240,7 +240,7 @@ class ProcessorMotion : public ProcessorBase, public MotionProvider
*
* The ProcessorMotion only processes incoming captures (it is not called).
*/
- void processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tol_other) override {};
+ void processKeyFrame(FrameBasePtr _keyframe_ptr) override {};
/** \brief trigger in capture
*
@@ -252,7 +252,7 @@ class ProcessorMotion : public ProcessorBase, public MotionProvider
*
* The ProcessorMotion only processes incoming captures, then it returns false.
*/
- bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tol_other) const override {return false;}
+ bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr) const override {return false;}
/** \brief store key frame
*
diff --git a/include/core/processor/processor_pose.h b/include/core/processor/processor_pose.h
index 01e4a2c33720ca7cc9b54e3a92c06ce16a83f38e..38cf14684499db35bad296d2fd3610f8343d18b8 100644
--- a/include/core/processor/processor_pose.h
+++ b/include/core/processor/processor_pose.h
@@ -58,10 +58,10 @@ class ProcessorPose : public ProcessorBase{
void configure(SensorBasePtr _sensor) override;
- void processCapture(CaptureBasePtr) override;
- void processKeyFrame(FrameBasePtr, const double&) override;
+ void processCapture(CaptureBasePtr _cap) override;
+ void processKeyFrame(FrameBasePtr _frm) override;
bool triggerInCapture(CaptureBasePtr _cap) const override { return true;};
- bool triggerInKeyFrame(FrameBasePtr _frm, const double& _time_tol) const override { return true;};
+ bool triggerInKeyFrame(FrameBasePtr _frm) const override { return true;};
bool storeKeyFrame(FrameBasePtr _frm) override { return true;};
bool storeCapture(CaptureBasePtr _cap) override { return true;};
bool voteForKeyFrame() const override { return false;};
diff --git a/include/core/processor/processor_tracker.h b/include/core/processor/processor_tracker.h
index bd2889842739917b5dd210a9a5ab71cdd50a8b5c..21560a5b2808d96a507faab816b9f35c87a30066 100644
--- a/include/core/processor/processor_tracker.h
+++ b/include/core/processor/processor_tracker.h
@@ -159,7 +159,7 @@ class ProcessorTracker : public ProcessorBase
*
* The ProcessorTracker only processes incoming captures (it is not called).
*/
- void processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance) override {}
+ void processKeyFrame(FrameBasePtr _keyframe_ptr) override {}
/** \brief trigger in capture
*
@@ -171,7 +171,7 @@ class ProcessorTracker : public ProcessorBase
*
* The ProcessorTracker only processes incoming captures, then it returns false.
*/
- bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance) const override {return false;}
+ bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr) const override {return false;}
/** \brief store key frame
*
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index 33bf4ec25081b5864e76f406ff0528a57279b363..9511b531f10bf1bdac4b8b6214c7d5da7ed87647 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -40,8 +40,7 @@ FrameBase::FrameBase(const TimeStamp& _ts,
HasStateBlocks(_frame_structure),
trajectory_ptr_(),
frame_id_(++frame_id_count_),
- time_stamp_(_ts),
- time_tolerance_(0)
+ time_stamp_(_ts)
{
assert(_state.includesStructure(_frame_structure) && "_state does not include all keys of _frame_structure");
@@ -60,8 +59,7 @@ FrameBase::FrameBase(const TimeStamp& _ts,
HasStateBlocks(_frame_structure),
trajectory_ptr_(),
frame_id_(++frame_id_count_),
- time_stamp_(_ts),
- time_tolerance_(0)
+ time_stamp_(_ts)
{
assert(_frame_structure.size() == _vectors.size() && "Structure size does not match number of provided vectors!");
@@ -87,8 +85,7 @@ FrameBase::FrameBase(const TimeStamp& _ts,
HasStateBlocks(""),
trajectory_ptr_(),
frame_id_(++frame_id_count_),
- time_stamp_(_ts),
- time_tolerance_(0)
+ time_stamp_(_ts)
{
if (_p_ptr)
{
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 9a3523cc35f4b7b5e945107db273aa8c5e85449b..a4b0e723afdeb6686323aa47d4099398db6d5b51 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -206,7 +206,6 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
else if (prior_mode == "factor")
{
problem->setPriorOptions(prior_mode,
- _server.getParam<double>("problem/prior/time_tolerance"),
_server.getParam<VectorComposite>("problem/prior/state"),
_server.getParam<VectorComposite>("problem/prior/sigma"));
@@ -217,7 +216,6 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
{
WOLF_TRACE("Prior mode: ", prior_mode);
problem->setPriorOptions(prior_mode,
- _server.getParam<double>("problem/prior/time_tolerance"),
_server.getParam<VectorComposite>("problem/prior/state"));
}
@@ -696,7 +694,7 @@ bool Problem::permitKeyFrame(ProcessorBasePtr _processor_ptr) const
return true;
}
-void Problem::keyFrameCallback(FrameBasePtr _keyframe_ptr, ProcessorBasePtr _processor_ptr, const double& _time_tolerance)
+void Problem::keyFrameCallback(FrameBasePtr _keyframe_ptr, ProcessorBasePtr _processor_ptr)
{
WOLF_DEBUG_COND(_processor_ptr!=nullptr,(_processor_ptr->isMotionProvider() ? "PM " : "PT "), _processor_ptr->getName(), ": KF", _keyframe_ptr->id(), " Callback emitted with ts = ", _keyframe_ptr->getTimeStamp());
WOLF_DEBUG_COND(_processor_ptr==nullptr,"External callback: KF", _keyframe_ptr->id(), " Callback emitted with ts = ", _keyframe_ptr->getTimeStamp());
@@ -712,7 +710,7 @@ void Problem::keyFrameCallback(FrameBasePtr _keyframe_ptr, ProcessorBasePtr _pro
#ifdef PROFILING
auto start = std::chrono::high_resolution_clock::now();
#endif
- processor->keyFrameCallback(_keyframe_ptr, _time_tolerance);
+ processor->keyFrameCallback(_keyframe_ptr);
#ifdef PROFILING
auto stop = std::chrono::high_resolution_clock::now();
auto duration = std::chrono::duration_cast<std::chrono::microseconds>(stop - start);
@@ -1030,7 +1028,6 @@ FrameBasePtr Problem::closestFrameToTimeStamp(const TimeStamp& _ts) const
}
void Problem::setPriorOptions(const std::string& _mode,
- const double _time_tolerance ,
const VectorComposite& _state ,
const VectorComposite& _sigma )
{
@@ -1044,13 +1041,10 @@ void Problem::setPriorOptions(const std::string& _mode,
if (prior_options_->mode != "nothing")
{
- assert(_time_tolerance > 0 && "time tolerance should be bigger than 0");
assert(_state.includesStructure(frame_structure_) && "any missing key in prior state");
WOLF_TRACE("prior state: ", _state);
- WOLF_TRACE("prior time tolerance: ", _time_tolerance);
prior_options_->state = _state;
- prior_options_->time_tolerance = _time_tolerance;
if (prior_options_->mode == "factor")
{
@@ -1086,7 +1080,6 @@ 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_)
@@ -1138,7 +1131,7 @@ FrameBasePtr Problem::applyPriorOptions(const TimeStamp& _ts)
assert(prior_options_->mode == "initial_guess" && "wrong prior_options->mode");
// notify all processors
- keyFrameCallback(prior_keyframe, nullptr, prior_options_->time_tolerance);
+ keyFrameCallback(prior_keyframe, nullptr);
}
// remove prior options
prior_options_ = nullptr;
@@ -1148,27 +1141,24 @@ FrameBasePtr Problem::applyPriorOptions(const TimeStamp& _ts)
FrameBasePtr Problem::setPriorFactor(const VectorComposite &_state,
const VectorComposite &_sigma,
- const TimeStamp &_ts,
- const double &_time_tol)
+ const TimeStamp &_ts)
{
- setPriorOptions("factor", _time_tol, _state, _sigma);
+ setPriorOptions("factor", _state, _sigma);
return applyPriorOptions(_ts);
}
FrameBasePtr Problem::setPriorFix(const VectorComposite &_state,
- const TimeStamp &_ts,
- const double &_time_tol)
+ const TimeStamp &_ts)
{
- setPriorOptions("fix", _time_tol, _state);
+ setPriorOptions("fix", _state);
return applyPriorOptions(_ts);
}
FrameBasePtr Problem::setPriorInitialGuess(const VectorComposite &_state,
- const TimeStamp &_ts,
- const double &_time_tol)
+ const TimeStamp &_ts)
{
- setPriorOptions("initial_guess", _time_tol, _state);
+ setPriorOptions("initial_guess", _state);
return applyPriorOptions(_ts);
}
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index 61f145811aabebbbdeaaba70b87f0ac5602e80f3..3a79503910406c38e6f6b250e80381fa20bd8387 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -54,7 +54,7 @@ bool ProcessorBase::permittedKeyFrame()
return isVotingActive() && getProblem()->permitKeyFrame(shared_from_this());
}
-void ProcessorBase::keyFrameCallback(FrameBasePtr _keyframe, const double& _time_tol_other)
+void ProcessorBase::keyFrameCallback(FrameBasePtr _keyframe)
{
assert(_keyframe != nullptr && "keyFrameCallback with a nullptr frame");
WOLF_DEBUG("P", isMotionProvider() ? "M " : "T ", getName(), ": KF", _keyframe->id(), " callback received with ts = ", _keyframe->getTimeStamp());
@@ -63,15 +63,13 @@ void ProcessorBase::keyFrameCallback(FrameBasePtr _keyframe, const double& _time
n_kf_callback_++;
startKFProfiling();
- _keyframe->setTimeTolerance(_time_tol_other);
-
// asking if frame should be stored
if (storeKeyFrame(_keyframe))
buffer_frame_.emplace(_keyframe->getTimeStamp(), _keyframe);
// asking if frame should be processed
- if (triggerInKeyFrame(_keyframe, _time_tol_other))
- processKeyFrame(_keyframe, _time_tol_other);
+ if (triggerInKeyFrame(_keyframe))
+ processKeyFrame(_keyframe);
// profiling
stopKFProfiling();
diff --git a/src/processor/processor_fix_wing_model.cpp b/src/processor/processor_fix_wing_model.cpp
index dcd8eca0fa32c2a413a47341b6aa1fbd16c7bf60..d25f2c97145e611c5287278dca7730ebcf5506ad 100644
--- a/src/processor/processor_fix_wing_model.cpp
+++ b/src/processor/processor_fix_wing_model.cpp
@@ -50,7 +50,7 @@ void ProcessorFixWingModel::configure(SensorBasePtr _sensor)
assert(_sensor->getProblem()->getFrameStructure().find('V') != std::string::npos && "Processor only works with problems with V");
}
-void ProcessorFixWingModel::processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance)
+void ProcessorFixWingModel::processKeyFrame(FrameBasePtr _keyframe_ptr)
{
if (_keyframe_ptr->getV()->isFixed())
return;
diff --git a/src/processor/processor_loop_closure.cpp b/src/processor/processor_loop_closure.cpp
index 8b39b0230e3834df0957f005e0aa59ba59e29ad4..51001cdaed74aa3e368b3bc60b81beb13b659327 100644
--- a/src/processor/processor_loop_closure.cpp
+++ b/src/processor/processor_loop_closure.cpp
@@ -78,7 +78,7 @@ void ProcessorLoopClosure::processCapture(CaptureBasePtr _capture)
buffer_capture_.emplace(_capture->getTimeStamp(), _capture);
}
-void ProcessorLoopClosure::processKeyFrame(FrameBasePtr _keyframe, const double& _time_tolerance)
+void ProcessorLoopClosure::processKeyFrame(FrameBasePtr _keyframe)
{
/* This function has 4 scenarios:
* 1. Frame already have a capture of the sensor -> process
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index a236a9022530d6bae0cbcfdb4c53e753de664ff7..78d99903a18eeaa75fa81599c52aad95fbac66be 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -464,7 +464,6 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// Set the frame of last_ptr as key
auto keyframe = last_ptr_->getFrame();
- keyframe ->setTimeTolerance(params_motion_->time_tolerance);
keyframe ->link(getProblem());
// create motion feature and add it to the key_capture
@@ -498,7 +497,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
last_frame_ptr_ = frame_new;
// callback to other processors
- getProblem()->keyFrameCallback(keyframe, shared_from_this(), params_motion_->time_tolerance);
+ getProblem()->keyFrameCallback(keyframe, shared_from_this());
}
@@ -931,8 +930,7 @@ FrameBasePtr ProcessorMotion::computeProcessingStep()
if (keyframe_from_callback)
{
- if (buffer_frame_.doubleCheckTimeTolerance(keyframe_from_callback->getTimeStamp(),
- keyframe_from_callback->getTimeTolerance(),
+ if (buffer_frame_.simpleCheckTimeTolerance(keyframe_from_callback->getTimeStamp(),
incoming_ptr_->getTimeStamp(),
params_motion_->time_tolerance))
{
@@ -968,8 +966,7 @@ FrameBasePtr ProcessorMotion::computeProcessingStep()
if (keyframe_from_callback)
{
- if (buffer_frame_.doubleCheckTimeTolerance(keyframe_from_callback->getTimeStamp(),
- keyframe_from_callback->getTimeTolerance(),
+ if (buffer_frame_.simpleCheckTimeTolerance(keyframe_from_callback->getTimeStamp(),
origin_ptr_->getTimeStamp(),
params_motion_->time_tolerance))
diff --git a/src/processor/processor_pose.cpp b/src/processor/processor_pose.cpp
index e9458c8c6d6e16594a7ec96be7b9c0a073d44367..21a1a5624eb55aff99cb8919ad931216ad873172 100644
--- a/src/processor/processor_pose.cpp
+++ b/src/processor/processor_pose.cpp
@@ -50,12 +50,7 @@ void ProcessorPose::createFactorIfNecessary(){
while (kf_it != buffer_frame_.getContainer().end())
{
TimeStamp t = kf_it->first;
- double time_tolerance = std::min(getTimeTolerance(), kf_it->second->getTimeTolerance());
- if (getTimeTolerance() == 0.0){
- WOLF_WARN("Time tolerance set to zero -> value not used");
- time_tolerance = kf_it->second->getTimeTolerance();
- }
- auto cap_it = buffer_capture_.selectIterator(t, time_tolerance);
+ auto cap_it = buffer_capture_.selectIterator(t, getTimeTolerance());
// if capture with corresponding timestamp is not found, assume you will get it later
if (cap_it != buffer_capture_.getContainer().end())
@@ -109,7 +104,7 @@ inline void ProcessorPose::processCapture(CaptureBasePtr _capture)
}
-inline void ProcessorPose::processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance)
+inline void ProcessorPose::processKeyFrame(FrameBasePtr _keyframe_ptr)
{
if (!_keyframe_ptr)
{
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index c41ee4f6c963ce45ba3a427a3c2e27dc818b185b..7ead05cd8886cb85308063e07c4cebc4449fe4f9 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -107,7 +107,6 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
incoming_ptr_->getTimeStamp(),
getProblem()->getFrameStructure(),
getProblem()->getState());
- keyframe->setTimeTolerance(params_tracker_->time_tolerance);
// link incoming to the new KF
incoming_ptr_->link(keyframe);
@@ -117,7 +116,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
// 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(keyframe, shared_from_this(), params_tracker_->time_tolerance);
+ getProblem()->keyFrameCallback(keyframe, shared_from_this());
resetDerived();
@@ -225,7 +224,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
establishFactors();
// Call the new keyframe callback in order to let the other processors to establish their factors
- getProblem()->keyFrameCallback(last_ptr_->getFrame(), shared_from_this(), params_tracker_->time_tolerance);
+ getProblem()->keyFrameCallback(last_ptr_->getFrame(), shared_from_this());
// Update pointers
resetDerived();
diff --git a/test/dummy/processor_motion_provider_dummy.h b/test/dummy/processor_motion_provider_dummy.h
index 7337132a0dc857a8a8948abee1b9ad29b4943fdc..186b4018569028225580fc780468d14a43f397ff 100644
--- a/test/dummy/processor_motion_provider_dummy.h
+++ b/test/dummy/processor_motion_provider_dummy.h
@@ -66,9 +66,9 @@ class MotionProviderDummy : public ProcessorBase, public MotionProvider
void configure(SensorBasePtr _sensor) override {};
void processCapture(CaptureBasePtr) override {};
- void processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance) override {};
+ void processKeyFrame(FrameBasePtr _keyframe_ptr) override {};
bool triggerInCapture(CaptureBasePtr) const override { return false; };
- bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance) const override { return false; };
+ bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr) const override { return false; };
bool storeKeyFrame(FrameBasePtr) override { return false; };
bool storeCapture(CaptureBasePtr) override { return false; };
bool voteForKeyFrame() const override { return false; };
diff --git a/test/gtest_buffer_frame.cpp b/test/gtest_buffer_frame.cpp
index 2f94d78e36f2771b0a4eae0979c6b3d58c5b2ed5..f314325e629d867ca69e7313b58a9adb5e9f2577 100644
--- a/test/gtest_buffer_frame.cpp
+++ b/test/gtest_buffer_frame.cpp
@@ -64,11 +64,6 @@ class BufferFrameTest : public testing::Test
tt20 = 1.0;
tt21 = 1.0;
tt28 = 1.0;
-
- f10->setTimeTolerance(tt10);
- f20->setTimeTolerance(tt20);
- f21->setTimeTolerance(tt21);
- f28->setTimeTolerance(tt28);
};
};
@@ -256,7 +251,6 @@ TEST_F(BufferFrameTest, removeUpTo)
buffer_kf.emplace(28, f28);
ASSERT_EQ(buffer_kf.size(), (unsigned int) 2);
FrameBasePtr f22 = std::make_shared<FrameBase>(TimeStamp(22),nullptr,nullptr,nullptr);
- f22->setTimeTolerance(5.0);
// PackKeyFramePtr pack22 = std::make_shared<PackKeyFrame>(f22,5);
buffer_kf.removeUpTo( f22->getTimeStamp() );
ASSERT_EQ(buffer_kf.size(), (unsigned int) 1);
diff --git a/test/gtest_factor_block_difference.cpp b/test/gtest_factor_block_difference.cpp
index bd99a5ef79c32b06388b440b4fd171e349eaf618..067a7a1dc8eecedfe10f9f5740fc137b63d192aa 100644
--- a/test/gtest_factor_block_difference.cpp
+++ b/test/gtest_factor_block_difference.cpp
@@ -71,7 +71,7 @@ class FixtureFactorBlockDifference : public testing::Test
VectorComposite x_origin(problem_->stateZero().vector("POV"), "POV", {3, 4, 3});
// Eigen::Matrix9d cov_prior = 1e-3 * Eigen::Matrix9d::Identity();
VectorComposite cov_prior("POV", {Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)),Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)),Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1))});
- KF0_ = problem_->setPriorFactor(x_origin, cov_prior, t0, 0.1);
+ KF0_ = problem_->setPriorFactor(x_origin, cov_prior, t0);
//CaptureBasePtr capV0 = CaptureBase::emplace<CaptureBase>(KF0_, "Vel0", t0);
//FeatureBasePtr featV0 = FeatureBase::emplace<FeatureBase>(capV0, "Vel0", x_origin.tail(3), cov_prior.bottomRightCorner<3,3>());
diff --git a/test/gtest_factor_diff_drive.cpp b/test/gtest_factor_diff_drive.cpp
index df96a961b7153364f8c67cb25caacd3069097a7a..031bc040406cf5b851c73555b5cb88e0a4fe942b 100644
--- a/test/gtest_factor_diff_drive.cpp
+++ b/test/gtest_factor_diff_drive.cpp
@@ -487,7 +487,7 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt)
VectorComposite s0(Vector3d(0.1,0.1,0.1), "PO", {2,1});
// set prior at t=0 and processor origin
- auto F0 = problem->setPriorFactor(x0, s0, t, 0.1);
+ auto F0 = problem->setPriorFactor(x0, s0, t);
processor->setOrigin(F0);
// right turn 90 deg in N steps --> ends up in (1.5, -1.5, -pi/2)
@@ -621,7 +621,7 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
VectorComposite s0(Vector3d(1,1,1), "PO", {2,1});
// set prior at t=0 and processor origin
- auto F0 = problem->setPriorFactor(x0, s0, t, 0.1);
+ auto F0 = problem->setPriorFactor(x0, s0, t);
processor->setOrigin(F0);
// right turn 90 deg in N steps --> ends up in (1.5, -1.5, -pi/2)
diff --git a/test/gtest_odom_2d.cpp b/test/gtest_odom_2d.cpp
index de4d10cdcb576669aea72361954096335187593e..0a6deee8d7739fdc5cd05aa04cc8fadf4d8512e6 100644
--- a/test/gtest_odom_2d.cpp
+++ b/test/gtest_odom_2d.cpp
@@ -146,7 +146,7 @@ TEST(Odom2d, FactorFix_and_FactorOdom2d)
SolverCeres solver(Pr);
// KF0 and absolute prior
- FrameBasePtr F0 = Pr->setPriorFactor(x0, s0,t0, dt/2);
+ FrameBasePtr F0 = Pr->setPriorFactor(x0, s0,t0);
// KF1 and motion from KF0
t += dt;
@@ -239,7 +239,7 @@ TEST(Odom2d, VoteForKfAndSolve)
SolverCeres solver(problem);
// Origin Key Frame
- auto KF = problem->setPriorFactor(x0, s0, t, dt/2);
+ auto KF = problem->setPriorFactor(x0, s0, t);
processor_odom2d->setOrigin(KF);
solver.solve(SolverManager::ReportVerbosity::BRIEF);
@@ -377,7 +377,7 @@ TEST(Odom2d, KF_callback)
SolverCeres solver(problem);
// Origin Key Frame
- FrameBasePtr keyframe_0 = problem->setPriorFactor(x0, x0_cov, t0, dt/2);
+ FrameBasePtr keyframe_0 = problem->setPriorFactor(x0, x0_cov, t0);
processor_odom2d->setOrigin(keyframe_0);
// Check covariance values
@@ -447,7 +447,7 @@ TEST(Odom2d, KF_callback)
ASSERT_EQ(problem->getLastFrame()->getTimeStamp().getNanoSeconds(), 80000000);
ASSERT_TRUE(problem->check(0));
- processor_odom2d->keyFrameCallback(keyframe_2, dt/2);
+ processor_odom2d->keyFrameCallback(keyframe_2);
ASSERT_TRUE(problem->check(0));
t += dt;
capture = std::make_shared<CaptureOdom2d>(t, sensor_odom2d, data, data_cov, nullptr);
@@ -479,7 +479,7 @@ TEST(Odom2d, KF_callback)
FrameBasePtr keyframe_1 = problem->emplaceFrame(t_split, x_split);
ASSERT_TRUE(problem->check(0));
- processor_odom2d->keyFrameCallback(keyframe_1, dt/2);
+ processor_odom2d->keyFrameCallback(keyframe_1);
ASSERT_TRUE(problem->check(0));
t += dt;
capture = std::make_shared<CaptureOdom2d>(t, sensor_odom2d, data, data_cov, nullptr);
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index 461e7c130cc1dba7a08d9b0cadffaf7c0ea7bc7b..887c9612f62e38fa75bcb0d46559f72d88a7274e 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -130,7 +130,7 @@ TEST(Problem, SetOrigin_PO_2d)
// Eigen::MatrixXd P0(Eigen::MatrixXd::Identity(3,3) * 0.1); // P0 is 0.1*Id
VectorComposite s0(Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)), "PO", {2,1});
- P->setPriorFactor(x0, s0, t0, 1.0);
+ P->setPriorFactor(x0, s0, t0);
WOLF_INFO("printing.-..");
P->print(4,1,1,1);
@@ -189,7 +189,7 @@ TEST(Problem, SetOrigin_PO_3d)
Eigen::VectorXd vec6(6); vec6 << sqrt(0.1), sqrt(0.1), sqrt(0.1), sqrt(0.1), sqrt(0.1), sqrt(0.1);
VectorComposite s0(vec6, "PO", {3,3});
- P->setPriorFactor(x0, s0, t0, 1.0);
+ P->setPriorFactor(x0, s0, t0);
// check that no sensor has been added
ASSERT_EQ(P->getHardware()->getSensorList().size(), (SizeStd) 0);
diff --git a/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp b/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp
index 1b93bf69a4538cf1ce4dcc65144b2c5f52dba607..bd3627fc3781929ca6f5508c254c933a5f5d6ebe 100644
--- a/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp
+++ b/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp
@@ -196,9 +196,9 @@ class FactorPose3dWithExtrinsics_ProcessorWithKeyFrameCallbackFirst_Test : publi
KF1_ = problem_->emplaceFrame(1, pose1_);
KF2_ = problem_->emplaceFrame(2, pose2_);
KF3_ = problem_->emplaceFrame(3, pose3_);
- problem_->keyFrameCallback(KF1_, nullptr, 0.5);
- problem_->keyFrameCallback(KF2_, nullptr, 0.5);
- problem_->keyFrameCallback(KF3_, nullptr, 0.5);
+ problem_->keyFrameCallback(KF1_, nullptr);
+ problem_->keyFrameCallback(KF2_, nullptr);
+ problem_->keyFrameCallback(KF3_, nullptr);
///////////////////
// Create factor graph
@@ -259,9 +259,9 @@ class FactorPose3dWithExtrinsics_ProcessorWithProcessFirst_Test : public FactorP
cap_mocap3->process();
// keyframe callback called after all mocap captures have been processed
- problem_->keyFrameCallback(KF1_, nullptr, 0.5);
- problem_->keyFrameCallback(KF2_, nullptr, 0.5);
- problem_->keyFrameCallback(KF3_, nullptr, 0.5);
+ problem_->keyFrameCallback(KF1_, nullptr);
+ problem_->keyFrameCallback(KF2_, nullptr);
+ problem_->keyFrameCallback(KF3_, nullptr);
}
void TearDown() override{};
diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp
index 52aa50d70997a4d34d1d5d0ce652b34ecf9008cf..af5b15e84b05810de7d837450b7f5f9d252f0f51 100644
--- a/test/gtest_processor_base.cpp
+++ b/test/gtest_processor_base.cpp
@@ -127,7 +127,7 @@ TEST(ProcessorBase, KeyFrameCallback)
VectorComposite x(Vector3d(0,0,0), "PO", {2,1});
// Matrix3d P = Matrix3d::Identity() * 0.1;
VectorComposite P(Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)), "PO", {2,1});
- problem->setPriorFactor(x, P, t, dt/2); // KF1
+ problem->setPriorFactor(x, P, t); // KF1
CaptureOdom2dPtr capt_odo = make_shared<CaptureOdom2d>(t, sens_odo, Vector2d(0.5,0));
diff --git a/test/gtest_processor_diff_drive.cpp b/test/gtest_processor_diff_drive.cpp
index 3ba4a47b50175e432a43b0a65d43ab9b25d2f077..d317c0a951118c9e2eda0f91943cec76476abcd2 100644
--- a/test/gtest_processor_diff_drive.cpp
+++ b/test/gtest_processor_diff_drive.cpp
@@ -335,7 +335,7 @@ TEST_F(ProcessorDiffDriveTest, process)
// Matrix3d P; P.setIdentity();
VectorComposite P(Vector3d(1,1,1), "PO", {2,1});
- auto F0 = problem->setPriorFactor(x, P, t, 0.1);
+ auto F0 = problem->setPriorFactor(x, P, t);
processor->setOrigin(F0);
// 1. left turn 90 deg in N steps of 90/N deg --> ends up in (1.5, 1.5, pi/2)
@@ -366,7 +366,7 @@ TEST_F(ProcessorDiffDriveTest, linear)
// Matrix3d P; P.setIdentity();
VectorComposite P(Vector3d(1,1,1), "PO", {2,1});
- auto F0 = problem->setPriorFactor(x, P, t, 0.1);
+ auto F0 = problem->setPriorFactor(x, P, t);
processor->setOrigin(F0);
// Straight one turn of the wheels, in one go
@@ -396,7 +396,7 @@ TEST_F(ProcessorDiffDriveTest, angular)
// Matrix3d P; P.setIdentity();
VectorComposite P(Vector3d(1,1,1), "PO", {2,1});
- auto F0 = problem->setPriorFactor(x, P, t, 0.1);
+ auto F0 = problem->setPriorFactor(x, P, t);
processor->setOrigin(F0);
// Straight one turn of the wheels, in one go
diff --git a/test/gtest_processor_fix_wing_model.cpp b/test/gtest_processor_fix_wing_model.cpp
index bb409f34b551085f7ddc0efc883715a2e4b4a372..fd759a4c09b6f9cbb1f22039a31abc87510f1cf2 100644
--- a/test/gtest_processor_fix_wing_model.cpp
+++ b/test/gtest_processor_fix_wing_model.cpp
@@ -83,7 +83,7 @@ TEST_F(ProcessorFixWingModelTest, keyFrameCallback)
auto frm1 = emplaceFrame(1, (Vector10d() << 0,0,0,0,0,0,1,1,0,0).finished());
// keyframecallback
- problem->keyFrameCallback(frm1, nullptr, 0.5);
+ problem->keyFrameCallback(frm1, nullptr);
// check one capture
ASSERT_EQ(frm1->getCapturesOf(sensor).size(), 1);
@@ -106,10 +106,10 @@ TEST_F(ProcessorFixWingModelTest, keyFrameCallbackRepeated)
auto frm1 = emplaceFrame(1, (Vector10d() << 0,0,0,0,0,0,1,1,0,0).finished());
// keyframecallback
- problem->keyFrameCallback(frm1, nullptr, 0.5);
+ problem->keyFrameCallback(frm1, nullptr);
// repeated keyframecallback
- problem->keyFrameCallback(frm1, nullptr, 0.5);
+ problem->keyFrameCallback(frm1, nullptr);
// check one capture
ASSERT_EQ(frm1->getCapturesOf(sensor).size(), 1);
@@ -132,7 +132,7 @@ TEST_F(ProcessorFixWingModelTest, solve_origin)
auto frm1 = emplaceFrame(1, (Vector10d() << 0,0,0,0,0,0,1,1,0,0).finished());
// keyframecallback
- problem->keyFrameCallback(frm1, nullptr, 0.5);
+ problem->keyFrameCallback(frm1, nullptr);
// perturb
frm1->getP()->fix();
diff --git a/test/gtest_processor_loop_closure.cpp b/test/gtest_processor_loop_closure.cpp
index 687036278606e5078ec99de3665c0c7d034cd2b9..b5e61e64033f3ebfd07d69603721a1598de83d87 100644
--- a/test/gtest_processor_loop_closure.cpp
+++ b/test/gtest_processor_loop_closure.cpp
@@ -95,7 +95,7 @@ TEST_F(ProcessorLoopClosureTest, frame_stored)
auto frm1 = emplaceFrame(1, Vector3d::Zero());
// keyframecallback
- problem->keyFrameCallback(frm1, nullptr, 0.5);
+ problem->keyFrameCallback(frm1, nullptr);
EXPECT_EQ(processor->getNStoredFrames(), 1);
EXPECT_EQ(processor->getNStoredCaptures(), 0);
@@ -136,7 +136,7 @@ TEST_F(ProcessorLoopClosureTest, captureCallbackCase2)
auto cap1 = createCapture(1);
// keyframecallback
- problem->keyFrameCallback(frm1, nullptr, 0.5);
+ problem->keyFrameCallback(frm1, nullptr);
// captureCallback
processor->captureCallback(cap1);
@@ -155,7 +155,7 @@ TEST_F(ProcessorLoopClosureTest, captureCallbackCase3)
auto cap1 = createCapture(2);
// keyframecallback
- problem->keyFrameCallback(frm1, nullptr, 0.5);
+ problem->keyFrameCallback(frm1, nullptr);
// captureCallback
processor->captureCallback(cap1);
@@ -173,7 +173,7 @@ TEST_F(ProcessorLoopClosureTest, keyFrameCallbackCase1)
auto cap1 = emplaceCapture(frm1);
// keyframecallback
- problem->keyFrameCallback(frm1, nullptr, 0.5);
+ problem->keyFrameCallback(frm1, nullptr);
EXPECT_EQ(cap1->getFeatureList().size(), 1); // capture processed by the processor
EXPECT_EQ(processor->getNStoredFrames(), 0);
@@ -191,7 +191,7 @@ TEST_F(ProcessorLoopClosureTest, keyFrameCallbackCase2)
processor->captureCallback(cap1);
// keyframecallback
- problem->keyFrameCallback(frm1, nullptr, 0.5);
+ problem->keyFrameCallback(frm1, nullptr);
EXPECT_EQ(cap1->getFrame(), frm1); // capture processed by the processor
EXPECT_EQ(cap1->getFeatureList().size(), 1); // capture processed by the processor
@@ -210,7 +210,7 @@ TEST_F(ProcessorLoopClosureTest, keyFrameCallbackCase3)
processor->captureCallback(cap1);
// keyframecallback
- problem->keyFrameCallback(frm1, nullptr, 0.5);
+ problem->keyFrameCallback(frm1, nullptr);
EXPECT_TRUE(cap1->getFrame() == nullptr);
EXPECT_EQ(cap1->getFeatureList().size(), 0);
@@ -229,7 +229,7 @@ TEST_F(ProcessorLoopClosureTest, keyFrameCallbackCase4)
processor->captureCallback(cap1);
// keyframecallback
- problem->keyFrameCallback(frm1, nullptr, 0.5);
+ problem->keyFrameCallback(frm1, nullptr);
EXPECT_TRUE(cap1->getFrame() == nullptr);
EXPECT_EQ(cap1->getFeatureList().size(), 0);
@@ -249,11 +249,11 @@ TEST_F(ProcessorLoopClosureTest, captureCallbackMatch)
auto cap4 = createCapture(4);
// keyframecallback
- problem->keyFrameCallback(frm1, nullptr, 0.5);
- problem->keyFrameCallback(frm2, nullptr, 0.5);
- problem->keyFrameCallback(frm3, nullptr, 0.5);
- problem->keyFrameCallback(frm4, nullptr, 0.5);
- problem->keyFrameCallback(frm5, nullptr, 0.5);
+ problem->keyFrameCallback(frm1, nullptr);
+ problem->keyFrameCallback(frm2, nullptr);
+ problem->keyFrameCallback(frm3, nullptr);
+ problem->keyFrameCallback(frm4, nullptr);
+ problem->keyFrameCallback(frm5, nullptr);
// captureCallback
processor->captureCallback(cap4);
@@ -290,7 +290,7 @@ TEST_F(ProcessorLoopClosureTest, keyFrameCallbackMatch)
processor->captureCallback(cap5);
// keyframecallback
- problem->keyFrameCallback(frm2, nullptr, 0.5);
+ problem->keyFrameCallback(frm2, nullptr);
EXPECT_TRUE(cap1->getFrame() == nullptr);
EXPECT_TRUE(cap2->getFrame() == frm2);
diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp
index 8fbdd2e18f64f02d71c96ecd84b6470b30ecdc63..c6bef13ef0c9a73f3271cfd788cdb0bf5e859a41 100644
--- a/test/gtest_processor_motion.cpp
+++ b/test/gtest_processor_motion.cpp
@@ -187,7 +187,7 @@ TEST_F(ProcessorMotion_test, IntegrateStraightFactorPrior)
VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
// Matrix3d P0; P0.setIdentity();
VectorComposite s0(Vector3d(1,1,1), "PO", {2,1});
- auto KF_0 = problem->setPriorFactor(x0, s0, t, 0.01);
+ auto KF_0 = problem->setPriorFactor(x0, s0, t);
processor->setOrigin(KF_0);
data << 1, 0; // advance straight
@@ -214,7 +214,7 @@ TEST_F(ProcessorMotion_test, IntegrateStraightFixPrior)
// Matrix3d P0; P0.setIdentity();
VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
VectorComposite s0(Vector3d(1,1,1), "PO", {2,1});
- auto KF_0 = problem->setPriorFix(x0, t, 0.01);
+ auto KF_0 = problem->setPriorFix(x0, t);
processor->setOrigin(KF_0);
data << 1, 0; // advance straight
@@ -264,7 +264,7 @@ TEST_F(ProcessorMotion_test, IntegrateCircleFactorPrior)
VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
// Matrix3d P0; P0.setIdentity();
VectorComposite s0(Vector3d(1,1,1), "PO", {2,1});
- auto KF_0 = problem->setPriorFactor(x0, s0, t, 0.01);
+ auto KF_0 = problem->setPriorFactor(x0, s0, t);
processor->setOrigin(KF_0);
data << 1, 2*M_PI/10; // advance in circle
@@ -291,7 +291,7 @@ TEST_F(ProcessorMotion_test, IntegrateCircleFixPrior)
// Matrix3d P0; P0.setIdentity();
VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
VectorComposite s0(Vector3d(1,1,1), "PO", {2,1});
- auto KF_0 = problem->setPriorFix(x0, t, 0.01);
+ auto KF_0 = problem->setPriorFix(x0, t);
processor->setOrigin(KF_0);
data << 1, 2*M_PI/10; // advance in circle
@@ -436,7 +436,7 @@ TEST_F(ProcessorMotion_test, splitBufferFactorPrior)
VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
// Matrix3d P0; P0.setIdentity();
VectorComposite s0(Vector3d(1,1,1), "PO", {2,1});
- auto KF_0 = problem->setPriorFactor(x0, s0, t, 0.01);
+ auto KF_0 = problem->setPriorFactor(x0, s0, t);
processor->setOrigin(KF_0);
data << 1, 2*M_PI/10; // advance in circle
@@ -481,7 +481,7 @@ TEST_F(ProcessorMotion_test, splitBufferFixPrior)
// Matrix3d P0; P0.setIdentity();
VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
VectorComposite s0(Vector3d(1,1,1), "PO", {2,1});
- auto KF_0 = problem->setPriorFix(x0, t, 0.01);
+ auto KF_0 = problem->setPriorFix(x0, t);
processor->setOrigin(KF_0);
data << 1, 2*M_PI/10; // advance in circle
diff --git a/test/gtest_tree_manager.cpp b/test/gtest_tree_manager.cpp
index 938fdfb2a0c64b46f3ed09a8796a78ee022d7b21..97f43682e77e1d35526fd80fd0294df617a93fd9 100644
--- a/test/gtest_tree_manager.cpp
+++ b/test/gtest_tree_manager.cpp
@@ -115,7 +115,7 @@ TEST(TreeManager, keyFrameCallback)
ASSERT_EQ(GM->n_KF_, 0);
auto F0 = P->emplaceFrame(0, "PO", 3, VectorXd(7) );
- P->keyFrameCallback(F0, nullptr, 0);
+ P->keyFrameCallback(F0, nullptr);
ASSERT_EQ(GM->n_KF_, 1);
}
diff --git a/test/gtest_tree_manager_sliding_window.cpp b/test/gtest_tree_manager_sliding_window.cpp
index b6451a249b0ababcbabfc6d8300ad2ee3bedba92..916428c866d1aca77786cc2d01af92396c4a4f32 100644
--- a/test/gtest_tree_manager_sliding_window.cpp
+++ b/test/gtest_tree_manager_sliding_window.cpp
@@ -111,7 +111,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
// FRAME 2 ----------------------------------------------------------
auto F2 = P->emplaceFrame(TimeStamp(2), "PO", 3, state);
- P->keyFrameCallback(F2, nullptr, 0);
+ P->keyFrameCallback(F2, nullptr);
// absolute factor
auto C2 = CaptureBase::emplace<CaptureVoid>(F2, TimeStamp(2), nullptr);
@@ -130,7 +130,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
// FRAME 3 ----------------------------------------------------------
auto F3 = P->emplaceFrame(TimeStamp(3), "PO", 3, state);
- P->keyFrameCallback(F3, nullptr, 0);
+ P->keyFrameCallback(F3, nullptr);
// absolute factor
auto C3 = CaptureBase::emplace<CaptureVoid>(F3, TimeStamp(3), nullptr);
@@ -150,7 +150,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
// FRAME 4 ----------------------------------------------------------
auto F4 = P->emplaceFrame(TimeStamp(4), "PO", 3, state);
- P->keyFrameCallback(F4, nullptr, 0);
+ P->keyFrameCallback(F4, nullptr);
// absolute factor
auto C4 = CaptureBase::emplace<CaptureVoid>(F4, TimeStamp(4), nullptr);
@@ -173,7 +173,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
// FRAME 5 ----------------------------------------------------------
auto F5 = P->emplaceFrame(TimeStamp(5), "PO", 3, state);
- P->keyFrameCallback(F5, nullptr, 0);
+ P->keyFrameCallback(F5, nullptr);
// absolute factor
auto C5 = CaptureBase::emplace<CaptureVoid>(F5, TimeStamp(5), nullptr);
@@ -220,7 +220,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
// FRAME 2 ----------------------------------------------------------
auto F2 = P->emplaceFrame(TimeStamp(2), "PO", 3, state);
- P->keyFrameCallback(F2, nullptr, 0);
+ P->keyFrameCallback(F2, nullptr);
// absolute factor
auto C2 = CaptureBase::emplace<CaptureVoid>(F2, TimeStamp(2), nullptr);
@@ -239,7 +239,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
// FRAME 3 ----------------------------------------------------------
auto F3 = P->emplaceFrame(TimeStamp(3), "PO", 3, state);
- P->keyFrameCallback(F3, nullptr, 0);
+ P->keyFrameCallback(F3, nullptr);
// absolute factor
auto C3 = CaptureBase::emplace<CaptureVoid>(F3, TimeStamp(3), nullptr);
@@ -259,7 +259,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
// FRAME 4 ----------------------------------------------------------
auto F4 = P->emplaceFrame(TimeStamp(4), "PO", 3, state);
- P->keyFrameCallback(F4, nullptr, 0);
+ P->keyFrameCallback(F4, nullptr);
// absolute factor
auto C4 = CaptureBase::emplace<CaptureVoid>(F4, TimeStamp(4), nullptr);
@@ -282,7 +282,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
// FRAME 5 ----------------------------------------------------------
auto F5 = P->emplaceFrame(TimeStamp(5), "PO", 3, state);
- P->keyFrameCallback(F5, nullptr, 0);
+ P->keyFrameCallback(F5, nullptr);
// absolute factor
auto C5 = CaptureBase::emplace<CaptureVoid>(F5, TimeStamp(5), nullptr);
diff --git a/test/gtest_tree_manager_sliding_window_dual_rate.cpp b/test/gtest_tree_manager_sliding_window_dual_rate.cpp
index 75b7c6e0e7b58d408221e88f231aa62957242695..1c6636ffd57995248b0ad52ee591c881987b4389 100644
--- a/test/gtest_tree_manager_sliding_window_dual_rate.cpp
+++ b/test/gtest_tree_manager_sliding_window_dual_rate.cpp
@@ -141,7 +141,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
* fix fix
*/
auto F2 = P->emplaceFrame(TimeStamp(2), "PO", 3, state);
- P->keyFrameCallback(F2, nullptr, 0);
+ P->keyFrameCallback(F2, nullptr);
// absolute factor
auto C2 = CaptureBase::emplace<CaptureVoid>(F2, TimeStamp(2), nullptr);
@@ -176,7 +176,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
* fix fix
*/
auto F3 = P->emplaceFrame(TimeStamp(3), "PO", 3, state);
- P->keyFrameCallback(F3, nullptr, 0);
+ P->keyFrameCallback(F3, nullptr);
// absolute factor
auto C3 = CaptureBase::emplace<CaptureVoid>(F3, TimeStamp(3), nullptr);
@@ -221,7 +221,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
* fix fix
*/
auto F4 = P->emplaceFrame(TimeStamp(4), "PO", 3, state);
- P->keyFrameCallback(F4, nullptr, 0);
+ P->keyFrameCallback(F4, nullptr);
// absolute factor
auto C4 = CaptureBase::emplace<CaptureVoid>(F4, TimeStamp(4), nullptr);
@@ -275,7 +275,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
* fix fix
*/
auto F5 = P->emplaceFrame(TimeStamp(5), "PO", 3, state);
- P->keyFrameCallback(F5, nullptr, 0);
+ P->keyFrameCallback(F5, nullptr);
// absolute factor
auto C5 = CaptureBase::emplace<CaptureVoid>(F5, TimeStamp(5), nullptr);
@@ -337,7 +337,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
* fix fix
*/
auto F6 = P->emplaceFrame(TimeStamp(6), "PO", 3, state);
- P->keyFrameCallback(F6, nullptr, 0);
+ P->keyFrameCallback(F6, nullptr);
// absolute factor
auto C6 = CaptureBase::emplace<CaptureVoid>(F6, TimeStamp(6), nullptr);
@@ -408,7 +408,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
* fix fix
*/
auto F7 = P->emplaceFrame(TimeStamp(7), "PO", 3, state);
- P->keyFrameCallback(F7, nullptr, 0);
+ P->keyFrameCallback(F7, nullptr);
// absolute factor
auto C7 = CaptureBase::emplace<CaptureVoid>(F7, TimeStamp(7), nullptr);
@@ -487,7 +487,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
* fix fix
*/
auto F8 = P->emplaceFrame(TimeStamp(8), "PO", 3, state);
- P->keyFrameCallback(F8, nullptr, 0);
+ P->keyFrameCallback(F8, nullptr);
// absolute factor
auto C8 = CaptureBase::emplace<CaptureVoid>(F8, TimeStamp(8), nullptr);
@@ -615,7 +615,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowNoFixNoViral)
* ( ) ( ) ( )(F1)(F2)
*/
auto F2 = P->emplaceFrame(TimeStamp(2), "PO", 3, state);
- P->keyFrameCallback(F2, nullptr, 0);
+ P->keyFrameCallback(F2, nullptr);
// absolute factor
auto C2 = CaptureBase::emplace<CaptureVoid>(F2, TimeStamp(2), nullptr);
@@ -649,7 +649,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowNoFixNoViral)
* ( ) ( ) (F1)(F2)(F3)
*/
auto F3 = P->emplaceFrame(TimeStamp(3), "PO", 3, state);
- P->keyFrameCallback(F3, nullptr, 0);
+ P->keyFrameCallback(F3, nullptr);
// absolute factor
auto C3 = CaptureBase::emplace<CaptureVoid>(F3, TimeStamp(3), nullptr);
@@ -692,7 +692,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowNoFixNoViral)
* ( ) (F1)(F2)(F3)(F4)
*/
auto F4 = P->emplaceFrame(TimeStamp(4), "PO", 3, state);
- P->keyFrameCallback(F4, nullptr, 0);
+ P->keyFrameCallback(F4, nullptr);
// absolute factor
auto C4 = CaptureBase::emplace<CaptureVoid>(F4, TimeStamp(4), nullptr);
@@ -744,7 +744,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowNoFixNoViral)
* ( ) (F1) (F3)(F4)(F5)
*/
auto F5 = P->emplaceFrame(TimeStamp(5), "PO", 3, state);
- P->keyFrameCallback(F5, nullptr, 0);
+ P->keyFrameCallback(F5, nullptr);
// absolute factor
auto C5 = CaptureBase::emplace<CaptureVoid>(F5, TimeStamp(5), nullptr);
@@ -804,7 +804,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowNoFixNoViral)
* (F1) (F3)(F4)(F5)(F6)
*/
auto F6 = P->emplaceFrame(TimeStamp(6), "PO", 3, state);
- P->keyFrameCallback(F6, nullptr, 0);
+ P->keyFrameCallback(F6, nullptr);
// absolute factor
auto C6 = CaptureBase::emplace<CaptureVoid>(F6, TimeStamp(6), nullptr);
@@ -873,7 +873,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowNoFixNoViral)
* (F1) (F3) (F5)(F6)(F7)
*/
auto F7 = P->emplaceFrame(TimeStamp(7), "PO", 3, state);
- P->keyFrameCallback(F7, nullptr, 0);
+ P->keyFrameCallback(F7, nullptr);
// absolute factor
auto C7 = CaptureBase::emplace<CaptureVoid>(F7, TimeStamp(7), nullptr);
@@ -950,7 +950,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowNoFixNoViral)
* (F3) (F5)(F6)(F7)(F8)
*/
auto F8 = P->emplaceFrame(TimeStamp(8), "PO", 3, state);
- P->keyFrameCallback(F8, nullptr, 0);
+ P->keyFrameCallback(F8, nullptr);
// absolute factor
auto C8 = CaptureBase::emplace<CaptureVoid>(F8, TimeStamp(8), nullptr);