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
gtsam::NonlinearFactor::shared_ptr
std::shared_ptr< This > shared_ptr
Definition: NonlinearFactor.h:78
Eigen::internal::print
EIGEN_STRONG_INLINE Packet4f print(const Packet4f &a)
Definition: NEON/PacketMath.h:3115
gtsam::ProjectionFactorRollingShutter::measured_
Point2 measured_
2D measurement
Definition: ProjectionFactorRollingShutter.h:47
pointKey
const gtsam::Key pointKey
Definition: testRelativeElevationFactor.cpp:25
alpha
RealScalar alpha
Definition: level1_cplx_impl.h:147
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
Cal3_S2.h
The most common 5DOF 3D->2D calibration.
gtsam::ProjectionFactorRollingShutter::alpha_
double alpha_
Definition: ProjectionFactorRollingShutter.h:48
gtsam::ProjectionFactorRollingShutter::Base
NoiseModelFactor3< Pose3, Pose3, Point3 > Base
shorthand for base class type
Definition: ProjectionFactorRollingShutter.h:62
measured
Point2 measured(-17, 30)
gtsam::Factor
Definition: Factor.h:70
gtsam::ProjectionFactorRollingShutter::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition: ProjectionFactorRollingShutter.h:164
gtsam::ProjectionFactorRollingShutter::alpha
double alpha() const
Definition: ProjectionFactorRollingShutter.h:188
body_P_sensor
static Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0))
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::ProjectionFactorRollingShutter::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Definition: ProjectionFactorRollingShutter.h:152
CalibratedCamera.h
Calibrated camera for which only pose is unknown.
gtsam::DefaultKeyFormatter
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
Definition: Key.cpp:30
gtsam::ProjectionFactorRollingShutter::body_P_sensor_
std::optional< Pose3 > body_P_sensor_
The pose of the sensor in the body frame.
Definition: ProjectionFactorRollingShutter.h:52
gtsam::ProjectionFactorRollingShutter::This
ProjectionFactorRollingShutter This
shorthand for this class
Definition: ProjectionFactorRollingShutter.h:69
gtsam_unstable.tests.test_ProjectionFactorRollingShutter.point
point
Definition: test_ProjectionFactorRollingShutter.py:25
gtsam::KeyFormatter
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
gtsam::Pose3
Definition: Pose3.h:37
gtsam::NoiseModelFactorN
Definition: NonlinearFactor.h:431
gtsam::ProjectionFactorRollingShutter
Definition: ProjectionFactorRollingShutter.h:43
gtsam::ProjectionFactorRollingShutter::verboseCheirality
bool verboseCheirality() const
Definition: ProjectionFactorRollingShutter.h:191
PinholeCamera.h
Base class for all pinhole cameras.
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
#define EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Definition: Memory.h:841
gtsam::ProjectionFactorRollingShutter::~ProjectionFactorRollingShutter
virtual ~ProjectionFactorRollingShutter()
Definition: ProjectionFactorRollingShutter.h:139
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
gtsam::ProjectionFactorRollingShutter::ProjectionFactorRollingShutter
ProjectionFactorRollingShutter()
Default constructor.
Definition: ProjectionFactorRollingShutter.h:75
NonlinearFactor.h
Non-linear factor base classes.
gtsam::ProjectionFactorRollingShutter::shared_ptr
std::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
Definition: ProjectionFactorRollingShutter.h:72
gtsam::ProjectionFactorRollingShutter::ProjectionFactorRollingShutter
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={})
Definition: ProjectionFactorRollingShutter.h:94
gtsam
traits
Definition: SFMdata.h:40
gtsam::Testable
Definition: Testable.h:152
gtsam::traits
Definition: Group.h:36
K
#define K
Definition: igam.h:8
gtsam::NonlinearFactor
Definition: NonlinearFactor.h:68
p
float * p
Definition: Tutorial_Map_using.cpp:9
gtsam::ProjectionFactorRollingShutter::measured
const Point2 & measured() const
Definition: ProjectionFactorRollingShutter.h:182
This
#define This
Definition: ActiveSetSolver-inl.h:27
gtsam::Print
void Print(const CONTAINER &keys, const string &s, const KeyFormatter &keyFormatter)
Definition: Key.cpp:65
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
gtsam::OptionalMatrixType
Matrix * OptionalMatrixType
Definition: NonlinearFactor.h:55
gtsam::ProjectionFactorRollingShutter::calibration
const std::shared_ptr< Cal3_S2 > calibration() const
Definition: ProjectionFactorRollingShutter.h:185
Base
Definition: test_virtual_functions.cpp:156
gtsam::ProjectionFactorRollingShutter::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: ProjectionFactorRollingShutter.h:142
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
gtsam::ProjectionFactorRollingShutter::ProjectionFactorRollingShutter
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={})
Definition: ProjectionFactorRollingShutter.h:124
gtsam::ProjectionFactorRollingShutter::throwCheirality_
bool throwCheirality_
Definition: ProjectionFactorRollingShutter.h:55
gtsam::ProjectionFactorRollingShutter::throwCheirality
bool throwCheirality() const
Definition: ProjectionFactorRollingShutter.h:194


gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:03:53