gtsam  4.1.0
gtsam
EssentialMatrixConstraint.h
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010-2014, 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 
19 #pragma once
20 
22 #include <gtsam/geometry/EssentialMatrix.h>
23 
24 namespace gtsam {
25 
30 class GTSAM_EXPORT EssentialMatrixConstraint: public NoiseModelFactor2<Pose3, Pose3> {
31 
32 private:
33 
36 
37  EssentialMatrix measuredE_;
39 public:
40 
41  // shorthand for a smart pointer to a factor
42  typedef boost::shared_ptr<EssentialMatrixConstraint> shared_ptr;
43 
46  }
47 
56  const EssentialMatrix& measuredE, const SharedNoiseModel& model) :
57  Base(model, key1, key2), measuredE_(measuredE) {
58  }
59 
60  virtual ~EssentialMatrixConstraint() {
61  }
62 
64  gtsam::NonlinearFactor::shared_ptr clone() const override {
65  return boost::static_pointer_cast<gtsam::NonlinearFactor>(
66  gtsam::NonlinearFactor::shared_ptr(new This(*this)));
67  }
68 
72  void print(const std::string& s = "",
73  const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
74 
76  bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
77 
81  Vector evaluateError(const Pose3& p1, const Pose3& p2,
82  boost::optional<Matrix&> Hp1 = boost::none, //
83  boost::optional<Matrix&> Hp2 = boost::none) const override;
84 
86  const EssentialMatrix& measured() const {
87  return measuredE_;
88  }
89 
90 private:
91 
93  friend class boost::serialization::access;
94  template<class ARCHIVE>
95  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
96  ar
97  & boost::serialization::make_nvp("NoiseModelFactor2",
98  boost::serialization::base_object<Base>(*this));
99  ar & BOOST_SERIALIZATION_NVP(measuredE_);
100  }
101 
102 public:
104 };
105 // \class EssentialMatrixConstraint
106 
107 }
gtsam::equals
Template to create a binary predicate.
Definition: Testable.h:110
gtsam::EssentialMatrixConstraint::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: EssentialMatrixConstraint.h:64
gtsam::EssentialMatrixConstraint::EssentialMatrixConstraint
EssentialMatrixConstraint()
default constructor - only use for serialization
Definition: EssentialMatrixConstraint.h:45
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Note, deliberately not in noiseModel namespace.
Definition: NoiseModel.h:734
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::serialize
std::string serialize(const T &input)
serializes to a string
Definition: serialization.h:100
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:155
gtsam::EssentialMatrixConstraint
Definition: EssentialMatrixConstraint.h:30
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition: NonlinearFactor.h:43
NonlinearFactor.h
Non-linear factor base classes.
gtsam::EssentialMatrixConstraint::EssentialMatrixConstraint
EssentialMatrixConstraint(Key key1, Key key2, const EssentialMatrix &measuredE, const SharedNoiseModel &model)
Constructor.
Definition: EssentialMatrixConstraint.h:55
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition: types.h:269
gtsam::EssentialMatrix
An essential matrix is like a Pose3, except with translation up to scale It is named after the 3*3 ma...
Definition: EssentialMatrix.h:26
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::EssentialMatrixConstraint::measured
const EssentialMatrix & measured() const
return the measured
Definition: EssentialMatrixConstraint.h:86
gtsam::EssentialMatrixConstraint::shared_ptr
boost::shared_ptr< EssentialMatrixConstraint > shared_ptr
The measurement is an essential matrix.
Definition: EssentialMatrixConstraint.h:42