ov_core::FeatureInitializer class

Class that triangulates feature.

Contents

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 3D 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(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_gaussnewton(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.

Protected functions

auto compute_error(std::unordered_map<size_t, std::unordered_map<double, ClonePose>>& clonesCAM, 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(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)

bool ov_core::FeatureInitializer::single_gaussnewton(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)

double ov_core::FeatureInitializer::compute_error(std::unordered_map<size_t, std::unordered_map<double, ClonePose>>& clonesCAM, 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