diff --git a/src/processors/processor_tracker_landmark_apriltag.cpp b/src/processors/processor_tracker_landmark_apriltag.cpp index 6cc3fd332c81210779b282c7f2a030ba8c1155a0..c91887d977b181497a9d6783be6af6caf0b7e96e 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);