gtsam  4.1.0
gtsam
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 
23 namespace gtsam {
24 
27 typedef Vector2 Point2;
28 
30 GTSAM_EXPORT double norm2(const Point2& p, OptionalJacobian<1, 2> H = boost::none);
31 
33 GTSAM_EXPORT double distance2(const Point2& p1, const Point2& q,
34  OptionalJacobian<1, 2> H1 = boost::none,
35  OptionalJacobian<1, 2> H2 = boost::none);
36 
37 // Convenience typedef
38 typedef std::pair<Point2, Point2> Point2Pair;
39 GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const gtsam::Point2Pair &p);
40 
41 // For MATLAB wrapper
42 typedef std::vector<Point2, Eigen::aligned_allocator<Point2> > Point2Vector;
43 
45 inline Point2 operator*(double s, const Point2& p) {
46  return p * s;
47 }
48 
49 /*
50  * @brief Circle-circle intersection, given normalized radii.
51  * Calculate f and h, respectively the parallel and perpendicular distance of
52  * the intersections of two circles along and from the line connecting the centers.
53  * Both are dimensionless fractions of the distance d between the circle centers.
54  * If the circles do not intersect or they are identical, returns boost::none.
55  * If one solution (touching circles, as determined by tol), h will be exactly zero.
56  * h is a good measure for how accurate the intersection will be, as when circles touch
57  * or nearly touch, the intersection is ill-defined with noisy radius measurements.
58  * @param R_d : R/d, ratio of radius of first circle to distance between centers
59  * @param r_d : r/d, ratio of radius of second circle to distance between centers
60  * @param tol: absolute tolerance below which we consider touching circles
61  * @return optional Point2 with f and h, boost::none if no solution.
62  */
63 GTSAM_EXPORT boost::optional<Point2> circleCircleIntersection(double R_d, double r_d, double tol = 1e-9);
64 
65 /*
66  * @brief Circle-circle intersection, from the normalized radii solution.
67  * @param c1 center of first circle
68  * @param c2 center of second circle
69  * @return list of solutions (0,1, or 2). Identical circles will return empty list, as well.
70  */
71 GTSAM_EXPORT std::list<Point2> circleCircleIntersection(Point2 c1, Point2 c2, boost::optional<Point2> fh);
72 
82 GTSAM_EXPORT std::list<Point2> circleCircleIntersection(Point2 c1, double r1,
83  Point2 c2, double r2, double tol = 1e-9);
84 
85 } // \ namespace gtsam
86 
gtsam::distance2
double distance2(const Point2 &p, const Point2 &q, OptionalJacobian< 1, 2 > H1, OptionalJacobian< 1, 2 > H2)
distance between two points
Definition: Point2.cpp:39
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition: OptionalJacobian.h:39
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::Point2Pair
std::pair< Point2, Point2 > Point2Pair
Calculate pose between a vector of 2D point correspondences (p,q) where q = Pose2::transformFrom(p) =...
Definition: Point2.h:38
gtsam::Point2
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
gtsam::operator*
Point2 operator*(double s, const Point2 &p)
multiply with scalar
Definition: Point2.h:45
gtsam::norm2
double norm2(const Point2 &p, OptionalJacobian< 1, 2 > H)
Distance of the point from the origin, with Jacobian.
Definition: Point2.cpp:27