Generic math helpers for constellation optimizer code. More...

#include "internal_math.hpp"
#include "imu_preintegration.hpp"

Include dependency graph for internal_math.cpp:

Functions
static Matrix3d	xrt::tracking::constellation::optimizer::skew (const Vector3d &v)
	The matrix which cross-products with v, so skew(v) * u == v.cross(u).

static Matrix3d	xrt::tracking::constellation::optimizer::rightJacobianSo3 (const Vector3d &phi)
	The right Jacobian of SO(3), which converts a perturbation of a rotation vector into the body-frame rotation it produces: `Exp(phi + delta) ~= Exp(phi) * Exp(rightJacobianSo3(phi) * delta)`.

template<typename Derived >
static Matrix< double, Derived::RowsAtCompileTime *Derived::ColsAtCompileTime, ImuBias< double >::kNumParameters >	xrt::tracking::constellation::optimizer::extractJacobianFromJetMatrix (const MatrixBase< Derived > &m)

template<int N>
static Matrix3d	xrt::tracking::constellation::optimizer::quaternionJetToSO3Jacobian (const Quaternion< ceres::Jet< double, N > > &q_jet, int parameter_offset)

static void	xrt::tracking::constellation::optimizer::propagateCovariance (const Quaterniond &Q_start_cur, const Vector3d &corrected_accel, const Vector3d &corrected_gyro, const Vector3d &accel_scale, double dt, PreintegrationCovarianceMatrix &covariance)
	Propagates the covariance for this sample.

static void	xrt::tracking::constellation::optimizer::integrateSingleSample (const xrt_imu_sample &sample, double dt, const Vector3< ImuBiasJet > &accel_bias, const Vector3< ImuBiasJet > &gyro_bias, const Vector3< ImuBiasJet > &accel_scale, Quaternion< ImuBiasJet > &Q_start_cur, Vector3< ImuBiasJet > &delta_velocity, Vector3< ImuBiasJet > &delta_position, PreintegrationCovarianceMatrix &covariance)

static void	xrt::tracking::constellation::optimizer::robustComputeSqrtAndPinvSqrt (const MatrixXd &a, MatrixXd &out_j, MatrixXd &out_j_pinv_t)
	Decompose a symmetric positive semi-definite `a` into a square root and the transposed pseudo-inverse of that square root, dropping the eigenvalues that are numerically indistinguishable from zero so that a rank-deficient input still gives usable factors instead of infinities.

FactorFitness	xrt::tracking::constellation::optimizer::evaluateFactorFitness (ceres::Problem &problem, const std::vector< ceres::ResidualBlockId > &residual_blocks)
	Measures how well one kind of factor was fit, at whatever the problem's parameters currently hold.

void	xrt::tracking::constellation::optimizer::matrixCrsToEigen (const ceres::CRSMatrix &mat_crs, Eigen::MatrixXd &mat)
	Converts a CRS matrix to a dense Eigen matrix.

void	xrt::tracking::constellation::optimizer::marginalize (const VectorXd &residual, const MatrixXd &jacobian, const int num_base_states, VectorXd &out_e0, MatrixXd &out_j)
	Marginalize the first `num_base_states` states out of the linearized system given by `jacobian` and `residual`, handing back the linear prior that replaces them.

PreintegratedImuSamples	xrt::tracking::constellation::optimizer::preintegrate (const std::span< const xrt_imu_sample > &imu_samples, const xrt_imu_sample &first_sample_after_end_time, const ImuBias< ImuBiasJet > &imu_bias, timepoint_ns start_time_ns, timepoint_ns end_time_ns)
	Does IMU pre-integration on a set of IMU samples, interpolates the starting sample to the start time, and returns the pre-integrated samples.

Detailed Description

Generic math helpers for constellation optimizer code.

Author: Beyley Cardellio ep1cm.nosp@m.1n10.nosp@m.n123@.nosp@m.gmai.nosp@m.l.com

Function Documentation

◆ evaluateFactorFitness()

FactorFitness xrt::tracking::constellation::optimizer::evaluateFactorFitness	(	ceres::Problem &	problem,
		const std::vector< ceres::ResidualBlockId > &	residual_blocks
	)

Measures how well one kind of factor was fit, at whatever the problem's parameters currently hold.

Both factor types are whitened, so this is directly comparable between them and against 1.

Parameters

problem	The problem, evaluated at its current parameter values.
residual_blocks	The blocks belonging to this kind of factor.

References xrt::tracking::constellation::optimizer::evaluateFactorFitness(), and xrt::tracking::constellation::optimizer::FactorFitness::num_residuals.

Referenced by xrt::tracking::constellation::optimizer::evaluateFactorFitness().

◆ marginalize()

void xrt::tracking::constellation::optimizer::marginalize	(	const VectorXd &	residual,
		const MatrixXd &	jacobian,
		const int	num_base_states,
		VectorXd &	out_e0,
		MatrixXd &	out_j
	)

Marginalize the first num_base_states states out of the linearized system given by jacobian and residual, handing back the linear prior that replaces them.

Inspired/specialized from MarginalizationError::marginalizeOut() from OKVIS.

References xrt::tracking::constellation::optimizer::marginalize(), and xrt::tracking::constellation::optimizer::robustComputeSqrtAndPinvSqrt().

Referenced by xrt::tracking::constellation::optimizer::marginalize().

◆ matrixCrsToEigen()

