12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge pull request #783 from borglab/feature/point2range 

Added Range to Point2
release/4.3a0
Fan Jiang 2021-06-04 23:23:48 -04:00 committed by GitHub
parent 2e29d45e1a cb7adde416
commit 5c9e4136b7
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 40 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
gtsam/geometry/Point2.h
View File

@ -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, GTSAM_EXPORT std::list<Point2> circleCircleIntersection(Point2 c1, double r1,
Point2 c2, double r2, double tol = 1e-9); 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 } // \ namespace gtsam

37
gtsam/sam/tests/testRangeFactor.cpp
View File

@ -40,9 +40,9 @@ typedef RangeFactorWithTransform<Pose2, Point2> RangeFactorWithTransform2D;
typedef RangeFactorWithTransform<Pose3, Point3> RangeFactorWithTransform3D; typedef RangeFactorWithTransform<Pose3, Point3> RangeFactorWithTransform3D;
// Keys are deliberately *not* in sorted order to test that case. // Keys are deliberately *not* in sorted order to test that case.
Key poseKey(2); constexpr Key poseKey(2);
Key pointKey(1); constexpr Key pointKey(1);
double measurement(10.0); constexpr double measurement(10.0);
/* ************************************************************************* */ /* ************************************************************************* */
Vector factorError2D(const Pose2& pose, const Point2& point, Vector factorError2D(const Pose2& pose, const Point2& point,
@ -364,20 +364,37 @@ TEST( RangeFactor, Jacobian3DWithTransform ) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
// Do a test with Point3 // Do a test with Point2
TEST(RangeFactor, Point3) { TEST(RangeFactor, Point2) {
// Create a factor // Create a factor
RangeFactor<Point3> factor(poseKey, pointKey, measurement, model); RangeFactor<Point2> factor(11, 22, measurement, model);
// Set the linearization point // Set the linearization point
Point3 pose(1.0, 2.0, 00); Point2 p11(1.0, 2.0), p22(-4.0, 11.0);
Point3 point(-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(); Vector expectedError = (Vector(1) << 0.295630141).finished();
// Verify we get the expected error // 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<Point3> 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));
} }
/* ************************************************************************* */ /* ************************************************************************* */