gtsam  4.1.0
gtsam
ProjectionFactorPPPC.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 ProjectionFactorPPPC: public NoiseModelFactor4<POSE, POSE, LANDMARK, CALIBRATION> {
35  protected:
36 
38 
39  // verbosity handling for Cheirality Exceptions
42 
43  public:
44 
47 
50 
52  typedef boost::shared_ptr<This> shared_ptr;
53 
56  measured_(0.0, 0.0), throwCheirality_(false), verboseCheirality_(false) {
57  }
58 
69  Key poseKey, Key transformKey, Key pointKey, Key calibKey) :
70  Base(model, poseKey, transformKey, pointKey, calibKey), measured_(measured),
71  throwCheirality_(false), verboseCheirality_(false) {}
72 
85  Key poseKey, Key transformKey, Key pointKey, Key calibKey,
87  Base(model, poseKey, transformKey, pointKey, calibKey), measured_(measured),
89 
91  virtual ~ProjectionFactorPPPC() {}
92 
94  NonlinearFactor::shared_ptr clone() const override {
95  return boost::static_pointer_cast<NonlinearFactor>(
96  NonlinearFactor::shared_ptr(new This(*this))); }
97 
103  void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
104  std::cout << s << "ProjectionFactorPPPC, z = ";
106  Base::print("", keyFormatter);
107  }
108 
110  bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
111  const This *e = dynamic_cast<const This*>(&p);
112  return e
113  && Base::equals(p, tol)
114  && traits<Point2>::Equals(this->measured_, e->measured_, tol);
115  }
116 
118  Vector evaluateError(const Pose3& pose, const Pose3& transform, const Point3& point, const CALIBRATION& K,
119  boost::optional<Matrix&> H1 = boost::none,
120  boost::optional<Matrix&> H2 = boost::none,
121  boost::optional<Matrix&> H3 = boost::none,
122  boost::optional<Matrix&> H4 = boost::none) const override {
123  try {
124  if(H1 || H2 || H3 || H4) {
125  Matrix H0, H02;
126  PinholeCamera<CALIBRATION> camera(pose.compose(transform, H0, H02), K);
127  Point2 reprojectionError(camera.project(point, H1, H3, H4) - measured_);
128  *H2 = *H1 * H02;
129  *H1 = *H1 * H0;
130  return reprojectionError;
131  } else {
132  PinholeCamera<CALIBRATION> camera(pose.compose(transform), K);
133  return camera.project(point, H1, H3, H4) - measured_;
134  }
135  } catch( CheiralityException& e) {
136  if (H1) *H1 = Matrix::Zero(2,6);
137  if (H2) *H2 = Matrix::Zero(2,6);
138  if (H3) *H3 = Matrix::Zero(2,3);
139  if (H4) *H4 = Matrix::Zero(2,CALIBRATION::Dim());
140  if (verboseCheirality_)
141  std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
142  " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
143  if (throwCheirality_)
144  throw e;
145  }
146  return Vector::Ones(2) * 2.0 * K.fx();
147  }
148 
150  const Point2& measured() const {
151  return measured_;
152  }
153 
155  inline bool verboseCheirality() const { return verboseCheirality_; }
156 
158  inline bool throwCheirality() const { return throwCheirality_; }
159 
160  private:
161 
164  template<class ARCHIVE>
165  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
166  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
167  ar & BOOST_SERIALIZATION_NVP(measured_);
168  ar & BOOST_SERIALIZATION_NVP(throwCheirality_);
169  ar & BOOST_SERIALIZATION_NVP(verboseCheirality_);
170  }
171  };
172 
174  template<class POSE, class LANDMARK, class CALIBRATION>
175  struct traits<ProjectionFactorPPPC<POSE, LANDMARK, CALIBRATION> > :
176  public Testable<ProjectionFactorPPPC<POSE, LANDMARK, CALIBRATION> > {
177  };
178 
179 } // \ namespace gtsam
gtsam::ProjectionFactorPPPC::verboseCheirality_
bool verboseCheirality_
If true, prints text for Cheirality exceptions (default: false)
Definition: ProjectionFactorPPPC.h:41
gtsam::ProjectionFactorPPPC::throwCheirality_
bool throwCheirality_
If true, rethrows Cheirality exceptions (default: false)
Definition: ProjectionFactorPPPC.h:40
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::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::ProjectionFactorPPPC::Base
NoiseModelFactor4< POSE, POSE, LANDMARK, CALIBRATION > Base
shorthand for base class type
Definition: ProjectionFactorPPPC.h:46
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::ProjectionFactorPPPC::ProjectionFactorPPPC
ProjectionFactorPPPC()
Default constructor.
Definition: ProjectionFactorPPPC.h:55
gtsam::ProjectionFactorPPPC::shared_ptr
boost::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
Definition: ProjectionFactorPPPC.h:52
gtsam::ProjectionFactorPPPC::access
friend class boost::serialization::access
Serialization function.
Definition: ProjectionFactorPPPC.h:163
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
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::ProjectionFactorPPPC::clone
NonlinearFactor::shared_ptr clone() const override
Definition: ProjectionFactorPPPC.h:94
gtsam::ProjectionFactorPPPC::measured
const Point2 & measured() const
return the measurement
Definition: ProjectionFactorPPPC.h:150
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:150
gtsam::ProjectionFactorPPPC::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: ProjectionFactorPPPC.h:103
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition: NonlinearFactor.h:43
gtsam::ProjectionFactorPPPC::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition: ProjectionFactorPPPC.h:110
gtsam::ProjectionFactorPPPC
Definition: ProjectionFactorPPPC.h:34
NonlinearFactor.h
Non-linear factor base classes.
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::ProjectionFactorPPPC::ProjectionFactorPPPC
ProjectionFactorPPPC(const Point2 &measured, const SharedNoiseModel &model, Key poseKey, Key transformKey, Key pointKey, Key calibKey)
Constructor TODO: Mark argument order standard (keys, measurement, parameters)
Definition: ProjectionFactorPPPC.h:68
gtsam::ProjectionFactorPPPC::evaluateError
Vector evaluateError(const Pose3 &pose, const Pose3 &transform, const Point3 &point, const CALIBRATION &K, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none, boost::optional< Matrix & > H3=boost::none, boost::optional< Matrix & > H4=boost::none) const override
Evaluate error h(x)-z and optionally derivatives.
Definition: ProjectionFactorPPPC.h:118
gtsam::ProjectionFactorPPPC::ProjectionFactorPPPC
ProjectionFactorPPPC(const Point2 &measured, const SharedNoiseModel &model, Key poseKey, Key transformKey, Key pointKey, Key calibKey, bool throwCheirality, bool verboseCheirality)
Constructor with exception-handling flags TODO: Mark argument order standard (keys,...
Definition: ProjectionFactorPPPC.h:84
gtsam::ProjectionFactorPPPC::~ProjectionFactorPPPC
virtual ~ProjectionFactorPPPC()
Virtual destructor.
Definition: ProjectionFactorPPPC.h:91
gtsam::PinholeCamera
Definition: PinholeCamera.h:33
gtsam::NoiseModelFactor4< POSE, POSE, LANDMARK, Cal3_S2 >::key1
Key key1() const
methods to retrieve keys
Definition: NonlinearFactor.h:543
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::ProjectionFactorPPPC::measured_
Point2 measured_
2D measurement
Definition: ProjectionFactorPPPC.h:37
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::ProjectionFactorPPPC::throwCheirality
bool throwCheirality() const
return flag for throwing cheirality exceptions
Definition: ProjectionFactorPPPC.h:158
gtsam::ProjectionFactorPPPC::This
ProjectionFactorPPPC< POSE, LANDMARK, CALIBRATION > This
shorthand for this class
Definition: ProjectionFactorPPPC.h:49
gtsam::NoiseModelFactor4
A convenient base class for creating your own NoiseModelFactor with 4 variables.
Definition: NonlinearFactor.h:507
gtsam::ProjectionFactorPPPC::verboseCheirality
bool verboseCheirality() const
return verbosity
Definition: ProjectionFactorPPPC.h:155
Cal3_S2.h
The most common 5DOF 3D->2D calibration.