Point2.h

Go to the documentation of this file.

/* ----------------------------------------------------------------------------

 * 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

 * -------------------------------------------------------------------------- */

#pragma once

#include <gtsam/base/VectorSpace.h>
#include <gtsam/base/std_optional_serialization.h>
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
#include <boost/serialization/nvp.hpp>
#endif

#include <optional>

namespace gtsam {

typedef Vector2 Point2;
  
// Convenience typedef
using Point2Pair = std::pair<Point2, Point2>;
GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const gtsam::Point2Pair &p);

using Point2Pairs = std::vector<Point2Pair>;

GTSAM_EXPORT double norm2(const Point2& p, OptionalJacobian<1, 2> H = {});

GTSAM_EXPORT double distance2(const Point2& p1, const Point2& q,
                 OptionalJacobian<1, 2> H1 = {},
                 OptionalJacobian<1, 2> H2 = {});

// For MATLAB wrapper
typedef std::vector<Point2, Eigen::aligned_allocator<Point2> > Point2Vector;

inline Point2 operator*(double s, const Point2& p) {
  return Point2(s * p.x(), s * p.y());
}

/*
 * @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 {}.
 * 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, {} if no solution.
 */
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.
 * @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.
 */
GTSAM_EXPORT std::list<Point2> circleCircleIntersection(Point2 c1, Point2 c2, std::optional<Point2> fh);
  
GTSAM_EXPORT Point2Pair means(const std::vector<Point2Pair> &abPointPairs);

GTSAM_EXPORT std::list<Point2> circleCircleIntersection(Point2 c1, double r1,
    Point2 c2, double r2, double tol = 1e-9);

template <typename A1, typename A2>
struct Range;

template <>
struct Range<Point2, Point2> {
  typedef double result_type;
  double operator()(const Point2& p, const Point2& q,
                    OptionalJacobian<1, 2> H1 = {},
                    OptionalJacobian<1, 2> H2 = {}) {
    return distance2(p, q, H1, H2);
  }
};

} // \ namespace gtsam