Skip to content
Snippets Groups Projects
gtest_imu.cpp 62.72 KiB
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program.  If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
/*
 * gtest_Imu.cpp
 *
 *  Created on: Nov 14, 2017
 *      Author: jsola
 */

//Wolf
#include <core/ceres_wrapper/solver_ceres.h>

#include "imu/processor/processor_imu.h"
#include "imu/sensor/sensor_imu.h"

#include "imu/internal/config.h"

#include <core/common/wolf.h>
#include <core/sensor/sensor_odom_3d.h>
#include <core/processor/processor_odom_3d.h>
#include <core/utils/utils_gtest.h>
//#include <core/utils/logging.h>

#include "imu/math/imu_tools.h"

// make my life easier
using namespace Eigen;
using namespace wolf;
using std::shared_ptr;
using std::make_shared;
using std::static_pointer_cast;
using std::string;

class Process_Factor_Imu : public testing::Test
{
    public:
        // Wolf objects
        ProblemPtr          problem;
        SolverCeresPtr      solver;
        SensorImuPtr        sensor_imu;
        ProcessorImuPtr     processor_imu;
        CaptureImuPtr       capture_imu;
        FrameBasePtr        KF_0, KF_1;     // keyframes
        CaptureBasePtr      C_0 , C_1;      // base captures
        CaptureMotionPtr    CM_0, CM_1;     // motion captures

        // time
        TimeStamp           t0, t;
        double              dt, DT;
        int                 num_integrations;
        // initial state
        Vector3d            p0, v0;                             // initial pos and vel
        Quaterniond         q0, q;                              // initial and current orientations
        VectorXd            x0;                                 // initial state
        Matrix<double,9,9>  P0;                                 // initial state covariance
        VectorComposite     x0c;                                // initial state composite
        VectorComposite     s0c;                                // initial sigmas composite

        // bias
        Vector6d            bias_real, bias_preint, bias_null;  // real, pre-int and zero biases.
        Vector6d            bias_0, bias_1;                     // optimized bias at KF's 0 and 1

        // input
        Matrix<double, 6, Dynamic> motion;                      // Motion in Imu frame. Each column is a motion step. If just one column, then the number of steps is defined in num_integrations
        Matrix<double, 3, Dynamic> a, w;                        // True acc and angvel in Imu frame. Each column is a motion step. Used to create motion with `motion << a,w;`
        Vector6d            data;                               // Imu data. It's the motion with gravity and bias. See imu::motion2data().

        // Deltas and states (exact, integrated, corrected, solved, etc)
        VectorXd        D_exact,         x1_exact;          // exact or ground truth
        VectorXd        D_preint_imu,    x1_preint_imu;     // preintegrated with imu_tools
        VectorXd        D_corrected_imu, x1_corrected_imu;  // corrected with imu_tools
        VectorXd        D_preint,        x1_preint;         // preintegrated with processor_imu
        VectorXd        D_corrected,     x1_corrected;      // corrected with processor_imu
        VectorXd        D_optim,         x1_optim;          // optimized using factor_imu
        VectorXd        D_optim_imu,     x1_optim_imu;      // corrected with imu_tools using optimized bias
        VectorXd                         x0_optim;          // optimized using factor_imu

        // Trajectory buffer of correction Jacobians
        std::vector<MatrixXd> Buf_Jac_preint_prc;

        // Trajectory matrices
        MatrixXd Trj_D_exact, Trj_D_preint_imu, Trj_D_preint_prc, Trj_D_corrected_imu, Trj_D_corrected_prc;
        MatrixXd Trj_x_exact, Trj_x_preint_imu, Trj_x_preint_prc, Trj_x_corrected_imu, Trj_x_corrected_prc;

        // Delta correction Jacobian and step
        Matrix<double,9,6>  J_D_bias;                           // Jacobian of pre-integrated delta w
        Vector9d            step;

        // Flags for fixing/unfixing state blocks
        bool                p0_fixed, q0_fixed, v0_fixed;
        bool                p1_fixed, q1_fixed, v1_fixed;

        VectorXd getDeltaCorrected(CaptureMotionPtr cap, const VectorXd& bias_real)
        {
            const auto& motion = cap->getBuffer().back();
            VectorXd step = motion.jacobian_calib_ * (bias_real - cap->getCalibrationPreint());
            VectorXd delta_corrected = imu::plus(motion.delta_integr_, step);
            return delta_corrected;
        }

