Added motion bias update test

test/CMakeLists.txt

@@ -32,3 +32,6 @@ target_link_libraries(gtest_feature_imu ${PLUGIN_NAME} ${wolf_LIBRARY})
 # Maybe call an archeologist to fix this thing?
 # wolf_add_gtest(gtest_factor_imu gtest_factor_imu.cpp)
 # target_link_libraries(gtest_factor_imu ${PLUGIN_NAME} ${wolf_LIBRARY})
+
+wolf_add_gtest(gtest_processor_motion_intrinsics_update gtest_processor_motion_intrinsics_update.cpp)
+target_link_libraries(gtest_processor_motion_intrinsics_update ${PLUGIN_NAME} ${wolf_LIBRARY})

test/gtest_processor_motion_intrinsics_update.cpp

+#include <fstream>
+
+#include "core/utils/utils_gtest.h"
+
+#include "core/common/wolf.h"
+#include "core/problem/problem.h"
+#include <core/ceres_wrapper/solver_ceres.h>
+#include "imu/sensor/sensor_imu.h"
+#include "imu/processor/processor_imu.h"
+#include "imu/capture/capture_imu.h"
+
+using namespace wolf;
+using namespace Eigen;
+
+
+
+/**
+Simplified test rolling along x axis only:
+Delta_x = 2*pi*R*(ticks/ticks_per_wheel_revolution)
+
+If we take ticks_per_wheel_revolution = 2*pi, even simpler formulat:
+Delta_x = R*ticks
+
+**/
+
+Vector10d integrateX(double ax, double dt){
+    Vector10d x; x << 0.5*ax*pow(dt,2),0,0, 0,0,0,1, ax*dt,0,0;
+    return x; 
+}
+
+class ProcessorImuTest : public testing::Test
+{
+    public:
+        ParamsSensorImuPtr      intr_;
+        SensorImuPtr          sensor_;
+        ParamsProcessorImuPtr params_;
+        ProcessorImuPtr       processor_;
+        ProblemPtr                  problem_;
+        SolverCeresPtr solver_;
+        FrameBasePtr KF0_;
+        double radius;
+
+        void SetUp() override
+        {
+            problem_ = Problem::create("POV", 3);
+            solver_ = std::make_shared<SolverCeres>(problem_);
+
+            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()});
+
+            // ground truth of wheels radius
+            radius = 1;
+            // make a sensor first
+            intr_ = std::make_shared<ParamsSensorImu>();
+            intr_->a_noise = 0.001;
+            intr_->w_noise = 0.001;
+            intr_->ab_initial_stdev = 0.001; 
+            intr_->wb_initial_stdev = 0.001;
+            intr_->ab_rate_stdev = 0.001;
+            intr_->wb_rate_stdev = 0.001;
+            
+            Vector7d extr; extr << 0,0,0, 0,0,0,1;
+            sensor_ = std::static_pointer_cast<SensorImu>(problem_->installSensor("SensorImu", "sensor imu", extr, intr_));
+
+            // params and processor
+            params_ = std::make_shared<ParamsProcessorImu>();
+            params_->voting_active = true;
+            params_->voting_aux_active = false;
+            params_->max_time_span = 0.9999;  // create one KF every second
+            params_->max_buff_length = 1000000000;
+            params_->dist_traveled = 100000000000;
+            params_->angle_turned = 10000000000;
+            
+            params_->unmeasured_perturbation_std = 1e-4;
+            params_->time_tolerance = 0.0025;
+            processor_ = std::static_pointer_cast<ProcessorImu>(problem_->installProcessor("ProcessorImu", "processor imu", sensor_, params_));
+
+            KF0_ = problem_->setPriorFactor(x_origin, std_origin, 0, 0.01);
+
+            Vector6d bias; bias << 0.42,0,0, 0,0,0;
+            // Vector6d bias; bias << 0.0,0,0, 0.42,0,0;
+            sensor_->getIntrinsic(0)->setState(bias);
+
+            processor_->setOrigin(KF0_);
+        }
+
+        void TearDown() override{}
+
+};
+
+TEST_F(ProcessorImuTest, test1)
+{
+    Matrix6d data_cov; data_cov. setIdentity();
+    // Advances at constant speed on both wheels
+    double AX = 0;
+
+    problem_->print(4,true,true,true);
+    CaptureMotionPtr CM;
+
+    CM = std::static_pointer_cast<CaptureMotion>(KF0_->getCaptureOf(sensor_));
+    std::cout << "bias preint before" << CM->getTimeStamp() << ": " << CM->getCalibrationPreint().transpose() << " , biais est: " << sensor_->getIntrinsic(CM->getTimeStamp())->getState().transpose() << std::endl;
+    solver_->solve();
+    std::cout << "bias preint after" << CM->getTimeStamp() << ": " << CM->getCalibrationPreint().transpose() << " , biais est: " << sensor_->getIntrinsic(CM->getTimeStamp())->getState().transpose() << std::endl;
+
+
+    Vector6d data; data << -wolf::gravity(), 0,0,0; data(0) = AX;
+    Vector10d state_pred; state_pred << 0,0,0, 0,0,0,1, 0,0,0;
+    auto C1 = std::make_shared<CaptureImu>(1, sensor_, data, data_cov, KF0_->getCaptureList().front()); C1->process();
+    FrameBasePtr KF1 = problem_->getTrajectory()->getLastFrame();
+    KF1->setState(integrateX(AX,1)); KF1->fix();
+    CM = std::static_pointer_cast<CaptureMotion>(KF1->getCaptureOf(sensor_));
+    std::cout << "bias preint before" << CM->getTimeStamp() << ": " << CM->getCalibrationPreint().transpose() << " , biais est: " << sensor_->getIntrinsic(CM->getTimeStamp())->getState().transpose() << std::endl;
+    solver_->solve();
+    std::cout << "bias preint after" << CM->getTimeStamp() << ": " << CM->getCalibrationPreint().transpose() << " , biais est: " << sensor_->getIntrinsic(CM->getTimeStamp())->getState().transpose() << std::endl;
+    // problem_->print(4,true,true,true);
+
+    auto C2 = std::make_shared<CaptureImu>(2, sensor_, data, data_cov, KF0_->getCaptureList().front()); C2->process();
+    FrameBasePtr KF2 = problem_->getTrajectory()->getLastFrame();
+    KF2->setState(integrateX(AX,2)); KF2->fix();
+    CM = std::static_pointer_cast<CaptureMotion>(KF2->getCaptureOf(sensor_));
+    std::cout << "bias preint before" << CM->getTimeStamp() << ": " << CM->getCalibrationPreint().transpose() << " , biais est: " << sensor_->getIntrinsic(CM->getTimeStamp())->getState().transpose() << std::endl;
+    solver_->solve();
+    std::cout << "bias preint after" << CM->getTimeStamp() << ": " << CM->getCalibrationPreint().transpose() << " , biais est: " << sensor_->getIntrinsic(CM->getTimeStamp())->getState().transpose() << std::endl;
+    // problem_->print(4,true,true,true);
+
+    auto C3 = std::make_shared<CaptureImu>(3, sensor_, data, data_cov, KF0_->getCaptureList().front()); C3->process();
+    FrameBasePtr KF3 = problem_->getTrajectory()->getLastFrame();
+    KF3->setState(integrateX(AX,3)); KF3->fix();
+    CM = std::static_pointer_cast<CaptureMotion>(KF3->getCaptureOf(sensor_));
+    std::cout << "bias preint before" << CM->getTimeStamp() << ": " << CM->getCalibrationPreint().transpose() << " , biais est: " << sensor_->getIntrinsic(CM->getTimeStamp())->getState().transpose() << std::endl;
+    solver_->solve();
+    std::cout << "bias preint after" << CM->getTimeStamp() << ": " << CM->getCalibrationPreint().transpose() << " , biais est: " << sensor_->getIntrinsic(CM->getTimeStamp())->getState().transpose() << std::endl;
+    // problem_->print(4,true,true,true);
+
+    auto C4 = std::make_shared<CaptureImu>(4, sensor_, data, data_cov, KF0_->getCaptureList().front()); C4->process();
+    FrameBasePtr KF4 = problem_->getTrajectory()->getLastFrame();
+    KF4->setState(integrateX(AX,4)); KF4->fix();
+    CM = std::static_pointer_cast<CaptureMotion>(KF4->getCaptureOf(sensor_));
+    std::cout << "bias preint before" << CM->getTimeStamp() << ": " << CM->getCalibrationPreint().transpose() << " , biais est: " << sensor_->getIntrinsic(CM->getTimeStamp())->getState().transpose() << std::endl;
+    solver_->solve();
+    std::cout << "bias preint after" << CM->getTimeStamp() << ": " << CM->getCalibrationPreint().transpose() << " , biais est: " << sensor_->getIntrinsic(CM->getTimeStamp())->getState().transpose() << std::endl;
+    // problem_->print(4,true,true,true);
+
+    for(auto kf: problem_->getTrajectory()->getFrameMap()){
+        CaptureMotionPtr cap = std::static_pointer_cast<CaptureMotion>(kf.second->getCaptureOf(sensor_));
+    }
+
+
+}
+
+
+
+TEST_F(ProcessorImuTest, getState)
+{
+    Matrix6d data_cov; data_cov. setIdentity();
+    // Advances at constant speed on both wheels
+    double AX = 0;
+
+    problem_->print(4,true,true,true);
+    Vector6d data; data << -wolf::gravity(), 0,0,0; data(0) = AX;
+    CaptureImuPtr C;
+
+    TimeStamp t(0);
+    double dt(0.1);
+    int nb_kf = 1;
+
+
+    std::fstream file_est; 
+    file_est.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/PhdTests/est.csv", std::fstream::out); 
+    std::string header_est = "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz,bax,bax_preint\n";
+    file_est << header_est;
+
+
+    while (t < 4){
+        auto C = std::make_shared<CaptureImu>(t, sensor_, data, data_cov, KF0_->getCaptureList().front()); 
+        C->process();
+        VectorComposite state = problem_->getState(t);
+        VectorXd calib = sensor_->getIntrinsic(t)->getState();
+        VectorXd calib_preint = processor_->getLast()->getCalibrationPreint();
+        std::cout << "calib size: " << calib.size() << std::endl;
+        std::cout << "calib_preint size: " << calib_preint.size() << std::endl;
+        file_est << t << ","
+            << state['P'](0) << ","
+            << state['P'](1) << ","
+            << state['P'](2) << ","
+            << state['O'](0) << ","
+            << state['O'](1) << ","
+            << state['O'](2) << ","
+            << state['O'](3) << ","
+            << state['V'](0) << ","
+            << state['V'](1) << ","
+            << state['V'](2) << ","
+            << calib(0) << ","
+            << calib_preint(0) << "\n";
+
+
+
+        if (problem_->getTrajectory()->getFrameMap().size() > nb_kf){
+            FrameBasePtr KF = problem_->getTrajectory()->getLastFrame();
+            KF->setState(integrateX(AX, KF->getTimeStamp().get())); 
+            KF->fix();
+
+            solver_->solve();
+            problem_->print(4, true, true, true);
+            nb_kf++;
+
+        }
+
+        t += dt;
+    }
+
+}
+
+
+
+
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}