Merge branch 'devel' into 458-hasstateblocks-getstate-behaves-in-a-non-predictible-way

.clang-format

@@ -6,7 +6,7 @@ AccessModifierOffset: -2
 AlignAfterOpenBracket: Align
 AlignConsecutiveAssignments: true
 AlignConsecutiveDeclarations: true
-AlignEscapedNewlines: Left
+AlignEscapedNewlines: Right
 AlignOperands: true
 AlignTrailingComments: true
 AllowAllParametersOfDeclarationOnNextLine: false
@@ -97,6 +97,7 @@ PenaltyExcessCharacter: 1000000
 PenaltyReturnTypeOnItsOwnLine: 200
 PointerAlignment: Left
 ReflowComments: true
+SeparateDefinitionBlocks: Always
 SortIncludes: false
 SortUsingDeclarations: true
 SpaceAfterCStyleCast: false

.gitignore

@@ -34,7 +34,5 @@ src/examples/map_apriltag_save.yaml
 
 \.vscode/
 .clangd
-wolfcore.found
-/wolf.found
 .ccls*
 compile_commands.json

CMakeLists.txt

@@ -16,7 +16,7 @@ SET(CMAKE_SKIP_INSTALL_ALL_DEPENDENCY FALSE)
 SET(EXECUTABLE_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/bin)
 SET(LIBRARY_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/lib)
 SET(LIB_INSTALL_DIR lib)
-SET(INCLUDE_INSTALL_DIR include/iri-algorithms/wolf)
+SET(INCLUDE_INSTALL_DIR include/wolf)
 
 # Build type
 IF (NOT CMAKE_BUILD_TYPE)
@@ -80,6 +80,7 @@ option(_WOLF_TRACE "Enable wolf tracing macro" ON)
 FIND_PACKAGE(Threads REQUIRED MODULE)
 FIND_PACKAGE(Ceres REQUIRED CONFIG)
 FIND_PACKAGE(Eigen3 3.3 REQUIRED CONFIG)
+FIND_PACKAGE(spdlog REQUIRED CONFIG)
 if(${EIGEN3_VERSION_STRING} VERSION_LESS 3.3)
   message(FATAL_ERROR "Wolf requires Eigen >= 3.3. Found Eigen ${EIGEN3_VERSION_STRING}")
 endif()
@@ -89,7 +90,7 @@ FIND_PACKAGE(yaml-cpp REQUIRED CONFIG)
 set(_WOLF_ROOT_DIR ${CMAKE_SOURCE_DIR})
 set(_WOLF_LIB_DIR ${CMAKE_INSTALL_PREFIX}/${LIB_INSTALL_DIR})
 # Define the directory where will be the configured config.h
-SET(WOLF_CONFIG_DIR ${PROJECT_BINARY_DIR}/conf/core/internal)
+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}")
@@ -106,151 +107,142 @@ message(STATUS "WOLF CONFIG DIRECTORY ${WOLF_CONFIG_DIR}")
 message(STATUS "WOLF CONFIG FILE      ${WOLF_CONFIG_DIR}/config.h")
 include_directories("${PROJECT_BINARY_DIR}/conf")
 
