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

processorLandmarkExternal is PT now

parent 2ac5a46a
No related branches found
No related tags found
1 merge request!466devel->main
Pipeline #14206 canceled
This commit is part of merge request !466. Comments created here will be created in the context of that merge request.
......@@ -23,14 +23,15 @@
#pragma once
#include "core/common/wolf.h"
#include "core/processor/processor_tracker_feature.h"
#include "core/processor/processor_tracker.h"
#include "core/processor/track_matrix.h"
namespace wolf
{
WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorTrackerFeatureLandmarkExternal);
WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorLandmarkExternal);
struct ParamsProcessorTrackerFeatureLandmarkExternal : public ParamsProcessorTrackerFeature
struct ParamsProcessorLandmarkExternal : public ParamsProcessorTracker
{
bool use_orientation; ///< use orientation measure or not when emplacing factors
double filter_quality_th; ///< min quality to consider the detection
......@@ -38,10 +39,10 @@ struct ParamsProcessorTrackerFeatureLandmarkExternal : public ParamsProcessorTra
unsigned int filter_track_length_th; ///< length of the track necessary to consider the detection
double time_span; ///< for keyframe voting: time span since last frame
ParamsProcessorTrackerFeatureLandmarkExternal() = default;
ParamsProcessorTrackerFeatureLandmarkExternal(std::string _unique_name,
ParamsProcessorLandmarkExternal() = default;
ParamsProcessorLandmarkExternal(std::string _unique_name,
const wolf::ParamsServer & _server):
ParamsProcessorTrackerFeature(_unique_name, _server)
ParamsProcessorTracker(_unique_name, _server)
{
use_orientation = _server.getParam<bool> (prefix + _unique_name + "/use_orientation");
filter_quality_th = _server.getParam<double> (prefix + _unique_name + "/filter_quality_th");
......@@ -51,50 +52,45 @@ struct ParamsProcessorTrackerFeatureLandmarkExternal : public ParamsProcessorTra
}
};
WOLF_PTR_TYPEDEFS(ProcessorTrackerFeatureLandmarkExternal);
WOLF_PTR_TYPEDEFS(ProcessorLandmarkExternal);
//Class
class ProcessorTrackerFeatureLandmarkExternal : public ProcessorTrackerFeature
class ProcessorLandmarkExternal : public ProcessorTracker
{
public:
ProcessorTrackerFeatureLandmarkExternal(ParamsProcessorTrackerFeatureLandmarkExternalPtr _params_tracker_feature);
~ProcessorTrackerFeatureLandmarkExternal() override;
ProcessorLandmarkExternal(ParamsProcessorLandmarkExternalPtr _params_tracker_feature);
~ProcessorLandmarkExternal() override;
// Factory method for high level API
WOLF_PROCESSOR_CREATE(ProcessorTrackerFeatureLandmarkExternal, ParamsProcessorTrackerFeatureLandmarkExternal);
WOLF_PROCESSOR_CREATE(ProcessorLandmarkExternal, ParamsProcessorLandmarkExternal);
void configure(SensorBasePtr _sensor) override { };
protected:
ParamsProcessorTrackerFeatureLandmarkExternalPtr params_tfle_;
//std::set<FeatureBasePtr> unmatched_detections_incoming_, unmatched_detections_last_;
std::list<FeatureBasePtr> unmatched_detections_incoming_;
ParamsProcessorLandmarkExternalPtr params_tfle_;
TrackMatrix track_matrix_;
/** \brief Track provided features in \b _capture
* \param _features_in input list of features in \b last to track
* \param _capture the capture in which the _features_in should be searched
* \param _features_out returned list of features found in \b _capture
* \param _feature_correspondences returned map of correspondences: _feature_correspondences[feature_out_ptr] = feature_in_ptr
/** Pre-process incoming Capture
*
* This is called by process() just after assigning incoming_ptr_ to a valid Capture.
*
* IMPORTANT: The features in _features_out should be emplaced. Don't use `make_shared`, use `FeatureBase::emplace` instead.
* Then, they will be already linked to the _capture.
* Extract the detections and fill unmatched_detections_incoming_ (FeaturePtrList)
*/
void preProcess() override;
/** \brief Process known Features
* \return The number of successful matches.
*
* \return the number of features tracked
* This function tracks features from last to incoming and starts new tracks for new (not tracked) features in incoming
*/
unsigned int trackFeatures(const FeatureBasePtrList& _features_in,
const CaptureBasePtr& _capture,
FeatureBasePtrList& _features_out,
FeatureMatchMap& _feature_correspondences) override;
/** \brief Correct the drift in incoming feature by re-comparing against the corresponding feature in origin.
* \param _last_feature input feature in last capture tracked
* \param _incoming_feature input/output feature in incoming capture to be corrected
* \return false if the the process discards the correspondence with origin's feature
unsigned int processKnown() override;
/**\brief Process new Features
*
*/
bool correctFeatureDrift(const FeatureBasePtr _origin_feature,
const FeatureBasePtr _last_feature,
FeatureBasePtr _incoming_feature) override;
unsigned int processNew(const int& _max_features) override { return 0;};
/** \brief Vote for KeyFrame generation
*
......@@ -104,40 +100,28 @@ class ProcessorTrackerFeatureLandmarkExternal : public ProcessorTrackerFeature
* WARNING! This function only votes! It does not create KeyFrames!
*/
bool voteForKeyFrame() const override;
/** \brief Detect new Features
* \param _max_features maximum number of features detected (-1: unlimited. 0: none)
* \param _capture The capture in which the new features should be detected.
* \param _features_out The list of detected Features in _capture.
* \return The number of detected Features.
*
* This function detects Features that do not correspond to known Features/Landmarks in the system.
*
* IMPORTANT: The features in _features_out should be emplaced. Don't use `make_shared`, use `FeatureBase::emplace` instead.
* Then, they will be already linked to the _capture.
*
* The function is called in ProcessorTrackerFeature::processNew() to set the member new_features_last_,
* the list of newly detected features of the capture last_ptr_.
/** \brief Emplaces a new factor for each known feature in Capture \b last
*/
unsigned int detectNewFeatures(const int& _max_new_features,
const CaptureBasePtr& _capture,
FeatureBasePtrList& _features_out) override;
void establishFactors() override;
/** \brief Emplaces a new factor
* \param _feature_ptr pointer to the parent Feature
* \param _feature_other_ptr pointer to the other feature constrained.
*
* This function emplaces a factor of the appropriate type for the derived processor.
* \param _feature feature pointer
* \param _landmark pointer to the landmark
*/
FactorBasePtr emplaceFactor(FeatureBasePtr _feature, LandmarkBasePtr _landmark);
/** \brief Emplaces a landmark or modifies (if needed) a landmark
* \param _feature_ptr pointer to the Feature
*/
FactorBasePtr emplaceFactor(FeatureBasePtr _feature_ptr,
FeatureBasePtr _feature_other_ptr) override;
LandmarkBasePtr emplaceLandmark(FeatureBasePtr _feature_ptr);
/** Pre-process incoming Capture
*
* This is called by process() just after assigning incoming_ptr_ to a valid Capture.
*
* extract the detections and fill detections_incoming_ (FeaturePtrList)
/** \brief Modifies (if needed) a landmark
* \param _landmark pointer to the landmark
* \param _feature pointer to the Feature.
*/
void preProcess() override;
void modifyLandmark(LandmarkBasePtr _landmark,
FeatureBasePtr _feature);
/** Post-process
*
......@@ -157,23 +141,16 @@ class ProcessorTrackerFeatureLandmarkExternal : public ProcessorTrackerFeature
const VectorComposite& _pose_sen) const;
};
inline ProcessorTrackerFeatureLandmarkExternal::ProcessorTrackerFeatureLandmarkExternal(ParamsProcessorTrackerFeatureLandmarkExternalPtr _params_tfle) :
ProcessorTrackerFeature("ProcessorTrackerFeatureLandmarkExternal", "PO", 0, _params_tfle),
inline ProcessorLandmarkExternal::ProcessorLandmarkExternal(ParamsProcessorLandmarkExternalPtr _params_tfle) :
ProcessorTracker("ProcessorLandmarkExternal", "PO", 0, _params_tfle),
params_tfle_(_params_tfle)
{
//
}
inline ProcessorTrackerFeatureLandmarkExternal::~ProcessorTrackerFeatureLandmarkExternal()
inline ProcessorLandmarkExternal::~ProcessorLandmarkExternal()
{
//
}
inline bool ProcessorTrackerFeatureLandmarkExternal::correctFeatureDrift(const FeatureBasePtr _origin_feature,
const FeatureBasePtr _last_feature,
FeatureBasePtr _incoming_feature)
{
return true;
}
} // namespace wolf
\ No newline at end of file
This diff is collapsed.
......@@ -158,8 +158,8 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
incoming_ptr_->link(frame);
// Process info
// TrackerLandmark: If we have been given a map, all landmarks in the map are known. Process them.
if (not getProblem()->getMap()->getLandmarkList().empty())
// If we have known landmarks or features. Process them.
if (not getProblem()->getMap()->getLandmarkList().empty() or not known_features_last_.empty())
processKnown();
// Both Trackers: We have a last_ Capture with not enough features, so populate it.
......
This diff is collapsed.
......@@ -197,15 +197,15 @@ wolf_add_gtest(gtest_map_yaml gtest_map_yaml.cpp)
# Parameter prior test
wolf_add_gtest(gtest_param_prior gtest_param_prior.cpp)
# ProcessorTrackerFeatureLandmarkExternal class test
wolf_add_gtest(gtest_processor_tracker_feature_landmark_external gtest_processor_tracker_feature_landmark_external.cpp)
# ProcessorFixedWingModel class test
wolf_add_gtest(gtest_processor_fixed_wing_model gtest_processor_fixed_wing_model.cpp)
# ProcessorDiffDrive class test
wolf_add_gtest(gtest_processor_diff_drive gtest_processor_diff_drive.cpp)
# ProcessorLandmarkExternal class test
wolf_add_gtest(gtest_processor_landmark_external gtest_processor_landmark_external.cpp)
# ProcessorLoopClosure class test
wolf_add_gtest(gtest_processor_loop_closure gtest_processor_loop_closure.cpp)
......
......@@ -33,7 +33,7 @@
#include "core/state_block/state_quaternion.h"
#include "core/ceres_wrapper/solver_ceres.h"
#include "core/processor/processor_tracker_feature_landmark_external.h"
#include "core/processor/processor_landmark_external.h"
// STL
#include <iterator>
......@@ -42,7 +42,7 @@
using namespace wolf;
using namespace Eigen;
class ProcessorTrackerFeatureLandmarkExternalTest : public testing::Test
class ProcessorLandmarkExternalTest : public testing::Test
{
protected:
int dim;
......@@ -53,7 +53,7 @@ class ProcessorTrackerFeatureLandmarkExternalTest : public testing::Test
ProblemPtr problem;
SolverManagerPtr solver;
SensorBasePtr sensor, sensor_odom;
ProcessorTrackerFeatureLandmarkExternalPtr processor;
ProcessorLandmarkExternalPtr processor;
ProcessorMotionPtr processor_motion;
std::vector<LandmarkBasePtr> landmarks;
......@@ -81,7 +81,7 @@ class ProcessorTrackerFeatureLandmarkExternalTest : public testing::Test
void assertVectorComposite(const VectorComposite& v1, const VectorComposite& v2) const;
};
void ProcessorTrackerFeatureLandmarkExternalTest::initProblem(int _dim,
void ProcessorLandmarkExternalTest::initProblem(int _dim,
bool _orientation,
double _quality_th,
double _dist_th,
......@@ -125,9 +125,8 @@ void ProcessorTrackerFeatureLandmarkExternalTest::initProblem(int _dim,
}
// Processors
ParamsProcessorTrackerFeatureLandmarkExternalPtr params = std::make_shared<ParamsProcessorTrackerFeatureLandmarkExternal>();
ParamsProcessorLandmarkExternalPtr params = std::make_shared<ParamsProcessorLandmarkExternal>();
params->time_tolerance = dt/2;
params->min_features_for_keyframe = 0;
params->max_new_features = -1;
params->voting_active = true;
params->apply_loss_function = false;
......@@ -136,7 +135,7 @@ void ProcessorTrackerFeatureLandmarkExternalTest::initProblem(int _dim,
params->filter_dist_th = _dist_th;
params->filter_track_length_th = _track_length_th;
params->time_span = _time_span;
processor = ProcessorBase::emplace<ProcessorTrackerFeatureLandmarkExternal>(sensor, params);
processor = ProcessorBase::emplace<ProcessorLandmarkExternal>(sensor, params);
if (dim == 2)
{
......@@ -190,7 +189,7 @@ void ProcessorTrackerFeatureLandmarkExternalTest::initProblem(int _dim,
state_sensor = sensor->getState("PO");
}
void ProcessorTrackerFeatureLandmarkExternalTest::randomStep()
void ProcessorLandmarkExternalTest::randomStep()
{
// compute delta
VectorXd delta;
......@@ -215,7 +214,7 @@ void ProcessorTrackerFeatureLandmarkExternalTest::randomStep()
state_robot = processor_motion->getState("PO");
}
CaptureLandmarksExternalPtr ProcessorTrackerFeatureLandmarkExternalTest::computeCaptureLandmarks() const
CaptureLandmarksExternalPtr ProcessorLandmarkExternalTest::computeCaptureLandmarks() const
{
// Detections
auto cap = std::make_shared<CaptureLandmarksExternal>(t, sensor);
......@@ -253,7 +252,7 @@ CaptureLandmarksExternalPtr ProcessorTrackerFeatureLandmarkExternalTest::compute
return cap;
}
void ProcessorTrackerFeatureLandmarkExternalTest::testConfiguration(int dim,
void ProcessorLandmarkExternalTest::testConfiguration(int dim,
bool orientation,
double quality_th,
double dist_th,
......@@ -363,7 +362,7 @@ void ProcessorTrackerFeatureLandmarkExternalTest::testConfiguration(int dim,
}
}
void ProcessorTrackerFeatureLandmarkExternalTest::assertVectorComposite(const VectorComposite& v1, const VectorComposite& v2) const
void ProcessorLandmarkExternalTest::assertVectorComposite(const VectorComposite& v1, const VectorComposite& v2) const
{
if (v1.includesStructure("PO") and v2.includesStructure("PO"))
{
......@@ -385,7 +384,7 @@ void ProcessorTrackerFeatureLandmarkExternalTest::assertVectorComposite(const Ve
}
// / TESTS //////////////////////////////////////////
TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, P_2d_existing_lmks)
TEST_F(ProcessorLandmarkExternalTest, P_2d_existing_lmks)
{
testConfiguration(2, //int dim
false, //bool orientation
......@@ -396,7 +395,7 @@ TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, P_2d_existing_lmks)
true); //bool add_landmarks
}
TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, P_2d_no_lmks)
TEST_F(ProcessorLandmarkExternalTest, P_2d_no_lmks)
{
testConfiguration(2, //int dim
false, //bool orientation
......@@ -407,7 +406,7 @@ TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, P_2d_no_lmks)
false); //bool add_landmarks
}
TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, P_2d_no_lmks_quality)
TEST_F(ProcessorLandmarkExternalTest, P_2d_no_lmks_quality)
{
testConfiguration(2, //int dim
false, //bool orientation
......@@ -418,7 +417,7 @@ TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, P_2d_no_lmks_quality)
false); //bool add_landmarks
}
TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, PO_2d_existing_lmks)
TEST_F(ProcessorLandmarkExternalTest, PO_2d_existing_lmks)
{
testConfiguration(2, //int dim
true, //bool orientation
......@@ -429,7 +428,7 @@ TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, PO_2d_existing_lmks)
true); //bool add_landmarks
}
TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, PO_2d_no_lmks)
TEST_F(ProcessorLandmarkExternalTest, PO_2d_no_lmks)
{
testConfiguration(2, //int dim
true, //bool orientation
......@@ -440,7 +439,7 @@ TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, PO_2d_no_lmks)
false); //bool add_landmarks
}
TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, PO_2d_no_lmks_quality)
TEST_F(ProcessorLandmarkExternalTest, PO_2d_no_lmks_quality)
{
testConfiguration(2, //int dim
true, //bool orientation
......@@ -451,7 +450,7 @@ TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, PO_2d_no_lmks_quality)
false); //bool add_landmarks
}
TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, P_3d_existing_lmks)
TEST_F(ProcessorLandmarkExternalTest, P_3d_existing_lmks)
{
testConfiguration(3, //int dim
false, //bool orientation
......@@ -462,7 +461,7 @@ TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, P_3d_existing_lmks)
true); //bool add_landmarks
}
TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, P_3d_no_lmks)
TEST_F(ProcessorLandmarkExternalTest, P_3d_no_lmks)
{
testConfiguration(3, //int dim
false, //bool orientation
......@@ -473,7 +472,7 @@ TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, P_3d_no_lmks)
false); //bool add_landmarks
}
TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, P_3d_no_lmks_quality)
TEST_F(ProcessorLandmarkExternalTest, P_3d_no_lmks_quality)
{
testConfiguration(3, //int dim
false, //bool orientation
......@@ -484,7 +483,7 @@ TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, P_3d_no_lmks_quality)
false); //bool add_landmarks
}
TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, PO_3d_existing_lmks)
TEST_F(ProcessorLandmarkExternalTest, PO_3d_existing_lmks)
{
testConfiguration(3, //int dim
true, //bool orientation
......@@ -495,7 +494,7 @@ TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, PO_3d_existing_lmks)
true); //bool add_landmarks
}
TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, PO_3d_no_lmks)
TEST_F(ProcessorLandmarkExternalTest, PO_3d_no_lmks)
{
testConfiguration(3, //int dim
true, //bool orientation
......@@ -506,7 +505,7 @@ TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, PO_3d_no_lmks)
false); //bool add_landmarks
}
TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, PO_3d_no_lmks_quality)
TEST_F(ProcessorLandmarkExternalTest, PO_3d_no_lmks_quality)
{
testConfiguration(3, //int dim
true, //bool orientation
......
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