Merge branch 'devel' into 'main'

Devel

See merge request !56

CMakeLists.txt

 # Pre-requisites about cmake itself
-CMAKE_MINIMUM_REQUIRED(VERSION 3.10)
+CMAKE_MINIMUM_REQUIRED(VERSION 3.16)
 
 # MAC OSX RPATH
 SET(CMAKE_MACOSX_RPATH 1)

include/imu/math/imu2d_tools.h

@@ -183,7 +183,6 @@ inline void compose(const MatrixBase<D1>& d1,
     Map<const Matrix<typename D2::Scalar, 2, 1> >   dp2    ( & d2( 0 ) );
     Map<const Matrix<typename D2::Scalar, 2, 1> >   dv2    ( & d2( 3 ) );
     Map<Matrix<typename D3::Scalar, 2, 1> >         sum_p  ( & sum( 0 ) );
-    sum(2)                                          =       d1(2) + d2(2);
     Map<Matrix<typename D3::Scalar, 2, 1> >         sum_v  ( & sum( 3 ) );
 
     compose(dp1, d1(2), dv1, dp2, d2(2), dv2, dt, sum_p, sum(2), sum_v);

test/gtest/CMakeLists.txt

-if(${CMAKE_VERSION} VERSION_LESS "3.11.0") 
-  message("CMake version less than 3.11.0")
 
-  # Enable ExternalProject CMake module
-  include(ExternalProject)
+include(FetchContent)
 
-  set(GTEST_FORCE_SHARED_CRT ON)
-  set(GTEST_DISABLE_PTHREADS ON) # without this in ubuntu 18.04 we get linking errors
+FetchContent_Declare(
+  googletest
+  GIT_REPOSITORY https://github.com/google/googletest.git 
+  GIT_TAG main)
 
-  # Download GoogleTest
-  ExternalProject_Add(googletest
-      GIT_REPOSITORY https://github.com/google/googletest.git
-      GIT_TAG        v1.8.x
-      # TIMEOUT 1 # We'll try this
-      CMAKE_ARGS -DCMAKE_ARCHIVE_OUTPUT_DIRECTORY_DEBUG:PATH=DebugLibs
-      -DCMAKE_ARCHIVE_OUTPUT_DIRECTORY_RELEASE:PATH=ReleaseLibs
-      -DCMAKE_CXX_FLAGS=${MSVC_COMPILER_DEFS}
-      -Dgtest_force_shared_crt=${GTEST_FORCE_SHARED_CRT}
-      -Dgtest_disable_pthreads=${GTEST_DISABLE_PTHREADS}
-      -DBUILD_GTEST=ON
-      PREFIX "${CMAKE_CURRENT_BINARY_DIR}"
-      # Disable install step
-      INSTALL_COMMAND ""
-      UPDATE_DISCONNECTED 1 # 1: do not update googletest; 0: update googletest via github
-  )
+SET(INSTALL_GTEST OFF) # Disable installation of googletest
+FetchContent_MakeAvailable(googletest)
 
-  # Get GTest source and binary directories from CMake project
-
-  # Specify include dir
-  ExternalProject_Get_Property(googletest source_dir)
-  set(GTEST_INCLUDE_DIRS ${source_dir}/googletest/include PARENT_SCOPE)
-
-  # Specify MainTest's link libraries
-  ExternalProject_Get_Property(googletest binary_dir)
-  set(GTEST_LIBS_DIR ${binary_dir}/googlemock/gtest PARENT_SCOPE)
-
-  # Create a libgtest target to be used as a dependency by test programs
-  add_library(libgtest IMPORTED STATIC GLOBAL)
-  add_dependencies(libgtest googletest)
-
-  # Set libgtest properties
-  set_target_properties(libgtest PROPERTIES
-      "IMPORTED_LOCATION" "${binary_dir}/googlemock/gtest/libgtest.a"
-      "IMPORTED_LINK_INTERFACE_LIBRARIES" "${CMAKE_THREAD_LIBS_INIT}"
-  )
-
-else()
-
-  message("CMake version equal or greater than 3.11.0")
+function(wolf_add_gtest target)
+  add_executable(${target} ${ARGN})
+  target_link_libraries(${target} gtest_main ${PLUGIN_NAME})
+  add_test(NAME ${target} COMMAND ${target})
+endfunction()
+include(FetchContent)
 
-  include(FetchContent)
+FetchContent_Declare(
+  googletest
+  GIT_REPOSITORY https://github.com/google/googletest.git 
+  GIT_TAG main)
 
-  FetchContent_Declare(
-    googletest
-    GIT_REPOSITORY https://github.com/google/googletest.git 
-    GIT_TAG main)
+SET(INSTALL_GTEST OFF) # Disable installation of googletest
+FetchContent_MakeAvailable(googletest)
 
-  SET(INSTALL_GTEST OFF) # Disable installation of googletest
-  FetchContent_MakeAvailable(googletest)
-    
-endif()
-  
 function(wolf_add_gtest target)
   add_executable(${target} ${ARGN})
-  if(${CMAKE_VERSION} VERSION_LESS "3.11.0") 
-    add_dependencies(${target} libgtest)
-    target_link_libraries(${target} libgtest ${PLUGIN_NAME})
-    target_include_directories(${target} PUBLIC ${GTEST_INCLUDE_DIRS})
-  else()
-    target_link_libraries(${target} gtest_main ${PLUGIN_NAME})
-  endif()
+  target_link_libraries(${target} gtest_main ${PLUGIN_NAME})
   add_test(NAME ${target} COMMAND ${target})
 endfunction()

test/gtest_feature_imu.cpp

@@ -81,15 +81,13 @@ class FeatureImu_test : public testing::Test
         origin_frame = problem->setPriorFactor(x0, s0, t);
         processor_motion_ptr_->setOrigin(origin_frame);
 
-    // Emplace one capture to store the Imu data arriving from (sensor / callback / file / etc.)
+    // Create one capture to store the Imu data arriving from (sensor / callback / file / etc.)
     // give the capture a big covariance, otherwise it will be so small that it won't pass following assertions
-        imu_ptr = std::static_pointer_cast<CaptureImu>(
-                CaptureBase::emplace<CaptureImu>(origin_frame,
-                                                 t,
-                                                 sensor_ptr,
-                                                 data_,
-                                                 Eigen::Matrix6d::Identity(),
-                                                 Eigen::Vector6d::Zero().eval()) );
+    imu_ptr = std::make_shared<CaptureImu>( t,
+                                            sensor_ptr,
+                                            data_,
+                                            Eigen::Matrix6d::Identity(),
+                                            Eigen::Vector6d::Zero().eval());
 
     //process data
         data_ << 2, 0, 9.8, 0, 0, 0;
@@ -102,22 +100,21 @@ class FeatureImu_test : public testing::Test
     // process data in capture
         sensor_ptr->process(imu_ptr);
 
-    //emplace Frame
-        ts          = problem->getTimeStamp();
-        state_vec   = problem->getState().vector(problem->getFrameStructure());
-        last_frame  = problem->emplaceFrame(ts, state_vec);
 
     //emplace a feature
         delta_preint            = processor_motion_ptr_->getMotion().delta_integr_;
         delta_preint_cov        = processor_motion_ptr_->getMotion().delta_integr_cov_ + MatrixXd::Identity(9,9)*1e-08;
         VectorXd calib_preint   = processor_motion_ptr_->getLast()->getCalibrationPreint();
         dD_db_jacobians         = processor_motion_ptr_->getMotion().jacobian_calib_;
-        feat_imu                = FeatureBase::emplace<FeatureImu>(imu_ptr,
+        feat_imu                = FeatureBase::emplace<FeatureImu>(processor_motion_ptr_->getOrigin(),
                                                                    delta_preint,
                                                                    delta_preint_cov,
                                                                    calib_preint,
                                                                    dD_db_jacobians,
-                                                                   imu_ptr) ;
+                                                                   processor_motion_ptr_->getOrigin()) ;
+
+        problem->print(4,1,1,1);        
+
     }
 
     void TearDown() override
@@ -155,7 +152,7 @@ TEST_F(FeatureImu_test, check_frame)
     left_vptr = left_frame->getV();
 
     ASSERT_MATRIX_APPROX(origin_pptr->getState(), left_pptr->getState(), wolf::Constants::EPS_SMALL);
-    Eigen::Map<const Eigen::Quaterniond> origin_Quat(origin_optr->getState().data()), left_Quat(left_optr->getState().data());
+    Eigen::Quaterniond origin_Quat(origin_optr->getState().data()), left_Quat(left_optr->getState().data());
     ASSERT_QUATERNION_APPROX(origin_Quat, left_Quat, wolf::Constants::EPS_SMALL);
     ASSERT_MATRIX_APPROX(origin_vptr->getState(), left_vptr->getState(), wolf::Constants::EPS_SMALL);