Cal3.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/Point2.h>
 
namespace gtsam {
 
template <typename Cal, size_t Dim>
void calibrateJacobians(const Cal& calibration, const Point2& pn,
                        OptionalJacobian<2, Dim> Dcal = boost::none,
                        OptionalJacobian<2, 2> Dp = boost::none) {
  if (Dcal || Dp) {
    Eigen::Matrix<double, 2, Dim> H_uncal_K;
    Matrix22 H_uncal_pn, H_uncal_pn_inv;
 
    // Compute uncalibrate Jacobians
    calibration.uncalibrate(pn, Dcal ? &H_uncal_K : nullptr, H_uncal_pn);
 
    H_uncal_pn_inv = H_uncal_pn.inverse();
 
    if (Dp) *Dp = H_uncal_pn_inv;
    if (Dcal) *Dcal = -H_uncal_pn_inv * H_uncal_K;
  }
}
 
class GTSAM_EXPORT Cal3 {
 protected:
  double fx_ = 1.0f, fy_ = 1.0f;  
  double s_ = 0.0f;               
  double u0_ = 0.0f, v0_ = 0.0f;  
 
 public:
  enum { dimension = 5 };
  using shared_ptr = boost::shared_ptr<Cal3>;
 
 
  Cal3() = default;
 
  Cal3(double fx, double fy, double s, double u0, double v0)
      : fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0) {}
 
  Cal3(const Vector5& d)
      : fx_(d(0)), fy_(d(1)), s_(d(2)), u0_(d(3)), v0_(d(4)) {}
 
  Cal3(double fov, int w, int h);
 
  virtual ~Cal3() {}
 
 
  Cal3(const std::string& path);
 
 
  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
                                               const Cal3& cal);
 
  virtual void print(const std::string& s = "") const;
 
  bool equals(const Cal3& K, double tol = 10e-9) const;
 
 
  inline double fx() const { return fx_; }
 
  inline double fy() const { return fy_; }
 
  inline double aspectRatio() const { return fx_ / fy_; }
 
  inline double skew() const { return s_; }
 
  inline double px() const { return u0_; }
 
  inline double py() const { return v0_; }
 
  Point2 principalPoint() const { return Point2(u0_, v0_); }
 
  Vector5 vector() const {
    Vector5 v;
    v << fx_, fy_, s_, u0_, v0_;
    return v;
  }
 
  virtual Matrix3 K() const {
    Matrix3 K;
    K << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0;
    return K;
  }
 
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
  Matrix3 matrix() const { return K(); }
#endif
 
  Matrix3 inverse() const;
 
  inline virtual size_t dim() const { return Dim(); }
 
  inline static size_t Dim() { return dimension; }
 
 
 private:
  friend class boost::serialization::access;
  template <class Archive>
  void serialize(Archive& ar, const unsigned int /*version*/) {
    ar& BOOST_SERIALIZATION_NVP(fx_);
    ar& BOOST_SERIALIZATION_NVP(fy_);
    ar& BOOST_SERIALIZATION_NVP(s_);
    ar& BOOST_SERIALIZATION_NVP(u0_);
    ar& BOOST_SERIALIZATION_NVP(v0_);
  }
 
};
 
}  // \ namespace gtsam