22#include <gtsam/geometry/EssentialMatrix.h>
42 typedef boost::shared_ptr<EssentialMatrixConstraint>
shared_ptr;
57 Base(model, key1, key2), measuredE_(measuredE) {
64 gtsam::NonlinearFactor::shared_ptr
clone()
const override {
65 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
66 gtsam::NonlinearFactor::shared_ptr(
new This(*
this)));
72 void print(
const std::string& s =
"",
73 const KeyFormatter& keyFormatter = DefaultKeyFormatter)
const override;
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;
93 friend class boost::serialization::access;
94 template<
class ARCHIVE>
95 void serialize(ARCHIVE & ar,
const unsigned int ) {
98 & boost::serialization::make_nvp(
"NoiseModelFactor2",
99 boost::serialization::base_object<Base>(*
this));
100 ar & BOOST_SERIALIZATION_NVP(measuredE_);
#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