ProjectionFactorPPP.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 
19 #pragma once
20 
23 #include <gtsam/geometry/Cal3_S2.h>
24 
25 namespace gtsam {
26 
32  template<class POSE, class LANDMARK, class CALIBRATION = Cal3_S2>
33  class ProjectionFactorPPP: public NoiseModelFactorN<POSE, POSE, LANDMARK> {
34  protected:
35 
36  // Keep a copy of measurement and calibration for I/O
38  std::shared_ptr<CALIBRATION> K_;
39 
40  // verbosity handling for Cheirality Exceptions
43 
44  public:
45 
47  typedef NoiseModelFactor3<POSE, POSE, LANDMARK> Base;
48 
49  // Provide access to the Matrix& version of evaluateError:
50  using Base::evaluateError;
51 
54 
56  typedef std::shared_ptr<This> shared_ptr;
57 
60  measured_(0.0, 0.0), throwCheirality_(false), verboseCheirality_(false) {
61  }
62 
74  Key poseKey, Key transformKey, Key pointKey,
75  const std::shared_ptr<CALIBRATION>& K) :
76  Base(model, poseKey, transformKey, pointKey), measured_(measured), K_(K),
77  throwCheirality_(false), verboseCheirality_(false) {}
78 
91  Key poseKey, Key transformKey, Key pointKey,
92  const std::shared_ptr<CALIBRATION>& K,
94  Base(model, poseKey, transformKey, pointKey), measured_(measured), K_(K),
96 
98  ~ProjectionFactorPPP() override {}
99 
102  return std::static_pointer_cast<NonlinearFactor>(
103  NonlinearFactor::shared_ptr(new This(*this))); }
104 
110  void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
111  std::cout << s << "ProjectionFactorPPP, z = ";
113  Base::print("", keyFormatter);
114  }
115 
117  bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
118  const This *e = dynamic_cast<const This*>(&p);
119  return e
120  && Base::equals(p, tol)
121  && traits<Point2>::Equals(this->measured_, e->measured_, tol)
122  && this->K_->equals(*e->K_, tol);
123  }
124 
127  OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3) const override {
128  try {
129  if(H1 || H2 || H3) {
130  Matrix H0, H02;
131  PinholeCamera<CALIBRATION> camera(pose.compose(transform, H0, H02), *K_);
132  Point2 reprojectionError(camera.project(point, H1, H3, {}) - measured_);
133  *H2 = *H1 * H02;
134  *H1 = *H1 * H0;
135  return reprojectionError;
136  } else {
138  return camera.project(point, H1, H3, {}) - measured_;
139  }
140  } catch( CheiralityException& e) {
141  if (H1) *H1 = Matrix::Zero(2,6);
142  if (H2) *H2 = Matrix::Zero(2,6);
143  if (H3) *H3 = Matrix::Zero(2,3);
144  if (verboseCheirality_)
145  std::cout << e.what() << ": Landmark "
146  << DefaultKeyFormatter(this->key2())
147  << " moved behind camera "
148  << DefaultKeyFormatter(this->key1())
149  << std::endl;
150  if (throwCheirality_)
151  throw e;
152  }
153  return Vector::Ones(2) * 2.0 * K_->fx();
154  }
155 
157  const Point2& measured() const {
158  return measured_;
159  }
160 
162  inline const std::shared_ptr<CALIBRATION> calibration() const {
163  return K_;
164  }
165 
167  inline bool verboseCheirality() const { return verboseCheirality_; }
168 
170  inline bool throwCheirality() const { return throwCheirality_; }
171 
172  private:
173 
174 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
175  friend class boost::serialization::access;
177  template<class ARCHIVE>
178  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
179  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
180  ar & BOOST_SERIALIZATION_NVP(measured_);
181  ar & BOOST_SERIALIZATION_NVP(K_);
182  ar & BOOST_SERIALIZATION_NVP(throwCheirality_);
183  ar & BOOST_SERIALIZATION_NVP(verboseCheirality_);
184  }
185 #endif
186  };
187 
189  template<class POSE, class LANDMARK, class CALIBRATION>
190  struct traits<ProjectionFactorPPP<POSE, LANDMARK, CALIBRATION> > :
191  public Testable<ProjectionFactorPPP<POSE, LANDMARK, CALIBRATION> > {
192  };
193 
194 } // \ namespace gtsam
gtsam::NonlinearFactor::shared_ptr
std::shared_ptr< This > shared_ptr
Definition: NonlinearFactor.h:79
gtsam::ProjectionFactorPPP::throwCheirality_
bool throwCheirality_
If true, rethrows Cheirality exceptions (default: false)
Definition: ProjectionFactorPPP.h:41
Eigen::internal::print
EIGEN_STRONG_INLINE Packet4f print(const Packet4f &a)
Definition: NEON/PacketMath.h:3115
pose
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
gtsam::ProjectionFactorPPP
Definition: ProjectionFactorPPP.h:33
pointKey
const gtsam::Key pointKey
Definition: testRelativeElevationFactor.cpp:25
gtsam::ProjectionFactorPPP::~ProjectionFactorPPP
~ProjectionFactorPPP() override
Definition: ProjectionFactorPPP.h:98
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::ProjectionFactorPPP::ProjectionFactorPPP
ProjectionFactorPPP(const Point2 &measured, const SharedNoiseModel &model, Key poseKey, Key transformKey, Key pointKey, const std::shared_ptr< CALIBRATION > &K, bool throwCheirality, bool verboseCheirality)
Definition: ProjectionFactorPPP.h:90
gtsam::ProjectionFactorPPP::measured
const Point2 & measured() const
Definition: ProjectionFactorPPP.h:157
gtsam::ProjectionFactorPPP::clone
NonlinearFactor::shared_ptr clone() const override
Definition: ProjectionFactorPPP.h:101
gtsam::ProjectionFactorPPP::throwCheirality
bool throwCheirality() const
Definition: ProjectionFactorPPP.h:170
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
gtsam::Factor
Definition: Factor.h:70
gtsam::ProjectionFactorPPP::evaluateError
Vector evaluateError(const Pose3 &pose, const Pose3 &transform, const Point3 &point, OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3) const override
Evaluate error h(x)-z and optionally derivatives.
Definition: ProjectionFactorPPP.h:126
gtsam::ProjectionFactorPPP::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition: ProjectionFactorPPP.h:117
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:39
gtsam::ProjectionFactorPPP::ProjectionFactorPPP
ProjectionFactorPPP()
Default constructor.
Definition: ProjectionFactorPPP.h:59
gtsam::ProjectionFactorPPP::ProjectionFactorPPP
ProjectionFactorPPP(const Point2 &measured, const SharedNoiseModel &model, Key poseKey, Key transformKey, Key pointKey, const std::shared_ptr< CALIBRATION > &K)
Definition: ProjectionFactorPPP.h:73
poseKey
const gtsam::Key poseKey
Definition: testPoseRotationPrior.cpp:29
gtsam::DefaultKeyFormatter
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
Definition: Key.cpp:30
gtsam::CheiralityException
Definition: CalibratedCamera.h:34
gtsam::ProjectionFactorPPP::measured_
Point2 measured_
2D measurement
Definition: ProjectionFactorPPP.h:37
gtsam_unstable.tests.test_ProjectionFactorRollingShutter.point
point
Definition: test_ProjectionFactorRollingShutter.py:25
gtsam::ProjectionFactorPPP::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Definition: ProjectionFactorPPP.h:110
gtsam::ProjectionFactorPPP::shared_ptr
std::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
Definition: ProjectionFactorPPP.h:56
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::PinholeCamera
Definition: PinholeCamera.h:33
gtsam::NoiseModelFactorN
Definition: NonlinearFactor.h:432
gtsam::ProjectionFactorPPP::Base
NoiseModelFactor3< POSE, POSE, LANDMARK > Base
shorthand for base class type
Definition: ProjectionFactorPPP.h:47
gtsam::ProjectionFactorPPP::K_
std::shared_ptr< CALIBRATION > K_
shared pointer to calibration object
Definition: ProjectionFactorPPP.h:38
PinholeCamera.h
Base class for all pinhole cameras.
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
transform
EIGEN_DONT_INLINE void transform(const Transformation &t, Data &data)
Definition: geometry.cpp:25
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
gtsam::NoiseModelFactorN< POSE, POSE, LANDMARK >::key1
Key key1() const
Definition: NonlinearFactor.h:732
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
gtsam::ProjectionFactorPPP::This
ProjectionFactorPPP< POSE, LANDMARK, CALIBRATION > This
shorthand for this class
Definition: ProjectionFactorPPP.h:53
NonlinearFactor.h
Non-linear factor base classes.
gtsam::ProjectionFactorPPP::verboseCheirality_
bool verboseCheirality_
If true, prints text for Cheirality exceptions (default: false)
Definition: ProjectionFactorPPP.h:42
gtsam::NoiseModelFactorN< POSE, POSE, LANDMARK >::key2
Key key2() const
Definition: NonlinearFactor.h:736
gtsam::ProjectionFactorPPP::calibration
const std::shared_ptr< CALIBRATION > calibration() const
Definition: ProjectionFactorPPP.h:162
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::ProjectionFactorPPP::verboseCheirality
bool verboseCheirality() const
Definition: ProjectionFactorPPP.h:167
gtsam::NonlinearFactor
Definition: NonlinearFactor.h:69
p
float * p
Definition: Tutorial_Map_using.cpp:9
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:56
camera
static const CalibratedCamera camera(kDefaultPose)
Base
Definition: test_virtual_functions.cpp:156
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97


gtsam
Author(s):
autogenerated on Sun Dec 22 2024 04:13:01