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:

    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)){}


    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_; }


  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_);
    }

  };
} // \ namespace gtsam