diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5f5ab232e3f863c8ece60965bd87e581fde868e5..f4be61e30dcbae2284856e0733b8e3a908559882 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -195,6 +195,7 @@ SET(HDRS_PROBLEM
include/${PROJECT_NAME}/problem/problem.h
)
SET(HDRS_PROCESSOR
+ include/${PROJECT_NAME}/processor/buffer.h
include/${PROJECT_NAME}/processor/factory_processor.h
include/${PROJECT_NAME}/processor/motion_buffer.h
include/${PROJECT_NAME}/processor/motion_provider.h
diff --git a/include/core/factor/factor_diff_drive.h b/include/core/factor/factor_diff_drive.h
index b7546fdecd58343de3f826b10baf7d48600d4bbb..bb4135d5010a310d25baedbd39ab5caf84a6668c 100644
--- a/include/core/factor/factor_diff_drive.h
+++ b/include/core/factor/factor_diff_drive.h
@@ -147,16 +147,4 @@ inline bool FactorDiffDrive::operator()(const T* const _p1,
return true;
}
-// inline Eigen::VectorXd FactorDiffDrive::residual()
-// {
-// VectorXd residual(3);
-// operator()(getFrameOther()->getP()->getState().data(),
-// getFrameOther()->getO()->getState().data(),
-// getFrame()->getP()->getState().data(),
-// getFrame()->getO()->getState().data(),
-// getCaptureOther()->getSensorIntrinsic()->getState().data(),
-// residual.data());
-// return residual;
-// }
-
} // namespace wolf
diff --git a/include/core/factor/factor_relative_pose_2d_with_extrinsics.h b/include/core/factor/factor_relative_pose_2d_with_extrinsics.h
index 47ad5afde06b8f4e5eba9ebb23c2bdb6a8944c24..ee6f724f668a92cfaaaf47479110ee55ff5b4f3f 100644
--- a/include/core/factor/factor_relative_pose_2d_with_extrinsics.h
+++ b/include/core/factor/factor_relative_pose_2d_with_extrinsics.h
@@ -193,33 +193,4 @@ inline bool FactorRelativePose2dWithExtrinsics::operator()(const T* const _p_ref
return true;
}
-// inline Eigen::Vector3d FactorRelativePose2dWithExtrinsics::residual() const
-// {
-// Eigen::Vector3d res;
-// Eigen::VectorXd p_sensor, o_sensor, p_ref, o_ref, p_target, o_target;
-// p_sensor = getCapture()->getSensorP()->getState();
-// o_sensor = getCapture()->getSensorO()->getState();
-// // FRAME CASE
-// if (not getFramesFactored().empty())
-// {
-// p_ref = getFrameOther()->getP()->getState();
-// o_ref = getFrameOther()->getO()->getState();
-// p_target = getCapture()->getFrame()->getP()->getState();
-// o_target = getCapture()->getFrame()->getO()->getState();
-// }
-// // LANDMARK CASE
-// else if (not getLandmarksFactored().empty())
-// {
-// p_ref = getCapture()->getFrame()->getP()->getState();
-// o_ref = getCapture()->getFrame()->getO()->getState();
-// p_target = getLandmarkOther()->getP()->getState();
-// o_target = getLandmarkOther()->getO()->getState();
-// }
-
-// operator()(
-// p_ref.data(), o_ref.data(), p_target.data(), o_target.data(), p_sensor.data(), o_sensor.data(), res.data());
-
-// return res;
-// }
-
} // namespace wolf
diff --git a/include/core/factor/factor_relative_pose_3d_with_extrinsics.h b/include/core/factor/factor_relative_pose_3d_with_extrinsics.h
index 123434427823f41505aa4b5753fc62c4743359fc..f5debb1cc274f3bf221291095bb429d4695183ff 100644
--- a/include/core/factor/factor_relative_pose_3d_with_extrinsics.h
+++ b/include/core/factor/factor_relative_pose_3d_with_extrinsics.h
@@ -186,38 +186,4 @@ inline bool FactorRelativePose3dWithExtrinsics::operator()(const T* const _p_ref
return true;
}
-// inline Eigen::Vector6d FactorRelativePose3dWithExtrinsics::residual() const
-// {
-// Eigen::Vector6d res;
-// Eigen::VectorXd p_sensor, o_sensor, p_ref, o_ref, p_target, o_target;
-// p_sensor = getCapture()->getSensorP()->getState();
-// o_sensor = getCapture()->getSensorO()->getState();
-// // FRAME CASE
-// if (not getFramesFactored().empty())
-// {
-// p_ref = getFrameOther()->getP()->getState();
-// o_ref = getFrameOther()->getO()->getState();
-// p_target = getCapture()->getFrame()->getP()->getState();
-// o_target = getCapture()->getFrame()->getO()->getState();
-// }
-// // LANDMARK CASE
-// else if (not getLandmarksFactored().empty())
-// {
-// p_ref = getCapture()->getFrame()->getP()->getState();
-// o_ref = getCapture()->getFrame()->getO()->getState();
-// p_target = getLandmarkOther()->getP()->getState();
-// o_target = getLandmarkOther()->getO()->getState();
-// }
-
-// operator()(
-// p_ref.data(), o_ref.data(), p_target.data(), o_target.data(), p_sensor.data(), o_sensor.data(), res.data());
-
-// return res;
-// }
-
-// inline double FactorRelativePose3dWithExtrinsics::cost() const
-// {
-// return residual().squaredNorm();
-// }
-
} // namespace wolf
diff --git a/include/core/factor/factor_relative_position_3d_with_extrinsics.h b/include/core/factor/factor_relative_position_3d_with_extrinsics.h
index 70bceceb6f703e050ed41c20d7ef4f1b729bd6bb..10cf7cfb9c0d1229d53522bce06fb39249b18ae3 100644
--- a/include/core/factor/factor_relative_position_3d_with_extrinsics.h
+++ b/include/core/factor/factor_relative_position_3d_with_extrinsics.h
@@ -123,38 +123,4 @@ inline bool FactorRelativePosition3dWithExtrinsics::operator()(const T* const _p
return true;
}
-// inline Eigen::Vector3d FactorRelativePosition3dWithExtrinsics::residual() const
-// {
-// Eigen::Vector3d res;
-// Eigen::VectorXd p_sensor, o_sensor, p_ref, o_ref, p_target;
-// p_sensor = getCapture()->getSensorP()->getState();
-// o_sensor = getCapture()->getSensorO()->getState();
-// // FRAME-FRAME CASE
-// if (getFramesFactored().size() == 2)
-// {
-// p_ref = getFrameOther()->getP()->getState();
-// o_ref = getFrameOther()->getO()->getState();
-// p_target = getCapture()->getFrame()->getP()->getState();
-// }
-// // FRAME-LANDMARK CASE
-// else if (getLandmarksFactored().size() == 1)
-// {
-// p_ref = getCapture()->getFrame()->getP()->getState();
-// o_ref = getCapture()->getFrame()->getO()->getState();
-// p_target = getLandmarkOther()->getP()->getState();
-// }
-// else
-// throw std::runtime_error(
-// "FactorRelativePosition3dWithExtrinsics::operator(): unknown case! not FRAME-FRAME or FRAME-LANDMARK");
-
-// operator()(p_ref.data(), o_ref.data(), p_target.data(), p_sensor.data(), o_sensor.data(), res.data());
-
-// return res;
-// }
-
-// inline double FactorRelativePosition3dWithExtrinsics::cost() const
-// {
-// return residual().squaredNorm();
-// }
-
} // namespace wolf
diff --git a/include/core/processor/buffer.h b/include/core/processor/buffer.h
new file mode 100644
index 0000000000000000000000000000000000000000..95d52eee7ce109594d8ae5f5c8b0937c376fc15e
--- /dev/null
+++ b/include/core/processor/buffer.h
@@ -0,0 +1,368 @@
+// WOLF - Copyright (C) 2020,2021,2022,2023
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
+// Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF: http://www.iri.upc.edu/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/>.
+
+#pragma once
+
+// Wolf includes
+#include "core/common/wolf.h"
+#include "core/capture/capture_base.h"
+#include "core/frame/frame_base.h"
+#include "core/common/time_stamp.h"
+
+// std
+#include <memory>
+#include <map>
+
+namespace wolf
+{
+
+/** \brief Buffer for arbitrary type objects
+ *
+ * Object and functions to manage a buffer of objects.
+ */
+template <typename T>
+class Buffer
+{
+ public:
+ typedef typename std::map<TimeStamp, T>::iterator Iterator; // buffer iterator
+ typedef typename std::map<TimeStamp, T>::const_iterator ConstIterator; // buffer iterator
+
+ Buffer(){};
+ ~Buffer(void){};
+
+ /**\brief Select an element from the buffer
+ *
+ * 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) const;
+
+ /**\brief Select an element iterator from the buffer
+ *
+ * Select from the buffer the iterator pointing to the closest element (w.r.t. time stamp),
+ * respecting a defined time tolerances
+ */
+ ConstIterator selectIterator(const TimeStamp& _time_stamp, const double& _time_tolerance) const;
+ Iterator selectIterator(const TimeStamp& _time_stamp, const double& _time_tolerance);
+
+ T selectFirstBefore(const TimeStamp& _time_stamp, const double& _time_tolerance) const;
+
+ T selectLastAfter(const TimeStamp& _time_stamp, const double& _time_tolerance) const;
+
+ T selectFirst() const;
+
+ T selectLast() const;
+
+ /**\brief Buffer size
+ *
+ */
+ SizeStd size(void) const;
+
+ /**\brief Add a element to the buffer
+ *
+ */
+ void emplace(const TimeStamp& _time_stamp, const T& _element);
+
+ /** \brief returns the container with elements of the buffer
+ *
+ * elements are ordered from most recent to oldest
+ */
+ const std::map<TimeStamp, T>& getContainer() const;
+ std::map<TimeStamp, T>& getContainer();
+
+ /**\brief Remove all elements in the buffer with a time stamp older than the specified
+ *
+ */
+ void removeUpTo(const TimeStamp& _time_stamp);
+
+ /**\brief Remove all elements in the buffer with a time stamp older than the specified
+ *
+ */
+ void removeUpToLower(const TimeStamp& _time_stamp);
+
+ /**\brief Clear the buffer
+ *
+ */
+ void clear();
+
+ /**\brief is the buffer empty ?
+ *
+ */
+ bool empty() const;
+
+ /**\brief Check time tolerance
+ *
+ * Check if the time distance between two time stamps is smaller than
+ * the time tolerance.
+ */
+ static bool checkTimeTolerance(const TimeStamp& _time_stamp1,
+ const TimeStamp& _time_stamp2,
+ const double& _time_tolerance);
+
+ /**\brief Check time tolerance
+ *
+ * Check if the time distance between two time stamps is smaller than
+ * the minimum time tolerance of the two frames.
+ */
+ static bool doubleCheckTimeTolerance(const TimeStamp& _time_stamp1,
+ const double& _time_tolerance1,
+ const TimeStamp& _time_stamp2,
+ const double& _time_tolerance2);
+
+ protected:
+ std::map<TimeStamp, T> container_; // Main buffer container
+};
+
+/** \brief Buffer of Frames
+ *
+ * Object and functions to manage a buffer of FrameBasePtr objects.
+ */
+class BufferFrame : public Buffer<FrameBasePtr>
+{
+};
+
+/** \brief Buffer of Captures
+ *
+ * Object and functions to manage a buffer of CaptureBasePtr objects.
+ */
+class BufferCapture : public Buffer<CaptureBasePtr>
+{
+};
+
+/////////////////////////////////////////////////////////////////////////////////////////
+
+template <typename T>
+typename Buffer<T>::ConstIterator Buffer<T>::selectIterator(const TimeStamp& _time_stamp,
+ const double& _time_tolerance) const
+{
+ Buffer<T>::ConstIterator post = container_.upper_bound(_time_stamp);
+
+ bool prev_exists = (post != container_.begin());
+ bool post_exists = (post != container_.end());
+
+ bool post_ok = post_exists && checkTimeTolerance(post->first, _time_stamp, _time_tolerance);
+
+ if (prev_exists)
+ {
+ Buffer<T>::ConstIterator prev = std::prev(post);
+
+ bool prev_ok = checkTimeTolerance(prev->first, _time_stamp, _time_tolerance);
+
+ if (prev_ok && !post_ok)
+ return prev;
+
+ else if (!prev_ok && post_ok)
+ return post;
+
+ else if (prev_ok && post_ok)
+ {
+ if (std::fabs(post->first - _time_stamp) < std::fabs(prev->first - _time_stamp))
+ return post;
+ else
+ return prev;
+ }
+ }
+ else if (post_ok)
+ return post;
+
+ return container_.end();
+}
+
+template <typename T>
+typename Buffer<T>::Iterator Buffer<T>::selectIterator(const TimeStamp& _time_stamp, const double& _time_tolerance)
+{
+ Buffer<T>::Iterator post = container_.upper_bound(_time_stamp);
+
+ bool prev_exists = (post != container_.begin());
+ bool post_exists = (post != container_.end());
+
+ 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 = checkTimeTolerance(prev->first, _time_stamp, _time_tolerance);
+
+ if (prev_ok && !post_ok)
+ return prev;
+
+ else if (!prev_ok && post_ok)
+ return post;
+
+ else if (prev_ok && post_ok)
+ {
+ if (std::fabs(post->first - _time_stamp) < std::fabs(prev->first - _time_stamp))
+ return post;
+ else
+ return prev;
+ }
+ }
+ else if (post_ok)
+ return post;
+
+ return container_.end();
+}
+
+template <typename T>
+T Buffer<T>::select(const TimeStamp& _time_stamp, const double& _time_tolerance) const
+{
+ if (container_.empty()) return nullptr;
+
+ auto it = selectIterator(_time_stamp, _time_tolerance);
+
+ // end is returned from selectIterator if an element of the buffer complying with the time stamp
+ // and time tolerance has not been found
+ if (it != container_.end())
+ {
+ return it->second;
+ }
+
+ return nullptr;
+}
+
+template <typename T>
+T Buffer<T>::selectFirstBefore(const TimeStamp& _time_stamp, const double& _time_tolerance) const
+{
+ // There is no element
+ if (container_.empty()) return nullptr;
+
+ // 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 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 element before the provided ts or within the tolerance was found)
+ return nullptr;
+}
+
+template <typename T>
+T Buffer<T>::selectLastAfter(const TimeStamp& _time_stamp, const double& _time_tolerance) const
+{
+ // There is no element
+ if (container_.empty()) return nullptr;
+
+ // 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 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 element after the provided ts or within the tolerance was found)
+ return nullptr;
+}
+
+template <typename T>
+T Buffer<T>::selectFirst() const
+{
+ // There is no element
+ if (container_.empty()) return nullptr;
+
+ // Returning first map element
+ return container_.begin()->second;
+}
+
+template <typename T>
+T Buffer<T>::selectLast() const
+{
+ // There is no element
+ if (container_.empty()) return nullptr;
+
+ // Returning last map element
+ return container_.rbegin()->second;
+}
+
+template <typename T>
+void Buffer<T>::emplace(const TimeStamp& _time_stamp, const T& _element)
+{
+ container_.emplace(_time_stamp, _element);
+}
+
+template <typename T>
+const std::map<TimeStamp, T>& Buffer<T>::getContainer() const
+{
+ return container_;
+}
+
+template <typename T>
+std::map<TimeStamp, T>& Buffer<T>::getContainer()
+{
+ return container_;
+}
+
+template <typename T>
+inline void Buffer<T>::clear()
+{
+ container_.clear();
+}
+
+template <typename T>
+inline bool Buffer<T>::empty() const
+{
+ return container_.empty();
+}
+
+template <typename T>
+inline SizeStd Buffer<T>::size(void) const
+{
+ return container_.size();
+}
+
+template <typename T>
+inline void Buffer<T>::removeUpTo(const TimeStamp& _time_stamp)
+{
+ Buffer::Iterator post = container_.upper_bound(_time_stamp);
+ container_.erase(container_.begin(), post); // erasing by range
+}
+
+template <typename T>
+inline void Buffer<T>::removeUpToLower(const TimeStamp& _time_stamp)
+{
+ Buffer::Iterator post = container_.lower_bound(_time_stamp);
+ container_.erase(container_.begin(), post); // erasing by range
+}
+
+template <typename T>
+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);
+ bool pass = time_diff <= time_tol;
+ return pass;
+}
+
+template <typename T>
+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;
+ return pass;
+}
+
+} // namespace wolf
\ No newline at end of file
diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h
index eb4111ed335c33a28fa05502bf6f6d859e5b473e..db9447d577c9d69e9076ddc16c5cca6179c5ff79 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.h
@@ -31,9 +31,9 @@ class SensorBase;
#include "core/common/node_base.h"
#include "core/processor/motion_provider.h"
#include "core/processor/factory_processor.h"
-#include "core/sensor/sensor_base.h"
#include "core/frame/frame_base.h"
#include "core/common/time_stamp.h"
+#include "core/processor/buffer.h"
// std
#include <memory>
@@ -115,118 +115,6 @@ namespace wolf
return create(server.getNode(), {}); \
}
-/** \brief Buffer for arbitrary type objects
- *
- * Object and functions to manage a buffer of objects.
- */
-template <typename T>
-class Buffer
-{
- public:
- typedef typename std::map<TimeStamp, T>::iterator Iterator; // buffer iterator
- typedef typename std::map<TimeStamp, T>::const_iterator ConstIterator; // buffer iterator
-
- Buffer(){};
- ~Buffer(void){};
-
- /**\brief Select an element from the buffer
- *
- * 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) const;
-
- /**\brief Select an element iterator from the buffer
- *
- * Select from the buffer the iterator pointing to the closest element (w.r.t. time stamp),
- * respecting a defined time tolerances
- */
- ConstIterator selectIterator(const TimeStamp& _time_stamp, const double& _time_tolerance) const;
- Iterator selectIterator(const TimeStamp& _time_stamp, const double& _time_tolerance);
-
- T selectFirstBefore(const TimeStamp& _time_stamp, const double& _time_tolerance) const;
-
- T selectLastAfter(const TimeStamp& _time_stamp, const double& _time_tolerance) const;
-
- T selectFirst() const;
-
- T selectLast() const;
-
- /**\brief Buffer size
- *
- */
- SizeStd size(void) const;
-
- /**\brief Add a element to the buffer
- *
- */
- void emplace(const TimeStamp& _time_stamp, const T& _element);
-
- /** \brief returns the container with elements of the buffer
- *
- * elements are ordered from most recent to oldest
- */
- const std::map<TimeStamp, T>& getContainer() const;
- std::map<TimeStamp, T>& getContainer();
-
- /**\brief Remove all elements in the buffer with a time stamp older than the specified
- *
- */
- void removeUpTo(const TimeStamp& _time_stamp);
-
- /**\brief Remove all elements in the buffer with a time stamp older than the specified
- *
- */
- void removeUpToLower(const TimeStamp& _time_stamp);
-
- /**\brief Clear the buffer
- *
- */
- void clear();
-
- /**\brief is the buffer empty ?
- *
- */
- bool empty() const;
-
- /**\brief Check time tolerance
- *
- * Check if the time distance between two time stamps is smaller than
- * the time tolerance.
- */
- static bool checkTimeTolerance(const TimeStamp& _time_stamp1,
- const TimeStamp& _time_stamp2,
- const double& _time_tolerance);
-
- /**\brief Check time tolerance
- *
- * Check if the time distance between two time stamps is smaller than
- * the minimum time tolerance of the two frames.
- */
- static bool doubleCheckTimeTolerance(const TimeStamp& _time_stamp1,
- const double& _time_tolerance1,
- const TimeStamp& _time_stamp2,
- const double& _time_tolerance2);
-
- protected:
- std::map<TimeStamp, T> container_; // Main buffer container
-};
-
-/** \brief Buffer of Frames
- *
- * Object and functions to manage a buffer of FrameBasePtr objects.
- */
-class BufferFrame : public Buffer<FrameBasePtr>
-{
-};
-
-/** \brief Buffer of Captures
- *
- * Object and functions to manage a buffer of CaptureBasePtr objects.
- */
-class BufferCapture : public Buffer<CaptureBasePtr>
-{
-};
// class ProcessorBase
class ProcessorBase : public NodeBase, public std::enable_shared_from_this<ProcessorBase>
@@ -499,223 +387,4 @@ std::shared_ptr<classType> ProcessorBase::emplace(SensorBasePtr
return prc;
}
-/////////////////////////////////////////////////////////////////////////////////////////
-
-template <typename T>
-typename Buffer<T>::ConstIterator Buffer<T>::selectIterator(const TimeStamp& _time_stamp,
- const double& _time_tolerance) const
-{
- Buffer<T>::ConstIterator post = container_.upper_bound(_time_stamp);
-
- bool prev_exists = (post != container_.begin());
- bool post_exists = (post != container_.end());
-
- bool post_ok = post_exists && checkTimeTolerance(post->first, _time_stamp, _time_tolerance);
-
- if (prev_exists)
- {
- Buffer<T>::ConstIterator prev = std::prev(post);
-
- bool prev_ok = checkTimeTolerance(prev->first, _time_stamp, _time_tolerance);
-
- if (prev_ok && !post_ok)
- return prev;
-
- else if (!prev_ok && post_ok)
- return post;
-
- else if (prev_ok && post_ok)
- {
- if (std::fabs(post->first - _time_stamp) < std::fabs(prev->first - _time_stamp))
- return post;
- else
- return prev;
- }
- }
- else if (post_ok)
- return post;
-
- return container_.end();
-}
-
-template <typename T>
-typename Buffer<T>::Iterator Buffer<T>::selectIterator(const TimeStamp& _time_stamp, const double& _time_tolerance)
-{
- Buffer<T>::Iterator post = container_.upper_bound(_time_stamp);
-
- bool prev_exists = (post != container_.begin());
- bool post_exists = (post != container_.end());
-
- 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 = checkTimeTolerance(prev->first, _time_stamp, _time_tolerance);
-
- if (prev_ok && !post_ok)
- return prev;
-
- else if (!prev_ok && post_ok)
- return post;
-
- else if (prev_ok && post_ok)
- {
- if (std::fabs(post->first - _time_stamp) < std::fabs(prev->first - _time_stamp))
- return post;
- else
- return prev;
- }
- }
- else if (post_ok)
- return post;
-
- return container_.end();
-}
-
-template <typename T>
-T Buffer<T>::select(const TimeStamp& _time_stamp, const double& _time_tolerance) const
-{
- if (container_.empty()) return nullptr;
-
- auto it = selectIterator(_time_stamp, _time_tolerance);
-
- // end is returned from selectIterator if an element of the buffer complying with the time stamp
- // and time tolerance has not been found
- if (it != container_.end())
- {
- return it->second;
- }
-
- return nullptr;
-}
-
-template <typename T>
-T Buffer<T>::selectFirstBefore(const TimeStamp& _time_stamp, const double& _time_tolerance) const
-{
- // There is no element
- if (container_.empty()) return nullptr;
-
- // 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 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 element before the provided ts or within the tolerance was found)
- return nullptr;
-}
-
-template <typename T>
-T Buffer<T>::selectLastAfter(const TimeStamp& _time_stamp, const double& _time_tolerance) const
-{
- // There is no element
- if (container_.empty()) return nullptr;
-
- // 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 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 element after the provided ts or within the tolerance was found)
- return nullptr;
-}
-
-template <typename T>
-T Buffer<T>::selectFirst() const
-{
- // There is no element
- if (container_.empty()) return nullptr;
-
- // Returning first map element
- return container_.begin()->second;
-}
-
-template <typename T>
-T Buffer<T>::selectLast() const
-{
- // There is no element
- if (container_.empty()) return nullptr;
-
- // Returning last map element
- return container_.rbegin()->second;
-}
-
-template <typename T>
-void Buffer<T>::emplace(const TimeStamp& _time_stamp, const T& _element)
-{
- container_.emplace(_time_stamp, _element);
-}
-
-template <typename T>
-const std::map<TimeStamp, T>& Buffer<T>::getContainer() const
-{
- return container_;
-}
-
-template <typename T>
-std::map<TimeStamp, T>& Buffer<T>::getContainer()
-{
- return container_;
-}
-
-template <typename T>
-inline void Buffer<T>::clear()
-{
- container_.clear();
-}
-
-template <typename T>
-inline bool Buffer<T>::empty() const
-{
- return container_.empty();
-}
-
-template <typename T>
-inline SizeStd Buffer<T>::size(void) const
-{
- return container_.size();
-}
-
-template <typename T>
-inline void Buffer<T>::removeUpTo(const TimeStamp& _time_stamp)
-{
- Buffer::Iterator post = container_.upper_bound(_time_stamp);
- container_.erase(container_.begin(), post); // erasing by range
-}
-
-template <typename T>
-inline void Buffer<T>::removeUpToLower(const TimeStamp& _time_stamp)
-{
- Buffer::Iterator post = container_.lower_bound(_time_stamp);
- container_.erase(container_.begin(), post); // erasing by range
-}
-
-template <typename T>
-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);
- bool pass = time_diff <= time_tol;
- return pass;
-}
-
-template <typename T>
-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;
- return pass;
-}
-
} // namespace wolf
diff --git a/src/common/node_state_blocks.cpp b/src/common/node_state_blocks.cpp
index 82d01d1d283143a5aed7228efd6ba751a132b9f1..3c6f7b599d0dccaf03d101c1f4e6050b55adbcb7 100644
--- a/src/common/node_state_blocks.cpp
+++ b/src/common/node_state_blocks.cpp
@@ -114,19 +114,6 @@ void NodeStateBlocks::emplaceStateBlocks(const SpecStateComposite& _specs)
void NodeStateBlocks::emplaceStateBlocks(const TypeComposite& _types, const VectorComposite& _vectors, bool _fix)
{
- // if (not _types.has(_vectors.getKeys()) or not _vectors.has(_types.getKeys()))
- // {
- // throw std::runtime_error(
- // "NodeStateBlocks::emplaceStateBlocksAndPriors: provided type and vector composites don't have the same "
- // "structure");
- // }
-
- // // join types and vectors into specs (assuming "initial guess", not fixed or factor)
- // SpecStateComposite specs;
- // for (auto type_pair : _types)
- // {
- // specs.emplace(type_pair.first, SpecState(type_pair.second, _vectors.at(type_pair.first), "initial_guess"));
- // }
emplaceStateBlocks(SpecStateComposite(_types, _vectors, _fix));
}