19#include <Eigen/Geometry>
34namespace xrt::tracking::constellation::optimizer {
41using Eigen::MatrixBase;
42using Eigen::Quaternion;
43using Eigen::Quaterniond;
59constexpr double kAccelBiasRandomWalkSigma = 1e-3;
60constexpr double kGyroBiasRandomWalkSigma = 1e-4;
61constexpr double kAccelScaleRandomWalkSigma = 1e-5;
64constexpr double kAccelBiasAnchorSigma = 0.687;
65constexpr double kGyroBiasAnchorSigma = 0.0175;
66constexpr double kAccelScaleAnchorSigma = 0.05;
74constexpr double kMinAccelScale = 0.8;
75constexpr double kMaxAccelScale = 1.0 / kMinAccelScale;
86enum class ImuBiasStateIndex :
int
103typedef ceres::Jet<double, static_cast<int>(ImuBiasStateIndex::NumIndices)> ImuBiasJet;
113 static_assert(!std::is_const_v<T>,
114 "block scalar must not be const — did a `T*` overload deduce T = const double?");
116 static constexpr int kNumParameters =
static_cast<int>(ImuBiasStateIndex::NumIndices);
118 static constexpr int kAccelBiasIndex =
static_cast<int>(ImuBiasStateIndex::AccelBiasX);
119 static constexpr int kGyroBiasIndex =
static_cast<int>(ImuBiasStateIndex::GyroBiasX);
120 static constexpr int kAccelScaleIndex =
static_cast<int>(ImuBiasStateIndex::AccelScaleX);
132 : accel_bias(accel_bias), gyro_bias(gyro_bias), accel_scale(accel_scale)
135 ImuBias(Eigen::Ref<
const Vector<T, kNumParameters>> parameters)
136 : accel_bias(parameters.template segment<3>(kAccelBiasIndex)),
137 gyro_bias(parameters.template segment<3>(kGyroBiasIndex)),
138 accel_scale(parameters.template segment<3>(kAccelScaleIndex))
142 pack(Eigen::Ref<Vector<T, kNumParameters>> parameters)
144 parameters.template segment<3>(kAccelBiasIndex) = this->accel_bias;
145 parameters.template segment<3>(kGyroBiasIndex) = this->gyro_bias;
146 parameters.template segment<3>(kAccelScaleIndex) = this->accel_scale;
153 seeded.accel_bias = {
154 {this->accel_bias.x(), kAccelBiasIndex + 0},
155 {this->accel_bias.y(), kAccelBiasIndex + 1},
156 {this->accel_bias.z(), kAccelBiasIndex + 2},
159 {this->gyro_bias.x(), kGyroBiasIndex + 0},
160 {this->gyro_bias.y(), kGyroBiasIndex + 1},
161 {this->gyro_bias.z(), kGyroBiasIndex + 2},
163 seeded.accel_scale = {
164 {this->accel_scale.x(), kAccelScaleIndex + 0},
165 {this->accel_scale.y(), kAccelScaleIndex + 1},
166 {this->accel_scale.z(), kAccelScaleIndex + 2},
172enum class PoseCovarianceIndex :
int
189static constexpr int kPosePosCovIndex =
static_cast<int>(PoseCovarianceIndex::PositionX);
190static constexpr int kPoseRotCovIndex =
static_cast<int>(PoseCovarianceIndex::RotX);
191static constexpr int kPoseVelCovIndex =
static_cast<int>(PoseCovarianceIndex::VelocityX);
192static constexpr int kNumPoseCovIndices =
static_cast<int>(PoseCovarianceIndex::NumIndices);
211 static constexpr int kNumResiduals = 9;
230 Vector3d accel_bias_at_integration;
231 Vector3d gyro_bias_at_integration;
233 Vector3d accel_scale_at_integration;
254 .delta_velocity = Vector3d::Zero(),
255 .delta_position = Vector3d::Zero(),
256 .body_angular_velocity_valid =
false,
257 .body_angular_velocity = Vector3d::Zero(),
259 .whitening = PoseCovarianceMatrix::Zero(),
263 .accel_bias_at_integration = Vector3d::Zero(),
264 .gyro_bias_at_integration = Vector3d::Zero(),
265 .accel_scale_at_integration = Vector3d::Constant(1.0),
267 .J_R_bg = Matrix3d::Zero(),
268 .J_v_ba = Matrix3d::Zero(),
269 .J_v_sa = Matrix3d::Zero(),
270 .J_v_bg = Matrix3d::Zero(),
271 .J_p_ba = Matrix3d::Zero(),
272 .J_p_sa = Matrix3d::Zero(),
273 .J_p_bg = Matrix3d::Zero(),
289preintegrate(
const std::span<const xrt_imu_sample> &imu_samples,
309 Vector<T, PreintegratedImuSamples::kNumResiduals> &residual)
312 start_T_world_device.getQuaternion(Q_world_device_start);
313 auto start_Q_device_world = Q_world_device_start.conjugate();
316 end_T_world_device.getQuaternion(end_Q_world_device);
321 auto end_predicted_Q_start = start_Q_device_world * end_Q_world_device;
322 auto predicted_start_delta_velocity = start_Q_device_world *
323 (end_T_world_device.velocity -
324 start_T_world_device.velocity -
325 (gravity_vector * imu_preintegration.dt));
326 auto predicted_start_delta_position =
327 start_Q_device_world *
328 (end_T_world_device.translation -
329 start_T_world_device.translation -
330 start_T_world_device.velocity * imu_preintegration.dt -
331 (T(0.5) * gravity_vector * imu_preintegration.dt * imu_preintegration.dt));
334 Vector3<T> delta_accel_bias = imu_bias.accel_bias - imu_preintegration.accel_bias_at_integration.cast<T>();
335 Vector3<T> delta_gyro_bias = imu_bias.gyro_bias - imu_preintegration.gyro_bias_at_integration.cast<T>();
336 Vector3<T> delta_accel_scale = imu_bias.accel_scale - imu_preintegration.accel_scale_at_integration.cast<T>();
346 + imu_preintegration.
J_v_ba.cast<T>() * delta_accel_bias
347 + imu_preintegration.
J_v_bg.cast<T>() * delta_gyro_bias
348 + imu_preintegration.
J_v_sa.cast<T>() * delta_accel_scale;
352 + imu_preintegration.
J_p_ba.cast<T>() * delta_accel_bias
353 + imu_preintegration.
J_p_bg.cast<T>() * delta_gyro_bias
354 + imu_preintegration.
J_p_sa.cast<T>() * delta_accel_scale;
357 auto delta_rotation_error = corrected_Q_start_end.conjugate() * end_predicted_Q_start;
358 if (delta_rotation_error.w() < T(0)) {
359 delta_rotation_error.coeffs() = -delta_rotation_error.coeffs();
362 residual.template segment<3>(kPosePosCovIndex) =
363 predicted_start_delta_position - corrected_start_delta_position;
364 residual.template segment<3>(kPoseRotCovIndex) =
quat_ln_so3(delta_rotation_error);
365 residual.template segment<3>(kPoseVelCovIndex) =
366 predicted_start_delta_velocity - corrected_start_delta_velocity;
384 template <
typename T>
386 operator()(
const T *
const world_gravity_parameters,
387 const T *
const imu_bias_parameters,
388 const T *
const imu_extrinsics_parameters,
389 const T *
const start_keyframe_parameters,
390 const T *
const end_keyframe_parameters,
394 Map<const Vector<T, WorldGravity<T>::kNumParameters>>(world_gravity_parameters),
397 const ImuBias<T> imu_bias{Map<const Vector<T, ImuBias<T>::kNumParameters>>(imu_bias_parameters)};
400 Map<const Vector<T, ImuExtrinsics<T>::kNumParameters>>(imu_extrinsics_parameters),
404 Map<const Vector<T, PoseWithVelocity<T>::kNumParameters>>(start_keyframe_parameters),
407 Map<const Vector<T, PoseWithVelocity<T>::kNumParameters>>(end_keyframe_parameters),
410 Vector<T, PreintegratedImuSamples::kNumResiduals> computed_residual;
411 computeImuResidual<T>(this->preintegration,
412 imu_extrinsics.toWorldImu(start_T_world_model),
413 imu_extrinsics.toWorldImu(end_T_world_model),
418 Map<Vector<T, PreintegratedImuSamples::kNumResiduals>> out_residual(residuals);
419 out_residual = this->preintegration.
whitening.cast<T>() * computed_residual;
426 PreintegratedImuSamples::kNumResiduals,
442 template <
typename T>
444 operator()(
const T *
const start_bias_parameters,
445 const T *
const end_bias_parameters,
448 const ImuBias<T> start{Map<const Vector<T, ImuBias<T>::kNumParameters>>(start_bias_parameters)};
449 const ImuBias<T> end{Map<const Vector<T, ImuBias<T>::kNumParameters>>(end_bias_parameters)};
453 const T sqrt_dt = T(std::sqrt(this->dt));
455 Map<Vector3<T>> accel_residual(residuals + 0);
456 Map<Vector3<T>> gyro_residual(residuals + 3);
457 Map<Vector3<T>> scale_residual(residuals + 6);
463 accel_residual = (end.accel_bias - start.accel_bias) / (T(kAccelBiasRandomWalkSigma) * sqrt_dt);
464 gyro_residual = (end.gyro_bias - start.gyro_bias) / (T(kGyroBiasRandomWalkSigma) * sqrt_dt);
465 scale_residual = (end.accel_scale - start.accel_scale) / (T(kAccelScaleRandomWalkSigma) * sqrt_dt);
474 ImuBiasRandomWalkCostFunctor::kNumResiduals,
477 ImuBiasRandomWalkCostFunction;
486 template <
typename T>
488 operator()(
const T *
const bias_parameters,
491 const ImuBias<T> bias{Map<const Vector<T, ImuBias<T>::kNumParameters>>(bias_parameters)};
493 Map<Vector3<T>> accel_residual(residuals + 0);
494 Map<Vector3<T>> gyro_residual(residuals + 3);
495 Map<Vector3<T>> scale_residual(residuals + 6);
498 accel_residual = (bias.accel_bias - this->anchor.accel_bias) / T(kAccelBiasAnchorSigma);
499 gyro_residual = (bias.gyro_bias - this->anchor.gyro_bias) / T(kGyroBiasAnchorSigma);
500 scale_residual = (bias.accel_scale - this->anchor.accel_scale) / T(kAccelScaleAnchorSigma);
509 ImuBiasAnchorCostFunctor::kNumResiduals,
511 ImuBiasAnchorCostFunction;
Definition utility_northstar.h:779
Definition utility_northstar.h:16
int64_t timepoint_ns
Integer timestamp type.
Definition u_time.h:77
int64_t time_duration_ns
Integer duration type in nanoseconds.
Definition u_time.h:88
constexpr double kMaxGyroBias
Largest gyroscope bias to allow, in rad/s. The BMI055's zero-rate offset is +-1 deg/s,...
Definition imu_preintegration.hpp:71
constexpr time_duration_ns kKeyframeGroupingToleranceNs
Camera samples from the same mosaic closer together than this are treated as one keyframe.
Definition imu_preintegration.hpp:84
PreintegratedImuSamples 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,...
Definition internal_math.cpp:406
Matrix< double, kNumPoseCovIndices, kNumPoseCovIndices > PoseCovarianceMatrix
[position; rotation; velocity], the order computeImuResidual writes its residual in.
Definition imu_preintegration.hpp:195
constexpr double kMaxAccelBias
Largest accelerometer bias to allow, in m/s^2. The BMI055's zero-g offset is +-70mg,...
Definition imu_preintegration.hpp:69
constexpr double kGyroNoiseDensity
BMI055 gyroscope noise density, 0.014 deg/s/sqrt(Hz) from the datasheet, in rad/s/sqrt(Hz).
Definition imu_preintegration.hpp:54
constexpr double kAccelNoiseDensity
BMI055 accelerometer noise density, 150 ug/sqrt(Hz) from the datasheet, in (m/s^2)/sqrt(Hz).
Definition imu_preintegration.hpp:56
void covariancePositionRadius(const PoseCovarianceMatrix &covariance, xrt_vec3 &out_radii, xrt_quat &out_Q_origin_cov)
Computes the radii and covariance orientation from the 3x3 position part of the pose covariance matri...
Definition internal_math.cpp:295
Generic math helpers for constellation optimizer code.
Interoperability helpers connecting internal math types and Eigen.
Base implementations for math library.
Helpers for quatexpmap math for bigceres usage.
C++-only functionality in the Math helper library.
Definition m_documentation.hpp:15
Eigen::Quaternion< typename Derived::Scalar > quat_exp_so3(Eigen::MatrixBase< Derived > const &vec)
Fully-templated free function for quaternion exponentiation, SO(3) version as described by Grassia.
Definition m_quatexpmap.hpp:119
Eigen::Matrix< Scalar, 3, 1 > quat_ln_so3(Eigen::Quaternion< Scalar > const &quat)
Fully-templated free function for quaternion log map, SO(3) version.
Definition m_quatexpmap.hpp:148
Wrapper around OS native time functions.
IMU Bias anchor cost functor, to hold it at some value when marginalization has no guess for it.
Definition imu_preintegration.hpp:481
A full-state IMU bias, with Accelerometer Bias, Gyroscope Bias, and Accelerometer Scale.
Definition imu_preintegration.hpp:112
IMU Bias random walk cost functor, between each keyframe.
Definition imu_preintegration.hpp:436
double dt
Seconds spanned by this interval.
Definition imu_preintegration.hpp:440
IMU preintegration factor, between each camera keyframe.
Definition imu_preintegration.hpp:381
The LED model -> IMU rotation.
Definition internal_math.hpp:377
Pose with 9 degrees of freedom, translation, rotation, and linear velocity.
Definition internal_math.hpp:340
Definition imu_preintegration.hpp:210
Matrix3d J_p_sa
d(delta_position) / d(accel_scale)
Definition imu_preintegration.hpp:246
Vector3d body_angular_velocity
The body-frame angular velocity at the end of the window. Raw reading, apply calibration before using...
Definition imu_preintegration.hpp:223
Matrix3d J_v_sa
d(delta_velocity) / d(accel_scale)
Definition imu_preintegration.hpp:242
PoseCovarianceMatrix whitening
The whitening matrix for this preintegration, the lower triangular.
Definition imu_preintegration.hpp:226
Quaterniond Q_start_end
The preintegrated rotation from the IMU samples, as a quaternion.
Definition imu_preintegration.hpp:214
Matrix3d J_v_bg
d(delta_velocity) / d(gyro_bias)
Definition imu_preintegration.hpp:243
Matrix3d J_p_bg
d(delta_position) / d(gyro_bias)
Definition imu_preintegration.hpp:247
bool body_angular_velocity_valid
Whether the body angular velocity is valid.
Definition imu_preintegration.hpp:221
Matrix3d J_p_ba
d(delta_position) / d(accel_bias)
Definition imu_preintegration.hpp:245
Matrix3d J_v_ba
d(delta_velocity) / d(accel_bias)
Definition imu_preintegration.hpp:241
Vector3d delta_position
This is really "delta double-integrated specific force", but we call it delta_position for simplicity...
Definition imu_preintegration.hpp:218
Matrix3d J_R_bg
d(Q_start_cur) / d(gyro_bias)
Definition imu_preintegration.hpp:239
Vector3d delta_velocity
This is really "delta integrated specific force", but we call it delta_velocity for simplicity.
Definition imu_preintegration.hpp:216
The strength of gravity in the world, as a parameter block of its own.
Definition internal_math.hpp:193
Vector3< T > toVector() const
Gravity as a vector, which points along +Y since world up is -Y in the OpenCV space the optimizer wor...
Definition internal_math.hpp:219
IMU Sample.
Definition xrt_tracking.h:143
A quaternion with single floats.
Definition xrt_defines.h:246
A 3 element vector with single floats.
Definition xrt_defines.h:310
#define U_TIME_1MS_IN_NS
The number of nanoseconds in a millisecond.
Definition u_time.h:54