diff --git a/CMakeLists.txt b/CMakeLists.txt
index 51eb17354b1a4a5bab11f89924d8ee63c47bcc67..c6c2db62aa3d682dd1c78bdfbb696b837fa07120 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -248,7 +248,7 @@ SET(HDRS_PROCESSOR
include/core/processor/processor_diff_drive.h
include/core/processor/factory_processor.h
include/core/processor/processor_logging.h
- include/core/processor/processor_loopclosure.h
+ include/core/processor/processor_loop_closure.h
include/core/processor/processor_motion.h
include/core/processor/processor_odom_2d.h
include/core/processor/processor_odom_3d.h
@@ -345,7 +345,7 @@ SET(SRCS_PROCESSOR
src/processor/motion_buffer.cpp
src/processor/processor_base.cpp
src/processor/processor_diff_drive.cpp
- src/processor/processor_loopclosure.cpp
+ src/processor/processor_loop_closure.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_loop_closure.h b/include/core/processor/processor_loop_closure.h
new file mode 100644
index 0000000000000000000000000000000000000000..bb0c9f17041de6cf80919b046c6a5218eaf05518
--- /dev/null
+++ b/include/core/processor/processor_loop_closure.h
@@ -0,0 +1,110 @@
+#ifndef _WOLF_PROCESSOR_LOOP_CLOSURE_BASE_H
+#define _WOLF_PROCESSOR_LOOP_CLOSURE_BASE_H
+
+// Wolf related headers
+#include "core/processor/processor_base.h"
+
+namespace wolf{
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorLoopClosure);
+
+struct ParamsProcessorLoopClosure : public ParamsProcessorBase
+{
+ int max_loops=-1;
+
+ ParamsProcessorLoopClosure() = default;
+ ParamsProcessorLoopClosure(std::string _unique_name, const ParamsServer& _server):
+ ParamsProcessorBase(_unique_name, _server)
+ {
+ max_loops = _server.getParam<int>(prefix + _unique_name + "/max_loops");
+ }
+
+ std::string print() const override
+ {
+ return "\n" + ParamsProcessorBase::print()
+ + "max_loops: " + std::to_string(max_loops) + "\n";
+ }
+};
+
+WOLF_STRUCT_PTR_TYPEDEFS(MatchLoopClosure);
+
+/** \brief Match between a capture and a capture
+ *
+ * Match between a capture and a capture (capture-capture correspondence)
+ *
+ */
+struct MatchLoopClosure
+{
+ CaptureBasePtr capture_reference_ptr_; ///< Capture reference
+ CaptureBasePtr capture_target_ptr_; ///< Capture target
+ double normalized_score_; ///< normalized similarity score (0 is bad, 1 is good)
+};
+
+/** \brief General loop closure processor
+ *
+ * This is an abstract class.
+ * + You must define the following classes :
+ * - voteFindLoopClosures(CaptureBasePtr)
+ * - emplaceFeatures(CaptureBasePtr)
+ * - findLoopClosures(CaptureBasePtr)
+ * - validateLoop(CaptureBasePtr, CaptureBasePtr)
+ * - emplaceFactors(CaptureBasePtr, CaptureBasePtr)
+ * + You can override the following classes :
+ * - process(CaptureBasePtr)
+ */
+class ProcessorLoopClosure : public ProcessorBase
+{
+protected:
+
+ ParamsProcessorLoopClosurePtr params_loop_closure_;
+
+public:
+
+ ProcessorLoopClosure(const std::string& _type, int _dim, ParamsProcessorLoopClosurePtr _params_loop_closure);
+
+ ~ProcessorLoopClosure() override = default;
+ void configure(SensorBasePtr _sensor) override { };
+
+protected:
+
+ /** \brief Process a capture (linked to a frame)
+ * If voteFindLoopClosures() returns true, findLoopClosures() is called.
+ * emplaceFactors() is called for pairs of current capture and each capture returned by findLoopClosures()
+ */
+ virtual void process(CaptureBasePtr);
+
+ /** \brief Returns if findLoopClosures() has to be called for the given capture
+ */
+ virtual bool voteFindLoopClosures(CaptureBasePtr cap) = 0;
+
+ /** \brief detects and emplaces all features of the given capture
+ */
+ virtual void emplaceFeatures(CaptureBasePtr cap) = 0;
+
+ /** \brief Find captures that correspond to loop closures with the given capture
+ */
+ virtual std::map<double,MatchLoopClosurePtr> findLoopClosures(CaptureBasePtr _capture) = 0;
+
+ /** \brief validates a loop closure
+ */
+ virtual bool validateLoopClosure(MatchLoopClosurePtr) = 0;
+
+ /** \brief emplaces the factor(s) corresponding to a Loop Closure between two captures
+ */
+ virtual void emplaceFactors(MatchLoopClosurePtr) = 0;
+
+ void processCapture(CaptureBasePtr) override;
+ void processKeyFrame(FrameBasePtr, const double&) override;
+
+ bool triggerInCapture(CaptureBasePtr _cap) const override { return true;};
+ bool triggerInKeyFrame(FrameBasePtr _frm, const double& _time_tol) const override { return true;};
+
+ bool storeKeyFrame(FrameBasePtr _frm) override { return false;};
+ bool storeCapture(CaptureBasePtr _cap) override { return false;};
+
+ bool voteForKeyFrame() const override { return false;};
+};
+
+} // namespace wolf
+
+#endif /* _WOLF_PROCESSOR_LOOP_CLOSURE_BASE_H */
diff --git a/include/core/processor/processor_loopclosure.h b/include/core/processor/processor_loopclosure.h
deleted file mode 100644
index 12e1fd8781627154315b4815a86105e1d404e86f..0000000000000000000000000000000000000000
--- a/include/core/processor/processor_loopclosure.h
+++ /dev/null
@@ -1,183 +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(ParamsProcessorLoopClosure);
-
-struct ParamsProcessorLoopClosure : public ParamsProcessorBase
-{
- using ParamsProcessorBase::ParamsProcessorBase;
- // virtual ~ParamsProcessorLoopClosure() = 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:
-
- ParamsProcessorLoopClosurePtr params_loop_closure_;
-
-public:
-
- ProcessorLoopClosure(const std::string& _type, int _dim, ParamsProcessorLoopClosurePtr _params_loop_closure);
-
- ~ProcessorLoopClosure() override = default;
- void configure(SensorBasePtr _sensor) override { };
-
-protected:
- /** \brief process an incoming capture
- *
- * The ProcessorLoopClosure is only triggered in KF (see triggerInCapture()) so this function is not called.
- */
- void processCapture(CaptureBasePtr) override {};
-
- /** \brief process an incoming key-frame
- *
- * Each derived processor should implement this function. It will be called if:
- * - A new KF arrived and triggerInKF() returned true.
- */
- void processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance) override;
-
- /** \brief trigger in capture
- *
- * The ProcessorLoopClosure only processes incoming KF, then it returns false.
- */
- bool triggerInCapture(CaptureBasePtr) const override {return false;}
-
- /** \brief trigger in key-frame
- *
- * Returns true if processKeyFrame() should be called after the provided KF arrived.
- */
- bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tol_other) const override;
-
- /** \brief store key frame
- *
- * Returns true if the key frame should be stored
- */
- bool storeKeyFrame(FrameBasePtr) override;
-
- /** \brief store capture
- *
- * Returns true if the capture should be stored
- */
- bool storeCapture(CaptureBasePtr) override;
-
- /** \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() const override
- {
- return false;
- };
-};
-
-} // namespace wolf
-
-#endif /* _WOLF_PROCESSOR_LOOPCLOSURE_BASE_H */
diff --git a/src/processor/processor_loop_closure.cpp b/src/processor/processor_loop_closure.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..486be875ac6eb306e6e0a7973742b7f3e52241c7
--- /dev/null
+++ b/src/processor/processor_loop_closure.cpp
@@ -0,0 +1,154 @@
+#include "core/processor/processor_loop_closure.h"
+
+namespace wolf
+{
+
+ProcessorLoopClosure::ProcessorLoopClosure(const std::string& _type,
+ int _dim,
+ ParamsProcessorLoopClosurePtr _params_loop_closure):
+ ProcessorBase(_type, _dim, _params_loop_closure),
+ params_loop_closure_(_params_loop_closure)
+{
+ //
+}
+
+void ProcessorLoopClosure::processCapture(CaptureBasePtr _capture)
+{
+ /* This function has 3 scenarios:
+ * 1. Capture already linked to a frame (in trajectory) -> process
+ * 2. Capture has a timestamp compatible with any stored frame -> link + process
+ * 3. Otherwise -> store capture (Note that more than one processor can be emplacing frames, so an older frame can arrive later than this one)
+ */
+
+ WOLF_DEBUG("ProcessorLoopClosure::processCapture capture ", _capture->id());
+
+ // CASE 1:
+ if (_capture->getFrame() and _capture->getFrame()->getTrajectory())
+ {
+ WOLF_DEBUG("CASE 1");
+
+ process(_capture);
+
+ // remove the frame and older frames
+ buffer_pack_kf_.removeUpTo(_capture->getFrame()->getTimeStamp());
+
+ return;
+ }
+
+ // Search for any stored frame within time tolerance of capture
+ auto frame_pack = buffer_pack_kf_.select(_capture->getTimeStamp(), params_->time_tolerance);
+
+ // CASE 2:
+ if (_capture->getFrame() == nullptr and frame_pack)
+ {
+ WOLF_DEBUG("CASE 2");
+
+ _capture->link(frame_pack->key_frame);
+
+ process(_capture);
+
+ // remove the frame and older frames
+ buffer_pack_kf_.removeUpTo(frame_pack->key_frame->getTimeStamp());
+
+ return;
+ }
+ // CASE 3:
+ WOLF_DEBUG("CASE 3");
+ buffer_capture_.add(_capture->getTimeStamp(), _capture);
+}
+
+void ProcessorLoopClosure::processKeyFrame(FrameBasePtr _frame, const double& _time_tolerance)
+{
+ /* This function has 4 scenarios:
+ * 1. Frame already have a capture of the sensor -> process
+ * 2. Frame has a timestamp within time tolerances of any stored capture -> link + process
+ * 3. Frame is more recent than any stored capture -> store frame to be processed later in processCapture
+ * 4. Otherwise: The frame is not compatible with any stored capture -> discard frame
+ */
+
+ WOLF_DEBUG("ProcessorLoopClosure::processKeyFrame frame ", _frame->id());
+
+ // CASE 1:
+ auto cap = _frame->getCaptureOf(getSensor());
+ if (cap)
+ {
+ WOLF_DEBUG("CASE 1");
+
+ process(cap);
+
+ // remove the capture (if stored)
+ buffer_capture_.getContainer().erase(cap->getTimeStamp());
+
+ return;
+ }
+
+ // Search for any stored capture within time tolerance of frame
+ auto capture = buffer_capture_.select(_frame->getTimeStamp(), params_->time_tolerance);
+
+ // CASE 2:
+ if (capture and not capture->getFrame())
+ {
+ WOLF_DEBUG("CASE 2");
+
+ capture->link(_frame);
+
+ process(capture);
+
+ // remove the capture (if stored)
+ buffer_capture_.getContainer().erase(capture->getTimeStamp());
+
+ // remove old captures (10s of old captures are kept in case frames arrives unordered)
+ buffer_capture_.removeUpTo(_frame->getTimeStamp() - 10);
+
+ return;
+ }
+ // CASE 3:
+ if (buffer_capture_.selectLastAfter(_frame->getTimeStamp(), params_->time_tolerance) == nullptr)
+ {
+ WOLF_DEBUG("CASE 3");
+
+ // store frame
+ buffer_pack_kf_.add(_frame, _time_tolerance);
+
+ return;
+ }
+ // CASE 4:
+ WOLF_DEBUG("CASE 4");
+ // nothing (discard frame)
+}
+
+void ProcessorLoopClosure::process(CaptureBasePtr _capture)
+{
+ assert(_capture->getFrame() != nullptr && "ProcessorLoopClosure::process _capture not linked to _frame");
+ WOLF_DEBUG("ProcessorLoopClosure::process frame ", _capture->getFrame()->id(), " capture ", _capture->id());
+
+ // Detect and emplace features
+ WOLF_DEBUG("emplacing features...");
+ emplaceFeatures(_capture);
+
+ // Vote for loop closure search
+ if (voteFindLoopClosures(_capture))
+ {
+ WOLF_DEBUG("finding loop closures...");
+
+ // Find loop closures
+ auto match_lc_map = findLoopClosures(_capture);
+
+ WOLF_DEBUG(match_lc_map.size(), " loop closures found");
+
+ // Emplace factors for each LC if validated
+ auto n_loops = 0;
+ for (const auto& match_pair : match_lc_map)
+ if (validateLoopClosure(match_pair.second))
+ {
+ emplaceFactors(match_pair.second);
+ n_loops++;
+
+ if (params_loop_closure_->max_loops > 0 and
+ n_loops >= params_loop_closure_->max_loops)
+ break;
+ }
+ }
+}
+
+}// namespace wolf
diff --git a/src/processor/processor_loopclosure.cpp b/src/processor/processor_loopclosure.cpp
deleted file mode 100644
index 16e037b1c7634c4d0d7509fea51e075fbc8d0066..0000000000000000000000000000000000000000
--- a/src/processor/processor_loopclosure.cpp
+++ /dev/null
@@ -1,119 +0,0 @@
-/**
- * \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,
- int _dim,
- ParamsProcessorLoopClosurePtr _params_loop_closure):
- ProcessorBase(_type, _dim, _params_loop_closure),
- params_loop_closure_(_params_loop_closure)
-{
- //
-}
-
-//##############################################################################
-void ProcessorLoopClosure::processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance)
-{
- // 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))
- 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();
-}
-
-bool ProcessorLoopClosure::triggerInKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tol_other) const
-{
- return true;
-}
-bool ProcessorLoopClosure::storeKeyFrame(FrameBasePtr _frame_ptr)
-{
- return true;
-}
-bool ProcessorLoopClosure::storeCapture(CaptureBasePtr _cap_ptr)
-{
- return true;
-}
-
-/**
- * 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()
-{
- auto 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/test/CMakeLists.txt b/test/CMakeLists.txt
index be33df04ad6dc395ec55b63b9de14a4ea73ff904..4e03c1a2a284f02b2fe26869ad656e5c959a450a 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -233,9 +233,9 @@ target_link_libraries(gtest_param_prior ${PLUGIN_NAME})
wolf_add_gtest(gtest_processor_diff_drive gtest_processor_diff_drive.cpp)
target_link_libraries(gtest_processor_diff_drive ${PLUGIN_NAME})
-# ProcessorLoopClosureBase class test
-wolf_add_gtest(gtest_processor_loopclosure gtest_processor_loopclosure.cpp)
-target_link_libraries(gtest_processor_loopclosure ${PLUGIN_NAME})
+# ProcessorLoopClosure class test
+wolf_add_gtest(gtest_processor_loop_closure gtest_processor_loop_closure.cpp)
+target_link_libraries(gtest_processor_loop_closure ${PLUGIN_NAME})
# ProcessorFrameNearestNeighborFilter class test
# wolf_add_gtest(gtest_processor_frame_nearest_neighbor_filter_2d gtest_processor_frame_nearest_neighbor_filter_2d.cpp)
diff --git a/test/dummy/processor_loop_closure_dummy.h b/test/dummy/processor_loop_closure_dummy.h
new file mode 100644
index 0000000000000000000000000000000000000000..a391ad9bd593b164b5bfbb0607295e72763852e4
--- /dev/null
+++ b/test/dummy/processor_loop_closure_dummy.h
@@ -0,0 +1,93 @@
+#ifndef TEST_DUMMY_PROCESSOR_LOOP_CLOSURE_DUMMY_H_
+#define TEST_DUMMY_PROCESSOR_LOOP_CLOSURE_DUMMY_H_
+
+#include "core/processor/processor_loop_closure.h"
+
+using namespace wolf;
+using namespace Eigen;
+
+WOLF_PTR_TYPEDEFS(ProcessorLoopClosureDummy);
+
+// dummy class:
+class ProcessorLoopClosureDummy : public ProcessorLoopClosure
+{
+ public:
+ ProcessorLoopClosureDummy(ParamsProcessorLoopClosurePtr _params) :
+ ProcessorLoopClosure("ProcessorLoopClosureDummy", 2, _params)
+ {
+ }
+
+ protected:
+ bool voteFindLoopClosures(CaptureBasePtr cap) override { return true;};
+ bool validateLoopClosure(MatchLoopClosurePtr match) override { return true;};
+
+ void emplaceFeatures(CaptureBasePtr cap) override
+ {
+ // feature = frame pose
+ FeatureBase::emplace<FeatureBase>(cap,
+ "FeatureLoopClosureDummy",
+ cap->getFrame()->getState().vector("PO"),
+ MatrixXd::Identity(3,3));
+ }
+
+ std::map<double,MatchLoopClosurePtr> findLoopClosures(CaptureBasePtr _capture) override
+ {
+ std::map<double,MatchLoopClosurePtr> match_lc_map;
+
+ auto old_frame = _capture->getFrame()->getPreviousFrame();
+ while (old_frame)
+ {
+ // match if features (frames psoe) are close enough
+ for (auto cap : old_frame->getCaptureList())
+ for (auto feat : cap->getFeatureList())
+ if (feat->getType() == "FeatureLoopClosureDummy" and
+ (feat->getMeasurement() - _capture->getFeatureList().front()->getMeasurement()).norm() < 1e-3)
+ {
+ MatchLoopClosurePtr match = std::make_shared<MatchLoopClosure>();
+ match->capture_reference_ptr_ = cap;
+ match->capture_target_ptr_ = _capture;
+ match->normalized_score_ = 1;
+
+ while (match_lc_map.count(match->normalized_score_))
+ match->normalized_score_ -= 1e-9;
+
+ match_lc_map.emplace(match->normalized_score_, match);
+ }
+
+ old_frame = old_frame->getPreviousFrame();
+ }
+
+ return match_lc_map;
+ }
+
+ void emplaceFactors(MatchLoopClosurePtr match) override
+ {
+ FeatureBasePtr feat_2;
+ for (auto feat : match->capture_target_ptr_->getFeatureList())
+ if (feat->getType() == "FeatureLoopClosureDummy")
+ {
+ feat_2 = feat;
+ break;
+ }
+
+ FactorBase::emplace<FactorRelativePose2d>(feat_2, feat_2,
+ match->capture_reference_ptr_->getFrame(),
+ shared_from_this(),
+ false,
+ TOP_LOOP);
+ }
+
+ public:
+ unsigned int getNStoredFrames()
+ {
+ return buffer_pack_kf_.getContainer().size();
+ }
+
+ unsigned int getNStoredCaptures()
+ {
+ return buffer_capture_.getContainer().size();
+ }
+};
+
+
+#endif /* TEST_DUMMY_PROCESSOR_LOOP_CLOSURE_DUMMY_H_ */
diff --git a/test/gtest_processor_loop_closure.cpp b/test/gtest_processor_loop_closure.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..aec8092de22eaee6a4577b43d8b7a10f8528582b
--- /dev/null
+++ b/test/gtest_processor_loop_closure.cpp
@@ -0,0 +1,327 @@
+
+#include "core/utils/utils_gtest.h"
+#include "core/problem/problem.h"
+#include "core/capture/capture_base.h"
+#include "core/factor/factor_relative_pose_2d.h"
+
+#include "dummy/processor_loop_closure_dummy.h"
+
+// STL
+#include <iterator>
+#include <iostream>
+
+using namespace wolf;
+using namespace Eigen;
+
+class ProcessorLoopClosureTest : public testing::Test
+{
+ protected:
+ // Wolf problem
+ ProblemPtr problem = Problem::create("PO", 2);
+ SensorBasePtr sensor;
+ ProcessorLoopClosureDummyPtr processor;
+
+ virtual void SetUp()
+ {
+ // Emplace sensor
+ sensor = SensorBase::emplace<SensorBase>(problem->getHardware(),
+ "SensorBase",
+ std::make_shared<StateBlock>(Vector2d::Zero()),
+ std::make_shared<StateBlock>(Vector1d::Zero()),
+ nullptr,
+ 2);
+
+ // Emplace processor
+ ParamsProcessorLoopClosurePtr params = std::make_shared<ParamsProcessorLoopClosure>();
+ params->time_tolerance = 0.5;
+ processor = ProcessorBase::emplace<ProcessorLoopClosureDummy>(sensor,
+ params);
+ }
+
+ FrameBasePtr emplaceFrame(TimeStamp ts, const Vector3d& x)
+ {
+ // new frame
+ return problem->emplaceFrame(ts, x);
+ }
+
+ CaptureBasePtr emplaceCapture(FrameBasePtr frame)
+ {
+ // new capture
+ return CaptureBase::emplace<CaptureBase>(frame,
+ "CaptureBase",
+ frame->getTimeStamp(),
+ sensor);
+ }
+
+ CaptureBasePtr createCapture(TimeStamp ts)
+ {
+ // new capture
+ return std::make_shared<CaptureBase>("CaptureBase",
+ ts,
+ sensor);
+ }
+};
+
+TEST_F(ProcessorLoopClosureTest, installProcessor)
+{
+ EXPECT_EQ(processor->getNStoredFrames(), 0);
+ EXPECT_EQ(processor->getNStoredCaptures(), 0);
+}
+
+TEST_F(ProcessorLoopClosureTest, frame_stored)
+{
+ // new frame
+ auto frm1 = emplaceFrame(1, Vector3d::Zero());
+
+ // keyframecallback
+ problem->keyFrameCallback(frm1, nullptr, 0.5);
+
+ EXPECT_EQ(processor->getNStoredFrames(), 1);
+ EXPECT_EQ(processor->getNStoredCaptures(), 0);
+}
+
+TEST_F(ProcessorLoopClosureTest, capture_stored)
+{
+ // new capture
+ auto cap1 = createCapture(1);
+
+ // captureCallback
+ processor->captureCallback(cap1);
+
+ EXPECT_EQ(processor->getNStoredFrames(), 0);
+ EXPECT_EQ(processor->getNStoredCaptures(), 1);
+}
+
+TEST_F(ProcessorLoopClosureTest, captureCallbackCase1)
+{
+ // emplace frame and capture
+ auto frm1 = emplaceFrame(1, Vector3d::Zero());
+ auto cap1 = emplaceCapture(frm1);
+
+ // captureCallback
+ processor->captureCallback(cap1);
+
+ EXPECT_EQ(cap1->getFeatureList().size(), 1); // capture processed by the processor
+ EXPECT_EQ(processor->getNStoredFrames(), 0);
+ EXPECT_EQ(processor->getNStoredCaptures(), 0);
+}
+
+TEST_F(ProcessorLoopClosureTest, captureCallbackCase2)
+{
+ // new frame
+ auto frm1 = emplaceFrame(1, Vector3d::Zero());
+
+ // new capture
+ auto cap1 = createCapture(1);
+
+ // keyframecallback
+ problem->keyFrameCallback(frm1, nullptr, 0.5);
+
+ // captureCallback
+ processor->captureCallback(cap1);
+
+ EXPECT_EQ(cap1->getFrame(), frm1); // capture processed by the processor
+ EXPECT_EQ(cap1->getFeatureList().size(), 1); // capture processed by the processor
+ EXPECT_EQ(processor->getNStoredFrames(), 0);
+ EXPECT_EQ(processor->getNStoredCaptures(), 0);
+}
+
+TEST_F(ProcessorLoopClosureTest, captureCallbackCase3)
+{
+ // new frame
+ auto frm1 = emplaceFrame(1, Vector3d::Zero());
+ // new capture
+ auto cap1 = createCapture(2);
+
+ // keyframecallback
+ problem->keyFrameCallback(frm1, nullptr, 0.5);
+
+ // captureCallback
+ processor->captureCallback(cap1);
+
+ EXPECT_TRUE(cap1->getFrame() == nullptr);
+ EXPECT_EQ(cap1->getFeatureList().size(), 0);
+ EXPECT_EQ(processor->getNStoredFrames(), 1);
+ EXPECT_EQ(processor->getNStoredCaptures(), 1);
+}
+
+TEST_F(ProcessorLoopClosureTest, keyFrameCallbackCase1)
+{
+ // emplace frame and capture
+ auto frm1 = emplaceFrame(1, Vector3d::Zero());
+ auto cap1 = emplaceCapture(frm1);
+
+ // keyframecallback
+ problem->keyFrameCallback(frm1, nullptr, 0.5);
+
+ EXPECT_EQ(cap1->getFeatureList().size(), 1); // capture processed by the processor
+ EXPECT_EQ(processor->getNStoredFrames(), 0);
+ EXPECT_EQ(processor->getNStoredCaptures(), 0);
+}
+
+TEST_F(ProcessorLoopClosureTest, keyFrameCallbackCase2)
+{
+ // new frame
+ auto frm1 = emplaceFrame(1, Vector3d::Zero());
+ // new capture
+ auto cap1 = createCapture(1);
+
+ // captureCallback
+ processor->captureCallback(cap1);
+
+ // keyframecallback
+ problem->keyFrameCallback(frm1, nullptr, 0.5);
+
+ EXPECT_EQ(cap1->getFrame(), frm1); // capture processed by the processor
+ EXPECT_EQ(cap1->getFeatureList().size(), 1); // capture processed by the processor
+ EXPECT_EQ(processor->getNStoredFrames(), 0);
+ EXPECT_EQ(processor->getNStoredCaptures(), 0);
+}
+
+TEST_F(ProcessorLoopClosureTest, keyFrameCallbackCase3)
+{
+ // new frame
+ auto frm1 = emplaceFrame(2, Vector3d::Zero());
+ // new capture
+ auto cap1 = createCapture(1);
+
+ // captureCallback
+ processor->captureCallback(cap1);
+
+ // keyframecallback
+ problem->keyFrameCallback(frm1, nullptr, 0.5);
+
+ EXPECT_TRUE(cap1->getFrame() == nullptr);
+ EXPECT_EQ(cap1->getFeatureList().size(), 0);
+ EXPECT_EQ(processor->getNStoredFrames(), 1);
+ EXPECT_EQ(processor->getNStoredCaptures(), 1);
+}
+
+TEST_F(ProcessorLoopClosureTest, keyFrameCallbackCase4)
+{
+ // new frame
+ auto frm1 = emplaceFrame(1, Vector3d::Zero());
+ // new capture
+ auto cap1 = createCapture(2);
+
+ // captureCallback
+ processor->captureCallback(cap1);
+
+ // keyframecallback
+ problem->keyFrameCallback(frm1, nullptr, 0.5);
+
+ EXPECT_TRUE(cap1->getFrame() == nullptr);
+ EXPECT_EQ(cap1->getFeatureList().size(), 0);
+ EXPECT_EQ(processor->getNStoredFrames(), 0);
+ EXPECT_EQ(processor->getNStoredCaptures(), 1);
+}
+
+TEST_F(ProcessorLoopClosureTest, captureCallbackMatch)
+{
+ // new frame
+ auto frm1 = emplaceFrame(1, Vector3d::Zero());
+ auto frm2 = emplaceFrame(2, Vector3d::Zero());
+ auto frm3 = emplaceFrame(3, Vector3d::Zero());
+ auto frm4 = emplaceFrame(4, Vector3d::Zero());
+ auto frm5 = emplaceFrame(5, Vector3d::Zero());
+ // new captures
+ auto cap4 = createCapture(4);
+
+ // keyframecallback
+ problem->keyFrameCallback(frm1, nullptr, 0.5);
+ problem->keyFrameCallback(frm2, nullptr, 0.5);
+ problem->keyFrameCallback(frm3, nullptr, 0.5);
+ problem->keyFrameCallback(frm4, nullptr, 0.5);
+ problem->keyFrameCallback(frm5, nullptr, 0.5);
+
+ // captureCallback
+ processor->captureCallback(cap4);
+
+ EXPECT_EQ(frm1->getCaptureList().size(), 0);
+ EXPECT_EQ(frm2->getCaptureList().size(), 0);
+ EXPECT_EQ(frm3->getCaptureList().size(), 0);
+ EXPECT_EQ(frm4->getCaptureList().size(), 1);
+ EXPECT_EQ(frm5->getCaptureList().size(), 0);
+
+ EXPECT_TRUE(cap4->getFrame() == frm4);
+ EXPECT_EQ(cap4->getFeatureList().size(), 1);
+
+ EXPECT_EQ(processor->getNStoredFrames(), 1); // all oldest frames are removed from buffer
+ EXPECT_EQ(processor->getNStoredCaptures(), 0);
+}
+
+TEST_F(ProcessorLoopClosureTest, keyFrameCallbackMatch)
+{
+ // new frame
+ auto frm2 = emplaceFrame(2, Vector3d::Zero());
+ // new captures
+ auto cap1 = createCapture(1);
+ auto cap2 = createCapture(2);
+ auto cap3 = createCapture(3);
+ auto cap4 = createCapture(4);
+ auto cap5 = createCapture(5);
+
+ // captureCallback
+ processor->captureCallback(cap1);
+ processor->captureCallback(cap2);
+ processor->captureCallback(cap3);
+ processor->captureCallback(cap4);
+ processor->captureCallback(cap5);
+
+ // keyframecallback
+ problem->keyFrameCallback(frm2, nullptr, 0.5);
+
+ EXPECT_TRUE(cap1->getFrame() == nullptr);
+ EXPECT_TRUE(cap2->getFrame() == frm2);
+ EXPECT_TRUE(cap3->getFrame() == nullptr);
+ EXPECT_TRUE(cap4->getFrame() == nullptr);
+ EXPECT_TRUE(cap5->getFrame() == nullptr);
+
+ EXPECT_EQ(cap1->getFeatureList().size(), 0);
+ EXPECT_EQ(cap2->getFeatureList().size(), 1);
+ EXPECT_EQ(cap3->getFeatureList().size(), 0);
+ EXPECT_EQ(cap4->getFeatureList().size(), 0);
+ EXPECT_EQ(cap5->getFeatureList().size(), 0);
+
+ EXPECT_EQ(processor->getNStoredFrames(), 0);
+ EXPECT_EQ(processor->getNStoredCaptures(), 4);
+}
+
+TEST_F(ProcessorLoopClosureTest, emplaceFactors)
+{
+ // emplace frame and capture
+ auto cap1 = emplaceCapture(emplaceFrame(1, Vector3d::Zero()));
+ processor->captureCallback(cap1);
+
+ auto cap2 = emplaceCapture(emplaceFrame(2, Vector3d::Ones()));
+ processor->captureCallback(cap2);
+
+ auto cap3 = emplaceCapture(emplaceFrame(3, 2*Vector3d::Ones()));
+ processor->captureCallback(cap3);
+
+ auto cap4 = emplaceCapture(emplaceFrame(4, Vector3d::Zero()));
+ processor->captureCallback(cap4);
+
+ EXPECT_EQ(cap1->getFrame()->getConstrainedByList().size(), 1);
+ EXPECT_EQ(cap2->getFrame()->getConstrainedByList().size(), 0);
+ EXPECT_EQ(cap3->getFrame()->getConstrainedByList().size(), 0);
+ EXPECT_EQ(cap4->getFrame()->getConstrainedByList().size(), 0);
+
+ EXPECT_EQ(cap1->getFeatureList().size(), 1);
+ EXPECT_EQ(cap2->getFeatureList().size(), 1);
+ EXPECT_EQ(cap3->getFeatureList().size(), 1);
+ EXPECT_EQ(cap4->getFeatureList().size(), 1);
+
+ EXPECT_EQ(cap1->getFeatureList().front()->getFactorList().size(), 0);
+ EXPECT_EQ(cap2->getFeatureList().front()->getFactorList().size(), 0);
+ EXPECT_EQ(cap3->getFeatureList().front()->getFactorList().size(), 0);
+ EXPECT_EQ(cap4->getFeatureList().front()->getFactorList().size(), 1);
+
+ EXPECT_EQ(cap1->getFrame()->getConstrainedByList().front(), cap4->getFeatureList().front()->getFactorList().front());
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_processor_loopclosure.cpp b/test/gtest_processor_loopclosure.cpp
deleted file mode 100644
index 4e50c870348da1877b3a599c48158730e2df47ab..0000000000000000000000000000000000000000
--- a/test/gtest_processor_loopclosure.cpp
+++ /dev/null
@@ -1,104 +0,0 @@
-
-#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(ParamsProcessorLoopClosurePtr _params_loop_closure, bool& factor_created):
- ProcessorLoopClosure("LOOP CLOSURE DUMMY", 0, _params_loop_closure),
- factor_created(&factor_created){};
- std::pair<FrameBasePtr,CaptureBasePtr> public_selectPairKC(){ return selectPairKC();};
-
-protected:
- bool voteComputeFeatures() override { return true;};
- bool voteSearchLoopClosure() override { return true;};
- bool detectFeatures(CaptureBasePtr cap) override { return true;};
- CaptureBasePtr findLoopCandidate(CaptureBasePtr _capture) override
- {
- for (FrameBasePtr kf : *getProblem()->getTrajectory())
- for (CaptureBasePtr cap : kf->getCaptureList())
- if (cap != _capture)
- return cap;
- return nullptr;
- };
- void emplaceFactors(CaptureBasePtr _capture_1, CaptureBasePtr _capture_2) override
- {
- 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::Vector2d;
-
- bool factor_created = false;
-
-
- double 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::VectorXd::Zero(2)),
- std::make_shared<StateBlock>(Eigen::VectorXd::Zero(1)),
- std::make_shared<StateBlock>(Eigen::VectorXd::Zero(2)), 2);
- ParamsProcessorLoopClosurePtr params = std::make_shared<ParamsProcessorLoopClosure>();
- auto proc_lc = ProcessorBase::emplace<ProcessorLoopClosureDummy>(sens_lc, params, factor_created);
- std::cout << "sensor & processor created and added to wolf problem" << std::endl;
-
- // initialize
- TimeStamp t(0.0);
- // Vector3d x(0,0,0);
- VectorComposite x(Vector3d(0,0,0), "PO", {2,1});
- // Matrix3d P = Matrix3d::Identity() * 0.1;
- VectorComposite P(Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)), "PO", {2,1});
- problem->setPriorFactor(x, P, t, dt/2); // KF1
-
-
- // new KF
- t += dt;
- auto kf = problem->emplaceFrame(t, x); //KF2
- // emplace a capture in KF
- auto capt_lc = CaptureBase::emplace<CaptureVoid>(kf, t, sens_lc);
- proc_lc->captureCallback(capt_lc);
-
- // callback KF
- proc_lc->keyFrameCallback(kf, dt/2);
-
- ASSERT_TRUE(factor_created);
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}