gtsam  4.1.0
gtsam
GeneralSFMFactor.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 
24 #include <gtsam/geometry/Point2.h>
25 #include <gtsam/geometry/Point3.h>
26 #include <gtsam/geometry/Pose3.h>
30 #include <gtsam/base/concepts.h>
31 #include <gtsam/base/Manifold.h>
32 #include <gtsam/base/Matrix.h>
34 #include <gtsam/base/types.h>
35 #include <gtsam/base/Testable.h>
36 #include <gtsam/base/Vector.h>
37 #include <gtsam/base/timing.h>
38 
39 #include <boost/none.hpp>
40 #include <boost/optional/optional.hpp>
41 #include <boost/serialization/nvp.hpp>
42 #include <boost/smart_ptr/shared_ptr.hpp>
43 #include <iostream>
44 #include <string>
45 
46 namespace boost {
47 namespace serialization {
48 class access;
49 } /* namespace serialization */
50 } /* namespace boost */
51 
52 namespace gtsam {
53 
59 template<class CAMERA, class LANDMARK>
60 class GeneralSFMFactor: public NoiseModelFactor2<CAMERA, LANDMARK> {
61 
62  GTSAM_CONCEPT_MANIFOLD_TYPE(CAMERA);
63  GTSAM_CONCEPT_MANIFOLD_TYPE(LANDMARK);
64 
65  static const int DimC = FixedDimension<CAMERA>::value;
66  static const int DimL = FixedDimension<LANDMARK>::value;
67  typedef Eigen::Matrix<double, 2, DimC> JacobianC;
68  typedef Eigen::Matrix<double, 2, DimL> JacobianL;
69 
70 protected:
71 
73 
74 public:
75 
78 
79  // shorthand for a smart pointer to a factor
80  typedef boost::shared_ptr<This> shared_ptr;
81 
89  GeneralSFMFactor(const Point2& measured, const SharedNoiseModel& model, Key cameraKey, Key landmarkKey) :
90  Base(model, cameraKey, landmarkKey), measured_(measured) {}
91 
92  GeneralSFMFactor():measured_(0.0,0.0) {}
93  GeneralSFMFactor(const Point2 & p):measured_(p) {}
94  GeneralSFMFactor(double x, double y):measured_(x,y) {}
95 
96  virtual ~GeneralSFMFactor() {}
97 
99  gtsam::NonlinearFactor::shared_ptr clone() const override {
100  return boost::static_pointer_cast<gtsam::NonlinearFactor>(
101  gtsam::NonlinearFactor::shared_ptr(new This(*this)));}
102 
108  void print(const std::string& s = "SFMFactor", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
109  Base::print(s, keyFormatter);
110  traits<Point2>::Print(measured_, s + ".z");
111  }
112 
116  bool equals(const NonlinearFactor &p, double tol = 1e-9) const override {
117  const This* e = dynamic_cast<const This*>(&p);
118  return e && Base::equals(p, tol) && traits<Point2>::Equals(this->measured_, e->measured_, tol);
119  }
120 
122  Vector evaluateError(const CAMERA& camera, const LANDMARK& point,
123  boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const override {
124  try {
125  return camera.project2(point,H1,H2) - measured_;
126  }
127  catch( CheiralityException& e) {
128  if (H1) *H1 = JacobianC::Zero();
129  if (H2) *H2 = JacobianL::Zero();
130  // TODO warn if verbose output asked for
131  return Z_2x1;
132  }
133  }
134 
136  boost::shared_ptr<GaussianFactor> linearize(const Values& values) const override {
137  // Only linearize if the factor is active
138  if (!this->active(values)) return boost::shared_ptr<JacobianFactor>();
139 
140  const Key key1 = this->key1(), key2 = this->key2();
141  JacobianC H1;
142  JacobianL H2;
143  Vector2 b;
144  try {
145  const CAMERA& camera = values.at<CAMERA>(key1);
146  const LANDMARK& point = values.at<LANDMARK>(key2);
147  b = measured() - camera.project2(point, H1, H2);
148  } catch (CheiralityException& e) {
149  H1.setZero();
150  H2.setZero();
151  b.setZero();
152  // TODO warn if verbose output asked for
153  }
154 
155  // Whiten the system if needed
156  const SharedNoiseModel& noiseModel = this->noiseModel();
157  if (noiseModel && !noiseModel->isUnit()) {
158  // TODO: implement WhitenSystem for fixed size matrices and include
159  // above
160  H1 = noiseModel->Whiten(H1);
161  H2 = noiseModel->Whiten(H2);
162  b = noiseModel->Whiten(b);
163  }
164 
165  // Create new (unit) noiseModel, preserving constraints if applicable
166  SharedDiagonal model;
167  if (noiseModel && noiseModel->isConstrained()) {
168  model = boost::static_pointer_cast<noiseModel::Constrained>(noiseModel)->unit();
169  }
170 
171  return boost::make_shared<BinaryJacobianFactor<2, DimC, DimL> >(key1, H1, key2, H2, b, model);
172  }
173 
175  inline const Point2 measured() const {
176  return measured_;
177  }
178 
179 private:
182  template<class Archive>
183  void serialize(Archive & ar, const unsigned int /*version*/) {
184  ar & boost::serialization::make_nvp("NoiseModelFactor2",
185  boost::serialization::base_object<Base>(*this));
186  ar & BOOST_SERIALIZATION_NVP(measured_);
187  }
188 };
189 
190 template<class CAMERA, class LANDMARK>
191 struct traits<GeneralSFMFactor<CAMERA, LANDMARK> > : Testable<
192  GeneralSFMFactor<CAMERA, LANDMARK> > {
193 };
194 
199 template<class CALIBRATION>
200 class GeneralSFMFactor2: public NoiseModelFactor3<Pose3, Point3, CALIBRATION> {
201 
202  GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION);
203  static const int DimK = FixedDimension<CALIBRATION>::value;
204 
205 protected:
206 
208 
209 public:
210 
214 
215  // shorthand for a smart pointer to a factor
216  typedef boost::shared_ptr<This> shared_ptr;
217 
226  GeneralSFMFactor2(const Point2& measured, const SharedNoiseModel& model, Key poseKey, Key landmarkKey, Key calibKey) :
227  Base(model, poseKey, landmarkKey, calibKey), measured_(measured) {}
229 
230  virtual ~GeneralSFMFactor2() {}
231 
233  gtsam::NonlinearFactor::shared_ptr clone() const override {
234  return boost::static_pointer_cast<gtsam::NonlinearFactor>(
235  gtsam::NonlinearFactor::shared_ptr(new This(*this)));}
236 
242  void print(const std::string& s = "SFMFactor2", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
243  Base::print(s, keyFormatter);
244  traits<Point2>::Print(measured_, s + ".z");
245  }
246 
250  bool equals(const NonlinearFactor &p, double tol = 1e-9) const override {
251  const This* e = dynamic_cast<const This*>(&p);
252  return e && Base::equals(p, tol) && traits<Point2>::Equals(this->measured_, e->measured_, tol);
253  }
254 
256  Vector evaluateError(const Pose3& pose3, const Point3& point, const CALIBRATION &calib,
257  boost::optional<Matrix&> H1=boost::none,
258  boost::optional<Matrix&> H2=boost::none,
259  boost::optional<Matrix&> H3=boost::none) const override
260  {
261  try {
262  Camera camera(pose3,calib);
263  return camera.project(point, H1, H2, H3) - measured_;
264  }
265  catch( CheiralityException& e) {
266  if (H1) *H1 = Matrix::Zero(2, 6);
267  if (H2) *H2 = Matrix::Zero(2, 3);
268  if (H3) *H3 = Matrix::Zero(2, DimK);
269  std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2())
270  << " behind Camera " << DefaultKeyFormatter(this->key1()) << std::endl;
271  }
272  return Z_2x1;
273  }
274 
276  inline const Point2 measured() const {
277  return measured_;
278  }
279 
280 private:
283  template<class Archive>
284  void serialize(Archive & ar, const unsigned int /*version*/) {
285  ar & boost::serialization::make_nvp("NoiseModelFactor3",
286  boost::serialization::base_object<Base>(*this));
287  ar & BOOST_SERIALIZATION_NVP(measured_);
288  }
289 };
290 
291 template<class CALIBRATION>
292 struct traits<GeneralSFMFactor2<CALIBRATION> > : Testable<
293  GeneralSFMFactor2<CALIBRATION> > {
294 };
295 
296 } //namespace
timing.h
Timing utilities.
gtsam::GeneralSFMFactor::print
void print(const std::string &s="SFMFactor", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: GeneralSFMFactor.h:108
Testable.h
Concept check for values that can be used in unit tests.
PinholeCamera.h
Base class for all pinhole cameras.
gtsam::PinholeBaseK::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::GeneralSFMFactor::Base
NoiseModelFactor2< CAMERA, LANDMARK > Base
typedef for the base class
Definition: GeneralSFMFactor.h:77
gtsam::GeneralSFMFactor2::Camera
PinholeCamera< CALIBRATION > Camera
typedef for camera type
Definition: GeneralSFMFactor.h:212
gtsam::NoiseModelFactor3
A convenient base class for creating your own NoiseModelFactor with 3 variables.
Definition: NonlinearFactor.h:430
gtsam::GeneralSFMFactor::This
GeneralSFMFactor< CAMERA, LANDMARK > This
typedef for this object
Definition: GeneralSFMFactor.h:76
gtsam::GeneralSFMFactor2::Base
NoiseModelFactor3< Pose3, Point3, CALIBRATION > Base
typedef for the base class
Definition: GeneralSFMFactor.h:213
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::GeneralSFMFactor2::print
void print(const std::string &s="SFMFactor2", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: GeneralSFMFactor.h:242
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::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::NoiseModelFactor::noiseModel
const SharedNoiseModel & noiseModel() const
access to the noise model
Definition: NonlinearFactor.h:203
gtsam::GeneralSFMFactor::GeneralSFMFactor
GeneralSFMFactor()
default constructor
Definition: GeneralSFMFactor.h:92
gtsam::GeneralSFMFactor
Definition: GeneralSFMFactor.h:60
gtsam::GeneralSFMFactor2::access
friend class boost::serialization::access
Serialization function.
Definition: GeneralSFMFactor.h:282
gtsam::NoiseModelFactor2< CAMERA, LANDMARK >::key1
Key key1() const
methods to retrieve both keys
Definition: NonlinearFactor.h:387
gtsam::GeneralSFMFactor2::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: GeneralSFMFactor.h:233
gtsam::GeneralSFMFactor::GeneralSFMFactor
GeneralSFMFactor(const Point2 &p)
constructor that takes a Point2
Definition: GeneralSFMFactor.h:93
gtsam::GeneralSFMFactor2::GeneralSFMFactor2
GeneralSFMFactor2(const Point2 &measured, const SharedNoiseModel &model, Key poseKey, Key landmarkKey, Key calibKey)
Constructor.
Definition: GeneralSFMFactor.h:226
types.h
Typedefs for easier changing of types.
Pose3.h
3D Pose
gtsam::FixedDimension
Give fixed size dimension of a type, fails at compile time if dynamic.
Definition: Manifold.h:164
gtsam::NoiseModelFactor3< Pose3, Point3, CALIBRATION >::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
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::GeneralSFMFactor2::measured
const Point2 measured() const
return the measured
Definition: GeneralSFMFactor.h:276
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:150
Vector.h
typedef and functions to augment Eigen's VectorXd
gtsam::GeneralSFMFactor2::evaluateError
Vector evaluateError(const Pose3 &pose3, const Point3 &point, const CALIBRATION &calib, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none, boost::optional< Matrix & > H3=boost::none) const override
h(x)-z
Definition: GeneralSFMFactor.h:256
gtsam::GeneralSFMFactor2::GeneralSFMFactor2
GeneralSFMFactor2()
default constructor
Definition: GeneralSFMFactor.h:228
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition: NonlinearFactor.h:43
NonlinearFactor.h
Non-linear factor base classes.
gtsam::GeneralSFMFactor::GeneralSFMFactor
GeneralSFMFactor(double x, double y)
constructor that takes doubles x,y to make a Point2
Definition: GeneralSFMFactor.h:94
Point3.h
3D Point
gtsam::NonlinearFactor::active
virtual bool active(const Values &) const
Checks whether a factor should be used based on a set of values.
Definition: NonlinearFactor.h:106
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
BinaryJacobianFactor.h
A binary JacobianFactor specialization that uses fixed matrix math for speed.
gtsam::Values::at
ValueType at(Key j) const
Retrieve a variable by key j.
Definition: Values-inl.h:342
gtsam::GeneralSFMFactor2
Non-linear factor for a constraint derived from a 2D measurement.
Definition: GeneralSFMFactor.h:200
gtsam::GeneralSFMFactor::measured
const Point2 measured() const
return the measured
Definition: GeneralSFMFactor.h:175
gtsam::GeneralSFMFactor2::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition: GeneralSFMFactor.h:250
gtsam::PinholeCamera
Definition: PinholeCamera.h:33
gtsam::GeneralSFMFactor2::measured_
Point2 measured_
the 2D measurement
Definition: GeneralSFMFactor.h:207
gtsam::Pose3
Definition: Pose3.h:37
gtsam::GeneralSFMFactor::linearize
boost::shared_ptr< GaussianFactor > linearize(const Values &values) const override
Linearize using fixed-size matrices.
Definition: GeneralSFMFactor.h:136
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
Matrix.h
typedef and functions to augment Eigen's MatrixXd
gtsam::NoiseModelFactor2
A convenient base class for creating your own NoiseModelFactor with 2 variables.
Definition: NonlinearFactor.h:355
Point2.h
2D Point
gtsam::GeneralSFMFactor::GeneralSFMFactor
GeneralSFMFactor(const Point2 &measured, const SharedNoiseModel &model, Key cameraKey, Key landmarkKey)
Constructor.
Definition: GeneralSFMFactor.h:89
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::GeneralSFMFactor::measured_
Point2 measured_
the 2D measurement
Definition: GeneralSFMFactor.h:72
SymmetricBlockMatrix.h
Access to matrices via blocks of pre-defined sizes.
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:71
Manifold.h
Base class and basic functions for Manifold types.
NoiseModel.h
gtsam::GeneralSFMFactor2::~GeneralSFMFactor2
virtual ~GeneralSFMFactor2()
destructor
Definition: GeneralSFMFactor.h:230
gtsam::GeneralSFMFactor::~GeneralSFMFactor
virtual ~GeneralSFMFactor()
destructor
Definition: GeneralSFMFactor.h:96
gtsam::GeneralSFMFactor::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition: GeneralSFMFactor.h:116
gtsam::GeneralSFMFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: GeneralSFMFactor.h:99
gtsam::GeneralSFMFactor::evaluateError
Vector evaluateError(const CAMERA &camera, const LANDMARK &point, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override
h(x)-z
Definition: GeneralSFMFactor.h:122
gtsam::GeneralSFMFactor::access
friend class boost::serialization::access
Serialization function.
Definition: GeneralSFMFactor.h:181