StereoCamera.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_S2Stereo.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/StereoPoint2.h>

namespace gtsam {

class GTSAM_EXPORT StereoCheiralityException: public std::runtime_error {
public:
  StereoCheiralityException()
    : StereoCheiralityException(std::numeric_limits<Key>::max()) {}

  StereoCheiralityException(Key j)
    : std::runtime_error("Stereo Cheirality Exception"),
      j_(j) {}

  Key nearbyVariable() const {
    return j_;
  }

private:
  Key j_;
};

class GTSAM_EXPORT StereoCamera {

public:

  typedef StereoPoint2 Measurement;
  typedef StereoPoint2Vector MeasurementVector;

private:
  Pose3 leftCamPose_;
  Cal3_S2Stereo::shared_ptr K_;

public:

  enum {
    dimension = 6
  };


  StereoCamera() :
      K_(new Cal3_S2Stereo()) {
  }

  StereoCamera(const Pose3& leftCamPose, const Cal3_S2Stereo::shared_ptr K);

  const Cal3_S2Stereo& calibration() const {
    return *K_;
  }


  void print(const std::string& s = "") const {
    leftCamPose_.print(s + ".camera.");
    K_->print(s + ".calibration.");
  }

  bool equals(const StereoCamera &camera, double tol = 1e-9) const {
    return leftCamPose_.equals(camera.leftCamPose_, tol)
        && K_->equals(*camera.K_, tol);
  }


  inline size_t dim() const {
    return 6;
  }

  static inline size_t Dim() {
    return 6;
  }

  inline StereoCamera retract(const Vector& v) const {
    return StereoCamera(pose().retract(v), K_);
  }

  inline Vector6 localCoordinates(const StereoCamera& t2) const {
    return leftCamPose_.localCoordinates(t2.leftCamPose_);
  }


  const Pose3& pose() const {
    return leftCamPose_;
  }

  double baseline() const {
    return K_->baseline();
  }

  StereoPoint2 project(const Point3& point) const;

  StereoPoint2 project2(const Point3& point, OptionalJacobian<3, 6> H1 =
      {}, OptionalJacobian<3, 3> H2 = {}) const;

  Point3 backproject(const StereoPoint2& z) const;

  Point3 backproject2(const StereoPoint2& z,
                      OptionalJacobian<3, 6> H1 = {},
                      OptionalJacobian<3, 3> H2 = {}) const;


  StereoPoint2 project(const Point3& point, OptionalJacobian<3, 6> H1,
      OptionalJacobian<3, 3> H2 = {}, OptionalJacobian<3, 0> H3 =
          {}) const;

  Vector defaultErrorWhenTriangulatingBehindCamera() const {
    return Eigen::Matrix<double,traits<Measurement>::dimension,1>::Constant(2.0 * K_->fx());
  }


private:

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

};

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

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