SaDVIO
Public Member Functions | Public Attributes | Static Public Attributes | Protected Attributes | List of all members
isae::ALandmark Class Reference

Abstract class for 3D Landmarks. More...

#include <ALandmark.h>

Inheritance diagram for isae::ALandmark:
Inheritance graph
[legend]
Collaboration diagram for isae::ALandmark:
Collaboration graph
[legend]

Public Member Functions

 ALandmark ()
 
 ALandmark (Eigen::Affine3d T_w_l, std::vector< std::shared_ptr< isae::AFeature >> features)
 
 ~ALandmark ()
 
virtual void init (Eigen::Affine3d T_w_l, std::vector< std::shared_ptr< isae::AFeature >> features)
 Virtual Function to properly initialize a landmark. More...
 
void addFeature (std::shared_ptr< AFeature > feature)
 
std::vector< std::weak_ptr< AFeature > > getFeatures ()
 
void removeExpiredFeatures ()
 Remove features that are not linked to anything. More...
 
void removeFeature (std::shared_ptr< AFeature > f)
 
bool fuseWithLandmark (std::shared_ptr< ALandmark > landmark)
 Fuse this landmark with another one, the feature sets are merged. More...
 
void setPose (Eigen::Affine3d T_w_l)
 
void setPosition (Eigen::Vector3d t_w_l)
 Set only the translation of the landmark pose. More...
 
Eigen::Affine3d getPose () const
 
std::vector< Eigen::Vector3d > getModelPoints ()
 
std::shared_ptr< AModel3dgetModel () const
 
cv::Mat getDescriptor () const
 
void setDescriptor (cv::Mat descriptor)
 
bool isInitialized () const
 
void setUninitialized ()
 
void setInMap ()
 
bool isInMap ()
 
bool sanityCheck ()
 Check if the landmark is valid in terms of reprojection error and number of features. More...
 
virtual double chi2err (std::shared_ptr< AFeature > f)
 Compute the chi2 error for a given feature. More...
 
double avgChi2err ()
 Compute the average chi2 error for all features associated to the landmark. More...
 
bool isOutlier () const
 
void setOutlier ()
 
void setInlier ()
 
bool isResurected () const
 
void setResurected ()
 
bool hasPrior () const
 
void setPrior ()
 
bool isMarg () const
 
void setMarg ()
 

Public Attributes

int _id
 Unique id for the landmark, used for bookeeping. More...
 
std::string _label
 Label for the landmark. More...
 

Static Public Attributes

static int _landmark_count = 0
 Static counter for landmarks, used for unique id generation. More...
 

Protected Attributes

bool _initialized = false
 Flag to check if the landmark is initialized. More...
 
bool _in_map = false
 Flag to check if the landmark is in the map. More...
 
bool _outlier = false
 Flag to check if the landmark is an outlier. More...
 
bool _is_resurected = false
 Flag to check if the landmark has been resurected. More...
 
bool _has_prior = false
 Flag to check if the landmark has a prior. More...
 
bool _is_marg = false
 Flag to check if the landmark is marginalised. More...
 
Eigen::Affine3d _T_w_l
 Landmark pose in world frame. More...
 
cv::Mat _descriptor
 Descriptor of the landmark, used for matching. More...
 
std::shared_ptr< AModel3d_model
 3D model of the landmark More...
 
std::vector< std::weak_ptr< AFeature > > _features
 Features associated to the landmark. More...
 
std::mutex _lmk_mtx
 

Detailed Description

Abstract class for 3D Landmarks.

It contains a 6DoF pose and a 3D model for a general formulation. It also contains a set of features that are associated to the landmark.

Constructor & Destructor Documentation

◆ ALandmark() [1/2]

isae::ALandmark::ALandmark ( )
inline

◆ ALandmark() [2/2]

isae::ALandmark::ALandmark ( Eigen::Affine3d  T_w_l,
std::vector< std::shared_ptr< isae::AFeature >>  features 
)

◆ ~ALandmark()

isae::ALandmark::~ALandmark ( )
inline

Member Function Documentation

◆ addFeature()

void isae::ALandmark::addFeature ( std::shared_ptr< AFeature feature)

◆ avgChi2err()

double isae::ALandmark::avgChi2err ( )

Compute the average chi2 error for all features associated to the landmark.

◆ chi2err()

double isae::ALandmark::chi2err ( std::shared_ptr< AFeature f)
virtual

Compute the chi2 error for a given feature.

Reimplemented in isae::Line3D.

