diff --git a/src/test/CMakeLists.txt b/src/test/CMakeLists.txt index 27d9c93d5fe3771bbf888a8fa0db5149720d49e1..b36a0d94be4f90a0a349314193bdda29f80723e5 100644 --- a/src/test/CMakeLists.txt +++ b/src/test/CMakeLists.txt @@ -85,3 +85,6 @@ target_link_libraries(gtest_time_stamp ${PROJECT_NAME}) wolf_add_gtest(gtest_trajectory gtest_trajectory.cpp) 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}) \ No newline at end of file diff --git a/src/test/gtest_processorIMU_jacobians.cpp b/src/test/gtest_processorIMU_jacobians.cpp index 64d21e0a5b6c5c17275960f056d22f02cfd11031..6762c596450333b81ab147d8b43e79f2d91038fe 100644 --- a/src/test/gtest_processorIMU_jacobians.cpp +++ b/src/test/gtest_processorIMU_jacobians.cpp @@ -5,4 +5,62 @@ * \author: AtDinesh */ - \ No newline at end of file + //Wolf +#include "wolf.h" +#include "problem.h" +#include "sensor_imu.h" +#include "capture_imu.h" +#include "state_block.h" +#include "state_quaternion.h" +#include "processor_imu_UnitTester.h" + +//std +#include <iostream> +#include <fstream> +#include <iomanip> +#include <ctime> +#include <cmath> + +//google test +#include "utils_gtest.h" + +//#define DEBUG_RESULTS +//#define WRITE_RESULTS + +using namespace wolf; + +// A new one of these is created for each test +class ProcessorIMU_jacobians : public testing::Test +{ + public: + TimeStamp t; + Eigen::Vector6s data_; + + virtual void SetUp() + { + wolf::Scalar deg_to_rad = M_PI/180.0; + data_ << 10,0.5,3, 100*deg_to_rad,110*deg_to_rad,30*deg_to_rad; + + // Wolf problem + ProblemPtr 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 + //SensorBase* sensor_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", IMU_extrinsics, nullptr); + //wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", ""); + } + + virtual void TearDown(){} +}; + +TEST_F(ProcessorIMU_jacobians, Dummy) +{ + ASSERT_TRUE(data_.size() == 6); +} + +int main(int argc, char **argv) +{ + using namespace wolf; + + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} \ No newline at end of file