start work on test to fix the static initialization

demos/imu2d_static_init.yaml

+type: "ProcessorImu2d"              # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+
+time_tolerance: 0.0025         # Time tolerance for joining KFs
+unmeasured_perturbation_std: 0.00001
+
+keyframe_vote:
+    voting_active:      true
+    voting_aux_active:  false
+    max_time_span:      1.0   # seconds
+    max_buff_length:  20000    # motion deltas
+    dist_traveled:      2.0   # meters
+    angle_turned:       0.2   # radians (1 rad approx 57 deg, approx 60 deg)

test/CMakeLists.txt

@@ -37,6 +37,9 @@ target_link_libraries(gtest_feature_imu ${PLUGIN_NAME} ${wolf_LIBRARY})
 wolf_add_gtest(gtest_factor_imu2d gtest_factor_imu2d.cpp)
 target_link_libraries(gtest_factor_imu2d ${PLUGIN_NAME} ${wolf_LIBRARY})
 
+wolf_add_gtest(gtest_imu2d_static_init gtest_imu2d_static_init.cpp)
+target_link_libraries(gtest_imu2d_static_init ${PLUGIN_NAME} ${wolf_LIBRARY})
+
 wolf_add_gtest(gtest_imu_mocap_fusion gtest_imu_mocap_fusion.cpp)
 target_link_libraries(gtest_imu_mocap_fusion ${PLUGIN_NAME} ${wolf_LIBRARY})
 

test/gtest_imu2d_static_init.cpp

