gtsam  4.1.0
gtsam
ProjectionFactor.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 
21 #pragma once
22 
25 #include <gtsam/geometry/Cal3_S2.h>
26 #include <boost/optional.hpp>
27 
28 namespace gtsam {
29 
35  template<class POSE, class LANDMARK, class CALIBRATION = Cal3_S2>
36  class GenericProjectionFactor: public NoiseModelFactor2<POSE, LANDMARK> {
37  protected:
38 
39  // Keep a copy of measurement and calibration for I/O
41  boost::shared_ptr<CALIBRATION> K_;
42  boost::optional<POSE> body_P_sensor_;
43 
44  // verbosity handling for Cheirality Exceptions
47 
48  public:
49 
52 
55 
57  typedef boost::shared_ptr<This> shared_ptr;
58 
61  measured_(0, 0), throwCheirality_(false), verboseCheirality_(false) {
62  }
63 
75  Key poseKey, Key pointKey, const boost::shared_ptr<CALIBRATION>& K,
76  boost::optional<POSE> body_P_sensor = boost::none) :
77  Base(model, poseKey, pointKey), measured_(measured), K_(K), body_P_sensor_(body_P_sensor),
78  throwCheirality_(false), verboseCheirality_(false) {}
79 
93  Key poseKey, Key pointKey, const boost::shared_ptr<CALIBRATION>& K,
95  boost::optional<POSE> body_P_sensor = boost::none) :
96  Base(model, poseKey, pointKey), measured_(measured), K_(K), body_P_sensor_(body_P_sensor),
98 
101 
103  gtsam::NonlinearFactor::shared_ptr clone() const override {
104  return boost::static_pointer_cast<gtsam::NonlinearFactor>(
105  gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
106 
112  void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
113  std::cout << s << "GenericProjectionFactor, z = ";
115  if(this->body_P_sensor_)
116  this->body_P_sensor_->print(" sensor pose in body frame: ");
117  Base::print("", keyFormatter);
118  }
119 
121  bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
122  const This *e = dynamic_cast<const This*>(&p);
123  return e
124  && Base::equals(p, tol)
125  && traits<Point2>::Equals(this->measured_, e->measured_, tol)
126  && this->K_->equals(*e->K_, tol)
127  && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
128  }
129 
131  Vector evaluateError(const Pose3& pose, const Point3& point,
132  boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const override {
133  try {
134  if(body_P_sensor_) {
135  if(H1) {
136  gtsam::Matrix H0;
137  PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_, H0), *K_);
138  Point2 reprojectionError(camera.project(point, H1, H2, boost::none) - measured_);
139  *H1 = *H1 * H0;
140  return reprojectionError;
141  } else {
142  PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_), *K_);
143  return camera.project(point, H1, H2, boost::none) - measured_;
144  }
145  } else {
146  PinholeCamera<CALIBRATION> camera(pose, *K_);
147  return camera.project(point, H1, H2, boost::none) - measured_;
148  }
149  } catch( CheiralityException& e) {
150  if (H1) *H1 = Matrix::Zero(2,6);
151  if (H2) *H2 = Matrix::Zero(2,3);
152  if (verboseCheirality_)
153  std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
154  " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
155  if (throwCheirality_)
156  throw CheiralityException(this->key2());
157  }
158  return Vector2::Constant(2.0 * K_->fx());
159  }
160 
162  const Point2& measured() const {
163  return measured_;
164  }
165 
167  inline const boost::shared_ptr<CALIBRATION> calibration() const {
168  return K_;
169  }
170 
172  inline bool verboseCheirality() const { return verboseCheirality_; }
173 
175  inline bool throwCheirality() const { return throwCheirality_; }
176 
177  private:
178 
181  template<class ARCHIVE>
182  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
183  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
184  ar & BOOST_SERIALIZATION_NVP(measured_);
185  ar & BOOST_SERIALIZATION_NVP(K_);
186  ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
187  ar & BOOST_SERIALIZATION_NVP(throwCheirality_);
188  ar & BOOST_SERIALIZATION_NVP(verboseCheirality_);
189  }
190 
191  public:
193 };
194 
196  template<class POSE, class LANDMARK, class CALIBRATION>
197  struct traits<GenericProjectionFactor<POSE, LANDMARK, CALIBRATION> > :
198  public Testable<GenericProjectionFactor<POSE, LANDMARK, CALIBRATION> > {
199  };
200 
201 } // \ namespace gtsam
gtsam::GenericProjectionFactor::GenericProjectionFactor
GenericProjectionFactor()
Default constructor.
Definition: ProjectionFactor.h:60
gtsam::GenericProjectionFactor::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition: ProjectionFactor.h:121
gtsam::GenericProjectionFactor::throwCheirality_
bool throwCheirality_
If true, rethrows Cheirality exceptions (default: false)
Definition: ProjectionFactor.h:45
gtsam::GenericProjectionFactor::verboseCheirality
bool verboseCheirality() const
return verbosity
Definition: ProjectionFactor.h:172
gtsam::GenericProjectionFactor::evaluateError
Vector evaluateError(const Pose3 &pose, const Point3 &point, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override
Evaluate error h(x)-z and optionally derivatives.
Definition: ProjectionFactor.h:131
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::GenericProjectionFactor::Base
NoiseModelFactor2< POSE, LANDMARK > Base
shorthand for base class type
Definition: ProjectionFactor.h:51
gtsam::GenericProjectionFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: ProjectionFactor.h:103
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::GenericProjectionFactor::measured_
Point2 measured_
2D measurement
Definition: ProjectionFactor.h:40
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::GenericProjectionFactor::body_P_sensor_
boost::optional< POSE > body_P_sensor_
The pose of the sensor in the body frame.
Definition: ProjectionFactor.h:42
gtsam::NoiseModelFactor2< POSE, LANDMARK >::key1
Key key1() const
methods to retrieve both keys
Definition: NonlinearFactor.h:387
gtsam::GenericProjectionFactor::verboseCheirality_
bool verboseCheirality_
If true, prints text for Cheirality exceptions (default: false)
Definition: ProjectionFactor.h:46
gtsam::GenericProjectionFactor::~GenericProjectionFactor
virtual ~GenericProjectionFactor()
Virtual destructor.
Definition: ProjectionFactor.h:100
gtsam::GenericProjectionFactor
Definition: ProjectionFactor.h:36
gtsam::GenericProjectionFactor::GenericProjectionFactor
GenericProjectionFactor(const Point2 &measured, const SharedNoiseModel &model, Key poseKey, Key pointKey, const boost::shared_ptr< CALIBRATION > &K, boost::optional< POSE > body_P_sensor=boost::none)
Constructor TODO: Mark argument order standard (keys, measurement, parameters)
Definition: ProjectionFactor.h:74
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::Testable
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:150
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition: NonlinearFactor.h:43
NonlinearFactor.h
Non-linear factor base classes.
gtsam::GenericProjectionFactor::This
GenericProjectionFactor< POSE, LANDMARK, CALIBRATION > This
shorthand for this class
Definition: ProjectionFactor.h:54
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::GenericProjectionFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
Definition: ProjectionFactor.h:57
gtsam::GenericProjectionFactor::throwCheirality
bool throwCheirality() const
return flag for throwing cheirality exceptions
Definition: ProjectionFactor.h:175
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition: types.h:269
gtsam::GenericProjectionFactor::measured
const Point2 & measured() const
return the measurement
Definition: ProjectionFactor.h:162
gtsam::GenericProjectionFactor::calibration
const boost::shared_ptr< CALIBRATION > calibration() const
return the calibration object
Definition: ProjectionFactor.h:167
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::NoiseModelFactor2
A convenient base class for creating your own NoiseModelFactor with 2 variables.
Definition: NonlinearFactor.h:355
gtsam::GenericProjectionFactor::K_
boost::shared_ptr< CALIBRATION > K_
shared pointer to calibration object
Definition: ProjectionFactor.h:41
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::GenericProjectionFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: ProjectionFactor.h:112
gtsam::GenericProjectionFactor::GenericProjectionFactor
GenericProjectionFactor(const Point2 &measured, const SharedNoiseModel &model, Key poseKey, Key pointKey, const boost::shared_ptr< CALIBRATION > &K, bool throwCheirality, bool verboseCheirality, boost::optional< POSE > body_P_sensor=boost::none)
Constructor with exception-handling flags TODO: Mark argument order standard (keys,...
Definition: ProjectionFactor.h:92
Cal3_S2.h
The most common 5DOF 3D->2D calibration.
gtsam::GenericProjectionFactor::access
friend class boost::serialization::access
Serialization function.
Definition: ProjectionFactor.h:180