CalibratedCamera.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/Pose2.h>
#include <gtsam/geometry/Pose3.h>

namespace gtsam {

class GTSAM_EXPORT CheiralityException: public ThreadsafeException<
    CheiralityException> {
public:
  CheiralityException() :
      ThreadsafeException<CheiralityException>("Cheirality Exception") {
  }
};

class GTSAM_EXPORT CalibratedCamera: public DerivedValue<CalibratedCamera> {
private:
  Pose3 pose_; // 6DOF pose

public:


  CalibratedCamera() {
  }

  explicit CalibratedCamera(const Pose3& pose);


  explicit CalibratedCamera(const Vector &v);


  virtual void print(const std::string& s = "") const {
    pose_.print(s);
  }

  bool equals(const CalibratedCamera &camera, double tol = 1e-9) const {
    return pose_.equals(camera.pose(), tol);
  }


  virtual ~CalibratedCamera() {
  }

  inline const Pose3& pose() const {
    return pose_;
  }

  inline const CalibratedCamera compose(const CalibratedCamera &c,
      boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
          boost::none) const {
    return CalibratedCamera(pose_.compose(c.pose(), H1, H2));
  }

  inline const CalibratedCamera between(const CalibratedCamera& c,
      boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
          boost::none) const {
    return CalibratedCamera(pose_.between(c.pose(), H1, H2));
  }

  inline const CalibratedCamera inverse(boost::optional<Matrix&> H1 =
      boost::none) const {
    return CalibratedCamera(pose_.inverse(H1));
  }

  static CalibratedCamera Level(const Pose2& pose2, double height);


  CalibratedCamera retract(const Vector& d) const;

  Vector localCoordinates(const CalibratedCamera& T2) const;

  inline size_t dim() const {
    return 6;
  }

  inline static size_t Dim() {
    return 6;
  }

  /* ************************************************************************* */
  // measurement functions and derivatives
  /* ************************************************************************* */


  Point2 project(const Point3& point,
      boost::optional<Matrix&> Dpose = boost::none,
      boost::optional<Matrix&> Dpoint = boost::none) const;

  static Point2 project_to_camera(const Point3& cameraPoint,
      boost::optional<Matrix&> H1 = boost::none);

  static Point3 backproject_from_camera(const Point2& p, const double scale);

  double range(const Point3& point, boost::optional<Matrix&> H1 = boost::none,
      boost::optional<Matrix&> H2 = boost::none) const {
    return pose_.range(point, H1, H2);
  }

  double range(const Pose3& pose, boost::optional<Matrix&> H1 = boost::none,
      boost::optional<Matrix&> H2 = boost::none) const {
    return pose_.range(pose, H1, H2);
  }

  double range(const CalibratedCamera& camera, boost::optional<Matrix&> H1 =
      boost::none, boost::optional<Matrix&> H2 = boost::none) const {
    return pose_.range(camera.pose_, H1, H2);
  }

private:


  friend class boost::serialization::access;
  template<class Archive>
  void serialize(Archive & ar, const unsigned int version) {
    ar
        & boost::serialization::make_nvp("CalibratedCamera",
            boost::serialization::base_object<Value>(*this));
    ar & BOOST_SERIALIZATION_NVP(pose_);
  }

      };}