From a55be2276b16117c7a58e2ad21f63f494ad8bca5 Mon Sep 17 00:00:00 2001 From: Oscar Mendez Date: Mon, 18 Dec 2023 09:55:10 +0000 Subject: [PATCH] Applying simple covariance correctly, and fixing documentation --- ...3d_landmark_simple_covariance_constraint.h | 4 +-- ..._landmark_simple_covariance_cost_functor.h | 25 ++----------------- 2 files changed, 4 insertions(+), 25 deletions(-) diff --git a/fuse_constraints/include/fuse_constraints/fixed_3d_landmark_simple_covariance_constraint.h b/fuse_constraints/include/fuse_constraints/fixed_3d_landmark_simple_covariance_constraint.h index 539ff8047..ccec9a91d 100644 --- a/fuse_constraints/include/fuse_constraints/fixed_3d_landmark_simple_covariance_constraint.h +++ b/fuse_constraints/include/fuse_constraints/fixed_3d_landmark_simple_covariance_constraint.h @@ -91,7 +91,7 @@ class Fixed3DLandmarkSimpleCovarianceConstraint : public fuse_core::Constraint * @param[in] pts3d Matrix of 3D points in marker coordiate frame. * @param[in] observations The 2D (pixel) observations of each marker's corners. * @param[in] mean The measured/prior pose of the marker as a vector (7x1 vector: x, y, z, qw, qx, qy, qz) - * @param[in] covariance The measurement/prior marker pose covariance (6x6 matrix: x, y, z, qx, qy, qz) + * @param[in] covariance The detection covariance, in pixels (2x2 matrix: u, v) */ Fixed3DLandmarkSimpleCovarianceConstraint(const std::string& source, const fuse_variables::Position3DStamped& position, @@ -113,7 +113,7 @@ class Fixed3DLandmarkSimpleCovarianceConstraint : public fuse_core::Constraint * @param[in] marker_size The size of the marker, in meters. Assumed to be square. * @param[in] observations The 2D (pixel) observations of each marker's corners. * @param[in] mean The measured/prior pose of the marker as a vector (7x1 vector: x, y, z, qw, qx, qy, qz) - * @param[in] covariance The measurement/prior marker pose covariance (6x6 matrix: x, y, z, qx, qy, qz) + * @param[in] covariance The detection covariance, in pixels (2x2 matrix: u, v) */ Fixed3DLandmarkSimpleCovarianceConstraint(const std::string& source, const fuse_variables::Position3DStamped& position, diff --git a/fuse_constraints/include/fuse_constraints/fixed_3d_landmark_simple_covariance_cost_functor.h b/fuse_constraints/include/fuse_constraints/fixed_3d_landmark_simple_covariance_cost_functor.h index 31fa886bf..6a1177eb8 100644 --- a/fuse_constraints/include/fuse_constraints/fixed_3d_landmark_simple_covariance_cost_functor.h +++ b/fuse_constraints/include/fuse_constraints/fixed_3d_landmark_simple_covariance_cost_functor.h @@ -175,31 +175,10 @@ bool Fixed3DLandmarkSimpleCovarianceCostFunctor::operator()(const T* const posit auto d = (obs_.cast() - xp.block(0, 0, xp.rows(), 2)); - T fx = calibration[0]; - T fy = calibration[1]; for (uint i = 0; i < pts3d_.cols(); i++) { - // Get the covariance weigthing to point losses from a pose uncertainty - // From https://arxiv.org/pdf/2103.15980.pdf , equation A.7: - // dh( e A p ) = dh(p') * d(e A p) - // d(e) d(p') d(e) - // where e is a small increment around the SE(3) manifold of A, A is a pose, p is a point, - // h is the projection function, and p' = Ap = g, the jacobian is thus 2x6: - // J = - // [ (fx/gz) (0) (-fx * gx / gz^2) (-fx * gx gy / gz^2) fx(1+gx^2/gz^2) -fx gy/gz] - // [ 0 (fy/gz)) (-fy * gy / gz^2) -fy(1+gy^2/gz^2) (-fy * gx gy / gz^2) -fy gx/gz] - T gx = pts3d_.cast().col(i)[0]; - T gy = pts3d_.cast().col(i)[1]; - T gz = pts3d_.cast().col(i)[2]; - T gz2 = gz * gz; - T gxyz = (gx * gy) / gz2; - Eigen::Matrix J; - J << fx / gz, T(0), -fx * (gx / gz2), -fx * gxyz, fx * (T(1) + (gx * gx) / gz2), -fx * gy / gz, T(0), fy / gz, - -fy * (gy / gz2), -fy * (T(1) + (gy * gy) / gz2), fy * gxyz, fy * gx / gz; - Eigen::Matrix A = J * A_ * J.transpose(); - - // Weight Residuals - auto r = A * d.row(i).transpose(); + // Weight Residuals, in this case, we assume a flat error on the pixels + auto r = A_ * d.row(i).transpose(); residual[i * 2] = r[0]; residual[i * 2 + 1] = r[1]; }