Merge branch 'devel' into 'main'

After cmake and const refactor

See merge request !36

.gitignore

@@ -4,7 +4,10 @@
 README.txt
 bin/
 build/
+build_debug/
 build_release/
+build-debug/
+build-release/
 lib/
 .idea/
 ./Wolf.user
@@ -31,7 +34,6 @@ src/examples/map_apriltag_save.yaml
 demos/map_apriltag_save.yaml
 
 \.vscode/
-build_release/
 
 apriltag.found
 apriltagw.found

.gitlab-ci.yml

@@ -3,6 +3,12 @@ stages:
   - build_and_test
 
 ############ YAML ANCHORS ############
+.print_variables_template: &print_variables_definition
+  # Print variables
+  - echo $CI_COMMIT_BRANCH
+  - echo $WOLF_VISION_BRANCH
+  - echo $WOLF_APRILTAG_BRANCH
+
 .preliminaries_template: &preliminaries_definition
   ## Install ssh-agent if not already installed, it is required by Docker.
   ## (change apt-get to yum if you use an RPM-based image)
@@ -155,6 +161,7 @@ license_headers:
       paths:
       - ci_deps/wolf/
   before_script:
+    - *print_variables_definition
     - *preliminaries_definition
     - *install_wolf_definition
   script:
@@ -178,6 +185,7 @@ build_and_test:bionic:
       paths:
       - ci_deps/apriltaglib/
   before_script:
+    - *print_variables_definition
     - *preliminaries_definition
     - *install_wolf_definition
     - *install_visionutils_definition
@@ -205,6 +213,7 @@ build_and_test:focal:
       paths:
       - ci_deps/apriltaglib/
   before_script:
+    - *print_variables_definition
     - *preliminaries_definition
     - *install_wolf_definition
     - *install_visionutils_definition

CMakeLists.txt

 # Pre-requisites about cmake itself
-CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
+CMAKE_MINIMUM_REQUIRED(VERSION 3.10)
 
-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 true)
 
@@ -16,7 +12,8 @@ MESSAGE("Starting ${PROJECT_NAME} CMakeLists ...")
 
 SET(EXECUTABLE_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/bin)
 SET(LIBRARY_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/lib)
-SET(CMAKE_INSTALL_PREFIX /usr/local)
+set(INCLUDE_INSTALL_DIR include/iri-algorithms/wolf)
+set(LIB_INSTALL_DIR lib/)
 
 IF (NOT CMAKE_BUILD_TYPE)
   SET(CMAKE_BUILD_TYPE "DEBUG")
@@ -67,10 +64,6 @@ if(BUILD_TESTS)
     enable_testing()
 endif()
 
-#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)
@@ -80,12 +73,14 @@ option(_WOLF_TRACE "Enable wolf tracing macro" ON)
 
 
 # ============ DEPENDENCIES ============ 
+FIND_PACKAGE(wolfcore REQUIRED CONFIG)
 FIND_PACKAGE(wolfvision REQUIRED)
 FIND_PACKAGE(apriltag REQUIRED)
 
  #============ CONFIG.H ============  
-string(TOUPPER ${PROJECT_NAME} UPPER_NAME)
 set(_WOLF_ROOT_DIR ${CMAKE_SOURCE_DIR})
+# variable used to compile the config.h.in file
+string(TOUPPER ${PROJECT_NAME} PROJECT_NAME_UPPER)
 
 # Define the directory where will be the configured config.h
 SET(WOLF_CONFIG_DIR ${PROJECT_BINARY_DIR}/conf/${PROJECT_NAME}/internal)
@@ -105,10 +100,7 @@ message("CONFIG DIRECTORY ${PROJECT_BINARY_DIR}")
 include_directories("${PROJECT_BINARY_DIR}/conf")
 
 # ============ INCLUDES ============ 
-INCLUDE_DIRECTORIES(${wolfcore_INCLUDE_DIRS})
-INCLUDE_DIRECTORIES(${wolfvision_INCLUDE_DIRS})
 INCLUDE_DIRECTORIES(BEFORE "include")
