diff --git a/CMakeLists.txt b/CMakeLists.txt
index 2b7248061485afe23b1315e3fc46b4d4e7c170d3..c9c4eb7fa799e3660c7463fc5ac4370cc2e44506 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)
@@ -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)
@@ -81,10 +74,12 @@ option(_WOLF_TRACE "Enable wolf tracing macro" ON)
# ============ DEPENDENCIES ============
FIND_PACKAGE(wolfcore REQUIRED)
FIND_PACKAGE(wolfimu REQUIRED)
+FIND_PACKAGE(Eigen3 3.3 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)
@@ -103,7 +98,6 @@ configure_file(${CMAKE_CURRENT_SOURCE_DIR}/internal/config.h.in "${WOLF_CONFIG_D
include_directories("${PROJECT_BINARY_DIR}/conf")
# ============ INCLUDES ============
-INCLUDE_DIRECTORIES(${wolfimu_INCLUDE_DIRS})
INCLUDE_DIRECTORIES(BEFORE "include")
# ============ HEADERS ============
@@ -185,7 +179,9 @@ endif()
#Link the created libraries
#===============EXAMPLE=========================
-TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${wolfimu_LIBRARIES})
+TARGET_LINK_LIBRARIES(${PLUGIN_NAME} PUBLIC wolfcore)
+TARGET_LINK_LIBRARIES(${PLUGIN_NAME} PUBLIC wolfimu)
+TARGET_LINK_LIBRARIES(${PLUGIN_NAME} PUBLIC Eigen3::Eigen)
# Build demos
IF(BUILD_DEMOS)
@@ -199,14 +195,38 @@ 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)
+ 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)
@@ -221,14 +241,9 @@ INSTALL(FILES ${HDRS_PROCESSOR}
INSTALL(FILES ${HDRS_SENSOR}
DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/sensor)
-FILE(WRITE ${PROJECT_NAME}.found "")
-INSTALL(FILES ${PROJECT_NAME}.found
- DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME})
INSTALL(FILES "${WOLF_CONFIG_DIR}/config.h"
DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/internal)
-INSTALL(FILES "${CMAKE_SOURCE_DIR}/cmake_modules/${PLUGIN_NAME}Config.cmake" DESTINATION "lib/cmake/${PLUGIN_NAME}")
-
export(PACKAGE ${PLUGIN_NAME})
FIND_PACKAGE(Doxygen)
diff --git a/bodydynamics.found b/bodydynamics.found
deleted file mode 100644
index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..0000000000000000000000000000000000000000
diff --git a/cmake_modules/wolfbodydynamicsConfig.cmake b/cmake_modules/wolfbodydynamicsConfig.cmake
deleted file mode 100644
index cfb538c1d7d49ef39ea0f6994488f9bc59d27724..0000000000000000000000000000000000000000
--- a/cmake_modules/wolfbodydynamicsConfig.cmake
+++ /dev/null
@@ -1,91 +0,0 @@
-#edit the following line to add the librarie's header files
-FIND_PATH(
- wolfbodydynamics_INCLUDE_DIRS
- NAMES bodydynamics.found
- PATHS /usr/local/include/iri-algorithms/wolf/plugin_bodydynamics)
-IF(wolfbodydynamics_INCLUDE_DIRS)
- MESSAGE("Found wolf bodydynamics include dirs: ${wolfbodydynamics_INCLUDE_DIRS}")
-ELSE(wolfbodydynamics_INCLUDE_DIRS)
- MESSAGE("Couldn't find wolf bodydynamics include dirs")
-ENDIF(wolfbodydynamics_INCLUDE_DIRS)
-
-FIND_LIBRARY(
- wolfbodydynamics_LIBRARIES
- NAMES libwolfbodydynamics.so
- PATHS /usr/local/lib)
-IF(wolfbodydynamics_LIBRARIES)
- MESSAGE("Found wolf bodydynamics lib: ${wolfbodydynamics_LIBRARIES}")
-ELSE(wolfbodydynamics_LIBRARIES)
- MESSAGE("Couldn't find wolf bodydynamics lib")
-ENDIF(wolfbodydynamics_LIBRARIES)
-
-IF (wolfbodydynamics_INCLUDE_DIRS AND wolfbodydynamics_LIBRARIES)
- SET(wolfbodydynamics_FOUND TRUE)
- ELSE(wolfbodydynamics_INCLUDE_DIRS AND wolfbodydynamics_LIBRARIES)
- set(wolfbodydynamics_FOUND FALSE)
-ENDIF (wolfbodydynamics_INCLUDE_DIRS AND wolfbodydynamics_LIBRARIES)
-
-IF (wolfbodydynamics_FOUND)
- IF (NOT wolfbodydynamics_FIND_QUIETLY)
- MESSAGE(STATUS "Found wolf bodydynamics: ${wolfbodydynamics_LIBRARIES}")
- ENDIF (NOT wolfbodydynamics_FIND_QUIETLY)
-ELSE (wolfbodydynamics_FOUND)
- IF (wolfbodydynamics_FIND_REQUIRED)
- MESSAGE(FATAL_ERROR "Could not find wolf bodydynamics")
- ENDIF (wolfbodydynamics_FIND_REQUIRED)
-ENDIF (wolfbodydynamics_FOUND)
-
-
-macro(wolf_report_not_found REASON_MSG)
- set(wolfbodydynamics_FOUND FALSE)
- unset(wolfbodydynamics_INCLUDE_DIRS)
- unset(wolfbodydynamics_LIBRARIES)
-
- # Reset the CMake module path to its state when this script was called.
- set(CMAKE_MODULE_PATH ${CALLERS_CMAKE_MODULE_PATH})
-
- # Note <package>_FIND_[REQUIRED/QUIETLY] variables defined by
- # FindPackage() use the camelcase library name, not uppercase.
- if (wolfbodydynamics_FIND_QUIETLY)
- message(STATUS "Failed to find wolf bodydynamics- " ${REASON_MSG} ${ARGN})
- elseif(wolfbodydynamics_FIND_REQUIRED)
- message(FATAL_ERROR "Failed to find wolf bodydynamics - " ${REASON_MSG} ${ARGN})
- else()
- # Neither QUIETLY nor REQUIRED, use SEND_ERROR which emits an error
- # that prevents generation, but continues configuration.
- message(SEND_ERROR "Failed to find wolf bodydynamics - " ${REASON_MSG} ${ARGN})
- endif ()
- return()
-endmacro(wolf_report_not_found)
-
-if(NOT wolfbodydynamics_FOUND)
- wolf_report_not_found("Something went wrong while setting up wolf bodydynamics.")
-endif(NOT wolfbodydynamics_FOUND)
-# Set the include directories for wolf (itself).
-set(wolfbodydynamics_FOUND TRUE)
-
-# Now we gather all the required dependencies for Wolf Template
-
-#EXAMPLE Suppose that Wolf Template depends on laser_scan_utils
-# FIND_PACKAGE(laser_scan_utils REQUIRED)
-# list(APPEND wolflaser_INCLUDE_DIRS ${laser_scan_utils_INCLUDE_DIRS})
-# list(APPEND wolflaser_LIBRARIES ${laser_scan_utils_LIBRARY})
-
-# Making sure that wolf is always looked for
-if(NOT wolf_FOUND)
- FIND_PACKAGE(wolfcore REQUIRED)
-
- #We reverse in order to insert at the start
- list(REVERSE wolfbodydynamics_INCLUDE_DIRS)
- list(APPEND wolfbodydynamics_INCLUDE_DIRS ${wolfcore_INCLUDE_DIRS})
- list(REVERSE wolfbodydynamics_INCLUDE_DIRS)
-
- list(REVERSE wolfbodydynamics_LIBRARIES)
- list(APPEND wolfbodydynamics_LIBRARIES ${wolfcore_LIBRARIES})
- list(REVERSE wolfbodydynamics_LIBRARIES)
-endif()
-
-# provide both INCLUDE_DIR and INCLUDE_DIRS
-SET(wolfbodydynamics_INCLUDE_DIR ${wolfbodydynamics_INCLUDE_DIRS})
-# provide both LIBRARY and LIBRARIES
-SET(wolfbodydynamics_LIBRARY ${wolfbodydynamics_LIBRARIES})
\ No newline at end of file
diff --git a/demos/CMakeLists.txt b/demos/CMakeLists.txt
index ce0f3ecb7bf989be57688faae510ac3320114188..4b0ffd1421c71d6ae56ad8704489d6c3231ec391 100644
--- a/demos/CMakeLists.txt
+++ b/demos/CMakeLists.txt
@@ -10,46 +10,36 @@ if (pinocchio_FOUND)
)
add_executable(solo_real_povcdl_estimation solo_real_povcdl_estimation.cpp)
target_link_libraries(solo_real_povcdl_estimation
- mcapi_utils
- ${wolfcore_LIBRARIES}
- ${wolfimu_LIBRARIES}
+ wolfcore
+ wolfimu
${PLUGIN_NAME}
${pinocchio_LIBRARIES}
/usr/local/lib/libcnpy.so
z
)
-
- add_executable(solo_real_pov_estimation solo_real_pov_estimation.cpp)
- target_link_libraries(solo_real_pov_estimation
- mcapi_utils
- ${wolfcore_LIBRARIES}
- ${wolfimu_LIBRARIES}
+
+ add_executable(solo_imu_mocap solo_imu_mocap.cpp)
+ target_link_libraries(solo_imu_mocap
+ wolfcore
+ wolfimu
${PLUGIN_NAME}
${pinocchio_LIBRARIES}
/usr/local/lib/libcnpy.so
z
)
-
- add_executable(solo_mocap_imu solo_mocap_imu.cpp)
- target_link_libraries(solo_mocap_imu
- ${wolfcore_LIBRARIES}
- ${wolfimu_LIBRARIES}
- ${PLUGIN_NAME}
- ${pinocchio_LIBRARIES}
- /usr/local/lib/libcnpy.so
- z
+
+ add_executable(solo_imu_kine_mocap solo_imu_kine_mocap.cpp)
+ target_link_libraries(solo_imu_kine_mocap
+ wolfcore
+ wolfimu
+ ${PLUGIN_NAME}
+ ${pinocchio_LIBRARIES}
+ /usr/local/lib/libcnpy.so
+ z
)
+
endif()
-add_executable(solo_imu_kine_mocap solo_imu_kine_mocap.cpp)
-target_link_libraries(solo_imu_kine_mocap
- ${wolfcore_LIBRARIES}
- ${wolfimu_LIBRARIES}
- ${PLUGIN_NAME}
- ${pinocchio_LIBRARIES}
- /usr/local/lib/libcnpy.so
- z
-)
diff --git a/demos/solo_imu_mocap.cpp b/demos/solo_imu_mocap.cpp
index 273355c0aa1981866ef7c0c309eee2fa73ba8d50..f7ad2d04ed1bfe843a903b768a81d8683090c254 100644
--- a/demos/solo_imu_mocap.cpp
+++ b/demos/solo_imu_mocap.cpp
@@ -191,9 +191,9 @@ int main (int argc, char **argv) {
// double* o_a_oi_arr = o_a_oi_npyarr.data<double>();
double* i_omg_oi_arr = i_omg_oi_npyarr.data<double>();
double* qa_arr = qa_npyarr.data<double>();
- double* dqa_arr = dqa_npyarr.data<double>();
- double* tau_arr = tau_npyarr.data<double>();
- double* l_forces_arr = l_forces_npyarr.data<double>();
+ // double* dqa_arr = dqa_npyarr.data<double>();
+ // double* tau_arr = tau_npyarr.data<double>();
+ // double* l_forces_arr = l_forces_npyarr.data<double>();
// double* w_pose_wm_arr = w_pose_wm_npyarr.data<double>();
double* m_v_wm_arr = m_v_wm_npyarr.data<double>();
double* w_v_wm_arr = w_v_wm_npyarr.data<double>();
@@ -366,8 +366,6 @@ int main (int argc, char **argv) {
// recover covariances at this point
auto kf_last = problem->getTrajectory()->getLastFrame();
- CaptureBasePtr cap_imu = kf_last->getCaptureOf(sen_imu);
-
// compute covariances of KF and capture stateblocks
Eigen::MatrixXd Qp_fbk = Eigen::Matrix3d::Identity();
Eigen::MatrixXd Qo_fbk = Eigen::Matrix3d::Identity(); // global or local?
diff --git a/demos/solo_real_povcdl_estimation.cpp b/demos/solo_real_povcdl_estimation.cpp
index 8ba3ea20404fe8b1dc8e0ad2cb51131c08a5710e..44819e61223a3461f03cfaab16957676991557b9 100644
--- a/demos/solo_real_povcdl_estimation.cpp
+++ b/demos/solo_real_povcdl_estimation.cpp
@@ -69,9 +69,6 @@
#include "bodydynamics/sensor/sensor_point_feet_nomove.h"
#include "bodydynamics/processor/processor_point_feet_nomove.h"
-// UTILS
-#include "mcapi_utils.h"
-
using namespace Eigen;
using namespace wolf;
@@ -334,7 +331,7 @@ int main (int argc, char **argv) {
// 1. Call setOrigin processorIMU to generate a fake captureIMU -> generates b_imu
// 2. process one IKin capture corresponding to the same KF, the factor needing b_imu -> generates a b_pbc
// 3. set processorForceTorquePreint origin which requires both b_imu and b_pbc
- proc_inkin->keyFrameCallback(KF1, 0.0005);
+ proc_inkin->keyFrameCallback(KF1);
proc_imu->setOrigin(KF1);
// proc_legodom->setOrigin(KF1);
diff --git a/include/bodydynamics/factor/factor_point_feet_nomove.h b/include/bodydynamics/factor/factor_point_feet_nomove.h
index 1e92c70c10ba0e9987b3f265129a3118038090da..9e17e16b75f6cc703ea6f5f7c58b35df27b39aad 100644
--- a/include/bodydynamics/factor/factor_point_feet_nomove.h
+++ b/include/bodydynamics/factor/factor_point_feet_nomove.h
@@ -158,7 +158,8 @@ bool FactorPointFeetNomove::operator () (
Eigen::Quaternion<T> bm_q_l(bm_qvec_l);
Eigen::Quaternion<T> b_q_l(b_qvec_l);
- double radius = std::dynamic_pointer_cast<SensorPointFeetNomove>(getSensor())->getFootRadius();
+ // double radius = std::dynamic_pointer_cast<SensorPointFeetNomove>(getSensor())->getFootRadius(); // DOES NOT COMPILE
+ double radius = 0.0; // deactivate, was not doing so great anyway
// If the radius of the foot is not negligeable, try to account for it with a more complex model
Matrix<T,3,1> err;
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/processor/processor_point_feet_nomove.cpp b/src/processor/processor_point_feet_nomove.cpp
index 041a6f094d1e80a556d10e70272133c80dd31760..161c16dd18e4bdd512868d6c9bc76ed2d4f87442 100644
--- a/src/processor/processor_point_feet_nomove.cpp
+++ b/src/processor/processor_point_feet_nomove.cpp
@@ -61,11 +61,11 @@ inline void ProcessorPointFeetNomove::processCapture(CaptureBasePtr _capture)
incoming_ = _capture;
// nothing to do if any of the two buffer is empty
- if(buffer_pack_kf_.empty()){
+ if(buffer_frame_.empty()){
WOLF_DEBUG("PInertialKinematic: KF pack buffer empty, time ", _capture->getTimeStamp());
return;
}
- if(buffer_pack_kf_.empty()){
+ if(buffer_frame_.empty()){
WOLF_DEBUG("PInertialKinematics: Capture buffer empty, time ", _capture->getTimeStamp());
return;
}
@@ -75,7 +75,7 @@ inline void ProcessorPointFeetNomove::processCapture(CaptureBasePtr _capture)
}
-inline void ProcessorPointFeetNomove::processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance)
+inline void ProcessorPointFeetNomove::processKeyFrame(FrameBasePtr _keyframe_ptr)
{
if (!_keyframe_ptr)
{
@@ -83,11 +83,11 @@ inline void ProcessorPointFeetNomove::processKeyFrame(FrameBasePtr _keyframe_ptr
return;
}
// nothing to do if any of the two buffer is empty
- if(buffer_pack_kf_.empty()){
+ if(buffer_frame_.empty()){
WOLF_DEBUG("ProcessorPointFeetNomove: KF pack buffer empty, time ", _keyframe_ptr->getTimeStamp());
return;
}
- if(buffer_pack_kf_.empty()){
+ if(buffer_frame_.empty()){
WOLF_DEBUG("ProcessorPointFeetNomove: Capture buffer empty, time ", _keyframe_ptr->getTimeStamp());
return;
}
@@ -112,10 +112,10 @@ void ProcessorPointFeetNomove::processKeyFrameCreateFactorFar(){
auto sensor_pfnm = std::static_pointer_cast<SensorPointFeetNomove>(getSensor());
- for (auto kf_pk_it: buffer_pack_kf_.getContainer()){
- TimeStamp ts = kf_pk_it.first;
- auto kf_pk = kf_pk_it.second;
- auto cap_it = buffer_capture_.selectIterator(ts, kf_pk->time_tolerance);
+ for (auto kf_it: buffer_frame_.getContainer()){
+ TimeStamp ts = kf_it.first;
+ auto kf = kf_it.second;
+ auto cap_it = buffer_capture_.selectIterator(ts, params_->time_tolerance);
// We have a match between the KF and a capture from the capture buffer
if (cap_it != buffer_capture_.getContainer().end()){
@@ -129,13 +129,13 @@ void ProcessorPointFeetNomove::processKeyFrameCreateFactorFar(){
// if not, define current KF as the leg track origin and link the capture to this KF
// Error occurs if we try to link several time the same Cap->kf due to the loop on legs -> do only once
if (!cap_curr->getFrame()){
- cap_curr->link(kf_pk->key_frame);
+ cap_curr->link(kf);
}
tracks_[leg_id] = cap_curr;
}
else {
if (!cap_curr->getFrame()){
- cap_curr->link(kf_pk->key_frame);
+ cap_curr->link(kf);
}
// if it has, create a factor
auto cap_origin = std::static_pointer_cast<CapturePointFeetNomove>(tracks_[leg_id]);
@@ -149,12 +149,12 @@ void ProcessorPointFeetNomove::processKeyFrameCreateFactorFar(){
// Zero Altitude factor
Vector4d meas_altitude; meas_altitude << kin.second.head<3>(), 0;
Matrix1d alt_cov = pow(0.01, 2)*Matrix1d::Identity();
- CaptureBasePtr cap_alt = CaptureBase::emplace<CaptureBase>(kf_pk->key_frame, "Alt0", cap_curr->getTimeStamp());
+ CaptureBasePtr cap_alt = CaptureBase::emplace<CaptureBase>(kf, "Alt0", cap_curr->getTimeStamp());
FeatureBasePtr feat_alt = FeatureBase::emplace<FeatureBase>(cap_alt, "Alt0", meas_altitude, alt_cov);
FactorBasePtr fac_alt = FactorBase::emplace<FactorPointFeetAltitude>(feat_alt, feat_alt, nullptr, true);
}
}
- buffer_pack_kf_.removeUpTo(cap_curr->getTimeStamp());
+ buffer_frame_.removeUpTo(cap_curr->getTimeStamp());
buffer_capture_.removeUpTo(cap_curr->getTimeStamp());
}
}
@@ -227,12 +227,12 @@ void ProcessorPointFeetNomove::createFactorConsecutive(){
double dt_kf = cap2->getTimeStamp() - cap1->getTimeStamp();
Matrix3d cov_int = dt_kf*sensor_pfnm->getCovFootNomove(); // integrate zero velocity cov during dt_kf
FeatureBasePtr feat = FeatureBase::emplace<FeatureBase>(cap2, "PointFeet", meas, cov_int);
- FactorPointFeetNomovePtr fac = FactorBase::emplace<FactorPointFeetNomove>(feat, feat, kf1_it->second->key_frame, nullptr, true);
+ FactorPointFeetNomovePtr fac = FactorBase::emplace<FactorPointFeetNomove>(feat, feat, kf1_it->second, nullptr, true);
// Zero Altitude factor
Vector4d meas_altitude; meas_altitude << kin_pair2_it->second.head<3>(), 0;
Matrix1d alt_cov = pow(0.01, 2)*Matrix1d::Identity();
- CaptureBasePtr cap_alt = CaptureBase::emplace<CaptureBase>(kf2_it->second->key_frame, "Alt0", cap2->getTimeStamp());
+ CaptureBasePtr cap_alt = CaptureBase::emplace<CaptureBase>(kf2_it->second, "Alt0", cap2->getTimeStamp());
FeatureBasePtr feat_alt = FeatureBase::emplace<FeatureBase>(cap_alt, "Alt0", meas_altitude, alt_cov);
FactorBasePtr fac_alt = FactorBase::emplace<FactorPointFeetAltitude>(feat_alt, feat_alt, nullptr, true);
}
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 60f23c1fe3f823d3c1c848365735928550d24095..3fa9269b031c95c9d5bc1a679d09989221f400bc 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -1,66 +1,30 @@
# Retrieve googletest from github & compile
add_subdirectory(gtest)
-
-## Added these two include_dirs: ######################
-#
-#CMAKE modules
-#SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake_modules")
-#MESSAGE(STATUS ${CMAKE_MODULE_PATH})
-#
-# Include Eigen
-#FIND_PACKAGE(Eigen 3 REQUIRED)
-#INCLUDE_DIRECTORIES(${EIGEN_INCLUDE_DIRS})
-#
-# Include Ceres
-#FIND_PACKAGE(Ceres QUIET) #Ceres is not required
-#IF(Ceres_FOUND)
-# INCLUDE_DIRECTORIES(${CERES_INCLUDE_DIRS})
-#ENDIF(Ceres_FOUND)
-#
-## and now gtest_motion_2d works ######################
-
-
-# Include gtest directory.
-include_directories(${GTEST_INCLUDE_DIRS})
-
################# ADD YOUR TESTS BELOW ####################
# #
# ==== IN ALPHABETICAL ORDER! ==== #
# #
wolf_add_gtest(gtest_capture_inertial_kinematics gtest_capture_inertial_kinematics.cpp)
-target_link_libraries(gtest_capture_inertial_kinematics ${PLUGIN_NAME})
wolf_add_gtest(gtest_capture_leg_odom gtest_capture_leg_odom.cpp)
-target_link_libraries(gtest_capture_leg_odom ${PLUGIN_NAME})
wolf_add_gtest(gtest_feature_inertial_kinematics gtest_feature_inertial_kinematics.cpp)
-target_link_libraries(gtest_feature_inertial_kinematics ${PLUGIN_NAME})
wolf_add_gtest(gtest_factor_inertial_kinematics gtest_factor_inertial_kinematics.cpp)
-target_link_libraries(gtest_factor_inertial_kinematics ${PLUGIN_NAME})
wolf_add_gtest(gtest_factor_force_torque gtest_factor_force_torque.cpp)
-target_link_libraries(gtest_factor_force_torque ${PLUGIN_NAME})
wolf_add_gtest(gtest_force_torque_delta_tools gtest_force_torque_delta_tools.cpp)
-target_link_libraries(gtest_force_torque_delta_tools ${PLUGIN_NAME})
+# TODO: revive those
# wolf_add_gtest(gtest_feature_force_torque_preint gtest_feature_force_torque_preint.cpp)
-# target_link_libraries(gtest_feature_force_torque_preint ${PLUGIN_NAME})
-
# wolf_add_gtest(gtest_processor_inertial_kinematics gtest_processor_inertial_kinematics.cpp)
-# target_link_libraries(gtest_processor_inertial_kinematics ${PLUGIN_NAME})
-
# wolf_add_gtest(gtest_processor_force_torque_preint gtest_processor_force_torque_preint.cpp)
-# target_link_libraries(gtest_processor_force_torque_preint ${PLUGIN_NAME})
wolf_add_gtest(gtest_processor_point_feet_nomove gtest_processor_point_feet_nomove.cpp)
-target_link_libraries(gtest_processor_point_feet_nomove ${PLUGIN_NAME})
wolf_add_gtest(gtest_sensor_force_torque gtest_sensor_force_torque.cpp)
-target_link_libraries(gtest_sensor_force_torque ${PLUGIN_NAME})
wolf_add_gtest(gtest_sensor_inertial_kinematics gtest_sensor_inertial_kinematics.cpp)
-target_link_libraries(gtest_sensor_inertial_kinematics ${PLUGIN_NAME})
diff --git a/test/gtest/CMakeLists.txt b/test/gtest/CMakeLists.txt
index 559756b02472653c655c44152c452ba8767ef6f2..0e851c5fbfce34b87a3f91e2a70ad0b18bb0dca7 100644
--- a/test/gtest/CMakeLists.txt
+++ b/test/gtest/CMakeLists.txt
@@ -1,65 +1,80 @@
-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 OFF)
-set(GTEST_FORCE_SHARED_CRT ON)
-set(GTEST_DISABLE_PTHREADS OFF)
+ # For some reason I need to disable PTHREADS
+ # with g++ (Ubuntu 4.9.3-8ubuntu2~14.04) 4.9.3
+ # This is a known issue for MinGW :
+ # https://github.com/google/shaderc/pull/174
+ #if(MINGW)
+ # set(GTEST_DISABLE_PTHREADS ON)
+ #endif()
-# For some reason I need to disable PTHREADS
-# with g++ (Ubuntu 4.9.3-8ubuntu2~14.04) 4.9.3
-# This is a known issue for MinGW :
-# https://github.com/google/shaderc/pull/174
-#if(MINGW)
- set(GTEST_DISABLE_PTHREADS ON)
-#endif()
+ # Download GoogleTest
+ ExternalProject_Add(googletest
+ GIT_REPOSITORY https://github.com/google/googletest.git
+ GIT_TAG v1.8.x
+ # TIMEOUT 1 # We'll try this
+ CMAKE_ARGS -DCMAKE_ARCHIVE_OUTPUT_DIRECTORY_DEBUG:PATH=DebugLibs
+ -DCMAKE_ARCHIVE_OUTPUT_DIRECTORY_RELEASE:PATH=ReleaseLibs
+ -DCMAKE_CXX_FLAGS=${MSVC_COMPILER_DEFS}
+ -Dgtest_force_shared_crt=${GTEST_FORCE_SHARED_CRT}
+ -Dgtest_disable_pthreads=${GTEST_DISABLE_PTHREADS}
+ -DBUILD_GTEST=ON
+ PREFIX "${CMAKE_CURRENT_BINARY_DIR}"
+ # Disable install step
+ INSTALL_COMMAND ""
+ UPDATE_DISCONNECTED 1 # 1: do not update googletest; 0: update googletest via github
+ )
-# Download GoogleTest
-ExternalProject_Add(googletest
- GIT_REPOSITORY https://github.com/google/googletest.git
- GIT_TAG v1.8.x
- # TIMEOUT 1 # We'll try this
- CMAKE_ARGS -DCMAKE_ARCHIVE_OUTPUT_DIRECTORY_DEBUG:PATH=DebugLibs
- -DCMAKE_ARCHIVE_OUTPUT_DIRECTORY_RELEASE:PATH=ReleaseLibs
- -DCMAKE_CXX_FLAGS=${MSVC_COMPILER_DEFS}
- -Dgtest_force_shared_crt=${GTEST_FORCE_SHARED_CRT}
- -Dgtest_disable_pthreads=${GTEST_DISABLE_PTHREADS}
- -DBUILD_GTEST=ON
- PREFIX "${CMAKE_CURRENT_BINARY_DIR}"
- # Disable install step
- INSTALL_COMMAND ""
- UPDATE_DISCONNECTED 1 # 1: do not update googletest; 0: update googletest via github
-)
+ # Get GTest source and binary directories from CMake project
-# Get GTest source and binary directories from CMake project
+ # Specify include dir
+ ExternalProject_Get_Property(googletest source_dir)
+ set(GTEST_INCLUDE_DIRS ${source_dir}/googletest/include PARENT_SCOPE)
-# Specify include dir
-ExternalProject_Get_Property(googletest source_dir)
-set(GTEST_INCLUDE_DIRS ${source_dir}/googletest/include PARENT_SCOPE)
+ # Specify MainTest's link libraries
+ ExternalProject_Get_Property(googletest binary_dir)
+ set(GTEST_LIBS_DIR ${binary_dir}/googlemock/gtest PARENT_SCOPE)
-# Specify MainTest's link libraries
-ExternalProject_Get_Property(googletest binary_dir)
-set(GTEST_LIBS_DIR ${binary_dir}/googlemock/gtest PARENT_SCOPE)
+ # Create a libgtest target to be used as a dependency by test programs
+ add_library(libgtest IMPORTED STATIC GLOBAL)
+ add_dependencies(libgtest googletest)
-# Create a libgtest target to be used as a dependency by test programs
-add_library(libgtest IMPORTED STATIC GLOBAL)
-add_dependencies(libgtest googletest)
+ # Set libgtest properties
+ set_target_properties(libgtest PROPERTIES
+ "IMPORTED_LOCATION" "${binary_dir}/googlemock/gtest/libgtest.a"
+ "IMPORTED_LINK_INTERFACE_LIBRARIES" "${CMAKE_THREAD_LIBS_INIT}"
+ )
-# Set libgtest properties
-set_target_properties(libgtest PROPERTIES
- "IMPORTED_LOCATION" "${binary_dir}/googlemock/gtest/libgtest.a"
- "IMPORTED_LINK_INTERFACE_LIBRARIES" "${CMAKE_THREAD_LIBS_INIT}"
-)
+else()
+ 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)
+
+ 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} ${GTEST_INCLUDE_DIRS})
+ else()
+ target_link_libraries(${target} gtest_main ${PLUGIN_NAME})
+ endif()
add_test(NAME ${target} COMMAND ${target})
endfunction()