diff --git a/.gitignore b/.gitignore
index 4d9c52e82fe8dee50c99c9a1c8548575962ce658..09710a7b21fdda851139ab38f69dfe0832dab035 100644
--- a/.gitignore
+++ b/.gitignore
@@ -41,3 +41,4 @@ compile_commands.json
.vimspector.json
est*.csv
.clang-format
+test/yaml/imu_mocap_fusion/*.csv
diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index 370c97f013efc61b16bd8d27a2115f15ff35b4d7..4ad302cc17068ae8f9471d7d72c105f81a477e5e 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -1,8 +1,22 @@
include:
- - 'https://gitlab.iri.upc.edu/mobile_robotics/wolf_projects/wolf_lib/wolf/-/raw/$WOLF_CORE_BRANCH/.license_template_ci.yml'
+ - 'https://gitlab.iri.upc.edu/mobile_robotics/wolf_projects/wolf_lib/wolf/-/raw/$WOLF_CORE_BRANCH/.ci_templates/.license_headers.yml'
+ - 'https://gitlab.iri.upc.edu/mobile_robotics/wolf_projects/wolf_lib/wolf/-/raw/$WOLF_CORE_BRANCH/.ci_templates/.yaml_schema_cpp.yml'
+ - 'https://gitlab.iri.upc.edu/mobile_robotics/wolf_projects/wolf_lib/wolf/-/raw/$WOLF_CORE_BRANCH/.ci_templates/.clang_format.yml'
+ - 'https://gitlab.iri.upc.edu/mobile_robotics/wolf_projects/wolf_lib/wolf/-/raw/$WOLF_CORE_BRANCH/.ci_templates/.install_core.yml'
+
+workflow:
+ rules:
+ - if: '$CI_PIPELINE_SOURCE == "web"'
+ when: always
+ - if: '$CI_PIPELINE_SOURCE == "merge_request_event"'
+ - if: '$CI_COMMIT_BRANCH && $CI_OPEN_MERGE_REQUESTS'
+ when: never
+ - if: '$CI_COMMIT_BRANCH'
stages:
- license
+ - clang
+ - yaml_templates
- build_and_test
############ YAML ANCHORS ############
@@ -13,7 +27,7 @@ stages:
- echo $WOLF_IMU_BRANCH
.preliminaries_template: &preliminaries_definition
- ## OVERRIDE VARIABLES
+ ## OVERRIDE VARIABLES use it for working with a MR in core
- export WOLF_CORE_BRANCH="454-implementation-of-new-nodes-creation"
## PRINT VARIABLES
@@ -87,42 +101,42 @@ stages:
# - echo "No changes, nothing to commit!"
# - fi
-.install_yamlschemacpp_template: &install_yamlschemacpp_definition
- - cd ${CI_PROJECT_DIR}/ci_deps
- - if [ -d yaml-schema-cpp ]; then
- - echo "directory yaml-schema-cpp exists"
- - cd yaml-schema-cpp
- - git checkout main
- - git pull
- - else
- - git clone -b main ssh://git@gitlab.iri.upc.edu:2202/labrobotica/algorithms/yaml-schema-cpp.git
- - cd yaml-schema-cpp
- - fi
- - mkdir -pv build
- - cd build
- - cmake -DCMAKE_BUILD_TYPE=release -DBUILD_TESTS=OFF ..
- - make -j$(nproc)
- - make install
- - ldconfig
-
-.install_wolf_template: &install_wolf_definition
- - cd ${CI_PROJECT_DIR}/ci_deps
- - if [ -d wolf ]; then
- - echo "directory wolf exists"
- - cd wolf
- - git checkout devel
- - git pull
- - git checkout $WOLF_CORE_BRANCH
- - git pull
- - else
- - git clone -b $WOLF_CORE_BRANCH ssh://git@gitlab.iri.upc.edu:2202/mobile_robotics/wolf_projects/wolf_lib/wolf.git
- - cd wolf
- - fi
- - mkdir -pv build
- - cd build
- - cmake -DCMAKE_BUILD_TYPE=release -DBUILD_DEMOS=OFF -DBUILD_TESTS=OFF ..
- - make -j$(nproc)
- - make install
+# .install_yamlschemacpp_template: &install_yamlschemacpp_definition
+# - cd ${CI_PROJECT_DIR}/ci_deps
+# - if [ -d yaml-schema-cpp ]; then
+# - echo "directory yaml-schema-cpp exists"
+# - cd yaml-schema-cpp
+# - git checkout main
+# - git pull
+# - else
+# - git clone -b main ssh://git@gitlab.iri.upc.edu:2202/labrobotica/algorithms/yaml-schema-cpp.git
+# - cd yaml-schema-cpp
+# - fi
+# - mkdir -pv build
+# - cd build
+# - cmake -DCMAKE_BUILD_TYPE=release -DBUILD_TESTS=OFF ..
+# - make -j$(nproc)
+# - make install
+# - ldconfig
+
+# .install_wolf_template: &install_wolf_definition
+# - cd ${CI_PROJECT_DIR}/ci_deps
+# - if [ -d wolf ]; then
+# - echo "directory wolf exists"
+# - cd wolf
+# - git checkout devel
+# - git pull
+# - git checkout $WOLF_CORE_BRANCH
+# - git pull
+# - else
+# - git clone -b $WOLF_CORE_BRANCH ssh://git@gitlab.iri.upc.edu:2202/mobile_robotics/wolf_projects/wolf_lib/wolf.git
+# - cd wolf
+# - fi
+# - mkdir -pv build
+# - cd build
+# - cmake -DCMAKE_BUILD_TYPE=release -DBUILD_DEMOS=OFF -DBUILD_TESTS=OFF ..
+# - make -j$(nproc)
+# - make install
.build_and_test_template: &build_and_test_definition
- cd $CI_PROJECT_DIR
@@ -140,9 +154,31 @@ license_headers:
before_script:
- *preliminaries_definition
script:
- # - *license_header_definition
- !reference [.license_header_script]
+############ CLANG FORMAT ############
+clang_format:
+ stage: clang
+ image: labrobotica/wolf_deps:20.04
+ before_script:
+ - *preliminaries_definition
+ script:
+ - !reference [.clang_format_script]
+
+############ YAML TEMPLATES GENERATION ############
+yaml_templates_generation:
+ stage: yaml_templates
+ image: labrobotica/wolf_deps:20.04
+ cache:
+ - key: yamlschemacpp-focal
+ paths:
+ - ci_deps/yaml-schema-cpp/
+ before_script:
+ - *preliminaries_definition
+ - !reference [.install_yamlschemacpp_script]
+ script:
+ - !reference [.generate_yaml_templates_script]
+
############ UBUNTU 18.04 TESTS ############
build_and_test:bionic:
stage: build_and_test
@@ -154,13 +190,10 @@ build_and_test:bionic:
- key: yamlschemacpp-bionic
paths:
- ci_deps/yaml_schema_cpp/
- - key: yamlschemacpp-bionic
- paths:
- - ci_deps/yaml_schema_cpp/
before_script:
- *preliminaries_definition
- - *install_yamlschemacpp_definition
- - *install_wolf_definition
+ - !reference [.install_yamlschemacpp_script]
+ - !reference [.install_wolf_script]
script:
- *build_and_test_definition
@@ -175,12 +208,9 @@ build_and_test:focal:
- key: yamlschemacpp-focal
paths:
- ci_deps/yaml_schema_cpp/
- - key: yamlschemacpp-focal
- paths:
- - ci_deps/yaml_schema_cpp/
before_script:
- *preliminaries_definition
- - *install_yamlschemacpp_definition
- - *install_wolf_definition
+ - !reference [.install_yamlschemacpp_script]
+ - !reference [.install_wolf_script]
script:
- *build_and_test_definition
diff --git a/include/imu/common/imu.h b/include/imu/common/imu.h
index c39ffab348bb1322b89b101d0a4db15e822b3c8e..717552662ef1424c2b3cdb76b47a876290a6d8aa 100644
--- a/include/imu/common/imu.h
+++ b/include/imu/common/imu.h
@@ -22,6 +22,7 @@
// Enable project-specific definitions and macros
#include "imu/internal/config.h"
+#include "imu/utils/load_imu.h"
#include <core/common/wolf.h>
namespace wolf
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 6ac75d86079d86af8c31b9a578f60d2ac6ef7ba0..d55e5dec9f1c8169e57edc6283af16fe77555733 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -23,13 +23,13 @@ wolf_add_gtest(gtest_feature_imu_3d gtest_feature_imu_3d.cpp)
wolf_add_gtest(gtest_imu_2d_static_init gtest_imu_2d_static_init.cpp)
-# wolf_add_gtest(gtest_imu_2d_tools gtest_imu_2d_tools.cpp)
+wolf_add_gtest(gtest_imu_2d_tools gtest_imu_2d_tools.cpp)
-# wolf_add_gtest(gtest_imu_3d_mocap_fusion gtest_imu_3d_mocap_fusion.cpp)
+wolf_add_gtest(gtest_imu_3d_mocap_fusion gtest_imu_3d_mocap_fusion.cpp)
-# wolf_add_gtest(gtest_imu_3d_static_init gtest_imu_3d_static_init.cpp)
+wolf_add_gtest(gtest_imu_3d_static_init gtest_imu_3d_static_init.cpp)
-# wolf_add_gtest(gtest_imu_3d_tools gtest_imu_3d_tools.cpp)
+wolf_add_gtest(gtest_imu_3d_tools gtest_imu_3d_tools.cpp)
# wolf_add_gtest(gtest_imu_3d gtest_imu_3d.cpp)
@@ -43,6 +43,6 @@ wolf_add_gtest(gtest_imu_2d_static_init gtest_imu_2d_static_init.cpp)
# wolf_add_gtest(gtest_processor_motion_intrinsics_update gtest_processor_motion_intrinsics_update.cpp)
-# wolf_add_gtest(gtest_schema gtest_schema.cpp)
+wolf_add_gtest(gtest_schema gtest_schema.cpp)
wolf_add_gtest(gtest_sensor_compass gtest_sensor_compass.cpp)
diff --git a/test/gtest_factor_compass_3d.cpp b/test/gtest_factor_compass_3d.cpp
index 85001276b5487dc025899815e35123874578b392..865d3644054457ea8c3cba7c7f7081bdeb32909e 100644
--- a/test/gtest_factor_compass_3d.cpp
+++ b/test/gtest_factor_compass_3d.cpp
@@ -20,7 +20,7 @@
#include "core/utils/utils_gtest.h"
-#include "imu/internal/config.h"
+#include "imu/common/imu.h"
#include "imu/factor/factor_compass_3d.h"
#include "imu/capture/capture_compass.h"
#include "imu/sensor/sensor_compass.h"
diff --git a/test/gtest_factor_imu_3d.cpp b/test/gtest_factor_imu_3d.cpp
index 82580f1696e2abfd4ac878386537c552a0b55c45..0ed5b1b01dd016ae68ba34efb10604e6438641c6 100644
--- a/test/gtest_factor_imu_3d.cpp
+++ b/test/gtest_factor_imu_3d.cpp
@@ -23,7 +23,7 @@
#include <core/utils/logging.h>
// Imu
-#include "imu/internal/config.h"
+#include "imu/common/imu.h"
#include "imu/capture/capture_imu.h"
#include "imu/processor/processor_imu_3d.h"
#include "imu/sensor/sensor_imu.h"
diff --git a/test/gtest_imu_2d_static_init.cpp b/test/gtest_imu_2d_static_init.cpp
index 86f4128b3f1dd9c9b5da831c4866b584a5ae3ad8..949efceaf2f8c5d5e5e8b2ba26502be183e3dfdb 100644
--- a/test/gtest_imu_2d_static_init.cpp
+++ b/test/gtest_imu_2d_static_init.cpp
@@ -20,7 +20,7 @@
#include <fstream>
#include <core/utils/utils_gtest.h>
-#include "imu/internal/config.h"
+#include "imu/common/imu.h"
#include "imu/factor/factor_imu_2d.h"
#include "imu/math/imu_2d_tools.h"
@@ -59,10 +59,9 @@ class ProcessorImu2dStaticInitTest : public testing::Test
WOLF_INFO("Doing setup...");
// Wolf problem
- problem_ = Problem::create("POV", 2);
- sensor_ = problem_->installSensor(imu_dir + "/test/yaml/sensor_imu_2d.yaml", {imu_dir});
- processor_motion_ = std::static_pointer_cast<ProcessorMotion>(
- problem_->installProcessor(imu_dir + "/test/yaml/processor_imu_2d_static_init.yaml", {imu_dir}));
+ problem_ = Problem::autoSetup(imu_dir + "/test/yaml/problem_gtest_imu_2d_static_init.yaml", {imu_dir});
+ sensor_ = problem_->findSensor("cool sensor imu");
+ processor_motion_ = std::static_pointer_cast<ProcessorMotion>(sensor_->getProcessorList().front());
// Time and data variables
dt = 0.1;
@@ -78,15 +77,7 @@ class ProcessorImu2dStaticInitTest : public testing::Test
"SolverCeres", problem_, imu_dir + "/test/yaml/solver_ceres.yaml", {wolf_schema_dir});
// Set the origin
- // VectorComposite x_origin("POV", {Vector2d::Zero(), Vector1d::Zero(), Vector2d::Zero()});
- // VectorComposite std_origin("POV", {0.001*Vector2d::Ones(), 0.001*Vector1d::Ones(), 0.001*Vector2d::Ones()});
- // //KF0_ = problem_->setPriorFix(x_origin, t0);
- // KF0_ = problem_->setPriorFactor(x_origin, std_origin, 0);
- SpecComposite problem_prior{
- {'P', SpecState("StatePoint2d", Vector2d::Zero(), "factor", Vector2d::Constant(0.001))},
- {'O', SpecState("StateAngle", Vector1d::Zero(), "factor", Vector1d::Constant(0.001))},
- {'V', SpecState("StateVector2d", Vector2d::Zero(), "factor", Vector2d::Constant(0.001))}};
- KF0_ = problem_->setPrior(problem_prior, t0);
+ KF0_ = problem_->applyPriorOptions(t0);
}
void TestStatic(const Vector3d& body_magnitudes,
@@ -309,8 +300,14 @@ class ProcessorImu2dStaticInitTest : public testing::Test
auto feature_zero = FeatureBase::emplace<FeatureBase>(
capture_zero, "FeatureZeroOdom", Vector3d::Zero(), MatrixXd::Identity(3, 3) * 0.01);
- FactorBase::emplace<FactorRelativePose2d>(
- feature_zero, feature_zero, frm_pair.second, nullptr, false, TOP_MOTION);
+ FactorBase::emplace<FactorRelativePose2d>(feature_zero,
+ feature_zero->getMeasurement(),
+ feature_zero->getMeasurementSquareRootInformationUpper(),
+ frm_pair.second,
+ last_frame_,
+ nullptr,
+ false,
+ TOP_MOTION);
}
// impose static
diff --git a/test/gtest_imu_3d.cpp b/test/gtest_imu_3d.cpp
index 1bec04ae8133bdffcb3ad01e2431d876dacf6612..a2457310349eda5813873632ad1aa41c20ce328b 100644
--- a/test/gtest_imu_3d.cpp
+++ b/test/gtest_imu_3d.cpp
@@ -23,7 +23,7 @@
#include "imu/processor/processor_imu_3d.h"
#include "imu/sensor/sensor_imu.h"
#include "imu/math/imu_3d_tools.h"
-#include "imu/internal/config.h"
+#include "imu/common/imu.h"
#include <core/utils/utils_gtest.h>
#include <core/common/wolf.h>
diff --git a/test/gtest_imu_3d_mocap_fusion.cpp b/test/gtest_imu_3d_mocap_fusion.cpp
index dd13a7c4929e2b151f1d44a890c74d3093b97ae5..4dbc45f25d998fdf74b3d4c151ab53fefb4b60a0 100644
--- a/test/gtest_imu_3d_mocap_fusion.cpp
+++ b/test/gtest_imu_3d_mocap_fusion.cpp
@@ -20,6 +20,11 @@
#include <fstream>
+#include "imu/common/imu.h"
+#include "imu/capture/capture_imu.h"
+#include "imu/sensor/sensor_imu.h"
+#include "imu/processor/processor_imu_3d.h"
+
#include "core/utils/utils_gtest.h"
#include "core/ceres_wrapper/solver_ceres.h"
#include "core/capture/capture_base.h"
@@ -29,37 +34,33 @@
#include "core/capture/capture_odom_3d.h"
#include "core/factor/factor_pose_3d_with_extrinsics.h"
-#include "imu/internal/config.h"
-#include "imu/capture/capture_imu.h"
-#include "imu/sensor/sensor_imu.h"
-#include "imu/processor/processor_imu_3d.h"
-
#include "Eigen/Dense"
#include <Eigen/SparseQR>
#include <Eigen/OrderingMethods>
-
using namespace Eigen;
using namespace wolf;
-const Vector3d zero3 = Vector3d::Zero();
const double dt = 0.001;
-std::string wolf_root = _WOLF_IMU_CODE_DIR;
+std::string imu_dir = _WOLF_IMU_CODE_DIR;
+std::string wolf_schema_dir = _WOLF_SCHEMA_DIR;
+
+#define WRITE_CSV_FILE false
class ImuMocapFusion_Test : public testing::Test
{
- public:
- ProblemPtr problem_;
- SolverCeresPtr solver_;
- SensorBasePtr sensor_pose_;
- SensorBasePtr sensor_imu_;
-
- Vector3d b_p_bm_;
- Quaterniond b_q_m_;
- Vector3d ba_;
- Vector3d bw_;
- double t_max;
+ public:
+ ProblemPtr problem_;
+ SolverManagerPtr solver_;
+ SensorBasePtr sensor_pose_;
+ SensorBasePtr sensor_imu_;
+
+ Vector3d b_p_bm_;
+ Quaterniond b_q_m_;
+ Vector3d ba_;
+ Vector3d bw_;
+ double t_max;
void SetUp() override
{
@@ -67,13 +68,17 @@ class ImuMocapFusion_Test : public testing::Test
// simulate trajectory
//////////////////////
// Amplitudes
- Vector3d freq_lin; freq_lin << 2, 2.5, 3; // in seconds
- Vector3d freq_ang; freq_ang << 3.5, 3, 2.5; // in seconds
- freq_lin *= 2*M_PI; // pass in radians
- freq_ang *= 2*M_PI;
- Vector3d amp_p; amp_p << 1.2, 1, 0.8;
+ Vector3d freq_lin;
+ freq_lin << 2, 2.5, 3; // in seconds
+ Vector3d freq_ang;
+ freq_ang << 3.5, 3, 2.5; // in seconds
+ freq_lin *= 2 * M_PI; // pass in radians
+ freq_ang *= 2 * M_PI;
+ Vector3d amp_p;
+ amp_p << 1.2, 1, 0.8;
Vector3d amp_a = amp_p.array() * freq_lin.array().square();
- Vector3d amp_o; amp_o << 1, 1, 1;
+ Vector3d amp_o;
+ amp_o << 1, 1, 1;
// Vector3d amp_o; amp_o << 0,0,0;
Vector3d amp_w = amp_o.array() * freq_ang.array();
@@ -82,132 +87,100 @@ class ImuMocapFusion_Test : public testing::Test
// A = -freq^2*cos(freq*t)
// biases and extrinsics
- // ba_ = Vector3d::Zero();
- // bw_ = Vector3d::Zero();
ba_ << 0.003, 0.002, 0.002;
bw_ << 0.004, 0.003, 0.002;
- // b_p_bm_ = Vector3d::Zero();
- // b_q_m_ = Quaterniond::Identity();
b_p_bm_ << 0.1, 0.2, 0.3;
- b_q_m_ = Quaterniond(0,0,0,1);
-
+ b_q_m_ = Quaterniond(0, 0, 0, 1);
+
// initialize state
- Vector3d w_p_wb = Vector3d::Zero();
- Quaterniond w_q_b = Quaterniond::Identity();
- Vector3d w_v_wb = Vector3d::Zero(); // * cos(0)
-
- // Problem and solver_
- problem_ = Problem::create("POV", 3);
- solver_ = std::make_shared<SolverCeres>(problem_);
- solver_->getSolverOptions().max_num_iterations = 500;
- // initial guess
- // VectorComposite x_origin("POV", {w_p_wb, w_q_b.coeffs(), w_v_wb});
- // FrameBasePtr KF1 = problem_->setPriorInitialGuess(x_origin, 0); // if mocap used
- SpecComposite problem_prior{{'P', SpecState("StatePoint3d", w_p_wb, "initial_guess")},
- {'O', SpecState("StateQuaternion", w_q_b.coeffs(), "initial_guess")},
- {'V', SpecState("StateVector3d", w_v_wb, "initial_guess")}};
- FrameBasePtr KF1 = problem_->setPrior(problem_prior, 0);
-
- // pose sensor and proc (unfixed extrinsics)
- sensor_pose_ = problem_->installSensor("SensorPose3d",
- wolf_root + "/test/yaml/imu_mocap_fusion/sensor_pose_3d.yaml",
- {wolf_root});
- auto proc_pose = problem_->installProcessor("ProcessorPose",
- sensor_pose_,
- wolf_root + "/test/yaml/imu_mocap_fusion/processor_pose_3d.yaml",
- {wolf_root});
+ Vector3d w_p_wb = Vector3d::Zero();
+ Quaterniond w_q_b = Quaterniond::Identity();
+ Vector3d w_v_wb = Vector3d::Zero(); // * cos(0)
+
+ // Problem
+ problem_ = Problem::autoSetup(imu_dir + "/test/yaml/imu_mocap_fusion/problem_3d.yaml", {imu_dir});
+ sensor_pose_ = problem_->findSensor("cool sensor pose");
+ sensor_imu_ = problem_->findSensor("cool sensor imu");
Matrix6d cov_pose = sensor_pose_->computeNoiseCov(VectorXd(0));
+ Matrix6d cov_imu = sensor_imu_->computeNoiseCov(VectorXd(0));
+
+ // Create the solver
+ solver_ = FactorySolverFile::create(
+ "SolverCeres", problem_, imu_dir + "/test/yaml/solver_ceres.yaml", {wolf_schema_dir});
- // IMU sensor (unfixed intrinsics)
- sensor_imu_ = problem_->installSensor("SensorImu3d",
- wolf_root + "/test/yaml/imu_mocap_fusion/sensor_imu.yaml",
- {wolf_root});
- ProcessorImuPtr proc_imu = std::static_pointer_cast<ProcessorImu>(problem_->installProcessor("ProcessorImu",
- sensor_imu_,
- wolf_root + "/test/yaml/imu_mocap_fusion/processor_imu.yaml",
- {wolf_root}));
+ // first frame
+ FrameBasePtr KF1 = problem_->applyPriorOptions(0);
+ auto proc_imu = std::static_pointer_cast<ProcessorImu3d>(sensor_imu_->getProcessorList().front());
proc_imu->setOrigin(KF1);
- Matrix6d cov_imu = sensor_imu_->computeNoiseCov(VectorXd(0));
+#if WRITE_CSV_FILE
// Store necessary values in vectors
- // std::vector<Vector3d> w_p_wb_vec;
- // std::vector<Vector3d> w_p_wb_sin_vec;
- // std::vector<Quaterniond> w_q_b_vec;
- // std::vector<Vector3d> w_v_wb_vec;
-
- // w_p_wb_vec.push_back(w_p_wb);
- // w_p_wb_sin_vec.push_back(w_p_wb);
- // w_q_b_vec.push_back(w_q_b);
- // w_v_wb_vec.push_back(w_v_wb);
+ std::vector<Vector3d> w_p_wb_vec;
+ std::vector<Vector3d> w_p_wb_sin_vec;
+ std::vector<Quaterniond> w_q_b_vec;
+ std::vector<Vector3d> w_v_wb_vec;
+ w_p_wb_vec.push_back(w_p_wb);
+ w_p_wb_sin_vec.push_back(w_p_wb);
+ w_q_b_vec.push_back(w_q_b);
+ w_v_wb_vec.push_back(w_v_wb);
- // std::fstream file_gtr;
- // file_gtr.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/PhdTests/gtest_gtr.csv", std::fstream::out);
- // std::string header = "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz\n";
- // file_gtr << header;
+ std::fstream file_gtr;
+ file_gtr.open(imu_dir + "/test/yaml/imu_mocap_fusion/gtest_gtr.csv", std::fstream::out);
+ file_gtr << "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz\n";
+#endif
double t = 0;
- t_max = 5.0;
- while (t <= t_max){
+ t_max = 5.0;
+ while (t <= t_max)
+ {
// P = cos(freq*t) + cst
// V = -freq*sin(freq*t)
// A = -freq^2*cos(freq*t)
- Vector3d b_omg_b = -amp_w.array()*Eigen::sin((freq_ang*t).array());
- Vector3d b_a_wb = -amp_a.array()*Eigen::cos((freq_lin*t).array());
+ Vector3d b_omg_b = -amp_w.array() * Eigen::sin((freq_ang * t).array());
+ Vector3d b_a_wb = -amp_a.array() * Eigen::cos((freq_lin * t).array());
- // imu measurements (BETTER HERE)
+ // imu measurements
Vector3d acc_meas = b_a_wb - w_q_b.conjugate() * wolf::gravity() + ba_;
Vector3d omg_meas = b_omg_b + bw_;
// integrate simulated traj
- w_p_wb = w_p_wb + w_v_wb*dt + 0.5*(w_q_b*b_a_wb)*dt*dt;
- w_v_wb = w_v_wb + w_q_b*b_a_wb*dt;
- w_q_b = w_q_b*wolf::exp_q(b_omg_b*dt);
-
- // file_gtr << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
- // << t << ","
- // << w_p_wb(0) << ","
- // << w_p_wb(1) << ","
- // << w_p_wb(2) << ","
- // << w_q_b.coeffs()(0) << ","
- // << w_q_b.coeffs()(1) << ","
- // << w_q_b.coeffs()(2) << ","
- // << w_q_b.coeffs()(3) << ","
- // << w_v_wb(0) << ","
- // << w_v_wb(1) << ","
- // << w_v_wb(2)
- // << "\n";
-
- // // imu measurements
- // Vector3d acc_meas = b_a_wb - w_q_b.conjugate() * wolf::gravity() + ba_;
- // Vector3d omg_meas = b_omg_b + bw_;
+ w_p_wb = w_p_wb + w_v_wb * dt + 0.5 * (w_q_b * b_a_wb) * dt * dt;
+ w_v_wb = w_v_wb + w_q_b * b_a_wb * dt;
+ w_q_b = w_q_b * wolf::exp_q(b_omg_b * dt);
+
+#if WRITE_CSV_FILE
+ file_gtr << std::setprecision(std::numeric_limits<long double>::digits10 + 1) << t << "," << w_p_wb(0)
+ << "," << w_p_wb(1) << "," << w_p_wb(2) << "," << w_q_b.coeffs()(0) << "," << w_q_b.coeffs()(1)
+ << "," << w_q_b.coeffs()(2) << "," << w_q_b.coeffs()(3) << "," << w_v_wb(0) << "," << w_v_wb(1)
+ << "," << w_v_wb(2) << "\n";
+#endif
// mocap measurements
- Vector3d w_p_wm = w_p_wb + w_q_b*b_p_bm_;
- Quaterniond w_q_m = w_q_b * b_q_m_;
+ Vector3d w_p_wm = w_p_wb + w_q_b * b_p_bm_;
+ Quaterniond w_q_m = w_q_b * b_q_m_;
// process data
- Vector6d imu_data; imu_data << acc_meas, omg_meas;
+ Vector6d imu_data;
+ imu_data << acc_meas, omg_meas;
CaptureBasePtr CIMU = std::make_shared<CaptureImu>(t, sensor_imu_, imu_data, cov_imu);
CIMU->process();
- // sensor_imu_->fixIntrinsics();
sensor_imu_->unfixIntrinsics();
- Vector7d pose_data; pose_data << w_p_wm, w_q_m.coeffs();
+ Vector7d pose_data;
+ pose_data << w_p_wm, w_q_m.coeffs();
CaptureBasePtr CP = std::make_shared<CapturePose>(t, sensor_pose_, pose_data, cov_pose);
CP->process();
- t+=dt;
+ t += dt;
}
- // file_gtr.close();
-
+#if WRITE_CSV_FILE
+ file_gtr.close();
+#endif
}
void TearDown() override{};
};
-
-
-
TEST_F(ImuMocapFusion_Test, check_tree)
{
ASSERT_TRUE(problem_->check(0));
@@ -215,60 +188,64 @@ TEST_F(ImuMocapFusion_Test, check_tree)
TEST_F(ImuMocapFusion_Test, solve)
{
+#if WRITE_CSV_FILE
+ std::fstream file_int;
+ file_int.open(imu_dir + "/test/yaml/imu_mocap_fusion/gtest_int.csv", std::fstream::out);
+ file_int << "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz\n";
+
+ double t = 0;
+ while (t <= t_max){
+ auto s = problem_->getState(t);
+ file_int << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
+ << t << ","
+ << s['P'](0) << ","
+ << s['P'](1) << ","
+ << s['P'](2) << ","
+ << s['O'](0) << ","
+ << s['O'](1) << ","
+ << s['O'](2) << ","
+ << s['O'](3) << ","
+ << s['V'](0) << ","
+ << s['V'](1) << ","
+ << s['V'](2)
+ << "\n";
+ t+=dt;
+ }
+ file_int.close();
+#endif
- // std::fstream file_int;
- // std::fstream file_est;
- // file_int.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/PhdTests/gtest_int.csv", std::fstream::out);
- // file_est.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/PhdTests/gtest_est.csv", std::fstream::out);
- // std::string header = "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz\n";
- // file_int << header;
- // file_est << header;
-
- // double t = 0;
- // while (t <= t_max){
- // auto s = problem_->getState(t);
- // file_int << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
- // << t << ","
- // << s['P'](0) << ","
- // << s['P'](1) << ","
- // << s['P'](2) << ","
- // << s['O'](0) << ","
- // << s['O'](1) << ","
- // << s['O'](2) << ","
- // << s['O'](3) << ","
- // << s['V'](0) << ","
- // << s['V'](1) << ","
- // << s['V'](2)
- // << "\n";
- // t+=dt;
- // }
// problem_->print(4, 1, 1, 1);
problem_->perturb();
std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL);
// std::cout << report << std::endl;
// problem_->print(4, 1, 1, 1);
- // t = 0;
- // while (t <= t_max){
- // auto s = problem_->getState(t);
- // file_est << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
- // << t << ","
- // << s['P'](0) << ","
- // << s['P'](1) << ","
- // << s['P'](2) << ","
- // << s['O'](0) << ","
- // << s['O'](1) << ","
- // << s['O'](2) << ","
- // << s['O'](3) << ","
- // << s['V'](0) << ","
- // << s['V'](1) << ","
- // << s['V'](2)
- // << "\n";
- // t+=dt;
- // }
-
- // file_int.close();
- // file_est.close();
+#if WRITE_CSV_FILE
+
+ std::fstream file_est;
+ file_est.open(imu_dir + "/test/yaml/imu_mocap_fusion/gtest_est.csv", std::fstream::out);
+ file_est << "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz\n";
+
+ t = 0;
+ while (t <= t_max){
+ auto s = problem_->getState(t);
+ file_est << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
+ << t << ","
+ << s['P'](0) << ","
+ << s['P'](1) << ","
+ << s['P'](2) << ","
+ << s['O'](0) << ","
+ << s['O'](1) << ","
+ << s['O'](2) << ","
+ << s['O'](3) << ","
+ << s['V'](0) << ","
+ << s['V'](1) << ","
+ << s['V'](2)
+ << "\n";
+ t+=dt;
+ }
+ file_est.close();
+#endif
ASSERT_MATRIX_APPROX(sensor_pose_->getP()->getState(), b_p_bm_, 1e-6);
ASSERT_QUATERNION_VECTOR_APPROX(sensor_pose_->getO()->getState(), b_q_m_.coeffs(), 1e-6);
@@ -277,6 +254,6 @@ TEST_F(ImuMocapFusion_Test, solve)
int main(int argc, char **argv)
{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
}
diff --git a/test/gtest_imu_3d_static_init.cpp b/test/gtest_imu_3d_static_init.cpp
index 648f42a6b9622567ac905876a5637506fd0f3a9e..eeb3a6f88531f2605a237de1a83169f6ee74d6b6 100644
--- a/test/gtest_imu_3d_static_init.cpp
+++ b/test/gtest_imu_3d_static_init.cpp
@@ -17,531 +17,487 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-/*
- * gtest_imu_static_init.cpp
- *
- * Created on: Sept 2021
- * Author: igeer
- */
+
#include <fstream>
#include <core/ceres_wrapper/solver_ceres.h>
#include <core/utils/utils_gtest.h>
-#include "imu/internal/config.h"
+#include "imu/common/imu.h"
-#include "imu/factor/factor_imu.h"
+#include "imu/factor/factor_imu_3d.h"
#include "imu/math/imu_3d_tools.h"
#include "imu/sensor/sensor_imu.h"
+#include "core/processor/processor_motion.h"
#include "core/capture/capture_void.h"
#include <core/factor/factor_relative_pose_3d.h>
using namespace Eigen;
using namespace wolf;
+std::string imu_dir = _WOLF_IMU_CODE_DIR;
+std::string wolf_schema_dir = _WOLF_SCHEMA_DIR;
+
/**
* SET TO TRUE TO PRODUCE CSV FILE
* SET TO FALSE TO STOP PRODUCING CSV FILE
*/
-#define WRITE_CSV_FILE true
+#define WRITE_CSV_FILE true
class ProcessorImuStaticInitTest : public testing::Test
{
-
- public: //These can be accessed in fixtures
- wolf::ProblemPtr problem_ptr_;
- wolf::SensorBasePtr sensor_ptr_;
- wolf::ProcessorMotionPtr processor_motion_;
- wolf::TimeStamp t;
- wolf::TimeStamp t0;
- double dt;
- Vector6d data;
- Matrix6d data_cov;
- FrameBasePtr KF0_;
- FrameBasePtr first_frame_;
- FrameBasePtr last_frame_;
- SolverCeresPtr solver_;
-
- //a new of this will be created for each new test
+ public: // These can be accessed in fixtures
+ ProblemPtr problem_;
+ SensorBasePtr sensor_;
+ ProcessorMotionPtr processor_motion_;
+ TimeStamp t;
+ TimeStamp t0;
+ double dt;
+ Vector6d data;
+ Matrix6d data_cov;
+ FrameBasePtr KF0_;
+ FrameBasePtr first_frame_;
+ FrameBasePtr last_frame_;
+ SolverManagerPtr solver_;
+
+ // a new of this will be created for each new test
void SetUp() override
{
WOLF_INFO("Doing setup...");
using namespace Eigen;
- using std::shared_ptr;
using std::make_shared;
+ using std::shared_ptr;
using std::static_pointer_cast;
- using namespace wolf::Constants;
-
- std::string wolf_root = _WOLF_IMU_CODE_DIR;
+ using namespace Constants;
// Wolf problem
- problem_ptr_ = Problem::create("POV", 3);
- sensor_ptr_ = problem_ptr_->installSensor("SensorImu", wolf_root + "/test/yaml/sensor_imu.yaml", {wolf_root});
- ProcessorBasePtr processor_ptr = problem_ptr_->installProcessor("ProcessorImu", "Main Imu", wolf_root + "/test/yaml/processor_imu_static_init.yaml", {wolf_root});
- processor_motion_ = std::static_pointer_cast<ProcessorMotion>(processor_ptr);
+ problem_ = Problem::autoSetup(imu_dir + "/test/yaml/problem_gtest_imu_3d_static_init.yaml", {imu_dir});
+ sensor_ = problem_->findSensor("cool sensor imu");
+ processor_motion_ = std::static_pointer_cast<ProcessorMotion>(sensor_->getProcessorList().front());
// Time and data variables
dt = 0.1;
t0.set(0);
- t = t0;
- data = Vector6d::Random();
- data_cov = Matrix6d::Identity();
- last_frame_ = nullptr;
+ t = t0;
+ data = Vector6d::Random();
+ data_cov = Matrix6d::Identity();
+ last_frame_ = nullptr;
first_frame_ = nullptr;
// Create the solver
- solver_ = make_shared<SolverCeres>(problem_ptr_);
-
- // Set the origin
- VectorComposite x0c("POV", {Vector3d::Zero(), (Vector4d() << 0,0,0,1).finished(), Vector3d::Zero()});
- WOLF_INFO("x0c is: \n", x0c);
- //KF0_ = problem_ptr_->setPriorFix(x0c, t0);
-
- Vector4d q_init; q_init << 0,0,0,1;
- VectorComposite x_origin("POV", {Vector3d::Zero(), q_init, Vector3d::Zero()});
- VectorComposite std_origin("POV", {0.001*Vector3d::Ones(), 0.001*Vector3d::Ones(), 0.001*Vector3d::Ones()});
- KF0_ = problem_ptr_->setPriorFactor(x_origin, std_origin, 0);
+ solver_ = FactorySolverFile::create(
+ "SolverCeres", problem_, imu_dir + "/test/yaml/solver_ceres.yaml", {wolf_schema_dir});
+ // Set the origin
+ KF0_ = problem_->applyPriorOptions(t0);
}
- void TestStatic(const Vector6d& body_magnitudes, const Vector6d& bias_groundtruth, const Vector6d& bias_initial_guess, const string& test_name, int testing_var, bool small_tol)
+ void TestStatic(const Vector6d& body_magnitudes,
+ const Vector6d& bias_groundtruth,
+ const Vector6d& bias_initial_guess,
+ const std::string& test_name,
+ int testing_var,
+ bool small_tol)
{
- //Data
- data = body_magnitudes;
- data.head(3) -= Quaterniond(Vector4d(KF0_->getO()->getState())).conjugate() * wolf::gravity();
- data += bias_groundtruth;
+ // Data
+ data = body_magnitudes;
+ data.head(3) -= Quaterniond(Vector4d(KF0_->getO()->getState())).conjugate() * gravity();
+ data += bias_groundtruth;
- //Set bias initial guess
- sensor_ptr_->getIntrinsic(t0)->setState(bias_initial_guess);
- processor_motion_->setOrigin(KF0_);
+ // Set bias initial guess
+ sensor_->getIntrinsic(t0)->setState(bias_initial_guess);
+ processor_motion_->setOrigin(KF0_);
#if WRITE_CSV_FILE
- std::fstream file_est;
- file_est.open("./est-"+test_name+".csv", std::fstream::out);
- // std::string header_est = "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz,bax_est,bax_preint\n";
- std::string header_est;
- if(testing_var == 0) header_est = "t;px;vx;ox;bax_est;bgx_est;bax_preint;bgx_preint\n";
- if(testing_var == 1) header_est = "t;py;vy;oy;bay_est;bgy_est;bay_preint;bgy_preint\n";
- if(testing_var == 2) header_est = "t;pz;vz;oz:baz_est;bgz_est;baz_preint;bgz_preint\n";
- if(testing_var == 3) header_est = "t;pnorm;vnorm;onorm;banorm_est;bgnorm_est;banorm_preint;bgnorm_preint\n";
- file_est << header_est;
+ std::fstream file_est;
+ file_est.open("./est-" + test_name + ".csv", std::fstream::out);
+ // std::string header_est = "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz,bax_est,bax_preint\n";
+ std::string header_est;
+ if (testing_var == 0) header_est = "t;px;vx;ox;bax_est;bgx_est;bax_preint;bgx_preint\n";
+ if (testing_var == 1) header_est = "t;py;vy;oy;bay_est;bgy_est;bay_preint;bgy_preint\n";
+ if (testing_var == 2) header_est = "t;pz;vz;oz:baz_est;bgz_est;baz_preint;bgz_preint\n";
+ if (testing_var == 3) header_est = "t;pnorm;vnorm;onorm;banorm_est;bgnorm_est;banorm_preint;bgnorm_preint\n";
+ file_est << header_est;
#endif
-
- int n_frames = 0;
- for(t = t0; t <= t0 + 9.9 + dt/2; t+=dt){
- WOLF_INFO("\n------------------------------------------------------------------------");
-
- //Create and process capture
- auto C = std::make_shared<CaptureImu>(t, sensor_ptr_, data, data_cov, KF0_->getCaptureList().front());
- C->process();
-
- auto state = problem_ptr_->getState();
- auto bias_est = sensor_ptr_->getIntrinsic()->getState();
- auto bias_preint = processor_motion_->getLast()->getCalibrationPreint();
-
-#if WRITE_CSV_FILE
- // pre-solve print to CSV
- if(testing_var == 3){
- file_est << std::fixed << t << ";"
- << state['P'].norm() << ";"
- << state['V'].norm() << ";"
- << state['O'].norm() << ";"
- << bias_est.head(3).norm() << ";"
- << bias_est.tail(3).norm() << ";"
- << bias_preint.head(3).norm() << ";"
- << bias_preint.tail(3).norm() << "\n";
- }
- else
+ int n_frames = 0;
+ for (t = t0; t <= t0 + 9.9 + dt / 2; t += dt)
{
- file_est << std::fixed << t << ";"
- << state['P'](testing_var) << ";"
- << state['V'](testing_var) << ";"
- << state['O'](testing_var) << ";"
- << bias_est(testing_var) << ";"
- << bias_est(testing_var+3) << ";"
- << bias_preint(testing_var) << ";"
- << bias_preint(testing_var+3) << "\n";
+ WOLF_INFO("\n------------------------------------------------------------------------");
- }
-#endif
+ // Create and process capture
+ auto C = std::make_shared<CaptureImu>(t, sensor_, data, data_cov, KF0_->getCaptureList().front());
+ C->process();
- // new frame
- if (last_frame_ != processor_motion_->getOrigin()->getFrame())
- {
- n_frames++;
- last_frame_ = processor_motion_->getOrigin()->getFrame();
-
- // impose static
- last_frame_->getP()->setState(KF0_->getP()->getState());
- last_frame_->getO()->setState(KF0_->getO()->getState());
- last_frame_->getV()->setZero();
-
- //Fix frame
- last_frame_->fix();
-
- //Solve
- if(n_frames > 0)
- {
- std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL);
- //WOLF_INFO("Solver Report: ", report);
-
- state = problem_ptr_->getState();
- bias_est = sensor_ptr_->getIntrinsic()->getState();
- bias_preint = processor_motion_->getLast()->getCalibrationPreint();
-
- WOLF_INFO("The current problem is:");
- problem_ptr_->print(4);
+ auto state = problem_->getState();
+ auto bias_est = sensor_->getIntrinsic()->getState();
+ auto bias_preint = processor_motion_->getLast()->getCalibrationPreint();
#if WRITE_CSV_FILE
- // post-solve print to CSV with time-stamp shifted by dt/2 to separate from pre-solve result
- if(testing_var == 3){
- file_est << std::fixed << t+dt/2 << ";"
- << state['P'].norm() << ";"
- << state['V'].norm() << ";"
- << state['O'].norm() << ";"
- << bias_est.head(3).norm() << ";"
- << bias_est.tail(3).norm() << ";"
- << bias_preint.head(3).norm() << ";"
- << bias_preint.tail(3).norm() << "\n";
+ // pre-solve print to CSV
+ if (testing_var == 3)
+ {
+ file_est << std::fixed << t << ";" << state['P'].norm() << ";" << state['V'].norm() << ";"
+ << state['O'].norm() << ";" << bias_est.head(3).norm() << ";" << bias_est.tail(3).norm()
+ << ";" << bias_preint.head(3).norm() << ";" << bias_preint.tail(3).norm() << "\n";
}
else
{
- file_est << std::fixed << t+dt/2 << ";"
- << state['P'](testing_var) << ";"
- << state['V'](testing_var) << ";"
- << state['O'](testing_var) << ";"
- << bias_est(testing_var) << ";"
- << bias_est(testing_var+3) << ";"
- << bias_preint(testing_var) << ";"
- << bias_preint(testing_var+3) << "\n";
-
+ file_est << std::fixed << t << ";" << state['P'](testing_var) << ";" << state['V'](testing_var) << ";"
+ << state['O'](testing_var) << ";" << bias_est(testing_var) << ";" << bias_est(testing_var + 3)
+ << ";" << bias_preint(testing_var) << ";" << bias_preint(testing_var + 3) << "\n";
}
#endif
- }
- }
+ // new frame
+ if (last_frame_ != processor_motion_->getOrigin()->getFrame())
+ {
+ n_frames++;
+ last_frame_ = processor_motion_->getOrigin()->getFrame();
+ // impose static
+ last_frame_->getP()->setState(KF0_->getP()->getState());
+ last_frame_->getO()->setState(KF0_->getO()->getState());
+ last_frame_->getV()->setZero();
+ // Fix frame
+ last_frame_->fix();
- WOLF_INFO("Number of frames is ", n_frames);
- WOLF_INFO("The state is: ", state);
- WOLF_INFO("The estimated bias is: ", bias_est.transpose());
- WOLF_INFO("The preintegrated bias is: ", bias_preint.transpose());
- if(small_tol)
- {
- if(n_frames == 2)
- {
- EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-6);
- EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-6);
- }
- else if (n_frames > 2)
- {
- EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-6);
- EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-6);
- EXPECT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-6);
- }
- }
- else
- {
- if(n_frames == 2)
- {
- EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-3);
- EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-3);
- }
- else if (n_frames > 2)
- {
- EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-3);
- EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-3);
- EXPECT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-3);
- }
- }
- }
+ // Solve
+ if (n_frames > 0)
+ {
+ std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL);
+ // WOLF_INFO("Solver Report: ", report);
+
+ state = problem_->getState();
+ bias_est = sensor_->getIntrinsic()->getState();
+ bias_preint = processor_motion_->getLast()->getCalibrationPreint();
+
+ WOLF_INFO("The current problem is:");
+ problem_->print(4);
#if WRITE_CSV_FILE
- file_est.close();
+ // post-solve print to CSV with time-stamp shifted by dt/2 to separate from pre-solve result
+ if (testing_var == 3)
+ {
+ file_est << std::fixed << t + dt / 2 << ";" << state['P'].norm() << ";" << state['V'].norm()
+ << ";" << state['O'].norm() << ";" << bias_est.head(3).norm() << ";"
+ << bias_est.tail(3).norm() << ";" << bias_preint.head(3).norm() << ";"
+ << bias_preint.tail(3).norm() << "\n";
+ }
+ else
+ {
+ file_est << std::fixed << t + dt / 2 << ";" << state['P'](testing_var) << ";"
+ << state['V'](testing_var) << ";" << state['O'](testing_var) << ";"
+ << bias_est(testing_var) << ";" << bias_est(testing_var + 3) << ";"
+ << bias_preint(testing_var) << ";" << bias_preint(testing_var + 3) << "\n";
+ }
#endif
+ }
+ }
+ WOLF_INFO("Number of frames is ", n_frames);
+ WOLF_INFO("The state is: ", state);
+ WOLF_INFO("The estimated bias is: ", bias_est.transpose());
+ WOLF_INFO("The preintegrated bias is: ", bias_preint.transpose());
+ if (small_tol)
+ {
+ if (n_frames == 2)
+ {
+ EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-6);
+ EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-6);
+ }
+ else if (n_frames > 2)
+ {
+ EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-6);
+ EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-6);
+ EXPECT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-6);
+ }
+ }
+ else
+ {
+ if (n_frames == 2)
+ {
+ EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-3);
+ EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-3);
+ }
+ else if (n_frames > 2)
+ {
+ EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-3);
+ EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-3);
+ EXPECT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-3);
+ }
+ }
+ }
+
+#if WRITE_CSV_FILE
+ file_est.close();
+#endif
}
- void TestStaticZeroOdom(const Vector6d& body_magnitudes, const Vector6d& bias_groundtruth, const Vector6d& bias_initial_guess, const string& test_name, int testing_var, bool small_tol)
+ void TestStaticZeroOdom(const Vector6d& body_magnitudes,
+ const Vector6d& bias_groundtruth,
+ const Vector6d& bias_initial_guess,
+ const std::string& test_name,
+ int testing_var,
+ bool small_tol)
{
- //Data
- data = body_magnitudes;
- data.head(3) -= Quaterniond(Vector4d(KF0_->getO()->getState())).conjugate() * wolf::gravity();
- data += bias_groundtruth;
+ // Data
+ data = body_magnitudes;
+ data.head(3) -= Quaterniond(Vector4d(KF0_->getO()->getState())).conjugate() * gravity();
+ data += bias_groundtruth;
- //Set bias initial guess
- sensor_ptr_->getIntrinsic(t0)->setState(bias_initial_guess);
- processor_motion_->setOrigin(KF0_);
+ // Set bias initial guess
+ sensor_->getIntrinsic(t0)->setState(bias_initial_guess);
+ processor_motion_->setOrigin(KF0_);
#if WRITE_CSV_FILE
- std::fstream file_est;
- file_est.open("./estzeroodom-"+test_name+".csv", std::fstream::out);
- // std::string header_est = "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz,bax_est,bax_preint\n";
- std::string header_est;
- if(testing_var == 0) header_est = "t;px;vx;ox;bax_est;bgx_est;bax_preint;bgx_preint\n";
- if(testing_var == 1) header_est = "t;py;vy;oy;bay_est;bgy_est;bay_preint;bgy_preint\n";
- if(testing_var == 2) header_est = "t;pz;vz;oz:baz_est;bgz_est;baz_preint;bgz_preint\n";
- if(testing_var == 3) header_est = "t;pnorm;vnorm;onorm;banorm_est;bgnorm_est;banorm_preint;bgnorm_preint\n";
- file_est << header_est;
+ std::fstream file_est;
+ file_est.open("./estzeroodom-" + test_name + ".csv", std::fstream::out);
+ // std::string header_est = "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz,bax_est,bax_preint\n";
+ std::string header_est;
+ if (testing_var == 0) header_est = "t;px;vx;ox;bax_est;bgx_est;bax_preint;bgx_preint\n";
+ if (testing_var == 1) header_est = "t;py;vy;oy;bay_est;bgy_est;bay_preint;bgy_preint\n";
+ if (testing_var == 2) header_est = "t;pz;vz;oz:baz_est;bgz_est;baz_preint;bgz_preint\n";
+ if (testing_var == 3) header_est = "t;pnorm;vnorm;onorm;banorm_est;bgnorm_est;banorm_preint;bgnorm_preint\n";
+ file_est << header_est;
#endif
-
- int n_frames = 0;
- for(t = t0; t <= t0 + 9.9 + dt/2; t+=dt){
- WOLF_INFO("\n------------------------------------------------------------------------");
-
- //Create and process capture
- auto C = std::make_shared<CaptureImu>(t, sensor_ptr_, data, data_cov, KF0_->getCaptureList().front());
- C->process();
-
- auto state = problem_ptr_->getState();
- auto bias_est = sensor_ptr_->getIntrinsic()->getState();
- auto bias_preint = processor_motion_->getLast()->getCalibrationPreint();
-
-#if WRITE_CSV_FILE
- // pre-solve print to CSV
- if(testing_var == 3){
- file_est << std::fixed << t << ";"
- << state['P'].norm() << ";"
- << state['V'].norm() << ";"
- << state['O'].norm() << ";"
- << bias_est.head(3).norm() << ";"
- << bias_est.tail(3).norm() << ";"
- << bias_preint.head(3).norm() << ";"
- << bias_preint.tail(3).norm() << "\n";
- }
- else
+ int n_frames = 0;
+ for (t = t0; t <= t0 + 9.9 + dt / 2; t += dt)
{
- file_est << std::fixed << t << ";"
- << state['P'](testing_var) << ";"
- << state['V'](testing_var) << ";"
- << state['O'](testing_var) << ";"
- << bias_est(testing_var) << ";"
- << bias_est(testing_var+3) << ";"
- << bias_preint(testing_var) << ";"
- << bias_preint(testing_var+3) << "\n";
+ WOLF_INFO("\n------------------------------------------------------------------------");
- }
-#endif
+ // Create and process capture
+ auto C = std::make_shared<CaptureImu>(t, sensor_, data, data_cov, KF0_->getCaptureList().front());
+ C->process();
- // new frame
- if (last_frame_ != processor_motion_->getOrigin()->getFrame())
- {
- n_frames++;
- last_frame_ = processor_motion_->getOrigin()->getFrame();
-
- // impose zero odometry
- processor_motion_->setOdometry(sensor_ptr_->getProblem()->stateZero(processor_motion_->getStateStructure()));
-
- // add zero displacement and rotation capture & feature & factor with all previous frames
- assert(sensor_ptr_->getProblem());
- for (auto frm_pair : sensor_ptr_->getProblem()->getTrajectory()->getFrameMap())
- {
- if (frm_pair.second == last_frame_)
- break;
- auto capture_zero = CaptureBase::emplace<CaptureVoid>(last_frame_, last_frame_->getTimeStamp(), nullptr);
-
- auto feature_zero = FeatureBase::emplace<FeatureBase>(capture_zero,
- "FeatureZeroOdom",
- Vector7d::Zero(),
- Eigen::MatrixXd::Identity(6,6) * 0.01);
-
- FactorBase::emplace<FactorRelativePose3d>(feature_zero,
- feature_zero,
- frm_pair.second,
- nullptr,
- false,
- TOP_MOTION);
- }
-
- // impose static
- last_frame_->getV()->setZero();
-
- //Fix frame
- last_frame_->getV()->fix();
-
- //Solve
- if(n_frames > 0)
- {
- std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL);
- //WOLF_INFO("Solver Report: ", report);
-
- state = problem_ptr_->getState();
- bias_est = sensor_ptr_->getIntrinsic()->getState();
- bias_preint = processor_motion_->getLast()->getCalibrationPreint();
-
- WOLF_INFO("The current problem is:");
- problem_ptr_->print(4);
+ auto state = problem_->getState();
+ auto bias_est = sensor_->getIntrinsic()->getState();
+ auto bias_preint = processor_motion_->getLast()->getCalibrationPreint();
#if WRITE_CSV_FILE
- // post-solve print to CSV with time-stamp shifted by dt/2 to separate from pre-solve result
- if(testing_var == 3){
- file_est << std::fixed << t+dt/2 << ";"
- << state['P'].norm() << ";"
- << state['V'].norm() << ";"
- << state['O'].norm() << ";"
- << bias_est.head(3).norm() << ";"
- << bias_est.tail(3).norm() << ";"
- << bias_preint.head(3).norm() << ";"
- << bias_preint.tail(3).norm() << "\n";
+ // pre-solve print to CSV
+ if (testing_var == 3)
+ {
+ file_est << std::fixed << t << ";" << state['P'].norm() << ";" << state['V'].norm() << ";"
+ << state['O'].norm() << ";" << bias_est.head(3).norm() << ";" << bias_est.tail(3).norm()
+ << ";" << bias_preint.head(3).norm() << ";" << bias_preint.tail(3).norm() << "\n";
}
else
{
- file_est << std::fixed << t+dt/2 << ";"
- << state['P'](testing_var) << ";"
- << state['V'](testing_var) << ";"
- << state['O'](testing_var) << ";"
- << bias_est(testing_var) << ";"
- << bias_est(testing_var+3) << ";"
- << bias_preint(testing_var) << ";"
- << bias_preint(testing_var+3) << "\n";
-
+ file_est << std::fixed << t << ";" << state['P'](testing_var) << ";" << state['V'](testing_var) << ";"
+ << state['O'](testing_var) << ";" << bias_est(testing_var) << ";" << bias_est(testing_var + 3)
+ << ";" << bias_preint(testing_var) << ";" << bias_preint(testing_var + 3) << "\n";
}
#endif
- }
-
- }
+ // new frame
+ if (last_frame_ != processor_motion_->getOrigin()->getFrame())
+ {
+ n_frames++;
+ last_frame_ = processor_motion_->getOrigin()->getFrame();
+
+ // impose zero odometry
+ processor_motion_->setOdometry(sensor_->getProblem()->stateZero(processor_motion_->getStateKeys()));
+
+ // add zero displacement and rotation capture & feature & factor with all previous frames
+ assert(sensor_->getProblem());
+ for (auto frm_pair : sensor_->getProblem()->getTrajectory()->getFrameMap())
+ {
+ if (frm_pair.second == last_frame_) break;
+ auto capture_zero =
+ CaptureBase::emplace<CaptureVoid>(last_frame_, last_frame_->getTimeStamp(), nullptr);
+
+ auto feature_zero = FeatureBase::emplace<FeatureBase>(
+ capture_zero, "FeatureZeroOdom", Vector7d::Zero(), Eigen::MatrixXd::Identity(6, 6) * 0.01);
+
+ FactorBase::emplace<FactorRelativePose3d>(feature_zero,
+ feature_zero->getMeasurement(),
+ feature_zero->getMeasurementSquareRootInformationUpper(),
+ frm_pair.second,
+ last_frame_,
+ nullptr,
+ false,
+ TOP_MOTION);
+ }
+
+ // impose static
+ last_frame_->getV()->setZero();
+
+ // Fix frame
+ last_frame_->getV()->fix();
+
+ // Solve
+ if (n_frames > 0)
+ {
+ std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL);
+ // WOLF_INFO("Solver Report: ", report);
+
+ state = problem_->getState();
+ bias_est = sensor_->getIntrinsic()->getState();
+ bias_preint = processor_motion_->getLast()->getCalibrationPreint();
+
+ WOLF_INFO("The current problem is:");
+ problem_->print(4);
+#if WRITE_CSV_FILE
+ // post-solve print to CSV with time-stamp shifted by dt/2 to separate from pre-solve result
+ if (testing_var == 3)
+ {
+ file_est << std::fixed << t + dt / 2 << ";" << state['P'].norm() << ";" << state['V'].norm()
+ << ";" << state['O'].norm() << ";" << bias_est.head(3).norm() << ";"
+ << bias_est.tail(3).norm() << ";" << bias_preint.head(3).norm() << ";"
+ << bias_preint.tail(3).norm() << "\n";
+ }
+ else
+ {
+ file_est << std::fixed << t + dt / 2 << ";" << state['P'](testing_var) << ";"
+ << state['V'](testing_var) << ";" << state['O'](testing_var) << ";"
+ << bias_est(testing_var) << ";" << bias_est(testing_var + 3) << ";"
+ << bias_preint(testing_var) << ";" << bias_preint(testing_var + 3) << "\n";
+ }
+#endif
+ }
+ }
- WOLF_INFO("Number of frames is ", n_frames);
- WOLF_INFO("The state is: ", state);
- WOLF_INFO("The estimated bias is: ", bias_est.transpose());
- WOLF_INFO("The preintegrated bias is: ", bias_preint.transpose());
- if(small_tol)
- {
- if(n_frames == 2)
- {
- EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-6);
- EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-6);
- }
- else if (n_frames > 2)
- {
- EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-6);
- EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-6);
- EXPECT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-6);
- }
- }
- else
- {
- if(n_frames == 2)
- {
- EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-3);
- EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-3);
- }
- else if (n_frames > 2)
- {
- EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-3);
- EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-3);
- EXPECT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-3);
- }
+ WOLF_INFO("Number of frames is ", n_frames);
+ WOLF_INFO("The state is: ", state);
+ WOLF_INFO("The estimated bias is: ", bias_est.transpose());
+ WOLF_INFO("The preintegrated bias is: ", bias_preint.transpose());
+ if (small_tol)
+ {
+ if (n_frames == 2)
+ {
+ EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-6);
+ EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-6);
+ }
+ else if (n_frames > 2)
+ {
+ EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-6);
+ EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-6);
+ EXPECT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-6);
+ }
+ }
+ else
+ {
+ if (n_frames == 2)
+ {
+ EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-3);
+ EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-3);
+ }
+ else if (n_frames > 2)
+ {
+ EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-3);
+ EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-3);
+ EXPECT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-3);
+ }
+ }
}
- }
#if WRITE_CSV_FILE
- file_est.close();
+ file_est.close();
#endif
-
}
};
-
-
TEST_F(ProcessorImuStaticInitTest, static_bias_aX_initial_guess_zero)
{
- Vector6d body_magnitudes = Vector6d::Zero();
- Vector6d bias_groundtruth = (Vector6d() << 0.42, 0, 0, 0, 0, 0).finished();
- Vector6d bias_initial_guess = Vector6d::Zero();
+ Vector6d body_magnitudes = Vector6d::Zero();
+ Vector6d bias_groundtruth = (Vector6d() << 0.42, 0, 0, 0, 0, 0).finished();
+ Vector6d bias_initial_guess = Vector6d::Zero();
- TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_aX_initial_guess_zero", 0, true);
+ TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_aX_initial_guess_zero", 0, true);
}
TEST_F(ProcessorImuStaticInitTest, static_bias_zero_initial_guess_aX)
{
- Vector6d body_magnitudes = Vector6d::Zero();
- Vector6d bias_groundtruth = Vector6d::Zero();
- Vector6d bias_initial_guess = (Vector6d() << 0.42, 0, 0, 0, 0, 0).finished();
+ Vector6d body_magnitudes = Vector6d::Zero();
+ Vector6d bias_groundtruth = Vector6d::Zero();
+ Vector6d bias_initial_guess = (Vector6d() << 0.42, 0, 0, 0, 0, 0).finished();
- TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_aX", 0, true);
+ TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_aX", 0, true);
}
TEST_F(ProcessorImuStaticInitTest, static_bias_gX_initial_guess_zero)
{
- Vector6d body_magnitudes = Vector6d::Zero();
- Vector6d bias_groundtruth = (Vector6d() << 0, 0, 0, 0.01, 0, 0).finished();
- Vector6d bias_initial_guess = Vector6d::Zero();
+ Vector6d body_magnitudes = Vector6d::Zero();
+ Vector6d bias_groundtruth = (Vector6d() << 0, 0, 0, 0.01, 0, 0).finished();
+ Vector6d bias_initial_guess = Vector6d::Zero();
- TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_gX_initial_guess_zero", 0, false);
+ TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_gX_initial_guess_zero", 0, false);
}
TEST_F(ProcessorImuStaticInitTest, static_bias_zero_initial_guess_gX)
{
- Vector6d body_magnitudes = Vector6d::Zero();
- Vector6d bias_groundtruth = Vector6d::Zero();
- Vector6d bias_initial_guess = (Vector6d() << 0, 0, 0, 0.01, 0, 0).finished();
+ Vector6d body_magnitudes = Vector6d::Zero();
+ Vector6d bias_groundtruth = Vector6d::Zero();
+ Vector6d bias_initial_guess = (Vector6d() << 0, 0, 0, 0.01, 0, 0).finished();
- TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_gX", 0, false);
+ TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_gX", 0, false);
}
TEST_F(ProcessorImuStaticInitTest, static_bias_aX_initial_guess_zero_zeroodom)
{
- Vector6d body_magnitudes = Vector6d::Zero();
- Vector6d bias_groundtruth = (Vector6d() << 0.42, 0, 0, 0, 0, 0).finished();
- Vector6d bias_initial_guess = Vector6d::Zero();
+ Vector6d body_magnitudes = Vector6d::Zero();
+ Vector6d bias_groundtruth = (Vector6d() << 0.42, 0, 0, 0, 0, 0).finished();
+ Vector6d bias_initial_guess = Vector6d::Zero();
- TestStaticZeroOdom(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_aX_initial_guess_zero", 0, true);
+ TestStaticZeroOdom(
+ body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_aX_initial_guess_zero", 0, true);
}
TEST_F(ProcessorImuStaticInitTest, static_bias_zero_initial_guess_aX_zeroodom)
{
- Vector6d body_magnitudes = Vector6d::Zero();
- Vector6d bias_groundtruth = Vector6d::Zero();
- Vector6d bias_initial_guess = (Vector6d() << 0.42, 0, 0, 0, 0, 0).finished();
+ Vector6d body_magnitudes = Vector6d::Zero();
+ Vector6d bias_groundtruth = Vector6d::Zero();
+ Vector6d bias_initial_guess = (Vector6d() << 0.42, 0, 0, 0, 0, 0).finished();
- TestStaticZeroOdom(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_aX", 0, true);
+ TestStaticZeroOdom(
+ body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_aX", 0, true);
}
TEST_F(ProcessorImuStaticInitTest, static_bias_gX_initial_guess_zero_zeroodom)
{
- Vector6d body_magnitudes = Vector6d::Zero();
- Vector6d bias_groundtruth = (Vector6d() << 0, 0, 0, 0.01, 0, 0).finished();
- Vector6d bias_initial_guess = Vector6d::Zero();
+ Vector6d body_magnitudes = Vector6d::Zero();
+ Vector6d bias_groundtruth = (Vector6d() << 0, 0, 0, 0.01, 0, 0).finished();
+ Vector6d bias_initial_guess = Vector6d::Zero();
- TestStaticZeroOdom(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_gX_initial_guess_zero", 0, false);
+ TestStaticZeroOdom(
+ body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_gX_initial_guess_zero", 0, false);
}
TEST_F(ProcessorImuStaticInitTest, static_bias_zero_initial_guess_gX_zeroodom)
{
- Vector6d body_magnitudes = Vector6d::Zero();
- Vector6d bias_groundtruth = Vector6d::Zero();
- Vector6d bias_initial_guess = (Vector6d() << 0, 0, 0, 0.01, 0, 0).finished();
+ Vector6d body_magnitudes = Vector6d::Zero();
+ Vector6d bias_groundtruth = Vector6d::Zero();
+ Vector6d bias_initial_guess = (Vector6d() << 0, 0, 0, 0.01, 0, 0).finished();
- TestStaticZeroOdom(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_gX", 0, false);
+ TestStaticZeroOdom(
+ body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_gX", 0, false);
}
-
TEST_F(ProcessorImuStaticInitTest, static_bias_zero_initial_guess_a_random)
{
- Vector6d body_magnitudes = Vector6d::Zero();
- Vector6d bias_groundtruth = Vector6d::Zero();
- Vector6d bias_initial_guess = Vector6d::Random()*100;
- bias_initial_guess.tail(3) = Vector3d::Zero();
+ Vector6d body_magnitudes = Vector6d::Zero();
+ Vector6d bias_groundtruth = Vector6d::Zero();
+ Vector6d bias_initial_guess = Vector6d::Random() * 100;
+ bias_initial_guess.tail(3) = Vector3d::Zero();
- TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_a_random", 3, true);
+ TestStatic(
+ body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_a_random", 3, true);
}
TEST_F(ProcessorImuStaticInitTest, static_bias_zero_initial_guess_random)
{
- Vector6d body_magnitudes = Vector6d::Zero();
- Vector6d bias_groundtruth = Vector6d::Zero();
- Vector6d bias_initial_guess = Vector6d::Random()*0.01;
+ Vector6d body_magnitudes = Vector6d::Zero();
+ Vector6d bias_groundtruth = Vector6d::Zero();
+ Vector6d bias_initial_guess = Vector6d::Random() * 0.01;
- TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_random", 3, false);
+ TestStatic(
+ body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_random", 3, false);
}
-
-int main(int argc, char **argv)
+int main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
diff --git a/test/gtest_processor_imu_2d.cpp b/test/gtest_processor_imu_2d.cpp
index 7cd9f3336d73047fb32333c809eff18f052870fc..04d00d85acb423465e4d873f292ca5daf7858b56 100644
--- a/test/gtest_processor_imu_2d.cpp
+++ b/test/gtest_processor_imu_2d.cpp
@@ -18,7 +18,7 @@
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-#include "imu/internal/config.h"
+#include "imu/common/imu.h"
#include "imu/capture/capture_imu.h"
#include "imu/processor/processor_imu_2d.h"
#include "imu/sensor/sensor_imu.h"
diff --git a/test/gtest_processor_imu_2d_with_gravity.cpp b/test/gtest_processor_imu_2d_with_gravity.cpp
index 91dd896c12925059b283282ea8107b5261193914..9d30d56fb789a3ca143628a33cfaeef09e516dc5 100644
--- a/test/gtest_processor_imu_2d_with_gravity.cpp
+++ b/test/gtest_processor_imu_2d_with_gravity.cpp
@@ -18,7 +18,7 @@
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-#include "imu/internal/config.h"
+#include "imu/common/imu.h"
#include "imu/capture/capture_imu.h"
#include "imu/processor/processor_imu_2d.h"
#include "imu/sensor/sensor_imu.h"
diff --git a/test/gtest_processor_imu_3d.cpp b/test/gtest_processor_imu_3d.cpp
index 62c22ee1002ee0594d1ce31cf92d6b54172b8ee9..281a96dc065a9a4cd1a40f3358293f15e3a6a176 100644
--- a/test/gtest_processor_imu_3d.cpp
+++ b/test/gtest_processor_imu_3d.cpp
@@ -18,7 +18,7 @@
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-#include "imu/internal/config.h"
+#include "imu/common/imu.h"
#include "imu/capture/capture_imu.h"
#include "imu/processor/processor_imu_3d.h"
#include "imu/sensor/sensor_imu.h"
diff --git a/test/gtest_processor_motion_intrinsics_update.cpp b/test/gtest_processor_motion_intrinsics_update.cpp
index 8f9f3e68d7076331becdb64cbd0f1dedbb71f4d6..b6e170671c5c76a8ce358f05b5fa168410a460c0 100644
--- a/test/gtest_processor_motion_intrinsics_update.cpp
+++ b/test/gtest_processor_motion_intrinsics_update.cpp
@@ -20,7 +20,7 @@
#include <fstream>
-#include "imu/internal/config.h"
+#include "imu/common/imu.h"
#include "core/utils/utils_gtest.h"
#include "core/utils/logging.h"
diff --git a/test/gtest_schema.cpp b/test/gtest_schema.cpp
index 6e95b908004dffada91c3c87b2af7912531cacfa..d125b6a4a2b2eb299e70c82e8f32741819a4fa87 100644
--- a/test/gtest_schema.cpp
+++ b/test/gtest_schema.cpp
@@ -18,10 +18,8 @@
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-#include "gnss/common/gnss.h"
+#include "imu/common/imu.h"
#include "core/utils/utils_gtest.h"
-#include "gnss/capture/capture_gnss_fix.h"
-#include "gnss/sensor/sensor_gnss.h"
#include "core/map/map_base.h"
#include "core/sensor/sensor_base.h"
#include "core/solver/solver_manager.h"
diff --git a/test/gtest_sensor_compass.cpp b/test/gtest_sensor_compass.cpp
index 62b4a557c5993b823e48a30ffc38ac58f09dd00b..114eccf360f55ecea422ce7db8277ce21165d508 100644
--- a/test/gtest_sensor_compass.cpp
+++ b/test/gtest_sensor_compass.cpp
@@ -20,7 +20,7 @@
#include "core/utils/utils_gtest.h"
-#include "imu/internal/config.h"
+#include "imu/common/imu.h"
#include "imu/capture/capture_compass.h"
#include "imu/sensor/sensor_compass.h"
diff --git a/test/yaml/imu_mocap_fusion/problem_3d.yaml b/test/yaml/imu_mocap_fusion/problem_3d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..a6e96e14f6d8df9b1bf7fc3b88058e49e6e4a86b
--- /dev/null
+++ b/test/yaml/imu_mocap_fusion/problem_3d.yaml
@@ -0,0 +1,22 @@
+problem:
+ dimension: 3
+ first_frame:
+ P:
+ state: [0,0,0]
+ mode: "initial_guess"
+ O:
+ state: [0,0,0,1]
+ mode: "initial_guess"
+ V:
+ state: [0,0,0]
+ mode: "initial_guess"
+ tree_manager:
+ type: "None"
+
+sensors:
+ - follow: sensor_pose_3d.yaml
+ - follow: sensor_imu_3d.yaml
+
+processors:
+ - follow: processor_pose_3d.yaml
+ - follow: processor_imu_3d.yaml
\ No newline at end of file
diff --git a/test/yaml/imu_mocap_fusion/processor_imu_3d.yaml b/test/yaml/imu_mocap_fusion/processor_imu_3d.yaml
index ceb0ec1e48fc486c4d36e9c613aa8cb5986123cf..d3112f04549aaced60113266a2c7a61fc9d7f461 100644
--- a/test/yaml/imu_mocap_fusion/processor_imu_3d.yaml
+++ b/test/yaml/imu_mocap_fusion/processor_imu_3d.yaml
@@ -1,8 +1,9 @@
name: "cool processor imu"
plugin: imu
type: ProcessorImu3d
+sensor_name: "cool sensor imu"
-time_tolerance: 0.0025 # Time tolerance for joining KFs
+time_tolerance: 0.0005 # Time tolerance for joining KFs
unmeasured_perturbation_std: 0.00001
apply_loss_function: false
@@ -10,8 +11,11 @@ keyframe_vote:
voting_active: true
max_time_span: 0.199999 # seconds
max_buff_length: 1000000000 # motion deltas
- dist_traveled: 1000000000 # meters
- angle_turned: 1000000000 # radians (1 rad approx 57 deg, approx 60 deg)
+ max_dist_traveled: 1000000000 # meters
+ max_angle_turned: 1000000000 # radians (1 rad approx 57 deg, approx 60 deg)
+
+bootstrap:
+ enabled: false
state_provider: true
state_provider_order: 1
diff --git a/test/yaml/imu_mocap_fusion/processor_pose_3d.yaml b/test/yaml/imu_mocap_fusion/processor_pose_3d.yaml
index 5d2b7c3e1414275ddd82ce810c376ed0da568f0b..5fe93f43df7446583a5b3a7eb8d5ad035a9a8ffc 100644
--- a/test/yaml/imu_mocap_fusion/processor_pose_3d.yaml
+++ b/test/yaml/imu_mocap_fusion/processor_pose_3d.yaml
@@ -1,10 +1,11 @@
name: "cool processor pose"
plugin: core
type: ProcessorPose3d
+sensor_name: "cool sensor pose"
time_tolerance: 0.0005 # seconds
apply_loss_function: false
keyframe_vote:
- voting_active: true
\ No newline at end of file
+ voting_active: false
\ No newline at end of file
diff --git a/test/yaml/imu_mocap_fusion/sensor_imu_3d.yaml b/test/yaml/imu_mocap_fusion/sensor_imu_3d.yaml
index ac53d7de4e9205ee53f6da758fa1795265d7dff6..355b445f8804ba6989e7403cd0dded578915e798 100644
--- a/test/yaml/imu_mocap_fusion/sensor_imu_3d.yaml
+++ b/test/yaml/imu_mocap_fusion/sensor_imu_3d.yaml
@@ -1,6 +1,6 @@
name: "cool sensor imu"
plugin: imu
-type: SensorImu
+type: SensorImu3d
states:
P:
diff --git a/test/yaml/imu_mocap_fusion/sensor_pose_3d.yaml b/test/yaml/imu_mocap_fusion/sensor_pose_3d.yaml
index 39acfcd702e3de7966441d9ab3be47c7ab755dab..55a1253cfe4ba81da57586230b0519321870dc6b 100644
--- a/test/yaml/imu_mocap_fusion/sensor_pose_3d.yaml
+++ b/test/yaml/imu_mocap_fusion/sensor_pose_3d.yaml
@@ -12,5 +12,4 @@ states:
state: [0, 0, 0, 1]
dynamic: false
-std_p: 0.01
-std_o: 0.01
+std_noise: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01]
\ No newline at end of file
diff --git a/test/yaml/problem_gtest_imu_2d_static_init.yaml b/test/yaml/problem_gtest_imu_2d_static_init.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..d92a98b8a90a4cc44c7f9f19416bab8a926117b7
--- /dev/null
+++ b/test/yaml/problem_gtest_imu_2d_static_init.yaml
@@ -0,0 +1,23 @@
+problem:
+ dimension: 2
+ first_frame:
+ P:
+ state: [0,0]
+ mode: "factor"
+ noise_std: [.001, .001]
+ O:
+ state: [0]
+ mode: "factor"
+ noise_std: [.001]
+ V:
+ state: [0,0]
+ mode: "factor"
+ noise_std: [.001, .001]
+ tree_manager:
+ type: "None"
+
+sensors:
+ - follow: sensor_imu_2d.yaml
+
+processors:
+ - follow: processor_imu_2d_static_init.yaml
\ No newline at end of file
diff --git a/test/yaml/problem_gtest_imu_3d_static_init.yaml b/test/yaml/problem_gtest_imu_3d_static_init.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..d25288ebcc5ed8bb568c46576e52fa0f02e27285
--- /dev/null
+++ b/test/yaml/problem_gtest_imu_3d_static_init.yaml
@@ -0,0 +1,23 @@
+problem:
+ dimension: 3
+ first_frame:
+ P:
+ state: [0, 0, 0]
+ mode: "factor"
+ noise_std: [.001, .001]
+ O:
+ state: [0, 0, 0, 1]
+ mode: "factor"
+ noise_std: [.001, .001, .001]
+ V:
+ state: [0, 0, 0]
+ mode: "factor"
+ noise_std: [.001, .001, .001]
+ tree_manager:
+ type: "None"
+
+sensors:
+ - follow: sensor_imu_3d.yaml
+
+processors:
+ - follow: processor_imu_3d_static_init.yaml
\ No newline at end of file
diff --git a/test/yaml/processor_imu_2d_static_init.yaml b/test/yaml/processor_imu_2d_static_init.yaml
index 10aa1163e0b7dcfe0326d2b15fc3ee74e208d24d..0bc741fcab19bed2f140d0bc8f32a211c651d12f 100644
--- a/test/yaml/processor_imu_2d_static_init.yaml
+++ b/test/yaml/processor_imu_2d_static_init.yaml
@@ -1,6 +1,7 @@
name: "cool processor odom"
plugin: imu
type: ProcessorImu2d
+sensor_name: "cool sensor imu"
time_tolerance: 0.0025 # Time tolerance for joining KFs
unmeasured_perturbation_std: 0.0001
diff --git a/test/yaml/processor_imu_3d_static_init.yaml b/test/yaml/processor_imu_3d_static_init.yaml
index c1b733c3b96b6ac23626cf63127f4b1dcc08d671..42e5310a2829b7e855f43ee00e7742831cf232ce 100644
--- a/test/yaml/processor_imu_3d_static_init.yaml
+++ b/test/yaml/processor_imu_3d_static_init.yaml
@@ -1,6 +1,7 @@
name: "cool processor imu"
plugin: imu
type: ProcessorImu3d
+sensor_name: "cool sensor imu"
time_tolerance: 0.0025 # Time tolerance for joining KFs
unmeasured_perturbation_std: 0.0001