gtsam  4.0.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 
25 #ifdef GTSAM_TYPEDEF_POINTS_TO_VECTORS
26 
29  typedef Vector2 Point2;
30 
31 #else
32 
40 class GTSAM_EXPORT Point2 : public Vector2 {
41 private:
42 
43 public:
44  enum { dimension = 2 };
47 
49 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
50  // Deprecated default constructor initializes to zero, in contrast to new behavior below
51  Point2() { setZero(); }
52 #else
53  Point2() {}
54 #endif
55 
56  using Vector2::Vector2;
57 
61 
63  explicit Point2(const Vector2& v):Vector2(v) {}
67 
69  void print(const std::string& s = "") const;
70 
72  bool equals(const Point2& q, double tol = 1e-9) const;
73 
77 
79  inline static Point2 identity() {return Point2(0,0);}
80 
84 
86  Point2 unit() const { return *this/norm(); }
87 
89  double norm(OptionalJacobian<1,2> H = boost::none) const;
90 
92  double distance(const Point2& p2, OptionalJacobian<1,2> H1 = boost::none,
93  OptionalJacobian<1,2> H2 = boost::none) const;
94 
98 
100  inline bool operator ==(const Point2& q) const {return x()==q.x() && y()==q.y();}
101 
103  inline double x() const {return (*this)[0];}
104 
106  inline double y() const {return (*this)[1];}
107 
109  const Vector2& vector() const { return *this; }
110 
112 
114  GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Point2& p);
115 
116 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
117  Point2 inverse() const { return -(*this); }
120  Point2 compose(const Point2& q) const { return (*this)+q;}
121  Point2 between(const Point2& q) const { return q-(*this);}
122  Vector2 localCoordinates(const Point2& q) const { return between(q);}
123  Point2 retract(const Vector2& v) const { return compose(Point2(v));}
124  static Vector2 Logmap(const Point2& p) { return p;}
125  static Point2 Expmap(const Vector2& v) { return Point2(v);}
126  inline double dist(const Point2& p2) const {return distance(p2);}
127  static boost::optional<Point2> CircleCircleIntersection(double R_d, double r_d, double tol = 1e-9);
128  static std::list<Point2> CircleCircleIntersection(Point2 c1, Point2 c2, boost::optional<Point2> fh);
129  static std::list<Point2> CircleCircleIntersection(Point2 c1, double r1, Point2 c2, double r2, double tol = 1e-9);
131 #endif
132 
133 private:
134 
137 
139  friend class boost::serialization::access;
140  template<class ARCHIVE>
141  void serialize(ARCHIVE & ar, const unsigned int /*version*/)
142  {
143  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Vector2);}
144 
146 };
147 
148 template<>
149 struct traits<Point2> : public internal::VectorSpace<Point2> {
150 };
151 
152 #endif // GTSAM_TYPEDEF_POINTS_TO_VECTORS
153 
155 GTSAM_EXPORT double norm2(const Point2& p, OptionalJacobian<1, 2> H = boost::none);
156 
158 GTSAM_EXPORT double distance2(const Point2& p1, const Point2& q,
159  OptionalJacobian<1, 2> H1 = boost::none,
160  OptionalJacobian<1, 2> H2 = boost::none);
161 
162 // Convenience typedef
163 typedef std::pair<Point2, Point2> Point2Pair;
164 GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const gtsam::Point2Pair &p);
165 
166 // For MATLAB wrapper
167 typedef std::vector<Point2, Eigen::aligned_allocator<Point2> > Point2Vector;
168 
170 inline Point2 operator*(double s, const Point2& p) {
171  return p * s;
172 }
173 
174 /*
175  * @brief Circle-circle intersection, given normalized radii.
176  * Calculate f and h, respectively the parallel and perpendicular distance of
177  * the intersections of two circles along and from the line connecting the centers.
178  * Both are dimensionless fractions of the distance d between the circle centers.
179  * If the circles do not intersect or they are identical, returns boost::none.
180  * If one solution (touching circles, as determined by tol), h will be exactly zero.
181  * h is a good measure for how accurate the intersection will be, as when circles touch
182  * or nearly touch, the intersection is ill-defined with noisy radius measurements.
183  * @param R_d : R/d, ratio of radius of first circle to distance between centers
184  * @param r_d : r/d, ratio of radius of second circle to distance between centers
185  * @param tol: absolute tolerance below which we consider touching circles
186  * @return optional Point2 with f and h, boost::none if no solution.
187  */
188 GTSAM_EXPORT boost::optional<Point2> circleCircleIntersection(double R_d, double r_d, double tol = 1e-9);
189 
190 /*
191  * @brief Circle-circle intersection, from the normalized radii solution.
192  * @param c1 center of first circle
193  * @param c2 center of second circle
194  * @return list of solutions (0,1, or 2). Identical circles will return empty list, as well.
195  */
196 GTSAM_EXPORT std::list<Point2> circleCircleIntersection(Point2 c1, Point2 c2, boost::optional<Point2> fh);
197 
207 GTSAM_EXPORT std::list<Point2> circleCircleIntersection(Point2 c1, double r1,
208  Point2 c2, double r2, double tol = 1e-9);
209 
210 } // \ namespace gtsam
211 
const Vector2 & vector() const
return vectorized form (column-wise).
Definition: Point2.h:109
double norm2(const Point2 &p, OptionalJacobian< 1, 2 > H)
Distance of the point from the origin, with Jacobian.
Definition: Point2.cpp:27
static Point2 identity()
identity
Definition: Point2.h:79
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:141
Point2 unit() const
creates a unit vector
Definition: Point2.h:86
Point2(const Vector2 &v)
construct from 2D vector
Definition: Point2.h:63
Template to create a binary predicate.
Definition: Testable.h:110
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition: OptionalJacobian.h:39
Point2()
default constructor
Definition: Point2.h:53
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
double y() const
get y
Definition: Point2.h:106
Definition: Point2.h:40
double distance2(const Point2 &p, const Point2 &q, OptionalJacobian< 1, 2 > H1, OptionalJacobian< 1, 2 > H2)
distance between two points
Definition: Point2.cpp:39
VectorSpace provides both Testable and VectorSpaceTraits.
Definition: VectorSpace.h:207
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
T between(const T &t1, const T &t2)
binary functions
Definition: lieProxies.h:36
bool operator==(const Matrix &A, const Matrix &B)
equality is just equal_with_abs_tol 1e-9
Definition: Matrix.h:102
std::pair< Point2, Point2 > Point2Pair
Calculate pose between a vector of 2D point correspondences (p,q) where q = Pose2::transformFrom(p) =...
Definition: Point2.h:163
Point2 operator *(double s, const Point2 &p)
multiply with scalar
Definition: Point2.h:170
double x() const
get x
Definition: Point2.h:103