Implemented all object slam classes, very simple processor WIP

.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
+
+objectslam.found
+\.vscode/
+build_release/
+
+Testing/Temporary/LastTest.log
+Testing/Temporary/CTestCostData.txt

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(objectslam)
+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)
+
+
+IF(NOT BUILD_TESTS)
+  OPTION(BUILD_TESTS "Build Unit tests" ON)
+ENDIF(NOT BUILD_TESTS)
+
+IF(NOT BUILD_DEMOS)
+  OPTION(BUILD_DEMOS "Build Demos" OFF)
+ENDIF(NOT BUILD_DEMOS)
+
+IF(NOT BUILD_DOC)
+  OPTION(BUILD_DOC "Build Documentation" OFF)
+ENDIF(NOT BUILD_DOC)
+
+#############
+## 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()
+
+MESSAGE("Starting ${PROJECT_NAME} 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)
+
+# 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.
+# ============EXAMPLE==================
+FIND_PACKAGE(wolfcore 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("CONFIG DIRECTORY ${PROJECT_BINARY_DIR}")
+include_directories("${PROJECT_BINARY_DIR}/conf")
+
+#INCLUDES SECTION
+# ============EXAMPLE==================
+INCLUDE_DIRECTORIES(${wolfcore_INCLUDE_DIRS})
+include_directories(BEFORE "include")
+
+#HEADERS
+SET(HDRS_CAPTURE
+  )
+SET(HDRS_COMMON
+  )
+SET(HDRS_FACTOR
+include/core/factor/factor_kf_lmk_pose_3d_with_extrinsics.h
+  )
+SET(HDRS_FEATURE
+include/objectslam/feature/feature_object.h
+  )
+SET(HDRS_LANDMARK
+include/objectslam/landmark/landmark_object.h
+  )
+SET(HDRS_MATH
+  )
+SET(HDRS_PROCESSOR
+include/objectslam/processor/processor_tracker_landmark_object.h
+  )
+SET(HDRS_SENSOR
+  )
+SET(HDRS_SOLVER
+  )
+SET(HDRS_UTILS
+  )
+SET(HDRS_YAML
+  )
+
+#SOURCES
+
+SET(SRCS_CAPTURE
+src/capture/capture_object.cpp
+)
+SET(SRCS_COMMON
+  )
+SET(SRCS_FACTOR
+  )
+SET(SRCS_FEATURE
+src/feature/feature_object.cpp
+  )
+SET(SRCS_LANDMARK
+src/landmark/landmark_object.cpp
+  )
+SET(SRCS_MATH
+  )
+SET(SRCS_PROCESSOR
+src/processor/processor_tracker_landmark_object.cpp
+  )
+SET(SRCS_SENSOR
+  )
+SET(SRCS_SOLVER
+  )
+SET(SRCS_UTILS
+  )
+SET(SRCS_YAML
+src/yaml/processor_tracker_landmark_object_yaml.cpp
+  )
+
+# create the shared library
+ADD_LIBRARY(${PLUGIN_NAME}
+  SHARED
+  ${SRCS_CAPTURE}
+  ${SRCS_COMMON}
+  ${SRCS_FACTOR}
+  ${SRCS_FEATURE}
+  ${SRCS_LANDMARK}
+  ${SRCS_MATH}
+  ${SRCS_PROCESSOR}
+  ${SRCS_SENSOR}
+  ${SRCS_SOLVER}
+  ${SRCS_UTILS}
+  ${SRCS_YAML}
+  )
+  
+  
+# Set compiler options
+# ====================
+if (CMAKE_CXX_COMPILER_ID STREQUAL "Clang")
+  message(STATUS "Using C++ compiler clang")
+  target_compile_options(${PLUGIN_NAME} PRIVATE -Winconsistent-missing-override)
+  # using Clang
+elseif (CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
+  message(STATUS "Using C++ compiler gnu")
+  target_compile_options(${PLUGIN_NAME} PRIVATE -Wsuggest-override)
+  # using GCC
+endif()
+
+
+TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${wolfcore_LIBRARIES})
+#Link the created libraries
+#===============EXAMPLE=========================
+# IF (Ceres_FOUND)
+#     TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${CERES_LIBRARIES})
+# ENDIF(Ceres_FOUND)
+
+
+#Build tests
+#===============EXAMPLE=========================
+IF(BUILD_TESTS)
+  MESSAGE("Building tests.")
+  add_subdirectory(test)
+ENDIF(BUILD_TESTS)
+
+#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_CAPTURE}
+  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/capture)
+INSTALL(FILES ${HDRS_COMMON}
+  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/common)
+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_LANDMARK}
+  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/landmark)
+INSTALL(FILES ${HDRS_MATH}
+  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/math)
+INSTALL(FILES ${HDRS_PROCESSOR}
+  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/processor)
+INSTALL(FILES ${HDRS_SENSOR}
+  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/sensor)
+INSTALL(FILES ${HDRS_SOLVER}
+  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/solver)
+INSTALL(FILES ${HDRS_UTILS}
+  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/utils)
+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}")
+
+INSTALL(DIRECTORY ${SPDLOG_INCLUDE_DIRS} DESTINATION "include/iri-algorithms/")
+
+export(PACKAGE ${PLUGIN_NAME})
+
+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)

README.md

+WOLF - Windowed Localization Frames | objectslam 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/).

cmake_modules/wolfobjectslamConfig.cmake

