ov_msckf::UpdaterHelper class

Class that has helper functions for our updaters.

Can compute the Jacobian for a single feature representation. This will create the Jacobian based on what representation our state is in. If we are using the anchor representation then we also have additional Jacobians in respect to the anchor state. Also has functions such as nullspace projection and full jacobian construction. For derivations look at Camera Measurement Update page which has detailed equations.

Public types

struct UpdaterHelperFeature
Feature object that our UpdaterHelper leverages, has all measurements and means.

Public static functions

static void get_feature_jacobian_representation(State* state, UpdaterHelperFeature& feature, Eigen::MatrixXd& H_f, std::vector<Eigen::MatrixXd>& H_x, std::vector<Type*>& x_order)
This gets the feature and state Jacobian in respect to the feature representation.
static void get_feature_jacobian_intrinsics(State* state, const Eigen::Vector2d& uv_norm, bool isfisheye, Eigen::Matrix<double, 8, 1> cam_d, Eigen::Matrix<double, 2, 2>& dz_dzn, Eigen::Matrix<double, 2, 8>& dz_dzeta)
This will compute the Jacobian in respect to the intrinsic calibration parameters and normalized coordinates.
static void get_feature_jacobian_full(State* state, UpdaterHelperFeature& feature, Eigen::MatrixXd& H_f, Eigen::MatrixXd& H_x, Eigen::VectorXd& res, std::vector<Type*>& x_order)
Will construct the "stacked" Jacobians for a single feature from all its measurements.
static void nullspace_project_inplace(Eigen::MatrixXd& H_f, Eigen::MatrixXd& H_x, Eigen::VectorXd& res)
This will project the left nullspace of H_f onto the linear system.
static void measurement_compress_inplace(Eigen::MatrixXd& H_x, Eigen::VectorXd& res)
This will perform measurement compression.

Function documentation

static void ov_msckf::UpdaterHelper::get_feature_jacobian_representation(State* state, UpdaterHelperFeature& feature, Eigen::MatrixXd& H_f, std::vector<Eigen::MatrixXd>& H_x, std::vector<Type*>& x_order)

This gets the feature and state Jacobian in respect to the feature representation.

Parameters
state in State of the filter system
feature in Feature we want to get Jacobians of (must have feature means)
H_f out Jacobians in respect to the feature error state (will be either 3x3 or 3x1 for single depth)
H_x out Extra Jacobians in respect to the state (for example anchored pose)
x_order out Extra variables our extra Jacobian has (for example anchored pose)

CASE: Estimate single depth of the feature using the initial bearing

static void ov_msckf::UpdaterHelper::get_feature_jacobian_intrinsics(State* state, const Eigen::Vector2d& uv_norm, bool isfisheye, Eigen::Matrix<double, 8, 1> cam_d, Eigen::Matrix<double, 2, 2>& dz_dzn, Eigen::Matrix<double, 2, 8>& dz_dzeta)

This will compute the Jacobian in respect to the intrinsic calibration parameters and normalized coordinates.

Parameters
state State of the filter system
uv_norm Normalized image coordinates
isfisheye If this feature is for a fisheye model
cam_d Camera intrinsics values
dz_dzn Derivative in respect to normalized coordinates
dz_dzeta Derivative in respect to distortion paramters

static void ov_msckf::UpdaterHelper::get_feature_jacobian_full(State* state, UpdaterHelperFeature& feature, Eigen::MatrixXd& H_f, Eigen::MatrixXd& H_x, Eigen::VectorXd& res, std::vector<Type*>& x_order)

Will construct the "stacked" Jacobians for a single feature from all its measurements.

Parameters
state in State of the filter system
feature in Feature we want to get Jacobians of (must have feature means)
H_f out Jacobians in respect to the feature error state
H_x out Extra Jacobians in respect to the state (for example anchored pose)
res out Measurement residual for this feature
x_order out Extra variables our extra Jacobian has (for example anchored pose)

static void ov_msckf::UpdaterHelper::nullspace_project_inplace(Eigen::MatrixXd& H_f, Eigen::MatrixXd& H_x, Eigen::VectorXd& res)

This will project the left nullspace of H_f onto the linear system.

Parameters
H_f Jacobian with nullspace we want to project onto the system [res = Hx*(x-xhat)+Hf(f-fhat)+n]
H_x State jacobian
res Measurement residual

Please see the MSCKF Nullspace Projection for details on how this works. This is the MSCKF nullspace projection which removes the dependency on the feature state. Note that this is done in place so all matrices will be different after a function call.

static void ov_msckf::UpdaterHelper::measurement_compress_inplace(Eigen::MatrixXd& H_x, Eigen::VectorXd& res)

This will perform measurement compression.

Parameters
H_x State jacobian
res Measurement residual

Please see the Measurement Compression for details on how this works. Note that this is done in place so all matrices will be different after a function call.