ProjectionFactorRollingShutter.h
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
18 #pragma once
19 
20 #include <gtsam/geometry/Cal3_S2.h>
24 #include <gtsam_unstable/dllexport.h>
25 
26 
27 namespace gtsam {
28 
43 class GTSAM_UNSTABLE_EXPORT ProjectionFactorRollingShutter
44  : public NoiseModelFactorN<Pose3, Pose3, Point3> {
45  protected:
46  // Keep a copy of measurement and calibration for I/O
48  double alpha_;
49  std::shared_ptr<Cal3_S2> K_;
51  std::optional<Pose3>
53 
54  // verbosity handling for Cheirality Exceptions
56  bool verboseCheirality_;
58 
60  public:
62  typedef NoiseModelFactor3<Pose3, Pose3, Point3> Base;
63 
64  // Provide access to the Matrix& version of evaluateError:
65  using Base::evaluateError;
66 
67 
70 
72  typedef std::shared_ptr<This> shared_ptr;
73 
76  : measured_(0, 0),
77  alpha_(0),
78  throwCheirality_(false),
79  verboseCheirality_(false) {}
80 
95  const Point2& measured, double alpha, const SharedNoiseModel& model,
96  Key poseKey_a, Key poseKey_b, Key pointKey,
97  const std::shared_ptr<Cal3_S2>& K,
98  std::optional<Pose3> body_P_sensor = {})
99  : Base(model, poseKey_a, poseKey_b, pointKey),
100  measured_(measured),
101  alpha_(alpha),
102  K_(K),
103  body_P_sensor_(body_P_sensor),
104  throwCheirality_(false),
105  verboseCheirality_(false) {}
106 
125  const Point2& measured, double alpha, const SharedNoiseModel& model,
126  Key poseKey_a, Key poseKey_b, Key pointKey,
127  const std::shared_ptr<Cal3_S2>& K, bool throwCheirality,
128  bool verboseCheirality,
129  std::optional<Pose3> body_P_sensor = {})
130  : Base(model, poseKey_a, poseKey_b, pointKey),
131  measured_(measured),
132  alpha_(alpha),
133  K_(K),
134  body_P_sensor_(body_P_sensor),
135  throwCheirality_(throwCheirality),
136  verboseCheirality_(verboseCheirality) {}
137 
140 
143  return std::static_pointer_cast<gtsam::NonlinearFactor>(
145  }
146 
152  void print(
153  const std::string& s = "",
154  const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
155  std::cout << s << "ProjectionFactorRollingShutter, z = ";
156  traits<Point2>::Print(measured_);
157  std::cout << " rolling shutter interpolation param = " << alpha_;
158  if (this->body_P_sensor_)
159  this->body_P_sensor_->print(" sensor pose in body frame: ");
160  Base::print("", keyFormatter);
161  }
162 
164  bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
165  const This* e = dynamic_cast<const This*>(&p);
166  return e && Base::equals(p, tol) && (alpha_ == e->alpha()) &&
167  traits<Point2>::Equals(this->measured_, e->measured_, tol) &&
168  this->K_->equals(*e->K_, tol) &&
169  (this->throwCheirality_ == e->throwCheirality_) &&
170  (this->verboseCheirality_ == e->verboseCheirality_) &&
171  ((!body_P_sensor_ && !e->body_P_sensor_) ||
172  (body_P_sensor_ && e->body_P_sensor_ &&
173  body_P_sensor_->equals(*e->body_P_sensor_)));
174  }
175 
177  Vector evaluateError(
178  const Pose3& pose_a, const Pose3& pose_b, const Point3& point,
179  OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3) const override;
180 
182  const Point2& measured() const { return measured_; }
183 
185  inline const std::shared_ptr<Cal3_S2> calibration() const { return K_; }
186 
188  inline double alpha() const { return alpha_; }
189 
191  inline bool verboseCheirality() const { return verboseCheirality_; }
192 
194  inline bool throwCheirality() const { return throwCheirality_; }
195 
196  private:
197 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
198  friend class boost::serialization::access;
200  template <class ARCHIVE>
201  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
202  ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
203  ar& BOOST_SERIALIZATION_NVP(measured_);
204  ar& BOOST_SERIALIZATION_NVP(alpha_);
205  ar& BOOST_SERIALIZATION_NVP(K_);
206  ar& BOOST_SERIALIZATION_NVP(body_P_sensor_);
207  ar& BOOST_SERIALIZATION_NVP(throwCheirality_);
208  ar& BOOST_SERIALIZATION_NVP(verboseCheirality_);
209  }
210 #endif
211 
212  public:
214 };
215 
217 template <>
219  : public Testable<ProjectionFactorRollingShutter> {};
220 
221 } // namespace gtsam
const std::shared_ptr< Cal3_S2 > calibration() const
Point2 measured(-17, 30)
std::string serialize(const T &input)
serializes to a string
noiseModel::Diagonal::shared_ptr model
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={})
Vector2 Point2
Definition: Point2.h:32
static Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0))
#define EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Definition: Memory.h:841
static Cal3_S2 K(500, 500, 0.1, 640/2, 480/2)
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
std::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
EIGEN_STRONG_INLINE Packet4f print(const Packet4f &a)
static const KeyFormatter DefaultKeyFormatter
Definition: Key.h:43
const gtsam::Key pointKey
Base class for all pinhole cameras.
ProjectionFactorRollingShutter This
shorthand for this class
Matrix * OptionalMatrixType
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::NonlinearFactor::shared_ptr clone() const override
RealScalar alpha
Array< double, 1, 3 > e(1./3., 0.5, 2.)
RealScalar s
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition: Key.h:35
#define This
Calibrated camera for which only pose is unknown.
NoiseModelFactor3< Pose3, Pose3, Point3 > Base
shorthand for base class type
traits
Definition: chartTesting.h:28
Non-linear factor base classes.
std::shared_ptr< This > shared_ptr
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
float * p
std::optional< Pose3 > body_P_sensor_
The pose of the sensor in the body frame.
const G double tol
Definition: Group.h:86
Vector3 Point3
Definition: Point3.h:38
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={})
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
void Print(const CONTAINER &keys, const string &s, const KeyFormatter &keyFormatter)
Definition: Key.cpp:62
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:741
The most common 5DOF 3D->2D calibration.
bool equals(const This &other, double tol=1e-9) const
check equality
Definition: Factor.cpp:42


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:35:26