◆ fuseWithLandmark()

bool isae::ALandmark::fuseWithLandmark ( std::shared_ptr< ALandmark landmark)

Fuse this landmark with another one, the feature sets are merged.

If there is a wrong behavior in the association, it returns false

◆ getDescriptor()

cv::Mat isae::ALandmark::getDescriptor ( ) const
inline

◆ getFeatures()

std::vector<std::weak_ptr<AFeature> > isae::ALandmark::getFeatures ( )
inline

◆ getModel()

std::shared_ptr<AModel3d> isae::ALandmark::getModel ( ) const
inline

◆ getModelPoints()

std::vector<Eigen::Vector3d> isae::ALandmark::getModelPoints ( )
inline

◆ getPose()

Eigen::Affine3d isae::ALandmark::getPose ( ) const
inline

◆ hasPrior()

bool isae::ALandmark::hasPrior ( ) const
inline

◆ init()

void isae::ALandmark::init ( Eigen::Affine3d  T_w_l,
std::vector< std::shared_ptr< isae::AFeature >>  features 
)
virtual

Virtual Function to properly initialize a landmark.

◆ isInitialized()

bool isae::ALandmark::isInitialized ( ) const
inline

◆ isInMap()

bool isae::ALandmark::isInMap ( )
inline

◆ isMarg()

bool isae::ALandmark::isMarg ( ) const
inline

◆ isOutlier()

bool isae::ALandmark::isOutlier ( ) const
inline

◆ isResurected()

bool isae::ALandmark::isResurected ( ) const
inline

◆ removeExpiredFeatures()

void isae::ALandmark::removeExpiredFeatures ( )

Remove features that are not linked to anything.

◆ removeFeature()

void isae::ALandmark::removeFeature ( std::shared_ptr< AFeature f)

◆ sanityCheck()

bool isae::ALandmark::sanityCheck ( )

Check if the landmark is valid in terms of reprojection error and number of features.

◆ setDescriptor()

void isae::ALandmark::setDescriptor ( cv::Mat  descriptor)
inline

◆ setInlier()

void isae::ALandmark::setInlier ( )
inline

◆ setInMap()

void isae::ALandmark::setInMap ( )
inline

◆ setMarg()

void isae::ALandmark::setMarg ( )
inline

◆ setOutlier()

void isae::ALandmark::setOutlier ( )
inline

◆ setPose()

void isae::ALandmark::setPose ( Eigen::Affine3d  T_w_l)
inline

◆ setPosition()

void isae::ALandmark::setPosition ( Eigen::Vector3d  t_w_l)
inline

Set only the translation of the landmark pose.

◆ setPrior()

void isae::ALandmark::setPrior ( )
inline

◆ setResurected()

void isae::ALandmark::setResurected ( )
inline

◆ setUninitialized()

void isae::ALandmark::setUninitialized ( )
inline

Member Data Documentation

◆ _descriptor

cv::Mat isae::ALandmark::_descriptor
protected

Descriptor of the landmark, used for matching.

◆ _features

std::vector<std::weak_ptr<AFeature> > isae::ALandmark::_features
protected

Features associated to the landmark.

◆ _has_prior

bool isae::ALandmark::_has_prior = false
protected

Flag to check if the landmark has a prior.

◆ _id

int isae::ALandmark::_id

Unique id for the landmark, used for bookeeping.

◆ _in_map

bool isae::ALandmark::_in_map = false
protected

Flag to check if the landmark is in the map.

◆ _initialized

bool isae::ALandmark::_initialized = false
protected

Flag to check if the landmark is initialized.

◆ _is_marg

bool isae::ALandmark::_is_marg = false
protected

Flag to check if the landmark is marginalised.

◆ _is_resurected

bool isae::ALandmark::_is_resurected = false
protected

Flag to check if the landmark has been resurected.

◆ _label

std::string isae::ALandmark::_label

Label for the landmark.

◆ _landmark_count

int isae::ALandmark::_landmark_count = 0
static

Static counter for landmarks, used for unique id generation.

◆ _lmk_mtx

std::mutex isae::ALandmark::_lmk_mtx
mutableprotected

◆ _model

std::shared_ptr<AModel3d> isae::ALandmark::_model
protected

3D model of the landmark

◆ _outlier

bool isae::ALandmark::_outlier = false
protected

Flag to check if the landmark is an outlier.

◆ _T_w_l

Eigen::Affine3d isae::ALandmark::_T_w_l
protected

Landmark pose in world frame.


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