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

uops

parent bdd11183
No related branches found
No related tags found
1 merge request!462Resolve "Subscriber&processor for landmark external detections"
Pipeline #13885 passed
...@@ -175,9 +175,19 @@ bool ProcessorTrackerFeatureLandmarkExternal::voteForKeyFrame() const ...@@ -175,9 +175,19 @@ bool ProcessorTrackerFeatureLandmarkExternal::voteForKeyFrame() const
{ {
auto matches_origin_last = track_matrix_.matches(origin_ptr_, last_ptr_); auto matches_origin_last = track_matrix_.matches(origin_ptr_, last_ptr_);
WOLF_INFO("Nbr. of active feature tracks: " , matches_origin_last.size() ); // no tracks longer than filter_track_length_th
auto n_tracks = 0;
for (auto match : matches_origin_last)
{
if (track_matrix_.track(match.first).size() >= params_tfle_->filter_track_length_th)
n_tracks++;
}
if (n_tracks == 0)
return false;
WOLF_INFO("Nbr. of active feature tracks: " , n_tracks );
bool vote = matches_origin_last.size() < params_tracker_feature_->min_features_for_keyframe; bool vote = n_tracks < params_tracker_feature_->min_features_for_keyframe;
if (origin_ptr_) if (origin_ptr_)
{ {
......
...@@ -65,7 +65,8 @@ class ProcessorTrackerFeatureLandmarkExternalTest : public testing::Test ...@@ -65,7 +65,8 @@ class ProcessorTrackerFeatureLandmarkExternalTest : public testing::Test
double _dist_th, double _dist_th,
unsigned int _track_length_th, unsigned int _track_length_th,
double _time_span); double _time_span);
CaptureLandmarksExternalPtr randomStep(); void randomStep();
CaptureLandmarksExternalPtr computeCaptureLandmarks() const;
void addRandomDetection(CaptureLandmarksExternalPtr _cap, int _id, double _quality) const; void addRandomDetection(CaptureLandmarksExternalPtr _cap, int _id, double _quality) const;
}; };
...@@ -176,7 +177,7 @@ void ProcessorTrackerFeatureLandmarkExternalTest::initProblem(int _dim, ...@@ -176,7 +177,7 @@ void ProcessorTrackerFeatureLandmarkExternalTest::initProblem(int _dim,
o_sensor = sensor->getO()->getState(); o_sensor = sensor->getO()->getState();
} }
CaptureLandmarksExternalPtr ProcessorTrackerFeatureLandmarkExternalTest::randomStep() void ProcessorTrackerFeatureLandmarkExternalTest::randomStep()
{ {
// compute delta // compute delta
VectorXd delta; VectorXd delta;
...@@ -198,7 +199,10 @@ CaptureLandmarksExternalPtr ProcessorTrackerFeatureLandmarkExternalTest::randomS ...@@ -198,7 +199,10 @@ CaptureLandmarksExternalPtr ProcessorTrackerFeatureLandmarkExternalTest::randomS
auto state = processor_motion->getState("PO"); auto state = processor_motion->getState("PO");
p_robot = state.vector("P"); p_robot = state.vector("P");
o_robot = state.vector("O"); o_robot = state.vector("O");
}
CaptureLandmarksExternalPtr ProcessorTrackerFeatureLandmarkExternalTest::computeCaptureLandmarks() const
{
// Detections // Detections
auto cap = std::make_shared<CaptureLandmarksExternal>(t, sensor); auto cap = std::make_shared<CaptureLandmarksExternal>(t, sensor);
VectorXd p_lmk, o_lmk; VectorXd p_lmk, o_lmk;
...@@ -281,68 +285,93 @@ TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, check_p_2d) ...@@ -281,68 +285,93 @@ TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, check_p_2d)
double dist_th = 1e6; double dist_th = 1e6;
int track_length = 5; int track_length = 5;
double time_span = 3*dt; double time_span = 3*dt;
bool remove_landmarks = true;
initProblem(dim, orientation, quality_th, dist_th, track_length, time_span); initProblem(dim, orientation, quality_th, dist_th, track_length, time_span);
if (remove_landmarks)
for (auto lmk : landmarks)
problem->getMap()->removeLanlmk->remove();
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("\n================= STEP ", i, " t = ", t, " =================");
WOLF_INFO("STEP ", i, " t = ", t, " =================");
auto cap = randomStep(); // detection of landmarks
auto cap = computeCaptureLandmarks();
ASSERT_TRUE(problem->check()); ASSERT_TRUE(problem->check());
// process detections
cap->process(); cap->process();
ASSERT_TRUE(problem->check()); ASSERT_TRUE(problem->check());
//problem->print(4,1,1,1);
problem->print(4,1,1,1); // CHECKS
// Check vote for keyframe
if (t.getSeconds() >= time_span)
{
ASSERT_TRUE(problem->getTrajectory()->size() > 1);
}
FactorBasePtrList fac_list; FactorBasePtrList fac_list;
problem->getTrajectory()->getFactorList(fac_list); problem->getTrajectory()->getFactorList(fac_list);
if (i>=track_length-1) bool should_emplace_KF = t.get() >= time_span and i >= track_length-1;
{ if (should_emplace_KF)
// Check track_length {
// voted for keyframe
ASSERT_TRUE(problem->getTrajectory()->size() > 1);
// emplaced factors
ASSERT_FALSE(fac_list.empty()); ASSERT_FALSE(fac_list.empty());
}
else // factors' type
{ if (should_emplace_KF)
// CHECK track_length for (auto fac : fac_list)
ASSERT_TRUE(fac_list.empty());
// Check factors
for (auto fac : fac_list)
{
// Correct type
if (fac->getProcessor() == processor)
{ {
if (dim==2 and orientation) if (fac->getProcessor() == processor)
{
ASSERT_EQ(fac->getType(), "FactorRelativePose2dWithExtrinsiscs");
}
if (dim==3 and not orientation)
{ {
ASSERT_EQ(fac->getType(), "FactorRelativePose3dWithExtrinsiscs"); ASSERT_EQ(fac->getType(), std::string("FactorRelative") +
} (orientation ? "Pose" : "Position") +
if (dim==2 and orientation) (dim == 2 ? "2d" : "3d") +
{ "WithExtrinsiscs");
ASSERT_EQ(fac->getType(), "FactorRelativePosition2dWithExtrinsiscs");
}
if (dim==3 and not orientation)
{
ASSERT_EQ(fac->getType(), "FactorRelativePosition3dWithExtrinsiscs");
} }
} }
// landmarks
auto landmarks_map = problem->getMap()->getLandmarkList();
ASSERT_EQ(landmarks_map.size(), landmarks.size());
for (auto lmk_map : landmarks_map)
{
ASSERT_TRUE(lmk_map->id() < landmarks.size());
auto lmk_gt = landmarks.at(lmk_map->id());
ASSERT_EQ(lmk_map->id(), lmk_gt->id());
if (dim == 2 and orientation)
{
ASSERT_POSE2d_APPROX(lmk_map->getState().vector("PO"), lmk_gt->getState().vector("PO"), Constants::EPS);
}
else if (dim == 2 and not orientation)
{
ASSERT_MATRIX_APPROX(lmk_map->getState().vector("P"), lmk_gt->getState().vector("P"), Constants::EPS);
}
else if (dim == 3 and orientation)
{
ASSERT_POSE3d_APPROX(lmk_map->getState().vector("PO"), lmk_gt->getState().vector("PO"), Constants::EPS);
}
else if (dim == 3 and not orientation)
{
ASSERT_MATRIX_APPROX(lmk_map->getState().vector("P"), lmk_gt->getState().vector("P"), Constants::EPS);
}
} }
} }
else
{
// didn't vote for keyframe
ASSERT_FALSE(problem->getTrajectory()->size() > 1);
// no factors emplaced
ASSERT_TRUE(fac_list.empty());
// landmarks
ASSERT_EQ(problem->getMap()->getLandmarkList().empty(), remove_landmarks);
}
// step with random movement
t+=dt;
randomStep();
} }
} }
......
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