pi/ar_basalt
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

v01

Browse Source
This commit is contained in:
Ivan
2022-04-05 11:42:28 +03:00
commit 6dc0eb0fcf
5565 changed files with 1200500 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

283
thirdparty/basalt-headers/include/basalt/imu/imu_types.h vendored Normal file
View File

@@ -0,0 +1,283 @@
/**
BSD 3-Clause License
This file is part of the Basalt project.
https://gitlab.com/VladyslavUsenko/basalt-headers.git
Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
* Neither the name of the copyright holder nor the names of its
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
@file
@brief Type definitions for IMU preintegration
*/
#pragma once
#include <basalt/utils/sophus_utils.hpp>
#include <memory>
namespace basalt {
constexpr size_t POSE_SIZE = 6; ///< Dimentionality of the pose state
constexpr size_t POSE_VEL_SIZE =
9; ///< Dimentionality of the pose-velocity state
constexpr size_t POSE_VEL_BIAS_SIZE =
15; ///< Dimentionality of the pose-velocity-bias state
/// @brief State that consists of SE(3) pose at a certain time.
template <class Scalar_>
struct PoseState {
using Scalar = Scalar_;
using VecN = Eigen::Matrix<Scalar, POSE_SIZE, 1>;
using Vec6 = Eigen::Matrix<Scalar, 6, 1>;
using SO3 = Sophus::SO3<Scalar>;
using SE3 = Sophus::SE3<Scalar>;
/// @brief Default constructor with Identity pose and zero timestamp.
PoseState() { t_ns = 0; }
/// @brief Constructor with timestamp and pose.
///
/// @param t_ns timestamp of the state in nanoseconds
/// @param T_w_i transformation from the body frame to the world frame
PoseState(int64_t t_ns, const SE3& T_w_i) : t_ns(t_ns), T_w_i(T_w_i) {}
/// @brief Create copy with different Scalar type.
template <class Scalar2>
PoseState<Scalar2> cast() const {
PoseState<Scalar2> a;
a.t_ns = t_ns;
a.T_w_i = T_w_i.template cast<Scalar2>();
return a;
}
/// @brief Apply increment to the pose
///
/// For details see \ref incPose
/// @param[in] inc 6x1 increment vector
void applyInc(const VecN& inc) { incPose(inc, T_w_i); }
/// @brief Apply increment to the pose
///
/// The incremernt vector consists of translational and rotational parts \f$
/// [\upsilon, \omega]^T \f$. Given the current pose \f$ R \in
/// SO(3), p \in \mathbb{R}^3\f$ the updated pose is: \f{align}{ R' &=
/// \exp(\omega) R
/// \\ p' &= p + \upsilon
/// \f}
/// The increment is consistent with \ref
/// Se3Spline::applyInc.
///
/// @param[in] inc 6x1 increment vector
/// @param[in,out] T the pose to update
inline static void incPose(const Vec6& inc, SE3& T) {
T.translation() += inc.template head<3>();
T.so3() = SO3::exp(inc.template tail<3>()) * T.so3();
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
int64_t t_ns; ///< timestamp of the state in nanoseconds
SE3 T_w_i; ///< pose of the state
};
/// @brief State that consists of SE(3) pose and linear velocity at a certain
/// time.
template <class Scalar_>
struct PoseVelState : public PoseState<Scalar_> {
using Scalar = Scalar_;
using VecN = Eigen::Matrix<Scalar, POSE_VEL_SIZE, 1>;
using Vec3 = Eigen::Matrix<Scalar, 3, 1>;
using SE3 = Sophus::SE3<Scalar>;
/// @brief Default constructor with Identity pose and zero other values.
PoseVelState() { vel_w_i.setZero(); };
/// @brief Constructor with timestamp, pose and linear velocity.
///
/// @param t_ns timestamp of the state in nanoseconds
/// @param T_w_i transformation from the body frame to the world frame
/// @param vel_w_i linear velocity in world coordinate frame
PoseVelState(int64_t t_ns, const SE3& T_w_i, const Vec3& vel_w_i)
: PoseState<Scalar>(t_ns, T_w_i), vel_w_i(vel_w_i) {}
/// @brief Create copy with different Scalar type.
template <class Scalar2>
PoseVelState<Scalar2> cast() const {
PoseVelState<Scalar2> a;
static_cast<PoseState<Scalar2>&>(a) =
PoseState<Scalar>::template cast<Scalar2>();
a.vel_w_i = vel_w_i.template cast<Scalar2>();
return a;
}
/// @brief Apply increment to the state.
///
/// For pose see \ref incPose. For velocity simple addition.
/// @param[in] inc 9x1 increment vector [trans, rot, vel]
void applyInc(const VecN& inc) {
PoseState<Scalar>::applyInc(inc.template head<6>());
vel_w_i += inc.template tail<3>();
}
/// @brief Compute difference to other state.
///
/// ```
/// PoseVelState::VecN inc;
/// PoseVelState p0, p1;
/// // Initialize p0 and inc
/// p1 = p0;
/// p1.applyInc(inc);
/// p0.diff(p1) == inc; // Should be true.
/// ```
/// @param other state to compute difference.
VecN diff(const PoseVelState<Scalar>& other) const {
VecN res;
res.template segment<3>(0) =
other.T_w_i.translation() - this->T_w_i.translation();
res.template segment<3>(3) =
(other.T_w_i.so3() * this->T_w_i.so3().inverse()).log();
res.template tail<3>() = other.vel_w_i - vel_w_i;
return res;
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Vec3 vel_w_i; ///< Linear velocity of the state
};
/// @brief State that consists of SE(3) pose, linear velocity, gyroscope and
/// accelerometer biases at a certain time.
template <class Scalar_>
struct PoseVelBiasState : public PoseVelState<Scalar_> {
using Scalar = Scalar_;
using Ptr = std::shared_ptr<PoseVelBiasState<Scalar>>;
using VecN = Eigen::Matrix<Scalar, POSE_VEL_BIAS_SIZE, 1>;
using Vec3 = Eigen::Matrix<Scalar, 3, 1>;
using SO3 = Sophus::SO3<Scalar>;
using SE3 = Sophus::SE3<Scalar>;
/// @brief Default constructor with Identity pose and zero other values.
PoseVelBiasState() {
bias_gyro.setZero();
bias_accel.setZero();
};
/// @brief Constructor with timestamp, pose, linear velocity, gyroscope and
/// accelerometer biases.
///
/// @param t_ns timestamp of the state in nanoseconds
/// @param T_w_i transformation from the body frame to the world frame
/// @param vel_w_i linear velocity in world coordinate frame
/// @param bias_gyro gyroscope bias
/// @param bias_accel accelerometer bias
PoseVelBiasState(int64_t t_ns, const SE3& T_w_i, const Vec3& vel_w_i,
const Vec3& bias_gyro, const Vec3& bias_accel)
: PoseVelState<Scalar>(t_ns, T_w_i, vel_w_i),
bias_gyro(bias_gyro),
bias_accel(bias_accel) {}
/// @brief Create copy with different Scalar type.
template <class Scalar2>
PoseVelBiasState<Scalar2> cast() const {
PoseVelBiasState<Scalar2> a;
static_cast<PoseVelState<Scalar2>&>(a) =
PoseVelState<Scalar>::template cast<Scalar2>();
a.bias_gyro = bias_gyro.template cast<Scalar2>();
a.bias_accel = bias_accel.template cast<Scalar2>();
return a;
}
/// @brief Apply increment to the state.
///
/// For pose see \ref incPose. For velocity and biases simple addition.
/// @param[in] inc 15x1 increment vector [trans, rot, vel, bias_gyro,
/// bias_accel]
void applyInc(const VecN& inc) {
PoseVelState<Scalar>::applyInc(inc.template head<9>());
bias_gyro += inc.template segment<3>(9);
bias_accel += inc.template segment<3>(12);
}
/// @brief Compute difference to other state.
///
/// ```
/// PoseVelBiasState::VecN inc;
/// PoseVelBiasState p0, p1;
/// // Initialize p0 and inc
/// p1 = p0;
/// p1.applyInc(inc);
/// p0.diff(p1) == inc; // Should be true.
/// ```
/// @param other state to compute difference.
VecN diff(const PoseVelBiasState<Scalar>& other) const {
VecN res;
res.segment<9>(0) = PoseVelState<Scalar>::diff(other);
res.segment<3>(9) = other.bias_gyro - bias_gyro;
res.segment<3>(12) = other.bias_accel - bias_accel;
return res;
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Vec3 bias_gyro; ///< Gyroscope bias
Vec3 bias_accel; ///< Accelerometer bias
};
/// @brief Timestamped gyroscope and accelerometer measurements.
template <class Scalar_>
struct ImuData {
using Scalar = Scalar_;
using Ptr = std::shared_ptr<ImuData>;
using Vec3 = Eigen::Matrix<Scalar, 3, 1>;
int64_t t_ns; ///< timestamp in nanoseconds
Vec3 accel; ///< Accelerometer measurement
Vec3 gyro; ///< Gyroscope measurement
/// @brief Default constructor with zero measurements.
ImuData() {
t_ns = 0;
accel.setZero();
gyro.setZero();
}
/// @brief Create copy with different Scalar type.
template <class Scalar2>
ImuData<Scalar2> cast() const {
ImuData<Scalar2> a;
a.t_ns = t_ns;
a.accel = accel.template cast<Scalar2>();
a.gyro = gyro.template cast<Scalar2>();
return a;
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace basalt

371
thirdparty/basalt-headers/include/basalt/imu/preintegration.h vendored Normal file
View File

@@ -0,0 +1,371 @@
/**
BSD 3-Clause License
This file is part of the Basalt project.
https://gitlab.com/VladyslavUsenko/basalt-headers.git
Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
* Neither the name of the copyright holder nor the names of its
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
@file
@brief IMU preintegration
*/
#pragma once
#include <basalt/imu/imu_types.h>
#include <basalt/utils/assert.h>
#include <basalt/utils/sophus_utils.hpp>
namespace basalt {
/// @brief Integrated pseudo-measurement that combines several consecutive IMU
/// measurements.
template <class Scalar_>
class IntegratedImuMeasurement {
public:
using Scalar = Scalar_;
using Ptr = std::shared_ptr<IntegratedImuMeasurement<Scalar>>;
using Vec3 = Eigen::Matrix<Scalar, 3, 1>;
using VecN = Eigen::Matrix<Scalar, POSE_VEL_SIZE, 1>;
using Mat3 = Eigen::Matrix<Scalar, 3, 3>;
using MatNN = Eigen::Matrix<Scalar, POSE_VEL_SIZE, POSE_VEL_SIZE>;
using MatN3 = Eigen::Matrix<Scalar, POSE_VEL_SIZE, 3>;
using MatN6 = Eigen::Matrix<Scalar, POSE_VEL_SIZE, 6>;
using SO3 = Sophus::SO3<Scalar>;
/// @brief Propagate current state given ImuData and optionally compute
/// Jacobians.
///
/// @param[in] curr_state current state
/// @param[in] data IMU data
/// @param[out] next_state predicted state
/// @param[out] d_next_d_curr Jacobian of the predicted state with respect
/// to current state
/// @param[out] d_next_d_accel Jacobian of the predicted state with respect
/// accelerometer measurement
/// @param[out] d_next_d_gyro Jacobian of the predicted state with respect
/// gyroscope measurement
inline static void propagateState(const PoseVelState<Scalar>& curr_state,
const ImuData<Scalar>& data,
PoseVelState<Scalar>& next_state,
MatNN* d_next_d_curr = nullptr,
MatN3* d_next_d_accel = nullptr,
MatN3* d_next_d_gyro = nullptr) {
BASALT_ASSERT_STREAM(
data.t_ns > curr_state.t_ns,
"data.t_ns " << data.t_ns << " curr_state.t_ns " << curr_state.t_ns);
int64_t dt_ns = data.t_ns - curr_state.t_ns;
Scalar dt = dt_ns * Scalar(1e-9);
SO3 R_w_i_new_2 =
curr_state.T_w_i.so3() * SO3::exp(Scalar(0.5) * dt * data.gyro);
Mat3 RR_w_i_new_2 = R_w_i_new_2.matrix();
Vec3 accel_world = RR_w_i_new_2 * data.accel;
next_state.t_ns = data.t_ns;
next_state.T_w_i.so3() = curr_state.T_w_i.so3() * SO3::exp(dt * data.gyro);
next_state.vel_w_i = curr_state.vel_w_i + accel_world * dt;
next_state.T_w_i.translation() = curr_state.T_w_i.translation() +
curr_state.vel_w_i * dt +
0.5 * accel_world * dt * dt;
if (d_next_d_curr) {
d_next_d_curr->setIdentity();
d_next_d_curr->template block<3, 3>(0, 6).diagonal().setConstant(dt);
d_next_d_curr->template block<3, 3>(6, 3) = SO3::hat(-accel_world * dt);
d_next_d_curr->template block<3, 3>(0, 3) =
d_next_d_curr->template block<3, 3>(6, 3) * dt * Scalar(0.5);
}
if (d_next_d_accel) {
d_next_d_accel->setZero();
d_next_d_accel->template block<3, 3>(0, 0) =
Scalar(0.5) * RR_w_i_new_2 * dt * dt;
d_next_d_accel->template block<3, 3>(6, 0) = RR_w_i_new_2 * dt;
}
if (d_next_d_gyro) {
d_next_d_gyro->setZero();
Mat3 Jr;
Sophus::rightJacobianSO3(dt * data.gyro, Jr);
Mat3 Jr2;
Sophus::rightJacobianSO3(Scalar(0.5) * dt * data.gyro, Jr2);
d_next_d_gyro->template block<3, 3>(3, 0) =
next_state.T_w_i.so3().matrix() * Jr * dt;
d_next_d_gyro->template block<3, 3>(6, 0) =
SO3::hat(-accel_world * dt) * RR_w_i_new_2 * Jr2 * Scalar(0.5) * dt;
d_next_d_gyro->template block<3, 3>(0, 0) =
Scalar(0.5) * dt * d_next_d_gyro->template block<3, 3>(6, 0);
}
}
/// @brief Default constructor.
IntegratedImuMeasurement() noexcept {
cov_.setZero();
d_state_d_ba_.setZero();
d_state_d_bg_.setZero();
bias_gyro_lin_.setZero();
bias_accel_lin_.setZero();
}
/// @brief Constructor with start time and bias estimates.
IntegratedImuMeasurement(int64_t start_t_ns, const Vec3& bias_gyro_lin,
const Vec3& bias_accel_lin) noexcept
: start_t_ns_(start_t_ns),
bias_gyro_lin_(bias_gyro_lin),
bias_accel_lin_(bias_accel_lin) {
cov_.setZero();
d_state_d_ba_.setZero();
d_state_d_bg_.setZero();
}
/// @brief Integrate IMU data
///
/// @param[in] data IMU data
/// @param[in] accel_cov diagonal of accelerometer noise covariance matrix
/// @param[in] gyro_cov diagonal of gyroscope noise covariance matrix
void integrate(const ImuData<Scalar>& data, const Vec3& accel_cov,
const Vec3& gyro_cov) {
ImuData<Scalar> data_corrected = data;
data_corrected.t_ns -= start_t_ns_;
data_corrected.accel -= bias_accel_lin_;
data_corrected.gyro -= bias_gyro_lin_;
PoseVelState<Scalar> new_state;
MatNN F;
MatN3 A;
MatN3 G;
propagateState(delta_state_, data_corrected, new_state, &F, &A, &G);
delta_state_ = new_state;
cov_ = F * cov_ * F.transpose() +
A * accel_cov.asDiagonal() * A.transpose() +
G * gyro_cov.asDiagonal() * G.transpose();
sqrt_cov_inv_computed_ = false;
d_state_d_ba_ = -A + F * d_state_d_ba_;
d_state_d_bg_ = -G + F * d_state_d_bg_;
}
/// @brief Predict state given this pseudo-measurement
///
/// @param[in] state0 current state
/// @param[in] g gravity vector
/// @param[out] state1 predicted state
void predictState(const PoseVelState<Scalar>& state0, const Vec3& g,
PoseVelState<Scalar>& state1) const {
Scalar dt = delta_state_.t_ns * Scalar(1e-9);
state1.T_w_i.so3() = state0.T_w_i.so3() * delta_state_.T_w_i.so3();
state1.vel_w_i =
state0.vel_w_i + g * dt + state0.T_w_i.so3() * delta_state_.vel_w_i;
state1.T_w_i.translation() =
state0.T_w_i.translation() + state0.vel_w_i * dt +
Scalar(0.5) * g * dt * dt +
state0.T_w_i.so3() * delta_state_.T_w_i.translation();
}
/// @brief Compute residual between two states given this pseudo-measurement
/// and optionally compute Jacobians.
///
/// @param[in] state0 initial state
/// @param[in] g gravity vector
/// @param[in] state1 next state
/// @param[in] curr_bg current estimate of gyroscope bias
/// @param[in] curr_ba current estimate of accelerometer bias
/// @param[out] d_res_d_state0 if not nullptr, Jacobian of the residual with
/// respect to state0
/// @param[out] d_res_d_state1 if not nullptr, Jacobian of the residual with
/// respect to state1
/// @param[out] d_res_d_bg if not nullptr, Jacobian of the residual with
/// respect to gyroscope bias
/// @param[out] d_res_d_ba if not nullptr, Jacobian of the residual with
/// respect to accelerometer bias
/// @return residual
VecN residual(const PoseVelState<Scalar>& state0, const Vec3& g,
const PoseVelState<Scalar>& state1, const Vec3& curr_bg,
const Vec3& curr_ba, MatNN* d_res_d_state0 = nullptr,
MatNN* d_res_d_state1 = nullptr, MatN3* d_res_d_bg = nullptr,
MatN3* d_res_d_ba = nullptr) const {
Scalar dt = delta_state_.t_ns * Scalar(1e-9);
VecN res;
VecN bg_diff;
VecN ba_diff;
bg_diff = d_state_d_bg_ * (curr_bg - bias_gyro_lin_);
ba_diff = d_state_d_ba_ * (curr_ba - bias_accel_lin_);
BASALT_ASSERT(ba_diff.template segment<3>(3).isApproxToConstant(0));
Mat3 R0_inv = state0.T_w_i.so3().inverse().matrix();
Vec3 tmp =
R0_inv * (state1.T_w_i.translation() - state0.T_w_i.translation() -
state0.vel_w_i * dt - Scalar(0.5) * g * dt * dt);
res.template segment<3>(0) =
tmp - (delta_state_.T_w_i.translation() +
bg_diff.template segment<3>(0) + ba_diff.template segment<3>(0));
res.template segment<3>(3) =
(SO3::exp(bg_diff.template segment<3>(3)) * delta_state_.T_w_i.so3() *
state1.T_w_i.so3().inverse() * state0.T_w_i.so3())
.log();
Vec3 tmp2 = R0_inv * (state1.vel_w_i - state0.vel_w_i - g * dt);
res.template segment<3>(6) =
tmp2 - (delta_state_.vel_w_i + bg_diff.template segment<3>(6) +
ba_diff.template segment<3>(6));
if (d_res_d_state0 || d_res_d_state1) {
Mat3 J;
Sophus::rightJacobianInvSO3(res.template segment<3>(3), J);
if (d_res_d_state0) {
d_res_d_state0->setZero();
d_res_d_state0->template block<3, 3>(0, 0) = -R0_inv;
d_res_d_state0->template block<3, 3>(0, 3) = SO3::hat(tmp) * R0_inv;
d_res_d_state0->template block<3, 3>(3, 3) = J * R0_inv;
d_res_d_state0->template block<3, 3>(6, 3) = SO3::hat(tmp2) * R0_inv;
d_res_d_state0->template block<3, 3>(0, 6) = -R0_inv * dt;
d_res_d_state0->template block<3, 3>(6, 6) = -R0_inv;
}
if (d_res_d_state1) {
d_res_d_state1->setZero();
d_res_d_state1->template block<3, 3>(0, 0) = R0_inv;
d_res_d_state1->template block<3, 3>(3, 3) = -J * R0_inv;
d_res_d_state1->template block<3, 3>(6, 6) = R0_inv;
}
}
if (d_res_d_ba) {
*d_res_d_ba = -d_state_d_ba_;
}
if (d_res_d_bg) {
d_res_d_bg->setZero();
*d_res_d_bg = -d_state_d_bg_;
Mat3 J;
Sophus::leftJacobianInvSO3(res.template segment<3>(3), J);
d_res_d_bg->template block<3, 3>(3, 0) =
J * d_state_d_bg_.template block<3, 3>(3, 0);
}
return res;
}
/// @brief Time duretion of preintegrated measurement in nanoseconds.
int64_t get_dt_ns() const { return delta_state_.t_ns; }
/// @brief Start time of preintegrated measurement in nanoseconds.
int64_t get_start_t_ns() const { return start_t_ns_; }
/// @brief Inverse of the measurement covariance matrix
inline MatNN get_cov_inv() const {
if (!sqrt_cov_inv_computed_) {
compute_sqrt_cov_inv();
sqrt_cov_inv_computed_ = true;
}
return sqrt_cov_inv_.transpose() * sqrt_cov_inv_;
}
/// @brief Square root inverse of the measurement covariance matrix
inline const MatNN& get_sqrt_cov_inv() const {
if (!sqrt_cov_inv_computed_) {
compute_sqrt_cov_inv();
sqrt_cov_inv_computed_ = true;
}
return sqrt_cov_inv_;
}
/// @brief Measurement covariance matrix
const MatNN& get_cov() const { return cov_; }
// Just for testing...
/// @brief Delta state
const PoseVelState<Scalar>& getDeltaState() const { return delta_state_; }
/// @brief Jacobian of delta state with respect to accelerometer bias
const MatN3& get_d_state_d_ba() const { return d_state_d_ba_; }
/// @brief Jacobian of delta state with respect to gyroscope bias
const MatN3& get_d_state_d_bg() const { return d_state_d_bg_; }
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
/// @brief Helper function to compute square root of the inverse covariance
void compute_sqrt_cov_inv() const {
sqrt_cov_inv_.setIdentity();
auto ldlt = cov_.ldlt();
sqrt_cov_inv_ = ldlt.transpositionsP() * sqrt_cov_inv_;
ldlt.matrixL().solveInPlace(sqrt_cov_inv_);
VecN D_inv_sqrt;
for (size_t i = 0; i < POSE_VEL_SIZE; i++) {
if (ldlt.vectorD()[i] < std::numeric_limits<Scalar>::min()) {
D_inv_sqrt[i] = 0;
} else {
D_inv_sqrt[i] = Scalar(1.0) / sqrt(ldlt.vectorD()[i]);
}
}
sqrt_cov_inv_ = D_inv_sqrt.asDiagonal() * sqrt_cov_inv_;
}
int64_t start_t_ns_{0}; ///< Integration start time in nanoseconds
PoseVelState<Scalar> delta_state_; ///< Delta state
MatNN cov_; ///< Measurement covariance
mutable MatNN
sqrt_cov_inv_; ///< Cached square root inverse of measurement covariance
mutable bool sqrt_cov_inv_computed_{
false}; ///< If the cached square root inverse
///< covariance is computed
MatN3 d_state_d_ba_, d_state_d_bg_;
Vec3 bias_gyro_lin_, bias_accel_lin_;
};
} // namespace basalt