-INCLUDE_DIRECTORIES(${APRILTAG_INCLUDE_DIRS})
 
 # ============ HEADERS ============ 
 SET(HDRS_FACTOR
@@ -163,10 +155,8 @@ endif()
 
 #Link the created libraries
 #===============EXAMPLE=========================
-# IF (Ceres_FOUND)
-#     TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${CERES_LIBRARIES})
-# ENDIF(Ceres_FOUND)
-TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${wolfvision_LIBRARIES})
+TARGET_LINK_LIBRARIES(${PLUGIN_NAME} wolfcore)
+TARGET_LINK_LIBRARIES(${PLUGIN_NAME} wolfvision)
 TARGET_LINK_LIBRARIES(${PLUGIN_NAME} apriltag)
 
 #Build tests
@@ -176,38 +166,57 @@ IF(BUILD_TESTS)
   add_subdirectory(test)
 ENDIF(BUILD_TESTS)
 
+
 #install library
 #=============================================================
 INSTALL(TARGETS ${PLUGIN_NAME} EXPORT ${PLUGIN_NAME}Targets
-      RUNTIME DESTINATION bin
-      LIBRARY DESTINATION lib
-      ARCHIVE DESTINATION lib)
+  RUNTIME DESTINATION bin
+  LIBRARY DESTINATION ${LIB_INSTALL_DIR}
+  ARCHIVE DESTINATION ${LIB_INSTALL_DIR}
+)
+install(EXPORT ${PLUGIN_NAME}Targets DESTINATION lib/${PLUGIN_NAME}/cmake)
+
+
+# Configure the package installation
+include(CMakePackageConfigHelpers)
+configure_package_config_file(
+  ${CMAKE_SOURCE_DIR}/cmake_modules/${PLUGIN_NAME}Config.cmake.in
+  ${CMAKE_CURRENT_BINARY_DIR}/${PLUGIN_NAME}Config.cmake
+  INSTALL_DESTINATION ${LIB_INSTALL_DIR}/${PLUGIN_NAME}/cmake
+  PATH_VARS INCLUDE_INSTALL_DIR LIB_INSTALL_DIR
+)
+
+install(
+  FILES 
+  ${CMAKE_CURRENT_BINARY_DIR}/${PLUGIN_NAME}Config.cmake
+  DESTINATION 
+  ${LIB_INSTALL_DIR}/${PLUGIN_NAME}/cmake
+)
+
+# Specifies include directories to use when compiling the plugin target
+# This way, include_directories does not need to be called in plugins depending on this one
+target_include_directories(${PLUGIN_NAME} INTERFACE
+  $<INSTALL_INTERFACE:${INCLUDE_INSTALL_DIR}>
+)
 
-install(EXPORT ${PLUGIN_NAME}Targets DESTINATION lib/cmake/${PLUGIN_NAME})
 #install headers
 INSTALL(FILES ${HDRS_FACTOR}
-  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/factor)
+  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/factor)
 INSTALL(FILES ${HDRS_FEATURE}
-  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/feature)
+  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/feature)
 INSTALL(FILES ${HDRS_LANDMARK}
-  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/landmark)
+  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/landmark)
 INSTALL(FILES ${HDRS_PROCESSOR}
-  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/processor)
-
-FILE(WRITE ${PROJECT_NAME}.found "")
-INSTALL(FILES ${PROJECT_NAME}.found
-  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME})
+  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/processor)
 
 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}")
+  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/internal)
 
 INSTALL(DIRECTORY ${SPDLOG_INCLUDE_DIRS} DESTINATION "include/iri-algorithms/")
 
 export(PACKAGE ${PLUGIN_NAME})
 
-FIND_PACKAGE(Doxygen)
+FIND_PACKAGE(Doxygen MODULE)
 
 FIND_PATH(IRI_DOC_DIR doxygen.conf ${CMAKE_SOURCE_DIR}/doc/iri_doc/)
 IF (IRI_DOC_DIR)

cmake_modules/wolfapriltagConfig.cmake

