diff --git a/CMakeLists.txt b/CMakeLists.txt
index 608f836ca8b9ddee82ab349a593831000eb62da6..0279507ae8eeba59f96b8cfb776b9261fc1233a0 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -254,9 +254,8 @@ SET(HDRS_PROCESSOR
include/core/processor/processor_capture_holder.h
include/core/processor/processor_diff_drive.h
include/core/processor/processor_factory.h
- include/core/processor/processor_frame_nearest_neighbor_filter.h
include/core/processor/processor_logging.h
- include/core/processor/processor_loopclosure_base.h
+ include/core/processor/processor_loopclosure.h
include/core/processor/processor_motion.h
include/core/processor/processor_odom_2D.h
include/core/processor/processor_odom_3D.h
@@ -350,8 +349,7 @@ SET(SRCS_PROCESSOR
src/processor/processor_base.cpp
src/processor/processor_capture_holder.cpp
src/processor/processor_diff_drive.cpp
- src/processor/processor_frame_nearest_neighbor_filter.cpp
- src/processor/processor_loopclosure_base.cpp
+ src/processor/processor_loopclosure.cpp
src/processor/processor_motion.cpp
src/processor/processor_odom_2D.cpp
src/processor/processor_odom_3D.cpp
diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h
index 372c25282212f5990c2cb7ebb8c2c4e4fec0ddfc..4ac6c161d079579783d13ae7a232c0f915a1170f 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.h
@@ -36,19 +36,89 @@ class PackKeyFrame
WOLF_PTR_TYPEDEFS(PackKeyFrame);
+
+/** \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
+
+ Buffer(){};
+ ~Buffer(void){};
+
+ /**\brief Select a Pack from the buffer
+ *
+ * Select from the buffer the closest pack (w.r.t. time stamp),
+ * respecting a defined time tolerances
+ */
+ T select(const TimeStamp& _time_stamp, const Scalar& _time_tolerance);
+
+ T selectFirstBefore(const TimeStamp& _time_stamp, const Scalar& _time_tolerance);
+
+ /**\brief Buffer size
+ *
+ */
+ SizeStd size(void);
+
+ /**\brief Add a pack to the buffer
+ *
+ */
+ void add(const TimeStamp& _time_stamp, const T& _element); //const Scalar& _time_tolerance);
+
+ /** \brief returns the container with elements of the buffer
+ *
+ * elements are ordered from most recent to oldest
+ */
+ std::map<TimeStamp,T> getContainer();
+
+ /**\brief Remove all packs in the buffer with a time stamp older than the specified
+ *
+ */
+ void removeUpTo(const TimeStamp& _time_stamp);
+
+ /**\brief Clear the buffer
+ *
+ */
+ void clear();
+
+ /**\brief is the buffer empty ?
+ *
+ */
+ bool empty();
+
+ /**\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 Scalar& _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 Scalar& _time_tolerance1, const TimeStamp& _time_stamp2, const Scalar& _time_tolerance2);
+
+protected:
+
+ std::map<TimeStamp,T> container_; // Main buffer container
+};
+
+
/** \brief Buffer of Key frame class objects
*
* Object and functions to manage a buffer of KFPack objects.
*/
-class BufferPackKeyFrame
+class BufferPackKeyFrame : public Buffer<PackKeyFramePtr>
{
public:
- typedef std::map<TimeStamp,PackKeyFramePtr>::iterator Iterator; // buffer iterator
-
- BufferPackKeyFrame(void);
- ~BufferPackKeyFrame(void);
-
/**\brief Select a Pack from the buffer
*
* Select from the buffer the closest pack (w.r.t. time stamp),
@@ -60,48 +130,32 @@ class BufferPackKeyFrame
PackKeyFramePtr selectFirstPackBefore(const TimeStamp& _time_stamp, const Scalar& _time_tolerance);
PackKeyFramePtr selectFirstPackBefore(const CaptureBasePtr _capture, const Scalar& _time_tolerance);
- /**\brief Buffer size
- *
- */
- SizeStd size(void);
-
/**\brief Add a pack to the buffer
*
*/
void add(const FrameBasePtr& _key_frame, const Scalar& _time_tolerance);
- /**\brief Remove all packs in the buffer with a time stamp older than the specified
- *
- */
- void removeUpTo(const TimeStamp& _time_stamp);
-
- /**\brief Check time tolerance
- *
- * Check if the time distance between two time stamps is smaller than
- * the minimum time tolerance of the two frames.
- */
- bool checkTimeTolerance(const TimeStamp& _time_stamp1, const Scalar& _time_tolerance1, const TimeStamp& _time_stamp2, const Scalar& _time_tolerance2);
-
- /**\brief Clear the buffer
- *
- */
- void clear();
-
- /**\brief Empty the buffer
- *
- */
- bool empty();
-
/**\brief Print buffer information
*
*/
void print();
- private:
+ /**\brief Alias funct
+ *
+ */
+ static bool checkTimeTolerance(const TimeStamp& _time_stamp1, const Scalar& _time_tolerance1, const TimeStamp& _time_stamp2, const Scalar& _time_tolerance2)
+ { return doubleCheckTimeTolerance(_time_stamp1, _time_tolerance1, _time_stamp2, _time_tolerance2); };
- std::map<TimeStamp,PackKeyFramePtr> container_; // Main buffer container
};
+
+/** \brief Buffer of Capture class objects
+ *
+ * Object and functions to manage a buffer of Capture objects.
+ */
+class BufferCapture : public Buffer<CaptureBasePtr> {};
+
+
/** \brief base struct for processor parameters
*
* Derive from this struct to create structs of processor parameters.
@@ -150,6 +204,7 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
unsigned int processor_id_;
ProcessorParamsBasePtr params_;
BufferPackKeyFrame buffer_pack_kf_;
+ BufferCapture buffer_capture_;
private:
SensorBaseWPtr sensor_ptr_;
@@ -207,6 +262,7 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
FrameBasePtr emplaceFrame(FrameType _type, CaptureBasePtr _capture_ptr, const Eigen::VectorXs& _state);
virtual void keyFrameCallback(FrameBasePtr _keyframe_ptr, const Scalar& _time_tol_other);
+ virtual void captureCallback(CaptureBasePtr _capture_ptr);
SensorBasePtr getSensor();
const SensorBasePtr getSensor() const;
@@ -288,31 +344,124 @@ inline void ProcessorBase::setTimeTolerance(Scalar _time_tolerance)
params_->time_tolerance = _time_tolerance;
}
-inline BufferPackKeyFrame::BufferPackKeyFrame(void)
+/////////////////////////////////////////////////////////////////////////////////////////
+
+template <typename T>
+T Buffer<T>::select(const TimeStamp& _time_stamp, const Scalar& _time_tolerance)
{
+ if (container_.empty())
+ return nullptr;
+
+ 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 && simpleCheckTimeTolerance(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);
+
+ 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;
}
-inline BufferPackKeyFrame::~BufferPackKeyFrame(void)
+template <typename T>
+T Buffer<T>::selectFirstBefore(const TimeStamp& _time_stamp, const Scalar& _time_tolerance)
{
+ // There is no element
+ 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 (simpleCheckTimeTolerance(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)
+ return nullptr;
}
-inline void BufferPackKeyFrame::clear()
+template <typename T>
+void Buffer<T>::add(const TimeStamp& _time_stamp, const T& _element)
+{
+ container_.emplace(_time_stamp, _element);
+}
+
+template <typename T>
+std::map<TimeStamp,T> Buffer<T>::getContainer()
+{
+ return container_;
+}
+
+template <typename T>
+inline void Buffer<T>::clear()
{
container_.clear();
}
-inline bool BufferPackKeyFrame::empty()
+template <typename T>
+inline bool Buffer<T>::empty()
{
return container_.empty();
}
-inline SizeStd BufferPackKeyFrame::size(void)
+template <typename T>
+inline SizeStd Buffer<T>::size(void)
{
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 bool Buffer<T>::doubleCheckTimeTolerance(const TimeStamp& _time_stamp1, const Scalar& _time_tolerance1,
+ const TimeStamp& _time_stamp2, const Scalar& _time_tolerance2)
+{
+ Scalar time_diff = std::fabs(_time_stamp1 - _time_stamp2);
+ Scalar time_tol = std::min(_time_tolerance1, _time_tolerance2);
+ bool pass = time_diff <= time_tol;
+ return pass;
+}
+
+template <typename T>
+inline bool Buffer<T>::simpleCheckTimeTolerance(const TimeStamp& _time_stamp1, const TimeStamp& _time_stamp2,
+ const Scalar& _time_tolerance)
+{
+ Scalar time_diff = std::fabs(_time_stamp1 - _time_stamp2);
+ bool pass = time_diff <= _time_tolerance;
+ return pass;
+}
+
} // namespace wolf
#endif
diff --git a/include/core/processor/processor_frame_nearest_neighbor_filter.h b/include/core/processor/processor_frame_nearest_neighbor_filter.h
deleted file mode 100644
index dffee75750a452891432aa255447ad1ffb8491b2..0000000000000000000000000000000000000000
--- a/include/core/processor/processor_frame_nearest_neighbor_filter.h
+++ /dev/null
@@ -1,128 +0,0 @@
-#ifndef _WOLF_SRC_PROCESSOR_FRAME_NEAREST_NEIGHBOR_FILTER_H
-#define _WOLF_SRC_PROCESSOR_FRAME_NEAREST_NEIGHBOR_FILTER_H
-
-// Wolf related headers
-#include "core/processor/processor_loopclosure_base.h"
-#include "core/state_block/state_block.h"
-#include "core/utils/params_server.hpp"
-
-namespace wolf{
-
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsFrameNearestNeighborFilter)
-WOLF_PTR_TYPEDEFS(ProcessorFrameNearestNeighborFilter)
-
-enum class LoopclosureDistanceType : std::size_t
-{
- LC_POINT_ELLIPSE = 1, // 2D
- LC_ELLIPSE_ELLIPSE, // 2D
- LC_POINT_ELLIPSOID, // 3D
- LC_ELLIPSOID_ELLIPSOID, // 3D
- LC_MAHALANOBIS_DISTANCE // 2D & 3D
-};
-
-struct ProcessorParamsFrameNearestNeighborFilter : public ProcessorParamsLoopClosure
-{
- using DistanceType = LoopclosureDistanceType;
-
- ProcessorParamsFrameNearestNeighborFilter() :
- buffer_size_(10),
- sample_step_degree_(10),
- distance_type_(LoopclosureDistanceType::LC_POINT_ELLIPSE),
- probability_(5.991) { }
-
- ProcessorParamsFrameNearestNeighborFilter(
- const ProcessorParamsFrameNearestNeighborFilter& o) :
- buffer_size_(o.buffer_size_),
- sample_step_degree_(o.sample_step_degree_),
- distance_type_(o.distance_type_),
- probability_(o.probability_)
- {
- //
- }
- ProcessorParamsFrameNearestNeighborFilter(std::string _unique_name, const paramsServer& _server):
- ProcessorParamsLoopClosure(_unique_name, _server)
- {
- buffer_size_ = _server.getParam<int>(_unique_name + "/buffer_size");
- sample_step_degree_ = _server.getParam<int>(_unique_name + "/sample_step_degree");
- auto distance_type_str = _server.getParam<std::string>(_unique_name + "/distance_type");
- if(distance_type_str.compare("LC_POINT_ELLIPSE")) distance_type_ = LoopclosureDistanceType::LC_POINT_ELLIPSE;
- else if(distance_type_str.compare("LC_ELLIPSE_ELLIPSE")) distance_type_ = LoopclosureDistanceType::LC_ELLIPSE_ELLIPSE;
- else if(distance_type_str.compare("LC_POINT_ELLIPSOID")) distance_type_ = LoopclosureDistanceType::LC_POINT_ELLIPSOID;
- else if(distance_type_str.compare("LC_ELLIPSOID_ELLIPSOID")) distance_type_ = LoopclosureDistanceType::LC_ELLIPSOID_ELLIPSOID;
- else if(distance_type_str.compare("LC_MAHALANOBIS_DISTANCE")) distance_type_ = LoopclosureDistanceType::LC_MAHALANOBIS_DISTANCE;
- else throw std::runtime_error("Failed to fetch a valid value for the enumerate LoopclosureDistanceType. Value provided: " + distance_type_str);
- probability_ = _server.getParam<Scalar>(_unique_name + "/probability");
- }
- virtual ~ProcessorParamsFrameNearestNeighborFilter() = default;
-
- int buffer_size_;
- int sample_step_degree_;
- DistanceType distance_type_;
- Scalar probability_;
-};
-
-class ProcessorFrameNearestNeighborFilter : public ProcessorLoopClosureBase
-{
-private:
-
- // area of the computed covariance ellipse.
- // depends on how many percent of data should be considered.
- Scalar area_;
-
- ProcessorParamsFrameNearestNeighborFilterPtr params_NNF;
-
-public:
-
- using Params = ProcessorParamsFrameNearestNeighborFilter;
- using ParamsPtr = ProcessorParamsFrameNearestNeighborFilterPtr;
- using DistanceType = Params::DistanceType;
-
- ProcessorFrameNearestNeighborFilter(ParamsPtr _params_NNF);
- virtual ~ProcessorFrameNearestNeighborFilter() = default;
- virtual void configure(SensorBasePtr _sensor) { };
-
- inline DistanceType getDistanceType() const noexcept {return params_NNF->distance_type_;}
-
-protected:
-
- virtual bool findCandidates(const CaptureBasePtr& _incoming_ptr);
-
- // returns Ellipse in 2D case [ pos_x, pos_y, a, b, tilt]
- bool computeEllipse2D(const FrameBasePtr& frame_ptr,
- Eigen::Vector5s& ellipse);
-
- // returns Ellipsoid in 3D case
- // [ pos_x, pos_y, pos_z, a, b, c, quat_w, quat_z, quat_y, quat_z]
- bool computeEllipsoid3D(const FrameBasePtr& frame_ptr,
- Eigen::Vector10s& ellipsoid);
-
- // returns true if the two 2D ellipses intersect
- bool ellipse2DIntersect(const Eigen::VectorXs &ellipse1,
- const Eigen::VectorXs &ellipse2);
-
- // returns true if a 2D point lies inside a 2D ellipse
- bool point2DIntersect(const Eigen::VectorXs &point,
- const Eigen::VectorXs &ellipse);
-
- // returns true if the two 3D ellipsoids intersect
- bool ellipsoid3DIntersect(const Eigen::VectorXs &ellipsoid1,
- const Eigen::VectorXs &ellipsoid2);
-
- // returns true if a 3D point lies inside a 3D ellipsoid
- bool point3DIntersect(const Eigen::VectorXs &point,
- const Eigen::VectorXs &ellipsoid);
-
- // returns true if frame lies within Mahalanobis Distance
- bool insideMahalanisDistance(const FrameBasePtr& trajectory_frame,
- const FrameBasePtr& query_frame);
-
- // computes the Mahalanobis Distance
- Scalar MahalanobisDistance(const FrameBasePtr& trajectory_frame,
- const FrameBasePtr& query_frame);
-
- bool frameInsideBuffer(const FrameBasePtr& frame_ptr);
-};
-
-} // namespace wolf
-
-#endif // _WOLF_SRC_PROCESSOR_FRAME_NEAREST_NEIGHBOR_FILTER_H_
diff --git a/include/core/processor/processor_loopclosure.h b/include/core/processor/processor_loopclosure.h
new file mode 100644
index 0000000000000000000000000000000000000000..afd9855b5087e61825706b006456039cba06e93d
--- /dev/null
+++ b/include/core/processor/processor_loopclosure.h
@@ -0,0 +1,154 @@
+#ifndef _WOLF_PROCESSOR_LOOPCLOSURE_BASE_H
+#define _WOLF_PROCESSOR_LOOPCLOSURE_BASE_H
+
+// Wolf related headers
+#include "core/processor/processor_base.h"
+
+namespace wolf{
+
+WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsLoopClosure);
+
+struct ProcessorParamsLoopClosure : public ProcessorParamsBase
+{
+ using ProcessorParamsBase::ProcessorParamsBase;
+ // virtual ~ProcessorParamsLoopClosure() = default;
+
+ // add neccesery parameters for loop closure initialisation here and initialize
+ // them in constructor
+};
+
+/** \brief General loop closure processor
+ *
+ * This is an abstract class.
+ * + You must define the following classes :
+ * - voteComputeFeatures()
+ * - voteSearchLoopClosure()
+ * - computeFeatures()
+ * - findLoopCandidate()
+ * - createFactors()
+ * + You can override the following classes :
+ * - selectPairKC()
+ * - validateLoop()
+ * - processLoopClosure()
+ *
+ * It establishes factors XXX
+ *
+ * Should you need extra functionality for your derived types,
+ * you can overload these two methods,
+ *
+ * - preProcess() { }
+ * - postProcess() { }
+ *
+ * which are called at the beginning and at the end of process() respectively.
+ */
+
+class ProcessorLoopClosure : public ProcessorBase
+{
+protected:
+
+ ProcessorParamsLoopClosurePtr params_loop_closure_;
+
+public:
+
+ ProcessorLoopClosure(const std::string& _type, ProcessorParamsLoopClosurePtr _params_loop_closure);
+
+ virtual ~ProcessorLoopClosure() = default;
+ virtual void configure(SensorBasePtr _sensor) override { };
+
+ /** \brief Full processing of an incoming Capture.
+ *
+ * Usually you do not need to overload this method in derived classes.
+ * Overload it only if you want to alter this algorithm.
+ */
+ virtual void process(CaptureBasePtr _incoming_ptr) override;
+
+protected:
+
+ /** \brief Called by process(). Tells if computeFeatures() will be called
+ */
+ virtual bool voteComputeFeatures() = 0;
+
+ /** \brief Called by process(). Tells if findLoopCandidate() and createFactors() will be called
+ *
+ * WARNING : A LC can be searched only when voteComputeFeatures() return true
+ */
+ virtual bool voteSearchLoopClosure() = 0;
+
+ /** \brief returns a KeyFrame-Capture pair compatible together (selected from the buffers)
+ *
+ * Should clear elements before the ones selected in buffers.
+ * In the default implementation, we select the KF with the most recent TimeStamp
+ * and that is compatible with at least a Capture
+ */
+ virtual std::pair<FrameBasePtr,CaptureBasePtr> selectPairKC(void);
+
+ /** \brief add the Capture and all features needed to the corresponding KF
+ *
+ * If the loop closure process requires features associated to each capture,
+ * the computations to create these features must be done here.
+ *
+ * Important: All detected features should be emplaced to the capture.
+ *
+ * Returns a bool that tells if features were successfully created
+ */
+ virtual bool detectFeatures(CaptureBasePtr cap) = 0;
+
+ /** \brief Find a KF that would be a good candidate to close a loop
+ * if validateLoop is not overwritten, a loop will be closed with the returned candidate
+ * if no good candidate is found, return nullptr
+ */
+ virtual CaptureBasePtr findLoopCandidate(CaptureBasePtr _capture) = 0;
+
+ /** \brief validate/discard a loop closure
+ *
+ * overwrite it if you want an additional test after findLoopCandidate()
+ */
+ virtual bool validateLoop(CaptureBasePtr _capture_1, CaptureBasePtr _capture_2) {return true;};
+
+ /** \brief emplace the factor(s)
+ *
+ */
+ virtual void emplaceFactors(CaptureBasePtr _capture_1, CaptureBasePtr _capture_2) = 0;
+
+
+ /** Pre-process incoming Capture
+ *
+ * This is called by process() just after assigning incoming_ptr_ to a valid Capture.
+ *
+ * Overload this function to prepare stuff on derived classes.
+ *
+ * Typical uses of prePrecess() are:
+ * - casting base types to derived types
+ * - initializing counters, flags, or any derived variables
+ * - initializing algorithms needed for processing the derived data
+ */
+ virtual void preProcess() { }
+
+ /** Post-process
+ *
+ * This is called by process() after finishing the processing algorithm.
+ *
+ * Overload this function to post-process stuff on derived classes.
+ *
+ * Typical uses of postPrecess() are:
+ * - resetting and/or clearing variables and/or algorithms at the end of processing
+ * - drawing / printing / logging the results of the processing
+ */
+ virtual void postProcess() { }
+
+ /** \brief Vote for KeyFrame generation
+ *
+ * If a KeyFrame criterion is validated, this function returns true,
+ * meaning that it wants to create a KeyFrame at the \b last Capture.
+ *
+ * WARNING! This function only votes! It does not create KeyFrames!
+ */
+ bool voteForKeyFrame()
+ {
+ return false;
+ };
+};
+
+} // namespace wolf
+
+#endif /* _WOLF_PROCESSOR_LOOPCLOSURE_BASE_H */
diff --git a/include/core/processor/processor_loopclosure_base.h b/include/core/processor/processor_loopclosure_base.h
deleted file mode 100644
index a5dc8589156b6b525b0a9b551ec3dc55ab7f5af7..0000000000000000000000000000000000000000
--- a/include/core/processor/processor_loopclosure_base.h
+++ /dev/null
@@ -1,140 +0,0 @@
-#ifndef _WOLF_PROCESSOR_LOOPCLOSURE_BASE_H
-#define _WOLF_PROCESSOR_LOOPCLOSURE_BASE_H
-
-// Wolf related headers
-#include "core/processor/processor_base.h"
-
-namespace wolf{
-
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsLoopClosure);
-
-struct ProcessorParamsLoopClosure : public ProcessorParamsBase
-{
- using ProcessorParamsBase::ProcessorParamsBase;
-// virtual ~ProcessorParamsLoopClosure() = default;
-
- // add neccesery parameters for loop closure initialisation here and initialize
- // them in constructor
-};
-
-/** \brief General loop closure processor
- *
- * This is an abstract class.
- *
- * It establishes factors XXX
- *
- * Should you need extra functionality for your derived types,
- * you can overload these two methods,
- *
- * - preProcess() { }
- * - postProcess() { }
- *
- * which are called at the beginning and at the end of process() respectively.
- */
-
-class ProcessorLoopClosureBase : public ProcessorBase
-{
-protected:
-
- ProcessorParamsLoopClosurePtr params_loop_closure_;
-
- // Frames that are possible loop closure candidates according to
- // findLoopClosure()
- std::vector<FrameBasePtr> loop_closure_candidates;
-
- // Frames that are possible loop closure candidates according to
- // findLoopClosure(), but are too recent in the timeline, aka still in a
- // 'buffer zone'. This vector will capture the frames that were set just before
- // the last keyframe.
- std::vector<FrameBasePtr> close_candidates;
-
-public:
-
- ProcessorLoopClosureBase(const std::string& _type, ProcessorParamsLoopClosurePtr _params_loop_closure);
-
- virtual ~ProcessorLoopClosureBase() = default;
- virtual void configure(SensorBasePtr _sensor) override { };
-
- /** \brief Full processing of an incoming Capture.
- *
- * Usually you do not need to overload this method in derived classes.
- * Overload it only if you want to alter this algorithm.
- */
- virtual void process(CaptureBasePtr _incoming_ptr) override;
-
- virtual void keyFrameCallback(FrameBasePtr _keyframe_ptr,
- const Scalar& _time_tol_other) override ;
-
- const std::vector<FrameBasePtr>& getCandidates() const noexcept;
-
- const std::vector<FrameBasePtr>& getCloseCandidates() const noexcept;
-
-protected:
-
- /** Pre-process incoming Capture
- *
- * This is called by process() just after assigning incoming_ptr_ to a valid Capture.
- *
- * Overload this function to prepare stuff on derived classes.
- *
- * Typical uses of prePrecess() are:
- * - casting base types to derived types
- * - initializing counters, flags, or any derived variables
- * - initializing algorithms needed for processing the derived data
- */
- virtual void preProcess() { }
-
- /** Post-process
- *
- * This is called by process() after finishing the processing algorithm.
- *
- * Overload this function to post-process stuff on derived classes.
- *
- * Typical uses of postPrecess() are:
- * - resetting and/or clearing variables and/or algorithms at the end of processing
- * - drawing / printing / logging the results of the processing
- */
- virtual void postProcess() { }
-
- /** Find a loop closure between incoming data and all keyframe data
- *
- * This is called by process() .
- *
- * Overload this function in derived classes to find loop closure.
- */
- virtual bool findCandidates(const CaptureBasePtr& _incoming_ptr) = 0;
-
- /** perform a validation among the found possible loop closures to confirm
- * or dismiss them based on available data
- *
- * This is called by process() .
- *
- * Overload this function in derived classes to confirm loop closure.
- */
- virtual bool confirmLoopClosure() = 0;
-
- /** \brief Vote for KeyFrame generation
- *
- * If a KeyFrame criterion is validated, this function returns true,
- * meaning that it wants to create a KeyFrame at the \b last Capture.
- *
- * WARNING! This function only votes! It does not create KeyFrames!
- */
- virtual bool voteForKeyFrame() override = 0;
-};
-
-inline const std::vector<FrameBasePtr>&
-ProcessorLoopClosureBase::getCandidates() const noexcept
-{
- return loop_closure_candidates;
-}
-
-inline const std::vector<FrameBasePtr>&
-ProcessorLoopClosureBase::getCloseCandidates() const noexcept
-{
- return close_candidates;
-}
-
-} // namespace wolf
-
-#endif /* _WOLF_PROCESSOR_LOOPCLOSURE_BASE_H */
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index 5c2dd85aeb423f4879ac12c135cd18e08233c3a1..a70930c6b936ef898db10f22a01d8b8010cdc1c3 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -54,7 +54,14 @@ void ProcessorBase::keyFrameCallback(FrameBasePtr _keyframe_ptr, const Scalar& _
{
WOLF_DEBUG("P", isMotion() ? "M " : "T ", getName(), ": KF", _keyframe_ptr->id(), " callback received with ts = ", _keyframe_ptr->getTimeStamp());
if (_keyframe_ptr != nullptr)
- buffer_pack_kf_.add(_keyframe_ptr,_time_tol_other);
+ buffer_pack_kf_.add(_keyframe_ptr, _time_tol_other);
+}
+
+void ProcessorBase::captureCallback(CaptureBasePtr _capture_ptr)
+{
+ WOLF_DEBUG("P", isMotion() ? "M " : "T ", getName(), ": Capture", _capture_ptr->id(), " callback received with ts = ", _capture_ptr->getTimeStamp());
+ if (_capture_ptr != nullptr)
+ buffer_capture_.add(_capture_ptr->getTimeStamp(), _capture_ptr);
}
void ProcessorBase::remove()
@@ -93,17 +100,11 @@ void ProcessorBase::remove()
}
/////////////////////////////////////////////////////////////////////////////////////////
-void BufferPackKeyFrame::removeUpTo(const TimeStamp& _time_stamp)
-{
- BufferPackKeyFrame::Iterator post = container_.upper_bound(_time_stamp);
- container_.erase(container_.begin(), post); // erasing by range
-}
-
void BufferPackKeyFrame::add(const FrameBasePtr& _key_frame, const Scalar& _time_tolerance)
{
TimeStamp time_stamp = _key_frame->getTimeStamp();
PackKeyFramePtr kfpack = std::make_shared<PackKeyFrame>(_key_frame, _time_tolerance);
- container_.emplace(time_stamp, kfpack);
+ Buffer::add(time_stamp, kfpack);
}
PackKeyFramePtr BufferPackKeyFrame::selectPack(const TimeStamp& _time_stamp, const Scalar& _time_tolerance)
@@ -122,13 +123,13 @@ PackKeyFramePtr BufferPackKeyFrame::selectPack(const TimeStamp& _time_stamp, con
bool prev_exists = (post != container_.begin());
bool post_exists = (post != container_.end());
- bool post_ok = post_exists && checkTimeTolerance(post->first, post->second->time_tolerance, _time_stamp, _time_tolerance);
+ 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 = checkTimeTolerance(prev->first, prev->second->time_tolerance, _time_stamp, _time_tolerance);
+ bool prev_ok = doubleCheckTimeTolerance(prev->first, prev->second->time_tolerance, _time_stamp, _time_tolerance);
if (prev_ok && !post_ok)
return prev->second;
@@ -170,7 +171,7 @@ PackKeyFramePtr BufferPackKeyFrame::selectFirstPackBefore(const TimeStamp& _time
return container_.begin()->second;
// 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))
+ 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)
@@ -193,12 +194,4 @@ void BufferPackKeyFrame::print(void)
std::cout << "]" << std::endl;
}
-bool BufferPackKeyFrame::checkTimeTolerance(const TimeStamp& _time_stamp1, const Scalar& _time_tolerance1,
- const TimeStamp& _time_stamp2, const Scalar& _time_tolerance2)
-{
- Scalar time_diff = std::fabs(_time_stamp1 - _time_stamp2);
- Scalar time_tol = std::min(_time_tolerance1, _time_tolerance2);
- bool pass = time_diff <= time_tol;
- return pass;
-}
} // namespace wolf
diff --git a/src/processor/processor_frame_nearest_neighbor_filter.cpp b/src/processor/processor_frame_nearest_neighbor_filter.cpp
deleted file mode 100644
index b4f6303b128b37ba74104cddf02dd82e328c08e8..0000000000000000000000000000000000000000
--- a/src/processor/processor_frame_nearest_neighbor_filter.cpp
+++ /dev/null
@@ -1,506 +0,0 @@
-#include "core/processor/processor_frame_nearest_neighbor_filter.h"
-
-namespace wolf
-{
-ProcessorFrameNearestNeighborFilter::ProcessorFrameNearestNeighborFilter(ParamsPtr _params_NNF):
- ProcessorLoopClosureBase("FRAME NEAREST NEIGHBOR FILTER", _params_NNF),
- params_NNF(_params_NNF)
-{
- // area of ellipse based on the Chi-Square Probabilities
- // https://people.richland.edu/james/lecture/m170/tbl-chi.html
- // cover both 2D and 3D cases
-
- if(params_NNF->distance_type_ == DistanceType::LC_POINT_ELLIPSE ||
- params_NNF->distance_type_ == DistanceType::LC_ELLIPSE_ELLIPSE)
- {
- switch ((int)(params_NNF->probability_*100))
- {
- case 900: area_ = 4.605; // 90% probability
- break;
- case 950: area_ = 5.991; // 95% probability
- break;
- case 990: area_ = 9.210; // 99% probability
- break;
- default : area_ = 5.991; // 95% probability
- break;
- }
- }
- if (params_NNF->distance_type_ == DistanceType::LC_POINT_ELLIPSOID ||
- params_NNF->distance_type_ == DistanceType::LC_ELLIPSOID_ELLIPSOID)
- {
- switch ((int)(params_NNF->probability_*100))
- {
- case 900: area_ = 6.251; // 90% probability
- break;
- case 950: area_ = 7.815; // 95% probability
- break;
- case 990: area_ = 11.345; // 99% probability
- break;
- default : area_ = 7.815; // 95% probability
- break;
- }
- }
-
- /*
- * The area is based on the Chi-Square Probabilities
- * Becasue they are very big for high likelihood, it is here manually set
- * on ONE. Therefore the ellipses/ ellipsoids are based on the unit ellipse/
- * ellipsoids and the axis are scaled by a / b / c
- */
-
- area_ = 1;
-}
-
-//##############################################################################
-bool ProcessorFrameNearestNeighborFilter::findCandidates(const CaptureBasePtr& /*_incoming_ptr*/)
-{
- const ProblemPtr problem_ptr = getProblem();
-
- const std::string frame_structure =
- problem_ptr->getTrajectory()->getFrameStructure();
-
- // get the list of all frames
- const FrameBasePtrList& frame_list = problem_ptr->
- getTrajectory()->
- getFrameList();
-
- bool found_possible_candidate = false;
-
- for (const FrameBasePtr& key_it : frame_list)
- {
- // check for LC just if frame is key frame
- // Assert that the evaluated KF has a capture of the
- // same sensor as this processor
- if (key_it->isKey() && key_it->getCaptureOf(getSensor()/*, "LASER 2D"*/) != nullptr)
- {
- // Check if the two frames currently evaluated are already
- // constrained one-another.
- const FactorBasePtrList& fac_list = key_it->getConstrainedByList();
-
- bool are_constrained = false;
- for (const FactorBasePtr& crt : fac_list)
- {
- // Are the two frames constrained one-another ?
- if (crt->getFrameOther() == problem_ptr->getLastKeyFrame())
- {
- // By this very processor ?
- if (crt->getProcessor() == shared_from_this())
- {
- WOLF_DEBUG("Frames are already constrained together.");
- are_constrained = true;
- break;
- }
- }
- }
- if (are_constrained) continue;
-
- // check if feame is potential candidate for loop closure with
- // chosen distance type
- switch (params_NNF->distance_type_)
- {
- case LoopclosureDistanceType::LC_POINT_ELLIPSE:
- {
- if (frame_structure == "PO 3D" || frame_structure == "POV 3D") // @todo add frame structure "PQVBB 3D"
- {
- WOLF_ERROR("Distance Type LC_POINT_ELLIPSE does not fit 3D frame structure");
- return false;
- }
-
- Eigen::Vector5s frame_covariance;
- if (!computeEllipse2D(key_it, frame_covariance)) continue;
-
- const Eigen::VectorXs current_position = getProblem()->getCurrentState();
- found_possible_candidate = point2DIntersect(current_position,
- frame_covariance);
- break;
- }
-
- case LoopclosureDistanceType::LC_ELLIPSE_ELLIPSE:
- {
- if (frame_structure == "PO 3D" || frame_structure == "POV 3D")
- {
- WOLF_ERROR("Distance Type LC_ELLIPSE_ELLIPSE does not fit 3D frame structure");
- return false;
- }
-
- Eigen::Vector5s frame_covariance, current_covariance;
- if (!computeEllipse2D(key_it,
- frame_covariance)) continue;
- if (!computeEllipse2D(getProblem()->getLastKeyFrame(),
- current_covariance)) continue;
- found_possible_candidate = ellipse2DIntersect(frame_covariance,
- current_covariance);
- break;
- }
-
- case LoopclosureDistanceType::LC_POINT_ELLIPSOID:
- {
- if (frame_structure == "PO 2D")
- {
- WOLF_ERROR("Distance Type POINT_ELLIPSOID does not fit 2D frame structure");
- return false;
- }
- Eigen::Vector10s frame_covariance;
- if (!computeEllipsoid3D(key_it,
- frame_covariance)) continue;
- const Eigen::VectorXs current_position = getProblem()->getCurrentState();
- found_possible_candidate = point3DIntersect(current_position,
- frame_covariance);
- break;
- }
-
- case LoopclosureDistanceType::LC_ELLIPSOID_ELLIPSOID:
- {
- if (frame_structure == "PO 2D")
- {
- WOLF_ERROR("Distance Type ELLIPSOID_ELLIPSOID does not fit 2D frame structure");
- return false;
- }
- Eigen::Vector10s frame_covariance, current_covariance;
- if (!computeEllipsoid3D(key_it,
- frame_covariance)) continue;
- if (!computeEllipsoid3D(getProblem()->getLastKeyFrame(),
- frame_covariance)) continue;
- found_possible_candidate = ellipsoid3DIntersect(frame_covariance,
- current_covariance);
- break;
- }
-
- case LoopclosureDistanceType::LC_MAHALANOBIS_DISTANCE:
- {
- found_possible_candidate = insideMahalanisDistance(key_it,
- problem_ptr->getLastKeyFrame());
- break;
- }
-
- default:
- WOLF_WARN("NO IMPLEMENTATION FOR OTHER DISTANCE CALCULATION TYPES YET.");
- found_possible_candidate = false;
- return false;
- }
-
- // if a candidate was detected, push it to the appropiate list
- if (found_possible_candidate)
- {
- if (!frameInsideBuffer(key_it))
- loop_closure_candidates.push_back(key_it);
- else
- close_candidates.push_back(key_it);
- }
-
- } // end if key_it is keyframe
- } // end loop through every frame in list
-
- return found_possible_candidate;
-}
-
-//##############################################################################
-bool ProcessorFrameNearestNeighborFilter::computeEllipse2D(const FrameBasePtr& frame_ptr,
- Eigen::Vector5s& ellipse)
-{
- // get 3x3 covariance matrix AKA variance
- Eigen::MatrixXs covariance;
- if (!getProblem()->getFrameCovariance(frame_ptr, covariance))
- {
- WOLF_WARN("Frame covariance not found!");
- return false;
- }
-
- if (!isCovariance(covariance))
- {
- WOLF_WARN("Covariance is not proper !");
- return false;
- }
-
- // get position of frame AKA mean [x, y]
- const Eigen::VectorXs frame_position = frame_ptr->getP()->getState();
- ellipse(0) = frame_position(0);
- ellipse(1) = frame_position(1);
-
- // compute covariance error ellipse around state
- Eigen::SelfAdjointEigenSolver<Eigen::Matrix2s> solver(covariance.block(0,0,2,2));
- Scalar eigenvalue1 = solver.eigenvalues()[0]; // smaller value
- Scalar eigenvalue2 = solver.eigenvalues()[1]; // bigger value
- //Eigen::VectorXs eigenvector1 = solver.eigenvectors().col(0);
- Eigen::VectorXs eigenvector2 = solver.eigenvectors().col(1);
-
- const Scalar scale = std::sqrt(area_);
- ellipse(2) = scale * std::sqrt(eigenvalue2) / 2.; // majorAxis
- ellipse(3) = scale * std::sqrt(eigenvalue1) / 2.; // minorAxis
- ellipse(4) = std::atan2(eigenvector2[1], eigenvector2[0]); // tilt
-
- if (ellipse(4) < Scalar(0)) ellipse(4) += Scalar(2) * M_PI;
-
- // [ pos_x, pos_y, a, b, tilt]
- return true;
-}
-
-//##############################################################################
-bool ProcessorFrameNearestNeighborFilter::computeEllipsoid3D(const FrameBasePtr& frame_pointer,
- Eigen::Vector10s& ellipsoid)
-{
- // Ellipse description [ pos_x, pos_y, pos_z, a, b, c, quat_w, quat_z, quat_y, quat_z]
-
- // get position of frame AKA mean [x, y, z]
- const Eigen::VectorXs frame_position = frame_pointer->getP()->getState();
- ellipsoid(0) = frame_position(0);
- ellipsoid(1) = frame_position(1);
- ellipsoid(2) = frame_position(2);
-
- // get 9x9 covariance matrix AKA variance
- Eigen::MatrixXs covariance;
- if (!getProblem()->getFrameCovariance(frame_pointer, covariance))
- {
- WOLF_WARN("Frame covariance not found!");
- return false;
- }
-
- if (!isCovariance(covariance))
- {
- WOLF_WARN("Covariance is not proper !");
- return false;
- }
-
- Eigen::SelfAdjointEigenSolver<Eigen::Matrix3s> solver(covariance.block(0,0,3,3));
- const Scalar eigenvalue1 = solver.eigenvalues()[0]; // smaller value
- const Scalar eigenvalue2 = solver.eigenvalues()[1]; // mediate value
- const Scalar eigenvalue3 = solver.eigenvalues()[2]; // bigger value
-
- const Scalar scale = std::sqrt(area_);
-
- ellipsoid(3) = scale * std::sqrt(std::abs(eigenvalue3)) / Scalar(2); // majorAxis
- ellipsoid(4) = scale * std::sqrt(std::abs(eigenvalue2)) / Scalar(2); // mediumAxis
- ellipsoid(5) = scale * std::sqrt(std::abs(eigenvalue1)) / Scalar(2); // minorAxis
-
- // ROTATION COMPUTATION get rotation of the three axis from eigenvector
- // eigenvector belonging to biggest eigenvalue gives direction / z_axis
- // of ellipsoid
-
- // get eigenvectors that correspond to eigenvalues
- //const Eigen::Vector3s eigenvector1 = solver.eigenvectors().col(0); // minorAxis
- const Eigen::Vector3s eigenvector2 = solver.eigenvectors().col(1); // mediumAxis ->y
- const Eigen::Vector3s eigenvector3 = solver.eigenvectors().col(2); // majorAxis -> x
-
- // computed axis of coordinate system in ellipsoid center
- const Eigen::Vector3s z_axis = eigenvector3.cross(eigenvector2);
- const Eigen::Vector3s y_axis = z_axis.cross(eigenvector3);
-
- // use them to fill rotation matrix
- Eigen::Matrix3s rotationMatrix;
- rotationMatrix.col(0) = eigenvector3.normalized();
- rotationMatrix.col(1) = y_axis.normalized();
- rotationMatrix.col(2) = z_axis.normalized();
-
- // get quaternions for transformation
- Eigen::Quaternions rotation(rotationMatrix);
- rotation.normalize();
-
- ellipsoid(6) = rotation.w();
- ellipsoid(7) = rotation.x();
- ellipsoid(8) = rotation.y();
- ellipsoid(9) = rotation.z();
-
- WOLF_DEBUG("made ellipsoid: \n[", ellipsoid.transpose(), "]");
-
- // [ pos_x, pos_y, pos_z, a, b, c, alpha, beta, gamma]
- return true;
-}
-
-//##############################################################################
-bool ProcessorFrameNearestNeighborFilter::ellipse2DIntersect(const Eigen::VectorXs& ellipse1,
- const Eigen::VectorXs& ellipse2)
-{
- for (int i = 0 ; i < 360 ; i += params_NNF->sample_step_degree_)
- {
- // parameterized form of first ellipse gives point on first ellipse
- // https://www.uwgb.edu/dutchs/Geometry/HTMLCanvas/ObliqueEllipses5.HTM
- Eigen::VectorXs pointOnEllipse(2);
- const Scalar theta = Scalar(i) * M_PI / 180.0;
-
- pointOnEllipse(0) = ellipse1(0) +
- ellipse1(2) * std::cos(ellipse1(4)) * std::cos(theta) -
- ellipse1(3) * std::sin(ellipse1(4)) * std::sin(theta);
-
- pointOnEllipse(1) = ellipse1(1) +
- ellipse1(2) * std::sin(ellipse1(4)) * std::cos(theta) +
- ellipse1(3) * std::cos(ellipse1(4)) * std::sin(theta);
-
- //WOLF_DEBUG(" for ", i, " deg / ", theta " rad ---> [" ,
- // pointOnEllipse.transpose(), "]");
-
- // check if point lies inside the other ellipse
- if (point2DIntersect(pointOnEllipse, ellipse2)) return true;
- }
-
- return false;
-}
-
-//##############################################################################
-bool ProcessorFrameNearestNeighborFilter::point2DIntersect(const Eigen::VectorXs& point,
- const Eigen::VectorXs& ellipse)
-{
- const Scalar area51 = std::pow( ((point(0) - ellipse(0)) * cos(ellipse(4))
- + (point(1) - ellipse(1)) * sin(ellipse(4))), 2 )
- / std::pow( ellipse(2), 2 )
- + std::pow( ((point(0) - ellipse(0)) * sin(ellipse(4))
- - (point(1) - ellipse(1)) * cos(ellipse(4))), 2 )
- / std::pow( ellipse(3), 2 );
-
- //WOLF_DEBUG("computed area = ", area51);
-
- if ( area51 - area_ <= 0) return true;
-
- return false;
-}
-
-//##############################################################################
-bool ProcessorFrameNearestNeighborFilter::ellipsoid3DIntersect(const Eigen::VectorXs &ellipsoid1,
- const Eigen::VectorXs &ellipsoid2)
-{
- // get transformation from elliposiod1 center frame to world frame
- Eigen::Matrix4s transformation_matrix;
- Eigen::Quaternions rotation(ellipsoid1(6),ellipsoid1(7),
- ellipsoid1(8),ellipsoid1(9));
-// Eigen::Vector4s translation;
-// translation << ellipsoid1(0), ellipsoid1(1), ellipsoid1(2), 1;
- transformation_matrix.block(0,0,3,3) = rotation.toRotationMatrix();
- transformation_matrix.col(3) << ellipsoid1(0), ellipsoid1(1), ellipsoid1(2), 1;
-
- Eigen::Vector4s point_hom = Eigen::Vector4s::Constant(1);
-
- Scalar omega = 0;
- for (int i = 0 ; i < 360 ; i += params_NNF->sample_step_degree_)
- {
- const Scalar theta = Scalar(i) * M_PI / 180.0;
- for( int j = 0 ; j < 180 ; j += params_NNF->sample_step_degree_)
- {
- omega = Scalar(j) * M_PI / 180.0;
-
- // compute point on surface of first ellipsoid
- point_hom(0) = ellipsoid1(3) * std::cos(theta) * std::sin(omega);
- point_hom(1) = ellipsoid1(4) * std::sin(theta) * std::sin(omega);
- point_hom(2) = ellipsoid1(5) * std::cos(omega);
-
- // transform point into world frame
- const Eigen::Vector4s point_on_ellipsoid = transformation_matrix * point_hom;
-
- // check if 3D point lies inside the second ellipsoid
- if (point3DIntersect(point_on_ellipsoid, ellipsoid2))
- return true;
- }
- }
- return false;
-}
-
-//##############################################################################
-bool ProcessorFrameNearestNeighborFilter::point3DIntersect(const Eigen::VectorXs &point,
- const Eigen::VectorXs &ellipsoid)
-{
- // get transformation from ellipsoid center frame to world frame
- Eigen::Matrix4s transformation_matrix = Eigen::Matrix4s::Identity();
-
- Eigen::Quaternions rotation(ellipsoid(6),ellipsoid(7),
- ellipsoid(8),ellipsoid(9));
-// Eigen::Vector4s translation;
-// translation << ellipsoid(0), ellipsoid(1), ellipsoid(2), 1;
- transformation_matrix.block(0,0,3,3) = rotation.toRotationMatrix();
- transformation_matrix.col(3) << ellipsoid(0), ellipsoid(1), ellipsoid(2), 1;
- // inverse to get transformation from world frame to ellipsoid center frame
- transformation_matrix = transformation_matrix.inverse().eval();
-
- // homogenize 3D point
- // ???
- Eigen::Vector4s point_hom;
- point_hom << point(0), point(1), point(2), 1;
-
- // transform point from world frame to elliposiod center frame
- Eigen::Vector4s transformed_point = transformation_matrix * point_hom;
-
- // check if point is inside ellipsoid with general equation
- Scalar area51 = std::pow(transformed_point(0), 2) / std::pow( ellipsoid(3), 2) +
- std::pow(transformed_point(1), 2) / std::pow(ellipsoid(4), 2) +
- std::pow(transformed_point(2), 2) / std::pow(ellipsoid(5), 2);
-
- WOLF_DEBUG("computed area = ", area51);
-
- if (area51 - area_ <= 0) return true;
-
- return false;
-}
-
-//##############################################################################
-bool ProcessorFrameNearestNeighborFilter::insideMahalanisDistance(const FrameBasePtr& trajectory_frame,
- const FrameBasePtr& query_frame)
-{
- const Scalar distance = MahalanobisDistance(trajectory_frame, query_frame);
-
- WOLF_DEBUG("Mahalanobis Distance = ", distance);
-
- if ( distance < 0 )
- {
- WOLF_DEBUG("NO COVARIANCE AVAILABLE");
- }
-
- /*
- * TODO:
- * check if distance is smaller than a threshold,
- * if yes -> return true
- * else -> return false
- *
- */
- return false;
-}
-
-//##############################################################################
-Scalar ProcessorFrameNearestNeighborFilter::MahalanobisDistance(const FrameBasePtr& trajectory,
- const FrameBasePtr& query)
-{
- Scalar distance = -1;
- Eigen::VectorXs traj_pose, query_pose;
-
- // get state and covariance matrix for both frames
- if (trajectory->getP()->getState().size() == 2)
- {
- traj_pose.resize(3);
- query_pose.resize(3);
- }
- else
- {
- traj_pose.resize(7);
- query_pose.resize(7);
- }
-
- traj_pose << trajectory->getP()->getState(),
- trajectory->getO()->getState();
-
- query_pose << query->getP()->getState(),
- query->getO()->getState();
-
- Eigen::MatrixXs traj_covariance, query_covariance;
- if ( !getProblem()->getFrameCovariance(trajectory, traj_covariance) ||
- !getProblem()->getFrameCovariance(query, query_covariance) ||
- !isCovariance(traj_covariance) ||
- !isCovariance(query_covariance))
- return distance;
-
- const Eigen::MatrixXs covariance = traj_covariance * query_covariance.transpose();
-
- const Eigen::VectorXs pose_difference = traj_pose - query_pose;
- distance = pose_difference.transpose() * covariance * pose_difference;
- distance = std::sqrt(distance);
-
- return distance;
-}
-
-//##############################################################################
-bool ProcessorFrameNearestNeighborFilter::frameInsideBuffer(const FrameBasePtr& frame_ptr)
-{
- FrameBasePtr keyframe = getProblem()->getLastKeyFrame();
- if ( (int)frame_ptr->id() < ( (int)keyframe->id() - params_NNF->buffer_size_ ))
- return false;
- else
- return true;
-}
-
-} // namespace wolf
-
diff --git a/src/processor/processor_loopclosure.cpp b/src/processor/processor_loopclosure.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..bbc8c1c41d601759475b810f37f004326a22cada
--- /dev/null
+++ b/src/processor/processor_loopclosure.cpp
@@ -0,0 +1,104 @@
+/**
+ * \file processor_loop_closure.h
+ *
+ * Created on: Mai 31, 2019
+ * \author: Pierre Guetschel
+ */
+
+#include <core/processor/processor_loopclosure.h>
+
+
+namespace wolf
+{
+
+ProcessorLoopClosure::ProcessorLoopClosure(const std::string& _type, ProcessorParamsLoopClosurePtr _params_loop_closure):
+ ProcessorBase(_type, _params_loop_closure),
+ params_loop_closure_(_params_loop_closure)
+{
+ //
+}
+
+//##############################################################################
+void ProcessorLoopClosure::process(CaptureBasePtr _incoming_ptr)
+{
+ // the pre-process, if necessary, is implemented in the derived classes
+ preProcess();
+
+ if (voteComputeFeatures())
+ {
+ std::pair<FrameBasePtr,CaptureBasePtr> pairKC = selectPairKC();
+
+
+ auto cap_1 = pairKC.second;
+ auto kf_1 = pairKC.first;
+
+ if (kf_1==nullptr || cap_1==nullptr) return;
+ bool success_computeFeatures = detectFeatures(cap_1);
+
+ // if succeded
+ if (success_computeFeatures)
+ {
+ // link the capture to the KF (if not already linked)
+ if (cap_1->getFrame() != kf_1)
+ {
+ assert(cap_1->getFrame() == nullptr && "capture already linked to a different frame"); //FIXME
+ cap_1->link(kf_1);
+ }
+
+ // search loop closure
+ if(voteSearchLoopClosure())
+ {
+ auto cap_2 = findLoopCandidate(cap_1);
+ if (cap_2==nullptr)
+ return;
+ if (validateLoop(cap_1, cap_2)==false)
+ return;
+ if (cap_1->getFrame() == nullptr || cap_2->getFrame() == nullptr)
+ {
+ WOLF_WARN("ProcessorLoopClosureBase : tried to close a loop with captures linked to no KF");
+ return;
+ }
+ if (cap_1->getFrame() == cap_2->getFrame())
+ {
+ WOLF_WARN("ProcessorLoopClosureBase : findLoopCandidate() returned two captures of the same frame");
+ return;
+ }
+ emplaceFactors(cap_1, cap_2);
+ }
+ }
+ };
+
+ // the post-process, if necessary, is implemented in the derived classes
+ postProcess();
+}
+
+/**
+ * In the default implementation, we select the KF with the most recent TimeStamp
+ * and that is compatible with at least a Capture
+ */
+std::pair<FrameBasePtr,CaptureBasePtr> ProcessorLoopClosure::selectPairKC()
+{
+ std::map<TimeStamp,PackKeyFramePtr> kf_container = buffer_pack_kf_.getContainer();
+ if (kf_container.empty()){
+ return std::make_pair(nullptr, nullptr);};
+
+ for (auto kf_it=kf_container.begin(); kf_it!=kf_container.end(); ++kf_it)
+ {
+ CaptureBasePtr cap_ptr = buffer_capture_.select(kf_it->first, kf_it->second->time_tolerance);
+ if (cap_ptr != nullptr)
+ {
+ // clear the buffers :
+ buffer_capture_.removeUpTo(cap_ptr->getTimeStamp());
+ buffer_pack_kf_.removeUpTo(kf_it->first);
+ // return the KF-Cap pair :
+ return std::make_pair(kf_it->second->key_frame, cap_ptr);
+ };
+ }
+ return std::make_pair(nullptr, nullptr);
+}
+
+
+//##############################################################################
+
+
+}// namespace wolf
diff --git a/src/processor/processor_loopclosure_base.cpp b/src/processor/processor_loopclosure_base.cpp
deleted file mode 100644
index 7ce19414499ead4b38e8432077f47e15116ca27e..0000000000000000000000000000000000000000
--- a/src/processor/processor_loopclosure_base.cpp
+++ /dev/null
@@ -1,49 +0,0 @@
-/**
- * \file processor_loop_closure.h
- *
- * Created on: Aug 10, 2017
- * \author: Tessa Johanna
- */
-
-#include "core/processor/processor_loopclosure_base.h"
-
-namespace wolf
-{
-
-ProcessorLoopClosureBase::ProcessorLoopClosureBase(const std::string& _type, ProcessorParamsLoopClosurePtr _params_loop_closure):
- ProcessorBase(_type, _params_loop_closure),
- params_loop_closure_(_params_loop_closure)
-{
- //
-}
-
-//##############################################################################
-void ProcessorLoopClosureBase::process(CaptureBasePtr _incoming_ptr)
-{
- // clear all previous data from vector
- loop_closure_candidates.clear();
- close_candidates.clear();
-
- // the pre-process, if necessary, is implemented in the derived classes
- preProcess();
-
- findCandidates(_incoming_ptr);
-
- confirmLoopClosure();
-
- WOLF_DEBUG("ProcessorLoopClosureBase::process found ",
- loop_closure_candidates.size(), " candidates found.");
-
- // the post-process, if necessary, is implemented in the derived classes
- postProcess();
-}
-
-//##############################################################################
-void ProcessorLoopClosureBase::keyFrameCallback(FrameBasePtr /*_keyframe_ptr*/,
- const Scalar& /*_time_tol_other*/)
-{
- //
-}
-
-}// namespace wolf
-
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 2bdc605a30dc5f70f4028c6c5007f8b731b48467..f4d96771cf0b3b1c7d8be66202d129296bb02daf 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -178,9 +178,9 @@ target_link_libraries(gtest_map_yaml ${PROJECT_NAME})
wolf_add_gtest(gtest_param_prior gtest_param_prior.cpp)
target_link_libraries(gtest_param_prior ${PROJECT_NAME})
-# ProcessorFrameNearestNeighborFilter class test
-wolf_add_gtest(gtest_processor_frame_nearest_neighbor_filter_2D gtest_processor_frame_nearest_neighbor_filter_2D.cpp)
-target_link_libraries(gtest_processor_frame_nearest_neighbor_filter_2D ${PROJECT_NAME})
+# ProcessorLoopClosureBase class test
+wolf_add_gtest(gtest_processor_loopclosure gtest_processor_loopclosure.cpp)
+target_link_libraries(gtest_processor_loopclosure ${PROJECT_NAME})
# ProcessorMotion in 2D
wolf_add_gtest(gtest_odom_2D gtest_odom_2D.cpp)
diff --git a/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp b/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp
deleted file mode 100644
index 82b3d2ae02e1e1821b9dca28b35792f6855a8f5e..0000000000000000000000000000000000000000
--- a/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp
+++ /dev/null
@@ -1,228 +0,0 @@
-
-/**
- * \file gtest_processor_frame_nearest_neighbor_filter.cpp
- *
- * Created on: Aug 2, 2017
- * \author: tessajohanna
- */
-
-#include "core/utils/utils_gtest.h"
-#include "core/utils/logging.h"
-
-#include "core/sensor/sensor_odom_2D.h"
-#include "core/processor/processor_frame_nearest_neighbor_filter.h"
-
-#include <iostream>
-
-// Dummy class so that it is no pur virtual
-struct DummyLoopCloser : public wolf::ProcessorFrameNearestNeighborFilter
-{
- DummyLoopCloser(ParamsPtr _params) :
- wolf::ProcessorFrameNearestNeighborFilter(_params) { }
-
- bool confirmLoopClosure() override { return false; }
- bool voteForKeyFrame() override { return false; }
-};
-
-// Declare Wolf problem
-wolf::ProblemPtr problem = wolf::Problem::create("PO", 2);
-
-// Declare Sensor
-Eigen::Vector3s odom_extrinsics = Eigen::Vector3s(0,0,0);
-
-std::shared_ptr<wolf::IntrinsicsOdom2D> odom_intrinsics =
- std::make_shared<wolf::IntrinsicsOdom2D>(wolf::IntrinsicsOdom2D());
-
-wolf::SensorBasePtr sensor_ptr;
-
-// Declare Processors
-wolf::ProcessorFrameNearestNeighborFilterPtr processor_ptr_point2d;
-wolf::ProcessorParamsFrameNearestNeighborFilterPtr processor_params_point2d =
- std::make_shared<wolf::ProcessorParamsFrameNearestNeighborFilter>();
-
-wolf::ProcessorFrameNearestNeighborFilterPtr processor_ptr_ellipse2d;
-wolf::ProcessorParamsFrameNearestNeighborFilterPtr processor_params_ellipse2d =
- std::make_shared<wolf::ProcessorParamsFrameNearestNeighborFilter>();
-
-// Declare Frame Pointer
-wolf::StateBlockPtr stateblock_ptr1, stateblock_ptr2, stateblock_ptr3, stateblock_ptr4;
-wolf::FrameBasePtr F1, F2, F3, F4;
-wolf::CaptureBasePtr capture_dummy, incomming_dummy;
-
-//##############################################################################
-TEST(ProcessorFrameNearestNeighborFilter, PointInEllipseRotated)
-{
- // four different states [x, y]
- Eigen::Vector3s state1, state2, state3, state4;
- state1 << 3.0, 5.0, 0.0;
- state2 << 3.0, 6.0, 0.0;
- state3 << 3.0, 7.0, 0.0;
- state4 << 100.0, 100.0, 0.0;
-
-
- // create Keyframes
- F1 = problem->emplaceFrame(wolf::KEY, state1, 1);
- F2 = problem->emplaceFrame(wolf::KEY, state2, 2);
- F3 = problem->emplaceFrame(wolf::KEY, state3, 3);
- F4 = problem->emplaceFrame(wolf::KEY, state4, 4);
-
- auto stateblock_pptr1 = F1->getP();
- auto stateblock_optr1 = F1->getO();
-
- auto stateblock_pptr2 = F2->getP();
- auto stateblock_optr2 = F2->getO();
-
- auto stateblock_pptr3 = F3->getP();
- auto stateblock_optr3 = F3->getO();
-
- auto stateblock_pptr4 = F4->getP();
- auto stateblock_optr4 = F4->getO();
-
- // add dummy capture
- wolf::CaptureBase::emplace<wolf::CaptureBase>(F1, "DUMMY", 1.0, sensor_ptr);
- wolf::CaptureBase::emplace<wolf::CaptureBase>(F2, "DUMMY", 1.0, sensor_ptr);
- wolf::CaptureBase::emplace<wolf::CaptureBase>(F3, "DUMMY", 1.0, sensor_ptr);
- wolf::CaptureBase::emplace<wolf::CaptureBase>(F4, "DUMMY", 1.0, sensor_ptr);
-
- // capture_dummy = std::make_shared<wolf::CaptureBase>("DUMMY",
- // 1.0,
- // sensor_ptr);
- // F1->addCapture(capture_dummy);
- // F2->addCapture(capture_dummy);
- // F3->addCapture(capture_dummy);
- // F4->addCapture(capture_dummy);
-
- // Add same covariances for all states
- Eigen::Matrix2s position_covariance_matrix;
- position_covariance_matrix << 9.0, 0.0,
- 0.0, 5.0;
-
- Eigen::Matrix1s orientation_covariance_matrix;
- orientation_covariance_matrix << 0.01;
-
- Eigen::Vector2s tt_covariance_matrix;
- tt_covariance_matrix << 0.0, 0.0;
-
- problem->addCovarianceBlock( stateblock_pptr1, stateblock_pptr1,
- position_covariance_matrix);
- problem->addCovarianceBlock( stateblock_optr1, stateblock_optr1,
- orientation_covariance_matrix);
- problem->addCovarianceBlock( stateblock_pptr1, stateblock_optr1,
- tt_covariance_matrix);
-
- problem->addCovarianceBlock( stateblock_pptr2, stateblock_pptr2,
- position_covariance_matrix);
- problem->addCovarianceBlock( stateblock_optr2, stateblock_optr2,
- orientation_covariance_matrix);
- problem->addCovarianceBlock( stateblock_pptr2, stateblock_optr2,
- tt_covariance_matrix);
-
- problem->addCovarianceBlock( stateblock_pptr3, stateblock_pptr3,
- position_covariance_matrix);
- problem->addCovarianceBlock( stateblock_optr3, stateblock_optr3,
- orientation_covariance_matrix);
- problem->addCovarianceBlock( stateblock_pptr3, stateblock_optr3,
- tt_covariance_matrix);
-
- problem->addCovarianceBlock( stateblock_pptr4, stateblock_pptr4,
- position_covariance_matrix);
- problem->addCovarianceBlock( stateblock_optr4, stateblock_optr4,
- orientation_covariance_matrix);
- problem->addCovarianceBlock( stateblock_pptr4, stateblock_optr4,
- tt_covariance_matrix);
- // create dummy capture
- incomming_dummy = wolf::CaptureBase::emplace<wolf::CaptureBase>(nullptr, "DUMMY", 1.2, sensor_ptr);
-
- // Make 3rd frame last Key frame
- F3->setTimeStamp(wolf::TimeStamp(2.0));
- problem->getTrajectory()->sortFrame(F3);
-
- // trigger search for loopclosure
- processor_ptr_point2d->process(incomming_dummy);
-
- // const std::vector<wolf::FrameBasePtr> &testloops =
- // processor_ptr_point2d->getCandidates();
-
- //TODO: Due to changes in the emplace refactor these tests are not working. To be fixed.
- // ASSERT_EQ(testloops.size(), (unsigned int) 1);
- // ASSERT_EQ(testloops[0]->id(), (unsigned int) 2);
-}
-
-//##############################################################################
-TEST(ProcessorFrameNearestNeighborFilter, EllipseEllipseRotatedCrosscovariance)
-{
- // set covariance to a -90 degree turned ellipse
- Eigen::Matrix2s position_covariance_matrix;
-// position_covariance_matrix << 9.0, 1.8,
-// 1.8, 5.0;
-
- position_covariance_matrix << 5.0, 0.0,
- 0.0, 9.0;
-
- problem->addCovarianceBlock( F1->getP(), F1->getP(),
- position_covariance_matrix);
- problem->addCovarianceBlock( F2->getP(), F2->getP(),
- position_covariance_matrix);
- problem->addCovarianceBlock( F3->getP(), F3->getP(),
- position_covariance_matrix);
- problem->addCovarianceBlock( F4->getP(), F4->getP(),
- position_covariance_matrix);
-
- // Make 3rd frame last Key frame
- F3->setTimeStamp(wolf::TimeStamp(2.0));
- problem->getTrajectory()->sortFrame(F3);
-
- // trigger search for loopclosure
- processor_ptr_ellipse2d->process(incomming_dummy);
- const std::vector<wolf::FrameBasePtr> &testloops =
- processor_ptr_ellipse2d->getCandidates();
-
- //TODO: Due to changes in the emplace refactor these tests are not working. To be fixed.
- // ASSERT_EQ(testloops.size(), (unsigned int) 2);
- // ASSERT_EQ(testloops[0]->id(), (unsigned int) 1);
- // ASSERT_EQ(testloops[1]->id(), (unsigned int) 2);
-
- // Make 4th frame last Key frame
- F4->setTimeStamp(wolf::TimeStamp(3.0));
- problem->getTrajectory()->sortFrame(F4);
-
- // trigger search for loopclosure again
- processor_ptr_ellipse2d->process(incomming_dummy);
- ASSERT_EQ(testloops.size(), (unsigned int) 0);
-}
-
-//##############################################################################
-int main(int argc, char **argv)
-{
- // SENSOR PARAMETERS
- odom_intrinsics->k_disp_to_disp = 0.2;
- odom_intrinsics->k_rot_to_rot = 0.2;
-
- // install sensor
- sensor_ptr = problem->installSensor("ODOM 2D", "odom",
- odom_extrinsics, odom_intrinsics);
-
- // install the processors
- processor_params_point2d->probability_ = 0.95;
- processor_params_point2d->buffer_size_ = 0; // just exclude identical frameIDs
- processor_params_point2d->distance_type_ = wolf::LoopclosureDistanceType::LC_POINT_ELLIPSE;
-
- // processor_ptr_point2d = std::make_shared<DummyLoopCloser>(processor_params_point2d);
- processor_ptr_point2d = std::static_pointer_cast<wolf::ProcessorFrameNearestNeighborFilter>(wolf::ProcessorBase::emplace<DummyLoopCloser>(sensor_ptr, processor_params_point2d));
- processor_ptr_point2d->setName("processor2Dpoint");
-
- // sensor_ptr->addProcessor(processor_ptr_point2d);
-
- processor_params_ellipse2d->probability_ = 0.95;
- processor_params_ellipse2d->buffer_size_ = 0; // just exclude identical frameIDs
- processor_params_ellipse2d->distance_type_ = wolf::LoopclosureDistanceType::LC_ELLIPSE_ELLIPSE;
-
- // processor_ptr_ellipse2d = std::make_shared<DummyLoopCloser>(processor_params_ellipse2d);
- processor_ptr_ellipse2d = std::static_pointer_cast<wolf::ProcessorFrameNearestNeighborFilter>(wolf::ProcessorBase::emplace<DummyLoopCloser>(sensor_ptr, processor_params_ellipse2d));
- processor_ptr_ellipse2d->setName("processor2Dellipse");
-
- // sensor_ptr->addProcessor(processor_ptr_ellipse2d);
-
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
diff --git a/test/gtest_processor_loopclosure.cpp b/test/gtest_processor_loopclosure.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f2c99aac536fb2265ad100f00f41def2aa2abeae
--- /dev/null
+++ b/test/gtest_processor_loopclosure.cpp
@@ -0,0 +1,101 @@
+
+#include "core/utils/utils_gtest.h"
+#include "core/problem/problem.h"
+#include "core/capture/capture_void.h"
+
+#include "core/processor/processor_loopclosure.h"
+
+
+// STL
+#include <iterator>
+#include <iostream>
+
+using namespace wolf;
+using namespace Eigen;
+
+
+WOLF_PTR_TYPEDEFS(ProcessorLoopClosureDummy);
+
+// dummy class:
+class ProcessorLoopClosureDummy : public ProcessorLoopClosure
+{
+private:
+ bool* factor_created;
+
+public:
+ ProcessorLoopClosureDummy(ProcessorParamsLoopClosurePtr _params_loop_closure, bool& factor_created):
+ ProcessorLoopClosure("LOOP CLOSURE DUMMY", _params_loop_closure),
+ factor_created(&factor_created){};
+ std::pair<FrameBasePtr,CaptureBasePtr> public_selectPairKC(){ return selectPairKC();};
+
+protected:
+ bool voteComputeFeatures() { return true;};
+ bool voteSearchLoopClosure() { return true;};
+ bool detectFeatures(CaptureBasePtr cap) { return true;};
+ CaptureBasePtr findLoopCandidate(CaptureBasePtr _capture) {
+ for (FrameBasePtr kf : getProblem()->getTrajectory()->getFrameList()) {
+ if (kf->isKey()) {
+ for (CaptureBasePtr cap : kf->getCaptureList()) {
+ return cap; }
+ };} return nullptr;
+ };
+ void emplaceFactors(CaptureBasePtr _capture_1, CaptureBasePtr _capture_2) {
+ std::cout << "factor created\n";
+ *factor_created = true;
+ };
+};
+
+
+
+TEST(ProcessorLoopClosure, installProcessor)
+{
+ using namespace wolf;
+ using std::shared_ptr;
+ using std::make_shared;
+ using std::static_pointer_cast;
+ using Eigen::Vector2s;
+
+ bool factor_created = false;
+
+
+ Scalar dt = 0.01;
+
+ // Wolf problem
+ ProblemPtr problem = Problem::create("PO", 2);
+
+ // Install tracker (sensor and processor)
+ auto sens_lc = SensorBase::emplace<SensorBase>(problem->getHardware(),
+ "SENSOR BASE",
+ std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)),
+ std::make_shared<StateBlock>(Eigen::VectorXs::Zero(1)),
+ std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), 2);
+ ProcessorParamsLoopClosurePtr params = std::make_shared<ProcessorParamsLoopClosure>();
+ ProcessorLoopClosureDummyPtr proc_lc = std::make_shared<ProcessorLoopClosureDummy>(params, factor_created);
+ proc_lc->link(sens_lc);
+ std::cout << "sensor & processor created and added to wolf problem" << std::endl;
+
+ // initialize
+ TimeStamp t(0.0);
+ Vector3s x(0,0,0);
+ Matrix3s P = Matrix3s::Identity() * 0.1;
+ problem->setPrior(x, P, t, dt/2); // KF1
+
+ // new KF
+ t += dt;
+ FrameBasePtr kf = FrameBase::create_PO_2D (KEY, t, x); //KF2
+ proc_lc->keyFrameCallback(kf, dt/2);
+
+ // new capture
+ CaptureVoidPtr capt_lc(make_shared<CaptureVoid>(t, sens_lc));
+ proc_lc->captureCallback(capt_lc);
+
+ proc_lc->process(capt_lc);
+
+ ASSERT_TRUE(factor_created);
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}