From c318529d5a808ea4eb3aa5e15606259fa221b08f Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Fri, 18 Jan 2019 22:54:07 +0100
Subject: [PATCH] Debug info
---
.../processor_tracker_landmark_apriltag.cpp | 11 ++++++++++-
1 file changed, 10 insertions(+), 1 deletion(-)
diff --git a/src/processors/processor_tracker_landmark_apriltag.cpp b/src/processors/processor_tracker_landmark_apriltag.cpp
index 6cc3fd332..c91887d97 100644
--- a/src/processors/processor_tracker_landmark_apriltag.cpp
+++ b/src/processors/processor_tracker_landmark_apriltag.cpp
@@ -168,6 +168,13 @@ void ProcessorTrackerLandmarkApriltag::preProcess()
// M_april = umich_pose_estimation(det, cv_K_, tag_width);
//////////////////
+ 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());
+
+ c_M_t = M_ippe1;
+
// set the measured pose vector
Eigen::Vector3s translation ( c_M_t.translation() ); // translation vector in apriltag meters
Eigen::Vector7s pose;
@@ -182,9 +189,11 @@ void ProcessorTrackerLandmarkApriltag::preProcess()
// Put a very high covariance on angles measurements
cov.bottomRightCorner(3, 3) = 1000000*Eigen::Matrix3s::Identity();
}
- WOLF_TRACE("cov \n", cov);
+ WOLF_TRACE("cov diagonal: [", cov.diagonal(), "]");
// add to detected features list
detections_incoming_.push_back(std::make_shared<FeatureApriltag>(pose, cov.inverse(), tag_id, *det, FeatureBase::UncertaintyType::UNCERTAINTY_IS_INFO));
+
+ WOLF_TRACE("---------------------\n");
}
apriltag_detections_destroy(detections);
--
GitLab