+#include <core/ceres_wrapper/solver_ceres.h>
+#include <core/utils/utils_gtest.h>
+#include "imu/internal/config.h"
+
+#include "imu/factor/factor_imu2d.h"
+#include "imu/math/imu2d_tools.h"
+#include "imu/sensor/sensor_imu2d.h"
+#include "core/capture/capture_void.h"
+#include <core/factor/factor_relative_pose_2d.h>
+
+using namespace Eigen;
+using namespace wolf;
+
+class ProcessorImu2dStaticInitTest : public testing::Test
+{
+
+    public: //These can be accessed in fixtures
+        wolf::ProblemPtr _problem_ptr;
+        wolf::SensorBasePtr sensor_ptr;
+        wolf::ProcessorMotionPtr processor_motion;
+        wolf::TimeStamp t;
+        wolf::TimeStamp t0;
+        double mti_clock, tmp;
+        double dt;
+        bool second_frame;
+        Vector6d data;
+        Matrix6d data_cov;
+        VectorComposite     x0c;                                // initial state composite
+        VectorComposite     s0c;                                // initial sigmas composite
+        std::shared_ptr<wolf::CaptureImu> C;
+        FrameBasePtr last_frame_;
+
+    //a new of this will be created for each new test
+    void SetUp() override
+    {
+        using namespace Eigen;
+        using std::shared_ptr;
+        using std::make_shared;
+        using std::static_pointer_cast;
+        using namespace wolf::Constants;
+
+        std::string wolf_root = _WOLF_IMU_ROOT_DIR;
+
+        // Wolf problem
+        _problem_ptr = Problem::create("POV", 2);
+        Vector3d extrinsics = (Vector3d() << 0,0, 0).finished();
+        sensor_ptr = _problem_ptr->installSensor("SensorImu2d", "Main Imu", extrinsics,  wolf_root + "/demos/sensor_imu2d.yaml");
+        ProcessorBasePtr processor_ptr = _problem_ptr->installProcessor("ProcessorImu2d", "Imu pre-integrator", "Main Imu", wolf_root + "/demos/imu2d_static_init.yaml");
+        processor_motion = std::static_pointer_cast<ProcessorMotion>(processor_ptr);
+
+        // Time and data variables
+        second_frame = false;
+        dt = 0.01;
+        t0.set(0);
+        t = t0;
+        data = Vector6d::Random();
+        data_cov = Matrix6d::Identity();
+        last_frame_ = nullptr;
+
+        // Create one capture to store the Imu data arriving from (sensor / callback / file / etc.)
+        C = make_shared<CaptureImu>(t, sensor_ptr, data, data_cov, Vector3d::Zero());
+    }
+
+};
+
+//// Input odometry data and covariance
+//Matrix6d data_cov = 0.1*Matrix6d::Identity();
+//Matrix5d delta_cov = 0.1*Matrix5d::Identity();
+//
+//// Jacobian of the bias
+//MatrixXd jacobian_bias((MatrixXd(5,3) << 1, 0, 0,
+//                                         0, 1, 0,
+//                                         0, 0, 1,
+//                                         1, 0, 0,
+//                                         0, 1, 0 ).finished());
+////preintegration bias
+//Vector3d b0_preint = Vector3d::Zero();
+//
+//// Problem and solver
+//ProblemPtr _problem_ptr_ptr = Problem::create("POV", 2);
+//SolverCeres solver(problem_ptr);
+//
+//// Two frames
+//FrameBasePtr frm0 = problem_ptr->emplaceFrame(TimeStamp(0), Vector5d::Zero());
+//FrameBasePtr frm1 = problem_ptr->emplaceFrame(TimeStamp(1), Vector5d::Zero());
+//
+//// Imu2d sensor
+//SensorBasePtr sensor = std::make_shared<SensorImu2d>( Vector3d::Zero(), ParamsSensorImu2d() ); // default params: see sensor_imu2d.h
+//
+////capture from frm1 to frm0
+//auto cap0 = CaptureBase::emplace<CaptureImu>(frm0, 0, sensor, Vector6d::Zero(), data_cov, Vector3d::Zero(), nullptr);
+//auto cap1 = CaptureBase::emplace<CaptureImu>(frm1, 1, sensor, Vector6d::Zero(), data_cov, Vector3d::Zero(), cap0);
+//
+//TEST(imu2d_static_init, check_tree)
+//{
+//    ASSERT_TRUE(problem_ptr->check(0));
+//}
+//
+//
+//TEST(imu2d_static_init, solve_b0)
+//{
+//  for(int i = 0; i < 50; i++){
+//    // Random delta
+//    Vector5d delta_biased = Vector5d::Random() * 10;
+//    delta_biased(2) = pi2pi(delta_biased(2));
+//
+//    // Random initial pose
+//    Vector5d x0 = Vector5d::Random() * 10;
+//    x0(2) = pi2pi(x0(2));
+//
+//    //Random Initial bias
+//    Vector3d b0 = Vector3d::Random();
+//
+//    //Corrected delta
+//    Vector5d delta_step = jacobian_bias*(b0-b0_preint);
+//    Vector5d delta = imu2d::plus(delta_biased, delta_step);
+//
+//    // x1 groundtruth
+//    Vector5d x1;
+//    x1 = imu2d::compose(x0, delta, 1);
+//
+//    // Set states
+//    frm0->setState(x0);
+//    frm1->setState(x1);
+//    frm0->getV()->setZero();
+//    frm0->getV()->fix();
+//    frm1->getV()->setZero();
+//    frm1->getV()->fix();
+//    cap0->getStateBlock('I')->setState(b0);
+//    cap1->getStateBlock('I')->setState(b0);
+//    //WOLF_INFO("states set");
+//
+//    // feature & factor with delta measurement
+//    auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
+//    FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false);
+//
+//    // Fix frm1, perturb frm0
+//    frm0->fix();
+//    cap0->unfix();
+//    frm1->fix();
+//    cap1->fix();
+//    cap0->perturb(0.1);
+//
+//    //do static init stuff on frm1
+//    Eigen::VectorXd zero_motion = Eigen::VectorXd::Zero(3);
+//    //WOLF_INFO("zero_motion created");
+//    for (auto frm_pair =  problem_ptr->getTrajectory()->begin();
+//        frm_pair != problem_ptr->getTrajectory()->end();
+//        frm_pair++)
+//    {
+//      //WOLF_INFO("entered for");
+//      if (frm_pair->second == frm1)
+//        break;
+//      auto capture_zero = CaptureBase::emplace<CaptureVoid>(frm1, frm1->getTimeStamp(), nullptr);
+//      auto feature_zero = FeatureBase::emplace<FeatureBase>(capture_zero,
+//                                                            "FeatureZeroOdom",
+//                                                            zero_motion,
+//                                                            Eigen::MatrixXd::Identity(3,3) * 0.01);
+//      FactorBase::emplace<FactorRelativePose2d>(feature_zero,
+//                                                feature_zero,
+//                                                frm_pair->second,
+//                                                nullptr,
+//                                                false,
+//                                                TOP_MOTION);
+//
+//    }
+//
+//    // solve 
+//    std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+//
+//    ASSERT_POSE2d_APPROX(cap0->getStateVector(), b0, 1e-6);
+//    //WOLF_INFO(cap0->getStateVector());
+//
+//    // remove feature (and factor) for the next loop
+//    fea1->remove();
+//  }
+//}
+
+TEST_F(ProcessorImu2dStaticInitTest, static)
+{
+   Vector2d pos;
+   // Set the origin
+   x0c['P'] = Vector2d(0, 0);
+   x0c['O'] = Vector1d(0);
+   x0c['V'] = Vector2d(0, 0);
+
+   data_cov *= 1e-3;
+   dt = 0.5;
+   auto KF0 = _problem_ptr->setPriorFix(x0c, t0, dt/2);
+   processor_motion->setOrigin(KF0);
+
+   data = Vector6d::Random();
+   WOLF_INFO("Data is: ", data);
+   int size = 3;
+   second_frame = false;
+   FrameBasePtr first_frame = nullptr;
+
+
+   for(t = t0+dt; t <= t0 + 4 + dt/2; t+=dt){
+       if(first_frame) WOLF_INFO("The first frame is ", first_frame, " and it's currently estimated bias is: ", first_frame->getCaptureOf(sensor_ptr)->getStateBlock('I')->getState());
+       WOLF_INFO("Last frame is: ", last_frame_);
+       WOLF_INFO("Current state is: ", processor_motion->getState().vector("POV"));
+       WOLF_INFO("second_frame is: ", second_frame);
+       WOLF_INFO("Currently estimated bias is: ", C->getStateBlock('I')->getState());
+       WOLF_INFO("The current problem is:");
+       _problem_ptr->print(4);
+       C->setTimeStamp(t);
+       C->setData(data);
+       C->setDataCovariance(data_cov);
+       C->process();
+       if(second_frame){      
+         //ASSERT_MATRIX_APPROX(processor_motion->getState().vector("POV"), Vector5d::Zero(), 1e-9);
+       }
+       if (not last_frame_)
+       {
+         last_frame_ = processor_motion->getOrigin()->getFrame();
+         first_frame = last_frame_;
+
+         // impose zero velocity
+         last_frame_->getV()->setZero();
+         last_frame_->getV()->fix();
+
+         // impose zero odometry
+         //processor_motion_->setOdometry(sensor_ptr->getProblem()->stateZero(processor_motion_->getStateStructure()));
+       }
+       else
+       {
+         assert(processor_motion->getOrigin() && "SubscriberImuEnableable::callback: nullptr origin");
+         assert(processor_motion->getOrigin()->getFrame() && "SubscriberImuEnableable::callback: nullptr origin frame");
+
+         // new frame
+         if (last_frame_ != processor_motion->getOrigin()->getFrame())
+         {
+             second_frame = true;
+             last_frame_ = processor_motion->getOrigin()->getFrame();
+
+             // impose zero velocity
+             last_frame_->getV()->setZero();
+             last_frame_->getV()->fix();
+
+             // impose zero odometry
+             //processor_motion_->setOdometry(sensor_ptr->getProblem()->stateZero(processor_motion_->getStateStructure()));
+
+             // add zero displacement and rotation capture & feature & factor with all previous frames
+             assert(sensor_ptr->getProblem());
+             Eigen::VectorXd zero_motion = Eigen::VectorXd::Zero(size);
+             for (auto frm_pair =  sensor_ptr->getProblem()->getTrajectory()->begin();
+                 frm_pair != sensor_ptr->getProblem()->getTrajectory()->end();
+                 frm_pair++)
+             {
+               if (frm_pair->second == last_frame_)
+                 break;
+               auto capture_zero = CaptureBase::emplace<CaptureVoid>(last_frame_, last_frame_->getTimeStamp(), nullptr);
+               auto feature_zero = FeatureBase::emplace<FeatureBase>(capture_zero,
+                   "FeatureZeroOdom",
+                   zero_motion,
+                   Eigen::MatrixXd::Identity(size,size) * 0.01);
+
+               FactorBase::emplace<FactorRelativePose2d>(feature_zero,
+                     feature_zero,
+                     frm_pair->second,
+                     nullptr,
+                     false,
+                     TOP_MOTION);
+
+             }
+           }
+       }
+   }
+}
+
+int main(int argc, char **argv)
+{
+    testing::InitGoogleTest(&argc, argv);
+    //    ::testing::GTEST_FLAG(filter) = "FactorImu2d.no_bias_fixed"; // Test only this one
+    return RUN_ALL_TESTS();
+}