diff --git a/.gitignore b/.gitignore
index 8fd4190ee6d52f3aac6f090728b06fe6a862513b..6b43f90c185cd290052c10a6a508d8f1998a895c 100644
--- a/.gitignore
+++ b/.gitignore
@@ -6,6 +6,8 @@ bin/
build/
build_debug/
build_release/
+build-debug/
+build-release/
lib/
.idea/
./Wolf.user
diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index 2501eb45659b7ef96c55f3c25780523a04bfe189..0aabb6a729351c494488494dbfb8d40687eddabf 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -6,6 +6,13 @@ stages:
- csm_falko
############ YAML ANCHORS ############
+.print_variables_template: &print_variables_definition
+ # Print variables
+ - echo $WOLF_CORE_BRANCH
+ - echo $CI_COMMIT_BRANCH
+ - echo $WOLF_LASER_BRANCH
+ - echo $LASERSCANUTILS_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)
@@ -89,9 +96,12 @@ stages:
- if [ -d laser_scan_utils ]; then
- echo "directory laser_scan_utils exists"
- cd laser_scan_utils
+ - git checkout main
+ - git pull
+ - git checkout $LASERSCANUTILS_BRANCH
- git pull
- else
- - git clone https://gitlab.iri.upc.edu/labrobotica/algorithms/laser_scan_utils.git
+ - git clone -b $LASERSCANUTILS_BRANCH https://gitlab.iri.upc.edu/labrobotica/algorithms/laser_scan_utils.git
- cd laser_scan_utils
- fi
- mkdir -pv build
@@ -153,6 +163,7 @@ license_headers:
paths:
- ci_deps/wolf/
before_script:
+ - *print_variables_definition
- *preliminaries_definition
- *install_wolf_definition
script:
@@ -170,6 +181,7 @@ build_and_test_none:bionic:
paths:
- ci_deps/laser_scan_utils/
before_script:
+ - *print_variables_definition
- *preliminaries_definition
- *install_wolf_definition
- *install_laserscanutils_definition
@@ -190,6 +202,7 @@ build_and_test_csm:bionic:
paths:
- ci_deps/csm/
before_script:
+ - *print_variables_definition
- *preliminaries_definition
- *install_wolf_definition
- *install_csm_definition
@@ -211,6 +224,7 @@ build_and_test_falko:bionic:
paths:
- ci_deps/falkolib/
before_script:
+ - *print_variables_definition
- *preliminaries_definition
- *install_wolf_definition
- *install_falko_definition
@@ -235,6 +249,7 @@ build_and_test_csm_falko:bionic:
paths:
- ci_deps/falkolib/
before_script:
+ - *print_variables_definition
- *preliminaries_definition
- *install_wolf_definition
- *install_falko_definition
@@ -255,6 +270,7 @@ build_and_test_none:focal:
paths:
- ci_deps/laser_scan_utils/
before_script:
+ - *print_variables_definition
- *preliminaries_definition
- *install_wolf_definition
- *install_laserscanutils_definition
@@ -275,6 +291,7 @@ build_and_test_csm:focal:
paths:
- ci_deps/csm/
before_script:
+ - *print_variables_definition
- *preliminaries_definition
- *install_wolf_definition
- *install_csm_definition
@@ -296,6 +313,7 @@ build_and_test_falko:focal:
paths:
- ci_deps/falkolib/
before_script:
+ - *print_variables_definition
- *preliminaries_definition
- *install_wolf_definition
- *install_falko_definition
@@ -320,6 +338,7 @@ build_and_test_csm_falko:focal:
paths:
- ci_deps/falkolib/
before_script:
+ - *print_variables_definition
- *preliminaries_definition
- *install_wolf_definition
- *install_falko_definition
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 6a8ad1eab8d460fd4c256b2f48b0bed9106b361e..8687647283fcf148c50d38ba5de14afa9e3efb8f 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,10 +1,6 @@
# 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 1)
@@ -14,9 +10,11 @@ set(PLUGIN_NAME wolf${PROJECT_NAME})
MESSAGE("Starting ${PROJECT_NAME} CMakeLists ...")
+# Paths
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")
@@ -69,10 +67,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)
@@ -81,14 +75,19 @@ ENDIF()
option(_WOLF_TRACE "Enable wolf tracing macro" ON)
# ============ DEPENDENCIES ============
-FIND_PACKAGE(wolfcore REQUIRED)
-FIND_PACKAGE(laser_scan_utils REQUIRED)
-FIND_PACKAGE(csm QUIET)
+FIND_PACKAGE(wolfcore REQUIRED CONFIG)
+FIND_PACKAGE(laser_scan_utils REQUIRED CONFIG)
+# found only to set the XXX_FOUND variables
+# -> It would be better to look for them in the the laser_scan_utils target
FIND_PACKAGE(falkolib QUIET)
+find_package(PkgConfig)
+pkg_check_modules(csm QUIET csm)
+# link_directories(${csm_LIBRARY_DIRS})
# ============ 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)
@@ -108,8 +107,6 @@ message("CONFIG DIRECTORY ${PROJECT_BINARY_DIR}")
include_directories("${PROJECT_BINARY_DIR}/conf")
# ============ INCLUDES ============
-INCLUDE_DIRECTORIES(${wolfcore_INCLUDE_DIRS})
-INCLUDE_DIRECTORIES(${laser_scan_utils_INCLUDE_DIRS})
INCLUDE_DIRECTORIES(BEFORE "include")
# ============ HEADERS ============
@@ -244,10 +241,10 @@ ENDIF()
#Link the created libraries
#===============EXAMPLE=========================
-TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${wolfcore_LIBRARIES})
-TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${laser_scan_utils_LIBRARY})
+TARGET_LINK_LIBRARIES(${PLUGIN_NAME} wolfcore)
+TARGET_LINK_LIBRARIES(${PLUGIN_NAME} laser_scan_utils)
IF (falkolib_FOUND)
- TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${wolfcore_LIBRARIES} ${laser_scan_utils_LIBRARY} ${falkolib_LIBRARY})
+ TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${falkolib_LIBRARY})
ENDIF(falkolib_FOUND)
#Build tests
@@ -257,45 +254,67 @@ 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_CAPTURE}
- DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/capture)
+ DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/capture)
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_MATH}
- DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/math)
+ DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/math)
INSTALL(FILES ${HDRS_PROCESSOR}
- DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/processor)
+ DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/processor)
INSTALL(FILES ${HDRS_SENSOR}
- DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/sensor)
+ DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/sensor)
INSTALL(FILES ${HDRS_STATE_BLOCK}
- DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/state_block)
+ DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/state_block)
FILE(WRITE laser.found "")
INSTALL(FILES laser.found
- DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME})
+ DESTINATION ${INCLUDE_INSTALL_DIR})
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)
diff --git a/cmake_modules/Findcsm.cmake b/cmake_modules/Findcsm.cmake
deleted file mode 100755
index 6dafd97ded05ef9df2b4b19b318639855fa48af5..0000000000000000000000000000000000000000
--- a/cmake_modules/Findcsm.cmake
+++ /dev/null
@@ -1,64 +0,0 @@
-FIND_PATH(
- csm_INCLUDE_DIR
- NAMES algos.h
- PATHS /usr/local/include/csm)
-IF(csm_INCLUDE_DIR)
- MESSAGE("Found csm include dirs: ${csm_INCLUDE_DIR}")
-ELSE(csm_INCLUDE_DIR)
- MESSAGE("Couldn't find csm include dirs")
-ENDIF(csm_INCLUDE_DIR)
-
-FIND_LIBRARY(
- csm_LIBRARY
- NAMES libcsm.so libcsm.dylib
- PATHS /usr/local/lib)
-IF(csm_LIBRARY)
- MESSAGE("Found csm lib: ${csm_LIBRARY}")
-ELSE(csm_LIBRARY)
- MESSAGE("Couldn't find csm lib")
-ENDIF(csm_LIBRARY)
-
-IF (csm_INCLUDE_DIR AND csm_LIBRARY)
- SET(csm_FOUND TRUE)
- ELSE(csm_INCLUDE_DIR AND csm_LIBRARY)
- set(csm_FOUND FALSE)
-ENDIF (csm_INCLUDE_DIR AND csm_LIBRARY)
-
-IF (csm_FOUND)
- IF (NOT csm_FIND_QUIETLY)
- MESSAGE(STATUS "Found csm: ${csm_LIBRARY}")
- ENDIF (NOT csm_FIND_QUIETLY)
-ELSE (csm_FOUND)
- IF (csm_FIND_REQUIRED)
- MESSAGE(FATAL_ERROR "Could not find csm")
- ENDIF (csm_FIND_REQUIRED)
-ENDIF (csm_FOUND)
-
-
-macro(csm_report_not_found REASON_MSG)
- set(csm_FOUND FALSE)
- unset(csm_INCLUDE_DIR)
- unset(csm_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 (csm_FIND_QUIETLY)
- message(STATUS "Failed to find csm- " ${REASON_MSG} ${ARGN})
- elseif (csm_FIND_REQUIRED)
- message(FATAL_ERROR "Failed to find csm - " ${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 csm - " ${REASON_MSG} ${ARGN})
- endif ()
- return()
-endmacro(csm_report_not_found)
-
-if(NOT csm_FOUND)
- csm_report_not_found("Something went wrong while setting up csm.")
-endif(NOT csm_FOUND)
-# Set the include directories for csm (itself).
-set(csm_FOUND TRUE)
\ No newline at end of file
diff --git a/cmake_modules/wolflaserConfig.cmake b/cmake_modules/wolflaserConfig.cmake
deleted file mode 100644
index 3344e8e033dcd453defca02e0c177f3bc9cce28c..0000000000000000000000000000000000000000
--- a/cmake_modules/wolflaserConfig.cmake
+++ /dev/null
@@ -1,91 +0,0 @@
-#edit the following line to add the librarie's header files
-FIND_PATH(
- wolflaser_INCLUDE_DIRS
- NAMES laser.found
- PATHS /usr/local/include/iri-algorithms/wolf/plugin_laser)
-IF(wolflaser_INCLUDE_DIRS)
- MESSAGE("Found laser include dirs: ${wolflaser_INCLUDE_DIRS}")
-ELSE(wolflaser_INCLUDE_DIRS)
- MESSAGE("Couldn't find laser include dirs")
-ENDIF(wolflaser_INCLUDE_DIRS)
-
-FIND_LIBRARY(
- wolflaser_LIBRARIES
- NAMES libwolflaser.so
- PATHS /usr/local/lib)
-IF(wolflaser_LIBRARIES)
- MESSAGE("Found laser lib: ${wolflaser_LIBRARIES}")
-ELSE(wolflaser_LIBRARIES)
- MESSAGE("Couldn't find wolf laser lib")
-ENDIF(wolflaser_LIBRARIES)
-
-IF (wolflaser_INCLUDE_DIRS AND wolflaser_LIBRARIES)
- SET(wolflaser_FOUND TRUE)
- ELSE(wolflaser_INCLUDE_DIRS AND wolflaser_LIBRARIES)
- set(wolflaser_FOUND FALSE)
-ENDIF (wolflaser_INCLUDE_DIRS AND wolflaser_LIBRARIES)
-
-IF (wolflaser_FOUND)
- IF (NOT wolflaser_FIND_QUIETLY)
- MESSAGE(STATUS "Found laser: ${wolflaser_LIBRARIES}")
- ENDIF (NOT wolflaser_FIND_QUIETLY)
-ELSE (wolflaser_FOUND)
- IF (wolflaser_FIND_REQUIRED)
- MESSAGE(FATAL_ERROR "Could not find wolf laser")
- ENDIF (wolflaser_FIND_REQUIRED)
-ENDIF (wolflaser_FOUND)
-
-
-macro(wolf_report_not_found REASON_MSG)
- set(wolflaser_FOUND FALSE)
- unset(wolflaser_INCLUDE_DIRS)
- unset(wolflaser_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 (wolflaser_FIND_QUIETLY)
- message(STATUS "Failed to find wolf laser- " ${REASON_MSG} ${ARGN})
- elseif(wolflaser_FIND_REQUIRED)
- message(FATAL_ERROR "Failed to find wolf laser - " ${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 laser - " ${REASON_MSG} ${ARGN})
- endif ()
- return()
-endmacro(wolf_report_not_found)
-
-if(NOT wolflaser_FOUND)
- wolf_report_not_found("Something went wrong while setting up wolf vision.")
-endif(NOT wolflaser_FOUND)
-# Set the include directories for wolf (itself).
-set(wolflaser_FOUND TRUE)
-
-# Now we gather all the required dependencies for Wolf Laser
-
-FIND_PACKAGE(laser_scan_utils REQUIRED)
-list(APPEND wolflaser_INCLUDE_DIRS ${laser_scan_utils_INCLUDE_DIRS})
-list(APPEND wolflaser_LIBRARIES ${laser_scan_utils_LIBRARY})
-
-#Making sure Wolf is looked for
-if(NOT wolf_FOUND)
- FIND_PACKAGE(wolfcore REQUIRED)
-
- #We reverse in order to insert at the start
- list(REVERSE wolflaser_INCLUDE_DIRS)
- list(APPEND wolflaser_INCLUDE_DIRS ${wolfcore_INCLUDE_DIRS})
- list(REVERSE wolflaser_INCLUDE_DIRS)
-
- list(REVERSE wolflaser_LIBRARIES)
- list(APPEND wolflaser_LIBRARIES ${wolfcore_LIBRARIES})
- list(REVERSE wolflaser_LIBRARIES)
-
-endif()
-
-# provide both INCLUDE_DIR and INCLUDE_DIRS
-SET(wolflaser_INCLUDE_DIR ${wolflaser_INCLUDE_DIRS})
-# provide both LIBRARY and LIBRARIES
-SET(wolflaser_LIBRARY ${wolflaser_LIBRARIES})
\ No newline at end of file
diff --git a/cmake_modules/wolflaserConfig.cmake.in b/cmake_modules/wolflaserConfig.cmake.in
new file mode 100644
index 0000000000000000000000000000000000000000..b079ac9ac39e5aa16a16f30982abb9c13047e3c0
--- /dev/null
+++ b/cmake_modules/wolflaserConfig.cmake.in
@@ -0,0 +1,13 @@
+set(@PLUGIN_NAME@_VERSION 0.0.1)
+
+
+@PACKAGE_INIT@
+
+include(CMakeFindDependencyMacro)
+FIND_DEPENDENCY(wolfcore REQUIRED)
+FIND_DEPENDENCY(laser_scan_utils REQUIRED)
+FIND_DEPENDENCY(Eigen3 3.3 REQUIRED)
+
+include("${CMAKE_CURRENT_LIST_DIR}/@PLUGIN_NAME@Targets.cmake")
+
+check_required_components(@PLUGIN_NAME@)
\ No newline at end of file
diff --git a/include/laser/capture/capture_laser_2d.h b/include/laser/capture/capture_laser_2d.h
index 579f4475b3e2bd3a59fd41c4fb3fb9dc24db883a..fca364b7234505008a311b93ebbe60007ed4c4a6 100644
--- a/include/laser/capture/capture_laser_2d.h
+++ b/include/laser/capture/capture_laser_2d.h
@@ -47,6 +47,7 @@ class CaptureLaser2d : public CaptureBase
CaptureLaser2d(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const std::vector<float>& _ranges);
~CaptureLaser2d() override = default;
+ const laserscanutils::LaserScan& getScan() const;
laserscanutils::LaserScan& getScan();
void setSensor(const SensorBasePtr sensor_ptr) override;
@@ -57,6 +58,11 @@ class CaptureLaser2d : public CaptureBase
};
+inline const laserscanutils::LaserScan& CaptureLaser2d::getScan() const
+{
+ return scan_;
+}
+
inline laserscanutils::LaserScan& CaptureLaser2d::getScan()
{
return scan_;
diff --git a/include/laser/factor/factor_point_2d.h b/include/laser/factor/factor_point_2d.h
index 30b4884d08fa0874c97722f47f0804b3b0e3b3c4..15d6ee7185659a0e5068388748140d09993ce02b 100644
--- a/include/laser/factor/factor_point_2d.h
+++ b/include/laser/factor/factor_point_2d.h
@@ -88,6 +88,10 @@ class FactorPoint2d: public FactorAutodiff<FactorPoint2d, 2,2,1,2,1,2>
* @brief getLandmarkPtr
* @return
*/
+ LandmarkPolyline2dConstPtr getLandmark() const
+ {
+ return std::static_pointer_cast<const LandmarkPolyline2d>(getLandmarkOther());
+ }
LandmarkPolyline2dPtr getLandmark()
{
return std::static_pointer_cast<LandmarkPolyline2d>(getLandmarkOther());
@@ -97,7 +101,7 @@ class FactorPoint2d: public FactorAutodiff<FactorPoint2d, 2,2,1,2,1,2>
* @brief getLandmarkPointId
* @return
*/
- int getLandmarkPointId()
+ int getLandmarkPointId() const
{
return landmark_point_id_;
}
@@ -106,7 +110,7 @@ class FactorPoint2d: public FactorAutodiff<FactorPoint2d, 2,2,1,2,1,2>
* @brief getFeaturePointId
* @return
*/
- unsigned int getFeaturePointId()
+ unsigned int getFeaturePointId() const
{
return feature_point_id_;
}
@@ -115,6 +119,10 @@ class FactorPoint2d: public FactorAutodiff<FactorPoint2d, 2,2,1,2,1,2>
* @brief getLandmarkPointPtr
* @return
*/
+ StateBlockConstPtr getLandmarkPoint() const
+ {
+ return point_state_ptr_;
+ }
StateBlockPtr getLandmarkPoint()
{
return point_state_ptr_;
diff --git a/include/laser/factor/factor_point_to_line_2d.h b/include/laser/factor/factor_point_to_line_2d.h
index cc975d802a098f3dc610bb5b185e5184e489c69c..f2035e32e3f84b8ff44c1bae60b95ecb6636f4e4 100644
--- a/include/laser/factor/factor_point_to_line_2d.h
+++ b/include/laser/factor/factor_point_to_line_2d.h
@@ -36,8 +36,8 @@ WOLF_PTR_TYPEDEFS(FactorPointToLine2d);
class FactorPointToLine2d: public FactorAutodiff<FactorPointToLine2d, 1,2,1,2,1,2,2>
{
protected:
- int landmark_point_id_;
- int landmark_point_aux_id_;
+ int landmark_point_id_;
+ int landmark_point_aux_id_;
unsigned int feature_point_id_;
StateBlockPtr point_state_ptr_;
StateBlockPtr point_aux_state_ptr_;
@@ -91,31 +91,45 @@ class FactorPointToLine2d: public FactorAutodiff<FactorPointToLine2d, 1,2,1,2,1,
~FactorPointToLine2d() override = default;
+ LandmarkPolyline2dConstPtr getLandmark() const
+ {
+ return std::static_pointer_cast<const LandmarkPolyline2d>( getLandmarkOther() );
+ }
LandmarkPolyline2dPtr getLandmark()
{
return std::static_pointer_cast<LandmarkPolyline2d>( getLandmarkOther() );
}
- int getLandmarkPointId()
+ int getLandmarkPointId() const
{
return landmark_point_id_;
}
- int getLandmarkPointAuxId()
+ int getLandmarkPointAuxId() const
{
return landmark_point_aux_id_;
}
- unsigned int getFeaturePointId()
+ unsigned int getFeaturePointId() const
{
return feature_point_id_;
}
+ StateBlockConstPtr getLandmarkPoint() const
+ {
+ return point_state_ptr_;
+ }
+
StateBlockPtr getLandmarkPoint()
{
return point_state_ptr_;
}
+ StateBlockConstPtr getLandmarkPointAux() const
+ {
+ return point_state_ptr_;
+ }
+
StateBlockPtr getLandmarkPointAux()
{
return point_state_ptr_;
diff --git a/include/laser/feature/feature_polyline_2d.h b/include/laser/feature/feature_polyline_2d.h
index b889c49b827b3f76f197b9f4fa7f801285cecf82..c5cd36350ff9757a9dcbc058ae65d2292e8ada64 100644
--- a/include/laser/feature/feature_polyline_2d.h
+++ b/include/laser/feature/feature_polyline_2d.h
@@ -55,13 +55,8 @@ class FeaturePolyline2d : public FeatureBase
int getNPoints() const;
};
-//<<<<<<< HEAD
inline FeaturePolyline2d::FeaturePolyline2d(const Eigen::MatrixXd& _points, const Eigen::MatrixXd& _points_cov, const bool& _first_defined, const bool& _last_defined) :
FeatureBase("FeaturePolyline2d", Eigen::Vector1d::Zero(), Eigen::Vector1d::Ones()),
-//=======
-//inline FeaturePolyline2d::FeaturePolyline2d(const Eigen::MatrixXd& _points, const Eigen::MatrixXd& _points_cov, const bool& _first_defined, const bool& _last_defined) :
-// FeatureBase("POLYLINE 2d", Eigen::Vector1d::Zero(), Eigen::Vector1d::Ones()),
-//>>>>>>> 8-replace-scalar-to-double
points_(_points),
points_cov_(_points_cov),
first_defined_(_first_defined),
diff --git a/include/laser/feature/feature_scene_falko.h b/include/laser/feature/feature_scene_falko.h
index c571db55da8177dadbd4e63818317ce2271a76d3..1d584c999059ad60385ef51476dd6ae1d8cdde17 100644
--- a/include/laser/feature/feature_scene_falko.h
+++ b/include/laser/feature/feature_scene_falko.h
@@ -43,34 +43,17 @@ template <typename D> class FeatureSceneFalko : public FeatureBase
~FeatureSceneFalko() override{};
std::shared_ptr<laserscanutils::SceneFalko<D>> getScene() { return scene_; }
+ std::shared_ptr<const laserscanutils::SceneFalko<D>> getScene() const { return scene_; }
private:
std::shared_ptr<laserscanutils::SceneFalko<D>> scene_;
};
-WOLF_PTR_TYPEDEFS(FeatureSceneFalkoBsc);
+typedef FeatureSceneFalko<laserscanutils::bsc> FeatureSceneFalkoBsc;
+WOLF_DECLARED_PTR_TYPEDEFS(FeatureSceneFalkoBsc);
-class FeatureSceneFalkoBsc : public FeatureSceneFalko<laserscanutils::bsc>
-{
- public:
- FeatureSceneFalkoBsc(const std::shared_ptr<laserscanutils::SceneFalko<laserscanutils::bsc>> _falko_scene)
- : FeatureSceneFalko(_falko_scene)
- {
- }
- ~FeatureSceneFalkoBsc();
-};
-
-WOLF_PTR_TYPEDEFS(FeatureSceneFalkoCgh);
-
-class FeatureSceneFalkoCgh : public FeatureSceneFalko<laserscanutils::cgh>
-{
- public:
- FeatureSceneFalkoCgh(const std::shared_ptr<laserscanutils::SceneFalko<laserscanutils::cgh>> _falko_scene)
- : FeatureSceneFalko(_falko_scene)
- {
- }
- ~FeatureSceneFalkoCgh() override{};
-};
+typedef FeatureSceneFalko<laserscanutils::cgh> FeatureSceneFalkoCgh;
+WOLF_DECLARED_PTR_TYPEDEFS(FeatureSceneFalkoCgh);
} // namespace wolf
diff --git a/include/laser/landmark/landmark_polyline_2d.h b/include/laser/landmark/landmark_polyline_2d.h
index 4d1574cf3bb765c674435ba65cd2afb1c632b584..4d930a8edcc460bf117d9f4fd38fdde6c1bb4da0 100644
--- a/include/laser/landmark/landmark_polyline_2d.h
+++ b/include/laser/landmark/landmark_polyline_2d.h
@@ -39,6 +39,12 @@
namespace wolf
{
+static std::vector<char> available_chars({'0','1','2','3','4','5','6','7','8','9',
+ 'a','b','c','d','e','f','g','h','i','j','k','l',
+ 'm','n','r','s','t','u','v','w','x','y','z',
+ 'A','B','C','D','E','F','G','H','I','J','K','L',
+ 'M','N','R','S','T','U','V','W','X','Y','Z'});
+
//landmark classification
typedef enum
{
@@ -92,7 +98,7 @@ WOLF_LIST_TYPEDEFS(LandmarkPolyline2d);
class LandmarkPolyline2d : public LandmarkBase
{
protected:
- std::map<int,StateBlockPtr> point_state_ptr_map_; ///< polyline points state blocks
+ std::map<int,StateBlockPtr> point_state_map_; ///< polyline points state blocks
bool first_defined_; ///< Wether the first point is an extreme of a line or the line may continue
bool last_defined_; ///< Wether the last point is an extreme of a line or the line may continue
bool closed_; ///< Wether the polyline is closed or not
@@ -101,19 +107,29 @@ class LandmarkPolyline2d : public LandmarkBase
LandmarkPolyline2dPtr merged_in_lmk_;
private:
-
- StateBlockPtr& firstStateBlock();
- StateBlockPtr& lastStateBlock();
+ char idToChar(int id) const;
+ StateBlockPtr firstStateBlock();
+ StateBlockPtr lastStateBlock();
public:
- LandmarkPolyline2d(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const Eigen::MatrixXd& _points, const bool _first_defined, const bool _last_defined, unsigned int _first_id = 0);
- LandmarkPolyline2d(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const Eigen::MatrixXd& _points, const bool _first_defined, const bool _last_defined, unsigned int _first_id, PolylineRectangularClass _class);
+ LandmarkPolyline2d(StateBlockPtr _p_ptr,
+ StateBlockPtr _o_ptr,
+ const Eigen::MatrixXd& _points,
+ const bool _first_defined,
+ const bool _last_defined,
+ unsigned int _first_id = 0);
+ LandmarkPolyline2d(StateBlockPtr _p_ptr,
+ StateBlockPtr _o_ptr,
+ const Eigen::MatrixXd& _points,
+ const bool _first_defined,
+ const bool _last_defined,
+ unsigned int _first_id,
+ PolylineRectangularClass _class);
~LandmarkPolyline2d() override;
/** \brief Gets a const reference to the point state block pointer vector
**/
- std::map<int,StateBlockPtr>& getPointStatePtrMap();
-
+ std::map<int,StateBlockPtr>& getPointStateMap();
/** \brief Gets wether the first/last points are defined or not
**/
@@ -126,7 +142,8 @@ class LandmarkPolyline2d : public LandmarkBase
/** \brief Get the landmark pointer in which was merged (nullptr in case of no merged)
**/
- LandmarkPolyline2dPtr getMergedInLandmark() const;
+ LandmarkPolyline2dConstPtr getMergedInLandmark() const;
+ LandmarkPolyline2dPtr getMergedInLandmark();
/** \brief Set the landmark pointer in which was merged
**/
@@ -134,7 +151,7 @@ class LandmarkPolyline2d : public LandmarkBase
/** \brief Gets whether the given state block point is defined or not (assumes the state block is in the landmark)
**/
- bool isDefined(StateBlockPtr _state_block) const;
+ bool isDefined(StateBlockConstPtr _state_block) const;
/** \brief Sets the first/last extreme point
**/
@@ -152,18 +169,15 @@ class LandmarkPolyline2d : public LandmarkBase
int getPrevValidId(const int& i) const;
int getVersion() const;
- const Eigen::VectorXd getPointVector(int _i) const;
+ Eigen::VectorXd getPointVector(int _i) const;
Eigen::MatrixXd computePointsGlobal() const;
Eigen::MatrixXd computePointsGlobal(const int& _from_id, const int& _to_id) const;
Eigen::MatrixXd computePointsCovGlobal() const;
+ StateBlockConstPtr getPointStateBlock(int _i) const;
StateBlockPtr getPointStateBlock(int _i);
- /** \brief Gets a vector of all state blocks pointers
- **/
- virtual std::vector<StateBlockPtr> getPointsStateBlockVector() const;
-
/** \brief Adds a new point to the landmark
* \param _point: the point to be added
* \param _extreme: if its extreme or not
@@ -219,11 +233,6 @@ class LandmarkPolyline2d : public LandmarkBase
**/
PolylineRectangularClass getClassification() const;
- /** \brief Adds all stateBlocks of the frame to the wolfProblem list of new stateBlocks
- **/
- virtual void registerNewStateBlocks() const;
- virtual void removeStateBlocks();
-
/** Factory method to create new landmarks from YAML nodes
*/
static LandmarkBasePtr create(const YAML::Node& _lmk_node);
@@ -240,19 +249,19 @@ class LandmarkPolyline2d : public LandmarkBase
// STL
#include <iterator>
-inline StateBlockPtr& LandmarkPolyline2d::firstStateBlock()
+inline StateBlockPtr LandmarkPolyline2d::firstStateBlock()
{
- return point_state_ptr_map_.begin()->second;
+ return point_state_map_.begin()->second;
}
-inline StateBlockPtr& LandmarkPolyline2d::lastStateBlock()
+inline StateBlockPtr LandmarkPolyline2d::lastStateBlock()
{
- return point_state_ptr_map_.rbegin()->second;
+ return point_state_map_.rbegin()->second;
}
-inline std::map<int,StateBlockPtr>& LandmarkPolyline2d::getPointStatePtrMap()
+inline std::map<int,StateBlockPtr>& LandmarkPolyline2d::getPointStateMap()
{
- return point_state_ptr_map_;
+ return point_state_map_;
}
inline bool LandmarkPolyline2d::isFirstDefined() const
@@ -267,7 +276,7 @@ inline bool LandmarkPolyline2d::isLastDefined() const
inline bool LandmarkPolyline2d::isValidId(const int& i) const
{
- return point_state_ptr_map_.count(i) != 0;
+ return point_state_map_.count(i) != 0;
};
inline bool LandmarkPolyline2d::isClosed() const
@@ -275,7 +284,15 @@ inline bool LandmarkPolyline2d::isClosed() const
return closed_;
}
-inline LandmarkPolyline2dPtr LandmarkPolyline2d::getMergedInLandmark() const
+inline LandmarkPolyline2dConstPtr LandmarkPolyline2d::getMergedInLandmark() const
+{
+ // recursive call
+ if (merged_in_lmk_ != nullptr && merged_in_lmk_->getMergedInLandmark() != nullptr)
+ return merged_in_lmk_->getMergedInLandmark();
+ return merged_in_lmk_;
+}
+
+inline LandmarkPolyline2dPtr LandmarkPolyline2d::getMergedInLandmark()
{
// recursive call
if (merged_in_lmk_ != nullptr && merged_in_lmk_->getMergedInLandmark() != nullptr)
@@ -283,12 +300,12 @@ inline LandmarkPolyline2dPtr LandmarkPolyline2d::getMergedInLandmark() const
return merged_in_lmk_;
}
-inline bool LandmarkPolyline2d::isDefined(StateBlockPtr _state_block) const
+inline bool LandmarkPolyline2d::isDefined(StateBlockConstPtr _state_block) const
{
- if (_state_block == point_state_ptr_map_.begin()->second)
+ if (_state_block == point_state_map_.begin()->second)
return first_defined_;
- if (_state_block == point_state_ptr_map_.rbegin()->second)
+ if (_state_block == point_state_map_.rbegin()->second)
return last_defined_;
return true;
@@ -296,18 +313,18 @@ inline bool LandmarkPolyline2d::isDefined(StateBlockPtr _state_block) const
inline int LandmarkPolyline2d::getNPoints() const
{
- return (int)point_state_ptr_map_.size();
+ return (int)point_state_map_.size();
}
inline int LandmarkPolyline2d::getNDefinedPoints() const
{
- return (int)point_state_ptr_map_.size() - (first_defined_ ? 0 : 1) - (last_defined_ ? 0 : 1);
+ return (int)point_state_map_.size() - (first_defined_ ? 0 : 1) - (last_defined_ ? 0 : 1);
}
inline std::vector<int> LandmarkPolyline2d::getValidIds() const
{
std::vector<int> valid_ids;
- for (auto st_pair : point_state_ptr_map_)
+ for (auto st_pair : point_state_map_)
valid_ids.push_back(st_pair.first);
return valid_ids;
@@ -316,17 +333,17 @@ inline std::vector<int> LandmarkPolyline2d::getValidIds() const
inline int LandmarkPolyline2d::id2idx(int id) const
{
assert(isValidId(id));
- return std::distance(point_state_ptr_map_.begin(),point_state_ptr_map_.find(id));
+ return std::distance(point_state_map_.begin(),point_state_map_.find(id));
}
inline int LandmarkPolyline2d::getFirstId() const
{
- return point_state_ptr_map_.begin()->first;
+ return point_state_map_.begin()->first;
}
inline int LandmarkPolyline2d::getLastId() const
{
- return point_state_ptr_map_.rbegin()->first;
+ return point_state_map_.rbegin()->first;
}
@@ -337,7 +354,7 @@ inline int LandmarkPolyline2d::getNextValidId(const int& i) const
if (i == getLastId() && closed_)
return getFirstId();
- return std::next(point_state_ptr_map_.find(i))->first;
+ return std::next(point_state_map_.find(i))->first;
}
inline int LandmarkPolyline2d::getPrevValidId(const int& i) const
@@ -347,7 +364,7 @@ inline int LandmarkPolyline2d::getPrevValidId(const int& i) const
if (i == getFirstId() && closed_)
return getLastId();
- return std::prev(point_state_ptr_map_.find(i))->first;
+ return std::prev(point_state_map_.find(i))->first;
}
inline int LandmarkPolyline2d::getVersion() const
@@ -355,15 +372,6 @@ inline int LandmarkPolyline2d::getVersion() const
return version_;
}
-inline std::vector<StateBlockPtr> LandmarkPolyline2d::getPointsStateBlockVector() const
-{
- std::vector<StateBlockPtr> state_block_vec;
- for (auto st_pair : point_state_ptr_map_)
- state_block_vec.push_back(st_pair.second);
-
- return state_block_vec;
-}
-
inline void LandmarkPolyline2d::classify(PolylineRectangularClass _class)
{
classification_ = _class;
diff --git a/include/laser/processor/processor_loop_closure_falko.h b/include/laser/processor/processor_loop_closure_falko.h
index 755b702fa89231af99f6b43330a6393e40bb1e7a..cf9bfb58bfa78df8ecb470c0a71972f0170ee2c5 100644
--- a/include/laser/processor/processor_loop_closure_falko.h
+++ b/include/laser/processor/processor_loop_closure_falko.h
@@ -51,7 +51,7 @@ WOLF_STRUCT_PTR_TYPEDEFS(MatchLoopClosureFalko);
*/
struct MatchLoopClosureFalko : public MatchLoopClosure
{
- laserscanutils::MatchLoopClosureScenePtr match_falko_;
+ laserscanutils::MatchLoopClosureScenePtr match_falko_;
};
WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorLoopClosureFalko);
@@ -255,8 +255,6 @@ class ProcessorLoopClosureFalko : public ProcessorLoopClosure
~ProcessorLoopClosureFalko() { outdata.close(); };
- std::list<MatchLoopClosurePtr> match_list_;
-
protected:
/** \brief Function not implemented
**/
@@ -539,8 +537,10 @@ class ProcessorLoopClosureFalko : public ProcessorLoopClosure
return match_lc_map_robust;
}
- void printOutData(std::list<laserscanutils::sceneBasePtr> &list_scene_reference, int num_ref_scenes,
- std::map<double, MatchLoopClosurePtr> &match_lc_map, std::vector<falkolib::FALKO> &keypoints_list,
+ void printOutData(const std::list<laserscanutils::sceneBasePtr> &list_scene_reference,
+ int num_ref_scenes,
+ const std::map<double, MatchLoopClosurePtr> &match_lc_map,
+ const std::vector<falkolib::FALKO> &keypoints_list,
int acum_score)
{
if (init_outData_ == 1)
@@ -596,15 +596,27 @@ class ProcessorLoopClosureFalko : public ProcessorLoopClosure
std::chrono::duration<long, std::micro> duration_validation_nok;
std::chrono::duration<long, std::micro> duration_extract_scene;
+ std::list<MatchLoopClosurePtr> match_list_;
+ int num_ok_;
+ int num_nok_;
+ int init_outData_;
+
public:
- unsigned int getNStoredFrames() { return buffer_frame_.getContainer().size(); }
- unsigned int getNStoredCaptures() { return buffer_capture_.getContainer().size(); }
- int num_ok_;
- int num_nok_;
- int init_outData_;
+ SizeStd getNStoredFrames() const { return buffer_frame_.size(); }
+ SizeStd getNStoredCaptures() const { return buffer_capture_.size(); }
+ int getNumOk() const {return num_ok_;}
+ int getNumNOk() const {return num_nok_;}
+ int getInitOutData() const {return init_outData_;}
+ std::list<MatchLoopClosureConstPtr> getMatchList() const
+ {
+ std::list<MatchLoopClosureConstPtr> const_list;
+ for (auto el : match_list_)
+ const_list.push_back(el);
+ return const_list;
+ }
};
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorLoopClosureFalkoNnBsc);
+WOLF_PTR_TYPEDEFS(ProcessorLoopClosureFalkoNnBsc);
/** \brief A class that implements the loop closure with BSC descriptors and NN matcher
**/
@@ -621,7 +633,7 @@ class ProcessorLoopClosureFalkoNnBsc
~ProcessorLoopClosureFalkoNnBsc() = default;
};
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorLoopClosureFalkoAhtBsc);
+WOLF_PTR_TYPEDEFS(ProcessorLoopClosureFalkoAhtBsc);
/** \brief A class that implements the loop closure with BSC descriptors and AHT matcher
**/
@@ -638,7 +650,7 @@ class ProcessorLoopClosureFalkoAhtBsc : public ProcessorLoopClosureFalko<lasersc
~ProcessorLoopClosureFalkoAhtBsc() = default;
};
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorLoopClosureFalkoNnCgh);
+WOLF_PTR_TYPEDEFS(ProcessorLoopClosureFalkoNnCgh);
/** \brief A class that implements the loop closure with CGH descriptors and NN matcher
**/
@@ -655,7 +667,7 @@ class ProcessorLoopClosureFalkoNnCgh : public ProcessorLoopClosureFalko<lasersca
~ProcessorLoopClosureFalkoNnCgh() = default;
};
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorLoopClosureFalkoAhtCgh);
+WOLF_PTR_TYPEDEFS(ProcessorLoopClosureFalkoAhtCgh);
/** \brief A class that implements the loop closure with CGH descriptors and AHT matcher
**/
diff --git a/include/laser/processor/processor_loop_closure_icp.h b/include/laser/processor/processor_loop_closure_icp.h
index 5741d1110f42beca9cf2c473917bde618d164119..f2324d07117dfc842aefd4c02befd2c9f7f47cbc 100644
--- a/include/laser/processor/processor_loop_closure_icp.h
+++ b/include/laser/processor/processor_loop_closure_icp.h
@@ -138,7 +138,7 @@ class ProcessorLoopClosureIcp : public ProcessorLoopClosure
*/
std::map<double,MatchLoopClosurePtr> findLoopClosures(CaptureBasePtr _capture) override;
- MatchLoopClosurePtr matchScans(CaptureLaser2dPtr cap_ref, CaptureLaser2dPtr cap_target) const;
+ MatchLoopClosurePtr matchScans(CaptureLaser2dPtr cap_ref, CaptureLaser2dPtr cap_target);
/** \brief validates a loop closure
*/
@@ -150,7 +150,7 @@ class ProcessorLoopClosureIcp : public ProcessorLoopClosure
/** \brief returns a list containing the loop closure frame candidates list
*/
- FrameBasePtrList generateSearchList(CaptureBasePtr) const;
+ FrameBasePtrList generateSearchList(CaptureBasePtr);
};
}
diff --git a/internal/config.h.in b/internal/config.h.in
index 7014ca320e5c5fea1f59a6fe5d9d1ea0987bf95e..35f3468331656ba09a63ad7f26e5c0fb78c8d5bc 100644
--- a/internal/config.h.in
+++ b/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_ */
diff --git a/src/landmark/landmark_polyline_2d.cpp b/src/landmark/landmark_polyline_2d.cpp
index bdd6566a85dcf025d6053ce62cfc7b47ab2e98a4..7be128506e5bd3451cb5399bf68b3e9d0c22139e 100644
--- a/src/landmark/landmark_polyline_2d.cpp
+++ b/src/landmark/landmark_polyline_2d.cpp
@@ -44,31 +44,25 @@ LandmarkPolyline2d::LandmarkPolyline2d(StateBlockPtr _p_ptr, StateBlockPtr _o_pt
{
}
-//<<<<<<< HEAD
LandmarkPolyline2d::LandmarkPolyline2d(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const Eigen::MatrixXd& _points, const bool _first_defined, const bool _last_defined, unsigned int _first_id, PolylineRectangularClass _class) :
LandmarkBase("LandmarkPolyline2d", _p_ptr, _o_ptr), first_defined_(_first_defined), last_defined_(_last_defined), closed_(false), classification_(_class), version_(0), merged_in_lmk_(nullptr)
-//=======
-//LandmarkPolyline2d::LandmarkPolyline2d(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const Eigen::MatrixXd& _points, const bool _first_defined, const bool _last_defined, unsigned int _first_id, PolylineRectangularClass _class) :
-// LandmarkBase("POLYLINE 2d", _p_ptr, _o_ptr), first_defined_(_first_defined), last_defined_(_last_defined), closed_(false), classification_(_class), version_(0), merged_in_lmk_(nullptr)
-//>>>>>>> 8-replace-scalar-to-double
{
//std::cout << "LandmarkPolyline2d::LandmarkPolyline2d" << std::endl;
assert(_points.cols() >= 2 && "2 points at least needed.");
for (auto i = 0; i < _points.cols(); i++)
- point_state_ptr_map_[i+_first_id] = std::make_shared<StateBlock>(_points.col(i).head<2>());
+ point_state_map_[i+_first_id] = std::make_shared<StateBlock>(_points.col(i).head<2>());
if (!first_defined_)
- firstStateBlock()->setLocalParametrization(std::make_shared<LocalParametrizationPolylineExtreme>(std::next(point_state_ptr_map_.begin())->second));
+ firstStateBlock()->setLocalParametrization(std::make_shared<LocalParametrizationPolylineExtreme>(std::next(point_state_map_.begin())->second));
if (!last_defined_)
- lastStateBlock()->setLocalParametrization(std::make_shared<LocalParametrizationPolylineExtreme>(std::next(point_state_ptr_map_.rbegin())->second));
+ lastStateBlock()->setLocalParametrization(std::make_shared<LocalParametrizationPolylineExtreme>(std::next(point_state_map_.rbegin())->second));
//std::cout << "LandmarkPolyline2d " << id() << " created. First: " << getFirstId() << " last: "<< getLastId() << std::endl;
}
LandmarkPolyline2d::~LandmarkPolyline2d()
{
- removeStateBlocks();
}
void LandmarkPolyline2d::setFirst(const Eigen::VectorXd& _point, bool _defined)
@@ -91,16 +85,16 @@ void LandmarkPolyline2d::setLast(const Eigen::VectorXd& _point, bool _defined)
defineLast();
}
-const Eigen::VectorXd LandmarkPolyline2d::getPointVector(int _i) const
+Eigen::VectorXd LandmarkPolyline2d::getPointVector(int _i) const
{
- assert(point_state_ptr_map_.find(_i) != point_state_ptr_map_.end());
- return point_state_ptr_map_.at(_i)->getState();
+ assert(point_state_map_.find(_i) != point_state_map_.end());
+ return point_state_map_.at(_i)->getState();
}
StateBlockPtr LandmarkPolyline2d::getPointStateBlock(int _i)
{
- assert(point_state_ptr_map_.find(_i) != point_state_ptr_map_.end());
- return point_state_ptr_map_.at(_i);
+ assert(point_state_map_.find(_i) != point_state_map_.end());
+ return point_state_map_.at(_i);
}
Eigen::MatrixXd LandmarkPolyline2d::computePointsGlobal() const
@@ -155,8 +149,6 @@ Eigen::MatrixXd LandmarkPolyline2d::computePointsGlobal(const int& _from_id, con
return points_global;
}
-
-
Eigen::MatrixXd LandmarkPolyline2d::computePointsCovGlobal() const
{
Eigen::MatrixXd points_cov_global = Eigen::MatrixXd::Zero(2,2*getNPoints());
@@ -173,6 +165,17 @@ Eigen::MatrixXd LandmarkPolyline2d::computePointsCovGlobal() const
return points_cov_global;
}
+char LandmarkPolyline2d::idToChar(int id) const
+{
+ if (id < 0)
+ id += available_chars.size();
+
+ if (id < 0 or id >= available_chars.size())
+ throw std::runtime_error("Too many points in LandmarkPolyline2d");
+
+ return available_chars[id];
+}
+
void LandmarkPolyline2d::addPoint(const Eigen::VectorXd& _point, const bool& _defined, const bool& _back)
{
assert(!closed_ && "adding point to a closed polyline!");
@@ -192,10 +195,9 @@ void LandmarkPolyline2d::addPoint(const Eigen::VectorXd& _point, const bool& _de
(!_defined ?
std::make_shared<LocalParametrizationPolylineExtreme>(lastStateBlock()) :
nullptr));
- point_state_ptr_map_[getLastId()+1]= new_sb_ptr;
-
- if (getProblem())
- getProblem()->notifyStateBlock(new_sb_ptr,ADD);
+ auto point_id = getLastId()+1;
+ point_state_map_[point_id]= new_sb_ptr;
+ HasStateBlocks::addStateBlock(idToChar(point_id),new_sb_ptr,getProblem());
last_defined_ = _defined;
}
@@ -206,10 +208,9 @@ void LandmarkPolyline2d::addPoint(const Eigen::VectorXd& _point, const bool& _de
(!_defined ?
std::make_shared<LocalParametrizationPolylineExtreme>(firstStateBlock()) :
nullptr));
- point_state_ptr_map_[getFirstId()-1] = new_sb_ptr;
-
- if (getProblem())
- getProblem()->notifyStateBlock(new_sb_ptr,ADD);
+ auto point_id = getLastId()-1;
+ point_state_map_[point_id] = new_sb_ptr;
+ HasStateBlocks::addStateBlock(idToChar(point_id),new_sb_ptr,getProblem());
first_defined_ = _defined;
}
@@ -241,10 +242,9 @@ void LandmarkPolyline2d::addPoints(const Eigen::MatrixXd& _points, const unsigne
(i == _points.cols()-1 && !_defined ?
std::make_shared<LocalParametrizationPolylineExtreme>(lastStateBlock()) :
nullptr));
- point_state_ptr_map_[getLastId()+1] = new_sb_ptr;
-
- if (getProblem())
- getProblem()->notifyStateBlock(new_sb_ptr,ADD);
+ auto point_id = getLastId()+1;
+ point_state_map_[point_id]= new_sb_ptr;
+ HasStateBlocks::addStateBlock(idToChar(point_id),new_sb_ptr,getProblem());
}
last_defined_ = _defined;
}
@@ -257,10 +257,9 @@ void LandmarkPolyline2d::addPoints(const Eigen::MatrixXd& _points, const unsigne
(i == 0 && !_defined ?
std::make_shared<LocalParametrizationPolylineExtreme>(firstStateBlock()) :
nullptr));
- point_state_ptr_map_[getFirstId()-1] = new_sb_ptr;
-
- if (getProblem())
- getProblem()->notifyStateBlock(new_sb_ptr,ADD);
+ auto point_id = getLastId()-1;
+ point_state_map_[point_id] = new_sb_ptr;
+ HasStateBlocks::addStateBlock(idToChar(point_id),new_sb_ptr,getProblem());
}
first_defined_ = _defined;
}
@@ -481,16 +480,13 @@ void LandmarkPolyline2d::setClosed()
void LandmarkPolyline2d::mergePoints(int _remove_id, int _remain_id)
{
- assert(point_state_ptr_map_.find(_remove_id) != point_state_ptr_map_.end() && "removing an inexistent point");
- assert(point_state_ptr_map_.find(_remain_id) != point_state_ptr_map_.end() && "merging an inexistent point");
+ assert(point_state_map_.find(_remove_id) != point_state_map_.end() && "removing an inexistent point");
+ assert(point_state_map_.find(_remain_id) != point_state_map_.end() && "merging an inexistent point");
assert(!(_remain_id == getLastId() && !last_defined_) && "in merging points, the remaining point must be defined");
assert(!(_remain_id == getFirstId() && !first_defined_) && "in merging points, the remaining point must be defined");
//std::cout << "merge points: remove " << _remove_id << " and keep " << _remain_id << " (ids: " << getFirstId() << " to " << getLastId() << ")" << std::endl;
- StateBlockPtr remove_state = getPointStateBlock(_remove_id);
- //std::cout << "state block to remove " << remove_state->getState().transpose() << std::endl;
-
// Change factors from remove_state to remain_state
FactorBasePtrList old_factors_list = getConstrainedByList();
//std::cout << "changing " << old_factors_list.size() << " factors." << std::endl;
@@ -565,12 +561,11 @@ void LandmarkPolyline2d::mergePoints(int _remove_id, int _remain_id)
last_defined_ = true;
// Remove remove_state
- if (getProblem() != nullptr)
- getProblem()->notifyStateBlock(remove_state, REMOVE);
+ HasStateBlocks::removeStateBlock(idToChar(_remove_id));
//std::cout << "state removed " << std::endl;
// remove element from deque
- point_state_ptr_map_.erase(_remove_id);
+ point_state_map_.erase(_remove_id);
//std::cout << "state removed from point vector " << std::endl;
// new version
@@ -774,7 +769,7 @@ bool LandmarkPolyline2d::tryClassify(const double& _dist_tol, std::vector<Polyli
getPointStateBlock(points_ids[3])->setState(Eigen::Vector2d( classification_.L / 2,-classification_.W / 2));
}
- for (auto st_pair : point_state_ptr_map_)
+ for (auto st_pair : point_state_map_)
st_pair.second->fix();
return true;
@@ -933,27 +928,6 @@ void LandmarkPolyline2d::mergeLandmark(const LandmarkPolyline2dPtr _merged_lmk,
//std::cout << "\tLandmark deleted\n";
}
-void LandmarkPolyline2d::registerNewStateBlocks() const
-{
- LandmarkBase::registerNewStateBlocks(getProblem());
- if (getProblem())
- for (auto state_it : point_state_ptr_map_)
- getProblem()->notifyStateBlock(state_it.second, ADD);
-}
-
-void LandmarkPolyline2d::removeStateBlocks()
-{
- auto sbp_pair = point_state_ptr_map_.begin();
- while (sbp_pair != point_state_ptr_map_.end())
- {
- if (getProblem())
- getProblem()->notifyStateBlock(sbp_pair->second, REMOVE);
-
- sbp_pair = point_state_ptr_map_.erase(sbp_pair);
- }
- LandmarkBase::removeStateBlocks(getProblem());
-}
-
// static
LandmarkBasePtr LandmarkPolyline2d::create(const YAML::Node& _lmk_node)
{
@@ -989,7 +963,7 @@ LandmarkBasePtr LandmarkPolyline2d::create(const YAML::Node& _lmk_node)
lmk_ptr->setId(id);
// fix all points
- for (auto st_pair : lmk_ptr->getPointStatePtrMap())
+ for (auto st_pair : lmk_ptr->getPointStateMap())
st_pair.second->fix();
return lmk_ptr;
@@ -1008,8 +982,8 @@ YAML::Node LandmarkPolyline2d::saveToYaml() const
node["classification_L"] = classification_.L;
node["classification_W"] = classification_.W;
- for (auto st : getPointsStateBlockVector())
- node["points"].push_back(st->getState());
+ for (auto st_id : getValidIds())
+ node["points"].push_back(getPointVector(st_id));
return node;
}
diff --git a/src/processor/processor_loop_closure_falko.cpp b/src/processor/processor_loop_closure_falko.cpp
index 92ba29fa13ecaecfb684287e56426c4102ecc467..91259910eba66e2baf445fa5a01a26d7b3ff0db5 100644
--- a/src/processor/processor_loop_closure_falko.cpp
+++ b/src/processor/processor_loop_closure_falko.cpp
@@ -20,6 +20,7 @@
//
//--------LICENSE_END--------
#include "laser/processor/processor_loop_closure_falko.h"
+
// Register in the FactorySensor
#include "core/processor/factory_processor.h"
namespace wolf {
diff --git a/src/processor/processor_loop_closure_icp.cpp b/src/processor/processor_loop_closure_icp.cpp
index a57559753b2cd4cf8503ab1f10a422ba18b52dfc..09d35ccdf3fcc2eb75ced42a97da1d4844b798db 100644
--- a/src/processor/processor_loop_closure_icp.cpp
+++ b/src/processor/processor_loop_closure_icp.cpp
@@ -113,7 +113,7 @@ std::map<double,MatchLoopClosurePtr> ProcessorLoopClosureIcp::findLoopClosures(C
return match_lc_map;
}
-MatchLoopClosurePtr ProcessorLoopClosureIcp::matchScans(CaptureLaser2dPtr cap_ref, CaptureLaser2dPtr cap_tar) const
+MatchLoopClosurePtr ProcessorLoopClosureIcp::matchScans(CaptureLaser2dPtr cap_ref, CaptureLaser2dPtr cap_tar)
{
auto frm_tar = cap_tar->getFrame();
auto frm_ref = cap_ref->getFrame();
@@ -212,18 +212,18 @@ void ProcessorLoopClosureIcp::emplaceFactors(MatchLoopClosurePtr match)
last_loop_closure_ = match;
}
-FrameBasePtrList ProcessorLoopClosureIcp::generateSearchList(CaptureBasePtr _cap) const
+FrameBasePtrList ProcessorLoopClosureIcp::generateSearchList(CaptureBasePtr _cap)
{
assert(_cap->getFrame() and _cap->getFrame()->getTrajectory());
- int N = getProblem()->getTrajectory()->getFrameMap().size() - 1 - params_loop_closure_icp_->recent_frames_ignored;
+ int N = getProblem()->getTrajectory()->size() - 1 - params_loop_closure_icp_->recent_frames_ignored;
WOLF_DEBUG("ProcessorLoopClosureIcp::generateSearchList N = ", N);
if (N <= 0)
return FrameBasePtrList();
- auto map_begin = getProblem()->getTrajectory()->getFrameMap().begin();
+ auto first_frame = getProblem()->getTrajectory()->getFirstFrame();
//std::vector<int> idxs;
std::vector<bool> done(N, false);
@@ -241,10 +241,10 @@ FrameBasePtrList ProcessorLoopClosureIcp::generateSearchList(CaptureBasePtr _cap
// initialize with first: 0
done.at(0) = true;
- if (not map_begin->second->getCapturesOfType<CaptureLaser2d>().empty())
+ if (not first_frame->getCapturesOfType<CaptureLaser2d>().empty())
{
//idxs.push_back(0);
- frame_list.push_back(map_begin->second);
+ frame_list.push_back(first_frame);
}
// continue with tree
@@ -262,7 +262,7 @@ FrameBasePtrList ProcessorLoopClosureIcp::generateSearchList(CaptureBasePtr _cap
{
done.at(idx) = true;
checked++;
- auto frame_idx = std::next(map_begin, idx)->second;
+ auto frame_idx = first_frame->getNextFrame(idx);
assert(frame_idx);
assert(frame_idx != _cap->getFrame());
if (not frame_idx->getCapturesOfType<CaptureLaser2d>().empty())
@@ -297,7 +297,7 @@ FrameBasePtrList ProcessorLoopClosureIcp::generateSearchList(CaptureBasePtr _cap
checked++;
// consider adding
- auto frame_idx = std::next(map_begin, idx)->second;
+ auto frame_idx = first_frame->getNextFrame(idx);
assert(frame_idx);
assert(frame_idx != _cap->getFrame());
if (not frame_idx->getCapturesOfType<CaptureLaser2d>().empty())
diff --git a/src/processor/processor_tracker_feature_polyline_2d.cpp b/src/processor/processor_tracker_feature_polyline_2d.cpp
index be4e0ad864a4d5ec1aafd8d9b71aa6f5bf90f3a8..d5a6c8ce3958b1fada4636b17c136818231ffc30 100644
--- a/src/processor/processor_tracker_feature_polyline_2d.cpp
+++ b/src/processor/processor_tracker_feature_polyline_2d.cpp
@@ -622,7 +622,7 @@ LandmarkBasePtr ProcessorTrackerFeaturePolyline2d::emplaceLandmark(FeatureBasePt
//std::cout << "Robot global pose: " << t_world_robot_.transpose() << std::endl;
//std::cout << "Sensor global pose: " << t_world_sensor_.transpose() << std::endl;
assert(_feature_ptr->getType() == "POLYLINE 2d");
- assert(std::find(last_ptr_->getFeatureList().begin(),last_ptr_->getFeatureList().end(),_feature_ptr) != last_ptr_->getFeatureList().end() && "creating a landmark for a feature which is not in last capture");
+ assert(last_ptr_->hasFeature(_feature_ptr) && "creating a landmark for a feature which is not in last capture");
FeaturePolyline2dPtr polyline_ptr = std::static_pointer_cast<FeaturePolyline2d>(_feature_ptr);
diff --git a/src/yaml/processor_odom_icp_yaml.cpp b/src/yaml/processor_odom_icp_yaml.cpp
index 0d4d68b039b94e7d5b0f7ff879cb6b3826bb158a..5bc41cbcb5bad2e4761f86050c6c4850c0a63ba2 100644
--- a/src/yaml/processor_odom_icp_yaml.cpp
+++ b/src/yaml/processor_odom_icp_yaml.cpp
@@ -29,7 +29,7 @@
// wolf
#include "laser/processor/processor_odom_icp.h"
-#include "core/common/factory.h"
+#include "core/processor/factory_processor.h"
// wolf yaml
#include "core/yaml/yaml_conversion.h"
diff --git a/src/yaml/sensor_laser_2d_yaml.cpp b/src/yaml/sensor_laser_2d_yaml.cpp
index cc1329162cc0f9bb3f5859fab1fe711c99fbac67..eecb788cf782145a7f67e0d7ae2e805a0f71d82a 100644
--- a/src/yaml/sensor_laser_2d_yaml.cpp
+++ b/src/yaml/sensor_laser_2d_yaml.cpp
@@ -31,7 +31,7 @@
// wolf
//#include "core/intrinsics_factory.h"
-#include "core/common/factory.h"
+#include "core/sensor/factory_sensor.h"
#include "laser/sensor/sensor_laser_2d.h"
// yaml library
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index c75a1957be69ff023966f3c9d0c59dd1fd7eb0f6..1d3f41289deb112f091202b31669f289598b7832 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -1,37 +1,26 @@
# 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) #
-target_link_libraries(gtest_example ${PLUGIN_NAME}) #
+# wolf_add_gtest(gtest_example gtest_example.cpp) #
# #
###########################################################
wolf_add_gtest(gtest_landmark_polyline gtest_landmark_polyline.cpp)
-target_link_libraries(gtest_landmark_polyline ${PLUGIN_NAME})
if(csm_FOUND)
wolf_add_gtest(gtest_processor_odom_icp gtest_processor_odom_icp.cpp)
- target_link_libraries(gtest_processor_odom_icp ${PLUGIN_NAME} ${wolf_LIBRARY})
-
wolf_add_gtest(gtest_processor_loop_closure_icp gtest_processor_loop_closure_icp.cpp)
- target_link_libraries(gtest_processor_loop_closure_icp ${PLUGIN_NAME} ${wolf_LIBRARY})
endif(csm_FOUND)
wolf_add_gtest(gtest_polyline_2d gtest_polyline_2d.cpp)
-target_link_libraries(gtest_polyline_2d ${PLUGIN_NAME})
if(falkolib_FOUND)
wolf_add_gtest(gtest_processor_loop_closure_falko gtest_processor_loop_closure_falko.cpp)
- target_link_libraries(gtest_processor_loop_closure_falko ${PLUGIN_NAME} ${wolf_LIBRARY})
if(csm_FOUND)
wolf_add_gtest(gtest_processor_loop_closure_falko_icp gtest_processor_loop_closure_falko_icp.cpp)
- target_link_libraries(gtest_processor_loop_closure_falko_icp ${PLUGIN_NAME} ${wolf_LIBRARY})
endif(csm_FOUND)
endif(falkolib_FOUND)
diff --git a/test/gtest/CMakeLists.txt b/test/gtest/CMakeLists.txt
index 559756b02472653c655c44152c452ba8767ef6f2..6209d341bcd32308bd522ab1990eb0cc82692012 100644
--- a/test/gtest/CMakeLists.txt
+++ b/test/gtest/CMakeLists.txt
@@ -1,65 +1,73 @@
-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
-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
-)
+ # Specify include dir
+ ExternalProject_Get_Property(googletest source_dir)
+ set(GTEST_INCLUDE_DIRS ${source_dir}/googletest/include PARENT_SCOPE)
-# Get GTest source and binary directories from CMake project
+ # Specify MainTest's link libraries
+ ExternalProject_Get_Property(googletest binary_dir)
+ set(GTEST_LIBS_DIR ${binary_dir}/googlemock/gtest PARENT_SCOPE)
-# Specify include dir
-ExternalProject_Get_Property(googletest source_dir)
-set(GTEST_INCLUDE_DIRS ${source_dir}/googletest/include 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)
-# Specify MainTest's link libraries
-ExternalProject_Get_Property(googletest binary_dir)
-set(GTEST_LIBS_DIR ${binary_dir}/googlemock/gtest PARENT_SCOPE)
+ # Set libgtest properties
+ set_target_properties(libgtest PROPERTIES
+ "IMPORTED_LOCATION" "${binary_dir}/googlemock/gtest/libgtest.a"
+ "IMPORTED_LINK_INTERFACE_LIBRARIES" "${CMAKE_THREAD_LIBS_INIT}"
+ )
-# Create a libgtest target to be used as a dependency by test programs
-add_library(libgtest IMPORTED STATIC GLOBAL)
-add_dependencies(libgtest googletest)
+else()
-# Set libgtest properties
-set_target_properties(libgtest PROPERTIES
- "IMPORTED_LOCATION" "${binary_dir}/googlemock/gtest/libgtest.a"
- "IMPORTED_LINK_INTERFACE_LIBRARIES" "${CMAKE_THREAD_LIBS_INIT}"
-)
+ message("CMake version equal or greater than 3.11.0")
+ include(FetchContent)
+
+ FetchContent_Declare(
+ googletest
+ GIT_REPOSITORY https://github.com/google/googletest.git
+ GIT_TAG main)
+
+ SET(INSTALL_GTEST OFF) # Disable installation of googletest
+ FetchContent_MakeAvailable(googletest)
+
+endif()
+
function(wolf_add_gtest target)
add_executable(${target} ${ARGN})
- add_dependencies(${target} libgtest)
- target_link_libraries(${target} libgtest)
-
- #WORKING_DIRECTORY ${CMAKE_BINARY_DIR}/bin
+ if(${CMAKE_VERSION} VERSION_LESS "3.11.0")
+ add_dependencies(${target} libgtest)
+ target_link_libraries(${target} libgtest ${PLUGIN_NAME})
+ target_include_directories(${target} PUBLIC ${GTEST_INCLUDE_DIRS})
+ else()
+ target_link_libraries(${target} gtest_main ${PLUGIN_NAME})
+ endif()
add_test(NAME ${target} COMMAND ${target})
endfunction()
diff --git a/test/gtest_landmark_polyline.cpp b/test/gtest_landmark_polyline.cpp
index d84c18d1ac15c24aac47545cae3513561b823aca..93dd5c8fc27e83ea3685b9ab4bfd2f742c9e234f 100644
--- a/test/gtest_landmark_polyline.cpp
+++ b/test/gtest_landmark_polyline.cpp
@@ -143,7 +143,7 @@ TEST_F(LandmarkPolylineTest, Classify)
// test points fixed and center unfixed
ASSERT_FALSE(lmk_ptr->getP()->isFixed());
ASSERT_FALSE(lmk_ptr->getO()->isFixed());
- for (auto st_pair : lmk_ptr->getPointStatePtrMap())
+ for (auto st_pair : lmk_ptr->getPointStateMap())
ASSERT_TRUE(st_pair.second->isFixed());
// test points are in the same place (compoping with object pose)
diff --git a/test/gtest_processor_loop_closure_falko.cpp b/test/gtest_processor_loop_closure_falko.cpp
index 87d7868972e2be0f9e4c88e5a9ab869a0907408b..13b2338a8737c6c8ed9296a3d32ab40df531d72e 100644
--- a/test/gtest_processor_loop_closure_falko.cpp
+++ b/test/gtest_processor_loop_closure_falko.cpp
@@ -409,17 +409,17 @@ TEST_F(ProcessorLoopClosureFalkoTest, findLoopClosures)
// captureCallback
processor->captureCallback(cap1);
- EXPECT_EQ(processor->match_list_.size(), 0);
+ EXPECT_EQ(processor->getMatchList().size(), 0);
processor->captureCallback(cap2);
- EXPECT_EQ(processor->match_list_.size(), 1);
- EXPECT_EQ(processor->match_list_.back()->capture_reference, cap1);
- EXPECT_EQ(processor->match_list_.back()->capture_target, cap2);
+ EXPECT_EQ(processor->getMatchList().size(), 1);
+ EXPECT_EQ(processor->getMatchList().back()->capture_reference, cap1);
+ EXPECT_EQ(processor->getMatchList().back()->capture_target, cap2);
processor->captureCallback(cap3);
- EXPECT_EQ(processor->match_list_.size(), 3);
- EXPECT_EQ(processor->match_list_.back()->capture_reference, cap1);
- EXPECT_EQ(processor->match_list_.back()->capture_target, cap3);
+ EXPECT_EQ(processor->getMatchList().size(), 3);
+ EXPECT_EQ(processor->getMatchList().back()->capture_reference, cap1);
+ EXPECT_EQ(processor->getMatchList().back()->capture_target, cap3);
// Verify factors
EXPECT_EQ(cap1->getFrame()->getConstrainedByList().size(), 0);
@@ -474,7 +474,7 @@ TEST_F(ProcessorLoopClosureFalkoTest, TestExtractScene)
std::vector<double> x_ref, x_target;
std::vector<double> y_ref, y_target;
- auto match_falko = std::static_pointer_cast<MatchLoopClosureFalko>(processor->match_list_.front());
+ auto match_falko = std::static_pointer_cast<const MatchLoopClosureFalko>(processor->getMatchList().front());
auto associations = match_falko->match_falko_->associations;
@@ -677,7 +677,7 @@ TEST_F(ProcessorLoopClosureFalkoTest, TestProcessCapture)
// std::cout << "reference keypoints size : " << key_ref.size() << std::endl;
// std::cout << "target keypoints size : " << key_target.size() << std::endl;
- // std::cout << "processor->match_list_.size() : " << processor->match_list_.size() << std::endl;
+ // std::cout << "processor->getMatchList().size() : " << processor->getMatchList().size() << std::endl;
std::vector<double> x_ref_all, x_target_all;
std::vector<double> y_ref_all, y_target_all;
@@ -698,7 +698,7 @@ TEST_F(ProcessorLoopClosureFalkoTest, TestProcessCapture)
std::vector<double> x_ref, x_target;
std::vector<double> y_ref, y_target;
- auto match_falko = std::static_pointer_cast<MatchLoopClosureFalko>(processor->match_list_.front());
+ auto match_falko = std::static_pointer_cast<const MatchLoopClosureFalko>(processor->getMatchList().front());
auto associations = match_falko->match_falko_->associations;
for (auto asso : associations)
diff --git a/test/gtest_processor_loop_closure_falko_icp.cpp b/test/gtest_processor_loop_closure_falko_icp.cpp
index a12e6ecd45e89bc7f6e92021e28a5800d14e97b0..a6e4baf9bf24842fe69b34385905677c32a03b61 100644
--- a/test/gtest_processor_loop_closure_falko_icp.cpp
+++ b/test/gtest_processor_loop_closure_falko_icp.cpp
@@ -101,17 +101,17 @@ TEST_F(ProcessorLoopClosureFalkoIcpTest, validatefindLoopClosures)
// captureCallback
processor->captureCallback(cap1);
- EXPECT_EQ(processor->match_list_.size(), 0);
+ EXPECT_EQ(processor->getMatchList().size(), 0);
processor->captureCallback(cap2);
- EXPECT_EQ(processor->match_list_.size(), 1);
- EXPECT_EQ(processor->match_list_.back()->capture_reference, cap1);
- EXPECT_EQ(processor->match_list_.back()->capture_target, cap2);
+ EXPECT_EQ(processor->getMatchList().size(), 1);
+ EXPECT_EQ(processor->getMatchList().back()->capture_reference, cap1);
+ EXPECT_EQ(processor->getMatchList().back()->capture_target, cap2);
processor->captureCallback(cap3);
- EXPECT_EQ(processor->match_list_.size(), 3);
- EXPECT_EQ(processor->match_list_.back()->capture_reference, cap1);
- EXPECT_EQ(processor->match_list_.back()->capture_target, cap3);
+ EXPECT_EQ(processor->getMatchList().size(), 3);
+ EXPECT_EQ(processor->getMatchList().back()->capture_reference, cap1);
+ EXPECT_EQ(processor->getMatchList().back()->capture_target, cap3);
}
int main(int argc, char **argv)
diff --git a/test/gtest_processor_odom_icp.cpp b/test/gtest_processor_odom_icp.cpp
index b0f24d0d3bbedd22b82cda213ec0499925ea2669..93fc7cf2ab8d5fa881e137b5d4c827b0891bb85d 100644
--- a/test/gtest_processor_odom_icp.cpp
+++ b/test/gtest_processor_odom_icp.cpp
@@ -35,6 +35,8 @@
#include "laser/processor/processor_odom_icp.h"
#include "laser/capture/capture_laser_2d.h"
+#include <core/processor/factory_processor.h>
+
using namespace wolf;
using namespace Eigen;
@@ -199,15 +201,15 @@ TEST_F(ProcessorOdomIcp_Test, solve)
t += 1.0;
}
- for (auto F : *problem->getTrajectory())
- F->perturb(0.5);
+ for (auto F_pair : problem->getTrajectory()->getFrameMap())
+ F_pair.second->perturb(0.5);
std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
WOLF_TRACE(report);
- for (auto F : *problem->getTrajectory())
+ for (auto F_pair : problem->getTrajectory()->getFrameMap())
{
- ASSERT_MATRIX_APPROX(F->getState().vector("PO") , x0.vector("PO") , 1e-6);
+ ASSERT_MATRIX_APPROX(F_pair.second->getState().vector("PO") , x0.vector("PO") , 1e-6);
}
}
@@ -242,15 +244,15 @@ TEST_F(ProcessorOdomIcp_Test, solve_3D)
t += 1.0;
}
- for (auto F : *problem->getTrajectory())
- F->perturb(0.5);
+ for (auto F_pair : problem->getTrajectory()->getFrameMap())
+ F_pair.second->perturb(0.5);
- std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
WOLF_TRACE(report);
- for (auto F : *problem->getTrajectory())
+ for (auto F_pair : problem->getTrajectory()->getFrameMap())
{
- ASSERT_MATRIX_APPROX(F->getState().vector("PO") , x0.vector("PO") , 1e-6);
+ ASSERT_MATRIX_APPROX(F_pair.second->getState().vector("PO") , x0.vector("PO") , 1e-6);
}
}