gtsam/gtsam/geometry/Point3.h

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

 * 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

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

/**
 * @file   Point3.h
 * @brief  3D Point
 * @author Alireza Fathi
 * @author Christian Potthast
 * @author Frank Dellaert
 */

// \callgraph

#pragma once

#include <gtsam/base/VectorSpace.h>
#include <gtsam/base/Vector.h>
#include <gtsam/dllexport.h>
#include <boost/serialization/nvp.hpp>

namespace gtsam {

#define GTSAM_USE_VECTOR3_POINTS
#ifdef GTSAM_USE_VECTOR3_POINTS

  /// As of GTSAM 4, in order to make GTSAM more lean,
  /// it is now possible to just typedef Point3 to Vector3
  typedef Vector3 Point3;

#else

/**
   * A 3D point is just a Vector3 with some additional methods
   * @addtogroup geometry
   * \nosubgrouping
   */
class GTSAM_EXPORT Point3 : public Vector3 {

  public:

    enum { dimension = 3 };

    /// @name Standard Constructors
    /// @{

#ifndef GTSAM_ALLOW_DEPRECATED_SINCE_V4
    /// Default constructor no longer initializes, just like Vector3
    Point3() {}
#endif

    /// Construct from x, y, and z coordinates
    Point3(double x, double y, double z): Vector3(x,y, z) {}

    /// Construct from other vector
    template<typename Derived>
    inline Point3(const Eigen::MatrixBase<Derived>& v): Vector3(v) {}

    /// @}
    /// @name Testable
    /// @{

    /** print with optional string */
    void print(const std::string& s = "") const;

    /** equals with an tolerance */
    bool equals(const Point3& p, double tol = 1e-9) const;

    /// @}
    /// @name Group
    /// @{

    /// identity for group operation
    inline static Point3 identity() { return Point3(0.0, 0.0, 0.0); }

    /// @}
    /// @name Vector Space
    /// @{

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

    /** Distance of the point from the origin, with Jacobian */
    double norm(OptionalJacobian<1,3> H = boost::none) const;

    /** normalize, with optional Jacobian */
    Point3 normalized(OptionalJacobian<3, 3> H = boost::none) const;

    /** cross product @return this x q */
    Point3 cross(const Point3 &q, OptionalJacobian<3, 3> H_p = boost::none, //
                                  OptionalJacobian<3, 3> H_q = boost::none) const;

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

    /// @}
    /// @name Standard Interface
    /// @{

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

    /// @}

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

#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
    /// @name Deprecated
    /// @{
    Point3() { setZero(); } // initializes to zero, in contrast to new behavior
    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).vector();}
    Point3 retract(const Vector3& v) const { return compose(Point3(v));}
    static Vector3 Logmap(const Point3& p) { return p.vector();}
    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:

    /** Serialization function */
    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

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

/// distance between two points
double distance(const Point3& p1, const Point3& q,
                OptionalJacobian<1, 3> H1 = boost::none,
                OptionalJacobian<1, 3> H2 = boost::none);

/// Distance of the point from the origin, with Jacobian
double norm(const Point3& p, OptionalJacobian<1, 3> H = boost::none);

/// normalize, with optional Jacobian
Point3 normalize(const Point3& p, OptionalJacobian<3, 3> H = boost::none);

/// cross product @return this x q
Point3 cross(const Point3& p, const Point3& q,
             OptionalJacobian<3, 3> H_p = boost::none,
             OptionalJacobian<3, 3> H_q = boost::none);

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

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

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 distance(p, q, H1, H2);
  }
};

}  // namespace gtsam