Point3.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

 * -------------------------------------------------------------------------- */

// \callgraph

#pragma once

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

#include <boost/serialization/nvp.hpp>

#include <cmath>

namespace gtsam {

  class GTSAM_EXPORT Point3 : public DerivedValue<Point3> {
  public:
    static const size_t dimension = 3;

  private:
    double x_, y_, z_;  
    
  public:


    Point3(): x_(0), y_(0), z_(0) {}

    Point3(double x, double y, double z): x_(x), y_(y), z_(z) {}


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


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

    bool equals(const Point3& p, double tol = 1e-9) const;


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

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

    Point3 operator - () const { return Point3(-x_,-y_,-z_);}

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

    Point3 operator + (const Point3& q) const;

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

    Point3 operator - (const Point3& q) const;


    inline static size_t Dim() { return dimension; }

    inline size_t dim() const { return dimension; }

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

    inline Vector3 localCoordinates(const Point3& q) const { return (q -*this).vector(); }


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

    static inline Vector3 Logmap(const Point3& dp) { return Vector3(dp.x(), dp.y(), dp.z()); }

    static Matrix dexpL(const Vector& v) {
      return eye(3);
    }

    static Matrix dexpInvL(const Vector& v) {
      return eye(3);
    }


    Point3 operator * (double s) const;

    Point3 operator / (double s) const;

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

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

    double norm() const;

    Point3 normalize(boost::optional<Matrix&> H = boost::none) const;

    Point3 cross(const Point3 &q) const;

    double dot(const Point3 &q) const;


    bool   operator ==(const Point3& q) const;

    Vector3 vector() const { return Vector3(x_,y_,z_); }

    inline double x() const {return x_;}

    inline double y() const {return y_;}

    inline double z() const {return z_;}

    Point3 add (const Point3 &q,
          boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;

    Point3 sub (const Point3 &q,
          boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;


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

  private:


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


  };

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

}