Skip to content
Snippets Groups Projects
Commit 33cee2de authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

Some few changes (mainly naming)

parent 99aeb151
No related branches found
No related tags found
1 merge request!290Resolve "ProcessorLoopClosureBase class"
Pipeline #3744 passed
...@@ -250,7 +250,7 @@ SET(HDRS_PROCESSOR ...@@ -250,7 +250,7 @@ SET(HDRS_PROCESSOR
include/core/processor/processor_diff_drive.h include/core/processor/processor_diff_drive.h
include/core/processor/processor_factory.h include/core/processor/processor_factory.h
include/core/processor/processor_logging.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_motion.h
include/core/processor/processor_odom_2D.h include/core/processor/processor_odom_2D.h
include/core/processor/processor_odom_3D.h include/core/processor/processor_odom_3D.h
...@@ -344,7 +344,7 @@ SET(SRCS_PROCESSOR ...@@ -344,7 +344,7 @@ SET(SRCS_PROCESSOR
src/processor/processor_base.cpp src/processor/processor_base.cpp
src/processor/processor_capture_holder.cpp src/processor/processor_capture_holder.cpp
src/processor/processor_diff_drive.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_motion.cpp
src/processor/processor_odom_2D.cpp src/processor/processor_odom_2D.cpp
src/processor/processor_odom_3D.cpp src/processor/processor_odom_3D.cpp
......
...@@ -42,7 +42,7 @@ struct ProcessorParamsLoopClosure : public ProcessorParamsBase ...@@ -42,7 +42,7 @@ struct ProcessorParamsLoopClosure : public ProcessorParamsBase
* which are called at the beginning and at the end of process() respectively. * which are called at the beginning and at the end of process() respectively.
*/ */
class ProcessorLoopClosureBase : public ProcessorBase class ProcessorLoopClosure : public ProcessorBase
{ {
protected: protected:
...@@ -50,9 +50,9 @@ protected: ...@@ -50,9 +50,9 @@ protected:
public: 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 { }; virtual void configure(SensorBasePtr _sensor) override { };
/** \brief Full processing of an incoming Capture. /** \brief Full processing of an incoming Capture.
...@@ -66,13 +66,13 @@ protected: ...@@ -66,13 +66,13 @@ protected:
/** \brief Called by process(). Tells if computeFeatures() will be called /** \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 /** \brief Called by process(). Tells if findLoopCandidate() and createFactors() will be called
* *
* WARNING : A LC can be searched only when voteComputeFeatures() return true * 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) /** \brief returns a KeyFrame-Capture pair compatible together (selected from the buffers)
* *
...@@ -80,19 +80,18 @@ protected: ...@@ -80,19 +80,18 @@ protected:
* In the default implementation, we select the KF with the most recent TimeStamp * In the default implementation, we select the KF with the most recent TimeStamp
* and that is compatible with at least a Capture * 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 /** \brief add the Capture and all features needed to the corresponding KF
* *
* If the loop closure process requires features associated to each capture, * 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 * Important: All detected features should be emplaced to the capture.
* and add the features to the capture
* *
* Returns a bool that tells if features were successfully created * 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 /** \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 validateLoop is not overwritten, a loop will be closed with the returned candidate
...@@ -104,13 +103,12 @@ protected: ...@@ -104,13 +103,12 @@ protected:
* *
* overwrite it if you want an additional test after findLoopCandidate() * 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 /** Pre-process incoming Capture
......
...@@ -5,13 +5,13 @@ ...@@ -5,13 +5,13 @@
* \author: Pierre Guetschel * \author: Pierre Guetschel
*/ */
#include "core/processor/processor_loopclosure_base.h" #include <core/processor/processor_loopclosure.h>
namespace wolf namespace wolf
{ {
ProcessorLoopClosureBase::ProcessorLoopClosureBase(const std::string& _type, ProcessorParamsLoopClosurePtr _params_loop_closure): ProcessorLoopClosure::ProcessorLoopClosure(const std::string& _type, ProcessorParamsLoopClosurePtr _params_loop_closure):
ProcessorBase(_type, _params_loop_closure), ProcessorBase(_type, _params_loop_closure),
params_loop_closure_(_params_loop_closure) params_loop_closure_(_params_loop_closure)
{ {
...@@ -19,33 +19,53 @@ ProcessorLoopClosureBase::ProcessorLoopClosureBase(const std::string& _type, Pro ...@@ -19,33 +19,53 @@ ProcessorLoopClosureBase::ProcessorLoopClosureBase(const std::string& _type, Pro
} }
//############################################################################## //##############################################################################
void ProcessorLoopClosureBase::process(CaptureBasePtr _incoming_ptr) void ProcessorLoopClosure::process(CaptureBasePtr _incoming_ptr)
{ {
// the pre-process, if necessary, is implemented in the derived classes // the pre-process, if necessary, is implemented in the derived classes
preProcess(); preProcess();
if (voteComputeFeatures(_incoming_ptr)) if (voteComputeFeatures())
{ {
std::pair<FrameBasePtr,CaptureBasePtr> pairKC = selectPairKC(); 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))
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)
{ {
CaptureBasePtr capture_1 = pairKC.second; // link the capture to the KF (if not already linked)
CaptureBasePtr capture_2 = findLoopCandidate(capture_1); if (cap_1->getFrame() != kf_1)
if (validateLoop(capture_1, capture_2)==false) return; {
if (capture_2==nullptr) return; assert(cap_1->getFrame() == nullptr && "capture already linked to a different frame"); //FIXME
if (capture_1->getFrame() == nullptr || capture_2->getFrame() == nullptr) { cap_1->link(kf_1);
WOLF_WARN("ProcessorLoopClosureBase : tried to close a loop with captures linked to no KF"); }
return;
}; // search loop closure
if (capture_1->getFrame() == capture_2->getFrame()) { if(voteSearchLoopClosure())
WOLF_WARN("ProcessorLoopClosureBase : findLoopCandidate() returned two captures of the same frame"); {
return; auto cap_2 = findLoopCandidate(cap_1);
}; if (cap_2==nullptr)
createFactors(capture_1, capture_2); 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 // the post-process, if necessary, is implemented in the derived classes
...@@ -56,13 +76,13 @@ void ProcessorLoopClosureBase::process(CaptureBasePtr _incoming_ptr) ...@@ -56,13 +76,13 @@ void ProcessorLoopClosureBase::process(CaptureBasePtr _incoming_ptr)
* In the default implementation, we select the KF with the most recent TimeStamp * In the default implementation, we select the KF with the most recent TimeStamp
* and that is compatible with at least a Capture * and that is compatible with at least a Capture
*/ */
std::pair<FrameBasePtr,CaptureBasePtr> ProcessorLoopClosureBase::selectPairKC() std::pair<FrameBasePtr,CaptureBasePtr> ProcessorLoopClosure::selectPairKC()
{ {
std::map<TimeStamp,PackKeyFramePtr> kf_container = buffer_pack_kf_.getContainer(); std::map<TimeStamp,PackKeyFramePtr> kf_container = buffer_pack_kf_.getContainer();
if (kf_container.empty()){ if (kf_container.empty()){
return std::make_pair(nullptr, nullptr);}; return std::make_pair(nullptr, nullptr);};
BufferPackKeyFrame::Iterator kf_it;
for (kf_it=kf_container.begin(); kf_it!=kf_container.end(); ++kf_it) 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); CaptureBasePtr cap_ptr = buffer_capture_.select(kf_it->first, kf_it->second->time_tolerance);
if (cap_ptr != nullptr) if (cap_ptr != nullptr)
......
...@@ -179,8 +179,8 @@ wolf_add_gtest(gtest_param_prior gtest_param_prior.cpp) ...@@ -179,8 +179,8 @@ wolf_add_gtest(gtest_param_prior gtest_param_prior.cpp)
target_link_libraries(gtest_param_prior ${PROJECT_NAME}) target_link_libraries(gtest_param_prior ${PROJECT_NAME})
# ProcessorLoopClosureBase class test # ProcessorLoopClosureBase class test
wolf_add_gtest(gtest_processor_loopclosure_base gtest_processor_loopclosure_base.cpp) wolf_add_gtest(gtest_processor_loopclosure gtest_processor_loopclosure.cpp)
target_link_libraries(gtest_processor_loopclosure_base ${PROJECT_NAME}) target_link_libraries(gtest_processor_loopclosure ${PROJECT_NAME})
# ProcessorMotion in 2D # ProcessorMotion in 2D
wolf_add_gtest(gtest_odom_2D gtest_odom_2D.cpp) wolf_add_gtest(gtest_odom_2D gtest_odom_2D.cpp)
......
...@@ -3,7 +3,7 @@ ...@@ -3,7 +3,7 @@
#include "core/problem/problem.h" #include "core/problem/problem.h"
#include "core/capture/capture_void.h" #include "core/capture/capture_void.h"
#include "core/processor/processor_loopclosure_base.h" #include "core/processor/processor_loopclosure.h"
// STL // STL
...@@ -17,23 +17,21 @@ using namespace Eigen; ...@@ -17,23 +17,21 @@ using namespace Eigen;
WOLF_PTR_TYPEDEFS(ProcessorLoopClosureDummy); WOLF_PTR_TYPEDEFS(ProcessorLoopClosureDummy);
// dummy class: // dummy class:
class ProcessorLoopClosureDummy : public ProcessorLoopClosureBase class ProcessorLoopClosureDummy : public ProcessorLoopClosure
{ {
private: private:
bool* factor_created; bool* factor_created;
public: public:
ProcessorLoopClosureDummy(ProcessorParamsLoopClosurePtr _params_loop_closure, bool& factor_created): 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){}; factor_created(&factor_created){};
std::pair<FrameBasePtr,CaptureBasePtr> public_selectPairKC(){ return selectPairKC();}; std::pair<FrameBasePtr,CaptureBasePtr> public_selectPairKC(){ return selectPairKC();};
protected: protected:
bool voteComputeFeatures(CaptureBasePtr _incoming_ptr) { return true;}; bool voteComputeFeatures() { return true;};
bool voteSearchLoopClosure(CaptureBasePtr _incoming_ptr) { return true;}; bool voteSearchLoopClosure() { return true;};
bool computeFeatures(std::pair<FrameBasePtr,CaptureBasePtr> kc_pair) { bool detectFeatures(CaptureBasePtr cap) { return true;};
kc_pair.second->setFrame(kc_pair.first);
return true;};
CaptureBasePtr findLoopCandidate(CaptureBasePtr _capture) { CaptureBasePtr findLoopCandidate(CaptureBasePtr _capture) {
for (FrameBasePtr kf : getProblem()->getTrajectory()->getFrameList()) { for (FrameBasePtr kf : getProblem()->getTrajectory()->getFrameList()) {
if (kf->isKey()) { if (kf->isKey()) {
...@@ -41,7 +39,7 @@ protected: ...@@ -41,7 +39,7 @@ protected:
return cap; } return cap; }
};} return nullptr; };} return nullptr;
}; };
void createFactors(CaptureBasePtr _capture_1, CaptureBasePtr _capture_2) { void emplaceFactors(CaptureBasePtr _capture_1, CaptureBasePtr _capture_2) {
std::cout << "factor created\n"; std::cout << "factor created\n";
*factor_created = true; *factor_created = true;
}; };
...@@ -49,7 +47,7 @@ protected: ...@@ -49,7 +47,7 @@ protected:
TEST(ProcessorLoopClosureBase, installProcessor) TEST(ProcessorLoopClosure, installProcessor)
{ {
using namespace wolf; using namespace wolf;
using std::shared_ptr; using std::shared_ptr;
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment