gtsam 4.2.0
gtsam
Loading...
Searching...
No Matches
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>
21#include <boost/serialization/nvp.hpp>
22
23namespace gtsam {
24
27typedef Vector2 Point2;
28
29// Convenience typedef
30using Point2Pair = std::pair<Point2, Point2>;
31GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const gtsam::Point2Pair &p);
32
33using Point2Pairs = std::vector<Point2Pair>;
34
36GTSAM_EXPORT double norm2(const Point2& p, OptionalJacobian<1, 2> H = boost::none);
37
39GTSAM_EXPORT double distance2(const Point2& p1, const Point2& q,
40 OptionalJacobian<1, 2> H1 = boost::none,
41 OptionalJacobian<1, 2> H2 = boost::none);
42
43// For MATLAB wrapper
44typedef std::vector<Point2, Eigen::aligned_allocator<Point2> > Point2Vector;
45
47inline Point2 operator*(double s, const Point2& p) {
48 return Point2(s * p.x(), s * p.y());
49}
50
51/*
52 * @brief Circle-circle intersection, given normalized radii.
53 * Calculate f and h, respectively the parallel and perpendicular distance of
54 * the intersections of two circles along and from the line connecting the centers.
55 * Both are dimensionless fractions of the distance d between the circle centers.
56 * If the circles do not intersect or they are identical, returns boost::none.
57 * If one solution (touching circles, as determined by tol), h will be exactly zero.
58 * h is a good measure for how accurate the intersection will be, as when circles touch
59 * or nearly touch, the intersection is ill-defined with noisy radius measurements.
60 * @param R_d : R/d, ratio of radius of first circle to distance between centers
61 * @param r_d : r/d, ratio of radius of second circle to distance between centers
62 * @param tol: absolute tolerance below which we consider touching circles
63 * @return optional Point2 with f and h, boost::none if no solution.
64 */
65GTSAM_EXPORT boost::optional<Point2> circleCircleIntersection(double R_d, double r_d, double tol = 1e-9);
66
67/*
68 * @brief Circle-circle intersection, from the normalized radii solution.
69 * @param c1 center of first circle
70 * @param c2 center of second circle
71 * @return list of solutions (0,1, or 2). Identical circles will return empty list, as well.
72 */
73GTSAM_EXPORT std::list<Point2> circleCircleIntersection(Point2 c1, Point2 c2, boost::optional<Point2> fh);
74
76GTSAM_EXPORT Point2Pair means(const std::vector<Point2Pair> &abPointPairs);
77
87GTSAM_EXPORT std::list<Point2> circleCircleIntersection(Point2 c1, double r1,
88 Point2 c2, double r2, double tol = 1e-9);
89
90template <typename A1, typename A2>
91struct Range;
92
93template <>
95 typedef double result_type;
96 double operator()(const Point2& p, const Point2& q,
97 OptionalJacobian<1, 2> H1 = boost::none,
98 OptionalJacobian<1, 2> H2 = boost::none) {
99 return distance2(p, q, H1, H2);
100 }
101};
102
103} // \ namespace gtsam
104
Global functions in a separate testing namespace.
Definition chartTesting.h:28
Point2 operator*(double s, const Point2 &p)
multiply with scalar
Definition Point2.h:47
Vector2 Point2
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2...
Definition Point2.h:27
Point2Pair means(const std::vector< Point2Pair > &abPointPairs)
Calculate the two means of a set of Point2 pairs.
Definition Point2.cpp:116
double distance2(const Point2 &p, const Point2 &q, OptionalJacobian< 1, 2 > H1, OptionalJacobian< 1, 2 > H2)
distance between two points
Definition Point2.cpp:39
double norm2(const Point2 &p, OptionalJacobian< 1, 2 > H)
Distance of the point from the origin, with Jacobian.
Definition Point2.cpp:27
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
Definition BearingRange.h:40