SimpleCamera.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/BearingRange.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Cal3_S2.h>

namespace gtsam {

typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2;

class GTSAM_EXPORT SimpleCamera : public PinholeCameraCal3_S2 {

  typedef PinholeCamera<Cal3_S2> Base;
  typedef boost::shared_ptr<SimpleCamera> shared_ptr;

public:


  SimpleCamera() :
      Base() {
  }

  explicit SimpleCamera(const Pose3& pose) :
      Base(pose) {
  }

  SimpleCamera(const Pose3& pose, const Cal3_S2& K) :
      Base(pose, K) {
  }


   static SimpleCamera Level(const Cal3_S2 &K, const Pose2& pose2,
       double height) {
     return SimpleCamera(Base::LevelPose(pose2, height), K);
   }

   static SimpleCamera Level(const Pose2& pose2, double height) {
    return SimpleCamera::Level(Cal3_S2(), pose2, height);
   }

   static SimpleCamera Lookat(const Point3& eye, const Point3& target,
       const Point3& upVector, const Cal3_S2& K = Cal3_S2()) {
    return SimpleCamera(Base::LookatPose(eye, target, upVector), K);
   }


   explicit SimpleCamera(const Vector &v) :
       Base(v) {
   }

   SimpleCamera(const Vector &v, const Vector &K) :
       Base(v, K) {
   }

   SimpleCamera::shared_ptr clone() const { return boost::make_shared<SimpleCamera>(*this); }



   SimpleCamera retract(const Vector& d) const {
     if ((size_t) d.size() == 6)
       return SimpleCamera(this->pose().retract(d), calibration());
     else
       return SimpleCamera(this->pose().retract(d.head(6)),
           calibration().retract(d.tail(calibration().dim())));
   }


};

GTSAM_EXPORT SimpleCamera simpleCamera(const Matrix34& P);

// manifold traits
template <>
struct traits<SimpleCamera> : public internal::Manifold<SimpleCamera> {};

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

// range traits, used in RangeFactor
template <typename T>
struct Range<SimpleCamera, T> : HasRange<SimpleCamera, T, double> {};

}  // namespace gtsam