From 83fb9cb0babd7f38909b438f6bdc458b360165e6 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Mon, 13 Nov 2017 12:17:55 +0100
Subject: [PATCH] Improve KF creation for IMU test
---
src/processor_imu.h | 1 -
src/processor_motion.h | 4 ----
src/test/gtest_constraint_imu.cpp | 32 ++++++++++++++++---------------
3 files changed, 17 insertions(+), 20 deletions(-)
diff --git a/src/processor_imu.h b/src/processor_imu.h
index 82abfa1da..47decd316 100644
--- a/src/processor_imu.h
+++ b/src/processor_imu.h
@@ -72,7 +72,6 @@ class ProcessorIMU : public ProcessorMotion{
const MatrixXs& _data_cov,
const FrameBasePtr& _frame_origin) override;
virtual FeatureBasePtr createFeature(CaptureMotionPtr _capture_motion) override;
- public:
virtual ConstraintBasePtr emplaceConstraint(FeatureBasePtr _feature_motion,
CaptureBasePtr _capture_origin) override;
diff --git a/src/processor_motion.h b/src/processor_motion.h
index a95243fbd..412b36f64 100644
--- a/src/processor_motion.h
+++ b/src/processor_motion.h
@@ -374,22 +374,18 @@ class ProcessorMotion : public ProcessorBase
const MatrixXs& _data_cov,
const FrameBasePtr& _frame_origin) = 0;
- public:
/** \brief create a feature corresponding to given capture and add the feature to this capture
* \param _capture_motion: the parent capture
*/
FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_own);
- protected:
virtual FeatureBasePtr createFeature(CaptureMotionPtr _capture_own) = 0;
- public:
/** \brief create a constraint and link it in the wolf tree
* \param _feature_motion: the parent feature
* \param _frame_origin: the frame constrained by this motion constraint
*/
virtual ConstraintBasePtr emplaceConstraint(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin) = 0;
- protected:
Motion motionZero(const TimeStamp& _ts);
CaptureMotionPtr getCaptureMotionContainingTimeStamp(const TimeStamp& _ts);
diff --git a/src/test/gtest_constraint_imu.cpp b/src/test/gtest_constraint_imu.cpp
index 18f2ed633..b63126439 100644
--- a/src/test/gtest_constraint_imu.cpp
+++ b/src/test/gtest_constraint_imu.cpp
@@ -1346,7 +1346,7 @@ class ConstraintIMU_biasTest : public testing::Test
SensorIMUPtr sensor_imu;
ProcessorIMUPtr processor_imu;
FrameBasePtr KF_0, KF_1;
- CaptureBasePtr C_0;
+ CaptureBasePtr C_0, C_1;
CaptureMotionPtr CM_0, CM_1;
CaptureIMUPtr capture_imu;
CeresManagerPtr ceres_manager;
@@ -1375,9 +1375,12 @@ class ConstraintIMU_biasTest : public testing::Test
// 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.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
@@ -1521,7 +1524,7 @@ TEST_F(ConstraintIMU_biasTest, VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
a << 1,2,3;
w << 1,2,3;
- WOLF_TRACE("w * DT (rather be lower than 1.507 approx) = ", (DT*w).transpose()); // beware if w*DT is large (>~ 1.507) then Jacobian for correction is poor
+ 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
motion << a, w;
@@ -1580,8 +1583,11 @@ TEST_F(ConstraintIMU_biasTest, VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
ASSERT_MATRIX_APPROX(imu::composeOverState(x0, D_corrected_imu , DT ), x_exact, 1e-5);
// ================================ SET KF AND SOLVE
- KF_1->setState(x_exact);
- KF_1->setKey();
+ 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();
KF_0->getPPtr()->fix();
KF_0->getOPtr()->fix();
@@ -1590,15 +1596,10 @@ TEST_F(ConstraintIMU_biasTest, VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
KF_1->getOPtr()->fix();
KF_1->getVPtr()->fix();
- FeatureBasePtr ft_1 = processor_imu->emplaceFeature(CM_1);
- processor_imu->emplaceConstraint(ft_1, C_0);
-
-// problem->print(4,1,1,1);
-
- string report = ceres_manager->solve(1);
+ string report = ceres_manager->solve(2);
- Vector6s bias_0 = C_0 ->getCalibration();
- Vector6s bias_1 = CM_1->getCalibration();
+ Vector6s bias_0 = C_0->getCalibration();
+ Vector6s bias_1 = C_1->getCalibration();
WOLF_TRACE("bias preint : ", bias_preint.transpose());
WOLF_TRACE("bias real : ", bias_real .transpose());
@@ -1619,6 +1620,7 @@ TEST_F(ConstraintIMU_biasTest, VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
ASSERT_MATRIX_APPROX(x_optim, x_exact, 1e-5);
+ cout << report << endl;
}
// tests with following conditions :
--
GitLab