+#edit the following line to add the librarie's header files
+FIND_PATH(
+    wolfobjectslam_INCLUDE_DIRS
+    NAMES objectslam.found
+    PATHS /usr/local/include/iri-algorithms/wolf/plugin_objectslam)
+IF(wolfobjectslam_INCLUDE_DIRS)
+  MESSAGE("Found wolf objectslam include dirs: ${wolfobjectslam_INCLUDE_DIRS}")
+ELSE(wolfobjectslam_INCLUDE_DIRS)
+  MESSAGE("Couldn't find wolf objectslam include dirs")
+ENDIF(wolfobjectslam_INCLUDE_DIRS)
+FIND_LIBRARY(
+    wolfobjectslam_LIBRARIES
+    NAMES libwolfobjectslam.so libwolfobjectslam.dylib
+    PATHS /usr/local/lib/iri-algorithms)
+IF(wolfobjectslam_LIBRARIES)
+  MESSAGE("Found wolf objectslam lib: ${wolfobjectslam_LIBRARIES}")
+ELSE(wolfobjectslam_LIBRARIES)
+  MESSAGE("Couldn't find wolf objectslam lib")
+ENDIF(wolfobjectslam_LIBRARIES)
+
+IF (wolfobjectslam_INCLUDE_DIRS AND wolfobjectslam_LIBRARIES)
+   SET(wolfobjectslam_FOUND TRUE)
+ ELSE(wolfobjectslam_INCLUDE_DIRS AND wolfobjectslam_LIBRARIES)
+   set(wolfobjectslam_FOUND FALSE)
+ENDIF (wolfobjectslam_INCLUDE_DIRS AND wolfobjectslam_LIBRARIES)
+
+IF (wolfobjectslam_FOUND)
+   IF (NOT wolfobjectslam_FIND_QUIETLY)
+      MESSAGE(STATUS "Found wolf objectslam: ${wolfobjectslam_LIBRARIES}")
+   ENDIF (NOT wolfobjectslam_FIND_QUIETLY)
+ELSE (wolfobjectslam_FOUND)
+   IF (wolfobjectslam_FIND_REQUIRED)
+      MESSAGE(FATAL_ERROR "Could not find wolf objectslam")
+   ENDIF (wolfobjectslam_FIND_REQUIRED)
+ENDIF (wolfobjectslam_FOUND)
+
+
+macro(wolf_report_not_found REASON_MSG)
+  set(wolfobjectslam_FOUND FALSE)
+  unset(wolfobjectslam_INCLUDE_DIRS)
+  unset(wolfobjectslam_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 (wolfobjectslam_FIND_QUIETLY)
+    message(STATUS "Failed to find wolf objectslam- " ${REASON_MSG} ${ARGN})
+  else (wolfobjectslam_FIND_REQUIRED)
+    message(FATAL_ERROR "Failed to find wolf objectslam - " ${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 objectslam - " ${REASON_MSG} ${ARGN})
+  endif ()
+  return()
+endmacro(wolf_report_not_found)
+
+if(NOT wolfobjectslam_FOUND)
+  wolf_report_not_found("Something went wrong while setting up wolf objectslam.")
+endif(NOT wolfobjectslam_FOUND)
+# Set the include directories for wolf (itself).
+set(wolfobjectslam_FOUND TRUE)
+
+# Now we gather all the required dependencies for Wolf Laser
+
+FIND_PACKAGE(wolfvision REQUIRED)
+list(APPEND wolfobjectslam_INCLUDE_DIRS ${wolfvision_INCLUDE_DIR})
+list(APPEND wolfobjectslam_LIBRARIES ${wolfvision_LIBRARY})
+
+if(NOT OpenCV_FOUND)
+  FIND_PACKAGE(OpenCV REQUIRED)
+  list(APPEND wolfobjectslam_INCLUDE_DIRS ${OpenCV_INCLUDE_DIRS})
+  list(APPEND wolfobjectslam_LIBRARIES ${OpenCV_LIBS})
+endif()
+
+#Making sure wolf is looked for
+if(NOT wolf_FOUND)
+  FIND_PACKAGE(wolfcore REQUIRED)
+
+  #We reverse in order to insert at the start
+  list(REVERSE wolfobjectslam_INCLUDE_DIRS)
+  list(APPEND wolfobjectslam_INCLUDE_DIRS ${wolfcore_INCLUDE_DIRS})
+  list(REVERSE wolfobjectslam_INCLUDE_DIRS)
+
+  list(REVERSE wolfobjectslam_LIBRARIES)
+  list(APPEND wolfobjectslam_LIBRARIES ${wolfcore_LIBRARIES})
+  list(REVERSE wolfobjectslam_LIBRARIES)
+endif()

demos/CMakeLists.txt

+
+# Vision
+IF(vision_utils_FOUND)
+
+    IF(Ceres_FOUND)
+	    # IF (APRILTAG_LIBRARY)
+    	# 	ADD_EXECUTABLE(demo_apriltag demo_apriltag.cpp)
+    	# 	TARGET_LINK_LIBRARIES(demo_apriltag ${PROJECT_NAME})
+    	# ENDIF(APRILTAG_LIBRARY)
+    ENDIF(Ceres_FOUND)
+
+ENDIF(vision_utils_FOUND)
+

include/objectslam/capture/capture_object.h

+#ifndef CAPTURE_OBJECT_H_
+#define CAPTURE_OBJECT_H_
+
+//Wolf includes
+#include "core/capture/capture_base.h"
+
+
+namespace wolf {
+
+typedef struct ObjectDetection
+{
+    const Eigen::Vector7d & measurement;
+    const Eigen::Matrix6d & meas_covariance; 
+    const std::string & object_type;
+
+} ObjectDetection;
+
+typedef std::list<ObjectDetection> ObjectDetections;
+
+    
+WOLF_PTR_TYPEDEFS(CaptureObject);
+
+class CaptureObject : public CaptureBase
+{
+    public:
+        CaptureObject(const TimeStamp& _ts, const ObjectDetections& _object_dets);
+        ~CaptureObject() override;
+        
+        /** \brief Returns the object detected in current sensor input (image for instance)
+         * 
+         **/
+        ObjectDetections getObjectDetections() const; 
+
+    private:
+        ObjectDetections object_detections_;
+};
+
+} // namespace wolf
+
+#endif

include/objectslam/feature/feature_object.h

+#ifndef FEATURE_OBJECT_H_
+#define FEATURE_OBJECT_H_
+
+//Wolf includes
+#include "core/feature/feature_base.h"
+
+
+namespace wolf {
+    
+WOLF_PTR_TYPEDEFS(FeatureObject);
+
+//class FeatureObject
+class FeatureObject : public FeatureBase
+{
+    public:
+
+        FeatureObject(const Eigen::Vector7d & _measurement, 
+                      const Eigen::Matrix6d & _meas_covariance, 
+                      const std::string & _object_type,
+                      UncertaintyType _uncertainty_type = UNCERTAINTY_IS_COVARIANCE);
+        ~FeatureObject() override;
+        
+        /** \brief Returns object type
+         * 
+         * Returns object type
+         * 
+         **/
+        std::string getObjectType() const; 
+
+
+    private:
+        std::string object_type_;
+        
+};
+
+} // namespace wolf
+
+#endif

include/objectslam/landmark/landmark_object.h

+
+#ifndef LANDMARK_OBJECT_H_
+#define LANDMARK_OBJECT_H_
+
+//Wolf includes
+#include "core/landmark/landmark_base.h"
+
+#include "core/state_block/state_quaternion.h"
+
+// Std includes
+
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(LandmarkObject);
+    
+//class LandmarkObject
+class LandmarkObject : public LandmarkBase
+{
+    public:
+        /** \brief Constructor with type, time stamp and the position state pointer
+         *
+         * Constructor with type, and state pointer
+         * \param _pose: Concatenation of landmark position and quaternion vectors
+         * \param _object_type : type of the object represented by the landmark
+         *
+         **/
+		LandmarkObject(Eigen::Vector7d& _pose, const std::string& _object_type);
+
+        ~LandmarkObject() override;
+        
+        /** \brief Returns the object type
+         * 
+         **/
+        const std::string& getObjectType() const;
+
+        /** Factory method to create new landmarks from YAML nodes
+         */
+        static LandmarkBasePtr create(const YAML::Node& _lmk_node);
+
+        YAML::Node saveToYaml() const override;
+
+
+    private:
+        const std::string object_type_;
+        
+};
+
+} // namespace wolf
+
+#endif

include/objectslam/processor/processor_tracker_landmark_object.h

+#ifndef _PROCESSOR_TRACKER_LANDMARK_OBJECT_H_
+#define _PROCESSOR_TRACKER_LANDMARK_OBJECT_H_
+
+//Wolf includes
+#include "core/common/wolf.h"
+#include "core/processor/processor_tracker_landmark.h"
+#include <core/factor/factor_distance_3d.h>
+
+
+namespace wolf
+{
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorTrackerLandmarkObject);
+
+struct ParamsProcessorTrackerLandmarkObject : public ParamsProcessorTrackerLandmark
+{
+
+    double min_time_vote_;
+    double max_time_vote_;
+    int nb_vote_for_every_first_;
+    bool add_3d_cstr_;
+    bool reestimate_last_frame_;
+
+    ParamsProcessorTrackerLandmarkObject() = default;
+    ParamsProcessorTrackerLandmarkObject(std::string _unique_name, const ParamsServer& _server):
+        ParamsProcessorTrackerLandmark(_unique_name, _server)
+    {
+        min_time_vote_              = _server.getParam<double>(prefix + _unique_name                 + "/keyframe_vote/min_time_vote");
+        max_time_vote_              = _server.getParam<double>(prefix + _unique_name                 + "/keyframe_vote/max_time_vote");
+        nb_vote_for_every_first_    = _server.getParam<int>(prefix + _unique_name                    + "/keyframe_vote/nb_vote_for_every_first");
+        add_3d_cstr_                = _server.getParam<bool>(prefix + _unique_name                   + "/add_3d_cstr");
+        reestimate_last_frame_      = _server.getParam<bool>(prefix + _unique_name                   + "/reestimate_last_frame");
+    }
+    std::string print() const override
+    {
+        return ParamsProcessorTrackerLandmark::print()                                  + "\n"
+        + "min_time_vote_: "            + std::to_string(min_time_vote_)                + "\n"
+        + "max_time_vote_: "            + std::to_string(max_time_vote_)                + "\n"
+        + "nb_vote_for_every_first_: "  + std::to_string(nb_vote_for_every_first_)      + "\n"
+        + "add_3d_cstr_: "              + std::to_string(add_3d_cstr_)                  + "\n"
+        + "reestimate_last_frame_: "    + std::to_string(reestimate_last_frame_)        + "\n";
+    }
+};
+
+
+
+WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmarkObject);
+
+class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark
+{
+    public:
+
+
+        /** \brief Class constructor
+         */
+        ProcessorTrackerLandmarkObject( ParamsProcessorTrackerLandmarkObjectPtr _params_tracker_landmark_object);
+        WOLF_PROCESSOR_CREATE(ProcessorTrackerLandmarkObject, ParamsProcessorTrackerLandmarkObject);
+
+        /** \brief Class Destructor
+         */
+        ~ProcessorTrackerLandmarkObject() override;
+
+        void preProcess() override;
+        void postProcess() override;
+
+        /** \brief Find provided landmarks as features in the provided capture
+         * \param _landmarks_in input list of landmarks to be found
+         * \param _capture the capture in which the _landmarks_in should be searched
+         * \param _features_out returned list of features  found in \b _capture corresponding to a landmark of _landmarks_in
+         * \param _feature_landmark_correspondences returned map of landmark correspondences: _feature_landmark_correspondences[_feature_out_ptr] = landmark_in_ptr
+         *
+         * \return the number of landmarks found
+         */
+        unsigned int findLandmarks(const LandmarkBasePtrList& _landmarks_in,
+                                           const CaptureBasePtr& _capture,
+                                           FeatureBasePtrList& _features_out,
+                                           LandmarkMatchMap& _feature_landmark_correspondences) override;
+
+        /** \brief Vote for KeyFrame generation
+         *
+         * If a KeyFrame criterion is validated, this function returns true,
+         * meaning that it wants to create a KeyFrame at the \b last Capture.
+         *
+         * WARNING! This function only votes! It does not create KeyFrames!
+         */
+        bool voteForKeyFrame() const override;
+
+        /** \brief Detect new Features
+         * \param _max_features maximum number of features detected (-1: unlimited. 0: none)
+         * \param _capture The capture in which the new features should be detected.
+         * \param _features_out The list of detected Features in _capture.
+         * \return The number of detected Features.
+         *
+         * This function detects Features that do not correspond to known Features/Landmarks in the system.
+         *
+         * The function is called in ProcessorTrackerLandmark::processNew() to set the member new_features_last_,
+         * the list of newly detected features of the capture last_ptr_.
+         */
+        unsigned int detectNewFeatures(const int& _max_new_features,
+                                               const CaptureBasePtr& _capture,
+                                               FeatureBasePtrList& _features_out) override;
+
+        /** \brief Emplace one landmark
+         *
+         */
+        LandmarkBasePtr emplaceLandmark(FeatureBasePtr _feature_ptr) override;
+
+        /** \brief Emplace a new factor
+         * \param _feature_ptr pointer to the Feature
+         * \param _landmark_ptr LandmarkBase pointer to the Landmark.
+         */
+        FactorBasePtr emplaceFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr) override;
+
+        void configure(SensorBasePtr _sensor) override;
+
+        void reestimateLastFrame();
+
+    public:
+        FeatureBasePtrList getIncomingDetections() const;
+        FeatureBasePtrList getLastDetections() const;
+
+    protected:
+        void advanceDerived() override;
+        void resetDerived() override;
+
+    private:
+        bool reestimate_last_frame_;
+        int n_reset_;
+
+    protected:
+        FeatureBasePtrList detections_incoming_;   ///< detected tags in wolf form, incoming image
+        FeatureBasePtrList detections_last_;       ///< detected tags in wolf form, last image
+
+
+    // To be able to access them in unit tests
+    protected:
+        double          min_time_vote_;
+        double          max_time_vote_;
+        unsigned int    min_features_for_keyframe_;
+        int             nb_vote_for_every_first_;
+        bool            add_3d_cstr_;
+        int             nb_vote_;
+
+    public:
+      EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+};
+
+} // namespace wolf
+
+#endif /* _PROCESSOR_TRACKER_LANDMARK_OBJECT_H_ */

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_${UPPER_NAME}_CONFIG_H_
+#define WOLF_INTERNAL_${UPPER_NAME}_CONFIG_H_
+
+#cmakedefine _WOLF_DEBUG
+
+#cmakedefine _WOLF_TRACE
+
+#define _WOLF_${UPPER_NAME}_ROOT_DIR "${_WOLF_ROOT_DIR}"
+
+#endif /* WOLF_INTERNAL_CONFIG_H_ */

