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_
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::
Default constructor.
| Parameters | |
|---|---|
| options | Options for the initializer |
bool ov_core::
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::
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::
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::
Gets the current configuration of the feature initializer.
| Returns | Const feature initializer config |
|---|
double ov_core::
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 |