Init commit for bodydynamics plugin + beginning to implement the inertial kinematics factor

b750cf4b · Médéric Fourmy · b750cf4b · b750cf4b · b750cf4b · b750cf4b
Commit b750cf4b authored 5 years ago by Médéric Fourmy
--- a/.gitignore
+++ b/.gitignore
+.cproject
+.project
+.settings/
+README.txt
+bin/
+build/
+build_release/
+lib/
+.idea/
+./Wolf.user
+./Wolf.config
+./Wolf.creator
+./Wolf.files
+./Wolf.includes
+./Wolf/
+/CMakeLists.txt.user
+src/examples/map_polyline_example_write.yaml
+*.gch
+/CMakeFiles/
+/CMakeCache.txt
+*.dat
+.DS_Store
+
+*.graffle
+/Default/
+
+src/CMakeCache.txt
+
+src/CMakeFiles/cmake.check_cache
+src/examples/map_apriltag_save.yaml
+
+\.vscode/
+build_release/
+
+wolf.found
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
+image: segaleran/ceres
+
+before_script:
+  - ls
+  - apt-get update
+  - apt-get install -y build-essential cmake 
+
+# SPDLOG
+#  - apt-get install -y libspdlog-dev
+  - if [ -d spdlog ]; then
+  -   echo "directory exists" 
+  -   if [ "$(ls -A ./spdlog)" ]; then 
+  -     echo "directory not empty" 
+  -     cd spdlog
+  -     git pull
+  -   else 
+  -     echo "directory empty" 
+  -     git clone https://github.com/gabime/spdlog.git
+  -     cd spdlog
+  -   fi
+  - else
+  -   echo "directory inexistent" 
+  -   git clone https://github.com/gabime/spdlog.git
+  -   cd spdlog
+  - fi
+  - git fetch
+  - git checkout v0.17.0
+  - mkdir -pv build
+  - cd build
+  - ls
+  - cmake -DCMAKE_CXX_FLAGS="${CMAKE_CXX_FLAGS} -fPIC" -DSPDLOG_BUILD_TESTING=OFF ..
+  - make install
+  - cd ../..
+
+# YAML
+#  - apt-get install -y libyaml-cpp-dev
+  - if [ -d yaml-cpp ]; then
+  -   echo "directory exists" 
+  -   if [ "$(ls -A ./yaml-cpp)" ]; then 
+  -     echo "directory not empty" 
+  -     cd yaml-cpp
+  -     git pull
+  -   else 
+  -     echo "directory empty" 
+  -     git clone https://github.com/jbeder/yaml-cpp.git
+  -     cd yaml-cpp
+  -   fi
+  - else
+  -   echo "directory inexistent" 
+  -   git clone https://github.com/jbeder/yaml-cpp.git
+  -   cd yaml-cpp
+  - fi
+  - mkdir -pv build
+  - cd build
+  - ls
+  - cmake -DCMAKE_CXX_FLAGS="${CMAKE_CXX_FLAGS} -fPIC" -DYAML_CPP_BUILD_TESTS=OFF ..
+  - make install
+  - cd ../..
+
+wolf_build_and_test:
+  stage: build
+  except:
+    - master
+  script:
+    - mkdir -pv build
+    - cd build
+    - ls # we can check whether the directory was already full
+    - cmake -DCMAKE_BUILD_TYPE=release -DBUILD_EXAMPLES=ON -DBUILD_TESTS=ON ..
+    - make -j$(nproc)
+    - ctest -j$(nproc)
+    - make install
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
+# Pre-requisites about cmake itself
+CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
+
+if(COMMAND cmake_policy)
+  cmake_policy(SET CMP0005 NEW)
+  cmake_policy(SET CMP0003 NEW)
+endif(COMMAND cmake_policy)
+# MAC OSX RPATH
+SET(CMAKE_MACOSX_RPATH 1)
+
+
+# The project name
+PROJECT(bodydynamics)
+set(PLUGIN_NAME wolf${PROJECT_NAME})
+
+SET(EXECUTABLE_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/bin)
+SET(LIBRARY_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/lib)
+SET(CMAKE_INSTALL_PREFIX /usr/local)
+
+IF (NOT CMAKE_BUILD_TYPE)
+  SET(CMAKE_BUILD_TYPE "DEBUG")
+ENDIF (NOT CMAKE_BUILD_TYPE)
+message(STATUS "Configured to compile in ${CMAKE_BUILD_TYPE} mode.")
+
+#Set Flags
+SET(CMAKE_CXX_FLAGS_DEBUG "-g -Wall -D_REENTRANT")
+SET(CMAKE_CXX_FLAGS_RELEASE "-O3 -D_REENTRANT")
+
+#Set compiler according C++11 support
+include(CheckCXXCompilerFlag)
+CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
+CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
+if(COMPILER_SUPPORTS_CXX11)
+		message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has C++11 support.")
+    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+elseif(COMPILER_SUPPORTS_CXX0X)
+		message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has C++0x support.")
+    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
+else()
+  message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
+endif()
+
+if(UNIX)
+  # GCC is not strict enough by default, so enable most of the warnings.
+  set(CMAKE_CXX_FLAGS
+    "${CMAKE_CXX_FLAGS} -Werror=all -Werror=extra -Wno-unknown-pragmas -Wno-sign-compare -Wno-unused-parameter -Wno-missing-field-initializers")
+endif(UNIX)
+
+
+#OPTION(BUILD_DOC "Build Documentation" OFF)
+OPTION(BUILD_TESTS "Build Unit tests" ON)
+#############
+## Testing ##
+#############
+#
+if(BUILD_TESTS)
+    # Enables testing for this directory and below.
+    # Note that ctest expects to find a test file in the build directory root.
+    # Therefore, this command should be in the source directory root.
+    #include(CTest) # according to http://public.kitware.com/pipermail/cmake/2012-June/050853.html
+    enable_testing()
+endif()
+
+#+START_SRC --------------------------------------------------------------------------------------------------------------------------------
+#Start WOLF build
+MESSAGE("Starting WOLF CMakeLists ...")
+CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
+
+#CMAKE modules
+
+SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake_modules")
+MESSAGE(STATUS ${CMAKE_MODULE_PATH})
+
+# Some wolf compilation options
+
+IF((CMAKE_BUILD_TYPE MATCHES DEBUG) OR (CMAKE_BUILD_TYPE MATCHES debug) OR (CMAKE_BUILD_TYPE MATCHES Debug))
+  set(_WOLF_DEBUG true)
+ENDIF()
+
+option(_WOLF_TRACE "Enable wolf tracing macro" ON)
+
+# option(BUILD_EXAMPLES "Build examples" OFF)
+set(BUILD_TESTS true)
+set(BUILD_EXAMPLES false)
+
+# Does this has any other interest
+# but for the examples ?
+# yes, for the tests !
+IF(BUILD_EXAMPLES OR BUILD_TESTS)
+  string(TOUPPER ${PROJECT_NAME} UPPER_NAME)
+  set(_WOLF_ROOT_DIR ${CMAKE_SOURCE_DIR})
+ENDIF(BUILD_EXAMPLES OR BUILD_TESTS)
+
+
+#find dependencies.
+FIND_PACKAGE(wolf REQUIRED)
+
+# Define the directory where will be the configured config.h
+SET(WOLF_CONFIG_DIR ${PROJECT_BINARY_DIR}/conf/${PROJECT_NAME}/internal)
+
+# Create the specified output directory if it does not exist.
+IF(NOT EXISTS "${WOLF_CONFIG_DIR}")
+  message(STATUS "Creating config output directory: ${WOLF_CONFIG_DIR}")
+  file(MAKE_DIRECTORY "${WOLF_CONFIG_DIR}")
+ENDIF()
+IF(EXISTS "${WOLF_CONFIG_DIR}" AND NOT IS_DIRECTORY "${WOLF_CONFIG_DIR}")
+  message(FATAL_ERROR "Bug: Specified CONFIG_DIR: "
+    "${WOLF_CONFIG_DIR} exists, but is not a directory.")
+ENDIF()
+# Configure config.h
+configure_file(${CMAKE_CURRENT_SOURCE_DIR}/internal/config.h.in "${WOLF_CONFIG_DIR}/config.h")
+message("WOLF CONFIG ${WOLF_CONFIG_DIR}/config.h")
+message("CONFIG DIRECTORY ${PROJECT_BINARY_DIR}")
+include_directories("${PROJECT_BINARY_DIR}/conf")
+
+include_directories("include")
+include_directories(${wolf_INCLUDE_DIRS})
+
+#HEADERS
+
+SET(HDRS_COMMON
+  )
+SET(HDRS_MATH
+  )
+SET(HDRS_UTILS
+  )
+SET(HDRS_PROBLEM
+  )
+SET(HDRS_HARDWARE
+  )
+SET(HDRS_TRAJECTORY
+  )
+SET(HDRS_MAP
+  )
+SET(HDRS_FRAME
+  )
+SET(HDRS_STATE_BLOCK
+  )
+SET(HDRS_CAPTURE
+  )
+SET(HDRS_FACTOR
+include/bodydynamics/factor/factor_autodiff_inertial_kinematics.h
+  )
+SET(HDRS_FEATURE
+include/bodydynamics/feature/feature_inertial_kinematics.h
+  )
+SET(HDRS_LANDMARK
+  )
+SET(HDRS_PROCESSOR
+  )
+SET(HDRS_SENSOR
+  )
+SET(HDRS_SOLVER
+  )
+SET(HDRS_DTASSC
+  )
+SET(HDRS_YAML
+  )
+
+#SOURCES
+SET(SRCS_PROBLEM
+  )
+SET(SRCS_HARDWARE
+  )
+SET(SRCS_TRAJECTORY
+  )
+SET(SRCS_MAP
+  )
+SET(SRCS_FRAME
+  )
+SET(SRCS_STATE_BLOCK
+  )
+SET(SRCS_COMMON
+  )
+SET(SRCS_MATH
+  )
+SET(SRCS_UTILS
+  )
+SET(SRCS_CAPTURE
+  )
+SET(SRCS_FACTOR
+  )
+SET(SRCS_FEATURE
+src/feature/feature_inertial_kinematics.cpp
+  )
+SET(SRCS_LANDMARK
+  )
+SET(SRCS_PROCESSOR
+  )
+SET(SRCS_SENSOR
+  )
+SET(SRCS_DTASSC
+  )
+SET(SRCS_SOLVER
+  )
+SET(SRCS_YAML
+  )
+
+# create the shared library
+ADD_LIBRARY(${PLUGIN_NAME}
+  SHARED
+  ${SRCS_CAPTURE}
+  ${SRCS_COMMON}
+  ${SRCS_DTASSC}
+  ${SRCS_FACTOR}
+  ${SRCS_FEATURE}
+  ${SRCS_FRAME}
+  ${SRCS_HARDWARE}
+  ${SRCS_LANDMARK}
+  ${SRCS_MAP}
+  ${SRCS_MATH}
+  ${SRCS_PROBLEM}
+  ${SRCS_PROCESSOR}
+  ${SRCS_SENSOR}
+  ${SRCS_SOLVER}
+  ${SRCS_STATE_BLOCK}
+  ${SRCS_TRAJECTORY}
+  ${SRCS_UTILS}
+  ${SRCS_WRAPPER}
+  ${SRCS_YAML}
+  )
+TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${wolf_LIBRARIES})
+# Link the library with the required dependencies
+# TARGET_LINK_LIBRARIES(${PLUGIN_NAME} )
+
+IF(BUILD_DEMOS)
+  #Build examples
+  MESSAGE("Building demos.")
+  ADD_SUBDIRECTORY(demos)
+ENDIF(BUILD_DEMOS)
+
+
+#install library
+
+#=============================================================
+INSTALL(TARGETS ${PLUGIN_NAME} EXPORT ${PLUGIN_NAME}Targets
+  RUNTIME DESTINATION bin
+  LIBRARY DESTINATION lib/iri-algorithms
+  ARCHIVE DESTINATION lib/iri-algorithms)
+
+install(EXPORT ${PLUGIN_NAME}Targets DESTINATION lib/cmake/${PLUGIN_NAME})
+#install headers
+INSTALL(FILES ${HDRS_MATH}
+  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/math)
+INSTALL(FILES ${HDRS_UTILS}
+  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/utils)
+INSTALL(FILES ${HDRS_PROBLEM}
+  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/problem)
+INSTALL(FILES ${HDRS_HARDWARE}
+  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/hardware)
+INSTALL(FILES ${HDRS_TRAJECTORY}
+  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/trajectory)
+INSTALL(FILES ${HDRS_MAP}
+  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/map)
+INSTALL(FILES ${HDRS_FRAME}
+  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/frame)
+INSTALL(FILES ${HDRS_STATE_BLOCK}
+  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/state_block)
+INSTALL(FILES ${HDRS_COMMON}
+  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/common)
+INSTALL(FILES ${HDRS_DTASSC}
+  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/association)
+INSTALL(FILES ${HDRS_CAPTURE}
+   DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/capture)
+INSTALL(FILES ${HDRS_FACTOR}
+  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/factor)
+INSTALL(FILES ${HDRS_FEATURE}
+  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/feature)
+INSTALL(FILES ${HDRS_SENSOR}
+  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/sensor)
+INSTALL(FILES ${HDRS_PROCESSOR}
+  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/processor)
+INSTALL(FILES ${HDRS_LANDMARK}
+  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/landmark)
+INSTALL(FILES ${HDRS_WRAPPER}
+  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/ceres_wrapper)
+INSTALL(FILES ${HDRS_SOLVER_SUITESPARSE}
+  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/solver_suitesparse)
+INSTALL(FILES ${HDRS_SOLVER}
+  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/solver)
+INSTALL(FILES ${HDRS_YAML}
+  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/yaml)
+
+FILE(WRITE ${PROJECT_NAME}.found "")
+INSTALL(FILES ${PROJECT_NAME}.found
+  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME})
+
+
+INSTALL(FILES "${WOLF_CONFIG_DIR}/config.h"
+DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/internal)
+
+INSTALL(FILES "${CMAKE_SOURCE_DIR}/cmake_modules/${PLUGIN_NAME}Config.cmake" DESTINATION "lib/cmake/${PLUGIN_NAME}")
+
+export(PACKAGE ${PLUGIN_NAME})
+
+#-END_SRC --------------------------------------------------------------------------------------------------------------------------------
+FIND_PACKAGE(Doxygen)
+
+FIND_PATH(IRI_DOC_DIR doxygen.conf ${CMAKE_SOURCE_DIR}/doc/iri_doc/)
+IF (IRI_DOC_DIR)
+  ADD_CUSTOM_TARGET (doc ${DOXYGEN_EXECUTABLE} ${CMAKE_SOURCE_DIR}/doc/iri_doc/doxygen.conf)
+ELSE (IRI_DOC_DIR)
+  ADD_CUSTOM_TARGET (doc ${DOXYGEN_EXECUTABLE} ${CMAKE_SOURCE_DIR}/doc/doxygen.conf)
+ENDIF (IRI_DOC_DIR)
+
+ADD_CUSTOM_TARGET (distclean @echo cleaning cmake files)
+
+IF (UNIX)
+  ADD_CUSTOM_COMMAND(
+    COMMENT "distribution clean"
+    COMMAND make ARGS clean
+    COMMAND rm ARGS -rf ${CMAKE_SOURCE_DIR}/build/*
+
+    TARGET  distclean
+    )
+ELSE(UNIX)
+  ADD_CUSTOM_COMMAND(
+    COMMENT "distclean only implemented in unix"
+    TARGET  distclean
+    )
+ENDIF(UNIX)
+
+ADD_CUSTOM_TARGET (uninstall @echo uninstall package)
+
+IF (UNIX)
+  ADD_CUSTOM_COMMAND(
+    COMMENT "uninstall package"
+    COMMAND xargs ARGS rm < install_manifest.txt
+
+    TARGET  uninstall
+    )
+ELSE(UNIX)
+  ADD_CUSTOM_COMMAND(
+    COMMENT "uninstall only implemented in unix"
+    TARGET  uninstall
+    )
+ENDIF(UNIX)
+
+IF (UNIX)
+  SET(CPACK_PACKAGE_FILE_NAME "iri-${PROJECT_NAME}-dev-${CPACK_PACKAGE_VERSION}${CPACK_DEBIAN_PACKAGE_ARCHITECTURE}")
+  SET(CPACK_PACKAGE_NAME "iri-${PROJECT_NAME}-dev")
+  SET(CPACK_PACKAGE_DESCRIPTION_SUMMARY "...Enter something here...")
+  SET(CPACK_PACKAGING_INSTALL_PREFIX ${CMAKE_INSTALL_PREFIX})
+  SET(CPACK_GENERATOR "DEB")
+  SET(CPACK_DEBIAN_PACKAGE_MAINTAINER "galenya - labrobotica@iri.upc.edu")
+  SET(CPACK_SET_DESTDIR "ON")  # Necessary because of the absolute install paths
+  INCLUDE(CPack)
+ELSE(UNIX)
+  ADD_CUSTOM_COMMAND(
+    COMMENT "packaging only implemented in unix"
+    TARGET  uninstall)
+ENDIF(UNIX)
--- a/README.md
+++ b/README.md
+WOLF - Windowed Localization Frames | bodydynamics Plugin
+===================================
+
+For installation guide and code documentation, please visit the [documentation website](http://mobile_robotics.pages.iri.upc-csic.es/wolf_projects/wolf_lib/wolf_doc/).
+
+TODO
\ No newline at end of file
--- a/bodydynamics.found
+++ b/bodydynamics.found
--- a/cmake_modules/wolfConfig.cmake
+++ b/cmake_modules/wolfConfig.cmake
+#edit the following line to add the librarie's header files
+FIND_PATH(
+    wolfbodydynamics_INCLUDE_DIRS
+    NAMES bodydynamics.found
+    PATHS /usr/local/include/iri-algorithms/wolf/plugin_bodydynamics)
+IF(wolfbodydynamics_INCLUDE_DIRS)
+  MESSAGE("Found bodydynamics include dirs: ${wolfbodydynamics_INCLUDE_DIRS}")
+ELSE(wolfbodydynamics_INCLUDE_DIRS)
+  MESSAGE("Couldn't find bodydynamics include dirs")
+ENDIF(wolfbodydynamics_INCLUDE_DIRS)
+
+FIND_LIBRARY(
+    wolfbodydynamics_LIBRARIES
+    NAMES libwolfbodydynamics.so libwolfbodydynamics.dylib
+    PATHS /usr/local/lib/iri-algorithms)
+IF(wolfbodydynamics_LIBRARIES)
+  MESSAGE("Found bodydynamics lib: ${wolfbodydynamics_LIBRARIES}")
+ELSE(wolfbodydynamics_LIBRARIES)
+  MESSAGE("Couldn't find wolf bodydynamics lib")
+ENDIF(wolfbodydynamics_LIBRARIES)
+
+IF (wolfbodydynamics_INCLUDE_DIRS AND wolfbodydynamics_LIBRARIES)
+   SET(wolfbodydynamics_FOUND TRUE)
+ ELSE(wolfbodydynamics_INCLUDE_DIRS AND wolfbodydynamics_LIBRARIES)
+   set(wolfbodydynamics_FOUND FALSE)
+ENDIF (wolfbodydynamics_INCLUDE_DIRS AND wolfbodydynamics_LIBRARIES)
+
+IF (wolfbodydynamics_FOUND)
+   IF (NOT wolfbodydynamics_FIND_QUIETLY)
+      MESSAGE(STATUS "Found bodydynamics: ${wolfbodydynamics_LIBRARIES}")
+   ENDIF (NOT wolfbodydynamics_FIND_QUIETLY)
+ELSE (wolfbodydynamics_FOUND)
+   IF (wolfbodydynamics_FIND_REQUIRED)
+      MESSAGE(FATAL_ERROR "Could not find wolf bodydynamics")
+   ENDIF (wolfbodydynamics_FIND_REQUIRED)
+ENDIF (wolfbodydynamics_FOUND)
+
+
+macro(wolf_report_not_found REASON_MSG)
+  set(wolfbodydynamics_FOUND FALSE)
+  unset(wolfbodydynamics_INCLUDE_DIRS)
+  unset(wolfbodydynamics_LIBRARIES)
+
+  # Reset the CMake module path to its state when this script was called.
+  set(CMAKE_MODULE_PATH ${CALLERS_CMAKE_MODULE_PATH})
+
+  # Note <package>_FIND_[REQUIRED/QUIETLY] variables defined by
+  # FindPackage() use the camelcase library name, not uppercase.
+  if (wolfbodydynamics_FIND_QUIETLY)
+    message(STATUS "Failed to find wolfbodydynamics- " ${REASON_MSG} ${ARGN})
+  else (wolfbodydynamics_FIND_REQUIRED)
+    message(FATAL_ERROR "Failed to find wolfbodydynamics - " ${REASON_MSG} ${ARGN})
+  else()
+    # Neither QUIETLY nor REQUIRED, use SEND_ERROR which emits an error
+    # that prevents generation, but continues configuration.
+    message(SEND_ERROR "Failed to find wolfbodydynamics - " ${REASON_MSG} ${ARGN})
+  endif ()
+  return()
+endmacro(wolf_report_not_found)
+
+if(NOT wolfbodydynamics_FOUND)
+  wolf_report_not_found("Something went wrong while setting up wolf bodydynamics.")
+endif(NOT wolfbodydynamics_FOUND)
+# Set the include directories for wolf (itself).
+set(wolfbodydynamics_FOUND TRUE)
+
+# Suppose that our plugin requires openCV & vision_utils
+
+FIND_PACKAGE(vision_utils REQUIRED)
+list(APPEND wolfbodydynamics_INCLUDE_DIRS ${vision_utils_INCLUDE_DIR})
+list(APPEND wolfbodydynamics_LIBRARIES ${vision_utils_LIBRARY})
+
+FIND_PACKAGE(OpenCV REQUIRED)
+list(APPEND wolfbodydynamics_INCLUDE_DIRS ${OpenCV_INCLUDE_DIRS})
+list(APPEND wolfbodydynamics_LIBRARIES ${OpenCV_LIBS})
+
+#Making sure wolf is looked for
+if(NOT wolf_FOUND)
+  FIND_PACKAGE(wolf REQUIRED)
+
+  #We reverse in order to insert at the start
+  list(REVERSE wolfbodydynamics_INCLUDE_DIRS)
+  list(APPEND wolfbodydynamics_INCLUDE_DIRS ${wolf_INCLUDE_DIRS})
+  list(REVERSE wolfbodydynamics_INCLUDE_DIRS)
+
+  list(REVERSE wolfbodydynamics_LIBRARIES)
+  list(APPEND wolfbodydynamics_LIBRARIES ${wolf_LIBRARIES})
+  list(REVERSE wolfbodydynamics_LIBRARIES)
+endif()
\ No newline at end of file
--- a/cmake_modules/wolfbodydynamicsConfig.cmake
+++ b/cmake_modules/wolfbodydynamicsConfig.cmake
+#edit the following line to add the librarie's header files
+FIND_PATH(
+    wolfbodydynamics_INCLUDE_DIRS
+    NAMES bodydynamics.found
+    PATHS /usr/local/include/iri-algorithms/wolf/plugin_bodydynamics)
+IF(wolfbodydynamics_INCLUDE_DIRS)
+  MESSAGE("Found wolf bodydynamics include dirs: ${wolfbodydynamics_INCLUDE_DIRS}")
+ELSE(wolfbodydynamics_INCLUDE_DIRS)
+  MESSAGE("Couldn't find wolf bodydynamics include dirs")
+ENDIF(wolfbodydynamics_INCLUDE_DIRS)
+
+FIND_LIBRARY(
+    wolfbodydynamics_LIBRARIES
+    NAMES libwolfbodydynamics.so
+    PATHS /usr/local/lib/iri-algorithms)
+IF(wolfbodydynamics_LIBRARIES)
+  MESSAGE("Found wolf bodydynamics lib: ${wolfbodydynamics_LIBRARIES}")
+ELSE(wolfbodydynamics_LIBRARIES)
+  MESSAGE("Couldn't find wolf bodydynamics lib")
+ENDIF(wolfbodydynamics_LIBRARIES)
+
+IF (wolfbodydynamics_INCLUDE_DIRS AND wolfbodydynamics_LIBRARIES)
+   SET(wolfbodydynamics_FOUND TRUE)
+ ELSE(wolfbodydynamics_INCLUDE_DIRS AND wolfbodydynamics_LIBRARIES)
+   set(wolfbodydynamics_FOUND FALSE)
+ENDIF (wolfbodydynamics_INCLUDE_DIRS AND wolfbodydynamics_LIBRARIES)
+
+IF (wolfbodydynamics_FOUND)
+   IF (NOT wolfbodydynamics_FIND_QUIETLY)
+      MESSAGE(STATUS "Found wolf bodydynamics: ${wolfbodydynamics_LIBRARIES}")
+   ENDIF (NOT wolfbodydynamics_FIND_QUIETLY)
+ELSE (wolfbodydynamics_FOUND)
+   IF (wolfbodydynamics_FIND_REQUIRED)
+      MESSAGE(FATAL_ERROR "Could not find wolf bodydynamics")
+   ENDIF (wolfbodydynamics_FIND_REQUIRED)
+ENDIF (wolfbodydynamics_FOUND)
+
+
+macro(wolf_report_not_found REASON_MSG)
+  set(wolfbodydynamics_FOUND FALSE)
+  unset(wolfbodydynamics_INCLUDE_DIRS)
+  unset(wolfbodydynamics_LIBRARIES)
+
+  # Reset the CMake module path to its state when this script was called.
+  set(CMAKE_MODULE_PATH ${CALLERS_CMAKE_MODULE_PATH})
+
+  # Note <package>_FIND_[REQUIRED/QUIETLY] variables defined by
+  # FindPackage() use the camelcase library name, not uppercase.
+  if (wolfbodydynamics_FIND_QUIETLY)
+    message(STATUS "Failed to find wolf bodydynamics- " ${REASON_MSG} ${ARGN})
+  else (wolfbodydynamics_FIND_REQUIRED)
+    message(FATAL_ERROR "Failed to find wolf bodydynamics - " ${REASON_MSG} ${ARGN})
+  else()
+    # Neither QUIETLY nor REQUIRED, use SEND_ERROR which emits an error
+    # that prevents generation, but continues configuration.
+    message(SEND_ERROR "Failed to find wolf bodydynamics - " ${REASON_MSG} ${ARGN})
+  endif ()
+  return()
+endmacro(wolf_report_not_found)
+
+if(NOT wolfbodydynamics_FOUND)
+  wolf_report_not_found("Something went wrong while setting up wolf bodydynamics.")
+endif(NOT wolfbodydynamics_FOUND)
+# Set the include directories for wolf (itself).
+set(wolfbodydynamics_FOUND TRUE)
+
+# Now we gather all the required dependencies for Wolf Template
+
+#EXAMPLE Suppose that Wolf Template depends on laser_scan_utils
+# FIND_PACKAGE(laser_scan_utils REQUIRED)
+# list(APPEND wolflaser_INCLUDE_DIRS ${laser_scan_utils_INCLUDE_DIRS})
+# list(APPEND wolflaser_LIBRARIES ${laser_scan_utils_LIBRARY})
+
+# Making sure that wolf is always looked for
+if(NOT wolf_FOUND)
+  FIND_PACKAGE(wolf REQUIRED)
+
+  #We reverse in order to insert at the start
+  list(REVERSE wolfbodydynamics_INCLUDE_DIRS)
+  list(APPEND wolfbodydynamics_INCLUDE_DIRS ${wolf_INCLUDE_DIRS})
+  list(REVERSE wolfbodydynamics_INCLUDE_DIRS)
+
+  list(REVERSE wolfbodydynamics_LIBRARIES)
+  list(APPEND wolfbodydynamics_LIBRARIES ${wolf_LIBRARIES})
+  list(REVERSE wolfbodydynamics_LIBRARIES)
+endif()
\ No newline at end of file
--- a/include/bodydynamics/factor/factor_autodiff_inertial_kinematics.h
+++ b/include/bodydynamics/factor/factor_autodiff_inertial_kinematics.h
+/**
+ * \file factor_autodiff_inertial_kinematics.h
+ *
+ *  Created on: Feb 27, 2020
+ *      \author: mfourmy
+ */
+
+#ifndef FACTOR_AUTODIFF_INERTIAL_KINEMATICS_H_
+#define FACTOR_AUTODIFF_INERTIAL_KINEMATICS_H_
+
+#include "core/factor/factor_autodiff.h"
+
+namespace wolf
+{
+
+WOLF_PTR_TYPEDEFS(FactorAutodiffInertialKinematics);
+
+class FactorAutodiffInertialKinematics : public FactorAutodiff<FactorAutodiffInertialKinematics, 9, 3, 4, 3, 3, 3, 3>//, 3>
+{
+    public:
+        FactorAutodiffInertialKinematics(const FeatureBasePtr&   _feat,
+                                         const ProcessorBasePtr& _processor_ptr,
+                                         bool                    _apply_loss_function,
+                                         FactorStatus            _status) :
+            FactorAutodiff("INERTIAL KINEMATICS",
+                            nullptr,              // _frame_other_ptr
+                            nullptr,              // _capture_other_ptr
+                            nullptr,              // _feature_other_ptr
+                            nullptr,              // _landmark_other_ptr
+                            _processor_ptr,
+                            _apply_loss_function,
+                            _status,
+                            _feat->getFrame()->getP(),
+                            _feat->getFrame()->getO(),
+                            _feat->getFrame()->getV(),
+                            _feat->getFrame()->getStateBlock("C"),  // COM position
+                            _feat->getFrame()->getStateBlock("D"),  // COM velocity (bad name)
+                            _feat->getFrame()->getStateBlock("L"),  // Angular momentum
+                            _feat->getFrame()->getStateBlock("B")   // BC relative position bias (bad name)
+                            // _feat->getFrame()->getStateBlock("B"),   // TODO: how to get gyro bias?
+
+        {
+        }
+
+        virtual ~FactorAutodiffInertialKinematics() { /* nothing */ }
+
+        virtual std::string getTopology() const override
+        {
+            return std::string("GEOM");
+        }
+
+       /*
+        _pb: W_p_WB -> base position in world frame
+        _qb: W_q_B  -> base orientation in world frame
+        _vb: W_v_WB -> base velocity in world frame
+        _c: W_p_WC -> COM position in world frame
+        _cd: W_v_WC -> COM velocity in world frame
+        _Lc: W_Lc   -> angular momentum at the COM in world frame
+        _bp: B_b_BC -> bias on relative body->COM position kinematic measurement
+        */
+        template<typename T>
+        bool operator () (const T* const _pb,
+                          const T* const _qb,
+                          const T* const _vb,
+                          const T* const _c,
+                          const T* const _cd,
+                          const T* const _Lc,
+                          const T* const _bp,
+                          T* _res) const
+        {
+            using namespace Eigen;
+
+            FeatureInertialKinematicsPtr feat = std::static_pointer_cast<FeatureInertialKinematics>(getFeature());
+                    
+
+            // State variables instanciation
+            Map<const Matrix<T,3,1> > pb(_pb);
+            Map<const Quaternion<T> > qb(_qb);
+            Map<const Matrix<T,3,1> > vb(_vb);
+            Map<const Matrix<T,3,1> > c(_c);
+            Map<const Matrix<T,3,1> > cd(_cd);
+            Map<const Matrix<T,3,1> > Lc(_Lc);
+            Map<const Matrix<T,3,1> > bp(_bp);
+
+            // Measurements retrieval
+            Eigen::Matrix<T,3,1> pBC_m = getMeasurement().head<3>().cast<T>();      // B_p_BC
+            Eigen::Matrix<T,3,1> vBC_m = getMeasurement().segment<3>(3).cast<T>();  // B_v_BC
+            Eigen::Matrix<T,3,1> w_m   = getMeasurement().tail<3>().cast<T>();      // B_w_WB
+
+            // Error variable instanciation
+            Eigen::Matrix<T, 9, 1> err;
+            Eigen::Map<Eigen::Matrix<T, 3, 1> >   err_c(err.data()    );
+            Eigen::Map<Eigen::Matrix<T, 3, 1> >   err_cd(err.data() + 3);
+            Eigen::Map<Eigen::Matrix<T, 3, 1> >   err_Lc(err.data() + 6);
+
+
+            err_c = pBC_m - (qb.conjugate()(c - pb) + bp);
+            // TODO: add gyro bias
+            err_cd = (w_m).cross(pBC_m - bp) + vBC_m - qb.conjugate()*(cd - vb);
+            // TODO: where to put I(q) and 
+            err_Lc = feat->getIq() * w_m + feat->getLq() -  q.conjugate()*Lc;
+
+            return true;
+        }
+};
+
+} /* namespace wolf */
+
+#endif /* FACTOR_AUTODIFF_INERTIAL_KINEMATICS_H_ */
--- a/include/bodydynamics/feature/feature_inertial_kinematics.h
+++ b/include/bodydynamics/feature/feature_inertial_kinematics.h
+#ifndef FEATURE_INERTIAL_KINEMATICS_H_
+#define FEATURE_INERTIAL_KINEMATICS_H_
+
+//Wolf includes
+#include "core/feature/feature_base.h"
+
+//std includes
+namespace wolf {
+    
+WOLF_PTR_TYPEDEFS(FeatureApriltag);
+
+//class FeatureApriltag
+class FeatureInertialKinematics : public FeatureBase
+{
+    public:
+
+        FeatureInertialKinematics(const Eigen::Vector9d & _measurement, 
+                                  const Eigen::Matrix9d & _meas_uncertainty,
+                                  const Eigen::Matrix3d & _Iq, 
+                                  const Eigen::Vector3d & _Lq,
+                                  UncertaintyType _uncertainty_type = UNCERTAINTY_IS_INFO);
+        virtual ~FeatureInertialKinematics();
+
+        const Eigen::Matrix3d & getIq(){return Iq_;}
+        const Eigen::Vector3d & getLq(){return Lq_;}
+
+    private:
+        Eigen::Matrix3d Iq_;
+        Eigen::Vector3d Lq_;
+};
+
+} // namespace wolf
+
+#endif
--- a/include/bodydynamics/processor/.blank
+++ b/include/bodydynamics/processor/.blank
--- a/include/bodydynamics/sensor/.blank
+++ b/include/bodydynamics/sensor/.blank
--- a/internal/config.h.in
+++ b/internal/config.h.in
+// wolf - A structure for generic and extendable,
+// keyframe-based localization amd mapping algorithms
+//
+// https://gitlab.iri.upc.edu/mobile_robotics/wolf
+//
+// Author: jeremie.deray@pal-robotics.com (Jeremie Deray)
+
+// Configuration options for wolf.
+//
+// Do not edit this file, it was automatically configured by CMake when
+// wolf was compiled with the relevant configuration.
+//
+// wolf Developers: All options should have the same name as their mapped
+//                   CMake options, in the preconfigured version of this file
+//                   all options should be enclosed in '@'.
+//
+
+// Default (empty) configuration options for wolf.
+//
+// IMPORTANT: Most users of wolf will not use this file, when
+//            compiling wolf with CMake, CMake will configure a new
+//            config.h with the currently selected wolf compile
+//            options in <BUILD_DIR>/include/iri-algorithms/wolf/internal,
+//            which will be added to the include path for compilation,
+//            and installed with the public wolf headers.
+
+#ifndef WOLF_INTERNAL_${UPPERNAME}_CONFIG_H_
+#define WOLF_INTERNAL_${UPPERNAME}_CONFIG_H_
+
+#cmakedefine _WOLF_DEBUG
+
+#cmakedefine _WOLF_TRACE
+
+#define _WOLF_${UPPER_NAME}_ROOT_DIR "${_WOLF_ROOT_DIR}"
+
+#endif /* WOLF_INTERNAL_CONFIG_H_ */
--- a/src/capture/.blank
+++ b/src/capture/.blank
--- a/src/factor/.blank
+++ b/src/factor/.blank
--- a/src/feature/feature_inertial_kinematics.cpp
+++ b/src/feature/feature_inertial_kinematics.cpp
+
+
+#include "bodydynamics/feature/feature_inertial_kinematics.h"
+
+
+namespace wolf {
+
+FeatureInertialKinematics::FeatureInertialKinematics(
+    const Eigen::Vector9d & _measurement, 
+    const Eigen::Matrix9d & _meas_uncertainty,
+    const Eigen::Matrix3d & _Iq, 
+    const Eigen::Vector3d & _Lq,
+    UncertaintyType _uncertainty_type) :
+    FeatureBase("BODYDYNAMICS", _measurement, _meas_uncertainty, _uncertainty_type),
+    Iq_(_Iq),
+    Lq_(_Lq)
+{}
+
+FeatureInertialKinematics::~FeatureInertialKinematics(){}
+
+} // namespace wolf
\ No newline at end of file
--- a/src/processor/.blank
+++ b/src/processor/.blank
--- a/src/sensor/.blank
+++ b/src/sensor/.blank
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
+# Retrieve googletest from github & compile
+add_subdirectory(gtest)
+
+
+## Added these two include_dirs: ######################
+#
+#CMAKE modules
+#SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake_modules")
+#MESSAGE(STATUS ${CMAKE_MODULE_PATH})
+#
+# Include Eigen
+#FIND_PACKAGE(Eigen 3 REQUIRED)
+#INCLUDE_DIRECTORIES(${EIGEN_INCLUDE_DIRS})
+#
+# Include Ceres
+#FIND_PACKAGE(Ceres QUIET) #Ceres is not required
+#IF(Ceres_FOUND)
+#    INCLUDE_DIRECTORIES(${CERES_INCLUDE_DIRS})
+#ENDIF(Ceres_FOUND)
+#
+## and now gtest_motion_2d works ######################
+
+
+# Include gtest directory.
+include_directories(${GTEST_INCLUDE_DIRS})
+
+############# USE THIS TEST AS AN EXAMPLE #################
+#                                                         #
+# Create a specific test executable for gtest_example     #
+wolf_add_gtest(gtest_example gtest_example.cpp)           #
+target_link_libraries(gtest_example ${PLUGIN_NAME})      #
+#                                                         #
+###########################################################
+
+
+################# ADD YOUR TESTS BELOW ####################
+#                                                         #
+#           ==== IN ALPHABETICAL ORDER! ====              #
+#                                                         #
--- a/test/gtest/CMakeLists.txt
+++ b/test/gtest/CMakeLists.txt
+cmake_minimum_required(VERSION 2.8.8)
+project(gtest_builder C CXX)
+
+# We need thread support
+#find_package(Threads REQUIRED)
+
+# Enable ExternalProject CMake module
+include(ExternalProject)
+
+set(GTEST_FORCE_SHARED_CRT ON)
+set(GTEST_DISABLE_PTHREADS OFF)
+
+# For some reason I need to disable PTHREADS
+# with g++ (Ubuntu 4.9.3-8ubuntu2~14.04) 4.9.3
+# This is a known issue for MinGW :
+# https://github.com/google/shaderc/pull/174
+#if(MINGW)
+    set(GTEST_DISABLE_PTHREADS ON)
+#endif()
+
+# 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
+)
+
+# 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}"
+)
+
+function(wolf_add_gtest target)
+  add_executable(${target} ${ARGN})
+  add_dependencies(${target} libgtest)
+  target_link_libraries(${target} libgtest)
+
+  #WORKING_DIRECTORY ${CMAKE_BINARY_DIR}/bin
+  add_test(NAME ${target} COMMAND ${target})
+endfunction()
--- a/test/gtest_example.cpp
+++ b/test/gtest_example.cpp
+#include "utils_gtest.h"
+
+TEST(TestTest, DummyTestExample)
+{
+  EXPECT_FALSE(false);
+
+  ASSERT_TRUE(true);
+
+  int my_int = 5;
+
+  ASSERT_EQ(my_int, 5);
+
+//  PRINTF("All good at TestTest::DummyTestExample !\n");
+}
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}