create gtest_feature_imu file init

src/test/CMakeLists.txt

@@ -87,4 +87,8 @@ target_link_libraries(gtest_trajectory ${PROJECT_NAME})
 
 # ProcessorIMU_jacobians test
 wolf_add_gtest(gtest_processorIMU_jacobians gtest_processorIMU_jacobians.cpp)
-target_link_libraries(gtest_processorIMU_jacobians ${PROJECT_NAME})
+target_link_libraries(gtest_processorIMU_jacobians ${PROJECT_NAME})
\ No newline at end of file
+
+# FeatureIMU test
+wolf_add_gtest(gtest_feature_imu gtest_feature_imu.cpp)
+target_link_libraries(gtest_feature_imu ${PROJECT_NAME})
\ No newline at end of file

src/test/gtest_feature_imu.cpp

+//Wolf
+#include "wolf.h"
+#include "problem.h"
+#include "sensor_imu.h"
+#include "capture_imu.h"
+#include "constraint_odom_3D.h"
+#include "state_block.h"
+#include "state_quaternion.h"
+#include "processor_imu.h"
+
+//#define DEBUG_RESULTS
+
+
+class FeatureIMU : public testing::Test
+{
+
+    public: //These can be accessed in fixtures
+        wolf::ProblemPtr wolf_problem_ptr_;
+        wolf::TimeStamp ts;
+
+    //a new of this will be created for each new test
+    virtual void SetUp()
+    {
+        using namespace wolf;
+        using std::shared_ptr;
+        using std::make_shared;
+        using std::static_pointer_cast;
+
+        // Wolf problem
+        wolf_problem_ptr_ = Problem::create(FRM_PQVBB_3D);
+        Eigen::VectorXs IMU_extrinsics(7);
+        IMU_extrinsics << 0,0,0, 0,0,0,1; // IMU pose in the robot
+        SensorBasePtr sensor_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", IMU_extrinsics, shared_ptr<IntrinsicsBase>());
+        wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", "");
+
+    // Time and data variables
+        TimeStamp t;
+        Eigen::Vector6s data_;
+        Scalar mpu_clock = 0;
+
+        t.set(mpu_clock);
+
+    // Set the origin
+        Eigen::VectorXs x0(16);
+        x0 << 0,0,0,  0,0,0,1,  0,0,0,  0,0,.001,  0,0,.002; // Try some non-zero biases
+        wolf_problem_ptr_->getProcessorMotionPtr()->setOrigin(x0, t);
+
+    //create a keyframe at origin
+        ts = wolf_problem_ptr_->getProcessorMotionPtr()->getBuffer().get().back().ts_;
+        Eigen::VectorXs origin_state = x0;
+        FrameBasePtr origin_frame = std::make_shared<FrameIMU>(KEY_FRAME, ts, origin_state);
+        wolf_problem_ptr_->getTrajectoryPtr()->addFrame(origin_frame);
+    
+    // Create one capture to store the IMU data arriving from (sensor / callback / file / etc.)
+        CaptureIMUPtr imu_ptr( std::make_shared<CaptureIMU>(t, sensor_ptr, data_) );
+        imu_ptr->setFramePtr(origin_frame);
+
+    //process data
+        mpu_clock = 0.001003;
+        data_ << 0.579595, -0.143701, 9.939331, 0.127445, 0.187814, -0.055003;
+        t.set(mpu_clock);
+    // assign data to capture
+        imu_ptr->setData(data_);
+        imu_ptr->setTimeStamp(t);
+    // process data in capture
+        sensor_ptr->process(imu_ptr);
+
+
+
+    }
+
+    virtual void TearDown()
+    {
+        // code here will be called just after the test completes
+        // ok to through exceptions from here if need be
+        /*
+            You can do deallocation of resources in TearDown or the destructor routine. 
+                However, if you want exception handling you must do it only in the TearDown code because throwing an exception 
+                from the destructor results in undefined behavior.
+            The Google assertion macros may throw exceptions in platforms where they are enabled in future releases. 
+                Therefore, it's a good idea to use assertion macros in the TearDown code for better maintenance.
+        */
+    }
+};
+
+TEST_F(FeatureIMU, test0)
+{
+    // set variables
+    using namespace std;
+    Eigen::VectorXs state_vec;
+    Eigen::VectorXs delta_preint;
+    //FrameIMUPtr last_frame;
+    Eigen::Matrix<wolf::Scalar,9,9> delta_preint_cov;
+
+    //create the constraint
+        //create FrameIMU
+    ts = wolf_problem_ptr_->getProcessorMotionPtr()->getBuffer().get().back().ts_;
+    state_vec = wolf_problem_ptr_->getProcessorMotionPtr()->getCurrentState();
+    FrameIMUPtr last_frame = std::make_shared<FrameIMU>(KEY_FRAME, ts, state_vec);
+    wolf_problem_ptr_->getTrajectoryPtr()->addFrame(last_frame);
+
+        //create a feature
+    delta_preint_cov = wolf_problem_ptr_->getProcessorMotionPtr()->getCurrentDeltaPreintCov();
+    delta_preint = wolf_problem_ptr_->getProcessorMotionPtr()->getMotion().delta_integr_;
+    std::shared_ptr<FeatureIMU> feat_imu = std::make_shared<FeatureIMU>(delta_preint, delta_preint_cov);
+    feat_imu->setCapturePtr(imu_ptr);
+
+        //create a constraintIMU
+    //ConstraintIMUPtr constraint_imu = std::make_shared<ConstraintIMU>(feat_imu, last_frame);
+    //FIXME : Feature not linked to origin frame
+    FrameBasePtr frame_base = feat_imu->getFramePtr();
+    //ConstraintIMU constraint_imu(feat_imu, last_frame);
+    //feat_imu->addConstraint(constraint_imu);
+    //previous_frame->addConstrainedBy(constraint_imu);
+}
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}