point2.h
parent
3ebada2dc4
commit
02b5485c76
|
@ -1,6 +1,7 @@
|
|||
// A hack to serialize std::optional<T> to boost::archive
|
||||
// Don't know if it will work. Trying to follow this:
|
||||
// PR: https://github.com/boostorg/serialization/pull/148/files#
|
||||
#pragma once
|
||||
#include <optional>
|
||||
#include <boost/config.hpp>
|
||||
|
||||
|
|
|
@ -52,7 +52,7 @@ double distance2(const Point2& p, const Point2& q, OptionalJacobian<1, 2> H1,
|
|||
|
||||
/* ************************************************************************* */
|
||||
// Math inspired by http://paulbourke.net/geometry/circlesphere/
|
||||
boost::optional<Point2> circleCircleIntersection(double R_d, double r_d,
|
||||
std::optional<Point2> circleCircleIntersection(double R_d, double r_d,
|
||||
double tol) {
|
||||
|
||||
double R2_d2 = R_d*R_d; // Yes, RD-D2 !
|
||||
|
@ -61,17 +61,17 @@ boost::optional<Point2> circleCircleIntersection(double R_d, double r_d,
|
|||
|
||||
// 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
|
||||
if (h2<-tol) return {}; // allow *slightly* negative
|
||||
else if (h2<tol) return Point2(f,0.0); // one solution
|
||||
else return Point2(f,std::sqrt(h2)); // two solutions
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
list<Point2> circleCircleIntersection(Point2 c1, Point2 c2,
|
||||
boost::optional<Point2> fh) {
|
||||
std::optional<Point2> fh) {
|
||||
|
||||
list<Point2> solutions;
|
||||
// If fh==boost::none, there are no solutions, i.e., d > (R + r) || d < (R - r)
|
||||
// If fh==std::nullopt, there are no solutions, i.e., d > (R + r) || d < (R - r)
|
||||
if (fh) {
|
||||
// vector between circle centers
|
||||
Point2 c12 = c2-c1;
|
||||
|
@ -107,7 +107,7 @@ list<Point2> circleCircleIntersection(Point2 c1, double r1, Point2 c2,
|
|||
|
||||
// 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);
|
||||
std::optional<Point2> fh = circleCircleIntersection(R_d,r_d);
|
||||
|
||||
// Call version that takes fh
|
||||
return circleCircleIntersection(c1, c2, fh);
|
||||
|
|
|
@ -18,8 +18,11 @@
|
|||
#pragma once
|
||||
|
||||
#include <gtsam/base/VectorSpace.h>
|
||||
#include <gtsam/base/std_optional_serialization.h>
|
||||
#include <boost/serialization/nvp.hpp>
|
||||
|
||||
#include <optional>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/// As of GTSAM 4, in order to make GTSAM more lean,
|
||||
|
@ -62,7 +65,7 @@ inline Point2 operator*(double s, const Point2& p) {
|
|||
* @param tol: absolute tolerance below which we consider touching circles
|
||||
* @return optional Point2 with f and h, boost::none if no solution.
|
||||
*/
|
||||
GTSAM_EXPORT boost::optional<Point2> circleCircleIntersection(double R_d, double r_d, double tol = 1e-9);
|
||||
GTSAM_EXPORT std::optional<Point2> circleCircleIntersection(double R_d, double r_d, double tol = 1e-9);
|
||||
|
||||
/*
|
||||
* @brief Circle-circle intersection, from the normalized radii solution.
|
||||
|
@ -70,7 +73,7 @@ GTSAM_EXPORT boost::optional<Point2> circleCircleIntersection(double R_d, double
|
|||
* @param c2 center of second circle
|
||||
* @return list of solutions (0,1, or 2). Identical circles will return empty list, as well.
|
||||
*/
|
||||
GTSAM_EXPORT std::list<Point2> circleCircleIntersection(Point2 c1, Point2 c2, boost::optional<Point2> fh);
|
||||
GTSAM_EXPORT std::list<Point2> circleCircleIntersection(Point2 c1, Point2 c2, std::optional<Point2> fh);
|
||||
|
||||
/// Calculate the two means of a set of Point2 pairs
|
||||
GTSAM_EXPORT Point2Pair means(const std::vector<Point2Pair> &abPointPairs);
|
||||
|
|
|
@ -19,6 +19,7 @@
|
|||
#include <stdexcept>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <optional>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
@ -105,7 +106,7 @@ class SmartRangeFactor: public NoiseModelFactor {
|
|||
}
|
||||
|
||||
Circle2 circle1 = circles.front();
|
||||
boost::optional<Point2> best_fh;
|
||||
std::optional<Point2> best_fh;
|
||||
auto bestCircle2 = boost::make_optional(false, circle1); // fixes issue #38
|
||||
|
||||
// loop over all circles
|
||||
|
@ -115,7 +116,7 @@ class SmartRangeFactor: public NoiseModelFactor {
|
|||
if (d < 1e-9)
|
||||
continue; // skip circles that are in the same location
|
||||
// Find f and h, the intersection points in normalized circles
|
||||
boost::optional<Point2> fh = circleCircleIntersection(
|
||||
std::optional<Point2> fh = circleCircleIntersection(
|
||||
circle1.radius / d, it.radius / d);
|
||||
// Check if this pair is better by checking h = fh->y()
|
||||
// if h is large, the intersections are well defined.
|
||||
|
|
Loading…
Reference in New Issue