gtsam  4.0.0
gtsam
gtsam::RelativeElevationFactor Class Reference

Detailed Description

Binary factor for a relative elevation.

Note that this factor takes into account only elevation, and corrects for orientation. Unlike a range factor, the relative elevation is signed, and only affects the Z coordinate. Measurement function h(pose, pt) = h.z() - pt.z()

Dimension: 1

TODO: enable use of a Pose3 for the target as well

+ Inheritance diagram for gtsam::RelativeElevationFactor:

Public Member Functions

 RelativeElevationFactor (Key poseKey, Key pointKey, double measured, const SharedNoiseModel &model)
 
virtual gtsam::NonlinearFactor::shared_ptr clone () const
 
Vector evaluateError (const Pose3 &pose, const Point3 &point, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const
 h(x)-z
 
double measured () const
 return the measured
 
virtual bool equals (const NonlinearFactor &expected, double tol=1e-9) const
 equals specialized to this factor
 
void print (const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
 print contents
 
- Public Member Functions inherited from gtsam::NoiseModelFactor2< Pose3, Point3 >
 NoiseModelFactor2 ()
 Default Constructor for I/O.
 
 NoiseModelFactor2 (const SharedNoiseModel &noiseModel, Key j1, Key j2)
 Constructor. More...
 
Key key1 () const
 methods to retrieve both keys
 
Key key2 () const
 
virtual Vector unwhitenedError (const Values &x, boost::optional< std::vector< Matrix > & > H=boost::none) const
 Calls the 2-key specific version of evaluateError, which is pure virtual so must be implemented in the derived class. More...
 

Friends

class boost::serialization::access
 Serialization function.
 

Additional Inherited Members

- Public Types inherited from gtsam::NoiseModelFactor2< Pose3, Point3 >
typedef Pose3 X1
 
typedef Point3 X2
 
- Protected Types inherited from gtsam::NoiseModelFactor2< Pose3, Point3 >
typedef NoiseModelFactor Base
 
typedef NoiseModelFactor2< Pose3, Point3This
 

Member Function Documentation

◆ clone()

virtual gtsam::NonlinearFactor::shared_ptr gtsam::RelativeElevationFactor::clone ( ) const
inlinevirtual
Returns
a deep copy of this factor

Reimplemented from gtsam::NonlinearFactor.


The documentation for this class was generated from the following files: