release/4.3a0
kartik arcot 2023-01-11 15:21:41 -08:00
parent 3ebada2dc4
commit 02b5485c76
4 changed files with 14 additions and 9 deletions

View File

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

View File

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

View File

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

View File

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