diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 0b98845f07bc3bf920e1ead358bb6b5d91be4e73..9def7598d38875f4347f67da4f1a4a78eac523e9 100644 --- a/test/CMakeLists.txt +++ b/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}) diff --git a/test/gtest_processor_loopclosure_base.cpp b/test/gtest_processor_loopclosure_base.cpp new file mode 100644 index 0000000000000000000000000000000000000000..28f3ba4c1c271624a18095943e1f9bcb2bce3cfc --- /dev/null +++ b/test/gtest_processor_loopclosure_base.cpp @@ -0,0 +1,103 @@ + +#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(); +}