Improve KF creation for IMU test

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;
 

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);
 

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 :