Unit3.h

/* ----------------------------------------------------------------------------
 
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
 
 * See LICENSE for the license information
 
 * -------------------------------------------------------------------------- */
 
/*
 * @file Unit3.h
 * @date Feb 02, 2011
 * @author Can Erdogan
 * @author Frank Dellaert
 * @author Alex Trevor
 * @brief Develop a Unit3 class - basically a point on a unit sphere
 */
 
#pragma once
 
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/base/Manifold.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/dllexport.h>
 
#include <boost/optional.hpp>
#include <boost/serialization/nvp.hpp>
 
#include <random>
#include <string>
 
#ifdef GTSAM_USE_TBB
#include <mutex> // std::mutex
#endif
 
namespace gtsam {
 
class Unit3 {
 
private:
 
  Vector3 p_; 
  mutable boost::optional<Matrix32> B_; 
  mutable boost::optional<Matrix62> H_B_; 
 
#ifdef GTSAM_USE_TBB
  mutable std::mutex B_mutex_; 
#endif
 
public:
 
  enum {
    dimension = 2
  };
 
 
  Unit3() :
      p_(1.0, 0.0, 0.0) {
  }
 
  explicit Unit3(const Vector3& p) :
      p_(p.normalized()) {
  }
 
  Unit3(double x, double y, double z) :
      p_(x, y, z) {
    p_.normalize();
  }
 
  explicit Unit3(const Point2& p, double f) : p_(p.x(), p.y(), f) {
    p_.normalize();
  }
 
  Unit3(const Unit3& u) {
    p_ = u.p_;
  }
 
  Unit3& operator=(const Unit3 & u) {
    p_ = u.p_;
    B_ = u.B_;
    H_B_ = u.H_B_;
    return *this;
  }
 
  GTSAM_EXPORT static Unit3 FromPoint3(const Point3& point, //
      OptionalJacobian<2, 3> H = boost::none);
 
  GTSAM_EXPORT static Unit3 Random(std::mt19937 & rng);
 
 
 
  friend std::ostream& operator<<(std::ostream& os, const Unit3& pair);
 
  GTSAM_EXPORT void print(const std::string& s = std::string()) const;
 
  bool equals(const Unit3& s, double tol = 1e-9) const {
    return equal_with_abs_tol(p_, s.p_, tol);
  }
 
 
  GTSAM_EXPORT const Matrix32& basis(OptionalJacobian<6, 2> H = boost::none) const;
 
  GTSAM_EXPORT Matrix3 skew() const;
 
  GTSAM_EXPORT Point3 point3(OptionalJacobian<3, 2> H = boost::none) const;
 
  GTSAM_EXPORT Vector3 unitVector(OptionalJacobian<3, 2> H = boost::none) const;
 
  friend Point3 operator*(double s, const Unit3& d) {
    return Point3(s * d.p_);
  }
 
  GTSAM_EXPORT double dot(const Unit3& q, OptionalJacobian<1,2> H1 = boost::none, //
                             OptionalJacobian<1,2> H2 = boost::none) const;
 
  GTSAM_EXPORT Vector2 error(const Unit3& q, OptionalJacobian<2, 2> H_q = boost::none) const;
 
  GTSAM_EXPORT Vector2 errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p = boost::none, //
                      OptionalJacobian<2, 2> H_q = boost::none) const;
 
  GTSAM_EXPORT double distance(const Unit3& q, OptionalJacobian<1, 2> H = boost::none) const;
 
  Unit3 cross(const Unit3& q) const {
    return Unit3(p_.cross(q.p_));
  }
 
  Point3 cross(const Point3& q) const {
    return point3().cross(q);
  }
 
 
 
  inline static size_t Dim() {
    return 2;
  }
 
  inline size_t dim() const {
    return 2;
  }
 
  enum CoordinatesMode {
    EXPMAP, 
    RENORM 
  };
 
  GTSAM_EXPORT Unit3 retract(const Vector2& v, OptionalJacobian<2,2> H = boost::none) const;
 
  GTSAM_EXPORT Vector2 localCoordinates(const Unit3& s) const;
 
 
private:
 
 
  friend class boost::serialization::access;
  template<class ARCHIVE>
  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
    ar & BOOST_SERIALIZATION_NVP(p_);
  }
 
 
public:
  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
 
// Define GTSAM traits
template<> struct traits<Unit3> : public internal::Manifold<Unit3> {
};
 
template<> struct traits<const Unit3> : public internal::Manifold<Unit3> {
};
 
} // namespace gtsam