From 9b9557988230d24980ff19a8fbf855245f09241f Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?M=C3=A9d=C3=A9ric=20Fourmy?= <mfourmy@laas.fr>
Date: Wed, 10 Mar 2021 13:45:49 +0100
Subject: [PATCH] fix factor error() method (not in gtest) + gtest_imu use
perturb
---
include/imu/factor/factor_imu.h | 8 ++++----
test/gtest_imu.cpp | 26 ++++----------------------
2 files changed, 8 insertions(+), 26 deletions(-)
diff --git a/include/imu/factor/factor_imu.h b/include/imu/factor/factor_imu.h
index 17c803162..a7ff2d7b0 100644
--- a/include/imu/factor/factor_imu.h
+++ b/include/imu/factor/factor_imu.h
@@ -235,9 +235,9 @@ Eigen::Vector9d FactorImu::error()
Map<const Vector3d > acc_bias(bias.data());
Map<const Vector3d > gyro_bias(bias.data() + 3);
- Eigen::Vector9d delta_exp = expectation();
+ Eigen::Vector10d delta_exp = expectation();
- Eigen::Vector9d delta_preint = getMeasurement();
+ Eigen::Vector10d delta_preint = getMeasurement();
Eigen::Vector9d delta_step;
@@ -247,9 +247,9 @@ Eigen::Vector9d FactorImu::error()
Eigen::VectorXd delta_corr = imu::plus(delta_preint, delta_step);
- Eigen::Vector9d res = imu::diff(delta_exp, delta_corr);
+ Eigen::Vector9d err = imu::diff(delta_exp, delta_corr);
- return res;
+ return err;
}
template<typename D1, typename D2, typename D3>
diff --git a/test/gtest_imu.cpp b/test/gtest_imu.cpp
index 161157b8e..d666285f4 100644
--- a/test/gtest_imu.cpp
+++ b/test/gtest_imu.cpp
@@ -475,28 +475,10 @@ class Process_Factor_Imu : public testing::Test
{
// This perturbs states to estimate around the exact value, then assigns to the keyframe
// Perturbations are applied only if the state block is unfixed
-
- VectorXd x_pert(10);
-
- // KF 0
- x_pert = x0;
- if (!p0_fixed)
- x_pert.head(3) += Vector3d::Random() * 0.01;
- if (!q0_fixed)
- x_pert.segment(3,4) = (Quaterniond(x_pert.data() + 3) * exp_q(Vector3d::Random() * 0.01)).coeffs().normalized();
- if (!v0_fixed)
- x_pert.tail(3) += Vector3d::Random() * 0.01;
- KF_0->setState(x_pert);
-
- // KF 1
- x_pert = x1_exact;
- if (!p1_fixed)
- x_pert.head(3) += Vector3d::Random() * 0.01;
- if (!q1_fixed)
- x_pert.segment(3,4) = (Quaterniond(x_pert.data() + 3) * exp_q(Vector3d::Random() * 0.01)).coeffs().normalized();
- if (!v1_fixed)
- x_pert.tail(3) += Vector3d::Random() * 0.01;
- KF_1->setState(x_pert);
+ KF_0->setState(x0);
+ KF_0->perturb();
+ KF_1->setState(x1_exact);
+ KF_1->perturb();
}
virtual void buildProblem()
--
GitLab