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

Split up circle intersection code into three routines that are used in SmartRangeFactor

release/4.3a0
Frank Dellaert 2013-06-24 08:24:56 +00:00
parent 140e8a8c7a
commit d041c5b8a8
5 changed files with 293 additions and 94 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

121
gtsam/geometry/Point2.cpp
View File

@ -68,59 +68,54 @@ double Point2::distance(const Point2& point, boost::optional<Matrix&> H1,
return d.norm(); 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/ // Math inspired by http://paulbourke.net/geometry/circlesphere/
static boost::optional<double> circleCircleQuality(double R_d, double r_d, double tol=1e-9) { boost::optional<Point2> Point2::CircleCircleIntersection(double R_d, double r_d,
double tol) {
double R2_d2 = R_d*R_d; // Yes, RD-D2 ! double R2_d2 = R_d*R_d; // Yes, RD-D2 !
double f = 0.5 + 0.5*(R2_d2 - r_d*r_d); 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)) // h^2<0 is equivalent to (d > (R + r) || d < (R - r))
// Hence, there are only solutions if >=0 // Hence, there are only solutions if >=0
if (h2<-tol) return boost::none; // allow *slightly* negative if (h2<-tol) return boost::none; // allow *slightly* negative
else if (h2<tol) return 0.0; // one solution else if (h2<tol) return Point2(f,0.0); // one solution
else return sqrt(h2); // two solutions else return Point2(f,sqrt(h2)); // two solutions
} }
/* ************************************************************************* */ /* ************************************************************************* */
// Math inspired by http://paulbourke.net/geometry/circlesphere/ list<Point2> Point2::CircleCircleIntersection(Point2 c1, Point2 c2,
list<Point2> Point2::CircleCircleIntersection(double R, Point2 c, double r) { boost::optional<Point2> fh) {
list<Point2> solutions; list<Point2> 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<double> 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 // Determine p2, the point where the line through the circle
// intersection points crosses the line between the circle centers. // 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 = c1 + fh->x() * c12;
Point2 p2 = f * c;
// If h == 0, the circles are touching, so just return one point // 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); solutions.push_back(p2);
else { else {
// determine the offsets of the intersection points from p // 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. // Determine the absolute intersection points.
solutions.push_back(p2 + offset); solutions.push_back(p2 + offset);
@ -130,56 +125,22 @@ list<Point2> Point2::CircleCircleIntersection(double R, Point2 c, double r) {
return solutions; return solutions;
} }
//list<Point2> Point2::CircleCircleIntersection(double R, Point2 c, double r) {
//
// list<Point2> 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> Point2::CircleCircleIntersection(Point2 c1, double r1, Point2 c2, double r2) { list<Point2> Point2::CircleCircleIntersection(Point2 c1, double r1, Point2 c2,
list<Point2> solutions = Point2::CircleCircleIntersection(r1,c2-c1,r2); double r2, double tol) {
BOOST_FOREACH(Point2& p, solutions) p+= c1;
return solutions; // 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<Point2>();
// Calculate f and h given normalized radii
double _d = 1.0/d, R_d = r1*_d, r_d=r2*_d;
boost::optional<Point2> fh = CircleCircleIntersection(R_d,r_d);
// Call version that takes fh
return CircleCircleIntersection(c1, c2, fh);
} }
/* ************************************************************************* */ /* ************************************************************************* */

35
gtsam/geometry/Point2.h
View File

