diff --git a/src/examples/processor_tracker_landmark_apriltag.yaml b/src/examples/processor_tracker_landmark_apriltag.yaml
index ef8ccdb04c1722f47f6459bde348bb6410881441..15be7c6e8563beb00d8954ec3b95a5f5c4b23c08 100644
--- a/src/examples/processor_tracker_landmark_apriltag.yaml
+++ b/src/examples/processor_tracker_landmark_apriltag.yaml
@@ -1,16 +1,15 @@
processor type: "TRACKER LANDMARK APRILTAG"
processor name: "tracker landmark apriltag example"
-
detector parameters:
- quad_decimate: 0.0
- quad_sigma: 2.0
- nthreads: 4
- debug: false
- refine_edges: true
- refine_decode: true
- refine_pose: true
- ippe_min_ratio: 3.0 # quite arbitrary, always > 1 (to deactive, set at 0 for example)
+ quad_decimate: 1.5 # doing quad detection at lower resolution to speed things up (see end of file)
+ quad_sigma: 0.8 # gaussian blur good for noisy images, may be recommended with quad_decimate. Kernel size adapted (see end of this file)
+ nthreads: 8 # how many thread during tag detection (does not change much?)
+ debug: false # write some debugging images
+ refine_edges: true # better edge detection if quad_decimate > 1 (quite inexpensive, almost no diff)
+ refine_decode: false # improve detection probability for small tags (quite expensive (~*3)
+ refine_pose: false # improves detection by maximizing local contrast so that future pose extraction works better (VERY expensive ~*10-20)
+ ippe_min_ratio: 3.0 # quite arbitrary, always > 1 (to deactive, set at 0 for example)
ippe_max_rep_error: 2.0 # to deactivate, set at something big (100)
tag parameters:
@@ -33,3 +32,21 @@ vote:
voting active: true
min_time_vote: 0.2 # s
min_features_for_keyframe: 1
+
+
+
+
+# Annexes:
+### Quad decimate: the higher, the lower the resolution
+# Does nothing if <= 1.0
+# Only values taken into account:
+# 1.5, 2, 3, 4
+# 1.5 -> ~*2 speed up
+
+# Higher values use a "bad" code according to commentaries in the library, smaller do nothing
+### Gaussian blur table:
+# max sigma kernel size
+# 0.499 1 (disabled)
+# 0.999 3
+# 1.499 5
+# 1.999 7
diff --git a/src/processors/processor_tracker_landmark_apriltag.cpp b/src/processors/processor_tracker_landmark_apriltag.cpp
index a4b9de0ecc1499d41963dc570b9dc1cb28ca103e..100beeaeb722a79934d832402ee9607c89f138a1 100644
--- a/src/processors/processor_tracker_landmark_apriltag.cpp
+++ b/src/processors/processor_tracker_landmark_apriltag.cpp
@@ -69,6 +69,26 @@ ProcessorTrackerLandmarkApriltag::ProcessorTrackerLandmarkApriltag( ProcessorPar
detector_ = *apriltag_detector_create();
apriltag_detector_add_family(&detector_, &tag_family_);
+ WOLF_TRACE("\n\n\n\n", _params_tracker_landmark_apriltag->nthreads_, "\n\n\n\n");
+ WOLF_TRACE("\n\n\n\n", _params_tracker_landmark_apriltag->nthreads_, "\n\n\n\n");
+ WOLF_TRACE("\n\n\n\n", _params_tracker_landmark_apriltag->nthreads_, "\n\n\n\n");
+ WOLF_TRACE("\n\n\n\n", _params_tracker_landmark_apriltag->nthreads_, "\n\n\n\n");
+ WOLF_TRACE("\n\n\n\n", _params_tracker_landmark_apriltag->nthreads_, "\n\n\n\n");
+ WOLF_TRACE("\n\n\n\n", _params_tracker_landmark_apriltag->nthreads_, "\n\n\n\n");
+ WOLF_TRACE("\n\n\n\n", _params_tracker_landmark_apriltag->nthreads_, "\n\n\n\n");
+ WOLF_TRACE("\n\n\n\n", _params_tracker_landmark_apriltag->nthreads_, "\n\n\n\n");
+ WOLF_TRACE("\n\n\n\n", _params_tracker_landmark_apriltag->nthreads_, "\n\n\n\n");
+ WOLF_TRACE("\n\n\n\n", _params_tracker_landmark_apriltag->nthreads_, "\n\n\n\n");
+ WOLF_TRACE("\n\n\n\n", _params_tracker_landmark_apriltag->nthreads_, "\n\n\n\n");
+ WOLF_TRACE("\n\n\n\n", _params_tracker_landmark_apriltag->nthreads_, "\n\n\n\n");
+ WOLF_TRACE("\n\n\n\n", _params_tracker_landmark_apriltag->nthreads_, "\n\n\n\n");
+ WOLF_TRACE("\n\n\n\n", _params_tracker_landmark_apriltag->nthreads_, "\n\n\n\n");
+ WOLF_TRACE("\n\n\n\n", _params_tracker_landmark_apriltag->nthreads_, "\n\n\n\n");
+ WOLF_TRACE("\n\n\n\n", _params_tracker_landmark_apriltag->nthreads_, "\n\n\n\n");
+ WOLF_TRACE("\n\n\n\n", _params_tracker_landmark_apriltag->nthreads_, "\n\n\n\n");
+ WOLF_TRACE("\n\n\n\n", _params_tracker_landmark_apriltag->nthreads_, "\n\n\n\n");
+
+
detector_.quad_decimate = _params_tracker_landmark_apriltag->quad_decimate_;
detector_.quad_sigma = _params_tracker_landmark_apriltag->quad_sigma_;
detector_.nthreads = _params_tracker_landmark_apriltag->nthreads_;
@@ -126,7 +146,9 @@ void ProcessorTrackerLandmarkApriltag::preProcess()
};
// run Apriltag detector
+// const clock_t begin_time_detection = clock();
zarray_t *detections = apriltag_detector_detect(&detector_, &im);
+// WOLF_DEBUG("tag detection: ", (double)(clock() - begin_time_detection) / CLOCKS_PER_SEC);
// loop all detections
for (int i = 0; i < zarray_size(detections); i++) {
@@ -148,7 +170,6 @@ void ProcessorTrackerLandmarkApriltag::preProcess()
Scalar rep_error1;
Scalar rep_error2;
ippePoseEstimation(det, cv_K_, tag_width, M_ippe1, rep_error1, M_ippe2, rep_error2);
-
// If not so sure about whether we have the right solution or not, do not create a feature
is_ambiguous = !((rep_error2 / rep_error1 > ippe_min_ratio_) && rep_error1 < ippe_max_rep_error_);
//////////////////
@@ -169,8 +190,8 @@ void ProcessorTrackerLandmarkApriltag::preProcess()
// M_april = umichPoseEstimation(det, cv_K_, tag_width);
//////////////////
- WOLF_DEBUG("ippe1\n", M_ippe1 .matrix());
- WOLF_DEBUG("ippe2\n", M_ippe2 .matrix());
+// WOLF_DEBUG("ippe1\n", M_ippe1 .matrix());
+// WOLF_DEBUG("ippe2\n", M_ippe2 .matrix());
// WOLF_DEBUG("M_PnP\n", M_PnP .matrix());
// WOLF_DEBUG("M_april\n", M_april .matrix());
@@ -191,7 +212,7 @@ void ProcessorTrackerLandmarkApriltag::preProcess()
Eigen::Matrix6s info = computeInformation(translation, c_M_t.linear(), K_, tag_width, std_pix_); // Lie jacobians covariance
if (is_ambiguous){
- WOLF_INFO("Ambiguity on estimated rotation is likely");
+// WOLF_INFO("Ambiguity on estimated rotation is likely");
// Put a very high covariance on angles measurements (low info matrix ?)
// cov.bottomRightCorner(3, 3) = 1000000*Eigen::Matrix3s::Identity();
Eigen::Matrix6s new_info = 0.00001*Eigen::Matrix6s::Identity();
@@ -207,7 +228,7 @@ void ProcessorTrackerLandmarkApriltag::preProcess()
// WOLF_TRACE("tag ", tag_id, " cov diagonal: [", cov.diagonal().transpose(), "]");
// add to detected features list
detections_incoming_.push_back(std::make_shared<FeatureApriltag>(pose, info, tag_id, *det, FeatureBase::UncertaintyType::UNCERTAINTY_IS_INFO));
- WOLF_TRACE("Meas Covariance tag ", tag_id, "\n", info.inverse());
+// WOLF_TRACE("Meas Covariance tag ", tag_id, "\n", info.inverse());
WOLF_TRACE("---------------------\n");
}