Merge branch 'motion-intrinsics-update-gtest' into 'devel'

Motion intrinsics update gtest

See merge request !26

.gitignore

@@ -33,3 +33,4 @@ src/examples/map_apriltag_save.yaml
 build_release/
 
 IMU.found
+est.csv

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 "imu/internal/config.h"
+#include "core/utils/utils_gtest.h"
+#include "core/utils/logging.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_->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_));
+    }
+
+
+}
+
+
+/**
+ *  SET TO TRUE  TO PRODUCE CSV FILE
+ *  SET TO FALSE TO STOP PRODUCING CSV FILE
+ */
+#define WRITE_CSV_FILE  false
+
+TEST_F(ProcessorImuTest, getState)
+{
+    Matrix6d data_cov; data_cov. setIdentity();
+    // Advances at constant acceleration
+    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;
+
+#if WRITE_CSV_FILE
+    std::fstream file_est; 
+    file_est.open("./est.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 = "t;px;vx;bax_est;bax_preint\n";
+    file_est << header_est;
+#endif
+
+    while (t < 4){
+        auto C = std::make_shared<CaptureImu>(t, sensor_, data, data_cov, KF0_->getCaptureList().front()); 
+        C->process();
+
+
+        std::cout << "t " << t << "; cap id " << sensor_->findLastCaptureBefore(t) << "; cap ts " << sensor_->findLastCaptureBefore(t)->getTimeStamp() << std::endl;
+
+
+#if WRITE_CSV_FILE
+        // pre-solve print to CSV
+        VectorComposite state = problem_->getState(t);
+        VectorXd calib_estim = sensor_->getIntrinsic(t)->getState();
+        VectorXd calib_preint = processor_->getLast()->getCalibrationPreint();
+        file_est << std::fixed << t << ";"
+            << state['P'](0) << ";"
+            << state['V'](0) << ";"
+            << calib_estim(0) << ";"
+            << calib_preint(0) << "\n";
+#endif
+
+
+        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 = problem_->getTrajectory()->getFrameMap().size();
+
+#if WRITE_CSV_FILE
+            // post-solve print to CSV with time-stamp shifted by dt/2 to separate from pre-solve  result
+            state = problem_->getState(t);
+            calib_estim = sensor_->getIntrinsic(t)->getState();
+            calib_preint = processor_->getLast()->getCalibrationPreint();
+            file_est << std::fixed << t+dt/2 << ";"
+                << state['P'](0) << ";"
+                << state['V'](0) << ";"
+                << calib_estim(0) << ";"
+                << calib_preint(0) << "\n";
+#endif
+        }
+
+        t += dt;
+    }
+
+#if WRITE_CSV_FILE
+    file_est.close();
+#endif
+
+    /** assert final results
+     *
+     * t            0   .9  1   1.9 2   3
+     * BIAS         .4  .4  0   0   0   0
+     * BIAS PREINT  .4  .4  .4  .4  0   0
+     * X            0   !0  0   0   0   0
+     * V            0   !0  0   0   0   0
+     *
+     * though we only test at KFs so t = { 0, 1, 2, 3 }
+     */
+
+    for (auto F : *problem_->getTrajectory() )
+    {
+        auto ts = F->getTimeStamp();
+
+        double bias = (t == 0 ? 0.42 : 0.0);
+        ASSERT_NEAR(F->getCaptureOf(sensor_)->getSensorIntrinsic()->getState()(0), bias, 1e-6);
+        ASSERT_NEAR(F->getP()->getState()(0),0.0,1e-6);
+        ASSERT_NEAR(F->getV()->getState()(0),0.0,1e-6);
+    }
+
+}
+
+
+
+
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}