        void SetUp( ) override
        {
            string wolf_root = _WOLF_IMU_ROOT_DIR;

            //===================================== SETTING PROBLEM
            problem = Problem::create("POV", 3);

            // CERES WRAPPER
            solver = make_shared<SolverCeres>(problem);

            // SENSOR + PROCESSOR Imu
            SensorBasePtr       sensor = problem->installSensor   ("SensorImu", "Main Imu", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml");
            ProcessorBasePtr processor = problem->installProcessor("ProcessorImu", "Imu pre-integrator", "Main Imu", wolf_root + "/demos/processor_imu.yaml");
            sensor_imu    = static_pointer_cast<SensorImu>   (sensor);
            processor_imu = static_pointer_cast<ProcessorImu>(processor);

            dt = 0.01;
            processor_imu->setTimeTolerance(dt/2);

            // Some initializations
            bias_null   .setZero();
            x0          .resize(10);
            D_preint    .resize(10);
            D_corrected .resize(10);
            x1_optim    .resize(10);
            x1_optim_imu.resize(10);

            x0c = VectorComposite("POV", {Vector3d(0,0,0), Quaterniond::Identity().coeffs(), Vector3d(0,0,0)});
            s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)});
        }

        
        /* Integrate one step of acc and angVel motion, obtain Delta_preintegrated
         * Input:
         *   q: current orientation
         *   motion: [ax, ay, az, wx, wy, wz] as the true magnitudes in body frame
         *   bias_real: the real bias of the Imu
         *   bias_preint: the bias used for Delta pre-integration
         * Input/output
         *   Delta: the preintegrated delta
         *   J_D_b_ptr: a pointer to the Jacobian of Delta wrt bias. Defaults to nullptr.
         */
        static void integrateOneStep(const VectorXd& motion, const VectorXd& bias_real, const VectorXd& bias_preint, double dt, Quaterniond& q_real, VectorXd& Delta, Matrix<double, 9, 6>* J_D_b_ptr = nullptr)
        {
            VectorXd delta(10), data(6);
            VectorXd Delta_plus(10);
            Matrix<double, 9, 6> J_d_d, J_D_b, J_d_b;
            Matrix<double, 9, 9> J_D_D, J_D_d;

            data                = imu::motion2data(motion, q_real, bias_real);
            q_real              = q_real*exp_q(motion.tail(3)*dt);
            Vector6d body       = data - bias_preint;
            if (J_D_b_ptr == nullptr)
            {
                delta           = imu::body2delta(body, dt);
                Delta_plus      = imu::compose(Delta, delta, dt);
            }
            else
            {
                imu::body2delta(body, dt, delta, J_d_d);
                imu::compose(Delta, delta, dt, Delta_plus, J_D_D, J_D_d);
                J_D_b           = *J_D_b_ptr;
                J_d_b           = - J_d_d;
                J_D_b           = J_D_D*J_D_b + J_D_d*J_d_b;
                *J_D_b_ptr      = J_D_b;
            }
            Delta               = Delta_plus;
        }

        /* Integrate acc and angVel motion, obtain Delta_preintegrated
         * Input:
         *   N: number of steps
         *   q0: initial orientation
         *   motion: [ax, ay, az, wx, wy, wz] as the true magnitudes in body frame
         *   bias_real: the real bias of the Imu
         *   bias_preint: the bias used for Delta pre-integration
         * Output:
         *   return: the preintegrated delta
         */
        static VectorXd integrateDelta(int N, const Quaterniond& q0, const VectorXd& motion, const VectorXd& bias_real, const VectorXd& bias_preint, double dt)
        {
            VectorXd    Delta(10);
            Delta       = imu::identity();
            Quaterniond q(q0);
            for (int n = 0; n < N; n++)
            {
                integrateOneStep(motion, bias_real, bias_preint, dt, q, Delta);
            }
            return Delta;
        }

        /* Integrate acc and angVel motion, obtain Delta_preintegrated
         * Input:
         *   N: number of steps
         *   q0: initial orientation
         *   motion: [ax, ay, az, wx, wy, wz] as the true magnitudes in body frame
         *   bias_real: the real bias of the Imu
         *   bias_preint: the bias used for Delta pre-integration
         * Output:
         *   J_D_b: the Jacobian of the preintegrated delta wrt the bias
         *   return: the preintegrated delta
         */
        static VectorXd integrateDelta(int N, const Quaterniond& q0, const VectorXd& motion, const VectorXd& bias_real, const VectorXd& bias_preint, double dt, Matrix<double, 9, 6>& J_D_b)
        {
            VectorXd    Delta(10);
            Quaterniond q;

            Delta   = imu::identity();
            J_D_b   .setZero();
            q       = q0;
            for (int n = 0; n < N; n++)
            {
                integrateOneStep(motion, bias_real, bias_preint, dt, q, Delta, &J_D_b);
            }
            return Delta;
        }

        /* Integrate acc and angVel motion, obtain Delta_preintegrated
         * Input:
         *   q0: initial orientation
         *   motion: Matrix with N columns [ax, ay, az, wx, wy, wz] with the true magnitudes in body frame
         *   bias_real: the real bias of the Imu
         *   bias_preint: the bias used for Delta pre-integration
         * Output:
         *   J_D_b: the Jacobian of the preintegrated delta wrt the bias
         *   return: the preintegrated delta
         */
        static VectorXd integrateDelta(const Quaterniond& q0, const MatrixXd& motion, const VectorXd& bias_real, const VectorXd& bias_preint, double dt, Matrix<double, 9, 6>& J_D_b)
        {
            VectorXd    Delta(10);
            Quaterniond q;

            Delta   = imu::identity();
            J_D_b   .setZero();
            q       = q0;
            for (int n = 0; n < motion.cols(); n++)
            {
                integrateOneStep(motion.col(n), bias_real, bias_preint, dt, q, Delta, &J_D_b);
            }
            return Delta;
        }

        /* Integrate acc and angVel motion, obtain Delta_preintegrated
         * Input:
         *   q0: initial orientation
         *   motion: Matrix with N columns [ax, ay, az, wx, wy, wz] with the true magnitudes in body frame
         *   bias_real: the real bias of the Imu
         *   bias_preint: the bias used for Delta pre-integration
         * Output:
         *   J_D_b: the Jacobian of the preintegrated delta wrt the bias
         *   return: the preintegrated delta
         */
        static MotionBuffer integrateDeltaTrajectory(const Quaterniond& q0, const MatrixXd& motion, const VectorXd& bias_real, const VectorXd& bias_preint, double dt, Matrix<double, 9, 6>& J_D_b)
        {
            MotionBuffer trajectory;
            VectorXd    Delta(10);
            MatrixXd    M9(9,9), M6(6,6), J9(9,9), J96(9,6), V10(10,1), V6(6,1);
            Quaterniond q;

            Delta   = imu::identity();
            J_D_b   .setZero();
            q       = q0;
            TimeStamp t(0);
            trajectory.emplace_back(t, Vector6d::Zero(), M6, VectorXd::Zero(10), M9, imu::identity(), M9, J9, J9, MatrixXd::Zero(9,6));
            for (int n = 0; n < motion.cols(); n++)
            {
                t += dt;
                integrateOneStep(motion.col(n), bias_real, bias_preint, dt, q, Delta, &J_D_b);
                trajectory.emplace_back(t, motion.col(n), M6, V10, M9, Delta, M9, J9, J9, J_D_b);
            }
            return trajectory;
        }

        MotionBuffer integrateWithProcessor(int N, const TimeStamp& t0, const Quaterniond q0, const MatrixXd& motion, const VectorXd& bias_real, const VectorXd& bias_preint, double dt, VectorXd& D_preint, VectorXd& D_corrected)
        {
            Vector6d      motion_now;
            data        = imu::motion2data(motion.col(0), q0, bias_real);
            capture_imu = make_shared<CaptureImu>(t0, sensor_imu, data, sensor_imu->getNoiseCov());
            q           = q0;
            t           = t0;
            for (int i= 0; i < N; i++)
            {
                t   += dt;
                motion_now = motion.cols() == 1
                                ? motion
                                : motion.col(i);
                data = imu::motion2data(motion_now, q, bias_real);
                w    = motion_now.tail<3>();
                q    = q * exp_q(w*dt);

                capture_imu->setTimeStamp(t);
                capture_imu->setData(data);

                sensor_imu->process(capture_imu);

                D_preint    = processor_imu->getLast()->getDeltaPreint();
                D_corrected = getDeltaCorrected(processor_imu->getLast(), bias_real);
            }
            return processor_imu->getBuffer();
        }

        // Initial configuration of variables
        virtual bool configureAll()
        {
            // initial state
            q0      .normalize();
            x0     << p0, q0.coeffs(), v0;
            P0      .setIdentity() * 0.01;

            // motion
            if (motion.cols() == 0)
            {
                motion.resize(6,a.cols());
                motion << a, w;
            }
            else
            {
                // if motion has any column at all, then it is already initialized in TEST_F(...) and we do nothing.
            }
            if (motion.cols() != 1)
            {
                // if motion has more than 1 col, make num_integrations consistent with nbr of cols, just for consistency
                num_integrations = motion.cols();
            }

            // total run time
            DT      = num_integrations * dt;

            // wolf objects
            WOLF_INFO("x0c: ", x0c);
            WOLF_INFO("s0c: ", s0c);
            KF_0    = problem->setPriorFactor(x0c, s0c, t0);
            processor_imu->setOrigin(KF_0);
            WOLF_INFO("prior set");

            C_0     = processor_imu->getOrigin();

            processor_imu->getLast()->setCalibrationPreint(bias_preint);

            return true;
        }

        // Integrate using all methods
        virtual void integrateAll()
        {
            // ===================================== INTEGRATE EXACTLY WITH Imu_TOOLS with no bias at all
            if (motion.cols() == 1)
                D_exact = integrateDelta(num_integrations, q0, motion, bias_null, bias_null, dt);
            else
                D_exact = integrateDelta(q0, motion, bias_null, bias_null, dt, J_D_bias);
            x1_exact = imu::composeOverState(x0, D_exact, DT );

            // ===================================== INTEGRATE USING Imu_TOOLS
            // pre-integrate
            if (motion.cols() == 1)
                D_preint_imu = integrateDelta(num_integrations, q0, motion, bias_real, bias_preint, dt, J_D_bias);
            else
                D_preint_imu = integrateDelta(q0, motion, bias_real, bias_preint, dt, J_D_bias);

            // correct perturbated
            step             = J_D_bias * (bias_real - bias_preint);
            D_corrected_imu  = imu::plus(D_preint_imu, step);

            // compose states
            x1_preint_imu    = imu::composeOverState(x0, D_preint_imu    , DT );
            x1_corrected_imu = imu::composeOverState(x0, D_corrected_imu , DT );

            // ===================================== INTEGRATE USING PROCESSOR_Imu

            problem->print(3,0,1,0);
            integrateWithProcessor(num_integrations, t0, q0, motion, bias_real, bias_preint, dt, D_preint, D_corrected);

            // compose states
            x1_preint        = imu::composeOverState(x0, D_preint        , DT );
            x1_corrected     = imu::composeOverState(x0, D_corrected     , DT );
        }

        // Integrate Trajectories all methods
        virtual void integrateAllTrajectories()
        {
            SizeEigen cols = motion.cols() + 1;
            Trj_D_exact.resize(10,cols); Trj_D_preint_imu.resize(10,cols); Trj_D_preint_prc.resize(10,cols); Trj_D_corrected_imu.resize(10,cols); Trj_D_corrected_prc.resize(10,cols);
            Trj_x_exact.resize(10,cols); Trj_x_preint_imu.resize(10,cols); Trj_x_preint_prc.resize(10,cols); Trj_x_corrected_imu.resize(10,cols); Trj_x_corrected_prc.resize(10,cols);

            // ===================================== INTEGRATE EXACTLY WITH Imu_TOOLS with no bias at all
            MotionBuffer Buf_exact = integrateDeltaTrajectory(q0, motion, bias_null, bias_null, dt, J_D_bias);

            // Build exact trajectories
            int col = 0;
            double Dt = 0;
            for (auto m : Buf_exact)
            {
                Trj_D_exact.col(col) = m.delta_integr_;
                Trj_x_exact.col(col) = imu::composeOverState(x0, m.delta_integr_, Dt );
                Dt += dt;
                col ++;
            }

            // set
            D_exact          = Trj_D_exact.col(cols-1);
            x1_exact         = Trj_x_exact.col(cols-1);

            // ===================================== INTEGRATE USING Imu_TOOLS
            // pre-integrate
            MotionBuffer Buf_preint_imu = integrateDeltaTrajectory(q0, motion, bias_real, bias_preint, dt, J_D_bias);

            // Build trajectories preintegrated and corrected with imu_tools
            col = 0;
            Dt = 0;
            for (auto m : Buf_preint_imu)
            {
                // preint
                Trj_D_preint_imu.col(col) = m.delta_integr_;
                Trj_x_preint_imu.col(col) = imu::composeOverState(x0, Trj_D_preint_imu.col(col).eval(), Dt );

                // corrected
                VectorXd step                = m.jacobian_calib_ * (bias_real - bias_preint);
                Trj_D_corrected_imu.col(col) = imu::plus(m.delta_integr_, step) ;
                Trj_x_corrected_imu.col(col) = imu::composeOverState(x0, Trj_D_corrected_imu.col(col).eval(), Dt );
                Dt += dt;
                col ++;
            }

            D_preint_imu     = Trj_D_preint_imu.col(cols-1);

            // correct perturbated
            step             = J_D_bias * (bias_real - bias_preint);
            D_corrected_imu  = imu::plus(D_preint_imu, step);

            // compose states
            x1_preint_imu    = imu::composeOverState(x0, D_preint_imu    , DT );
            x1_corrected_imu = imu::composeOverState(x0, D_corrected_imu , DT );

            // ===================================== INTEGRATE USING PROCESSOR_Imu

            MotionBuffer Buf_D_preint_prc = integrateWithProcessor(num_integrations, t0, q0, motion, bias_real, bias_preint, dt, D_preint, D_corrected);

            // Build trajectories preintegrated and corrected with processorImu
            Dt = 0;
            col = 0;
            Buf_Jac_preint_prc.clear();
            for (auto m : Buf_D_preint_prc)
            {
                // preint
                Trj_D_preint_prc.col(col) = m.delta_integr_;
                Trj_x_preint_prc.col(col) = imu::composeOverState(x0, Trj_D_preint_prc.col(col).eval(), Dt );

                // corrected
                VectorXd step                = m.jacobian_calib_ * (bias_real - bias_preint);
                Trj_D_corrected_prc.col(col) = imu::plus(m.delta_integr_, step) ;
                Trj_x_corrected_prc.col(col) = imu::composeOverState(x0, Trj_D_corrected_prc.col(col).eval(), Dt );

                Buf_Jac_preint_prc.push_back(m.jacobian_calib_);
                Dt += dt;
                col ++;
            }

            // compose states
            x1_preint        = imu::composeOverState(x0, D_preint        , DT );
            x1_corrected     = imu::composeOverState(x0, D_corrected     , DT );
        }

        // Set state_blocks status
        void setFixedBlocks()
        {
            // this sets each state block status fixed / unfixed
            KF_0->getP()->setFixed(p0_fixed);
            KF_0->getO()->setFixed(q0_fixed);
            KF_0->getV()->setFixed(v0_fixed);
            KF_1->getP()->setFixed(p1_fixed);
            KF_1->getO()->setFixed(q1_fixed);
            KF_1->getV()->setFixed(v1_fixed);
        }

        void setKfStates()
        {
            // This perturbs states to estimate around the exact value, then assigns to the keyframe
            // Perturbations are applied only if the state block is unfixed
            KF_0->setState(x0);
            KF_0->perturb();
            KF_1->setState(x1_exact);
            KF_1->perturb();
        }

        virtual void buildProblem()
        {
            // ===================================== SET KF in Wolf tree
            FrameBasePtr KF = problem->emplaceFrame(t, x1_exact);

            // ===================================== Imu CALLBACK
            problem->keyFrameCallback(KF, nullptr);

            // Process Imu for the callback to take effect
            data = Vector6d::Zero();
            capture_imu = make_shared<CaptureImu>(t+dt, sensor_imu, data, sensor_imu->getNoiseCov());
            sensor_imu->process(capture_imu);

            KF_1 = problem->getLastFrame();
            C_1  = KF_1->getCaptureList().front(); // front is Imu
            CM_1 = static_pointer_cast<CaptureMotion>(C_1);

            // ===================================== SET BOUNDARY CONDITIONS
            setFixedBlocks();
            setKfStates();
        }

        string solveProblem(SolverManager::ReportVerbosity verbose = SolverManager::ReportVerbosity::BRIEF)
        {
            string report   = solver->solve(verbose);

            bias_0          = C_0->getSensorIntrinsic()->getState();
            bias_1          = C_1->getSensorIntrinsic()->getState();

            // ===================================== GET INTEGRATED STATE WITH SOLVED BIAS
            // with processor
            x0_optim        = KF_0->getStateVector();
            D_optim         = getDeltaCorrected(CM_1, bias_0);
            x1_optim        = KF_1->getStateVector();

            // with imu_tools
            step            = J_D_bias * (bias_0 - bias_preint);
            D_optim_imu     = imu::plus(D_preint, step);
            x1_optim_imu    = imu::composeOverState(x0_optim, D_optim_imu, DT);

            return report;
        }

        string runAll(SolverManager::ReportVerbosity verbose)
        {
            configureAll();
            integrateAll();
            buildProblem();
            string report = solveProblem(verbose);
            return report;
        }

        void printAll(std::string report = "")
        {
            WOLF_TRACE(report);
            WOLF_TRACE("D_exact       : ", D_exact            .transpose() ); // exact delta integrated, with absolutely no bias
            WOLF_TRACE("D_preint      : ", D_preint           .transpose() ); // pre-integrated delta using processor
            WOLF_TRACE("D_preint_imu  : ", D_preint_imu       .transpose() ); // pre-integrated delta using imu_tools
            WOLF_TRACE("D_correct     : ", D_corrected        .transpose() ); // corrected delta using processor
            WOLF_TRACE("D_correct_imu : ", D_corrected_imu    .transpose() ); // corrected delta using imu_tools
            WOLF_TRACE("D_optim       : ", D_optim            .transpose() ); // corrected delta using optimum bias after solving using processor
            WOLF_TRACE("D_optim_imu   : ", D_optim_imu        .transpose() ); // corrected delta using optimum bias after solving using imu_tools

            WOLF_TRACE("bias real     : ", bias_real          .transpose() ); // real bias
            WOLF_TRACE("bias preint   : ", bias_preint        .transpose() ); // bias used during pre-integration
            WOLF_TRACE("bias optim 0  : ", bias_0             .transpose() ); // solved bias at KF_0
            WOLF_TRACE("bias optim 1  : ", bias_1             .transpose() ); // solved bias at KF_1

            // states at the end of integration, i.e., at KF_1
            WOLF_TRACE("X1_exact      : ", x1_exact           .transpose() ); // exact state
            WOLF_TRACE("X1_preint     : ", x1_preint          .transpose() ); // state using delta preintegrated by processor
            WOLF_TRACE("X1_preint_imu : ", x1_preint_imu      .transpose() ); // state using delta preintegrated by imu_tools
            WOLF_TRACE("X1_correct    : ", x1_corrected       .transpose() ); // corrected state vy processor
            WOLF_TRACE("X1_correct_imu: ", x1_corrected_imu   .transpose() ); // corrected state by imu_tools
            WOLF_TRACE("X1_optim      : ", x1_optim           .transpose() ); // optimal state after solving using processor
            WOLF_TRACE("X1_optim_imu  : ", x1_optim_imu       .transpose() ); // optimal state after solving using imu_tools
            WOLF_TRACE("err1_optim    : ", (x1_optim-x1_exact).transpose() ); // error of optimal state corrected by imu_tools w.r.t. exact state
            WOLF_TRACE("err1_optim_imu: ", (x1_optim_imu-x1_exact).transpose() ); // error of optimal state corrected by imu_tools w.r.t. exact state
            WOLF_TRACE("X0_optim      : ", x0_optim           .transpose() ); // optimal state after solving using processor
        }

        virtual void assertAll()
        {
            // check delta and state integrals
            ASSERT_MATRIX_APPROX(D_preint           , D_preint_imu      , 1e-8 );
            ASSERT_MATRIX_APPROX(D_corrected        , D_corrected_imu   , 1e-8 );
            ASSERT_MATRIX_APPROX(D_corrected_imu    , D_exact           , 1e-5 );
            ASSERT_MATRIX_APPROX(D_corrected        , D_exact           , 1e-5 );
            ASSERT_MATRIX_APPROX(x1_corrected_imu   , x1_exact          , 1e-5 );
            ASSERT_MATRIX_APPROX(x1_corrected       , x1_exact          , 1e-5 );

            // check optimal solutions
            ASSERT_MATRIX_APPROX(x0_optim           , x0        , 1e-5 );
            ASSERT_NEAR(x0_optim.segment(3,4).norm(), 1.0       , 1e-8 );
            ASSERT_MATRIX_APPROX(bias_0             , bias_real , 1e-4 );
            ASSERT_MATRIX_APPROX(bias_1             , bias_real , 1e-4 );
            ASSERT_MATRIX_APPROX(D_optim            , D_exact   , 1e-5 );
            ASSERT_MATRIX_APPROX(x1_optim           , x1_exact  , 1e-5 );
            ASSERT_MATRIX_APPROX(D_optim_imu        , D_exact   , 1e-5 );
            ASSERT_MATRIX_APPROX(x1_optim_imu       , x1_exact  , 1e-5 );
            ASSERT_NEAR(x1_optim.segment(3,4).norm(), 1.0       , 1e-8 );
        }

};

