Merge branch '19-gtest-imu-and-mocap' into 'devel'

Resolve "gtest imu and mocap"

Closes #19

See merge request !29

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})
+
 wolf_add_gtest(gtest_factor_compass_3d gtest_factor_compass_3d.cpp)
 target_link_libraries(gtest_factor_compass_3d ${PLUGIN_NAME} ${wolf_LIBRARY})
 

test/gtest_imu_mocap_fusion.cpp

+/**
+ * \file gtest_imu_mocap_fusion.cpp
+ *
+ *  Created on: Feb 25, 2020
+ *      \author: mfourmy
+ */
+
+#include <fstream>
+
+#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;
+
+const Vector3d zero3 = Vector3d::Zero();
+const double dt = 0.001;
+
+
+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;
+
+    void SetUp() override
+    {
+        std::string wolf_root = _WOLF_IMU_ROOT_DIR;
+
+        //////////////////////
+        // 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 amp_a = amp_p.array() * freq_lin.array().square();
+        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();
+
+        // P = cos(freq*t) + cst
+        // V = -freq*sin(freq*t)
+        // 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);
+        
+        // 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, 0.0005);  // if mocap used
+
+        // 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 << 0,0,0, 0,0,0,1;
+        sensor_pose_ = problem_->installSensor("SensorPose", "pose", extr, intr_sp);
+        auto params_proc = std::make_shared<ParamsProcessorPose>();
+        params_proc->time_tolerance = 0.0005;
+        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 proc_base = problem_->installProcessor("ProcessorImu", "Imu pre-integrator", sensor_imu_, prc_imu_params);
+        ProcessorImuPtr proc_imu = std::static_pointer_cast<ProcessorImu>(proc_base);
+        proc_imu->setOrigin(KF1);
+        Matrix6d cov_imu = sensor_imu_->getNoiseCov();
+        // sensor_imu_->fixIntrinsics();
+        sensor_imu_->unfixIntrinsics();
+
+        // 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::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;
+
+        double t = 0;
+        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());
+
+            // imu measurements (BETTER HERE)
+            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_;
+            // 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();
+            sensor_imu_->unfixIntrinsics();
+            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;
+        }
+
+        // file_gtr.close();
+
+    }
+
+    void TearDown() override{};
+};
+
+
+
+
+TEST_F(ImuMocapFusion_Test, check_tree)
+{
+    ASSERT_TRUE(problem_->check(0));
+}
+
+TEST_F(ImuMocapFusion_Test, solve)
+{
+
+    // 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();
+
+    ASSERT_MATRIX_APPROX(sensor_pose_->getP()->getState(), b_p_bm_, 1e-6);
+    ASSERT_MATRIX_APPROX(sensor_pose_->getO()->getState(), b_q_m_.coeffs(), 1e-6);
+    ASSERT_MATRIX_APPROX(sensor_imu_->getIntrinsic()->getState(), (Vector6d() << ba_, bw_).finished(), 1e-4);
+}
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
\ No newline at end of file