ProjectionFactorRollingShutter.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>
#include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam_unstable/dllexport.h>


namespace gtsam {

class GTSAM_UNSTABLE_EXPORT ProjectionFactorRollingShutter
    : public NoiseModelFactorN<Pose3, Pose3, Point3> {
 protected:
  // Keep a copy of measurement and calibration for I/O
  Point2 measured_;  
  double alpha_;     
  std::shared_ptr<Cal3_S2> K_;  
  std::optional<Pose3>
      body_P_sensor_;  

  // verbosity handling for Cheirality Exceptions
  bool throwCheirality_;  
  bool verboseCheirality_;  

 public:
  typedef NoiseModelFactor3<Pose3, Pose3, Point3> Base;

  // Provide access to the Matrix& version of evaluateError:
  using Base::evaluateError;


  typedef ProjectionFactorRollingShutter This;

  typedef std::shared_ptr<This> shared_ptr;

  ProjectionFactorRollingShutter()
      : measured_(0, 0),
        alpha_(0),
        throwCheirality_(false),
        verboseCheirality_(false) {}

  ProjectionFactorRollingShutter(
      const Point2& measured, double alpha, const SharedNoiseModel& model,
      Key poseKey_a, Key poseKey_b, Key pointKey,
      const std::shared_ptr<Cal3_S2>& K,
      std::optional<Pose3> body_P_sensor = {})
      : Base(model, poseKey_a, poseKey_b, pointKey),
        measured_(measured),
        alpha_(alpha),
        K_(K),
        body_P_sensor_(body_P_sensor),
        throwCheirality_(false),
        verboseCheirality_(false) {}

  ProjectionFactorRollingShutter(
      const Point2& measured, double alpha, const SharedNoiseModel& model,
      Key poseKey_a, Key poseKey_b, Key pointKey,
      const std::shared_ptr<Cal3_S2>& K, bool throwCheirality,
      bool verboseCheirality,
      std::optional<Pose3> body_P_sensor = {})
      : Base(model, poseKey_a, poseKey_b, pointKey),
        measured_(measured),
        alpha_(alpha),
        K_(K),
        body_P_sensor_(body_P_sensor),
        throwCheirality_(throwCheirality),
        verboseCheirality_(verboseCheirality) {}

  virtual ~ProjectionFactorRollingShutter() {}

  gtsam::NonlinearFactor::shared_ptr clone() const override {
    return std::static_pointer_cast<gtsam::NonlinearFactor>(
        gtsam::NonlinearFactor::shared_ptr(new This(*this)));
  }

  void print(
      const std::string& s = "",
      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
    std::cout << s << "ProjectionFactorRollingShutter, z = ";
    traits<Point2>::Print(measured_);
    std::cout << " rolling shutter interpolation param = " << alpha_;
    if (this->body_P_sensor_)
      this->body_P_sensor_->print("  sensor pose in body frame: ");
    Base::print("", keyFormatter);
  }

  bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
    const This* e = dynamic_cast<const This*>(&p);
    return e && Base::equals(p, tol) && (alpha_ == e->alpha()) &&
           traits<Point2>::Equals(this->measured_, e->measured_, tol) &&
           this->K_->equals(*e->K_, tol) &&
           (this->throwCheirality_ == e->throwCheirality_) &&
           (this->verboseCheirality_ == e->verboseCheirality_) &&
           ((!body_P_sensor_ && !e->body_P_sensor_) ||
            (body_P_sensor_ && e->body_P_sensor_ &&
             body_P_sensor_->equals(*e->body_P_sensor_)));
  }

  Vector evaluateError(
      const Pose3& pose_a, const Pose3& pose_b, const Point3& point,
      OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3) const override;

  const Point2& measured() const { return measured_; }

  inline const std::shared_ptr<Cal3_S2> calibration() const { return K_; }

  inline double alpha() const { return alpha_; }

  inline bool verboseCheirality() const { return verboseCheirality_; }

  inline bool throwCheirality() const { return throwCheirality_; }

 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_BASE_OBJECT_NVP(Base);
    ar& BOOST_SERIALIZATION_NVP(measured_);
    ar& BOOST_SERIALIZATION_NVP(alpha_);
    ar& BOOST_SERIALIZATION_NVP(K_);
    ar& BOOST_SERIALIZATION_NVP(body_P_sensor_);
    ar& BOOST_SERIALIZATION_NVP(throwCheirality_);
    ar& BOOST_SERIALIZATION_NVP(verboseCheirality_);
  }
#endif

 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

template <>
struct traits<ProjectionFactorRollingShutter>
    : public Testable<ProjectionFactorRollingShutter> {};

}  // namespace gtsam