Added parameter for standard variation in pixels. Tests on dataset give bad results.

src/examples/processor_tracker_landmark_apriltag.yaml

@@ -25,6 +25,7 @@ noise:
     std_xy :          0.05 # m 
     std_z :           0.05 # m 
     std_rpy_degrees : 20   # degrees
+    std_pix:          2    # pixel error
     
 vote:
     voting active:              true

src/examples/test_apriltag.cpp

@@ -69,10 +69,12 @@ int main(int argc, char *argv[])
 
     // set prior
     Eigen::Matrix6s covariance = Matrix6s::Identity();
-    Scalar stdev_m   = 0.00001;  // standard deviation on original translation
-    Scalar stdev_deg = 0.00001;  // standard deviation on original rotation
-    covariance.topLeftCorner(3,3)       =  stdev_m*stdev_m * covariance.topLeftCorner(3,3);
-    covariance.bottomRightCorner(3,3)   = (M_TORAD*stdev_deg)*(M_TORAD*stdev_deg) * covariance.bottomRightCorner(3,3);
+//    Scalar stdev_m   = 0.00001;  // standard deviation on original translation
+//    Scalar stdev_deg = 0.00001;  // standard deviation on original rotation
+    Scalar std_m   = 100;  // standard deviation on original translation
+    Scalar std_deg = 10;  // standard deviation on original rotation
+    covariance.topLeftCorner(3,3)       =  std_m*std_m * covariance.topLeftCorner(3,3);
+    covariance.bottomRightCorner(3,3)   = (M_TORAD*std_deg)*(M_TORAD*std_deg) * covariance.bottomRightCorner(3,3);
 
 //    FrameBasePtr F1 = problem->setPrior((Vector7s()<<0,0,0,0,0,0,1).finished(), covariance, 0.0, 0.1);
     FrameBasePtr F1 = problem->setPrior((Vector7s()<<0.08, 0, -0.75, 0, 0, 0, 1).finished(), covariance, 0.0, 0.1);

src/processors/processor_tracker_landmark_apriltag.cpp

@@ -33,6 +33,7 @@ ProcessorTrackerLandmarkApriltag::ProcessorTrackerLandmarkApriltag( ProcessorPar
         std_xy_ (_params_tracker_landmark_apriltag->std_xy_ ),
         std_z_  (_params_tracker_landmark_apriltag->std_z_  ),
         std_rpy_(_params_tracker_landmark_apriltag->std_rpy_),
+        std_pix_(_params_tracker_landmark_apriltag->std_pix_),
         min_time_vote_(_params_tracker_landmark_apriltag->min_time_vote),
         min_features_for_keyframe_(_params_tracker_landmark_apriltag->min_features_for_keyframe)
 {
@@ -161,10 +162,9 @@ void ProcessorTrackerLandmarkApriltag::preProcess()
         pose << translation, R2q(c_M_t.linear()).coeffs();
 
         // compute the covariance
-//        Eigen::Matrix6s cov = getVarVec().asDiagonal() ;
-        double sig_q = 2;
-        std::vector<Scalar> k_vec = {cx_, cy_, fx_, fy_};
-        Eigen::Matrix6s cov = computeCovariance(translation, c_M_t.linear(), k_vec, tag_width, sig_q);
+        Eigen::Matrix6s cov = getVarVec().asDiagonal() ;  // fixed dummy covariance
+//        std::vector<Scalar> k_vec = {cx_, cy_, fx_, fy_};
+//        Eigen::Matrix6s cov = computeCovariance(translation, c_M_t.linear(), k_vec, tag_width, std_pix_);
 
         // add to detected features list
         detections_incoming_.push_back(std::make_shared<FeatureApriltag>(pose, cov, tag_id, *det));
@@ -324,15 +324,13 @@ Eigen::Matrix6s ProcessorTrackerLandmarkApriltag::computeCovariance(Eigen::Vecto
          0,  k_vec[3], k_vec[1],
          0,  0,  1;
 
-    // position of the 4 corners of the tag in its reference frame (which is in its middle
+    // position of the 4 corners of the tag in its reference frame (which is in its middle)
     Eigen::Vector3s p1; p1 <<  1,  1, 0; p1 = p1*tag_width/2; // top left
     Eigen::Vector3s p2; p2 << -1,  1, 0; p2 = p2*tag_width/2; // top right
     Eigen::Vector3s p3; p3 << -1, -1, 0; p3 = p3*tag_width/2; // bottom right
     Eigen::Vector3s p4; p4 <<  1, -1, 0; p4 = p4*tag_width/2; // bottom left
     std::vector<Eigen::Vector3s> pvec = {p1, p2, p3, p4};
 
-//    WOLF_TRACE(pvec);
-
     // Initialize jacobian matrices
     Eigen::Matrix<Scalar, 8, 6> J_u_TR = Eigen::Matrix<Scalar, 8, 6>::Zero();
     Eigen::Vector3s h;

src/processors/processor_tracker_landmark_apriltag.h

@@ -38,6 +38,7 @@ struct ProcessorParamsTrackerLandmarkApriltag : public ProcessorParamsTrackerLan
     bool refine_pose_;
 
     Scalar std_xy_, std_z_, std_rpy_;
+    Scalar std_pix_;
     Scalar min_time_vote;
 };
 
@@ -130,6 +131,7 @@ class ProcessorTrackerLandmarkApriltag : public ProcessorTrackerLandmark
         apriltag_detector_t detector_;
         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
         Eigen::Affine3ds c_M_ac_;           ///< aprilCamera-to-camera transform
         double cx_, cy_, fx_, fy_;
 

src/yaml/processor_tracker_landmark_apriltag_yaml.cpp

@@ -62,6 +62,7 @@ static ProcessorParamsBasePtr createProcessorParamsLandmarkApriltag(const std::s
         params->std_xy_                      = noise["std_xy"]                       .as<Scalar>();
         params->std_z_                       = noise["std_z"]                        .as<Scalar>();
         params->std_rpy_           = M_TORAD * noise["std_rpy_degrees"]              .as<Scalar>();
+        params->std_pix_                     = noise["std_pix"]                      .as<Scalar>();
 
         YAML::Node vote                     = config["vote"];
         params->voting_active               = vote["voting active"]                  .as<bool>();