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

removed old files and WOLF_DEBUG

parent b0fe5fd1
No related branches found
No related tags found
1 merge request!413Resolve "New ProcessorLoopClosure"
Pipeline #6421 passed
This commit is part of merge request !413. Comments created here will be created in the context of that merge request.
...@@ -247,7 +247,6 @@ SET(HDRS_PROCESSOR ...@@ -247,7 +247,6 @@ SET(HDRS_PROCESSOR
include/core/processor/processor_diff_drive.h include/core/processor/processor_diff_drive.h
include/core/processor/factory_processor.h include/core/processor/factory_processor.h
include/core/processor/processor_logging.h include/core/processor/processor_logging.h
#include/core/processor/processor_loopclosure.h
include/core/processor/processor_loop_closure.h include/core/processor/processor_loop_closure.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
...@@ -344,7 +343,6 @@ SET(SRCS_PROCESSOR ...@@ -344,7 +343,6 @@ SET(SRCS_PROCESSOR
src/processor/motion_buffer.cpp src/processor/motion_buffer.cpp
src/processor/processor_base.cpp src/processor/processor_base.cpp
src/processor/processor_diff_drive.cpp src/processor/processor_diff_drive.cpp
#src/processor/processor_loopclosure.cpp
src/processor/processor_loop_closure.cpp src/processor/processor_loop_closure.cpp
src/processor/processor_motion.cpp src/processor/processor_motion.cpp
src/processor/processor_odom_2d.cpp src/processor/processor_odom_2d.cpp
......
#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 */
...@@ -20,12 +20,12 @@ void ProcessorLoopClosure::processCapture(CaptureBasePtr _capture) ...@@ -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) * 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: // CASE 1:
if (_capture->getFrame()) if (_capture->getFrame())
{ {
WOLF_INFO("CASE 1"); WOLF_DEBUG("CASE 1");
process(_capture->getFrame(), _capture); process(_capture->getFrame(), _capture);
...@@ -41,7 +41,7 @@ void ProcessorLoopClosure::processCapture(CaptureBasePtr _capture) ...@@ -41,7 +41,7 @@ void ProcessorLoopClosure::processCapture(CaptureBasePtr _capture)
// CASE 2: // CASE 2:
if (frame_pack) if (frame_pack)
{ {
WOLF_INFO("CASE 2"); WOLF_DEBUG("CASE 2");
_capture->link(frame_pack->key_frame); _capture->link(frame_pack->key_frame);
...@@ -53,7 +53,7 @@ void ProcessorLoopClosure::processCapture(CaptureBasePtr _capture) ...@@ -53,7 +53,7 @@ void ProcessorLoopClosure::processCapture(CaptureBasePtr _capture)
return; return;
} }
// CASE 3: // CASE 3:
WOLF_INFO("CASE 3"); WOLF_DEBUG("CASE 3");
buffer_capture_.add(_capture->getTimeStamp(), _capture); buffer_capture_.add(_capture->getTimeStamp(), _capture);
} }
...@@ -66,13 +66,13 @@ void ProcessorLoopClosure::processKeyFrame(FrameBasePtr _frame, const double& _t ...@@ -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 * 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: // CASE 1:
auto cap = _frame->getCaptureOf(getSensor()); auto cap = _frame->getCaptureOf(getSensor());
if (cap) if (cap)
{ {
WOLF_INFO("CASE 1"); WOLF_DEBUG("CASE 1");
process(_frame, cap); process(_frame, cap);
...@@ -88,7 +88,7 @@ void ProcessorLoopClosure::processKeyFrame(FrameBasePtr _frame, const double& _t ...@@ -88,7 +88,7 @@ void ProcessorLoopClosure::processKeyFrame(FrameBasePtr _frame, const double& _t
// CASE 2: // CASE 2:
if (capture) if (capture)
{ {
WOLF_INFO("CASE 2"); WOLF_DEBUG("CASE 2");
capture->link(_frame); capture->link(_frame);
...@@ -105,7 +105,7 @@ void ProcessorLoopClosure::processKeyFrame(FrameBasePtr _frame, const double& _t ...@@ -105,7 +105,7 @@ void ProcessorLoopClosure::processKeyFrame(FrameBasePtr _frame, const double& _t
// CASE 3: // CASE 3:
if (buffer_capture_.selectLastAfter(_frame->getTimeStamp(), params_->time_tolerance) == nullptr) if (buffer_capture_.selectLastAfter(_frame->getTimeStamp(), params_->time_tolerance) == nullptr)
{ {
WOLF_INFO("CASE 3"); WOLF_DEBUG("CASE 3");
// store frame // store frame
buffer_pack_kf_.add(_frame, _time_tolerance); buffer_pack_kf_.add(_frame, _time_tolerance);
...@@ -113,28 +113,28 @@ void ProcessorLoopClosure::processKeyFrame(FrameBasePtr _frame, const double& _t ...@@ -113,28 +113,28 @@ void ProcessorLoopClosure::processKeyFrame(FrameBasePtr _frame, const double& _t
return; return;
} }
// CASE 4: // CASE 4:
WOLF_INFO("CASE 4"); WOLF_DEBUG("CASE 4");
// nothing (discard frame) // nothing (discard frame)
} }
void ProcessorLoopClosure::process(FrameBasePtr _frame, CaptureBasePtr _capture) 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"); assert(_capture->getFrame() == _frame && "ProcessorLoopClosure::process _capture not linked to _frame");
// Detect and emplace features // Detect and emplace features
WOLF_INFO("emplacing features..."); WOLF_DEBUG("emplacing features...");
emplaceFeatures(_capture); emplaceFeatures(_capture);
// Vote for loop closure search // Vote for loop closure search
if (voteFindLoopClosures(_capture)) if (voteFindLoopClosures(_capture))
{ {
WOLF_INFO("finding loop closures..."); WOLF_DEBUG("finding loop closures...");
// Find loop closures // Find loop closures
auto cap_lc_list = findLoopClosures(_capture); 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 // Emplace factors for each LC if validated
for (auto cap_lc : cap_lc_list) for (auto cap_lc : cap_lc_list)
......
/**
* \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
...@@ -225,10 +225,6 @@ target_link_libraries(gtest_param_prior ${PLUGIN_NAME}) ...@@ -225,10 +225,6 @@ target_link_libraries(gtest_param_prior ${PLUGIN_NAME})
wolf_add_gtest(gtest_processor_diff_drive gtest_processor_diff_drive.cpp) wolf_add_gtest(gtest_processor_diff_drive gtest_processor_diff_drive.cpp)
target_link_libraries(gtest_processor_diff_drive ${PLUGIN_NAME}) 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 # ProcessorLoopClosure class test
wolf_add_gtest(gtest_processor_loop_closure gtest_processor_loop_closure.cpp) wolf_add_gtest(gtest_processor_loop_closure gtest_processor_loop_closure.cpp)
target_link_libraries(gtest_processor_loop_closure ${PLUGIN_NAME}) target_link_libraries(gtest_processor_loop_closure ${PLUGIN_NAME})
......
#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();
}
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