Simulation of imu + mocap implemented, extr pose not observable yet

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)

test/gtest_imu_mocap_fusion.cpp

+/**
+ * \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