imu_preintegration.hpp File Reference

Generic math helpers for IMU preintegration. More...

#include "os/os_time.h"
#include "math/m_eigen_interop.hpp"
#include "math/m_quatexpmap.hpp"
#include "math/m_quatexpmap_bigceres.hpp"
#include <Eigen/Core>
#include <Eigen/Geometry>
#include "ceres/jet.h"
#include "internal_math.hpp"
#include <cstring>
#include <cmath>
#include <mutex>
#include <string>
#include <algorithm>
#include <span>
#include <cassert>

Include dependency graph for imu_preintegration.hpp:

Go to the source code of this file.

Data Structures
struct	xrt::tracking::constellation::optimizer::ImuBias< T >
	A full-state IMU bias, with Accelerometer Bias, Gyroscope Bias, and Accelerometer Scale. More...
struct	xrt::tracking::constellation::optimizer::PreintegratedImuSamples
struct	xrt::tracking::constellation::optimizer::ImuCostFunctor
	IMU preintegration factor, between each camera keyframe. More...
struct	xrt::tracking::constellation::optimizer::ImuBiasRandomWalkCostFunctor
	IMU Bias random walk cost functor, between each keyframe. More...
struct	xrt::tracking::constellation::optimizer::ImuBiasAnchorCostFunctor
	IMU Bias anchor cost functor, to hold it at some value when marginalization has no guess for it. More...

Typedefs
typedef ceres::Jet< double, static_cast< int >(ImuBiasStateIndex::NumIndices)>	xrt::tracking::constellation::optimizer::ImuBiasJet
typedef Matrix< double, kNumImuBiasCovIndices, kNumImuBiasCovIndices >	xrt::tracking::constellation::optimizer::PreintegrationCovarianceMatrix
	[rotation; velocity; position], the order computeImuResidual writes its residual in.
typedef ceres::AutoDiffCostFunction< ImuCostFunctor, PreintegratedImuSamples::kNumResiduals, WorldGravity< double >::kNumParameters, ImuBias< double >::kNumParameters, PoseWithVelocity< double >::kNumParameters, PoseWithVelocity< double >::kNumParameters >	xrt::tracking::constellation::optimizer::ImuCostFunction
typedef ceres::AutoDiffCostFunction< ImuBiasRandomWalkCostFunctor, ImuBiasRandomWalkCostFunctor::kNumResiduals, ImuBias< double >::kNumParameters, ImuBias< double >::kNumParameters >	xrt::tracking::constellation::optimizer::ImuBiasRandomWalkCostFunction
typedef ceres::AutoDiffCostFunction< ImuBiasAnchorCostFunctor, ImuBiasAnchorCostFunctor::kNumResiduals, ImuBias< double >::kNumParameters >	xrt::tracking::constellation::optimizer::ImuBiasAnchorCostFunction

Enumerations
enum class	ImuBiasStateIndex : int {   AccelBiasX , AccelBiasY , AccelBiasZ , GyroBiasX ,   GyroBiasY , GyroBiasZ , AccelScaleX , AccelScaleY ,   AccelScaleZ , NumIndices }
enum class	ImuBiasCovarianceIndex : int {   RotX , RotY , RotZ , VelocityX ,   VelocityY , VelocityZ , PositionX , PositionY ,   PositionZ , NumIndices }

Functions
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.
template<typename T >
static void	xrt::tracking::constellation::optimizer::computeImuResidual (const PreintegratedImuSamples &imu_preintegration, const PoseWithVelocity< T > &start_T_world_device, const PoseWithVelocity< T > &end_T_world_device, const ImuBias< T > &imu_bias, const WorldGravity< T > &world_gravity, Vector< T, PreintegratedImuSamples::kNumResiduals > &residual)

Variables
constexpr double	xrt::tracking::constellation::optimizer::kGyroNoiseDensity = 2.44e-4
	BMI055 gyroscope noise density, 0.014 deg/s/sqrt(Hz) from the datasheet, in rad/s/sqrt(Hz).
constexpr double	xrt::tracking::constellation::optimizer::kAccelNoiseDensity = 1.47e-3
	BMI055 accelerometer noise density, 150 ug/sqrt(Hz) from the datasheet, in (m/s^2)/sqrt(Hz).
