diff --git a/src/capture_motion.cpp b/src/capture_motion.cpp
index eed9f5ba3b9bf6e27a3531105491b64945528735..93203d74dffb7ade049df63a2874d3882082e320 100644
--- a/src/capture_motion.cpp
+++ b/src/capture_motion.cpp
@@ -19,7 +19,8 @@ CaptureMotion::CaptureMotion(const TimeStamp& _ts,
CaptureBase("MOTION", _ts, _sensor_ptr),
data_(_data),
data_cov_(_sensor_ptr ? _sensor_ptr->getNoiseCov() : Eigen::MatrixXs::Zero(_data.rows(), _data.rows())), // Someone should test this zero and do something smart accordingly
- buffer_(_data.size(), _delta_size, _delta_cov_size, computeCalibSize()), origin_frame_ptr_(_origin_frame_ptr)
+ buffer_(_data.size(), _delta_size, _delta_cov_size, computeCalibSize()),
+ origin_frame_ptr_(_origin_frame_ptr)
{
//
}
@@ -54,22 +55,23 @@ CaptureMotion::~CaptureMotion()
Eigen::VectorXs CaptureMotion::getDeltaCorrected(const VectorXs& _calib_current)
{
- VectorXs calib_preint = getCalibrationPreint();
- VectorXs delta_preint = getBuffer().get().back().delta_integr_;
- MatrixXs jac_calib = getBuffer().get().back().jacobian_calib_;
- VectorXs delta_error = jac_calib * (_calib_current - calib_preint);
- VectorXs delta = correctDelta(delta_preint, delta_error);
- return delta;
+ VectorXs calib_preint = getCalibrationPreint();
+ VectorXs delta_preint = getBuffer().get().back().delta_integr_;
+ MatrixXs jac_calib = getBuffer().get().back().jacobian_calib_;
+ VectorXs delta_error = jac_calib * (_calib_current - calib_preint);
+ VectorXs delta_corrected = correctDelta(delta_preint, delta_error);
+ return delta_corrected;
}
Eigen::VectorXs CaptureMotion::getDeltaCorrected(const VectorXs& _calib_current, const TimeStamp& _ts)
{
- VectorXs calib_preint = getCalibrationPreint();
- VectorXs delta_preint = getBuffer().getMotion(_ts).delta_integr_;
- MatrixXs jac_calib = getBuffer().getMotion(_ts).jacobian_calib_;
- VectorXs delta_error = jac_calib * (_calib_current - calib_preint);
- VectorXs delta = correctDelta(delta_preint, delta_error);
- return delta;
+ VectorXs calib_preint = getBuffer().getCalibrationPreint();
+ Motion motion = getBuffer().getMotion(_ts);
+ VectorXs delta_preint = motion.delta_integr_;
+ MatrixXs jac_calib = motion.jacobian_calib_;
+ VectorXs delta_error = jac_calib * (_calib_current - calib_preint);
+ VectorXs delta_corrected = correctDelta(delta_preint, delta_error);
+ return delta_corrected;
}
}
diff --git a/src/examples/processor_imu.yaml b/src/examples/processor_imu.yaml
index 8a3aaf4708b56c97615fabf37962c0ea1db658bf..7e684c8833a6e9e3123863c71366a989b30e4004 100644
--- a/src/examples/processor_imu.yaml
+++ b/src/examples/processor_imu.yaml
@@ -5,4 +5,5 @@ keyframe vote:
max buffer length: 20000 # motion deltas
dist traveled: 2.0 # meters
angle turned: 0.2 # radians (1 rad approx 57 deg, approx 60 deg)
- voting_active: false
\ No newline at end of file
+ voting_active: false
+
\ No newline at end of file
diff --git a/src/examples/processor_imu_no_vote.yaml b/src/examples/processor_imu_no_vote.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..4f6ad39556cd9a09a215f043d4beb0066d4a37bb
--- /dev/null
+++ b/src/examples/processor_imu_no_vote.yaml
@@ -0,0 +1,9 @@
+processor type: "IMU" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+processor name: "Main imu" # This is ignored. The name provided to the SensorFactory prevails
+keyframe vote:
+ max time span: 999999.0 # seconds
+ max buffer length: 999999 # motion deltas
+ dist traveled: 999999.0 # meters
+ angle turned: 999999 # radians (1 rad approx 57 deg, approx 60 deg)
+ voting_active: false
+
\ No newline at end of file
diff --git a/src/examples/processor_imu_t1.yaml b/src/examples/processor_imu_t1.yaml
index 9924b3ddd020333ef9d5f907f9a284db9c78166e..e0c21758c11ed2a684b2f3f2bc2aeb4c557c84ef 100644
--- a/src/examples/processor_imu_t1.yaml
+++ b/src/examples/processor_imu_t1.yaml
@@ -5,4 +5,5 @@ keyframe vote:
max buffer length: 10000 # motion deltas
dist traveled: 10000 # meters
angle turned: 10000 # radians (1 rad approx 57 deg, approx 60 deg)
- voting_active: true
\ No newline at end of file
+ voting_active: true
+
\ No newline at end of file
diff --git a/src/examples/processor_imu_t6.yaml b/src/examples/processor_imu_t6.yaml
index 7313a387eea81d7fd6bcd6ad1ff888c6c9962522..e3a4b17df72c957fec49d935ddcd3a9a8c824a96 100644
--- a/src/examples/processor_imu_t6.yaml
+++ b/src/examples/processor_imu_t6.yaml
@@ -5,4 +5,5 @@ keyframe vote:
max buffer length: 10000 # motion deltas
dist traveled: 10000 # meters
angle turned: 10000 # radians (1 rad approx 57 deg, approx 60 deg)
- voting_active: true
\ No newline at end of file
+ voting_active: true
+
\ No newline at end of file
diff --git a/src/problem.cpp b/src/problem.cpp
index 513cc64c2c4628b26eb6cda6eb63d55a08164063..8c639c114c44031bfcb73a7dadd915b296ec43fb 100644
--- a/src/problem.cpp
+++ b/src/problem.cpp
@@ -613,7 +613,7 @@ FrameBasePtr Problem::setPrior(const Eigen::VectorXs& _prior_state, const Eigen:
// Capture fix only takes 3D position and Quaternion orientation
CaptureFixPtr init_capture;
if (trajectory_ptr_->getFrameStructure() == "POV 3D")
- init_capture = std::make_shared<CaptureFix>(_ts, nullptr, _prior_state.head(7), _prior_cov);
+ init_capture = std::make_shared<CaptureFix>(_ts, nullptr, _prior_state.head(7), _prior_cov.topLeftCorner(6,6));
else
init_capture = std::make_shared<CaptureFix>(_ts, nullptr, _prior_state, _prior_cov);
diff --git a/src/processor_imu.cpp b/src/processor_imu.cpp
index a2d526cbd7c1fb4f6a01b96bcf650302ef97ca84..4390b71829755b196740e178e3c6d2d7ca378e63 100644
--- a/src/processor_imu.cpp
+++ b/src/processor_imu.cpp
@@ -39,20 +39,22 @@ bool ProcessorIMU::voteForKeyFrame()
// time span
if (getBuffer().get().back().ts_ - getBuffer().get().front().ts_ > max_time_span_)
{
+ WOLF_DEBUG( "PM: vote: time span" );
return true;
}
// buffer length
if (getBuffer().get().size() > max_buff_length_)
{
+ WOLF_DEBUG( "PM: vote: buffer length" );
return true;
}
- /*// angle turned
- Scalar angle = 2.0 * acos(delta_integrated_(6));
+ // angle turned
+ Scalar angle = 2.0 * asin(delta_integrated_.segment(4,3).norm());
if (angle > angle_turned_)
{
WOLF_DEBUG( "PM: vote: angle turned" );
return true;
- }*/
+ }
//WOLF_DEBUG( "PM: do not vote" );
return false;
}
diff --git a/src/test/gtest_constraint_imu.cpp b/src/test/gtest_constraint_imu.cpp
index 0d5cf8b78e753a540240a514f49bc48da44f5c9d..10ae5c40027089a03d1be05ebaf2c88866d79f92 100644
--- a/src/test/gtest_constraint_imu.cpp
+++ b/src/test/gtest_constraint_imu.cpp
@@ -1339,6 +1339,394 @@ class ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test
virtual void TearDown(){}
};
+class ConstraintIMU_biasTest : public testing::Test
+{
+ public:
+ ProblemPtr problem;
+ SensorIMUPtr sensor_imu;
+ ProcessorIMUPtr processor_imu;
+ FrameBasePtr KF_0, KF_1;
+ CaptureBasePtr C_0, C_1;
+ CaptureMotionPtr CM_0, CM_1;
+ CaptureIMUPtr capture_imu;
+ CeresManagerPtr ceres_manager;
+
+ TimeStamp t0, t;
+ Scalar dt, DT;
+ int num_integrations;
+
+ VectorXs x0;
+ Vector3s p0, v0;
+ Quaternions q0, q;
+ Matrix<Scalar, 9, 9> P0;
+ Vector6s motion, data, bias_real, bias_preint, bias_null;
+ Vector6s bias_0, bias_1;
+ Vector3s a, w, am, wm;
+
+ VectorXs D_exact, x_exact;
+ VectorXs D_preint_imu, D_corrected_imu;
+ VectorXs x_preint_imu, x_corrected_imu;
+ VectorXs D_preint, D_corrected;
+ VectorXs x_preint, x_corrected;
+ VectorXs D_optim, x_optim;
+ VectorXs D_optim_imu, x_optim_imu;
+ Matrix<Scalar, 9, 6> J_b;
+ Vector9s step;
+
+ bool p0_fixed, q0_fixed, v0_fixed;
+ bool p1_fixed, q1_fixed, v1_fixed;
+
+
+
+ virtual void SetUp( )
+ {
+ using std::shared_ptr;
+ using std::make_shared;
+ using std::static_pointer_cast;
+
+ std::string wolf_root = _WOLF_ROOT_DIR;
+
+ //===================================== SETTING PROBLEM
+ problem = Problem::create("POV 3D");
+
+ // CERES WRAPPER
+ ceres::Solver::Options ceres_options;
+ // ceres_options.minimizer_type = ceres::TRUST_REGION;
+ // ceres_options.max_line_search_step_contraction = 1e-3;
+ // ceres_options.max_num_iterations = 1e4;
+ // ceres_options.min_relative_decrease = 1e-10;
+ // ceres_options.parameter_tolerance = 1e-10;
+ // ceres_options.function_tolerance = 1e-10;
+ ceres_manager = std::make_shared<CeresManager>(problem, ceres_options);
+
+ // SENSOR + PROCESSOR IMU
+ SensorBasePtr sensor = problem->installSensor ("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
+ ProcessorBasePtr processor = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu_no_vote.yaml");
+ sensor_imu = std::static_pointer_cast<SensorIMU> (sensor);
+ processor_imu = std::static_pointer_cast<ProcessorIMU>(processor);
+
+ bias_null.setZero();
+
+ x0.resize(10);
+ D_preint.resize(10);
+ D_corrected.resize(10);
+
+ }
+
+ virtual void TearDown( )
+ {
+ //
+ }
+
+ /* Create IMU data from body motion
+ * Input:
+ * motion: [ax, ay, az, wx, wy, wz] the motion in body frame
+ * q: the current orientation wrt horizontal
+ * bias: the bias of the IMU
+ * Output:
+ * return: the data vector as created by the IMU (with motion, gravity, and bias)
+ */
+ VectorXs motion2data(const VectorXs& body, const Quaternions& q, const VectorXs& bias)
+ {
+ VectorXs data(6);
+ data = body; // start with body motion
+ data.head(3) -= q.conjugate()*gravity(); // add -g
+ data = data + bias; // add bias
+ return data;
+ }
+
+ void setFixedBlocks()
+ {
+ KF_0->getPPtr()->setFixed(p0_fixed);
+ KF_0->getOPtr()->setFixed(q0_fixed);
+ KF_0->getVPtr()->setFixed(v0_fixed);
+ KF_1->getPPtr()->setFixed(p1_fixed);
+ KF_1->getOPtr()->setFixed(q1_fixed);
+ KF_1->getVPtr()->setFixed(v1_fixed);
+ }
+
+
+
+ /* Integrate acc and angVel motion, obtain Delta_preintegrated
+ * Input:
+ * N: number of steps
+ * q0: initial orientaiton
+ * motion: [ax, ay, az, wx, wy, wz] as the true magnitudes in body brame
+ * bias_real: the real bias of the IMU
+ * bias_preint: the bias used for Delta pre-integration
+ * Output:
+ * return: the preintegrated delta
+ */
+ VectorXs integrateDelta(int N, const Quaternions& q0, const VectorXs& motion, const VectorXs& bias_real, const VectorXs& bias_preint, Scalar dt)
+ {
+ VectorXs data(6);
+ VectorXs body(6);
+ VectorXs delta(10);
+ VectorXs Delta(10), Delta_plus(10);
+ Delta = imu::identity();
+ Quaternions q(q0);
+ for (int n = 0; n<N; n++)
+ {
+ data = motion2data(motion, q, bias_real);
+ q = q*exp_q(motion.tail(3)*dt);
+ body = data - bias_preint;
+ delta = imu::body2delta(body, dt);
+ Delta_plus = imu::compose(Delta, delta, dt);
+ Delta = Delta_plus;
+ }
+ return Delta;
+ }
+
+ /* Integrate acc and angVel motion, obtain Delta_preintegrated
+ * Input:
+ * N: number of steps
+ * q0: initial orientaiton
+ * motion: [ax, ay, az, wx, wy, wz] as the true magnitudes in body brame
+ * 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
+ */
+ VectorXs integrateDelta(int N, const Quaternions& q0, const VectorXs& motion, const VectorXs& bias_real, const VectorXs& bias_preint, Scalar dt, Matrix<Scalar, 9, 6>& J_D_b)
+ {
+ VectorXs data(6);
+ VectorXs body(6);
+ VectorXs delta(10);
+ Matrix<Scalar, 9, 6> J_d_d, J_d_b;
+ Matrix<Scalar, 9, 9> J_D_D, J_D_d;
+ VectorXs Delta(10), Delta_plus(10);
+ Quaternions q;
+
+ Delta = imu::identity();
+ J_D_b.setZero();
+ q = q0;
+ for (int n = 0; n<N; n++)
+ {
+ // Simulate data
+ data = motion2data(motion, q, bias_real);
+ q = q*exp_q(motion.tail(3)*dt);
+ // Motion::integrateOneStep()
+ { // IMU::computeCurrentDelta
+ body = data - bias_preint;
+ imu::body2delta(body, dt, delta, J_d_d);
+ J_d_b = - J_d_d;
+ }
+ { // IMU::deltaPlusDelta
+ imu::compose(Delta, delta, dt, Delta_plus, J_D_D, J_D_d);
+ }
+ // Motion:: jac calib
+ J_D_b = J_D_D*J_D_b + J_D_d*J_d_b;
+ // Motion:: buffer
+ Delta = Delta_plus;
+ }
+ return Delta;
+ }
+
+ void integrateWithProcessor(int N, const TimeStamp& t0, const Quaternions q0, const VectorXs& motion, const VectorXs& bias_real, const VectorXs& bias_preint, Scalar dt, VectorXs& D_preint, VectorXs& D_corrected)
+ {
+ data = motion2data(motion, q0, bias_real);
+ capture_imu = std::make_shared<CaptureIMU>(t0, sensor_imu, data, sensor_imu->getNoiseCov());
+ q = q0;
+ t = t0;
+ for (int i= 0; i < N; i++)
+ {
+ t += dt;
+ data = motion2data(motion, q, bias_real);
+ q = q * exp_q(w*dt);
+
+ capture_imu->setTimeStamp(t);
+ capture_imu->setData(data);
+
+ sensor_imu->process(capture_imu);
+
+ D_preint = processor_imu->getLastPtr()->getDeltaPreint();
+ D_corrected = processor_imu->getLastPtr()->getDeltaCorrected(bias_real);
+ }
+ }
+
+ bool configure()
+ {
+ DT = num_integrations * dt;
+ x0 << p0, q0.coeffs(), v0;
+ P0 .setIdentity() * 0.01;
+ KF_0 = problem->setPrior(x0, P0, t0);
+ C_0 = processor_imu->getOriginPtr();
+ CM_1 = processor_imu->getLastPtr();
+ KF_1 = CM_1->getFramePtr();
+ motion << a, w;
+
+ processor_imu->getLastPtr()->setCalibrationPreint(bias_preint);
+
+ return true;
+ }
+
+ void integrateAll()
+ {
+ // ===================================== INTEGRATE EXACTLY WITH IMU_TOOLS with no bias at all
+ D_exact = integrateDelta(num_integrations, q0, motion, bias_null, bias_null, dt);
+ x_exact = imu::composeOverState(x0, D_exact, DT );
+
+
+ // ===================================== INTEGRATE USING IMU_TOOLS
+ // pre-integrate
+ D_preint_imu = integrateDelta(num_integrations, q0, motion, bias_real, bias_preint, dt, J_b);
+
+ // correct perturbated
+ step = J_b * (bias_real - bias_preint);
+ D_corrected_imu = imu::plus(D_preint_imu, step);
+
+ // compose states
+ x_preint_imu = imu::composeOverState(x0, D_preint_imu , DT );
+ x_corrected_imu = imu::composeOverState(x0, D_corrected_imu , DT );
+
+ // ===================================== INTEGRATE USING PROCESSOR
+
+ integrateWithProcessor(num_integrations, t0, q0, motion, bias_real, bias_preint, dt, D_preint, D_corrected);
+
+ // compose states
+ x_preint = imu::composeOverState(x0, D_preint , DT );
+ x_corrected = imu::composeOverState(x0, D_corrected , DT );
+ }
+
+ void buildProblem()
+ {
+ // ===================================== SET KF in Wolf tree
+ FrameBasePtr KF = problem->emplaceFrame(KEY_FRAME, x_exact, t);
+ processor_imu->keyFrameCallback(KF, 0.01);
+
+ KF_1 = problem->getLastKeyFramePtr();
+ C_1 = KF_1->getCaptureList().back();
+ CM_1 = std::static_pointer_cast<CaptureMotion>(C_1);
+
+ // ===================================== SET BOUNDARY CONDITIONS
+ setFixedBlocks();
+ }
+
+ string solveProblem(int verbose = 1)
+ {
+ string report = ceres_manager->solve(verbose);
+
+ bias_0 = C_0->getCalibration();
+ bias_1 = C_1->getCalibration();
+
+ // ===================================== GET INTEGRATED STATE WITH SOLVED BIAS
+ // with processor
+ D_optim = CM_1->getDeltaCorrected(bias_0);
+ x_optim = processor_imu->getCurrentState();
+
+ // with imu_tools
+ step = J_b * (bias_0 - bias_preint);
+ D_optim_imu = imu::plus(D_preint, step);
+ x_optim_imu = imu::composeOverState(x0, D_optim, DT);
+
+ return report;
+ }
+
+ string run(int verbose)
+ {
+ string report;
+ configure();
+ integrateAll();
+ buildProblem();
+ report = solveProblem(verbose);
+ return report;
+ }
+
+
+};
+
+TEST_F(ConstraintIMU_biasTest, VarB1B2V2_InvarP1Q1V1P2Q2)
+{
+
+ // ================================================================================================================ //
+ // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
+ // ================================================================================================================ //
+ //
+ // ---------- time
+ t0 = 0;
+ dt = 0.01;
+ num_integrations = 50;
+
+ // ---------- initial pose
+ p0 << 0,0,0;
+ q0 .setIdentity();
+ v0 << 0,0,0;
+
+ // ---------- bias
+ bias_real << .001, .002, .003, -.001, -.002, -.003;
+ bias_preint = -bias_real;
+
+ // ---------- motion params
+ a << 1,2,3;
+ w << 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 = run(1);
+ cout << report << endl;
+
+ WOLF_TRACE("w * DT (rather be lower than 1.507 approx) = ", w.transpose() * DT); // beware if w*DT is large (>~ 1.507) then Jacobian for correction is poor
+
+
+ // ===================================== PRINT RESULTS
+ 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("X_exact : ", x_exact .transpose() ); // exact state
+ WOLF_TRACE("X_preint : ", x_preint .transpose() ); // state using delta preintegrated by processor
+ WOLF_TRACE("X_preint_imu : ", x_preint_imu .transpose() ); // state using delta preintegrated by imu_tools
+ WOLF_TRACE("X_correct : ", x_corrected .transpose() ); // corrected state vy processor
+ WOLF_TRACE("X_correct_imu: ", x_corrected_imu .transpose() ); // corrected state by imu_tools
+ WOLF_TRACE("X_optim : ", x_optim .transpose() ); // optimal state after solving using processor
+ WOLF_TRACE("X_optim_imu : ", x_optim_imu .transpose() ); // optimal state after solving using imu_tools
+ WOLF_TRACE("err_optim_imu: ", (x_optim_imu - x_exact).transpose() ); // error of optimal state corrected by imu_tools w.r.t. exact state
+
+
+ // ===================================== CHECK ALL (SEE RESULTS SECTION BELOW FOT THE MEANING OF ALL VARIABLES)
+
+ // 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(x_corrected_imu, x_exact , 1e-5 );
+ ASSERT_MATRIX_APPROX(x_corrected , x_exact , 1e-5 );
+
+ // check optimal solutions
+ 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(x_optim , x_exact , 1e-5 );
+ ASSERT_MATRIX_APPROX(D_optim_imu, D_exact , 1e-5 );
+ ASSERT_MATRIX_APPROX(x_optim_imu, x_exact , 1e-5 );
+
+}
+
+
+
// tests with following conditions :
// var(b1,b2), invar(p1,q1,v1,p2,q2,v2), factor : imu(p,q,v)
@@ -2840,11 +3228,11 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2P2Q2V2_InvarP1Q1V
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
- ::testing::GTEST_FLAG(filter) = "ConstraintIMU_biasTest_Move_NonNullBiasRot.*:ConstraintIMU_biasTest_Static_NullBias.*:ConstraintIMU_biasTest_Static_NonNullAccBias.*:ConstraintIMU_biasTest_Static_NonNullGyroBias.*";
+ ::testing::GTEST_FLAG(filter) = "ConstraintIMU_biasTest.*:ConstraintIMU_biasTest_Move_NonNullBiasRot.*:ConstraintIMU_biasTest_Static_NullBias.*:ConstraintIMU_biasTest_Static_NonNullAccBias.*:ConstraintIMU_biasTest_Static_NonNullGyroBias.*";
// ::testing::GTEST_FLAG(filter) = "ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY.VarB1B2V1V2_InvarP1Q1P2Q2_initOK";
// ::testing::GTEST_FLAG(filter) = "ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot.VarB1B2_InvarP1Q1V1P2Q2V2_initOK";
// ::testing::GTEST_FLAG(filter) = "ConstraintIMU_biasTest_Move_NonNullBiasRot.VarB1B2V1P2V2_InvarP1Q1Q2_initOK";
-
+// ::testing::GTEST_FLAG(filter) = "ConstraintIMU_biasTest.*";
return RUN_ALL_TESTS();