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/config.h>
#include <gtsam/base/VectorSpace.h>
#include <gtsam/base/Vector.h>
#include <gtsam/dllexport.h>
#include <boost/serialization/nvp.hpp>

namespace gtsam {

#ifdef GTSAM_TYPEDEF_POINTS_TO_VECTORS

  typedef Vector3 Point3;

#else

class GTSAM_EXPORT Point3 : public Vector3 {

  public:

    enum { dimension = 3 };


    // Deprecated default constructor initializes to zero, in contrast to new behavior below
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
    Point3() { setZero(); }
#endif

    using Vector3::Vector3;


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

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


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


    double distance(const Point3& p2, OptionalJacobian<1, 3> H1 = boost::none,
                    OptionalJacobian<1, 3> H2 = boost::none) const;

    double norm(OptionalJacobian<1,3> H = boost::none) const;

    Point3 normalized(OptionalJacobian<3, 3> H = boost::none) const;

    Point3 cross(const Point3 &q, OptionalJacobian<3, 3> H_p = boost::none, //
                                  OptionalJacobian<3, 3> H_q = boost::none) const;

    double dot(const Point3 &q, OptionalJacobian<1, 3> H_p = boost::none, //
                                OptionalJacobian<1, 3> H_q = boost::none) const;


    const Vector3& vector() const { return *this; }

    inline double x() const {return (*this)[0];}

    inline double y() const {return (*this)[1];}

    inline double z() const {return (*this)[2];}


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

#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
    Point3 inverse() const { return -(*this);}
    Point3 compose(const Point3& q) const { return (*this)+q;}
    Point3 between(const Point3& q) const { return q-(*this);}
    Vector3 localCoordinates(const Point3& q) const { return between(q);}
    Point3 retract(const Vector3& v) const { return compose(Point3(v));}
    static Vector3 Logmap(const Point3& p) { return p;}
    static Point3 Expmap(const Vector3& v) { return Point3(v);}
    inline double dist(const Point3& q) const { return (q - *this).norm(); }
    Point3 normalize(OptionalJacobian<3, 3> H = boost::none) const { return normalized(H);}
    Point3 add(const Point3& q, OptionalJacobian<3, 3> H1 = boost::none,
               OptionalJacobian<3, 3> H2 = boost::none) const;
    Point3 sub(const Point3& q, OptionalJacobian<3, 3> H1 = boost::none,
               OptionalJacobian<3, 3> H2 = boost::none) const;
#endif

  private:

    friend class boost::serialization::access;
    template<class ARCHIVE>
    void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
      ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Vector3);
    }
  };

template<>
struct traits<Point3> : public internal::VectorSpace<Point3> {};

template<>
struct traits<const Point3> : public internal::VectorSpace<Point3> {};

#endif // GTSAM_TYPEDEF_POINTS_TO_VECTORS

// Convenience typedef
typedef std::pair<Point3, Point3> Point3Pair;
GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const gtsam::Point3Pair &p);

GTSAM_EXPORT double distance3(const Point3& p1, const Point3& q,
                                  OptionalJacobian<1, 3> H1 = boost::none,
                              OptionalJacobian<1, 3> H2 = boost::none);

GTSAM_EXPORT double norm3(const Point3& p, OptionalJacobian<1, 3> H = boost::none);

GTSAM_EXPORT Point3 normalize(const Point3& p, OptionalJacobian<3, 3> H = boost::none);

GTSAM_EXPORT Point3 cross(const Point3& p, const Point3& q,
                          OptionalJacobian<3, 3> H_p = boost::none,
                          OptionalJacobian<3, 3> H_q = boost::none);

GTSAM_EXPORT double dot(const Point3& p, const Point3& q,
                        OptionalJacobian<1, 3> H_p = boost::none,
                        OptionalJacobian<1, 3> H_q = boost::none);

template <typename A1, typename A2>
struct Range;

template <>
struct Range<Point3, Point3> {
  typedef double result_type;
  double operator()(const Point3& p, const Point3& q,
                    OptionalJacobian<1, 3> H1 = boost::none,
                    OptionalJacobian<1, 3> H2 = boost::none) {
    return distance3(p, q, H1, H2);
  }
};

}  // namespace gtsam