src/capture/capture_object.cpp

+#include "objectslam/capture/capture_object.h"
+
+
+namespace wolf {
+
+CaptureObject::CaptureObject(const TimeStamp& ts, const ObjectDetections& _object_dets) :
+    CaptureBase("CaptureObject", ts),
+    object_detections_(_object_dets)
+{
+}
+
+CaptureObject::~CaptureObject()
+{
+    //
+}
+
+ObjectDetections CaptureObject::getObjectDetections() const
+// ObjectDetections CaptureObject::getObjectDetectio() const
+{
+    return object_detections_;
+}
+
+
+} // namespace wolf

src/feature/feature_object.cpp

+#include "objectslam/feature/feature_object.h"
+
+namespace wolf {
+
+FeatureObject::FeatureObject(const Eigen::Vector7d & _measurement, 
+                             const Eigen::Matrix6d & _meas_uncertainty, 
+                             const std::string & _object_type,
+                             UncertaintyType _uncertainty_type) :
+    FeatureBase("FeatureObject", _measurement, _meas_uncertainty, _uncertainty_type),
+    object_type_(_object_type)
+{
+}
+
+FeatureObject::~FeatureObject()
+{
+    //
+}
+
+std::string FeatureObject::getObjectType() const
+{
+    return object_type_;
+}
+
+
+} // namespace wolf

src/landmark/landmark_object.cpp

+#include "core/common/factory.h"
+#include "core/math/rotations.h"
+#include "core/yaml/yaml_conversion.h"
+#include "objectslam/landmark/landmark_object.h"
+
+namespace wolf {
+
+LandmarkObject::LandmarkObject(Eigen::Vector7d& _pose, const std::string& _object_type) :
+	LandmarkBase("LandmarkObject", std::make_shared<StateBlock>(_pose.head(3)), std::make_shared<StateQuaternion>(_pose.tail(4))),
+	object_type_(_object_type)
+{
+  	// setDescriptor(Eigen::VectorXd::Constant(1,_tagid));  // not used in the end in apriltag
+    // setId(_tagid); // overwrite lmk ID to match tag_id.  // doubfull apriltag choice
+}
+
+LandmarkObject::~LandmarkObject()
+{
+    //
+}
+
+
+const std::string& LandmarkObject::getObjectType() const
+{
+    return object_type_;
+}
+
+
+// LANDMARK SAVE AND LOAD FROM YAML
+
+// static
+LandmarkBasePtr LandmarkObject::create(const YAML::Node& _lmk_node)
+{
+    // Parse YAML node with lmk info and data
+    unsigned int    id                      = _lmk_node["id"]                   .as<unsigned int>();
+    std::string     object_type             = _lmk_node["object type"]          .as<std::string>();
+    Eigen::Vector3d pos                     = _lmk_node["position"]             .as<Eigen::Vector3d>();
+    bool            pos_fixed               = _lmk_node["position fixed"]       .as<bool>();
+    Eigen::Vector4d vquat;
+    if (_lmk_node["orientation"].size() == 3)
+    {
+        // we have been given 3 Euler angles in degrees
+        Eigen::Vector3d   euler = M_TORAD * ( _lmk_node["orientation"]          .as<Eigen::Vector3d>() );
+        Eigen::Matrix3d       R = e2R(euler);
+        Eigen::Quaterniond quat = R2q(R);
+        vquat                   = quat.coeffs();
+    }
+    else if (_lmk_node["orientation"].size() == 4)
+    {
+        // we have been given a quaternion
+        vquat                               = _lmk_node["orientation"]          .as<Eigen::Vector4d>();
+    }
+    bool            ori_fixed               = _lmk_node["orientation fixed"]    .as<bool>();
+
+    Eigen::Vector7d pose; pose << pos, vquat;
+
+    // Create a new landmark
+    LandmarkObjectPtr lmk_ptr = std::make_shared<LandmarkObject>(pose, object_type);
+    lmk_ptr->setId(id);
+    lmk_ptr->getP()->setFixed(pos_fixed);
+    lmk_ptr->getO()->setFixed(ori_fixed);
+
+    return lmk_ptr;
+
+}
+
+YAML::Node LandmarkObject::saveToYaml() const
+{
+    // First base things
+    YAML::Node node = LandmarkBase::saveToYaml();
+
+    // Then Object specific things
+    node["object type"] = getObjectType();
+
+    return node;
+}
+
+
+// Register landmark creator
+namespace
+{
+const bool WOLF_UNUSED registered_lmk_apriltag = FactoryLandmark::registerCreator("LandmarkObject", LandmarkObject::create);
+}
+
+
+} // namespace wolf

src/processor/processor_tracker_landmark_object.cpp

