Skip to content
Snippets Groups Projects
Commit c14f214a authored by Mederic Fourmy's avatar Mederic Fourmy
Browse files

Added parameters for disambiguation using IPPE reprojection error

parent bbf1211f
No related branches found
No related tags found
1 merge request!233WIP: Apriltag
Pipeline #2438 passed
...@@ -117,9 +117,9 @@ template<typename T> bool ConstraintAutodiffApriltag::operator ()( const T* cons ...@@ -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; 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 // 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; 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; // Eigen::Transform<T, 3, Eigen::Affine> c_M_err = c_M_l_est.inverse() * c_M_l_meas;
......
...@@ -10,6 +10,8 @@ detector parameters: ...@@ -10,6 +10,8 @@ detector parameters:
refine_edges: true refine_edges: true
refine_decode: true refine_decode: true
refine_pose: 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 parameters:
tag_family: "tag36h11" tag_family: "tag36h11"
...@@ -24,8 +26,8 @@ tag widths: ...@@ -24,8 +26,8 @@ tag widths:
noise: noise:
std_xy : 0.1 # m std_xy : 0.1 # m
std_z : 0.1 # m std_z : 0.1 # m
std_rpy_degrees : 20 # degrees std_rpy_degrees : 5 # degrees
std_pix: 2 # pixel error std_pix: 20 # pixel error
vote: vote:
voting active: true voting active: true
......
...@@ -37,6 +37,8 @@ ProcessorTrackerLandmarkApriltag::ProcessorTrackerLandmarkApriltag( ProcessorPar ...@@ -37,6 +37,8 @@ ProcessorTrackerLandmarkApriltag::ProcessorTrackerLandmarkApriltag( ProcessorPar
std_z_ (_params_tracker_landmark_apriltag->std_z_ ), std_z_ (_params_tracker_landmark_apriltag->std_z_ ),
std_rpy_(_params_tracker_landmark_apriltag->std_rpy_), std_rpy_(_params_tracker_landmark_apriltag->std_rpy_),
std_pix_(_params_tracker_landmark_apriltag->std_pix_), 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), cv_K_(3,3),
min_time_vote_(_params_tracker_landmark_apriltag->min_time_vote), min_time_vote_(_params_tracker_landmark_apriltag->min_time_vote),
min_features_for_keyframe_(_params_tracker_landmark_apriltag->min_features_for_keyframe) min_features_for_keyframe_(_params_tracker_landmark_apriltag->min_features_for_keyframe)
...@@ -149,7 +151,7 @@ void ProcessorTrackerLandmarkApriltag::preProcess() ...@@ -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 // If not so sure about whether we have the right solution or not, do not create a feature
// TODO: use parameters // 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() ...@@ -190,9 +192,12 @@ void ProcessorTrackerLandmarkApriltag::preProcess()
Eigen::Matrix6s info = computeInformation(translation, c_M_t.linear(), K_, tag_width, std_pix_); // Lie jacobians covariance Eigen::Matrix6s info = computeInformation(translation, c_M_t.linear(), K_, tag_width, std_pix_); // Lie jacobians covariance
if (is_ambiguous){ 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 // Put a very high covariance on angles measurements (low info matrix ?)
// cov.bottomRightCorner(3, 3) = 1000000*Eigen::Matrix3s::Identity(); // 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){ // if (tag_id == 1){
// // Put a very high covariance on angles measurements // // Put a very high covariance on angles measurements
...@@ -203,7 +208,7 @@ void ProcessorTrackerLandmarkApriltag::preProcess() ...@@ -203,7 +208,7 @@ void ProcessorTrackerLandmarkApriltag::preProcess()
// WOLF_TRACE("tag ", tag_id, " cov diagonal: [", cov.diagonal().transpose(), "]"); // WOLF_TRACE("tag ", tag_id, " cov diagonal: [", cov.diagonal().transpose(), "]");
// add to detected features list // add to detected features list
detections_incoming_.push_back(std::make_shared<FeatureApriltag>(pose, info, tag_id, *det, FeatureBase::UncertaintyType::UNCERTAINTY_IS_INFO)); 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"); WOLF_TRACE("---------------------\n");
} }
......
...@@ -40,6 +40,8 @@ struct ProcessorParamsTrackerLandmarkApriltag : public ProcessorParamsTrackerLan ...@@ -40,6 +40,8 @@ struct ProcessorParamsTrackerLandmarkApriltag : public ProcessorParamsTrackerLan
Scalar std_xy_, std_z_, std_rpy_; Scalar std_xy_, std_z_, std_rpy_;
Scalar std_pix_; Scalar std_pix_;
Scalar min_time_vote; Scalar min_time_vote;
Scalar ippe_min_ratio_;
Scalar ippe_max_rep_error_;
}; };
...@@ -142,6 +144,8 @@ class ProcessorTrackerLandmarkApriltag : public ProcessorTrackerLandmark ...@@ -142,6 +144,8 @@ class ProcessorTrackerLandmarkApriltag : public ProcessorTrackerLandmark
apriltag_family_t tag_family_; apriltag_family_t tag_family_;
Scalar std_xy_, std_z_, std_rpy_; ///< dummy std values for covariance computation 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 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 // Eigen::Affine3ds c_M_ac_; ///< aprilCamera-to-camera transform not used with solvePnP
// double cx_, cy_, fx_, fy_; // double cx_, cy_, fx_, fy_;
Matrix3s K_; Matrix3s K_;
......
...@@ -36,13 +36,15 @@ static ProcessorParamsBasePtr createProcessorParamsLandmarkApriltag(const std::s ...@@ -36,13 +36,15 @@ static ProcessorParamsBasePtr createProcessorParamsLandmarkApriltag(const std::s
params->name = config["processor name"] .as<std::string>(); params->name = config["processor name"] .as<std::string>();
YAML::Node detector_parameters = config["detector parameters"]; YAML::Node detector_parameters = config["detector parameters"];
params->quad_decimate_ = detector_parameters["quad_decimate"] .as<Scalar>(); params->quad_decimate_ = detector_parameters["quad_decimate"] .as<Scalar>();
params->quad_sigma_ = detector_parameters["quad_sigma"] .as<Scalar>(); params->quad_sigma_ = detector_parameters["quad_sigma"] .as<Scalar>();
params->nthreads_ = detector_parameters["nthreads"] .as<int>(); params->nthreads_ = detector_parameters["nthreads"] .as<int>();
params->debug_ = detector_parameters["debug"] .as<bool>(); params->debug_ = detector_parameters["debug"] .as<bool>();
params->refine_edges_ = detector_parameters["refine_edges"] .as<bool>(); params->refine_edges_ = detector_parameters["refine_edges"] .as<bool>();
params->refine_decode_ = detector_parameters["refine_decode"] .as<bool>(); params->refine_decode_ = detector_parameters["refine_decode"] .as<bool>();
params->refine_pose_ = detector_parameters["refine_pose"] .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"]; YAML::Node tag_parameters = config["tag parameters"];
params->tag_family_ = tag_parameters["tag_family"] .as<std::string>(); params->tag_family_ = tag_parameters["tag_family"] .as<std::string>();
......
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