diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index e6574fe41..17f87f656 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -82,5 +82,18 @@ GTSAM_EXPORT std::list circleCircleIntersection(Point2 c1, Point2 c2, bo GTSAM_EXPORT std::list circleCircleIntersection(Point2 c1, double r1, Point2 c2, double r2, double tol = 1e-9); +template +struct Range; + +template <> +struct Range { + typedef double result_type; + double operator()(const Point2& p, const Point2& q, + OptionalJacobian<1, 2> H1 = boost::none, + OptionalJacobian<1, 2> H2 = boost::none) { + return distance2(p, q, H1, H2); + } +}; + } // \ namespace gtsam diff --git a/gtsam/sam/tests/testRangeFactor.cpp b/gtsam/sam/tests/testRangeFactor.cpp index 8ae3d818b..673d4e052 100644 --- a/gtsam/sam/tests/testRangeFactor.cpp +++ b/gtsam/sam/tests/testRangeFactor.cpp @@ -40,9 +40,9 @@ typedef RangeFactorWithTransform RangeFactorWithTransform2D; typedef RangeFactorWithTransform RangeFactorWithTransform3D; // Keys are deliberately *not* in sorted order to test that case. -Key poseKey(2); -Key pointKey(1); -double measurement(10.0); +constexpr Key poseKey(2); +constexpr Key pointKey(1); +constexpr double measurement(10.0); /* ************************************************************************* */ Vector factorError2D(const Pose2& pose, const Point2& point, @@ -364,20 +364,37 @@ TEST( RangeFactor, Jacobian3DWithTransform ) { } /* ************************************************************************* */ -// Do a test with Point3 -TEST(RangeFactor, Point3) { +// Do a test with Point2 +TEST(RangeFactor, Point2) { // Create a factor - RangeFactor factor(poseKey, pointKey, measurement, model); + RangeFactor factor(11, 22, measurement, model); // Set the linearization point - Point3 pose(1.0, 2.0, 00); - Point3 point(-4.0, 11.0, 0); + Point2 p11(1.0, 2.0), p22(-4.0, 11.0); - // The expected error is ||(5.0, 9.0)|| - 10.0 = 0.295630141 meter / UnitCovariance + // The expected error is ||(5.0, 9.0)|| - 10.0 = 0.295630141 meter Vector expectedError = (Vector(1) << 0.295630141).finished(); // Verify we get the expected error - CHECK(assert_equal(expectedError, factor.unwhitenedError({{poseKey, genericValue(pose)}, {pointKey, genericValue(point)}}), 1e-9)); + Values values {{11, genericValue(p11)}, {22, genericValue(p22)}}; + CHECK(assert_equal(expectedError, factor.unwhitenedError(values), 1e-9)); +} + +/* ************************************************************************* */ +// Do a test with Point3 +TEST(RangeFactor, Point3) { + // Create a factor + RangeFactor factor(11, 22, measurement, model); + + // Set the linearization point + Point3 p11(1.0, 2.0, 0.0), p22(-4.0, 11.0, 0); + + // The expected error is ||(5.0, 9.0)|| - 10.0 = 0.295630141 meter + Vector expectedError = (Vector(1) << 0.295630141).finished(); + + // Verify we get the expected error + Values values {{11, genericValue(p11)}, {22, genericValue(p22)}}; + CHECK(assert_equal(expectedError, factor.unwhitenedError(values), 1e-9)); } /* ************************************************************************* */