diff --git a/src/constraints/constraint_autodiff_apriltag.h b/src/constraints/constraint_autodiff_apriltag.h index 6cfb83f2f421e048d1b485df0ca0fee3b979dea7..f122cbaa4158fd1684abbcc2ba3edf12112ea46a 100644 --- a/src/constraints/constraint_autodiff_apriltag.h +++ b/src/constraints/constraint_autodiff_apriltag.h @@ -117,9 +117,9 @@ template<typename T> bool ConstraintAutodiffApriltag::operator ()( const T* cons Eigen::Transform<T, 3, Eigen::Affine> c_M_l_est = (w_M_r * r_M_c).inverse() * w_M_l; // expectation error, in camera frame - // left-minus + // right-minus Eigen::Transform<T, 3, Eigen::Affine> c_M_err = c_M_l_meas.inverse() * c_M_l_est; - // right-minus also works + // opposite of the previous formula and therefore equivalent // Eigen::Transform<T, 3, Eigen::Affine> c_M_err = c_M_l_est.inverse() * c_M_l_meas; diff --git a/src/examples/processor_tracker_landmark_apriltag.yaml b/src/examples/processor_tracker_landmark_apriltag.yaml index 2b4ef47f15196b8a236d5cac8f13cf5debb6dc9e..ef8ccdb04c1722f47f6459bde348bb6410881441 100644 --- a/src/examples/processor_tracker_landmark_apriltag.yaml +++ b/src/examples/processor_tracker_landmark_apriltag.yaml @@ -10,6 +10,8 @@ detector parameters: 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) + ippe_max_rep_error: 2.0 # to deactivate, set at something big (100) tag parameters: tag_family: "tag36h11" @@ -24,8 +26,8 @@ tag widths: noise: std_xy : 0.1 # m std_z : 0.1 # m - std_rpy_degrees : 20 # degrees - std_pix: 2 # pixel error + std_rpy_degrees : 5 # degrees + std_pix: 20 # pixel error vote: voting active: true diff --git a/src/processors/processor_tracker_landmark_apriltag.cpp b/src/processors/processor_tracker_landmark_apriltag.cpp index 916ab0f9717c21f001888aa042e71698ef6c1b53..588a215610f56f036b77f944782914605147e4aa 100644 --- a/src/processors/processor_tracker_landmark_apriltag.cpp +++ b/src/processors/processor_tracker_landmark_apriltag.cpp @@ -37,6 +37,8 @@ ProcessorTrackerLandmarkApriltag::ProcessorTrackerLandmarkApriltag( ProcessorPar std_z_ (_params_tracker_landmark_apriltag->std_z_ ), std_rpy_(_params_tracker_landmark_apriltag->std_rpy_), std_pix_(_params_tracker_landmark_apriltag->std_pix_), + ippe_min_ratio_(_params_tracker_landmark_apriltag->ippe_min_ratio_), + ippe_max_rep_error_(_params_tracker_landmark_apriltag->ippe_max_rep_error_), cv_K_(3,3), min_time_vote_(_params_tracker_landmark_apriltag->min_time_vote), min_features_for_keyframe_(_params_tracker_landmark_apriltag->min_features_for_keyframe) @@ -149,7 +151,7 @@ void ProcessorTrackerLandmarkApriltag::preProcess() // If not so sure about whether we have the right solution or not, do not create a feature // TODO: use parameters - is_ambiguous = !((rep_error2 / rep_error1 > 5.0) && rep_error1 < 1.0); + is_ambiguous = !((rep_error2 / rep_error1 > ippe_min_ratio_) && rep_error1 < ippe_max_rep_error_); ////////////////// ////////////////// @@ -190,9 +192,12 @@ 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"); -// // Put a very high covariance on angles measurements + 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(); + new_info.topLeftCorner(3, 3) = info.topLeftCorner(3, 3); + info = new_info; } // if (tag_id == 1){ // // Put a very high covariance on angles measurements @@ -203,7 +208,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("---------------------\n"); } diff --git a/src/processors/processor_tracker_landmark_apriltag.h b/src/processors/processor_tracker_landmark_apriltag.h index 888dadf714ae86227774a53902ec6edee035819b..bb4c8d2fef00726fd6f62ca9247b68abcc38939d 100644 --- a/src/processors/processor_tracker_landmark_apriltag.h +++ b/src/processors/processor_tracker_landmark_apriltag.h @@ -40,6 +40,8 @@ struct ProcessorParamsTrackerLandmarkApriltag : public ProcessorParamsTrackerLan Scalar std_xy_, std_z_, std_rpy_; Scalar std_pix_; Scalar min_time_vote; + Scalar ippe_min_ratio_; + Scalar ippe_max_rep_error_; }; @@ -142,6 +144,8 @@ class ProcessorTrackerLandmarkApriltag : public ProcessorTrackerLandmark apriltag_family_t tag_family_; Scalar std_xy_, std_z_, std_rpy_; ///< dummy std values for covariance computation Scalar std_pix_; ///< pixel error to be propagated to a camera to tag transformation covariance + Scalar ippe_min_ratio_; + Scalar ippe_max_rep_error_; // Eigen::Affine3ds c_M_ac_; ///< aprilCamera-to-camera transform not used with solvePnP // double cx_, cy_, fx_, fy_; Matrix3s K_; diff --git a/src/yaml/processor_tracker_landmark_apriltag_yaml.cpp b/src/yaml/processor_tracker_landmark_apriltag_yaml.cpp index 42913851a959ac4c8e15a3a57cb4f1f2f5794370..24e258d63514e9851ac9ffd63a375973ed1ae154 100644 --- a/src/yaml/processor_tracker_landmark_apriltag_yaml.cpp +++ b/src/yaml/processor_tracker_landmark_apriltag_yaml.cpp @@ -36,13 +36,15 @@ static ProcessorParamsBasePtr createProcessorParamsLandmarkApriltag(const std::s params->name = config["processor name"] .as<std::string>(); YAML::Node detector_parameters = config["detector parameters"]; - params->quad_decimate_ = detector_parameters["quad_decimate"] .as<Scalar>(); - params->quad_sigma_ = detector_parameters["quad_sigma"] .as<Scalar>(); - params->nthreads_ = detector_parameters["nthreads"] .as<int>(); - params->debug_ = detector_parameters["debug"] .as<bool>(); - params->refine_edges_ = detector_parameters["refine_edges"] .as<bool>(); - params->refine_decode_ = detector_parameters["refine_decode"] .as<bool>(); - params->refine_pose_ = detector_parameters["refine_pose"] .as<bool>(); + params->quad_decimate_ = detector_parameters["quad_decimate"] .as<Scalar>(); + params->quad_sigma_ = detector_parameters["quad_sigma"] .as<Scalar>(); + params->nthreads_ = detector_parameters["nthreads"] .as<int>(); + params->debug_ = detector_parameters["debug"] .as<bool>(); + params->refine_edges_ = detector_parameters["refine_edges"] .as<bool>(); + params->refine_decode_ = detector_parameters["refine_decode"] .as<bool>(); + params->refine_pose_ = detector_parameters["refine_pose"] .as<bool>(); + params->ippe_min_ratio_ = detector_parameters["ippe_min_ratio"] .as<Scalar>(); + params->ippe_max_rep_error_ = detector_parameters["ippe_max_rep_error"] .as<Scalar>(); YAML::Node tag_parameters = config["tag parameters"]; params->tag_family_ = tag_parameters["tag_family"] .as<std::string>();