Point2.h
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
18 #pragma once
19 
20 #include <gtsam/base/VectorSpace.h>
22 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
23 #include <boost/serialization/nvp.hpp>
24 #endif
25 
26 #include <optional>
27 
28 namespace gtsam {
29 
32 typedef Vector2 Point2;
33 
34 // Convenience typedef
35 using Point2Pair = std::pair<Point2, Point2>;
36 GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const gtsam::Point2Pair &p);
37 
38 using Point2Pairs = std::vector<Point2Pair>;
39 
41 GTSAM_EXPORT double norm2(const Point2& p, OptionalJacobian<1, 2> H = {});
42 
44 GTSAM_EXPORT double distance2(const Point2& p1, const Point2& q,
45  OptionalJacobian<1, 2> H1 = {},
46  OptionalJacobian<1, 2> H2 = {});
47 
48 // For MATLAB wrapper
49 typedef std::vector<Point2, Eigen::aligned_allocator<Point2> > Point2Vector;
50 
52 inline Point2 operator*(double s, const Point2& p) {
53  return Point2(s * p.x(), s * p.y());
54 }
55 
56 /*
57  * @brief Circle-circle intersection, given normalized radii.
58  * Calculate f and h, respectively the parallel and perpendicular distance of
59  * the intersections of two circles along and from the line connecting the centers.
60  * Both are dimensionless fractions of the distance d between the circle centers.
61  * If the circles do not intersect or they are identical, returns {}.
62  * If one solution (touching circles, as determined by tol), h will be exactly zero.
63  * h is a good measure for how accurate the intersection will be, as when circles touch
64  * or nearly touch, the intersection is ill-defined with noisy radius measurements.
65  * @param R_d : R/d, ratio of radius of first circle to distance between centers
66  * @param r_d : r/d, ratio of radius of second circle to distance between centers
67  * @param tol: absolute tolerance below which we consider touching circles
68  * @return optional Point2 with f and h, {} if no solution.
69  */
70 GTSAM_EXPORT std::optional<Point2> circleCircleIntersection(double R_d, double r_d, double tol = 1e-9);
71 
72 /*
73  * @brief Circle-circle intersection, from the normalized radii solution.
74  * @param c1 center of first circle
75  * @param c2 center of second circle
76  * @return list of solutions (0,1, or 2). Identical circles will return empty list, as well.
77  */
78 GTSAM_EXPORT std::list<Point2> circleCircleIntersection(Point2 c1, Point2 c2, std::optional<Point2> fh);
79 
81 GTSAM_EXPORT Point2Pair means(const std::vector<Point2Pair> &abPointPairs);
82 
92 GTSAM_EXPORT std::list<Point2> circleCircleIntersection(Point2 c1, double r1,
93  Point2 c2, double r2, double tol = 1e-9);
94 
95 template <typename A1, typename A2>
96 struct Range;
97 
98 template <>
99 struct Range<Point2, Point2> {
100  typedef double result_type;
101  double operator()(const Point2& p, const Point2& q,
102  OptionalJacobian<1, 2> H1 = {},
103  OptionalJacobian<1, 2> H2 = {}) {
104  return distance2(p, q, H1, H2);
105  }
106 };
107 
108 } // \ namespace gtsam
109 
std::ostream & operator<<(std::ostream &os, const Dih6 &m)
Definition: testGroup.cpp:109
Vector3f p1
Vector2 Point2
Definition: Point2.h:32
double operator()(const Point2 &p, const Point2 &q, OptionalJacobian< 1, 2 > H1={}, OptionalJacobian< 1, 2 > H2={})
Definition: Point2.h:101
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate set rrange [*:*] noreverse nowriteback set trange [*:*] noreverse nowriteback set urange [*:*] noreverse nowriteback set vrange [*:*] noreverse nowriteback set xlabel matrix size set x2label set timefmt d m y n H
Point2 operator*(double s, const Point2 &p)
multiply with scalar
Definition: Point2.h:52
std::vector< Point2Pair > Point2Pairs
Definition: Point2.h:38
std::optional< Point2 > circleCircleIntersection(double R_d, double r_d, double tol)
Definition: Point2.cpp:55
Array< double, 1, 3 > e(1./3., 0.5, 2.)
RealScalar s
static const double r2
EIGEN_DEVICE_FUNC const Scalar & q
traits
Definition: chartTesting.h:28
Point2Pair means(const std::vector< Point2Pair > &abPointPairs)
Calculate the two means of a set of Point2 pairs.
Definition: Point2.cpp:116
static const double r1
ofstream os("timeSchurFactors.csv")
double distance2(const Point2 &p, const Point2 &q, OptionalJacobian< 1, 2 > H1, OptionalJacobian< 1, 2 > H2)
distance between two points
Definition: Point2.cpp:39
std::pair< Point2, Point2 > Point2Pair
Definition: Point2.h:35
std::vector< Point2, Eigen::aligned_allocator< Point2 > > Point2Vector
Definition: Point2.h:49
float * p
const G double tol
Definition: Group.h:86
double norm2(const Point2 &p, OptionalJacobian< 1, 2 > H)
Distance of the point from the origin, with Jacobian.
Definition: Point2.cpp:27


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:35:14