diff --git a/src/capture_base.cpp b/src/capture_base.cpp
index d5d76ba8e37c92d2d17a00cf40b59fea18157b33..880303f494e5e7fc2e00d3471a04c2c8c6a09dde 100644
--- a/src/capture_base.cpp
+++ b/src/capture_base.cpp
@@ -18,7 +18,6 @@ CaptureBase::CaptureBase(const std::string& _type,
sensor_ptr_(_sensor_ptr),
state_block_vec_(3),
calib_size_(0),
- is_removing_(false),
capture_id_(++capture_id_count_),
time_stamp_(_ts)
{
diff --git a/src/capture_base.h b/src/capture_base.h
index d705be3764535be62e13f5067867de4a2e7a29b0..9da734e64f3b60ed580c69b4f970cf650b9cf616 100644
--- a/src/capture_base.h
+++ b/src/capture_base.h
@@ -29,7 +29,6 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
SizeEigen calib_size_; ///< size of the calibration parameters (dynamic or static sensor params that are not fixed)
static unsigned int capture_id_count_;
- bool is_removing_; ///< A flag for safely removing nodes from the Wolf tree. See remove().
protected:
unsigned int capture_id_;
@@ -45,7 +44,7 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
StateBlockPtr _intr_ptr = nullptr);
virtual ~CaptureBase();
- void remove();
+ virtual void remove();
// Type
virtual bool isMotion() const { return false; }
diff --git a/src/constraint_base.cpp b/src/constraint_base.cpp
index d88dedbf05f89fcf7386b447520c31a4752a71a0..3e33cc6cc6d84dc1bded350956e29a548dacef22 100644
--- a/src/constraint_base.cpp
+++ b/src/constraint_base.cpp
@@ -11,7 +11,6 @@ ConstraintBase::ConstraintBase(const std::string& _tp,
ConstraintStatus _status) :
NodeBase("CONSTRAINT", _tp),
feature_ptr_(), // nullptr
- is_removing_(false),
constraint_id_(++constraint_id_count_),
status_(_status),
apply_loss_function_(_apply_loss_function),
@@ -32,7 +31,6 @@ ConstraintBase::ConstraintBase(const std::string& _tp,
bool _apply_loss_function, ConstraintStatus _status) :
NodeBase("CONSTRAINT", _tp),
feature_ptr_(),
- is_removing_(false),
constraint_id_(++constraint_id_count_),
status_(_status),
apply_loss_function_(_apply_loss_function),
diff --git a/src/constraint_base.h b/src/constraint_base.h
index 838d50c23659e1da94c60e23580c8ceb1a0feda2..3341a55c882d64262fb2bc6f6a7251a0f3acf6c9 100644
--- a/src/constraint_base.h
+++ b/src/constraint_base.h
@@ -42,7 +42,6 @@ class ConstraintBase : public NodeBase, public std::enable_shared_from_this<Cons
FeatureBaseWPtr feature_ptr_; ///< FeatureBase pointer (upper node)
static unsigned int constraint_id_count_;
- bool is_removing_; ///< A flag for safely removing nodes from the Wolf tree. See remove().
protected:
unsigned int constraint_id_;
@@ -75,7 +74,7 @@ class ConstraintBase : public NodeBase, public std::enable_shared_from_this<Cons
virtual ~ConstraintBase() = default;
- void remove();
+ virtual void remove();
unsigned int id() const;
diff --git a/src/feature_base.cpp b/src/feature_base.cpp
index 3cc0b6ca3d64b7299641de401fb67068c0af1b08..6459ffc244964171a58b839590fc1a5b392fa21b 100644
--- a/src/feature_base.cpp
+++ b/src/feature_base.cpp
@@ -9,7 +9,6 @@ unsigned int FeatureBase::feature_id_count_ = 0;
FeatureBase::FeatureBase(const std::string& _type, const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_covariance) :
NodeBase("FEATURE", _type),
capture_ptr_(),
- is_removing_(false),
feature_id_(++feature_id_count_),
track_id_(0),
landmark_id_(0),
diff --git a/src/feature_base.h b/src/feature_base.h
index 3bd60033a40929c7e6a18705acbd46a2a56a94e3..1eb4fbf851feb0257b11b210dd597c1ab3710286 100644
--- a/src/feature_base.h
+++ b/src/feature_base.h
@@ -25,7 +25,6 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
ConstraintBaseList constrained_by_list_;
static unsigned int feature_id_count_;
- bool is_removing_; ///< A flag for safely removing nodes from the Wolf tree. See remove().
protected:
unsigned int feature_id_;
@@ -46,7 +45,7 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
FeatureBase(const std::string& _type, const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_covariance);
virtual ~FeatureBase();
- void remove();
+ virtual void remove();
virtual void setProblem(ProblemPtr _problem) final;
diff --git a/src/frame_base.cpp b/src/frame_base.cpp
index c605ae015c384e235a5409d215a3b2903c1bb439..4f0f619fe36767466f7db210e36ac4da03452892 100644
--- a/src/frame_base.cpp
+++ b/src/frame_base.cpp
@@ -15,7 +15,6 @@ FrameBase::FrameBase(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _
NodeBase("FRAME", "Base"),
trajectory_ptr_(),
state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed.
- is_removing_(false),
frame_id_(++frame_id_count_),
type_(NON_KEY_FRAME),
time_stamp_(_ts)
@@ -29,7 +28,6 @@ FrameBase::FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr
NodeBase("FRAME", "Base"),
trajectory_ptr_(),
state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed.
- is_removing_(false),
frame_id_(++frame_id_count_),
type_(_tp),
time_stamp_(_ts)
diff --git a/src/frame_base.h b/src/frame_base.h
index d5202bb69e636d7b3f9a8509a0af2ebcb57b6f28..f7e5f8379772502c5169d03aa11a7cbdae39f2d3 100644
--- a/src/frame_base.h
+++ b/src/frame_base.h
@@ -37,7 +37,6 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
std::vector<StateBlockPtr> state_block_vec_; ///< vector of state blocks, in the order: Position, Orientation, Velocity.
static unsigned int frame_id_count_;
- bool is_removing_; ///< A flag for safely removing nodes from the Wolf tree. See remove().
protected:
unsigned int frame_id_;
@@ -67,7 +66,7 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr = nullptr, StateBlockPtr _v_ptr = nullptr);
virtual ~FrameBase();
- void remove();
+ virtual void remove();
diff --git a/src/landmark_base.cpp b/src/landmark_base.cpp
index 750911457c058acf179d635d24bc289df8237bad..830ae2940f201c291d9d12f6be529a591320f8ca 100644
--- a/src/landmark_base.cpp
+++ b/src/landmark_base.cpp
@@ -13,7 +13,6 @@ LandmarkBase::LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, State
NodeBase("LANDMARK", _type),
map_ptr_(),
state_block_vec_(2), // allow for 2 state blocks by default. Resize in derived constructors if needed.
- is_removing_(false),
landmark_id_(++landmark_id_count_)
{
state_block_vec_[0] = _p_ptr;
diff --git a/src/landmark_base.h b/src/landmark_base.h
index e4b721db95a7fe30db760f3a517a10e11f4f6872..66485387abcbdfdaec03f0f6fa93cc92d7998f1b 100644
--- a/src/landmark_base.h
+++ b/src/landmark_base.h
@@ -29,7 +29,6 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
std::vector<StateBlockPtr> state_block_vec_; ///< vector of state blocks, in the order P, O.
static unsigned int landmark_id_count_;
- bool is_removing_; ///< A flag for safely removing nodes from the Wolf tree. See remove().
protected:
unsigned int landmark_id_; ///< landmark unique id
@@ -48,7 +47,7 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
**/
LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr = nullptr);
virtual ~LandmarkBase();
- void remove();
+ virtual void remove();
virtual YAML::Node saveToYaml() const;
// Properties
diff --git a/src/node_base.h b/src/node_base.h
index 1cf31eb69c4cbddaa87b20a208abd5dcd317110a..bedc866122037c8be8a1d8cee8db2dd009d28f88 100644
--- a/src/node_base.h
+++ b/src/node_base.h
@@ -67,6 +67,8 @@ class NodeBase
std::string node_type_; ///< Text label identifying the type or subcategory of node ("Pin Hole", "Point 2D", etc)
std::string node_name_; ///< Text label identifying each specific object ("left camera", "LIDAR 1", "PointGrey", "Andrew", etc)
+ bool is_removing_; ///< A flag for safely removing nodes from the Wolf tree. See remove().
+
public:
NodeBase(const std::string& _category, const std::string& _type = "Undefined", const std::string& _name = "");
@@ -76,6 +78,7 @@ class NodeBase
std::string getCategory() const;
std::string getType() const;
std::string getName() const;
+ bool isRemoving() const;
void setType(const std::string& _type);
void setName(const std::string& _name);
@@ -95,7 +98,8 @@ inline NodeBase::NodeBase(const std::string& _category, const std::string& _type
node_id_(++node_id_count_),
node_category_(_category),
node_type_(_type),
- node_name_(_name)
+ node_name_(_name),
+ is_removing_(false)
{
//
}
@@ -120,6 +124,11 @@ inline std::string NodeBase::getName() const
return node_name_;
}
+inline bool NodeBase::isRemoving() const
+{
+ return is_removing_;
+}
+
inline void NodeBase::setType(const std::string& _type)
{
node_type_ = _type;
diff --git a/src/processor_base.cpp b/src/processor_base.cpp
index 28a6f3dde73ebba6bba0e9c632660d9d13180803..ea4bdc4e82e1edbcb83c9500656f0b0d05de6ab8 100644
--- a/src/processor_base.cpp
+++ b/src/processor_base.cpp
@@ -11,8 +11,7 @@ ProcessorBase::ProcessorBase(const std::string& _type, ProcessorParamsBasePtr _p
NodeBase("PROCESSOR", _type, _params->name),
processor_id_(++processor_id_count_),
params_(_params),
- sensor_ptr_(),
- is_removing_(false)
+ sensor_ptr_()
{
// WOLF_DEBUG("constructed +p" , id());
}
@@ -98,6 +97,12 @@ PackKeyFramePtr PackKeyFrameBuffer::selectPack(const TimeStamp& _time_stamp, con
PackKeyFrameBuffer::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());
@@ -133,33 +138,33 @@ PackKeyFramePtr PackKeyFrameBuffer::selectPack(const CaptureBasePtr _capture, co
return selectPack(_capture->getTimeStamp(), _time_tolerance);
}
-PackKeyFramePtr PackKeyFrameBuffer::selectPackBefore(const TimeStamp& _time_stamp, const Scalar& _time_tolerance)
+PackKeyFramePtr PackKeyFrameBuffer::selectFirstPackBefore(const TimeStamp& _time_stamp, const Scalar& _time_tolerance)
{
- if (container_.empty())
- return nullptr;
+ // remove packs corresponding to removed KFs
+ while (!container_.empty() && container_.begin()->second->key_frame->isRemoving())
+ container_.erase(container_.begin());
- PackKeyFrameBuffer::Iterator post = container_.upper_bound(_time_stamp);
+ // There is no pack
+ if (container_.empty())
+ return nullptr;
- bool prev_exists = (post != container_.begin());
+ // 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;
- if (prev_exists)
+ // Return first pack if despite being newer, it is within the time tolerance
+ if (checkTimeTolerance(container_.begin()->first, container_.begin()->second->time_tolerance, _time_stamp, _time_tolerance))
return container_.begin()->second;
- else
- {
- bool post_exists = (post != container_.end());
- bool post_ok = post_exists && checkTimeTolerance(post->first, post->second->time_tolerance, _time_stamp, _time_tolerance);
-
- if (post_ok)
- return post->second;
- }
-
+ // otherwise return nullptr (no pack before the provided ts or within the tolerance was found)
return nullptr;
+
}
-PackKeyFramePtr PackKeyFrameBuffer::selectPackBefore(const CaptureBasePtr _capture, const Scalar& _time_tolerance)
+PackKeyFramePtr PackKeyFrameBuffer::selectFirstPackBefore(const CaptureBasePtr _capture, const Scalar& _time_tolerance)
{
- return selectPackBefore(_capture->getTimeStamp(), _time_tolerance);
+ return selectFirstPackBefore(_capture->getTimeStamp(), _time_tolerance);
}
void PackKeyFrameBuffer::print(void)
diff --git a/src/processor_base.h b/src/processor_base.h
index b9dc4b8d6464062e21a91e8d4133462731ac1be6..bcf5c46b62730d593165f99fdb8d021296964812 100644
--- a/src/processor_base.h
+++ b/src/processor_base.h
@@ -58,8 +58,8 @@ class PackKeyFrameBuffer
PackKeyFramePtr selectPack(const TimeStamp& _time_stamp, const Scalar& _time_tolerance);
PackKeyFramePtr selectPack(const CaptureBasePtr _capture, const Scalar& _time_tolerance);
- PackKeyFramePtr selectPackBefore(const TimeStamp& _time_stamp, const Scalar& _time_tolerance);
- PackKeyFramePtr selectPackBefore(const CaptureBasePtr _capture, const Scalar& _time_tolerance);
+ PackKeyFramePtr selectFirstPackBefore(const TimeStamp& _time_stamp, const Scalar& _time_tolerance);
+ PackKeyFramePtr selectFirstPackBefore(const CaptureBasePtr _capture, const Scalar& _time_tolerance);
/**\brief Buffer size
*
@@ -147,15 +147,13 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
private:
SensorBaseWPtr sensor_ptr_;
- bool is_removing_; ///< A flag for safely removing nodes from the Wolf tree. See remove().
-
static unsigned int processor_id_count_;
public:
ProcessorBase(const std::string& _type, ProcessorParamsBasePtr _params);
virtual ~ProcessorBase();
virtual void configure(SensorBasePtr _sensor) = 0;
- void remove();
+ virtual void remove();
unsigned int id();
diff --git a/src/processor_motion.cpp b/src/processor_motion.cpp
index 450cd9d8c2333e06ef76a269939430fe42a18705..c72c02973a4c57abea464811a32f59f707271c7a 100644
--- a/src/processor_motion.cpp
+++ b/src/processor_motion.cpp
@@ -584,7 +584,7 @@ PackKeyFramePtr ProcessorMotion::computeProcessingStep()
throw std::runtime_error("ProcessorMotion received data before being initialized.");
}
- PackKeyFramePtr pack = kf_pack_buffer_.selectPackBefore(last_ptr_, params_motion_->time_tolerance);
+ PackKeyFramePtr pack = kf_pack_buffer_.selectFirstPackBefore(last_ptr_, params_motion_->time_tolerance);
if (pack)
{
diff --git a/src/sensor_base.cpp b/src/sensor_base.cpp
index e16a01fb375c4809b36be966d8c500b81aa536df..429aaf9f142d08a3374d8242df8e79379e5c6d90 100644
--- a/src/sensor_base.cpp
+++ b/src/sensor_base.cpp
@@ -17,7 +17,6 @@ SensorBase::SensorBase(const std::string& _type,
hardware_ptr_(),
state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed.
calib_size_(0),
- is_removing_(false),
sensor_id_(++sensor_id_count_), // simple ID factory
extrinsic_dynamic_(_extr_dyn),
intrinsic_dynamic_(_intr_dyn),
@@ -44,7 +43,6 @@ SensorBase::SensorBase(const std::string& _type,
hardware_ptr_(),
state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed.
calib_size_(0),
- is_removing_(false),
sensor_id_(++sensor_id_count_), // simple ID factory
extrinsic_dynamic_(_extr_dyn),
intrinsic_dynamic_(_intr_dyn),
diff --git a/src/sensor_base.h b/src/sensor_base.h
index 3c740bc88233645d9d3d6767e65c048267761acc..b52e8816e8d91455cda89c1ee2687c6754569f92 100644
--- a/src/sensor_base.h
+++ b/src/sensor_base.h
@@ -39,7 +39,6 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
SizeEigen calib_size_;
static unsigned int sensor_id_count_; ///< Object counter (acts as simple ID factory)
- bool is_removing_; ///< A flag for safely removing nodes from the Wolf tree. See remove().
protected:
unsigned int sensor_id_; // sensor ID
@@ -91,7 +90,7 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
const bool _intr_dyn = false);
virtual ~SensorBase();
- void remove();
+ virtual void remove();
unsigned int id();
diff --git a/src/test/gtest_pack_KF_buffer.cpp b/src/test/gtest_pack_KF_buffer.cpp
index dab222c29248b5be35c61d9de10ffb041647217f..1204254c1024266c98cfe2c8e3764f76453fe2ee 100644
--- a/src/test/gtest_pack_KF_buffer.cpp
+++ b/src/test/gtest_pack_KF_buffer.cpp
@@ -144,7 +144,7 @@ TEST_F(BufferPackKeyFrameTest, selectPack)
}
}
-TEST_F(BufferPackKeyFrameTest, selectPackBefore)
+TEST_F(BufferPackKeyFrameTest, selectFirstPackBefore)
{
pack_kf_buffer.clear();
@@ -194,7 +194,7 @@ TEST_F(BufferPackKeyFrameTest, selectPackBefore)
int j = 0;
for (auto ts : q_ts)
{
- packQ = pack_kf_buffer.selectPackBefore(ts, tt);
+ packQ = pack_kf_buffer.selectFirstPackBefore(ts, tt);
if (packQ)
res(i,j) = packQ->key_frame->getTimeStamp().get();