-# include spdlog (logging library)
-FIND_PATH(SPDLOG_INCLUDE_DIR spdlog.h /usr/local/include/spdlog /usr/include/spdlog)
-IF (SPDLOG_INCLUDE_DIR)
-  # INCLUDE_DIRECTORIES(${SPDLOG_INCLUDE_DIR})
-  MESSAGE(STATUS "Found spdlog: ${SPDLOG_INCLUDE_DIR}")
-ELSE (SPDLOG_INCLUDE_DIR)
- MESSAGE(FATAL_ERROR "Could not find spdlog")
-ENDIF (SPDLOG_INCLUDE_DIR)
-
 # ============ INCLUDES ============ 
 INCLUDE_DIRECTORIES("include") # In this same project
 
 # ============ HEADERS ============ 
 SET(HDRS_CAPTURE
-  include/core/capture/capture_base.h
-  include/core/capture/capture_motion.h
-  include/core/capture/capture_odom_2d.h
-  include/core/capture/capture_odom_3d.h
-  include/core/capture/capture_pose.h
-  include/core/capture/capture_void.h
-  include/core/capture/capture_diff_drive.h
+  include/${PROJECT_NAME}/capture/capture_base.h
+  include/${PROJECT_NAME}/capture/capture_motion.h
+  include/${PROJECT_NAME}/capture/capture_odom_2d.h
+  include/${PROJECT_NAME}/capture/capture_odom_3d.h
+  include/${PROJECT_NAME}/capture/capture_pose.h
+  include/${PROJECT_NAME}/capture/capture_void.h
+  include/${PROJECT_NAME}/capture/capture_diff_drive.h
   )
 SET(HDRS_COMMON
-  include/core/common/factory.h
-  include/core/common/node_base.h
-  include/core/common/time_stamp.h
-  include/core/common/wolf.h
-  include/core/common/params_base.h
+  include/${PROJECT_NAME}/common/factory.h
+  include/${PROJECT_NAME}/common/node_base.h
+  include/${PROJECT_NAME}/common/time_stamp.h
+  include/${PROJECT_NAME}/common/wolf.h
+  include/${PROJECT_NAME}/common/params_base.h
   )
 SET(HDRS_FACTOR
-  include/core/factor/factor_analytic.h
-  include/core/factor/factor_autodiff.h
-  include/core/factor/factor_base.h
-  include/core/factor/factor_block_absolute.h
-  include/core/factor/factor_block_difference.h
-  include/core/factor/factor_diff_drive.h
-  include/core/factor/factor_distance_3d.h
-  include/core/factor/factor_pose_2d.h
-  include/core/factor/factor_pose_3d.h
-  include/core/factor/factor_quaternion_absolute.h
-  include/core/factor/factor_relative_pose_2d.h
-  include/core/factor/factor_relative_pose_2d_with_extrinsics.h
-  include/core/factor/factor_relative_pose_3d.h
-  include/core/factor/factor_pose_3d_with_extrinsics.h
-  include/core/factor/factor_relative_pose_3d_with_extrinsics.h
-  include/core/factor/factor_velocity_local_direction_3d.h
+  include/${PROJECT_NAME}/factor/factor_analytic.h
+  include/${PROJECT_NAME}/factor/factor_autodiff.h
+  include/${PROJECT_NAME}/factor/factor_base.h
+  include/${PROJECT_NAME}/factor/factor_block_absolute.h
+  include/${PROJECT_NAME}/factor/factor_block_difference.h
+  include/${PROJECT_NAME}/factor/factor_diff_drive.h
+  include/${PROJECT_NAME}/factor/factor_distance_3d.h
+  include/${PROJECT_NAME}/factor/factor_pose_2d.h
+  include/${PROJECT_NAME}/factor/factor_pose_3d.h
+  include/${PROJECT_NAME}/factor/factor_quaternion_absolute.h
+  include/${PROJECT_NAME}/factor/factor_relative_pose_2d.h
+  include/${PROJECT_NAME}/factor/factor_relative_pose_2d_with_extrinsics.h
+  include/${PROJECT_NAME}/factor/factor_relative_pose_3d.h
+  include/${PROJECT_NAME}/factor/factor_pose_3d_with_extrinsics.h
+  include/${PROJECT_NAME}/factor/factor_relative_pose_3d_with_extrinsics.h
+  include/${PROJECT_NAME}/factor/factor_velocity_local_direction_3d.h
   )
 SET(HDRS_FEATURE
-  include/core/feature/feature_base.h
-  include/core/feature/feature_diff_drive.h
-  include/core/feature/feature_match.h
-  include/core/feature/feature_motion.h
-  include/core/feature/feature_odom_2d.h
-  include/core/feature/feature_pose.h
+  include/${PROJECT_NAME}/feature/feature_base.h
+  include/${PROJECT_NAME}/feature/feature_diff_drive.h
+  include/${PROJECT_NAME}/feature/feature_match.h
+  include/${PROJECT_NAME}/feature/feature_motion.h
+  include/${PROJECT_NAME}/feature/feature_odom_2d.h
+  include/${PROJECT_NAME}/feature/feature_pose.h
   )
 SET(HDRS_FRAME
-  include/core/frame/frame_base.h
+  include/${PROJECT_NAME}/frame/frame_base.h
   )
 SET(HDRS_HARDWARE
-  include/core/hardware/hardware_base.h
+  include/${PROJECT_NAME}/hardware/hardware_base.h
   )
 SET(HDRS_LANDMARK
-  include/core/landmark/landmark_base.h
-  include/core/landmark/landmark_match.h
+  include/${PROJECT_NAME}/landmark/landmark_base.h
+  include/${PROJECT_NAME}/landmark/landmark_match.h
   )
 SET(HDRS_MATH
-  include/core/math/SE2.h
-  include/core/math/SE3.h
-  include/core/math/rotations.h
-  include/core/math/covariance.h
+  include/${PROJECT_NAME}/math/SE2.h
+  include/${PROJECT_NAME}/math/SE3.h
+  include/${PROJECT_NAME}/math/rotations.h
+  include/${PROJECT_NAME}/math/covariance.h
   )
 SET(HDRS_MAP
-  include/core/map/factory_map.h
-  include/core/map/map_base.h
+  include/${PROJECT_NAME}/map/factory_map.h
+  include/${PROJECT_NAME}/map/map_base.h
   )
 SET(HDRS_PROBLEM
-  include/core/problem/problem.h
+  include/${PROJECT_NAME}/problem/problem.h
   )
 SET(HDRS_PROCESSOR
-  include/core/processor/motion_buffer.h
-  include/core/processor/motion_provider.h
-  include/core/processor/processor_base.h
-  include/core/processor/processor_diff_drive.h
-  include/core/processor/processor_fixed_wing_model.h
-  include/core/processor/factory_processor.h
-  include/core/processor/processor_loop_closure.h
-  include/core/processor/processor_motion.h
-  include/core/processor/processor_odom_2d.h
-  include/core/processor/processor_odom_3d.h
-  include/core/processor/processor_pose.h
-  include/core/processor/processor_tracker.h
-  include/core/processor/processor_tracker_feature.h
-  include/core/processor/processor_tracker_landmark.h
-  include/core/processor/track_matrix.h
+  include/${PROJECT_NAME}/processor/motion_buffer.h
+  include/${PROJECT_NAME}/processor/motion_provider.h
+  include/${PROJECT_NAME}/processor/processor_base.h
+  include/${PROJECT_NAME}/processor/processor_diff_drive.h
+  include/${PROJECT_NAME}/processor/processor_fixed_wing_model.h
+  include/${PROJECT_NAME}/processor/factory_processor.h
+  include/${PROJECT_NAME}/processor/processor_loop_closure.h
+  include/${PROJECT_NAME}/processor/processor_motion.h
+  include/${PROJECT_NAME}/processor/processor_odom_2d.h
+  include/${PROJECT_NAME}/processor/processor_odom_3d.h
+  include/${PROJECT_NAME}/processor/processor_pose.h
+  include/${PROJECT_NAME}/processor/processor_tracker.h
+  include/${PROJECT_NAME}/processor/processor_tracker_feature.h
+  include/${PROJECT_NAME}/processor/processor_tracker_landmark.h
+  include/${PROJECT_NAME}/processor/track_matrix.h
   )
 SET(HDRS_SENSOR
-  include/core/sensor/sensor_base.h
-  include/core/sensor/sensor_diff_drive.h
-  include/core/sensor/factory_sensor.h
-  include/core/sensor/sensor_motion_model.h
-  include/core/sensor/sensor_odom_2d.h
-  include/core/sensor/sensor_odom_3d.h
-  include/core/sensor/sensor_pose.h
+  include/${PROJECT_NAME}/sensor/sensor_base.h
+  include/${PROJECT_NAME}/sensor/sensor_diff_drive.h
+  include/${PROJECT_NAME}/sensor/factory_sensor.h
+  include/${PROJECT_NAME}/sensor/sensor_motion_model.h
+  include/${PROJECT_NAME}/sensor/sensor_odom_2d.h
+  include/${PROJECT_NAME}/sensor/sensor_odom_3d.h
+  include/${PROJECT_NAME}/sensor/sensor_pose.h
   )
 SET(HDRS_SOLVER
-  include/core/solver/solver_manager.h
-  include/core/solver/factory_solver.h
+  include/${PROJECT_NAME}/solver/solver_manager.h
+  include/${PROJECT_NAME}/solver/factory_solver.h
   )
 SET(HDRS_STATE_BLOCK
-  include/core/state_block/factory_state_block.h
-  include/core/state_block/has_state_blocks.h
-  include/core/state_block/local_parametrization_angle.h
-  include/core/state_block/local_parametrization_base.h
-  include/core/state_block/local_parametrization_homogeneous.h
-  include/core/state_block/local_parametrization_quaternion.h
-  include/core/state_block/state_angle.h
-  include/core/state_block/state_block.h
-  include/core/state_block/state_composite.h
-  include/core/state_block/state_homogeneous_3d.h
-  include/core/state_block/state_quaternion.h
+  include/${PROJECT_NAME}/state_block/factory_state_block.h
+  include/${PROJECT_NAME}/state_block/has_state_blocks.h
+  include/${PROJECT_NAME}/state_block/local_parametrization_angle.h
+  include/${PROJECT_NAME}/state_block/local_parametrization_base.h
+  include/${PROJECT_NAME}/state_block/local_parametrization_homogeneous.h
+  include/${PROJECT_NAME}/state_block/local_parametrization_quaternion.h
+  include/${PROJECT_NAME}/state_block/state_angle.h
+  include/${PROJECT_NAME}/state_block/state_block.h
+  include/${PROJECT_NAME}/state_block/state_composite.h
+  include/${PROJECT_NAME}/state_block/state_homogeneous_3d.h
+  include/${PROJECT_NAME}/state_block/state_quaternion.h
   )
 SET(HDRS_TRAJECTORY
-  include/core/trajectory/trajectory_base.h
+  include/${PROJECT_NAME}/trajectory/trajectory_base.h
   )
 SET(HDRS_TREE_MANAGER
-  include/core/tree_manager/factory_tree_manager.h
-  include/core/tree_manager/tree_manager_base.h
-  include/core/tree_manager/tree_manager_sliding_window.h
-  include/core/tree_manager/tree_manager_sliding_window_dual_rate.h
+  include/${PROJECT_NAME}/tree_manager/factory_tree_manager.h
+  include/${PROJECT_NAME}/tree_manager/tree_manager_base.h
+  include/${PROJECT_NAME}/tree_manager/tree_manager_sliding_window.h
+  include/${PROJECT_NAME}/tree_manager/tree_manager_sliding_window_dual_rate.h
   )
 SET(HDRS_UTILS
-  include/core/utils/check_log.h
-  include/core/utils/converter.h
-  include/core/utils/eigen_assert.h
-  include/core/utils/graph_search.h
-  include/core/utils/loader.h
-  include/core/utils/logging.h
-  include/core/utils/params_server.h
-  include/core/utils/singleton.h
-  include/core/utils/utils_gtest.h
-  include/core/utils/converter_utils.h
+  include/${PROJECT_NAME}/utils/check_log.h
+  include/${PROJECT_NAME}/utils/converter.h
+  include/${PROJECT_NAME}/utils/eigen_assert.h
+  include/${PROJECT_NAME}/utils/graph_search.h
+  include/${PROJECT_NAME}/utils/loader.h
+  include/${PROJECT_NAME}/utils/logging.h
+  include/${PROJECT_NAME}/utils/params_server.h
+  include/${PROJECT_NAME}/utils/singleton.h
+  include/${PROJECT_NAME}/utils/utils_gtest.h
+  include/${PROJECT_NAME}/utils/converter_utils.h
   )
 SET(HDRS_YAML
-  include/core/yaml/parser_yaml.h
-  include/core/yaml/yaml_conversion.h
+  include/${PROJECT_NAME}/yaml/parser_yaml.h
+  include/${PROJECT_NAME}/yaml/yaml_conversion.h
   )
   
 # ============ SOURCES ============ 
