TriangulationFactor.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 
18 #pragma once
19 
22 
23 namespace gtsam {
24 
30 template<class CAMERA>
31 class TriangulationFactor: public NoiseModelFactorN<Point3> {
32 
33 public:
34 
36  using Camera = CAMERA;
38  using Measurement = typename CAMERA::Measurement;
39 
40 protected:
41 
44 
47 
48  // Keep a copy of measurement and calibration for I/O
49  const CAMERA camera_;
51 
52  // verbosity handling for Cheirality Exceptions
53  const bool throwCheirality_;
54  const bool verboseCheirality_;
55 
56 public:
58 
60  using shared_ptr = std::shared_ptr<This>;
61 
62  // Provide access to the Matrix& version of evaluateError:
63  using NoiseModelFactor1<Point3>::evaluateError;
64 
67  throwCheirality_(false), verboseCheirality_(false) {
68  }
69 
80  const SharedNoiseModel& model, Key pointKey, bool throwCheirality = false,
81  bool verboseCheirality = false) :
85  throw std::invalid_argument(
86  "TriangulationFactor must be created with "
87  + std::to_string((int) traits<Measurement>::dimension)
88  + "-dimensional noise model.");
89  }
90 
92  ~TriangulationFactor() override {
93  }
94 
97  return std::static_pointer_cast<gtsam::NonlinearFactor>(
99  }
100 
106  void print(const std::string& s = "", const KeyFormatter& keyFormatter =
107  DefaultKeyFormatter) const override {
108  std::cout << s << "TriangulationFactor,";
109  camera_.print("camera");
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 && Base::equals(p, tol) && this->camera_.equals(e->camera_, tol)
118  && traits<Measurement>::Equals(this->measured_, e->measured_, tol);
119  }
120 
122  Vector evaluateError(const Point3& point, OptionalMatrixType H2) const override {
123  try {
125  } catch (CheiralityException& e) {
126  if (H2)
127  *H2 = Matrix::Zero(traits<Measurement>::dimension, 3);
128  if (verboseCheirality_)
129  std::cout << e.what() << ": Landmark "
130  << DefaultKeyFormatter(this->key()) << " moved behind camera"
131  << std::endl;
132  if (throwCheirality_)
133  throw e;
134  return camera_.defaultErrorWhenTriangulatingBehindCamera();
135  }
136  }
137 
140  mutable Matrix A;
141  mutable Vector b;
142 
148  std::shared_ptr<GaussianFactor> linearize(const Values& x) const override {
149  // Only linearize if the factor is active
150  if (!this->active(x))
151  return std::shared_ptr<JacobianFactor>();
152 
153  // Allocate memory for Jacobian factor, do only once
154  if (Ab.rows() == 0) {
155  std::vector<size_t> dimensions(1, 3);
159  }
160 
161  // Would be even better if we could pass blocks to project
162  const Point3& point = x.at<Point3>(key());
164  if (noiseModel_)
165  this->noiseModel_->WhitenSystem(A, b);
166 
167  Ab(0) = A;
168  Ab(1) = b;
169 
170  return std::make_shared<JacobianFactor>(this->keys_, Ab);
171  }
172 
174  const Measurement& measured() const {
175  return measured_;
176  }
177 
179  inline bool verboseCheirality() const {
180  return verboseCheirality_;
181  }
182 
184  inline bool throwCheirality() const {
185  return throwCheirality_;
186  }
187 
188 private:
189 
190 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
191  friend class boost::serialization::access;
193  template<class ARCHIVE>
194  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
195  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
196  ar & BOOST_SERIALIZATION_NVP(camera_);
197  ar & BOOST_SERIALIZATION_NVP(measured_);
198  ar & BOOST_SERIALIZATION_NVP(throwCheirality_);
199  ar & BOOST_SERIALIZATION_NVP(verboseCheirality_);
200  }
201 #endif
202 };
203 } // \ namespace gtsam
204 
gtsam::NonlinearFactor::shared_ptr
std::shared_ptr< This > shared_ptr
Definition: NonlinearFactor.h:78
gtsam::TriangulationFactor::TriangulationFactor
TriangulationFactor(const CAMERA &camera, const Measurement &measured, const SharedNoiseModel &model, Key pointKey, bool throwCheirality=false, bool verboseCheirality=false)
Definition: TriangulationFactor.h:79
dimensions
const std::vector< size_t > dimensions
Definition: testVerticalBlockMatrix.cpp:27
pointKey
const gtsam::Key pointKey
Definition: testRelativeElevationFactor.cpp:25
gtsam::TriangulationFactor::verboseCheirality
bool verboseCheirality() const
Definition: TriangulationFactor.h:179
gtsam::TriangulationFactor
Definition: TriangulationFactor.h:31
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
Camera
Definition: camera.h:36
gtsam::TriangulationFactor::throwCheirality_
const bool throwCheirality_
If true, rethrows Cheirality exceptions (default: false)
Definition: TriangulationFactor.h:53
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
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
gtsam::Factor
Definition: Factor.h:70
gtsam::NoiseModelFactor::noiseModel_
SharedNoiseModel noiseModel_
Definition: NonlinearFactor.h:205
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
CalibratedCamera.h
Calibrated camera for which only pose is unknown.
gtsam::TriangulationFactor::b
Vector b
Definition: TriangulationFactor.h:141
gtsam::TriangulationFactor::measured
const Measurement & measured() const
Definition: TriangulationFactor.h:174
gtsam::DefaultKeyFormatter
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
Definition: Key.cpp:30
gtsam::CheiralityException
Definition: CalibratedCamera.h:34
gtsam::TriangulationFactor::This
TriangulationFactor< CAMERA > This
shorthand for this class
Definition: TriangulationFactor.h:46
gtsam::TriangulationFactor::camera_
const CAMERA camera_
CAMERA in which this landmark was seen.
Definition: TriangulationFactor.h:49
gtsam::TriangulationFactor::evaluateError
Vector evaluateError(const Point3 &point, OptionalMatrixType H2) const override
Evaluate error h(x)-z and optionally derivatives.
Definition: TriangulationFactor.h:122
gtsam::TriangulationFactor::verboseCheirality_
const bool verboseCheirality_
If true, prints text for Cheirality exceptions (default: false)
Definition: TriangulationFactor.h:54
simulated2D::Measurement
GenericMeasurement< Point2, Point2 > Measurement
Definition: simulated2D.h:278
gtsam_unstable.tests.test_ProjectionFactorRollingShutter.point
point
Definition: test_ProjectionFactorRollingShutter.py:25
gtsam::TriangulationFactor::measured_
const Measurement measured_
2D measurement
Definition: TriangulationFactor.h:50
gtsam::TriangulationFactor::A
Matrix A
Definition: TriangulationFactor.h:140
gtsam::TriangulationFactor::TriangulationFactor
TriangulationFactor()
Default constructor.
Definition: TriangulationFactor.h:66
A
Definition: test_numpy_dtypes.cpp:298
gtsam::VerticalBlockMatrix
Definition: VerticalBlockMatrix.h:42
gtsam::TriangulationFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: TriangulationFactor.h:96
OptionalNone
#define OptionalNone
Definition: NonlinearFactor.h:49
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::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::TriangulationFactor::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition: TriangulationFactor.h:115
gtsam::NoiseModelFactor::equals
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
Definition: NonlinearFactor.cpp:80
gtsam::TriangulationFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Definition: TriangulationFactor.h:106
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
#define EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Definition: Memory.h:841
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
NonlinearFactor.h
Non-linear factor base classes.
gtsam::TriangulationFactor::linearize
std::shared_ptr< GaussianFactor > linearize(const Values &x) const override
Definition: TriangulationFactor.h:148
gtsam::TriangulationFactor::Measurement
typename CAMERA::Measurement Measurement
shorthand for measurement type, e.g. Point2 or StereoPoint2
Definition: TriangulationFactor.h:38
gtsam
traits
Definition: SFMdata.h:40
gtsam::Factor::keys_
KeyVector keys_
The keys involved in this factor.
Definition: Factor.h:88
gtsam::traits
Definition: Group.h:36
gtsam::TriangulationFactor::Ab
VerticalBlockMatrix Ab
thread-safe (?) scratch memory for linearize
Definition: TriangulationFactor.h:139
gtsam::Values
Definition: Values.h:65
gtsam::NonlinearFactor
Definition: NonlinearFactor.h:68
gtsam::TriangulationFactor::throwCheirality
bool throwCheirality() const
Definition: TriangulationFactor.h:184
p
float * p
Definition: Tutorial_Map_using.cpp:9
gtsam::Print
void Print(const CONTAINER &keys, const string &s, const KeyFormatter &keyFormatter)
Definition: Key.cpp:65
gtsam::VerticalBlockMatrix::rows
DenseIndex rows() const
Row size.
Definition: VerticalBlockMatrix.h:115
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
gtsam::TriangulationFactor::~TriangulationFactor
~TriangulationFactor() override
Definition: TriangulationFactor.h:92
gtsam::NoiseModelFactorN< Point3 >::key
Key key() const
Definition: NonlinearFactor.h:582
camera
static const CalibratedCamera camera(kDefaultPose)
Base
Definition: test_virtual_functions.cpp:156
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:42:54