diff --git a/CMakeLists.txt b/CMakeLists.txt
index b9f3ad7358ad2dad416d6cf0c8923f23d5a90ece..0bcdb9f10bfcfb6d4dc59a279649335f3a25b165 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -247,7 +247,8 @@ 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_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
@@ -343,7 +344,8 @@ 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_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..39813708a7954a94a883b5fbbbdbcedf873f3fc9
--- /dev/null
+++ b/include/core/processor/processor_loop_closure.h
@@ -0,0 +1,88 @@
+#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
+{
+ 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 :
+ * - voteFindLoopClosures(CaptureBasePtr)
+ * - emplaceFeatures(CaptureBasePtr)
+ * - findLoopClosures(CaptureBasePtr)
+ * - validateLoop(CaptureBasePtr, CaptureBasePtr)
+ * - emplaceFactors(CaptureBasePtr, CaptureBasePtr)
+ * + You can override the following classes :
+ * - process()
+ */
+
+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 frame containing a capture.
+ * If voteFindLoopClosures() returns true, findLoopClosures() is called.
+ * emplaceFactors() is called for pairs of current capture and each capture returned by findLoopClosures()
+ */
+ virtual void process(FrameBasePtr, 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 CaptureBasePtrList findLoopClosures(CaptureBasePtr _capture) = 0;
+
+ /** \brief validates a loop closure
+ */
+ virtual bool validateLoopClosure(CaptureBasePtr _capture_1, CaptureBasePtr _capture_2) = 0;
+
+ /** \brief emplaces the factor(s) corresponding to a Loop Closure between two captures
+ */
+ virtual void emplaceFactors(CaptureBasePtr _capture_1, CaptureBasePtr _capture_2) = 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/src/processor/processor_loop_closure.cpp b/src/processor/processor_loop_closure.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..bf70934d9e820fd910ee7de744b6896b261ae1f4
--- /dev/null
+++ b/src/processor/processor_loop_closure.cpp
@@ -0,0 +1,113 @@
+#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 -> 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)
+ */
+
+ // CASE 1:
+ if (_capture->getFrame())
+ {
+ process(_capture->getFrame(), _capture);
+
+ // remove the frame and older frames
+ buffer_pack_kf_.removeUpTo(_capture->getFrame()->getTimeStamp());
+ }
+
+ // 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 (frame_pack)
+ {
+ _capture->link(frame_pack->key_frame);
+
+ process(frame_pack->key_frame, _capture);
+
+ // remove the frame and older frames
+ buffer_pack_kf_.removeUpTo(frame_pack->key_frame->getTimeStamp());
+ }
+ // CASE 3:
+ else
+ 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
+ */
+
+ // CASE 1:
+ auto cap = _frame->getCaptureOf(getSensor());
+ if (cap)
+ {
+ process(_frame, cap);
+
+ // remove the capture (if stored)
+ buffer_capture_.getContainer().erase(cap->getTimeStamp());
+ }
+
+ // Search for any stored capture within time tolerance of frame
+ auto capture = buffer_capture_.select(_frame->getTimeStamp(), params_->time_tolerance);
+
+ // CASE 2:
+ if (capture)
+ {
+ process(_frame, 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);
+ }
+ // CASE 3:
+ else if (buffer_capture_.selectLastAfter(_frame->getTimeStamp(), params_->time_tolerance) == nullptr)
+ {
+ // store frame
+ buffer_pack_kf_.add(_frame, _time_tolerance);
+ }
+ // CASE 4:
+ // nothing (discard frame)
+}
+
+void ProcessorLoopClosure::process(FrameBasePtr _frame, CaptureBasePtr _capture)
+{
+ assert(_capture->getFrame() == _frame && "ProcessorLoopClosure::process _capture not linked to _frame");
+
+ // Detect and emplace features
+ emplaceFeatures(_capture);
+
+ // Vote for loop closure search
+ if (voteFindLoopClosures(_capture))
+ {
+ // Find loop closures
+ auto cap_lc_list = findLoopClosures(_capture);
+
+ // Emplace factors for each LC if validated
+ for (auto cap_lc : cap_lc_list)
+ if (validateLoopClosure(cap_lc, _capture))
+ emplaceFactors(cap_lc, _capture);
+ }
+}
+
+}// namespace wolf
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 885c1f53a456deb4b74392d011d560cb7216bafe..b48adb281744d8bd30314bd19546e85a0eea9cf6 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -225,9 +225,13 @@ 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_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..8fff4db18d933975bb25cd0c5df327c4e2f101b9
--- /dev/null
+++ b/test/dummy/processor_loop_closure_dummy.h
@@ -0,0 +1,85 @@
+#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(CaptureBasePtr, CaptureBasePtr) 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));
+ }
+
+ CaptureBasePtrList findLoopClosures(CaptureBasePtr _capture) override
+ {
+ CaptureBasePtrList cap_lc_list;
+
+ 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)
+ {
+ cap_lc_list.push_back(cap);
+ }
+
+ old_frame = old_frame->getPreviousFrame();
+ }
+
+ return cap_lc_list;
+ }
+
+ void emplaceFactors(CaptureBasePtr _capture_1, CaptureBasePtr _capture_2) override
+ {
+ FeatureBasePtr feat_2;
+ for (auto feat : _capture_2->getFeatureList())
+ if (feat->getType() == "FeatureLoopClosureDummy")
+ {
+ feat_2 = feat;
+ break;
+ }
+
+ FactorBase::emplace<FactorRelativePose2d>(feat_2, feat_2,
+ _capture_1->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..2cd9fe39c0142018dcf88fe558e492b38518671c
--- /dev/null
+++ b/test/gtest_processor_loop_closure.cpp
@@ -0,0 +1,76 @@
+
+#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;
+
+// Wolf problem
+ProblemPtr problem = Problem::create("PO", 2);
+
+ProcessorLoopClosureDummyPtr proc_lc;
+
+void setup()
+{
+ // Emplace sensor
+ auto sens_lc = 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>();
+ proc_lc = ProcessorBase::emplace<ProcessorLoopClosureDummy>(sens_lc, params);
+}
+
+TEST(ProcessorLoopClosure, installProcessor)
+{
+ setup();
+
+ EXPECT_EQ(proc_lc->getNStoredFrames(), 0);
+ EXPECT_EQ(proc_lc->getNStoredCaptures(), 0);
+}
+
+TEST(ProcessorLoopClosure, frame_stored)
+{
+ setup();
+
+ // new frame
+ auto fr1 = problem->emplaceFrame(0, Vector3d::Zero());
+
+ // keyframecallback
+ problem->keyFrameCallback(fr1, nullptr, 1);
+
+ EXPECT_EQ(proc_lc->getNStoredFrames(), 1);
+ EXPECT_EQ(proc_lc->getNStoredCaptures(), 0);
+}
+
+TEST(ProcessorLoopClosure, capture_stored)
+{
+ setup();
+
+ // new capture
+ auto cap1 = std::make_shared<CaptureBase>("CaptureBase", 0, nullptr);
+
+ // keyframecallback
+ proc_lc->captureCallback(cap1);
+
+ EXPECT_EQ(proc_lc->getNStoredFrames(), 0);
+ EXPECT_EQ(proc_lc->getNStoredCaptures(), 1);
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}