|
Vector | evaluateError (const Pose3 &wTwi, const Pose3 &wTwa, const OrientedPlane3 &a_plane, OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3) const override |
|
| LocalOrientedPlane3Factor () |
| Constructor. More...
|
|
| LocalOrientedPlane3Factor (const OrientedPlane3 &z, const SharedNoiseModel &noiseModel, Key poseKey, Key anchorPoseKey, Key landmarkKey) |
|
| LocalOrientedPlane3Factor (const Vector4 &z, const SharedNoiseModel &noiseModel, Key poseKey, Key anchorPoseKey, Key landmarkKey) |
|
void | print (const std::string &s="LocalOrientedPlane3Factor", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override |
| print More...
|
|
| ~LocalOrientedPlane3Factor () override |
|
Key | key () const |
|
virtual Vector | unwhitenedError (const Values &x, OptionalMatrixVecType H=nullptr) const =0 |
|
Vector | unwhitenedError (const Values &x, std::vector< Matrix > &H) const |
|
| ~NoiseModelFactorN () override |
|
| NoiseModelFactorN () |
| Default Constructor for I/O. More...
|
|
| NoiseModelFactorN (const SharedNoiseModel &noiseModel, KeyType< ValueTypes >... keys) |
|
| NoiseModelFactorN (const SharedNoiseModel &noiseModel, CONTAINER keys) |
|
Vector | unwhitenedError (const Values &x, OptionalMatrixVecType H=nullptr) const override |
|
virtual Vector | evaluateError (const ValueTypes &... x, OptionalMatrixTypeT< ValueTypes >... H) const=0 |
|
Vector | evaluateError (const ValueTypes &... x, MatrixTypeT< ValueTypes > &... H) const |
|
Vector | evaluateError (const ValueTypes &... x) const |
|
AreAllMatrixRefs< Vector, OptionalJacArgs... > | evaluateError (const ValueTypes &... x, OptionalJacArgs &&... H) const |
|
AreAllMatrixPtrs< Vector, OptionalJacArgs... > | evaluateError (const ValueTypes &... x, OptionalJacArgs &&... H) const |
|
Key | key1 () const |
|
Key | key2 () const |
|
Key | key3 () const |
|
Key | key4 () const |
|
Key | key5 () const |
|
Key | key6 () const |
|
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) |
|
Vector | unweightedWhitenedError (const Values &c) const |
|
Vector | unwhitenedError (const Values &x, std::vector< Matrix > &H) const |
|
double | weight (const Values &c) const |
|
Vector | whitenedError (const Values &c) const |
|
| ~NoiseModelFactor () override |
|
| 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 |
|
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 () |
|
|
typedef NoiseModelFactorN< Pose3, Pose3, OrientedPlane3 > | Base |
|
using | Base = NoiseModelFactor |
|
using | KeyType = Key |
|
using | MatrixTypeT = Matrix |
|
using | OptionalMatrixTypeT = Matrix * |
|
using | This = NoiseModelFactorN< ValueTypes... > |
|
using | IsConvertible = typename std::enable_if< std::is_convertible< From, To >::value, void >::type |
|
using | IndexIsValid = typename std::enable_if<(I >=1) &&(I<=N), void >::type |
|
using | ContainerElementType = typename std::decay< decltype(*std::declval< Container >().begin())>::type |
|
using | IsContainerOfKeys = IsConvertible< ContainerElementType< Container >, Key > |
|
using | AreAllMatrixRefs = std::enable_if_t<(... &&std::is_convertible< Args, Matrix & >::value), Ret > |
|
using | IsMatrixPointer = std::is_same< typename std::decay_t< Arg >, Matrix * > |
|
using | IsNullpointer = std::is_same< typename std::decay_t< Arg >, std::nullptr_t > |
|
using | AreAllMatrixPtrs = std::enable_if_t<(... &&(IsMatrixPointer< Args >::value||IsNullpointer< Args >::value)), Ret > |
|
typedef NonlinearFactor | Base |
|
typedef NoiseModelFactor | This |
|
typedef Factor | Base |
|
typedef NonlinearFactor | This |
|
Factor to measure a planar landmark from a given pose, with a given local linearization point.
This factor is based on the relative plane factor formulation proposed in: Equation 25, M. Kaess, "Simultaneous Localization and Mapping with Infinite Planes", IEEE International Conference on Robotics and Automation, 2015.
The main purpose of this factor is to improve the numerical stability of the optimization, especially compared to gtsam::OrientedPlane3Factor. This is especially relevant when the sensor is far from the origin (and thus the derivatives associated to transforming the plane are large).
x0 is the current sensor pose, and x1 is the local "anchor pose" - i.e. a local linearisation point for the plane. The plane is representated and optimized in x1 frame in the optimization.
Definition at line 37 of file LocalOrientedPlane3Factor.h.