void xrt::tracking::constellation::optimizer::matrixCrsToEigen	(	const ceres::CRSMatrix &	mat_crs,
		Eigen::MatrixXd &	mat
	)

Converts a CRS matrix to a dense Eigen matrix.

References xrt::tracking::constellation::optimizer::matrixCrsToEigen().

Referenced by xrt::tracking::constellation::optimizer::matrixCrsToEigen().

◆ preintegrate()

PreintegratedImuSamples xrt::tracking::constellation::optimizer::preintegrate	(	const std::span< const xrt_imu_sample > &	imu_samples,
		const xrt_imu_sample &	first_sample_after_end_time,
		const ImuBias< ImuBiasJet > &	imu_bias,
		timepoint_ns	start_time_ns,
		timepoint_ns	end_time_ns
	)

Does IMU pre-integration on a set of IMU samples, interpolates the starting sample to the start time, and returns the pre-integrated samples.

Parameters

imu_samples	The IMU samples to preintegrate
first_sample_after_end_time	The first sample after the end timestamp
imu_bias	The Jet-seeded IMU biases
start_time_ns	The start time of the preintegration
end_time_ns	The end time of the preintegration

References xrt::tracking::constellation::optimizer::preintegrate(), xrt::tracking::constellation::optimizer::PreintegratedImuSamples::Q_start_end, and xrt::tracking::constellation::optimizer::PreintegratedImuSamples::whitening.

Referenced by xrt::tracking::constellation::optimizer::preintegrate().

◆ propagateCovariance()

static void xrt::tracking::constellation::optimizer::propagateCovariance	(	const Quaterniond &	Q_start_cur,
		const Vector3d &	corrected_accel,
		const Vector3d &	corrected_gyro,
		const Vector3d &	accel_scale,
		double	dt,
		PreintegrationCovarianceMatrix &	covariance
	)

static

Propagates the covariance for this sample.

Parameters

[in]	Q_start_cur	Rotation accumulated before this sample.
[in]	corrected_accel	Bias and scale-corrected acceleration, in the IMU frame.
[in]	corrected_gyro	Bias-corrected gyroscope reading.
[in]	accel_scale	The accelerometer scale.
	dt	Delta time
[out]	covariance	Out covariance matrix

References xrt::tracking::constellation::optimizer::kAccelNoiseDensity, xrt::tracking::constellation::optimizer::kGyroNoiseDensity, xrt::tracking::constellation::optimizer::propagateCovariance(), xrt::auxiliary::math::quat_exp_so3(), xrt::tracking::constellation::optimizer::rightJacobianSo3(), and xrt::tracking::constellation::optimizer::skew().

Referenced by xrt::tracking::constellation::optimizer::propagateCovariance().

◆ rightJacobianSo3()

static Matrix3d xrt::tracking::constellation::optimizer::rightJacobianSo3 ( const Vector3d & phi )

static

The right Jacobian of SO(3), which converts a perturbation of a rotation vector into the body-frame rotation it produces: Exp(phi + delta) ~= Exp(phi) * Exp(rightJacobianSo3(phi) * delta).

Parameters

phi	The rotation vector to evaluate at, in radians.

References xrt::tracking::constellation::optimizer::rightJacobianSo3(), and xrt::tracking::constellation::optimizer::skew().

Referenced by xrt::tracking::constellation::optimizer::propagateCovariance(), and xrt::tracking::constellation::optimizer::rightJacobianSo3().

◆ robustComputeSqrtAndPinvSqrt()

static void xrt::tracking::constellation::optimizer::robustComputeSqrtAndPinvSqrt	(	const MatrixXd &	a,
		MatrixXd &	out_j,
		MatrixXd &	out_j_pinv_t
	)

static

Decompose a symmetric positive semi-definite a into a square root and the transposed pseudo-inverse of that square root, dropping the eigenvalues that are numerically indistinguishable from zero so that a rank-deficient input still gives usable factors instead of infinities.

With the eigendecomposition a = E S E^T, this hands back out_j = S^1/2 E^T and out_j_pinv_t = S^-1/2 E^T, so that out_j^T * out_j == a and out_j_pinv_t == (out_j^-1)^T.

This is what OKVIS does at the end of MarginalizationError::updateErrorComputation(), minus its Jacobi preconditioner: out_j is its J_ and out_j_pinv_t its J_pinv_T.

References xrt::tracking::constellation::optimizer::robustComputeSqrtAndPinvSqrt().

Referenced by xrt::tracking::constellation::optimizer::marginalize(), and xrt::tracking::constellation::optimizer::robustComputeSqrtAndPinvSqrt().

◆ skew()

static Matrix3d xrt::tracking::constellation::optimizer::skew ( const Vector3d & v )

static

The matrix which cross-products with v, so skew(v) * u == v.cross(u).

References xrt::tracking::constellation::optimizer::skew().

Referenced by xrt::tracking::constellation::optimizer::propagateCovariance(), xrt::tracking::constellation::optimizer::rightJacobianSo3(), and xrt::tracking::constellation::optimizer::skew().

Functions

Detailed Description

Function Documentation

◆ evaluateFactorFitness()

◆ marginalize()

◆ matrixCrsToEigen()

◆ preintegrate()

◆ propagateCovariance()

◆ rightJacobianSo3()

◆ robustComputeSqrtAndPinvSqrt()

◆ skew()