diff --git a/.gitignore b/.gitignore
index 94d4918bdec0d0d02df589c529d938e6c87d72d4..b690efc9b285d63727f435a9db3ee8b2039aee82 100644
--- a/.gitignore
+++ b/.gitignore
@@ -32,4 +32,5 @@ src/examples/map_apriltag_save.yaml
\.vscode/
build_release/
.clangd
-wolf.found
+wolfcore.found
+/wolf.found
diff --git a/CMakeLists.txt b/CMakeLists.txt
index a6e7ee166720a726fc6c9178acc4119e0c4286e9..b84a4aa218a28e25b767d517f055f0c901bf8e8d 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -10,7 +10,8 @@ SET(CMAKE_MACOSX_RPATH 1)
# The project name
-PROJECT(wolf)
+PROJECT(core)
+set(PLUGIN_NAME "wolf${PROJECT_NAME}")
#string(COMPARE EQUAL "${CMAKE_BINARY_DIR}" "" result)
#IF(result)
@@ -421,7 +422,7 @@ IF (Suitesparse_FOUND)
ENDIF(Suitesparse_FOUND)
# create the shared library
-ADD_LIBRARY(${PROJECT_NAME}
+ADD_LIBRARY(${PLUGIN_NAME}
SHARED
${SRCS}
${SRCS_BASE}
@@ -450,24 +451,24 @@ ADD_LIBRARY(${PROJECT_NAME}
# ====================
if (CMAKE_CXX_COMPILER_ID STREQUAL "Clang")
message(STATUS "Using C++ compiler clang")
- target_compile_options(${PROJECT_NAME} PRIVATE -Winconsistent-missing-override)
+ target_compile_options(${PLUGIN_NAME} PRIVATE -Winconsistent-missing-override)
# using Clang
elseif (CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
message(STATUS "Using C++ compiler gnu")
- target_compile_options(${PROJECT_NAME} PRIVATE -Wsuggest-override)
+ target_compile_options(${PLUGIN_NAME} PRIVATE -Wsuggest-override)
# using GCC
endif()
#Link the created libraries
#=============================================================
-TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${CMAKE_THREAD_LIBS_INIT} dl)
-TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${YAMLCPP_LIBRARY})
+TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${CMAKE_THREAD_LIBS_INIT} dl)
+TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${YAMLCPP_LIBRARY})
IF (Ceres_FOUND)
- TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${CERES_LIBRARIES})
+ TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${CERES_LIBRARIES})
ENDIF(Ceres_FOUND)
# IF (GLOG_FOUND)
-# TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${GLOG_LIBRARY})
+# TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${GLOG_LIBRARY})
# ENDIF (GLOG_FOUND)
#check if this is done correctly
@@ -486,12 +487,12 @@ ENDIF(BUILD_DEMOS)
#install library
#=============================================================
-INSTALL(TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME}Targets
+INSTALL(TARGETS ${PLUGIN_NAME} EXPORT ${PLUGIN_NAME}Targets
RUNTIME DESTINATION bin
LIBRARY DESTINATION lib/iri-algorithms
ARCHIVE DESTINATION lib/iri-algorithms)
-install(EXPORT ${PROJECT_NAME}Targets DESTINATION lib/cmake/${PROJECT_NAME})
+install(EXPORT ${PLUGIN_NAME}Targets DESTINATION lib/cmake/${PLUGIN_NAME})
#install headers
INSTALL(FILES ${HDRS_CAPTURE}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/capture)
@@ -536,13 +537,13 @@ INSTALL(FILES ${HDRS_WRAPPER}
INSTALL(FILES ${HDRS_YAML}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/yaml)
-FILE(WRITE ${PROJECT_NAME}.found "")
-INSTALL(FILES ${PROJECT_NAME}.found
+FILE(WRITE ${PLUGIN_NAME}.found "")
+INSTALL(FILES ${PLUGIN_NAME}.found
DESTINATION include/iri-algorithms/wolf/plugin_core)
#install Find*.cmake
-configure_file("${CMAKE_SOURCE_DIR}/cmake_modules/wolfConfig.cmake"
- "${CMAKE_BINARY_DIR}/wolfConfig.cmake" @ONLY)
+configure_file("${CMAKE_SOURCE_DIR}/cmake_modules/${PLUGIN_NAME}Config.cmake"
+ "${CMAKE_BINARY_DIR}/${PLUGIN_NAME}Config.cmake" @ONLY)
configure_file("${CMAKE_SOURCE_DIR}/cmake_modules/FindYamlCpp.cmake"
"${CMAKE_BINARY_DIR}/FindYamlCpp.cmake" @ONLY)
@@ -550,12 +551,12 @@ configure_file("${CMAKE_SOURCE_DIR}/cmake_modules/FindYamlCpp.cmake"
INSTALL(FILES "${WOLF_CONFIG_DIR}/config.h"
DESTINATION include/iri-algorithms/wolf/plugin_core/core/internal)
-INSTALL(FILES "${CMAKE_BINARY_DIR}/wolfConfig.cmake" DESTINATION "lib/cmake/${PROJECT_NAME}")
-INSTALL(FILES "${CMAKE_BINARY_DIR}/FindYamlCpp.cmake" DESTINATION "lib/cmake/${PROJECT_NAME}")
+INSTALL(FILES "${CMAKE_BINARY_DIR}/${PLUGIN_NAME}Config.cmake" DESTINATION "lib/cmake/${PLUGIN_NAME}")
+INSTALL(FILES "${CMAKE_BINARY_DIR}/FindYamlCpp.cmake" DESTINATION "lib/cmake/${PLUGIN_NAME}")
INSTALL(DIRECTORY ${SPDLOG_INCLUDE_DIRS} DESTINATION "include/iri-algorithms/")
-export(PACKAGE ${PROJECT_NAME})
+export(PACKAGE ${PLUGIN_NAME})
#-END_SRC --------------------------------------------------------------------------------------------------------------------------------
FIND_PACKAGE(Doxygen)
@@ -601,8 +602,8 @@ ELSE(UNIX)
ENDIF(UNIX)
IF (UNIX)
- SET(CPACK_PACKAGE_FILE_NAME "iri-${PROJECT_NAME}-dev-${CPACK_PACKAGE_VERSION}${CPACK_DEBIAN_PACKAGE_ARCHITECTURE}")
- SET(CPACK_PACKAGE_NAME "iri-${PROJECT_NAME}-dev")
+ SET(CPACK_PACKAGE_FILE_NAME "iri-${PLUGIN_NAME}-dev-${CPACK_PACKAGE_VERSION}${CPACK_DEBIAN_PACKAGE_ARCHITECTURE}")
+ SET(CPACK_PACKAGE_NAME "iri-${PLUGIN_NAME}-dev")
SET(CPACK_PACKAGE_DESCRIPTION_SUMMARY "...Enter something here...")
SET(CPACK_PACKAGING_INSTALL_PREFIX ${CMAKE_INSTALL_PREFIX})
SET(CPACK_GENERATOR "DEB")
diff --git a/cmake_modules/wolfConfig.cmake b/cmake_modules/wolfConfig.cmake
deleted file mode 100644
index 74ede89df7d20ffdfe00d7cd669bce317fc501a9..0000000000000000000000000000000000000000
--- a/cmake_modules/wolfConfig.cmake
+++ /dev/null
@@ -1,99 +0,0 @@
-#edit the following line to add the librarie's header files
-FIND_PATH(
- wolf_INCLUDE_DIRS
- NAMES wolf.found
- PATHS /usr/local/include/iri-algorithms/wolf/plugin_core)
-IF(wolf_INCLUDE_DIRS)
- MESSAGE("Found wolf include dirs: ${wolf_INCLUDE_DIRS}")
-ELSE(wolf_INCLUDE_DIRS)
- MESSAGE("Couldn't find wolf include dirs")
-ENDIF(wolf_INCLUDE_DIRS)
-
-FIND_LIBRARY(
- wolf_LIBRARIES
- NAMES libwolf.so libwolf.dylib
- PATHS /usr/local/lib/iri-algorithms)
-IF(wolf_LIBRARIES)
- MESSAGE("Found wolf lib: ${wolf_LIBRARIES}")
-ELSE(wolf_LIBRARIES)
- MESSAGE("Couldn't find wolf lib")
-ENDIF(wolf_LIBRARIES)
-
-IF (wolf_INCLUDE_DIRS AND wolf_LIBRARIES)
- SET(wolf_FOUND TRUE)
- ELSE(wolf_INCLUDE_DIRS AND wolf_LIBRARIES)
- set(wolf_FOUND FALSE)
-ENDIF (wolf_INCLUDE_DIRS AND wolf_LIBRARIES)
-
-IF (wolf_FOUND)
- IF (NOT wolf_FIND_QUIETLY)
- MESSAGE(STATUS "Found wolf: ${wolf_LIBRARIES}")
- ENDIF (NOT wolf_FIND_QUIETLY)
-ELSE (wolf_FOUND)
- IF (wolf_FIND_REQUIRED)
- MESSAGE(FATAL_ERROR "Could not find wolf")
- ENDIF (wolf_FIND_REQUIRED)
-ENDIF (wolf_FOUND)
-
-
-macro(wolf_report_not_found REASON_MSG)
- set(wolf_FOUND FALSE)
- unset(wolf_INCLUDE_DIRS)
- unset(wolf_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 (wolf_FIND_QUIETLY)
- message(STATUS "Failed to find wolf- " ${REASON_MSG} ${ARGN})
- else (wolf_FIND_REQUIRED)
- message(FATAL_ERROR "Failed to find wolf - " ${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 - " ${REASON_MSG} ${ARGN})
- endif ()
- return()
-endmacro(wolf_report_not_found)
-
-if(NOT wolf_FOUND)
- wolf_report_not_found("Something went wrong while setting up wolf.")
-endif(NOT wolf_FOUND)
-# Set the include directories for wolf (itself).
-set(wolf_FOUND TRUE)
-
-# Now we gather all the required dependencies for Wolf
-
-get_filename_component(WOLF_CURRENT_CONFIG_DIR
- "${CMAKE_CURRENT_LIST_FILE}" PATH)
-
-SET(BACKUP_MODULE_PATH ${CMAKE_MODULE_PATH})
-LIST(APPEND CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${WOLF_CURRENT_CONFIG_DIR})
-
-FIND_PACKAGE(Threads REQUIRED)
-list(APPEND wolf_LIBRARIES ${CMAKE_THREAD_LIBS_INIT})
-
-FIND_PACKAGE(Ceres REQUIRED)
-list(APPEND wolf_INCLUDE_DIRS ${CERES_INCLUDE_DIRS})
-list(APPEND wolf_LIBRARIES ${CERES_LIBRARIES})
-
-# YAML with yaml-cpp
-FIND_PACKAGE(YamlCpp REQUIRED)
-list(APPEND wolf_INCLUDE_DIRS ${YAMLCPP_INCLUDE_DIRS})
-list(APPEND wolf_LIBRARIES ${YAMLCPP_LIBRARY})
-
-#Eigen
-# FIND_PACKAGE(Eigen3 REQUIRED)
-# list(APPEND wolf_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIRS})
-
-if(NOT Eigen3_FOUND)
- FIND_PACKAGE(Eigen3 REQUIRED)
-endif()
-if(${EIGEN3_VERSION_STRING} VERSION_LESS 3.3)
- message(FATAL_ERROR "Found Eigen ${EIGEN3_VERSION_STRING}. The minimum version required is Eigen 3.3")
-endif()
-list(APPEND wolf_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIRS})
-
-SET(CMAKE_MODULE_PATH ${BACKUP_MODULE_PATH})
diff --git a/cmake_modules/wolfcoreConfig.cmake b/cmake_modules/wolfcoreConfig.cmake
new file mode 100644
index 0000000000000000000000000000000000000000..b4df7e15bab20bc8d1090ffbd5428ac7a3ab5cd6
--- /dev/null
+++ b/cmake_modules/wolfcoreConfig.cmake
@@ -0,0 +1,99 @@
+#edit the following line to add the librarie's header files
+FIND_PATH(
+ wolfcore_INCLUDE_DIRS
+ NAMES wolfcore.found
+ PATHS /usr/local/include/iri-algorithms/wolf/plugin_core)
+IF(wolfcore_INCLUDE_DIRS)
+ MESSAGE("Found wolf core include dirs: ${wolfcore_INCLUDE_DIRS}")
+ELSE(wolfcore_INCLUDE_DIRS)
+ MESSAGE("Couldn't find wolf core include dirs")
+ENDIF(wolfcore_INCLUDE_DIRS)
+
+FIND_LIBRARY(
+ wolfcore_LIBRARIES
+ NAMES libwolfcore.so libwolfcore.dylib
+ PATHS /usr/local/lib/iri-algorithms)
+IF(wolfcore_LIBRARIES)
+ MESSAGE("Found wolf core lib: ${wolfcore_LIBRARIES}")
+ELSE(wolfcore_LIBRARIES)
+ MESSAGE("Couldn't find wolf core lib")
+ENDIF(wolfcore_LIBRARIES)
+
+IF (wolfcore_INCLUDE_DIRS AND wolfcore_LIBRARIES)
+ SET(wolfcore_FOUND TRUE)
+ ELSE(wolfcore_INCLUDE_DIRS AND wolfcore_LIBRARIES)
+ set(wolfcore_FOUND FALSE)
+ENDIF (wolfcore_INCLUDE_DIRS AND wolfcore_LIBRARIES)
+
+IF (wolfcore_FOUND)
+ IF (NOT wolfcore_FIND_QUIETLY)
+ MESSAGE(STATUS "Found wolf core: ${wolfcore_LIBRARIES}")
+ ENDIF (NOT wolfcore_FIND_QUIETLY)
+ELSE (wolfcore_FOUND)
+ IF (wolfcore_FIND_REQUIRED)
+ MESSAGE(FATAL_ERROR "Could not find wolf core")
+ ENDIF (wolfcore_FIND_REQUIRED)
+ENDIF (wolfcore_FOUND)
+
+
+macro(wolfcore_report_not_found REASON_MSG)
+ set(wolfcore_FOUND FALSE)
+ unset(wolfcore_INCLUDE_DIRS)
+ unset(wolfcore_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 (wolfcore_FIND_QUIETLY)
+ message(STATUS "Failed to find wolf core - " ${REASON_MSG} ${ARGN})
+ elseif (wolfcore_FIND_REQUIRED)
+ message(FATAL_ERROR "Failed to find wolf core - " ${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 core - " ${REASON_MSG} ${ARGN})
+ endif ()
+ return()
+endmacro(wolfcore_report_not_found)
+
+if(NOT wolfcore_FOUND)
+ wolfcore_report_not_found("Something went wrong while setting up wolf.")
+else (NOT wolfcore_FOUND)
+ # Set the include directories for wolf core (itself).
+ set(wolfcore_FOUND TRUE)
+
+ # Now we gather all the required dependencies for Wolf
+
+ get_filename_component(WOLFCORE_CURRENT_CONFIG_DIR
+ "${CMAKE_CURRENT_LIST_FILE}" PATH)
+
+ SET(BACKUP_MODULE_PATH ${CMAKE_MODULE_PATH})
+ LIST(APPEND CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${WOLFCORE_CURRENT_CONFIG_DIR})
+
+ FIND_PACKAGE(Threads REQUIRED)
+ list(APPEND wolfcore_LIBRARIES ${CMAKE_THREAD_LIBS_INIT})
+
+ FIND_PACKAGE(Ceres REQUIRED)
+ list(APPEND wolfcore_INCLUDE_DIRS ${CERES_INCLUDE_DIRS})
+ list(APPEND wolfcore_LIBRARIES ${CERES_LIBRARIES})
+
+ # YAML with yaml-cpp
+ FIND_PACKAGE(YamlCpp REQUIRED)
+ list(APPEND wolfcore_INCLUDE_DIRS ${YAMLCPP_INCLUDE_DIRS})
+ list(APPEND wolfcore_LIBRARIES ${YAMLCPP_LIBRARY})
+
+ #Eigen
+ # FIND_PACKAGE(Eigen3 REQUIRED)
+ # list(APPEND wolfcore_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIRS})
+
+ if(NOT Eigen3_FOUND)
+ FIND_PACKAGE(Eigen3 REQUIRED)
+ endif()
+ if(${EIGEN3_VERSION_STRING} VERSION_LESS 3.3)
+ message(FATAL_ERROR "Found Eigen ${EIGEN3_VERSION_STRING}. The minimum version required is Eigen 3.3")
+ endif()
+ list(APPEND wolfcore_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIRS})
+endif(NOT wolfcore_FOUND)
+SET(CMAKE_MODULE_PATH ${BACKUP_MODULE_PATH})
diff --git a/demos/demo_analytic_autodiff_factor.cpp b/demos/demo_analytic_autodiff_factor.cpp
index d72f6ce2dde3025570786da0a532361c47da4d6b..0fd8d42cc5280016d7473e23cb2624615221fda7 100644
--- a/demos/demo_analytic_autodiff_factor.cpp
+++ b/demos/demo_analytic_autodiff_factor.cpp
@@ -274,8 +274,8 @@ int main(int argc, char** argv)
printf("\nError opening file\n");
// PRIOR
- FrameBasePtr first_frame_autodiff = wolf_problem_autodiff->getTrajectory()->getFrameList().front();
- FrameBasePtr first_frame_analytic = wolf_problem_analytic->getTrajectory()->getFrameList().front();
+ FrameBasePtr first_frame_autodiff = wolf_problem_autodiff->getTrajectory()->getFrameMap().front();
+ FrameBasePtr first_frame_analytic = wolf_problem_analytic->getTrajectory()->getFrameMap().front();
CaptureFix* initial_covariance_autodiff = new CaptureFix(TimeStamp(0), new SensorBase("ABSOLUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_autodiff->getState(), Eigen::Matrix3d::Identity() * 0.01);
CaptureFix* initial_covariance_analytic = new CaptureFix(TimeStamp(0), new SensorBase("ABSOLUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_analytic->getState(), Eigen::Matrix3d::Identity() * 0.01);
first_frame_autodiff->addCapture(initial_covariance_autodiff);
diff --git a/demos/demo_wolf_imported_graph.cpp b/demos/demo_wolf_imported_graph.cpp
index d3be8bc31e3a1821e2ff7442f56e33e6456b2e89..c0afa76e96229181122659244a20fd1fab433354 100644
--- a/demos/demo_wolf_imported_graph.cpp
+++ b/demos/demo_wolf_imported_graph.cpp
@@ -301,8 +301,8 @@ int main(int argc, char** argv)
printf("\nError opening file\n");
// PRIOR
- FrameBasePtr first_frame_full = wolf_problem_full->getTrajectory()->getFrameList().front();
- FrameBasePtr first_frame_prun = wolf_problem_prun->getTrajectory()->getFrameList().front();
+ FrameBasePtr first_frame_full = wolf_problem_full->getTrajectory()->getFrameMap().front();
+ FrameBasePtr first_frame_prun = wolf_problem_prun->getTrajectory()->getFrameMap().front();
CaptureFix* initial_covariance_full = new CaptureFix(TimeStamp(0), new SensorBase("ABSLOUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_full->getState(), Eigen::Matrix3d::Identity() * 0.01);
CaptureFix* initial_covariance_prun = new CaptureFix(TimeStamp(0), new SensorBase("ABSLOUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_prun->getState(), Eigen::Matrix3d::Identity() * 0.01);
first_frame_full->addCapture(initial_covariance_full);
diff --git a/demos/hello_wolf/CMakeLists.txt b/demos/hello_wolf/CMakeLists.txt
index bf223af61a3358cfc458d8360c1b07e7ab1aff01..81291a479cd3e524ba77b3676982ed0e140d965b 100644
--- a/demos/hello_wolf/CMakeLists.txt
+++ b/demos/hello_wolf/CMakeLists.txt
@@ -22,17 +22,17 @@ SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
add_library(hellowolf SHARED ${SRCS_HELLOWOLF})
-TARGET_LINK_LIBRARIES(hellowolf ${PROJECT_NAME})
+TARGET_LINK_LIBRARIES(hellowolf ${PLUGIN_NAME})
ADD_EXECUTABLE(hello_wolf hello_wolf.cpp)
-TARGET_LINK_LIBRARIES(hello_wolf ${PROJECT_NAME} hellowolf)
+TARGET_LINK_LIBRARIES(hello_wolf ${PLUGIN_NAME} hellowolf)
ADD_EXECUTABLE(hello_wolf_autoconf hello_wolf_autoconf.cpp)
-#TARGET_LINK_LIBRARIES(hello_wolf_autoconf ${PROJECT_NAME})
-TARGET_LINK_LIBRARIES(hello_wolf_autoconf ${PROJECT_NAME} hellowolf)
+#TARGET_LINK_LIBRARIES(hello_wolf_autoconf ${PLUGIN_NAME})
+TARGET_LINK_LIBRARIES(hello_wolf_autoconf ${PLUGIN_NAME} hellowolf)
#add_library(sensor_odom SHARED ../src/sensor/sensor_odom_2d.cpp ../src/processor/processor_odom_2d.cpp)
-#TARGET_LINK_LIBRARIES(sensor_odom ${PROJECT_NAME} hellowolf)
+#TARGET_LINK_LIBRARIES(sensor_odom ${PLUGIN_NAME} hellowolf)
#
#add_library(range_bearing SHARED sensor_range_bearing.cpp processor_range_bearing.cpp)
-#TARGET_LINK_LIBRARIES(range_bearing ${PROJECT_NAME} hellowolf)
+#TARGET_LINK_LIBRARIES(range_bearing ${PLUGIN_NAME} hellowolf)
diff --git a/demos/hello_wolf/hello_wolf.cpp b/demos/hello_wolf/hello_wolf.cpp
index b747cbee6e2939e9ee096e9241d185bbd6ca1da9..ef4f173494fbb602b2042088b065f45bf28ae43b 100644
--- a/demos/hello_wolf/hello_wolf.cpp
+++ b/demos/hello_wolf/hello_wolf.cpp
@@ -105,7 +105,7 @@ int main()
// Wolf problem and solver
ProblemPtr problem = Problem::create("PO", 2);
SolverCeresPtr ceres = std::make_shared<SolverCeres>(problem);
- ceres.getSolverOptions().max_num_iterations = 1000; // We depart far from solution, need a lot of iterations
+ ceres->getSolverOptions().max_num_iterations = 1000; // We depart far from solution, need a lot of iterations
// sensor odometer 2d
ParamsSensorOdom2dPtr intrinsics_odo = std::make_shared<ParamsSensorOdom2d>();
@@ -232,8 +232,8 @@ int main()
// GET COVARIANCES of all states
WOLF_TRACE("======== COVARIANCES OF SOLVED PROBLEM =======")
ceres->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
- for (auto& kf : problem->getTrajectory()->getFrameList())
- if (kf->isKeyOrAux())
+ for (auto& kf : *problem->getTrajectory())
+ if (kf->isKey())
{
Eigen::MatrixXd cov;
kf->getCovariance(cov);
diff --git a/demos/hello_wolf/hello_wolf_autoconf.cpp b/demos/hello_wolf/hello_wolf_autoconf.cpp
index 4a12a89073ecc0b53c24f87a273fcb5621433e09..e975bb30fa55bb3cb39026a4377f7f4e62b2e58b 100644
--- a/demos/hello_wolf/hello_wolf_autoconf.cpp
+++ b/demos/hello_wolf/hello_wolf_autoconf.cpp
@@ -7,7 +7,8 @@
// wolf core includes
-#include "../../include/core/ceres_wrapper/solver_ceres.h"
+#include "core/ceres_wrapper/solver_ceres.h"
+#include "core/solver/factory_solver.h"
#include "core/common/wolf.h"
#include "core/capture/capture_odom_2d.h"
#include "core/processor/processor_motion.h"
@@ -110,9 +111,10 @@ int main()
problem->print(4,0,1,0);
// Solver. Configure a Ceres solver
- ceres::Solver::Options options;
- options.max_num_iterations = 1000; // We depart far from solution, need a lot of iterations
- SolverCeresPtr ceres = std::make_shared<SolverCeres>(problem, options);
+ // ceres::Solver::Options options;
+ // options.max_num_iterations = 1000; // We depart far from solution, need a lot of iterations
+ // SolverCeresPtr ceres = std::make_shared<SolverCeres>(problem, options);
+ SolverManagerPtr ceres = FactorySolver::create("SolverCeres", problem, server);
// recover sensor pointers and other stuff for later use (access by sensor name)
SensorBasePtr sensor_odo = problem->getSensor("sen odom");
@@ -216,8 +218,8 @@ int main()
// GET COVARIANCES of all states
WOLF_TRACE("======== COVARIANCES OF SOLVED PROBLEM =======")
ceres->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
- for (auto& kf : problem->getTrajectory()->getFrameList())
- if (kf->isKeyOrAux())
+ for (auto& kf : *problem->getTrajectory())
+ if (kf->isKey())
{
Eigen::MatrixXd cov;
kf->getCovariance(cov);
diff --git a/demos/hello_wolf/processor_range_bearing.cpp b/demos/hello_wolf/processor_range_bearing.cpp
index 08e77f76c8d7d2ec70c9b1ac6689e724a67e5c7c..7df38e73367b7b45aff81cdbf5f00099d64aa990 100644
--- a/demos/hello_wolf/processor_range_bearing.cpp
+++ b/demos/hello_wolf/processor_range_bearing.cpp
@@ -46,7 +46,7 @@ void ProcessorRangeBearing::processCapture(CaptureBasePtr _capture)
if (!kf)
{
// No KeyFrame callback received -- we assume a KF is available to hold this _capture (checked in assert below)
- kf = getProblem()->closestKeyFrameToTimeStamp(_capture->getTimeStamp());
+ kf = getProblem()->closestFrameToTimeStamp(_capture->getTimeStamp());
assert( (fabs(kf->getTimeStamp() - _capture->getTimeStamp()) < params_->time_tolerance) && "Could not find a KF close enough to _capture!");
}
diff --git a/demos/hello_wolf/yaml/hello_wolf_config.yaml b/demos/hello_wolf/yaml/hello_wolf_config.yaml
index f2e4f8db31f97ef8059b4d59483a747456e6d19c..a7d674d13d929cdcd260c571d9c12cde02e67658 100644
--- a/demos/hello_wolf/yaml/hello_wolf_config.yaml
+++ b/demos/hello_wolf/yaml/hello_wolf_config.yaml
@@ -1,7 +1,6 @@
config:
problem:
-
tree_manager:
type: "TreeManagerSlidingWindow"
n_key_frames: -1
@@ -20,7 +19,12 @@ config:
P: [0.31, 0.31]
O: [0.31]
time_tolerance: 0.1
-
+
+ solver:
+ max_num_iterations: 100
+ verbose: 0
+ period: 0.2
+ update_immediately: false
sensors:
- type: "SensorOdom2d"
diff --git a/demos/solver/test_iQR_wolf2.cpp b/demos/solver/test_iQR_wolf2.cpp
index de9f6f157cc58ef2b5a542056cae7d7e32b16afd..82aab6219874c3575dc14fdb24af6d2cc0e2d07a 100644
--- a/demos/solver/test_iQR_wolf2.cpp
+++ b/demos/solver/test_iQR_wolf2.cpp
@@ -363,8 +363,8 @@ int main(int argc, char *argv[])
// Vehicle poses
std::cout << "Vehicle poses..." << std::endl;
int i = 0;
- Eigen::VectorXd state_poses(wolf_manager_QR->getProblem()->getTrajectory()->getFrameList().size() * 3);
- for (auto frame_it = wolf_manager_QR->getProblem()->getTrajectory()->getFrameList().begin(); frame_it != wolf_manager_QR->getProblem()->getTrajectory()->getFrameList().end(); frame_it++)
+ Eigen::VectorXd state_poses(wolf_manager_QR->getProblem()->getTrajectory()->getFrameMap().size() * 3);
+ for (auto frame_it = wolf_manager_QR->getProblem()->getTrajectory()->getFrameMap().begin(); frame_it != wolf_manager_QR->getProblem()->getTrajectory()->getFrameMap().end(); frame_it++)
{
if (complex_angle)
state_poses.segment(i, 3) << *(*frame_it)->getP()->get(), *((*frame_it)->getP()->get() + 1), atan2(*(*frame_it)->getO()->get(), *((*frame_it)->getO()->get() + 1));
diff --git a/include/core/capture/capture_base.h b/include/core/capture/capture_base.h
index 324b83297d987561c4a119e699ac44c2d0eb4ec5..40d3603da035205b145df7d1b7d43541a52b6a44 100644
--- a/include/core/capture/capture_base.h
+++ b/include/core/capture/capture_base.h
@@ -157,7 +157,8 @@ inline unsigned int CaptureBase::id() const
inline FrameBasePtr CaptureBase::getFrame() const
{
- return frame_ptr_.lock();
+ if(frame_ptr_.expired()) return nullptr;
+ else return frame_ptr_.lock();
}
inline void CaptureBase::setFrame(const FrameBasePtr _frm_ptr)
diff --git a/include/core/ceres_wrapper/solver_ceres.h b/include/core/ceres_wrapper/solver_ceres.h
index e4306aa8ca126185a6343435405c1bcb5940e650..c899b7a054d6e1a9ebab5f45cca35fa0bd86c3d0 100644
--- a/include/core/ceres_wrapper/solver_ceres.h
+++ b/include/core/ceres_wrapper/solver_ceres.h
@@ -116,6 +116,10 @@ class SolverCeres : public SolverManager
const Eigen::SparseMatrixd computeHessian() const;
+ virtual int numStateBlocksDerived() const override;
+
+ virtual int numFactorsDerived() const override;
+
protected:
bool checkDerived(std::string prefix="") const override;
@@ -138,7 +142,14 @@ class SolverCeres : public SolverManager
bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const override;
- bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) override;
+ bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) const override;
+
+ bool isStateBlockFixedDerived(const StateBlockPtr& st) override;
+
+ bool hasThisLocalParametrizationDerived(const StateBlockPtr& st,
+ const LocalParametrizationBasePtr& local_param) override;
+
+ bool hasLocalParametrizationDerived(const StateBlockPtr& st) const override;
};
inline ceres::Solver::Summary SolverCeres::getSummary()
@@ -158,16 +169,35 @@ inline ceres::Solver::Options& SolverCeres::getSolverOptions()
inline bool SolverCeres::isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const
{
- return fac_2_residual_idx_.find(fac_ptr) != fac_2_residual_idx_.end()
- && fac_2_costfunction_.find(fac_ptr) != fac_2_costfunction_.end();
+ return fac_2_residual_idx_.find(fac_ptr) != fac_2_residual_idx_.end() and
+ fac_2_costfunction_.find(fac_ptr) != fac_2_costfunction_.end();
}
-inline bool SolverCeres::isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr)
+inline bool SolverCeres::isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) const
{
- return state_blocks_local_param_.find(state_ptr) != state_blocks_local_param_.end()
- && ceres_problem_->HasParameterBlock(getAssociatedMemBlockPtr(state_ptr));
+ return ceres_problem_->HasParameterBlock(getAssociatedMemBlockPtr(state_ptr));
}
+inline bool SolverCeres::isStateBlockFixedDerived(const StateBlockPtr& st)
+{
+ return ceres_problem_->IsParameterBlockConstant(SolverManager::getAssociatedMemBlockPtr(st));
+};
+
+inline bool SolverCeres::hasLocalParametrizationDerived(const StateBlockPtr& st) const
+{
+ return state_blocks_local_param_.count(st) == 1;
+};
+
+inline int SolverCeres::numStateBlocksDerived() const
+{
+ return ceres_problem_->NumParameterBlocks();
+}
+
+inline int SolverCeres::numFactorsDerived() const
+{
+ return ceres_problem_->NumResidualBlocks();
+};
+
} // namespace wolf
#endif
diff --git a/include/core/common/wolf.h b/include/core/common/wolf.h
index 229e10f95dd8f7506846fe75536a85c6604ae521..97f9a50ff1eb1b0a345461af2fbdce2b81b32491 100644
--- a/include/core/common/wolf.h
+++ b/include/core/common/wolf.h
@@ -23,6 +23,7 @@
#include <list>
#include <map>
#include <memory> // shared_ptr and weak_ptr
+#include <set>
// System specifics
#include <sys/stat.h>
@@ -230,10 +231,14 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorBase);
// Trajectory
WOLF_PTR_TYPEDEFS(TrajectoryBase);
+
// - Frame
WOLF_PTR_TYPEDEFS(FrameBase);
WOLF_LIST_TYPEDEFS(FrameBase);
+class TimeStamp;
+typedef std::map<TimeStamp, FrameBasePtr> FrameMap;
+
// - Capture
WOLF_PTR_TYPEDEFS(CaptureBase);
WOLF_LIST_TYPEDEFS(CaptureBase);
diff --git a/include/core/factor/factor_base.h b/include/core/factor/factor_base.h
index 1d4f01a35e5a1541ec6b147ab3155d268a7f9a9c..c600f8a2433a024f63d2af21fe39105c761e7300 100644
--- a/include/core/factor/factor_base.h
+++ b/include/core/factor/factor_base.h
@@ -4,6 +4,8 @@
// Forward declarations for node templates
namespace wolf{
class FeatureBase;
+class TrajectoryIter;
+class TrajectoryRevIter;
}
//Wolf includes
diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h
index a28e13e1e73c089f717a43dcf23ed8d2cc486fe3..1b368bf02df6a89567d90455edba72cf02f54bf0 100644
--- a/include/core/frame/frame_base.h
+++ b/include/core/frame/frame_base.h
@@ -1,4 +1,5 @@
+
#ifndef FRAME_BASE_H_
#define FRAME_BASE_H_
@@ -12,8 +13,8 @@ class StateBlock;
*/
typedef enum
{
- KEY = 2, ///< key frame. It plays at optimizations (estimated).
- AUXILIARY = 1, ///< auxiliary frame. It plays at optimizations (estimated).
+ KEY = 1, ///< key frame. It plays at optimizations (estimated).
+ // AUXILIARY = 1, ///< auxiliary frame. It plays at optimizations (estimated).
NON_ESTIMATED = 0 ///< regular frame. It does not play at optimization.
} FrameType;
}
@@ -82,13 +83,10 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
// get type
bool isKey() const;
- bool isAux() const;
- bool isKeyOrAux() const;
// set type
void setNonEstimated();
- void setKey();
- void setAux();
+ void setKey(ProblemPtr _prb);
// Frame values ------------------------------------------------
public:
@@ -131,7 +129,9 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
bool isConstrainedBy(const FactorBasePtr& _factor) const;
void link(TrajectoryBasePtr);
template<typename classType, typename... T>
- static std::shared_ptr<classType> emplace(TrajectoryBasePtr _ptr, T&&... all);
+ static std::shared_ptr<classType> emplaceKeyFrame(TrajectoryBasePtr _ptr, T&&... all);
+ template<typename classType, typename... T>
+ static std::shared_ptr<classType> createNonKeyFrame(T&&... all);
virtual void printHeader(int depth, //
bool constr_by, //
@@ -172,13 +172,20 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
namespace wolf {
template<typename classType, typename... T>
-std::shared_ptr<classType> FrameBase::emplace(TrajectoryBasePtr _ptr, T&&... all)
+std::shared_ptr<classType> FrameBase::emplaceKeyFrame(TrajectoryBasePtr _ptr, T&&... all)
{
- std::shared_ptr<classType> frm = std::make_shared<classType>(std::forward<T>(all)...);
+ std::shared_ptr<classType> frm = std::make_shared<classType>(FrameType::KEY, std::forward<T>(all)...);
frm->link(_ptr);
return frm;
}
+template<typename classType, typename... T>
+std::shared_ptr<classType> FrameBase::createNonKeyFrame(T&&... all)
+{
+ std::shared_ptr<classType> frm = std::make_shared<classType>(FrameType::NON_ESTIMATED, std::forward<T>(all)...);
+ return frm;
+}
+
inline unsigned int FrameBase::id() const
{
return frame_id_;
@@ -189,16 +196,6 @@ inline bool FrameBase::isKey() const
return (type_ == KEY);
}
-inline bool FrameBase::isAux() const
-{
- return (type_ == AUXILIARY);
-}
-
-inline bool FrameBase::isKeyOrAux() const
-{
- return (type_ == KEY || type_ == AUXILIARY);
-}
-
inline void FrameBase::getTimeStamp(TimeStamp& _ts) const
{
_ts = time_stamp_;
@@ -237,7 +234,7 @@ inline const FactorBasePtrList& FrameBase::getConstrainedByList() const
inline StateBlockPtr FrameBase::addStateBlock(const char& _sb_type,
const StateBlockPtr& _sb)
{
- if (isKeyOrAux())
+ if (isKey())
HasStateBlocks::addStateBlock(_sb_type, _sb, getProblem());
else
HasStateBlocks::addStateBlock(_sb_type, _sb, nullptr);
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 63512663b68076b6c7da86285a548691889b37f4..002015f83efac35a081aa24873f9c684cb1b36b7 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -217,11 +217,10 @@ class Problem : public std::enable_shared_from_this<Problem>
* - Add it to Trajectory
* - If it is key-frame, update state-block lists in Problem
*/
- FrameBasePtr emplaceFrame(FrameType _frame_key_type, //
- const TimeStamp& _time_stamp, //
- const StateStructure& _frame_structure, //
- const SizeEigen _dim, //
- const Eigen::VectorXd& _frame_state);
+ FrameBasePtr emplaceKeyFrame(const TimeStamp& _time_stamp, //
+ const StateStructure& _frame_structure, //
+ const SizeEigen _dim, //
+ const Eigen::VectorXd& _frame_state);
/** \brief Emplace frame from string frame_structure and state
* \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED
@@ -236,10 +235,9 @@ class Problem : public std::enable_shared_from_this<Problem>
* - Add it to Trajectory
* - If it is key-frame, update state-block lists in Problem
*/
- FrameBasePtr emplaceFrame(FrameType _frame_key_type, //
- const TimeStamp& _time_stamp, //
- const StateStructure& _frame_structure, //
- const VectorComposite& _frame_state);
+ FrameBasePtr emplaceKeyFrame(const TimeStamp& _time_stamp, //
+ const StateStructure& _frame_structure, //
+ const VectorComposite& _frame_state);
/** \brief Emplace frame from state
* \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED
@@ -254,9 +252,8 @@ class Problem : public std::enable_shared_from_this<Problem>
* - Add it to Trajectory
* - If it is key-frame, update state-block lists in Problem
*/
- FrameBasePtr emplaceFrame(FrameType _frame_key_type, //
- const TimeStamp& _time_stamp, //
- const VectorComposite& _frame_state);
+ FrameBasePtr emplaceKeyFrame(const TimeStamp& _time_stamp, //
+ const VectorComposite& _frame_state);
/** \brief Emplace frame from string frame_structure and dimension
* \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED
@@ -272,10 +269,9 @@ class Problem : public std::enable_shared_from_this<Problem>
* - Add it to Trajectory
* - If it is key-frame, update state-block lists in Problem
*/
- FrameBasePtr emplaceFrame(FrameType _frame_key_type, //
- const TimeStamp& _time_stamp, //
- const StateStructure& _frame_structure, //
- const SizeEigen _dim);
+ FrameBasePtr emplaceKeyFrame(const TimeStamp& _time_stamp, //
+ const StateStructure& _frame_structure, //
+ const SizeEigen _dim);
/** \brief Emplace frame from state vector
* \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED
@@ -290,9 +286,8 @@ class Problem : public std::enable_shared_from_this<Problem>
* - Add it to Trajectory
* - If it is key-frame, update state-block lists in Problem
*/
- FrameBasePtr emplaceFrame(FrameType _frame_key_type, //
- const TimeStamp& _time_stamp, //
- const Eigen::VectorXd& _frame_state);
+ FrameBasePtr emplaceKeyFrame(const TimeStamp& _time_stamp, //
+ const Eigen::VectorXd& _frame_state);
/** \brief Emplace frame, guess all values
* \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED
@@ -307,15 +302,12 @@ class Problem : public std::enable_shared_from_this<Problem>
* - Add it to Trajectory
* - If it is key-frame, update state-block lists in Problem
*/
- FrameBasePtr emplaceFrame(FrameType _frame_key_type, //
- const TimeStamp& _time_stamp);
+ FrameBasePtr emplaceKeyFrame(const TimeStamp& _time_stamp);
// Frame getters
FrameBasePtr getLastFrame( ) const;
- FrameBasePtr getLastKeyFrame( ) const;
FrameBasePtr getLastKeyOrAuxFrame( ) const;
- FrameBasePtr closestKeyFrameToTimeStamp(const TimeStamp& _ts) const;
- FrameBasePtr closestKeyOrAuxFrameToTimeStamp(const TimeStamp& _ts) const;
+ FrameBasePtr closestFrameToTimeStamp(const TimeStamp& _ts) const;
/** \brief Give the permission to a processor to create a new key Frame
*
@@ -380,7 +372,7 @@ class Problem : public std::enable_shared_from_this<Problem>
bool getCovarianceBlock(std::map<StateBlockPtr, unsigned int> _sb_2_idx, Eigen::MatrixXd& _cov) const;
bool getCovarianceBlock(StateBlockPtr _state, Eigen::MatrixXd& _cov, const int _row_and_col = 0) const;
bool getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXd& _covariance) const;
- bool getLastKeyFrameCovariance(Eigen::MatrixXd& _covariance) const;
+ bool getLastFrameCovariance(Eigen::MatrixXd& _covariance) const;
bool getLastKeyOrAuxFrameCovariance(Eigen::MatrixXd& _covariance) const;
bool getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::MatrixXd& _covariance) const;
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index e1caf715503a64568df40611c49f5b11a35f3e84..1f62a82a39dfcf5ae2de6183d341051d5785b45c 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -486,7 +486,9 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
CaptureMotionPtr origin_ptr_;
CaptureMotionPtr last_ptr_;
CaptureMotionPtr incoming_ptr_;
-
+
+ FrameBasePtr last_frame_ptr_;
+
protected:
// helpers to avoid allocation
double dt_; ///< Time step
diff --git a/include/core/processor/processor_tracker.h b/include/core/processor/processor_tracker.h
index 0f4c3180003140544f251113583e0184e630d9b8..1a05397319a431fc04a8b33642d90ad09edb524f 100644
--- a/include/core/processor/processor_tracker.h
+++ b/include/core/processor/processor_tracker.h
@@ -101,6 +101,7 @@ class ProcessorTracker : public ProcessorBase
CaptureBasePtr origin_ptr_; ///< Pointer to the origin of the tracker.
CaptureBasePtr last_ptr_; ///< Pointer to the last tracked capture.
CaptureBasePtr incoming_ptr_; ///< Pointer to the incoming capture being processed.
+ FrameBasePtr last_frame_ptr_;
FeatureBasePtrList new_features_last_; ///< List of new features in \b last for landmark initialization and new key-frame creation.
FeatureBasePtrList new_features_incoming_; ///< list of the new features of \b last successfully tracked in \b incoming
FeatureBasePtrList known_features_last_; ///< list of the known features in previous captures successfully tracked in \b last
diff --git a/include/core/solver/solver_manager.h b/include/core/solver/solver_manager.h
index 17341101c0d2ee4ee7074d220fda006e42fd0b31..bdd8a37aaa817b36609cc672402161f60a5fdba2 100644
--- a/include/core/solver/solver_manager.h
+++ b/include/core/solver/solver_manager.h
@@ -119,31 +119,47 @@ class SolverManager
ReportVerbosity getVerbosity() const;
- virtual bool isStateBlockRegistered(const StateBlockPtr& state_ptr);
+ virtual bool isStateBlockRegistered(const StateBlockPtr& state_ptr) const final;
- virtual bool isFactorRegistered(const FactorBasePtr& fac_ptr) const;
+ virtual bool isStateBlockFloating(const StateBlockPtr& state_ptr) const final;
- bool check(std::string prefix="") const;
+ virtual bool isFactorRegistered(const FactorBasePtr& fac_ptr) const final;
+ virtual bool isStateBlockFixed(const StateBlockPtr& st) final;
+
+ virtual bool hasThisLocalParametrization(const StateBlockPtr& st,
+ const LocalParametrizationBasePtr& local_param) final;
+
+ virtual bool hasLocalParametrization(const StateBlockPtr& st) const final;
+
+ virtual int numFactors() const final;
+ virtual int numStateBlocks() const final;
+ virtual int numStateBlocksFloating() const final;
+
+ virtual int numFactorsDerived() const = 0;
+ virtual int numStateBlocksDerived() const = 0;
+
+ virtual bool check(std::string prefix="") const final;
protected:
std::map<StateBlockPtr, Eigen::VectorXd> state_blocks_;
std::map<StateBlockPtr, FactorBasePtrList> state_blocks_2_factors_;
std::set<FactorBasePtr> factors_;
+ std::set<StateBlockPtr> floating_state_blocks_;
virtual Eigen::VectorXd& getAssociatedMemBlock(const StateBlockPtr& state_ptr);
const double* getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr) const;
- virtual double* getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr);
+ double* getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr);
private:
// SolverManager functions
- void addFactor(const FactorBasePtr& fac_ptr);
- void removeFactor(const FactorBasePtr& fac_ptr);
- void addStateBlock(const StateBlockPtr& state_ptr);
- void removeStateBlock(const StateBlockPtr& state_ptr);
- void updateStateBlockState(const StateBlockPtr& state_ptr);
- void updateStateBlockStatus(const StateBlockPtr& state_ptr);
- void updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr);
+ virtual void addFactor(const FactorBasePtr& fac_ptr) final;
+ virtual void removeFactor(const FactorBasePtr& fac_ptr) final;
+ virtual void addStateBlock(const StateBlockPtr& state_ptr) final;
+ virtual void removeStateBlock(const StateBlockPtr& state_ptr) final;
+ virtual void updateStateBlockState(const StateBlockPtr& state_ptr) final;
+ virtual void updateStateBlockStatus(const StateBlockPtr& state_ptr) final;
+ virtual void updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr) final;
protected:
// Derived virtual functions
@@ -154,9 +170,13 @@ class SolverManager
virtual void removeStateBlockDerived(const StateBlockPtr& state_ptr) = 0;
virtual void updateStateBlockStatusDerived(const StateBlockPtr& state_ptr) = 0;
virtual void updateStateBlockLocalParametrizationDerived(const StateBlockPtr& state_ptr) = 0;
- virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) = 0;
+ virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) const = 0;
virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const = 0;
virtual bool checkDerived(std::string prefix="") const = 0;
+ virtual bool isStateBlockFixedDerived(const StateBlockPtr& st) = 0;
+ virtual bool hasThisLocalParametrizationDerived(const StateBlockPtr& st,
+ const LocalParametrizationBasePtr& local_param) = 0;
+ virtual bool hasLocalParametrizationDerived(const StateBlockPtr& st) const = 0;
};
// Params (here becaure it needs of declaration of SolverManager::ReportVerbosity)
diff --git a/include/core/trajectory/trajectory_base.h b/include/core/trajectory/trajectory_base.h
index 152a7b40e8d154f0b251d8fd322c5f0473126972..cb89fa517f981153b84812b3382e852989d3f8a1 100644
--- a/include/core/trajectory/trajectory_base.h
+++ b/include/core/trajectory/trajectory_base.h
@@ -6,42 +6,76 @@
namespace wolf{
class Problem;
class FrameBase;
-class TimeStamp;
}
//Wolf includes
#include "core/common/wolf.h"
#include "core/common/node_base.h"
+#include "core/common/time_stamp.h"
//std includes
namespace wolf {
+class TrajectoryIter : public std::map<TimeStamp, FrameBasePtr>::const_iterator
+{
+ public:
+ TrajectoryIter(std::map<TimeStamp, FrameBasePtr>::const_iterator src)
+ : std::map<TimeStamp, FrameBasePtr>::const_iterator(std::move(src))
+ {
+
+ }
+
+ // override the indirection operator
+ const FrameBasePtr& operator*() const {
+ return std::map<TimeStamp, FrameBasePtr>::const_iterator::operator*().second;
+ }
+};
+
+class TrajectoryRevIter : public std::map<TimeStamp, FrameBasePtr>::const_reverse_iterator
+{
+ public:
+ TrajectoryRevIter(std::map<TimeStamp, FrameBasePtr>::const_reverse_iterator src)
+ : std::map<TimeStamp, FrameBasePtr>::const_reverse_iterator(std::move(src))
+ {
+
+ }
+
+ // override the indirection operator
+ const FrameBasePtr& operator*() const {
+ return std::map<TimeStamp, FrameBasePtr>::const_reverse_iterator::operator*().second;
+ }
+};
+// typedef std::map<TimeStamp, FrameBasePtr> FrameMap;
+// typedef std::map<TimeStamp, FrameBasePtr>::const_iterator TrajectoryIter;
+// typedef std::map<TimeStamp, FrameBasePtr>::const_reverse_iterator TrajectoryRevIter;
+
//class TrajectoryBase
class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<TrajectoryBase>
{
friend FrameBase;
private:
- std::list<FrameBasePtr> frame_list_;
+ FrameMap frame_map_;
protected:
- FrameBasePtr last_key_frame_ptr_; // keeps pointer to the last key frame
- FrameBasePtr last_key_or_aux_frame_ptr_; // keeps pointer to the last estimated frame
+ std::string frame_structure_; // Defines the structure of the Frames in the Trajectory.
+ // FrameBasePtr last_key_frame_ptr_; // keeps pointer to the last key frame
+ // FrameBasePtr last_key_or_aux_frame_ptr_; // keeps pointer to the last estimated frame
public:
TrajectoryBase();
~TrajectoryBase() override;
// Frames
- const FrameBasePtrList& getFrameList() const;
+ const FrameMap& getFrameMap() const;
FrameBasePtr getLastFrame() const;
- FrameBasePtr getLastKeyFrame() const;
- FrameBasePtr getLastKeyOrAuxFrame() const;
- FrameBasePtr closestKeyFrameToTimeStamp(const TimeStamp& _ts) const;
- FrameBasePtr closestKeyOrAuxFrameToTimeStamp(const TimeStamp& _ts) const;
- void sortFrame(FrameBasePtr _frm_ptr);
- void updateLastFrames();
+ FrameBasePtr getFirstFrame() const;
+ FrameBasePtr closestFrameToTimeStamp(const TimeStamp& _ts) const;
+ TrajectoryIter begin() const;
+ TrajectoryIter end() const;
+ TrajectoryRevIter rbegin() const;
+ TrajectoryRevIter rend() const;
virtual void printHeader(int depth, //
bool constr_by, //
@@ -65,30 +99,47 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj
public:
// factors
void getFactorList(FactorBasePtrList & _fac_list) const;
-
- protected:
- FrameBaseConstIter computeFrameOrder(FrameBasePtr _frame_ptr);
- void moveFrame(FrameBasePtr _frm_ptr, FrameBaseConstIter _place);
};
-inline const FrameBasePtrList& TrajectoryBase::getFrameList() const
+inline const FrameMap& TrajectoryBase::getFrameMap() const
{
- return frame_list_;
+ return frame_map_;
+ // return frame_map_;
}
-inline FrameBasePtr TrajectoryBase::getLastFrame() const
+// inline FrameBasePtr TrajectoryBase::getLastFrame() const
+// {
+// auto it = static_cast<TrajectoryRevIter>(frame_map_.rbegin());
+// return *it;
+// }
+inline FrameBasePtr TrajectoryBase::getFirstFrame() const
{
- return frame_list_.back();
+ auto it = static_cast<TrajectoryIter>(frame_map_.begin());
+ return *it;
}
-
-inline FrameBasePtr TrajectoryBase::getLastKeyFrame() const
+inline TrajectoryIter TrajectoryBase::begin() const
+{
+ return static_cast<TrajectoryIter>(frame_map_.begin());
+}
+inline TrajectoryIter TrajectoryBase::end() const
{
- return last_key_frame_ptr_;
+ return static_cast<TrajectoryIter>(frame_map_.end());
+}
+inline TrajectoryRevIter TrajectoryBase::rbegin() const
+{
+ return static_cast<TrajectoryRevIter>(frame_map_.rbegin());
+}
+inline TrajectoryRevIter TrajectoryBase::rend() const
+{
+ return static_cast<TrajectoryRevIter>(frame_map_.rend());
}
-inline FrameBasePtr TrajectoryBase::getLastKeyOrAuxFrame() const
+inline FrameBasePtr TrajectoryBase::getLastFrame() const
{
- return last_key_or_aux_frame_ptr_;
+ // return last_key_frame_ptr_;
+ auto last = this->rbegin();
+ if(last != this->rend()) return *(this->rbegin());
+ else return nullptr;
}
} // namespace wolf
diff --git a/include/core/utils/converter.h b/include/core/utils/converter.h
index 76e710f75d4aeb31589e27aa819304ec0b8d9cec..0c9502c4ec0cc12ce041611d3a86d5c035af9d09 100644
--- a/include/core/utils/converter.h
+++ b/include/core/utils/converter.h
@@ -20,9 +20,35 @@
/**
@file
+ converter.h
+ @brief This file implements a set of simple functions that deal with the correspondence between
+ classes and their string representation. The YAML autosetup framework makes heavy use of this file.
+ */
+
+//Note: In order to deal with string representations we make heavy use of regexps.
+// We use the standard C++11 regular expressions capabilities.
+
+/*
+** This file is structured essentially in two parts:
+** in the first part (which starts after the CONVERTERS ~~~~ STRING ----> TYPE line)
+** we have functions to convert from string to C++ class. For example:
+ string v1 = "[3,4,5,6,7,8,9,10,11]";
+ vector<int> v = converter<vector<int>>::convert(v1);
+ This code snippet transforms the string v1 which represents an std::vector into an actual std::vector value.
+ The second part (which starts after the TYPES ----> ToSTRING line) has the functions to
+ transform from C++ classes to strings. For instance, if we wanted to convert from a C++ class
+ to a string we would do something like this:
+ vector<int> vect{ 10, 20, 30 };
+ string str = converter<std::string>::convert(vect);
+ //str == "[10,20,30]"
*/
namespace wolf{
+ //This definition is a bit of a "hack". The reason of redefining the pair class is to be able
+ //to have two string representations of a pair, namely
+ //"(·,·)" -> typical pair/tuple representation
+ //"{·,·}" -> key-value pair representation used to represent maps.
+ //This is purely an aesthetic reason and could be removed without problems.
template<class A, class B>
struct Wpair : std::pair<A,B>
{
diff --git a/src/ceres_wrapper/solver_ceres.cpp b/src/ceres_wrapper/solver_ceres.cpp
index 74802c255736656930cd348158f62d339bd0b86a..1d9cc7dd2e8f43c76820974caa473ad876b0a97c 100644
--- a/src/ceres_wrapper/solver_ceres.cpp
+++ b/src/ceres_wrapper/solver_ceres.cpp
@@ -99,8 +99,8 @@ void SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks)
// first create a vector containing all state blocks
std::vector<StateBlockPtr> all_state_blocks, landmark_state_blocks;
//frame state blocks
- for(auto fr_ptr : wolf_problem_->getTrajectory()->getFrameList())
- if (fr_ptr->isKeyOrAux())
+ for(auto fr_ptr : *wolf_problem_->getTrajectory())
+ if (fr_ptr->isKey())
for (const auto& key : fr_ptr->getStructure())
{
const auto& sb = fr_ptr->getStateBlock(key);
@@ -126,8 +126,8 @@ void SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks)
case CovarianceBlocksToBeComputed::ALL_MARGINALS:
{
// first create a vector containing all state blocks
- for(auto fr_ptr : wolf_problem_->getTrajectory()->getFrameList())
- if (fr_ptr->isKeyOrAux())
+ for(auto fr_ptr : *wolf_problem_->getTrajectory())
+ if (fr_ptr->isKey())
for (const auto& key1 : wolf_problem_->getFrameStructure())
for (const auto& key2 : wolf_problem_->getFrameStructure())
{
@@ -165,7 +165,7 @@ void SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks)
case CovarianceBlocksToBeComputed::ROBOT_LANDMARKS:
{
//robot-robot
- auto last_key_frame = wolf_problem_->getLastKeyFrame();
+ auto last_key_frame = wolf_problem_->getLastFrame();
state_block_pairs.emplace_back(last_key_frame->getP(), last_key_frame->getP());
state_block_pairs.emplace_back(last_key_frame->getP(), last_key_frame->getO());
@@ -579,6 +579,14 @@ const Eigen::SparseMatrixd SolverCeres::computeHessian() const
return H;
}
+bool SolverCeres::hasThisLocalParametrizationDerived(const StateBlockPtr& st,
+ const LocalParametrizationBasePtr& local_param)
+{
+ return state_blocks_local_param_.count(st) == 1
+ && state_blocks_local_param_.at(st)->getLocalParametrization() == local_param
+ && ceres_problem_->GetParameterization(getAssociatedMemBlockPtr(st))
+ == state_blocks_local_param_.at(st).get();
+}
} // namespace wolf
#include "core/solver/factory_solver.h"
diff --git a/src/factor/factor_base.cpp b/src/factor/factor_base.cpp
index 4e18cb47d8485a7b9ee83853e863ddf20bb7e78a..a0b4bc90005cb0ffa48c7ef21d87c758a7a52a36 100644
--- a/src/factor/factor_base.cpp
+++ b/src/factor/factor_base.cpp
@@ -151,20 +151,23 @@ void FactorBase::remove(bool viral_remove_empty_parent)
CaptureBasePtr FactorBase::getCapture() const
{
- assert(getFeature() != nullptr && "calling getCapture before linking with a feature");
- return getFeature()->getCapture();
+ auto ftr = getFeature();
+ if (ftr != nullptr) return ftr->getCapture();
+ else return nullptr;
}
FrameBasePtr FactorBase::getFrame() const
{
- assert(getCapture() != nullptr && "calling getFrame before linking with a capture");
- return getCapture()->getFrame();
+ auto cpt = getCapture();
+ if(cpt != nullptr) return cpt->getFrame();
+ else return nullptr;
}
SensorBasePtr FactorBase::getSensor() const
{
- assert(getCapture() != nullptr && "calling getSensor before linking with a capture");
- return getCapture()->getSensor();
+ auto cpt = getCapture();
+ if(cpt != nullptr) return cpt->getSensor();
+ else return nullptr;
}
void FactorBase::setStatus(FactorStatus _status)
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index a8f56d1a20302504ae0acad701a9fce7c86e91d9..f7c7ce6ea257a48f73c68003b8c1efbfeb4f6a6b 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -116,7 +116,7 @@ void FrameBase::remove(bool viral_remove_empty_parent)
}
// Remove Frame State Blocks
- if ( isKeyOrAux() )
+ if ( isKey() )
removeStateBlocks(getProblem());
// remove from upstream
@@ -131,52 +131,23 @@ void FrameBase::remove(bool viral_remove_empty_parent)
void FrameBase::setTimeStamp(const TimeStamp& _ts)
{
time_stamp_ = _ts;
- if (isKeyOrAux() && getTrajectory() != nullptr)
- getTrajectory()->sortFrame(shared_from_this());
}
void FrameBase::setNonEstimated()
{
// unregister if previously estimated
- if (isKeyOrAux())
+ if (isKey())
for (const auto& sb : getStateBlockVec())
getProblem()->notifyStateBlock(sb, REMOVE);
type_ = NON_ESTIMATED;
- if (getTrajectory())
- {
- getTrajectory()->sortFrame(shared_from_this());
- getTrajectory()->updateLastFrames();
- }
}
-void FrameBase::setKey()
+void FrameBase::setKey(ProblemPtr _prb)
{
- // register if previously not estimated
- if (!isKeyOrAux())
- registerNewStateBlocks(getProblem());
-
+ assert(_prb != nullptr && "setting Key fram with a null problem pointer");
type_ = KEY;
-
- if (getTrajectory())
- {
- getTrajectory()->sortFrame(shared_from_this());
- getTrajectory()->updateLastFrames();
- }
-}
-
-void FrameBase::setAux()
-{
- if (!isKeyOrAux())
- registerNewStateBlocks(getProblem());
-
- type_ = AUXILIARY;
-
- if (getTrajectory())
- {
- getTrajectory()->sortFrame(shared_from_this());
- getTrajectory()->updateLastFrames();
- }
+ this->link(_prb->getTrajectory());
}
bool FrameBase::getCovariance(Eigen::MatrixXd& _cov) const
@@ -189,12 +160,12 @@ FrameBasePtr FrameBase::getPreviousFrame() const
assert(getTrajectory() != nullptr && "This Frame is not linked to any trajectory");
//look for the position of this node in the upper list (frame list of trajectory)
- for (auto f_it = getTrajectory()->getFrameList().rbegin(); f_it != getTrajectory()->getFrameList().rend(); f_it++ )
+ for (auto f_it = getTrajectory()->rbegin(); f_it != getTrajectory()->rend(); f_it++ )
{
if ( this->frame_id_ == (*f_it)->id() )
{
f_it++;
- if (f_it != getTrajectory()->getFrameList().rend())
+ if (f_it != getTrajectory()->rend())
{
return *f_it;
}
@@ -210,11 +181,11 @@ FrameBasePtr FrameBase::getPreviousFrame() const
FrameBasePtr FrameBase::getNextFrame() const
{
//std::cout << "finding next frame of " << this->frame_id_ << std::endl;
- auto f_it = getTrajectory()->getFrameList().rbegin();
+ auto f_it = getTrajectory()->rbegin();
f_it++; //starting from second last frame
//look for the position of this node in the frame list of trajectory
- while (f_it != getTrajectory()->getFrameList().rend())
+ while (f_it != getTrajectory()->rend())
{
if ( this->frame_id_ == (*f_it)->id())
{
@@ -316,6 +287,7 @@ void FrameBase::link(TrajectoryBasePtr _trj_ptr)
{
assert(!is_removing_ && "linking a removed frame");
assert(this->getTrajectory() == nullptr && "linking an already linked frame");
+ assert(isKey() && "Trying to link a non keyframe");
if(_trj_ptr)
{
@@ -344,7 +316,7 @@ void FrameBase::setProblem(ProblemPtr _problem)
void FrameBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
{
- _stream << _tabs << (isKeyOrAux() ? (isKey() ? "KFrm" : "AFrm") : "Frm") << id()
+ _stream << _tabs << (isKey() ? "KFrm" : "Frm") << id()
<< " " << getStructure()
<< " ts = " << std::setprecision(3) << getTimeStamp()
<< ((_depth < 2) ? " -- " + std::to_string(getCaptureList().size()) + "C " : "");
@@ -399,7 +371,7 @@ CheckLog FrameBase::localCheck(bool _verbose, FrameBasePtr _frm_ptr, std::ostrea
std::stringstream inconsistency_explanation;
if (_verbose) {
- _stream << _tabs << (isKeyOrAux() ? (isKey() ? "KFrm" : "EFrm") : "Frm")
+ _stream << _tabs << (isKey() ? "KFrm" : "Frm")
<< id() << " @ " << _frm_ptr.get() << std::endl;
_stream << _tabs << " " << "-> Prb @ " << getProblem().get() << std::endl;
_stream << _tabs << " " << "-> Trj @ " << getTrajectory().get() << std::endl;
@@ -472,9 +444,8 @@ CheckLog FrameBase::localCheck(bool _verbose, FrameBasePtr _frm_ptr, std::ostrea
inconsistency_explanation << "Frm" << id() << " @ " << _frm_ptr
<< " ---> Trj" << " @ " << trj_ptr
<< " -X-> Frm" << id();
- auto trj_frm_list = trj_ptr->getFrameList();
- auto trj_has_frm = std::find_if(trj_frm_list.begin(), trj_frm_list.end(), [&_frm_ptr](FrameBasePtr frm){ return frm == _frm_ptr;});
- log.assertTrue(trj_has_frm != trj_frm_list.end(), inconsistency_explanation);
+ auto trj_has_frm = std::find_if(trj_ptr->begin(), trj_ptr->end(), [&_frm_ptr](FrameBasePtr frm){ return frm == _frm_ptr;});
+ log.assertTrue(trj_has_frm != trj_ptr->end(), inconsistency_explanation);
for(auto C : getCaptureList())
{
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 9e84a825a30ffacf0ffb2e66fbb3670697901cd9..b151a6929ed93ecb4c231ffcd409df89ee3c36d1 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -325,11 +325,10 @@ SensorBasePtr Problem::getSensor(const std::string& _sensor_name) const
return (*sen_it);
}
-FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, //
- const TimeStamp& _time_stamp, //
- const StateStructure& _frame_structure, //
- const SizeEigen _dim, //
- const Eigen::VectorXd& _frame_state)
+FrameBasePtr Problem::emplaceKeyFrame(const TimeStamp& _time_stamp, //
+ const StateStructure& _frame_structure, //
+ const SizeEigen _dim, //
+ const Eigen::VectorXd& _frame_state)
{
VectorComposite state;
SizeEigen index = 0;
@@ -353,66 +352,55 @@ FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, //
assert (_frame_state.size() == index && "State vector incompatible with given structure and dimension! Vector too large.");
- auto frm = FrameBase::emplace<FrameBase>(trajectory_ptr_,
- _frame_key_type,
- _time_stamp,
- _frame_structure,
- state);
+ auto frm = FrameBase::emplaceKeyFrame<FrameBase>(trajectory_ptr_,
+ _time_stamp,
+ _frame_structure,
+ state);
return frm;
}
-FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, //
- const TimeStamp& _time_stamp, //
- const StateStructure& _frame_structure, //
- const SizeEigen _dim)
+FrameBasePtr Problem::emplaceKeyFrame(const TimeStamp& _time_stamp, //
+ const StateStructure& _frame_structure, //
+ const SizeEigen _dim)
{
- return emplaceFrame(_frame_key_type,
- _time_stamp,
- _frame_structure,
- getState(_time_stamp));
+ return emplaceKeyFrame(_time_stamp,
+ _frame_structure,
+ getState(_time_stamp));
}
-FrameBasePtr Problem::emplaceFrame (FrameType _frame_key_type, //
- const TimeStamp& _time_stamp, //
- const StateStructure& _frame_structure, //
- const VectorComposite& _frame_state)
+FrameBasePtr Problem::emplaceKeyFrame (const TimeStamp& _time_stamp, //
+ const StateStructure& _frame_structure, //
+ const VectorComposite& _frame_state)
{
- return FrameBase::emplace<FrameBase>(getTrajectory(),
- _frame_key_type,
- _time_stamp,
- _frame_structure,
- _frame_state);
+ return FrameBase::emplaceKeyFrame<FrameBase>(getTrajectory(),
+ _time_stamp,
+ _frame_structure,
+ _frame_state);
}
-FrameBasePtr Problem::emplaceFrame (FrameType _frame_key_type, //
- const TimeStamp& _time_stamp, //
- const VectorComposite& _frame_state)
+FrameBasePtr Problem::emplaceKeyFrame (const TimeStamp& _time_stamp, //
+ const VectorComposite& _frame_state)
{
- return FrameBase::emplace<FrameBase>(getTrajectory(),
- _frame_key_type,
- _time_stamp,
- getFrameStructure(),
- _frame_state);
+ return FrameBase::emplaceKeyFrame<FrameBase>(getTrajectory(),
+ _time_stamp,
+ getFrameStructure(),
+ _frame_state);
}
-FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, //
- const TimeStamp& _time_stamp, //
- const Eigen::VectorXd& _frame_state)
+FrameBasePtr Problem::emplaceKeyFrame(const TimeStamp& _time_stamp, //
+ const Eigen::VectorXd& _frame_state)
{
- return emplaceFrame(_frame_key_type,
- _time_stamp,
- this->getFrameStructure(),
- this->getDim(),
- _frame_state);
+ return emplaceKeyFrame(_time_stamp,
+ this->getFrameStructure(),
+ this->getDim(),
+ _frame_state);
}
-FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, //
- const TimeStamp& _time_stamp)
+FrameBasePtr Problem::emplaceKeyFrame(const TimeStamp& _time_stamp)
{
- return emplaceFrame(_frame_key_type,
- _time_stamp,
- this->getFrameStructure(),
- this->getDim());
+ return emplaceKeyFrame(_time_stamp,
+ this->getFrameStructure(),
+ this->getDim());
}
TimeStamp Problem::getTimeStamp ( ) const
@@ -427,7 +415,7 @@ TimeStamp Problem::getTimeStamp ( ) const
if (not ts.ok())
{
- const auto& last_kf_or_aux = trajectory_ptr_->getLastKeyOrAuxFrame();
+ const auto& last_kf_or_aux = trajectory_ptr_->getLastFrame();
if (last_kf_or_aux)
ts = last_kf_or_aux->getTimeStamp(); // Use last estimated frame's state
@@ -461,7 +449,7 @@ VectorComposite Problem::getState(const StateStructure& _structure) const
VectorComposite state_last;
- const auto& last_kf_or_aux = trajectory_ptr_->getLastKeyOrAuxFrame();
+ const auto& last_kf_or_aux = trajectory_ptr_->getLastFrame();
if (last_kf_or_aux)
state_last = last_kf_or_aux->getState(structure);
@@ -510,7 +498,7 @@ VectorComposite Problem::getState (const TimeStamp& _ts, const StateStructure& _
VectorComposite state_last;
- const auto& last_kf_or_aux = trajectory_ptr_->closestKeyOrAuxFrameToTimeStamp(_ts);
+ const auto& last_kf_or_aux = trajectory_ptr_->closestFrameToTimeStamp(_ts);
if (last_kf_or_aux)
state_last = last_kf_or_aux->getState(structure);
@@ -871,15 +859,9 @@ bool Problem::getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXd&
return success;
}
-bool Problem::getLastKeyFrameCovariance(Eigen::MatrixXd& cov) const
+bool Problem::getLastFrameCovariance(Eigen::MatrixXd& cov) const
{
- FrameBasePtr frm = getLastKeyFrame();
- return getFrameCovariance(frm, cov);
-}
-
-bool Problem::getLastKeyOrAuxFrameCovariance(Eigen::MatrixXd& cov) const
-{
- FrameBasePtr frm = getLastKeyOrAuxFrame();
+ FrameBasePtr frm = getLastFrame();
return getFrameCovariance(frm, cov);
}
@@ -963,24 +945,9 @@ FrameBasePtr Problem::getLastFrame() const
return trajectory_ptr_->getLastFrame();
}
-FrameBasePtr Problem::getLastKeyFrame() const
-{
- return trajectory_ptr_->getLastKeyFrame();
-}
-
-FrameBasePtr Problem::getLastKeyOrAuxFrame() const
-{
- return trajectory_ptr_->getLastKeyOrAuxFrame();
-}
-
-FrameBasePtr Problem::closestKeyFrameToTimeStamp(const TimeStamp& _ts) const
-{
- return trajectory_ptr_->closestKeyFrameToTimeStamp(_ts);
-}
-
-FrameBasePtr Problem::closestKeyOrAuxFrameToTimeStamp(const TimeStamp& _ts) const
+FrameBasePtr Problem::closestFrameToTimeStamp(const TimeStamp& _ts) const
{
- return trajectory_ptr_->closestKeyOrAuxFrameToTimeStamp(_ts);
+ return trajectory_ptr_->closestFrameToTimeStamp(_ts);
}
void Problem::setPriorOptions(const std::string& _mode,
@@ -1036,9 +1003,8 @@ FrameBasePtr Problem::applyPriorOptions(const TimeStamp& _ts)
if (prior_options_->mode != "nothing" and prior_options_->mode != "")
{
- prior_keyframe = emplaceFrame(KEY,
- _ts,
- prior_options_->state);
+ prior_keyframe = emplaceKeyFrame(_ts,
+ prior_options_->state);
if (prior_options_->mode == "fix")
prior_keyframe->fix();
@@ -1198,9 +1164,9 @@ void Problem::perturb(double amplitude)
S->perturb(amplitude);
// Frames and Captures
- for (auto& F : getTrajectory()->getFrameList())
+ for (auto& F : *(getTrajectory()))
{
- if (F->isKeyOrAux())
+ if (F->isKey())
{
F->perturb(amplitude);
for (auto& C : F->getCaptureList())
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index f831e077a2b1ea698b24438a8c29fe3d7982dbc6..f56b8c80a344e864f16be3ca2f8f006a0996052f 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -34,7 +34,8 @@ ProcessorMotion::ProcessorMotion(const std::string& _type,
origin_ptr_(),
last_ptr_(),
incoming_ptr_(),
- dt_(0.0),
+ last_frame_ptr_(),
+ dt_(0.0),
x_(_state_size),
delta_(_delta_size),
delta_cov_(_delta_cov_size, _delta_cov_size),
@@ -385,7 +386,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// Set the frame of last_ptr as key
auto key_frame = last_ptr_->getFrame();
- key_frame ->setKey();
+ key_frame ->setKey(getProblem());
// create motion feature and add it to the key_capture
auto key_feature = emplaceFeature(last_ptr_);
@@ -394,10 +395,9 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
auto factor = emplaceFactor(key_feature, origin_ptr_);
// create a new frame
- auto frame_new = getProblem()->emplaceFrame(NON_ESTIMATED,
- getTimeStamp(),
- getStateStructure(),
- getProblem()->getState());
+ auto frame_new = FrameBase::createNonKeyFrame<FrameBase>(getTimeStamp(),
+ getStateStructure(),
+ getProblem()->getState());
// create a new capture
auto capture_new = emplaceCapture(frame_new,
getSensor(),
@@ -416,15 +416,18 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// Reset pointers
origin_ptr_ = last_ptr_;
last_ptr_ = capture_new;
+ last_frame_ptr_ = frame_new;
// callback to other processors
getProblem()->keyFrameCallback(key_frame, shared_from_this(), params_motion_->time_tolerance);
+
}
// clear incoming just in case
incoming_ptr_ = nullptr; // This line is not really needed, but it makes things clearer.
postProcess();
+
}
VectorComposite ProcessorMotion::getState(const StateStructure& _structure) const
@@ -622,10 +625,10 @@ VectorComposite ProcessorMotion::getState(const TimeStamp& _ts, const StateStruc
FrameBasePtr ProcessorMotion::setOrigin(const VectorComposite& _x_origin, const TimeStamp& _ts_origin)
{
- FrameBasePtr key_frame_ptr = getProblem()->emplaceFrame(KEY,
- _ts_origin,
- getStateStructure(),
- _x_origin);
+ FrameBasePtr key_frame_ptr = FrameBase::emplaceKeyFrame<FrameBase>(getProblem()->getTrajectory(),
+ _ts_origin,
+ getStateStructure(),
+ _x_origin);
setOrigin(key_frame_ptr);
return key_frame_ptr;
@@ -653,13 +656,12 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
// ---------- LAST ----------
// Make non-key-frame for last Capture
- auto new_frame_ptr = getProblem()->emplaceFrame(NON_ESTIMATED,
- origin_ts,
- getProblem()->getFrameStructure(),
- _origin_frame->getState());
+ last_frame_ptr_ = FrameBase::createNonKeyFrame<FrameBase>(origin_ts,
+ getStateStructure(),
+ _origin_frame->getState());
// emplace (emtpy) last Capture
- last_ptr_ = emplaceCapture(new_frame_ptr,
+ last_ptr_ = emplaceCapture(last_frame_ptr_,
getSensor(),
origin_ts,
Eigen::VectorXd::Zero(data_size_),
@@ -793,6 +795,19 @@ void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr)
CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp& _ts) const
{
assert(_ts.ok());
+ // assert(last_ptr_ != nullptr);
+
+ //Need to uncomment this line so that wolf_ros_apriltag doesn't crash
+ if(last_ptr_ == nullptr) return nullptr;
+
+ // First check if last_ptr is the one we are looking for
+ if (last_ptr_->containsTimeStamp(_ts, this->params_->time_tolerance))
+ return last_ptr_;
+
+
+ // Then look in the Wolf tree...
+ // -----------------------------
+ //
// We need to search in previous keyframes for the capture containing a motion buffer with the queried time stamp
// Note: since the buffer goes from a KF in the past until the next KF, we need to:
@@ -802,8 +817,8 @@ CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp
FrameBasePtr frame = nullptr;
CaptureBasePtr capture = nullptr;
CaptureMotionPtr capture_motion = nullptr;
- for (auto frame_rev_iter = getProblem()->getTrajectory()->getFrameList().rbegin();
- frame_rev_iter != getProblem()->getTrajectory()->getFrameList().rend();
+ for (auto frame_rev_iter = getProblem()->getTrajectory()->rbegin();
+ frame_rev_iter != getProblem()->getTrajectory()->rend();
++frame_rev_iter)
{
frame = *frame_rev_iter;
@@ -921,10 +936,10 @@ void ProcessorMotion::printHeader(int _depth, bool _constr_by, bool _metric, boo
{
_stream << _tabs << "PrcM" << id() << " " << getType() << " \"" << getName() << "\"" << std::endl;
if (getOrigin())
- _stream << _tabs << " " << "o: Cap" << getOrigin()->id() << " - " << (getOrigin()->getFrame()->isKeyOrAux() ? (getOrigin()->getFrame()->isKey() ? " KFrm" : " AFrm" ) : " Frm")
+ _stream << _tabs << " " << "o: Cap" << getOrigin()->id() << " - " << (getOrigin()->getFrame()->isKey() ? " KFrm" : " Frm" )
<< getOrigin()->getFrame()->id() << std::endl;
if (getLast())
- _stream << _tabs << " " << "l: Cap" << getLast()->id() << " - " << (getLast()->getFrame()->isKeyOrAux() ? (getLast()->getFrame()->isKey() ? " KFrm" : " AFrm") : " Frm")
+ _stream << _tabs << " " << "l: Cap" << getLast()->id() << " - " << (getLast()->getFrame()->isKey() ? " KFrm" : " Frm")
<< getLast()->getFrame()->id() << std::endl;
if (getIncoming())
_stream << _tabs << " " << "i: Cap" << getIncoming()->id() << std::endl;
diff --git a/src/processor/processor_odom_3d.cpp b/src/processor/processor_odom_3d.cpp
index d6f7e77a9562e8762f9ab99c9ac525b8fe592c54..c7be4d48c4d086d5d4f594dfa7849419d8cc5c30 100644
--- a/src/processor/processor_odom_3d.cpp
+++ b/src/processor/processor_odom_3d.cpp
@@ -13,7 +13,11 @@ ProcessorOdom3d::ProcessorOdom3d(ParamsProcessorOdom3dPtr _params) :
min_disp_var_ (0.1), // around 10cm error
min_rot_var_ (0.1) // around 6 degrees error
{
- //
+ // Set constant parts of Jacobians
+ jacobian_delta_preint_.setIdentity(6,6);
+ jacobian_delta_.setIdentity(6,6);
+ jacobian_calib_.setZero(6,0);
+ unmeasured_perturbation_cov_ = pow(params_odom_3d_->unmeasured_perturbation_std, 2.0) * Eigen::Matrix6d::Identity();
}
ProcessorOdom3d::~ProcessorOdom3d()
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index 14db03bed63e3ee272f450a15838758108778a4d..91867f39528b54cc7335fcca91e7fcc108757576 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -24,6 +24,7 @@ ProcessorTracker::ProcessorTracker(const std::string& _type,
origin_ptr_(nullptr),
last_ptr_(nullptr),
incoming_ptr_(nullptr),
+ last_frame_ptr_(nullptr),
state_structure_(_structure)
{
//
@@ -80,10 +81,10 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
{
WOLF_DEBUG( "PT ", getName(), " FIRST_TIME_WITHOUT_PACK" );
- FrameBasePtr kfrm = getProblem()->emplaceFrame(KEY,
- incoming_ptr_->getTimeStamp());//,
- //getStateStructure(),
- //getProblem()->getState(getStateStructure()));
+ FrameBasePtr kfrm = FrameBase::emplaceKeyFrame<FrameBase>(getProblem()->getTrajectory(),
+ incoming_ptr_->getTimeStamp(),
+ getStateStructure(),
+ getProblem()->getState(getStateStructure()));
incoming_ptr_->link(kfrm);
// Process info
@@ -113,10 +114,10 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
{
WOLF_DEBUG( "PT ", getName(), " SECOND_TIME_WITHOUT_PACK" );
- FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED,
- incoming_ptr_->getTimeStamp());
+ FrameBasePtr frm = FrameBase::createNonKeyFrame<FrameBase>(incoming_ptr_->getTimeStamp(),
+ getProblem()->getFrameStructure(),
+ getProblem()->getState());
incoming_ptr_->link(frm);
-
// We have a last_ Capture with no features, so we do not process known features, and we do not vote for KF.
// Process info
@@ -130,6 +131,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
resetDerived();
origin_ptr_ = last_ptr_;
last_ptr_ = incoming_ptr_;
+ last_frame_ptr_ = frm;
incoming_ptr_ = nullptr;
break;
@@ -149,8 +151,9 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
last_old_frame->remove();
// Create new frame
- FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED,
- incoming_ptr_->getTimeStamp());
+ FrameBasePtr frm = FrameBase::createNonKeyFrame<FrameBase>(incoming_ptr_->getTimeStamp(),
+ getProblem()->getFrameStructure(),
+ getProblem()->getState());
incoming_ptr_->link(frm);
// Detect new Features, initialize Landmarks, create Factors, ...
@@ -163,6 +166,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
resetDerived();
origin_ptr_ = last_ptr_;
last_ptr_ = incoming_ptr_;
+ last_frame_ptr_ = frm;
incoming_ptr_ = nullptr;
break;
@@ -183,10 +187,10 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
// We create a KF
// set KF on last
last_ptr_->getFrame()->setState(getProblem()->getState(last_ptr_->getTimeStamp()));
- last_ptr_->getFrame()->setKey();
+ last_ptr_->getFrame()->setKey(getProblem());
// // make F; append incoming to new F
- // FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, incoming_ptr_->getTimeStamp());
+ // FrameBasePtr frm = FrameBase::createNonKeyFrame<FrameBase>(incoming_ptr_->getTimeStamp());
// incoming_ptr_->link(frm);
// Establish factors
@@ -199,13 +203,13 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
resetDerived();
// make F; append incoming to new F
- FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED,
- incoming_ptr_->getTimeStamp(),
- last_ptr_->getFrame()->getState());
+ FrameBasePtr frm = FrameBase::createNonKeyFrame<FrameBase>(incoming_ptr_->getTimeStamp(),
+ getProblem()->getFrameStructure(),
+ last_ptr_->getFrame()->getState());
incoming_ptr_->link(frm);
-
origin_ptr_ = last_ptr_;
last_ptr_ = incoming_ptr_;
+ last_frame_ptr_ = frm;
incoming_ptr_ = nullptr;
}
@@ -219,7 +223,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
last_ptr_->getFrame()->setAuxiliary();
// make F; append incoming to new F
- FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, incoming_ptr_->getTimeStamp());
+ FrameBasePtr frm = FrameBase::createNonKeyFrame<FrameBase>(incoming_ptr_->getTimeStamp());
incoming_ptr_->link(frm);
// Establish factors
@@ -248,14 +252,15 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
advanceDerived();
// Replace last frame for a new one in incoming
- FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED,
- incoming_ptr_->getTimeStamp(),
- last_ptr_->getFrame()->getState());
+ FrameBasePtr frm = FrameBase::createNonKeyFrame<FrameBase>(incoming_ptr_->getTimeStamp(),
+ getProblem()->getFrameStructure(),
+ last_ptr_->getFrame()->getState());
incoming_ptr_->link(frm);
last_ptr_->getFrame()->remove(); // implicitly calling last_ptr_->remove();
// Update pointers
last_ptr_ = incoming_ptr_;
+ last_frame_ptr_ = frm;
incoming_ptr_ = nullptr;
}
break;
@@ -337,10 +342,10 @@ void ProcessorTracker::printHeader(int _depth, bool _constr_by, bool _metric, bo
{
_stream << _tabs << "PrcT" << id() << " " << getType() << " \"" << getName() << "\"" << std::endl;
if (getOrigin())
- _stream << _tabs << " " << "o: Cap" << getOrigin()->id() << " - " << (getOrigin()->getFrame()->isKeyOrAux() ? (getOrigin()->getFrame()->isKey() ? " KFrm" : " AFrm") : " Frm")
+ _stream << _tabs << " " << "o: Cap" << getOrigin()->id() << " - " << (getOrigin()->getFrame()->isKey() ? " KFrm" : " Frm")
<< getOrigin()->getFrame()->id() << std::endl;
if (getLast())
- _stream << _tabs << " " << "l: Cap" << getLast()->id() << " - " << (getLast()->getFrame()->isKeyOrAux() ? (getLast()->getFrame()->isKey() ? " KFrm" : " AFrm") : " Frm")
+ _stream << _tabs << " " << "l: Cap" << getLast()->id() << " - " << (getLast()->getFrame()->isKey() ? " KFrm" : " Frm")
<< getLast()->getFrame()->id() << std::endl;
if (getIncoming())
_stream << _tabs << " " << "i: Cap" << getIncoming()->id() << std::endl;
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index b32f02f3dca55c584567b8d84e220a2f2604114b..fefc4d332eca061b041585bf42c6248fb985d992 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -220,9 +220,10 @@ CaptureBasePtr SensorBase::lastCapture(void) const
CaptureBasePtr capture = nullptr;
if (getProblem())
{
- FrameBasePtrList frame_list = getProblem()->getTrajectory()->getFrameList();
- FrameBaseRevIter frame_rev_it = frame_list.rbegin();
- while (frame_rev_it != frame_list.rend())
+ // auto frame_list = getProblem()->getTrajectory()->getFrameMap();
+ auto trajectory = getProblem()->getTrajectory();
+ TrajectoryRevIter frame_rev_it = trajectory->rbegin();
+ while (frame_rev_it != trajectory->rend())
{
capture = (*frame_rev_it)->getCaptureOf(shared_from_this());
if (capture)
@@ -240,9 +241,9 @@ CaptureBasePtr SensorBase::lastKeyCapture(void) const
CaptureBasePtr capture = nullptr;
if (getProblem())
{
- FrameBasePtrList frame_list = getProblem()->getTrajectory()->getFrameList();
- FrameBaseRevIter frame_rev_it = frame_list.rbegin();
- while (frame_rev_it != frame_list.rend())
+ auto trajectory = getProblem()->getTrajectory();
+ TrajectoryRevIter frame_rev_it = trajectory->rbegin();
+ while (frame_rev_it != trajectory->rend())
{
if ((*frame_rev_it)->isKey())
{
@@ -263,11 +264,12 @@ CaptureBasePtr SensorBase::lastCapture(const TimeStamp& _ts) const
CaptureBasePtr capture = nullptr;
if (getProblem())
{
- FrameBasePtrList frame_list = getProblem()->getTrajectory()->getFrameList();
+ // auto frame_list = getProblem()->getTrajectory()->getFrameMap();
+ auto trajectory = getProblem()->getTrajectory();
// We iterate in reverse since we're likely to find it close to the rbegin() place.
- FrameBaseRevIter frame_rev_it = frame_list.rbegin();
- while (frame_rev_it != frame_list.rend())
+ TrajectoryRevIter frame_rev_it = trajectory->rbegin();
+ while (frame_rev_it != trajectory->rend())
{
if ((*frame_rev_it)->getTimeStamp() <= _ts)
{
diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp
index 4a74ca8194f9b2b6111fb69d4c97bb7a4875b60e..60b06fd1f7b0744f8c8a293ff4f35e0e265c9f60 100644
--- a/src/solver/solver_manager.cpp
+++ b/src/solver/solver_manager.cpp
@@ -56,12 +56,24 @@ void SolverManager::update()
// remove
else
- removeStateBlock(sb_notification_map.begin()->first);
+ {
+ if (floating_state_blocks_.count(sb_notification_map.begin()->first) == 1)
+ floating_state_blocks_.erase(sb_notification_map.begin()->first);
+ else
+ removeStateBlock(sb_notification_map.begin()->first);
+ }
// remove notification
sb_notification_map.erase(sb_notification_map.begin());
}
+ // ADD "floating" STATE BLOCKS (last update they weren't involved in any factor)
+ while (!floating_state_blocks_.empty())
+ {
+ addStateBlock(*floating_state_blocks_.begin());
+ floating_state_blocks_.erase(floating_state_blocks_.begin());
+ }
+
// ADD FACTORS
while (!fac_notification_map.empty())
{
@@ -79,6 +91,14 @@ void SolverManager::update()
{
auto state_ptr = state_pair.first;
+ // Check for "floating" state blocks (estimated but not involved in any factor -> not observable problem)
+ if (state_blocks_2_factors_.at(state_ptr).empty())
+ {
+ WOLF_INFO("SolverManager::update(): 'Floating' StateBlock ", state_ptr, " (not involved in any factor) Storing it apart.");
+ floating_state_blocks_.insert(state_ptr);
+ continue;
+ }
+
// state update
if (state_ptr->stateUpdated())
updateStateBlockState(state_ptr);
@@ -92,6 +112,16 @@ void SolverManager::update()
updateStateBlockLocalParametrization(state_ptr);
}
+ // REMOVE "floating" STATE BLOCKS (will be added next update() call)
+ for (auto state_ptr : floating_state_blocks_)
+ {
+ removeStateBlock(state_ptr);
+ // reset flags meaning "solver will handle this change" (state, fix and local param will be set in addStateBlock)
+ state_ptr->resetStateUpdated();
+ state_ptr->resetFixUpdated();
+ state_ptr->resetLocalParamUpdated();
+ }
+
#ifdef _WOLF_DEBUG
assert(check("after update()"));
#endif
@@ -321,14 +351,67 @@ SolverManager::ReportVerbosity SolverManager::getVerbosity() const
return params_->verbose;
}
-bool SolverManager::isStateBlockRegistered(const StateBlockPtr& state_ptr)
+bool SolverManager::isStateBlockRegistered(const StateBlockPtr& state_ptr) const
+{
+ return isStateBlockFloating(state_ptr) or (state_blocks_.count(state_ptr) == 1 and isStateBlockRegisteredDerived(state_ptr));
+}
+
+bool SolverManager::isStateBlockFloating(const StateBlockPtr& state_ptr) const
{
- return state_blocks_.count(state_ptr) ==1 && isStateBlockRegisteredDerived(state_ptr);
+ return floating_state_blocks_.count(state_ptr) == 1;
}
bool SolverManager::isFactorRegistered(const FactorBasePtr& fac_ptr) const
{
- return isFactorRegisteredDerived(fac_ptr);
+ return factors_.count(fac_ptr) and isFactorRegisteredDerived(fac_ptr);
+}
+
+bool SolverManager::isStateBlockFixed(const StateBlockPtr& st)
+{
+ if (!isStateBlockRegistered(st))
+ return false;
+
+ if (isStateBlockFloating(st))
+ return st->isFixed();
+
+ return isStateBlockFixedDerived(st);
+}
+
+bool SolverManager::hasThisLocalParametrization(const StateBlockPtr& st, const LocalParametrizationBasePtr& local_param)
+{
+ if (!isStateBlockRegistered(st))
+ return false;
+
+ if (isStateBlockFloating(st))
+ return st->getLocalParametrization() == local_param;
+
+ return hasThisLocalParametrizationDerived(st, local_param);
+};
+
+bool SolverManager::hasLocalParametrization(const StateBlockPtr& st) const
+{
+ if (!isStateBlockRegistered(st))
+ return false;
+
+ if (isStateBlockFloating(st))
+ return st->hasLocalParametrization();
+
+ return hasLocalParametrizationDerived(st);
+};
+
+int SolverManager::numFactors() const
+{
+ return factors_.size();
+}
+
+int SolverManager::numStateBlocks() const
+{
+ return state_blocks_.size() + floating_state_blocks_.size();
+}
+
+int SolverManager::numStateBlocksFloating() const
+{
+ return floating_state_blocks_.size();
}
double SolverManager::getPeriod() const
@@ -357,15 +440,32 @@ bool SolverManager::check(std::string prefix) const
ok = false;
}
- // factor involving state block in factors_
- for (auto fac : sb_fac_it->second)
+ // no factors involving state block
+ if (sb_fac_it->second.empty())
+ {
+ WOLF_ERROR("SolverManager::check: state block ", sb_fac_it->first, " is in state_blocks_ but not involved in any factor - in ", prefix);
+ ok = false;
+ }
+ else
{
- if (factors_.count(fac) == 0)
+ // factor involving state block in factors_
+ for (auto fac : sb_fac_it->second)
{
- WOLF_ERROR("SolverManager::check: factor ", fac->id(), " (involved in sb ", sb_fac_it->first, ") missing in factors_ map - in ", prefix);
- ok = false;
+ if (factors_.count(fac) == 0)
+ {
+ WOLF_ERROR("SolverManager::check: factor ", fac->id(), " (involved in sb ", sb_fac_it->first, ") missing in factors_ map - in ", prefix);
+ ok = false;
+ }
}
}
+
+ // can't be in both state_blocks_ and floating_state_blocks_
+ if (floating_state_blocks_.count(sb_fac_it->first) == 1)
+ {
+ WOLF_ERROR("SolverManager::check: state block ", sb_fac_it->first, " is both in state_blocks_ and floating_state_blocks_ - in ", prefix);
+ ok = false;
+ }
+
sb_vec_it++;
sb_fac_it++;
}
@@ -384,9 +484,38 @@ bool SolverManager::check(std::string prefix) const
}
}
- // checkDerived
+ // CHECK DERIVED ----------------------
ok &= checkDerived(prefix);
+ // CHECK IF DERIVED IS UP TO DATE ----------------------
+ // state blocks registered in derived solver
+ for (auto sb : state_blocks_)
+ if (!isStateBlockRegisteredDerived(sb.first))
+ {
+ WOLF_ERROR("SolverManager::check: state block ", sb.first, " is in state_blocks_ but not registered in derived solver - in ", prefix);
+ ok = false;
+ }
+
+ // factors registered in derived solver
+ for (auto fac : factors_)
+ if (!isFactorRegisteredDerived(fac))
+ {
+ WOLF_ERROR("SolverManager::check: factor ", fac->id(), " is in factors_ but not registered in derived solver - in ", prefix);
+ ok = false;
+ }
+
+ // numbers
+ if (numStateBlocksDerived() != state_blocks_.size())
+ {
+ WOLF_ERROR("SolverManager::check: numStateBlocksDerived() = ", numStateBlocksDerived(), " DIFFERENT THAN state_blocks_.size() = ", state_blocks_.size(), " - in ", prefix);
+ ok = false;
+ }
+ if (numFactorsDerived() != numFactors())
+ {
+ WOLF_ERROR("SolverManager::check: numFactorsDerived() = ", numFactorsDerived(), " DIFFERENT THAN numFactors() = ", numFactors(), " - in ", prefix);
+ ok = false;
+ }
+
return ok;
}
diff --git a/src/trajectory/trajectory_base.cpp b/src/trajectory/trajectory_base.cpp
index ad760c8c3ccf2942f734c256e7b04ed3d59aecf7..8ab61006c9e46acfdeee6ad735257bdf9d776ff7 100644
--- a/src/trajectory/trajectory_base.cpp
+++ b/src/trajectory/trajectory_base.cpp
@@ -4,11 +4,10 @@
namespace wolf {
TrajectoryBase::TrajectoryBase() :
- NodeBase("TRAJECTORY", "TrajectoryBase"),
- last_key_frame_ptr_(nullptr),
- last_key_or_aux_frame_ptr_(nullptr)
+ NodeBase("TRAJECTORY", "TrajectoryBase")
{
// WOLF_DEBUG("constructed T");
+ frame_map_ = FrameMap();
}
TrajectoryBase::~TrajectoryBase()
@@ -19,16 +18,9 @@ TrajectoryBase::~TrajectoryBase()
FrameBasePtr TrajectoryBase::addFrame(FrameBasePtr _frame_ptr)
{
// add to list
- frame_list_.push_back(_frame_ptr);
+ assert(frame_map_.count(_frame_ptr->getTimeStamp()) == 0 && "Trying to add a keyframe with the same timestamp of an existing one");
- if (_frame_ptr->isKeyOrAux())
- {
- // sort
- sortFrame(_frame_ptr);
-
- // update last_estimated and last_key
- updateLastFrames();
- }
+ frame_map_.emplace(_frame_ptr->getTimeStamp(), _frame_ptr);
return _frame_ptr;
}
@@ -36,114 +28,69 @@ FrameBasePtr TrajectoryBase::addFrame(FrameBasePtr _frame_ptr)
void TrajectoryBase::removeFrame(FrameBasePtr _frame_ptr)
{
// add to list
- frame_list_.remove(_frame_ptr);
-
- // update last_estimated and last_key
- if (_frame_ptr->isKeyOrAux())
- updateLastFrames();
+ // frame_map_.erase(_frame_ptr);
+ frame_map_.erase(_frame_ptr->getTimeStamp());
}
void TrajectoryBase::getFactorList(FactorBasePtrList & _fac_list) const
{
- for(auto fr_ptr : getFrameList())
+ for(auto fr_ptr : *this)
fr_ptr->getFactorList(_fac_list);
}
-void TrajectoryBase::sortFrame(FrameBasePtr _frame_ptr)
-{
- moveFrame(_frame_ptr, computeFrameOrder(_frame_ptr));
-}
-
-void TrajectoryBase::moveFrame(FrameBasePtr _frm_ptr, FrameBaseConstIter _place)
-{
- if (*_place != _frm_ptr)
- {
- frame_list_.remove(_frm_ptr);
- frame_list_.insert(_place, _frm_ptr);
- updateLastFrames();
- }
-}
-
-FrameBaseConstIter TrajectoryBase::computeFrameOrder(FrameBasePtr _frame_ptr)
-{
- for (auto frm_rit = getFrameList().rbegin(); frm_rit != getFrameList().rend(); frm_rit++)
- if ((*frm_rit)!= _frame_ptr && (*frm_rit)->isKeyOrAux() && (*frm_rit)->getTimeStamp() <= _frame_ptr->getTimeStamp())
- return frm_rit.base();
- return getFrameList().begin();
-}
-
-void TrajectoryBase::updateLastFrames()
-{
- bool last_estimated_set = false;
-
- // NOTE: Assumes estimated (key or auxiliary) frames are sorted by timestamp
- for (auto frm_rit = getFrameList().rbegin(); frm_rit != getFrameList().rend(); ++frm_rit)
- if ((*frm_rit)->isKeyOrAux())
- {
- if (!last_estimated_set)
- {
- last_key_or_aux_frame_ptr_ = (*frm_rit);
- last_estimated_set = true;
- }
- if ((*frm_rit)->isKey())
- {
- last_key_frame_ptr_ = (*frm_rit);
- break;
- }
- }
-}
-FrameBasePtr TrajectoryBase::closestKeyFrameToTimeStamp(const TimeStamp& _ts) const
+FrameBasePtr TrajectoryBase::closestFrameToTimeStamp(const TimeStamp& _ts) const
{
- // NOTE: Assumes estimated (key or auxiliary) frames are sorted by timestamp
FrameBasePtr closest_kf = nullptr;
- double min_dt = 1e9;
-
- for (auto frm_rit = frame_list_.rbegin(); frm_rit != frame_list_.rend(); frm_rit++)
- if ((*frm_rit)->isKey())
+ //If frame_map_ is empty then closestFrameToTimeStamp is meaningless
+ if(not frame_map_.empty())
+ {
+ //Let me use shorter names for this explanation: lower_bound -> lb & upper_bound -> ub
+ //In the std they fulfill the following property:
+ // lb is the first element such that ts <= lb, alternatively the smallest element that is NOT less than ts.
+ // lb definition is NOT the ACTUAL lower bound but the following position so, lb = lb_true + 1.
+
+ auto lower_bound = frame_map_.lower_bound(_ts);
+ if((lower_bound != this->end() and lower_bound->first == _ts) or lower_bound == this->begin() ) closest_kf = lower_bound->second;
+ else
{
- double dt = std::fabs((*frm_rit)->getTimeStamp() - _ts);
- if (dt < min_dt)
- {
- min_dt = dt;
- closest_kf = *frm_rit;
- }
+ auto upper_bound = lower_bound;
+ //I find it easier to reason if lb < ts < ub. Remember that we have got rid of the
+ //equality case and the out of bounds cases so this inequality is complete (it is not misssing cases).
+ //Therefore, we need to decrease the lower_bound to the previous element
+ lower_bound = std::prev(lower_bound);
+
+ //If ub points to end() it means that the last frame is still less than _ts, therefore certainly
+ //it will be the closest one
+ if(upper_bound == this->end()) closest_kf = lower_bound->second;
else
- break;
- }
- return closest_kf;
-}
-
-FrameBasePtr TrajectoryBase::closestKeyOrAuxFrameToTimeStamp(const TimeStamp& _ts) const
-{
- // NOTE: Assumes estimated (key or auxiliary) frames are sorted by timestamp
- FrameBasePtr closest_kf = nullptr;
- double min_dt = 1e9;
-
- for (auto frm_rit = frame_list_.rbegin(); frm_rit != frame_list_.rend(); frm_rit++)
- if ((*frm_rit)->isKeyOrAux())
- {
- double dt = std::fabs((*frm_rit)->getTimeStamp() - _ts);
- if (dt < min_dt)
{
- min_dt = dt;
- closest_kf = *frm_rit;
+ //Easy stuff just calculate the distance return minimum
+ auto lb_diff = fabs(lower_bound->first - _ts);
+ auto ub_diff = fabs(upper_bound->first - _ts);
+ if(lb_diff < ub_diff)
+ {
+ closest_kf = lower_bound->second;
+ }
+ else
+ {
+ closest_kf = upper_bound->second;
+ }
}
- else
- break;
}
+ }
return closest_kf;
}
void TrajectoryBase::printHeader(int _depth, bool _metric, bool _state_blocks, bool _constr_by, std::ostream& _stream, std::string _tabs) const
{
- _stream << _tabs << "Trajectory" << ((_depth < 1) ? (" -- " + std::to_string(getFrameList().size()) + "F") : "") << std::endl;
+ _stream << _tabs << "Trajectory" << ((_depth < 1) ? (" -- " + std::to_string(getFrameMap().size()) + "F") : "") << std::endl;
}
void TrajectoryBase::print(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
{
printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
if (_depth >= 1)
- for (auto F : getFrameList())
+ for (auto F : *this)
F->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
}
@@ -169,7 +116,7 @@ bool TrajectoryBase::check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr,
auto trj_ptr = std::static_pointer_cast<TrajectoryBase>(_node_ptr);
auto local_log = localCheck(_verbose, trj_ptr, _stream, _tabs);
_log.compose(local_log);
- for (auto F : getFrameList())
+ for (auto F : *this)
F->check(_log, F, _verbose, _stream, _tabs + " ");
return _log.is_consistent_;
}
diff --git a/src/tree_manager/tree_manager_sliding_window.cpp b/src/tree_manager/tree_manager_sliding_window.cpp
index 5816567557bdd2ca5779615834a3f48a418bbabe..623c7bec91214b55f7d2d8f2034dbba3e01c5ccb 100644
--- a/src/tree_manager/tree_manager_sliding_window.cpp
+++ b/src/tree_manager/tree_manager_sliding_window.cpp
@@ -7,7 +7,7 @@ void TreeManagerSlidingWindow::keyFrameCallback(FrameBasePtr _key_frame)
{
int n_kf(0);
FrameBasePtr first_KF(nullptr), second_KF(nullptr);
- for (auto frm : getProblem()->getTrajectory()->getFrameList())
+ for (auto frm : *getProblem()->getTrajectory())
{
if (frm->isKey())
{
diff --git a/src/yaml/parser_yaml.cpp b/src/yaml/parser_yaml.cpp
index d50e1b53096e248e5b5776243ad1aa831b41b50c..6efc250f44ed45f4da571bb747f8cc9fb875f4f6 100644
--- a/src/yaml/parser_yaml.cpp
+++ b/src/yaml/parser_yaml.cpp
@@ -345,6 +345,12 @@ void ParserYaml::updateActiveName(std::string _tag)
{
active_name_ = _tag;
}
+/*
+** @brief This function is responsible for parsing the first level of the YAML file.
+** The first level here can be thought as the entry point of the YAML file. Since we impose a certain structure
+** this function is responsible for enforcing said structure.
+**
+ */
void ParserYaml::parseFirstLevel(YAML::Node _n, std::string _file)
{
@@ -550,6 +556,9 @@ void ParserYaml::parse()
}
parsing_file_.pop();
}
+/*
+** @brief This function gives the ability to run the parser without enforcing the wolf YAML structure.
+ */
void ParserYaml::parse_freely()
{
parsing_file_.push(generatePath(file_));
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 96656415449da2d8b229f67d11e76529c34764d3..88bcfc88a48286994ec38aee96c3cb44eb25a200 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -28,7 +28,7 @@ include_directories(${GTEST_INCLUDE_DIRS})
# #
# Create a specific test executable for gtest_example #
wolf_add_gtest(gtest_example gtest_example.cpp) #
-target_link_libraries(gtest_example ${PROJECT_NAME}) #
+target_link_libraries(gtest_example ${PLUGIN_NAME}) #
# #
###########################################################
set(SRC_DUMMY
@@ -36,7 +36,7 @@ set(SRC_DUMMY
dummy/processor_tracker_landmark_dummy.cpp
)
add_library(dummy SHARED ${SRC_DUMMY})
-target_link_libraries(dummy ${PROJECT_NAME})
+target_link_libraries(dummy ${PLUGIN_NAME})
################# ADD YOUR TESTS BELOW ####################
# #
# ==== IN ALPHABETICAL ORDER! ==== #
@@ -46,221 +46,219 @@ target_link_libraries(dummy ${PROJECT_NAME})
# CaptureBase class test
wolf_add_gtest(gtest_capture_base gtest_capture_base.cpp)
-target_link_libraries(gtest_capture_base ${PROJECT_NAME})
+target_link_libraries(gtest_capture_base ${PLUGIN_NAME})
# Converter from String to various types used by the parameters server
wolf_add_gtest(gtest_converter gtest_converter.cpp)
-target_link_libraries(gtest_converter ${PROJECT_NAME})
+target_link_libraries(gtest_converter ${PLUGIN_NAME})
# FactorBase class test
wolf_add_gtest(gtest_factor_base gtest_factor_base.cpp)
-target_link_libraries(gtest_factor_base ${PROJECT_NAME})
+target_link_libraries(gtest_factor_base ${PLUGIN_NAME})
# FactorAutodiff class test
wolf_add_gtest(gtest_factor_autodiff gtest_factor_autodiff.cpp)
-target_link_libraries(gtest_factor_autodiff ${PROJECT_NAME})
+target_link_libraries(gtest_factor_autodiff ${PLUGIN_NAME})
# FactoryStateBlock factory test
wolf_add_gtest(gtest_factory_state_block gtest_factory_state_block.cpp)
-target_link_libraries(gtest_factory_state_block ${PROJECT_NAME})
+target_link_libraries(gtest_factory_state_block ${PLUGIN_NAME})
# FeatureBase classes test
wolf_add_gtest(gtest_eigen_predicates gtest_eigen_predicates.cpp)
-target_link_libraries(gtest_eigen_predicates ${PROJECT_NAME})
+target_link_libraries(gtest_eigen_predicates ${PLUGIN_NAME})
# Node Emplace test
wolf_add_gtest(gtest_emplace gtest_emplace.cpp)
-target_link_libraries(gtest_emplace ${PROJECT_NAME} dummy)
+target_link_libraries(gtest_emplace ${PLUGIN_NAME} dummy)
# FeatureBase classes test
wolf_add_gtest(gtest_feature_base gtest_feature_base.cpp)
-target_link_libraries(gtest_feature_base ${PROJECT_NAME})
+target_link_libraries(gtest_feature_base ${PLUGIN_NAME})
# FrameBase classes test
wolf_add_gtest(gtest_frame_base gtest_frame_base.cpp)
-target_link_libraries(gtest_frame_base ${PROJECT_NAME})
+target_link_libraries(gtest_frame_base ${PLUGIN_NAME})
# HasStateBlocks classes test
wolf_add_gtest(gtest_has_state_blocks gtest_has_state_blocks.cpp)
-target_link_libraries(gtest_has_state_blocks ${PROJECT_NAME})
+target_link_libraries(gtest_has_state_blocks ${PLUGIN_NAME})
# LocalParametrizationXxx classes test
wolf_add_gtest(gtest_local_param gtest_local_param.cpp)
-target_link_libraries(gtest_local_param ${PROJECT_NAME})
+target_link_libraries(gtest_local_param ${PLUGIN_NAME})
# Logging test
wolf_add_gtest(gtest_logging gtest_logging.cpp)
-target_link_libraries(gtest_logging ${PROJECT_NAME})
+target_link_libraries(gtest_logging ${PLUGIN_NAME})
# MotionBuffer class test
wolf_add_gtest(gtest_motion_buffer gtest_motion_buffer.cpp)
-target_link_libraries(gtest_motion_buffer ${PROJECT_NAME})
+target_link_libraries(gtest_motion_buffer ${PLUGIN_NAME})
# PackKFBuffer
wolf_add_gtest(gtest_pack_KF_buffer gtest_pack_KF_buffer.cpp)
-target_link_libraries(gtest_pack_KF_buffer ${PROJECT_NAME} dummy)
+target_link_libraries(gtest_pack_KF_buffer ${PLUGIN_NAME} dummy)
# Parameters server
wolf_add_gtest(gtest_param_server gtest_param_server.cpp ${CMAKE_CURRENT_LIST_DIR})
-target_link_libraries(gtest_param_server ${PROJECT_NAME})
+target_link_libraries(gtest_param_server ${PLUGIN_NAME})
# YAML parser
wolf_add_gtest(gtest_parser_yaml gtest_parser_yaml.cpp)
-target_link_libraries(gtest_parser_yaml ${PROJECT_NAME})
+target_link_libraries(gtest_parser_yaml ${PLUGIN_NAME})
# Problem class test
wolf_add_gtest(gtest_problem gtest_problem.cpp)
-target_link_libraries(gtest_problem ${PROJECT_NAME} dummy)
+target_link_libraries(gtest_problem ${PLUGIN_NAME} dummy)
# ProcessorBase class test
wolf_add_gtest(gtest_processor_base gtest_processor_base.cpp)
-target_link_libraries(gtest_processor_base ${PROJECT_NAME} dummy)
+target_link_libraries(gtest_processor_base ${PLUGIN_NAME} dummy)
# ProcessorMotion class test
wolf_add_gtest(gtest_processor_motion gtest_processor_motion.cpp)
-target_link_libraries(gtest_processor_motion ${PROJECT_NAME})
+target_link_libraries(gtest_processor_motion ${PLUGIN_NAME})
# Rotation test
wolf_add_gtest(gtest_rotation gtest_rotation.cpp)
# SE3 test
wolf_add_gtest(gtest_SE3 gtest_SE3.cpp)
-target_link_libraries(gtest_SE3 ${PROJECT_NAME})
+target_link_libraries(gtest_SE3 ${PLUGIN_NAME})
# SensorBase test
wolf_add_gtest(gtest_sensor_base gtest_sensor_base.cpp)
-target_link_libraries(gtest_sensor_base ${PROJECT_NAME})
+target_link_libraries(gtest_sensor_base ${PLUGIN_NAME})
# shared_from_this test
wolf_add_gtest(gtest_shared_from_this gtest_shared_from_this.cpp)
-target_link_libraries(gtest_shared_from_this ${PROJECT_NAME})
+target_link_libraries(gtest_shared_from_this ${PLUGIN_NAME})
# SolverManager test
wolf_add_gtest(gtest_solver_manager gtest_solver_manager.cpp)
-target_link_libraries(gtest_solver_manager ${PROJECT_NAME})
+target_link_libraries(gtest_solver_manager ${PLUGIN_NAME})
# StateBlock class test
wolf_add_gtest(gtest_state_block gtest_state_block.cpp)
-target_link_libraries(gtest_state_block ${PROJECT_NAME})
+target_link_libraries(gtest_state_block ${PLUGIN_NAME})
# StateBlock class test
wolf_add_gtest(gtest_state_composite gtest_state_composite.cpp)
-target_link_libraries(gtest_state_composite ${PROJECT_NAME})
+target_link_libraries(gtest_state_composite ${PLUGIN_NAME})
# TimeStamp class test
wolf_add_gtest(gtest_time_stamp gtest_time_stamp.cpp)
-target_link_libraries(gtest_time_stamp ${PROJECT_NAME})
+target_link_libraries(gtest_time_stamp ${PLUGIN_NAME})
# TrackMatrix class test
wolf_add_gtest(gtest_track_matrix gtest_track_matrix.cpp)
-target_link_libraries(gtest_track_matrix ${PROJECT_NAME})
+target_link_libraries(gtest_track_matrix ${PLUGIN_NAME})
# TrajectoryBase class test
wolf_add_gtest(gtest_trajectory gtest_trajectory.cpp)
-target_link_libraries(gtest_trajectory ${PROJECT_NAME})
+target_link_libraries(gtest_trajectory ${PLUGIN_NAME})
# TreeManager class test
wolf_add_gtest(gtest_tree_manager gtest_tree_manager.cpp)
-target_link_libraries(gtest_tree_manager ${PROJECT_NAME})
+target_link_libraries(gtest_tree_manager ${PLUGIN_NAME})
# ------- Now Derived classes ----------
# FactorAbs(P/O/V) classes test
wolf_add_gtest(gtest_factor_absolute gtest_factor_absolute.cpp)
-target_link_libraries(gtest_factor_absolute ${PROJECT_NAME})
+target_link_libraries(gtest_factor_absolute ${PLUGIN_NAME})
# FactorAutodiffDistance3d test
wolf_add_gtest(gtest_factor_autodiff_distance_3d gtest_factor_autodiff_distance_3d.cpp)
-target_link_libraries(gtest_factor_autodiff_distance_3d ${PROJECT_NAME})
+target_link_libraries(gtest_factor_autodiff_distance_3d ${PLUGIN_NAME})
# FactorBlockDifference class test
wolf_add_gtest(gtest_factor_block_difference gtest_factor_block_difference.cpp)
-target_link_libraries(gtest_factor_block_difference ${PROJECT_NAME})
+target_link_libraries(gtest_factor_block_difference ${PLUGIN_NAME})
# FactorOdom3d class test
wolf_add_gtest(gtest_factor_diff_drive gtest_factor_diff_drive.cpp)
-target_link_libraries(gtest_factor_diff_drive ${PROJECT_NAME})
+target_link_libraries(gtest_factor_diff_drive ${PLUGIN_NAME})
# FactorOdom2d class test
wolf_add_gtest(gtest_factor_odom_2d gtest_factor_odom_2d.cpp)
-target_link_libraries(gtest_factor_odom_2d ${PROJECT_NAME})
+target_link_libraries(gtest_factor_odom_2d ${PLUGIN_NAME})
# FactorOdom3d class test
wolf_add_gtest(gtest_factor_odom_3d gtest_factor_odom_3d.cpp)
-target_link_libraries(gtest_factor_odom_3d ${PROJECT_NAME})
+target_link_libraries(gtest_factor_odom_3d ${PLUGIN_NAME})
# FactorPose2d class test
wolf_add_gtest(gtest_factor_pose_2d gtest_factor_pose_2d.cpp)
-target_link_libraries(gtest_factor_pose_2d ${PROJECT_NAME})
+target_link_libraries(gtest_factor_pose_2d ${PLUGIN_NAME})
# FactorPose3d class test
wolf_add_gtest(gtest_factor_pose_3d gtest_factor_pose_3d.cpp)
-target_link_libraries(gtest_factor_pose_3d ${PROJECT_NAME})
+target_link_libraries(gtest_factor_pose_3d ${PLUGIN_NAME})
# FactorRelativePose2dWithExtrinsics class test
wolf_add_gtest(gtest_factor_relative_pose_2d_with_extrinsics gtest_factor_relative_pose_2d_with_extrinsics.cpp)
-target_link_libraries(gtest_factor_relative_pose_2d_with_extrinsics ${PROJECT_NAME})
+target_link_libraries(gtest_factor_relative_pose_2d_with_extrinsics ${PLUGIN_NAME})
# FactorRelative2dAnalytic class test
wolf_add_gtest(gtest_factor_relative_2d_analytic gtest_factor_relative_2d_analytic.cpp)
-target_link_libraries(gtest_factor_relative_2d_analytic ${PROJECT_NAME})
+target_link_libraries(gtest_factor_relative_2d_analytic ${PLUGIN_NAME})
# MakePosDef function test
wolf_add_gtest(gtest_make_posdef gtest_make_posdef.cpp)
-target_link_libraries(gtest_make_posdef ${PROJECT_NAME})
+target_link_libraries(gtest_make_posdef ${PLUGIN_NAME})
# Map yaml save/load test
wolf_add_gtest(gtest_map_yaml gtest_map_yaml.cpp)
-target_link_libraries(gtest_map_yaml ${PROJECT_NAME})
+target_link_libraries(gtest_map_yaml ${PLUGIN_NAME})
# Parameter prior test
wolf_add_gtest(gtest_param_prior gtest_param_prior.cpp)
-target_link_libraries(gtest_param_prior ${PROJECT_NAME})
+target_link_libraries(gtest_param_prior ${PLUGIN_NAME})
# ProcessorDiffDriveSelfcalib class test
wolf_add_gtest(gtest_processor_diff_drive gtest_processor_diff_drive.cpp)
-target_link_libraries(gtest_processor_diff_drive ${PROJECT_NAME})
+target_link_libraries(gtest_processor_diff_drive ${PLUGIN_NAME})
# ProcessorLoopClosureBase class test
wolf_add_gtest(gtest_processor_loopclosure gtest_processor_loopclosure.cpp)
-target_link_libraries(gtest_processor_loopclosure ${PROJECT_NAME})
+target_link_libraries(gtest_processor_loopclosure ${PLUGIN_NAME})
# ProcessorFrameNearestNeighborFilter class test
# wolf_add_gtest(gtest_processor_frame_nearest_neighbor_filter_2d gtest_processor_frame_nearest_neighbor_filter_2d.cpp)
-# target_link_libraries(gtest_processor_frame_nearest_neighbor_filter_2d ${PROJECT_NAME})
+# target_link_libraries(gtest_processor_frame_nearest_neighbor_filter_2d ${PLUGIN_NAME})
# ProcessorMotion in 2d
wolf_add_gtest(gtest_odom_2d gtest_odom_2d.cpp)
-target_link_libraries(gtest_odom_2d ${PROJECT_NAME})
+target_link_libraries(gtest_odom_2d ${PLUGIN_NAME})
# ProcessorOdom3d class test
wolf_add_gtest(gtest_processor_odom_3d gtest_processor_odom_3d.cpp)
-target_link_libraries(gtest_processor_odom_3d ${PROJECT_NAME})
+target_link_libraries(gtest_processor_odom_3d ${PLUGIN_NAME})
# ProcessorTrackerFeatureDummy class test
wolf_add_gtest(gtest_processor_tracker_feature_dummy gtest_processor_tracker_feature_dummy.cpp)
-target_link_libraries(gtest_processor_tracker_feature_dummy ${PROJECT_NAME} dummy)
+target_link_libraries(gtest_processor_tracker_feature_dummy ${PLUGIN_NAME} dummy)
# ProcessorTrackerLandmarkDummy class test
wolf_add_gtest(gtest_processor_tracker_landmark_dummy gtest_processor_tracker_landmark_dummy.cpp)
-target_link_libraries(gtest_processor_tracker_landmark_dummy ${PROJECT_NAME} dummy)
+target_link_libraries(gtest_processor_tracker_landmark_dummy ${PLUGIN_NAME} dummy)
# SensorDiffDriveSelfcalib class test
wolf_add_gtest(gtest_sensor_diff_drive gtest_sensor_diff_drive.cpp)
-target_link_libraries(gtest_sensor_diff_drive ${PROJECT_NAME})
+target_link_libraries(gtest_sensor_diff_drive ${PLUGIN_NAME})
IF (Ceres_FOUND)
# SolverCeres test
wolf_add_gtest(gtest_solver_ceres gtest_solver_ceres.cpp)
-target_link_libraries(gtest_solver_ceres ${PROJECT_NAME})
+target_link_libraries(gtest_solver_ceres ${PLUGIN_NAME})
ENDIF(Ceres_FOUND)
# TreeManagerSlidingWindow class test
wolf_add_gtest(gtest_tree_manager_sliding_window gtest_tree_manager_sliding_window.cpp)
-target_link_libraries(gtest_tree_manager_sliding_window ${PROJECT_NAME})
+target_link_libraries(gtest_tree_manager_sliding_window ${PLUGIN_NAME})
# yaml conversions
wolf_add_gtest(gtest_yaml_conversions gtest_yaml_conversions.cpp)
-target_link_libraries(gtest_yaml_conversions ${PROJECT_NAME})
-
-
+target_link_libraries(gtest_yaml_conversions ${PLUGIN_NAME})
diff --git a/test/dummy/solver_manager_dummy.h b/test/dummy/solver_manager_dummy.h
index 15db720a3c42f8151dd8745dc35d0232fc0fe885..5faef11f35874747a5f0b98f8d99b42911d116e1 100644
--- a/test/dummy/solver_manager_dummy.h
+++ b/test/dummy/solver_manager_dummy.h
@@ -17,7 +17,7 @@ WOLF_PTR_TYPEDEFS(SolverManagerDummy);
class SolverManagerDummy : public SolverManager
{
public:
- std::list<FactorBasePtr> factors_;
+ std::set<FactorBasePtr> factors_derived_;
std::map<StateBlockPtr,bool> state_block_fixed_;
std::map<StateBlockPtr,LocalParametrizationBasePtr> state_block_local_param_;
@@ -26,35 +26,34 @@ class SolverManagerDummy : public SolverManager
{
};
- bool isStateBlockRegistered(const StateBlockPtr& st) override
+ bool isStateBlockFixedDerived(const StateBlockPtr& st) override
{
- return state_blocks_.find(st)!=state_blocks_.end();
+ return state_block_fixed_.at(st);
};
- bool isStateBlockFixed(const StateBlockPtr& st) const
+ bool hasThisLocalParametrizationDerived(const StateBlockPtr& st, const LocalParametrizationBasePtr& local_param) override
{
- return state_block_fixed_.at(st);
+ return state_block_local_param_.at(st) == local_param;
};
- bool isFactorRegistered(const FactorBasePtr& fac_ptr) const override
+ bool hasLocalParametrizationDerived(const StateBlockPtr& st) const override
{
- return std::find(factors_.begin(), factors_.end(), fac_ptr) != factors_.end();
+ return state_block_local_param_.at(st) != nullptr;
};
- bool hasThisLocalParametrization(const StateBlockPtr& st, const LocalParametrizationBasePtr& local_param) const
+ virtual int numStateBlocksDerived() const override
{
- return state_block_local_param_.find(st) != state_block_local_param_.end() && state_block_local_param_.at(st) == local_param;
- };
+ return state_block_fixed_.size();
+ }
- bool hasLocalParametrization(const StateBlockPtr& st) const
+ virtual int numFactorsDerived() const override
{
- return state_block_local_param_.find(st) != state_block_local_param_.end();
+ return factors_derived_.size();
};
void computeCovariances(const CovarianceBlocksToBeComputed blocks) override {};
void computeCovariances(const std::vector<StateBlockPtr>& st_list) override {};
- bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) override {return true;};
- bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const override {return true;};
+
// The following are dummy implementations
bool hasConverged() override { return true; }
@@ -62,19 +61,17 @@ class SolverManagerDummy : public SolverManager
double initialCost() override { return double(1); }
double finalCost() override { return double(0); }
-
-
protected:
bool checkDerived(std::string prefix="") const override {return true;}
std::string solveDerived(const ReportVerbosity report_level) override { return std::string("");};
void addFactorDerived(const FactorBasePtr& fac_ptr) override
{
- factors_.push_back(fac_ptr);
+ factors_derived_.insert(fac_ptr);
};
void removeFactorDerived(const FactorBasePtr& fac_ptr) override
{
- factors_.remove(fac_ptr);
+ factors_derived_.erase(fac_ptr);
};
void addStateBlockDerived(const StateBlockPtr& state_ptr) override
{
@@ -92,10 +89,16 @@ class SolverManagerDummy : public SolverManager
};
void updateStateBlockLocalParametrizationDerived(const StateBlockPtr& state_ptr) override
{
- if (state_ptr->getLocalParametrization() == nullptr)
- state_block_local_param_.erase(state_ptr);
- else
- state_block_local_param_[state_ptr] = state_ptr->getLocalParametrization();
+ state_block_local_param_[state_ptr] = state_ptr->getLocalParametrization();
+ };
+ bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) const override
+ {
+ return state_block_fixed_.count(state_ptr) == 1 and state_block_local_param_.count(state_ptr) == 1;
+ };
+
+ bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const override
+ {
+ return factors_derived_.count(fac_ptr) == 1;
};
};
diff --git a/test/gtest_emplace.cpp b/test/gtest_emplace.cpp
index 2c28c2bd442bc8675bf5e71fb82d97141cac8390..ebca6bb09bf463ed022aa81a1629e0accc877ff8 100644
--- a/test/gtest_emplace.cpp
+++ b/test/gtest_emplace.cpp
@@ -42,8 +42,8 @@ TEST(Emplace, Frame)
ProblemPtr P = Problem::create("POV", 3);
ASSERT_NE(P->getTrajectory(), nullptr);
- FrameBase::emplace<FrameBase>(P->getTrajectory(), KEY, TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
- ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getTrajectory()->getProblem());
+ FrameBase::emplaceKeyFrame<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
+ ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getTrajectory()->getProblem());
}
TEST(Emplace, Processor)
@@ -67,12 +67,12 @@ TEST(Emplace, Capture)
ProblemPtr P = Problem::create("POV", 3);
ASSERT_NE(P->getTrajectory(), nullptr);
- auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), KEY, TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
- ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getTrajectory()->getProblem());
+ auto frm = FrameBase::emplaceKeyFrame<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
+ ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getTrajectory()->getProblem());
auto cpt = CaptureBase::emplace<CaptureBase>(frm, "Dummy", TimeStamp(0), nullptr, nullptr, nullptr, nullptr);
- ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFrame()->getTrajectory()->getProblem());
- ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getProblem());
+ ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getCaptureList().front()->getFrame()->getTrajectory()->getProblem());
+ ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getCaptureList().front()->getProblem());
ASSERT_EQ(frm, frm->getCaptureList().front()->getFrame());
}
@@ -81,17 +81,17 @@ TEST(Emplace, Feature)
ProblemPtr P = Problem::create("POV", 3);
ASSERT_NE(P->getTrajectory(), nullptr);
- auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), KEY, TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
- ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getTrajectory()->getProblem());
+ auto frm = FrameBase::emplaceKeyFrame<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
+ ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getTrajectory()->getProblem());
auto cpt = CaptureBase::emplace<CaptureBase>(frm, "Dummy", TimeStamp(0), nullptr, nullptr, nullptr, nullptr);
- ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFrame()->getTrajectory()->getProblem());
- ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getProblem());
+ ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getCaptureList().front()->getFrame()->getTrajectory()->getProblem());
+ ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getCaptureList().front()->getProblem());
ASSERT_EQ(frm, frm->getCaptureList().front()->getFrame());
auto cov = Eigen::MatrixXd::Identity(2,2);
FeatureBase::emplace<FeatureBase>(cpt, "Dummy", Eigen::VectorXd(2), cov);
- ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFeatureList().front()->getCapture()->getFrame()->getTrajectory()->getProblem());
- ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFeatureList().front()->getProblem());
+ ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getCaptureList().front()->getFeatureList().front()->getCapture()->getFrame()->getTrajectory()->getProblem());
+ ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getCaptureList().front()->getFeatureList().front()->getProblem());
ASSERT_EQ(cpt, cpt->getFeatureList().front()->getCapture());
}
TEST(Emplace, Factor)
@@ -99,17 +99,17 @@ TEST(Emplace, Factor)
ProblemPtr P = Problem::create("POV", 3);
ASSERT_NE(P->getTrajectory(), nullptr);
- auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), KEY, TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
- ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getTrajectory()->getProblem());
+ auto frm = FrameBase::emplaceKeyFrame<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
+ ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getTrajectory()->getProblem());
auto cpt = CaptureBase::emplace<CaptureBase>(frm, "Dummy", TimeStamp(0), nullptr, nullptr, nullptr, nullptr);
- ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFrame()->getTrajectory()->getProblem());
- ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getProblem());
+ ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getCaptureList().front()->getFrame()->getTrajectory()->getProblem());
+ ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getCaptureList().front()->getProblem());
ASSERT_EQ(frm, frm->getCaptureList().front()->getFrame());
auto cov = Eigen::MatrixXd::Identity(2,2);
auto ftr = FeatureBase::emplace<FeatureOdom2d>(cpt, Eigen::VectorXd(2), cov);
- ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFeatureList().front()->getCapture()->getFrame()->getTrajectory()->getProblem());
- ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFeatureList().front()->getProblem());
+ ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getCaptureList().front()->getFeatureList().front()->getCapture()->getFrame()->getTrajectory()->getProblem());
+ ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getCaptureList().front()->getFeatureList().front()->getProblem());
ASSERT_EQ(cpt, cpt->getFeatureList().front()->getCapture());
auto cnt = FactorBase::emplace<FactorOdom2d>(ftr, ftr, frm, nullptr, false);
ASSERT_NE(nullptr, ftr->getFactorList().front().get());
@@ -119,7 +119,7 @@ TEST(Emplace, EmplaceDerived)
{
ProblemPtr P = Problem::create("POV", 3);
- auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), KEY, TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
+ auto frm = FrameBase::emplaceKeyFrame<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
auto sen = SensorBase::emplace<SensorOdom2d>(P->getHardware(), Eigen::VectorXd(3), ParamsSensorOdom2d());
auto cov = Eigen::MatrixXd::Identity(2,2);
auto cpt = CaptureBase::emplace<CaptureOdom2d>(frm, TimeStamp(0), sen, Eigen::VectorXd(2), cov, nullptr);
@@ -130,7 +130,7 @@ TEST(Emplace, EmplaceDerived)
auto ftr = FeatureBase::emplace<FeatureOdom2d>(cpt, Eigen::VectorXd(2), cov);
ASSERT_EQ(sen, P->getHardware()->getSensorList().front());
- ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFeatureList().front()->getProblem());
+ ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getCaptureList().front()->getFeatureList().front()->getProblem());
}
TEST(Emplace, Nullpointer)
{
@@ -141,7 +141,7 @@ TEST(Emplace, ReturnDerived)
ProblemPtr P = Problem::create("POV", 3);
ASSERT_NE(P->getTrajectory(), nullptr);
- auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), KEY, TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
+ auto frm = FrameBase::emplaceKeyFrame<FrameBase>(P->getTrajectory(), TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
auto cpt = CaptureBase::emplace<CaptureBase>(frm, "Dummy", TimeStamp(0), nullptr, nullptr, nullptr, nullptr);
auto cov = Eigen::MatrixXd::Identity(2,2);
auto ftr = FeatureBase::emplace<FeatureOdom2d>(cpt, Eigen::VectorXd(2), cov);
diff --git a/test/gtest_factor_absolute.cpp b/test/gtest_factor_absolute.cpp
index 9e19dce8b5fad3d950461a74f13d4226036c704f..8c3b353ccb9493be57f2d38a6ffe257a6375f3b8 100644
--- a/test/gtest_factor_absolute.cpp
+++ b/test/gtest_factor_absolute.cpp
@@ -35,7 +35,7 @@ ProblemPtr problem_ptr = Problem::create("POV", 3);
SolverCeres solver(problem_ptr);
// Two frames
-FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, 0, problem_ptr->stateZero() );
+FrameBasePtr frm0 = problem_ptr->emplaceKeyFrame(0, problem_ptr->stateZero() );
// Capture
// auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "IMU ABS", 0, nullptr, pose10, 10, 9, nullptr);
diff --git a/test/gtest_factor_autodiff_distance_3d.cpp b/test/gtest_factor_autodiff_distance_3d.cpp
index c8f33528b9320d01e281add3bcaf0a74420ec35d..3e6974cb91303c8f864db2816d801207eba309ad 100644
--- a/test/gtest_factor_autodiff_distance_3d.cpp
+++ b/test/gtest_factor_autodiff_distance_3d.cpp
@@ -55,9 +55,9 @@ class FactorAutodiffDistance3d_Test : public testing::Test
problem = Problem::create("PO", 3);
solver = std::make_shared<SolverCeres>(problem);
- F1 = problem->emplaceFrame (KEY, 1.0, pose1);
+ F1 = problem->emplaceKeyFrame (1.0, pose1);
- F2 = problem->emplaceFrame (KEY, 2.0, pose2);
+ F2 = problem->emplaceKeyFrame (2.0, pose2);
C2 = CaptureBase::emplace<CaptureBase>(F2, "Base", 1.0);
}
diff --git a/test/gtest_factor_block_difference.cpp b/test/gtest_factor_block_difference.cpp
index 2b7c9cb0c3ff6deb8ae9ec0b2279963d46a6a090..9b572504fa686bd555ba3b5ec25f39d1a4fb7afd 100644
--- a/test/gtest_factor_block_difference.cpp
+++ b/test/gtest_factor_block_difference.cpp
@@ -58,7 +58,7 @@ class FixtureFactorBlockDifference : public testing::Test
//FeatureBasePtr featV0 = FeatureBase::emplace<FeatureBase>(capV0, "Vel0", x_origin.tail(3), cov_prior.bottomRightCorner<3,3>());
//FactorBasePtr facV0 = FactorBase::emplace<FactorBlockAbsolute>(featV0, KF0_->getV(), nullptr, false);
- KF1_ = problem_->emplaceFrame(KEY, t1, problem_->stateZero());
+ KF1_ = problem_->emplaceKeyFrame(t1, problem_->stateZero());
Cap_ = CaptureBase::emplace<CaptureBase>(KF1_, "BlockDifference", t1);
}
diff --git a/test/gtest_factor_diff_drive.cpp b/test/gtest_factor_diff_drive.cpp
index c8338be45a4388c282da32037a27e5b4530e3f02..51661f8ab26d1257f43dd4d4cbcc8dd5237b80d8 100644
--- a/test/gtest_factor_diff_drive.cpp
+++ b/test/gtest_factor_diff_drive.cpp
@@ -159,13 +159,11 @@ class FactorDiffDriveTest : public testing::Test
processor = std::static_pointer_cast<ProcessorDiffDrivePublic>(prc);
// frames
- F0 = FrameBase::emplace<FrameBase>(trajectory,
- KEY,
+ F0 = FrameBase::emplaceKeyFrame<FrameBase>(trajectory,
0.0,
"PO",
std::list<VectorXd>{Vector2d(0,0), Vector1d(0)});
- F1 = FrameBase::emplace<FrameBase>(trajectory,
- KEY,
+ F1 = FrameBase::emplaceKeyFrame<FrameBase>(trajectory,
1.0,
"PO",
std::list<VectorXd>{Vector2d(1,0), Vector1d(0)});
@@ -500,7 +498,7 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt)
C->process();
}
- auto F2 = problem->getLastKeyFrame();
+ auto F2 = problem->getLastFrame();
// Fix boundaries
F0->fix();
@@ -636,7 +634,7 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
C->process();
}
- auto F2 = problem->getLastKeyFrame();
+ auto F2 = problem->getLastFrame();
// VectorComposite x2c(x2, "PO", {2,1});
F2->setState(x2, "PO"); // Impose known final state regardless of integrated value.
diff --git a/test/gtest_factor_odom_2d.cpp b/test/gtest_factor_odom_2d.cpp
index 6230881af471e41fc187ddab8757845471311b50..baee1dddcaf2c1fb998e3ae551f29a39a335fd5b 100644
--- a/test/gtest_factor_odom_2d.cpp
+++ b/test/gtest_factor_odom_2d.cpp
@@ -19,8 +19,8 @@ ProblemPtr problem_ptr = Problem::create("PO", 2);
SolverCeres solver(problem_ptr);
// Two frames
-FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, TimeStamp(0), Vector3d::Zero());
-FrameBasePtr frm1 = problem_ptr->emplaceFrame(KEY, TimeStamp(1), Vector3d::Zero());
+FrameBasePtr frm0 = problem_ptr->emplaceKeyFrame(TimeStamp(0), Vector3d::Zero());
+FrameBasePtr frm1 = problem_ptr->emplaceKeyFrame(TimeStamp(1), Vector3d::Zero());
// Capture from frm1 to frm0
auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, nullptr, Vector3d::Zero(), data_cov);
diff --git a/test/gtest_factor_odom_3d.cpp b/test/gtest_factor_odom_3d.cpp
index 3ac18c88700d5d4169b6b9c6f59b607780b897a1..24daac332094bb8e65964eb0acdd9346341eb3f9 100644
--- a/test/gtest_factor_odom_3d.cpp
+++ b/test/gtest_factor_odom_3d.cpp
@@ -37,8 +37,8 @@ ProblemPtr problem_ptr = Problem::create("PO", 3);
SolverCeres solver(problem_ptr);
// Two frames
-FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, TimeStamp(0), problem_ptr->stateZero());
-FrameBasePtr frm1 = problem_ptr->emplaceFrame(KEY, TimeStamp(1), delta);
+FrameBasePtr frm0 = problem_ptr->emplaceKeyFrame(TimeStamp(0), problem_ptr->stateZero());
+FrameBasePtr frm1 = problem_ptr->emplaceKeyFrame(TimeStamp(1), delta);
// Capture, feature and factor from frm1 to frm0
auto cap1 = CaptureBase::emplace<CaptureMotion>(frm1, "CaptureOdom3d", 1, nullptr, delta, nullptr);
diff --git a/test/gtest_factor_pose_2d.cpp b/test/gtest_factor_pose_2d.cpp
index 6f9804f9d16518d1eaa349a1359a1f4da153cf3f..749d514d8cc4c0479cfdf5092dca6e5face76ea8 100644
--- a/test/gtest_factor_pose_2d.cpp
+++ b/test/gtest_factor_pose_2d.cpp
@@ -30,7 +30,7 @@ ProblemPtr problem = Problem::create("PO", 2);
SolverCeres solver(problem);
// Two frames
-FrameBasePtr frm0 = problem->emplaceFrame(KEY, TimeStamp(0), problem->stateZero());
+FrameBasePtr frm0 = problem->emplaceKeyFrame(TimeStamp(0), problem->stateZero());
// Capture, feature and factor from frm1 to frm0
auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "CaptureOdom2d", 0, nullptr, pose, nullptr);
diff --git a/test/gtest_factor_pose_3d.cpp b/test/gtest_factor_pose_3d.cpp
index d50660e2f0515aa5589d8fb0146e7ca75f8cfa84..88433b4b4433ce7e5290c81b34766d13fb2f9831 100644
--- a/test/gtest_factor_pose_3d.cpp
+++ b/test/gtest_factor_pose_3d.cpp
@@ -36,7 +36,7 @@ ProblemPtr problem = Problem::create("PO", 3);
SolverCeres solver(problem);
// Two frames
-FrameBasePtr frm0 = problem->emplaceFrame(KEY, 0, problem->stateZero() );
+FrameBasePtr frm0 = problem->emplaceKeyFrame(0, problem->stateZero() );
// Capture, feature and factor
auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "CaptureOdom3d", 0, nullptr, pose7, nullptr);
diff --git a/test/gtest_factor_relative_2d_analytic.cpp b/test/gtest_factor_relative_2d_analytic.cpp
index 5650f90618f6e556cd80a0f5cd423e7ac3b722b2..93581a6ac63b18da056f84ea5caed95ca009ebd4 100644
--- a/test/gtest_factor_relative_2d_analytic.cpp
+++ b/test/gtest_factor_relative_2d_analytic.cpp
@@ -19,8 +19,8 @@ ProblemPtr problem_ptr = Problem::create("PO", 2);
SolverCeres solver(problem_ptr);
// Two frames
-FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, 0.0, Vector3d::Zero());
-FrameBasePtr frm1 = problem_ptr->emplaceFrame(KEY, 1.0, Vector3d::Zero());
+FrameBasePtr frm0 = problem_ptr->emplaceKeyFrame(0.0, Vector3d::Zero());
+FrameBasePtr frm1 = problem_ptr->emplaceKeyFrame(1.0, Vector3d::Zero());
// Capture from frm1 to frm0
auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, nullptr, Vector3d::Zero(), data_cov);
diff --git a/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp b/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
index feee02d63ca843c9f89fd8efc3443fedc40567b1..78321478cc5126e419ca4fc8fea53c5ed79caa71 100644
--- a/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
+++ b/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
@@ -27,8 +27,8 @@ auto sensor_odom2d = problem_ptr->installSensor("SensorOdom2d", "odom", Vector3d
auto processor_odom2d = problem_ptr->installProcessor("ProcessorOdom2d", "odom", sensor_odom2d, std::make_shared<ParamsProcessorOdom2d>());
// Two frames
-FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, 0, Vector3d::Zero() );
-FrameBasePtr frm1 = problem_ptr->emplaceFrame(KEY, 1, Vector3d::Zero() );
+FrameBasePtr frm0 = problem_ptr->emplaceKeyFrame(0, Vector3d::Zero() );
+FrameBasePtr frm1 = problem_ptr->emplaceKeyFrame(1, Vector3d::Zero() );
// Capture from frm1 to frm0
auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, sensor_odom2d, Vector3d::Zero(), data_cov);
diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp
index 8f73d96ac2560d6366410b48da41e2a3ef3fe00b..7167566535827fa6281057903e6f11dafdf3810d 100644
--- a/test/gtest_frame_base.cpp
+++ b/test/gtest_frame_base.cpp
@@ -32,7 +32,7 @@ TEST(FrameBase, GettersAndSetters)
ASSERT_EQ(t, 1);
ASSERT_FALSE(F->isFixed());
ASSERT_EQ(F->isKey(), false);
- ASSERT_EQ(F->isKeyOrAux(), false);
+ ASSERT_EQ(F->isKey(), false);
}
TEST(FrameBase, StateBlocks)
@@ -69,8 +69,8 @@ TEST(FrameBase, LinksToTree)
intrinsics_odo.k_disp_to_disp = 1;
intrinsics_odo.k_rot_to_rot = 1;
auto S = SensorBase::emplace<SensorOdom2d>(P->getHardware(), Vector3d::Zero(), intrinsics_odo);
- auto F1 = FrameBase::emplace<FrameBase>(T, KEY, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
- auto F2 = FrameBase::emplace<FrameBase>(T, KEY, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+ auto F1 = FrameBase::emplaceKeyFrame<FrameBase>(T, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+ auto F2 = FrameBase::emplaceKeyFrame<FrameBase>(T, 2, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
auto C = CaptureBase::emplace<CaptureMotion>(F1, "CaptureMotion", 1, S, Vector3d::Zero(), nullptr);
WOLF_INFO("F2->getCaptureList().size() ", F2->getCaptureList().size());
auto p = std::make_shared<ProcessorOdom2d>(std::make_shared<ParamsProcessorOdom2d>());
@@ -116,10 +116,7 @@ TEST(FrameBase, LinksToTree)
F1->getO()->fix();
ASSERT_TRUE(F1->isFixed());
- // set key
- F1->setKey();
ASSERT_TRUE(F1->isKey());
- ASSERT_TRUE(F1->isKeyOrAux());
}
#include "core/state_block/state_quaternion.h"
diff --git a/test/gtest_has_state_blocks.cpp b/test/gtest_has_state_blocks.cpp
index e17124a2d56687d8fe8de4c8d1144e49285e1f86..646dd602e619b0c2d2fadd70eb6c6f94e7f79a5f 100644
--- a/test/gtest_has_state_blocks.cpp
+++ b/test/gtest_has_state_blocks.cpp
@@ -55,11 +55,11 @@ TEST_F(HasStateBlocksTest, Notifications_setKey_add)
Notification n;
ASSERT_FALSE(problem->getStateBlockNotification(sbp0, n));
- F0->link(problem->getTrajectory());
+ ASSERT_DEATH(F0->link(problem->getTrajectory()), "");
- ASSERT_FALSE(problem->getStateBlockNotification(sbp0, n));
+ // ASSERT_FALSE(problem->getStateBlockNotification(sbp0, n));
- F0->setKey();
+ F0->setKey(problem);
ASSERT_TRUE(problem->getStateBlockNotification(sbp0, n));
ASSERT_EQ(n, ADD);
@@ -69,21 +69,21 @@ TEST_F(HasStateBlocksTest, Notifications_add_makeKF)
{
Notification n;
- // First add SB, than make KF
+ // First add SB, then make KF
ASSERT_FALSE(problem->getStateBlockNotification(sbp0, n));
- F0->link(problem->getTrajectory());
+ // F0->link(problem->getTrajectory());
- ASSERT_FALSE(problem->getStateBlockNotification(sbp0, n));
+ // ASSERT_FALSE(problem->getStateBlockNotification(sbp0, n));
- ASSERT_FALSE(problem->getStateBlockNotification(sbv0, n));
+ // ASSERT_FALSE(problem->getStateBlockNotification(sbv0, n));
F0->addStateBlock('V', sbv0);
ASSERT_FALSE(problem->getStateBlockNotification(sbv0, n));
- F0->setKey();
+ F0->setKey(problem);
ASSERT_TRUE(problem->getStateBlockNotification(sbp0, n));
ASSERT_EQ(n, ADD);
@@ -99,8 +99,8 @@ TEST_F(HasStateBlocksTest, Notifications_makeKF_add)
// first make KF, then add SB
- F1->link(problem->getTrajectory());
- F1->setKey();
+ // F1->link(problem->getTrajectory());
+ F1->setKey(problem);
F1->addStateBlock('P', sbp1);
@@ -130,9 +130,8 @@ TEST_F(HasStateBlocksTest, Add_solve_notify_solve_add)
ASSERT_FALSE(problem->getStateBlockNotification(sbp0, n));
- F0->link(problem->getTrajectory());
F0->addStateBlock('V', sbv0);
- F0->setKey();
+ F0->setKey(problem);
ASSERT_TRUE(problem->getStateBlockNotification(sbv0, n));
ASSERT_EQ(n, ADD);
@@ -213,9 +212,6 @@ TEST_F(HasStateBlocksTest, getState_structure)
ASSERT_FALSE(state0.count('V'));
ASSERT_FALSE(state0.count('W'));
-
-
-
}
diff --git a/test/gtest_odom_2d.cpp b/test/gtest_odom_2d.cpp
index 62109f6323593cada613758a05e762618d33bd84..3b525a12e5210de4eeb54ef213d8446bd24b8329 100644
--- a/test/gtest_odom_2d.cpp
+++ b/test/gtest_odom_2d.cpp
@@ -74,7 +74,7 @@ void show(const ProblemPtr& problem)
using std::endl;
cout << std::setprecision(4);
- for (FrameBasePtr F : problem->getTrajectory()->getFrameList())
+ for (FrameBasePtr F : *problem->getTrajectory())
{
if (F->isKey())
{
@@ -132,14 +132,14 @@ TEST(Odom2d, FactorFix_and_FactorOdom2d)
// KF1 and motion from KF0
t += dt;
- FrameBasePtr F1 = Pr->emplaceFrame(KEY, t, Vector3d::Zero());
+ FrameBasePtr F1 = Pr->emplaceKeyFrame(t, Vector3d::Zero());
auto C1 = CaptureBase::emplace<CaptureBase>(F1, "CaptureOdom2d", t);
auto f1 = FeatureBase::emplace<FeatureBase>(C1, "FeatureOdom2d", delta, delta_cov);
auto c1 = FactorBase::emplace<FactorOdom2d>(f1, f1, F0, nullptr, false);
// KF2 and motion from KF1
t += dt;
- FrameBasePtr F2 = Pr->emplaceFrame(KEY, t, Vector3d::Zero());
+ FrameBasePtr F2 = Pr->emplaceKeyFrame(t, Vector3d::Zero());
auto C2 = CaptureBase::emplace<CaptureBase>(F2, "CaptureOdom2d", t);
auto f2 = FeatureBase::emplace<FeatureBase>(C2, "FeatureOdom2d", delta, delta_cov);
auto c2 = FactorBase::emplace<FactorOdom2d>(f2, f2, F1, nullptr, false);
@@ -228,7 +228,7 @@ TEST(Odom2d, VoteForKfAndSolve)
solver.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
// std::cout << "Initial pose : " << problem->getCurrentState().transpose() << std::endl;
- // std::cout << "Initial covariance : " << std::endl << problem->getLastKeyFrameCovariance() << std::endl;
+ // std::cout << "Initial covariance : " << std::endl << problem->getLastFrameCovariance() << std::endl;
// std::cout << "Motion data : " << data.transpose() << std::endl;
// Check covariance values
@@ -306,7 +306,7 @@ TEST(Odom2d, VoteForKfAndSolve)
// Check the 3 KFs' states and covariances
MatrixXd computed_cov;
int n = 0;
- for (auto F : problem->getTrajectory()->getFrameList())
+ for (auto F : *problem->getTrajectory())
{
if (!F->isKey()) break;
@@ -423,12 +423,12 @@ TEST(Odom2d, KF_callback)
std::cout << "-----------------------------\nSplit after last KF; time: " << t_split << std::endl;
Vector3d x_split = processor_odom2d->getState(t_split).vector("PO");
- FrameBasePtr keyframe_2 = problem->emplaceFrame(KEY, t_split, x_split);
+ FrameBasePtr keyframe_2 = problem->emplaceKeyFrame(t_split, x_split);
// there must be 2KF and one F
- ASSERT_EQ(problem->getTrajectory()->getFrameList().size(), 3);
+ ASSERT_EQ(problem->getTrajectory()->getFrameMap().size(), 2);
// The last KF must have TS = 0.08
- ASSERT_EQ(problem->getLastKeyFrame()->getTimeStamp().getNanoSeconds(), 80000000);
+ ASSERT_EQ(problem->getLastFrame()->getTimeStamp().getNanoSeconds(), 80000000);
ASSERT_TRUE(problem->check(0));
processor_odom2d->keyFrameCallback(keyframe_2, dt/2);
@@ -446,9 +446,9 @@ TEST(Odom2d, KF_callback)
// std::cout << report << std::endl;
solver.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
- ASSERT_POSE2d_APPROX(problem->getLastKeyFrame()->getStateVector() , integrated_pose_vector[n_split], 1e-6);
+ ASSERT_POSE2d_APPROX(problem->getLastFrame()->getStateVector() , integrated_pose_vector[n_split], 1e-6);
MatrixXd computed_cov;
- ASSERT_TRUE(problem->getLastKeyFrameCovariance(computed_cov));
+ ASSERT_TRUE(problem->getLastFrameCovariance(computed_cov));
ASSERT_MATRIX_APPROX(computed_cov, integrated_cov_vector [n_split], 1e-6);
////////////////////////////////////////////////////////////////
@@ -460,7 +460,7 @@ TEST(Odom2d, KF_callback)
problem->print(4,1,1,1);
x_split = processor_odom2d->getState(t_split).vector("PO");
- FrameBasePtr keyframe_1 = problem->emplaceFrame(KEY, t_split, x_split);
+ FrameBasePtr keyframe_1 = problem->emplaceKeyFrame(t_split, x_split);
ASSERT_TRUE(problem->check(0));
processor_odom2d->keyFrameCallback(keyframe_1, dt/2);
@@ -493,7 +493,7 @@ TEST(Odom2d, KF_callback)
// check other KF in the future of the split KF
MatrixXd KF2_cov;
ASSERT_TRUE(problem->getFrameCovariance(keyframe_2, KF2_cov));
- ASSERT_POSE2d_APPROX(problem->getLastKeyFrame()->getStateVector(), integrated_pose_vector[n_split], 1e-6);
+ ASSERT_POSE2d_APPROX(problem->getLastFrame()->getStateVector(), integrated_pose_vector[n_split], 1e-6);
ASSERT_MATRIX_APPROX(KF2_cov, integrated_cov_vector [n_split], 1e-6);
// Check full trajectory
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index 210fdbee0df5a8e4b6f3e189a28b4fe6e841b9fd..657ddc48a3ee49d8aefa709379415e780641aeec 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -132,7 +132,7 @@ TEST(Problem, SetOrigin_PO_2d)
F->getFactorList(fac_list);
// check that we have one frame (prior)
- ASSERT_EQ(T->getFrameList().size(), (SizeStd) 1);
+ ASSERT_EQ(T->getFrameMap().size(), (SizeStd) 1);
// check that we have one capture (prior)
ASSERT_EQ(F->getCaptureList().size(), (SizeStd) 1);
// check that we have two features (prior: one per state block)
@@ -191,7 +191,7 @@ TEST(Problem, SetOrigin_PO_3d)
F->getFactorList(fac_list);
// check that we have one frame (prior)
- ASSERT_EQ(T->getFrameList().size(), (SizeStd) 1);
+ ASSERT_EQ(T->getFrameMap().size(), (SizeStd) 1);
// check that we have one capture (prior)
ASSERT_EQ(F->getCaptureList().size(), (SizeStd) 1);
// check that we have two features (prior: one per state block)
@@ -224,12 +224,12 @@ TEST(Problem, emplaceFrame_factory)
{
ProblemPtr P = Problem::create("PO", 2);
- FrameBasePtr f0 = P->emplaceFrame(KEY, 0, "PO" , 2, VectorXd(3) );
- FrameBasePtr f1 = P->emplaceFrame(KEY, 1, "PO" , 3, VectorXd(7) );
- FrameBasePtr f2 = P->emplaceFrame(KEY, 2, "POV", 3, VectorXd(10) );
+ FrameBasePtr f0 = P->emplaceKeyFrame(0, "PO" , 2, VectorXd(3) );
+ FrameBasePtr f1 = P->emplaceKeyFrame(1, "PO" , 3, VectorXd(7) );
+ FrameBasePtr f2 = P->emplaceKeyFrame(2, "POV", 3, VectorXd(10) );
// check that all frames are effectively in the trajectory
- ASSERT_EQ(P->getTrajectory()->getFrameList().size(), (SizeStd) 3);
+ ASSERT_EQ(P->getTrajectory()->getFrameMap().size(), (SizeStd) 3);
ASSERT_EQ(f0->getStateVector().size(), 3 );
ASSERT_EQ(f1->getStateVector().size(), 7 );
@@ -261,7 +261,7 @@ TEST(Problem, StateBlocks)
auto pm = P->installProcessor("ProcessorOdom3d", "odom integrator", "odometer", wolf_root + "/test/yaml/processor_odom_3d.yaml");
// 2 state blocks, estimated
- auto KF = P->emplaceFrame(KEY, 0, "PO", 3, xs3d );
+ auto KF = P->emplaceKeyFrame(0, "PO", 3, xs3d );
ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)(2 + 2));
// Notifications
@@ -320,7 +320,7 @@ TEST(Problem, Covariances)
// 4 state blocks, estimated
St->unfixExtrinsics();
- FrameBasePtr F = P->emplaceFrame(KEY, 0, "PO", 3, xs3d );
+ FrameBasePtr F = P->emplaceKeyFrame(0, "PO", 3, xs3d );
// set covariance (they are not computed without a solver)
P->addCovarianceBlock(F->getP(), Eigen::Matrix3d::Identity());
@@ -356,7 +356,7 @@ TEST(Problem, perturb)
int i = 0;
for (TimeStamp t = 0.0; t < 2.0; t += 1.0)
{
- auto F = problem->emplaceFrame(KEY, t, pose );
+ auto F = problem->emplaceKeyFrame(t, pose );
if (i==0) F->fix();
for (int j = 0; j< 2 ; j++)
@@ -394,7 +394,7 @@ TEST(Problem, perturb)
ASSERT_FALSE(S->getIntrinsic()->getState().isApprox(Vector3d(1,1,1), prec) );
}
- for (auto F : problem->getTrajectory()->getFrameList())
+ for (auto F : *problem->getTrajectory())
{
if (F->isFixed()) // fixed
{
@@ -446,7 +446,7 @@ TEST(Problem, check)
int i = 0;
for (TimeStamp t = 0.0; t < 2.0; t += 1.0)
{
- auto F = problem->emplaceFrame(KEY, t, pose);
+ auto F = problem->emplaceKeyFrame(t, pose);
if (i==0) F->fix();
for (int j = 0; j< 2 ; j++)
@@ -477,7 +477,7 @@ TEST(Problem, check)
// remove stuff from problem, and re-check
- auto F = problem->getLastKeyFrame();
+ auto F = problem->getLastFrame();
auto cby = F->getConstrainedByList().front();
diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp
index ed01245b29c903a7584e932196fb93a2c01c5c20..c3277078df9300439a6dee3721f0084df9751522 100644
--- a/test/gtest_processor_base.cpp
+++ b/test/gtest_processor_base.cpp
@@ -117,12 +117,16 @@ TEST(ProcessorBase, KeyFrameCallback)
capt_odo->setTimeStamp(t);
std::cout << "2\n";
+// auto proc_odo_motion = std::static_pointer_cast<ProcessorMotion>(proc_odo);
+// auto last_ptr = proc_odo_motion->last_ptr_;
+// auto last_ptr_frame = last_ptr->getFrame();
proc_odo->captureCallback(capt_odo);
std::cout << "3\n";
// Track
capt_trk = make_shared<CaptureVoid>(t, sens_trk);
std::cout << "4\n";
+ problem->print(4,1,1,1, std::cout);
proc_trk->captureCallback(capt_trk);
std::cout << "5\n";
@@ -130,7 +134,7 @@ TEST(ProcessorBase, KeyFrameCallback)
std::cout << "6\n";
// Only odom creating KFs
- ASSERT_TRUE( problem->getLastKeyFrame()->getStructure().compare("PO")==0 );
+ ASSERT_TRUE( problem->getLastFrame()->getStructure().compare("PO")==0 );
}
}
diff --git a/test/gtest_processor_diff_drive.cpp b/test/gtest_processor_diff_drive.cpp
index b13061e657badbd2ed81e9ea82b0f6078f3803f0..fc30487fe96765f36caea276defddc7abee9a6ac 100644
--- a/test/gtest_processor_diff_drive.cpp
+++ b/test/gtest_processor_diff_drive.cpp
@@ -352,9 +352,11 @@ TEST_F(ProcessorDiffDriveTest, linear)
data(0) = 100.0 ; // one turn of the wheels
data(1) = 100.0 ;
+ t += 1.0;
auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, F0->getCaptureList().front());
C->process();
+
WOLF_TRACE("t = ", t, "; x = ", processor->getState().vector("PO").transpose());
// radius is 1.0m, 100 ticks per revolution, so advanced distance is
@@ -377,6 +379,7 @@ TEST_F(ProcessorDiffDriveTest, angular)
processor->setOrigin(F0);
// Straight one turn of the wheels, in one go
+ t += 1.0;
data(0) = -20.0 ; // one fifth of a turn of the left wheel, in reverse
data(1) = 20.0 ; // one fifth of a turn of the right wheel, forward --> we'll turn left --> positive angle
diff --git a/test/gtest_processor_loopclosure.cpp b/test/gtest_processor_loopclosure.cpp
index 90d55137f88921c61c2e79bee1ea19d0f9047111..6a4b40d32c0908484e31b576310657aba5d11fb4 100644
--- a/test/gtest_processor_loopclosure.cpp
+++ b/test/gtest_processor_loopclosure.cpp
@@ -34,7 +34,7 @@ protected:
bool detectFeatures(CaptureBasePtr cap) override { return true;};
CaptureBasePtr findLoopCandidate(CaptureBasePtr _capture) override
{
- for (FrameBasePtr kf : getProblem()->getTrajectory()->getFrameList())
+ for (FrameBasePtr kf : *getProblem()->getTrajectory())
if (kf->isKey())
for (CaptureBasePtr cap : kf->getCaptureList())
if (cap != _capture)
@@ -87,7 +87,7 @@ TEST(ProcessorLoopClosure, installProcessor)
// new KF
t += dt;
- auto kf = problem->emplaceFrame(KEY, t, x); //KF2
+ auto kf = problem->emplaceKeyFrame(t, x); //KF2
// emplace a capture in KF
auto capt_lc = CaptureBase::emplace<CaptureVoid>(kf, t, sens_lc);
proc_lc->captureCallback(capt_lc);
diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp
index a977672d0230aff9786590d10724d544036f7224..743b0fcc56a4088ceb87073a6941200fd74ccd33 100644
--- a/test/gtest_processor_motion.cpp
+++ b/test/gtest_processor_motion.cpp
@@ -50,11 +50,6 @@ class ProcessorMotion_test : public testing::Test{
Vector2d data;
Matrix2d data_cov;
-// ProcessorMotion_test() :
-// ProcessorOdom2d(std::make_shared<ProcessorParamsOdom2d>()),
-// dt(0)
-// { }
-
void SetUp() override
{
std::string wolf_root = _WOLF_ROOT_DIR;
@@ -63,7 +58,7 @@ class ProcessorMotion_test : public testing::Test{
problem = Problem::create("PO", 2);
sensor = static_pointer_cast<SensorOdom2d>(problem->installSensor("SensorOdom2d", "odom", Vector3d(0,0,0), wolf_root + "/test/yaml/sensor_odom_2d.yaml"));
ParamsProcessorOdom2dPtr params(std::make_shared<ParamsProcessorOdom2d>());
- params->time_tolerance = 0.25;
+ params->time_tolerance = 0.5;
params->dist_traveled = 100;
params->angle_turned = 6.28;
params->max_time_span = 2.5*dt; // force KF at every third process()
@@ -146,10 +141,12 @@ TEST_F(ProcessorMotion_test, getState_time_structure)
capture->setTimeStamp(t);
capture->setData(data);
capture->setDataCovariance(data_cov);
- processor->captureCallback(capture);
+ capture->process();
WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
}
+ problem->print(2,1,1,1);
+
ASSERT_TRUE (processor->getState(7, "P").count('P'));
ASSERT_FALSE(processor->getState(7, "P").count('O'));
ASSERT_FALSE(processor->getState(7, "O").count('P'));
@@ -315,7 +312,7 @@ TEST_F(ProcessorMotion_test, splitBufferAutoPrior)
SensorBasePtr S = processor->getSensor();
TimeStamp t_target = 8.5;
- FrameBasePtr F_target = problem->emplaceFrame(KEY, t_target);
+ FrameBasePtr F_target = problem->emplaceKeyFrame(t_target);
CaptureMotionPtr C_source = std::dynamic_pointer_cast<CaptureMotion>(processor->getLast());
CaptureMotionPtr C_target = CaptureBase::emplace<CaptureMotion>(F_target,
"ODOM 2d",
@@ -360,7 +357,7 @@ TEST_F(ProcessorMotion_test, splitBufferFactorPrior)
SensorBasePtr S = processor->getSensor();
TimeStamp t_target = 8.5;
- FrameBasePtr F_target = problem->emplaceFrame(KEY, t_target);
+ FrameBasePtr F_target = problem->emplaceKeyFrame(t_target);
CaptureMotionPtr C_source = std::dynamic_pointer_cast<CaptureMotion>(processor->getLast());
CaptureMotionPtr C_target = CaptureBase::emplace<CaptureMotion>(F_target,
"ODOM 2d",
@@ -405,7 +402,7 @@ TEST_F(ProcessorMotion_test, splitBufferFixPrior)
SensorBasePtr S = processor->getSensor();
TimeStamp t_target = 8.5;
- FrameBasePtr F_target = problem->emplaceFrame(KEY, t_target);
+ FrameBasePtr F_target = problem->emplaceKeyFrame(t_target);
CaptureMotionPtr C_source = std::dynamic_pointer_cast<CaptureMotion>(processor->getLast());
CaptureMotionPtr C_target = CaptureBase::emplace<CaptureMotion>(F_target,
"ODOM 2d",
diff --git a/test/gtest_processor_tracker_feature_dummy.cpp b/test/gtest_processor_tracker_feature_dummy.cpp
index 6238a5786533fc89df682364305db55661921e92..7b6dc1c40fe5a4eabcdeae278cd823fa01403fbb 100644
--- a/test/gtest_processor_tracker_feature_dummy.cpp
+++ b/test/gtest_processor_tracker_feature_dummy.cpp
@@ -287,8 +287,8 @@ TEST_F(ProcessorTrackerFeatureDummyTest, process)
CaptureBasePtr cap1 = std::make_shared<CaptureVoid>(0, sensor);
cap1->process();
- ASSERT_TRUE(problem->getTrajectory()->getLastKeyFrame() != nullptr);
- ASSERT_EQ(problem->getTrajectory()->getLastKeyFrame()->id(), cap1->getFrame()->id());
+ ASSERT_TRUE(problem->getTrajectory()->getLastFrame() != nullptr);
+ ASSERT_EQ(problem->getTrajectory()->getLastFrame()->id(), cap1->getFrame()->id());
ASSERT_TRUE(problem->check(0));
//2ND TIME
@@ -322,7 +322,7 @@ TEST_F(ProcessorTrackerFeatureDummyTest, process)
cap5->process();
ASSERT_TRUE(cap4->getFrame()->isKey());
- ASSERT_EQ(problem->getTrajectory()->getLastKeyFrame()->id(), cap4->getFrame()->id());
+ ASSERT_EQ(problem->getTrajectory()->getLastFrame()->id(), cap4->getFrame()->id());
ASSERT_TRUE(problem->check(0));
// check factors
diff --git a/test/gtest_processor_tracker_landmark_dummy.cpp b/test/gtest_processor_tracker_landmark_dummy.cpp
index 44dfbdf14cc1f53ea4962cc7f2517d37ef5210b2..d0fe1b1d8a748df603db674cf7604720c69e4d71 100644
--- a/test/gtest_processor_tracker_landmark_dummy.cpp
+++ b/test/gtest_processor_tracker_landmark_dummy.cpp
@@ -321,8 +321,8 @@ TEST_F(ProcessorTrackerLandmarkDummyTest, process)
CaptureBasePtr cap1 = std::make_shared<CaptureVoid>(0, sensor);
cap1->process();
- ASSERT_TRUE(problem->getTrajectory()->getLastKeyFrame() != nullptr);
- ASSERT_EQ(problem->getTrajectory()->getLastKeyFrame(), cap1->getFrame());
+ ASSERT_TRUE(problem->getTrajectory()->getLastFrame() != nullptr);
+ ASSERT_EQ(problem->getTrajectory()->getLastFrame(), cap1->getFrame());
//2ND TIME
WOLF_DEBUG("Second time...");
@@ -351,7 +351,7 @@ TEST_F(ProcessorTrackerLandmarkDummyTest, process)
CaptureBasePtr cap5 = std::make_shared<CaptureVoid>(4, sensor);
cap5->process();
- ASSERT_EQ(problem->getTrajectory()->getLastKeyFrame(), cap4->getFrame());
+ ASSERT_EQ(problem->getTrajectory()->getLastFrame(), cap4->getFrame());
ASSERT_EQ(processor->getOrigin(), cap4);
ASSERT_EQ(processor->getLast(), cap5);
diff --git a/test/gtest_solver_ceres.cpp b/test/gtest_solver_ceres.cpp
index 42b2f1c825fe7cfc79bbe44e8f577eed7ec1dfd8..9fe0e4306704919bcd7ab234716a3acf107e0227 100644
--- a/test/gtest_solver_ceres.cpp
+++ b/test/gtest_solver_ceres.cpp
@@ -1,5 +1,5 @@
/*
- * gtest_solver_ceres.cpp
+ * gtest_solver_ptr.cpp
*
* Created on: Jun, 2018
* Author: jvallve
@@ -13,6 +13,7 @@
#include "core/capture/capture_void.h"
#include "core/factor/factor_pose_2d.h"
#include "core/factor/factor_quaternion_absolute.h"
+#include "core/factor/factor_block_absolute.h"
#include "core/state_block/local_parametrization_angle.h"
#include "core/state_block/local_parametrization_quaternion.h"
@@ -27,604 +28,1533 @@
using namespace wolf;
using namespace Eigen;
-WOLF_PTR_TYPEDEFS(SolverCeresWrapper);
-class SolverCeresWrapper : public SolverCeres
-{
- public:
- SolverCeresWrapper(const ProblemPtr& wolf_problem) :
- SolverCeres(wolf_problem)
- {
- };
-
- bool isStateBlockRegisteredSolverCeres(const StateBlockPtr& st)
- {
- return ceres_problem_->HasParameterBlock(SolverManager::getAssociatedMemBlockPtr(st));
- };
-
- bool isStateBlockRegisteredSolverManager(const StateBlockPtr& st)
- {
- return state_blocks_.find(st)!=state_blocks_.end();
- };
-
- bool isStateBlockFixed(const StateBlockPtr& st)
- {
- return ceres_problem_->IsParameterBlockConstant(SolverManager::getAssociatedMemBlockPtr(st));
- };
-
- int numStateBlocks()
- {
- return ceres_problem_->NumParameterBlocks();
- };
-
- int numFactors()
- {
- return ceres_problem_->NumResidualBlocks();
- };
-
- bool isFactorRegistered(const FactorBasePtr& fac_ptr) const override
- {
- return fac_2_residual_idx_.find(fac_ptr) != fac_2_residual_idx_.end() && fac_2_costfunction_.find(fac_ptr) != fac_2_costfunction_.end();
- };
-
- bool hasThisLocalParametrization(const StateBlockPtr& st, const LocalParametrizationBasePtr& local_param)
- {
- return state_blocks_local_param_.find(st) != state_blocks_local_param_.end() &&
- state_blocks_local_param_.at(st)->getLocalParametrization() == local_param &&
- ceres_problem_->GetParameterization(getAssociatedMemBlockPtr(st)) == state_blocks_local_param_.at(st).get();
- };
+/*
+ * Following tests are the same as in gtest_solver_manager.cpp
+ * (modifications should be applied to both tests)
+ */
- bool hasLocalParametrization(const StateBlockPtr& st) const
- {
- return state_blocks_local_param_.find(st) != state_blocks_local_param_.end();
- };
+StateBlockPtr createStateBlock()
+{
+ Vector4d state; state << 1, 0, 0, 0;
+ return std::make_shared<StateBlock>(state);
+}
-};
+FactorBasePtr createFactor(StateBlockPtr sb_ptr)
+{
+ return std::make_shared<FactorBlockAbsolute>(std::make_shared<FeatureBase>("Dummy",
+ VectorXd::Zero(sb_ptr->getSize()),
+ MatrixXd::Identity(sb_ptr->getSize(),sb_ptr->getSize())),
+ sb_ptr,
+ nullptr,
+ false);
+}
TEST(SolverCeres, Create)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
- // check double ointers to branches
- ASSERT_EQ(P, solver_ceres->getProblem());
+ // check pointers
+ EXPECT_EQ(P, solver_ptr->getProblem());
- // run ceres manager check
- ASSERT_TRUE(solver_ceres->check());
+ // run solver check
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, AddStateBlock)
+////////////////////////////////////////////////////////
+// FLOATING STATE BLOCKS
+TEST(SolverCeres, FloatingStateBlock_Add)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
- // add stateblock
+ // notify stateblock
P->notifyStateBlock(sb_ptr,ADD);
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
// update solver
- solver_ceres->update();
+ solver_ptr->update();
- // check stateblock
- ASSERT_TRUE(solver_ceres->isStateBlockRegisteredSolverManager(sb_ptr));
- ASSERT_TRUE(solver_ceres->isStateBlockRegisteredSolverCeres(sb_ptr));
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
- // run ceres manager check
- ASSERT_TRUE(solver_ceres->check());
+ // check stateblock
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, DoubleAddStateBlock)
+TEST(SolverCeres, FloatingStateBlock_DoubleAdd)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
- // add stateblock
+ // notify stateblock
P->notifyStateBlock(sb_ptr,ADD);
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
// update solver
- solver_ceres->update();
+ solver_ptr->update();
- // add stateblock again
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // notify stateblock again
P->notifyStateBlock(sb_ptr,ADD);
+ // check notifications
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
// update solver
- solver_ceres->update();
+ solver_ptr->update();
- // check stateblock
- ASSERT_TRUE(solver_ceres->isStateBlockRegisteredSolverManager(sb_ptr));
- ASSERT_TRUE(solver_ceres->isStateBlockRegisteredSolverCeres(sb_ptr));
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
- // run ceres manager check
- ASSERT_TRUE(solver_ceres->check());
+ // check stateblock
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, UpdateStateBlock)
+TEST(SolverCeres, FloatingStateBlock_AddFix)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
- // add stateblock
+ // notify stateblock
P->notifyStateBlock(sb_ptr,ADD);
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
// update solver
- solver_ceres->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
// check stateblock unfixed
- ASSERT_FALSE(solver_ceres->isStateBlockFixed(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
// Fix frame
sb_ptr->fix();
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_TRUE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
// update solver manager
- solver_ceres->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
// check stateblock fixed
- ASSERT_TRUE(solver_ceres->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, FloatingStateBlock_AddFixed)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // Fix state block
+ sb_ptr->fix();
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_TRUE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // update solver manager
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
- // run ceres manager check
- ASSERT_TRUE(solver_ceres->check());
+ // check stateblock fixed
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, AddUpdateStateBlock)
+TEST(SolverCeres, FloatingStateBlock_AddUpdateLocalParam)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
- // add stateblock
+ // notify stateblock
P->notifyStateBlock(sb_ptr,ADD);
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // Local param
+ LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationQuaternionLocal>();
+ sb_ptr->setLocalParametrization(local_ptr);
+
// Fix state block
sb_ptr->fix();
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_TRUE(sb_ptr->fixUpdated());
+ EXPECT_TRUE(sb_ptr->localParamUpdated());
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
// update solver manager
- solver_ceres->update();
+ solver_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
// check stateblock fixed
- ASSERT_TRUE(solver_ceres->isStateBlockRegisteredSolverManager(sb_ptr));
- ASSERT_TRUE(solver_ceres->isStateBlockRegisteredSolverCeres(sb_ptr));
- ASSERT_TRUE(solver_ceres->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, FloatingStateBlock_AddLocalParamRemoveLocalParam)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Local param
+ LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationQuaternionLocal>();
+ sb_ptr->setLocalParametrization(local_ptr);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_TRUE(sb_ptr->localParamUpdated());
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // update solver manager
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
- // run ceres manager check
- ASSERT_TRUE(solver_ceres->check());
+ // check stateblock localparam
+ EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // Remove local param
+ sb_ptr->removeLocalParametrization();
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_TRUE(sb_ptr->localParamUpdated());
+
+ // update solver manager
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check stateblock localparam
+ EXPECT_FALSE(solver_ptr->hasLocalParametrization(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, RemoveStateBlock)
+TEST(SolverCeres, FloatingStateBlock_Remove)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
- // add stateblock
+ // notify stateblock
P->notifyStateBlock(sb_ptr,ADD);
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
// update solver
- solver_ceres->update();
+ solver_ptr->update();
- ASSERT_TRUE(solver_ceres->isStateBlockRegisteredSolverManager(sb_ptr));
- ASSERT_TRUE(solver_ceres->isStateBlockRegisteredSolverCeres(sb_ptr));
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// remove state_block
P->notifyStateBlock(sb_ptr,REMOVE);
+ // check notifications
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(REMOVE, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
// update solver
- solver_ceres->update();
+ solver_ptr->update();
- // check stateblock
- ASSERT_FALSE(solver_ceres->isStateBlockRegisteredSolverManager(sb_ptr));
- ASSERT_EQ(solver_ceres->numStateBlocks(), 0);
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
- // run ceres manager check
- ASSERT_TRUE(solver_ceres->check());
+ // check stateblock
+ EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, AddRemoveStateBlock)
+TEST(SolverCeres, FloatingStateBlock_AddRemove)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
- // add stateblock
+ // notify stateblock
P->notifyStateBlock(sb_ptr,ADD);
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
// remove state_block
- P->notifyStateBlock(sb_ptr,REMOVE);
+ P->notifyStateBlock(sb_ptr,REMOVE); // should cancell out the ADD
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// update solver
- solver_ceres->update();
+ solver_ptr->update();
- // check no stateblocks
- ASSERT_FALSE(solver_ceres->isStateBlockRegisteredSolverManager(sb_ptr));
- ASSERT_EQ(solver_ceres->numStateBlocks(), 0);
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
- // run ceres manager check
- ASSERT_TRUE(solver_ceres->check());
+ // check state block
+ EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, RemoveUpdateStateBlock)
+TEST(SolverCeres, FloatingStateBlock_AddRemoveAdd)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
- // add stateblock
+ // notify stateblock
P->notifyStateBlock(sb_ptr,ADD);
- // update solver
- solver_ceres->update();
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// remove state_block
- P->notifyStateBlock(sb_ptr,REMOVE);
+ P->notifyStateBlock(sb_ptr,REMOVE); // should cancell out the ADD
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD); // again ADD in notification list
+
+ // check notifications
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// update solver
- solver_ceres->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
- // run ceres manager check
- ASSERT_TRUE(solver_ceres->check());
+ // check state block
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, DoubleRemoveStateBlock)
+TEST(SolverCeres, FloatingStateBlock_DoubleRemove)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
- // add stateblock
+ // notify stateblock
P->notifyStateBlock(sb_ptr,ADD);
+ // update solver
+ solver_ptr->update();
+
// remove state_block
P->notifyStateBlock(sb_ptr,REMOVE);
// update solver
- solver_ceres->update();
+ solver_ptr->update();
// remove state_block
P->notifyStateBlock(sb_ptr,REMOVE);
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(REMOVE, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
// update solver manager
- solver_ceres->update();
+ solver_ptr->update();
- // run ceres manager check
- ASSERT_TRUE(solver_ceres->check());
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // checks
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, AddFactor)
+TEST(SolverCeres, FloatingStateBlock_AddUpdated)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Fix
+ sb_ptr->fix();
+
+ // Set State
+ sb_ptr->setState(2*sb_ptr->getState());
+
+ // Check flags have been set true
+ EXPECT_TRUE(sb_ptr->fixUpdated());
+ EXPECT_TRUE(sb_ptr->stateUpdated());
+
+ // == When an ADD is notified: a ADD notification should be stored ==
+
+ // notify state block
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // check notifications
+ // check notifications
+ Notification notif;
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),1);
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(notif, ADD);
+
+ // == Insert OTHER notifications ==
+
+ // Set State --> FLAG
+ sb_ptr->setState(2*sb_ptr->getState());
+
+ // Fix --> FLAG
+ sb_ptr->unfix();
+ // Check flag has been set true
+ EXPECT_TRUE(sb_ptr->fixUpdated());
+
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),1); // No new notifications (fix and set state are flags in sb)
+
+ // == consume empties the notification map ==
+ solver_ptr->update(); // it calls P->consumeStateBlockNotificationMap();
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ // Check flags have been reset
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+
+ // == When an REMOVE is notified: a REMOVE notification should be stored ==
- // Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceFrame(KEY, 0, P->stateZero() );
- auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
- FactorPose2dPtr c = FactorBase::emplace<FactorPose2d>(f,f,nullptr,false);
+ // remove state_block
+ P->notifyStateBlock(sb_ptr,REMOVE);
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),1);
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(notif, REMOVE);
+
+ // == REMOVE + ADD: notification map should be empty ==
+ P->notifyStateBlock(sb_ptr,ADD);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+
+ // == UPDATES should clear the list of notifications ==
+ // notify state block
+ P->notifyStateBlock(sb_ptr,ADD);
// update solver
- solver_ceres->update();
+ solver_ptr->update();
- // check factor
- ASSERT_TRUE(solver_ceres->isFactorRegistered(c));
- ASSERT_EQ(solver_ceres->numFactors(), 1);
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); // After solver_manager->update, notifications should be empty
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
- // run ceres manager check
- ASSERT_TRUE(solver_ceres->check());
+ // == ADD + REMOVE: notification map should be empty ==
+ P->notifyStateBlock(sb_ptr,ADD);
+ P->notifyStateBlock(sb_ptr,REMOVE);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
}
-TEST(SolverCeres, DoubleAddFactor)
+////////////////////////////////////////////////////////
+// STATE BLOCKS AND FACTOR
+TEST(SolverCeres, StateBlockAndFactor_Add)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
- // Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceFrame(KEY, 0, P->stateZero() );
- auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
- FactorPose2dPtr c = FactorBase::emplace<FactorPose2d>(f,f,nullptr,false);
+ // Create State block
+ auto sb_ptr = createStateBlock();
- // add factor again
- P->notifyFactor(c,ADD);
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify stateblock (floating for the moment)
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // update solver
+ solver_ptr->update();
+
+ // checks
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+
+ // notify factor (state block now not floating)
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
// update solver
- solver_ceres->update();
+ solver_ptr->update();
- // check factor
- ASSERT_TRUE(solver_ceres->isFactorRegistered(c));
- ASSERT_EQ(solver_ceres->numFactors(), 1);
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
- // run ceres manager check
- ASSERT_TRUE(solver_ceres->check());
+ // checks
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, RemoveFactor)
+TEST(SolverCeres, StateBlockAndFactor_DoubleAdd)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
- // Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceFrame(KEY, 0, P->stateZero() );
- auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
- FactorPose2dPtr c = FactorBase::emplace<FactorPose2d>(f,f,nullptr,false);
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
// update solver
- solver_ceres->update();
+ solver_ptr->update();
+ solver_ptr->update();
- // remove factor
- P->notifyFactor(c,REMOVE);
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // notify stateblock again
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
// update solver
- solver_ceres->update();
+ solver_ptr->update();
- // check factor
- ASSERT_FALSE(solver_ceres->isFactorRegistered(c));
- ASSERT_EQ(solver_ceres->numFactors(), 0);
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
- // run ceres manager check
- ASSERT_TRUE(solver_ceres->check());
+ // check stateblock
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, AddRemoveFactor)
+TEST(SolverCeres, StateBlockAndFactor_Fix)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
- // Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceFrame(KEY, 0, P->stateZero() );
- auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
- FactorPose2dPtr c = FactorBase::emplace<FactorPose2d>(f,f,nullptr,false);
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
- // remove factor
- P->notifyFactor(c,REMOVE);
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
- ASSERT_TRUE(P->getFactorNotificationMapSize() == 0); // add+remove = empty
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
// update solver
- solver_ceres->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // check stateblock unfixed
+ EXPECT_FALSE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+
+ // Fix frame
+ sb_ptr->fix();
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_TRUE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // update solver manager
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
- // check factor
- ASSERT_FALSE(solver_ceres->isFactorRegistered(c));
- ASSERT_EQ(solver_ceres->numFactors(), 0);
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
- // run ceres manager check
- ASSERT_TRUE(solver_ceres->check());
+ // check stateblock fixed
+ EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, DoubleRemoveFactor)
+TEST(SolverCeres, StateBlockAndFactor_AddFixed)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
- // Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceFrame(KEY, 0, P->stateZero() );
- auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
- FactorPose2dPtr c = FactorBase::emplace<FactorPose2d>(f,f,nullptr,false);
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // Fix state block
+ sb_ptr->fix();
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_TRUE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // update solver manager
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // check stateblock fixed
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, StateBlockAndFactor_UpdateLocalParam)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // Local param
+ LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationQuaternionLocal>();
+ sb_ptr->setLocalParametrization(local_ptr);
+
+ // Fix state block
+ sb_ptr->fix();
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_TRUE(sb_ptr->fixUpdated());
+ EXPECT_TRUE(sb_ptr->localParamUpdated());
+
+ // update solver manager
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // check stateblock fixed
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, StateBlockAndFactor_AddLocalParamRemoveLocalParam)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // Local param
+ LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationQuaternionLocal>();
+ sb_ptr->setLocalParametrization(local_ptr);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_TRUE(sb_ptr->localParamUpdated());
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // update solver manager
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check solver
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->hasLocalParametrization(sb_ptr));
+ EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // Remove local param
+ sb_ptr->removeLocalParametrization();
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_TRUE(sb_ptr->localParamUpdated());
+
+ // update solver manager
+ solver_ptr->update();
+
+ // check solver
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->hasLocalParametrization(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, StateBlockAndFactor_RemoveStateBlock)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
// update solver
- solver_ceres->update();
+ solver_ptr->update();
- // remove factor
- P->notifyFactor(c,REMOVE);
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // remove state_block
+ P->notifyStateBlock(sb_ptr,REMOVE);
+
+ // check notifications
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(REMOVE, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// update solver
- solver_ceres->update();
+ solver_ptr->update(); // there is a factor involved, removal posponed (REMOVE in notification list)
+
+ // check notifications
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(REMOVE, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // checks
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
- // remove factor
- P->notifyFactor(c,REMOVE);
+TEST(SolverCeres, StateBlockAndFactor_RemoveFactor)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // update solver
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // remove state_block
+ P->notifyFactor(fac_ptr,REMOVE); // state block should be automatically stored as floating
+
+ // check notifications
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(REMOVE, notif);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
// update solver
- solver_ceres->update();
+ solver_ptr->update();
- // check factor
- ASSERT_FALSE(solver_ceres->isFactorRegistered(c));
- ASSERT_EQ(solver_ceres->numFactors(), 0);
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
- // run ceres manager check
- ASSERT_TRUE(solver_ceres->check());
+ // checks
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, AddStateBlockLocalParam)
+TEST(SolverCeres, StateBlockAndFactor_AddRemoveStateBlock)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
// Create State block
- Vector1d state; state << 1;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
- // Local param
- LocalParametrizationBasePtr local_param_ptr = std::make_shared<LocalParametrizationAngle>();
- sb_ptr->setLocalParametrization(local_param_ptr);
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
- // add stateblock
+ // notify stateblock
P->notifyStateBlock(sb_ptr,ADD);
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // remove state_block
+ P->notifyStateBlock(sb_ptr,REMOVE); // cancells out ADD notification
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
// update solver
- solver_ceres->update();
+ solver_ptr->update(); // factor ADD notification is not executed since state block involved is missing (ADD notification added)
- // check stateblock
- ASSERT_TRUE(solver_ceres->hasLocalParametrization(sb_ptr));
- ASSERT_TRUE(solver_ceres->hasThisLocalParametrization(sb_ptr,local_param_ptr));
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
- // run ceres manager check
- ASSERT_TRUE(solver_ceres->check());
+ // check state block
+ EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, RemoveLocalParam)
+TEST(SolverCeres, StateBlockAndFactor_AddRemoveFactor)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
// Create State block
- Vector1d state; state << 1;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
- // Local param
- LocalParametrizationBasePtr local_param_ptr = std::make_shared<LocalParametrizationAngle>();
- sb_ptr->setLocalParametrization(local_param_ptr);
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
- // add stateblock
+ // notify stateblock
P->notifyStateBlock(sb_ptr,ADD);
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // remove state_block
+ P->notifyFactor(fac_ptr,REMOVE); // cancells out ADD notification
+
+ // check notifications
+ EXPECT_EQ(P->getFactorNotificationMapSize(),0);
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
// update solver
- solver_ceres->update();
+ solver_ptr->update(); // state block should be automatically stored as floating
- // Remove local param
- sb_ptr->removeLocalParametrization();
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // checks
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, StateBlockAndFactor_DoubleRemoveStateBlock)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // remove state_block
+ P->notifyStateBlock(sb_ptr,REMOVE); // cancells out the ADD
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
// update solver
- solver_ceres->update();
+ solver_ptr->update(); // factor ADD is posponed
- // check stateblock
- ASSERT_FALSE(solver_ceres->hasLocalParametrization(sb_ptr));
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // remove state_block
+ P->notifyStateBlock(sb_ptr,REMOVE);
- // run ceres manager check
- ASSERT_TRUE(solver_ceres->check());
+ // check notifications
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(REMOVE, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // update solver manager
+ solver_ptr->update(); // repeated REMOVE should be ignored
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // check state block
+ EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, AddLocalParam)
+TEST(SolverCeres, StateBlockAndFactor_DoubleRemoveFactor)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
// Create State block
- Vector1d state; state << 1;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
- // add stateblock
+ // notify stateblock
P->notifyStateBlock(sb_ptr,ADD);
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // remove state_block
+ P->notifyFactor(fac_ptr,REMOVE); // cancells out ADD
+
+ // check notifications
+ EXPECT_EQ(P->getFactorNotificationMapSize(),0);
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
// update solver
- solver_ceres->update();
+ solver_ptr->update(); // state block should be automatically stored as floating
- // check stateblock
- ASSERT_FALSE(solver_ceres->hasLocalParametrization(sb_ptr));
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(),0);
- // Local param
- LocalParametrizationBasePtr local_param_ptr = std::make_shared<LocalParametrizationAngle>();
- sb_ptr->setLocalParametrization(local_param_ptr);
+ // remove state_block
+ P->notifyFactor(fac_ptr,REMOVE);
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(REMOVE, notif);
// update solver
- solver_ceres->update();
+ solver_ptr->update(); // repeated REMOVE should be ignored
- // check stateblock
- ASSERT_TRUE(solver_ceres->hasLocalParametrization(sb_ptr));
- ASSERT_TRUE(solver_ceres->hasThisLocalParametrization(sb_ptr,local_param_ptr));
+ // checks
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, StateBlockAndFactor_AddUpdatedStateBlock)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // Fix
+ sb_ptr->fix();
- // run ceres manager check
- ASSERT_TRUE(solver_ceres->check());
+ // Change State
+ sb_ptr->setState(2*sb_ptr->getState());
+
+ // Check flags have been set true
+ EXPECT_TRUE(sb_ptr->fixUpdated());
+ EXPECT_TRUE(sb_ptr->stateUpdated());
+
+ // == When an ADD is notified: a ADD notification should be stored ==
+
+ // notify state block
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // == Insert OTHER notifications ==
+
+ // Set State --> FLAG
+ sb_ptr->setState(2*sb_ptr->getState());
+
+ // Fix --> FLAG
+ sb_ptr->unfix();
+ // Check flag has been set true
+ EXPECT_TRUE(sb_ptr->fixUpdated());
+
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),1); // No new notifications (fix and set state are flags in sb)
+
+ // == consume empties the notification map ==
+ solver_ptr->update(); // it calls P->consumeStateBlockNotificationMap();
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ // Check flags have been reset
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+
+ // == When an REMOVE is notified: a REMOVE notification should be stored ==
+
+ // remove state_block
+ P->notifyStateBlock(sb_ptr,REMOVE);
+
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),1);
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(notif, REMOVE);
+
+ // == REMOVE + ADD: notification map should be empty ==
+ P->notifyStateBlock(sb_ptr,ADD);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+
+ // == UPDATES should clear the list of notifications ==
+ // notify state block
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // update solver
+ solver_ptr->update();
+
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); // After solver_manager->update, notifications should be empty
+
+ // == ADD + REMOVE: notification map should be empty ==
+ P->notifyStateBlock(sb_ptr,ADD);
+ P->notifyStateBlock(sb_ptr,REMOVE);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
}
-TEST(SolverCeres, FactorsRemoveLocalParam)
+////////////////////////////////////////////////////////
+// ONLY FACTOR
+TEST(SolverCeres, OnlyFactor_Add)
{
ProblemPtr P = Problem::create("PO", 3);
- SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
- // Create (and add) 2 factors quaternion
- FrameBasePtr F = P->emplaceFrame(KEY, 0, P->stateZero() );
- auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
- FactorQuaternionAbsolutePtr c1 = FactorBase::emplace<FactorQuaternionAbsolute>(f, f, F->getO(),nullptr,false);
- FactorQuaternionAbsolutePtr c2 = FactorBase::emplace<FactorQuaternionAbsolute>(f, f, F->getO(),nullptr,false);
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify factor (state block has not been notified)
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
// update solver
- solver_ceres->update();
+ solver_ptr->update(); // factor ADD should be posponed (in the notification list again)
- // check local param
- ASSERT_TRUE(solver_ceres->hasLocalParametrization(F->getO()));
- ASSERT_TRUE(solver_ceres->hasThisLocalParametrization(F->getO(),F->getO()->getLocalParametrization()));
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
- // check factor
- ASSERT_TRUE(solver_ceres->isFactorRegistered(c1));
- ASSERT_TRUE(solver_ceres->isFactorRegistered(c2));
- ASSERT_EQ(solver_ceres->numFactors(), 2);
+ // checks
+ EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
- // remove local param
- F->getO()->removeLocalParametrization();
+TEST(SolverCeres, OnlyFactor_Remove)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify factor (state block has not been notified)
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
// update solver
- solver_ceres->update();
+ solver_ptr->update(); // ADD factor should be posponed (in the notification list again)
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
- // check local param
- ASSERT_FALSE(solver_ceres->hasLocalParametrization(F->getO()));
+ // notify factor
+ P->notifyFactor(fac_ptr,REMOVE);
- // check factor
- ASSERT_TRUE(solver_ceres->isFactorRegistered(c1));
- ASSERT_TRUE(solver_ceres->isFactorRegistered(c2));
- ASSERT_EQ(solver_ceres->numFactors(), 2);
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(),0);
- // run ceres manager check
- ASSERT_TRUE(solver_ceres->check());
+ // update solver
+ solver_ptr->update(); // nothing to update
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(),0);
+
+ // checks
+ EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, FactorsUpdateLocalParam)
+TEST(SolverCeres, OnlyFactor_AddRemove)
{
ProblemPtr P = Problem::create("PO", 3);
- SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
- // Create (and add) 2 factors quaternion
- FrameBasePtr F = P->emplaceFrame(KEY, 0, P->stateZero() );
- auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
- FactorQuaternionAbsolutePtr c1 = FactorBase::emplace<FactorQuaternionAbsolute>(f, f, F->getO(),nullptr,false);
- FactorQuaternionAbsolutePtr c2 = FactorBase::emplace<FactorQuaternionAbsolute>(f, f, F->getO(),nullptr,false);
+ // Create State block
+ auto sb_ptr = createStateBlock();
- // update solver
- solver_ceres->update();
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify factor (state block has not been notified)
+ P->notifyFactor(fac_ptr,ADD);
- // check local param
- ASSERT_TRUE(solver_ceres->hasLocalParametrization(F->getO()));
- ASSERT_TRUE(solver_ceres->hasThisLocalParametrization(F->getO(),F->getO()->getLocalParametrization()));
+ // check notifications
+ Notification notif;
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
- // check factor
- ASSERT_TRUE(solver_ceres->isFactorRegistered(c1));
- ASSERT_TRUE(solver_ceres->isFactorRegistered(c2));
- ASSERT_EQ(solver_ceres->numFactors(), 2);
+ // notify factor
+ P->notifyFactor(fac_ptr,REMOVE);
- // remove local param
- LocalParametrizationBasePtr local_param_ptr = std::make_shared<LocalParametrizationQuaternionGlobal>();
- F->getO()->setLocalParametrization(local_param_ptr);
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(),0);
// update solver
- solver_ceres->update();
+ solver_ptr->update(); // nothing to update
- // check local param
- ASSERT_TRUE(solver_ceres->hasLocalParametrization(F->getO()));
- ASSERT_TRUE(solver_ceres->hasThisLocalParametrization(F->getO(),local_param_ptr));
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(),0);
- // check factor
- ASSERT_TRUE(solver_ceres->isFactorRegistered(c1));
- ASSERT_TRUE(solver_ceres->isFactorRegistered(c2));
- ASSERT_EQ(solver_ceres->numFactors(), 2);
+ // checks
+ EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+////////////////////////////////////////////////////////
+// MULTITHREADING
+
+TEST(SolverCeres, MultiThreadingTruncatedNotifications)
+{
+ double Dt = 5.0;
+ ProblemPtr P = Problem::create("PO", 2);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
- // run ceres manager check
- ASSERT_TRUE(solver_ceres->check());
+ // loop updating (without sleep)
+ std::thread t([&](){
+ auto start_t = std::chrono::high_resolution_clock::now();
+ while (true)
+ {
+ solver_ptr->update();
+ ASSERT_TRUE(solver_ptr->check());
+ if (std::chrono::duration_cast<std::chrono::duration<double>>(std::chrono::high_resolution_clock::now() - start_t).count() > Dt)
+ break;
+ }});
+
+ // loop emplacing and removing frames (window of 10 KF)
+ auto start = std::chrono::high_resolution_clock::now();
+ TimeStamp ts(0);
+ while (true)
+ {
+ // Emplace Frame, Capture, feature and factor pose 2d
+ FrameBasePtr F = P->emplaceKeyFrame(ts, P->stateZero());
+ auto C = CaptureBase::emplace<CaptureVoid>(F, ts, nullptr);
+ auto f = FeatureBase::emplace<FeatureBase>(C, "FeaturePose2d", Vector3d::Zero(), Matrix3d::Identity());
+ auto c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false);
+
+ ts += 1.0;
+
+ if (P->getTrajectory()->getFrameMap().size() > 10)
+ (*P->getTrajectory()->begin())->remove();
+
+ if (std::chrono::duration_cast<std::chrono::duration<double>>(std::chrono::high_resolution_clock::now() - start).count() > Dt)
+ break;
+ }
+
+ t.join();
}
int main(int argc, char **argv)
diff --git a/test/gtest_solver_manager.cpp b/test/gtest_solver_manager.cpp
index 7e212b9971b5bf8fb1a46f94141ce61a1a5aab08..5226e125d79e059b29fbbfc3a432ce72452ef43c 100644
--- a/test/gtest_solver_manager.cpp
+++ b/test/gtest_solver_manager.cpp
@@ -13,8 +13,8 @@
#include "core/state_block/state_block.h"
#include "core/capture/capture_void.h"
#include "core/factor/factor_pose_2d.h"
-#include "core/state_block/local_parametrization_base.h"
-#include "core/state_block/local_parametrization_angle.h"
+#include "core/factor/factor_block_absolute.h"
+#include "core/state_block/local_parametrization_quaternion.h"
#include "dummy/solver_manager_dummy.h"
#include <iostream>
@@ -23,562 +23,1527 @@
using namespace wolf;
using namespace Eigen;
+/*
+ * Following tests are the same as in gtest_solver_ceres.cpp
+ * (modifications should be applied to both tests)
+ */
+
+StateBlockPtr createStateBlock()
+{
+ Vector4d state; state << 1, 0, 0, 0;
+ return std::make_shared<StateBlock>(state);
+}
+
+FactorBasePtr createFactor(StateBlockPtr sb_ptr)
+{
+ return std::make_shared<FactorBlockAbsolute>(std::make_shared<FeatureBase>("Dummy",
+ VectorXd::Zero(sb_ptr->getSize()),
+ MatrixXd::Identity(sb_ptr->getSize(),sb_ptr->getSize())),
+ sb_ptr,
+ nullptr,
+ false);
+}
+
TEST(SolverManager, Create)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
- // check double pointers to branches
- ASSERT_EQ(P, solver_manager_ptr->getProblem());
+ // check pointers
+ EXPECT_EQ(P, solver_ptr->getProblem());
+
+ // run solver check
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, AddStateBlock)
+////////////////////////////////////////////////////////
+// FLOATING STATE BLOCKS
+TEST(SolverManager, FloatingStateBlock_Add)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
- // add stateblock
+ // notify stateblock
P->notifyStateBlock(sb_ptr,ADD);
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// check stateblock
- ASSERT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, DoubleAddStateBlock)
+TEST(SolverManager, FloatingStateBlock_DoubleAdd)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
- // add stateblock
+ // notify stateblock
P->notifyStateBlock(sb_ptr,ADD);
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
- // add stateblock again
+ // notify stateblock again
P->notifyStateBlock(sb_ptr,ADD);
+ // check notifications
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// check stateblock
- ASSERT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, UpdateStateBlock)
+TEST(SolverManager, FloatingStateBlock_AddFix)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
- // add stateblock
+ // notify stateblock
P->notifyStateBlock(sb_ptr,ADD);
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
// check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_FALSE(sb_ptr->fixUpdated());
- ASSERT_FALSE(sb_ptr->localParamUpdated());
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_FALSE(sb_ptr->fixUpdated());
- ASSERT_FALSE(sb_ptr->localParamUpdated());
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
// check stateblock unfixed
- ASSERT_FALSE(solver_manager_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
// Fix frame
sb_ptr->fix();
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
// check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_TRUE(sb_ptr->fixUpdated());
- ASSERT_FALSE(sb_ptr->localParamUpdated());
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_TRUE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
// update solver manager
- solver_manager_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_FALSE(sb_ptr->fixUpdated());
- ASSERT_FALSE(sb_ptr->localParamUpdated());
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
// check stateblock fixed
- ASSERT_TRUE(solver_manager_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, AddUpdateStateBlock)
+TEST(SolverManager, FloatingStateBlock_AddFixed)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
- // add stateblock
+ // notify stateblock
P->notifyStateBlock(sb_ptr,ADD);
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
// check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_FALSE(sb_ptr->fixUpdated());
- ASSERT_FALSE(sb_ptr->localParamUpdated());
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
// Fix state block
sb_ptr->fix();
// check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_TRUE(sb_ptr->fixUpdated());
- ASSERT_FALSE(sb_ptr->localParamUpdated());
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_TRUE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
// update solver manager
- solver_manager_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_FALSE(sb_ptr->fixUpdated());
- ASSERT_FALSE(sb_ptr->localParamUpdated());
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
// check stateblock fixed
- ASSERT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
- ASSERT_TRUE(solver_manager_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, AddUpdateLocalParamStateBlock)
+TEST(SolverManager, FloatingStateBlock_AddUpdateLocalParam)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
- // add stateblock
+ // notify stateblock
P->notifyStateBlock(sb_ptr,ADD);
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
// check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_FALSE(sb_ptr->fixUpdated());
- ASSERT_FALSE(sb_ptr->localParamUpdated());
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
// Local param
- LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationAngle>();
+ LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationQuaternionLocal>();
sb_ptr->setLocalParametrization(local_ptr);
// Fix state block
sb_ptr->fix();
// check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_TRUE(sb_ptr->fixUpdated());
- ASSERT_TRUE(sb_ptr->localParamUpdated());
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_TRUE(sb_ptr->fixUpdated());
+ EXPECT_TRUE(sb_ptr->localParamUpdated());
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// update solver manager
- solver_manager_ptr->update();
+ solver_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_FALSE(sb_ptr->fixUpdated());
- ASSERT_FALSE(sb_ptr->localParamUpdated());
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
// check stateblock fixed
- ASSERT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
- ASSERT_TRUE(solver_manager_ptr->isStateBlockFixed(sb_ptr));
- ASSERT_TRUE(solver_manager_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, AddLocalParamRemoveLocalParamStateBlock)
+TEST(SolverManager, FloatingStateBlock_AddLocalParamRemoveLocalParam)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
// Local param
- LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationAngle>();
+ LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationQuaternionLocal>();
sb_ptr->setLocalParametrization(local_ptr);
// check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_FALSE(sb_ptr->fixUpdated());
- ASSERT_TRUE(sb_ptr->localParamUpdated());
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_TRUE(sb_ptr->localParamUpdated());
- // add stateblock
+ // notify stateblock
P->notifyStateBlock(sb_ptr,ADD);
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
// update solver manager
- solver_manager_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// check stateblock localparam
- ASSERT_TRUE(solver_manager_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
+ EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
// check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_FALSE(sb_ptr->fixUpdated());
- ASSERT_FALSE(sb_ptr->localParamUpdated());
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
// Remove local param
sb_ptr->removeLocalParametrization();
// check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_FALSE(sb_ptr->fixUpdated());
- ASSERT_TRUE(sb_ptr->localParamUpdated());
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_TRUE(sb_ptr->localParamUpdated());
// update solver manager
- solver_manager_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// check stateblock localparam
- ASSERT_FALSE(solver_manager_ptr->hasLocalParametrization(sb_ptr));
+ EXPECT_FALSE(solver_ptr->hasLocalParametrization(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, RemoveStateBlock)
+TEST(SolverManager, FloatingStateBlock_Remove)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
- // add stateblock
+ // notify stateblock
P->notifyStateBlock(sb_ptr,ADD);
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// remove state_block
P->notifyStateBlock(sb_ptr,REMOVE);
+ // check notifications
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(REMOVE, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// check stateblock
- ASSERT_FALSE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, AddRemoveStateBlock)
+TEST(SolverManager, FloatingStateBlock_AddRemove)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
- // add stateblock
+ // notify stateblock
P->notifyStateBlock(sb_ptr,ADD);
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
// remove state_block
- P->notifyStateBlock(sb_ptr,REMOVE);
+ P->notifyStateBlock(sb_ptr,REMOVE); // should cancel out the ADD
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// check state block
- ASSERT_FALSE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, RemoveUpdateStateBlock)
+TEST(SolverManager, FloatingStateBlock_AddRemoveAdd)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
- // add state_block
+ // notify stateblock
P->notifyStateBlock(sb_ptr,ADD);
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
// remove state_block
- P->notifyStateBlock(sb_ptr,REMOVE);
+ P->notifyStateBlock(sb_ptr,REMOVE); // should cancell out the ADD
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD); // again ADD in notification list
+
+ // check notifications
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check state block
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, DoubleRemoveStateBlock)
+TEST(SolverManager, FloatingStateBlock_DoubleRemove)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
- // add stateblock
+ // notify stateblock
P->notifyStateBlock(sb_ptr,ADD);
+ // update solver
+ solver_ptr->update();
+
// remove state_block
P->notifyStateBlock(sb_ptr,REMOVE);
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update();
// remove state_block
P->notifyStateBlock(sb_ptr,REMOVE);
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(REMOVE, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
// update solver manager
- solver_manager_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // checks
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, AddUpdatedStateBlock)
+TEST(SolverManager, FloatingStateBlock_AddUpdated)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
// Fix
sb_ptr->fix();
// Set State
- Vector2d state_2 = 2*state;
- sb_ptr->setState(state_2);
+ sb_ptr->setState(2*sb_ptr->getState());
// Check flags have been set true
- ASSERT_TRUE(sb_ptr->fixUpdated());
- ASSERT_TRUE(sb_ptr->stateUpdated());
+ EXPECT_TRUE(sb_ptr->fixUpdated());
+ EXPECT_TRUE(sb_ptr->stateUpdated());
// == When an ADD is notified: a ADD notification should be stored ==
- // add state_block
+ // notify state block
P->notifyStateBlock(sb_ptr,ADD);
+ // check notifications
+ // check notifications
Notification notif;
- ASSERT_EQ(P->getStateBlockNotificationMapSize(),1);
- ASSERT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
- ASSERT_EQ(notif, ADD);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),1);
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(notif, ADD);
// == Insert OTHER notifications ==
// Set State --> FLAG
- state_2 = 2*state;
- sb_ptr->setState(state_2);
+ sb_ptr->setState(2*sb_ptr->getState());
// Fix --> FLAG
sb_ptr->unfix();
// Check flag has been set true
- ASSERT_TRUE(sb_ptr->fixUpdated());
+ EXPECT_TRUE(sb_ptr->fixUpdated());
- ASSERT_EQ(P->getStateBlockNotificationMapSize(),1); // No new notifications (fix and set state are flags in sb)
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),1); // No new notifications (fix and set state are flags in sb)
// == consume empties the notification map ==
- solver_manager_ptr->update(); // it calls P->consumeStateBlockNotificationMap();
- ASSERT_EQ(P->getStateBlockNotificationMapSize(),0);
+ solver_ptr->update(); // it calls P->consumeStateBlockNotificationMap();
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
// Check flags have been reset
- ASSERT_FALSE(sb_ptr->fixUpdated());
- ASSERT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->stateUpdated());
// == When an REMOVE is notified: a REMOVE notification should be stored ==
// remove state_block
P->notifyStateBlock(sb_ptr,REMOVE);
- ASSERT_EQ(P->getStateBlockNotificationMapSize(),1);
- ASSERT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
- ASSERT_EQ(notif, REMOVE);
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),1);
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(notif, REMOVE);
// == REMOVE + ADD: notification map should be empty ==
P->notifyStateBlock(sb_ptr,ADD);
- ASSERT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
// == UPDATES should clear the list of notifications ==
- // add state_block
+ // notify state block
P->notifyStateBlock(sb_ptr,ADD);
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update();
- ASSERT_EQ(P->getStateBlockNotificationMapSize(),0); // After solver_manager->update, notifications should be empty
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); // After solver_manager->update, notifications should be empty
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// == ADD + REMOVE: notification map should be empty ==
P->notifyStateBlock(sb_ptr,ADD);
P->notifyStateBlock(sb_ptr,REMOVE);
- ASSERT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
}
-TEST(SolverManager, AddFactor)
+////////////////////////////////////////////////////////
+// STATE BLOCKS AND FACTOR
+TEST(SolverManager, StateBlockAndFactor_Add)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
- // Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceFrame(KEY, 0, P->stateZero() );
- auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
- auto c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false);
+ // notify stateblock (floating for the moment)
+ P->notifyStateBlock(sb_ptr,ADD);
- // notification
+ // check notifications
Notification notif;
- ASSERT_TRUE(P->getFactorNotification(c,notif));
- ASSERT_EQ(notif, ADD);
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update();
+
+ // checks
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+
+ // notify factor (state block now not floating)
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
- // check factor
- ASSERT_TRUE(solver_manager_ptr->isFactorRegistered(c));
+ // update solver
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // checks
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, RemoveFactor)
+TEST(SolverManager, StateBlockAndFactor_DoubleAdd)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
- // Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceFrame(KEY, 0, P->stateZero() );
- auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
- auto c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false);
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // notify stateblock again
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
- // add factor
- P->notifyFactor(c,REMOVE);
+ // check notifications
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
- // notification
+ // update solver
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check stateblock
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverManager, StateBlockAndFactor_Fix)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
Notification notif;
- ASSERT_TRUE(P->getFactorNotification(c,notif));
- ASSERT_EQ(notif, REMOVE);
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // check stateblock unfixed
+ EXPECT_FALSE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+
+ // Fix frame
+ sb_ptr->fix();
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_TRUE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // update solver manager
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
- // check factor
- ASSERT_FALSE(solver_manager_ptr->isFactorRegistered(c));
+ // check stateblock fixed
+ EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, AddRemoveFactor)
+TEST(SolverManager, StateBlockAndFactor_AddFixed)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
- // Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceFrame(KEY, 0, P->stateZero() );
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
- auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
- auto c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false);
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
- // notification
+ // check notifications
Notification notif;
- ASSERT_TRUE(P->getFactorNotification(c,notif));
- ASSERT_EQ(notif, ADD);
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
- // add factor
- P->notifyFactor(c,REMOVE);
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
- // notification
- ASSERT_EQ(P->getFactorNotificationMapSize(), 0); // ADD+REMOVE cancels out
- ASSERT_FALSE(P->getFactorNotification(c, notif)); // ADD+REMOVE cancels out
+ // Fix state block
+ sb_ptr->fix();
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_TRUE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // update solver manager
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // check stateblock fixed
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverManager, StateBlockAndFactor_UpdateLocalParam)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // Local param
+ LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationQuaternionLocal>();
+ sb_ptr->setLocalParametrization(local_ptr);
+
+ // Fix state block
+ sb_ptr->fix();
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_TRUE(sb_ptr->fixUpdated());
+ EXPECT_TRUE(sb_ptr->localParamUpdated());
+
+ // update solver manager
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // check stateblock fixed
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverManager, StateBlockAndFactor_AddLocalParamRemoveLocalParam)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // Local param
+ LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationQuaternionLocal>();
+ sb_ptr->setLocalParametrization(local_ptr);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_TRUE(sb_ptr->localParamUpdated());
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // update solver manager
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check solver
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->hasLocalParametrization(sb_ptr));
+ EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // Remove local param
+ sb_ptr->removeLocalParametrization();
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_TRUE(sb_ptr->localParamUpdated());
+
+ // update solver manager
+ solver_ptr->update();
+
+ // check solver
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->hasLocalParametrization(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverManager, StateBlockAndFactor_RemoveStateBlock)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update();
- // check factor
- ASSERT_FALSE(solver_manager_ptr->isFactorRegistered(c));
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // remove state_block
+ P->notifyStateBlock(sb_ptr,REMOVE);
+
+ // check notifications
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(REMOVE, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // update solver
+ solver_ptr->update(); // there is a factor involved, removal posponed (REMOVE in notification list)
+
+ // check notifications
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(REMOVE, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // checks
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, DoubleRemoveFactor)
+TEST(SolverManager, StateBlockAndFactor_RemoveFactor)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // update solver
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // remove state_block
+ P->notifyFactor(fac_ptr,REMOVE); // state block should be automatically stored as floating
+
+ // check notifications
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(REMOVE, notif);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+
+ // update solver
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // checks
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverManager, StateBlockAndFactor_AddRemoveStateBlock)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // remove state_block
+ P->notifyStateBlock(sb_ptr,REMOVE); // cancells out ADD notification
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // update solver
+ solver_ptr->update(); // factor ADD notification is not executed since state block involved is missing (ADD notification added)
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // check state block
+ EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverManager, StateBlockAndFactor_AddRemoveFactor)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // remove state_block
+ P->notifyFactor(fac_ptr,REMOVE); // cancells out ADD notification
+
+ // check notifications
+ EXPECT_EQ(P->getFactorNotificationMapSize(),0);
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // update solver
+ solver_ptr->update(); // state block should be automatically stored as floating
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // checks
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverManager, StateBlockAndFactor_DoubleRemoveStateBlock)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // remove state_block
+ P->notifyStateBlock(sb_ptr,REMOVE); // cancells out the ADD
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // update solver
+ solver_ptr->update(); // factor ADD is posponed
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // remove state_block
+ P->notifyStateBlock(sb_ptr,REMOVE);
+
+ // check notifications
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(REMOVE, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // update solver manager
+ solver_ptr->update(); // repeated REMOVE should be ignored
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // check state block
+ EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverManager, StateBlockAndFactor_DoubleRemoveFactor)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // remove state_block
+ P->notifyFactor(fac_ptr,REMOVE); // cancells out ADD
+
+ // check notifications
+ EXPECT_EQ(P->getFactorNotificationMapSize(),0);
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // update solver
+ solver_ptr->update(); // state block should be automatically stored as floating
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(),0);
+
+ // remove state_block
+ P->notifyFactor(fac_ptr,REMOVE);
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(REMOVE, notif);
+
+ // update solver
+ solver_ptr->update(); // repeated REMOVE should be ignored
+
+ // checks
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverManager, StateBlockAndFactor_AddUpdatedStateBlock)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
- // Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceFrame(KEY, TimeStamp(0), P->stateZero() );
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
- auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
- auto c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false);
+ // Fix
+ sb_ptr->fix();
+
+ // Change State
+ sb_ptr->setState(2*sb_ptr->getState());
+
+ // Check flags have been set true
+ EXPECT_TRUE(sb_ptr->fixUpdated());
+ EXPECT_TRUE(sb_ptr->stateUpdated());
+
+ // == When an ADD is notified: a ADD notification should be stored ==
+
+ // notify state block
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // == Insert OTHER notifications ==
+
+ // Set State --> FLAG
+ sb_ptr->setState(2*sb_ptr->getState());
+
+ // Fix --> FLAG
+ sb_ptr->unfix();
+ // Check flag has been set true
+ EXPECT_TRUE(sb_ptr->fixUpdated());
+
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),1); // No new notifications (fix and set state are flags in sb)
+
+ // == consume empties the notification map ==
+ solver_ptr->update(); // it calls P->consumeStateBlockNotificationMap();
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ // Check flags have been reset
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+
+ // == When an REMOVE is notified: a REMOVE notification should be stored ==
+
+ // remove state_block
+ P->notifyStateBlock(sb_ptr,REMOVE);
+
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),1);
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(notif, REMOVE);
+
+ // == REMOVE + ADD: notification map should be empty ==
+ P->notifyStateBlock(sb_ptr,ADD);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+
+ // == UPDATES should clear the list of notifications ==
+ // notify state block
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // update solver
+ solver_ptr->update();
+
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); // After solver_manager->update, notifications should be empty
+
+ // == ADD + REMOVE: notification map should be empty ==
+ P->notifyStateBlock(sb_ptr,ADD);
+ P->notifyStateBlock(sb_ptr,REMOVE);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+}
+
+////////////////////////////////////////////////////////
+// ONLY FACTOR
+TEST(SolverManager, OnlyFactor_Add)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify factor (state block has not been notified)
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // update solver
+ solver_ptr->update(); // factor ADD should be posponed (in the notification list again)
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // checks
+ EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverManager, OnlyFactor_Remove)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify factor (state block has not been notified)
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update(); // ADD factor should be posponed (in the notification list again)
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,REMOVE);
- // remove factor
- P->notifyFactor(c,REMOVE);
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(),0);
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update(); // nothing to update
- // remove factor
- P->notifyFactor(c,REMOVE);
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(),0);
+
+ // checks
+ EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverManager, OnlyFactor_AddRemove)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify factor (state block has not been notified)
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,REMOVE);
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(),0);
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update(); // nothing to update
- // check factor
- ASSERT_FALSE(solver_manager_ptr->isFactorRegistered(c));
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(),0);
+
+ // checks
+ EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
+////////////////////////////////////////////////////////
+// MULTITHREADING
+
TEST(SolverManager, MultiThreadingTruncatedNotifications)
{
double Dt = 5.0;
ProblemPtr P = Problem::create("PO", 2);
- SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
// loop updating (without sleep)
std::thread t([&](){
auto start_t = std::chrono::high_resolution_clock::now();
while (true)
{
- solver_manager_ptr->update();
+ solver_ptr->update();
+ ASSERT_TRUE(solver_ptr->check());
if (std::chrono::duration_cast<std::chrono::duration<double>>(std::chrono::high_resolution_clock::now() - start_t).count() > Dt)
break;
}});
// loop emplacing and removing frames (window of 10 KF)
auto start = std::chrono::high_resolution_clock::now();
+ TimeStamp ts(0);
while (true)
{
// Emplace Frame, Capture, feature and factor pose 2d
- FrameBasePtr F = P->emplaceFrame(KEY, TimeStamp(0), P->stateZero());
- auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
+ FrameBasePtr F = P->emplaceKeyFrame(ts, P->stateZero());
+ auto C = CaptureBase::emplace<CaptureVoid>(F, ts, nullptr);
auto f = FeatureBase::emplace<FeatureBase>(C, "FeaturePose2d", Vector3d::Zero(), Matrix3d::Identity());
auto c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false);
- if (P->getTrajectory()->getFrameList().size() > 10)
- P->getTrajectory()->getFrameList().front()->remove();
+ ts += 1.0;
+
+ if (P->getTrajectory()->getFrameMap().size() > 10)
+ (*P->getTrajectory()->begin())->remove();
if (std::chrono::duration_cast<std::chrono::duration<double>>(std::chrono::high_resolution_clock::now() - start).count() > Dt)
break;
diff --git a/test/gtest_track_matrix.cpp b/test/gtest_track_matrix.cpp
index 4e539e24b7554a7a2e5cfe88096cc4c861de4018..72bf3f7d8ce8124c4005daf4a9a82ba119504d5e 100644
--- a/test/gtest_track_matrix.cpp
+++ b/test/gtest_track_matrix.cpp
@@ -22,9 +22,11 @@ class TrackMatrixTest : public testing::Test
FrameBasePtr F0, F1, F2, F3, F4;
CaptureBasePtr C0, C1, C2, C3, C4;
FeatureBasePtr f0, f1, f2, f3, f4;
+ ProblemPtr problem;
void SetUp() override
{
+ problem = Problem::create("PO", 2);
// unlinked captures
// Some warnings will be thrown "linking with nullptr" for emplacing without providing frame pointer
C0 = CaptureBase::emplace<CaptureBase>(nullptr, "CaptureBase", 0.0);
@@ -35,11 +37,11 @@ class TrackMatrixTest : public testing::Test
// unlinked frames
// Some warnings will be thrown "linking with nullptr" for emplacing without providing frame pointer
- F0 = FrameBase::emplace<FrameBase>(nullptr, NON_ESTIMATED, 0.0, nullptr);
- F1 = FrameBase::emplace<FrameBase>(nullptr, NON_ESTIMATED, 1.0, nullptr);
- F2 = FrameBase::emplace<FrameBase>(nullptr, NON_ESTIMATED, 2.0, nullptr);
- F3 = FrameBase::emplace<FrameBase>(nullptr, NON_ESTIMATED, 3.0, nullptr);
- F4 = FrameBase::emplace<FrameBase>(nullptr, NON_ESTIMATED, 4.0, nullptr);
+ F0 = FrameBase::createNonKeyFrame<FrameBase>(0.0, nullptr);
+ F1 = FrameBase::createNonKeyFrame<FrameBase>(1.0, nullptr);
+ F2 = FrameBase::createNonKeyFrame<FrameBase>(2.0, nullptr);
+ F3 = FrameBase::createNonKeyFrame<FrameBase>(3.0, nullptr);
+ F4 = FrameBase::createNonKeyFrame<FrameBase>(4.0, nullptr);
// unlinked features
// Some warnings will be thrown "linking with nullptr" for emplacing without providing capture pointer
@@ -50,8 +52,8 @@ class TrackMatrixTest : public testing::Test
f4 = FeatureBase::emplace<FeatureBase>(nullptr, "FeatureBase", m, m_cov);
// F0 and F4 are keyframes
- F0->setKey();
- F4->setKey();
+ F0->setKey(problem);
+ F4->setKey(problem);
// link captures
C0->link(F0);
diff --git a/test/gtest_trajectory.cpp b/test/gtest_trajectory.cpp
index 00f7d83fff99ff39b16fc8a67c2eee9b767cfbe1..ff103bc097ba9d166ef76a97fcde20b15199507a 100644
--- a/test/gtest_trajectory.cpp
+++ b/test/gtest_trajectory.cpp
@@ -32,62 +32,34 @@ TEST(TrajectoryBase, ClosestKeyFrame)
// 1 2 3 4 time stamps
// --+-----+-----+-----+---> time
- FrameBasePtr F1 = P->emplaceFrame(KEY, 1, Eigen::Vector3d::Zero() );
- FrameBasePtr F2 = P->emplaceFrame(KEY, 2, Eigen::Vector3d::Zero() );
- FrameBasePtr F3 = P->emplaceFrame(AUXILIARY, 3, Eigen::Vector3d::Zero() );
- FrameBasePtr F4 = P->emplaceFrame(NON_ESTIMATED, 4, Eigen::Vector3d::Zero() );
+ FrameBasePtr F1 = P->emplaceKeyFrame( 1, Eigen::Vector3d::Zero() );
+ FrameBasePtr F2 = P->emplaceKeyFrame( 2, Eigen::Vector3d::Zero() );
+ // FrameBasePtr F3 = P->emplaceFrame(AUXILIARY, 3, Eigen::Vector3d::Zero() );
+ FrameBasePtr F3 = FrameBase::createNonKeyFrame<FrameBase>(3, P->getFrameStructure(),
+// P->getDim(),
+ std::list<VectorXd>({Eigen::Vector2d::Zero(),Eigen::Vector1d::Zero()}) );
+ FrameBasePtr F4 = FrameBase::createNonKeyFrame<FrameBase>(4, P->getFrameStructure(),
+// P->getDim(),
+ std::list<VectorXd>({Eigen::Vector2d::Zero(),Eigen::Vector1d::Zero()}) );
FrameBasePtr KF; // closest key-frame queried
- KF = T->closestKeyFrameToTimeStamp(-0.8); // before all keyframes --> return F1
+ KF = T->closestFrameToTimeStamp(-0.8); // before all keyframes --> return F1
ASSERT_EQ(KF->id(), F1->id()); // same id!
- KF = T->closestKeyFrameToTimeStamp(1.1); // between keyframes --> return F1
+ KF = T->closestFrameToTimeStamp(1.1); // between keyframes --> return F1
ASSERT_EQ(KF->id(), F1->id()); // same id!
- KF = T->closestKeyFrameToTimeStamp(1.9); // between keyframes --> return F2
+ KF = T->closestFrameToTimeStamp(1.9); // between keyframes --> return F2
ASSERT_EQ(KF->id(), F2->id()); // same id!
- KF = T->closestKeyFrameToTimeStamp(2.6); // between keyframe and auxiliary frame, but closer to auxiliary frame --> return F2
+ KF = T->closestFrameToTimeStamp(2.6); // between keyframe and auxiliary frame, but closer to auxiliary frame --> return F2
ASSERT_EQ(KF->id(), F2->id()); // same id!
- KF = T->closestKeyFrameToTimeStamp(3.2); // after the auxiliary frame, between closer to frame --> return F2
+ KF = T->closestFrameToTimeStamp(3.2); // after the auxiliary frame, between closer to frame --> return F2
ASSERT_EQ(KF->id(), F2->id()); // same id!
- KF = T->closestKeyFrameToTimeStamp(4.2); // after the last frame --> return F2
- ASSERT_EQ(KF->id(), F2->id()); // same id!
-}
-
-TEST(TrajectoryBase, ClosestKeyOrAuxFrame)
-{
-
- ProblemPtr P = Problem::create("PO", 2);
- TrajectoryBasePtr T = P->getTrajectory();
-
- // Trajectory status:
- // KF1 KF2 F3 frames
- // 1 2 3 time stamps
- // --+-----+-----+---> time
-
- FrameBasePtr F1 = P->emplaceFrame(KEY, 1, Eigen::Vector3d::Zero() );
- FrameBasePtr F2 = P->emplaceFrame(AUXILIARY, 2, Eigen::Vector3d::Zero() );
- FrameBasePtr F3 = P->emplaceFrame(NON_ESTIMATED, 3, Eigen::Vector3d::Zero() );
-
- FrameBasePtr KF; // closest key-frame queried
-
- KF = T->closestKeyOrAuxFrameToTimeStamp(-0.8); // before all keyframes --> return f0
- ASSERT_EQ(KF->id(), F1->id()); // same id!
-
- KF = T->closestKeyOrAuxFrameToTimeStamp(1.1); // between keyframes --> return F1
- ASSERT_EQ(KF->id(), F1->id()); // same id!
-
- KF = T->closestKeyOrAuxFrameToTimeStamp(1.9); // between keyframes --> return F2
- ASSERT_EQ(KF->id(), F2->id()); // same id!
-
- KF = T->closestKeyOrAuxFrameToTimeStamp(2.6); // between keyframe and frame, but closer to frame --> return F2
- ASSERT_EQ(KF->id(), F2->id()); // same id!
-
- KF = T->closestKeyOrAuxFrameToTimeStamp(3.2); // after the last frame --> return F2
+ KF = T->closestFrameToTimeStamp(4.2); // after the last frame --> return F2
ASSERT_EQ(KF->id(), F2->id()); // same id!
}
@@ -108,28 +80,29 @@ TEST(TrajectoryBase, Add_Remove_Frame)
std::cout << __LINE__ << std::endl;
// add F1
- FrameBasePtr F1 = P->emplaceFrame(KEY, 1, Eigen::Vector3d::Zero()); // 2 non-fixed
+ FrameBasePtr F1 = P->emplaceKeyFrame(1, Eigen::Vector3d::Zero()); // 2 non-fixed
if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getFrameList(). size(), (SizeStd) 1);
+ ASSERT_EQ(T->getFrameMap(). size(), (SizeStd) 1);
ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 2);
std::cout << __LINE__ << std::endl;
// add F2
- FrameBasePtr F2 = P->emplaceFrame(KEY, 2, Eigen::Vector3d::Zero()); // 1 fixed, 1 not
+ FrameBasePtr F2 = P->emplaceKeyFrame(2, Eigen::Vector3d::Zero()); // 1 fixed, 1 not
if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getFrameList(). size(), (SizeStd) 2);
+ ASSERT_EQ(T->getFrameMap(). size(), (SizeStd) 2);
ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 4);
std::cout << __LINE__ << std::endl;
// add F3
- FrameBasePtr F3 = P->emplaceFrame(NON_ESTIMATED, 3, Eigen::Vector3d::Zero());
+ FrameBasePtr F3 = FrameBase::createNonKeyFrame<FrameBase>(3, P->getFrameStructure(),
+// P->getDim(),
+ std::list<VectorXd>({Eigen::Vector2d::Zero(),Eigen::Vector1d::Zero()}));
if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getFrameList(). size(), (SizeStd) 3);
+ ASSERT_EQ(T->getFrameMap(). size(), (SizeStd) 2);
ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 4);
std::cout << __LINE__ << std::endl;
- ASSERT_EQ(T->getLastFrame()->id(), F3->id());
- ASSERT_EQ(T->getLastKeyFrame()->id(), F2->id());
+ ASSERT_EQ(T->getLastFrame()->id(), F2->id());
std::cout << __LINE__ << std::endl;
N.update();
@@ -139,24 +112,23 @@ TEST(TrajectoryBase, Add_Remove_Frame)
// remove frames and keyframes
F2->remove(); // KF
if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getFrameList(). size(), (SizeStd) 2);
+ ASSERT_EQ(T->getFrameMap(). size(), (SizeStd) 1);
ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 2);
std::cout << __LINE__ << std::endl;
- ASSERT_EQ(T->getLastFrame()->id(), F3->id());
- ASSERT_EQ(T->getLastKeyFrame()->id(), F1->id());
+ ASSERT_EQ(T->getLastFrame()->id(), F1->id());
std::cout << __LINE__ << std::endl;
F3->remove(); // F
if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getFrameList(). size(), (SizeStd) 1);
+ ASSERT_EQ(T->getFrameMap(). size(), (SizeStd) 1);
std::cout << __LINE__ << std::endl;
- ASSERT_EQ(T->getLastKeyFrame()->id(), F1->id());
+ ASSERT_EQ(T->getLastFrame()->id(), F1->id());
F1->remove(); // KF
if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getFrameList(). size(), (SizeStd) 0);
+ ASSERT_EQ(T->getFrameMap(). size(), (SizeStd) 0);
std::cout << __LINE__ << std::endl;
N.update();
@@ -165,123 +137,6 @@ TEST(TrajectoryBase, Add_Remove_Frame)
std::cout << __LINE__ << std::endl;
}
-TEST(TrajectoryBase, KeyFramesAreSorted)
-{
- using std::make_shared;
-
- ProblemPtr P = Problem::create("PO", 2);
- TrajectoryBasePtr T = P->getTrajectory();
-
- // Trajectory status:
- // KF1 KF2 F3 frames
- // 1 2 3 time stamps
- // --+-----+-----+---> time
-
- // add frames and keyframes in random order --> keyframes must be sorted after that
- FrameBasePtr F2 = P->emplaceFrame(KEY, 2, Eigen::Vector3d::Zero());
- if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getLastFrame() ->id(), F2->id());
- ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F2->id());
- ASSERT_EQ(T->getLastKeyFrame() ->id(), F2->id());
-
- FrameBasePtr F3 = P->emplaceFrame(NON_ESTIMATED, 3, Eigen::Vector3d::Zero()); // non-key-frame
- if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getLastFrame() ->id(), F3->id());
- ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F2->id());
- ASSERT_EQ(T->getLastKeyFrame() ->id(), F2->id());
-
- FrameBasePtr F1 = P->emplaceFrame(KEY, 1, Eigen::Vector3d::Zero());
- if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getLastFrame() ->id(), F3->id());
- ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F2->id());
- ASSERT_EQ(T->getLastKeyFrame() ->id(), F2->id());
-
- F3->setKey(); // set KF3
- if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getLastFrame() ->id(), F3->id());
- ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F3->id());
- ASSERT_EQ(T->getLastKeyFrame() ->id(), F3->id());
-
- auto F4 = P->emplaceFrame(NON_ESTIMATED, 1.5, Eigen::Vector3d::Zero());
- // Trajectory status:
- // KF1 KF2 KF3 F4 frames
- // 1 2 3 1.5 time stamps
- // --+-----+-----+------+---> time
- if (debug) P->print(2,0,1,0);
- ASSERT_EQ(T->getLastFrame() ->id(), F4->id());
- ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F3->id());
- ASSERT_EQ(T->getLastKeyFrame() ->id(), F3->id());
-
- F4->setKey();
- // Trajectory status:
- // KF1 KF4 KF2 KF3 frames
- // 1 1.5 2 3 time stamps
- // --+-----+-----+------+---> time
- if (debug) P->print(2,0,1,0);
- ASSERT_EQ(T->getLastFrame() ->id(), F3->id());
- ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F3->id());
- ASSERT_EQ(T->getLastKeyFrame() ->id(), F3->id());
-
- F2->setTimeStamp(4);
- // Trajectory status:
- // KF1 KF4 KF3 KF2 frames
- // 1 1.5 3 4 time stamps
- // --+-----+-----+------+---> time
- if (debug) P->print(2,0,1,0);
- ASSERT_EQ(T->getLastFrame() ->id(), F2->id());
- ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F2->id());
- ASSERT_EQ(T->getLastKeyFrame() ->id(), F2->id());
-
- F4->setTimeStamp(0.5);
- // Trajectory status:
- // KF4 KF2 KF3 KF2 frames
- // 0.5 1 3 4 time stamps
- // --+-----+-----+------+---> time
- if (debug) P->print(2,0,1,0);
- ASSERT_EQ(T->getFrameList().front()->id(), F4->id());
-
- auto F5 = P->emplaceFrame(AUXILIARY, 1.5, Eigen::Vector3d::Zero());
- // Trajectory status:
- // KF4 KF2 AuxF5 KF3 KF2 frames
- // 0.5 1 1.5 3 4 time stamps
- // --+-----+-----+-----+-----+---> time
- if (debug) P->print(2,0,1,0);
- ASSERT_EQ(T->getLastFrame() ->id(), F2->id());
- ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F2->id());
- ASSERT_EQ(T->getLastKeyFrame() ->id(), F2->id());
-
- F5->setTimeStamp(5);
- // Trajectory status:
- // KF4 KF2 KF3 KF2 AuxF5 frames
- // 0.5 1 3 4 5 time stamps
- // --+-----+-----+-----+-----+---> time
- if (debug) P->print(2,0,1,0);
- ASSERT_EQ(T->getLastFrame() ->id(), F5->id());
- ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F5->id());
- ASSERT_EQ(T->getLastKeyFrame() ->id(), F2->id());
-
- auto F6 = P->emplaceFrame(NON_ESTIMATED, 6, Eigen::Vector3d::Zero());
- // Trajectory status:
- // KF4 KF2 KF3 KF2 AuxF5 F6 frames
- // 0.5 1 3 4 5 6 time stamps
- // --+-----+-----+-----+-----+-----+---> time
- if (debug) P->print(2,0,1,0);
- ASSERT_EQ(T->getLastFrame() ->id(), F6->id());
- ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F5->id());
- ASSERT_EQ(T->getLastKeyFrame() ->id(), F2->id());
-
- auto F7 = P->emplaceFrame(NON_ESTIMATED, 5.5, Eigen::Vector3d::Zero());
- // Trajectory status:
- // KF4 KF2 KF3 KF2 AuxF5 F7 F6 frames
- // 0.5 1 3 4 5 5.5 6 time stamps
- // --+-----+-----+-----+-----+-----+-----+---> time
- if (debug) P->print(2,0,1,0);
- ASSERT_EQ(T->getLastFrame() ->id(), F7->id()); //Only auxiliary and key-frames are sorted
- ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F5->id());
- ASSERT_EQ(T->getLastKeyFrame() ->id(), F2->id());
-
-}
-
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
diff --git a/test/gtest_tree_manager.cpp b/test/gtest_tree_manager.cpp
index 1d97e2925487f9e363e1292b6ef2c2a82c29fc96..15bb1778940d6e2d1e80ad05e195caba66b6417d 100644
--- a/test/gtest_tree_manager.cpp
+++ b/test/gtest_tree_manager.cpp
@@ -94,7 +94,7 @@ TEST(TreeManager, keyFrameCallback)
ASSERT_EQ(GM->n_KF_, 0);
- auto F0 = P->emplaceFrame(KEY, 0, "PO", 3, VectorXd(7) );
+ auto F0 = P->emplaceKeyFrame(0, "PO", 3, VectorXd(7) );
P->keyFrameCallback(F0, nullptr, 0);
ASSERT_EQ(GM->n_KF_, 1);
diff --git a/test/gtest_tree_manager_sliding_window.cpp b/test/gtest_tree_manager_sliding_window.cpp
index 5cbc9550cc9a751a57046807f236486f4d5cbb92..b8ff8240e7e2c7e97fb4f56aeadb72d2b51a0027 100644
--- a/test/gtest_tree_manager_sliding_window.cpp
+++ b/test/gtest_tree_manager_sliding_window.cpp
@@ -79,7 +79,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
P->applyPriorOptions(0);
// FRAME 1 ----------------------------------------------------------
- auto F1 = P->getTrajectory()->getLastKeyFrame();
+ auto F1 = P->getTrajectory()->getLastFrame();
ASSERT_TRUE(F1 != nullptr);
Vector7d state = F1->getStateVector();
@@ -87,7 +87,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
Matrix6d cov = Matrix6d::Identity();
// FRAME 2 ----------------------------------------------------------
- auto F2 = P->emplaceFrame(KEY, TimeStamp(2), "PO", 3, state);
+ auto F2 = P->emplaceKeyFrame(TimeStamp(2), "PO", 3, state);
P->keyFrameCallback(F2, nullptr, 0);
// absolute factor
@@ -103,7 +103,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
ASSERT_FALSE(F1->isRemoving());
// FRAME 3 ----------------------------------------------------------
- auto F3 = P->emplaceFrame(KEY, TimeStamp(3), "PO", 3, state);
+ auto F3 = P->emplaceKeyFrame(TimeStamp(3), "PO", 3, state);
P->keyFrameCallback(F3, nullptr, 0);
// absolute factor
@@ -119,7 +119,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
ASSERT_FALSE(F1->isRemoving());
// FRAME 4 ----------------------------------------------------------
- auto F4 = P->emplaceFrame(KEY, TimeStamp(4), "PO", 3, state);
+ auto F4 = P->emplaceKeyFrame(TimeStamp(4), "PO", 3, state);
P->keyFrameCallback(F4, nullptr, 0);
// absolute factor
@@ -139,7 +139,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
ASSERT_TRUE(F2->isFixed()); //Fixed
// FRAME 5 ----------------------------------------------------------
- auto F5 = P->emplaceFrame(KEY, TimeStamp(5), "PO", 3, state);
+ auto F5 = P->emplaceKeyFrame(TimeStamp(5), "PO", 3, state);
P->keyFrameCallback(F5, nullptr, 0);
// absolute factor
@@ -175,7 +175,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
P->applyPriorOptions(0);
// FRAME 1 (prior) ----------------------------------------------------------
- auto F1 = P->getTrajectory()->getLastKeyFrame();
+ auto F1 = P->getTrajectory()->getLastFrame();
ASSERT_TRUE(F1 != nullptr);
Vector7d state = F1->getStateVector();
@@ -183,7 +183,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
Matrix6d cov = Matrix6d::Identity();
// FRAME 2 ----------------------------------------------------------
- auto F2 = P->emplaceFrame(KEY, TimeStamp(2), "PO", 3, state);
+ auto F2 = P->emplaceKeyFrame(TimeStamp(2), "PO", 3, state);
P->keyFrameCallback(F2, nullptr, 0);
// absolute factor
@@ -199,7 +199,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
ASSERT_FALSE(F1->isRemoving());
// FRAME 3 ----------------------------------------------------------
- auto F3 = P->emplaceFrame(KEY, TimeStamp(3), "PO", 3, state);
+ auto F3 = P->emplaceKeyFrame(TimeStamp(3), "PO", 3, state);
P->keyFrameCallback(F3, nullptr, 0);
// absolute factor
@@ -215,7 +215,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
ASSERT_FALSE(F1->isRemoving());
// FRAME 4 ----------------------------------------------------------
- auto F4 = P->emplaceFrame(KEY, TimeStamp(4), "PO", 3, state);
+ auto F4 = P->emplaceKeyFrame(TimeStamp(4), "PO", 3, state);
P->keyFrameCallback(F4, nullptr, 0);
// absolute factor
@@ -235,7 +235,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
ASSERT_FALSE(F2->isFixed()); //Not fixed
// FRAME 5 ----------------------------------------------------------
- auto F5 = P->emplaceFrame(KEY, TimeStamp(5), "PO", 3, state);
+ auto F5 = P->emplaceKeyFrame(TimeStamp(5), "PO", 3, state);
P->keyFrameCallback(F5, nullptr, 0);
// absolute factor
diff --git a/wolf_scripts/wolf_uninstall.sh b/wolf_scripts/wolf_uninstall.sh
index b381616ecf9c37423d5cd11e18bb3f5502cec641..ff022e0b25ebc31bc0d7d0e50124569896c8f6ba 100755
--- a/wolf_scripts/wolf_uninstall.sh
+++ b/wolf_scripts/wolf_uninstall.sh
@@ -1,20 +1,42 @@
#!/bin/bash
-echo “=â€
-echo “====================== UNINSTALLING WOLF ======================â€
-echo “=â€
+if [ -z "$1" ]
+then
+ echo “=â€
+ echo “====================== UNINSTALLING WOLF ======================â€
+ echo “=â€
-echo "cd /usr/local/include/iri-algorithms/"
-cd /usr/local/include/iri-algorithms/
-echo "sudo rm -rf wolf"
-sudo rm -rf wolf
+ echo "cd /usr/local/include/iri-algorithms/"
+ cd /usr/local/include/iri-algorithms/
+ echo "sudo rm -rf wolf"
+ sudo rm -rf wolf
-echo "cd /usr/local/lib/iri-algorithms/"
-echo "sudo rm libwolf*.*"
-cd /usr/local/lib/iri-algorithms/
-sudo rm libwolf*.*
+ echo "cd /usr/local/lib/iri-algorithms/"
+ echo "sudo rm libwolf*.*"
+ cd /usr/local/lib/iri-algorithms/
+ sudo rm libwolf*.*
-echo "cd /usr/local/lib/cmake/"
-echo "sudo rm -rf wolf*"
-cd /usr/local/lib/cmake/
-sudo rm -rf wolf*
\ No newline at end of file
+ echo "cd /usr/local/lib/cmake/"
+ echo "sudo rm -rf wolf*"
+ cd /usr/local/lib/cmake/
+ sudo rm -rf wolf*
+else
+ echo “=â€
+ echo “====================== UNINSTALLING PLUGIN $1 ======================â€
+ echo “=â€
+
+ echo "cd /usr/local/include/iri-algorithms/wolf"
+ cd /usr/local/include/iri-algorithms/wolf/
+ echo "sudo rm -rf plugin_$1"
+ sudo rm -rf plugin_$1
+
+ echo "cd /usr/local/lib/iri-algorithms/"
+ echo "sudo rm libwolf$1.*"
+ cd /usr/local/lib/iri-algorithms/
+ sudo rm libwolf$1.*
+
+ echo "cd /usr/local/lib/cmake/"
+ echo "sudo rm -rf wolf$1"
+ cd /usr/local/lib/cmake/
+ sudo rm -rf wolf$1
+fi