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 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
40 #include <boost/serialization/nvp.hpp>
41 #endif
42 #include <iostream>
43 #include <string>
44 
45 namespace boost {
46 namespace serialization {
47 class access;
48 } /* namespace serialization */
49 } /* namespace boost */
50 
51 namespace gtsam {
52 
58 template<class CAMERA, class LANDMARK>
59 class GeneralSFMFactor: public NoiseModelFactorN<CAMERA, LANDMARK> {
60 
63 
64  static const int DimC = FixedDimension<CAMERA>::value;
65  static const int DimL = FixedDimension<LANDMARK>::value;
66  typedef Eigen::Matrix<double, 2, DimC> JacobianC;
67  typedef Eigen::Matrix<double, 2, DimL> JacobianL;
68 
69 protected:
70 
72 
73 public:
74 
75  typedef GeneralSFMFactor<CAMERA, LANDMARK> This;
76  typedef NoiseModelFactorN<CAMERA, LANDMARK> Base;
77 
78  // Provide access to the Matrix& version of evaluateError:
79  using Base::evaluateError;
80 
81  // shorthand for a smart pointer to a factor
83 
92  Key cameraKey, Key landmarkKey)
93  : Base(model, cameraKey, landmarkKey), measured_(measured) {}
94 
95  GeneralSFMFactor() : measured_(0.0, 0.0) {}
96  GeneralSFMFactor(const Point2& p) : measured_(p) {}
99  GeneralSFMFactor(double x, double y) : measured_(x, y) {}
100 
101  ~GeneralSFMFactor() override {}
102 
104  gtsam::NonlinearFactor::shared_ptr clone() const override {
105  return std::static_pointer_cast<gtsam::NonlinearFactor>(
107 
113  void print(const std::string& s = "SFMFactor", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
114  Base::print(s, keyFormatter);
116  }
117 
121  bool equals(const NonlinearFactor &p, double tol = 1e-9) const override {
122  const This* e = dynamic_cast<const This*>(&p);
123  return e && Base::equals(p, tol) && traits<Point2>::Equals(this->measured_, e->measured_, tol);
124  }
125 
127  Vector evaluateError(const CAMERA& camera, const LANDMARK& point,
128  OptionalMatrixType H1, OptionalMatrixType H2) const override {
129  try {
130  return camera.project2(point,H1,H2) - measured_;
131  }
132  catch( CheiralityException& e [[maybe_unused]]) {
133  if (H1) *H1 = JacobianC::Zero();
134  if (H2) *H2 = JacobianL::Zero();
135  //TODO Print the exception via logging
136  return Z_2x1;
137  }
138  }
139 
141  std::shared_ptr<GaussianFactor> linearize(const Values& values) const override {
142  // Only linearize if the factor is active
143  if (!this->active(values)) return std::shared_ptr<JacobianFactor>();
144 
145  const Key key1 = this->key1(), key2 = this->key2();
146  JacobianC H1;
147  JacobianL H2;
148  Vector2 b;
149  try {
150  const CAMERA& camera = values.at<CAMERA>(key1);
151  const LANDMARK& point = values.at<LANDMARK>(key2);
152  b = measured() - camera.project2(point, H1, H2);
153  } catch (CheiralityException& e [[maybe_unused]]) {
154  H1.setZero();
155  H2.setZero();
156  b.setZero();
157  //TODO Print the exception via logging
158  }
159 
160  // Whiten the system if needed
161  const SharedNoiseModel& noiseModel = this->noiseModel();
162  if (noiseModel && !noiseModel->isUnit()) {
163  // TODO: implement WhitenSystem for fixed size matrices and include
164  // above
165  H1 = noiseModel->Whiten(H1);
166  H2 = noiseModel->Whiten(H2);
167  b = noiseModel->Whiten(b);
168  }
169 
170  // Create new (unit) noiseModel, preserving constraints if applicable
172  if (noiseModel && noiseModel->isConstrained()) {
173  model = std::static_pointer_cast<noiseModel::Constrained>(noiseModel)->unit();
174  }
175 
176  return std::make_shared<BinaryJacobianFactor<2, DimC, DimL> >(key1, H1, key2, H2, b, model);
177  }
178 
180  inline const Point2 measured() const {
181  return measured_;
182  }
183 
184 private:
185 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
186 
187  friend class boost::serialization::access;
188  template<class Archive>
189  void serialize(Archive & ar, const unsigned int /*version*/) {
190  // NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
191  ar & boost::serialization::make_nvp("NoiseModelFactor2",
192  boost::serialization::base_object<Base>(*this));
193  ar & BOOST_SERIALIZATION_NVP(measured_);
194  }
195 #endif
196 };
197 
198 template<class CAMERA, class LANDMARK>
199 struct traits<GeneralSFMFactor<CAMERA, LANDMARK> > : Testable<
200  GeneralSFMFactor<CAMERA, LANDMARK> > {
201 };
202 
207 template<class CALIBRATION>
208 class GeneralSFMFactor2: public NoiseModelFactorN<Pose3, Point3, CALIBRATION> {
209 
210  GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION)
211  static const int DimK = FixedDimension<CALIBRATION>::value;
212 
213 protected:
214 
216 
217 public:
218 
219  typedef GeneralSFMFactor2<CALIBRATION> This;
220  typedef PinholeCamera<CALIBRATION> Camera;
221  typedef NoiseModelFactorN<Pose3, Point3, CALIBRATION> Base;
222 
223  // shorthand for a smart pointer to a factor
225 
234  GeneralSFMFactor2(const Point2& measured, const SharedNoiseModel& model, Key poseKey, Key landmarkKey, Key calibKey) :
235  Base(model, poseKey, landmarkKey, calibKey), measured_(measured) {}
236  GeneralSFMFactor2():measured_(0.0,0.0) {}
237 
238  ~GeneralSFMFactor2() override {}
239 
242  return std::static_pointer_cast<gtsam::NonlinearFactor>(
244 
250  void print(const std::string& s = "SFMFactor2", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
251  Base::print(s, keyFormatter);
253  }
254 
258  bool equals(const NonlinearFactor &p, double tol = 1e-9) const override {
259  const This* e = dynamic_cast<const This*>(&p);
260  return e && Base::equals(p, tol) && traits<Point2>::Equals(this->measured_, e->measured_, tol);
261  }
262 
264  Vector evaluateError(const Pose3& pose3, const Point3& point, const CALIBRATION &calib,
265  OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3) const override {
266  try {
267  Camera camera(pose3,calib);
268  return camera.project(point, H1, H2, H3) - measured_;
269  }
270  catch( CheiralityException& e) {
271  if (H1) *H1 = Matrix::Zero(2, 6);
272  if (H2) *H2 = Matrix::Zero(2, 3);
273  if (H3) *H3 = Matrix::Zero(2, DimK);
274  std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2())
275  << " behind Camera " << DefaultKeyFormatter(this->key1()) << std::endl;
276  }
277  return Z_2x1;
278  }
279 
281  inline const Point2 measured() const {
282  return measured_;
283  }
284 
285 private:
286 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
287 
288  friend class boost::serialization::access;
289  template<class Archive>
290  void serialize(Archive & ar, const unsigned int /*version*/) {
291  // NoiseModelFactor3 instead of NoiseModelFactorN for backward compatibility
292  ar & boost::serialization::make_nvp("NoiseModelFactor3",
293  boost::serialization::base_object<Base>(*this));
294  ar & BOOST_SERIALIZATION_NVP(measured_);
295  }
296 #endif
297 };
298 
299 template<class CALIBRATION>
300 struct traits<GeneralSFMFactor2<CALIBRATION> > : Testable<
301  GeneralSFMFactor2<CALIBRATION> > {
302 };
303 
304 } //namespace
gtsam::GeneralSFMFactor::linearize
std::shared_ptr< GaussianFactor > linearize(const Values &values) const override
Linearize using fixed-size matrices.
Definition: GeneralSFMFactor.h:140
timing.h
Timing utilities.
gtsam::GeneralSFMFactor2::This
GeneralSFMFactor2< CALIBRATION > This
Definition: GeneralSFMFactor.h:219
gtsam::NonlinearFactor::shared_ptr
std::shared_ptr< This > shared_ptr
Definition: NonlinearFactor.h:78
Eigen
Namespace containing all symbols from the Eigen library.
Definition: jet.h:637
BinaryJacobianFactor.h
A binary JacobianFactor specialization that uses fixed matrix math for speed.
Vector.h
typedef and functions to augment Eigen's VectorXd
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::GeneralSFMFactor2::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: GeneralSFMFactor.h:241
Testable.h
Concept check for values that can be used in unit tests.
types.h
Typedefs for easier changing of types.
gtsam::GeneralSFMFactor::DimC
static const int DimC
Definition: GeneralSFMFactor.h:64
gtsam::GeneralSFMFactor2
Definition: GeneralSFMFactor.h:208
Matrix.h
typedef and functions to augment Eigen's MatrixXd
x
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
Definition: gnuplot_common_settings.hh:12
Values
gtsam::GeneralSFMFactor::This
GeneralSFMFactor< CAMERA, LANDMARK > This
typedef for this object
Definition: GeneralSFMFactor.h:75
pose3
static Pose3 pose3(rt3, pt3)
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
different_sigmas::values
HybridValues values
Definition: testHybridBayesNet.cpp:245
gtsam::Factor
Definition: Factor.h:70
concepts.h
Point3.h
3D Point
boost
Definition: boostmultiprec.cpp:109
gtsam::Factor::shared_ptr
std::shared_ptr< Factor > shared_ptr
A shared_ptr to this class.
Definition: Factor.h:76
gtsam::NonlinearFactor::active
virtual bool active(const Values &) const
Definition: NonlinearFactor.h:141
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
poseKey
const gtsam::Key poseKey
Definition: testPoseRotationPrior.cpp:29
Point2.h
2D Point
gtsam::DefaultKeyFormatter
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
Definition: Key.cpp:30
gtsam::CheiralityException
Definition: CalibratedCamera.h:34
gtsam::GeneralSFMFactor2::DimK
static const int DimK
Definition: GeneralSFMFactor.h:211
gtsam_unstable.tests.test_ProjectionFactorRollingShutter.point
point
Definition: test_ProjectionFactorRollingShutter.py:25
gtsam::GeneralSFMFactor2::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
Definition: GeneralSFMFactor.h:258
gtsam::GeneralSFMFactor::measured_
Point2 measured_
the 2D measurement
Definition: GeneralSFMFactor.h:71
gtsam::GeneralSFMFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: GeneralSFMFactor.h:103
gtsam::NoiseModelFactor::noiseModel
const SharedNoiseModel & noiseModel() const
access to the noise model
Definition: NonlinearFactor.h:245
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
SymmetricBlockMatrix.h
Access to matrices via blocks of pre-defined sizes. Used in GaussianFactor and GaussianConditional.
gtsam::GeneralSFMFactor::JacobianC
Eigen::Matrix< double, 2, DimC > JacobianC
Definition: GeneralSFMFactor.h:66
gtsam::PinholeCamera
Definition: PinholeCamera.h:33
gtsam::NoiseModelFactorN
Definition: NonlinearFactor.h:431
gtsam::NoiseModelFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Definition: NonlinearFactor.cpp:72
gtsam::SharedDiagonal
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:764
PinholeCamera.h
Base class for all pinhole cameras.
gtsam::GeneralSFMFactor2::evaluateError
Vector evaluateError(const Pose3 &pose3, const Point3 &point, const CALIBRATION &calib, OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3) const override
Definition: GeneralSFMFactor.h:264
gtsam::GeneralSFMFactor::evaluateError
Vector evaluateError(const CAMERA &camera, const LANDMARK &point, OptionalMatrixType H1, OptionalMatrixType H2) const override
Definition: GeneralSFMFactor.h:126
Manifold.h
Base class and basic functions for Manifold types.
gtsam::NoiseModelFactor::equals
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
Definition: NonlinearFactor.cpp:80
Vector2
Definition: test_operator_overloading.cpp:18
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
gtsam::GeneralSFMFactor::JacobianL
Eigen::Matrix< double, 2, DimL > JacobianL
Definition: GeneralSFMFactor.h:67
gtsam::NoiseModelFactorN< CAMERA, LANDMARK >::key1
Key key1() const
Definition: NonlinearFactor.h:731
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
gtsam::GeneralSFMFactor2::measured
const Point2 measured() const
Definition: GeneralSFMFactor.h:281
y
Scalar * y
Definition: level1_cplx_impl.h:124
NonlinearFactor.h
Non-linear factor base classes.
gtsam::b
const G & b
Definition: Group.h:79
gtsam::NoiseModelFactorN< CAMERA, LANDMARK >::key2
Key key2() const
Definition: NonlinearFactor.h:735
gtsam
traits
Definition: SFMdata.h:40
gtsam::Testable
Definition: Testable.h:152
gtsam::GeneralSFMFactor2::GeneralSFMFactor2
GeneralSFMFactor2(const Point2 &measured, const SharedNoiseModel &model, Key poseKey, Key landmarkKey, Key calibKey)
Definition: GeneralSFMFactor.h:234
NoiseModel.h
gtsam::traits
Definition: Group.h:36
NonlinearFactor
Continuous, differentiable manifold values for.
gtsam::GeneralSFMFactor::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
Definition: GeneralSFMFactor.h:120
std
Definition: BFloat16.h:88
gtsam::NonlinearFactor
Definition: NonlinearFactor.h:68
p
float * p
Definition: Tutorial_Map_using.cpp:9
gtsam::GeneralSFMFactor
Definition: GeneralSFMFactor.h:59
gtsam::Z_2x1
static const Eigen::MatrixBase< Vector2 >::ConstantReturnType Z_2x1
Definition: Vector.h:45
gtsam::Print
void Print(const CONTAINER &keys, const string &s, const KeyFormatter &keyFormatter)
Definition: Key.cpp:65
gtsam::GeneralSFMFactor2::print
void print(const std::string &s="SFMFactor2", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Definition: GeneralSFMFactor.h:250
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:55
Eigen::Matrix
The matrix class, also used for vectors and row-vectors.
Definition: 3rdparty/Eigen/Eigen/src/Core/Matrix.h:178
gtsam::FixedDimension
Give fixed size dimension of a type, fails at compile time if dynamic.
Definition: Manifold.h:161
camera
static const CalibratedCamera camera(kDefaultPose)
gtsam::GeneralSFMFactor2::measured_
Point2 measured_
the 2D measurement
Definition: GeneralSFMFactor.h:215
gtsam::GeneralSFMFactor::GeneralSFMFactor
GeneralSFMFactor(const Point2 &measured, const SharedNoiseModel &model, Key cameraKey, Key landmarkKey)
Definition: GeneralSFMFactor.h:91
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
GTSAM_CONCEPT_MANIFOLD_TYPE
#define GTSAM_CONCEPT_MANIFOLD_TYPE(T)
Definition: Manifold.h:178
gtsam::GeneralSFMFactor::measured
const Point2 measured() const
Definition: GeneralSFMFactor.h:179
gtsam::GeneralSFMFactor::print
void print(const std::string &s="SFMFactor", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Definition: GeneralSFMFactor.h:112
test_callbacks.value
value
Definition: test_callbacks.py:160
Pose3.h
3D Pose
gtsam::GeneralSFMFactor::DimL
static const int DimL
Definition: GeneralSFMFactor.h:65


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:32:36