Applying simple covariance correctly,

and fixing documentation
locusrobotics · Dec 18, 2023 · a55be22 · a55be22
1 parent a5e3c8c
commit a55be22
Show file tree

Hide file tree

Showing 2 changed files with 4 additions and 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
@@ -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
@@ -175,31 +175,10 @@ bool Fixed3DLandmarkSimpleCovarianceCostFunctor::operator()(const T* const posit
 
   auto d = (obs_.cast<T>() - 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<T>().col(i)[0];
-    T gy = pts3d_.cast<T>().col(i)[1];
-    T gz = pts3d_.cast<T>().col(i)[2];
-    T gz2 = gz * gz;
-    T gxyz = (gx * gy) / gz2;
-    Eigen::Matrix<T, 2, 6, Eigen::RowMajor> 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<T, 2, 2, Eigen::RowMajor> 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];
   }