diff --git a/gtsam/geometry/Point2.cpp b/gtsam/geometry/Point2.cpp index 325d24531..6f219b9db 100644 --- a/gtsam/geometry/Point2.cpp +++ b/gtsam/geometry/Point2.cpp @@ -68,59 +68,54 @@ double Point2::distance(const Point2& point, boost::optional H1, return d.norm(); } +/* + * Calculate f and h, respectively the parallel and perpendicular distance of + * the intersections of two circles along and from the line connecting the centers. + * Both are dimensionless fractions of the distance d between the circle centers. + * If the circles do not intersect or they are identical, returns boost::none. + * If one solution (touching circles, as determined by tol), h will be exactly zero. + * h is a good measure for how accurate the intersection will be, as when circles touch + * or nearly touch, the intersection is ill-defined with noisy radius measurements. + * @param R_d : R/d, ratio of radius of first circle to distance between centers + * @param r_d : r/d, ratio of radius of second circle to distance between centers + * @param tol: absolute tolerance below which we consider touching circles + */ /* ************************************************************************* */ -// Calculate h, the distance of the intersections of two circles from the center line. -// This is a dimensionless fraction of the distance d between the circle centers, -// and also determines how "good" the intersection is. If the circles do not intersect -// or they are identical, returns boost::none. If one solution, h -> 0. -// @param R_d : R/d, ratio of radius of first circle to distance between centers -// @param r_d : r/d, ratio of radius of second circle to distance between centers -// @param tol: absolute tolerance below which we consider touching circles // Math inspired by http://paulbourke.net/geometry/circlesphere/ -static boost::optional circleCircleQuality(double R_d, double r_d, double tol=1e-9) { +boost::optional Point2::CircleCircleIntersection(double R_d, double r_d, + double tol) { double R2_d2 = R_d*R_d; // Yes, RD-D2 ! double f = 0.5 + 0.5*(R2_d2 - r_d*r_d); - double h2 = R2_d2 - f*f; + double h2 = R2_d2 - f*f; // just right triangle rule // h^2<0 is equivalent to (d > (R + r) || d < (R - r)) // Hence, there are only solutions if >=0 if (h2<-tol) return boost::none; // allow *slightly* negative - else if (h2 Point2::CircleCircleIntersection(double R, Point2 c, double r) { +list Point2::CircleCircleIntersection(Point2 c1, Point2 c2, + boost::optional fh) { list solutions; + // If fh==boost::none, there are no solutions, i.e., d > (R + r) || d < (R - r) + if (fh) { + // vector between circle centers + Point2 c12 = c2-c1; - // distance between circle centers. - double d2 = c.x() * c.x() + c.y() * c.y(), d = sqrt(d2); - - // circles coincide, either no solution or infinite number of solutions. - if (d2<1e-9) return solutions; - - // Calculate h, the distance of the intersections from the center line, - // as a dimensionless fraction of the distance d. - // It is the solution of a quadratic, so it has either 2 solutions, is 0, or none - double _d = 1.0/d, R_d = R*_d, r_d=r*_d; - boost::optional h = circleCircleQuality(R_d,r_d); - - // If h== boost::none, there are no solutions, i.e., d > (R + r) || d < (R - r) - if (h) { // Determine p2, the point where the line through the circle // intersection points crosses the line between the circle centers. - double f = 0.5 + 0.5*(R_d*R_d - r_d*r_d); - Point2 p2 = f * c; + Point2 p2 = c1 + fh->x() * c12; // If h == 0, the circles are touching, so just return one point - if (h==0.0) + if (fh->y()==0.0) solutions.push_back(p2); else { // determine the offsets of the intersection points from p - Point2 offset = (*h) * Point2(-c.y(), c.x()); + Point2 offset = fh->y() * Point2(-c12.y(), c12.x()); // Determine the absolute intersection points. solutions.push_back(p2 + offset); @@ -130,56 +125,22 @@ list Point2::CircleCircleIntersection(double R, Point2 c, double r) { return solutions; } -//list Point2::CircleCircleIntersection(double R, Point2 c, double r) { -// -// list solutions; -// -// // Math inspired by http://paulbourke.net/geometry/circlesphere/ -// // Changed to avoid sqrt in case there are 0 or 1 intersections, and only one div -// -// // squared distance between circle centers. -// double d2 = c.x() * c.x() + c.y() * c.y(); -// -// // A crucial quantity we compute is h, a the distance of the intersections -// // from the center line, as a dimensionless fraction of the distance d. -// // It is the solution of a quadratic, so it has either 2 solutions, is 0, or none -// // We calculate it as sqrt(h^2*d^4)/d^2, but first check whether h^2*d^4>=0 -// double R2 = R*R; -// double R2d2 = R2*d2; // yes, R2-D2! -// double b = R2 + d2 - r*r; -// double b2 = b*b; -// double h2d4 = R2d2 - 0.25*b2; // h^2*d^4 -// -// // h^2*d^4<0 is equivalent to (d > (R + r) || d < (R - r)) -// // Hence, there are only solutions if >=0 -// if (h2d4>=0) { -// // Determine p2, the point where the line through the circle -// // intersection points crosses the line between the circle centers. -// double i2 = 1.0/d2; -// double f = 0.5*b*i2; -// Point2 p2 = f * c; -// -// // If h^2*d^4 == 0, the circles are touching, so just return one point -// if (h2d4 < 1e-9) -// solutions.push_back(p2); -// else { -// // determine the offsets of the intersection points from p -// double h = sqrt(h2d4)*i2; // h = sqrt(h^2*d^4)/d^2 -// Point2 offset = h * Point2(-c.y(), c.x()); -// -// // Determine the absolute intersection points. -// solutions.push_back(p2 + offset); -// solutions.push_back(p2 - offset); -// } -// } -// return solutions; -//} -// /* ************************************************************************* */ -list Point2::CircleCircleIntersection(Point2 c1, double r1, Point2 c2, double r2) { - list solutions = Point2::CircleCircleIntersection(r1,c2-c1,r2); - BOOST_FOREACH(Point2& p, solutions) p+= c1; - return solutions; +list Point2::CircleCircleIntersection(Point2 c1, double r1, Point2 c2, + double r2, double tol) { + + // distance between circle centers. + double d = c1.dist(c2); + + // centers coincide, either no solution or infinite number of solutions. + if (d<1e-9) return list(); + + // Calculate f and h given normalized radii + double _d = 1.0/d, R_d = r1*_d, r_d=r2*_d; + boost::optional fh = CircleCircleIntersection(R_d,r_d); + + // Call version that takes fh + return CircleCircleIntersection(c1, c2, fh); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index 05a47e379..73f0b83f6 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -65,14 +65,30 @@ public: y_ = v(1); } - /** - * @brief Intersect Circle with radius R at origin, with circle of radius r at c - * @param R radius of circle at origin - * @param center center of second circle - * @param r radius of second circle - * @return list of solutions (0,1, or 2 points) + /* + * @brief Circle-circle intersection, given normalized radii. + * Calculate f and h, respectively the parallel and perpendicular distance of + * the intersections of two circles along and from the line connecting the centers. + * Both are dimensionless fractions of the distance d between the circle centers. + * If the circles do not intersect or they are identical, returns boost::none. + * If one solution (touching circles, as determined by tol), h will be exactly zero. + * h is a good measure for how accurate the intersection will be, as when circles touch + * or nearly touch, the intersection is ill-defined with noisy radius measurements. + * @param R_d : R/d, ratio of radius of first circle to distance between centers + * @param r_d : r/d, ratio of radius of second circle to distance between centers + * @param tol: absolute tolerance below which we consider touching circles + * @return optional Point2 with f and h, boost::none if no solution. */ - static std::list CircleCircleIntersection(double R, Point2 c, double r); + static boost::optional CircleCircleIntersection(double R_d, double r_d, + double tol = 1e-9); + + /* + * @brief Circle-circle intersection, from the normalized radii solution. + * @param c1 center of first circle + * @param c2 center of second circle + * @return list of solutions (0,1, or 2). Identical circles will return empty list, as well. + */ + static std::list CircleCircleIntersection(Point2 c1, Point2 c2, boost::optional); /** * @brief Intersect 2 circles @@ -80,8 +96,11 @@ public: * @param r1 radius of first circle * @param c2 center of second circle * @param r2 radius of second circle + * @param tol: absolute tolerance below which we consider touching circles + * @return list of solutions (0,1, or 2). Identical circles will return empty list, as well. */ - static std::list CircleCircleIntersection(Point2 c1, double r1, Point2 c2, double r2); + static std::list CircleCircleIntersection(Point2 c1, double r1, + Point2 c2, double r2, double tol = 1e-9); /// @} /// @name Testable diff --git a/gtsam/geometry/tests/testPoint2.cpp b/gtsam/geometry/tests/testPoint2.cpp index 61aad1ce5..5a6f38f75 100644 --- a/gtsam/geometry/tests/testPoint2.cpp +++ b/gtsam/geometry/tests/testPoint2.cpp @@ -148,30 +148,30 @@ TEST( Point2, circleCircleIntersection) { double offset = 0.994987; // Test intersections of circle moving from inside to outside - list inside = Point2::CircleCircleIntersection(5,Point2(0,0),1); + list inside = Point2::CircleCircleIntersection(Point2(0,0),5,Point2(0,0),1); EXPECT_LONGS_EQUAL(0,inside.size()); - list touching1 = Point2::CircleCircleIntersection(5,Point2(4,0),1); + list touching1 = Point2::CircleCircleIntersection(Point2(0,0),5,Point2(4,0),1); EXPECT_LONGS_EQUAL(1,touching1.size()); EXPECT(assert_equal(Point2(5,0), touching1.front())); - list common = Point2::CircleCircleIntersection(5,Point2(5,0),1); + list common = Point2::CircleCircleIntersection(Point2(0,0),5,Point2(5,0),1); EXPECT_LONGS_EQUAL(2,common.size()); EXPECT(assert_equal(Point2(4.9, offset), common.front(), 1e-6)); EXPECT(assert_equal(Point2(4.9, -offset), common.back(), 1e-6)); - list touching2 = Point2::CircleCircleIntersection(5,Point2(6,0),1); + list touching2 = Point2::CircleCircleIntersection(Point2(0,0),5,Point2(6,0),1); EXPECT_LONGS_EQUAL(1,touching2.size()); EXPECT(assert_equal(Point2(5,0), touching2.front())); // test rotated case - list rotated = Point2::CircleCircleIntersection(5,Point2(0,5),1); + list rotated = Point2::CircleCircleIntersection(Point2(0,0),5,Point2(0,5),1); EXPECT_LONGS_EQUAL(2,rotated.size()); EXPECT(assert_equal(Point2(-offset, 4.9), rotated.front(), 1e-6)); EXPECT(assert_equal(Point2( offset, 4.9), rotated.back(), 1e-6)); // test r1 smaller = Point2::CircleCircleIntersection(1,Point2(5,0),5); + list smaller = Point2::CircleCircleIntersection(Point2(0,0),1,Point2(5,0),5); EXPECT_LONGS_EQUAL(2,smaller.size()); EXPECT(assert_equal(Point2(0.1, offset), smaller.front(), 1e-6)); EXPECT(assert_equal(Point2(0.1, -offset), smaller.back(), 1e-6)); diff --git a/gtsam_unstable/slam/SmartRangeFactor.h b/gtsam_unstable/slam/SmartRangeFactor.h new file mode 100644 index 000000000..b054a249f --- /dev/null +++ b/gtsam_unstable/slam/SmartRangeFactor.h @@ -0,0 +1,136 @@ +/** + * @file SmartRangeFactor.h + * + * @brief A smart factor for range-only SLAM that does initialization and marginalization + * + * @date Sep 10, 2012 + * @author Alex Cunningham + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace gtsam { + +/** + * Smart factor for range SLAM + * @addtogroup SLAM + */ +class GTSAM_UNSTABLE_EXPORT SmartRangeFactor: public NoiseModelFactor { +protected: + + struct KeyedRange { + KeyedRange(Key k, double r) : + key(k), range(r) { + } + Key key; + double range; + }; + + struct Circle2 { + Circle2(const Point2& p, double r) : + center(p), radius(r) { + } + Point2 center; + double radius; + }; + + /// Range measurements + std::list measurements_; + +public: + + /** Default constructor: don't use directly */ + SmartRangeFactor() { + } + + /** standard binary constructor */ + SmartRangeFactor(const SharedNoiseModel& model) { + } + + virtual ~SmartRangeFactor() { + } + + /// Add a range measurement to a pose with given key. + void addRange(Key key, double measuredRange) { + measurements_.push_back(KeyedRange(key, measuredRange)); + } + + /// Number of measurements added + size_t nrMeasurements() const { + return measurements_.size(); + } + + // testable + + /** print */ + virtual void print(const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + } + + /** Check if two factors are equal */ + virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const { + return false; + } + + // factor interface + + /** + * Triangulate a point from at least three pose-range pairs + * Checks for best pair that includes first point + */ + static Point2 triangulate( + const std::list& circles) { + Circle2 circle1 = circles.front(); + boost::optional best_fh; + boost::optional best_circle; + BOOST_FOREACH(const Circle2& it, circles) { + // distance between circle centers. + double d = circle1.center.dist(it.center); + if (d<1e-9) continue; + boost::optional fh = Point2::CircleCircleIntersection(circle1.radius/d,it.radius/d); + if (fh && (!best_fh || fh->y()>best_fh->y())) { + best_fh = fh; + best_circle = it; + } + } + std::list solutions = Point2::CircleCircleIntersection( + circle1.center, best_circle->center, best_fh); + solutions.front().print("front"); + solutions.back().print("back"); + return solutions.front(); + } + + /** + * Error function *without* the NoiseModel, \f$ z-h(x) \f$. + */ + virtual Vector unwhitenedError(const Values& x, + boost::optional&> H = boost::none) const { + size_t K = nrMeasurements(); + Vector errors = zero(K); + if (K >= 3) { + std::list circles; + BOOST_FOREACH(const KeyedRange& it, measurements_) { + const Pose2& pose = x.at(it.key); + circles.push_back( + Circle2(pose.translation(), it.range)); + } + Point2 optimizedPoint = triangulate(circles); + size_t i = 0; + BOOST_FOREACH(const Circle2& it, circles) { + errors[i++] = it.radius - it.center.distance(optimizedPoint); + } + } + return errors; + } + +}; + +} // \namespace gtsam + diff --git a/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp b/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp new file mode 100644 index 000000000..0b8be50ef --- /dev/null +++ b/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp @@ -0,0 +1,83 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testSmartRangeFactor.cpp + * @brief Unit tests for SmartRangeFactor Class + * @author Frank Dellaert + * @date Nov 2013 + */ + +#include +#include + +using namespace std; +using namespace gtsam; + +const noiseModel::Base::shared_ptr gaussian = noiseModel::Isotropic::Sigma(1, + 2.0); + +/* ************************************************************************* */ + +TEST( SmartRangeFactor, constructor ) { + SmartRangeFactor f1; + LONGS_EQUAL(0, f1.nrMeasurements()) + SmartRangeFactor f2(gaussian); + LONGS_EQUAL(0, f2.nrMeasurements()) +} + +/* ************************************************************************* */ + +TEST( SmartRangeFactor, addRange ) { + SmartRangeFactor f(gaussian); + f.addRange(1, 10); + f.addRange(1, 12); + LONGS_EQUAL(2, f.nrMeasurements()) +} + +/* ************************************************************************* */ + +TEST( SmartRangeFactor, unwhitenedError ) { + // Test situation: + Point2 p(0, 10); + Pose2 pose1(0, 0, 0), pose2(5, 0, 0), pose3(5, 5, 0); + double r1 = pose1.range(p), r2 = pose2.range(p), r3 = pose3.range(p); + DOUBLES_EQUAL(10, r1, 1e-9); + DOUBLES_EQUAL(sqrt(100+25), r2, 1e-9); + DOUBLES_EQUAL(sqrt(50), r3, 1e-9); + + Values values; // all correct + values.insert(1, pose1); + values.insert(2, pose2); + values.insert(3, pose3); + + SmartRangeFactor f(gaussian); + f.addRange(1, r1); + + // Whenever there are two ranges or less, error should be zero + Vector actual1 = f.unwhitenedError(values); + EXPECT(assert_equal(Vector_(1,0.0), actual1)); + f.addRange(2, r2); + Vector actual2 = f.unwhitenedError(values); + EXPECT(assert_equal(Vector2(0,0), actual2)); + + f.addRange(3, r3); + Vector actual3 = f.unwhitenedError(values); + EXPECT(assert_equal(Vector3(0,0,0), actual3)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ +