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 <boost/serialization/nvp.hpp>

#include <gtsam/base/DerivedValue.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Lie.h>

namespace gtsam {

class GTSAM_EXPORT Point2 : public DerivedValue<Point2> {
public:
  static const size_t dimension = 2;
private:
  double x_, y_;

public:


  Point2(): x_(0), y_(0) {}

  Point2(double x, double y): x_(x), y_(y) {}


  Point2(const Vector& v) {
    if(v.size() != 2)
      throw std::invalid_argument("Point2 constructor from Vector requires that the Vector have dimension 2");
    x_ = v(0);
    y_ = v(1);
  }

  /*
   * @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 boost::none.
   * 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, boost::none if no solution.
   */
  static boost::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.
   */
  static std::list<Point2> CircleCircleIntersection(Point2 c1, Point2 c2, boost::optional<Point2>);

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


  void print(const std::string& s = "") const;

  bool equals(const Point2& q, double tol = 1e-9) const;


  inline static Point2 identity() {
    return Point2();
  }

  inline Point2 inverse() const { return Point2(-x_, -y_); }

  inline Point2 operator- () const {return Point2(-x_,-y_);}

  inline Point2 compose(const Point2& q,
      boost::optional<Matrix&> H1=boost::none,
      boost::optional<Matrix&> H2=boost::none) const {
    if(H1) *H1 = eye(2);
    if(H2) *H2 = eye(2);
    return *this + q;
  }

  inline Point2 operator + (const Point2& q) const {return Point2(x_+q.x_,y_+q.y_);}

  inline Point2 between(const Point2& q,
      boost::optional<Matrix&> H1=boost::none,
      boost::optional<Matrix&> H2=boost::none) const {
    if(H1) *H1 = -eye(2);
    if(H2) *H2 = eye(2);
    return q - (*this);
  }

  inline Point2 operator - (const Point2& q) const {return Point2(x_-q.x_,y_-q.y_);}


  inline static size_t Dim() { return dimension; }

  inline size_t dim() const { return dimension; }

  inline Point2 retract(const Vector& v) const { return *this + Point2(v); }

  inline Vector localCoordinates(const Point2& t2) const { return Logmap(between(t2)); }


  static inline Point2 Expmap(const Vector& v) { return Point2(v); }

  static inline Vector Logmap(const Point2& dp) { return (Vector(2) << dp.x(), dp.y()); }


  Point2 unit() const { return *this/norm(); }

  double norm(boost::optional<Matrix&> H = boost::none) const;

  double distance(const Point2& p2, boost::optional<Matrix&> H1 = boost::none,
      boost::optional<Matrix&> H2 = boost::none) const;

  inline double dist(const Point2& p2) const {
    return (p2 - *this).norm();
  }

  inline Point2 operator * (double s) const {return Point2(x_*s,y_*s);}

  inline Point2 operator / (double q) const {return Point2(x_/q,y_/q);}


  inline bool operator ==(const Point2& q) const {return x_==q.x_ && q.y_==q.y_;}

  double x() const {return x_;}

  double y() const {return y_;}

  Vector2 vector() const { return Vector2(x_, y_); }

  inline void operator += (const Point2& q) {x_+=q.x_;y_+=q.y_;}
  inline void operator *= (double s) {x_*=s;y_*=s;}

  GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Point2& p);

private:


  friend class boost::serialization::access;
  template<class ARCHIVE>
  void serialize(ARCHIVE & ar, const unsigned int version)
  {
    ar & boost::serialization::make_nvp("Point2",
        boost::serialization::base_object<Value>(*this));
    ar & BOOST_SERIALIZATION_NVP(x_);
    ar & BOOST_SERIALIZATION_NVP(y_);
  }


};

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

}