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.cpp b/demos/hello_wolf/processor_range_bearing.cpp
index 8b64ac0d1ce0020633aa208b82f3f0288c0dd0ce..1cb86165fdd7b19cdfdbf305cc3e2be9810087a0 100644
--- a/demos/hello_wolf/processor_range_bearing.cpp
+++ b/demos/hello_wolf/processor_range_bearing.cpp
@@ -50,30 +50,27 @@ void ProcessorRangeBearing::processCapture(CaptureBasePtr _capture)
}
// 1. get KF
- FrameBasePtr kf(nullptr);
- if ( !buffer_pack_kf_.empty() )
+ FrameBasePtr keyframe(nullptr);
+ if ( !buffer_frame_.empty() )
{
// KeyFrame Callback received
- PackKeyFramePtr pack = buffer_pack_kf_.selectPack( _capture->getTimeStamp(), params_->time_tolerance );
+ keyframe = buffer_frame_.select( _capture->getTimeStamp(), params_->time_tolerance );
- if (pack!=nullptr)
- kf = pack->key_frame;
+ buffer_frame_.removeUpTo( _capture->getTimeStamp() );
- buffer_pack_kf_.removeUpTo( _capture->getTimeStamp() );
-
- assert( kf && "Callback KF is not close enough to _capture!");
+ assert( keyframe && "Callback KF is not close enough to _capture!");
}
- if (!kf)
+ if (!keyframe)
{
// No KeyFrame callback received -- we assume a KF is available to hold this _capture (checked in assert below)
- kf = getProblem()->closestFrameToTimeStamp(_capture->getTimeStamp());
- assert( (fabs(kf->getTimeStamp() - _capture->getTimeStamp()) < params_->time_tolerance) && "Could not find a KF close enough to _capture!");
+ keyframe = getProblem()->closestFrameToTimeStamp(_capture->getTimeStamp());
+ assert( (fabs(keyframe->getTimeStamp() - _capture->getTimeStamp()) < params_->time_tolerance) && "Could not find a KF close enough to _capture!");
}
// 2. cast incoming capture to the range-and-bearing type, add it to the keyframe
CaptureRangeBearingPtr capture_rb = std::static_pointer_cast<CaptureRangeBearing>(_capture);
- capture_rb->link(kf);
+ capture_rb->link(keyframe);
// 3. explore all observations in the capture
for (SizeEigen i = 0; i < capture_rb->getIds().size(); i++)
@@ -103,14 +100,14 @@ void ProcessorRangeBearing::processCapture(CaptureBasePtr _capture)
}
// 5. create feature
- Vector2d rb(range,bearing);
+ Vector2d measurement_rb(range,bearing);
auto ftr = FeatureBase::emplace<FeatureRangeBearing>(capture_rb,
- rb,
+ measurement_rb,
getSensor()->getNoiseCov());
// 6. create factor
auto prc = shared_from_this();
- auto ctr = FactorBase::emplace<FactorRangeBearing>(ftr,
+ auto fac = FactorBase::emplace<FactorRangeBearing>(ftr,
capture_rb,
ftr,
lmk,
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/factor/factor_distance_3d.h b/include/core/factor/factor_distance_3d.h
index 96f9dfb784576b07caebfdde9762fb77961ddb95..bbcb846f626ad98d4a89456157f50c7b996637c7 100644
--- a/include/core/factor/factor_distance_3d.h
+++ b/include/core/factor/factor_distance_3d.h
@@ -78,10 +78,6 @@ class FactorDistance3d : public FactorAutodiff<FactorDistance3d, 1, 3, 3>
norm_squared += (T)1e-8;
}
Matrix<T,1,1> dist_exp ( sqrt(norm_squared) );
- // Matrix<T,1,1> dist_meas (getMeasurement().cast<T>());
- // Matrix<T,1,1> sqrt_info_upper = getMeasurementSquareRootInformationUpper();
-
- // res = sqrt_info_upper * (dist_meas - dist_exp);
res = getMeasurementSquareRootInformationUpper() * (getMeasurement() - dist_exp);
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 84996921ca13b9813e23724e709da3ed30afe28e..e3931fa1720bd83e152f8fb42b07adb0ccade6b8 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.h
@@ -80,22 +80,6 @@ static ProcessorBasePtr create(const std::string& _unique_name, const ParamsProc
-/** \brief Key frame class pack
- *
- * To store a key_frame with an associated time tolerance.
- *
- * Used in keyframe callbacks as the minimal pack of information needed by the processor receiving the callback.
- */
-class PackKeyFrame
-{
- public:
- PackKeyFrame(const FrameBasePtr _key_frame, const double _time_tolerance) : key_frame(_key_frame), time_tolerance(_time_tolerance) {};
- ~PackKeyFrame(){};
- FrameBasePtr key_frame;
- double time_tolerance;
-};
-
-WOLF_PTR_TYPEDEFS(PackKeyFrame);
/** \brief Buffer for arbitrary type objects
@@ -112,16 +96,16 @@ public:
Buffer(){};
~Buffer(void){};
- /**\brief Select a Pack from the buffer
+ /**\brief Select an element from the buffer
*
- * Select from the buffer the closest pack (w.r.t. time stamp),
+ * Select from the buffer the closest element (w.r.t. time stamp),
* respecting a defined time tolerances
*/
T select(const TimeStamp& _time_stamp, const double& _time_tolerance);
- /**\brief Select a Pack iterator from the buffer
+ /**\brief Select an element iterator from the buffer
*
- * Select from the buffer the iterator pointing to the closest pack (w.r.t. time stamp),
+ * Select from the buffer the iterator pointing to the closest element (w.r.t. time stamp),
* respecting a defined time tolerances
*/
Iterator selectIterator(const TimeStamp& _time_stamp, const double& _time_tolerance);
@@ -139,10 +123,10 @@ public:
*/
SizeStd size(void);
- /**\brief Add a pack to the buffer
+ /**\brief Add a element to the buffer
*
*/
- void add(const TimeStamp& _time_stamp, const T& _element); //const double& _time_tolerance);
+ void emplace(const TimeStamp& _time_stamp, const T& _element); //const double& _time_tolerance);
/** \brief returns the container with elements of the buffer
*
@@ -150,12 +134,12 @@ public:
*/
std::map<TimeStamp,T>& getContainer();
- /**\brief Remove all packs in the buffer with a time stamp older than the specified
+ /**\brief Remove all elements in the buffer with a time stamp older than the specified
*
*/
void removeUpTo(const TimeStamp& _time_stamp);
- /**\brief Remove all packs in the buffer with a time stamp older than the specified
+ /**\brief Remove all elements in the buffer with a time stamp older than the specified
*
*/
void removeUpToLower(const TimeStamp& _time_stamp);
@@ -170,12 +154,13 @@ public:
*/
bool empty();
+protected:
/**\brief Check time tolerance
*
* Check if the time distance between two time stamps is smaller than
* the time tolerance.
*/
- static bool simpleCheckTimeTolerance(const TimeStamp& _time_stamp1, const TimeStamp& _time_stamp2, const double& _time_tolerance);
+ static bool checkTimeTolerance(const TimeStamp& _time_stamp1, const TimeStamp& _time_stamp2, const double& _time_tolerance);
/**\brief Check time tolerance
*
@@ -190,47 +175,16 @@ protected:
};
-/** \brief Buffer of Key frame class objects
+/** \brief Buffer of Frames
*
- * Object and functions to manage a buffer of KFPack objects.
+ * Object and functions to manage a buffer of FrameBasePtr objects.
*/
-class BufferPackKeyFrame : public Buffer<PackKeyFramePtr>
-{
- public:
+class BufferFrame : public Buffer<FrameBasePtr> { };
- /**\brief Select a Pack from the buffer
- *
- * Select from the buffer the closest pack (w.r.t. time stamp),
- * respecting a defined time tolerances
- */
- PackKeyFramePtr selectPack(const TimeStamp& _time_stamp, const double& _time_tolerance);
- PackKeyFramePtr selectPack(const CaptureBasePtr _capture, const double& _time_tolerance);
-
- PackKeyFramePtr selectFirstPackBefore(const TimeStamp& _time_stamp, const double& _time_tolerance);
- PackKeyFramePtr selectFirstPackBefore(const CaptureBasePtr _capture, const double& _time_tolerance);
-
- /**\brief Add a pack to the buffer
- *
- */
- void add(const FrameBasePtr& _key_frame, const double& _time_tolerance);
-
- /**\brief Print buffer information
- *
- */
- void print() const;
- /**\brief Alias funct
- *
- */
- static bool checkTimeTolerance(const TimeStamp& _time_stamp1, const double& _time_tolerance1, const TimeStamp& _time_stamp2, const double& _time_tolerance2)
- { return doubleCheckTimeTolerance(_time_stamp1, _time_tolerance1, _time_stamp2, _time_tolerance2); };
-
-};
-
-
-/** \brief Buffer of Capture class objects
+/** \brief Buffer of Captures
*
- * Object and functions to manage a buffer of Capture objects.
+ * Object and functions to manage a buffer of CaptureBasePtr objects.
*/
class BufferCapture : public Buffer<CaptureBasePtr> {};
@@ -276,7 +230,7 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
protected:
unsigned int processor_id_;
ParamsProcessorBasePtr params_;
- BufferPackKeyFrame buffer_pack_kf_;
+ BufferFrame buffer_frame_;
BufferCapture buffer_capture_;
int dim_;
@@ -320,7 +274,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
*
@@ -332,7 +286,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
*
@@ -362,16 +316,16 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
public:
/**\brief notify a new keyframe made by another processor
*
- * It stores the new KF in buffer_pack_kf_ and calls triggerInKF()
+ * 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);
/**\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:
@@ -380,7 +334,6 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
public:
bool isMotionProvider() const;
- void setTimeTolerance(double _time_tolerance);
bool isVotingActive() const;
@@ -388,7 +341,13 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
int getDim() const;
+ void setTimeTolerance(double _time_tolerance);
double getTimeTolerance() const;
+ bool checkTimeTolerance (const TimeStamp& _ts1, const TimeStamp& _ts2);
+ bool checkTimeTolerance (const CaptureBasePtr _cap, const TimeStamp& _ts);
+ bool checkTimeTolerance (const FrameBasePtr _frm, const TimeStamp& _ts);
+ bool checkTimeTolerance (const FrameBasePtr _frm, const CaptureBasePtr _cap);
+
void link(SensorBasePtr);
template<typename classType, typename... T>
@@ -464,14 +423,15 @@ inline SensorBasePtr ProcessorBase::getSensor() const
return sensor_ptr_.lock();
}
-inline void ProcessorBase::setTimeTolerance(double _time_tolerance)
-{
- params_->time_tolerance = _time_tolerance;
-}
inline int ProcessorBase::getDim() const
{
return dim_;
}
+
+inline void ProcessorBase::setTimeTolerance(double _time_tolerance)
+{
+ params_->time_tolerance = _time_tolerance;
+}
inline double ProcessorBase::getTimeTolerance() const
{
return params_->time_tolerance;
@@ -496,13 +456,13 @@ typename Buffer<T>::Iterator Buffer<T>::selectIterator(const TimeStamp& _time_st
bool prev_exists = (post != container_.begin());
bool post_exists = (post != container_.end());
- bool post_ok = post_exists && simpleCheckTimeTolerance(post->first, _time_stamp, _time_tolerance);
+ bool post_ok = post_exists && checkTimeTolerance(post->first, _time_stamp, _time_tolerance);
if (prev_exists)
{
Buffer<T>::Iterator prev = std::prev(post);
- bool prev_ok = simpleCheckTimeTolerance(prev->first, _time_stamp, _time_tolerance);
+ bool prev_ok = checkTimeTolerance(prev->first, _time_stamp, _time_tolerance);
if (prev_ok && !post_ok)
return prev;
@@ -548,16 +508,16 @@ T Buffer<T>::selectFirstBefore(const TimeStamp& _time_stamp, const double& _time
if (container_.empty())
return nullptr;
- // Checking on begin() since packs are ordered in time
- // Return first pack if is older than time stamp
+ // Checking on begin() since elements are ordered in time
+ // Return first element if is older than time stamp
if (container_.begin()->first < _time_stamp)
return container_.begin()->second;
- // Return first pack if despite being newer, it is within the time tolerance
- if (simpleCheckTimeTolerance(container_.begin()->first, _time_stamp, _time_tolerance))
+ // Return first element if despite being newer, it is within the time tolerance
+ if (checkTimeTolerance(container_.begin()->first, _time_stamp, _time_tolerance))
return container_.begin()->second;
- // otherwise return nullptr (no pack before the provided ts or within the tolerance was found)
+ // otherwise return nullptr (no element before the provided ts or within the tolerance was found)
return nullptr;
}
@@ -569,16 +529,16 @@ T Buffer<T>::selectLastAfter(const TimeStamp& _time_stamp, const double& _time_t
if (container_.empty())
return nullptr;
- // Checking on rbegin() since packs are ordered in time
- // Return last pack if is newer than time stamp
+ // Checking on rbegin() since elements are ordered in time
+ // Return last element if is newer than time stamp
if (container_.rbegin()->first > _time_stamp)
return container_.rbegin()->second;
- // Return last pack if despite being older, it is within the time tolerance
- if (simpleCheckTimeTolerance(container_.rbegin()->first, _time_stamp, _time_tolerance))
+ // Return last element if despite being older, it is within the time tolerance
+ if (checkTimeTolerance(container_.rbegin()->first, _time_stamp, _time_tolerance))
return container_.rbegin()->second;
- // otherwise return nullptr (no pack after the provided ts or within the tolerance was found)
+ // otherwise return nullptr (no element after the provided ts or within the tolerance was found)
return nullptr;
}
@@ -605,7 +565,7 @@ T Buffer<T>::selectLast()
}
template <typename T>
-void Buffer<T>::add(const TimeStamp& _time_stamp, const T& _element)
+void Buffer<T>::emplace(const TimeStamp& _time_stamp, const T& _element)
{
container_.emplace(_time_stamp, _element);
}
@@ -649,8 +609,10 @@ inline void Buffer<T>::removeUpToLower(const TimeStamp& _time_stamp)
}
template <typename T>
-inline bool Buffer<T>::doubleCheckTimeTolerance(const TimeStamp& _time_stamp1, const double& _time_tolerance1,
- const TimeStamp& _time_stamp2, const double& _time_tolerance2)
+inline bool Buffer<T>::doubleCheckTimeTolerance(const TimeStamp& _time_stamp1,
+ const double& _time_tolerance1,
+ const TimeStamp& _time_stamp2,
+ const double& _time_tolerance2)
{
double time_diff = std::fabs(_time_stamp1 - _time_stamp2);
double time_tol = std::min(_time_tolerance1, _time_tolerance2);
@@ -659,8 +621,9 @@ inline bool Buffer<T>::doubleCheckTimeTolerance(const TimeStamp& _time_stamp1, c
}
template <typename T>
-inline bool Buffer<T>::simpleCheckTimeTolerance(const TimeStamp& _time_stamp1, const TimeStamp& _time_stamp2,
- const double& _time_tolerance)
+inline bool Buffer<T>::checkTimeTolerance(const TimeStamp& _time_stamp1,
+ const TimeStamp& _time_stamp2,
+ const double& _time_tolerance)
{
double time_diff = std::fabs(_time_stamp1 - _time_stamp2);
bool pass = time_diff <= _time_tolerance;
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 7a657aaae5cb74890c2ab95cf647e689478fe0b3..5d9716490f1bfb230dba745e812a3a0104238718 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
*
@@ -301,7 +301,9 @@ class ProcessorMotion : public ProcessorBase, public MotionProvider
*/
virtual void postProcess(){ };
- PackKeyFramePtr computeProcessingStep();
+ FrameBasePtr computeProcessingStep();
+
+
// These are the pure virtual functions doing the mathematics
public:
@@ -541,6 +543,7 @@ class ProcessorMotion : public ProcessorBase, public MotionProvider
Eigen::MatrixXd unmeasured_perturbation_cov_; ///< Covariance of unmeasured DoF to avoid singularity
};
+
}
#include "core/frame/frame_base.h"
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 932b6d0ea55ff5b9a0f4df8b7457be90c8de62cc..370421716736811e1885d9ba05c405ddd5230a2d 100644
--- a/include/core/processor/processor_tracker.h
+++ b/include/core/processor/processor_tracker.h
@@ -108,12 +108,12 @@ class ProcessorTracker : public ProcessorBase
{
public:
typedef enum {
- FIRST_TIME_WITH_PACK,
- FIRST_TIME_WITHOUT_PACK,
- SECOND_TIME_WITH_PACK,
- SECOND_TIME_WITHOUT_PACK,
- RUNNING_WITH_PACK,
- RUNNING_WITHOUT_PACK
+ FIRST_TIME_WITH_KEYFRAME,
+ FIRST_TIME_WITHOUT_KEYFRAME,
+ SECOND_TIME_WITH_KEYFRAME,
+ SECOND_TIME_WITHOUT_KEYFRAME,
+ RUNNING_WITH_KEYFRAME,
+ RUNNING_WITHOUT_KEYFRAME
} ProcessingStep ;
protected:
@@ -138,11 +138,6 @@ class ProcessorTracker : public ProcessorBase
StateStructure getStateStructure() const;
- bool checkTimeTolerance(const TimeStamp& _ts1, const TimeStamp& _ts2);
- bool checkTimeTolerance(const CaptureBasePtr _cap, const TimeStamp& _ts);
- bool checkTimeTolerance(const FrameBasePtr _frm, const TimeStamp& _ts);
- bool checkTimeTolerance(const FrameBasePtr _frm, const CaptureBasePtr _cap);
-
virtual CaptureBasePtr getOrigin() const;
virtual CaptureBasePtr getLast() const;
virtual CaptureBasePtr getIncoming() const;
@@ -159,7 +154,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 +166,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
*
@@ -306,26 +301,6 @@ inline FeatureBasePtrList& ProcessorTracker::getNewFeaturesListIncoming()
return new_features_incoming_;
}
-inline bool ProcessorTracker::checkTimeTolerance(const TimeStamp& _ts1, const TimeStamp& _ts2)
-{
- return (std::fabs(_ts2 - _ts2) < params_tracker_->time_tolerance);
-}
-
-inline bool ProcessorTracker::checkTimeTolerance(const CaptureBasePtr _cap, const TimeStamp& _ts)
-{
- return checkTimeTolerance(_cap->getTimeStamp(), _ts);
-}
-
-inline bool ProcessorTracker::checkTimeTolerance(const FrameBasePtr _frm, const TimeStamp& _ts)
-{
- return checkTimeTolerance(_frm->getTimeStamp(), _ts);
-}
-
-inline bool ProcessorTracker::checkTimeTolerance(const FrameBasePtr _frm, const CaptureBasePtr _cap)
-{
- return checkTimeTolerance(_frm->getTimeStamp(), _cap->getTimeStamp());
-}
-
inline StateStructure ProcessorTracker::getStateStructure ( ) const
{
return state_structure_;
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 0627215bf19e5d8a50f7728e4c8ae376f320f77f..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")
{
@@ -1137,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;
@@ -1147,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 0e88a448e98041ce034390d8bfd324f41528b1d6..2673eb2469e5403cb0c985790a9f9452f1cdd507 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -54,31 +54,31 @@ 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)
{
- 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();
// asking if frame should be stored
- if (storeKeyFrame(_keyframe_ptr))
- buffer_pack_kf_.add(_keyframe_ptr, _time_tol_other);
+ if (storeKeyFrame(_keyframe))
+ buffer_frame_.emplace(_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))
+ processKeyFrame(_keyframe);
// 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_++;
@@ -86,15 +86,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_.emplace(_capture->getTimeStamp(), _capture);
// asking if capture should be processed
- if (triggerInCapture(_capture_ptr))
- processCapture(_capture_ptr);
+ if (triggerInCapture(_capture))
+ processCapture(_capture);
// profiling
stopCaptureProfiling();
@@ -149,107 +149,12 @@ void ProcessorBase::setProblem(ProblemPtr _problem)
NodeBase::setProblem(_problem);
- // adding processor is motion to the processor is motion vector
+ // adding motion provider to the motion providers vector
auto motion_provider_ptr = std::dynamic_pointer_cast<MotionProvider>(shared_from_this());
if (motion_provider_ptr)
motion_provider_ptr->addToProblem(_problem, motion_provider_ptr);
}
-/////////////////////////////////////////////////////////////////////////////////////////
-
-void BufferPackKeyFrame::add(const FrameBasePtr& _key_frame, const double& _time_tolerance)
-{
- TimeStamp time_stamp = _key_frame->getTimeStamp();
- PackKeyFramePtr kfpack = std::make_shared<PackKeyFrame>(_key_frame, _time_tolerance);
- Buffer::add(time_stamp, kfpack);
-}
-
-PackKeyFramePtr BufferPackKeyFrame::selectPack(const TimeStamp& _time_stamp, const double& _time_tolerance)
-{
- if (container_.empty())
- return nullptr;
-
- BufferPackKeyFrame::Iterator post = container_.upper_bound(_time_stamp);
-
- // remove packs corresponding to removed KFs (keeping the next iterator in post)
- while (post != container_.end() && post->second->key_frame->isRemoving())
- post = container_.erase(post);
- while (post != container_.begin() && std::prev(post)->second->key_frame->isRemoving())
- container_.erase(std::prev(post));
-
- bool prev_exists = (post != container_.begin());
- bool post_exists = (post != container_.end());
-
- bool post_ok = post_exists && doubleCheckTimeTolerance(post->first, post->second->time_tolerance, _time_stamp, _time_tolerance);
-
- if (prev_exists)
- {
- BufferPackKeyFrame::Iterator prev = std::prev(post);
-
- bool prev_ok = doubleCheckTimeTolerance(prev->first, prev->second->time_tolerance, _time_stamp, _time_tolerance);
-
- if (prev_ok && !post_ok)
- return prev->second;
-
- else if (!prev_ok && post_ok)
- return post->second;
-
- else if (prev_ok && post_ok)
- {
- if (std::fabs(post->first - _time_stamp) < std::fabs(prev->first - _time_stamp))
- return post->second;
- else
- return prev->second;
- }
- }
- else if (post_ok)
- return post->second;
-
- return nullptr;
-}
-PackKeyFramePtr BufferPackKeyFrame::selectPack(const CaptureBasePtr _capture, const double& _time_tolerance)
-{
- return selectPack(_capture->getTimeStamp(), _time_tolerance);
-}
-
-PackKeyFramePtr BufferPackKeyFrame::selectFirstPackBefore(const TimeStamp& _time_stamp, const double& _time_tolerance)
-{
- // remove packs corresponding to removed KFs
- while (!container_.empty() && container_.begin()->second->key_frame->isRemoving())
- container_.erase(container_.begin());
-
- // There is no pack
- if (container_.empty())
- return nullptr;
-
- // Checking on begin() since packs are ordered in time
- // Return first pack if is older than time stamp
- if (container_.begin()->first < _time_stamp)
- return container_.begin()->second;
-
- // Return first pack if despite being newer, it is within the time tolerance
- if (doubleCheckTimeTolerance(container_.begin()->first, container_.begin()->second->time_tolerance, _time_stamp, _time_tolerance))
- return container_.begin()->second;
-
- // otherwise return nullptr (no pack before the provided ts or within the tolerance was found)
- return nullptr;
-
-}
-
-PackKeyFramePtr BufferPackKeyFrame::selectFirstPackBefore(const CaptureBasePtr _capture, const double& _time_tolerance)
-{
- return selectFirstPackBefore(_capture->getTimeStamp(), _time_tolerance);
-}
-
-void BufferPackKeyFrame::print(void) const
-{
- std::cout << "[ ";
- for (auto iter : container_)
- {
- std::cout << "( tstamp: " << iter.first << ", id: " << iter.second->key_frame->id() << ") ";
- }
- std::cout << "]" << std::endl;
-}
void ProcessorBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
{
@@ -312,4 +217,25 @@ void ProcessorBase::printProfiling(std::ostream& _stream) const
}
+bool ProcessorBase::checkTimeTolerance (const TimeStamp& _ts1, const TimeStamp& _ts2)
+{
+ auto dt = std::fabs(_ts1 - _ts2);
+ return dt <= params_->time_tolerance;
+}
+
+bool ProcessorBase::checkTimeTolerance (const CaptureBasePtr _cap, const TimeStamp& _ts)
+{
+ return checkTimeTolerance(_cap->getTimeStamp(), _ts);
+}
+
+bool ProcessorBase::checkTimeTolerance (const FrameBasePtr _frm, const TimeStamp& _ts)
+{
+ return checkTimeTolerance(_frm->getTimeStamp(), _ts);
+}
+
+bool ProcessorBase::checkTimeTolerance (const FrameBasePtr _frm, const CaptureBasePtr _cap)
+{
+ return checkTimeTolerance(_frm->getTimeStamp(), _cap->getTimeStamp());
+}
+
} // namespace wolf
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 927d873f48fe70ffaef085a0d28a9fdf84bbf03f..51001cdaed74aa3e368b3bc60b81beb13b659327 100644
--- a/src/processor/processor_loop_closure.cpp
+++ b/src/processor/processor_loop_closure.cpp
@@ -51,34 +51,34 @@ void ProcessorLoopClosure::processCapture(CaptureBasePtr _capture)
process(_capture);
// remove the frame and older frames
- buffer_pack_kf_.removeUpTo(_capture->getFrame()->getTimeStamp());
+ buffer_frame_.removeUpTo(_capture->getFrame()->getTimeStamp());
return;
}
// Search for any stored frame within time tolerance of capture
- auto frame_pack = buffer_pack_kf_.select(_capture->getTimeStamp(), params_->time_tolerance);
+ auto keyframe_from_callback = buffer_frame_.select(_capture->getTimeStamp(), params_->time_tolerance);
// CASE 2:
- if (_capture->getFrame() == nullptr and frame_pack)
+ if (_capture->getFrame() == nullptr and keyframe_from_callback)
{
WOLF_DEBUG("CASE 2");
- _capture->link(frame_pack->key_frame);
+ _capture->link(keyframe_from_callback);
process(_capture);
// remove the frame and older frames
- buffer_pack_kf_.removeUpTo(frame_pack->key_frame->getTimeStamp());
+ buffer_frame_.removeUpTo(keyframe_from_callback->getTimeStamp());
return;
}
// CASE 3:
WOLF_DEBUG("CASE 3");
- buffer_capture_.add(_capture->getTimeStamp(), _capture);
+ buffer_capture_.emplace(_capture->getTimeStamp(), _capture);
}
-void ProcessorLoopClosure::processKeyFrame(FrameBasePtr _frame, const double& _time_tolerance)
+void ProcessorLoopClosure::processKeyFrame(FrameBasePtr _keyframe)
{
/* This function has 4 scenarios:
* 1. Frame already have a capture of the sensor -> process
@@ -87,31 +87,31 @@ void ProcessorLoopClosure::processKeyFrame(FrameBasePtr _frame, const double& _t
* 4. Otherwise: The frame is not compatible with any stored capture -> discard frame
*/
- WOLF_DEBUG("ProcessorLoopClosure::processKeyFrame frame ", _frame->id());
+ WOLF_DEBUG("ProcessorLoopClosure::processKeyFrame frame ", _keyframe->id());
// CASE 1:
- auto cap = _frame->getCaptureOf(getSensor());
- if (cap)
+ auto capture = _keyframe->getCaptureOf(getSensor());
+ if (capture)
{
WOLF_DEBUG("CASE 1");
- process(cap);
+ process(capture);
// remove the capture (if stored)
- buffer_capture_.getContainer().erase(cap->getTimeStamp());
+ buffer_capture_.getContainer().erase(capture->getTimeStamp());
return;
}
// Search for any stored capture within time tolerance of frame
- auto capture = buffer_capture_.select(_frame->getTimeStamp(), params_->time_tolerance);
+ capture = buffer_capture_.select(_keyframe->getTimeStamp(), params_->time_tolerance);
// CASE 2:
if (capture and not capture->getFrame())
{
WOLF_DEBUG("CASE 2");
- capture->link(_frame);
+ capture->link(_keyframe);
process(capture);
@@ -119,17 +119,17 @@ void ProcessorLoopClosure::processKeyFrame(FrameBasePtr _frame, const double& _t
buffer_capture_.getContainer().erase(capture->getTimeStamp());
// remove old captures (10s of old captures are kept in case frames arrives unordered)
- buffer_capture_.removeUpTo(_frame->getTimeStamp() - 10);
+ buffer_capture_.removeUpTo(_keyframe->getTimeStamp() - 10);
return;
}
// CASE 3:
- if (buffer_capture_.selectLastAfter(_frame->getTimeStamp(), params_->time_tolerance) == nullptr)
+ if (buffer_capture_.selectLastAfter(_keyframe->getTimeStamp(), params_->time_tolerance) == nullptr)
{
WOLF_DEBUG("CASE 3");
// store frame
- buffer_pack_kf_.add(_frame, _time_tolerance);
+ buffer_frame_.emplace(_keyframe->getTimeStamp(), _keyframe);
return;
}
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 6b9b4b031b1091ca2e832f3f76336fd7cffc943c..92c6776dce32024b8eea880040f561f760643888 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -183,9 +183,9 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
preProcess(); // Derived class operations
- PackKeyFramePtr pack = computeProcessingStep();
- if (pack)
- buffer_pack_kf_.removeUpTo( pack->key_frame->getTimeStamp() );
+ FrameBasePtr keyframe_from_callback = computeProcessingStep();
+ if (keyframe_from_callback)
+ buffer_frame_.removeUpTo( keyframe_from_callback->getTimeStamp() );
switch(processing_step_)
{
@@ -204,7 +204,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
case FIRST_TIME_WITH_KF_ON_INCOMING :
{
// can joint to the KF
- setOrigin(pack->key_frame);
+ setOrigin(keyframe_from_callback);
break;
}
case FIRST_TIME_WITH_KF_AFTER_INCOMING :
@@ -259,21 +259,20 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
* \_f_/ \_f_/
*/
- // extract pack elements
- FrameBasePtr keyframe_from_callback = pack->key_frame;
- TimeStamp ts_from_callback = keyframe_from_callback->getTimeStamp();
+ // extract KF elements
+ TimeStamp timestamp_from_callback = keyframe_from_callback->getTimeStamp();
// find the capture whose buffer is affected by the new keyframe
- auto capture_existing = findCaptureContainingTimeStamp(ts_from_callback); // k
+ auto capture_existing = findCaptureContainingTimeStamp(timestamp_from_callback); // k
if (!capture_existing)
{
- WOLF_WARN("A KF before first motion capture (TS = ", ts_from_callback, "). ProcessorMotion cannot do anything.");
+ WOLF_WARN("A KF before first motion capture (TS = ", timestamp_from_callback, "). ProcessorMotion cannot do anything.");
break;
}
// update KF state (adding missing StateBlocks)
- auto proc_state = getState(ts_from_callback);
+ auto proc_state = getState(timestamp_from_callback);
for (auto pair_ckey_vec : proc_state)
if (!keyframe_from_callback->isInStructure(pair_ckey_vec.first))
keyframe_from_callback->addStateBlock(pair_ckey_vec.first,
@@ -292,7 +291,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// emplace a new motion capture to the new keyframe
auto capture_for_keyframe_callback = emplaceCapture(keyframe_from_callback, // j
getSensor(),
- ts_from_callback,
+ timestamp_from_callback,
Eigen::VectorXd::Zero(data_size_),
getSensor()->getNoiseCov(),
calib_origin,
@@ -301,7 +300,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// split the buffer
// and give the part of the buffer before the new keyframe to the capture for the KF callback
- splitBuffer(capture_existing, ts_from_callback, keyframe_from_callback, capture_for_keyframe_callback);
+ splitBuffer(capture_existing, timestamp_from_callback, keyframe_from_callback, capture_for_keyframe_callback);
// create motion feature and add it to the capture
auto feature_new = emplaceFeature(capture_for_keyframe_callback);
@@ -316,20 +315,10 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
capture_existing->getFeatureList().back()->remove(); // factor is removed automatically
assert(capture_existing->getFeatureList().empty());// there was only one feature!
+
auto new_feature_existing = emplaceFeature(capture_existing);
- emplaceFactor(new_feature_existing, capture_for_keyframe_callback);
-// auto feature_existing = capture_existing->getFeatureList().back(); // there is only one feature!
-//
-// // Modify existing feature --------
-// feature_existing->setMeasurement (capture_existing->getBuffer().back().delta_integr_);
-// feature_existing->setMeasurementCovariance(capture_existing->getBuffer().back().delta_integr_cov_);
-//
-// // Modify existing factor --------
-// // Instead of modifying, we remove one ctr, and create a new one.
-// auto fac_to_remove = feature_existing->getFactorList().back(); // there is only one factor!
-// auto new_ctr = emplaceFactor(feature_existing, capture_for_keyframe_callback);
-// fac_to_remove ->remove(); // remove old factor now (otherwise c->remove() gets propagated to f, C, F, etc.)
+ emplaceFactor(new_feature_existing, capture_for_keyframe_callback);
}
break;
@@ -370,17 +359,16 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
* \____f____/ \_f_/
*/
- // extract pack elements
- FrameBasePtr keyframe_from_callback = pack->key_frame;
- TimeStamp ts_from_callback = keyframe_from_callback->getTimeStamp();
+ // extract KF elements
+ TimeStamp timestamp_from_callback = keyframe_from_callback->getTimeStamp();
// update KF state (adding missing StateBlocks)
- auto proc_state = getState(ts_from_callback);
- for (auto pair_ckey_vec : proc_state)
- if (!keyframe_from_callback->isInStructure(pair_ckey_vec.first))
- keyframe_from_callback->addStateBlock(pair_ckey_vec.first,
- FactoryStateBlock::create(string(1, pair_ckey_vec.first),
- pair_ckey_vec.second,
+ auto proc_state = this->getState(timestamp_from_callback);
+ for (auto pair_key_vector : proc_state)
+ if (!keyframe_from_callback->isInStructure(pair_key_vector.first))
+ keyframe_from_callback->addStateBlock(pair_key_vector.first,
+ FactoryStateBlock::create(string(1, pair_key_vector.first),
+ pair_key_vector.second,
false),
getProblem());
keyframe_from_callback->setState(proc_state);
@@ -396,7 +384,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// emplace a new motion capture to the new keyframe
auto capture_for_keyframe_callback = emplaceCapture(keyframe_from_callback,
getSensor(),
- ts_from_callback,
+ timestamp_from_callback,
Eigen::VectorXd::Zero(data_size_),
getSensor()->getNoiseCov(),
calib_origin,
@@ -405,7 +393,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// split the buffer
// and give the part of the buffer before the new keyframe to the capture for the KF callback
- splitBuffer(capture_existing, ts_from_callback, keyframe_from_callback, capture_for_keyframe_callback);
+ splitBuffer(capture_existing, timestamp_from_callback, keyframe_from_callback, capture_for_keyframe_callback);
// create motion feature and add it to the capture
auto feature_for_keyframe_callback = emplaceFeature(capture_for_keyframe_callback);
@@ -475,8 +463,8 @@ 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 ->link(getProblem());
// create motion feature and add it to the key_capture
auto key_feature = emplaceFeature(last_ptr_);
@@ -491,14 +479,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();
@@ -509,7 +497,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());
}
@@ -933,21 +921,22 @@ CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp
return capture_motion;
}
-PackKeyFramePtr ProcessorMotion::computeProcessingStep()
+FrameBasePtr ProcessorMotion::computeProcessingStep()
{
// Origin not set
if (!origin_ptr_)
{
- PackKeyFramePtr pack = buffer_pack_kf_.selectFirstPackBefore(incoming_ptr_, params_motion_->time_tolerance);
+ FrameBasePtr keyframe_from_callback = buffer_frame_.selectFirstBefore(incoming_ptr_->getTimeStamp(), params_motion_->time_tolerance);
- if (pack)
+ if (keyframe_from_callback)
{
- if (buffer_pack_kf_.checkTimeTolerance(pack->key_frame->getTimeStamp(), pack->time_tolerance, incoming_ptr_->getTimeStamp(), params_motion_->time_tolerance))
+ if (checkTimeTolerance(keyframe_from_callback,
+ incoming_ptr_))
{
WOLF_DEBUG("First time with a KF compatible.")
processing_step_ = FIRST_TIME_WITH_KF_ON_INCOMING;
}
- else if (pack->key_frame->getTimeStamp() < incoming_ptr_->getTimeStamp())
+ else if (keyframe_from_callback->getTimeStamp() < incoming_ptr_->getTimeStamp())
{
WOLF_DEBUG("First time with a KF too old. It seems the prior has been set before receiving the first capture of this processor.")
processing_step_ = FIRST_TIME_WITH_KF_BEFORE_INCOMING;
@@ -964,23 +953,24 @@ PackKeyFramePtr ProcessorMotion::computeProcessingStep()
processing_step_ = FIRST_TIME_WITHOUT_KF;
}
- return pack;
+ return keyframe_from_callback;
}
else
{
- PackKeyFramePtr pack = buffer_pack_kf_.selectFirstPackBefore(last_ptr_, params_motion_->time_tolerance);
+ FrameBasePtr keyframe_from_callback = buffer_frame_.selectFirstBefore(last_ptr_->getTimeStamp(), params_motion_->time_tolerance);
// ignore "future" KF to avoid MotionBuffer::split() error
- if (pack && pack->key_frame->getTimeStamp() > last_ptr_->getBuffer().back().ts_)
- pack = nullptr;
+ if (keyframe_from_callback && keyframe_from_callback->getTimeStamp() > last_ptr_->getBuffer().back().ts_)
+ keyframe_from_callback = nullptr;
- if (pack)
+ if (keyframe_from_callback)
{
- if (buffer_pack_kf_.checkTimeTolerance(pack->key_frame->getTimeStamp(), pack->time_tolerance, origin_ptr_->getTimeStamp(), params_motion_->time_tolerance))
+ if (checkTimeTolerance(keyframe_from_callback,
+ origin_ptr_))
processing_step_ = RUNNING_WITH_KF_ON_ORIGIN;
- else if (pack->key_frame->getTimeStamp() < origin_ptr_->getTimeStamp())
+ else if (keyframe_from_callback->getTimeStamp() < origin_ptr_->getTimeStamp())
processing_step_ = RUNNING_WITH_KF_BEFORE_ORIGIN;
@@ -992,7 +982,7 @@ PackKeyFramePtr ProcessorMotion::computeProcessingStep()
else
processing_step_ = RUNNING_WITHOUT_KF;
- return pack;
+ return keyframe_from_callback;
}
// not reached
diff --git a/src/processor/processor_pose.cpp b/src/processor/processor_pose.cpp
index c8e9c5aa9351ae19b232f63f4205347586985122..21a1a5624eb55aff99cb8919ad931216ad873172 100644
--- a/src/processor/processor_pose.cpp
+++ b/src/processor/processor_pose.cpp
@@ -45,24 +45,19 @@ void ProcessorPose::configure(SensorBasePtr _sensor)
void ProcessorPose::createFactorIfNecessary(){
auto sensor_pose = std::static_pointer_cast<SensorPose>(getSensor());
- auto kf_it_last = buffer_pack_kf_.getContainer().end();
- auto kf_it = buffer_pack_kf_.getContainer().begin();
- while (kf_it != buffer_pack_kf_.getContainer().end())
+ auto kf_it_last = buffer_frame_.getContainer().end();
+ auto kf_it = buffer_frame_.getContainer().begin();
+ while (kf_it != buffer_frame_.getContainer().end())
{
TimeStamp t = kf_it->first;
- double time_tolerance = std::min(getTimeTolerance(), kf_it->second->time_tolerance);
- if (getTimeTolerance() == 0.0){
- WOLF_WARN("Time tolerance set to zero -> value not used");
- time_tolerance = kf_it->second->time_tolerance;
- }
- 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())
{
// if a corresponding capture exists, link it to the KF and create a factor
auto cap = std::static_pointer_cast<CapturePose>(cap_it->second);
- cap->link(kf_it->second->key_frame);
+ cap->link(kf_it->second);
FeatureBasePtr feat = FeatureBase::emplace<FeatureBase>(cap, "Pose", cap->getData(), cap->getDataCovariance());
FactorPose3dWithExtrinsicsPtr fac = FactorBase::emplace<FactorPose3dWithExtrinsics>(feat, feat, shared_from_this(), false, TOP_MOTION);
@@ -78,11 +73,11 @@ void ProcessorPose::createFactorIfNecessary(){
}
// whatever happened, remove very old captures
- buffer_capture_.removeUpTo(buffer_pack_kf_.getContainer().begin()->first - 5);
+ buffer_capture_.removeUpTo(buffer_frame_.getContainer().begin()->first - 5);
// now we erase the kf buffer if there was a match
- if (kf_it_last != buffer_pack_kf_.getContainer().end()){
- buffer_pack_kf_.getContainer().erase(buffer_pack_kf_.getContainer().begin(), std::next(kf_it_last));
+ if (kf_it_last != buffer_frame_.getContainer().end()){
+ buffer_frame_.getContainer().erase(buffer_frame_.getContainer().begin(), std::next(kf_it_last));
}
}
@@ -96,11 +91,11 @@ inline void ProcessorPose::processCapture(CaptureBasePtr _capture)
return;
}
// nothing to do if any of the two buffer is empty
- if(buffer_pack_kf_.empty()){
- WOLF_DEBUG("PInertialKinematic: KF pack buffer empty, time ", _capture->getTimeStamp());
+ if(buffer_frame_.empty()){
+ WOLF_DEBUG("PInertialKinematic: Frame buffer empty, time ", _capture->getTimeStamp());
return;
}
- if(buffer_pack_kf_.empty()){
+ if(buffer_frame_.empty()){
WOLF_DEBUG("PInertialKinematics: Capture buffer empty, time ", _capture->getTimeStamp());
return;
}
@@ -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)
{
@@ -117,11 +112,11 @@ inline void ProcessorPose::processKeyFrame(FrameBasePtr _keyframe_ptr, const dou
return;
}
// nothing to do if any of the two buffer is empty
- if(buffer_pack_kf_.empty()){
- WOLF_DEBUG("ProcessorPose: KF pack buffer empty, time ", _keyframe_ptr->getTimeStamp());
+ if(buffer_frame_.empty()){
+ WOLF_DEBUG("ProcessorPose: Frame buffer empty, time ", _keyframe_ptr->getTimeStamp());
return;
}
- if(buffer_pack_kf_.empty()){
+ if(buffer_frame_.empty()){
WOLF_DEBUG("ProcessorPose: Capture buffer empty, time ", _keyframe_ptr->getTimeStamp());
return;
}
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index 961937caf44a126aebcf91e5586e3f07c0e3e979..32c74f2db2116b02c86b746152a949c8374f954b 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -41,7 +41,7 @@ ProcessorTracker::ProcessorTracker(const std::string& _type,
ParamsProcessorTrackerPtr _params_tracker) :
ProcessorBase(_type, _dim, _params_tracker),
params_tracker_(_params_tracker),
- processing_step_(FIRST_TIME_WITHOUT_PACK),
+ processing_step_(FIRST_TIME_WITHOUT_KEYFRAME),
origin_ptr_(nullptr),
last_ptr_(nullptr),
incoming_ptr_(nullptr),
@@ -75,15 +75,15 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
switch(processing_step_)
{
- case FIRST_TIME_WITH_PACK :
+ case FIRST_TIME_WITH_KEYFRAME :
{
- PackKeyFramePtr pack = buffer_pack_kf_.selectPack( incoming_ptr_, params_tracker_->time_tolerance);
- buffer_pack_kf_.removeUpTo( pack->key_frame->getTimeStamp() );
+ FrameBasePtr keyframe_from_callback = buffer_frame_.select( incoming_ptr_->getTimeStamp(), params_tracker_->time_tolerance);
+ buffer_frame_.removeUpTo( keyframe_from_callback->getTimeStamp() );
- WOLF_DEBUG( "PT ", getName(), " FIRST_TIME_WITH_PACK: KF" , pack->key_frame->id() , " callback unpacked with ts= " , pack->key_frame->getTimeStamp() );
+ WOLF_DEBUG( "PT ", getName(), " FIRST_TIME_WITH_KEYFRAME: KF" , keyframe_from_callback->id() , " callback unpacked with ts= " , keyframe_from_callback->getTimeStamp() );
// Append incoming to KF
- incoming_ptr_->link(pack->key_frame);
+ incoming_ptr_->link(keyframe_from_callback);
// Process info
// TrackerFeature: We only process new features in Last, here last = nullptr, so we do not have anything to do.
@@ -92,28 +92,31 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
// Update pointers
resetDerived();
- origin_ptr_ = incoming_ptr_;
- last_ptr_ = incoming_ptr_;
- incoming_ptr_ = nullptr;
+ origin_ptr_ = incoming_ptr_;
+ last_ptr_ = incoming_ptr_;
+ incoming_ptr_ = nullptr;
break;
}
- case FIRST_TIME_WITHOUT_PACK :
+ case FIRST_TIME_WITHOUT_KEYFRAME :
{
- WOLF_DEBUG( "PT ", getName(), " FIRST_TIME_WITHOUT_PACK" );
+ WOLF_DEBUG( "PT ", getName(), " FIRST_TIME_WITHOUT_KEYFRAME" );
+
+ // make a new KF at incoming
+ FrameBasePtr keyframe = FrameBase::emplace<FrameBase>(getProblem()->getTrajectory(),
+ incoming_ptr_->getTimeStamp(),
+ getProblem()->getFrameStructure(),
+ getProblem()->getState());
- FrameBasePtr kfrm = FrameBase::emplace<FrameBase>(getProblem()->getTrajectory(),
- incoming_ptr_->getTimeStamp(),
- getProblem()->getFrameStructure(),
- getProblem()->getState());
- incoming_ptr_->link(kfrm);
+ // 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());
resetDerived();
@@ -124,21 +127,23 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
break;
}
- case SECOND_TIME_WITH_PACK :
+ case SECOND_TIME_WITH_KEYFRAME :
{
// No-break case only for debug. Next case will be executed too.
- PackKeyFramePtr pack = buffer_pack_kf_.selectPack( incoming_ptr_, params_tracker_->time_tolerance);
- WOLF_DEBUG( "PT ", getName(), " SECOND_TIME_WITH_PACK: KF" , pack->key_frame->id() , " callback unpacked with ts= " , pack->key_frame->getTimeStamp() );
+ FrameBasePtr keyframe_from_callback = buffer_frame_.select( incoming_ptr_->getTimeStamp(),
+ params_tracker_->time_tolerance);
+
+ WOLF_DEBUG( "PT ", getName(), " SECOND_TIME_WITH_KEYFRAME: KF" , keyframe_from_callback->id() , " callback unpacked with ts= " , keyframe_from_callback->getTimeStamp() );
}
// Fall through
- case SECOND_TIME_WITHOUT_PACK :
+ case SECOND_TIME_WITHOUT_KEYFRAME :
{
- WOLF_DEBUG( "PT ", getName(), " SECOND_TIME_WITHOUT_PACK" );
+ WOLF_DEBUG( "PT ", getName(), " SECOND_TIME_WITHOUT_KEYFRAME" );
- FrameBasePtr frm = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
- getProblem()->getFrameStructure(),
- getProblem()->getState());
- incoming_ptr_->link(frm);
+ FrameBasePtr keyframe = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
+ getProblem()->getFrameStructure(),
+ getProblem()->getState());
+ incoming_ptr_->link(keyframe);
// We have a last_ Capture with no features, so we do not process known features, and we do not vote for KF.
// Process info
@@ -152,30 +157,31 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
resetDerived();
origin_ptr_ = last_ptr_;
last_ptr_ = incoming_ptr_;
- last_frame_ptr_ = frm;
+ last_frame_ptr_ = keyframe;
incoming_ptr_ = nullptr;
break;
}
- case RUNNING_WITH_PACK :
+ case RUNNING_WITH_KEYFRAME :
{
- PackKeyFramePtr pack = buffer_pack_kf_.selectPack( last_ptr_ , params_tracker_->time_tolerance);
- buffer_pack_kf_.removeUpTo( pack->key_frame->getTimeStamp() );
+ FrameBasePtr keyframe_from_callback = buffer_frame_.select( last_ptr_->getTimeStamp() ,
+ params_tracker_->time_tolerance);
+ buffer_frame_.removeUpTo( keyframe_from_callback->getTimeStamp() );
- WOLF_DEBUG( "PT ", getName(), " RUNNING_WITH_PACK: KF" , pack->key_frame->id() , " callback unpacked with ts= " , pack->key_frame->getTimeStamp() );
+ WOLF_DEBUG( "PT ", getName(), " RUNNING_WITH_KEYFRAME: KF" , keyframe_from_callback->id() , " callback unpacked with ts= " , keyframe_from_callback->getTimeStamp() );
processKnown();
// Capture last_ is added to the new keyframe
FrameBasePtr last_old_frame = last_ptr_->getFrame();
- last_ptr_->move(pack->key_frame);
+ last_ptr_->move(keyframe_from_callback);
last_old_frame->remove();
- // Create new frame
- FrameBasePtr frm = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
+ // Create new frame for incoming
+ FrameBasePtr frame = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
getProblem()->getFrameStructure(),
getProblem()->getState());
- incoming_ptr_->link(frm);
+ incoming_ptr_->link(frame);
// Detect new Features, initialize Landmarks, create Factors, ...
processNew(params_tracker_->max_new_features);
@@ -185,16 +191,16 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
// Update pointers
resetDerived();
- origin_ptr_ = last_ptr_;
- last_ptr_ = incoming_ptr_;
- last_frame_ptr_ = frm;
- incoming_ptr_ = nullptr;
+ origin_ptr_ = last_ptr_;
+ last_ptr_ = incoming_ptr_;
+ last_frame_ptr_ = frame;
+ incoming_ptr_ = nullptr;
break;
}
- case RUNNING_WITHOUT_PACK :
+ case RUNNING_WITHOUT_KEYFRAME :
{
- WOLF_DEBUG( "PT ", getName(), " RUNNING_WITHOUT_PACK");
+ WOLF_DEBUG( "PT ", getName(), " RUNNING_WITHOUT_KEYFRAME");
processKnown();
@@ -218,20 +224,20 @@ 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();
// make F; append incoming to new F
- FrameBasePtr frm = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
- getProblem()->getFrameStructure(),
- last_ptr_->getFrame()->getState());
- incoming_ptr_->link(frm);
- origin_ptr_ = last_ptr_;
- last_ptr_ = incoming_ptr_;
- last_frame_ptr_ = frm;
- incoming_ptr_ = nullptr;
+ FrameBasePtr frame = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
+ getProblem()->getFrameStructure(),
+ last_ptr_->getFrame()->getState());
+ incoming_ptr_ ->link(frame);
+ origin_ptr_ = last_ptr_;
+ last_ptr_ = incoming_ptr_;
+ last_frame_ptr_ = frame;
+ incoming_ptr_ = nullptr;
}
else
@@ -242,15 +248,15 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
advanceDerived();
// Replace last frame for a new one in incoming
- FrameBasePtr frm = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
- getProblem()->getFrameStructure(),
- last_ptr_->getFrame()->getState());
- incoming_ptr_->link(frm);
+ FrameBasePtr frame = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
+ getProblem()->getFrameStructure(),
+ last_ptr_->getFrame()->getState());
+ incoming_ptr_->link(frame);
last_ptr_->getFrame()->remove(); // implicitly calling last_ptr_->remove();
// Update pointers
last_ptr_ = incoming_ptr_;
- last_frame_ptr_ = frm;
+ last_frame_ptr_ = frame;
incoming_ptr_ = nullptr;
}
break;
@@ -278,39 +284,39 @@ void ProcessorTracker::computeProcessingStep()
{
case FIRST_TIME :
- if (buffer_pack_kf_.selectPack(incoming_ptr_, params_tracker_->time_tolerance))
- processing_step_ = FIRST_TIME_WITH_PACK;
+ if (buffer_frame_.select(incoming_ptr_->getTimeStamp(), params_tracker_->time_tolerance))
+ processing_step_ = FIRST_TIME_WITH_KEYFRAME;
else // ! last && ! pack(incoming)
- processing_step_ = FIRST_TIME_WITHOUT_PACK;
+ processing_step_ = FIRST_TIME_WITHOUT_KEYFRAME;
break;
case SECOND_TIME :
- if (buffer_pack_kf_.selectPack(last_ptr_, params_tracker_->time_tolerance))
- processing_step_ = SECOND_TIME_WITH_PACK;
+ if (buffer_frame_.select(last_ptr_->getTimeStamp(), params_tracker_->time_tolerance))
+ processing_step_ = SECOND_TIME_WITH_KEYFRAME;
else
- processing_step_ = SECOND_TIME_WITHOUT_PACK;
+ processing_step_ = SECOND_TIME_WITHOUT_KEYFRAME;
break;
case RUNNING :
default :
- if (buffer_pack_kf_.selectPack(last_ptr_, params_tracker_->time_tolerance))
+ if (buffer_frame_.select(last_ptr_->getTimeStamp(), params_tracker_->time_tolerance))
{
if (last_ptr_->getFrame()->getProblem())
{
WOLF_WARN("||*||");
WOLF_INFO(" ... It seems you missed something!");
- WOLF_INFO("Pack's KF and last's Frame have matching time stamps (i.e. below time tolerances)");
+ WOLF_INFO("Received KF and last's Frame have matching time stamps (i.e. below time tolerances)");
WOLF_INFO("Check the following for correctness:");
WOLF_INFO(" - You have all processors installed before starting receiving any data");
WOLF_INFO(" - You have configured all your processors with compatible time tolerances");
- WOLF_ERROR("Pack's KF and last's KF have matching time stamps (i.e. below time tolerances).");
+ WOLF_ERROR("Received KF and last's KF have matching time stamps (i.e. below time tolerances).");
}
- processing_step_ = RUNNING_WITH_PACK;
+ processing_step_ = RUNNING_WITH_KEYFRAME;
}
else
- processing_step_ = RUNNING_WITHOUT_PACK;
+ processing_step_ = RUNNING_WITHOUT_KEYFRAME;
break;
}
}
@@ -327,5 +333,6 @@ void ProcessorTracker::printHeader(int _depth, bool _constr_by, bool _metric, bo
if (getIncoming())
_stream << _tabs << " " << "i: Cap" << getIncoming()->id() << std::endl;
}
+
} // namespace wolf
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 0615b91df543890745e5aeb9f5f687aaead81ae8..a3da6a740948958190273e4526f997a6872a0cb1 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -44,6 +44,10 @@ target_link_libraries(dummy ${PLUGIN_NAME})
# ------- First Core classes ----------
+# BufferFrame
+wolf_add_gtest(gtest_buffer_frame gtest_buffer_frame.cpp)
+target_link_libraries(gtest_buffer_frame ${PLUGIN_NAME} dummy)
+
# CaptureBase class test
wolf_add_gtest(gtest_capture_base gtest_capture_base.cpp)
target_link_libraries(gtest_capture_base ${PLUGIN_NAME})
@@ -104,10 +108,6 @@ target_link_libraries(gtest_logging ${PLUGIN_NAME})
wolf_add_gtest(gtest_motion_buffer gtest_motion_buffer.cpp)
target_link_libraries(gtest_motion_buffer ${PLUGIN_NAME})
-# PackKFBuffer
-wolf_add_gtest(gtest_pack_KF_buffer gtest_pack_KF_buffer.cpp)
-target_link_libraries(gtest_pack_KF_buffer ${PLUGIN_NAME} dummy)
-
# Parameters server
wolf_add_gtest(gtest_param_server gtest_param_server.cpp ${CMAKE_CURRENT_LIST_DIR})
target_link_libraries(gtest_param_server ${PLUGIN_NAME})
diff --git a/test/dummy/processor_loop_closure_dummy.h b/test/dummy/processor_loop_closure_dummy.h
index e363ee52b96babf92decde84032b765ebbe70001..79a598790dfc58dd9aee764bd0611507aca93479 100644
--- a/test/dummy/processor_loop_closure_dummy.h
+++ b/test/dummy/processor_loop_closure_dummy.h
@@ -101,7 +101,7 @@ class ProcessorLoopClosureDummy : public ProcessorLoopClosure
public:
unsigned int getNStoredFrames()
{
- return buffer_pack_kf_.getContainer().size();
+ return buffer_frame_.getContainer().size();
}
unsigned int getNStoredCaptures()
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
new file mode 100644
index 0000000000000000000000000000000000000000..5c5fadc4ce4e3423005296445d753d3482660dc1
--- /dev/null
+++ b/test/gtest_buffer_frame.cpp
@@ -0,0 +1,266 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+/*
+ * gtest_buffer_frame.cpp
+ *
+ * Created on: Mar 5, 2018
+ * Author: jsola
+ */
+//Wolf
+#include "core/utils/utils_gtest.h"
+
+#include "core/processor/processor_odom_2d.h"
+#include "core/sensor/sensor_odom_2d.h"
+
+#include "dummy/processor_tracker_feature_dummy.h"
+#include "core/capture/capture_void.h"
+
+#include "core/problem/problem.h"
+
+// STL
+#include <iterator>
+#include <iostream>
+
+using namespace wolf;
+using namespace Eigen;
+
+class BufferFrameTest : public testing::Test
+{
+ public:
+
+ BufferFrame buffer_kf;
+ FrameBasePtr f10, f20, f21, f28;
+ double tt10, tt20, tt21, tt28;
+ TimeStamp timestamp;
+ double timetolerance;
+
+ void SetUp(void) override
+ {
+ f10 = std::make_shared<FrameBase>(TimeStamp(10),nullptr,nullptr,nullptr);
+ f20 = std::make_shared<FrameBase>(TimeStamp(20),nullptr,nullptr,nullptr);
+ f21 = std::make_shared<FrameBase>(TimeStamp(21),nullptr,nullptr,nullptr);
+ f28 = std::make_shared<FrameBase>(TimeStamp(28),nullptr,nullptr,nullptr);
+
+ tt10 = 1.0;
+ tt20 = 1.0;
+ tt21 = 1.0;
+ tt28 = 1.0;
+ };
+};
+
+TEST_F(BufferFrameTest, empty)
+{
+ ASSERT_TRUE(buffer_kf.empty());
+}
+
+TEST_F(BufferFrameTest, emplace)
+{
+ buffer_kf.emplace(10, f10);
+ ASSERT_EQ(buffer_kf.size(), (unsigned int) 1);
+ buffer_kf.emplace(20, f20);
+ ASSERT_EQ(buffer_kf.size(), (unsigned int) 2);
+}
+
+TEST_F(BufferFrameTest, clear)
+{
+ buffer_kf.emplace(10, f10);
+ buffer_kf.emplace(20, f20);
+ ASSERT_EQ(buffer_kf.size(), (unsigned int) 2);
+ buffer_kf.clear();
+ ASSERT_TRUE(buffer_kf.empty());
+}
+
+//TEST_F(BufferFrameTest, print)
+//{
+// kfpackbuffer.clear();
+// kfpackbuffer.emplace(f10, tt10);
+// kfpackbuffer.emplace(f20, tt20);
+// kfpackbuffer.print();
+//}
+
+//TEST_F(BufferFrameTest, doubleCheckTimeTolerance)
+//{
+// buffer_kf.clear();
+// buffer_kf.emplace(10, f10);
+// buffer_kf.emplace(20, f20);
+// // min time tolerance > diff between time stamps. It should return true
+// ASSERT_TRUE(buffer_kf.doubleCheckTimeTolerance(10, 20, 20, 20));
+// // min time tolerance < diff between time stamps. It should return true
+// ASSERT_FALSE(buffer_kf.doubleCheckTimeTolerance(10, 1, 20, 20));
+//}
+
+//TEST_F(BufferFrameTest, select)
+//{
+// // Evaluation using two packs (p1,p2)
+// // with different time tolerances (tp1,tp2)
+// // using a query pack (q) with also different time tolerances
+// // depending on these tolerances we will get one (p1) or the other (p2)
+// // packages from the buffer (res).
+// // This can be summarized in the table hereafter:
+// //
+// // p1 p2 q | resulting pack time stamp
+// // --------------------------------
+// // 2 2 2 | nullptr
+// // 2 2 5 | nullptr
+// // 2 2 7 | nullptr
+// // 2 7 2 | nullptr
+// // 2 7 5 | 20
+// // 2 7 7 | 20
+// // 7 2 2 | nullptr
+// // 7 2 5 | nullptr
+// // 7 2 7 | 10
+// // 7 7 2 | nullptr
+// // 7 7 5 | 20
+// // 7 7 7 | 20
+//
+// buffer_kf.clear();
+//
+// // input packages
+// std::vector<int> p1 = {2, 7}; // Pack 1 time tolerances
+// std::vector<int> p2 = {2, 7}; // Pack 2 time tolerances
+// std::vector<int> q = {2, 5, 7}; // Query pack time tolerances
+//
+// // Solution matrix
+// Eigen::VectorXi res = Eigen::VectorXi::Zero(12);
+// res(4) = 20;
+// res(5) = 20;
+// res(8) = 10;
+// res(10) = 20;
+// res(11) = 20;
+//
+// // test
+// for (unsigned int ip1=0;ip1<p1.size();++ip1)
+// {
+// for (unsigned int ip2=0;ip2<p2.size();++ip2)
+// {
+// buffer_kf.emplace(f10, p1[ip1]);
+// buffer_kf.emplace(f20, p2[ip2]);
+// for (unsigned int iq=0;iq<q.size();++iq)
+// {
+// PackKeyFramePtr packQ = buffer_kf.selectPack(16, q[iq]);
+// if (packQ!=nullptr)
+// {
+// ASSERT_EQ(packQ->key_frame->getTimeStamp(),res(ip1*6+ip2*3+iq));
+// }
+// }
+// buffer_kf.clear();
+// }
+// }
+//}
+
+//TEST_F(BufferFrameTest, selectFirstBefore)
+//{
+// buffer_kf.clear();
+//
+// buffer_kf.emplace(10, f10);
+// buffer_kf.emplace(20, f20);
+// buffer_kf.emplace(21, f21);
+//
+// // input time stamps
+// std::vector<TimeStamp> q_ts = {9.5, 9.995, 10.005, 19.5, 20.5, 21.5};
+// double tt = 0.01;
+//
+// // Solution matrix
+// // q_ts | pack
+// //=================
+// // first time
+// //-----------------
+// // 9.5 nullptr
+// // 9.995 10
+// // 10,005 10
+// // 19.5 10
+// // 20.5 10
+// // 21.5 10
+// // second time
+// //-----------------
+// // 9.5 nullptr
+// // 9.995 null
+// // 10,005 null
+// // 19.5 null
+// // 20.5 20
+// // 21.5 20
+// // third time
+// //-----------------
+// // 9.5 nullptr
+// // 9.995 null
+// // 10,005 null
+// // 19.5 null
+// // 20.5 null
+// // 21.5 21
+//
+// Eigen::MatrixXd truth(3,6), res(3,6);
+// truth << 0,10,10,10,10,10, 0,0,0,0,20,20, 0,0,0,0,0,21;
+// res.setZero();
+//
+// for (int i=0; i<3; i++)
+// {
+// PackKeyFramePtr packQ;
+// int j = 0;
+// for (auto ts : q_ts)
+// {
+// packQ = buffer_kf.selectFirstPackBefore(ts, tt);
+// if (packQ)
+// res(i,j) = packQ->key_frame->getTimeStamp().get();
+//
+// j++;
+// }
+// buffer_kf.removeUpTo(packQ->key_frame->getTimeStamp());
+// }
+//
+// ASSERT_MATRIX_APPROX(res, truth, 1e-6);
+//}
+
+TEST_F(BufferFrameTest, removeUpTo)
+{
+ // Small time tolerance for all test asserts
+ double tt = 0.1;
+ buffer_kf.clear();
+ buffer_kf.emplace(10, f10);
+ buffer_kf.emplace(20, f20);
+ buffer_kf.emplace(21, f21);
+
+ // it should remove f20 and f10, thus size should be 1 after removal
+ // Specifically, only f21 should remain
+ buffer_kf.removeUpTo( f20->getTimeStamp() );
+ ASSERT_EQ(buffer_kf.size(), (unsigned int) 1);
+ ASSERT_TRUE(buffer_kf.select(f10->getTimeStamp(),tt)==nullptr);
+ ASSERT_TRUE(buffer_kf.select(f20->getTimeStamp(),tt)==nullptr);
+ ASSERT_TRUE(buffer_kf.select(f21->getTimeStamp(),tt)!=nullptr);
+
+ // Chech removal of an imprecise time stamp
+ // Specifically, only f28 should remain
+ buffer_kf.emplace(28, f28);
+ ASSERT_EQ(buffer_kf.size(), (unsigned int) 2);
+ FrameBasePtr f22 = std::make_shared<FrameBase>(TimeStamp(22),nullptr,nullptr,nullptr);
+ // PackKeyFramePtr pack22 = std::make_shared<PackKeyFrame>(f22,5);
+ buffer_kf.removeUpTo( f22->getTimeStamp() );
+ ASSERT_EQ(buffer_kf.size(), (unsigned int) 1);
+ ASSERT_TRUE(buffer_kf.select(f21->getTimeStamp(),tt)==nullptr);
+ ASSERT_TRUE(buffer_kf.select(f28->getTimeStamp(),tt)!=nullptr);
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
+
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_pack_KF_buffer.cpp b/test/gtest_pack_KF_buffer.cpp
deleted file mode 100644
index 5512a2040c000c2ff7d71462fd158510c3572001..0000000000000000000000000000000000000000
--- a/test/gtest_pack_KF_buffer.cpp
+++ /dev/null
@@ -1,267 +0,0 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of WOLF
-// WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-/*
- * gtest_pack_KF_buffer.cpp
- *
- * Created on: Mar 5, 2018
- * Author: jsola
- */
-//Wolf
-#include "core/utils/utils_gtest.h"
-
-#include "core/processor/processor_odom_2d.h"
-#include "core/sensor/sensor_odom_2d.h"
-
-#include "dummy/processor_tracker_feature_dummy.h"
-#include "core/capture/capture_void.h"
-
-#include "core/problem/problem.h"
-
-// STL
-#include <iterator>
-#include <iostream>
-
-using namespace wolf;
-using namespace Eigen;
-
-class BufferPackKeyFrameTest : public testing::Test
-{
- public:
-
- BufferPackKeyFrame pack_kf_buffer;
- FrameBasePtr f10, f20, f21, f28;
- double tt10, tt20, tt21, tt28;
- TimeStamp timestamp;
- double timetolerance;
-
- void SetUp(void) override
- {
- f10 = std::make_shared<FrameBase>(TimeStamp(10),nullptr,nullptr,nullptr);
- f20 = std::make_shared<FrameBase>(TimeStamp(20),nullptr,nullptr,nullptr);
- f21 = std::make_shared<FrameBase>(TimeStamp(21),nullptr,nullptr,nullptr);
- f28 = std::make_shared<FrameBase>(TimeStamp(28),nullptr,nullptr,nullptr);
-
- tt10 = 1.0;
- tt20 = 1.0;
- tt21 = 1.0;
- tt28 = 1.0;
- };
-};
-
-TEST_F(BufferPackKeyFrameTest, empty)
-{
- ASSERT_TRUE(pack_kf_buffer.empty());
-}
-
-TEST_F(BufferPackKeyFrameTest, add)
-{
- pack_kf_buffer.add(f10, tt10);
- ASSERT_EQ(pack_kf_buffer.size(), (unsigned int) 1);
- pack_kf_buffer.add(f20, tt20);
- ASSERT_EQ(pack_kf_buffer.size(), (unsigned int) 2);
-}
-
-TEST_F(BufferPackKeyFrameTest, clear)
-{
- pack_kf_buffer.add(f10, tt10);
- pack_kf_buffer.add(f20, tt20);
- ASSERT_EQ(pack_kf_buffer.size(), (unsigned int) 2);
- pack_kf_buffer.clear();
- ASSERT_TRUE(pack_kf_buffer.empty());
-}
-
-//TEST_F(BufferPackKeyFrameTest, print)
-//{
-// kfpackbuffer.clear();
-// kfpackbuffer.add(f10, tt10);
-// kfpackbuffer.add(f20, tt20);
-// kfpackbuffer.print();
-//}
-
-TEST_F(BufferPackKeyFrameTest, checkTimeTolerance)
-{
- pack_kf_buffer.clear();
- pack_kf_buffer.add(f10, tt10);
- pack_kf_buffer.add(f20, tt20);
- // min time tolerance > diff between time stamps. It should return true
- ASSERT_TRUE(pack_kf_buffer.checkTimeTolerance(10, 20, 20, 20));
- // min time tolerance < diff between time stamps. It should return true
- ASSERT_FALSE(pack_kf_buffer.checkTimeTolerance(10, 1, 20, 20));
-}
-
-TEST_F(BufferPackKeyFrameTest, selectPack)
-{
- // Evaluation using two packs (p1,p2)
- // with different time tolerances (tp1,tp2)
- // using a query pack (q) with also different time tolerances
- // depending on these tolerances we will get one (p1) or the other (p2)
- // packages from the buffer (res).
- // This can be summarized in the table hereafter:
- //
- // p1 p2 q | resulting pack time stamp
- // --------------------------------
- // 2 2 2 | nullptr
- // 2 2 5 | nullptr
- // 2 2 7 | nullptr
- // 2 7 2 | nullptr
- // 2 7 5 | 20
- // 2 7 7 | 20
- // 7 2 2 | nullptr
- // 7 2 5 | nullptr
- // 7 2 7 | 10
- // 7 7 2 | nullptr
- // 7 7 5 | 20
- // 7 7 7 | 20
-
- pack_kf_buffer.clear();
-
- // input packages
- std::vector<int> p1 = {2, 7}; // Pack 1 time tolerances
- std::vector<int> p2 = {2, 7}; // Pack 2 time tolerances
- std::vector<int> q = {2, 5, 7}; // Query pack time tolerances
-
- // Solution matrix
- Eigen::VectorXi res = Eigen::VectorXi::Zero(12);
- res(4) = 20;
- res(5) = 20;
- res(8) = 10;
- res(10) = 20;
- res(11) = 20;
-
- // test
- for (unsigned int ip1=0;ip1<p1.size();++ip1)
- {
- for (unsigned int ip2=0;ip2<p2.size();++ip2)
- {
- pack_kf_buffer.add(f10, p1[ip1]);
- pack_kf_buffer.add(f20, p2[ip2]);
- for (unsigned int iq=0;iq<q.size();++iq)
- {
- PackKeyFramePtr packQ = pack_kf_buffer.selectPack(16, q[iq]);
- if (packQ!=nullptr)
- {
- ASSERT_EQ(packQ->key_frame->getTimeStamp(),res(ip1*6+ip2*3+iq));
- }
- }
- pack_kf_buffer.clear();
- }
- }
-}
-
-TEST_F(BufferPackKeyFrameTest, selectFirstPackBefore)
-{
- pack_kf_buffer.clear();
-
- pack_kf_buffer.add(f10, tt10);
- pack_kf_buffer.add(f20, tt20);
- pack_kf_buffer.add(f21, tt21);
-
- // input time stamps
- std::vector<TimeStamp> q_ts = {9.5, 9.995, 10.005, 19.5, 20.5, 21.5};
- double tt = 0.01;
-
- // Solution matrix
- // q_ts | pack
- //=================
- // first time
- //-----------------
- // 9.5 nullptr
- // 9.995 10
- // 10,005 10
- // 19.5 10
- // 20.5 10
- // 21.5 10
- // second time
- //-----------------
- // 9.5 nullptr
- // 9.995 null
- // 10,005 null
- // 19.5 null
- // 20.5 20
- // 21.5 20
- // third time
- //-----------------
- // 9.5 nullptr
- // 9.995 null
- // 10,005 null
- // 19.5 null
- // 20.5 null
- // 21.5 21
-
- Eigen::MatrixXd truth(3,6), res(3,6);
- truth << 0,10,10,10,10,10, 0,0,0,0,20,20, 0,0,0,0,0,21;
- res.setZero();
-
- for (int i=0; i<3; i++)
- {
- PackKeyFramePtr packQ;
- int j = 0;
- for (auto ts : q_ts)
- {
- packQ = pack_kf_buffer.selectFirstPackBefore(ts, tt);
- if (packQ)
- res(i,j) = packQ->key_frame->getTimeStamp().get();
-
- j++;
- }
- pack_kf_buffer.removeUpTo(packQ->key_frame->getTimeStamp());
- }
-
- ASSERT_MATRIX_APPROX(res, truth, 1e-6);
-}
-
-TEST_F(BufferPackKeyFrameTest, removeUpTo)
-{
- // Small time tolerance for all test asserts
- double tt = 0.1;
- pack_kf_buffer.clear();
- pack_kf_buffer.add(f10, tt10);
- pack_kf_buffer.add(f20, tt20);
- pack_kf_buffer.add(f21, tt21);
-
- // it should remove f20 and f10, thus size should be 1 after removal
- // Specifically, only f21 should remain
- PackKeyFramePtr pack20 = std::make_shared<PackKeyFrame>(f20,tt20);
- pack_kf_buffer.removeUpTo( pack20->key_frame->getTimeStamp() );
- ASSERT_EQ(pack_kf_buffer.size(), (unsigned int) 1);
- ASSERT_TRUE(pack_kf_buffer.selectPack(f10->getTimeStamp(),tt)==nullptr);
- ASSERT_TRUE(pack_kf_buffer.selectPack(f20->getTimeStamp(),tt)==nullptr);
- ASSERT_TRUE(pack_kf_buffer.selectPack(f21->getTimeStamp(),tt)!=nullptr);
-
- // Chech removal of an imprecise time stamp
- // Specifically, only f28 should remain
- pack_kf_buffer.add(f28, tt28);
- ASSERT_EQ(pack_kf_buffer.size(), (unsigned int) 2);
- FrameBasePtr f22 = std::make_shared<FrameBase>(TimeStamp(22),nullptr,nullptr,nullptr);
- PackKeyFramePtr pack22 = std::make_shared<PackKeyFrame>(f22,5);
- pack_kf_buffer.removeUpTo( pack22->key_frame->getTimeStamp() );
- ASSERT_EQ(pack_kf_buffer.size(), (unsigned int) 1);
- ASSERT_TRUE(pack_kf_buffer.selectPack(f21->getTimeStamp(),tt)==nullptr);
- ASSERT_TRUE(pack_kf_buffer.selectPack(f28->getTimeStamp(),tt)!=nullptr);
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
-
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);