+#include "core/math/rotations.h"
+#include <core/factor/factor_distance_3d.h>
+#include "core/factor/factor_kf_lmk_pose_3d_with_extrinsics.h"
+
+// Wolf object plugin
+#include "objectslam/processor/processor_tracker_landmark_object.h"
+#include "objectslam/capture/capture_object.h"
+#include "objectslam/feature/feature_object.h"
+#include "objectslam/landmark/landmark_object.h"
+
+
+
+namespace wolf {
+
+
+// Constructor
+ProcessorTrackerLandmarkObject::ProcessorTrackerLandmarkObject( ParamsProcessorTrackerLandmarkObjectPtr _params_tracker_landmark_object) :
+        ProcessorTrackerLandmark("ProcessorTrackerLandmarkObject", "PO", _params_tracker_landmark_object ),
+        reestimate_last_frame_(_params_tracker_landmark_object->reestimate_last_frame_),
+        n_reset_(0),
+        min_time_vote_(_params_tracker_landmark_object->min_time_vote_),
+        max_time_vote_(_params_tracker_landmark_object->max_time_vote_),
+        min_features_for_keyframe_(_params_tracker_landmark_object->min_features_for_keyframe),
+        nb_vote_for_every_first_(_params_tracker_landmark_object->nb_vote_for_every_first_),
+        add_3d_cstr_(_params_tracker_landmark_object->add_3d_cstr_),
+        nb_vote_(0)
+{
+
+}
+
+ProcessorTrackerLandmarkObject::~ProcessorTrackerLandmarkObject(){}
+
+void ProcessorTrackerLandmarkObject::configure(SensorBasePtr sen){}
+
+
+
+void ProcessorTrackerLandmarkObject::preProcess()
+{
+    //clear wolf detections so that new ones will be stored inside
+    detections_incoming_.clear();
+
+    auto incoming_ptr = std::dynamic_pointer_cast<CaptureObject>(incoming_ptr_);
+    assert(incoming_ptr != nullptr && "Capture type mismatch. ProcessorTrackerLandmarkObject can only process captures of type CaptureObject");
+
+    // for (auto det: incoming_ptr->getObjectDetections()){
+    //     detections_incoming_.push_back(FeatureBase::emplace<FeatureObject>(incoming_ptr, det.measurement, det.meas_covariance, det.object_type));
+    // }
+
+}
+
+
+
+void ProcessorTrackerLandmarkObject::postProcess()
+{
+    nb_vote_++;
+}
+
+FactorBasePtr ProcessorTrackerLandmarkObject::emplaceFactor(FeatureBasePtr _feature_ptr,
+                                                            LandmarkBasePtr _landmark_ptr)
+{
+    return FactorBase::emplace<FactorKfLmkPose3dWithExtrinsics>(_feature_ptr,
+                                                       getSensor(),
+                                                       getLast()->getFrame(),
+                                                       std::static_pointer_cast<LandmarkObject>(_landmark_ptr),
+                                                       std::static_pointer_cast<FeatureObject> (_feature_ptr ),
+                                                       shared_from_this(),
+                                                       params_->apply_loss_function,
+                                                       FAC_ACTIVE);
+}
+
+LandmarkBasePtr ProcessorTrackerLandmarkObject::emplaceLandmark(FeatureBasePtr _feature_ptr)
+{
+    // world to rob
+    Vector3d pos = getLast()->getFrame()->getP()->getState();
+    Quaterniond quat (getLast()->getFrame()->getO()->getState().data());
+    Eigen::Isometry3d w_M_r = Eigen::Translation<double,3>(pos.head(3)) * quat;
+
+    // rob to sensor
+    pos = getSensor()->getP()->getState();
+    quat.coeffs() = getSensor()->getO()->getState();
+    Eigen::Isometry3d r_M_c = Eigen::Translation<double,3>(pos.head(3)) * quat;
+
+    // camera to lmk
+    pos = _feature_ptr->getMeasurement().head(3);
+    quat.coeffs() = _feature_ptr->getMeasurement().tail(4);
+    Eigen::Isometry3d c_M_t = Eigen::Translation<double,3>(pos) * quat;
+
+    // world to lmk
+    Eigen::Isometry3d w_M_t = w_M_r * r_M_c * c_M_t;
+
+    // make 7-vector for lmk pose
+    pos  = w_M_t.translation();
+    Eigen::Matrix3d wRt = w_M_t.linear();
+    quat.coeffs() = R2q(wRt).coeffs().transpose();
+    Vector7d w_pose_t;
+    w_pose_t << pos, quat.coeffs();
+
+    FeatureObjectPtr feat_obj = std::static_pointer_cast<FeatureObject>(_feature_ptr);
+    std::string object_type = feat_obj->getObjectType();
+
+    return LandmarkBase::emplace<LandmarkObject>(getProblem()->getMap(), w_pose_t, object_type);
+}
+
+unsigned int ProcessorTrackerLandmarkObject::detectNewFeatures(const int& _max_new_features,
+                                                                 const CaptureBasePtr& _capture,
+                                                                 FeatureBasePtrList& _features_out)
+{
+    // list of landmarks in the map
+    auto lmk_lst = getProblem()->getMap()->getLandmarkList();
+    for (auto feat : detections_last_)
+    {
+        // max_new_features reached
+        if (_max_new_features != -1 && _features_out.size() == _max_new_features)
+            break;
+
+        bool feature_matched = false;
+        auto feat_obj = std::static_pointer_cast<FeatureObject>(feat);
+
+        // Loop over the landmark to find if one is associated to feat
+        // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
+        // !! Assume that the object type is unique -> to be adapted if necessary 
+        // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
+        for(auto lmk: lmk_lst){
+            LandmarkObjectPtr lmk_obj = std::dynamic_pointer_cast<LandmarkObject>(lmk);
+            if(lmk_obj != nullptr and lmk_obj->getObjectType() == feat_obj->getObjectType()){
+                feature_matched = true;
+                break;
+            }
+        }
+
+        if (!feature_matched)
+        {
+            // If the feature is not in the map & not in the list of newly detected features yet, then we add it.
+            _features_out.push_back(feat);
+
+            // link feature (they are created unlinked in preprocess())
+            feat->link(_capture);
+
+        }
+    }
+
+    return _features_out.size();
+}
+
+bool ProcessorTrackerLandmarkObject::voteForKeyFrame() const
+{   
+    // if no detections in last capture, no case where it is usefull to create a KF from last
+    if (detections_last_.empty())
+        return false;
+
+    double dt_incoming_origin = getIncoming()->getTimeStamp().get() - getOrigin()->getTimeStamp().get();
+    bool more_than_min_time_vote = dt_incoming_origin > min_time_vote_; 
+    bool too_long_since_last_KF = dt_incoming_origin > max_time_vote_;
+    // the elapsed time since last KF is too long 
+    if (too_long_since_last_KF){
+        return true;
+    }
+    // no detection in incoming capture and a minimum time since last KF has past
+    if ((detections_incoming_.size() < min_features_for_keyframe_) and more_than_min_time_vote)
+        return true;
+
+    // Vote for every image processed at the beginning if possible
+    if (nb_vote_ < nb_vote_for_every_first_){
+        return true;
+    }
+
+    return false;
+}
+
+unsigned int ProcessorTrackerLandmarkObject::findLandmarks(const LandmarkBasePtrList& _landmarks_in,
+                                                             const CaptureBasePtr& _capture,
+                                                             FeatureBasePtrList& _features_out,
+                                                             LandmarkMatchMap& _feature_landmark_correspondences)
+{   
+    for (auto feat : detections_incoming_)
+    {
+        std::string object_type = std::static_pointer_cast<FeatureObject>(feat)->getObjectType();
+        for (auto lmk : _landmarks_in)
+        {   
+            auto lmk_obj = std::dynamic_pointer_cast<LandmarkObject>(lmk);
+            if(lmk_obj != nullptr and lmk_obj->getObjectType() == object_type)
+            {
+                _features_out.push_back(feat);
+                double score(1.0);
+                LandmarkMatchPtr matched_landmark = std::make_shared<LandmarkMatch>(lmk, score);
+                _feature_landmark_correspondences.emplace ( feat, matched_landmark );
+                feat->link(_capture); // since all features are created in preProcess() are unlinked
+                break;
+            }
+        }
+    }
+
+    return _features_out.size();
+}
+
+
+
+
+
+FeatureBasePtrList ProcessorTrackerLandmarkObject::getIncomingDetections() const
+{
+    return detections_incoming_;
+}
+
+FeatureBasePtrList ProcessorTrackerLandmarkObject::getLastDetections() const
+{
+    return detections_last_;
+}
+
+
+
+void ProcessorTrackerLandmarkObject::advanceDerived()
+{
+    ProcessorTrackerLandmark::advanceDerived();
+    detections_last_ = std::move(detections_incoming_);
+}
+
+void ProcessorTrackerLandmarkObject::resetDerived()
+{   
+    // Add 3d distance constraint between 2 frames
+    if (getProblem()->getProcessorIsMotionMap().empty() && add_3d_cstr_){
+        if ((getOrigin() != nullptr) && 
+            (getOrigin()->getFrame() != nullptr) && 
+            (getOrigin() != getLast()) &&
+            (getOrigin()->getFrame() != getLast()->getFrame()) 
+            )
+        {
+            FrameBasePtr ori_frame = getOrigin()->getFrame();
+            Eigen::Vector1d dist_meas; dist_meas << 0.0;
+            double dist_std = 0.5;
+            Eigen::Matrix1d cov0(dist_std*dist_std);
+ 
+            auto capt3d = CaptureBase::emplace<CaptureBase>(getLast()->getFrame(),"Dist",getLast()->getTimeStamp());
+            auto feat_dist = FeatureBase::emplace<FeatureBase>(capt3d, "Dist", dist_meas, cov0);
+            auto cstr = FactorBase::emplace<FactorDistance3d>(feat_dist, feat_dist, ori_frame, shared_from_this(), params_->apply_loss_function, FAC_ACTIVE);
+        }
+    }
+    
+    if (getProblem()->getProcessorIsMotionMap().empty() && reestimate_last_frame_){
+        reestimateLastFrame();
+    }
+
+    ProcessorTrackerLandmark::resetDerived();
+    detections_last_ = std::move(detections_incoming_);
+}
+
+void ProcessorTrackerLandmarkObject::reestimateLastFrame(){
+    // !!!!!!!!!!!!!!!!!!!!!!!!!!!
+    // !! TOO COMPLEX: to rewrite
+    // !!!!!!!!!!!!!!!!!!!!!!!!!!!
+
+
+    // // Rewrite the last frame state and landmark state initialised during last frame creation to account
+    // // for a better estimate of the current state using the last landmark detection.
+    // // Otherwise, default behaviour is to take the last KF as an initialization of the new KF which can lead
+    // // to the solver finding local minima
+
+    // // When called for the first time, no feature list initialized in ori/last(?)
+    // if (n_reset_ < 1){
+    //     n_reset_ += 1;
+    //     return;
+    // }
+
+    // // A TESTER
+    // // (getOrigin() != nullptr)
+
+    // // Retrieve camera extrinsics
+    // Eigen::Quaterniond last_q_cam(getSensor()->getO()->getState().data());
+    // Eigen::Isometry3d last_M_cam = Eigen::Translation3d(getSensor()->getP()->getState()) * last_q_cam;
+
+    // // get features list of KF linked to origin capture and last capture
+    // FeatureBasePtrList ori_feature_list = getOrigin()->getFeatureList();
+    // FeatureBasePtrList last_feature_list = getLast()->getFeatureList();
+
+    // if (last_feature_list.size() == 0 || ori_feature_list.size() == 0){
+    //     return;
+    // }
+    
+    // // Among landmarks detected in origin and last, find the one that has the smallest error ratio (best confidence)
+    // double lowest_ration = 1;  // rep_error1/rep_error2 cannot be higher than 1
+    // FeatureObjectPtr best_feature;
+    // bool useable_feature = false;
+    // for (auto it_last = last_feature_list.begin(); it_last != last_feature_list.end(); it_last++){
+    //     FeatureObjectPtr last_feat_ptr = std::static_pointer_cast<FeatureObject>(*it_last);
+    //     for (auto it_ori = ori_feature_list.begin(); it_ori != ori_feature_list.end(); it_ori++){
+    //         FeatureObjectPtr ori_feat_ptr =  std::static_pointer_cast<FeatureObject>(*it_ori);
+    //         if (ori_feat_ptr->getTagId() == last_feat_ptr->getTagId()){
+    //             double ratio = ori_feat_ptr->getRepError1() / ori_feat_ptr->getRepError2();
+    //             //if (ratio < lowest_ration){
+    //             if (last_feat_ptr->getUserotation() && (ratio < lowest_ration)){
+    //                 useable_feature = true;
+    //                 lowest_ration = ratio;
+    //                 best_feature = last_feat_ptr;
+    //             }
+    //         }
+    //     }
+    // }
+    // // If there is no common feature between the two images, the continuity is broken and 
+    // // nothing can be estimated. In the case of objslam alone, this result in a broken factor map
+    // if (!useable_feature){
+    //     return;
+    // }
+    
+    // // std::cout << "Best feature id after: " << best_feature->getTagId() << std::endl;
+    // // Retrieve cam to landmark transform
+    // Eigen::Vector7d cam_pose_lmk = best_feature->getMeasurement();
+    // Eigen::Quaterniond cam_q_lmk(cam_pose_lmk.segment<4>(3).data());
+    // Eigen::Isometry3d cam_M_lmk = Eigen::Translation3d(cam_pose_lmk.head(3)) * cam_q_lmk;
+    
+    // // Get corresponding landmarks in origin/last landmark list
+    // Eigen::Isometry3d w_M_lmk;
+    // LandmarkBasePtrList lmk_list = getProblem()->getMap()->getLandmarkList();
+    // // Iterate in reverse order because landmark detected in last are at the end of the list (while still landmarks to discovers)
+    // for (std::list<LandmarkBasePtr>::reverse_iterator it_lmk = lmk_list.rbegin(); it_lmk != lmk_list.rend(); ++it_lmk){
+    //     LandmarkObjectPtr lmk_ptr = std::dynamic_pointer_cast<LandmarkObject>(*it_lmk);
+    //     // the map might contain other types of landmarks so check if the cast is valid
+    //     if (lmk_ptr == nullptr){
+    //         continue;
+    //     }
+    
+    //     if (lmk_ptr->getTagId() == best_feature->getTagId()){
+    //         Eigen::Vector3d w_t_lmk = lmk_ptr->getP()->getState();
+    //         Eigen::Quaterniond w_q_lmk(lmk_ptr->getO()->getState().data());
+    //         w_M_lmk = Eigen::Translation<double,3>(w_t_lmk) * w_q_lmk;
+    //     }
+    // }
+
+    // // Compute last frame estimate
+    // Eigen::Isometry3d w_M_last = w_M_lmk * (last_M_cam * cam_M_lmk).inverse();
+
+    // // Use the w_M_last to overide last key frame state estimation and keyframe estimation
+    // Eigen::Vector3d pos_last  = w_M_last.translation();
+    // Eigen::Quaterniond quat_last(w_M_last.linear());
+    // getLast()->getFrame()->getP()->setState(pos_last);
+    // getLast()->getFrame()->getO()->setState(quat_last.coeffs());
+
+    // // if (!best_feature->getUserotation()){
+    // //     return;
+    // // }
+    // ///////////////////
+    // // Reestimate position of landmark new in Last
+    // ///////////////////
+    // for (auto it_feat = new_features_last_.begin(); it_feat != new_features_last_.end(); it_feat++){
+    //     FeatureObjectPtr new_feature_last = std::static_pointer_cast<FeatureObject>(*it_feat);
+       
+    //     Eigen::Vector7d cam_pose_lmk = new_feature_last->getMeasurement();
+    //     Eigen::Quaterniond cam_q_lmk(cam_pose_lmk.segment<4>(3).data());
+    //     Eigen::Isometry3d cam_M_lmk_new = Eigen::Translation3d(cam_pose_lmk.head(3)) * cam_q_lmk;
+    //     Eigen::Isometry3d w_M_lmk =  w_M_last * last_M_cam * cam_M_lmk_new;
+
+    //     for (auto it_lmk = new_landmarks_.begin(); it_lmk != new_landmarks_.end(); ++it_lmk){
+    //         LandmarkObjectPtr lmk_ptr = std::dynamic_pointer_cast<LandmarkObject>(*it_lmk);
+    //         if (lmk_ptr == nullptr){
+    //             continue;
+    //         }
+    //         if (lmk_ptr->getTagId() == new_feature_last->getTagId()){
+    //             Eigen::Vector3d pos_lmk_last  = w_M_lmk.translation();
+    //             Eigen::Quaterniond quat_lmk_last(w_M_lmk.linear());
+    //             lmk_ptr->getP()->setState(pos_lmk_last);
+    //             lmk_ptr->getO()->setState(quat_lmk_last.coeffs());
+    //             break;
+    //         }
+    //     }
+    // }
+}
+
+
+} // namespace wolf
+
+// Register in the FactorySensor
+#include "core/processor/factory_processor.h"
+
+namespace wolf
+{
+WOLF_REGISTER_PROCESSOR(ProcessorTrackerLandmarkObject)
+WOLF_REGISTER_PROCESSOR_AUTO(ProcessorTrackerLandmarkObject)
+}
+

