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

processor_tracker_landmark_dummy working with LandmarkBase and FactorLandmarkDummy

parent ecb4ebf5
No related branches found
No related tags found
1 merge request!278Resolve "Revisit demos (formerly called examples) and update them"
......@@ -13,22 +13,37 @@
namespace wolf
{
WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTrackerLandmarkDummy);
struct ProcessorParamsTrackerLandmarkDummy : public ProcessorParamsTrackerLandmark
{
unsigned int loss_lmk_ratio; ///< ratio of loosing lmks one of each n
ProcessorParamsTrackerLandmarkDummy() = default;
ProcessorParamsTrackerLandmarkDummy(std::string _unique_name, const wolf::paramsServer & _server):
ProcessorParamsTrackerLandmark(_unique_name, _server)
{
loss_lmk_ratio = _server.getParam<unsigned int>(_unique_name + "/loss_lmk_ratio", "10");
}
};
WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmarkDummy);
class ProcessorTrackerLandmarkDummy : public ProcessorTrackerLandmark
{
public:
ProcessorTrackerLandmarkDummy(ProcessorParamsTrackerLandmarkPtr _params_tracker_landmark);
ProcessorTrackerLandmarkDummy(ProcessorParamsTrackerLandmarkDummyPtr _params_tracker_landmark_dummy);
virtual ~ProcessorTrackerLandmarkDummy();
virtual void configure(SensorBasePtr _sensor) { };
protected:
ProcessorParamsTrackerLandmarkDummyPtr params_tracker_landmark_dummy_;
unsigned int n_feature_;
unsigned int landmark_idx_non_visible_;
// virtual void preProcess() { }
virtual void postProcess(); // implemented
//virtual void preProcess() { }
//virtual void postProcess();
/** \brief Find provided landmarks in the incoming capture
* \param _landmarks_in input list of landmarks to be found in incoming
......@@ -74,20 +89,6 @@ class ProcessorTrackerLandmarkDummy : public ProcessorTrackerLandmark
virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr);
};
inline void ProcessorTrackerLandmarkDummy::postProcess()
{
landmark_idx_non_visible_++;
std::cout << "------- Landmarks until " << landmark_idx_non_visible_ << " are now out of scope" << std::endl
<< std::endl;
}
} // namespace wolf
// IMPLEMENTATION
namespace wolf
{
} // namespace wolf
#endif /* PROCESSOR_TRACKER_LANDMARK_DUMMY_H_ */
......@@ -6,16 +6,16 @@
*/
#include "core/processor/processor_tracker_landmark_dummy.h"
#include "core/landmark/landmark_corner_2D.h"
#include "core/factor/factor_corner_2D.h"
#include "core/landmark/landmark_base.h"
#include "core/factor/factor_landmark_dummy.h"
namespace wolf
{
ProcessorTrackerLandmarkDummy::ProcessorTrackerLandmarkDummy(ProcessorParamsTrackerLandmarkPtr _params_tracker_landmark) :
ProcessorTrackerLandmark("TRACKER LANDMARK DUMMY", _params_tracker_landmark),
n_feature_(0),
landmark_idx_non_visible_(0)
ProcessorTrackerLandmarkDummy::ProcessorTrackerLandmarkDummy(ProcessorParamsTrackerLandmarkDummyPtr _params_tracker_landmark_dummy) :
ProcessorTrackerLandmark("TRACKER LANDMARK DUMMY", _params_tracker_landmark_dummy),
params_tracker_landmark_dummy_(_params_tracker_landmark_dummy),
n_feature_(0)
{
//
......@@ -33,23 +33,19 @@ unsigned int ProcessorTrackerLandmarkDummy::findLandmarks(const LandmarkBasePtrL
std::cout << "\tProcessorTrackerLandmarkDummy::findLandmarks" << std::endl;
std::cout << "\t\t" << _landmarks_in.size() << " landmarks..." << std::endl;
// loosing the track of the first 2 features
auto landmarks_lost = 0;
// loosing the track of the first landmark_idx_non_visible_ features
for (auto landmark_in_ptr : _landmarks_in)
{
if (landmark_in_ptr->getDescriptor(0) <= landmark_idx_non_visible_)
{
landmarks_lost++;
if (landmark_in_ptr->id() % params_tracker_landmark_dummy_->loss_lmk_ratio == 0)
std::cout << "\t\tlandmark " << landmark_in_ptr->getDescriptor() << " lost!" << std::endl;
}
else
{
_features_incoming_out.push_back(std::make_shared<FeatureBase>(
"POINT IMAGE",
landmark_in_ptr->getDescriptor(),
Eigen::MatrixXs::Identity(1,1)));
_feature_landmark_correspondences[_features_incoming_out.back()] = std::make_shared<LandmarkMatch>(landmark_in_ptr, 1);
std::cout << "\t\tlandmark " << landmark_in_ptr->getDescriptor() << " found!" << std::endl;
FeatureBasePtr ftr(std::make_shared<FeatureBase>("DUMMY FEATURE",
n_feature_* Eigen::Vector1s::Ones(),
Eigen::MatrixXs::Ones(1, 1)));
_features_incoming_out.push_back(ftr);
_feature_landmark_correspondences[ftr] = std::make_shared<LandmarkMatch>(landmark_in_ptr, 1);
std::cout << "\t\tlandmark " << landmark_in_ptr->id() << " found!" << std::endl;
}
}
return _features_incoming_out.size();
......@@ -74,31 +70,37 @@ unsigned int ProcessorTrackerLandmarkDummy::detectNewFeatures(const int& _max_fe
max_features = 10;
WOLF_INFO("max_features unlimited, setting it to " , max_features);
}
WOLF_INFO("Detecting " , _max_features , " new features..." );
// detecting new features
for (unsigned int i = 1; i <= max_features; i++)
for (unsigned int i = 0; i < max_features; i++)
{
n_feature_++;
_features_incoming_out.push_back(std::make_shared<FeatureBase>("POINT IMAGE",
n_feature_ * Eigen::Vector1s::Ones(),
Eigen::MatrixXs::Ones(1, 1)));
std::cout << "\t\tfeature " << _features_incoming_out.back()->getMeasurement() << " detected!" << std::endl;
FeatureBasePtr ftr(std::make_shared<FeatureBase>("DUMMY FEATURE",
n_feature_* Eigen::Vector1s::Ones(),
Eigen::MatrixXs::Ones(1, 1)));
_features_incoming_out.push_back(ftr);
WOLF_INFO("feature " , ftr->id() , " detected!" );
}
WOLF_INFO(_features_incoming_out.size() , " features detected!");
return _features_incoming_out.size();
}
LandmarkBasePtr ProcessorTrackerLandmarkDummy::createLandmark(FeatureBasePtr _feature_ptr)
{
//std::cout << "ProcessorTrackerLandmarkDummy::createLandmark" << std::endl;
return std::make_shared<LandmarkCorner2D>(std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1), _feature_ptr->getMeasurement(0));
return std::make_shared<LandmarkBase>("BASE", std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1));
}
FactorBasePtr ProcessorTrackerLandmarkDummy::createFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr)
{
std::cout << "\tProcessorTrackerLandmarkDummy::createFactor" << std::endl;
std::cout << "\t\tfeature " << _feature_ptr->getMeasurement() << std::endl;
std::cout << "\t\tlandmark "<< _landmark_ptr->getDescriptor() << std::endl;
return std::make_shared<FactorCorner2D>(_feature_ptr, std::static_pointer_cast<LandmarkCorner2D>(_landmark_ptr), shared_from_this());
std::cout << "\t\tfeature " << _feature_ptr->id() << std::endl;
std::cout << "\t\tlandmark "<< _landmark_ptr->id() << std::endl;
return std::make_shared<FactorLandmarkDummy>(_feature_ptr, _landmark_ptr, shared_from_this());
}
} //namespace wolf
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