class Process_Factor_Imu_ODO : public Process_Factor_Imu
{
    public:
        // Wolf objects
        SensorOdom3dPtr     sensor_odo;
        ProcessorOdom3dPtr  processor_odo;
        CaptureOdom3dPtr    capture_odo;

        void SetUp( ) override
        {

            // ===================================== Imu
            Process_Factor_Imu::SetUp();

            // ===================================== ODO
            string wolf_root = _WOLF_IMU_ROOT_DIR;

            SensorBasePtr    sensor     = problem->installSensor   ("SensorOdom3d", "Odometer", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_odom_3d.yaml"   );
            ProcessorBasePtr processor  = problem->installProcessor("ProcessorOdom3d", "Odometer", "Odometer"                            , wolf_root + "/demos/processor_odom_3d.yaml");
            sensor_odo      = static_pointer_cast<SensorOdom3d>(sensor);

            processor_odo   = static_pointer_cast<ProcessorOdom3d>(processor);
            processor_odo->setTimeTolerance(dt/2);
            processor_odo->setVotingActive(false);
        }

        // Initial configuration of variables
        bool configureAll() override
        {
            // ===================================== Imu
            Process_Factor_Imu::configureAll();

            // ===================================== ODO
            processor_odo->setOrigin(KF_0);

            return true;
        }

        void integrateAll() override
        {
            // ===================================== Imu
            Process_Factor_Imu::integrateAll();

            // ===================================== ODO
            Vector6d    data;
            Vector3d    p1  = x1_exact.head(3);
            Quaterniond q1   (x1_exact.data() + 3);
            Vector3d    dp  =        q0.conjugate() * (p1 - p0);
            Vector3d    dth = log_q( q0.conjugate() *  q1     );
            data           << dp, dth;

            CaptureOdom3dPtr capture_odo = make_shared<CaptureOdom3d>(t, sensor_odo, data, sensor_odo->getNoiseCov());

            sensor_odo->process(capture_odo);
       }

        void integrateAllTrajectories() override
        {
            // ===================================== Imu
            Process_Factor_Imu::integrateAllTrajectories();

            // ===================================== ODO
            Vector6d    data;
            Vector3d    p1  = x1_exact.head(3);
            Quaterniond q1   (x1_exact.data() + 3);
            Vector3d    dp  =        q0.conjugate() * (p1 - p0);
            Vector3d    dth = log_q( q0.conjugate() *  q1     );
            data           << dp, dth;

            CaptureOdom3dPtr capture_odo = make_shared<CaptureOdom3d>(t, sensor_odo, data, sensor_odo->getNoiseCov());

            sensor_odo->process(capture_odo);
       }

        void buildProblem() override
        {
            // ===================================== Imu
            Process_Factor_Imu::buildProblem();

            // ===================================== ODO
            // Process ODO for the callback to take effect
            data = Vector6d::Zero();
            capture_odo = make_shared<CaptureOdom3d>(t+dt, sensor_odo, data, sensor_odo->getNoiseCov());
            sensor_odo->process(capture_odo);

        }

};
TEST_F(Process_Factor_Imu, MotionConstant_PQV_b__PQV_b) // F_ixed___e_stimated
{

    // ================================================================================================================ //
    // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
    // ================================================================================================================ //
    //
    // ---------- time
    t0                  = 0;
    num_integrations    = 50;

    // ---------- initial pose
    p0                 << 0,0,0;
    q0.coeffs()        << 0,0,0,1;
    v0                 << 0,0,0;

    // ---------- bias
    bias_real          << .001, .002, .003,    -.001, -.002, -.003;
    bias_preint         = -bias_real;

    // ---------- motion params
    a                  = Vector3d( 1,2,3 );
    w                  = Vector3d( 1,2,3 );

    // ---------- fix boundaries
    p0_fixed       = true;
    q0_fixed       = true;
    v0_fixed       = true;
    p1_fixed       = true;
    q1_fixed       = true;
    v1_fixed       = true;
    //
    // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
    // ================================================================================================================ //

    // ===================================== RUN ALL
    string report = runAll(SolverManager::ReportVerbosity::BRIEF);

//    printAll(report);

    assertAll();

}

TEST_F(Process_Factor_Imu, test_capture) // F_ixed___e_stimated
{

    // ================================================================================================================ //
    // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
    // ================================================================================================================ //
    //
    // ---------- time
    t0                  = 0;
    num_integrations    = 50;

    // ---------- initial pose
    p0                 << 0,0,0;
    q0.coeffs()        << 0,0,0,1;
    v0                 << 0,0,0;

    // ---------- bias
    bias_real          << .001, .002, .003,    -.001, -.002, -.003;
    bias_preint         = -bias_real;

    // ---------- motion params
    a                  = Vector3d( 1,2,3 );
    w                  = Vector3d( 1,2,3 );

    // ---------- fix boundaries
    p0_fixed       = true;
    q0_fixed       = true;
    v0_fixed       = true;
    p1_fixed       = true;
    q1_fixed       = true;
    v1_fixed       = true;
    //
    // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
    // ================================================================================================================ //

    // ===================================== RUN ALL
    Eigen::Vector6d initial_bias((Eigen::Vector6d()<< .002, .0007, -.001,   .003, -.002, .001).finished());
    sensor_imu->getIntrinsic()->setState(initial_bias);
    configureAll();
    integrateAll();
    buildProblem();
    //Since we did not solve, hence bias estimates did not change yet, both capture should have the same calibration
    ASSERT_MATRIX_APPROX(KF_0->getCaptureOf(sensor_imu)->getSensorIntrinsic()->getState(), initial_bias, 1e-8 );
    ASSERT_MATRIX_APPROX(KF_0->getCaptureOf(sensor_imu)->getSensorIntrinsic()->getState(), KF_1->getCaptureOf(sensor_imu)->getSensorIntrinsic()->getState(), 1e-8 );
}

TEST_F(Process_Factor_Imu, MotionConstant_pqv_b__PQV_b) // F_ixed___e_stimated
{

    // ================================================================================================================ //
    // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
    // ================================================================================================================ //
    //
    // ---------- time
    t0                  = 0;
    num_integrations    = 50;

    // ---------- initial pose
    p0                 << 0,0,0;
    q0.coeffs()        << 0,0,0,1;
    v0                 << 0,0,0;

    // ---------- bias
    bias_real          << .001, .002, .003,    -.001, -.002, -.003;
    bias_preint         = -bias_real;

    // ---------- motion params
    a                  = Vector3d( 1,2,3 );
    w                  = Vector3d( 1,2,3 );

    // ---------- fix boundaries
    p0_fixed       = false;
    q0_fixed       = false;
    v0_fixed       = false;
    p1_fixed       = true;
    q1_fixed       = true;
    v1_fixed       = true;
    //
    // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
    // ================================================================================================================ //

    // ===================================== RUN ALL
    string report = runAll(SolverManager::ReportVerbosity::BRIEF);

    //    printAll(report);

    assertAll();

}

TEST_F(Process_Factor_Imu, MotionConstant_pqV_b__PQv_b) // F_ixed___e_stimated
{

    // ================================================================================================================ //
    // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
    // ================================================================================================================ //
    //
    // ---------- time
    t0                  = 0;
    num_integrations    = 50;

    // ---------- initial pose
    p0                 << 0,0,0;
    q0.coeffs()        << 0,0,0,1;
    v0                 << 0,0,0;

    // ---------- bias
    bias_real          << .001, .002, .003,    -.001, -.002, -.003;
    bias_preint         = -bias_real;

    // ---------- motion params
    a                  = Vector3d( 1,2,3 );
    w                  = Vector3d( 1,2,3 );

    // ---------- fix boundaries
    p0_fixed       = false;
    q0_fixed       = false;
    v0_fixed       = true;
    p1_fixed       = true;
    q1_fixed       = true;
    v1_fixed       = false;
    //
    // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
    // ================================================================================================================ //

    // ===================================== RUN ALL
    string report = runAll(SolverManager::ReportVerbosity::BRIEF);

    //    printAll(report);

    assertAll();

}

TEST_F(Process_Factor_Imu, MotionRandom_PQV_b__PQV_b) // F_ixed___e_stimated
{

    // ================================================================================================================ //
    // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
    // ================================================================================================================ //
    //
    // ---------- time
    t0                  = 0;
    num_integrations    = 50;

    // ---------- initial pose
    p0                 << 0,0,0;
    q0.coeffs()        << 0,0,0,1;
    v0                 << 0,0,0;

    // ---------- bias
    bias_real          << .001, .002, .003,    -.001, -.002, -.003;
    bias_preint         = -bias_real;

    // ---------- motion params
    a                  = Matrix<double, 3, 50>::Random();
    w                  = Matrix<double, 3, 50>::Random();

    // ---------- fix boundaries
    p0_fixed       = true;
    q0_fixed       = true;
    v0_fixed       = true;
    p1_fixed       = true;
    q1_fixed       = true;
    v1_fixed       = true;
    //
    // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
    // ================================================================================================================ //

    // ===================================== RUN ALL
    string report = runAll(SolverManager::ReportVerbosity::BRIEF);

//    printAll(report);

    assertAll();

}

TEST_F(Process_Factor_Imu, MotionRandom_pqV_b__PQv_b) // F_ixed___e_stimated
{

    // ================================================================================================================ //
    // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
    // ================================================================================================================ //
    //
    // ---------- time
    t0                  = 0;
    num_integrations    = 50;

    // ---------- initial pose
    p0                 << 0,0,0;
    q0.coeffs()        << 0,0,0,1;
    v0                 << 0,0,0;

    // ---------- bias
    bias_real          << .001, .002, .003,    -.001, -.002, -.003;
    bias_preint         = -bias_real;

    // ---------- motion params
    a                  = Matrix<double, 3, 50>::Random();
    w                  = Matrix<double, 3, 50>::Random();

    // ---------- fix boundaries
    p0_fixed       = false;
    q0_fixed       = false;
    v0_fixed       = true;
    p1_fixed       = true;
    q1_fixed       = true;
    v1_fixed       = false;
    //
    // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
    // ================================================================================================================ //

    // ===================================== RUN ALL
    string report = runAll(SolverManager::ReportVerbosity::BRIEF);

//    printAll(report);

    assertAll();

}

TEST_F(Process_Factor_Imu, MotionRandom_pqV_b__pQV_b) // F_ixed___e_stimated
{

    // ================================================================================================================ //
    // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
    // ================================================================================================================ //
    //
    // ---------- time
    t0                  = 0;
    num_integrations    = 50;

    // ---------- initial pose
    p0                 << 0,0,0;
    q0.coeffs()        << 0,0,0,1;
    v0                 << 0,0,0;

    // ---------- bias
    bias_real          << .001, .002, .003,    -.001, -.002, -.003;
    bias_preint         = -bias_real;

    // ---------- motion params
    a                  = Matrix<double, 3, 50>::Random();
    w                  = Matrix<double, 3, 50>::Random();

    // ---------- fix boundaries
    p0_fixed       = false;
    q0_fixed       = false;
    v0_fixed       = true;
    p1_fixed       = false;
    q1_fixed       = true;
    v1_fixed       = true;
    //
    // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
    // ================================================================================================================ //

    // ===================================== RUN ALL
    string report = runAll(SolverManager::ReportVerbosity::BRIEF);

    // printAll(report);

    assertAll();

}

TEST_F(Process_Factor_Imu, MotionConstant_NonNullState_PQV_b__PQV_b) // F_ixed___e_stimated
{

    // ================================================================================================================ //
    // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
    // ================================================================================================================ //
    //
    // ---------- time
    t0                  = 0;
    num_integrations    = 50;

    // ---------- initial pose
    p0                 << 3,2,1;
    q0.coeffs()        << 0.5,0.5,0.5,.5;
    v0                 << 1,2,3;

    // ---------- bias
    bias_real          << .001, .002, .003,    -.001, -.002, -.003;
    bias_preint         = -bias_real;

    // ---------- motion params
    a                  = Vector3d( 1,2,3 );
    w                  = Vector3d( 1,2,3 );

    // ---------- fix boundaries
    p0_fixed       = true;
    q0_fixed       = true;
    v0_fixed       = true;
    p1_fixed       = true;
    q1_fixed       = true;
    v1_fixed       = true;
    //
    // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
    // ================================================================================================================ //

    // ===================================== RUN ALL
    string report = runAll(SolverManager::ReportVerbosity::BRIEF);

//    printAll(report);
    assertAll();

}

TEST_F(Process_Factor_Imu_ODO, MotionConstantRotation_PQV_b__PQV_b) // F_ixed___e_stimated
{

    // ================================================================================================================ //
    // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
    // ================================================================================================================ //
    //
    // ---------- time
    t0                  = 0;
    num_integrations    = 50;

    // ---------- initial pose
    p0                 << 0,0,0;
    q0.coeffs()        << 0,0,0,1;
    v0                 << 0,0,0;

    // ---------- bias
    bias_real          << .001, .002, .003,    -.001, -.002, -.003;
    bias_preint         = -bias_real;

    // ---------- motion params
    a                  = Vector3d( 0,0,0 );
    w                  = Vector3d( 1,2,3 );

    // ---------- fix boundaries
    p0_fixed       = true;
    q0_fixed       = true;
    v0_fixed       = true;
    p1_fixed       = true;
    q1_fixed       = true;
    v1_fixed       = true;
    //
    // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
    // ================================================================================================================ //

    // ===================================== RUN ALL
    string report = runAll(SolverManager::ReportVerbosity::BRIEF);

    //    printAll(report);

    assertAll();

}

TEST_F(Process_Factor_Imu_ODO, MotionConstantRotation_PQV_b__PQv_b) // F_ixed___e_stimated
{

    // ================================================================================================================ //
    // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
    // ================================================================================================================ //
    //
    // ---------- time
    t0                  = 0;
    num_integrations    = 50;

    // ---------- initial pose
    p0                 << 0,0,0;
    q0.coeffs()        << 0,0,0,1;
    v0                 << 0,0,0;

    // ---------- bias
    bias_real          << .001, .002, .003,    -.001, -.002, -.003;
    bias_preint         = -bias_real;

    // ---------- motion params
    a                  = Vector3d( 0,0,0 );
    w                  = Vector3d( 1,2,3 );

    // ---------- fix boundaries
    p0_fixed       = true;
    q0_fixed       = true;
    v0_fixed       = true;
    p1_fixed       = true;
    q1_fixed       = true;
    v1_fixed       = false;
    //
    // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
    // ================================================================================================================ //

    // ===================================== RUN ALL
    string report = runAll(SolverManager::ReportVerbosity::BRIEF);

    //    printAll(report);

    assertAll();

}

TEST_F(Process_Factor_Imu_ODO, MotionConstantRotation_PQV_b__Pqv_b) // F_ixed___e_stimated
{

    // ================================================================================================================ //
    // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
    // ================================================================================================================ //
    //
    // ---------- time
    t0                  = 0;
    num_integrations    = 50;

    // ---------- initial pose
    p0                 << 0,0,0;
    q0.coeffs()        << 0,0,0,1;
    v0                 << 0,0,0;

    // ---------- bias
    bias_real          << .001, .002, .003,    -.001, -.002, -.003;
    bias_preint         = -bias_real;

    // ---------- motion params
    a                  = Vector3d( 0,0,0 );
    w                  = Vector3d( 1,2,3 );

    // ---------- fix boundaries
    p0_fixed       = true;
    q0_fixed       = true;
    v0_fixed       = true;
    p1_fixed       = true;
    q1_fixed       = false;
    v1_fixed       = false;
    //
    // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
    // ================================================================================================================ //

    // ===================================== RUN ALL
    string report = runAll(SolverManager::ReportVerbosity::BRIEF);

    // printAll(report);

    assertAll();

}

TEST_F(Process_Factor_Imu_ODO, MotionConstantRotation_PQV_b__pQv_b) // F_ixed___e_stimated
{

    // ================================================================================================================ //
    // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
    // ================================================================================================================ //
    //
    // ---------- time
    t0                  = 0;
    num_integrations    = 50;

    // ---------- initial pose
    p0                 << 0,0,0;
    q0.coeffs()        << 0,0,0,1;
    v0                 << 0,0,0;

    // ---------- bias
    bias_real          << .001, .002, .003,    -.001, -.002, -.003;
    bias_preint         = -bias_real;

    // ---------- motion params
    a                  = Vector3d( 0,0,0 );
    w                  = Vector3d( 1,2,3 );

    // ---------- fix boundaries
    p0_fixed       = true;
    q0_fixed       = true;
    v0_fixed       = true;
    p1_fixed       = false;
    q1_fixed       = true;
    v1_fixed       = false;
    //
    // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
    // ================================================================================================================ //

    // ===================================== RUN ALL
    string report = runAll(SolverManager::ReportVerbosity::BRIEF);

    //    printAll(report);

    assertAll();

}

TEST_F(Process_Factor_Imu_ODO, MotionConstantRotation_PQV_b__pqv_b) // F_ixed___e_stimated
{

    // ================================================================================================================ //
    // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
    // ================================================================================================================ //
    //
    // ---------- time
    t0                  = 0;
    num_integrations    = 50;

    // ---------- initial pose
    p0                 << 0,0,0;
    q0.coeffs()        << 0,0,0,1;
    v0                 << 0,0,0;

    // ---------- bias
    bias_real          << .001, .002, .003,    -.001, -.002, -.003;
    bias_preint         = -bias_real;

    // ---------- motion params
    a                  = Vector3d( 0,0,0 );
    w                  = Vector3d( 1,2,3 );

    // ---------- fix boundaries
    p0_fixed       = true;
    q0_fixed       = true;
    v0_fixed       = true;
    p1_fixed       = false;
    q1_fixed       = false;
    v1_fixed       = false;
    //
    // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
    // ================================================================================================================ //

    // ===================================== RUN ALL
    string report = runAll(SolverManager::ReportVerbosity::BRIEF);

    //    printAll(report);

    assertAll();

}

TEST_F(Process_Factor_Imu_ODO, MotionConstant_pqv_b__pqV_b) // F_ixed___e_stimated
{

    // ================================================================================================================ //
    // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
    // ================================================================================================================ //
    //
    // ---------- time
    t0                  = 0;
    num_integrations    = 50;

    // ---------- initial pose
    p0                 << 0,0,0;
    q0.coeffs()        << 0,0,0,1;
    v0                 << 0,0,0;

    // ---------- bias
    bias_real          << .001, .002, .003,    -.001, -.002, -.003;
    bias_preint         = -bias_real;

    // ---------- motion params
    a                  = Vector3d( 1,2,3 );
    w                  = Vector3d( 1,2,3 );

    // ---------- fix boundaries
    p0_fixed       = false;
    q0_fixed       = false;
    v0_fixed       = false;
    p1_fixed       = false;
    q1_fixed       = false;
    v1_fixed       = true;
    //
    // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
    // ================================================================================================================ //

    // ===================================== RUN ALL
    string report = runAll(SolverManager::ReportVerbosity::BRIEF);

//    printAll(report);

    assertAll();

}

TEST_F(Process_Factor_Imu_ODO, MotionConstant_pqV_b__pqv_b) // F_ixed___e_stimated
{

    // ================================================================================================================ //
    // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
    // ================================================================================================================ //
    //
    // ---------- time
    t0                  = 0;
    num_integrations    = 50;

    // ---------- initial pose
    p0                 << 0,0,0;
    q0.coeffs()        << 0,0,0,1;
    v0                 << 0,0,0;

    // ---------- bias
    bias_real          << .001, .002, .003,    -.001, -.002, -.003;
    bias_preint         = -bias_real;

    // ---------- motion params
    a                  = Vector3d( 1,2,3 );
    w                  = Vector3d( 1,2,3 );

    // ---------- fix boundaries
    p0_fixed       = false;
    q0_fixed       = false;
    v0_fixed       = true;
    p1_fixed       = false;
    q1_fixed       = false;
    v1_fixed       = false;
    //
    // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
    // ================================================================================================================ //

    // ===================================== RUN ALL
    string report = runAll(SolverManager::ReportVerbosity::BRIEF);

//    printAll(report);

    assertAll();

}

TEST_F(Process_Factor_Imu_ODO, MotionRandom_PQV_b__pqv_b) // F_ixed___e_stimated
{

    // ================================================================================================================ //
    // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
    // ================================================================================================================ //
    //
    // ---------- time
    t0                  = 0;
    num_integrations    = 50;

    // ---------- initial pose
    p0                 << 0,0,0;
    q0.coeffs()        << 0,0,0,1;
    v0                 << 0,0,0;

    // ---------- bias
    bias_real          << .001, .002, .003,    -.001, -.002, -.003;
    bias_preint         = -bias_real;

    // ---------- motion params
    a                  = Matrix<double, 3, 50>::Random();
    w                  = Matrix<double, 3, 50>::Random();

    // ---------- fix boundaries
    p0_fixed       = true;
    q0_fixed       = true;
    v0_fixed       = true;
    p1_fixed       = false;
    q1_fixed       = false;
    v1_fixed       = false;
    //
    // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
    // ================================================================================================================ //

    // ===================================== RUN ALL
    string report = runAll(SolverManager::ReportVerbosity::BRIEF);
    // printAll(report);

    assertAll();

}

TEST_F(Process_Factor_Imu_ODO, MotionRandom_PqV_b__pqV_b) // F_ixed___e_stimated
{
    // ================================================================================================================ //
    // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
    // ================================================================================================================ //
    //
    // ---------- time
    t0                  = 0;
    num_integrations    = 50;

    // ---------- initial pose
    p0                 << 0,0,0;
    q0.coeffs()        << 0,0,0,1;
    v0                 << 0,0,0;

    // ---------- bias
    bias_real          << .001, .002, .003,    -.001, -.002, -.003;
    bias_preint         = -bias_real;

    // ---------- motion params
    a                  = Matrix<double, 3, 50>::Random();
    w                  = Matrix<double, 3, 50>::Random();

    // ---------- fix boundaries
    p0_fixed       = true;
    q0_fixed       = false;
    v0_fixed       = true;
    p1_fixed       = false;
    q1_fixed       = false;
    v1_fixed       = true;
    //
    // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
    // ================================================================================================================ //

    // ===================================== RUN ALL
    string report = runAll(SolverManager::ReportVerbosity::BRIEF);

//    printAll(report);

    assertAll();

}

TEST_F(Process_Factor_Imu_ODO, RecoverTrajectory_MotionRandom_PqV_b__pqV_b) // F_ixed___e_stimated
{

    // ================================================================================================================ //
    // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
    // ================================================================================================================ //
    //
    // ---------- time
    t0                  = 0;
    num_integrations    = 25;

    // ---------- initial pose
    p0                 << 0,0,0;
    q0.coeffs()        << 0,0,0,1;
    v0                 << 0,0,0;

    // ---------- bias
    bias_real          << .001, .002, .003,    -.001, -.002, -.003;
    bias_preint         = -bias_real;

    // ---------- motion params
    a  = Matrix<double, 3, 25>::Ones() + 0.1 * Matrix<double, 3, 25>::Random();
    w  = Matrix<double, 3, 25>::Ones() + 0.1 * Matrix<double, 3, 25>::Random();

    // ---------- fix boundaries
    p0_fixed       = true;
    q0_fixed       = false;
    v0_fixed       = true;
    p1_fixed       = false;
    q1_fixed       = false;
    v1_fixed       = true;
    //
    // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
    // ================================================================================================================ //

    // ===================================== RUN ALL
    configureAll();
    WOLF_INFO("TEST configured");
    integrateAllTrajectories();
    WOLF_INFO("TEST integrated");
    buildProblem();
    WOLF_INFO("TEST built");
    string report = solveProblem(SolverManager::ReportVerbosity::BRIEF);
    WOLF_INFO("TEST solved");

    assertAll();
    WOLF_INFO("TEST asserted");

    // Build optimal trajectory
    MatrixXd Trj_x_optim(10,Trj_D_preint_prc.cols());
    double Dt = 0;
    SizeEigen i = 0;
    for (auto J_D_b : Buf_Jac_preint_prc)
    {
        VectorXd step       = J_D_b * (bias_0 - bias_preint);
        VectorXd D_opt      = imu::plus(Trj_D_preint_prc.col(i).eval(), step);
        Trj_x_optim.col(i)  = imu::composeOverState(x0_optim, D_opt, Dt);
        Dt += dt;
        i  ++;
    }
    WOLF_INFO("optimal trajectory built");

    // Get optimal trajectory via getState()
    i = 0;
    t = 0;
    MatrixXd Trj_x_optim_prc(10,Trj_D_preint_prc.cols());
    for (int i = 0; i < Trj_x_optim_prc.cols(); i++)
    {
        Trj_x_optim_prc.col(i) = problem->getState(t).vector("POV");
        t += dt;
    }
    WOLF_INFO("optimal trajectory get");

    // printAll(report);

//    WOLF_INFO("------------------------------------");
//    WOLF_INFO("Exact x0 \n", x0         .transpose());
//    WOLF_INFO("Optim x0 \n", x0_optim   .transpose());
//    WOLF_INFO("Optim x  \n", Trj_x_optim_prc.transpose());
//    WOLF_INFO("Optim x1 \n", x1_optim   .transpose());
//    WOLF_INFO("Exact x1 \n", x1_exact   .transpose());
//    WOLF_INFO("------------------------------------");

    // The Mother of Asserts !!!
    ASSERT_MATRIX_APPROX(Trj_x_optim, Trj_x_optim_prc, 1e-6);

    // First and last trj states match known extrema
    ASSERT_MATRIX_APPROX(Trj_x_optim_prc.leftCols (1), x0,       1e-6);
    ASSERT_MATRIX_APPROX(Trj_x_optim_prc.rightCols(1), x1_exact, 1e-6);

}

TEST_F(Process_Factor_Imu, bootstrap)
{
    processor_imu->setVotingActive(true);
    processor_imu->setMaxTimeSpan(0.4);

    auto KF0 = problem->emplaceFrame(0.0);
    problem->keyFrameCallback(KF0,nullptr);
    problem->print(4, 0, 1, 0);

    // Vector6d data(0,0,9.806, 0,0,0); // gravity on Z
    Vector6d data(0.0, 9.806, 0.0,  0.0, 0.0, 0.0); // gravity on Y

    capture_imu = std::make_shared<CaptureImu>(0.0, sensor_imu, data, Matrix6d::Identity());

    for (t = 0; t < 0.14; t += dt)
    {
        capture_imu->setTimeStamp(t);
        capture_imu->process();
        problem->print(4, 0, 1, 1);
    }
}

int main(int argc, char **argv)
{
    testing::InitGoogleTest(&argc, argv);
    ::testing::GTEST_FLAG(filter) = "Process_Factor_Imu.bootstrap";
    //    ::testing::GTEST_FLAG(filter) = "Process_Factor_Imu_ODO.*";
    //    ::testing::GTEST_FLAG(filter) = "Process_Factor_Imu_ODO.RecoverTrajectory_MotionRandom_PqV_b__pqV_b";

    return RUN_ALL_TESTS();
}

/* Some notes :
 *
 * - Process_Factor_Imu_ODO.MotionConstant_PQv_b__PQv_b :
 *      this test will not work + jacobian is rank deficient because of estimating both initial
 *      and final velocities. 
 *      Imu data integration is done with correct biases (so this is the case of a calibrated Imu). Before solving the problem, we perturbate the initial bias.
 *      We solve the problem by fixing all states excepted V1 and V2. while creating the factors, both velocities are corrected using the difference between the actual
 *      bias and the bias used during preintegration. One way to solve this in the solver side would be to make the actual bias match the preintegraion bias so that the
 *      difference is 0 and does not affect the states of the KF. Another possibility is to have both velocities modified without changing the biases. it is likely that this
 *      solution is chosen in this case (bias changes is penalized between 2 KeyFrames, but velocities have no other factors here.)
 * 
 *  - Bias evaluation with a precision of 1e-4 :
 *      The bias introduced in the data for the preintegration steps is different of the real bias. This is simulating the case of a non calibrated Imu
 *      Because of cross relations between acc and gyro biases (respectively a_b and w_b) it is difficult to expect a better estimation.
 *      A small change in a_b can be cancelled by a small variation in w_b. in other words : there are local minima.
 *      In addition, for Process_Factor_Imu tests, P and V are tested against 1e-5 precision while 1e-8 is used for Q.
 *      Errors tend to be distributed in different estimated variable when we get into a local minima (to minimize residuals in a least square sense).
 */