src/yaml/processor_tracker_landmark_object_yaml.cpp

+/**
+ * \file processor_tracker_landmark_object_yaml.cpp
+ *
+ *  Created on: June 4, 2021
+ *      \author: mfourmy
+ */
+
+
+// wolf
+#include "objectslam/processor/processor_tracker_landmark_object.h"
+#include "core/yaml/yaml_conversion.h"
+#include "core/common/factory.h"
+
+// yaml-cpp library
+#include <yaml-cpp/yaml.h>
+
+namespace wolf
+{
+
+namespace
+{
+static ParamsProcessorBasePtr createParamsProcessorLandmarkObject(const std::string & _filename_dot_yaml)
+{
+    YAML::Node config = YAML::LoadFile(_filename_dot_yaml);
+
+    if (config.IsNull())
+    {
+        WOLF_ERROR("Invalid YAML file!");
+        return nullptr;
+    }
+    else if (config["type"].as<std::string>() == "ProcessorTrackerLandmarkObject")
+    {
+        ParamsProcessorTrackerLandmarkObjectPtr params = std::make_shared<ParamsProcessorTrackerLandmarkObject>();
+
+        YAML::Node vote                     = config["vote"];
+        params->voting_active               = vote["voting active"]                  .as<bool>();
+        params->min_time_vote_              = vote["min_time_vote"]                  .as<double>();
+        params->max_time_vote_              = vote["max_time_vote"]                  .as<double>();
+        params->min_features_for_keyframe   = vote["min_features_for_keyframe"]      .as<unsigned int>();
+        params->nb_vote_for_every_first_    = vote["nb_vote_for_every_first"]        .as<int>();
+        
+        params->reestimate_last_frame_      = config["reestimate_last_frame"]        .as<bool>();
+        params->add_3d_cstr_                = config["add_3d_cstr"]                  .as<bool>();
+
+        params->max_new_features            = config["max_new_features"]             .as<int>();
+        params->apply_loss_function         = config["apply_loss_function"]          .as<bool>();
+
+        return params;
+    }
+    else
+    {
+        WOLF_ERROR("Wrong processor type! Should be \"ProcessorTrackerLandmarkObject\"");
+        return nullptr;
+    }
+    return nullptr;
+}
+
+// Register in the FactorySensor
+const bool WOLF_UNUSED registered_prc_object = FactoryParamsProcessor::registerCreator("ProcessorTrackerLandmarkObject", createParamsProcessorLandmarkObject);
+const bool WOLF_UNUSED registered_prc_object_wrapper = FactoryParamsProcessor::registerCreator("ProcessorTrackerLandmarkObject_Wrapper", createParamsProcessorLandmarkObject);
+
+} // namespace [unnamed]
+
+} // namespace wolf

