Add tests for ProcessorLoopClosureBase

test/CMakeLists.txt

@@ -178,6 +178,10 @@ target_link_libraries(gtest_map_yaml ${PROJECT_NAME})
 wolf_add_gtest(gtest_param_prior gtest_param_prior.cpp)
 target_link_libraries(gtest_param_prior ${PROJECT_NAME})
 
+# ProcessorLoopClosureBase class test
+wolf_add_gtest(gtest_processor_loopclosure_base gtest_processor_loopclosure_base.cpp)
+target_link_libraries(gtest_processor_loopclosure_base ${PROJECT_NAME})
+
 # ProcessorMotion in 2D
 wolf_add_gtest(gtest_odom_2D gtest_odom_2D.cpp)
 target_link_libraries(gtest_odom_2D ${PROJECT_NAME})

test/gtest_processor_loopclosure_base.cpp

+
+#include "core/utils/utils_gtest.h"
+#include "core/problem/problem.h"
+#include "core/capture/capture_void.h"
+
+#include "core/processor/processor_loopclosure_base.h"
+
+
+// STL
+#include <iterator>
+#include <iostream>
+
+using namespace wolf;
+using namespace Eigen;
+
+
+WOLF_PTR_TYPEDEFS(ProcessorLoopClosureDummy);
+
+// dummy class:
+class ProcessorLoopClosureDummy : public ProcessorLoopClosureBase
+{
+private:
+    bool* factor_created;
+
+public:
+    ProcessorLoopClosureDummy(ProcessorParamsLoopClosurePtr _params_loop_closure, bool& factor_created):
+        ProcessorLoopClosureBase("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;};
+    CaptureBasePtr findLoopCandidate(CaptureBasePtr _capture)   {
+        for (FrameBasePtr kf : getProblem()->getTrajectory()->getFrameList()) {
+            if (kf->isKey()) {
+                for (CaptureBasePtr cap : kf->getCaptureList()) {
+                    return cap; }
+        };} return nullptr;
+    };
+    void createFactors(CaptureBasePtr _capture_1, CaptureBasePtr _capture_2) {
+        std::cout << "factor created\n";
+        *factor_created = true;
+    };
+};
+
+
+
+TEST(ProcessorLoopClosureBase, installProcessor)
+{
+    using namespace wolf;
+    using std::shared_ptr;
+    using std::make_shared;
+    using std::static_pointer_cast;
+    using Eigen::Vector2s;
+
+    bool factor_created = false;
+
+
+    Scalar dt = 0.01;
+
+    // Wolf problem
+    ProblemPtr problem = Problem::create("PO", 2);
+
+    // Install tracker (sensor and processor)
+    auto sens_lc = SensorBase::emplace<SensorBase>(problem->getHardware(),
+                                                    "SENSOR BASE",
+                                                    std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)),
+                                                    std::make_shared<StateBlock>(Eigen::VectorXs::Zero(1)),
+                                                    std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), 2);
+    ProcessorParamsLoopClosurePtr params = std::make_shared<ProcessorParamsLoopClosure>();
+    ProcessorLoopClosureDummyPtr proc_lc = std::make_shared<ProcessorLoopClosureDummy>(params, factor_created);
+    proc_lc->link(sens_lc);
+    std::cout << "sensor & processor created and added to wolf problem" << std::endl;
+
+    // initialize
+    TimeStamp   t(0.0);
+    Vector3s    x(0,0,0);
+    Matrix3s    P = Matrix3s::Identity() * 0.1;
+    problem->setPrior(x, P, t, dt/2);             // KF1
+
+    // new KF
+    t += dt;
+    FrameBasePtr kf = FrameBase::create_PO_2D (KEY, t, x); //KF2
+    proc_lc->keyFrameCallback(kf, dt/2);
+
+    // new capture
+    CaptureVoidPtr capt_lc(make_shared<CaptureVoid>(t, sens_lc));
+    proc_lc->captureCallback(capt_lc);
+
+    proc_lc->process(capt_lc);
+
+    ASSERT_TRUE(factor_created);
+}
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}