diff --git a/src/examples/processor_tracker_landmark_apriltag.yaml b/src/examples/processor_tracker_landmark_apriltag.yaml
index ae9d373fbe94f9c3261fa77a8042545208f2e933..7dc97c7451f1f03560ae3eebba6f56d09592e891 100644
--- a/src/examples/processor_tracker_landmark_apriltag.yaml
+++ b/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
diff --git a/src/examples/test_apriltag.cpp b/src/examples/test_apriltag.cpp
index 4964ce4cd77943a5a863d3a09992ef6c9edcabfc..f4728ed9692976c26f1461be8e538afd78dd3bf2 100644
--- a/src/examples/test_apriltag.cpp
+++ b/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);
diff --git a/src/processors/processor_tracker_landmark_apriltag.cpp b/src/processors/processor_tracker_landmark_apriltag.cpp
index f6bd329178979acc00baa6cc823c928dfad5b295..a25157bf3a7fb35b7a9387068d1dcef410bae4d5 100644
--- a/src/processors/processor_tracker_landmark_apriltag.cpp
+++ b/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;
diff --git a/src/processors/processor_tracker_landmark_apriltag.h b/src/processors/processor_tracker_landmark_apriltag.h
index ea4cccb681e07d9ddfaf8b7912af336e40f442b6..5df0cd650b0349cd2b4a64d2f86ecb027f202f13 100644
--- a/src/processors/processor_tracker_landmark_apriltag.h
+++ b/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_;
diff --git a/src/yaml/processor_tracker_landmark_apriltag_yaml.cpp b/src/yaml/processor_tracker_landmark_apriltag_yaml.cpp
index aad5847e0fe264c4518a7a2c4ada6a9a3947fcb6..42913851a959ac4c8e15a3a57cb4f1f2f5794370 100644
--- a/src/yaml/processor_tracker_landmark_apriltag_yaml.cpp
+++ b/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>();