point2.h

gtsam/base/std_optional_serialization.h

@@ -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>
 

gtsam/geometry/Point2.cpp

@@ -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);

gtsam/geometry/Point2.h

@@ -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);

gtsam_unstable/slam/SmartRangeFactor.h

@@ -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.