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