test/CMakeLists.txt

+# Retrieve googletest from github & compile
+add_subdirectory(gtest)
+
+
+# Include gtest directory.
+include_directories(${GTEST_INCLUDE_DIRS})
+
+wolf_add_gtest(gtest_feature_object gtest_feature_object.cpp)
+target_link_libraries(gtest_feature_object ${PLUGIN_NAME} ${wolf_LIBRARY} ${OpenCV_LIBS})
+
+wolf_add_gtest(gtest_landmark_object gtest_landmark_object.cpp)
+target_link_libraries(gtest_landmark_object ${PLUGIN_NAME} ${wolf_LIBRARY} ${OpenCV_LIBS})
+
+# ProcessorTrackerLandmarkApriltag test
+wolf_add_gtest(gtest_processor_tracker_landmark_object gtest_processor_tracker_landmark_object.cpp)
+target_link_libraries(gtest_processor_tracker_landmark_object ${PLUGIN_NAME} ${wolf_LIBRARY} ${OpenCV_LIBS})
\ No newline at end of file

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()

test/gtest_feature_object.cpp

+/**
+ * \file gtest_feature_object.cpp
+ *
+ *  Created on: Dec 22, 2018
+ *      \author: jsola
+ */
+
+
+#include <core/utils/utils_gtest.h>
+
+#include "objectslam/feature/feature_object.h"
+
+using namespace wolf;
+
+class FeatureObject_test : public testing::Test
+{
+    public:
+        Eigen::Vector7d pose_;
+        Eigen::Matrix6d cov_;
+        std::string object_type_;
+
+        void SetUp() override
+        {
+            pose_ << 1,2,3,4,5,6,7;
+            cov_.setIdentity() * 2.0;
+            object_type_ = "thetype";
+        }
+};
+
+TEST_F(FeatureObject_test, type)
+{
+    FeatureObjectPtr f = std::make_shared<FeatureObject>(pose_, cov_, object_type_);
+
+    ASSERT_EQ(f->getType(), "FeatureObject");
+}
+
+TEST_F(FeatureObject_test, getObjectType)
+{
+    FeatureObjectPtr f = std::make_shared<FeatureObject>(pose_, cov_, object_type_);
+
+    ASSERT_EQ(f->getObjectType(), "thetype");
+}
+
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
+

test/gtest_landmark_object.cpp

+/**
+ * \file gtest_landmark_object.cpp
+ *
+ *  Created on: Dec 6, 2018
+ *      \author: jsola
+ */
+
+
+#include <core/utils/utils_gtest.h>
+
+
+#include "core/common/wolf.h"
+#include "core/utils/logging.h"
+
+#include "objectslam/landmark/landmark_object.h"
+#include "objectslam/internal/config.h"
+
+using namespace Eigen;
+using namespace wolf;
+using std::static_pointer_cast;
+
+class LandmarkObject_class : public testing::Test{
+    public:
+        void SetUp() override
+        {
+            wolf_root = _WOLF_OBJECTSLAM_ROOT_DIR;
+            problem = Problem::create("PO", 3);
+        }
+    public:
+        std::string wolf_root;
+        ProblemPtr   problem;
+};
+
+TEST(LandmarkObject, getObjectType)
+{
+    Vector7d p;
+    LandmarkObjectPtr l = std::make_shared<LandmarkObject>(p, "thetype"); // pose, tag_id, tag_width
+    ASSERT_EQ(l->getObjectType(), "thetype");
+}
+
+TEST(LandmarkObject, getPose)
+{
+    Vector7d p;
+    p << 0,0,0, 0,0,0,1;
+    LandmarkObjectPtr l = std::make_shared<LandmarkObject>(p, "thetype"); // pose, tag_id, tag_width
+    ASSERT_MATRIX_APPROX(l->getState().vector("PO"), p, 1e-6);
+}
+
+TEST_F(LandmarkObject_class, create)
+{
+    // load original hand-written map
+    problem->loadMap(wolf_root + "/test/map_objects.yaml"); // this will call create()
+    ASSERT_EQ(problem->getMap()->getLandmarkList().size(), 4);
+    ASSERT_EQ(problem->getMap()->getLandmarkList().front()->getType(), "LandmarkObject");
+}
+
+TEST_F(LandmarkObject_class, saveToYaml)
+{
+    // load original hand-written map
+    problem->loadMap(wolf_root + "/test/map_objects.yaml");
+    ASSERT_EQ(problem->getMap()->getLandmarkList().size(), 4);
+
+    // write map on new file
+    problem->saveMap(wolf_root + "/test/map_objects_save.yaml"); // this will call saveToYaml()
+
+    // delete existing map
+    while(!problem->getMap()->getLandmarkList().empty()) problem->getMap()->getLandmarkList().front()->remove();
+    ASSERT_EQ(problem->getMap()->getLandmarkList().size(), 0);
+
+    // reload the saved map
+    problem->loadMap(wolf_root + "/test/map_objects_save.yaml");
+    ASSERT_EQ(problem->getMap()->getLandmarkList().size(), 4);
+    ASSERT_EQ(problem->getMap()->getLandmarkList().front()->getType(), "LandmarkObject");
+}
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
+

