diff --git a/gtsam/navigation/MagFactor.h b/gtsam/navigation/MagFactor.h index 96a0971c5..e2a1ebd8c 100644 --- a/gtsam/navigation/MagFactor.h +++ b/gtsam/navigation/MagFactor.h @@ -74,7 +74,7 @@ public: */ Vector evaluateError(const Rot2& nRb, boost::optional H = boost::none) const { - // measured bM = nRbÕ * nM + b + // measured bM = nRb� * nM + b Point3 hx = unrotate(nRb, nM_, H) + bias_; return (hx - measured_).vector(); } @@ -112,7 +112,7 @@ public: */ Vector evaluateError(const Rot3& nRb, boost::optional H = boost::none) const { - // measured bM = nRbÕ * nM + b + // measured bM = nRb� * nM + b Point3 hx = nRb.unrotate(nM_, H, boost::none) + bias_; return (hx - measured_).vector(); } @@ -151,7 +151,7 @@ public: Vector evaluateError(const Point3& nM, const Point3& bias, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { - // measured bM = nRbÕ * nM + b, where b is unknown bias + // measured bM = nRb� * nM + b, where b is unknown bias Point3 hx = bRn_.rotate(nM, boost::none, H1) + bias; if (H2) *H2 = eye(3); @@ -189,11 +189,11 @@ public: * @param nM (unknown) local earth magnetic field vector, in nav frame * @param bias (unknown) 3D bias */ - Vector evaluateError(double scale, const Unit3& direction, + virtual Vector evaluateError(const double& scale, const Unit3& direction, const Point3& bias, boost::optional H1 = boost::none, boost::optional H2 = boost::none, boost::optional H3 = boost::none) const { - // measured bM = nRbÕ * nM + b, where b is unknown bias + // measured bM = nRb� * nM + b, where b is unknown bias Unit3 rotated = bRn_.rotate(direction, boost::none, H2); Point3 hx = scale * rotated.point3() + bias; if (H1) diff --git a/gtsam/navigation/tests/testMagFactor.cpp b/gtsam/navigation/tests/testMagFactor.cpp index 5599c93d6..6f4040929 100644 --- a/gtsam/navigation/tests/testMagFactor.cpp +++ b/gtsam/navigation/tests/testMagFactor.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include