Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Added Range to Point2 #783

Merged
merged 1 commit into from
Jun 5, 2021
Merged

Added Range to Point2 #783

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
13 changes: 13 additions & 0 deletions gtsam/geometry/Point2.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -82,5 +82,18 @@ GTSAM_EXPORT std::list<Point2> circleCircleIntersection(Point2 c1, Point2 c2, bo
GTSAM_EXPORT std::list<Point2> circleCircleIntersection(Point2 c1, double r1,
Point2 c2, double r2, double tol = 1e-9);

template <typename A1, typename A2>
struct Range;

template <>
struct Range<Point2, Point2> {
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

33 changes: 25 additions & 8 deletions gtsam/sam/tests/testRangeFactor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -40,9 +40,9 @@ typedef RangeFactorWithTransform<Pose2, Point2> RangeFactorWithTransform2D;
typedef RangeFactorWithTransform<Pose3, Point3> 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,
Expand Down Expand Up @@ -363,21 +363,38 @@ TEST( RangeFactor, Jacobian3DWithTransform ) {
CHECK(assert_equal(H2Expected, H2Actual, 1e-9));
}

/* ************************************************************************* */
// Do a test with Point2
TEST(RangeFactor, Point2) {
// Create a factor
RangeFactor<Point2> factor(11, 22, measurement, model);

// Set the linearization point
Point2 p11(1.0, 2.0), p22(-4.0, 11.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));
}

/* ************************************************************************* */
// Do a test with Point3
TEST(RangeFactor, Point3) {
// Create a factor
RangeFactor<Point3> factor(poseKey, pointKey, measurement, model);
RangeFactor<Point3> factor(11, 22, measurement, model);

// Set the linearization point
Point3 pose(1.0, 2.0, 00);
Point3 point(-4.0, 11.0, 0);
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 / 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));
}

/* ************************************************************************* */
Expand Down