test/gtest_processor_tracker_landmark_object.cpp

+#include <core/utils/utils_gtest.h>
+
+#include "core/common/wolf.h"
+#include "core/utils/logging.h"
+#include "core/sensor/sensor_pose.h"
+#include "core/capture/capture_pose.h"
+#include "core/processor/factory_processor.h"
+
+#include "objectslam/processor/processor_tracker_landmark_object.h"
+#include "objectslam/feature/feature_object.h"
+#include "objectslam/landmark/landmark_object.h"
+#include "objectslam/internal/config.h"
+using namespace Eigen;
+using namespace wolf;
+using std::static_pointer_cast;
+
+
+////////////////////////////////////////////////////////////////
+/*
+ * Wrapper class to be able to have setOriginPtr() and setLastPtr() in ProcessorTrackerLandmarkObject
+ */
+WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmarkObject_Wrapper);
+class ProcessorTrackerLandmarkObject_Wrapper : public ProcessorTrackerLandmarkObject
+{
+    public:
+        ProcessorTrackerLandmarkObject_Wrapper(ParamsProcessorTrackerLandmarkObjectPtr _params_tracker_landmark_object) :
+            ProcessorTrackerLandmarkObject(_params_tracker_landmark_object)
+        {
+            setType("ProcessorTrackerLandmarkObject_Wrapper");
+        };
+        ~ProcessorTrackerLandmarkObject_Wrapper() override{}
+        void setOriginPtr(const CaptureBasePtr _origin_ptr) { origin_ptr_ = _origin_ptr; }
+        void setLastPtr  (const CaptureBasePtr _last_ptr)   { last_ptr_ = _last_ptr; }
+        void setIncomingPtr  (const CaptureBasePtr _incoming_ptr)   { incoming_ptr_ = _incoming_ptr; }
+        unsigned int getMinFeaturesForKeyframe (){return min_features_for_keyframe_;}
+        double getMinTimeVote (){return min_time_vote_;}
+        void setIncomingDetections(const FeatureBasePtrList _incoming_detections) { detections_incoming_ = _incoming_detections; }
+        void setLastDetections(const FeatureBasePtrList _last_detections) { detections_last_ = _last_detections; }
+
+        // for factory
+        static ProcessorBasePtr create(const std::string& _unique_name, const ParamsProcessorBasePtr _params)
+        {
+            auto prc_object_params_ = std::static_pointer_cast<ParamsProcessorTrackerLandmarkObject>(_params);
+
+            auto prc_ptr = std::make_shared<ProcessorTrackerLandmarkObject_Wrapper>(prc_object_params_);
+
+            prc_ptr->setName(_unique_name);
+
+            return prc_ptr;
+        }
+
+};
+namespace wolf{
+// Register in the Factories
+WOLF_REGISTER_PROCESSOR(ProcessorTrackerLandmarkObject_Wrapper);
+}
+////////////////////////////////////////////////////////////////
+
+
+
+////////////////////////////////////////////////////////////////
+/*
+ * Test class to prepare a little wolf problem to test the class ProcessorTrackerLandmarkObject
+ *
+ * The class ProcessorTrackerLandmarkObject is sometimes tested via the wrapper ProcessorTrackerLandmarkObject_Wrapper
+ */
+// Use the following in case you want to initialize tests with predefined variables or methods.
+class ProcessorTrackerLandmarkObject_class : public testing::Test{
+    public:
+        void SetUp() override
+        {
+            wolf_root = _WOLF_OBJECTSLAM_ROOT_DIR;
+
+            // configure wolf problem
+            problem = Problem::create("PO", 3);
+            ParamsSensorPosePtr params_sp = std::make_shared<ParamsSensorPose>();
+            sen = problem->installSensor("SensorPose", "pose_sensor", (Vector7d()<<0,0,0,0,0,0,1).finished(), params_sp);
+            prc = problem->installProcessor("ProcessorTrackerLandmarkObject_Wrapper", "objects_wrapper", "pose_sensor", wolf_root + "/test/processor_tracker_landmark_object.yaml");
+            prc_obj = std::static_pointer_cast<ProcessorTrackerLandmarkObject_Wrapper>(prc);
+
+            // set prior
+            VectorComposite x0("PO", {Vector3d(0,0,0), Quaterniond::Identity().coeffs()});
+            VectorComposite s0("PO", {Vector3d(1,1,1), Vector3d(1,1,1)});
+            F1 = problem->setPriorFactor(x0, s0, 0.0, 0.1);
+
+            // minimal config for the processor to be operative
+            C1 = CaptureBase::emplace<CapturePose>(F1, 1.0, sen, Vector7d(), Matrix6d());
+            prc_obj->setOriginPtr(C1);
+            prc_obj->setLastPtr(C1);
+        }
+
+    public:
+        ProcessorTrackerLandmarkObject_WrapperPtr prc_obj;
+        std::string             wolf_root;
+        ProblemPtr              problem;
+        SensorBasePtr           sen;
+        ProcessorBasePtr        prc;
+        FrameBasePtr            F1;
+        CaptureBasePtr          C1;
+};
+////////////////////////////////////////////////////////////////
+
+
+
+/////////////////// TESTS START HERE ///////////////////////////
+//                                                            //
+TEST(ProcessorTrackerLandmarkObject, Constructor)
+{
+    // std::string s1;
+    // std::string s2;
+
+    // ParamsProcessorTrackerLandmarkObjectPtr params = std::make_shared<ParamsProcessorTrackerLandmarkObject>();
+
+    // ProcessorTrackerLandmarkObjectPtr p;
+
+    // params->tag_family_ = "wrong_family";
+    // WOLF_INFO("The following runtime error \"Unrecognized tag family name. Use e.g. \"tag36h11\".\" is expected and does not imply a failed test:");
+    // ASSERT_DEATH( { std::make_shared<ProcessorTrackerLandmarkObject>(params); }, "" );
+}
+
+//     //////////////////////////////////////
+//     //////////////////////////////////////
+//     // TODO: GTEST IMPLEMENTATION IS WRONG
+//     //////////////////////////////////////
+//     //////////////////////////////////////
+// TEST_F(ProcessorTrackerLandmarkObject_class, voteForKeyFrame)
+// {
+
+//     double min_time_vote = prc_obj->getMinTimeVote();
+//     unsigned int min_features_for_keyframe = prc_obj->getMinFeaturesForKeyframe();
+//     double start_ts = 2.0;
+
+//     CaptureBasePtr Ca = CaptureBase::emplace<CapturePose>(F1, start_ts, sen, Vector7d(), Matrix6d());
+//     CaptureBasePtr Cb = CaptureBase::emplace<CapturePose>(F1, start_ts + min_time_vote/2, sen, Vector7d(), Matrix6d());
+//     CaptureBasePtr Cc = CaptureBase::emplace<CapturePose>(F1, start_ts + 2*min_time_vote, sen, Vector7d(), Matrix6d());
+//     CaptureBasePtr Cd = CaptureBase::emplace<CapturePose>(F1, start_ts + 2.5*min_time_vote, sen, Vector7d(), Matrix6d());
+//     CaptureBasePtr Ce = CaptureBase::emplace<CapturePose>(F1, start_ts + 3*min_time_vote, sen, Vector7d(), Matrix6d());
+
+//     for (int i=0; i < min_features_for_keyframe; i++){
+//         det.id = i;
+//         FeatureBase::emplace<FeatureObject>(Ca, (Vector7d()<<0,0,0,0,0,0,1).finished(), Matrix6d::Identity(), i, det, rep_error1, rep_error2, use_rotation);
+//         FeatureBase::emplace<FeatureObject>(Cc, (Vector7d()<<0,0,0,0,0,0,1).finished(), Matrix6d::Identity(), i, det, rep_error1, rep_error2, use_rotation);
+//         if (i != min_features_for_keyframe-1){
+//             FeatureBase::emplace<FeatureObject>(Cd, (Vector7d()<<0,0,0,0,0,0,1).finished(), Matrix6d::Identity(), i, det, rep_error1, rep_error2, use_rotation);
+//             FeatureBase::emplace<FeatureObject>(Ce, (Vector7d()<<0,0,0,0,0,0,1).finished(), Matrix6d::Identity(), i, det, rep_error1, rep_error2, use_rotation);
+//         }
+//     }
+
+//     // CASE 1: Not enough time between origin and incoming
+//     prc_obj->setOriginPtr(Ca);
+//     prc_obj->setIncomingPtr(Cb);
+//     ASSERT_FALSE(prc_obj->voteForKeyFrame());
+
+//     // CASE 2: Enough time but still too many features in image to trigger a KF
+//     prc_obj->setOriginPtr(Ca);
+//     prc_obj->setLastPtr(Cb);
+//     prc_obj->setIncomingPtr(Cc);
+//     ASSERT_FALSE(prc_obj->voteForKeyFrame());
+
+//     // // CASE 3: Enough time, enough features in last, not enough features in incoming
+//     // prc_obj->setOriginPtr(Ca);
+//     // prc_obj->setLastPtr(Cc);
+//     // prc_obj->setIncomingPtr(Cd);
+//     // ASSERT_TRUE(prc_obj->voteForKeyFrame());
+
+//     // // CASE 4: Enough time, not enough features in last, not enough features in incoming
+//     // prc_obj->setOriginPtr(Ca);
+//     // prc_obj->setLastPtr(Cd);
+//     // prc_obj->setIncomingPtr(Ce);
+//     // ASSERT_FALSE(prc_obj->voteForKeyFrame());
+// }
+
+// !!!!!!!!!!!!!!!!!!!!!!!
+// not usefull principle?
+// !!!!!!!!!!!!!!!!!!!!!!!
+// TEST_F(ProcessorTrackerLandmarkObject_class, detectNewFeaturesDuplicated)
+// {
+//     // No detected features
+//     FeatureBasePtrList features_out;
+//     prc_obj->detectNewFeatures(1, C1, features_out);
+//     ASSERT_EQ(features_out.size(), 0);
+
+//     // Some detected features TODO
+//     FeatureBasePtrList features_in;
+//     Eigen::Vector3d pos;
+//     Eigen::Vector3d ori; //Euler angles in rad
+//     Eigen::Quaterniond quat;
+//     Eigen::Vector7d pose;
+//     Eigen::Matrix6d meas_cov = Matrix6d::Identity();
+//     std::string object_type;
+
+//     // feature 0
+//     pos << 0,2,0;
+//     ori << M_TORAD * 0, M_TORAD * 0, M_TORAD * 0;
+//     quat = e2q(ori);
+//     pose << pos, quat.coeffs();
+//     object_type = "type0";
+//     FeatureBasePtr f0 = std::make_shared<FeatureObject>(pose, meas_cov, object_type, "type0");
+
+//     // feature 1 (with same id of feature 0)
+//     pos << 1,2,0;
+//     ori << M_TORAD * 0, M_TORAD * 0, M_TORAD * 0;
+//     quat = e2q(ori);
+//     pose << pos, quat.coeffs();
+//     object_type = "type0";
+//     FeatureBasePtr f1 = std::make_shared<FeatureObject>(pose, meas_cov, object_type, "type0");
+
+//     features_in.push_back(f0);
+//     features_in.push_back(f1);
+
+//     // We just added two features with the same id in the list.
+//     prc_obj->setLastDetections(features_in);
+//     // at this point we have 0 detections in last, 2 detections in incoming with same id.
+//     prc_obj->detectNewFeatures(2, C1, features_out);
+//     ASSERT_EQ(features_out.size(), 1); // detectNewFeatures should keep only one in the final list of new detected features
+// }
+
+TEST_F(ProcessorTrackerLandmarkObject_class, detectNewFeatures)
+{
+    // No detected features
+    FeatureBasePtrList features_out;
+    prc_obj->detectNewFeatures(1, C1, features_out);
+    ASSERT_EQ(features_out.size(), 0);
+
+    // Some detected features TODO
+    FeatureBasePtrList features_in;
+    Eigen::Vector3d pos;
+    Eigen::Vector3d ori; //Euler angles in rad
+    Eigen::Quaterniond quat;
+    Eigen::Vector7d pose;
+    Eigen::Matrix6d meas_cov = Matrix6d::Identity();
+    std::string object_type;
+
+    // feature 0
+    pos << 0,2,0;
+    ori << M_TORAD * 0, M_TORAD * 0, M_TORAD * 0;
+    quat = e2q(ori);
+    pose << pos, quat.coeffs();
+    object_type = "type0";
+    FeatureBasePtr f0 = std::make_shared<FeatureObject>(pose, meas_cov, object_type);
+
+    // feature 1
+    pos << 1,2,0;
+    ori << M_TORAD * 0, M_TORAD * 0, M_TORAD * 0;
+    quat = e2q(ori);
+    pose << pos, quat.coeffs();
+    object_type = "type1";
+    FeatureBasePtr f1 = std::make_shared<FeatureObject>(pose, meas_cov, object_type);
+
+    // feature 2
+    pos << 0,2,1;
+    ori << M_TORAD * 0, M_TORAD * 0, M_TORAD * 0;
+    quat = e2q(ori);
+    pose << pos, quat.coeffs();
+    object_type = "type2";
+    FeatureBasePtr f2 = std::make_shared<FeatureObject>(pose, meas_cov, object_type);
+
+    //we add different features in the list
+    features_in.push_back(f0);
+    features_in.push_back(f1);
+    //these features are set as the predetected features in last to processing an image
+    prc_obj->setLastDetections(features_in);
+    // at this point we have 0 detections in last, 2 detections in predetected features with different ids, thus we should have 2 new detected features (if max_features set to >= 2)
+    prc_obj->detectNewFeatures(2, C1, features_out);
+    ASSERT_EQ(features_out.size(), 2);
+
+    // Put some of the features in the graph with emplaceLandmark() and detect some of them as well as others with detectNewFeatures() running again.
+    WOLF_WARN("call to function emplaceLandmark() in unit test for detectNewFeatures().")
+    WOLF_INFO("emplacing 0....");
+    LandmarkBasePtr lmk0 = prc_obj->emplaceLandmark(f0);
+    WOLF_INFO("emplacing 1....");
+    LandmarkBasePtr lmk1 = prc_obj->emplaceLandmark(f1);
+
+    // Add 1 one more new feature to the detection list
+    features_in.push_back(f2);
+    prc_obj->setLastDetections(features_in);
+    // At this point we have 2 landmarks (for f0 and f1), and 3 predetected features (f0, f1 and f2).
+    // Hence we should have 1 new detected feature : f2
+    features_out.clear();
+    WOLF_INFO("detecting....");
+    prc_obj->detectNewFeatures(2, C1, features_out);
+    ASSERT_EQ(features_out.size(), 1);
+    ASSERT_EQ(std::static_pointer_cast<FeatureObject>(features_out.front())->getObjectType(), "type2");
+}
+
+TEST_F(ProcessorTrackerLandmarkObject_class, emplaceLandmark)
+{
+    Vector7d pose_landmark((Vector7d()<<0,0,0,0,0,0,1).finished());
+    FeatureBasePtr f1 = FeatureBase::emplace<FeatureObject>(C1, pose_landmark, Matrix6d::Identity(), "thetype");
+
+    LandmarkBasePtr lmk = prc_obj->emplaceLandmark(f1);
+    LandmarkObjectPtr lmk_object = std::static_pointer_cast<LandmarkObject>(lmk);
+    ASSERT_TRUE(lmk_object->getMap() != nullptr);
+    ASSERT_TRUE(lmk_object->getType() == "LandmarkObject");
+    ASSERT_MATRIX_APPROX(lmk_object->getState().vector("PO"), pose_landmark, 1e-6);
+}
+
+TEST_F(ProcessorTrackerLandmarkObject_class, emplaceFactor)
+{
+    FeatureBasePtr f1 = FeatureBase::emplace<FeatureObject>(C1, (Vector7d()<<0,0,0,0,0,0,1).finished(), Matrix6d::Identity(), "thetype");
+
+    LandmarkBasePtr lmk = prc_obj->emplaceLandmark(f1);
+    LandmarkObjectPtr lmk_object = std::static_pointer_cast<LandmarkObject>(lmk);
+
+    FactorBasePtr ctr = prc_obj->emplaceFactor(f1, lmk);
+
+    ASSERT_TRUE(ctr->getFeature() == f1);
+    ASSERT_TRUE(ctr->getType() == "FactorKfLmkPose3dWithExtrinsics");
+}
+
+
+int main(int argc, char **argv)
+{
+    testing::InitGoogleTest(&argc, argv);
+    return RUN_ALL_TESTS();
+}
+