Loading...
Searching...
No Matches
EssentialMatrixConstraint.h
Go to the documentation of this file.
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010-2014, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
#pragma once
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/EssentialMatrix.h>
namespace gtsam {
class GTSAM_EXPORT EssentialMatrixConstraint: public NoiseModelFactorN<Pose3, Pose3> {
private:
typedef EssentialMatrixConstraint This;
typedef NoiseModelFactorN<Pose3, Pose3> Base;
EssentialMatrix measuredE_;
public:
// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<EssentialMatrixConstraint> shared_ptr;
EssentialMatrixConstraint() {
}
EssentialMatrixConstraint(Key key1, Key key2,
const EssentialMatrix& measuredE, const SharedNoiseModel& model) :
Base(model, key1, key2), measuredE_(measuredE) {
}
~EssentialMatrixConstraint() override {
}
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
Vector evaluateError(const Pose3& p1, const Pose3& p2,
boost::optional<Matrix&> Hp1 = boost::none, //
boost::optional<Matrix&> Hp2 = boost::none) const override;
const EssentialMatrix& measured() const {
return measuredE_;
}
private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
// NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
ar
& boost::serialization::make_nvp("NoiseModelFactor2",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(measuredE_);
}
public:
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
// \class EssentialMatrixConstraint
}
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
NonlinearFactor.h
Non-linear factor base classes.
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:156
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::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
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
A 3D pose (R,t) : (Rot3,Point3).
Definition Pose3.h:37
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
gtsam::NoiseModelFactorN< Pose3, Pose3 >::NoiseModelFactorN
NoiseModelFactorN()
Definition NonlinearFactor.h:469
gtsam::EssentialMatrixConstraint
Binary factor between two Pose3 variables induced by an EssentialMatrix measurement.
Definition EssentialMatrixConstraint.h:30
gtsam::EssentialMatrixConstraint::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition EssentialMatrixConstraint.h:64
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
gtsam::EssentialMatrixConstraint::EssentialMatrixConstraint
EssentialMatrixConstraint()
default constructor - only use for serialization
Definition EssentialMatrixConstraint.h:45
gtsam::EssentialMatrixConstraint::EssentialMatrixConstraint
EssentialMatrixConstraint(Key key1, Key key2, const EssentialMatrix &measuredE, const SharedNoiseModel &model)
Constructor.
Definition EssentialMatrixConstraint.h:55