Skip to content
Snippets Groups Projects
Commit c318529d authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Debug info

parent 087f62ee
No related branches found
No related tags found
1 merge request!233WIP: Apriltag
This commit is part of merge request !233. Comments created here will be created in the context of that merge request.
...@@ -168,6 +168,13 @@ void ProcessorTrackerLandmarkApriltag::preProcess() ...@@ -168,6 +168,13 @@ void ProcessorTrackerLandmarkApriltag::preProcess()
// M_april = umich_pose_estimation(det, cv_K_, tag_width); // 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 // set the measured pose vector
Eigen::Vector3s translation ( c_M_t.translation() ); // translation vector in apriltag meters Eigen::Vector3s translation ( c_M_t.translation() ); // translation vector in apriltag meters
Eigen::Vector7s pose; Eigen::Vector7s pose;
...@@ -182,9 +189,11 @@ void ProcessorTrackerLandmarkApriltag::preProcess() ...@@ -182,9 +189,11 @@ void ProcessorTrackerLandmarkApriltag::preProcess()
// Put a very high covariance on angles measurements // Put a very high covariance on angles measurements
cov.bottomRightCorner(3, 3) = 1000000*Eigen::Matrix3s::Identity(); cov.bottomRightCorner(3, 3) = 1000000*Eigen::Matrix3s::Identity();
} }
WOLF_TRACE("cov \n", cov); WOLF_TRACE("cov diagonal: [", cov.diagonal(), "]");
// add to detected features list // add to detected features list
detections_incoming_.push_back(std::make_shared<FeatureApriltag>(pose, cov.inverse(), tag_id, *det, FeatureBase::UncertaintyType::UNCERTAINTY_IS_INFO)); 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); apriltag_detections_destroy(detections);
......
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