Cal3_S2Stereo.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/geometry/Cal3_S2.h>

namespace gtsam {

  class Cal3_S2Stereo {
  private:

    Cal3_S2 K_;
    double b_;

  public:

    enum { dimension = 6 };
    typedef boost::shared_ptr<Cal3_S2Stereo> shared_ptr;  


    Cal3_S2Stereo() :
      K_(1, 1, 0, 0, 0), b_(1.0) {
    }

    Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b) :
      K_(fx, fy, s, u0, v0), b_(b) {
    }

    Cal3_S2Stereo(const Vector &d): K_(d(0), d(1), d(2), d(3), d(4)), b_(d(5)){}

    Cal3_S2Stereo(double fov, int w, int h, double b) :
      K_(fov, w, h), b_(b) {
    }


    void print(const std::string& s = "") const {
      K_.print(s+"K: ");
      std::cout << s << "Baseline: " << b_ << std::endl;
    }

    bool equals(const Cal3_S2Stereo& other, double tol = 10e-9) const {
      if (fabs(b_ - other.b_) > tol) return false;
      return K_.equals(other.K_,tol);
    }


    const Cal3_S2& calibration() const { return K_;}

    Matrix matrix() const { return K_.matrix();}

    inline double fx() const { return K_.fx();}

    inline double fy() const { return K_.fy();}

    inline double skew() const { return K_.skew();}

    inline double px() const { return K_.px();}

    inline double py() const { return K_.py();}

    Point2 principalPoint() const { return K_.principalPoint();}

    inline double baseline() const { return b_; }

    Vector6 vector() const {
      Vector6 v;
      v << K_.vector(), b_;
      return v;
    }


    inline size_t dim() const {
      return 6;
    }

    static size_t Dim() {
      return 6;
    }

    inline Cal3_S2Stereo retract(const Vector& d) const {
      return Cal3_S2Stereo(K_.fx() + d(0), K_.fy() + d(1), K_.skew() + d(2), K_.px() + d(3), K_.py() + d(4), b_ + d(5));
    }

    Vector6 localCoordinates(const Cal3_S2Stereo& T2) const {
      return T2.vector() - vector();
    }



  private:
    friend class boost::serialization::access;
    template<class Archive>
    void serialize(Archive & ar, const unsigned int /*version*/)
    {
      ar & BOOST_SERIALIZATION_NVP(K_);
      ar & BOOST_SERIALIZATION_NVP(b_);
    }

  };

  // Define GTSAM traits
  template<>
  struct traits<Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {
  };

  template<>
  struct traits<const Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {
  };

} // \ namespace gtsam