diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index a8cf0ab798de32fa1901d03d37e307295fa2c78b..f05894d954d230b2168c35243162417df664b678 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -28,6 +28,9 @@ target_link_libraries(gtest_processor_imu_jacobians ${PLUGIN_NAME} ${wolf_LIBRAR wolf_add_gtest(gtest_feature_imu gtest_feature_imu.cpp) target_link_libraries(gtest_feature_imu ${PLUGIN_NAME} ${wolf_LIBRARY}) +wolf_add_gtest(gtest_imu_mocap_fusion gtest_imu_mocap_fusion.cpp) +target_link_libraries(gtest_imu_mocap_fusion ${PLUGIN_NAME} ${wolf_LIBRARY}) + # Has been excluded from tests for god knows how long, so thing is broken. # Maybe call an archeologist to fix this thing? # wolf_add_gtest(gtest_factor_imu gtest_factor_imu.cpp) diff --git a/test/gtest_imu_mocap_fusion.cpp b/test/gtest_imu_mocap_fusion.cpp new file mode 100644 index 0000000000000000000000000000000000000000..956a87ba787d697415ffc27ea238b1f351fc33e3 --- /dev/null +++ b/test/gtest_imu_mocap_fusion.cpp @@ -0,0 +1,185 @@ +/** + * \file gtest_imu_mocap_fusion.cpp + * + * Created on: Feb 25, 2020 + * \author: mfourmy + */ + +#include "core/utils/utils_gtest.h" +#include "core/ceres_wrapper/solver_ceres.h" +#include "core/capture/capture_base.h" +#include "core/capture/capture_pose.h" +#include "core/sensor/sensor_pose.h" +#include "core/processor/processor_pose.h" +#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.h" + +#include "Eigen/Dense" +#include <Eigen/SparseQR> +#include <Eigen/OrderingMethods> + + +using namespace Eigen; +using namespace wolf; +using std::cout; +using std::endl; + +const Vector3d zero3 = Vector3d::Zero(); +const double dt = 0.0001; +const Vector3d freq = Vector3d::Ones(); + + +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_; + + void SetUp() override + { + std::string wolf_root = _WOLF_IMU_ROOT_DIR; + + ////////////////////// + // simulate trajectory + ////////////////////// + + // biases and extrinsics + ba_ = Vector3d::Zero(); + bw_ = Vector3d::Zero(); + b_p_bm_ = Vector3d::Zero(); + b_q_m_ = Quaterniond::Identity(); + + // initialize state + Vector3d w_p_wb = Vector3d::Zero(); + Quaterniond w_q_b = Quaterniond::Identity(); + Vector3d w_v_wb = freq.array(); // *(1,1,1) + + // Problem and solver_ + problem_ = Problem::create("PO", 3); + solver_ = std::make_shared<SolverCeres>(problem_); + solver_->getSolverOptions().max_num_iterations = 500; + + + // 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; + Vector7d extr; extr << b_p_bm_, b_q_m_.coeffs(); + sensor_pose_ = problem_->installSensor("SensorPose", "pose", extr, intr_sp); + auto params_proc = std::make_shared<ParamsProcessorPose>(); + auto proc_pose = problem_->installProcessor("ProcessorPose", "pose", sensor_pose_, params_proc); + // somehow by default the sensor extrinsics is fixed + sensor_pose_->unfixExtrinsics(); + // sensor_pose_->fixExtrinsics(); + Matrix6d cov_pose = sensor_pose_->getNoiseCov(); + + // IMU sensor + Vector7d imu_extr = (Vector7d() << 0,0,0, 0,0,0,1).finished(); + ParamsSensorImuPtr sen_imu_params = std::make_shared<ParamsSensorImu>(); + sen_imu_params->a_noise = 0.01; + sen_imu_params->w_noise = 0.01; + sen_imu_params->ab_rate_stdev = 0.00001; + sen_imu_params->wb_rate_stdev = 0.00001; + sensor_imu_ = problem_->installSensor("SensorImu", "Main Imu", imu_extr, sen_imu_params); + ParamsProcessorImuPtr prc_imu_params = std::make_shared<ParamsProcessorImu>(); + prc_imu_params->max_time_span = 0.199999; + prc_imu_params->max_buff_length = 1000000000; + prc_imu_params->dist_traveled = 10000000000; + prc_imu_params->angle_turned = 1000000000; + prc_imu_params->voting_active = true; + auto processor_ptr = problem_->installProcessor("ProcessorImu", "Imu pre-integrator", sensor_imu_, prc_imu_params); + Matrix6d cov_imu = sensor_imu_->getNoiseCov(); + sensor_imu_->fixIntrinsics(); + + // 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); + + int traj_size = 10001; + for (int i=0; i < traj_size; i++){ + double t = i*dt; + Vector3d w_p_wb_sin = Eigen::sin((freq*t).array()); + Vector3d w_omg_b = freq.array()* Eigen::cos((freq*t).array()); + Vector3d w_a_wb = -freq.array().square()*Eigen::sin((freq*t).array()); + + // integrate simulated traj + w_p_wb = w_p_wb + w_v_wb*dt + 0.5*w_a_wb*dt*dt; + w_v_wb = w_v_wb + w_a_wb*dt; + w_q_b = wolf::exp_q(w_omg_b*dt) * w_q_b; + + // imu measurements + Vector3d acc_meas = w_q_b.conjugate()*(w_a_wb - wolf::gravity()) + ba_; + Vector3d omg_meas = w_q_b.conjugate()*w_omg_b + bw_; + // 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_; + + // process data + 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(); + 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(); + + if ((i%1000) == 0){ + std::cout << "" << std::endl; + std::cout << w_p_wb.transpose() << std::endl; + std::cout << w_p_wb_sin.transpose() << std::endl; + } + } + + } + + void TearDown() override{}; +}; + + + + +TEST_F(ImuMocapFusion_Test, check_tree) +{ + ASSERT_TRUE(problem_->check(0)); +} + +TEST_F(ImuMocapFusion_Test, solve) +{ + problem_->print(4, 1, 1, 1); + problem_->perturb(); + std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); + std::cout << report << std::endl; + problem_->print(4, 1, 1, 1); + + + SensorPosePtr sp = std::static_pointer_cast<SensorPose>(sensor_pose_); + ASSERT_MATRIX_APPROX(sp->getP()->getState(), b_p_bm_, 1e-6); + ASSERT_MATRIX_APPROX(sp->getO()->getState(), b_q_m_.coeffs(), 1e-6); +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} \ No newline at end of file