Classes | Public Member Functions | Protected Types | Protected Attributes | List of all members
gtsam::SmartRangeFactor Class Reference

#include <SmartRangeFactor.h>

Inheritance diagram for gtsam::SmartRangeFactor:
Inheritance graph
[legend]

Classes

struct  Circle2
 

Public Member Functions

void addRange (Key key, double measuredRange)
 Add a range measurement to a pose with given key. More...
 
gtsam::NonlinearFactor::shared_ptr clone () const override
 
bool equals (const NonlinearFactor &f, double tol=1e-9) const override
 
void print (const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
 
 SmartRangeFactor ()
 
 SmartRangeFactor (double s)
 
Point2 triangulate (const Values &x) const
 
Vector unwhitenedError (const Values &x, OptionalMatrixVecType H=nullptr) const override
 
 ~SmartRangeFactor () override
 
- Public Member Functions inherited from gtsam::NoiseModelFactor
shared_ptr cloneWithNewNoiseModel (const SharedNoiseModel newNoise) const
 
size_t dim () const override
 
double error (const Values &c) const override
 
std::shared_ptr< GaussianFactorlinearize (const Values &x) const override
 
const SharedNoiseModelnoiseModel () 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
 
- 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 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 KeyVectorkeys () 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...
 
KeyVectorkeys ()
 
iterator begin ()
 
iterator end ()
 

Protected Types

typedef SmartRangeFactor This
 
- Protected Types inherited from gtsam::NoiseModelFactor
typedef NonlinearFactor Base
 
typedef NoiseModelFactor This
 
- Protected Types inherited from gtsam::NonlinearFactor
typedef Factor Base
 
typedef NonlinearFactor This
 

Protected Attributes

std::vector< double > measurements_
 Range measurements. More...
 
double variance_
 variance on noise More...
 
- Protected Attributes inherited from gtsam::NoiseModelFactor
SharedNoiseModel noiseModel_
 
- Protected Attributes inherited from gtsam::Factor
KeyVector keys_
 The keys involved in this factor. More...
 

Additional Inherited Members

- Public Types inherited from gtsam::NoiseModelFactor
typedef std::shared_ptr< Thisshared_ptr
 
- Public Types inherited from gtsam::NonlinearFactor
typedef std::shared_ptr< Thisshared_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...
 
- Protected Member Functions inherited from gtsam::NoiseModelFactor
 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)
 
- 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)
 

Detailed Description

Smart factor for range SLAM

Definition at line 30 of file SmartRangeFactor.h.

Member Typedef Documentation

◆ This

Definition at line 40 of file SmartRangeFactor.h.

Constructor & Destructor Documentation

◆ SmartRangeFactor() [1/2]

gtsam::SmartRangeFactor::SmartRangeFactor ( )
inline

Default constructor: don't use directly

Definition at line 51 of file SmartRangeFactor.h.

◆ SmartRangeFactor() [2/2]

gtsam::SmartRangeFactor::SmartRangeFactor ( double  s)
inlineexplicit

Constructor

Parameters
sstandard deviation of range measurement noise

Definition at line 58 of file SmartRangeFactor.h.

◆ ~SmartRangeFactor()

gtsam::SmartRangeFactor::~SmartRangeFactor ( )
inlineoverride

Definition at line 62 of file SmartRangeFactor.h.

Member Function Documentation

◆ addRange()

void gtsam::SmartRangeFactor::addRange ( Key  key,
double  measuredRange 
)
inline

Add a range measurement to a pose with given key.

Definition at line 66 of file SmartRangeFactor.h.

◆ clone()

gtsam::NonlinearFactor::shared_ptr gtsam::SmartRangeFactor::clone ( ) const
inlineoverridevirtual
Returns
a deep copy of this factor

Reimplemented from gtsam::NonlinearFactor.

Definition at line 182 of file SmartRangeFactor.h.

◆ equals()

bool gtsam::SmartRangeFactor::equals ( const NonlinearFactor f,
double  tol = 1e-9 
) const
inlineoverridevirtual

Check if two factors are equal

Reimplemented from gtsam::NoiseModelFactor.

Definition at line 88 of file SmartRangeFactor.h.

◆ print()

void gtsam::SmartRangeFactor::print ( const std::string &  s = "",
const KeyFormatter keyFormatter = DefaultKeyFormatter 
) const
inlineoverridevirtual

print

Reimplemented from gtsam::NoiseModelFactor.

Definition at line 81 of file SmartRangeFactor.h.

◆ triangulate()

Point2 gtsam::SmartRangeFactor::triangulate ( const Values x) const
inline

Triangulate a point from at least three pose-range pairs Checks for best pair that includes first point Raise runtime_error if not well defined.

Definition at line 99 of file SmartRangeFactor.h.

◆ unwhitenedError()

Vector gtsam::SmartRangeFactor::unwhitenedError ( const Values x,
OptionalMatrixVecType  H = nullptr 
) const
inlineoverridevirtual

Error function without the NoiseModel, $ z-h(x) $.

Implements gtsam::NoiseModelFactor.

Definition at line 151 of file SmartRangeFactor.h.

Member Data Documentation

◆ measurements_

std::vector<double> gtsam::SmartRangeFactor::measurements_
protected

Range measurements.

Definition at line 42 of file SmartRangeFactor.h.

◆ variance_

double gtsam::SmartRangeFactor::variance_
protected

variance on noise

Definition at line 43 of file SmartRangeFactor.h.


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


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:47:09