gtsam 4.2.0
gtsam
Loading...
Searching...
No Matches
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
24namespace gtsam {
25
30class GTSAM_EXPORT EssentialMatrixConstraint: public NoiseModelFactorN<Pose3, Pose3> {
31
32private:
33
36
37 EssentialMatrix measuredE_;
39public:
40
41 // shorthand for a smart pointer to a factor
42 typedef boost::shared_ptr<EssentialMatrixConstraint> shared_ptr;
43
47
56 const EssentialMatrix& measuredE, const SharedNoiseModel& model) :
57 Base(model, key1, key2), measuredE_(measuredE) {
58 }
59
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
90private:
91
93 friend class boost::serialization::access;
94 template<class ARCHIVE>
95 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
96 // NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
97 ar
98 & boost::serialization::make_nvp("NoiseModelFactor2",
99 boost::serialization::base_object<Base>(*this));
100 ar & BOOST_SERIALIZATION_NVP(measuredE_);
101 }
102
103public:
105};
106// \class EssentialMatrixConstraint
107
108}
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
Non-linear factor base classes.
Global functions in a separate testing namespace.
Definition chartTesting.h:28
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
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
Template to create a binary predicate.
Definition Testable.h:111
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
A 3D pose (R,t) : (Rot3,Point3)
Definition Pose3.h:37
Nonlinear factor base class.
Definition NonlinearFactor.h:42
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition NonlinearFactor.h:400
Binary factor between two Pose3 variables induced by an EssentialMatrix measurement.
Definition EssentialMatrixConstraint.h:30
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition EssentialMatrixConstraint.h:64
const EssentialMatrix & measured() const
return the measured
Definition EssentialMatrixConstraint.h:86
boost::shared_ptr< EssentialMatrixConstraint > shared_ptr
The measurement is an essential matrix.
Definition EssentialMatrixConstraint.h:42
EssentialMatrixConstraint()
default constructor - only use for serialization
Definition EssentialMatrixConstraint.h:45
EssentialMatrixConstraint(Key key1, Key key2, const EssentialMatrix &measuredE, const SharedNoiseModel &model)
Constructor.
Definition EssentialMatrixConstraint.h:55