NASA Astrobee Robot Software  0.19.1
Flight software for the Astrobee robots operating inside the International Space Station.
gtsam::InverseDepthProjectionFactor Class Reference

#include <inverse_depth_projection_factor.h>

Inheritance diagram for gtsam::InverseDepthProjectionFactor:
Inheritance graph

Public Types

typedef NoiseModelFactor3< Pose3, vision_common::InverseDepthMeasurement, Pose3 > Base
 
typedef boost::shared_ptr< InverseDepthProjectionFactorshared_ptr
 
typedef InverseDepthProjectionFactor This
 

Public Member Functions

 InverseDepthProjectionFactor ()
 
 InverseDepthProjectionFactor (const Point2 &measured, const SharedNoiseModel &model, Key sourcePoseKey, Key inverseDepthMeasurementKey, Key targetPoseKey)
 
 InverseDepthProjectionFactor (const Point2 &measured, const SharedNoiseModel &model, Key sourcePoseKey, Key inverseDepthCameraPoseKey, Key targetPoseKey, bool verboseCheirality)
 
virtual ~InverseDepthProjectionFactor ()
 
virtual NonlinearFactor::shared_ptr clone () const
 
void print (const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
 
bool equals (const NonlinearFactor &p, double tol=1e-9) const override
 
Vector evaluateError (const Pose3 &world_T_source, const vision_common::InverseDepthMeasurement &inverseDepthMeasurement, const Pose3 &world_T_target, boost::optional< Matrix & > d_projected_point_d_world_T_source=boost::none, boost::optional< Matrix & > d_projected_point_d_inverse_depth=boost::none, boost::optional< Matrix & > d_projected_point_d_world_T_target=boost::none) const override
 Evaluate error h(x)-z and optionally derivatives. More...
 
const Point2 & measured () const
 
bool verboseCheirality () const
 

Protected Attributes

Point2 measured_
 
bool verboseCheirality_
 

Friends

class boost::serialization::access
 Serialization function. More...
 

Detailed Description

Non-linear factor for a constraint derived from a 2D measurement. The calibration is known and the landmark point is represented using the inverse depth parameterization.

Member Typedef Documentation

◆ Base

◆ shared_ptr

◆ This

Constructor & Destructor Documentation

◆ InverseDepthProjectionFactor() [1/3]

gtsam::InverseDepthProjectionFactor::InverseDepthProjectionFactor ( )
inline

◆ InverseDepthProjectionFactor() [2/3]

gtsam::InverseDepthProjectionFactor::InverseDepthProjectionFactor ( const Point2 &  measured,
const SharedNoiseModel &  model,
Key  sourcePoseKey,
Key  inverseDepthMeasurementKey,
Key  targetPoseKey 
)
inline

Constructor

Parameters
measuredis the 2 dimensional location of point in image (the measurement)
modelis the standard deviation
sourcePoseKeyis the index of the source camera
inverseDepthMeasurementKeyis the index of the inverse depth measurement
targetPoseKeyis the index of the target camera

◆ InverseDepthProjectionFactor() [3/3]

gtsam::InverseDepthProjectionFactor::InverseDepthProjectionFactor ( const Point2 &  measured,
const SharedNoiseModel &  model,
Key  sourcePoseKey,
Key  inverseDepthCameraPoseKey,
Key  targetPoseKey,
bool  verboseCheirality 
)
inline

Constructor with exception-handling flags

Parameters
measuredis the 2 dimensional location of point in image (the measurement)
modelis the standard deviation
sourcePoseKeyis the index of the source camera
inverseDepthMeasurementKeyis the index of the inverse depth measurement
targetPoseKeyis the index of the target camera
verboseCheiralitydetermines whether exceptions are printed for Cheirality

◆ ~InverseDepthProjectionFactor()

virtual gtsam::InverseDepthProjectionFactor::~InverseDepthProjectionFactor ( )
inlinevirtual

Member Function Documentation

◆ clone()

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

◆ equals()

bool gtsam::InverseDepthProjectionFactor::equals ( const NonlinearFactor &  p,
double  tol = 1e-9 
) const
inlineoverride

◆ evaluateError()

Vector gtsam::InverseDepthProjectionFactor::evaluateError ( const Pose3 &  world_T_source,
const vision_common::InverseDepthMeasurement inverseDepthMeasurement,
const Pose3 &  world_T_target,
boost::optional< Matrix & >  d_projected_point_d_world_T_source = boost::none,
boost::optional< Matrix & >  d_projected_point_d_inverse_depth = boost::none,
boost::optional< Matrix & >  d_projected_point_d_world_T_target = boost::none 
) const
inlineoverride

Evaluate error h(x)-z and optionally derivatives.

◆ measured()

const Point2& gtsam::InverseDepthProjectionFactor::measured ( ) const
inline

◆ print()

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

print

Parameters
soptional string naming the factor
keyFormatteroptional formatter useful for printing Symbols

◆ verboseCheirality()

bool gtsam::InverseDepthProjectionFactor::verboseCheirality ( ) const
inline

Friends And Related Function Documentation

◆ boost::serialization::access

friend class boost::serialization::access
friend

Serialization function.

Member Data Documentation

◆ measured_

Point2 gtsam::InverseDepthProjectionFactor::measured_
protected

◆ verboseCheirality_

bool gtsam::InverseDepthProjectionFactor::verboseCheirality_
protected

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