diff --git a/src/processor/processor_tracker_feature_landmark_external.cpp b/src/processor/processor_tracker_feature_landmark_external.cpp
index e2778de7840793f1061fbddc996175a122b72f82..5b47399e2445a33c3af4b8bde838fc9019ae05e9 100644
--- a/src/processor/processor_tracker_feature_landmark_external.cpp
+++ b/src/processor/processor_tracker_feature_landmark_external.cpp
@@ -175,9 +175,19 @@ bool ProcessorTrackerFeatureLandmarkExternal::voteForKeyFrame() const
{
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_)
{
diff --git a/test/gtest_processor_tracker_feature_landmark_external.cpp b/test/gtest_processor_tracker_feature_landmark_external.cpp
index 1e98d296d221246d8d20e1b5b71f11158045ccc5..5ec2e7998ec1dd40259db5e3bb16ffc257b868e4 100644
--- a/test/gtest_processor_tracker_feature_landmark_external.cpp
+++ b/test/gtest_processor_tracker_feature_landmark_external.cpp
@@ -65,7 +65,8 @@ class ProcessorTrackerFeatureLandmarkExternalTest : public testing::Test
double _dist_th,
unsigned int _track_length_th,
double _time_span);
- CaptureLandmarksExternalPtr randomStep();
+ void randomStep();
+ CaptureLandmarksExternalPtr computeCaptureLandmarks() const;
void addRandomDetection(CaptureLandmarksExternalPtr _cap, int _id, double _quality) const;
};
@@ -176,7 +177,7 @@ void ProcessorTrackerFeatureLandmarkExternalTest::initProblem(int _dim,
o_sensor = sensor->getO()->getState();
}
-CaptureLandmarksExternalPtr ProcessorTrackerFeatureLandmarkExternalTest::randomStep()
+void ProcessorTrackerFeatureLandmarkExternalTest::randomStep()
{
// compute delta
VectorXd delta;
@@ -198,7 +199,10 @@ CaptureLandmarksExternalPtr ProcessorTrackerFeatureLandmarkExternalTest::randomS
auto state = processor_motion->getState("PO");
p_robot = state.vector("P");
o_robot = state.vector("O");
+}
+CaptureLandmarksExternalPtr ProcessorTrackerFeatureLandmarkExternalTest::computeCaptureLandmarks() const
+{
// Detections
auto cap = std::make_shared<CaptureLandmarksExternal>(t, sensor);
VectorXd p_lmk, o_lmk;
@@ -281,68 +285,93 @@ TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, check_p_2d)
double dist_th = 1e6;
int track_length = 5;
double time_span = 3*dt;
+ bool remove_landmarks = true;
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());
for (auto i = 0; i<10; i++)
{
- t+=dt;
- WOLF_INFO("STEP ", i, " t = ", t, " =================");
+ WOLF_INFO("\n================= STEP ", i, " t = ", t, " =================");
- auto cap = randomStep();
+ // detection of landmarks
+ auto cap = computeCaptureLandmarks();
ASSERT_TRUE(problem->check());
+ // process detections
cap->process();
ASSERT_TRUE(problem->check());
+ //problem->print(4,1,1,1);
- problem->print(4,1,1,1);
-
- // Check vote for keyframe
- if (t.getSeconds() >= time_span)
- {
- ASSERT_TRUE(problem->getTrajectory()->size() > 1);
- }
-
+ // CHECKS
FactorBasePtrList fac_list;
problem->getTrajectory()->getFactorList(fac_list);
- if (i>=track_length-1)
- {
- // Check track_length
+ bool should_emplace_KF = t.get() >= time_span and i >= track_length-1;
+ if (should_emplace_KF)
+ {
+ // voted for keyframe
+ ASSERT_TRUE(problem->getTrajectory()->size() > 1);
+
+ // emplaced factors
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)
+
+ // factors' type
+ if (should_emplace_KF)
+ for (auto fac : fac_list)
{
- if (dim==2 and orientation)
- {
- ASSERT_EQ(fac->getType(), "FactorRelativePose2dWithExtrinsiscs");
- }
- if (dim==3 and not orientation)
+ if (fac->getProcessor() == processor)
{
- 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");
+ ASSERT_EQ(fac->getType(), std::string("FactorRelative") +
+ (orientation ? "Pose" : "Position") +
+ (dim == 2 ? "2d" : "3d") +
+ "WithExtrinsiscs");
}
}
+ // 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();
}
}