@@ -354,13 +346,13 @@ SET(SRCS_YAML
 IF (Ceres_FOUND)
     SET(HDRS_CERES_WRAPPER
       #ceres_wrapper/qr_manager.h
-      include/core/ceres_wrapper/cost_function_wrapper.h
-      include/core/ceres_wrapper/create_numeric_diff_cost_function.h
-      include/core/ceres_wrapper/local_parametrization_wrapper.h
-      include/core/ceres_wrapper/iteration_update_callback.h
-      include/core/ceres_wrapper/solver_ceres.h
-      include/core/solver/solver_manager.h
-      include/core/solver_suitesparse/sparse_utils.h
+      include/${PROJECT_NAME}/ceres_wrapper/cost_function_wrapper.h
+      include/${PROJECT_NAME}/ceres_wrapper/create_numeric_diff_cost_function.h
+      include/${PROJECT_NAME}/ceres_wrapper/local_parametrization_wrapper.h
+      include/${PROJECT_NAME}/ceres_wrapper/iteration_update_callback.h
+      include/${PROJECT_NAME}/ceres_wrapper/solver_ceres.h
+      include/${PROJECT_NAME}/solver/solver_manager.h
+      include/${PROJECT_NAME}/solver_suitesparse/sparse_utils.h
       )
     SET(SRCS_CERES_WRAPPER
       #ceres_wrapper/qr_manager.cpp
@@ -413,6 +405,7 @@ endif()
 TARGET_LINK_LIBRARIES(${PLUGIN_NAME} PUBLIC ${CMAKE_THREAD_LIBS_INIT} dl)
 TARGET_LINK_LIBRARIES(${PLUGIN_NAME} PUBLIC yaml-cpp)
 TARGET_LINK_LIBRARIES(${PLUGIN_NAME} PUBLIC Eigen3::Eigen)
+TARGET_LINK_LIBRARIES(${PLUGIN_NAME} PUBLIC spdlog::spdlog)
 IF (Ceres_FOUND)
     TARGET_LINK_LIBRARIES(${PLUGIN_NAME} PUBLIC ceres)
 ENDIF(Ceres_FOUND)
@@ -493,7 +486,7 @@ INSTALL(FILES ${HDRS_SENSOR}
 INSTALL(FILES ${HDRS_SOLVER}
   DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/solver)
 #INSTALL(FILES ${HDRS_SOLVER_SUITESPARSE}
-#  DESTINATION ${INCLUDE_INSTALL_DIR}/core/solver_suitesparse)
+#  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/solver_suitesparse)
 INSTALL(FILES ${HDRS_STATE_BLOCK}
   DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/state_block)
 INSTALL(FILES ${HDRS_TRAJECTORY}
@@ -508,9 +501,6 @@ INSTALL(FILES ${HDRS_YAML}
 INSTALL(FILES "${WOLF_CONFIG_DIR}/config.h"
   DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/internal)
 
-
-INSTALL(DIRECTORY ${SPDLOG_INCLUDE_DIRS} DESTINATION "include/iri-algorithms/")
-
 export(PACKAGE ${PLUGIN_NAME})
 
 FIND_PACKAGE(Doxygen MODULE)

cmake_modules/wolfcoreConfig.cmake.in

@@ -8,6 +8,7 @@ FIND_DEPENDENCY(Threads REQUIRED)
 FIND_DEPENDENCY(Ceres REQUIRED)
 FIND_DEPENDENCY(Eigen3 3.3 REQUIRED)
 FIND_DEPENDENCY(yaml-cpp REQUIRED)
+FIND_DEPENDENCY(spdlog REQUIRED)
 
 include("${CMAKE_CURRENT_LIST_DIR}/@PLUGIN_NAME@Targets.cmake")