diff --git a/gtsam/sam/BearingFactor.h b/gtsam/sam/BearingFactor.h index ece26f3dc..7076708db 100644 --- a/gtsam/sam/BearingFactor.h +++ b/gtsam/sam/BearingFactor.h @@ -60,6 +60,21 @@ struct BearingFactor : public ExpressionFactorN { std::cout << s << "BearingFactor" << std::endl; Base::print(s, kf); } + + Vector evaluateError(const A1& a1, const A2& a2, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const + { + std::vector Hs(2); + const auto &keys = Factor::keys(); + const Vector error = unwhitenedError( + {{keys[0], genericValue(a1)}, {keys[1], genericValue(a2)}}, + Hs); + if (H1) *H1 = Hs[0]; + if (H2) *H2 = Hs[1]; + return error; + } + private: friend class boost::serialization::access; diff --git a/gtsam/sam/BearingRangeFactor.h b/gtsam/sam/BearingRangeFactor.h index 2bd5fa11c..af7b47446 100644 --- a/gtsam/sam/BearingRangeFactor.h +++ b/gtsam/sam/BearingRangeFactor.h @@ -66,6 +66,20 @@ class BearingRangeFactor Expression(keys[1])); } + Vector evaluateError(const A1& a1, const A2& a2, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const + { + std::vector Hs(2); + const auto &keys = Factor::keys(); + const Vector error = unwhitenedError( + {{keys[0], genericValue(a1)}, {keys[1], genericValue(a2)}}, + Hs); + if (H1) *H1 = Hs[0]; + if (H2) *H2 = Hs[1]; + return error; + } + /// print void print(const std::string& s = "", const KeyFormatter& kf = DefaultKeyFormatter) const override { diff --git a/gtsam/sam/RangeFactor.h b/gtsam/sam/RangeFactor.h index 0bc997044..0150505b2 100644 --- a/gtsam/sam/RangeFactor.h +++ b/gtsam/sam/RangeFactor.h @@ -58,6 +58,20 @@ class RangeFactor : public ExpressionFactorN { Expression a2_(keys[1]); return Expression(Range(), a1_, a2_); } + + Vector evaluateError(const A1& a1, const A2& a2, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const + { + std::vector Hs(2); + const auto &keys = Factor::keys(); + const Vector error = unwhitenedError( + {{keys[0], genericValue(a1)}, {keys[1], genericValue(a2)}}, + Hs); + if (H1) *H1 = Hs[0]; + if (H2) *H2 = Hs[1]; + return error; + } /// print void print(const std::string& s = "", diff --git a/gtsam/sam/tests/testRangeFactor.cpp b/gtsam/sam/tests/testRangeFactor.cpp index 7ec8dc580..8ae3d818b 100644 --- a/gtsam/sam/tests/testRangeFactor.cpp +++ b/gtsam/sam/tests/testRangeFactor.cpp @@ -47,29 +47,25 @@ double measurement(10.0); /* ************************************************************************* */ Vector factorError2D(const Pose2& pose, const Point2& point, const RangeFactor2D& factor) { - const auto &keys = factor.keys(); - return factor.unwhitenedError({{keys[0], genericValue(pose)}, {keys[1], genericValue(point)}}); + return factor.evaluateError(pose, point); } /* ************************************************************************* */ Vector factorError3D(const Pose3& pose, const Point3& point, const RangeFactor3D& factor) { - const auto &keys = factor.keys(); - return factor.unwhitenedError({{keys[0], genericValue(pose)}, {keys[1], genericValue(point)}}); + return factor.evaluateError(pose, point); } /* ************************************************************************* */ Vector factorErrorWithTransform2D(const Pose2& pose, const Point2& point, const RangeFactorWithTransform2D& factor) { - const auto &keys = factor.keys(); - return factor.unwhitenedError({{keys[0], genericValue(pose)}, {keys[1], genericValue(point)}}); + return factor.evaluateError(pose, point); } /* ************************************************************************* */ Vector factorErrorWithTransform3D(const Pose3& pose, const Point3& point, const RangeFactorWithTransform3D& factor) { - const auto &keys = factor.keys(); - return factor.unwhitenedError({{keys[0], genericValue(pose)}, {keys[1], genericValue(point)}}); + return factor.evaluateError(pose, point); } /* ************************************************************************* */ @@ -262,10 +258,8 @@ TEST( RangeFactor, Jacobian2D ) { Point2 point(-4.0, 11.0); // Use the factor to calculate the Jacobians - std::vector actualHs(2); - factor.unwhitenedError({{poseKey, genericValue(pose)}, {pointKey, genericValue(point)}}, actualHs); - const Matrix& H1Actual = actualHs.at(0); - const Matrix& H2Actual = actualHs.at(1); + Matrix H1Actual, H2Actual; + factor.evaluateError(pose, point, H1Actual, H2Actual); // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected;