Added parameters for disambiguation using IPPE reprojection error

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;
 
 

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

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");
     }
 

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_;

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>();