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

[skip ci] wip

parent 50d13cad
No related branches found
No related tags found
1 merge request!462Resolve "Subscriber&processor for landmark external detections"
Pipeline #13869 skipped
...@@ -323,7 +323,7 @@ inline void compose(const VectorComposite& _x1, ...@@ -323,7 +323,7 @@ inline void compose(const VectorComposite& _x1,
inline VectorComposite compose(const VectorComposite& x1, const VectorComposite& x2) inline VectorComposite compose(const VectorComposite& x1, const VectorComposite& x2)
{ {
VectorComposite c("PO", {2,1}); VectorComposite c("PO", {2,1});
compose(x1.at('P'), x1.at('O'), x2.at('P'), x2.at('O'), c['P'], c['O']); compose(x1, x2, c);
return c; return c;
} }
......
...@@ -35,6 +35,7 @@ struct ParamsProcessorTrackerFeatureLandmarkExternal : public ParamsProcessorTra ...@@ -35,6 +35,7 @@ struct ParamsProcessorTrackerFeatureLandmarkExternal : public ParamsProcessorTra
double filter_quality_th; ///< min quality to consider the detection double filter_quality_th; ///< min quality to consider the detection
double filter_dist_th; ///< for considering tracked detection: distance threshold to previous detection double filter_dist_th; ///< for considering tracked detection: distance threshold to previous detection
unsigned int filter_track_length_th; ///< length of the track necessary to consider the detection 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() = default;
ParamsProcessorTrackerFeatureLandmarkExternal(std::string _unique_name, ParamsProcessorTrackerFeatureLandmarkExternal(std::string _unique_name,
...@@ -44,6 +45,7 @@ struct ParamsProcessorTrackerFeatureLandmarkExternal : public ParamsProcessorTra ...@@ -44,6 +45,7 @@ struct ParamsProcessorTrackerFeatureLandmarkExternal : public ParamsProcessorTra
filter_quality_th = _server.getParam<double> (prefix + _unique_name + "/filter_quality_th"); filter_quality_th = _server.getParam<double> (prefix + _unique_name + "/filter_quality_th");
filter_dist_th = _server.getParam<double> (prefix + _unique_name + "/filter_dist_th"); filter_dist_th = _server.getParam<double> (prefix + _unique_name + "/filter_dist_th");
filter_track_length_th = _server.getParam<unsigned int>(prefix + _unique_name + "/filter_track_length_th"); filter_track_length_th = _server.getParam<unsigned int>(prefix + _unique_name + "/filter_track_length_th");
time_span = _server.getParam<double> (prefix + _unique_name + "/keyframe_vote/time_span");
} }
}; };
...@@ -64,7 +66,7 @@ class ProcessorTrackerFeatureLandmarkExternal : public ProcessorTrackerFeature ...@@ -64,7 +66,7 @@ class ProcessorTrackerFeatureLandmarkExternal : public ProcessorTrackerFeature
protected: protected:
ParamsProcessorTrackerFeatureLandmarkExternalPtr params_tfle_; ParamsProcessorTrackerFeatureLandmarkExternalPtr params_tfle_;
FeatureBasePtrList detections_incoming_, detections_last_; std::set<FeatureBasePtr> unmatched_detections_incoming_, unmatched_detections_last_;
/** \brief Track provided features in \b _capture /** \brief Track provided features in \b _capture
* \param _features_in input list of features in \b last to track * \param _features_in input list of features in \b last to track
......
...@@ -200,11 +200,11 @@ void ProcessorTrackerFeature::establishFactors() ...@@ -200,11 +200,11 @@ void ProcessorTrackerFeature::establishFactors()
auto fac_ptr = emplaceFactor(feature_in_last, feature_in_origin); auto fac_ptr = emplaceFactor(feature_in_last, feature_in_origin);
assert(fac_ptr->getFeature() != nullptr && "not emplaced factor returned by emplaceFactor()"); assert((fac_ptr == nullptr or fac_ptr->getFeature() != nullptr) && "not emplaced factor returned by emplaceFactor()");
WOLF_DEBUG( "Factor: track: " , feature_in_last->trackId(), WOLF_DEBUG_COND(fac_ptr, "New factor: track: " , feature_in_last->trackId(),
" origin: " , feature_in_origin->id() , " origin: " , feature_in_origin->id() ,
" from last: " , feature_in_last->id() ); " from last: " , feature_in_last->id() );
} }
} }
......
...@@ -41,7 +41,7 @@ namespace wolf ...@@ -41,7 +41,7 @@ namespace wolf
void ProcessorTrackerFeatureLandmarkExternal::preProcess() void ProcessorTrackerFeatureLandmarkExternal::preProcess()
{ {
assert(detections_incoming_.empty()); assert(unmatched_detections_incoming_.empty());
auto cap_landmarks = std::dynamic_pointer_cast<CaptureLandmarksExternal>(incoming_ptr_); auto cap_landmarks = std::dynamic_pointer_cast<CaptureLandmarksExternal>(incoming_ptr_);
if (not cap_landmarks) if (not cap_landmarks)
...@@ -66,7 +66,7 @@ void ProcessorTrackerFeatureLandmarkExternal::preProcess() ...@@ -66,7 +66,7 @@ void ProcessorTrackerFeatureLandmarkExternal::preProcess()
if (detection.id != -1 and detection.id != 0) if (detection.id != -1 and detection.id != 0)
ids.insert(detection.id); ids.insert(detection.id);
detections_incoming_.push_back(ftr); unmatched_detections_incoming_.insert(ftr);
} }
} }
...@@ -75,34 +75,62 @@ unsigned int ProcessorTrackerFeatureLandmarkExternal::trackFeatures(const Featur ...@@ -75,34 +75,62 @@ unsigned int ProcessorTrackerFeatureLandmarkExternal::trackFeatures(const Featur
FeatureBasePtrList& _features_out, FeatureBasePtrList& _features_out,
FeatureMatchMap& _feature_correspondences) FeatureMatchMap& _feature_correspondences)
{ {
WOLF_INFO("tracking " , _features_in.size() , " features..."); if (_capture != incoming_ptr_)
throw std::runtime_error("ProcessorTrackerFeatureLandmarkExternal::trackFeatures should be called with incoming capture");
if (not _features_in.empty() and _features_in.front()->getCapture() != last_ptr_)
throw std::runtime_error("ProcessorTrackerFeatureLandmarkExternal::trackFeatures should be called with features in not from last");
if (_features_in.empty())
return 0;
if (_capture != last_ptr_ and _capture != incoming_ptr_)
throw std::runtime_error("ProcessorTrackerFeatureLandmarkExternal::trackFeatures unknown capture");
FeatureBasePtrList& landmark_detections = (_capture == last_ptr_ ? detections_last_ : detections_incoming_);
assert(not _features_in.front() and not _features_in.front()->getCapture());
auto pose_sen = getSensor()->getState("PO"); auto pose_sen = getSensor()->getState("PO");
auto pose_in = getProblem()->getState(_features_in.front()->getCapture()->getTimeStamp(), "PO"); auto pose_in = getProblem()->getState(last_ptr_->getTimeStamp(), "PO");
auto pose_out = getProblem()->getState(_capture->getTimeStamp(), "PO"); auto pose_out = getProblem()->getState(incoming_ptr_->getTimeStamp(), "PO");
// Track features given by ProcessorTrackerFeature
for (auto feat_in : _features_in) for (auto feat_in : _features_in)
{ {
for (auto feat_candidate : landmark_detections) for (auto feat_candidate : unmatched_detections_incoming_)
{
if (feat_candidate->landmarkId() == feat_in->landmarkId() and
detectionDistance(feat_in, feat_candidate, pose_in, pose_out, pose_sen) < params_tfle_->filter_dist_th)
{
// add match
_features_out.push_back(feat_candidate);
_feature_correspondences[_features_out.back()] = std::make_shared<FeatureMatch>(FeatureMatch({feat_in,0}));
// remove from unmatched
unmatched_detections_incoming_.erase(feat_candidate);
break;
}
}
}
// Track features in last not matched yet
auto feat_last_it = unmatched_detections_last_.begin();
while (feat_last_it != unmatched_detections_last_.end())
{
auto feat_in = *feat_last_it;
bool found = false;
for (auto feat_candidate : unmatched_detections_incoming_)
{ {
if (feat_candidate->landmarkId() == feat_in->landmarkId() and if (feat_candidate->landmarkId() == feat_in->landmarkId() and
detectionDistance(feat_in, feat_candidate, pose_in, pose_out, pose_sen) < params_tfle_->filter_dist_th) detectionDistance(feat_in, feat_candidate, pose_in, pose_out, pose_sen) < params_tfle_->filter_dist_th)
{ {
// add track
track_matrix_.newTrack(feat_in);
// add match
_features_out.push_back(feat_candidate); _features_out.push_back(feat_candidate);
_feature_correspondences[_features_out.back()] = std::make_shared<FeatureMatch>(FeatureMatch({feat_in,0})); _feature_correspondences[_features_out.back()] = std::make_shared<FeatureMatch>(FeatureMatch({feat_in,0}));
WOLF_INFO("track: " , feat_in->trackId() , " last: " , feat_in->id() , " inc: " , feat_candidate->id() , " !" ); // remove from unmatched
unmatched_detections_incoming_.erase(feat_candidate);
found = true;
break;
} }
} }
if (found)
feat_last_it = unmatched_detections_last_.erase(feat_last_it);
else
feat_last_it++;
} }
return _features_out.size(); return _features_out.size();
...@@ -145,45 +173,37 @@ double ProcessorTrackerFeatureLandmarkExternal::detectionDistance(FeatureBasePtr ...@@ -145,45 +173,37 @@ double ProcessorTrackerFeatureLandmarkExternal::detectionDistance(FeatureBasePtr
bool ProcessorTrackerFeatureLandmarkExternal::voteForKeyFrame() const bool ProcessorTrackerFeatureLandmarkExternal::voteForKeyFrame() const
{ {
WOLF_INFO("Nbr. of active feature tracks: " , incoming_ptr_->getFeatureList().size() ); auto matches_origin_last = track_matrix_.matches(origin_ptr_, last_ptr_);
WOLF_INFO("Nbr. of active feature tracks: " , matches_origin_last.size() );
bool vote = incoming_ptr_->getFeatureList().size() < params_tracker_feature_->min_features_for_keyframe; bool vote = matches_origin_last.size() < params_tracker_feature_->min_features_for_keyframe;
if (origin_ptr_)
{
WOLF_INFO("Time span since last frame: " , last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp() );
vote = vote or last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp() > params_tfle_->time_span;
}
WOLF_INFO( (vote ? "Vote ": "Do not vote ") , "for KF" ); WOLF_INFO( (vote ? "Vote ": "Do not vote ") , "for KF" );
return incoming_ptr_->getFeatureList().size() < params_tracker_feature_->min_features_for_keyframe; return vote;
} }
unsigned int ProcessorTrackerFeatureLandmarkExternal::detectNewFeatures(const int& _max_new_features, unsigned int ProcessorTrackerFeatureLandmarkExternal::detectNewFeatures(const int& _max_new_features,
const CaptureBasePtr& _capture, const CaptureBasePtr& _capture,
FeatureBasePtrList& _features_out) FeatureBasePtrList& _features_out)
{ {
if (_capture != last_ptr_ and _capture != incoming_ptr_) // NOT NECESSARY SINCE ALL FEATURES ARE TRACKED IN trackFeatures(...)
throw std::runtime_error("ProcessorTrackerFeatureLandmarkExternal::detectNewFeatures unknown capture"); return 0;
auto cap_landmarks = std::dynamic_pointer_cast<CaptureLandmarksExternal>(_capture);
if (not cap_landmarks)
throw std::runtime_error("ProcessorTrackerFeatureLandmarkExternal capture should be of type 'CaptureLandmarksExternal'");
// detecting new features
FeatureBasePtrList& landmark_detections = (_capture == last_ptr_ ? detections_last_ : detections_incoming_);
while (not landmark_detections.empty())
{
_features_out.push_back(landmark_detections.front());
landmark_detections.pop_front();
if (_max_new_features != -1 and _features_out.size() >= _max_new_features)
break;
}
WOLF_INFO(_features_out.size() , " new features detected!");
return _features_out.size();
} }
FactorBasePtr ProcessorTrackerFeatureLandmarkExternal::emplaceFactor(FeatureBasePtr _feature_ptr, FactorBasePtr ProcessorTrackerFeatureLandmarkExternal::emplaceFactor(FeatureBasePtr _feature_ptr,
FeatureBasePtr _feature_other_ptr) FeatureBasePtr _feature_other_ptr)
{ {
assert(_feature_ptr);
assert(_feature_other_ptr);
assert(getProblem()); assert(getProblem());
assert(getProblem()->getMap()); assert(getProblem()->getMap());
...@@ -212,8 +232,8 @@ FactorBasePtr ProcessorTrackerFeatureLandmarkExternal::emplaceFactor(FeatureBase ...@@ -212,8 +232,8 @@ FactorBasePtr ProcessorTrackerFeatureLandmarkExternal::emplaceFactor(FeatureBase
{ {
Vector2d frm_p = _feature_ptr->getCapture()->getFrame()->getP()->getState(); Vector2d frm_p = _feature_ptr->getCapture()->getFrame()->getP()->getState();
Vector2d sen_p = _feature_ptr->getCapture()->getP()->getState(); Vector2d sen_p = _feature_ptr->getCapture()->getP()->getState();
double frm_o = _feature_ptr->getCapture()->getFrame()->getO()->getState()(0); double frm_o = _feature_ptr->getCapture()->getFrame()->getO()->getState()(0);
double sen_o = _feature_ptr->getCapture()->getO()->getState()(0); double sen_o = _feature_ptr->getCapture()->getO()->getState()(0);
Vector2d lmk_p = frm_p + Rotation2Dd(frm_o) * (sen_p + Rotation2Dd(sen_o) * _feature_ptr->getMeasurement()); Vector2d lmk_p = frm_p + Rotation2Dd(frm_o) * (sen_p + Rotation2Dd(sen_o) * _feature_ptr->getMeasurement());
...@@ -239,11 +259,11 @@ FactorBasePtr ProcessorTrackerFeatureLandmarkExternal::emplaceFactor(FeatureBase ...@@ -239,11 +259,11 @@ FactorBasePtr ProcessorTrackerFeatureLandmarkExternal::emplaceFactor(FeatureBase
{ {
Vector2d frm_p = _feature_ptr->getCapture()->getFrame()->getP()->getState(); Vector2d frm_p = _feature_ptr->getCapture()->getFrame()->getP()->getState();
Vector2d sen_p = _feature_ptr->getCapture()->getP()->getState(); Vector2d sen_p = _feature_ptr->getCapture()->getP()->getState();
double frm_o = _feature_ptr->getCapture()->getFrame()->getO()->getState()(0); double frm_o = _feature_ptr->getCapture()->getFrame()->getO()->getState()(0);
double sen_o = _feature_ptr->getCapture()->getO()->getState()(0); double sen_o = _feature_ptr->getCapture()->getO()->getState()(0);
Vector2d lmk_p = frm_p + Rotation2Dd(frm_o) * (sen_p + Rotation2Dd(sen_o) * _feature_ptr->getMeasurement().head<2>()); Vector2d lmk_p = frm_p + Rotation2Dd(frm_o) * (sen_p + Rotation2Dd(sen_o) * _feature_ptr->getMeasurement().head<2>());
double lmk_o = frm_o + sen_o + _feature_ptr->getMeasurement()(2); double lmk_o = frm_o + sen_o + _feature_ptr->getMeasurement()(2);
lmk = LandmarkBase::emplace<LandmarkBase>(getProblem()->getMap(), lmk = LandmarkBase::emplace<LandmarkBase>(getProblem()->getMap(),
"LandmarkRelativePose2d", "LandmarkRelativePose2d",
...@@ -277,8 +297,8 @@ FactorBasePtr ProcessorTrackerFeatureLandmarkExternal::emplaceFactor(FeatureBase ...@@ -277,8 +297,8 @@ FactorBasePtr ProcessorTrackerFeatureLandmarkExternal::emplaceFactor(FeatureBase
// no landmark, create it // no landmark, create it
if (not lmk) if (not lmk)
{ {
Vector3d frm_p = _feature_ptr->getCapture()->getFrame()->getP()->getState(); Vector3d frm_p = _feature_ptr->getCapture()->getFrame()->getP()->getState();
Vector3d sen_p = _feature_ptr->getCapture()->getP()->getState(); Vector3d sen_p = _feature_ptr->getCapture()->getP()->getState();
Quaterniond frm_o = Quaterniond(Vector4d(_feature_ptr->getCapture()->getFrame()->getO()->getState())); Quaterniond frm_o = Quaterniond(Vector4d(_feature_ptr->getCapture()->getFrame()->getO()->getState()));
Quaterniond sen_o = Quaterniond(Vector4d(_feature_ptr->getCapture()->getO()->getState())); Quaterniond sen_o = Quaterniond(Vector4d(_feature_ptr->getCapture()->getO()->getState()));
...@@ -304,12 +324,12 @@ FactorBasePtr ProcessorTrackerFeatureLandmarkExternal::emplaceFactor(FeatureBase ...@@ -304,12 +324,12 @@ FactorBasePtr ProcessorTrackerFeatureLandmarkExternal::emplaceFactor(FeatureBase
// no landmark, create it // no landmark, create it
if (not lmk) if (not lmk)
{ {
Vector3d frm_p = _feature_ptr->getCapture()->getFrame()->getP()->getState(); Vector3d frm_p = _feature_ptr->getCapture()->getFrame()->getP()->getState();
Vector3d sen_p = _feature_ptr->getCapture()->getP()->getState(); Vector3d sen_p = _feature_ptr->getCapture()->getP()->getState();
Quaterniond frm_o = Quaterniond(Vector4d(_feature_ptr->getCapture()->getFrame()->getO()->getState())); Quaterniond frm_o = Quaterniond(Vector4d(_feature_ptr->getCapture()->getFrame()->getO()->getState()));
Quaterniond sen_o = Quaterniond(Vector4d(_feature_ptr->getCapture()->getO()->getState())); Quaterniond sen_o = Quaterniond(Vector4d(_feature_ptr->getCapture()->getO()->getState()));
Vector3d lmk_p = frm_p + frm_o * (sen_p + sen_o * _feature_ptr->getMeasurement().head<3>()); Vector3d lmk_p = frm_p + frm_o * (sen_p + sen_o * _feature_ptr->getMeasurement().head<3>());
Quaterniond lmk_o = frm_o * sen_o * Quaterniond(_feature_ptr->getMeasurement().tail<4>()); Quaterniond lmk_o = frm_o * sen_o * Quaterniond(_feature_ptr->getMeasurement().tail<4>());
lmk = LandmarkBase::emplace<LandmarkBase>(getProblem()->getMap(), lmk = LandmarkBase::emplace<LandmarkBase>(getProblem()->getMap(),
...@@ -347,13 +367,13 @@ void ProcessorTrackerFeatureLandmarkExternal::advanceDerived() ...@@ -347,13 +367,13 @@ void ProcessorTrackerFeatureLandmarkExternal::advanceDerived()
{ {
ProcessorTrackerFeature::advanceDerived(); ProcessorTrackerFeature::advanceDerived();
detections_last_ = std::move(detections_incoming_); unmatched_detections_last_ = std::move(unmatched_detections_incoming_);
} }
void ProcessorTrackerFeatureLandmarkExternal::resetDerived() void ProcessorTrackerFeatureLandmarkExternal::resetDerived()
{ {
ProcessorTrackerFeature::resetDerived(); ProcessorTrackerFeature::resetDerived();
detections_last_ = std::move(detections_incoming_); unmatched_detections_last_ = std::move(unmatched_detections_incoming_);
} }
} // namespace wolf } // namespace wolf
......
...@@ -24,6 +24,10 @@ ...@@ -24,6 +24,10 @@
#include "core/problem/problem.h" #include "core/problem/problem.h"
#include "core/capture/capture_landmarks_external.h" #include "core/capture/capture_landmarks_external.h"
#include "core/landmark/landmark_base.h" #include "core/landmark/landmark_base.h"
#include "core/sensor/sensor_odom_2d.h"
#include "core/sensor/sensor_odom_3d.h"
#include "core/processor/processor_odom_2d.h"
#include "core/processor/processor_odom_3d.h"
#include "core/state_block/state_block_derived.h" #include "core/state_block/state_block_derived.h"
#include "core/state_block/state_angle.h" #include "core/state_block/state_angle.h"
#include "core/state_block/state_quaternion.h" #include "core/state_block/state_quaternion.h"
...@@ -46,20 +50,31 @@ class ProcessorTrackerFeatureLandmarkExternalTest : public testing::Test ...@@ -46,20 +50,31 @@ class ProcessorTrackerFeatureLandmarkExternalTest : public testing::Test
double dt; double dt;
ProblemPtr problem; ProblemPtr problem;
SensorBasePtr sensor; SensorBasePtr sensor, sensor_odom;
ProcessorTrackerFeatureLandmarkExternalPtr processor; ProcessorTrackerFeatureLandmarkExternalPtr processor;
ProcessorMotionPtr processor_motion;
std::vector<LandmarkBasePtr> landmarks; std::vector<LandmarkBasePtr> landmarks;
// Groundtruth poses // Groundtruth poses
VectorXd p_robot, o_robot, p_sensor, o_sensor; VectorXd p_robot, o_robot, p_sensor, o_sensor;
virtual void SetUp() {}; virtual void SetUp() {};
void initProblem(int _dim, bool _orientation); void initProblem(int _dim,
bool _orientation,
double _quality_th,
double _dist_th,
unsigned int _track_length_th,
double _time_span);
CaptureLandmarksExternalPtr randomStep(); CaptureLandmarksExternalPtr randomStep();
void addRandomDetection(CaptureLandmarksExternalPtr _cap, int _id) const; void addRandomDetection(CaptureLandmarksExternalPtr _cap, int _id, double _quality) const;
}; };
void ProcessorTrackerFeatureLandmarkExternalTest::initProblem(int _dim, bool _orientation) void ProcessorTrackerFeatureLandmarkExternalTest::initProblem(int _dim,
bool _orientation,
double _quality_th,
double _dist_th,
unsigned int _track_length_th,
double _time_span)
{ {
dim = _dim; dim = _dim;
orientation = _orientation; orientation = _orientation;
...@@ -68,35 +83,68 @@ void ProcessorTrackerFeatureLandmarkExternalTest::initProblem(int _dim, bool _or ...@@ -68,35 +83,68 @@ void ProcessorTrackerFeatureLandmarkExternalTest::initProblem(int _dim, bool _or
problem = Problem::create("PO", dim); problem = Problem::create("PO", dim);
// Sensors
if (dim == 2) if (dim == 2)
{
sensor = SensorBase::emplace<SensorBase>(problem->getHardware(), sensor = SensorBase::emplace<SensorBase>(problem->getHardware(),
"SensorBase", "SensorBase",
std::make_shared<StatePoint2d>(Vector2d::Random()), std::make_shared<StatePoint2d>(Vector2d::Random()),
std::make_shared<StateAngle>(Vector1d::Random() * M_PI), std::make_shared<StateAngle>(Vector1d::Random() * M_PI),
nullptr, nullptr,
2); 2);
sensor_odom = SensorBase::emplace<SensorOdom2d>(problem->getHardware(),
Vector3d::Zero(),
ParamsSensorOdom2d());
}
else else
{
sensor = SensorBase::emplace<SensorBase>(problem->getHardware(), sensor = SensorBase::emplace<SensorBase>(problem->getHardware(),
"SensorBase", "SensorBase",
std::make_shared<StatePoint3d>(Vector3d::Random()), std::make_shared<StatePoint3d>(Vector3d::Random()),
std::make_shared<StateQuaternion>(Vector4d::Random().normalized()), std::make_shared<StateQuaternion>(Vector4d::Random().normalized()),
nullptr, nullptr,
3); 3);
sensor_odom = SensorBase::emplace<SensorOdom3d>(problem->getHardware(),
(Vector7d() << 0,0,0,0,0,0,1).finished(),
ParamsSensorOdom3d());
}
// Emplace processor // Processors
ParamsProcessorTrackerFeatureLandmarkExternalPtr params = std::make_shared<ParamsProcessorTrackerFeatureLandmarkExternal>(); ParamsProcessorTrackerFeatureLandmarkExternalPtr params = std::make_shared<ParamsProcessorTrackerFeatureLandmarkExternal>();
params->time_tolerance = dt/2; params->time_tolerance = dt/2;
params->min_features_for_keyframe = 0; params->min_features_for_keyframe = 0;
params->max_new_features = -1; params->max_new_features = -1;
params->voting_active = true; params->voting_active = true;
params->apply_loss_function = false; params->apply_loss_function = false;
params->filter_quality_th = _quality_th;
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<ProcessorTrackerFeatureLandmarkExternal>(sensor, params);
if (dim == 2)
{
auto params_odom = std::make_shared<ParamsProcessorOdom2d>();
params_odom->state_getter = true;
params_odom->voting_active = false;
processor_motion = std::static_pointer_cast<ProcessorMotion>(ProcessorBase::emplace<ProcessorOdom2d>(sensor_odom,
params_odom));
}
else
{
auto params_odom = std::make_shared<ParamsProcessorOdom3d>();
params_odom->state_getter = true;
params_odom->voting_active = false;
processor_motion = std::static_pointer_cast<ProcessorMotion>(ProcessorBase::emplace<ProcessorOdom3d>(sensor_odom,
params_odom));
}
// Emplace frame // Emplace frame
auto F0 = problem->emplaceFrame(t, (dim == 2 ? VectorXd::Zero(3) : (VectorXd(7) << 0,0,0,0,0,0,1).finished())); auto F0 = problem->emplaceFrame(t, (dim == 2 ? VectorXd::Zero(3) : (VectorXd(7) << 0,0,0,0,0,0,1).finished()));
F0->fix(); F0->fix();
problem->keyFrameCallback(F0, nullptr); processor->keyFrameCallback(F0);
processor_motion->setOrigin(F0);
// Emplace 3 random landmarks // Emplace 3 random landmarks
for (auto i = 0; i < 3; i++) for (auto i = 0; i < 3; i++)
...@@ -122,20 +170,34 @@ void ProcessorTrackerFeatureLandmarkExternalTest::initProblem(int _dim, bool _or ...@@ -122,20 +170,34 @@ void ProcessorTrackerFeatureLandmarkExternalTest::initProblem(int _dim, bool _or
} }
// Store groundtruth poses // Store groundtruth poses
p_robot = F0->getP()->getState(); p_robot = F0->getP()->getState();
o_robot = F0->getO()->getState(); o_robot = F0->getO()->getState();
p_sensor = sensor->getP()->getState(); p_sensor = sensor->getP()->getState();
o_sensor = sensor->getO()->getState(); o_sensor = sensor->getO()->getState();
} }
CaptureLandmarksExternalPtr ProcessorTrackerFeatureLandmarkExternalTest::randomStep() CaptureLandmarksExternalPtr ProcessorTrackerFeatureLandmarkExternalTest::randomStep()
{ {
// update groundtruth pose with random movement // compute delta
p_robot += VectorXd::Random(dim) * 0.1; VectorXd delta;
if (dim == 2) if (dim == 2)
o_robot(0) = pi2pi(o_robot(0) + Vector1d::Random()(0) * 0.1); {
delta = Vector2d::Random() * 0.1;
}
else else
o_robot = (o_robot + Vector4d::Random() * 0.1).normalized(); {
delta = Vector7d::Random() * 0.1;
delta.tail<4>().normalize();
}
// Process odometry
auto cap_odom = std::make_shared<CaptureMotion>("CaptureOdom", t, sensor_odom, delta, nullptr);
cap_odom->process();
// update groundtruth pose with random movement
auto state = processor_motion->getState("PO");
p_robot = state.vector("P");
o_robot = state.vector("O");
// Detections // Detections
auto cap = std::make_shared<CaptureLandmarksExternal>(t, sensor); auto cap = std::make_shared<CaptureLandmarksExternal>(t, sensor);
...@@ -157,7 +219,7 @@ CaptureLandmarksExternalPtr ProcessorTrackerFeatureLandmarkExternalTest::randomS ...@@ -157,7 +219,7 @@ CaptureLandmarksExternalPtr ProcessorTrackerFeatureLandmarkExternalTest::randomS
else else
{ {
auto q_sensor = Quaterniond(Vector4d(o_sensor)); auto q_sensor = Quaterniond(Vector4d(o_sensor));
auto q_robot = Quaterniond(Vector4d(o_robot)); auto q_robot = Quaterniond(Vector4d(o_robot));
meas.head(3) = q_sensor.conjugate() * (q_robot.conjugate() * (p_lmk - p_robot) - p_sensor); meas.head(3) = q_sensor.conjugate() * (q_robot.conjugate() * (p_lmk - p_robot) - p_sensor);
if (orientation) if (orientation)
...@@ -168,13 +230,13 @@ CaptureLandmarksExternalPtr ProcessorTrackerFeatureLandmarkExternalTest::randomS ...@@ -168,13 +230,13 @@ CaptureLandmarksExternalPtr ProcessorTrackerFeatureLandmarkExternalTest::randomS
cov = MatrixXd::Identity(6, 6); cov = MatrixXd::Identity(6, 6);
// add detection // add detection
cap->addDetection(meas, cov, landmarks.at(i)->id()); cap->addDetection(landmarks.at(i)->id(), meas, cov, (double) i / (double) landmarks.size());
} }
return cap; return cap;
} }
void ProcessorTrackerFeatureLandmarkExternalTest::addRandomDetection(CaptureLandmarksExternalPtr _cap, int _id) const void ProcessorTrackerFeatureLandmarkExternalTest::addRandomDetection(CaptureLandmarksExternalPtr _cap, int _id, double _quality) const
{ {
VectorXd detection; VectorXd detection;
MatrixXd cov; MatrixXd cov;
...@@ -206,63 +268,119 @@ void ProcessorTrackerFeatureLandmarkExternalTest::addRandomDetection(CaptureLand ...@@ -206,63 +268,119 @@ void ProcessorTrackerFeatureLandmarkExternalTest::addRandomDetection(CaptureLand
cov = Matrix3d::Identity()*0.1; cov = Matrix3d::Identity()*0.1;
} }
} }
_cap->addDetection(detection, cov, _id); _cap->addDetection(_id, detection, cov, _quality);
} }
// TESTS ///////////////////////////////////////////////////////////////////////////////// // TESTS /////////////////////////////////////////////////////////////////////////////////
TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, check_p_2d) TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, check_p_2d)
{ {
initProblem(2, false); int dim = 2;
bool orientation = false;
double quality_th = 0;
double dist_th = 1e6;
int track_length = 5;
double time_span = 3*dt;
initProblem(dim, orientation, quality_th, dist_th, track_length, time_span);
ASSERT_TRUE(problem->check()); ASSERT_TRUE(problem->check());
for (auto i = 0; i<10; i++) for (auto i = 0; i<10; i++)
{ {
t+=dt;
WOLF_INFO("STEP ", i, " t = ", t, " =================");
auto cap = randomStep(); auto cap = randomStep();
ASSERT_TRUE(problem->check()); ASSERT_TRUE(problem->check());
cap->process(); cap->process();
ASSERT_TRUE(problem->check()); ASSERT_TRUE(problem->check());
t+=dt; problem->print(4,1,1,1);
// Check vote for keyframe
if (t.getSeconds() >= time_span)
{
ASSERT_TRUE(problem->getTrajectory()->size() > 1);
}
FactorBasePtrList fac_list;
problem->getTrajectory()->getFactorList(fac_list);
if (i>=track_length-1)
{
// Check track_length
ASSERT_FALSE(fac_list.empty());
}
else
{
// CHECK track_length
ASSERT_TRUE(fac_list.empty());
// Check factors
for (auto fac : fac_list)
{
// Correct type
if (fac->getProcessor() == processor)
{
if (dim==2 and orientation)
{
ASSERT_EQ(fac->getType(), "FactorRelativePose2dWithExtrinsiscs");
}
if (dim==3 and not orientation)
{
ASSERT_EQ(fac->getType(), "FactorRelativePose3dWithExtrinsiscs");
}
if (dim==2 and orientation)
{
ASSERT_EQ(fac->getType(), "FactorRelativePosition2dWithExtrinsiscs");
}
if (dim==3 and not orientation)
{
ASSERT_EQ(fac->getType(), "FactorRelativePosition3dWithExtrinsiscs");
}
}
}
}
} }
} }
TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, check_po_2d) // TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, check_po_2d)
{ // {
initProblem(2, true); // initProblem(2, true);
ASSERT_TRUE(problem->check()); // ASSERT_TRUE(problem->check());
auto cap = randomStep(); // auto cap = randomStep();
ASSERT_TRUE(problem->check()); // ASSERT_TRUE(problem->check());
cap->process(); // cap->process();
ASSERT_TRUE(problem->check()); // ASSERT_TRUE(problem->check());
} // }
TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, check_p_3d) // TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, check_p_3d)
{ // {
initProblem(3, false); // initProblem(3, false);
ASSERT_TRUE(problem->check()); // ASSERT_TRUE(problem->check());
auto cap = randomStep(); // auto cap = randomStep();
ASSERT_TRUE(problem->check()); // ASSERT_TRUE(problem->check());
cap->process(); // cap->process();
ASSERT_TRUE(problem->check()); // ASSERT_TRUE(problem->check());
} // }
TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, check_po_3d) // TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, check_po_3d)
{ // {
initProblem(3, true); // initProblem(3, true);
ASSERT_TRUE(problem->check()); // ASSERT_TRUE(problem->check());
auto cap = randomStep(); // auto cap = randomStep();
ASSERT_TRUE(problem->check()); // ASSERT_TRUE(problem->check());
cap->process(); // cap->process();
ASSERT_TRUE(problem->check()); // ASSERT_TRUE(problem->check());
} // }
......
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