diff --git a/CMakeLists.txt b/CMakeLists.txt
index 824ec0902e9c99ceb810ea71623f256c8bd2f8a4..426f4f0719d0911ca422473fc8519d072c3b4db2 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -250,7 +250,7 @@ SET(HDRS_PROCESSOR
include/core/processor/processor_diff_drive.h
include/core/processor/processor_factory.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
@@ -344,7 +344,7 @@ SET(SRCS_PROCESSOR
src/processor/processor_base.cpp
src/processor/processor_capture_holder.cpp
src/processor/processor_diff_drive.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_loopclosure_base.h b/include/core/processor/processor_loopclosure.h
similarity index 81%
rename from include/core/processor/processor_loopclosure_base.h
rename to include/core/processor/processor_loopclosure.h
index 944a1ca29ad72770c1bcb4ab8ec76ec6d12ad57b..afd9855b5087e61825706b006456039cba06e93d 100644
--- a/include/core/processor/processor_loopclosure_base.h
+++ b/include/core/processor/processor_loopclosure.h
@@ -42,7 +42,7 @@ struct ProcessorParamsLoopClosure : public ProcessorParamsBase
* which are called at the beginning and at the end of process() respectively.
*/
-class ProcessorLoopClosureBase : public ProcessorBase
+class ProcessorLoopClosure : public ProcessorBase
{
protected:
@@ -50,9 +50,9 @@ protected:
public:
- ProcessorLoopClosureBase(const std::string& _type, ProcessorParamsLoopClosurePtr _params_loop_closure);
+ ProcessorLoopClosure(const std::string& _type, ProcessorParamsLoopClosurePtr _params_loop_closure);
- virtual ~ProcessorLoopClosureBase() = default;
+ virtual ~ProcessorLoopClosure() = default;
virtual void configure(SensorBasePtr _sensor) override { };
/** \brief Full processing of an incoming Capture.
@@ -66,13 +66,13 @@ protected:
/** \brief Called by process(). Tells if computeFeatures() will be called
*/
- virtual bool voteComputeFeatures(CaptureBasePtr _incoming_ptr) = 0;
+ 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(CaptureBasePtr _incoming_ptr) = 0;
+ virtual bool voteSearchLoopClosure() = 0;
/** \brief returns a KeyFrame-Capture pair compatible together (selected from the buffers)
*
@@ -80,19 +80,18 @@ protected:
* 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> selectPairKC(void);
+ 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 thies featrues must be done here
+ * the computations to create these features must be done here.
*
- * In this method you should add the capture to the keyframe if necessary
- * and add the features to the capture
+ * Important: All detected features should be emplaced to the capture.
*
* Returns a bool that tells if features were successfully created
*/
- virtual bool computeFeatures(std::pair<FrameBasePtr,CaptureBasePtr>) = 0;
+ 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
@@ -104,13 +103,12 @@ protected:
*
* overwrite it if you want an additional test after findLoopCandidate()
*/
- bool validateLoop(CaptureBasePtr _capture_1, CaptureBasePtr _capture_2) {return true;};
+ virtual bool validateLoop(CaptureBasePtr _capture_1, CaptureBasePtr _capture_2) {return true;};
- /** \brief create the factor(s)
+ /** \brief emplace the factor(s)
*
- * overwrite it if needed
*/
- virtual void createFactors(CaptureBasePtr _capture_1, CaptureBasePtr _capture_2) = 0;
+ virtual void emplaceFactors(CaptureBasePtr _capture_1, CaptureBasePtr _capture_2) = 0;
/** Pre-process incoming Capture
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 242f2b445bf2549c373b8b747cd4f34282192fad..0000000000000000000000000000000000000000
--- a/src/processor/processor_loopclosure_base.cpp
+++ /dev/null
@@ -1,84 +0,0 @@
-/**
- * \file processor_loop_closure.h
- *
- * Created on: Mai 31, 2019
- * \author: Pierre Guetschel
- */
-
-#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)
-{
- // the pre-process, if necessary, is implemented in the derived classes
- preProcess();
-
- if (voteComputeFeatures(_incoming_ptr))
- {
- std::pair<FrameBasePtr,CaptureBasePtr> pairKC = selectPairKC();
- if (pairKC.first==nullptr || pairKC.second==nullptr) return;
- bool success_computeFeatures = computeFeatures(pairKC);
-
- if (success_computeFeatures && voteSearchLoopClosure(_incoming_ptr))
- {
- CaptureBasePtr capture_1 = pairKC.second;
- CaptureBasePtr capture_2 = findLoopCandidate(capture_1);
- if (validateLoop(capture_1, capture_2)==false) return;
- if (capture_2==nullptr) return;
- if (capture_1->getFrame() == nullptr || capture_2->getFrame() == nullptr) {
- WOLF_WARN("ProcessorLoopClosureBase : tried to close a loop with captures linked to no KF");
- return;
- };
- if (capture_1->getFrame() == capture_2->getFrame()) {
- WOLF_WARN("ProcessorLoopClosureBase : findLoopCandidate() returned two captures of the same frame");
- return;
- };
- createFactors(capture_1, capture_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> ProcessorLoopClosureBase::selectPairKC()
-{
- std::map<TimeStamp,PackKeyFramePtr> kf_container = buffer_pack_kf_.getContainer();
- if (kf_container.empty()){
- return std::make_pair(nullptr, nullptr);};
- BufferPackKeyFrame::Iterator kf_it;
- for (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 9def7598d38875f4347f67da4f1a4a78eac523e9..f4d96771cf0b3b1c7d8be66202d129296bb02daf 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -179,8 +179,8 @@ wolf_add_gtest(gtest_param_prior gtest_param_prior.cpp)
target_link_libraries(gtest_param_prior ${PROJECT_NAME})
# ProcessorLoopClosureBase class test
-wolf_add_gtest(gtest_processor_loopclosure_base gtest_processor_loopclosure_base.cpp)
-target_link_libraries(gtest_processor_loopclosure_base ${PROJECT_NAME})
+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_loopclosure_base.cpp b/test/gtest_processor_loopclosure.cpp
similarity index 80%
rename from test/gtest_processor_loopclosure_base.cpp
rename to test/gtest_processor_loopclosure.cpp
index 28f3ba4c1c271624a18095943e1f9bcb2bce3cfc..f2c99aac536fb2265ad100f00f41def2aa2abeae 100644
--- a/test/gtest_processor_loopclosure_base.cpp
+++ b/test/gtest_processor_loopclosure.cpp
@@ -3,7 +3,7 @@
#include "core/problem/problem.h"
#include "core/capture/capture_void.h"
-#include "core/processor/processor_loopclosure_base.h"
+#include "core/processor/processor_loopclosure.h"
// STL
@@ -17,23 +17,21 @@ using namespace Eigen;
WOLF_PTR_TYPEDEFS(ProcessorLoopClosureDummy);
// dummy class:
-class ProcessorLoopClosureDummy : public ProcessorLoopClosureBase
+class ProcessorLoopClosureDummy : public ProcessorLoopClosure
{
private:
bool* factor_created;
public:
ProcessorLoopClosureDummy(ProcessorParamsLoopClosurePtr _params_loop_closure, bool& factor_created):
- ProcessorLoopClosureBase("LOOP CLOSURE DUMMY", _params_loop_closure),
+ ProcessorLoopClosure("LOOP CLOSURE DUMMY", _params_loop_closure),
factor_created(&factor_created){};
std::pair<FrameBasePtr,CaptureBasePtr> public_selectPairKC(){ return selectPairKC();};
protected:
- bool voteComputeFeatures(CaptureBasePtr _incoming_ptr) { return true;};
- bool voteSearchLoopClosure(CaptureBasePtr _incoming_ptr) { return true;};
- bool computeFeatures(std::pair<FrameBasePtr,CaptureBasePtr> kc_pair) {
- kc_pair.second->setFrame(kc_pair.first);
- return true;};
+ 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()) {
@@ -41,7 +39,7 @@ protected:
return cap; }
};} return nullptr;
};
- void createFactors(CaptureBasePtr _capture_1, CaptureBasePtr _capture_2) {
+ void emplaceFactors(CaptureBasePtr _capture_1, CaptureBasePtr _capture_2) {
std::cout << "factor created\n";
*factor_created = true;
};
@@ -49,7 +47,7 @@ protected:
-TEST(ProcessorLoopClosureBase, installProcessor)
+TEST(ProcessorLoopClosure, installProcessor)
{
using namespace wolf;
using std::shared_ptr;