Something went wrong on our end
-
Joan Solà Ortega authoredJoan Solà Ortega authored
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).
*/