ov_core::FeatureInitializer class

Class that triangulates feature.

This class has the functions needed to triangulate and then refine a given 3D feature. As in the standard MSCKF, we know the clones of the camera from propagation and past updates. Thus, we just need to triangulate a feature in 3D with the known poses and then refine it. One should first call the single_triangulation() function afterwhich single_gaussnewton() allows for refinement. Please see the Feature Triangulation page for detailed derivations.

Public types

struct ClonePose
Structure which stores pose estimates for use in triangulation.

Constructors, destructors, conversion operators

FeatureInitializer(FeatureInitializerOptions& options)
Default constructor.

Public functions

auto single_triangulation(std::shared_ptr<Feature> feat, std::unordered_map<size_t, std::unordered_map<double, ClonePose>>& clonesCAM) -> bool
Uses a linear triangulation to get initial estimate for the feature.
auto single_triangulation_1d(std::shared_ptr<Feature> feat, std::unordered_map<size_t, std::unordered_map<double, ClonePose>>& clonesCAM) -> bool
Uses a linear triangulation to get initial estimate for the feature, treating the anchor observation as a true bearing.
auto single_gaussnewton(std::shared_ptr<Feature> feat, std::unordered_map<size_t, std::unordered_map<double, ClonePose>>& clonesCAM) -> bool
Uses a nonlinear triangulation to refine initial linear estimate of the feature.
auto config() -> const FeatureInitializerOptions
Gets the current configuration of the feature initializer.

Protected functions

auto compute_error(std::unordered_map<size_t, std::unordered_map<double, ClonePose>>& clonesCAM, std::shared_ptr<Feature> feat, double alpha, double beta, double rho) -> double
Helper function for the gauss newton method that computes error of the given estimate.

Protected variables

FeatureInitializerOptions _options
Contains options for the initializer process.

Function documentation

ov_core::FeatureInitializer::FeatureInitializer(FeatureInitializerOptions& options)

Default constructor.

Parameters
options Options for the initializer

bool ov_core::FeatureInitializer::single_triangulation(std::shared_ptr<Feature> feat, std::unordered_map<size_t, std::unordered_map<double, ClonePose>>& clonesCAM)

Uses a linear triangulation to get initial estimate for the feature.

Parameters
feat Pointer to feature
clonesCAM Map between camera ID to map of timestamp to camera pose estimate (rotation from global to camera, position of camera in global frame)
Returns Returns false if it fails to triangulate (based on the thresholds)

The derivations for this method can be found in the 3D Cartesian Triangulation documentation page.

bool ov_core::FeatureInitializer::single_triangulation_1d(std::shared_ptr<Feature> feat, std::unordered_map<size_t, std::unordered_map<double, ClonePose>>& clonesCAM)

Uses a linear triangulation to get initial estimate for the feature, treating the anchor observation as a true bearing.

Parameters
feat Pointer to feature
clonesCAM Map between camera ID to map of timestamp to camera pose estimate (rotation from global to camera, position of camera in global frame)
Returns Returns false if it fails to triangulate (based on the thresholds)

The derivations for this method can be found in the 1D Depth Triangulation documentation page. This function should be used if you want speed, or know your anchor bearing is reasonably accurate.

bool ov_core::FeatureInitializer::single_gaussnewton(std::shared_ptr<Feature> feat, std::unordered_map<size_t, std::unordered_map<double, ClonePose>>& clonesCAM)

Uses a nonlinear triangulation to refine initial linear estimate of the feature.

Parameters
feat Pointer to feature
clonesCAM Map between camera ID to map of timestamp to camera pose estimate (rotation from global to camera, position of camera in global frame)
Returns Returns false if it fails to be optimize (based on the thresholds)

const FeatureInitializerOptions ov_core::FeatureInitializer::config()

Gets the current configuration of the feature initializer.

Returns Const feature initializer config

double ov_core::FeatureInitializer::compute_error(std::unordered_map<size_t, std::unordered_map<double, ClonePose>>& clonesCAM, std::shared_ptr<Feature> feat, double alpha, double beta, double rho) protected

Helper function for the gauss newton method that computes error of the given estimate.

Parameters
clonesCAM Map between camera ID to map of timestamp to camera pose estimate
feat Pointer to the feature
alpha x/z in anchor
beta y/z in anchor
rho 1/z inverse depth