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

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

#pragma once

#include <gtsam/base/DerivedValue.h>
#include <gtsam/geometry/Point2.h>

namespace gtsam {

  class GTSAM_EXPORT StereoPoint2 : public DerivedValue<StereoPoint2> {
  public:
    static const size_t dimension = 3;
  private:
    double uL_, uR_, v_;

  public:


    StereoPoint2() :
      uL_(0), uR_(0), v_(0) {
    }

    StereoPoint2(double uL, double uR, double v) :
      uL_(uL), uR_(uR), v_(v) {
    }


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

    bool equals(const StereoPoint2& q, double tol=1e-9) const {
      return (fabs(uL_ - q.uL_) < tol && fabs(uR_ - q.uR_) < tol && fabs(v_
          - q.v_) < tol);
    }


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

    inline StereoPoint2 inverse() const {
      return StereoPoint2()- (*this);
    }

    inline StereoPoint2 compose(const StereoPoint2& p1) const {
      return *this + p1;
    }

    StereoPoint2 operator+(const StereoPoint2& b) const {
      return StereoPoint2(uL_ + b.uL_, uR_ + b.uR_, v_ + b.v_);
    }

    StereoPoint2 operator-(const StereoPoint2& b) const {
      return StereoPoint2(uL_ - b.uL_, uR_ - b.uR_, v_ - b.v_);
    }


    inline static size_t Dim() { return dimension; }

    inline size_t dim() const { return dimension; }

    inline StereoPoint2 retract(const Vector& v) const { return compose(Expmap(v)); }

    inline Vector localCoordinates(const StereoPoint2& t2) const { return Logmap(between(t2)); }


    static inline StereoPoint2 Expmap(const Vector& d) {
      return StereoPoint2(d(0), d(1), d(2));
    }

    static inline Vector Logmap(const StereoPoint2& p) {
      return p.vector();
    }

    inline StereoPoint2 between(const StereoPoint2& p2) const {
      return gtsam::between_default(*this, p2);
    }


    inline double uL() const {return uL_;}

    inline double uR() const {return uR_;}

    inline double v() const {return v_;}

    Vector3 vector() const {
      return Vector3(uL_, uR_, v_);
    }

    inline Point2 point2(){
      return Point2(uL_, v_);
    }

    inline Point2 right(){
      return Point2(uR_, v_);
    }

  private:


    friend class boost::serialization::access;
    template<class ARCHIVE>
    void serialize(ARCHIVE & ar, const unsigned int version) {
      ar & boost::serialization::make_nvp("StereoPoint2",
          boost::serialization::base_object<Value>(*this));
      ar & BOOST_SERIALIZATION_NVP(uL_);
      ar & BOOST_SERIALIZATION_NVP(uR_);
      ar & BOOST_SERIALIZATION_NVP(v_);
    }


  };

}