-#edit the following line to add the librarie's header files
-FIND_PATH(
-    wolfapriltag_INCLUDE_DIRS
-    NAMES apriltag.found
-    PATHS /usr/local/include/iri-algorithms/wolf/plugin_apriltag)
-IF(wolfapriltag_INCLUDE_DIRS)
-  MESSAGE("Found wolf apriltag include dirs: ${wolfapriltag_INCLUDE_DIRS}")
-ELSE(wolfapriltag_INCLUDE_DIRS)
-  MESSAGE("Couldn't find wolf apriltag include dirs")
-ENDIF(wolfapriltag_INCLUDE_DIRS)
-FIND_LIBRARY(
-    wolfapriltag_LIBRARIES
-    NAMES libwolfapriltag.so libwolfapriltag.dylib
-    PATHS /usr/local/lib)
-IF(wolfapriltag_LIBRARIES)
-  MESSAGE("Found wolf apriltag lib: ${wolfapriltag_LIBRARIES}")
-ELSE(wolfapriltag_LIBRARIES)
-  MESSAGE("Couldn't find wolf apriltag lib")
-ENDIF(wolfapriltag_LIBRARIES)
-
-IF (wolfapriltag_INCLUDE_DIRS AND wolfapriltag_LIBRARIES)
-   SET(wolfapriltag_FOUND TRUE)
- ELSE(wolfapriltag_INCLUDE_DIRS AND wolfapriltag_LIBRARIES)
-   set(wolfapriltag_FOUND FALSE)
-ENDIF (wolfapriltag_INCLUDE_DIRS AND wolfapriltag_LIBRARIES)
-
-IF (wolfapriltag_FOUND)
-   IF (NOT wolfapriltag_FIND_QUIETLY)
-      MESSAGE(STATUS "Found wolf apriltag: ${wolfapriltag_LIBRARIES}")
-   ENDIF (NOT wolfapriltag_FIND_QUIETLY)
-ELSE (wolfapriltag_FOUND)
-   IF (wolfapriltag_FIND_REQUIRED)
-      MESSAGE(FATAL_ERROR "Could not find wolf apriltag")
-   ENDIF (wolfapriltag_FIND_REQUIRED)
-ENDIF (wolfapriltag_FOUND)
-
-
-macro(wolf_report_not_found REASON_MSG)
-  set(wolfapriltag_FOUND FALSE)
-  unset(wolfapriltag_INCLUDE_DIRS)
-  unset(wolfapriltag_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 (wolfapriltag_FIND_QUIETLY)
-    message(STATUS "Failed to find wolf apriltag- " ${REASON_MSG} ${ARGN})
-  elseif(wolfapriltag_FIND_REQUIRED)
-    message(FATAL_ERROR "Failed to find wolf apriltag - " ${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 apriltag - " ${REASON_MSG} ${ARGN})
-  endif ()
-  return()
-endmacro(wolf_report_not_found)
-
-if(NOT wolfapriltag_FOUND)
-  wolf_report_not_found("Something went wrong while setting up wolf apriltag.")
-endif(NOT wolfapriltag_FOUND)
-# Set the include directories for wolf (itself).
-set(wolfapriltag_FOUND TRUE)
-
-# Now we gather all the required dependencies for Wolf Laser
-
-FIND_PACKAGE(wolfvision REQUIRED)
-list(APPEND wolfapriltag_INCLUDE_DIRS ${wolfvision_INCLUDE_DIR})
-list(APPEND wolfapriltag_LIBRARIES ${wolfvision_LIBRARY})
-
-if(NOT OpenCV_FOUND)
-  FIND_PACKAGE(OpenCV REQUIRED)
-  list(APPEND wolfapriltag_INCLUDE_DIRS ${OpenCV_INCLUDE_DIRS})
-  list(APPEND wolfapriltag_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 wolfapriltag_INCLUDE_DIRS)
-  list(APPEND wolfapriltag_INCLUDE_DIRS ${wolfcore_INCLUDE_DIRS})
-  list(REVERSE wolfapriltag_INCLUDE_DIRS)
-
-  list(REVERSE wolfapriltag_LIBRARIES)
-  list(APPEND wolfapriltag_LIBRARIES ${wolfcore_LIBRARIES})
-  list(REVERSE wolfapriltag_LIBRARIES)
-endif()
-
-# provide both INCLUDE_DIR and INCLUDE_DIRS
-SET(wolfapriltag_INCLUDE_DIR ${wolfapriltag_INCLUDE_DIRS})
-# provide both LIBRARY and LIBRARIES 
-SET(wolfapriltag_LIBRARY ${wolfapriltag_LIBRARIES})

cmake_modules/wolfapriltagConfig.cmake.in

+set(@PLUGIN_NAME@_VERSION 0.0.1)
+
+
+@PACKAGE_INIT@
+
+set_and_check(@PLUGIN_NAME@_INCLUDE_DIR "@PACKAGE_INCLUDE_INSTALL_DIR@")
+set(@PLUGIN_NAME@_INCLUDE_DIRS @PLUGIN_NAME@_INCLUDE_DIR)
+set_and_check(@PLUGIN_NAME@_LIB_INSTALL_DIR "@PACKAGE_LIB_INSTALL_DIR@")
+set(@PLUGIN_NAME@_LIB_INSTALL_DIRS @PLUGIN_NAME@_LIB_INSTALL_DIR)
+
+# forwards the correct parameters given to FIND_DEPENDENCIES
+include(CMakeFindDependencyMacro)
+FIND_DEPENDENCY(apriltag REQUIRED)
+
+include("${CMAKE_CURRENT_LIST_DIR}/@PLUGIN_NAME@Targets.cmake")
+
+check_required_components(@PLUGIN_NAME@)
\ No newline at end of file

include/apriltag/processor/processor_tracker_landmark_apriltag.h

@@ -22,16 +22,47 @@
 #ifndef _PROCESSOR_TRACKER_LANDMARK_APRILTAG_H_
 #define _PROCESSOR_TRACKER_LANDMARK_APRILTAG_H_
 
-//Wolf includes
-#include "core/common/wolf.h"
-#include "core/processor/processor_tracker_landmark.h"
-#include "vision/sensor/sensor_camera.h"
-#include <apriltag/apriltag.h>
-#include <core/factor/factor_distance_3d.h>
+// Wolf apriltag includes
+#include "apriltag/feature/feature_apriltag.h"
+#include "apriltag/landmark/landmark_apriltag.h"
+
+// IPPE (copy from https://github.com/tobycollins/IPPE)
+#include "ippe.h"
 
-// open cv
-#include <opencv/cv.h>
+// Wolf vision
+#include <vision/math/pinhole_tools.h>
+#include <vision/capture/capture_image.h>
+#include <vision/sensor/sensor_camera.h>
 
+// Wolf core
+#include <core/math/rotations.h>
+#include <core/state_block/state_quaternion.h>  /// REMOVE?
+#include <core/factor/factor_relative_pose_3d_with_extrinsics.h>
+#include <core/processor/processor_tracker_landmark.h>
+
+// apriltag detection Michigan library
+#include <apriltag/apriltag.h>
+#include <apriltag/common/homography.h>
+#include <apriltag/common/zarray.h>
+#include <apriltag/tag16h5.h>
+#include <apriltag/tag25h9.h>
+#include <apriltag/tag36h11.h>
+#include <apriltag/tagCircle21h7.h>
+#include <apriltag/tagCircle49h12.h>
+#include <apriltag/tagCustom48h12.h>
+#include <apriltag/tagStandard41h12.h>
+#include <apriltag/tagStandard52h13.h>
+#include "apriltag/common/homography.h"
+#include "apriltag/common/zarray.h"
+
+// opencv
+// OpenCV
+#include <opencv2/core.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/core/eigen.hpp>
+
+// Eigen
+#include <Eigen/Dense>
 
 namespace wolf
 {
@@ -183,7 +214,9 @@ class ProcessorTrackerLandmarkApriltag : public ProcessorTrackerLandmark
         double getTagWidth(int _id) const;
         std::string getTagFamily() const;
         FeatureBasePtrList getIncomingDetections() const;
+        FeatureBasePtrList getIncomingDetections();
         FeatureBasePtrList getLastDetections() const;
+        FeatureBasePtrList getLastDetections();
         void ippePoseEstimation(apriltag_detection_t *_det,
                                 cv::Mat_<double> _K,
                                 double _tag_width,

internal/config.h.in

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

src/processor/processor_tracker_landmark_apriltag.cpp

@@ -20,37 +20,7 @@
 //
 //--------LICENSE_END--------
 
-// this plugin
 #include "apriltag/processor/processor_tracker_landmark_apriltag.h"
-#include "apriltag/feature/feature_apriltag.h"
-#include "apriltag/landmark/landmark_apriltag.h"
-#include "apriltag/factor/factor_apriltag.h"
-#include "apriltag/processor/ippe.h"
-
-// dependent plugin
-#include <vision/capture/capture_image.h>
-#include <vision/math/pinhole_tools.h>
-
-// wolf
-#include <core/math/rotations.h>
-#include <core/state_block/state_quaternion.h>
-
-// apriltag library
-#include <apriltag/common/homography.h>
-#include <apriltag/common/zarray.h>
-#include <apriltag/tag16h5.h>
-#include <apriltag/tag25h9.h>
-#include <apriltag/tag36h11.h>
-#include <apriltag/tagCircle21h7.h>
-#include <apriltag/tagCircle49h12.h>
-#include <apriltag/tagCustom48h12.h>
-#include <apriltag/tagStandard41h12.h>
-#include <apriltag/tagStandard52h13.h>
-#include <core/factor/factor_distance_3d.h>
-
-// opencv
-#include <opencv2/imgproc/imgproc.hpp>
-#include <opencv2/core/eigen.hpp>
 
 namespace wolf {
 
@@ -266,14 +236,11 @@ void ProcessorTrackerLandmarkApriltag::postProcess()
 FactorBasePtr ProcessorTrackerLandmarkApriltag::emplaceFactor(FeatureBasePtr _feature_ptr,
                                                               LandmarkBasePtr _landmark_ptr)
 {
-    return FactorBase::emplace<FactorApriltag>(_feature_ptr,
-                                               getSensor(),
-                                               getLast()->getFrame(),
-                                               std::static_pointer_cast<LandmarkApriltag>(_landmark_ptr),
-                                               std::static_pointer_cast<FeatureApriltag> (_feature_ptr ),
+    return FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(_feature_ptr,
+                                               _feature_ptr,
+                                               _landmark_ptr,
                                                shared_from_this(),
-                                               params_->apply_loss_function,
-                                               FAC_ACTIVE);
+                                               params_->apply_loss_function);
 }
 
 LandmarkBasePtr ProcessorTrackerLandmarkApriltag::emplaceLandmark(FeatureBasePtr _feature_ptr)
@@ -369,16 +336,21 @@ bool ProcessorTrackerLandmarkApriltag::voteForKeyFrame() const
     if (detections_last_.empty())
         return false;
 
-    double dt = getIncoming()->getTimeStamp() - getOrigin()->getTimeStamp();
+    double dt_incoming_origin = getIncoming()->getTimeStamp().get() - getOrigin()->getTimeStamp().get();
+    bool more_than_min_time_vote = dt_incoming_origin > min_time_span_; 
+    bool too_long_since_last_KF = dt_incoming_origin > max_time_span_ + 1e-5;
 
-    // the elapsed time since last KF is too long 
-    if (dt > max_time_span_ + 1e-5){
+    bool enough_features_in_last = detections_last_.size() >= min_features_for_keyframe_;
+    bool enough_features_in_incoming = detections_incoming_.size() >= min_features_for_keyframe_;
+    
+    // not enough detection in incoming capture and a minimum time since last KF has past
+    if (enough_features_in_last and !enough_features_in_incoming and more_than_min_time_vote)
         return true;
-    }
 
-    // too few detections in incoming capture and a minimum time since last KF has past
-    if ((detections_incoming_.size() <= min_features_for_keyframe_) and (dt >= min_time_span_))
+    // the elapsed time since last KF is too long 
+    if (too_long_since_last_KF and enough_features_in_last){
         return true;
+    }
 
     // Vote for every image processed at the beginning if possible
     if (nb_vote_ < nb_vote_for_every_first_){
@@ -486,11 +458,27 @@ void ProcessorTrackerLandmarkApriltag::pinholeHomogeneous(Eigen::Matrix3d const
 }
 
 FeatureBasePtrList ProcessorTrackerLandmarkApriltag::getIncomingDetections() const
+{
+    FeatureBasePtrList list_const;
+    for (auto && obj : detections_incoming_)
+        list_const.push_back(obj);
+    return list_const;
+}
+
+FeatureBasePtrList ProcessorTrackerLandmarkApriltag::getIncomingDetections()
 {
     return detections_incoming_;
 }
 
 FeatureBasePtrList ProcessorTrackerLandmarkApriltag::getLastDetections() const
+{
+    FeatureBasePtrList list_const;
+    for (auto && obj : detections_last_)
+        list_const.push_back(obj);
+    return list_const;
+}
+
+FeatureBasePtrList ProcessorTrackerLandmarkApriltag::getLastDetections()
 {
     return detections_last_;
 }
@@ -523,25 +511,7 @@ void ProcessorTrackerLandmarkApriltag::advanceDerived()
 
 void ProcessorTrackerLandmarkApriltag::resetDerived()
 {   
-    // Add 3d distance constraint between 2 frames
-    if (getProblem()->getMotionProviderMap().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);
-        }
-    }
-    
+    // BAD -> should be rewritten some other way
     if (getProblem()->getMotionProviderMap().empty() && reestimate_last_frame_){
         reestimateLastFrame();
     }

src/yaml/processor_tracker_landmark_apriltag_yaml.cpp

@@ -30,7 +30,7 @@
 // wolf
 #include "apriltag/processor/processor_tracker_landmark_apriltag.h"
 #include "core/yaml/yaml_conversion.h"
-#include "core/common/factory.h"
+#include "core/processor/factory_processor.h"
 
 // yaml-cpp library
 #include <yaml-cpp/yaml.h>

test/CMakeLists.txt

 # Retrieve googletest from github & compile
 add_subdirectory(gtest)
 
-
-# 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)         #
+#                                                         #
+###########################################################
 
 wolf_add_gtest(gtest_feature_apriltag gtest_feature_apriltag.cpp)
-target_link_libraries(gtest_feature_apriltag ${PLUGIN_NAME})
 
 wolf_add_gtest(gtest_landmark_apriltag gtest_landmark_apriltag.cpp)
-target_link_libraries(gtest_landmark_apriltag ${PLUGIN_NAME})
 
-# ProcessorTrackerLandmarkApriltag test
 wolf_add_gtest(gtest_processor_tracker_landmark_apriltag gtest_processor_tracker_landmark_apriltag.cpp)
-target_link_libraries(gtest_processor_tracker_landmark_apriltag ${PLUGIN_NAME})
-
-wolf_add_gtest(gtest_factor_apriltag gtest_factor_apriltag.cpp)
-target_link_libraries(gtest_factor_apriltag ${PLUGIN_NAME})

test/gtest/CMakeLists.txt

-cmake_minimum_required(VERSION 2.8.8)
-project(gtest_builder C CXX)
+if(${CMAKE_VERSION} VERSION_LESS "3.11.0") 
+  message("CMake version less than 3.11.0")
 
-# We need thread support
-#find_package(Threads REQUIRED)
+  # Enable ExternalProject CMake module
+  include(ExternalProject)
 
-# Enable ExternalProject CMake module
-include(ExternalProject)
+  set(GTEST_FORCE_SHARED_CRT ON)
+  set(GTEST_DISABLE_PTHREADS ON) # without this in ubuntu 18.04 we get linking errors
 
-set(GTEST_FORCE_SHARED_CRT ON)
-set(GTEST_DISABLE_PTHREADS OFF)
+  # 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
+  )
 
-# 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()
+  # Get GTest source and binary directories from CMake project
 
-# Download GoogleTest