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/Point3.h>
#include <gtsam/base/DerivedValue.h>
#include <boost/random/mersenne_twister.hpp>
#include <boost/optional.hpp>

namespace gtsam {

class GTSAM_EXPORT Unit3: public DerivedValue<Unit3> {

private:

  typedef Eigen::Matrix<double,3,2> Matrix32;

  Point3 p_; 
  mutable boost::optional<Matrix32> B_; 

public:


  Unit3() :
      p_(1.0, 0.0, 0.0) {
  }

  explicit Unit3(const Point3& p) :
      p_(p / p.norm()) {
  }

  Unit3(double x, double y, double z) :
      p_(x, y, z) {
    p_ = p_ / p_.norm();
  }

  static Unit3 FromPoint3(const Point3& point, boost::optional<Matrix&> H =
      boost::none);

  static Unit3 Random(boost::mt19937 & rng);



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

  bool equals(const Unit3& s, double tol = 1e-9) const {
    return p_.equals(s.p_, tol);
  }


  const Matrix32& basis() const;

  Matrix skew() const;

  const Point3& point3(boost::optional<Matrix&> H = boost::none) const {
    if (H)
      *H = basis();
    return p_;
  }

  friend Point3 operator*(double s, const Unit3& d) {
    return s * d.p_;
  }

  Vector error(const Unit3& q,
      boost::optional<Matrix&> H = boost::none) const;

  double distance(const Unit3& q,
      boost::optional<Matrix&> H = boost::none) const;



  inline static size_t Dim() {
    return 2;
  }

  inline size_t dim() const {
    return 2;
  }

  enum CoordinatesMode {
    EXPMAP, 
    RENORM 
  };

  Unit3 retract(const Vector& v) const;

  Vector 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::make_nvp("Unit3",
          boost::serialization::base_object<Value>(*this));
      ar & BOOST_SERIALIZATION_NVP(p_);
      ar & BOOST_SERIALIZATION_NVP(B_);
    }


};

} // namespace gtsam