Split up circle intersection code into three routines that are used in SmartRangeFactor
Frank Dellaert
parent
140e8a8c7a
commit
d041c5b8a8
|
|
@ -68,59 +68,54 @@ double Point2::distance(const Point2& point, boost::optional<Matrix&> 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<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 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<tol) return 0.0; // one solution
|
||||
else return sqrt(h2); // two solutions
|
||||
else if (h2<tol) return Point2(f,0.0); // one solution
|
||||
else return Point2(f,sqrt(h2)); // two solutions
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Math inspired by http://paulbourke.net/geometry/circlesphere/
|
||||
list<Point2> Point2::CircleCircleIntersection(double R, Point2 c, double r) {
|
||||
list<Point2> Point2::CircleCircleIntersection(Point2 c1, Point2 c2,
|
||||
boost::optional<Point2> fh) {
|
||||
|
||||
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
|
||||
// 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> Point2::CircleCircleIntersection(double R, Point2 c, double r) {
|
|||
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> solutions = Point2::CircleCircleIntersection(r1,c2-c1,r2);
|
||||
BOOST_FOREACH(Point2& p, solutions) p+= c1;
|
||||
return solutions;
|
||||
list<Point2> 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<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);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -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<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
|
||||
|
|
@ -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<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
|
||||
|
|
|
|||
|
|
@ -148,30 +148,30 @@ TEST( Point2, circleCircleIntersection) {
|
|||
double offset = 0.994987;
|
||||
// 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());
|
||||
|
||||
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(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(assert_equal(Point2(4.9, offset), common.front(), 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(assert_equal(Point2(5,0), touching2.front()));
|
||||
|
||||
// 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(assert_equal(Point2(-offset, 4.9), rotated.front(), 1e-6));
|
||||
EXPECT(assert_equal(Point2( offset, 4.9), rotated.back(), 1e-6));
|
||||
|
||||
// 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(assert_equal(Point2(0.1, offset), smaller.front(), 1e-6));
|
||||
EXPECT(assert_equal(Point2(0.1, -offset), smaller.back(), 1e-6));
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
@ -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);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
Loading…
Reference in New Issue