gtsam  4.1.0
gtsam
TriangulationFactor.h
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 
20 #include <boost/make_shared.hpp>
21 #include <boost/lexical_cast.hpp>
22 
23 namespace gtsam {
24 
30 template<class CAMERA>
31 class TriangulationFactor: public NoiseModelFactor1<Point3> {
32 
33 public:
34 
36  typedef CAMERA Camera;
37 
38 protected:
39 
42 
45 
47  typedef typename CAMERA::Measurement Measurement;
48 
49  // Keep a copy of measurement and calibration for I/O
50  const CAMERA camera_;
52 
53  // verbosity handling for Cheirality Exceptions
54  const bool throwCheirality_;
55  const bool verboseCheirality_;
56 
57 public:
58 
60  typedef boost::shared_ptr<This> shared_ptr;
61 
64  throwCheirality_(false), verboseCheirality_(false) {
65  }
66 
76  TriangulationFactor(const CAMERA& camera, const Measurement& measured,
77  const SharedNoiseModel& model, Key pointKey, bool throwCheirality = false,
78  bool verboseCheirality = false) :
79  Base(model, pointKey), camera_(camera), measured_(measured), throwCheirality_(
81  if (model && model->dim() != traits<Measurement>::dimension)
82  throw std::invalid_argument(
83  "TriangulationFactor must be created with "
84  + boost::lexical_cast<std::string>((int) traits<Measurement>::dimension)
85  + "-dimensional noise model.");
86  }
87 
90  }
91 
93  gtsam::NonlinearFactor::shared_ptr clone() const override {
94  return boost::static_pointer_cast<gtsam::NonlinearFactor>(
95  gtsam::NonlinearFactor::shared_ptr(new This(*this)));
96  }
97 
103  void print(const std::string& s = "", const KeyFormatter& keyFormatter =
104  DefaultKeyFormatter) const override {
105  std::cout << s << "TriangulationFactor,";
106  camera_.print("camera");
108  Base::print("", keyFormatter);
109  }
110 
112  bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
113  const This *e = dynamic_cast<const This*>(&p);
114  return e && Base::equals(p, tol) && this->camera_.equals(e->camera_, tol)
115  && traits<Measurement>::Equals(this->measured_, e->measured_, tol);
116  }
117 
119  Vector evaluateError(const Point3& point, boost::optional<Matrix&> H2 =
120  boost::none) const override {
121  try {
122  return traits<Measurement>::Local(measured_, camera_.project2(point, boost::none, H2));
123  } catch (CheiralityException& e) {
124  if (H2)
125  *H2 = Matrix::Zero(traits<Measurement>::dimension, 3);
126  if (verboseCheirality_)
127  std::cout << e.what() << ": Landmark "
128  << DefaultKeyFormatter(this->key()) << " moved behind camera"
129  << std::endl;
130  if (throwCheirality_)
131  throw e;
132  return Eigen::Matrix<double,traits<Measurement>::dimension,1>::Constant(2.0 * camera_.calibration().fx());
133  }
134  }
135 
138  mutable Matrix A;
139  mutable Vector b;
140 
146  boost::shared_ptr<GaussianFactor> linearize(const Values& x) const override {
147  // Only linearize if the factor is active
148  if (!this->active(x))
149  return boost::shared_ptr<JacobianFactor>();
150 
151  // Allocate memory for Jacobian factor, do only once
152  if (Ab.rows() == 0) {
153  std::vector<size_t> dimensions(1, 3);
155  A.resize(traits<Measurement>::dimension,3);
157  }
158 
159  // Would be even better if we could pass blocks to project
160  const Point3& point = x.at<Point3>(key());
161  b = traits<Measurement>::Local(camera_.project2(point, boost::none, A), measured_);
162  if (noiseModel_)
163  this->noiseModel_->WhitenSystem(A, b);
164 
165  Ab(0) = A;
166  Ab(1) = b;
167 
168  return boost::make_shared<JacobianFactor>(this->keys_, Ab);
169  }
170 
172  const Measurement& measured() const {
173  return measured_;
174  }
175 
177  inline bool verboseCheirality() const {
178  return verboseCheirality_;
179  }
180 
182  inline bool throwCheirality() const {
183  return throwCheirality_;
184  }
185 
186 private:
187 
190  template<class ARCHIVE>
191  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
192  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
193  ar & BOOST_SERIALIZATION_NVP(camera_);
194  ar & BOOST_SERIALIZATION_NVP(measured_);
195  ar & BOOST_SERIALIZATION_NVP(throwCheirality_);
196  ar & BOOST_SERIALIZATION_NVP(verboseCheirality_);
197  }
198 };
199 } // \ namespace gtsam
200 
gtsam::TriangulationFactor::Measurement
CAMERA::Measurement Measurement
shorthand for measurement type, e.g. Point2 or StereoPoint2
Definition: TriangulationFactor.h:47
gtsam::TriangulationFactor::Camera
CAMERA Camera
CAMERA type.
Definition: TriangulationFactor.h:36
gtsam::TriangulationFactor::Ab
VerticalBlockMatrix Ab
thread-safe (?) scratch memory for linearize
Definition: TriangulationFactor.h:137
gtsam::TriangulationFactor::TriangulationFactor
TriangulationFactor()
Default constructor.
Definition: TriangulationFactor.h:63
gtsam::VerticalBlockMatrix::rows
DenseIndex rows() const
Row size.
Definition: VerticalBlockMatrix.h:114
gtsam::TriangulationFactor
Definition: TriangulationFactor.h:31
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::NoiseModelFactor1
A convenient base class for creating your own NoiseModelFactor with 1 variable.
Definition: NonlinearFactor.h:271
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::TriangulationFactor::evaluateError
Vector evaluateError(const Point3 &point, boost::optional< Matrix & > H2=boost::none) const override
Evaluate error h(x)-z and optionally derivatives.
Definition: TriangulationFactor.h:119
gtsam::TriangulationFactor::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition: TriangulationFactor.h:112
gtsam::TriangulationFactor::throwCheirality
bool throwCheirality() const
return flag for throwing cheirality exceptions
Definition: TriangulationFactor.h:182
gtsam::TriangulationFactor::~TriangulationFactor
virtual ~TriangulationFactor()
Virtual destructor.
Definition: TriangulationFactor.h:89
gtsam::TriangulationFactor::throwCheirality_
const bool throwCheirality_
If true, rethrows Cheirality exceptions (default: false)
Definition: TriangulationFactor.h:54
gtsam::TriangulationFactor::verboseCheirality
bool verboseCheirality() const
return verbosity
Definition: TriangulationFactor.h:177
gtsam::Factor::keys_
KeyVector keys_
The keys involved in this factor.
Definition: Factor.h:72
gtsam::TriangulationFactor::camera_
const CAMERA camera_
CAMERA in which this landmark was seen.
Definition: TriangulationFactor.h:50
gtsam::TriangulationFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
Definition: TriangulationFactor.h:60
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::TriangulationFactor::measured_
const Measurement measured_
2D measurement
Definition: TriangulationFactor.h:51
gtsam::TriangulationFactor::Base
NoiseModelFactor1< Point3 > Base
shorthand for base class type
Definition: TriangulationFactor.h:41
gtsam::TriangulationFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: TriangulationFactor.h:103
gtsam::TriangulationFactor::access
friend class boost::serialization::access
Serialization function.
Definition: TriangulationFactor.h:189
gtsam::TriangulationFactor::measured
const Measurement & measured() const
return the measurement
Definition: TriangulationFactor.h:172
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition: NonlinearFactor.h:43
NonlinearFactor.h
Non-linear factor base classes.
gtsam::TriangulationFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: TriangulationFactor.h:93
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::Values::at
ValueType at(Key j) const
Retrieve a variable by key j.
Definition: Values-inl.h:342
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::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::TriangulationFactor::linearize
boost::shared_ptr< GaussianFactor > linearize(const Values &x) const override
Linearize to a JacobianFactor, does not support constrained noise model ! Hence .
Definition: TriangulationFactor.h:146
gtsam::TriangulationFactor::verboseCheirality_
const bool verboseCheirality_
If true, prints text for Cheirality exceptions (default: false)
Definition: TriangulationFactor.h:55
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:71
CalibratedCamera.h
Calibrated camera for which only pose is unknown.
gtsam::TriangulationFactor::TriangulationFactor
TriangulationFactor(const CAMERA &camera, const Measurement &measured, const SharedNoiseModel &model, Key pointKey, bool throwCheirality=false, bool verboseCheirality=false)
Constructor with exception-handling flags.
Definition: TriangulationFactor.h:76
gtsam::VerticalBlockMatrix
Definition: VerticalBlockMatrix.h:42
gtsam::TriangulationFactor::This
TriangulationFactor< CAMERA > This
shorthand for this class
Definition: TriangulationFactor.h:44