diff --git a/CMakeLists.txt b/CMakeLists.txt index 0bcdb9f10bfcfb6d4dc59a279649335f3a25b165..336c233fc0828acb86eefb42da32dddb85dc1924 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -247,7 +247,6 @@ 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 @@ -344,7 +343,6 @@ 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 diff --git a/include/core/processor/processor_loopclosure.h b/include/core/processor/processor_loopclosure.h deleted file mode 100644 index 12e1fd8781627154315b4815a86105e1d404e86f..0000000000000000000000000000000000000000 --- a/include/core/processor/processor_loopclosure.h +++ /dev/null @@ -1,183 +0,0 @@ -#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 */ diff --git a/src/processor/processor_loop_closure.cpp b/src/processor/processor_loop_closure.cpp index 2c66b5019e3588854cf43cdd0c1006a698f2ce8c..8123737b2f99f06df351b9bddd6233b21b144dcc 100644 --- a/src/processor/processor_loop_closure.cpp +++ b/src/processor/processor_loop_closure.cpp @@ -20,12 +20,12 @@ void ProcessorLoopClosure::processCapture(CaptureBasePtr _capture) * 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_INFO("ProcessorLoopClosure::processCapture capture ", _capture->id()); + WOLF_DEBUG("ProcessorLoopClosure::processCapture capture ", _capture->id()); // CASE 1: if (_capture->getFrame()) { - WOLF_INFO("CASE 1"); + WOLF_DEBUG("CASE 1"); process(_capture->getFrame(), _capture); @@ -41,7 +41,7 @@ void ProcessorLoopClosure::processCapture(CaptureBasePtr _capture) // CASE 2: if (frame_pack) { - WOLF_INFO("CASE 2"); + WOLF_DEBUG("CASE 2"); _capture->link(frame_pack->key_frame); @@ -53,7 +53,7 @@ void ProcessorLoopClosure::processCapture(CaptureBasePtr _capture) return; } // CASE 3: - WOLF_INFO("CASE 3"); + WOLF_DEBUG("CASE 3"); buffer_capture_.add(_capture->getTimeStamp(), _capture); } @@ -66,13 +66,13 @@ void ProcessorLoopClosure::processKeyFrame(FrameBasePtr _frame, const double& _t * 4. Otherwise: The frame is not compatible with any stored capture -> discard frame */ - WOLF_INFO("ProcessorLoopClosure::processKeyFrame frame ", _frame->id()); + WOLF_DEBUG("ProcessorLoopClosure::processKeyFrame frame ", _frame->id()); // CASE 1: auto cap = _frame->getCaptureOf(getSensor()); if (cap) { - WOLF_INFO("CASE 1"); + WOLF_DEBUG("CASE 1"); process(_frame, cap); @@ -88,7 +88,7 @@ void ProcessorLoopClosure::processKeyFrame(FrameBasePtr _frame, const double& _t // CASE 2: if (capture) { - WOLF_INFO("CASE 2"); + WOLF_DEBUG("CASE 2"); capture->link(_frame); @@ -105,7 +105,7 @@ void ProcessorLoopClosure::processKeyFrame(FrameBasePtr _frame, const double& _t // CASE 3: if (buffer_capture_.selectLastAfter(_frame->getTimeStamp(), params_->time_tolerance) == nullptr) { - WOLF_INFO("CASE 3"); + WOLF_DEBUG("CASE 3"); // store frame buffer_pack_kf_.add(_frame, _time_tolerance); @@ -113,28 +113,28 @@ void ProcessorLoopClosure::processKeyFrame(FrameBasePtr _frame, const double& _t return; } // CASE 4: - WOLF_INFO("CASE 4"); + WOLF_DEBUG("CASE 4"); // nothing (discard frame) } void ProcessorLoopClosure::process(FrameBasePtr _frame, CaptureBasePtr _capture) { - WOLF_INFO("ProcessorLoopClosure::process frame ", _frame->id(), " capture ", _capture->id()); + WOLF_DEBUG("ProcessorLoopClosure::process frame ", _frame->id(), " capture ", _capture->id()); assert(_capture->getFrame() == _frame && "ProcessorLoopClosure::process _capture not linked to _frame"); // Detect and emplace features - WOLF_INFO("emplacing features..."); + WOLF_DEBUG("emplacing features..."); emplaceFeatures(_capture); // Vote for loop closure search if (voteFindLoopClosures(_capture)) { - WOLF_INFO("finding loop closures..."); + WOLF_DEBUG("finding loop closures..."); // Find loop closures auto cap_lc_list = findLoopClosures(_capture); - WOLF_INFO(cap_lc_list.size(), " loop closures found"); + WOLF_DEBUG(cap_lc_list.size(), " loop closures found"); // Emplace factors for each LC if validated for (auto cap_lc : cap_lc_list) diff --git a/src/processor/processor_loopclosure.cpp b/src/processor/processor_loopclosure.cpp deleted file mode 100644 index 16e037b1c7634c4d0d7509fea51e075fbc8d0066..0000000000000000000000000000000000000000 --- a/src/processor/processor_loopclosure.cpp +++ /dev/null @@ -1,119 +0,0 @@ -/** - * \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 diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index b48adb281744d8bd30314bd19546e85a0eea9cf6..7fc59fac6f5cac9285dde8c12205337e4768c36d 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -225,10 +225,6 @@ 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}) -# 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}) diff --git a/test/gtest_processor_loopclosure.cpp b/test/gtest_processor_loopclosure.cpp deleted file mode 100644 index 4e50c870348da1877b3a599c48158730e2df47ab..0000000000000000000000000000000000000000 --- a/test/gtest_processor_loopclosure.cpp +++ /dev/null @@ -1,104 +0,0 @@ - -#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(); -}