diff --git a/.gitignore b/.gitignore
index b4e53703e896e0c36e288c8c4e1e13e71c42ab2d..4d9c52e82fe8dee50c99c9a1c8548575962ce658 100644
--- a/.gitignore
+++ b/.gitignore
@@ -39,4 +39,5 @@ src/examples/map_apriltag_save.yaml
.ccls-root
compile_commands.json
.vimspector.json
-est.csv
+est*.csv
+.clang-format
diff --git a/CMakeLists.txt b/CMakeLists.txt
index f3ec610aa8fa2cb2bfb2991cc3ab5eb2d45b3b62..1def785eba87f87d2ce061b93927064b906240d9 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -30,7 +30,7 @@ include(CheckCXXCompilerFlag)
CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14)
if(COMPILER_SUPPORTS_CXX14)
message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has C++14 support.")
- set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14")
+ set(CMAKE_CXX_STANDARD 14)
else()
message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++14 support. Please use a different C++ compiler.")
endif()
@@ -93,13 +93,9 @@ IF(EXISTS "${WOLF_CONFIG_DIR}" AND NOT IS_DIRECTORY "${WOLF_CONFIG_DIR}")
message(FATAL_ERROR "Bug: Specified CONFIG_DIR: "
"${WOLF_CONFIG_DIR} exists, but is not a directory.")
ENDIF()
+
# Configure config.h
configure_file(${CMAKE_CURRENT_SOURCE_DIR}/internal/config.h.in "${WOLF_CONFIG_DIR}/config.h")
-message("CONFIG DIRECTORY ${PROJECT_BINARY_DIR}")
-include_directories("${PROJECT_BINARY_DIR}/conf")
-
-# ============ INCLUDES SECTION ============
-INCLUDE_DIRECTORIES(BEFORE "include")
# ============ HEADERS ============
SET(HDRS_CAPTURE
@@ -205,9 +201,9 @@ install(
${LIB_INSTALL_DIR}/${PLUGIN_NAME}/cmake
)
-# Specifies include directories to use when compiling the plugin target
-# This way, include_directories does not need to be called in plugins depending on this one
-target_include_directories(${PLUGIN_NAME} INTERFACE
+target_include_directories(${PLUGIN_NAME} PUBLIC
+ $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+ $<BUILD_INTERFACE:${PROJECT_BINARY_DIR}/conf>
$<INSTALL_INTERFACE:${INCLUDE_INSTALL_DIR}>
)
diff --git a/demos/processor_imu.yaml b/demos/processor_imu.yaml
index e1b0c2e86eb5212679e7c75db2ba2f25fc039348..1afbb00a6ab5112cbad1555098985ae1f0687f8b 100644
--- a/demos/processor_imu.yaml
+++ b/demos/processor_imu.yaml
@@ -9,3 +9,8 @@ keyframe_vote:
max_buff_length: 20000 # motion deltas
dist_traveled: 2.0 # meters
angle_turned: 0.2 # radians (1 rad approx 57 deg, approx 60 deg)
+
+bootstrap:
+ enable: false
+ method: "G"
+ averaging_length: 0.10 # seconds
\ No newline at end of file
diff --git a/demos/processor_imu_bootstrap.yaml b/demos/processor_imu_bootstrap.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..3a6c6c9587ab23aeb66ae95495c8054be46e0c96
--- /dev/null
+++ b/demos/processor_imu_bootstrap.yaml
@@ -0,0 +1,17 @@
+type: "ProcessorImu" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+
+time_tolerance: 0.0025 # Time tolerance for joining KFs
+unmeasured_perturbation_std: 0.00001
+
+keyframe_vote:
+ voting_active: false
+ max_time_span: 2.0 # seconds
+ max_buff_length: 20000 # motion deltas
+ dist_traveled: 2.0 # meters
+ angle_turned: 0.2 # radians (1 rad approx 57 deg, approx 60 deg)
+
+bootstrap:
+ enable: true
+ method: "G" # methods: "G", "STATIC" or "V0_G"
+ averaging_length: 0.10 # seconds
+ keyframe_provider_processor_name: "processor_other_name" # Not yet implemented
\ No newline at end of file
diff --git a/demos/sensor_imu.yaml b/demos/sensor_imu.yaml
index 3c78a00d35c785fe73381d8f6ce99a705d27ce77..38a3df944cc7e8a7021e0bfe418fb5c79b324b2f 100644
--- a/demos/sensor_imu.yaml
+++ b/demos/sensor_imu.yaml
@@ -7,3 +7,5 @@ motion_variances:
wb_initial_stdev: 0.350 # rad/sec - initial bias
ab_rate_stdev: 0.1 # m/s2/sqrt(s)
wb_rate_stdev: 0.0400 # rad/s/sqrt(s)
+
+dynamic_imu_bias: true
\ No newline at end of file
diff --git a/doc/doxygen.conf b/doc/doxygen.conf
index dc3608694f177138711f769ceadb7ef004665ad4..992561fe41fb811ed1906fb54511a14cb98900c3 100644
--- a/doc/doxygen.conf
+++ b/doc/doxygen.conf
@@ -429,7 +429,7 @@ EXTRACT_PACKAGE = NO
# included in the documentation.
# The default value is: NO.
-EXTRACT_STATIC = NO
+EXTRACT_STATIC = YES
# If the EXTRACT_LOCAL_CLASSES tag is set to YES classes (and structs) defined
# locally in source files will be included in the documentation. If set to NO
@@ -756,7 +756,9 @@ WARN_LOGFILE =
# Note: If this tag is empty the current directory is searched.
INPUT = ../doc/main.dox \
- ../src
+ ../src \
+ ../include/imu \
+ conf/imu/internal
# This tag can be used to specify the character encoding of the source files
# that doxygen parses. Internally doxygen uses the UTF-8 encoding. Doxygen uses
diff --git a/include/imu/capture/capture_compass.h b/include/imu/capture/capture_compass.h
index ab684fc09f0dda7da6b7f61f2e087c7ff3f3f8ce..22e6179ead557c4d43878ab44bd92d4214df1cd1 100644
--- a/include/imu/capture/capture_compass.h
+++ b/include/imu/capture/capture_compass.h
@@ -23,8 +23,8 @@
#define CAPTURE_COMPASS_H_
//Wolf includes
-#include <core/capture/capture_base.h>
#include "imu/sensor/sensor_compass.h"
+#include <core/capture/capture_base.h>
//std includes
//
diff --git a/include/imu/factor/factor_imu.h b/include/imu/factor/factor_imu.h
index 30dcbc15e244fb46a20f7a30c00413049e176839..71dd3de29f713b439e82d8a9634cfb125fb6ef29 100644
--- a/include/imu/factor/factor_imu.h
+++ b/include/imu/factor/factor_imu.h
@@ -36,7 +36,7 @@ namespace wolf {
WOLF_PTR_TYPEDEFS(FactorImu);
//class
-class FactorImu : public FactorAutodiff<FactorImu, 15, 3, 4, 3, 6, 3, 4, 3, 6>
+class FactorImu : public FactorAutodiff<FactorImu, 9, 3, 4, 3, 6, 3, 4, 3>
{
public:
FactorImu(const FeatureImuPtr& _ftr_ptr,
@@ -61,7 +61,6 @@ class FactorImu : public FactorAutodiff<FactorImu, 15, 3, 4, 3, 6, 3, 4, 3, 6>
const T* const _p2,
const T* const _o2,
const T* const _v2,
- const T* const _b2,
T* _res) const;
/** \brief Estimation error given the states in the wolf tree
@@ -95,8 +94,6 @@ class FactorImu : public FactorAutodiff<FactorImu, 15, 3, 4, 3, 6, 3, 4, 3, 6>
const Eigen::MatrixBase<D1> & _p2,
const Eigen::QuaternionBase<D2> & _q2,
const Eigen::MatrixBase<D1> & _v2,
- const Eigen::MatrixBase<D1> & _ab2,
- const Eigen::MatrixBase<D1> & _wb2,
Eigen::MatrixBase<D3> & _res) const;
/** Function expectation(...)
@@ -148,20 +145,6 @@ class FactorImu : public FactorAutodiff<FactorImu, 15, 3, 4, 3, 6, 3, 4, 3, 6>
/// Metrics
const double dt_; ///< delta-time and delta-time-squared between keyframes
- /** bias covariance and bias residuals
- *
- * continuous bias covariance matrix for accelerometer would then be A_r = diag(ab_stdev_^2, ab_stdev_^2, ab_stdev_^2)
- * To compute bias residuals, we will need to do (sqrt(A_r)).inverse() * ab_error
- *
- * In our case, we introduce time 'distance' in the computation of this residual (SEE FORSTER17), thus we have to use the discret covariance matrix
- * discrete bias covariance matrix for accelerometer : A_r_dt = dt_ * A_r
- * taking the square root for bias residuals : sqrt_A_r_dt = sqrt(dt_ * A_r) = sqrt(dt_) * sqrt(A_r)
- * then with the inverse : sqrt_A_r_dt_inv = (sqrt(dt_ * A_r)).inverse() = (1/sqrt(dt_)) * sqrt(A_r).inverse()
- *
- * same logic for gyroscope bias
- */
- const Eigen::Matrix6d sqrt_r_dt_inv;
-
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
};
@@ -172,8 +155,8 @@ inline FactorImu::FactorImu(const FeatureImuPtr& _ftr_ptr,
const CaptureImuPtr& _cap_origin_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
- FactorStatus _status) :
- FactorAutodiff<FactorImu, 15, 3, 4, 3, 6, 3, 4, 3, 6>( // ...
+ FactorStatus _status) :
+ FactorAutodiff<FactorImu, 9, 3, 4, 3, 6, 3, 4, 3>( // ...
"FactorImu",
TOP_MOTION,
_ftr_ptr,
@@ -190,8 +173,8 @@ inline FactorImu::FactorImu(const FeatureImuPtr& _ftr_ptr,
_cap_origin_ptr->getSensorIntrinsic(),
_ftr_ptr->getFrame()->getP(),
_ftr_ptr->getFrame()->getO(),
- _ftr_ptr->getFrame()->getV(),
- _ftr_ptr->getCapture()->getSensorIntrinsic()),
+ _ftr_ptr->getFrame()->getV()),
+ processor_imu_(std::static_pointer_cast<ProcessorImu>(_processor_ptr)),
dp_preint_(_ftr_ptr->getMeasurement().head<3>()), // dp, dv, dq at preintegration time
dq_preint_(_ftr_ptr->getMeasurement().data()+3),
dv_preint_(_ftr_ptr->getMeasurement().tail<3>()),
@@ -202,8 +185,7 @@ inline FactorImu::FactorImu(const FeatureImuPtr& _ftr_ptr,
dDp_dwb_(_ftr_ptr->getJacobianBias().block(0,3,3,3)),
dDv_dwb_(_ftr_ptr->getJacobianBias().block(6,3,3,3)),
dDq_dwb_(_ftr_ptr->getJacobianBias().block(3,3,3,3)),
- dt_(_ftr_ptr->getFrame()->getTimeStamp() - _cap_origin_ptr->getTimeStamp()),
- sqrt_r_dt_inv(_ftr_ptr->getCapture()->getSensor()->getDriftStd('I').cwiseInverse().asDiagonal())
+ dt_(_ftr_ptr->getFrame()->getTimeStamp() - _cap_origin_ptr->getTimeStamp())
{
//
}
@@ -216,7 +198,6 @@ inline bool FactorImu::operator ()(const T* const _p1,
const T* const _p2,
const T* const _q2,
const T* const _v2,
- const T* const _b2,
T* _res) const
{
using namespace Eigen;
@@ -231,12 +212,10 @@ inline bool FactorImu::operator ()(const T* const _p1,
Map<const Matrix<T,3,1> > p2(_p2);
Map<const Quaternion<T> > q2(_q2);
Map<const Matrix<T,3,1> > v2(_v2);
- Map<const Matrix<T,3,1> > ab2(_b2);
- Map<const Matrix<T,3,1> > wb2(_b2 + 3);
- Map<Matrix<T,15,1> > res(_res);
+ Map<Matrix<T,9,1> > res(_res);
- residual(p1, q1, v1, ab1, wb1, p2, q2, v2, ab2, wb2, res);
+ residual(p1, q1, v1, ab1, wb1, p2, q2, v2, res);
return true;
}
@@ -255,7 +234,7 @@ Eigen::Vector9d FactorImu::error()
Eigen::Vector9d delta_step;
delta_step.head<3>() = dDp_dab_ * (acc_bias - acc_bias_preint_ ) + dDp_dwb_ * (gyro_bias - gyro_bias_preint_);
- delta_step.segment<3>(3) = dDq_dwb_ * (gyro_bias - gyro_bias_preint_);
+ delta_step.segment<3>(3) = dDq_dwb_ * (gyro_bias - gyro_bias_preint_);
delta_step.tail<3>() = dDv_dab_ * (acc_bias - acc_bias_preint_ ) + dDv_dwb_ * (gyro_bias - gyro_bias_preint_);
Eigen::VectorXd delta_corr = imu::plus(delta_preint, delta_step);
@@ -274,8 +253,6 @@ inline bool FactorImu::residual(const Eigen::MatrixBase<D1> & _p1,
const Eigen::MatrixBase<D1> & _p2,
const Eigen::QuaternionBase<D2> & _q2,
const Eigen::MatrixBase<D1> & _v2,
- const Eigen::MatrixBase<D1> & _ab2,
- const Eigen::MatrixBase<D1> & _wb2,
Eigen::MatrixBase<D3> & _res) const
{
@@ -321,23 +298,23 @@ inline bool FactorImu::residual(const Eigen::MatrixBase<D1> & _p1,
//needed typedefs
typedef typename D1::Scalar T;
- EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(D3, 15);
+ EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(D3, 9);
// 1. Expected delta from states
Eigen::Matrix<T, 3, 1 > dp_exp;
Eigen::Quaternion<T> dq_exp;
Eigen::Matrix<T, 3, 1> dv_exp;
- imu::betweenStates(_p1, _q1, _v1, _p2, _q2, _v2, (T)dt_, dp_exp, dq_exp, dv_exp);
+ imu::betweenStates(_p1, _q1, _v1, _p2, _q2, _v2, dt_, dp_exp, dq_exp, dv_exp);
// 2. Corrected integrated delta: delta_corr = delta_preint (+) J_bias * (bias_current - bias_preint)
// 2.a. Compute the delta step in tangent space: step = J_bias * (bias - bias_preint)
Eigen::Matrix<T, 9, 1> d_step;
- d_step.block(0,0,3,1) = dDp_dab_.cast<T>() * (_ab1 - acc_bias_preint_ .cast<T>()) + dDp_dwb_.cast<T>() * (_wb1 - gyro_bias_preint_.cast<T>());
- d_step.block(3,0,3,1) = dDq_dwb_.cast<T>() * (_wb1 - gyro_bias_preint_.cast<T>());
- d_step.block(6,0,3,1) = dDv_dab_.cast<T>() * (_ab1 - acc_bias_preint_ .cast<T>()) + dDv_dwb_.cast<T>() * (_wb1 - gyro_bias_preint_.cast<T>());
+ d_step.block(0,0,3,1) = dDp_dab_ * (_ab1 - acc_bias_preint_) + dDp_dwb_ * (_wb1 - gyro_bias_preint_);
+ d_step.block(3,0,3,1) = dDq_dwb_ * (_wb1 - gyro_bias_preint_);
+ d_step.block(6,0,3,1) = dDv_dab_ * (_ab1 - acc_bias_preint_) + dDv_dwb_ * (_wb1 - gyro_bias_preint_);
#ifdef METHOD_1 // method 1
Eigen::Matrix<T, 3, 1> dp_step = d_step.block(0,0,3,1);
@@ -371,10 +348,10 @@ inline bool FactorImu::residual(const Eigen::MatrixBase<D1> & _p1,
J_err_corr.block(3,3,3,3) = J_do_dq2;
// 4. Residuals are the weighted errors
- _res.head(9) = J_err_corr.inverse().transpose() * getMeasurementSquareRootInformationUpper().cast<T>() * d_error;
+ _res.head(9) = J_err_corr.inverse().transpose() * getMeasurementSquareRootInformationUpper() * d_error;
#else
imu::diff(dp_exp, dq_exp, dv_exp, dp_correct, dq_correct, dv_correct, dp_error, do_error, dv_error);
- _res.head(9) = getMeasurementSquareRootInformationUpper().cast<T>() * d_error;
+ _res = getMeasurementSquareRootInformationUpper() * d_error;
#endif
#else // method 2
@@ -392,17 +369,10 @@ inline bool FactorImu::residual(const Eigen::MatrixBase<D1> & _p1,
d_error << d_diff - d_step;
// 4. Residuals are the weighted errors
- _res.head(9) = getMeasurementSquareRootInformationUpper().cast<T>() * d_error;
+ _res = getMeasurementSquareRootInformationUpper() * d_error;
#endif
- // Errors between biases (KF1.bias - KF2.bias)
- Eigen::Matrix<T,6,1> b_error;
- b_error.head(3) = _ab1 - _ab2; // accelerometer bias
- b_error.tail(3) = _wb1 - _wb2; // gyrometer bias
-
- // 4. Residuals are the weighted errors
- _res.tail(6) = sqrt_r_dt_inv.cast<T>() * b_error;
//////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////// PRINT VALUES ///////////////////////////////////
diff --git a/include/imu/factor/factor_imu2d.h b/include/imu/factor/factor_imu2d.h
index 3bbc11c9dccf41fb1a5d592bd1872b510d066327..42fe2fa292f4b71a5c031bbf68e1f3744ed872d8 100644
--- a/include/imu/factor/factor_imu2d.h
+++ b/include/imu/factor/factor_imu2d.h
@@ -35,7 +35,7 @@ namespace wolf {
WOLF_PTR_TYPEDEFS(FactorImu2d);
//class
-class FactorImu2d : public FactorAutodiff<FactorImu2d, 8, 2, 1, 2, 3, 2, 1, 2, 3>
+class FactorImu2d : public FactorAutodiff<FactorImu2d, 5, 2, 1, 2, 3, 2, 1, 2>
{
public:
FactorImu2d(const FeatureImu2dPtr& _ftr_ptr,
@@ -60,7 +60,6 @@ class FactorImu2d : public FactorAutodiff<FactorImu2d, 8, 2, 1, 2, 3, 2, 1, 2, 3
const T* const _p2,
const T* const _o2,
const T* const _v2,
- const T* const _b2,
T* _res) const;
Eigen::Matrix3d getBiasDriftSquareRootInformationUpper(const FeatureBaseConstPtr& _ftr_ptr) const
@@ -87,20 +86,7 @@ class FactorImu2d : public FactorAutodiff<FactorImu2d, 8, 2, 1, 2, 3, 2, 1, 2, 3
/// Metrics
const double dt_; ///< delta-time and delta-time-squared between keyframes
- /** bias covariance and bias residuals
- *
- * continuous bias covariance matrix for accelerometer would then be A_r = diag(ab_stdev_^2, ab_stdev_^2, ab_stdev_^2)
- * To compute bias residuals, we will need to do (sqrt(A_r)).inverse() * ab_error
- *
- * In our case, we introduce time 'distance' in the computation of this residual (SEE FORSTER17), thus we have to use the discret covariance matrix
- * discrete bias covariance matrix for accelerometer : A_r_dt = dt_ * A_r
- * taking the square root for bias residuals : sqrt_A_r_dt = sqrt(dt_ * A_r) = sqrt(dt_) * sqrt(A_r)
- * then with the inverse : sqrt_A_r_dt_inv = (sqrt(dt_ * A_r)).inverse() = (1/sqrt(dt_)) * sqrt(A_r).inverse()
- *
- * same logic for gyroscope bias
- */
const Eigen::Matrix5d sqrt_delta_preint_inv_;
- const Eigen::Matrix3d sqrt_bias_drift_dt_inv_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
@@ -113,7 +99,7 @@ inline FactorImu2d::FactorImu2d(const FeatureImu2dPtr& _ftr_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status) :
- FactorAutodiff<FactorImu2d, 8, 2, 1, 2, 3, 2, 1, 2, 3>( // ...
+ FactorAutodiff<FactorImu2d, 5, 2, 1, 2, 3, 2, 1, 2>( // ...
"FactorImu2d",
TOP_MOTION,
_ftr_ptr,
@@ -130,14 +116,12 @@ inline FactorImu2d::FactorImu2d(const FeatureImu2dPtr& _ftr_ptr,
_cap_origin_ptr->getSensorIntrinsic(),
_ftr_ptr->getFrame()->getP(),
_ftr_ptr->getFrame()->getO(),
- _ftr_ptr->getFrame()->getV(),
- _ftr_ptr->getCapture()->getSensorIntrinsic()),
+ _ftr_ptr->getFrame()->getV()),
delta_preint_(_ftr_ptr->getMeasurement()), // dp, dth, dv at preintegration time
bias_preint_(_ftr_ptr->getBiasPreint()), // state biases at preintegration time
jacobian_bias_(_ftr_ptr->getJacobianBias()), // Jacs of dp dv dq wrt biases
dt_(_ftr_ptr->getFrame()->getTimeStamp() - _cap_origin_ptr->getTimeStamp()),
- sqrt_delta_preint_inv_(_ftr_ptr->getMeasurementSquareRootInformationUpper()),
- sqrt_bias_drift_dt_inv_(getBiasDriftSquareRootInformationUpper(_ftr_ptr))
+ sqrt_delta_preint_inv_(_ftr_ptr->getMeasurementSquareRootInformationUpper())
{
//
}
@@ -150,7 +134,6 @@ inline bool FactorImu2d::operator ()(const T* const _p1,
const T* const _p2,
const T* const _th2,
const T* const _v2,
- const T* const _b2,
T* _res) const
{
using namespace Eigen;
@@ -164,9 +147,8 @@ inline bool FactorImu2d::operator ()(const T* const _p1,
Map<const Matrix<T,2,1> > p2(_p2);
const T& th2 = *_th2;
Map<const Matrix<T,2,1> > v2(_v2);
- Map<const Matrix<T,3,1> > b2(_b2);
- Map<Matrix<T,8,1> > res(_res);
+ Map<Matrix<T,5,1> > res(_res);
//residual
/*
@@ -186,12 +168,8 @@ inline bool FactorImu2d::operator ()(const T* const _p1,
Matrix<T, 5, 1> delta_error = delta_corr - delta_predicted;
delta_error(2) = pi2pi(delta_error(2));
- res.template head<5>() = sqrt_delta_preint_inv_*delta_error;
+ res = sqrt_delta_preint_inv_ * delta_error;
-
- //bias drift
- res.template tail<3>() = sqrt_bias_drift_dt_inv_*(b2 -b1);
-
return true;
}
diff --git a/include/imu/factor/factor_imu2d_with_gravity.h b/include/imu/factor/factor_imu2d_with_gravity.h
index 8c2abfb89088be2e2610c8af5f9a61ab57e996aa..9f022bce04a053b716bb54d7f7028222ab88ae07 100644
--- a/include/imu/factor/factor_imu2d_with_gravity.h
+++ b/include/imu/factor/factor_imu2d_with_gravity.h
@@ -34,7 +34,7 @@ namespace wolf {
WOLF_PTR_TYPEDEFS(FactorImu2dWithGravity);
//class
-class FactorImu2dWithGravity : public FactorAutodiff<FactorImu2dWithGravity, 8, 2, 1, 2, 3, 2, 1, 2, 3, 2>
+class FactorImu2dWithGravity : public FactorAutodiff<FactorImu2dWithGravity, 5, 2, 1, 2, 3, 2, 1, 2, 2>
{
public:
FactorImu2dWithGravity(const FeatureImu2dPtr& _ftr_ptr,
@@ -59,7 +59,6 @@ class FactorImu2dWithGravity : public FactorAutodiff<FactorImu2dWithGravity, 8,
const T* const _p2,
const T* const _o2,
const T* const _v2,
- const T* const _b2,
const T* const _g,
T* _res) const;
Eigen::Matrix3d getBiasDriftSquareRootInformationUpper(const FeatureBaseConstPtr& _ftr_ptr) const
@@ -86,20 +85,7 @@ class FactorImu2dWithGravity : public FactorAutodiff<FactorImu2dWithGravity, 8,
/// Metrics
const double dt_; ///< delta-time and delta-time-squared between keyframes
- /** bias covariance and bias residuals
- *
- * continuous bias covariance matrix for accelerometer would then be A_r = diag(ab_stdev_^2, ab_stdev_^2, ab_stdev_^2)
- * To compute bias residuals, we will need to do (sqrt(A_r)).inverse() * ab_error
- *
- * In our case, we introduce time 'distance' in the computation of this residual (SEE FORSTER17), thus we have to use the discret covariance matrix
- * discrete bias covariance matrix for accelerometer : A_r_dt = dt_ * A_r
- * taking the square root for bias residuals : sqrt_A_r_dt = sqrt(dt_ * A_r) = sqrt(dt_) * sqrt(A_r)
- * then with the inverse : sqrt_A_r_dt_inv = (sqrt(dt_ * A_r)).inverse() = (1/sqrt(dt_)) * sqrt(A_r).inverse()
- *
- * same logic for gyroscope bias
- */
const Eigen::Matrix5d sqrt_delta_preint_inv_;
- const Eigen::Matrix3d sqrt_bias_drift_dt_inv_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
@@ -112,7 +98,7 @@ inline FactorImu2dWithGravity::FactorImu2dWithGravity(const FeatureImu2dPtr&
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status) :
- FactorAutodiff<FactorImu2dWithGravity, 8, 2, 1, 2, 3, 2, 1, 2, 3, 2>( // ...
+ FactorAutodiff<FactorImu2dWithGravity, 5, 2, 1, 2, 3, 2, 1, 2, 2>( // ...
"FactorImu2dWithGravity",
TOP_MOTION,
_ftr_ptr,
@@ -130,14 +116,12 @@ inline FactorImu2dWithGravity::FactorImu2dWithGravity(const FeatureImu2dPtr&
_ftr_ptr->getFrame()->getP(),
_ftr_ptr->getFrame()->getO(),
_ftr_ptr->getFrame()->getV(),
- _ftr_ptr->getCapture()->getSensorIntrinsic(),
_cap_origin_ptr->getSensor()->getStateBlock('G')),
delta_preint_(_ftr_ptr->getMeasurement()), // dp, dth, dv at preintegration time
bias_preint_(_ftr_ptr->getBiasPreint()), // state biases at preintegration time
jacobian_bias_(_ftr_ptr->getJacobianBias()), // Jacs of dp dv dq wrt biases
dt_(_ftr_ptr->getFrame()->getTimeStamp() - _cap_origin_ptr->getTimeStamp()),
- sqrt_delta_preint_inv_(_ftr_ptr->getMeasurementSquareRootInformationUpper()),
- sqrt_bias_drift_dt_inv_(getBiasDriftSquareRootInformationUpper(_ftr_ptr))
+ sqrt_delta_preint_inv_(_ftr_ptr->getMeasurementSquareRootInformationUpper())
{
//
}
@@ -150,7 +134,6 @@ inline bool FactorImu2dWithGravity::operator ()(const T* const _p1,
const T* const _p2,
const T* const _th2,
const T* const _v2,
- const T* const _b2,
const T* const _g,
T* _res) const
{
@@ -165,10 +148,9 @@ inline bool FactorImu2dWithGravity::operator ()(const T* const _p1,
Map<const Matrix<T,2,1> > p2(_p2);
const T& th2 = *_th2;
Map<const Matrix<T,2,1> > v2(_v2);
- Map<const Matrix<T,3,1> > b2(_b2);
Map<const Matrix<T,2,1> > g(_g);
- Map<Matrix<T,8,1> > res(_res);
+ Map<Matrix<T,5,1> > res(_res);
//residual
/*
@@ -188,12 +170,8 @@ inline bool FactorImu2dWithGravity::operator ()(const T* const _p1,
Matrix<T, 5, 1> delta_error = imu2d::diff(delta_predicted, delta_corr);
delta_error(2) = pi2pi(delta_error(2));
- res.template head<5>() = sqrt_delta_preint_inv_*delta_error;
+ res = sqrt_delta_preint_inv_*delta_error;
-
- //bias drift
- res.template tail<3>() = sqrt_bias_drift_dt_inv_*(b2 -b1);
-
return true;
}
diff --git a/include/imu/math/imu2d_tools.h b/include/imu/math/imu2d_tools.h
index faf2fd9e1a3abd142a6599da42da066b21be07d9..2311caf0c35f19ad6721998b41eaac08523632ca 100644
--- a/include/imu/math/imu2d_tools.h
+++ b/include/imu/math/imu2d_tools.h
@@ -258,7 +258,7 @@ inline void compose(const MatrixBase<D1>& d1,
// Matrix3d dR1 = q2R(d1.at('O')); //dq1.matrix(); // First Delta, DR
// Matrix3d dR2 = q2R(d2.at('O')); //dq2.matrix(); // Second delta, dR
//
-// // Jac wrt first delta // TODO find optimal way to re-use memory allocation!!!
+// // Jac wrt first delta
// J_sum_d1.clear();
// J_sum_d1.emplace('P','P', Matrix3d::Identity()); // dDp'/dDp = I
// J_sum_d1.emplace('P','O', - dR1 * skew(d2.at('P'))) ; // dDp'/dDo
diff --git a/include/imu/math/imu_tools.h b/include/imu/math/imu_tools.h
index 7d9e79dfde8eb8780ae88259fa83dc0f91a57c82..68b49dc327204409ab35cbe24003e8ce60ed653a 100644
--- a/include/imu/math/imu_tools.h
+++ b/include/imu/math/imu_tools.h
@@ -283,7 +283,7 @@ inline void compose(const VectorComposite& d1,
Matrix3d dR1 = q2R(d1.at('O')); //dq1.matrix(); // First Delta, DR
Matrix3d dR2 = q2R(d2.at('O')); //dq2.matrix(); // Second delta, dR
- // Jac wrt first delta // TODO find optimal way to re-use memory allocation!!!
+ // Jac wrt first delta
J_sum_d1.clear();
J_sum_d1.emplace('P','P', Matrix3d::Identity()); // dDp'/dDp = I
J_sum_d1.emplace('P','O', - dR1 * skew(d2.at('P'))) ; // dDp'/dDo
@@ -869,6 +869,65 @@ Matrix<typename D1::Scalar, 6, 1> motion2data(const MatrixBase<D1>& motion, cons
return data;
}
+/** extract local g and v0 from three keyframes and deltas
+ * @param p0 position of kf0
+ * @param q0 orientation of kf0
+ * @param p1 position of kf0
+ * @param q1 orientation of kf0
+ * @param p2 position of kf0
+ * @param Dt01 time delta between kf0 and kf1
+ * @param Dp01 position delta between kf0 and kf1
+ * @param Dv01 orientation delta between kf0 and kf1
+ * @param Dt12 time delta between kf1 and kf2
+ * @param Dp12 position delta between kf1 and kf2
+ * @param Dv12 orientation delta between kf1 and kf2
+ *
+ * This method needs 3 kfs to solve. Inspired on Lupton-11 with a revised
+ * implementation by jsola-22, see https://www.overleaf.com/project/629e276e7f68b0c2bfa469ac
+ */
+template <typename TP0,
+ typename TQ0,
+ typename TP1,
+ typename TQ1,
+ typename TP2,
+ typename TDT01,
+ typename TDP01,
+ typename TDV01,
+ typename TDT12,
+ typename TDP12,
+ typename TV0,
+ typename TG>
+void extract_v0_g(const MatrixBase<TP0> &p0,
+ const QuaternionBase<TQ0> &q0,
+ const MatrixBase<TP1> &p1,
+ const QuaternionBase<TQ1> &q1,
+ const MatrixBase<TP2> &p2,
+ const TDT01 &Dt01,
+ const MatrixBase<TDP01> &Dp01,
+ const MatrixBase<TDV01> &Dv01,
+ const TDT12 &Dt12,
+ const MatrixBase<TDP12> &Dp12,
+ MatrixBase<TV0> &v0,
+ MatrixBase<TG> &g)
+{
+ // full time period between kfs 0 and 2
+ const TDT01 Dt02 = Dt01 + Dt12;
+
+ // Scalar coefficients of entries of 6x6 matrix A.inv
+ const TDT01 ai_v0 = (Dt02 / (Dt01 * Dt12));
+ const TDT01 ai_v1 = (-Dt01 / (Dt12 * Dt02));
+ const TDT01 ai_g0 = (-2 / (Dt01 * Dt12));
+ const TDT01 ai_g1 = (2 / (Dt12 * Dt02));
+
+ // 3-vector entries of 6-vector b
+ Matrix<typename TV0::Scalar, 3, 1> b0 = p1 - p0 - q0 * Dp01;
+ Matrix<typename TV0::Scalar, 3, 1> b1 = p2 - p0 - q0 * (Dp01 + Dv01 * Dt12) - q1 * Dp12;
+
+ // output vectors -- this is x = A.inv * b
+ v0 = ai_v0 * b0 + ai_v1 * b1;
+ g = ai_g0 * b0 + ai_g1 * b1;
+}
+
} // namespace imu
} // namespace wolf
diff --git a/include/imu/processor/processor_imu.h b/include/imu/processor/processor_imu.h
index d8efc7cbd3221654da4721ff8bc21e6c15bc4353..0fb04fd3fe13363407da78b03d3b9bb7d05a081e 100644
--- a/include/imu/processor/processor_imu.h
+++ b/include/imu/processor/processor_imu.h
@@ -22,9 +22,12 @@
#ifndef PROCESSOR_IMU_H
#define PROCESSOR_IMU_H
-// Wolf
+// Wolf Imu
+#include "imu/sensor/sensor_imu.h"
#include "imu/capture/capture_imu.h"
#include "imu/feature/feature_imu.h"
+// Wolf core
+
#include <core/processor/processor_motion.h>
namespace wolf {
@@ -32,11 +35,41 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorImu);
struct ParamsProcessorImu : public ParamsProcessorMotion
{
+ bool bootstrap_enable;
+ enum BootstrapMethod
+ {
+ BOOTSTRAP_STATIC,
+ BOOTSTRAP_G,
+ BOOTSTRAP_V0_G
+ } bootstrap_method;
+ double bootstrap_averaging_length;
+
ParamsProcessorImu() = default;
ParamsProcessorImu(std::string _unique_name, const ParamsServer& _server):
ParamsProcessorMotion(_unique_name, _server)
{
- //
+ bootstrap_enable = _server.getParam<bool>(prefix + _unique_name + "/bootstrap/enable");
+ if (_server.hasParam(prefix + _unique_name + "/bootstrap/method"))
+ {
+ string str = _server.getParam<string>(prefix + _unique_name + "/bootstrap/method");
+ std::transform(str.begin(), str.end(), str.begin(), ::toupper);
+ if (str == "STATIC" /**/)
+ {
+ bootstrap_method = BOOTSTRAP_STATIC;
+ bootstrap_averaging_length =
+ _server.getParam<double>(prefix + _unique_name + "/bootstrap/averaging_length");
+ }
+ if (str == "G" /* */)
+ {
+ bootstrap_method = BOOTSTRAP_G;
+ bootstrap_averaging_length =
+ _server.getParam<double>(prefix + _unique_name + "/bootstrap/averaging_length");
+ }
+ if (str == "V0_G" /* */)
+ {
+ bootstrap_method = BOOTSTRAP_V0_G;
+ }
+ }
}
std::string print() const override
{
@@ -51,55 +84,77 @@ class ProcessorImu : public ProcessorMotion{
public:
ProcessorImu(ParamsProcessorImuPtr _params_motion_Imu);
~ProcessorImu() override;
- void configure(SensorBasePtr _sensor) override { };
-
WOLF_PROCESSOR_CREATE(ProcessorImu, ParamsProcessorImu);
+ void configure(SensorBasePtr _sensor) override;
+
void preProcess() override;
- protected:
- void computeCurrentDelta(const Eigen::VectorXd& _data,
- const Eigen::MatrixXd& _data_cov,
- const Eigen::VectorXd& _calib,
- const double _dt,
- Eigen::VectorXd& _delta,
- Eigen::MatrixXd& _delta_cov,
- Eigen::MatrixXd& _jacobian_calib) const override;
- void deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
- const Eigen::VectorXd& _delta,
- const double _dt,
- Eigen::VectorXd& _delta_preint_plus_delta) const override;
- void deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
- const Eigen::VectorXd& _delta,
- const double _dt,
- Eigen::VectorXd& _delta_preint_plus_delta,
- Eigen::MatrixXd& _jacobian_delta_preint,
- Eigen::MatrixXd& _jacobian_delta) const override;
- void statePlusDelta(const VectorComposite& _x,
- const Eigen::VectorXd& _delta,
- const double _Dt,
- VectorComposite& _x_plus_delta) const override;
- Eigen::VectorXd deltaZero() const override;
- Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint,
- const Eigen::VectorXd& delta_step) const override;
- VectorXd getCalibration (const CaptureBaseConstPtr _capture = nullptr) const override;
- void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
- bool voteForKeyFrame() const override;
- CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
- const SensorBasePtr& _sensor,
- const TimeStamp& _ts,
- const VectorXd& _data,
- const MatrixXd& _data_cov,
- const VectorXd& _calib,
- const VectorXd& _calib_preint,
- const CaptureBasePtr& _capture_origin) override;
- FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_motion) override;
- FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion,
- CaptureBasePtr _capture_origin) override;
-
- protected:
- ParamsProcessorImuPtr params_motion_Imu_;
-};
+ protected:
+ void computeCurrentDelta(const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov,
+ const Eigen::VectorXd& _calib,
+ const double _dt,
+ Eigen::VectorXd& _delta,
+ Eigen::MatrixXd& _delta_cov,
+ Eigen::MatrixXd& _jacobian_calib) const override;
+ void deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
+ const Eigen::VectorXd& _delta,
+ const double _dt,
+ Eigen::VectorXd& _delta_preint_plus_delta) const override;
+ void deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
+ const Eigen::VectorXd& _delta,
+ const double _dt,
+ Eigen::VectorXd& _delta_preint_plus_delta,
+ Eigen::MatrixXd& _jacobian_delta_preint,
+ Eigen::MatrixXd& _jacobian_delta) const override;
+ void statePlusDelta(const VectorComposite& _x,
+ const Eigen::VectorXd& _delta,
+ const double _Dt,
+ VectorComposite& _x_plus_delta) const override;
+ Eigen::VectorXd deltaZero() const override;
+ Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint,
+ const Eigen::VectorXd& delta_step) const override;
+ VectorXd getCalibration(const CaptureBaseConstPtr _capture = nullptr) const override;
+ void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
+ bool voteForKeyFrame() const override;
+ CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
+ const SensorBasePtr& _sensor,
+ const TimeStamp& _ts,
+ const VectorXd& _data,
+ const MatrixXd& _data_cov,
+ const VectorXd& _calib,
+ const VectorXd& _calib_preint,
+ const CaptureBasePtr& _capture_origin) override;
+ virtual void emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own) override;
+
+ public:
+ /** \brief Enable bootstrapping process
+ */
+ void bootstrapEnable(bool _bootstrap_enable = true);
+ protected:
+ /** \brief Bootstrap the IMU initial conditions
+ */
+ virtual void bootstrap() override;
+ /** \brief Get the Capture where the IMU data started the bootstrap
+ */
+ CaptureBasePtr bootstrapOrigin() const;
+ /** \brief Compose all preintegrated Deltas accumulated during the whole bootstrap process.
+ */
+ VectorXd bootstrapDelta() const;
+ /** \brief Incrementally compute all state values according to the current initial conditions and all
+ * preintegrated deltas.
+ *
+ * This is executed at the end of the bootstrap process
+ */
+ bool recomputeStates() const;
+
+ protected:
+ ParamsProcessorImuPtr params_motion_Imu_;
+ std::list<FactorBasePtr> bootstrap_factor_list_; ///< List of all IMU factors created while IMU is bootstrapping
+ SensorImuPtr sensor_imu_;
+ Matrix6d imu_drift_cov_;
+};
}
/////////////////////////////////////////////////////////
diff --git a/include/imu/processor/processor_imu2d.h b/include/imu/processor/processor_imu2d.h
index 726666f48ab2878d2f86712a4fc35b79e7f7e31a..f8f4a547838bc7e13db224f915cb8d8934ea7975 100644
--- a/include/imu/processor/processor_imu2d.h
+++ b/include/imu/processor/processor_imu2d.h
@@ -23,6 +23,7 @@
#define PROCESSOR_IMU2D_H
// Wolf
+#include "imu/sensor/sensor_imu2d.h"
#include "imu/capture/capture_imu.h"
#include "imu/feature/feature_imu.h"
#include <core/processor/processor_motion.h>
@@ -51,7 +52,7 @@ class ProcessorImu2d : public ProcessorMotion{
public:
ProcessorImu2d(ParamsProcessorImu2dPtr _params_motion_Imu);
~ProcessorImu2d() override;
- void configure(SensorBasePtr _sensor) override { };
+ void configure(SensorBasePtr _sensor) override;
WOLF_PROCESSOR_CREATE(ProcessorImu2d, ParamsProcessorImu2d);
void preProcess() override;
@@ -92,12 +93,13 @@ class ProcessorImu2d : public ProcessorMotion{
const VectorXd& _calib,
const VectorXd& _calib_preint,
const CaptureBasePtr& _capture_origin) override;
- FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_motion) override;
- FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion,
- CaptureBasePtr _capture_origin) override;
+ virtual void emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own) override;
+
protected:
ParamsProcessorImu2dPtr params_motion_Imu_;
+ SensorImu2dPtr sensor_imu2d_;
+ Matrix3d imu_drift_cov_;
};
}
diff --git a/src/capture/capture_imu.cpp b/src/capture/capture_imu.cpp
index b56998c76f0af6702df07837065c0d6349790eaa..7f38375ad2335533033fb4352a3871c65a6007b2 100644
--- a/src/capture/capture_imu.cpp
+++ b/src/capture/capture_imu.cpp
@@ -21,49 +21,58 @@
//--------LICENSE_END--------
#include "imu/capture/capture_imu.h"
#include "imu/sensor/sensor_imu.h"
-#include "core/state_block/state_quaternion.h"
+
+#include <core/state_block/state_quaternion.h>
+#include <core/state_block/state_block_derived.h>
namespace wolf {
-CaptureImu::CaptureImu(const TimeStamp& _init_ts,
- SensorBasePtr _sensor_ptr,
+CaptureImu::CaptureImu(const TimeStamp& _init_ts,
+ SensorBasePtr _sensor_ptr,
const Eigen::Vector6d& _acc_gyro_data,
const Eigen::MatrixXd& _data_cov,
- CaptureBasePtr _capture_origin_ptr) :
- CaptureMotion("CaptureImu",
- _init_ts,
- _sensor_ptr,
- _acc_gyro_data,
- _data_cov,
- _capture_origin_ptr,
- nullptr,
- nullptr,
- _sensor_ptr->isStateBlockDynamic('I') ?
- std::make_shared<StateBlock>((_sensor_ptr->getProblem()->getDim() == 2 ? 3 : 6), false) :
- nullptr)
+ CaptureBasePtr _capture_origin_ptr)
+ : CaptureMotion(
+ "CaptureImu",
+ _init_ts,
+ _sensor_ptr,
+ _acc_gyro_data,
+ _data_cov,
+ _capture_origin_ptr,
+ nullptr,
+ nullptr,
+ (_sensor_ptr->isStateBlockDynamic('I'))
+ ? ((_sensor_ptr->getProblem()->getDim() == 2)
+ ? std::static_pointer_cast<StateBlock>(std::make_shared<StateParams3>(Vector3d::Zero(), false))
+ : std::static_pointer_cast<StateBlock>(std::make_shared<StateParams6>(Vector6d::Zero(), false)))
+ : nullptr)
{
//
}
-CaptureImu::CaptureImu(const TimeStamp& _init_ts,
- SensorBasePtr _sensor_ptr,
+CaptureImu::CaptureImu(const TimeStamp& _init_ts,
+ SensorBasePtr _sensor_ptr,
const Eigen::Vector6d& _acc_gyro_data,
const Eigen::MatrixXd& _data_cov,
- const VectorXd& _bias,
- CaptureBasePtr _capture_origin_ptr) :
- CaptureMotion("CaptureImu",
- _init_ts,
- _sensor_ptr,
- _acc_gyro_data,
- _data_cov,
- _capture_origin_ptr,
- nullptr,
- nullptr,
- _sensor_ptr->isStateBlockDynamic('I') ?
- std::make_shared<StateBlock>((_sensor_ptr->getProblem()->getDim() == 2 ? 3 : 6), false) :
- nullptr)
+ const VectorXd& _bias,
+ CaptureBasePtr _capture_origin_ptr)
+ : CaptureMotion(
+ "CaptureImu",
+ _init_ts,
+ _sensor_ptr,
+ _acc_gyro_data,
+ _data_cov,
+ _capture_origin_ptr,
+ nullptr,
+ nullptr,
+ (_sensor_ptr->isStateBlockDynamic('I'))
+ ? ((_sensor_ptr->getProblem()->getDim() == 2)
+ ? std::static_pointer_cast<StateBlock>(std::make_shared<StateParams3>(_bias, false))
+ : std::static_pointer_cast<StateBlock>(std::make_shared<StateParams6>(_bias, false)))
+ : nullptr)
{
assert((_bias.size() == 3) or (_bias.size() == 6));
+ WOLF_WARN_COND(_sensor_ptr->isStateBlockDynamic('I'), "Sensor bias was provided but bias is static in sensor. Bias discarded.");
}
CaptureImu::~CaptureImu()
diff --git a/src/processor/processor_imu.cpp b/src/processor/processor_imu.cpp
index ca8bb14f95861a1944d7b1be5ebb2a16f95b0a85..371e97b60b7137a35b8c7b440dffa319620ba19a 100644
--- a/src/processor/processor_imu.cpp
+++ b/src/processor/processor_imu.cpp
@@ -26,14 +26,16 @@
// wolf
#include <core/state_block/vector_composite.h>
+#include <core/factor/factor_block_difference.h>
namespace wolf {
ProcessorImu::ProcessorImu(ParamsProcessorImuPtr _params_motion_imu) :
ProcessorMotion("ProcessorImu", "POV", 3, 10, 10, 9, 6, 6, _params_motion_imu),
- params_motion_Imu_(std::make_shared<ParamsProcessorImu>(*_params_motion_imu))
+ params_motion_Imu_(_params_motion_imu)
{
- //
+ bootstrapping_ = params_motion_Imu_->bootstrap_enable;
+ bootstrap_factor_list_.clear();
}
ProcessorImu::~ProcessorImu()
@@ -48,13 +50,13 @@ void ProcessorImu::preProcess()
bool ProcessorImu::voteForKeyFrame() const
{
// time span
- if (getBuffer().back().ts_ - getBuffer().front().ts_ > params_motion_Imu_->max_time_span)
+ if (getBuffer().back().ts_ - getBuffer().front().ts_ >= params_motion_Imu_->max_time_span - Constants::EPS)
{
WOLF_DEBUG( "PM: vote: time span" );
return true;
}
// buffer length
- if (getBuffer().size() > params_motion_Imu_->max_buff_length)
+ if (getBuffer().size() -1 > params_motion_Imu_->max_buff_length)
{
WOLF_DEBUG( "PM: vote: buffer length" );
return true;
@@ -92,16 +94,6 @@ CaptureMotionPtr ProcessorImu::emplaceCapture(const FrameBasePtr& _frame_own,
return cap_motion;
}
-FeatureBasePtr ProcessorImu::emplaceFeature(CaptureMotionPtr _capture_motion)
-{
- auto feature = FeatureBase::emplace<FeatureImu>(_capture_motion,
- _capture_motion->getBuffer().back().delta_integr_,
- _capture_motion->getBuffer().back().delta_integr_cov_ + unmeasured_perturbation_cov_,
- _capture_motion->getCalibrationPreint(),
- _capture_motion->getBuffer().back().jacobian_calib_);
- return feature;
-}
-
VectorXd ProcessorImu::getCalibration (const CaptureBaseConstPtr _capture) const
{
if (_capture)
@@ -115,14 +107,77 @@ void ProcessorImu::setCalibration (const CaptureBasePtr _capture, const VectorXd
_capture->getSensorIntrinsic()->setState(_calibration);
}
-FactorBasePtr ProcessorImu::emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin)
+void ProcessorImu::configure(SensorBasePtr _sensor)
{
+ sensor_imu_ = std::static_pointer_cast<SensorImu>(_sensor);
+
+ auto acc_drift_std = sensor_imu_->getAbRateStdev();
+ auto gyro_drift_std = sensor_imu_->getAbRateStdev();
+
+ ArrayXd imu_drift_sigmas(6);
+ imu_drift_sigmas << acc_drift_std, acc_drift_std, acc_drift_std, gyro_drift_std, gyro_drift_std, gyro_drift_std;
+ imu_drift_cov_ = imu_drift_sigmas.square().matrix().asDiagonal();
+
+}
+void ProcessorImu::emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own)
+{
+ // 1. Feature and factor Imu
+ //---------------------------
+
+ auto ftr_imu = FeatureBase::emplace<FeatureImu>(
+ _capture_own, _capture_own->getBuffer().back().delta_integr_,
+ _capture_own->getBuffer().back().delta_integr_cov_ + unmeasured_perturbation_cov_,
+ _capture_own->getCalibrationPreint(), _capture_own->getBuffer().back().jacobian_calib_);
+
CaptureImuPtr cap_imu = std::static_pointer_cast<CaptureImu>(_capture_origin);
- FeatureImuPtr ftr_imu = std::static_pointer_cast<FeatureImu>(_feature_motion);
- auto fac_imu = FactorBase::emplace<FactorImu>(_feature_motion, ftr_imu, cap_imu, shared_from_this(), params_->apply_loss_function);
+ auto fac_imu =
+ FactorBase::emplace<FactorImu>(ftr_imu, ftr_imu, cap_imu, shared_from_this(), params_->apply_loss_function);
- return fac_imu;
+ if (bootstrapping_)
+ {
+ fac_imu->setStatus(FAC_INACTIVE);
+ bootstrap_factor_list_.push_back(fac_imu);
+ }
+
+ // 2. Feature and factor bias -- IMU bias drift for acc and gyro
+ //---------------------------------------------------------------
+ // - This factor only if IMU biases are Dynamic (check sensor->isStateBlockDynamic('I'))
+ // - feature has zero measurement size 6, with the cov of the bias drift size 6x6
+ // - factor relates bias(last capture) and bias(origin capture)
+ if (getSensor()->isStateBlockDynamic('I'))
+ {
+ const auto& sb_imubias_own = _capture_own->getStateBlock('I');
+ const auto& sb_imubias_origin = _capture_origin->getStateBlock('I');
+ if (sb_imubias_own != sb_imubias_origin) // make sure it's two different state blocks! -- just in case
+ {
+ auto dt = _capture_own->getTimeStamp() - _capture_origin->getTimeStamp();
+ auto ftr_bias =
+ FeatureBase::emplace<FeatureBase>(_capture_own, "FeatureBase",
+ Vector6d::Zero(), // mean IMU drift is zero
+ imu_drift_cov_ * dt); // IMU drift cov specified in continuous time
+ auto fac_bias =
+ FactorBase::emplace<FactorBlockDifference>(ftr_bias, ftr_bias,
+ sb_imubias_own, // IMU bias block at t=own
+ sb_imubias_origin, // IMU bias block at t=origin
+ nullptr, // frame other
+ _capture_origin, // origin capture
+ nullptr, // feature other
+ nullptr, // landmark other
+ 0, // take all of first state block
+ -1, // take all of first state block
+ 0, // take all of first second block
+ -1, // take all of first second block
+ shared_from_this(), // this processor
+ params_->apply_loss_function); // loss function
+
+ if (bootstrapping_)
+ {
+ fac_bias->setStatus(FAC_INACTIVE);
+ bootstrap_factor_list_.push_back(fac_bias);
+ }
+ }
+ }
}
void ProcessorImu::computeCurrentDelta(const Eigen::VectorXd& _data,
@@ -240,9 +295,201 @@ Eigen::VectorXd ProcessorImu::correctDelta (const Eigen::VectorXd& delta_preint,
return imu::plus(delta_preint, delta_step);
}
+void ProcessorImu::bootstrap()
+{
+ // TODO bootstrap strategies.
+ // See Sola-22 "Imu bootstrap strategies" https://www.overleaf.com/project/629e276e7f68b0c2bfa469ac
+ // frames:
+ // w: world global ( where g = [0,0,-9.806] );
+ // l: world local;
+ // r: robot;
+ // s: sensor (IMU)
+
+ CaptureBasePtr first_capture = bootstrapOrigin();
+ TimeStamp t_current = last_ptr_->getBuffer().back().ts_;
+ VectorComposite transfo_w_l("PO");
+ switch (params_motion_Imu_->bootstrap_method)
+ {
+ case ParamsProcessorImu::BootstrapMethod::BOOTSTRAP_STATIC:
+
+ // Implementation of static strategy
+ if (t_current - first_capture->getTimeStamp() >= params_motion_Imu_->bootstrap_averaging_length)
+ {
+ // get initial IMU frame 's' expressed in local world frame 'l'
+ Quaterniond q_l_r; q_l_r.coeffs() = first_capture->getFrame()->getO()->getState();
+ Quaterniond q_r_s; q_r_s.coeffs() = first_capture->getSensor()->getO()->getState();
+ const auto& q_l_s = q_l_r * q_r_s;
+
+ // Compute total integrated delta during bootstrap period
+ VectorXd delta_int = bootstrapDelta();
+
+ // compute local g and transformation to global g
+ double dt = t_current - first_capture->getTimeStamp(); //
+ const auto& dv = delta_int.segment(7, 3); //
+ Vector3d g_l = -(q_l_s * dv / dt); // See eq. (20)
+ const auto& g_w = gravity(); //
+ const auto& p_w_l = Vector3d::Zero(); // will pivot around the origin
+ Quaterniond q_w_l = Quaterniond::FromTwoVectors(g_l, g_w); //
+ transfo_w_l.at('P') = p_w_l; //
+ transfo_w_l.at('O') = q_w_l.coeffs(); //
+
+ // Transform problem to new reference
+ getProblem()->transform(transfo_w_l);
+
+ // Recompute states at keyframes if they were provided by this processor
+ bool recomputed = recomputeStates();
+ if (recomputed)
+ {
+ WOLF_DEBUG("IMU Keyframe states have been recomputed!");
+ }
+
+ // TODO: add factors for the STATIC strategy:
+ // - zero-velocity factors (at each KF)
+ // - zero-displaecement odom3d factors (between KFs)
+
+ // Activate factors that were inactive during bootstrap
+ while (not bootstrap_factor_list_.empty())
+ {
+ bootstrap_factor_list_.front()->setStatus(FAC_ACTIVE);
+ bootstrap_factor_list_.pop_front();
+ }
+
+ // Clear bootstrapping flag. This marks the end of the bootstrapping process
+ bootstrapping_ = false;
+ }
+ break;
+
+ case ParamsProcessorImu::BootstrapMethod::BOOTSTRAP_G:
+
+ // Implementation of G strategy.
+ if (t_current - first_capture->getTimeStamp() >= params_motion_Imu_->bootstrap_averaging_length)
+ {
+ // get initial IMU frame 's' expressed in local world frame 'l'
+ Quaterniond q_l_r; q_l_r.coeffs() = first_capture->getFrame()->getO()->getState();
+ Quaterniond q_r_s; q_r_s.coeffs() = first_capture->getSensor()->getO()->getState();
+ const auto& q_l_s = q_l_r * q_r_s;
+
+ // Compute total integrated delta during bootstrap period
+ VectorXd delta_int = bootstrapDelta();
+ // compute local g and transformation to global g
+ double dt = t_current - first_capture->getTimeStamp(); //
+ const auto& dv_l = delta_int.segment(7, 3); //
+ Vector3d g_l = -(q_l_s * dv_l / dt); // See eq. (20)
+ const auto& g_w = gravity(); //
+ const auto& p_w_l = Vector3d::Zero(); // will pivot around the origin
+ Quaterniond q_w_l = Quaterniond::FromTwoVectors(g_l, g_w); //
+ transfo_w_l.at('P') = p_w_l; //
+ transfo_w_l.at('O') = q_w_l.coeffs(); //
+
+ // Transform problem to new reference
+ getProblem()->transform(transfo_w_l);
+
+ // Recompute states at keyframes if they were provided by this processor
+ bool recomputed = recomputeStates();
+ if (recomputed)
+ {
+ WOLF_DEBUG("IMU Keyframe states have been recomputed!");
+ }
+
+ // Activate factors that were inactive during bootstrap
+ while (not bootstrap_factor_list_.empty())
+ {
+ bootstrap_factor_list_.front()->setStatus(FAC_ACTIVE);
+ bootstrap_factor_list_.pop_front();
+ }
+
+ // Clear bootstrapping flag. This marks the end of the bootstrapping process
+ bootstrapping_ = false;
+ }
+ break;
+
+ case ParamsProcessorImu::BootstrapMethod::BOOTSTRAP_V0_G:
+
+ // TODO implement v0-g strategy
+ WOLF_WARN("Bootstrapping strategy BOOTSTRAP_V0_G not yet implemented. Disabling bootstrap!");
+ bootstrapping_ = false;
+ break;
+
+ default:
+
+ // No strategy provided: clear bootstrapping flag and warn
+ WOLF_WARN("Bootstrapping enabled but no viable strategy detected. Disabling bootstrap!");
+ bootstrapping_ = false;
+ break;
+ }
+}
+
+void ProcessorImu::bootstrapEnable(bool _bootstrap_enable)
+{
+ params_motion_Imu_->bootstrap_enable = _bootstrap_enable;
+ bootstrapping_ = _bootstrap_enable;
+};
+
+CaptureBasePtr ProcessorImu::bootstrapOrigin() const
+{
+ if (bootstrap_factor_list_.empty())
+ return origin_ptr_;
+ else
+ return std::static_pointer_cast<CaptureMotion>(bootstrap_factor_list_.front()->getCapture())
+ ->getOriginCapture();
+}
+
+VectorXd ProcessorImu::bootstrapDelta() const
+{
+ // Compute total integrated delta during bootstrap period
+ // first, integrate all deltas in previous factors
+ VectorXd delta_int = deltaZero();
+ double dt;
+ for (const auto& fac : bootstrap_factor_list_)
+ // here, we take advantage of the list of IMU factors to recover all deltas
+ {
+ if (std::dynamic_pointer_cast<FactorImu>(fac) != nullptr)
+ {
+ dt = fac->getCapture()->getTimeStamp() - fac->getCaptureOther()->getTimeStamp();
+ const auto& delta = fac->getFeature()->getMeasurement(); // In FeatImu, delta = measurement
+ delta_int = imu::compose(delta_int, delta, dt);
+ }
+ }
+ // now compose with delta in last_ptr_
+ dt = last_ptr_->getBuffer().back().ts_ - origin_ptr_->getTimeStamp();
+ const auto& delta = last_ptr_->getBuffer().back().delta_integr_;
+ delta_int = imu::compose(delta_int, delta, dt);
+
+ return delta_int;
+}
+
+bool ProcessorImu::recomputeStates() const
+{
+ const auto& mp = getProblem()->getMotionProviderMap();
+ if (not mp.empty() and
+ mp.begin()->second == std::static_pointer_cast<const MotionProvider>(
+ std::static_pointer_cast<const ProcessorMotion>(shared_from_this())))
+ {
+ WOLF_DEBUG("Recomputing IMU keyframe states...");
+ for (const auto& fac : bootstrap_factor_list_)
+ {
+ if (std::dynamic_pointer_cast<FactorImu>(fac) != nullptr)
+ {
+ const auto& ftr = fac->getFeature();
+ const auto& cap = std::static_pointer_cast<CaptureMotion>(ftr->getCapture());
+ const auto& frm = cap->getFrame();
+ const auto& cap_origin = cap->getOriginCapture();
+ const auto& frm_origin = cap_origin->getFrame();
+ const auto& delta = VectorComposite(ftr->getMeasurement(), "POV", {3, 4, 3});
+ const auto& x_origin = frm_origin->getState();
+ auto dt = cap->getTimeStamp() - cap_origin->getTimeStamp();
+ auto x = imu::composeOverState(x_origin, delta, dt);
+ frm->setState(x);
+ }
+ }
+ return true;
+ }
+ else
+ return false;
+}
-} // namespace wolf
+} // namespace wolf
// Register in the FactoryProcessor
#include "core/processor/factory_processor.h"
diff --git a/src/processor/processor_imu2d.cpp b/src/processor/processor_imu2d.cpp
index 56736c210eb352c3049f6ace039e79de6a9a18ef..8e2d532adf16605a1a0f67c1d439d8dcbe47e8e9 100644
--- a/src/processor/processor_imu2d.cpp
+++ b/src/processor/processor_imu2d.cpp
@@ -27,13 +27,14 @@
*/
// imu
#include "imu/processor/processor_imu2d.h"
-#include "imu/sensor/sensor_imu.h"
+#include "imu/sensor/sensor_imu2d.h"
#include "imu/factor/factor_imu2d.h"
#include "imu/factor/factor_imu2d_with_gravity.h"
#include "imu/math/imu2d_tools.h"
// wolf
#include <core/state_block/vector_composite.h>
+#include <core/factor/factor_block_difference.h>
namespace wolf {
@@ -102,16 +103,6 @@ namespace wolf {
return cap_motion;
}
- FeatureBasePtr ProcessorImu2d::emplaceFeature(CaptureMotionPtr _capture_motion)
- {
- auto feature = FeatureBase::emplace<FeatureImu2d>(_capture_motion,
- _capture_motion->getBuffer().back().delta_integr_,
- _capture_motion->getBuffer().back().delta_integr_cov_ + unmeasured_perturbation_cov_,
- _capture_motion->getCalibrationPreint(),
- _capture_motion->getBuffer().back().jacobian_calib_);
- return feature;
- }
-
VectorXd ProcessorImu2d::getCalibration (const CaptureBaseConstPtr _capture) const
{
if (_capture)
@@ -124,16 +115,67 @@ namespace wolf {
{
_capture->getSensorIntrinsic()->setState(_calibration);
}
+void ProcessorImu2d::configure(SensorBasePtr _sensor)
+{
+ sensor_imu2d_ = std::static_pointer_cast<SensorImu2d>(_sensor);
+
+ auto acc_drift_std = sensor_imu2d_->getAbRateStdev();
+ auto gyro_drift_std = sensor_imu2d_->getWbRateStdev();
+
+ Array3d imu_drift_sigmas(acc_drift_std, acc_drift_std, gyro_drift_std);
+ imu_drift_cov_ = imu_drift_sigmas.square().matrix().asDiagonal();
- FactorBasePtr ProcessorImu2d::emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin)
+}
+
+ void ProcessorImu2d::emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own)
{
- CaptureImuPtr cap_imu = std::static_pointer_cast<CaptureImu>(_capture_origin);
- FeatureImu2dPtr ftr_imu = std::static_pointer_cast<FeatureImu2d>(_feature_motion);
+ auto feature = FeatureBase::emplace<FeatureImu2d>(
+ _capture_own, _capture_own->getBuffer().back().delta_integr_,
+ _capture_own->getBuffer().back().delta_integr_cov_ + unmeasured_perturbation_cov_,
+ _capture_own->getCalibrationPreint(), _capture_own->getBuffer().back().jacobian_calib_);
+
+ CaptureImuPtr cap_imu = std::static_pointer_cast<CaptureImu>(_capture_origin);
+ FeatureImu2dPtr ftr_imu = std::static_pointer_cast<FeatureImu2d>(feature);
+
+ if (std::static_pointer_cast<SensorImu2d>(getSensor())->isGravityOrthogonal())
+ FactorBase::emplace<FactorImu2d>(ftr_imu, ftr_imu, cap_imu, shared_from_this(),
+ params_->apply_loss_function);
+ else
+ FactorBase::emplace<FactorImu2dWithGravity>(ftr_imu, ftr_imu, cap_imu, shared_from_this(),
+ params_->apply_loss_function);
+
+ if (getSensor()->isStateBlockDynamic('I'))
+ {
+ const auto& sb_imubias_own = _capture_own->getStateBlock('I');
+ const auto& sb_imubias_origin = _capture_origin->getStateBlock('I');
+ if (sb_imubias_own != sb_imubias_origin) // make sure it's two different state blocks! -- just in case
+ {
+ auto dt = _capture_own->getTimeStamp() - _capture_origin->getTimeStamp();
+ auto ftr_bias = FeatureBase::emplace<FeatureBase>(
+ _capture_own, "FeatureBase",
+ Vector3d::Zero(), // mean IMU drift is zero
+ imu_drift_cov_ * dt); // IMU drift cov specified in continuous time
+ auto fac_bias =
+ FactorBase::emplace<FactorBlockDifference>(ftr_bias, ftr_bias,
+ sb_imubias_own, // IMU bias block at t=own
+ sb_imubias_origin, // IMU bias block at t=origin
+ nullptr, // frame other
+ _capture_origin, // origin capture
+ nullptr, // feature other
+ nullptr, // landmark other
+ 0, // take all of first state block
+ -1, // take all of first state block
+ 0, // take all of first second block
+ -1, // take all of first second block
+ shared_from_this(), // this processor
+ params_->apply_loss_function); // loss function
- if( std::static_pointer_cast<SensorImu>(getSensor())->isGravityOrthogonal() )
- return FactorBase::emplace<FactorImu2d>(_feature_motion, ftr_imu, cap_imu, shared_from_this(), params_->apply_loss_function);
- else
- return FactorBase::emplace<FactorImu2dWithGravity>(_feature_motion, ftr_imu, cap_imu, shared_from_this(), params_->apply_loss_function);
+ if( std::static_pointer_cast<SensorImu>(getSensor())->isGravityOrthogonal() )
+ return FactorBase::emplace<FactorImu2d>(_feature_motion, ftr_imu, cap_imu, shared_from_this(), params_->apply_loss_function);
+ else
+ return FactorBase::emplace<FactorImu2dWithGravity>(_feature_motion, ftr_imu, cap_imu, shared_from_this(), params_->apply_loss_function);
+ }
+ }
}
void ProcessorImu2d::computeCurrentDelta(const Eigen::VectorXd& _data,
diff --git a/src/sensor/sensor_compass.cpp b/src/sensor/sensor_compass.cpp
index cae3cce9a206592f544f721794ab3f13a13b68ce..7454777137ccaaec749d478cc363a030284387e4 100644
--- a/src/sensor/sensor_compass.cpp
+++ b/src/sensor/sensor_compass.cpp
@@ -20,6 +20,8 @@
//
//--------LICENSE_END--------
#include "imu/sensor/sensor_compass.h"
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_quaternion.h"
namespace wolf
{
diff --git a/test/gtest_factor_imu2d.cpp b/test/gtest_factor_imu2d.cpp
index 4ca9739fd76d524ec501b39e17da66bc1cc03c1e..8409d49f4e8835dc1627615545e2e5ddffb35f79 100644
--- a/test/gtest_factor_imu2d.cpp
+++ b/test/gtest_factor_imu2d.cpp
@@ -487,6 +487,6 @@ TEST_F(FactorImu2d_test, solve_f1_b1)
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
- // ::testing::GTEST_FLAG(filter) = "FactorImu2d.no_bias_fixed"; // Test only this one
+ // ::testing::GTEST_FLAG(filter) = "FactorImu2d_test.solve_f1_b1"; // Test only this one
return RUN_ALL_TESTS();
}
diff --git a/test/gtest_factor_imu2d_with_gravity.cpp b/test/gtest_factor_imu2d_with_gravity.cpp
index 60c47051701718bd68ef8603f8903735f4e90e33..3fab496313fccf2215d35dfa4c4aa32d9ba3f0e4 100644
--- a/test/gtest_factor_imu2d_with_gravity.cpp
+++ b/test/gtest_factor_imu2d_with_gravity.cpp
@@ -108,8 +108,8 @@ TEST_F(FactorImu2dwithGravity_test, bias_zero_solve_f1)
x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
// Set states
- frm0->setState(x0,"POV");
- frm1->setState(x1,"POV");
+ frm0->setState(x0);
+ frm1->setState(x1);
sensor->getStateBlock('G')->setState(g);
// feature & factor with delta measurement
@@ -134,7 +134,7 @@ TEST_F(FactorImu2dwithGravity_test, bias_zero_solve_f1)
// remove feature (and factor) for the next loop
fea1->remove();
-}
+ }
}
TEST_F(FactorImu2dwithGravity_test, bias_zero_solve_f0)
@@ -157,8 +157,8 @@ TEST_F(FactorImu2dwithGravity_test, bias_zero_solve_f0)
x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
// Set states
- frm0->setState(x0,"POV");
- frm1->setState(x1,"POV");
+ frm0->setState(x0);
+ frm1->setState(x1);
sensor->getStateBlock('G')->setState(g);
// feature & factor with delta measurement
@@ -174,18 +174,17 @@ TEST_F(FactorImu2dwithGravity_test, bias_zero_solve_f0)
frm0->perturb(0.01);
// solve
- std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+ std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
- ASSERT_POSE2d_APPROX(frm1->getStateVector("PO"), x1.head(3), 1e-6);
- ASSERT_MATRIX_APPROX(frm1->getStateVector("V"), x1.tail(2), 1e-6);
- //WOLF_INFO(frm1->getStateVector("POV"));
+ ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6);
+ //WOLF_INFO(frm1->getStateVector());
// remove feature (and factor) for the next loop
fea1->remove();
}
}
-TEST_F(FactorImu2dwithGravity_test, bias_zero_solve_b1)
+TEST(FactorImu2dWithGravity, bias_zero_solve_b1)
{
for(int i = 0; i < N_TESTS; i++){
// Random delta
@@ -227,19 +226,19 @@ TEST_F(FactorImu2dwithGravity_test, bias_zero_solve_b1)
cap1->perturb(0.01);
// solve to estimate bias at cap1
- std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+ std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
- ASSERT_MATRIX_APPROX(cap1->getStateVector("I"), b0, 1e-6);
- //WOLF_INFO(cap1->getStateVector("I"));
+ ASSERT_MATRIX_APPROX(cap1->getStateVector(), b0, 1e-6);
+ //WOLF_INFO(cap1->getStateVector());
// remove feature (and factor) for the next loop
fea1->remove();
}
}
-TEST_F(FactorImu2dwithGravity_test, solve_b0)
+TEST(FactorImu2dWithGravity, solve_b0)
{
- for(int i = 0; i < N_TESTS; i++){
+ for(int i = 0; i < 50; i++){
// Random delta
Vector5d delta_biased = Vector5d::Random() * 10;
delta_biased(2) = pi2pi(delta_biased(2));
diff --git a/test/gtest_imu.cpp b/test/gtest_imu.cpp
index f216b3a3890cf3237535fc136f1d2c58c32c2004..1e40e01894fb977bebd9f782d716b0fcd3303424 100644
--- a/test/gtest_imu.cpp
+++ b/test/gtest_imu.cpp
@@ -27,20 +27,19 @@
*/
//Wolf
-#include <core/ceres_wrapper/solver_ceres.h>
#include "imu/processor/processor_imu.h"
#include "imu/sensor/sensor_imu.h"
-
+#include "imu/math/imu_tools.h"
#include "imu/internal/config.h"
+#include <core/utils/utils_gtest.h>
#include <core/common/wolf.h>
#include <core/sensor/sensor_odom.h>
#include <core/processor/processor_odom_3d.h>
-#include <core/utils/utils_gtest.h>
+#include <core/ceres_wrapper/solver_ceres.h>
//#include <core/utils/logging.h>
-#include "imu/math/imu_tools.h"
// make my life easier
using namespace Eigen;
@@ -1537,10 +1536,44 @@ TEST_F(Process_Factor_Imu_ODO, RecoverTrajectory_MotionRandom_PqV_b__pqV_b) // F
}
+TEST_F(Process_Factor_Imu, bootstrap)
+{
+ processor_imu->setVotingActive(true);
+ processor_imu->setMaxTimeSpan(0.04);
+ processor_imu->bootstrapEnable(true);
+
+ auto KF0 = problem->emplaceFrame(0.0);
+ problem->keyFrameCallback(KF0,nullptr);
+ problem->print(4, 0, 1, 0);
+
+ // Vector6d data(0,0,9.806, 0,0,0); // gravity on Z
+ // Vector6d data(0.0, 9.806, 0.0, 0.0, 0.0, 0.0); // gravity on Y
+ Vector6d data;
+ data << 0.0, 9.806, 0.0, 0.0, 0.0, 0.0 ; // gravity on Y
+
+ capture_imu = std::make_shared<CaptureImu>(0.0, sensor_imu, data, Matrix6d::Identity());
+
+ for (t = 0; t < 0.14; t += dt)
+ {
+ capture_imu->setTimeStamp(t);
+ capture_imu->process();
+ }
+
+ problem->print(4, 0, 1, 1);
+
+ Quaterniond qref(AngleAxisd(M_PI / 2, Vector3d::UnitX())); // turn of +90deg over X
+ for (auto pair_ts_frame : problem->getTrajectory()->getFrameMap())
+ {
+ ASSERT_MATRIX_APPROX(pair_ts_frame.second->getStateVector("P"), Vector3d::Zero(), 1e-10);
+ ASSERT_MATRIX_APPROX(pair_ts_frame.second->getStateVector("O"), qref.coeffs(), 1e-10);
+ ASSERT_MATRIX_APPROX(pair_ts_frame.second->getStateVector("V"), Vector3d::Zero(), 1e-10);
+ }
+}
+
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
- // ::testing::GTEST_FLAG(filter) = "Process_Factor_Imu.*";
+ // ::testing::GTEST_FLAG(filter) = "Process_Factor_Imu.bootstrap";
// ::testing::GTEST_FLAG(filter) = "Process_Factor_Imu_ODO.*";
// ::testing::GTEST_FLAG(filter) = "Process_Factor_Imu_ODO.RecoverTrajectory_MotionRandom_PqV_b__pqV_b";
diff --git a/test/gtest_imu_tools.cpp b/test/gtest_imu_tools.cpp
index 4053b15f912ef7e5cb4b39181e7a5b81395d0c6a..12c6c97f966c2a78dc961d2bec6f39f4698adadf 100644
--- a/test/gtest_imu_tools.cpp
+++ b/test/gtest_imu_tools.cpp
@@ -641,6 +641,116 @@ TEST(Imu_tools, full_delta_residual)
// WOLF_TRACE("Delta err exp-pre: ", Delta_err.transpose());
}
+TEST(extract_v0_g, all_zeros)
+{
+ Vector3d p0(0, 0, 0);
+ Quaterniond q0(1, 0, 0, 0);
+ Vector3d p1(0, 0, 0);
+ Quaterniond q1(1, 0, 0, 0);
+ Vector3d p2(0, 0, 0);
+ double Dt01(1), Dt12(1);
+ Vector3d Dp01(0, 0, 0);
+ Vector3d Dv01(0, 0, 0);
+ Vector3d Dp12(0, 0, 0);
+ Vector3d v0, g;
+
+ extract_v0_g(p0, q0, p1, q1, p2, Dt01, Dp01, Dv01, Dt12, Dp12, v0, g);
+
+ ASSERT_MATRIX_APPROX(v0, Vector3d(0, 0, 0), 1e-15);
+ ASSERT_MATRIX_APPROX(g, Vector3d(0, 0, 0), 1e-15);
+}
+
+TEST(extract_v0_g, free_fall_no_rotation)
+{
+ /** free fall v0 = 0
+ *
+ * X p0 = 0, v0 = 0
+ * .
+ * X p1 = -4.9m
+ * .
+ * .
+ * .
+ * .
+ * X p2 = -19,6m
+ */
+
+ Vector3d p0(0, 0, 0);
+ Quaterniond q0(1, 0, 0, 0);
+ Vector3d p1(0, 0, -9.8 / 2);
+ Quaterniond q1(1, 0, 0, 0);
+ Vector3d p2(0, 0, -9.8 * 4 / 2);
+ double Dt01(1), Dt12(1);
+ Vector3d Dp01(0, 0, 0);
+ Vector3d Dv01(0, 0, 0);
+ Vector3d Dp12(0, 0, 0);
+ Vector3d v0, g;
+
+ extract_v0_g(p0, q0, p1, q1, p2, Dt01, Dp01, Dv01, Dt12, Dp12, v0, g);
+
+ ASSERT_MATRIX_APPROX(v0, Vector3d(0, 0, 0), 1e-15);
+ ASSERT_MATRIX_APPROX(g, Vector3d(0, 0, -9.8), 1e-15);
+}
+
+TEST(extract_v0_g, free_fall_no_rotation_hor_vel)
+{
+ /** free fall v0 = 1m/s horizontally --> parabolic fall
+ *
+ * X p0 = (0,0)m, v0 = (1,0)m/s
+ * .
+ * X p1 = (1,-4.9)m
+ * .
+ * .
+ * .
+ * .
+ * X p2 = (2,-19,6)m
+ */
+ Vector3d p0(0, 0, 0);
+ Quaterniond q0(1, 0, 0, 0);
+ Vector3d p1(1, 0, -9.8 / 2);
+ Quaterniond q1(1, 0, 0, 0);
+ Vector3d p2(2, 0, -9.8 * 4 / 2);
+ double Dt01(1), Dt12(1);
+ Vector3d Dp01(0, 0, 0);
+ Vector3d Dv01(0, 0, 0);
+ Vector3d Dp12(0, 0, 0);
+ Vector3d v0, g;
+
+ extract_v0_g(p0, q0, p1, q1, p2, Dt01, Dp01, Dv01, Dt12, Dp12, v0, g);
+
+ ASSERT_MATRIX_APPROX(v0, Vector3d(1, 0, 0), 1e-15);
+ ASSERT_MATRIX_APPROX(g, Vector3d(0, 0, -9.8), 1e-15);
+}
+
+TEST(extract_v0_g, free_fall_no_rotation_diag_vel)
+{
+ /** free fall v0 = 1m/s horizontally and 9.8 vert --> parabolic ballistic flight
+ *
+ *
+ * X p1 = (1, +4.9)m
+ * . .
+ * . .
+ * . .
+ * X p0 = (0,0)m X p2 = (2, 0)m
+ * v0 = (1, +9.8)m/s
+ */
+
+ Vector3d p0(0, 0, 0);
+ Quaterniond q0(1, 0, 0, 0);
+ Vector3d p1(1, 0, +9.8 / 2);
+ Quaterniond q1(1, 0, 0, 0);
+ Vector3d p2(2, 0, 0);
+ double Dt01(1), Dt12(1);
+ Vector3d Dp01(0, 0, 0);
+ Vector3d Dv01(0, 0, 0);
+ Vector3d Dp12(0, 0, 0);
+ Vector3d v0, g;
+
+ extract_v0_g(p0, q0, p1, q1, p2, Dt01, Dp01, Dv01, Dt12, Dp12, v0, g);
+
+ ASSERT_MATRIX_APPROX(v0, Vector3d(1, 0, 9.8), 1e-15);
+ ASSERT_MATRIX_APPROX(g, Vector3d(0, 0, -9.8), 1e-15);
+}
+
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
diff --git a/test/yaml/processor_imu2d_static_init.yaml b/test/yaml/processor_imu2d_static_init.yaml
index 3776c8720e581fada284e5581238e5c89f49c09f..9174c8e5aa4a62c5c1fddc911786cbf5e85dd5c0 100644
--- a/test/yaml/processor_imu2d_static_init.yaml
+++ b/test/yaml/processor_imu2d_static_init.yaml
@@ -9,5 +9,10 @@ keyframe_vote:
dist_traveled: 100000000000 # meters
angle_turned: 10000000000 # radians (1 rad approx 57 deg, approx 60 deg)
+bootstrap:
+ enable: false
+ method: "G"
+ averaging_length: 10
+
state_provider: true
state_provider_order: 1