gtsam  4.1.0
gtsam
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 #include <boost/optional.hpp>
25 
26 namespace gtsam {
27 
33  template<class POSE, class LANDMARK, class CALIBRATION = Cal3_S2>
34  class ProjectionFactorPPP: public NoiseModelFactor3<POSE, POSE, LANDMARK> {
35  protected:
36 
37  // Keep a copy of measurement and calibration for I/O
39  boost::shared_ptr<CALIBRATION> K_;
40 
41  // verbosity handling for Cheirality Exceptions
44 
45  public:
46 
49 
52 
54  typedef boost::shared_ptr<This> shared_ptr;
55 
58  measured_(0.0, 0.0), throwCheirality_(false), verboseCheirality_(false) {
59  }
60 
72  Key poseKey, Key transformKey, Key pointKey,
73  const boost::shared_ptr<CALIBRATION>& K) :
74  Base(model, poseKey, transformKey, pointKey), measured_(measured), K_(K),
75  throwCheirality_(false), verboseCheirality_(false) {}
76 
89  Key poseKey, Key transformKey, Key pointKey,
90  const boost::shared_ptr<CALIBRATION>& K,
92  Base(model, poseKey, transformKey, pointKey), measured_(measured), K_(K),
94 
96  virtual ~ProjectionFactorPPP() {}
97 
99  NonlinearFactor::shared_ptr clone() const override {
100  return boost::static_pointer_cast<NonlinearFactor>(
101  NonlinearFactor::shared_ptr(new This(*this))); }
102 
108  void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
109  std::cout << s << "ProjectionFactorPPP, z = ";
111  Base::print("", keyFormatter);
112  }
113 
115  bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
116  const This *e = dynamic_cast<const This*>(&p);
117  return e
118  && Base::equals(p, tol)
119  && traits<Point2>::Equals(this->measured_, e->measured_, tol)
120  && this->K_->equals(*e->K_, tol);
121  }
122 
124  Vector evaluateError(const Pose3& pose, const Pose3& transform, const Point3& point,
125  boost::optional<Matrix&> H1 = boost::none,
126  boost::optional<Matrix&> H2 = boost::none,
127  boost::optional<Matrix&> H3 = boost::none) 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, boost::none) - measured_);
133  *H2 = *H1 * H02;
134  *H1 = *H1 * H0;
135  return reprojectionError;
136  } else {
137  PinholeCamera<CALIBRATION> camera(pose.compose(transform), *K_);
138  return camera.project(point, H1, H3, boost::none) - 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 "<< DefaultKeyFormatter(this->key2()) <<
146  " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
147  if (throwCheirality_)
148  throw e;
149  }
150  return Vector::Ones(2) * 2.0 * K_->fx();
151  }
152 
154  const Point2& measured() const {
155  return measured_;
156  }
157 
159  inline const boost::shared_ptr<CALIBRATION> calibration() const {
160  return K_;
161  }
162 
164  inline bool verboseCheirality() const { return verboseCheirality_; }
165 
167  inline bool throwCheirality() const { return throwCheirality_; }
168 
169  private:
170 
173  template<class ARCHIVE>
174  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
175  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
176  ar & BOOST_SERIALIZATION_NVP(measured_);
177  ar & BOOST_SERIALIZATION_NVP(K_);
178  ar & BOOST_SERIALIZATION_NVP(throwCheirality_);
179  ar & BOOST_SERIALIZATION_NVP(verboseCheirality_);
180  }
181  };
182 
184  template<class POSE, class LANDMARK, class CALIBRATION>
185  struct traits<ProjectionFactorPPP<POSE, LANDMARK, CALIBRATION> > :
186  public Testable<ProjectionFactorPPP<POSE, LANDMARK, CALIBRATION> > {
187  };
188 
189 } // \ namespace gtsam
gtsam::ProjectionFactorPPP::ProjectionFactorPPP
ProjectionFactorPPP()
Default constructor.
Definition: ProjectionFactorPPP.h:57
gtsam::ProjectionFactorPPP::throwCheirality_
bool throwCheirality_
If true, rethrows Cheirality exceptions (default: false)
Definition: ProjectionFactorPPP.h:42
PinholeCamera.h
Base class for all pinhole cameras.
gtsam::PinholeBaseK< Calibration >::project
Point2 project(const Point3 &pw, OptionalJacobian< 2, 6 > Dpose=boost::none, OptionalJacobian< 2, 3 > Dpoint=boost::none, OptionalJacobian< 2, DimK > Dcal=boost::none) const
project a 3D point from world coordinates into the image
Definition: PinholePose.h:118
gtsam::ProjectionFactorPPP::shared_ptr
boost::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
Definition: ProjectionFactorPPP.h:54
gtsam::NoiseModelFactor3
A convenient base class for creating your own NoiseModelFactor with 3 variables.
Definition: NonlinearFactor.h:430
gtsam::CheiralityException
Definition: CalibratedCamera.h:32
gtsam::NoiseModelFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Print.
Definition: NonlinearFactor.cpp:63
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Note, deliberately not in noiseModel namespace.
Definition: NoiseModel.h:734
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
gtsam::ProjectionFactorPPP::measured_
Point2 measured_
2D measurement
Definition: ProjectionFactorPPP.h:38
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::ProjectionFactorPPP::K_
boost::shared_ptr< CALIBRATION > K_
shared pointer to calibration object
Definition: ProjectionFactorPPP.h:39
gtsam::ProjectionFactorPPP::throwCheirality
bool throwCheirality() const
return flag for throwing cheirality exceptions
Definition: ProjectionFactorPPP.h:167
gtsam::NoiseModelFactor3< POSE, POSE, LANDMARK >::key1
Key key1() const
methods to retrieve keys
Definition: NonlinearFactor.h:464
gtsam::NoiseModelFactor::equals
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
Check if two factors are equal.
Definition: NonlinearFactor.cpp:71
gtsam::ProjectionFactorPPP::verboseCheirality_
bool verboseCheirality_
If true, prints text for Cheirality exceptions (default: false)
Definition: ProjectionFactorPPP.h:43
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::ProjectionFactorPPP
Definition: ProjectionFactorPPP.h:34
gtsam::ProjectionFactorPPP::evaluateError
Vector evaluateError(const Pose3 &pose, const Pose3 &transform, const Point3 &point, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none, boost::optional< Matrix & > H3=boost::none) const override
Evaluate error h(x)-z and optionally derivatives.
Definition: ProjectionFactorPPP.h:124
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:150
gtsam::ProjectionFactorPPP::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition: ProjectionFactorPPP.h:115
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition: NonlinearFactor.h:43
gtsam::ProjectionFactorPPP::verboseCheirality
bool verboseCheirality() const
return verbosity
Definition: ProjectionFactorPPP.h:164
gtsam::ProjectionFactorPPP::This
ProjectionFactorPPP< POSE, LANDMARK, CALIBRATION > This
shorthand for this class
Definition: ProjectionFactorPPP.h:51
NonlinearFactor.h
Non-linear factor base classes.
gtsam::ProjectionFactorPPP::access
friend class boost::serialization::access
Serialization function.
Definition: ProjectionFactorPPP.h:172
gtsam::ProjectionFactorPPP::calibration
const boost::shared_ptr< CALIBRATION > calibration() const
return the calibration object
Definition: ProjectionFactorPPP.h:159
gtsam::Point2
Vector2 Point2
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2...
Definition: Point2.h:27
gtsam::ProjectionFactorPPP::measured
const Point2 & measured() const
return the measurement
Definition: ProjectionFactorPPP.h:154
gtsam::PinholeCamera
Definition: PinholeCamera.h:33
gtsam::Pose3
Definition: Pose3.h:37
gtsam::KeyFormatter
boost::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::Factor
This is the base class for all factor types.
Definition: Factor.h:55
gtsam::ProjectionFactorPPP::Base
NoiseModelFactor3< POSE, POSE, LANDMARK > Base
shorthand for base class type
Definition: ProjectionFactorPPP.h:48
gtsam::ProjectionFactorPPP::clone
NonlinearFactor::shared_ptr clone() const override
Definition: ProjectionFactorPPP.h:99
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition: Point3.h:35
gtsam::ProjectionFactorPPP::~ProjectionFactorPPP
virtual ~ProjectionFactorPPP()
Virtual destructor.
Definition: ProjectionFactorPPP.h:96
gtsam::ProjectionFactorPPP::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: ProjectionFactorPPP.h:108
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 boost::shared_ptr< CALIBRATION > &K)
Constructor TODO: Mark argument order standard (keys, measurement, parameters)
Definition: ProjectionFactorPPP.h:71
gtsam::ProjectionFactorPPP::ProjectionFactorPPP
ProjectionFactorPPP(const Point2 &measured, const SharedNoiseModel &model, Key poseKey, Key transformKey, Key pointKey, const boost::shared_ptr< CALIBRATION > &K, bool throwCheirality, bool verboseCheirality)
Constructor with exception-handling flags TODO: Mark argument order standard (keys,...
Definition: ProjectionFactorPPP.h:88