@ -65,14 +65,30 @@ public:
y_ = v(1); y_ = v(1);
} }
/** /*
* @brief Intersect Circle with radius R at origin, with circle of radius r at c * @brief Circle-circle intersection, given normalized radii.
* @param R radius of circle at origin * Calculate f and h, respectively the parallel and perpendicular distance of
* @param center center of second circle * the intersections of two circles along and from the line connecting the centers.
* @param r radius of second circle * Both are dimensionless fractions of the distance d between the circle centers.
* @return list of solutions (0,1, or 2 points) * 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<Point2> CircleCircleIntersection(double R, Point2 c, double r); static boost::optional<Point2> 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<Point2> CircleCircleIntersection(Point2 c1, Point2 c2, boost::optional<Point2>);
/** /**
* @brief Intersect 2 circles * @brief Intersect 2 circles
@ -80,8 +96,11 @@ public:
* @param r1 radius of first circle * @param r1 radius of first circle
* @param c2 center of second circle * @param c2 center of second circle
* @param r2 radius 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<Point2> CircleCircleIntersection(Point2 c1, double r1, Point2 c2, double r2); static std::list<Point2> CircleCircleIntersection(Point2 c1, double r1,
Point2 c2, double r2, double tol = 1e-9);
/// @} /// @}
/// @name Testable /// @name Testable

12
gtsam/geometry/tests/testPoint2.cpp
View File

@ -148,30 +148,30 @@ TEST( Point2, circleCircleIntersection) {
double offset = 0.994987; double offset = 0.994987;
// Test intersections of circle moving from inside to outside // Test intersections of circle moving from inside to outside
list<Point2> inside = Point2::CircleCircleIntersection(5,Point2(0,0),1); list<Point2> inside = Point2::CircleCircleIntersection(Point2(0,0),5,Point2(0,0),1);
EXPECT_LONGS_EQUAL(0,inside.size()); EXPECT_LONGS_EQUAL(0,inside.size());
list<Point2> touching1 = Point2::CircleCircleIntersection(5,Point2(4,0),1); list<Point2> touching1 = Point2::CircleCircleIntersection(Point2(0,0),5,Point2(4,0),1);
EXPECT_LONGS_EQUAL(1,touching1.size()); EXPECT_LONGS_EQUAL(1,touching1.size());
EXPECT(assert_equal(Point2(5,0), touching1.front())); EXPECT(assert_equal(Point2(5,0), touching1.front()));
list<Point2> common = Point2::CircleCircleIntersection(5,Point2(5,0),1); list<Point2> common = Point2::CircleCircleIntersection(Point2(0,0),5,Point2(5,0),1);
EXPECT_LONGS_EQUAL(2,common.size()); 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.front(), 1e-6));
EXPECT(assert_equal(Point2(4.9, -offset), common.back(), 1e-6)); EXPECT(assert_equal(Point2(4.9, -offset), common.back(), 1e-6));
list<Point2> touching2 = Point2::CircleCircleIntersection(5,Point2(6,0),1); list<Point2> touching2 = Point2::CircleCircleIntersection(Point2(0,0),5,Point2(6,0),1);
EXPECT_LONGS_EQUAL(1,touching2.size()); EXPECT_LONGS_EQUAL(1,touching2.size());
EXPECT(assert_equal(Point2(5,0), touching2.front())); EXPECT(assert_equal(Point2(5,0), touching2.front()));
// test rotated case // test rotated case
list<Point2> rotated = Point2::CircleCircleIntersection(5,Point2(0,5),1); list<Point2> rotated = Point2::CircleCircleIntersection(Point2(0,0),5,Point2(0,5),1);
EXPECT_LONGS_EQUAL(2,rotated.size()); 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.front(), 1e-6));
EXPECT(assert_equal(Point2( offset, 4.9), rotated.back(), 1e-6)); EXPECT(assert_equal(Point2( offset, 4.9), rotated.back(), 1e-6));
// test r1<r2 // test r1<r2
list<Point2> smaller = Point2::CircleCircleIntersection(1,Point2(5,0),5); list<Point2> smaller = Point2::CircleCircleIntersection(Point2(0,0),1,Point2(5,0),5);
EXPECT_LONGS_EQUAL(2,smaller.size()); 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.front(), 1e-6));
EXPECT(assert_equal(Point2(0.1, -offset), smaller.back(), 1e-6)); EXPECT(assert_equal(Point2(0.1, -offset), smaller.back(), 1e-6));

136
gtsam_unstable/slam/SmartRangeFactor.h Normal file
View File

@ -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 <gtsam_unstable/base/dllexport.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/Key.h>
#include <gtsam/geometry/Pose2.h>
#include <boost/foreach.hpp>
#include <map>
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<KeyedRange> 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<Circle2>& circles) {
Circle2 circle1 = circles.front();
boost::optional<Point2> best_fh;
boost::optional<Circle2> 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<Point2> 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<Point2> 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<std::vector<Matrix>&> H = boost::none) const {
size_t K = nrMeasurements();
Vector errors = zero(K);
if (K >= 3) {
std::list<Circle2> circles;
BOOST_FOREACH(const KeyedRange& it, measurements_) {
const Pose2& pose = x.at<Pose2>(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

83
gtsam_unstable/slam/tests/testSmartRangeFactor.cpp Normal file
View File

@ -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 <gtsam_unstable/slam/SmartRangeFactor.h>
#include <CppUnitLite/TestHarness.h>
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);
}
/* ************************************************************************* */