#include <NonlinearFactor.h>
Inherits gtsam::NonlinearFactor.
Inherited by gtsam::CustomFactor, gtsam::MultiProjectionFactor< POSE, LANDMARK, CALIBRATION >, gtsam::NoiseModelFactorN< ValueTypes >, gtsam::SmartRangeFactor, gtsam::NoiseModelFactorN< BASIS::Parameters >, gtsam::NoiseModelFactorN< CAMERA, LANDMARK >, gtsam::NoiseModelFactorN< double >, gtsam::NoiseModelFactorN< double, double >, gtsam::NoiseModelFactorN< double, double, double >, gtsam::NoiseModelFactorN< double, double, double, double >, gtsam::NoiseModelFactorN< double, Unit3, Point3 >, gtsam::NoiseModelFactorN< EssentialMatrix >, gtsam::NoiseModelFactorN< EssentialMatrix, CALIBRATION >, gtsam::NoiseModelFactorN< EssentialMatrix, double >, gtsam::NoiseModelFactorN< F, F >, gtsam::NoiseModelFactorN< Matrix >, gtsam::NoiseModelFactorN< NavState >, gtsam::NoiseModelFactorN< NavState, NavState >, gtsam::NoiseModelFactorN< NavState, NavState, imuBias::ConstantBias >, gtsam::NoiseModelFactorN< OrientedPlane3 >, gtsam::NoiseModelFactorN< POINT, TRANSFORM, POINT >, gtsam::NoiseModelFactorN< Point2 >, gtsam::NoiseModelFactorN< Point2, Point2 >, gtsam::NoiseModelFactorN< Point3 >, gtsam::NoiseModelFactorN< Point3, Point3 >, gtsam::NoiseModelFactorN< Point3, Point3, Vector1 >, gtsam::NoiseModelFactorN< POSE >, gtsam::NoiseModelFactorN< POSE, LANDMARK >, gtsam::NoiseModelFactorN< POSE, LANDMARK, INVDEPTH >, gtsam::NoiseModelFactorN< POSE, POINT >, gtsam::NoiseModelFactorN< POSE, POSE >, gtsam::NoiseModelFactorN< POSE, POSE, LANDMARK >, gtsam::NoiseModelFactorN< POSE, POSE, LANDMARK, Cal3_S2 >, gtsam::NoiseModelFactorN< POSE, VELOCITY, IMUBIAS, POSE, VELOCITY >, gtsam::NoiseModelFactorN< POSE, VELOCITY, POSE, VELOCITY >, gtsam::NoiseModelFactorN< Pose2 >, gtsam::NoiseModelFactorN< Pose2, Point2 >, gtsam::NoiseModelFactorN< Pose2, Pose2 >, gtsam::NoiseModelFactorN< Pose2, Pose2, Pose2, Point2 >, gtsam::NoiseModelFactorN< Pose2, Pose2, Pose2, Pose2 >, gtsam::NoiseModelFactorN< Pose3 >, gtsam::NoiseModelFactorN< Pose3, double >, gtsam::NoiseModelFactorN< Pose3, OrientedPlane3 >, gtsam::NoiseModelFactorN< Pose3, Point3 >, gtsam::NoiseModelFactorN< Pose3, Point3, CALIBRATION >, gtsam::NoiseModelFactorN< Pose3, Pose3 >, gtsam::NoiseModelFactorN< Pose3, Pose3, OrientedPlane3 >, gtsam::NoiseModelFactorN< Pose3, Pose3, Point3 >, gtsam::NoiseModelFactorN< Pose3, Pose3, Vector3 >, gtsam::NoiseModelFactorN< Pose3, Pose3, Vector6 >, gtsam::NoiseModelFactorN< Pose3, Vector3 >, gtsam::NoiseModelFactorN< Pose3, Vector3, Pose3, Vector3, imuBias::ConstantBias >, gtsam::NoiseModelFactorN< Pose3, Vector3, Pose3, Vector3, imuBias::ConstantBias, imuBias::ConstantBias >, gtsam::NoiseModelFactorN< Pose3, Vector6 >, gtsam::NoiseModelFactorN< PoseRTV >, gtsam::NoiseModelFactorN< PoseRTV, PoseRTV >, gtsam::NoiseModelFactorN< Rot >, gtsam::NoiseModelFactorN< Rot, Rot >, gtsam::NoiseModelFactorN< Rot2 >, gtsam::NoiseModelFactorN< Rot3 >, gtsam::NoiseModelFactorN< Rot3, Rot3, Vector3 >, gtsam::NoiseModelFactorN< SOn, SOn >, gtsam::NoiseModelFactorN< T >, gtsam::NoiseModelFactorN< T, T >, gtsam::NoiseModelFactorN< T1, T2 >, gtsam::NoiseModelFactorN< VALUE >, gtsam::NoiseModelFactorN< VALUE, VALUE >, gtsam::NoiseModelFactorN< VALUE1, VALUE2 >, gtsam::NoiseModelFactorN< Vector >, gtsam::NoiseModelFactorN< Vector2 >, and gtsam::NoiseModelFactorN< Vector6, Vector6, Pose3 >.
Public Types | |
typedef std::shared_ptr< This > | shared_ptr |
Public Types inherited from gtsam::NonlinearFactor | |
typedef std::shared_ptr< This > | shared_ptr |
Public Types inherited from gtsam::Factor | |
typedef KeyVector::const_iterator | const_iterator |
Const iterator over keys. More... | |
typedef KeyVector::iterator | iterator |
Iterator over keys. More... | |
Public Member Functions | |
shared_ptr | cloneWithNewNoiseModel (const SharedNoiseModel newNoise) const |
size_t | dim () const override |
bool | equals (const NonlinearFactor &f, double tol=1e-9) const override |
double | error (const Values &c) const override |
std::shared_ptr< GaussianFactor > | linearize (const Values &x) const override |
const SharedNoiseModel & | noiseModel () const |
access to the noise model More... | |
NoiseModelFactor () | |
template<typename CONTAINER > | |
NoiseModelFactor (const SharedNoiseModel &noiseModel, const CONTAINER &keys) | |
void | print (const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override |
Vector | unweightedWhitenedError (const Values &c) const |
virtual Vector | unwhitenedError (const Values &x, OptionalMatrixVecType H=nullptr) const =0 |
Vector | unwhitenedError (const Values &x, std::vector< Matrix > &H) const |
double | weight (const Values &c) const |
Vector | whitenedError (const Values &c) const |
~NoiseModelFactor () override | |
Public Member Functions inherited from gtsam::NonlinearFactor | |
NonlinearFactor () | |
template<typename CONTAINER > | |
NonlinearFactor (const CONTAINER &keys) | |
double | error (const HybridValues &c) const override |
virtual bool | active (const Values &) const |
virtual shared_ptr | clone () const |
virtual shared_ptr | rekey (const std::map< Key, Key > &rekey_mapping) const |
virtual shared_ptr | rekey (const KeyVector &new_keys) const |
virtual bool | sendable () const |
Public Member Functions inherited from gtsam::Factor | |
virtual | ~Factor ()=default |
Default destructor. More... | |
bool | empty () const |
Whether the factor is empty (involves zero variables). More... | |
Key | front () const |
First key. More... | |
Key | back () const |
Last key. More... | |
const_iterator | find (Key key) const |
find More... | |
const KeyVector & | keys () const |
Access the factor's involved variable keys. More... | |
const_iterator | begin () const |
const_iterator | end () const |
size_t | size () const |
virtual void | printKeys (const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const |
print only keys More... | |
bool | equals (const This &other, double tol=1e-9) const |
check equality More... | |
KeyVector & | keys () |
iterator | begin () |
iterator | end () |
Protected Types | |
typedef NonlinearFactor | Base |
typedef NoiseModelFactor | This |
Protected Types inherited from gtsam::NonlinearFactor | |
typedef Factor | Base |
typedef NonlinearFactor | This |
Protected Member Functions | |
NoiseModelFactor (const SharedNoiseModel &noiseModel) | |
Protected Member Functions inherited from gtsam::Factor | |
Factor () | |
template<typename CONTAINER > | |
Factor (const CONTAINER &keys) | |
template<typename ITERATOR > | |
Factor (ITERATOR first, ITERATOR last) | |
Protected Attributes | |
SharedNoiseModel | noiseModel_ |
Protected Attributes inherited from gtsam::Factor | |
KeyVector | keys_ |
The keys involved in this factor. More... | |
Additional Inherited Members | |
Static Protected Member Functions inherited from gtsam::Factor | |
template<typename CONTAINER > | |
static Factor | FromKeys (const CONTAINER &keys) |
template<typename ITERATOR > | |
static Factor | FromIterators (ITERATOR first, ITERATOR last) |
A nonlinear sum-of-squares factor with a zero-mean noise model implementing the density Templated on the parameter type X and the values structure Values There is no return type specified for h(x). Instead, we require the derived class implements This allows a graph to have factors with measurements of mixed type.
The noise model is typically Gaussian, but robust and constrained error models are also supported.
Definition at line 197 of file NonlinearFactor.h.
|
protected |
Definition at line 202 of file NonlinearFactor.h.
typedef std::shared_ptr<This> gtsam::NoiseModelFactor::shared_ptr |
Noise model
Definition at line 209 of file NonlinearFactor.h.
|
protected |
Definition at line 203 of file NonlinearFactor.h.
|
inline |
Default constructor for I/O only
Definition at line 212 of file NonlinearFactor.h.
|
inlineoverride |
Destructor
Definition at line 215 of file NonlinearFactor.h.
|
inline |
Constructor
Definition at line 221 of file NonlinearFactor.h.
|
inlineprotected |
Constructor - only for subclasses, as this does not set keys.
Definition at line 229 of file NonlinearFactor.h.
NoiseModelFactor::shared_ptr NoiseModelFactor::cloneWithNewNoiseModel | ( | const SharedNoiseModel | newNoise | ) | const |
Creates a shared_ptr clone of the factor with a new noise model
Definition at line 89 of file NonlinearFactor.cpp.
|
inlineoverridevirtual |
get the dimension of the factor (number of rows on linearization)
Implements gtsam::NonlinearFactor.
Reimplemented in NonlinearMeasurementModel, and NonlinearMotionModel.
Definition at line 240 of file NonlinearFactor.h.
|
overridevirtual |
Check if two factors are equal
Reimplemented from gtsam::NonlinearFactor.
Reimplemented in gtsam::GeneralSFMFactor2< CALIBRATION >, gtsam::InvDepthFactorVariant3b, gtsam::ProjectionFactorRollingShutter, gtsam::GenericProjectionFactor< POSE, LANDMARK, CALIBRATION >, gtsam::MultiProjectionFactor< POSE, LANDMARK, CALIBRATION >, gtsam::GeneralSFMFactor< CAMERA, LANDMARK >, gtsam::ProjectionFactorPPP< POSE, LANDMARK, CALIBRATION >, gtsam::TriangulationFactor< CAMERA >, gtsam::ProjectionFactorPPPC< POSE, LANDMARK, CALIBRATION >, gtsam::InvDepthFactor3< POSE, LANDMARK, INVDEPTH >, gtsam::InvDepthFactorVariant2, gtsam::InvDepthFactorVariant3a, gtsam::InvDepthFactorVariant1, gtsam::FunctorizedFactor< T, Matrix >, gtsam::FunctorizedFactor< Vector, Matrix >, gtsam::FunctorizedFactor< double, BASIS::Parameters >, gtsam::FunctorizedFactor< double, Vector >, gtsam::FunctorizedFactor< double, Matrix >, NonlinearMeasurementModel, NonlinearMotionModel, gtsam::GenericStereoFactor< POSE, LANDMARK >, gtsam::SmartRangeFactor, gtsam::EquivInertialNavFactor_GlobalVel_NoBias< POSE, VELOCITY >, gtsam::EquivInertialNavFactor_GlobalVel< POSE, VELOCITY, IMUBIAS >, gtsam::FrobeniusBetweenFactor< Rot >, gtsam::InertialNavFactor_GlobalVelocity< POSE, VELOCITY, IMUBIAS >, gtsam::PartialPriorFactor< VALUE >, gtsam::PartialPriorFactor< PoseRTV >, gtsam::BetweenFactor< VALUE >, gtsam::PriorFactor< VALUE >, gtsam::GaussMarkov1stOrderFactor< VALUE >, gtsam::PoseBetweenFactor< POSE >, gtsam::OrientedPlane3DirectionPrior, gtsam::PoseTranslationPrior< POSE >, gtsam::EssentialMatrixConstraint, gtsam::PosePriorFactor< POSE >, gtsam::BiasedGPSFactor, gtsam::ShonanFactor< d >, gtsam::PoseRotationPrior< POSE >, gtsam::PoseToPointFactor< POSE, POINT >, gtsam::RelativeElevationFactor, gtsam::FullIMUFactor< POSE >, and gtsam::IMUFactor< POSE >.
Definition at line 80 of file NonlinearFactor.cpp.
|
overridevirtual |
Calculate the error of the factor. This is the log-likelihood, e.g. in case of Gaussian. In this class, we take the raw prediction error , ask the noise model to transform it to , and then multiply by 0.5.
Reimplemented from gtsam::NonlinearFactor.
Reimplemented in NonlinearMeasurementModel, and NonlinearMotionModel.
Definition at line 136 of file NonlinearFactor.cpp.
|
overridevirtual |
Linearize a non-linearFactorN to get a GaussianFactor, Hence
Implements gtsam::NonlinearFactor.
Reimplemented in gtsam::TriangulationFactor< CAMERA >, gtsam::GeneralSFMFactor< CAMERA, LANDMARK >, NonlinearMeasurementModel, and NonlinearMotionModel.
Definition at line 150 of file NonlinearFactor.cpp.
|
inline |
access to the noise model
Definition at line 245 of file NonlinearFactor.h.
|
overridevirtual |
Reimplemented from gtsam::NonlinearFactor.
Reimplemented in gtsam::GeneralSFMFactor2< CALIBRATION >, gtsam::GeneralSFMFactor< CAMERA, LANDMARK >, gtsam::OrientedPlane3Factor, gtsam::OrientedPlane3DirectionPrior, gtsam::LocalOrientedPlane3Factor, gtsam::InvDepthFactorVariant3a, gtsam::InvDepthFactorVariant3b, gtsam::InvDepthFactorVariant2, gtsam::InvDepthFactorVariant1, gtsam::InvDepthFactor3< POSE, LANDMARK, INVDEPTH >, gtsam::InertialNavFactor_GlobalVelocity< POSE, VELOCITY, IMUBIAS >, gtsam::EquivInertialNavFactor_GlobalVel_NoBias< POSE, VELOCITY >, gtsam::EquivInertialNavFactor_GlobalVel< POSE, VELOCITY, IMUBIAS >, gtsam::EssentialMatrixFactor4< CALIBRATION >, NonlinearMeasurementModel, gtsam::EssentialMatrixFactor3, gtsam::EssentialMatrixFactor2, NonlinearMotionModel, gtsam::ProjectionFactorRollingShutter, gtsam::GenericProjectionFactor< POSE, LANDMARK, CALIBRATION >, gtsam::MultiProjectionFactor< POSE, LANDMARK, CALIBRATION >, gtsam::ProjectionFactorPPP< POSE, LANDMARK, CALIBRATION >, gtsam::GenericStereoFactor< POSE, LANDMARK >, gtsam::TriangulationFactor< CAMERA >, gtsam::FunctorizedFactor< T, Matrix >, gtsam::FunctorizedFactor< Vector, Matrix >, gtsam::FunctorizedFactor< double, BASIS::Parameters >, gtsam::FunctorizedFactor< double, Vector >, gtsam::FunctorizedFactor< double, Matrix >, gtsam::RotateDirectionsFactor, gtsam::ProjectionFactorPPPC< POSE, LANDMARK, CALIBRATION >, gtsam::BetweenFactor< VALUE >, gtsam::EssentialMatrixFactor, gtsam::PoseTranslationPrior< POSE >, gtsam::SmartRangeFactor, gtsam::EssentialMatrixConstraint, gtsam::PoseRotationPrior< POSE >, gtsam::RelativeElevationFactor, gtsam::RotateFactor, gtsam::ReferenceFrameFactor< POINT, TRANSFORM >, gtsam::VelocityConstraint, gtsam::FullIMUFactor< POSE >, gtsam::IMUFactor< POSE >, gtsam::FrobeniusBetweenFactor< Rot >, gtsam::PartialPriorFactor< VALUE >, gtsam::PartialPriorFactor< PoseRTV >, gtsam::PriorFactor< VALUE >, gtsam::GaussMarkov1stOrderFactor< VALUE >, gtsam::PoseBetweenFactor< POSE >, gtsam::PosePriorFactor< POSE >, gtsam::ShonanFactor< d >, gtsam::BiasedGPSFactor, and gtsam::PoseToPointFactor< POSE, POINT >.
Definition at line 72 of file NonlinearFactor.cpp.
Vector of errors, whitened, but unweighted by any loss function
Definition at line 114 of file NonlinearFactor.cpp.
|
pure virtual |
Error function without the NoiseModel, . Override this method to finish implementing an N-way factor. If the optional arguments is specified, it should compute both the function evaluation and its derivative(s) in H.
Implemented in gtsam::NoiseModelFactorN< ValueTypes >, gtsam::NoiseModelFactorN< Pose3, Pose3, Point3 >, gtsam::NoiseModelFactorN< Pose2 >, gtsam::NoiseModelFactorN< EssentialMatrix >, gtsam::NoiseModelFactorN< Pose3, Point3, CALIBRATION >, gtsam::NoiseModelFactorN< Pose3 >, gtsam::NoiseModelFactorN< F, F >, gtsam::NoiseModelFactorN< VALUE1, VALUE2 >, gtsam::NoiseModelFactorN< double >, gtsam::NoiseModelFactorN< NavState, NavState, imuBias::ConstantBias >, gtsam::NoiseModelFactorN< Pose3, Vector3, Pose3, Vector3, imuBias::ConstantBias >, gtsam::NoiseModelFactorN< POSE >, gtsam::NoiseModelFactorN< POSE, LANDMARK >, gtsam::NoiseModelFactorN< double, double, double, double >, gtsam::NoiseModelFactorN< Rot3 >, gtsam::NoiseModelFactorN< POSE, POSE, LANDMARK >, gtsam::NoiseModelFactorN< Pose2, Pose2, Pose2, Pose2 >, gtsam::NoiseModelFactorN< Rot2 >, gtsam::NoiseModelFactorN< Point3, Point3 >, gtsam::NoiseModelFactorN< Pose3, Vector6 >, gtsam::NoiseModelFactorN< Vector >, gtsam::NoiseModelFactorN< Point2, Point2 >, gtsam::NoiseModelFactorN< EssentialMatrix, double >, gtsam::NoiseModelFactorN< PoseRTV, PoseRTV >, gtsam::NoiseModelFactorN< PoseRTV >, gtsam::NoiseModelFactorN< double, Unit3, Point3 >, gtsam::NoiseModelFactorN< Vector2 >, gtsam::NoiseModelFactorN< T >, gtsam::NoiseModelFactorN< EssentialMatrix, CALIBRATION >, gtsam::NoiseModelFactorN< POINT, TRANSFORM, POINT >, gtsam::NoiseModelFactorN< Pose3, Pose3, Vector3 >, gtsam::NoiseModelFactorN< Pose3, Vector3, Pose3, Vector3, imuBias::ConstantBias, imuBias::ConstantBias >, gtsam::NoiseModelFactorN< Matrix >, gtsam::NoiseModelFactorN< Rot3, Rot3, Vector3 >, gtsam::NoiseModelFactorN< SOn, SOn >, gtsam::NoiseModelFactorN< NavState >, gtsam::NoiseModelFactorN< double, double >, gtsam::NoiseModelFactorN< POSE, POSE, LANDMARK, Cal3_S2 >, gtsam::NoiseModelFactorN< Pose3, OrientedPlane3 >, gtsam::NoiseModelFactorN< Rot, Rot >, gtsam::NoiseModelFactorN< BASIS::Parameters >, gtsam::NoiseModelFactorN< Pose2, Point2 >, gtsam::NoiseModelFactorN< Pose3, Pose3, Vector6 >, gtsam::NoiseModelFactorN< Pose3, Point3 >, gtsam::NoiseModelFactorN< Point3, Point3, Vector1 >, gtsam::NoiseModelFactorN< Vector6, Vector6, Pose3 >, gtsam::NoiseModelFactorN< Pose3, Pose3 >, gtsam::NoiseModelFactorN< POSE, POSE >, gtsam::NoiseModelFactorN< VALUE >, gtsam::NoiseModelFactorN< Pose2, Pose2, Pose2, Point2 >, gtsam::NoiseModelFactorN< POSE, LANDMARK, INVDEPTH >, gtsam::NoiseModelFactorN< Point3 >, gtsam::NoiseModelFactorN< CAMERA, LANDMARK >, gtsam::NoiseModelFactorN< NavState, NavState >, gtsam::NoiseModelFactorN< OrientedPlane3 >, gtsam::NoiseModelFactorN< Pose2, Pose2 >, gtsam::NoiseModelFactorN< Point2 >, gtsam::NoiseModelFactorN< T1, T2 >, gtsam::NoiseModelFactorN< T, T >, gtsam::NoiseModelFactorN< double, double, double >, gtsam::NoiseModelFactorN< Rot >, gtsam::NoiseModelFactorN< VALUE, VALUE >, gtsam::NoiseModelFactorN< POSE, VELOCITY, IMUBIAS, POSE, VELOCITY >, gtsam::NoiseModelFactorN< POSE, VELOCITY, POSE, VELOCITY >, gtsam::NoiseModelFactorN< Pose3, Vector3 >, gtsam::NoiseModelFactorN< Pose3, double >, gtsam::NoiseModelFactorN< Pose3, Pose3, OrientedPlane3 >, gtsam::NoiseModelFactorN< POSE, POINT >, gtsam::SmartRangeFactor, gtsam::MultiProjectionFactor< POSE, LANDMARK, CALIBRATION >, and gtsam::CustomFactor.
|
inline |
support taking in the actual vector instead of the pointer as well to get access to this version of the function from derived classes one will need to use the "using" keyword and specify that like this: public: using NoiseModelFactor::unwhitenedError;
Definition at line 263 of file NonlinearFactor.h.
double NoiseModelFactor::weight | ( | const Values & | c | ) | const |
Compute the effective weight of the factor from the noise model.
Definition at line 121 of file NonlinearFactor.cpp.
Vector of errors, whitened This is the raw error, i.e., i.e. in case of a Gaussian
Definition at line 107 of file NonlinearFactor.cpp.
|
protected |
Definition at line 205 of file NonlinearFactor.h.