Resolve "New ProcessorLoopClosure"

CMakeLists.txt

@@ -247,7 +247,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
@@ -343,7 +343,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

include/core/processor/processor_loop_closure.h

+#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 */

include/core/processor/processor_loopclosure.h

-#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 */

src/processor/processor_loop_closure.cpp

+#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

src/processor/processor_loopclosure.cpp

-/**
- * \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

test/CMakeLists.txt

@@ -229,9 +229,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)

test/dummy/processor_loop_closure_dummy.h

+#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_ */

test/gtest_processor_loop_closure.cpp

+
+#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();
+}

test/gtest_processor_loopclosure.cpp

-
-#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();
-}