constexpr double	xrt::tracking::constellation::optimizer::kAccelBiasRandomWalkSigma = 1e-3
constexpr double	xrt::tracking::constellation::optimizer::kGyroBiasRandomWalkSigma = 1e-4
constexpr double	xrt::tracking::constellation::optimizer::kAccelScaleRandomWalkSigma = 1e-5
constexpr double	xrt::tracking::constellation::optimizer::kAccelBiasAnchorSigma = 0.687
constexpr double	xrt::tracking::constellation::optimizer::kGyroBiasAnchorSigma = 0.0175
constexpr double	xrt::tracking::constellation::optimizer::kAccelScaleAnchorSigma = 0.05
constexpr double	xrt::tracking::constellation::optimizer::kMaxAccelBias = 1.0
	Largest accelerometer bias to allow, in m/s^2. The BMI055's zero-g offset is +-70mg, so this is ~1.4x that.
constexpr double	xrt::tracking::constellation::optimizer::kMaxGyroBias = 0.05
	Largest gyroscope bias to allow, in rad/s. The BMI055's zero-rate offset is +-1 deg/s, this is ~3 deg/s.
constexpr double	xrt::tracking::constellation::optimizer::kMinAccelScale = 0.8
constexpr double	xrt::tracking::constellation::optimizer::kMaxAccelScale = 1.0 / kMinAccelScale
constexpr time_duration_ns	xrt::tracking::constellation::optimizer::kKeyframeGroupingToleranceNs = U_TIME_1MS_IN_NS
	Camera samples from the same mosaic closer together than this are treated as one keyframe.
static constexpr int	xrt::tracking::constellation::optimizer::kImuBiasRotCovIndex = static_cast<int>(ImuBiasCovarianceIndex::RotX)
static constexpr int	xrt::tracking::constellation::optimizer::kImuBiasVelCovIndex = static_cast<int>(ImuBiasCovarianceIndex::VelocityX)
static constexpr int	xrt::tracking::constellation::optimizer::kImuBiasPosCovIndex = static_cast<int>(ImuBiasCovarianceIndex::PositionX)
static constexpr int	xrt::tracking::constellation::optimizer::kNumImuBiasCovIndices = static_cast<int>(ImuBiasCovarianceIndex::NumIndices)

Detailed Description

Generic math helpers for IMU preintegration.

Author

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

Typedef Documentation

PreintegrationCovarianceMatrix

typedef Matrix<double, kNumImuBiasCovIndices, kNumImuBiasCovIndices> xrt::tracking::constellation::optimizer::PreintegrationCovarianceMatrix

[rotation; velocity; position], the order computeImuResidual writes its residual in.

Function Documentation

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().

Variable Documentation

kAccelNoiseDensity

constexpr double xrt::tracking::constellation::optimizer::kAccelNoiseDensity = 1.47e-3

constexpr

BMI055 accelerometer noise density, 150 ug/sqrt(Hz) from the datasheet, in (m/s^2)/sqrt(Hz).

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

kGyroNoiseDensity

constexpr double xrt::tracking::constellation::optimizer::kGyroNoiseDensity = 2.44e-4

constexpr

BMI055 gyroscope noise density, 0.014 deg/s/sqrt(Hz) from the datasheet, in rad/s/sqrt(Hz).

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

kKeyframeGroupingToleranceNs

constexpr time_duration_ns xrt::tracking::constellation::optimizer::kKeyframeGroupingToleranceNs = U_TIME_1MS_IN_NS

constexpr

Camera samples from the same mosaic closer together than this are treated as one keyframe.

Sometimes frame timestamps are a couple microseconds off due to guesswork/interpolation, this groups them together.

Todo:

We should by default group based on sequence_id! However, to make external users more reliable, let's not rely on that just yet until the infrasturcture for matching sequence ID across multiple cameras is available.

Referenced by xrt::tracking::constellation::optimizer::sensor_fusion::SensorFusion::processCameraSampleLocked().

kMaxAccelBias

constexpr double xrt::tracking::constellation::optimizer::kMaxAccelBias = 1.0

constexpr

Largest accelerometer bias to allow, in m/s^2. The BMI055's zero-g offset is +-70mg, so this is ~1.4x that.

kMaxGyroBias

constexpr double xrt::tracking::constellation::optimizer::kMaxGyroBias = 0.05

constexpr

Largest gyroscope bias to allow, in rad/s. The BMI055's zero-rate offset is +-1 deg/s, this is ~3 deg/s.