diff --git a/.gitignore b/.gitignore
index cd7e137f773e8db3658210579cefcf02ee92d933..4d9c52e82fe8dee50c99c9a1c8548575962ce658 100644
--- a/.gitignore
+++ b/.gitignore
@@ -34,12 +34,10 @@ src/examples/map_apriltag_save.yaml
\.vscode/
-IMU.found
-imu.found
.ccls
.ccls-cache
.ccls-root
compile_commands.json
.vimspector.json
-est.csv
-/imu.found
+est*.csv
+.clang-format
diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index ef29f921fb416b8178f4452a1082ebad5b7b5383..3301cc59ed564f44ff28ae41484ce3ee4eb50ad0 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -92,7 +92,7 @@ stages:
- cd build
- cmake -DCMAKE_BUILD_TYPE=release -DBUILD_TESTS=ON ..
- make -j$(nproc)
- - ctest -j$(nproc)
+ - ctest -j$(nproc) --output-on-failure
- make install
############ LICENSE HEADERS ############
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5dc35a87041678cea8d4c047a345d7029f69b3ed..b8fa448312e146b9956a5c0396e0802f8c99dbf6 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -13,7 +13,7 @@ MESSAGE("Starting ${PROJECT_NAME} CMakeLists ...")
# Paths
SET(EXECUTABLE_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/bin)
SET(LIBRARY_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/lib)
-set(INCLUDE_INSTALL_DIR include/iri-algorithms/wolf)
+set(INCLUDE_INSTALL_DIR include/wolf)
set(LIB_INSTALL_DIR lib/)
IF (NOT CMAKE_BUILD_TYPE)
@@ -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,41 +93,37 @@ 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
- include/imu/capture/capture_compass.h
- include/imu/capture/capture_imu.h
+ include/${PROJECT_NAME}/capture/capture_compass.h
+ include/${PROJECT_NAME}/capture/capture_imu.h
)
SET(HDRS_FACTOR
- include/imu/factor/factor_compass_3d.h
- include/imu/factor/factor_imu.h
- include/imu/factor/factor_imu2d.h
- include/imu/factor/factor_imu2d_with_gravity.h
+ include/${PROJECT_NAME}/factor/factor_compass_3d.h
+ include/${PROJECT_NAME}/factor/factor_imu.h
+ include/${PROJECT_NAME}/factor/factor_imu2d.h
+ include/${PROJECT_NAME}/factor/factor_imu2d_with_gravity.h
)
SET(HDRS_FEATURE
- include/imu/feature/feature_imu.h
- include/imu/feature/feature_imu2d.h
+ include/${PROJECT_NAME}/feature/feature_imu.h
+ include/${PROJECT_NAME}/feature/feature_imu2d.h
)
SET(HDRS_MATH
- include/imu/math/imu_tools.h
+ include/${PROJECT_NAME}/math/imu_tools.h
)
SET(HDRS_PROCESSOR
- include/imu/processor/processor_compass.h
- include/imu/processor/processor_imu.h
- include/imu/processor/processor_imu2d.h
+ include/${PROJECT_NAME}/processor/processor_compass.h
+ include/${PROJECT_NAME}/processor/processor_imu.h
+ include/${PROJECT_NAME}/processor/processor_imu2d.h
)
SET(HDRS_SENSOR
- include/imu/sensor/sensor_compass.h
- include/imu/sensor/sensor_imu.h
- include/imu/sensor/sensor_imu2d.h
+ include/${PROJECT_NAME}/sensor/sensor_compass.h
+ include/${PROJECT_NAME}/sensor/sensor_imu.h
+ include/${PROJECT_NAME}/sensor/sensor_imu2d.h
)
# ============ SOURCES ============
@@ -214,9 +210,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}>
)
@@ -237,8 +233,6 @@ INSTALL(FILES ${HDRS_SENSOR}
INSTALL(FILES "${WOLF_CONFIG_DIR}/config.h"
DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/internal)
-INSTALL(DIRECTORY ${SPDLOG_INCLUDE_DIRS} DESTINATION "include/iri-algorithms/")
-
export(PACKAGE ${PLUGIN_NAME})
FIND_PACKAGE(Doxygen MODULE)
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 a9c164c5804e7d0c09041ef3507cf9f27f9d03d4..9f06db13324673b0ac0a2e6891bf38f2fc6c9ef6 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 9e752656f10de383513a406538da1f399c2ec1a7..e036d4582e0714bf0dae9d776a5f79db08628b2a 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(...)
@@ -150,23 +147,7 @@ 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
- const double ab_rate_stdev_, wb_rate_stdev_; //stdev for standard_deviation (= sqrt(variance))
- /** 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::Matrix3d sqrt_A_r_dt_inv;
- const Eigen::Matrix3d sqrt_W_r_dt_inv;
-
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
};
@@ -177,8 +158,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,
@@ -195,8 +176,7 @@ 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),
@@ -208,11 +188,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()),
- ab_rate_stdev_(std::static_pointer_cast<SensorImu>(_ftr_ptr->getCapture()->getSensor())->getAbRateStdev()),
- wb_rate_stdev_(std::static_pointer_cast<SensorImu>(_ftr_ptr->getCapture()->getSensor())->getWbRateStdev()),
- sqrt_A_r_dt_inv((Eigen::Matrix3d::Identity() * ab_rate_stdev_ * sqrt(dt_)).inverse()),
- sqrt_W_r_dt_inv((Eigen::Matrix3d::Identity() * wb_rate_stdev_ * sqrt(dt_)).inverse())
+ dt_(_ftr_ptr->getFrame()->getTimeStamp() - _cap_origin_ptr->getTimeStamp())
{
//
}
@@ -225,7 +201,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;
@@ -240,12 +215,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;
}
@@ -264,7 +237,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);
@@ -283,8 +256,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
{
@@ -298,8 +269,8 @@ inline bool FactorImu::residual(const Eigen::MatrixBase<D1> & _p1,
*
* We use the following functions from imu_tools.h:
* D = betweenStates(x1,x2,dt) : obtain a Delta from two states separated dt=t2-t1
- * D2 = plus(D1,T) : plus operator, D2 = D1 (+) T
- * T = diff(D1,D2) : minus operator, T = D2 (-) D1
+ * D2 = plus(D1,T) : blocl-plus operator, D2 = D1 <+> T
+ * T = diff(D1,D2) : block-minus operator, T = D2 <-> D1
*
* Two methods are possible (select with #define below this note) :
*
@@ -330,28 +301,25 @@ 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;
+ Eigen::Matrix<T, 3, 1> dp_step, do_step, dv_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>());
+ dp_step = dDp_dab_ * (_ab1 - acc_bias_preint_) + dDp_dwb_ * (_wb1 - gyro_bias_preint_);
+ do_step = /* it happens that dDq_dab = 0 ! */ dDq_dwb_ * (_wb1 - gyro_bias_preint_);
+ dv_step = 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);
- Eigen::Matrix<T, 3, 1> do_step = d_step.block(3,0,3,1);
- Eigen::Matrix<T, 3, 1> dv_step = d_step.block(6,0,3,1);
// 2.b. Add the correction step to the preintegrated delta: delta_cor = delta_preint (+) step
Eigen::Matrix<T,3,1> dp_correct;
@@ -365,7 +333,7 @@ inline bool FactorImu::residual(const Eigen::MatrixBase<D1> & _p1,
// 3. Delta error in minimal form: D_err = diff(D_exp , D_corr)
// Note the Dt here is zero because it's the delta-time between the same time stamps!
Eigen::Matrix<T, 9, 1> d_error;
- Eigen::Map<Eigen::Matrix<T, 3, 1> > dp_error(d_error.data() );
+ Eigen::Map<Eigen::Matrix<T, 3, 1> > dp_error(d_error.data() + 0);
Eigen::Map<Eigen::Matrix<T, 3, 1> > do_error(d_error.data() + 3);
Eigen::Map<Eigen::Matrix<T, 3, 1> > dv_error(d_error.data() + 6);
@@ -380,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
@@ -401,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
- Eigen::Matrix<T,3,1> ab_error(_ab1 - _ab2); // KF1.bias - KF2.bias
- Eigen::Matrix<T,3,1> wb_error(_wb1 - _wb2);
-
- // 4. Residuals are the weighted errors
- _res.segment(9,3) = sqrt_A_r_dt_inv.cast<T>() * ab_error;
- _res.tail(3) = sqrt_W_r_dt_inv.cast<T>() * wb_error;
//////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////// PRINT VALUES ///////////////////////////////////
diff --git a/include/imu/factor/factor_imu2d.h b/include/imu/factor/factor_imu2d.h
index 11b87fee551371907a9ac38883d7c0f2c15fb5a2..394dceecbb2d239585066818bc6b93e4ff53d559 100644
--- a/include/imu/factor/factor_imu2d.h
+++ b/include/imu/factor/factor_imu2d.h
@@ -37,7 +37,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,
@@ -62,8 +62,8 @@ 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 FeatureImu2dPtr& _ftr_ptr)
{
Eigen::Matrix3d res = Eigen::Matrix3d::Identity();
@@ -91,20 +91,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;
@@ -117,7 +104,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,
@@ -134,15 +121,13 @@ 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()),
processor_imu2d_(std::static_pointer_cast<ProcessorImu2d>(_processor_ptr)),
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())
{
//
}
@@ -155,7 +140,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;
@@ -169,9 +153,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
/*
@@ -191,12 +174,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 681eb46c285d45c0e8e33674e37b850ca21890fb..77fa8a50b0aa5e5db81fca43ca1355006c5ed7a6 100644
--- a/include/imu/factor/factor_imu2d_with_gravity.h
+++ b/include/imu/factor/factor_imu2d_with_gravity.h
@@ -36,7 +36,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,
@@ -61,9 +61,9 @@ 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 FeatureImu2dPtr& _ftr_ptr)
{
Eigen::Matrix3d res = Eigen::Matrix3d::Identity();
@@ -91,20 +91,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;
@@ -117,7 +104,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,
@@ -135,15 +122,13 @@ 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')),
processor_imu2d_(std::static_pointer_cast<ProcessorImu2d>(_processor_ptr)),
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())
{
//
}
@@ -156,7 +141,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
{
@@ -171,10 +155,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
/*
@@ -194,12 +177,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 f579ae2dc44fc2f7cd5a6d1b70ba49df21dfdcb7..316405e50afa3ce2a620766e6c3a910615797536 100644
--- a/include/imu/math/imu2d_tools.h
+++ b/include/imu/math/imu2d_tools.h
@@ -257,7 +257,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 7101c04eb11afd5285fd2f77dd2f9c281780c5fc..c5744fbae31fea609aad91e70da9cf55cf83bb66 100644
--- a/include/imu/math/imu_tools.h
+++ b/include/imu/math/imu_tools.h
@@ -282,7 +282,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
@@ -868,6 +868,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/include/imu/sensor/sensor_imu.h b/include/imu/sensor/sensor_imu.h
index 0b9cf145a65d88704296ff7f0dcf54069356595c..fc6f6990ecb20d5c63cdfac31f54bcebd9bcc547 100644
--- a/include/imu/sensor/sensor_imu.h
+++ b/include/imu/sensor/sensor_imu.h
@@ -47,6 +47,9 @@ struct ParamsSensorImu : public ParamsSensorBase
double ab_rate_stdev = 0.00001;
double wb_rate_stdev = 0.00001;
+ // TODO: NOT necessary after the big refactor
+ bool dynamic_imu_bias = true;
+
~ParamsSensorImu() override = default;
ParamsSensorImu()
{
@@ -61,6 +64,7 @@ struct ParamsSensorImu : public ParamsSensorBase
wb_initial_stdev = _server.getParam<double>(prefix + _unique_name + "/wb_initial_stdev");
ab_rate_stdev = _server.getParam<double>(prefix + _unique_name + "/ab_rate_stdev");
wb_rate_stdev = _server.getParam<double>(prefix + _unique_name + "/wb_rate_stdev");
+ dynamic_imu_bias = _server.getParam<bool> (prefix + _unique_name + "/dynamic_imu_bias");
}
std::string print() const override
{
@@ -70,7 +74,8 @@ struct ParamsSensorImu : public ParamsSensorBase
+ "ab_initial_stdev: " + std::to_string(ab_initial_stdev) + "\n"
+ "wb_initial_stdev: " + std::to_string(wb_initial_stdev) + "\n"
+ "ab_rate_stdev: " + std::to_string(ab_rate_stdev) + "\n"
- + "wb_rate_stdev: " + std::to_string(wb_rate_stdev) + "\n";
+ + "wb_rate_stdev: " + std::to_string(wb_rate_stdev) + "\n"
+ + "dynamic_imu_bias: " + std::to_string(wb_rate_stdev) + "\n";
}
};
@@ -95,12 +100,12 @@ class SensorImu : public SensorBase
SensorImu(const Eigen::VectorXd& _extrinsics, ParamsSensorImuPtr _params);
WOLF_SENSOR_CREATE(SensorImu, ParamsSensorImu, 7);
- double getGyroNoise() const;
double getAccelNoise() const;
- double getWbInitialStdev() const;
+ double getGyroNoise() const;
double getAbInitialStdev() const;
- double getWbRateStdev() const;
+ double getWbInitialStdev() const;
double getAbRateStdev() const;
+ double getWbRateStdev() const;
~SensorImu() override;
diff --git a/src/capture/capture_imu.cpp b/src/capture/capture_imu.cpp
index f8a4b0b4118c299fda57c59e77df1b922b2e292a..7f38375ad2335533033fb4352a3871c65a6007b2 100644
--- a/src/capture/capture_imu.cpp
+++ b/src/capture/capture_imu.cpp
@@ -21,45 +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,
- std::make_shared<StateBlock>((_sensor_ptr->getProblem()->getDim() == 2 ? 3 : 6), false))
+ 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,
- std::make_shared<StateBlock>(_bias, false))
+ 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 5e429fd80355bddf587eaccafec7bf4db8476b02..c4cba6b04638efb09da580494f56755caaf4d1f9 100644
--- a/src/processor/processor_imu.cpp
+++ b/src/processor/processor_imu.cpp
@@ -26,14 +26,16 @@
// wolf
#include <core/state_block/state_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 dc35d28c0a43844868505597948fed1315b83af3..10c6f25d5bd616737c4d7b0f65ceb2759aa64999 100644
--- a/src/processor/processor_imu2d.cpp
+++ b/src/processor/processor_imu2d.cpp
@@ -27,12 +27,14 @@
*/
// imu
#include "imu/processor/processor_imu2d.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/state_composite.h>
+#include <core/factor/factor_block_difference.h>
namespace wolf {
@@ -101,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)
@@ -123,16 +115,63 @@ 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<SensorImu2d>(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 43171641af0110385be84e72f40251bc5797b5cc..b474c523b103fbef8c1a654db69c11be73c3a31e 100644
--- a/src/sensor/sensor_compass.cpp
+++ b/src/sensor/sensor_compass.cpp
@@ -20,7 +20,7 @@
//
//--------LICENSE_END--------
#include "imu/sensor/sensor_compass.h"
-#include "core/state_block/state_block.h"
+#include "core/state_block/state_block_derived.h"
#include "core/state_block/state_quaternion.h"
namespace wolf
@@ -29,10 +29,10 @@ namespace wolf
SensorCompass::SensorCompass(const Eigen::VectorXd& _extrinsics,
ParamsSensorCompassPtr _params) :
SensorBase("SensorCompass",
- std::make_shared<StateBlock>(_extrinsics.head(3), true),
- std::make_shared<StateQuaternion>(_extrinsics.tail(4), true),
- std::make_shared<StateBlock>(_params->bias_prior_state,
- _params->bias_prior_mode == "fix"),
+ std::make_shared<StatePoint3d>(_extrinsics.head(3), true, false),
+ std::make_shared<StateQuaternion>(_extrinsics.tail(4), true, false),
+ std::make_shared<StateParams3>(_params->bias_prior_state,
+ _params->bias_prior_mode == "fix"),
3,
false, // P static
false, // O static
@@ -46,8 +46,8 @@ SensorCompass::SensorCompass(const Eigen::VectorXd& _extrinsics,
// Global Magnetic Field 'F' state block
addStateBlock('F',
- std::make_shared<StateBlock>(params_compass_->field_prior_state,
- params_compass_->field_prior_mode == "fix"));
+ std::make_shared<StateParams3>(params_compass_->field_prior_state,
+ params_compass_->field_prior_mode == "fix"));
if (params_compass_->field_prior_mode == "factor")
addPriorParameter('F',
diff --git a/src/sensor/sensor_imu.cpp b/src/sensor/sensor_imu.cpp
index bb6ec35e0061202819385fe04b5b2c58c54a3160..d5a676f66ac4297972f84706601fc963725805e7 100644
--- a/src/sensor/sensor_imu.cpp
+++ b/src/sensor/sensor_imu.cpp
@@ -20,8 +20,9 @@
//
//--------LICENSE_END--------
#include "imu/sensor/sensor_imu.h"
-#include "core/state_block/state_block.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 {
@@ -33,11 +34,11 @@ SensorImu::SensorImu(const Eigen::VectorXd& _extrinsics, ParamsSensorImuPtr _par
SensorImu::SensorImu(const Eigen::VectorXd& _extrinsics, const ParamsSensorImu& _params) :
SensorBase("SensorImu",
- std::make_shared<StateBlock>(_extrinsics.head(3), true),
- std::make_shared<StateQuaternion>(_extrinsics.tail(4), true),
- std::make_shared<StateBlock>(6, false, nullptr),
+ std::make_shared<StatePoint3d>(_extrinsics.head(3), true, false),
+ std::make_shared<StateQuaternion>(_extrinsics.tail(4), true, false),
+ std::make_shared<StateParams6>(Vector6d::Zero(), false),
(Eigen::Vector6d()<<_params.a_noise,_params.a_noise,_params.a_noise,_params.w_noise,_params.w_noise,_params.w_noise).finished(),
- false, false, true),
+ false, false, _params.dynamic_imu_bias),
a_noise(_params.a_noise),
w_noise(_params.w_noise),
ab_initial_stdev(_params.ab_initial_stdev),
diff --git a/src/sensor/sensor_imu2d.cpp b/src/sensor/sensor_imu2d.cpp
index f74b3baa23383ea5265d7d288abc8d97d225a0ff..3fe55da10b1fb5d9d303d5e2b1862085f42c815f 100644
--- a/src/sensor/sensor_imu2d.cpp
+++ b/src/sensor/sensor_imu2d.cpp
@@ -19,8 +19,9 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
-#include <imu/sensor/sensor_imu2d.h>
-#include <core/state_block/state_block.h>
+#include "imu/sensor/sensor_imu2d.h"
+
+#include <core/state_block/state_block_derived.h>
#include <core/state_block/state_angle.h>
namespace wolf {
@@ -31,20 +32,27 @@ SensorImu2d::SensorImu2d(const Eigen::VectorXd& _extrinsics, ParamsSensorImu2dPt
//
}
-SensorImu2d::SensorImu2d(const Eigen::VectorXd& _extrinsics, const ParamsSensorImu2d& _params) :
- SensorBase("SensorImu2d", std::make_shared<StateBlock>(_extrinsics.head(2), true), std::make_shared<StateAngle>(_extrinsics(2), true), std::make_shared<StateBlock>(3, false, nullptr), (Eigen::Vector3d()<<_params.a_noise,_params.a_noise,_params.w_noise).finished(), false, false, true),
- a_noise(_params.a_noise),
- w_noise(_params.w_noise),
- ab_initial_stdev(_params.ab_initial_stdev),
- wb_initial_stdev(_params.wb_initial_stdev),
- ab_rate_stdev(_params.ab_rate_stdev),
- wb_rate_stdev(_params.wb_rate_stdev),
- orthogonal_gravity(_params.orthogonal_gravity)
+SensorImu2d::SensorImu2d(const Eigen::VectorXd& _extrinsics, const ParamsSensorImu2d& _params)
+ : SensorBase("SensorImu2d",
+ std::make_shared<StatePoint2d>(_extrinsics.head(2), true, false),
+ std::make_shared<StateAngle>(_extrinsics(2), true, false),
+ std::make_shared<StateParams3>(Vector3d::Zero(3), false),
+ Eigen::Vector3d(_params.a_noise, _params.a_noise, _params.w_noise),
+ false,
+ false,
+ false),
+ a_noise(_params.a_noise),
+ w_noise(_params.w_noise),
+ ab_initial_stdev(_params.ab_initial_stdev),
+ wb_initial_stdev(_params.wb_initial_stdev),
+ ab_rate_stdev(_params.ab_rate_stdev),
+ wb_rate_stdev(_params.wb_rate_stdev),
+ orthogonal_gravity(_params.orthogonal_gravity)
{
assert(_extrinsics.size() == 3 && "Wrong extrinsics vector size! Should be 3 for 2d.");
- if(!orthogonal_gravity)
+ if (!orthogonal_gravity)
{
- addStateBlock('G', std::make_shared<StateBlock>(2, false), false);
+ addStateBlock('G', std::make_shared<StateVector2d>(Vector2d::Zero(), false, true), false);
}
}
diff --git a/src/yaml/processor_imu_yaml.cpp b/src/yaml/processor_imu_yaml.cpp
index 199f59af44bd61d5dcf60ce1a815def2e3cb950a..c5fd0333e06d6732128516081705339840100665 100644
--- a/src/yaml/processor_imu_yaml.cpp
+++ b/src/yaml/processor_imu_yaml.cpp
@@ -48,17 +48,41 @@ static ParamsProcessorBasePtr createProcessorImuParams(const std::string & _file
if (config["type"].as<std::string>() == "ProcessorImu")
{
- YAML::Node kf_vote = config["keyframe_vote"];
ParamsProcessorImuPtr params = std::make_shared<ParamsProcessorImu>();
params->time_tolerance = config["time_tolerance"] .as<double>();
params->unmeasured_perturbation_std = config["unmeasured_perturbation_std"].as<double>();
- params->max_time_span = kf_vote["max_time_span"] .as<double>();
- params->max_buff_length = kf_vote["max_buff_length"] .as<SizeEigen>();
- params->dist_traveled = kf_vote["dist_traveled"] .as<double>();
- params->angle_turned = kf_vote["angle_turned"] .as<double>();
- params->voting_active = kf_vote["voting_active"] .as<bool>();
+ YAML::Node kf_vote = config["keyframe_vote"];
+ params->max_time_span = kf_vote["max_time_span"].as<double>();
+ params->max_buff_length = kf_vote["max_buff_length"].as<SizeEigen>();
+ params->dist_traveled = kf_vote["dist_traveled"].as<double>();
+ params->angle_turned = kf_vote["angle_turned"].as<double>();
+ params->voting_active = kf_vote["voting_active"].as<bool>();
+
+ YAML::Node bootstrap = config["bootstrap"];
+ params->bootstrap_enable = bootstrap["enable"].as<bool>();
+ if (bootstrap["method"])
+ // if (params->bootstrap_enable)
+ {
+ string str = bootstrap["method"].as<string>();
+ std::transform(str.begin(), str.end(), str.begin(), ::toupper);
+ if (str == "STATIC" /**/)
+ {
+ params->bootstrap_method = ParamsProcessorImu::BootstrapMethod::BOOTSTRAP_STATIC;
+ params->bootstrap_averaging_length = bootstrap["averaging_length"].as<int>();
+ }
+ if (str == "G" /* */)
+ {
+ params->bootstrap_method = ParamsProcessorImu::BootstrapMethod::BOOTSTRAP_G;
+ params->bootstrap_averaging_length = bootstrap["averaging_length"].as<double>();
+ }
+ if (str == "V0_G" /* */)
+ {
+ params->bootstrap_method = ParamsProcessorImu::BootstrapMethod::BOOTSTRAP_V0_G;
+ }
+ }
+
return params;
}
diff --git a/src/yaml/sensor_imu_yaml.cpp b/src/yaml/sensor_imu_yaml.cpp
index e530b244596d202833a88b6c9c48150025d7ec6f..cff3ec8a4a936d450583c968d3db631adfbc6f8d 100644
--- a/src/yaml/sensor_imu_yaml.cpp
+++ b/src/yaml/sensor_imu_yaml.cpp
@@ -59,6 +59,8 @@ static ParamsSensorBasePtr createParamsSensorImu(const std::string & _filename_d
params->ab_rate_stdev = variances["ab_rate_stdev"] .as<double>();
params->wb_rate_stdev = variances["wb_rate_stdev"] .as<double>();
+ params->dynamic_imu_bias = config["dynamic_imu_bias"] .as<bool>();
+
return params;
}
diff --git a/test/gtest_factor_imu2d.cpp b/test/gtest_factor_imu2d.cpp
index 7dd71060608bd87f8b3188f9827f213d181322e6..7437f2df480b4834adc33c6942db41e5e5d54ddc 100644
--- a/test/gtest_factor_imu2d.cpp
+++ b/test/gtest_factor_imu2d.cpp
@@ -29,418 +29,332 @@
using namespace Eigen;
using namespace wolf;
-// Input odometry data and covariance
-Matrix6d data_cov = 0.1*Matrix6d::Identity();
-Matrix5d delta_cov = 0.1*Matrix5d::Identity();
-
-// Jacobian of the bias
-MatrixXd jacobian_bias((MatrixXd(5,3) << 1, 0, 0,
- 0, 1, 0,
- 0, 0, 1,
- 1, 0, 0,
- 0, 1, 0 ).finished());
-//preintegration bias
-Vector3d b0_preint = Vector3d::Zero();
-
-// Problem and solver
-ProblemPtr problem_ptr = Problem::create("POV", 2);
-SolverCeres solver(problem_ptr);
-
-// Two frames
-FrameBasePtr frm0 = problem_ptr->emplaceFrame(TimeStamp(0), Vector5d::Zero());
-FrameBasePtr frm1 = problem_ptr->emplaceFrame(TimeStamp(1), Vector5d::Zero());
-
-// Imu2d sensor
-SensorBasePtr sensor = std::make_shared<SensorImu2d>( Vector3d::Zero(), ParamsSensorImu2d() ); // default params: see sensor_imu2d.h
-
-//capture from frm1 to frm0
-auto cap0 = CaptureBase::emplace<CaptureImu>(frm0, 0, sensor, Vector6d::Zero(), data_cov, Vector3d::Zero(), nullptr);
-auto cap1 = CaptureBase::emplace<CaptureImu>(frm1, 1, sensor, Vector6d::Zero(), data_cov, Vector3d::Zero(), cap0);
-
-TEST(FactorImu2d, check_tree)
+class FactorImu2d_test : public testing::Test
{
- ASSERT_TRUE(problem_ptr->check(0));
-}
-
-TEST(FactorImu2d, bias_zero_solve_f1)
+ public:
+ ProblemPtr problem;
+ SolverCeresPtr solver;
+ SensorImu2dPtr sensor;
+ FrameBasePtr frm0, frm1;
+ CaptureImuPtr cap0, cap1;
+ Matrix6d data_cov;
+ Matrix5d delta_cov;
+ MatrixXd jacobian_bias;
+ Vector3d b0_preint;
+
+ void SetUp() override
+ {
+ // Input odometry data and covariance
+ data_cov = 0.1 * Matrix6d::Identity();
+ delta_cov = 0.1 * Matrix5d::Identity();
+
+ // Jacobian of the bias
+ jacobian_bias = ((MatrixXd(5, 3) << 1, 0, 0, //
+ 0, 1, 0, //
+ 0, 0, 1, //
+ 1, 0, 0, //
+ 0, 1, 0).finished());
+ // preintegration bias
+ b0_preint = Vector3d::Zero();
+
+ // Problem and solver
+ problem = Problem::create("POV", 2);
+ solver = std::make_shared<SolverCeres>(problem);
+
+ // sensor
+ sensor = SensorBase::emplace<SensorImu2d>(problem->getHardware(), Vector3d::Zero(), ParamsSensorImu2d());
+ sensor->setStateBlockDynamic('I', false);
+
+ // Two frames
+ frm0 = problem->emplaceFrame(TimeStamp(0), Vector5d::Zero());
+ frm1 = problem->emplaceFrame(TimeStamp(1), Vector5d::Zero());
+
+ // capture from frm1 to frm0
+ cap0 = CaptureBase::emplace<CaptureImu>(frm0, 0, sensor, Vector6d::Zero(), data_cov, nullptr);
+ cap1 = CaptureBase::emplace<CaptureImu>(frm1, 1, sensor, Vector6d::Zero(), data_cov, cap0);
+ }
+};
+
+TEST_F(FactorImu2d_test, problem_check)
{
- for(int i = 0; i < 50; i++){
- // Random delta
- Vector5d delta = Vector5d::Random() * 10; //delta *= 0;
- delta(2) = pi2pi(delta(2));
-
- // Random initial pose
- Vector5d x0 = Vector5d::Random() * 10; //x0 *= 0;
- x0(2) = pi2pi(x0(2));
-
- // x1 groundtruth
- Vector5d x1;
- x1 = imu2d::compose(x0, delta, 1);
-
- // Set states
- frm0->setState(x0);
- frm1->setState(x1);
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
- FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false);
-
- // Fix frm0 and biases, perturb frm1
- frm0->fix();
- cap0->fix();
- frm1->unfix();
- cap1->fix();
- frm1->perturb(0.01);
-
- // solve
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
- //WOLF_INFO(frm1->getStateVector());
-
- // remove feature (and factor) for the next loop
- fea1->remove();
-}
+ ASSERT_TRUE(problem->check(0));
}
-TEST(FactorImu2d, bias_zero_solve_f0)
+TEST_F(FactorImu2d_test, bias_zero_solve_f1)
{
- for(int i = 0; i < 50; i++){
- // Random delta
- Vector5d delta = Vector5d::Random() * 10; //delta *= 0;
- delta(2) = pi2pi(delta(2));
-
- // Random initial pose
- Vector5d x0 = Vector5d::Random() * 10; //x0 *= 0;
- x0(2) = pi2pi(x0(2));
-
- // x1 groundtruth
- Vector5d x1;
- x1 = imu2d::compose(x0, delta, 1);
-
- // Set states
- frm0->setState(x0);
- frm1->setState(x1);
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
- FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false);
-
- // Fix frm0 and biases, perturb frm1
- frm0->unfix();
- cap0->fix();
- frm1->fix();
- cap1->fix();
- frm0->perturb(0.01);
-
- // solve
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6);
- //WOLF_INFO(frm1->getStateVector());
-
- // remove feature (and factor) for the next loop
- fea1->remove();
-}
+ for (int i = 0; i < 50; i++)
+ {
+ // Random delta
+ Vector5d delta = Vector5d::Random() * 10; // delta *= 0;
+ delta(2) = pi2pi(delta(2));
+
+ // Random initial pose
+ Vector5d x0 = Vector5d::Random() * 10; // x0 *= 0;
+ x0(2) = pi2pi(x0(2));
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::compose(x0, delta, 1);
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
+ FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false);
+
+ // Fix frm0 and biases, perturb frm1
+ sensor->fix();
+ frm0->fix();
+ frm1->unfix();
+ frm1->perturb(0.01);
+
+ // solve
+ 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);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
}
-TEST(FactorImu2d, bias_zero_solve_b1)
+TEST_F(FactorImu2d_test, bias_zero_solve_f0)
{
- for(int i = 0; i < 50; i++){
- // Random delta
- Vector5d delta = Vector5d::Random() * 10; //delta *= 0;
- delta(2) = pi2pi(delta(2));
-
- // Random initial pose
- Vector5d x0 = Vector5d::Random() * 10; //x0 *= 0;
- x0(2) = pi2pi(x0(2));
-
- // x1 groundtruth
- Vector5d x1;
- x1 = imu2d::compose(x0, delta, 1);
-
- //0 Initial bias
- Vector3d b0 = Vector3d::Zero();
-
- // Set states
- frm0->setState(x0);
- frm1->setState(x1);
- cap0->getStateBlock('I')->setState(b0);
- cap1->getStateBlock('I')->setState(b0);
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
- FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false);
-
- // Fix frm0, frm1 and cap0, unfix cap1 (bias), perturb cap1
- frm0->fix();
- cap0->fix();
- frm1->fix();
- cap1->unfix();
- cap1->perturb(0.01);
-
- // solve to estimate bias at cap1
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_MATRIX_APPROX(cap1->getStateVector(), b0, 1e-6);
- //WOLF_INFO(cap1->getStateVector());
-
- // remove feature (and factor) for the next loop
- fea1->remove();
-}
+ for (int i = 0; i < 50; i++)
+ {
+ // Random delta
+ Vector5d delta = Vector5d::Random() * 10; // delta *= 0;
+ delta(2) = pi2pi(delta(2));
+
+ // Random initial pose
+ Vector5d x0 = Vector5d::Random() * 10; // x0 *= 0;
+ x0(2) = pi2pi(x0(2));
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::compose(x0, delta, 1);
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
+ FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false);
+
+ // Fix frm0 and biases, perturb frm1
+ sensor->fix();
+ frm0->unfix();
+ frm0->perturb(0.01);
+ frm1->fix();
+
+ // solve
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(frm0->getStateVector("PO"), x0.head(3), 1e-6);
+ ASSERT_MATRIX_APPROX(frm0->getStateVector("V"), x0.tail(2), 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
}
-TEST(FactorImu2d, solve_b0)
+TEST_F(FactorImu2d_test, bias_zero_solve)
{
- for(int i = 0; i < 50; i++){
- // Random delta
- Vector5d delta_biased = Vector5d::Random() * 10;
- delta_biased(2) = pi2pi(delta_biased(2));
-
- // Random initial pose
- Vector5d x0 = Vector5d::Random() * 10;
- x0(2) = pi2pi(x0(2));
-
- //Random Initial bias
- Vector3d b0 = Vector3d::Random();
-
- //Corrected delta
- Vector5d delta_step = jacobian_bias*(b0-b0_preint);
- Vector5d delta = imu2d::plus(delta_biased, delta_step);
-
- // x1 groundtruth
- Vector5d x1;
- x1 = imu2d::compose(x0, delta, 1);
-
- // Set states
- frm0->setState(x0);
- frm1->setState(x1);
- cap0->getStateBlock('I')->setState(b0);
- cap1->getStateBlock('I')->setState(b0);
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
- FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false);
-
- // Fix frm1, perturb frm0
- frm0->fix();
- cap0->unfix();
- frm1->fix();
- cap1->fix();
- cap0->perturb(0.1);
-
- // solve
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_POSE2d_APPROX(cap0->getStateVector(), b0, 1e-6);
- //WOLF_INFO(cap0->getStateVector());
-
- // remove feature (and factor) for the next loop
- fea1->remove();
-}
-}
+ for (int i = 0; i < 50; i++)
+ {
+ Vector5d delta = Vector5d::Random() * 10; // delta *= 0;
+ delta(2) = pi2pi(delta(2));
-TEST(FactorImu2d, solve_b1)
-{
- for(int i = 0; i < 50; i++){
- // Random delta
- Vector5d delta = Vector5d::Random() * 10; //delta *= 0;
- delta(2) = pi2pi(delta(2));
-
- // Random initial pose
- Vector5d x0 = Vector5d::Random() * 10; //x0 *= 0;
- x0(2) = pi2pi(x0(2));
-
- // x1 groundtruth
- Vector5d x1;
- x1 = imu2d::compose(x0, delta, 1);
-
- //0 Initial bias
- Vector3d b0 = Vector3d::Random();
-
- // Set states
- frm0->setState(x0);
- frm1->setState(x1);
- cap0->getStateBlock('I')->setState(b0);
- cap1->getStateBlock('I')->setState(b0);
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
- FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false);
-
- // Fix frm0 and frm1, unfix bias, perturb cap1
- frm0->fix();
- cap0->fix();
- frm1->fix();
- cap1->unfix();
- cap1->perturb(0.01);
-
- // solve to estimate bias at cap1
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_MATRIX_APPROX(cap1->getStateVector(), b0, 1e-6);
- //WOLF_INFO(cap1->getStateVector());
-
- // remove feature (and factor) for the next loop
- fea1->remove();
-}
+ // Random initial pose
+ Vector5d x0 = Vector5d::Random() * 10; // x0 *= 0;
+ x0(2) = pi2pi(x0(2));
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::compose(x0, delta, 1);
+
+ // 0 Initial bias
+ Vector3d b0 = Vector3d::Zero();
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+ sensor->getStateBlock('I')->setState(b0);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
+ FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false);
+
+ // Fix frm0, frm1 and cap0, unfix cap1 (bias), perturb cap1
+ sensor->getStateBlock('I')->unfix();
+ sensor->getStateBlock('I')->perturb(0.01);
+ frm0->fix();
+ frm1->fix();
+
+ // solve to estimate bias at cap1
+ auto report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_MATRIX_APPROX(sensor->getStateBlock('I')->getState(), b0, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
}
-TEST(FactorImu2d, solve_f0)
+TEST_F(FactorImu2d_test, solve_b0)
{
- for(int i = 0; i < 50; i++){
- // Random delta
- Vector5d delta_biased = Vector5d::Random() * 10;
- delta_biased(2) = pi2pi(delta_biased(2));
-
- // Random initial pose
- Vector5d x0 = Vector5d::Random() * 10;
- x0(2) = pi2pi(x0(2));
-
- //Random Initial bias
- Vector3d b0 = Vector3d::Random();
-
- //Corrected delta
- Vector5d delta_step = jacobian_bias*(b0-b0_preint);
- Vector5d delta = imu2d::plus(delta_biased, delta_step);
-
- // x1 groundtruth
- Vector5d x1;
- x1 = imu2d::compose(x0, delta, 1);
-
- // Set states
- frm0->setState(x0);
- frm1->setState(x1);
- cap0->getStateBlock('I')->setState(b0);
- cap1->getStateBlock('I')->setState(b0);
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
- FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false);
-
- // Fix frm1, perturb frm0
- frm0->unfix();
- cap0->fix();
- frm1->fix();
- cap1->fix();
- frm0->perturb(0.1);
-
- // solve
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6);
- //WOLF_INFO(frm0->getStateVector());
-
- // remove feature (and factor) for the next loop
- fea1->remove();
-}
+ for (int i = 0; i < 50; i++)
+ {
+ // Random delta
+ Vector5d delta_biased = Vector5d::Random() * 10;
+ delta_biased(2) = pi2pi(delta_biased(2));
+
+ // Random initial pose
+ Vector5d x0 = Vector5d::Random() * 10;
+ x0(2) = pi2pi(x0(2));
+
+ // Random Initial bias
+ Vector3d b0 = Vector3d::Random();
+
+ // Corrected delta
+ Vector5d delta_step = jacobian_bias * (b0 - b0_preint);
+ Vector5d delta = imu2d::plus(delta_biased, delta_step);
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::compose(x0, delta, 1);
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+ sensor->getStateBlock('I')->setState(b0);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
+ FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false);
+
+ // Fix frm1, perturb frm0
+ sensor->getStateBlock('I')->unfix();
+ sensor->getStateBlock('I')->perturb(0.1);
+ frm0->fix();
+ frm1->fix();
+
+ // solve
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_MATRIX_APPROX(sensor->getStateBlock('I')->getState(), b0, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
}
-TEST(FactorImu2d, solve_f1)
+TEST_F(FactorImu2d_test, solve_f0)
{
- for(int i = 0; i < 50; i++){
- // Random delta
- Vector5d delta_biased = Vector5d::Random() * 10;
- delta_biased(2) = pi2pi(delta_biased(2));
-
- // Random initial pose
- Vector5d x0 = Vector5d::Random() * 10;
- x0(2) = pi2pi(x0(2));
-
- //Random Initial bias
- Vector3d b0 = Vector3d::Random();
-
- //Corrected delta
- Vector5d delta_step = jacobian_bias*(b0-b0_preint);
- Vector5d delta = imu2d::plus(delta_biased, delta_step);
-
- // x1 groundtruth
- Vector5d x1;
- x1 = imu2d::compose(x0, delta, 1);
-
- // Set states
- frm0->setState(x0);
- frm1->setState(x1);
- cap0->getStateBlock('I')->setState(b0);
- cap1->getStateBlock('I')->setState(b0);
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
- FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false);
-
- // Fix frm1, perturb frm0
- frm0->fix();
- cap0->fix();
- frm1->unfix();
- cap1->fix();
- frm1->perturb(0.1);
-
- // solve
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
- //WOLF_INFO(frm0->getStateVector());
-
- // remove feature (and factor) for the next loop
- fea1->remove();
-}
+ for (int i = 0; i < 50; i++)
+ {
+ // Random delta
+ Vector5d delta_biased = Vector5d::Random() * 10;
+ delta_biased(2) = pi2pi(delta_biased(2));
+
+ // Random initial pose
+ Vector5d x0 = Vector5d::Random() * 10;
+ x0(2) = pi2pi(x0(2));
+
+ // Random Initial bias
+ Vector3d b0 = Vector3d::Random();
+
+ // Corrected delta
+ Vector5d delta_step = jacobian_bias * (b0 - b0_preint);
+ Vector5d delta = imu2d::plus(delta_biased, delta_step);
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::compose(x0, delta, 1);
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+ sensor->getStateBlock('I')->setState(b0);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
+ FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false);
+
+ // Fix frm1, perturb frm0
+ sensor->fix();
+ frm0->unfix();
+ frm0->perturb(0.1);
+ frm1->fix();
+
+ // solve
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(frm0->getStateVector("PO"), x0.head(3), 1e-6);
+ ASSERT_MATRIX_APPROX(frm0->getStateVector("V"), x0.tail(2), 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
}
-TEST(FactorImu2d, solve_f1_b1)
+TEST_F(FactorImu2d_test, solve_f1)
{
- for(int i = 0; i < 50; i++){
- // Random delta
- Vector5d delta_biased = Vector5d::Random() * 10;
- delta_biased(2) = pi2pi(delta_biased(2));
-
- // Random initial pose
- Vector5d x0 = Vector5d::Random() * 10;
- x0(2) = pi2pi(x0(2));
-
- //Random Initial bias
- Vector3d b0 = Vector3d::Random();
-
- //Corrected delta
- Vector5d delta_step = jacobian_bias*(b0-b0_preint);
- Vector5d delta = imu2d::plus(delta_biased, delta_step);
-
- // x1 groundtruth
- Vector5d x1;
- x1 = imu2d::compose(x0, delta, 1);
-
- // Set states
- frm0->setState(x0);
- frm1->setState(x1);
- cap0->getStateBlock('I')->setState(b0);
- cap1->getStateBlock('I')->setState(b0);
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
- FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false);
-
- // Fix frm1, perturb frm0
- frm0->fix();
- cap0->fix();
- frm1->unfix();
- cap1->unfix();
- frm1->perturb(0.1);
- cap1->perturb(0.1);
-
- // solve
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
- ASSERT_MATRIX_APPROX(cap1->getStateVector(), b0, 1e-6);
- //WOLF_INFO(frm0->getStateVector());
-
- // remove feature (and factor) for the next loop
- fea1->remove();
- }
+ for (int i = 0; i < 50; i++)
+ {
+ // Random delta
+ Vector5d delta_biased = Vector5d::Random() * 10;
+ delta_biased(2) = pi2pi(delta_biased(2));
+
+ // Random initial pose
+ Vector5d x0 = Vector5d::Random() * 10;
+ x0(2) = pi2pi(x0(2));
+
+ // Random Initial bias
+ Vector3d b0 = Vector3d::Random();
+
+ // Corrected delta
+ Vector5d delta_step = jacobian_bias * (b0 - b0_preint);
+ Vector5d delta = imu2d::plus(delta_biased, delta_step);
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::compose(x0, delta, 1);
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+ sensor->getStateBlock('I')->setState(b0);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
+ FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false);
+
+ // Fix frm1, perturb frm0
+ sensor->fix();
+ frm0->fix();
+ frm1->unfix();
+ frm1->perturb(0.1);
+
+ // solve
+ 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);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
}
+
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 931fcd3db298b8aa27e4f0a9edf189fbc98f9bb5..97eb0687a1b946a74876a285b4bb63425c582068 100644
--- a/test/gtest_factor_imu2d_with_gravity.cpp
+++ b/test/gtest_factor_imu2d_with_gravity.cpp
@@ -29,572 +29,477 @@
using namespace Eigen;
using namespace wolf;
-// Input odometry data and covariance
-Matrix6d data_cov = 0.1*Matrix6d::Identity();
-Matrix5d delta_cov = 0.1*Matrix5d::Identity();
-
-// Jacobian of the bias
-MatrixXd jacobian_bias((MatrixXd(5,3) << 1, 0, 0,
- 0, 1, 0,
- 0, 0, 1,
- 1, 0, 0,
- 0, 1, 0 ).finished());
-//preintegration bias
-Vector3d b0_preint = Vector3d::Zero();
-
-// Problem and solver
-ProblemPtr problem_ptr = Problem::create("POV", 2);
-SolverCeres solver(problem_ptr);
-
-// Two frames
-FrameBasePtr frm0 = problem_ptr->emplaceFrame(TimeStamp(0), Vector5d::Zero());
-FrameBasePtr frm1 = problem_ptr->emplaceFrame(TimeStamp(1), Vector5d::Zero());
-
-// Imu2d sensor
-ParamsSensorImu2dPtr sensorparams = std::make_shared<ParamsSensorImu2d>(false);
-SensorBasePtr sensor = SensorBase::emplace<SensorImu2d>(problem_ptr->getHardware(), Vector3d::Zero(), *sensorparams );
-
-//capture from frm1 to frm0
-auto cap0 = CaptureBase::emplace<CaptureImu>(frm0, 0, sensor, Vector6d::Zero(), data_cov, Vector3d::Zero(), nullptr);
-auto cap1 = CaptureBase::emplace<CaptureImu>(frm1, 1, sensor, Vector6d::Zero(), data_cov, Vector3d::Zero(), cap0);
-
-TEST(FactorImu2dWithGravity, check_tree)
+class FactorImu2dWithGravity_test : public testing::Test
{
- ASSERT_TRUE(problem_ptr->check(0));
-}
-
-TEST(FactorImu2dWithGravity, bias_zero_solve_f1)
+ public:
+ ProblemPtr problem;
+ SolverCeresPtr solver;
+ SensorImu2dPtr sensor;
+ FrameBasePtr frm0, frm1;
+ CaptureImuPtr cap0, cap1;
+ Matrix6d data_cov;
+ Matrix5d delta_cov;
+ MatrixXd jacobian_bias;
+ Vector3d b0_preint;
+
+ void SetUp() override
+ {
+ // Input odometry data and covariance
+ data_cov = 0.1 * Matrix6d::Identity();
+ delta_cov = 0.1 * Matrix5d::Identity();
+
+ // Jacobian of the bias
+ jacobian_bias = ((MatrixXd(5, 3) << 1, 0, 0, //
+ 0, 1, 0, //
+ 0, 0, 1, //
+ 1, 0, 0, //
+ 0, 1, 0)
+ .finished());
+ // preintegration bias
+ b0_preint = Vector3d::Zero();
+
+ // Problem and solver
+ problem = Problem::create("POV", 2);
+ solver = std::make_shared<SolverCeres>(problem);
+
+ // sensor
+ ParamsSensorImu2dPtr sensorparams = std::make_shared<ParamsSensorImu2d>(false);
+ sensor = SensorBase::emplace<SensorImu2d>(problem->getHardware(), Vector3d::Zero(), sensorparams);
+ sensor->setStateBlockDynamic('I', false);
+
+ // Two frames
+ frm0 = problem->emplaceFrame(TimeStamp(0), Vector5d::Zero());
+ frm1 = problem->emplaceFrame(TimeStamp(1), Vector5d::Zero());
+
+ // capture from frm1 to frm0
+ cap0 = CaptureBase::emplace<CaptureImu>(frm0, 0, sensor, Vector6d::Zero(), data_cov, nullptr);
+ cap1 = CaptureBase::emplace<CaptureImu>(frm1, 1, sensor, Vector6d::Zero(), data_cov, cap0);
+
+ problem->print(4,1,1,1);
+ }
+};
+
+TEST_F(FactorImu2dWithGravity_test, check_tree)
{
- for(int i = 0; i < 50; i++){
- //WOLF_INFO("gravity is orthogonal: ", sensorparams->orthogonal_gravity);
- // Random delta
- Vector5d delta = Vector5d::Random() * 10; //delta *= 0;
- delta(2) = pi2pi(delta(2));
-
- // Random initial pose
- Vector5d x0 = Vector5d::Random() * 10; //x0 *= 0;
- x0(2) = pi2pi(x0(2));
-
- //Random gravity
- Vector2d g = Vector2d::Random()*5;
- //Vector2d g = Vector2d::Zero();
-
- // x1 groundtruth
- Vector5d x1;
- x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
-
- // Set states
- frm0->setState(x0);
- frm1->setState(x1);
- sensor->getStateBlock('G')->setState(g);
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
- FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
-
- // Fix frm0 and biases, perturb frm1
- frm0->fix();
- cap0->fix();
- frm1->unfix();
- cap1->fix();
- sensor->getStateBlock('G')->fix();
- frm1->perturb(0.01);
-
- // solve
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
- //std::cout << report << std::endl;
-
- ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
- //WOLF_INFO(frm1->getStateVector());
-
- // remove feature (and factor) for the next loop
- fea1->remove();
-}
+ ASSERT_TRUE(problem->check(0));
}
-TEST(FactorImu2dWithGravity, bias_zero_solve_f0)
+TEST_F(FactorImu2dWithGravity_test, bias_zero_solve_f1)
{
- for(int i = 0; i < 50; i++){
- // Random delta
- Vector5d delta = Vector5d::Random() * 10; //delta *= 0;
- delta(2) = pi2pi(delta(2));
-
- // Random initial pose
- Vector5d x0 = Vector5d::Random() * 10; //x0 *= 0;
- x0(2) = pi2pi(x0(2));
-
- //Random gravity
- Vector2d g = Vector2d::Random()*5;
- //Vector2d g = Vector2d::Zero();
-
- // x1 groundtruth
- Vector5d x1;
- x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
-
- // Set states
- frm0->setState(x0);
- frm1->setState(x1);
- sensor->getStateBlock('G')->setState(g);
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
- FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
-
- // Fix frm1 and biases, perturb frm0
- frm0->unfix();
- cap0->fix();
- frm1->fix();
- cap1->fix();
- sensor->getStateBlock('G')->fix();
- frm0->perturb(0.01);
-
- // solve
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6);
- //WOLF_INFO(frm1->getStateVector());
-
- // remove feature (and factor) for the next loop
- fea1->remove();
-}
+ for (int i = 0; i < 50; i++)
+ {
+ // WOLF_INFO("gravity is orthogonal: ", sensorparams->orthogonal_gravity);
+ // Random delta
+ Vector5d delta = Vector5d::Random() * 10; // delta *= 0;
+ delta(2) = pi2pi(delta(2));
+
+ // Random initial pose
+ Vector5d x0 = Vector5d::Random() * 10; // x0 *= 0;
+ x0(2) = pi2pi(x0(2));
+
+ // Random gravity
+ Vector2d g = Vector2d::Random() * 5;
+ // Vector2d g = Vector2d::Zero();
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+ sensor->getStateBlock('G')->setState(g);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
+ FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
+
+ // Fix frm0 and biases, perturb frm1
+ sensor->fix();
+ frm0->fix();
+ frm1->unfix();
+ frm1->perturb(0.01);
+ sensor->getStateBlock('G')->fix();
+
+ // solve
+ std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
+ // std::cout << report << std::endl;
+
+ 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());
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
}
-TEST(FactorImu2dWithGravity, bias_zero_solve_b1)
+TEST_F(FactorImu2dWithGravity_test, bias_zero_solve_f0)
{
- for(int i = 0; i < 50; i++){
- // Random delta
- Vector5d delta = Vector5d::Random() * 10; //delta *= 0;
- delta(2) = pi2pi(delta(2));
-
- // Random initial pose
- Vector5d x0 = Vector5d::Random() * 10; //x0 *= 0;
- x0(2) = pi2pi(x0(2));
-
- //Random gravity
- Vector2d g = Vector2d::Random()*5;
- //Vector2d g = Vector2d::Zero();
-
- // x1 groundtruth
- Vector5d x1;
- x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
-
- //0 Initial bias
- Vector3d b0 = Vector3d::Zero();
-
- // Set states
- frm0->setState(x0);
- frm1->setState(x1);
- cap0->getStateBlock('I')->setState(b0);
- cap1->getStateBlock('I')->setState(b0);
- sensor->getStateBlock('G')->setState(g);
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
- FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
-
- // Fix frm0, frm1 and cap0, unfix cap1 (bias), perturb cap1
- frm0->fix();
- cap0->fix();
- frm1->fix();
- sensor->getStateBlock('G')->fix();
- cap1->unfix();
- cap1->perturb(0.01);
-
- // solve to estimate bias at cap1
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_MATRIX_APPROX(cap1->getStateVector(), b0, 1e-6);
- //WOLF_INFO(cap1->getStateVector());
-
- // remove feature (and factor) for the next loop
- fea1->remove();
- }
+ for (int i = 0; i < 50; i++)
+ {
+ // Random delta
+ Vector5d delta = Vector5d::Random() * 10; // delta *= 0;
+ delta(2) = pi2pi(delta(2));
+
+ // Random initial pose
+ Vector5d x0 = Vector5d::Random() * 10; // x0 *= 0;
+ x0(2) = pi2pi(x0(2));
+
+ // Random gravity
+ Vector2d g = Vector2d::Random() * 5;
+ // Vector2d g = Vector2d::Zero();
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+ sensor->getStateBlock('G')->setState(g);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
+ FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
+
+ // Fix frm1 and biases, perturb frm0
+ sensor->fix();
+ frm0->unfix();
+ frm0->perturb(0.01);
+ frm1->fix();
+ sensor->getStateBlock('G')->fix();
+
+ // solve
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(frm0->getStateVector("PO"), x0.head(3), 1e-6);
+ ASSERT_MATRIX_APPROX(frm0->getStateVector("V"), x0.tail(2), 1e-6);
+ // WOLF_INFO(frm1->getStateVector());
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
}
-TEST(FactorImu2dWithGravity, solve_b0)
+TEST_F(FactorImu2dWithGravity_test, bias_zero_solve)
{
- for(int i = 0; i < 50; i++){
- // Random delta
- Vector5d delta_biased = Vector5d::Random() * 10;
- delta_biased(2) = pi2pi(delta_biased(2));
-
- // Random initial pose
- Vector5d x0 = Vector5d::Random() * 10;
- x0(2) = pi2pi(x0(2));
-
- //Random gravity
- Vector2d g = Vector2d::Random()*5;
- //Vector2d g = Vector2d::Zero();
-
- //Random Initial bias
- Vector3d b0 = Vector3d::Random();
-
- //Corrected delta
- Vector5d delta_step = jacobian_bias*(b0-b0_preint);
- Vector5d delta = imu2d::plus(delta_biased, delta_step);
-
- // x1 groundtruth
- Vector5d x1;
- x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
-
- // Set states
- frm0->setState(x0);
- frm1->setState(x1);
- cap0->getStateBlock('I')->setState(b0);
- cap1->getStateBlock('I')->setState(b0);
- sensor->getStateBlock('G')->setState(g);
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
- FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
-
- // Fix frm1, perturb frm0
- frm0->fix();
- cap0->unfix();
- frm1->fix();
- cap1->fix();
- sensor->getStateBlock('G')->fix();
- cap0->perturb(0.1);
-
- // solve
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_MATRIX_APPROX(cap0->getStateVector(), b0, 1e-6);
- //WOLF_INFO(cap0->getStateVector());
-
- // remove feature (and factor) for the next loop
- fea1->remove();
- }
+ for (int i = 0; i < 50; i++)
+ {
+ // Random delta
+ Vector5d delta = Vector5d::Random() * 10; // delta *= 0;
+ delta(2) = pi2pi(delta(2));
+
+ // Random initial pose
+ Vector5d x0 = Vector5d::Random() * 10; // x0 *= 0;
+ x0(2) = pi2pi(x0(2));
+
+ // Random gravity
+ Vector2d g = Vector2d::Random() * 5;
+ // Vector2d g = Vector2d::Zero();
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
+
+ // 0 Initial bias
+ Vector3d b0 = Vector3d::Zero();
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+ sensor->getStateBlock('I')->setState(b0);
+ sensor->getStateBlock('G')->setState(g);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
+ FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
+
+ // Fix frm0, frm1 and cap0, unfix cap1 (bias), perturb cap1
+ sensor->getStateBlock('I')->unfix();
+ sensor->getStateBlock('I')->perturb(0.01);
+ frm0->fix();
+ frm1->fix();
+ sensor->getStateBlock('G')->fix();
+
+ // solve to estimate bias at cap1
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_MATRIX_APPROX(sensor->getStateBlock('I')->getState(), b0, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
}
-TEST(FactorImu2dWithGravity, solve_b1)
+TEST_F(FactorImu2dWithGravity_test, solve_b0)
{
- for(int i = 0; i < 50; i++){
- // Random delta
- Vector5d delta = Vector5d::Random() * 10; //delta *= 0;
- delta(2) = pi2pi(delta(2));
-
- // Random initial pose
- Vector5d x0 = Vector5d::Random() * 10; //x0 *= 0;
- x0(2) = pi2pi(x0(2));
-
- //Random gravity
- Vector2d g = Vector2d::Random()*5;
- //Vector2d g = Vector2d::Zero();
-
- // x1 groundtruth
- Vector5d x1;
- x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
-
- //0 Initial bias
- Vector3d b0 = Vector3d::Random();
-
- // Set states
- frm0->setState(x0);
- frm1->setState(x1);
- cap0->getStateBlock('I')->setState(b0);
- cap1->getStateBlock('I')->setState(b0);
- sensor->getStateBlock('G')->setState(g);
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
- FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
-
- // Fix frm0 and frm1, unfix bias, perturb cap1
- frm0->fix();
- cap0->fix();
- frm1->fix();
- sensor->getStateBlock('G')->fix();
- cap1->unfix();
- cap1->perturb(0.01);
-
- // solve to estimate bias at cap1
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_MATRIX_APPROX(cap1->getStateVector(), b0, 1e-6);
- //WOLF_INFO(cap1->getStateVector());
-
- // remove feature (and factor) for the next loop
- fea1->remove();
-}
+ for (int i = 0; i < 50; i++)
+ {
+ // Random delta
+ Vector5d delta_biased = Vector5d::Random() * 10;
+ delta_biased(2) = pi2pi(delta_biased(2));
+
+ // Random initial pose
+ Vector5d x0 = Vector5d::Random() * 10;
+ x0(2) = pi2pi(x0(2));
+
+ // Random gravity
+ Vector2d g = Vector2d::Random() * 5;
+ // Vector2d g = Vector2d::Zero();
+
+ // Random Initial bias
+ Vector3d b0 = Vector3d::Random();
+
+ // Corrected delta
+ Vector5d delta_step = jacobian_bias * (b0 - b0_preint);
+ Vector5d delta = imu2d::plus(delta_biased, delta_step);
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+ sensor->getStateBlock('I')->setState(b0);
+ sensor->getStateBlock('G')->setState(g);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
+ FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
+
+ // Fix frm1, perturb frm0
+ sensor->getStateBlock('I')->unfix();
+ sensor->getStateBlock('I')->perturb(0.1);
+ frm0->fix();
+ frm1->fix();
+ sensor->getStateBlock('G')->fix();
+
+ // solve
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_MATRIX_APPROX(sensor->getStateBlock('I')->getState(), b0, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
}
-TEST(FactorImu2dWithGravity, solve_f0)
+TEST_F(FactorImu2dWithGravity_test, solve_f0)
{
- for(int i = 0; i < 50; i++){
- // Random delta
- Vector5d delta_biased = Vector5d::Random() * 10;
- delta_biased(2) = pi2pi(delta_biased(2));
-
- // Random initial pose
- Vector5d x0 = Vector5d::Random() * 10;
- x0(2) = pi2pi(x0(2));
-
- //Random gravity
- Vector2d g = Vector2d::Random()*5;
- //Vector2d g = Vector2d::Zero();
-
- //Random Initial bias
- Vector3d b0 = Vector3d::Random();
-
- //Corrected delta
- Vector5d delta_step = jacobian_bias*(b0-b0_preint);
- Vector5d delta = imu2d::plus(delta_biased, delta_step);
-
- // x1 groundtruth
- Vector5d x1;
- x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
-
- // Set states
- frm0->setState(x0);
- frm1->setState(x1);
- cap0->getStateBlock('I')->setState(b0);
- cap1->getStateBlock('I')->setState(b0);
- sensor->getStateBlock('G')->setState(g);
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
- FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
-
- // Fix frm1, perturb frm0
- frm0->unfix();
- cap0->fix();
- frm1->fix();
- cap1->fix();
- sensor->getStateBlock('G')->fix();
- frm0->perturb(0.1);
-
- // solve
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6);
- //WOLF_INFO(frm0->getStateVector());
-
- // remove feature (and factor) for the next loop
- fea1->remove();
-}
+ for (int i = 0; i < 50; i++)
+ {
+ // Random delta
+ Vector5d delta_biased = Vector5d::Random() * 10;
+ delta_biased(2) = pi2pi(delta_biased(2));
+
+ // Random initial pose
+ Vector5d x0 = Vector5d::Random() * 10;
+ x0(2) = pi2pi(x0(2));
+
+ // Random gravity
+ Vector2d g = Vector2d::Random() * 5;
+ // Vector2d g = Vector2d::Zero();
+
+ // Random Initial bias
+ Vector3d b0 = Vector3d::Random();
+
+ // Corrected delta
+ Vector5d delta_step = jacobian_bias * (b0 - b0_preint);
+ Vector5d delta = imu2d::plus(delta_biased, delta_step);
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+ sensor->getStateBlock('I')->setState(b0);
+ sensor->getStateBlock('G')->setState(g);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
+ FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
+
+ // Fix frm1, perturb frm0
+ sensor->fix();
+ frm0->unfix();
+ frm0->perturb(0.1);
+ frm1->fix();
+ sensor->getStateBlock('G')->fix();
+
+ // solve
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(frm0->getStateVector("PO"), x0.head(3), 1e-6);
+ ASSERT_MATRIX_APPROX(frm0->getStateVector("V"), x0.tail(2), 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
}
-TEST(FactorImu2dWithGravity, solve_f1)
+TEST_F(FactorImu2dWithGravity_test, solve_f1)
{
- for(int i = 0; i < 50; i++){
- // Random delta
- Vector5d delta_biased = Vector5d::Random() * 10;
- delta_biased(2) = pi2pi(delta_biased(2));
-
- // Random initial pose
- Vector5d x0 = Vector5d::Random() * 10;
- x0(2) = pi2pi(x0(2));
-
- //Random gravity
- Vector2d g = Vector2d::Random()*5;
- //Vector2d g = Vector2d::Zero();
-
- //Random Initial bias
- Vector3d b0 = Vector3d::Random();
-
- //Corrected delta
- Vector5d delta_step = jacobian_bias*(b0-b0_preint);
- Vector5d delta = imu2d::plus(delta_biased, delta_step);
-
- // x1 groundtruth
- Vector5d x1;
- x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
-
- // Set states
- frm0->setState(x0);
- frm1->setState(x1);
- cap0->getStateBlock('I')->setState(b0);
- cap1->getStateBlock('I')->setState(b0);
- sensor->getStateBlock('G')->setState(g);
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
- FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
-
- // Fix frm1, perturb frm0
- frm0->fix();
- cap0->fix();
- frm1->unfix();
- cap1->fix();
- sensor->getStateBlock('G')->fix();
- frm1->perturb(0.1);
-
- // solve
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
- //WOLF_INFO(frm0->getStateVector());
-
- // remove feature (and factor) for the next loop
- fea1->remove();
- }
+ for (int i = 0; i < 50; i++)
+ {
+ // Random delta
+ Vector5d delta_biased = Vector5d::Random() * 10;
+ delta_biased(2) = pi2pi(delta_biased(2));
+
+ // Random initial pose
+ Vector5d x0 = Vector5d::Random() * 10;
+ x0(2) = pi2pi(x0(2));
+
+ // Random gravity
+ Vector2d g = Vector2d::Random() * 5;
+ // Vector2d g = Vector2d::Zero();
+
+ // Random Initial bias
+ Vector3d b0 = Vector3d::Random();
+
+ // Corrected delta
+ Vector5d delta_step = jacobian_bias * (b0 - b0_preint);
+ Vector5d delta = imu2d::plus(delta_biased, delta_step);
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+ sensor->getStateBlock('I')->setState(b0);
+ sensor->getStateBlock('G')->setState(g);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
+ FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
+
+ // Fix frm1, perturb frm0
+ sensor->fix();
+ frm0->fix();
+ frm1->unfix();
+ frm1->perturb(0.1);
+ sensor->getStateBlock('G')->fix();
+
+ // solve
+ 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);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
}
-TEST(FactorImu2dWithGravity, solve_f1_b1)
-{
- for(int i = 0; i < 50; i++){
- // Random delta
- Vector5d delta_biased = Vector5d::Random() * 10;
- delta_biased(2) = pi2pi(delta_biased(2));
-
- // Random initial pose
- Vector5d x0 = Vector5d::Random() * 10;
- x0(2) = pi2pi(x0(2));
-
- //Random gravity
- Vector2d g = Vector2d::Random()*5;
- //Vector2d g = Vector2d::Zero();
-
- //Random Initial bias
- Vector3d b0 = Vector3d::Random();
-
- //Corrected delta
- Vector5d delta_step = jacobian_bias*(b0-b0_preint);
- Vector5d delta = imu2d::plus(delta_biased, delta_step);
-
- // x1 groundtruth
- Vector5d x1;
- x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
-
- // Set states
- frm0->setState(x0);
- frm1->setState(x1);
- cap0->getStateBlock('I')->setState(b0);
- cap1->getStateBlock('I')->setState(b0);
- sensor->getStateBlock('G')->setState(g);
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
- FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
-
- // Fix frm1, perturb frm0
- frm0->fix();
- cap0->fix();
- sensor->getStateBlock('G')->fix();
- frm1->unfix();
- cap1->unfix();
- frm1->perturb(0.1);
- cap1->perturb(0.1);
-
- // solve
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
- ASSERT_MATRIX_APPROX(cap1->getStateVector(), b0, 1e-6);
- //WOLF_INFO(frm0->getStateVector());
-
- // remove feature (and factor) for the next loop
- fea1->remove();
- }
-}
-TEST(FactorImu2dWithGravity, bias_zero_solve_G)
+TEST_F(FactorImu2dWithGravity_test, bias_zero_solve_G)
{
- for(int i = 0; i < 50; i++){
- //WOLF_INFO("gravity is orthogonal: ", sensorparams->orthogonal_gravity);
- // Random delta
- Vector5d delta = Vector5d::Random() * 10; //delta *= 0;
- delta(2) = pi2pi(delta(2));
-
- // Random initial pose
- Vector5d x0 = Vector5d::Random() * 10; //x0 *= 0;
- x0(2) = pi2pi(x0(2));
-
- //Random gravity
- //Vector2d g = Vector2d::Random()*5;
- Vector2d g = Vector2d::Zero();
-
- //Zero bias
- Vector3d b0 = Vector3d::Zero();
-
- // x1 groundtruth
- Vector5d x1;
- x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
-
- // Set states
- frm0->setState(x0);
- frm1->setState(x1);
- cap0->getStateBlock('I')->setState(b0);
- cap1->getStateBlock('I')->setState(b0);
- sensor->getStateBlock('G')->setState(g);
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
- FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
-
- // Fix frames and biases, perturb g
- frm0->fix();
- cap0->fix();
- frm1->fix();
- cap1->fix();
- sensor->getStateBlock('G')->unfix();
- sensor->getStateBlock('G')->perturb(1);
-
- // solve
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
-
- ASSERT_MATRIX_APPROX(sensor->getStateBlock('G')->getState(), g, 1e-6);
- //WOLF_INFO(frm1->getStateVector());
-
- // remove feature (and factor) for the next loop
- fea1->remove();
- }
+ for (int i = 0; i < 50; i++)
+ {
+ // WOLF_INFO("gravity is orthogonal: ", sensorparams->orthogonal_gravity);
+ // Random delta
+ Vector5d delta = Vector5d::Random() * 10; // delta *= 0;
+ delta(2) = pi2pi(delta(2));
+
+ // Random initial pose
+ Vector5d x0 = Vector5d::Random() * 10; // x0 *= 0;
+ x0(2) = pi2pi(x0(2));
+
+ // Random gravity
+ // Vector2d g = Vector2d::Random()*5;
+ Vector2d g = Vector2d::Zero();
+
+ // Zero bias
+ Vector3d b0 = Vector3d::Zero();
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+ sensor->getStateBlock('I')->setState(b0);
+ sensor->getStateBlock('G')->setState(g);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
+ FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
+
+ // Fix frames and biases, perturb g
+ sensor->fix();
+ sensor->getStateBlock('G')->unfix();
+ sensor->getStateBlock('G')->perturb(1);
+ frm0->fix();
+ frm1->fix();
+
+ // solve
+ std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
+
+ ASSERT_MATRIX_APPROX(sensor->getStateBlock('G')->getState(), g, 1e-6);
+ // WOLF_INFO(frm1->getStateVector());
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
}
-TEST(FactorImu2dWithGravity, solve_G)
+TEST_F(FactorImu2dWithGravity_test, solve_G)
{
- for(int i = 0; i < 50; i++){
- // Random delta
- Vector5d delta_biased = Vector5d::Random() * 10;
- delta_biased(2) = pi2pi(delta_biased(2));
-
- // Random initial pose
- Vector5d x0 = Vector5d::Random() * 10;
- x0(2) = pi2pi(x0(2));
-
- //Random gravity
- Vector2d g = Vector2d::Random()*5;
- //Vector2d g = Vector2d::Zero();
-
- //Random Initial bias
- Vector3d b0 = Vector3d::Random();
-
- //Corrected delta
- Vector5d delta_step = jacobian_bias*(b0-b0_preint);
- Vector5d delta = imu2d::plus(delta_biased, delta_step);
-
- // x1 groundtruth
- Vector5d x1;
- x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
-
- // Set states
- frm0->setState(x0);
- frm1->setState(x1);
- cap0->getStateBlock('I')->setState(b0);
- cap1->getStateBlock('I')->setState(b0);
- sensor->getStateBlock('G')->setState(g);
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
- FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
-
- // Fix frames and captures, perturb g
- frm0->fix();
- cap0->fix();
- frm1->fix();
- cap1->fix();
- sensor->getStateBlock('G')->unfix();
- sensor->getStateBlock('G')->perturb(1);
-
- // solve
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_MATRIX_APPROX(sensor->getStateBlock('G')->getState(), g, 1e-6);
- //WOLF_INFO(cap0->getStateVector());
-
- // remove feature (and factor) for the next loop
- fea1->remove();
- }
+ for (int i = 0; i < 50; i++)
+ {
+ // Random delta
+ Vector5d delta_biased = Vector5d::Random() * 10;
+ delta_biased(2) = pi2pi(delta_biased(2));
+
+ // Random initial pose
+ Vector5d x0 = Vector5d::Random() * 10;
+ x0(2) = pi2pi(x0(2));
+
+ // Random gravity
+ Vector2d g = Vector2d::Random() * 5;
+ // Vector2d g = Vector2d::Zero();
+
+ // Random Initial bias
+ Vector3d b0 = Vector3d::Random();
+
+ // Corrected delta
+ Vector5d delta_step = jacobian_bias * (b0 - b0_preint);
+ Vector5d delta = imu2d::plus(delta_biased, delta_step);
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+ sensor->getStateBlock('I')->setState(b0);
+ sensor->getStateBlock('G')->setState(g);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
+ FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
+
+ // Fix frames and captures, perturb g
+ sensor->fix();
+ sensor->getStateBlock('G')->unfix();
+ sensor->getStateBlock('G')->perturb(1);
+ frm0->fix();
+ frm1->fix();
+
+ // solve
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_MATRIX_APPROX(sensor->getStateBlock('G')->getState(), g, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
}
int main(int argc, char **argv)
diff --git a/test/gtest_imu.cpp b/test/gtest_imu.cpp
index 6e63ec0bba182b5b0e163ed3c7ff094896f4a296..97d2538baeb45004dfe47ee39d38d3e42bf0e37d 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_3d.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;
@@ -1539,10 +1538,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_mocap_fusion.cpp b/test/gtest_imu_mocap_fusion.cpp
index 34cf4948419a04b2b8f0648fe75b020495509b70..a2a4dda3334c68e69df0c8334ef8a1901549c7ce 100644
--- a/test/gtest_imu_mocap_fusion.cpp
+++ b/test/gtest_imu_mocap_fusion.cpp
@@ -117,8 +117,12 @@ class ImuMocapFusion_Test : public testing::Test
// pose sensor and proc (to get extrinsics in the prb)
auto intr_sp = std::make_shared<ParamsSensorPose>();
- intr_sp->std_p = 0.001;
- intr_sp->std_o = 0.001;
+ intr_sp->std_px = 0.001;
+ intr_sp->std_py = 0.001;
+ intr_sp->std_pz = 0.001;
+ intr_sp->std_ox = 0.001;
+ intr_sp->std_oy = 0.001;
+ intr_sp->std_oz = 0.001;
Vector7d extr; extr << 0,0,0, 0,0,0,1;
sensor_pose_ = problem_->installSensor("SensorPose", "pose", extr, intr_sp);
auto params_proc = std::make_shared<ParamsProcessorPose>();
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/imu_static_init.yaml b/test/yaml/imu_static_init.yaml
index 46393f30b8f24bf645bfb1272764e6c8915e5dc2..cc07835915541bd4243b33370f3cf45c496cda32 100644
--- a/test/yaml/imu_static_init.yaml
+++ b/test/yaml/imu_static_init.yaml
@@ -10,3 +10,8 @@ keyframe_vote:
max_buff_length: 1000000000 # motion deltas
dist_traveled: 100000000000 # meters
angle_turned: 10000000000 # radians (1 rad approx 57 deg, approx 60 deg)
+
+bootstrap:
+ enable: false
+ method: "G"
+ averaging_length: 10
\ No newline at end of file
diff --git a/test/yaml/sensor_imu.yaml b/test/yaml/sensor_imu.yaml
index 3c78a00d35c785fe73381d8f6ce99a705d27ce77..38a3df944cc7e8a7021e0bfe418fb5c79b324b2f 100644
--- a/test/yaml/sensor_imu.yaml
+++ b/test/yaml/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/test/yaml/sensor_imu_static_init.yaml b/test/yaml/sensor_imu_static_init.yaml
index 700b8d6762b91b38e96391f6032b2c7c4221eabc..e8df83cd8f81b954affb9ebb05e8cb65c8b0f0de 100644
--- a/test/yaml/sensor_imu_static_init.yaml
+++ b/test/yaml/sensor_imu_static_init.yaml
@@ -7,3 +7,5 @@ motion_variances:
wb_initial_stdev: 0.001 # rad/sec - initial bias
ab_rate_stdev: 0.001 # m/s2/sqrt(s)
wb_rate_stdev: 0.001 # rad/s/sqrt(s)
+
+dynamic_imu_bias: false
\ No newline at end of file