diff --git a/CMakeLists.txt b/CMakeLists.txt
index cab800a231b74a05eb9c466e0ec5d508ca561918..1f1f99f4c4bfb71389afc7fe1695355ad801f3c0 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -63,7 +63,6 @@ if(UNIX)
set(CMAKE_CXX_FLAGS
"${CMAKE_CXX_FLAGS} -Werror=all -Werror=extra -Wno-unknown-pragmas -Wno-sign-compare -Wno-unused-parameter -Wno-missing-field-initializers")
endif(UNIX)
-
IF(NOT BUILD_TESTS)
OPTION(BUILD_TESTS "Build Unit tests" ON)
ENDIF(NOT BUILD_TESTS)
@@ -147,8 +146,8 @@ IF(EXISTS "${WOLF_CONFIG_DIR}" AND NOT IS_DIRECTORY "${WOLF_CONFIG_DIR}")
ENDIF()
# Configure config.h
configure_file(${CMAKE_CURRENT_SOURCE_DIR}/internal/config.h.in "${WOLF_CONFIG_DIR}/config.h")
-message("WOLF CONFIG ${WOLF_CONFIG_DIR}/config.h")
-message("CONFIG DIRECTORY ${PROJECT_BINARY_DIR}")
+message(STATUS "WOLF CONFIG DIRECTORY ${WOLF_CONFIG_DIR}")
+message(STATUS "WOLF CONFIG FILE ${WOLF_CONFIG_DIR}/config.h")
include_directories("${PROJECT_BINARY_DIR}/conf")
# include spdlog (logging library)
@@ -209,6 +208,7 @@ SET(HDRS_FRAME
include/core/frame/frame_base.h
)
SET(HDRS_STATE_BLOCK
+ include/core/state_block/factory_state_block.h
include/core/state_block/has_state_blocks.h
include/core/state_block/local_parametrization_angle.h
include/core/state_block/local_parametrization_base.h
@@ -216,6 +216,7 @@ SET(HDRS_STATE_BLOCK
include/core/state_block/local_parametrization_quaternion.h
include/core/state_block/state_angle.h
include/core/state_block/state_block.h
+ include/core/state_block/state_composite.h
include/core/state_block/state_homogeneous_3d.h
include/core/state_block/state_quaternion.h
)
@@ -226,7 +227,6 @@ SET(HDRS_CAPTURE
include/core/capture/capture_odom_2d.h
include/core/capture/capture_odom_3d.h
include/core/capture/capture_pose.h
- include/core/capture/capture_velocity.h
include/core/capture/capture_void.h
include/core/capture/capture_diff_drive.h
)
@@ -246,6 +246,7 @@ SET(HDRS_FACTOR
include/core/factor/factor_pose_3d.h
include/core/factor/factor_quaternion_absolute.h
include/core/factor/factor_relative_2d_analytic.h
+ include/core/factor/factor_relative_pose_2d_with_extrinsics.h
)
SET(HDRS_FEATURE
include/core/feature/feature_base.h
@@ -263,7 +264,7 @@ SET(HDRS_PROCESSOR
include/core/processor/motion_buffer.h
include/core/processor/processor_base.h
include/core/processor/processor_diff_drive.h
- include/core/processor/processor_factory.h
+ include/core/processor/factory_processor.h
include/core/processor/processor_logging.h
include/core/processor/processor_loopclosure.h
include/core/processor/processor_motion.h
@@ -277,15 +278,19 @@ SET(HDRS_PROCESSOR
SET(HDRS_SENSOR
include/core/sensor/sensor_base.h
include/core/sensor/sensor_diff_drive.h
- include/core/sensor/sensor_factory.h
+ include/core/sensor/factory_sensor.h
include/core/sensor/sensor_odom_2d.h
include/core/sensor/sensor_odom_3d.h
)
SET(HDRS_SOLVER
include/core/solver/solver_manager.h
- include/core/solver/solver_factory.h
+ include/core/solver/factory_solver.h
+ )
+SET(HDRS_TREE_MANAGER
+ include/core/tree_manager/factory_tree_manager.h
+ include/core/tree_manager/tree_manager_base.h
+ include/core/tree_manager/tree_manager_sliding_window.h
)
-
SET(HDRS_YAML
include/core/yaml/parser_yaml.h
include/core/yaml/yaml_conversion.h
@@ -312,6 +317,7 @@ SET(SRCS_STATE_BLOCK
src/state_block/local_parametrization_homogeneous.cpp
src/state_block/local_parametrization_quaternion.cpp
src/state_block/state_block.cpp
+ src/state_block/state_composite.cpp
)
SET(SRCS_COMMON
src/common/node_base.cpp
@@ -332,7 +338,6 @@ SET(SRCS_CAPTURE
src/capture/capture_odom_2d.cpp
src/capture/capture_odom_3d.cpp
src/capture/capture_pose.cpp
- src/capture/capture_velocity.cpp
src/capture/capture_void.cpp
src/capture/capture_diff_drive.cpp
)
@@ -350,6 +355,7 @@ SET(SRCS_LANDMARK
src/landmark/landmark_base.cpp
)
SET(SRCS_PROCESSOR
+ src/processor/is_motion.cpp
src/processor/motion_buffer.cpp
src/processor/processor_base.cpp
src/processor/processor_diff_drive.cpp
@@ -371,6 +377,9 @@ SET(SRCS_SENSOR
SET(SRCS_SOLVER
src/solver/solver_manager.cpp
)
+SET(SRCS_TREE_MANAGER
+ src/tree_manager/tree_manager_sliding_window.cpp
+ )
SET(SRCS_YAML
src/yaml/parser_yaml.cpp
src/yaml/processor_odom_3d_yaml.cpp
@@ -400,8 +409,6 @@ ELSE(Ceres_FOUND)
SET(SRCS_WRAPPER)
ENDIF(Ceres_FOUND)
-#SUBDIRECTORIES
-add_subdirectory(hello_wolf)
IF (cereal_FOUND)
ADD_SUBDIRECTORY(serialization/cereal)
ENDIF(cereal_FOUND)
@@ -431,10 +438,23 @@ ADD_LIBRARY(${PROJECT_NAME}
${SRCS_SOLVER}
${SRCS_STATE_BLOCK}
${SRCS_TRAJECTORY}
+ ${SRCS_TREE_MANAGER}
${SRCS_UTILS}
${SRCS_WRAPPER}
${SRCS_YAML}
)
+
+# Set compiler options
+# ====================
+if (CMAKE_CXX_COMPILER_ID STREQUAL "Clang")
+ message(STATUS "Using C++ compiler clang")
+ target_compile_options(${PROJECT_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)
+ # using GCC
+endif()
#Link the created libraries
#=============================================================
@@ -471,44 +491,46 @@ INSTALL(TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME}Targets
install(EXPORT ${PROJECT_NAME}Targets DESTINATION lib/cmake/${PROJECT_NAME})
#install headers
-INSTALL(FILES ${HDRS_MATH}
- DESTINATION include/iri-algorithms/wolf/plugin_core/core/math)
-INSTALL(FILES ${HDRS_UTILS}
- DESTINATION include/iri-algorithms/wolf/plugin_core/core/utils)
-INSTALL(FILES ${HDRS_PROBLEM}
- DESTINATION include/iri-algorithms/wolf/plugin_core/core/problem)
-INSTALL(FILES ${HDRS_HARDWARE}
- DESTINATION include/iri-algorithms/wolf/plugin_core/core/hardware)
-INSTALL(FILES ${HDRS_TRAJECTORY}
- DESTINATION include/iri-algorithms/wolf/plugin_core/core/trajectory)
-INSTALL(FILES ${HDRS_MAP}
- DESTINATION include/iri-algorithms/wolf/plugin_core/core/map)
-INSTALL(FILES ${HDRS_FRAME}
- DESTINATION include/iri-algorithms/wolf/plugin_core/core/frame)
-INSTALL(FILES ${HDRS_STATE_BLOCK}
- DESTINATION include/iri-algorithms/wolf/plugin_core/core/state_block)
-INSTALL(FILES ${HDRS_COMMON}
- DESTINATION include/iri-algorithms/wolf/plugin_core/core/common)
INSTALL(FILES ${HDRS_CAPTURE}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/capture)
+INSTALL(FILES ${HDRS_COMMON}
+ DESTINATION include/iri-algorithms/wolf/plugin_core/core/common)
INSTALL(FILES ${HDRS_FACTOR}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/factor)
INSTALL(FILES ${HDRS_FEATURE}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/feature)
-INSTALL(FILES ${HDRS_SENSOR}
- DESTINATION include/iri-algorithms/wolf/plugin_core/core/sensor)
-INSTALL(FILES ${HDRS_PROCESSOR}
- DESTINATION include/iri-algorithms/wolf/plugin_core/core/processor)
+INSTALL(FILES ${HDRS_FRAME}
+ DESTINATION include/iri-algorithms/wolf/plugin_core/core/frame)
+INSTALL(FILES ${HDRS_HARDWARE}
+ DESTINATION include/iri-algorithms/wolf/plugin_core/core/hardware)
INSTALL(FILES ${HDRS_LANDMARK}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/landmark)
-INSTALL(FILES ${HDRS_WRAPPER}
- DESTINATION include/iri-algorithms/wolf/plugin_core/core/ceres_wrapper)
-INSTALL(FILES ${HDRS_SOLVER_SUITESPARSE}
- DESTINATION include/iri-algorithms/wolf/plugin_core/core/solver_suitesparse)
-INSTALL(FILES ${HDRS_SOLVER}
- DESTINATION include/iri-algorithms/wolf/plugin_core/core/solver)
+INSTALL(FILES ${HDRS_MAP}
+ DESTINATION include/iri-algorithms/wolf/plugin_core/core/map)
+INSTALL(FILES ${HDRS_MATH}
+ DESTINATION include/iri-algorithms/wolf/plugin_core/core/math)
+INSTALL(FILES ${HDRS_PROBLEM}
+ DESTINATION include/iri-algorithms/wolf/plugin_core/core/problem)
+INSTALL(FILES ${HDRS_PROCESSOR}
+ DESTINATION include/iri-algorithms/wolf/plugin_core/core/processor)
+INSTALL(FILES ${HDRS_SENSOR}
+ DESTINATION include/iri-algorithms/wolf/plugin_core/core/sensor)
INSTALL(FILES ${HDRS_SERIALIZATION}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/serialization)
+INSTALL(FILES ${HDRS_SOLVER}
+ DESTINATION include/iri-algorithms/wolf/plugin_core/core/solver)
+INSTALL(FILES ${HDRS_SOLVER_SUITESPARSE}
+ DESTINATION include/iri-algorithms/wolf/plugin_core/core/solver_suitesparse)
+INSTALL(FILES ${HDRS_STATE_BLOCK}
+ DESTINATION include/iri-algorithms/wolf/plugin_core/core/state_block)
+INSTALL(FILES ${HDRS_TRAJECTORY}
+ DESTINATION include/iri-algorithms/wolf/plugin_core/core/trajectory)
+INSTALL(FILES ${HDRS_TREE_MANAGER}
+ DESTINATION include/iri-algorithms/wolf/plugin_core/core/tree_manager)
+INSTALL(FILES ${HDRS_UTILS}
+ DESTINATION include/iri-algorithms/wolf/plugin_core/core/utils)
+INSTALL(FILES ${HDRS_WRAPPER}
+ DESTINATION include/iri-algorithms/wolf/plugin_core/core/ceres_wrapper)
INSTALL(FILES ${HDRS_YAML}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/yaml)
diff --git a/demos/CMakeLists.txt b/demos/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..e6b2bb11cbe6b95dceba4095af5c57b2c76f16c0
--- /dev/null
+++ b/demos/CMakeLists.txt
@@ -0,0 +1,2 @@
+#Add hello_wolf demo
+add_subdirectory(hello_wolf)
diff --git a/demos/demo_map_yaml.cpp b/demos/demo_map_yaml.cpp
deleted file mode 100644
index 87023e52856c9e3ba937882cd5311e05260b507b..0000000000000000000000000000000000000000
--- a/demos/demo_map_yaml.cpp
+++ /dev/null
@@ -1,101 +0,0 @@
-/**
- * \file test_map_yaml.cpp
- *
- * Created on: Jul 27, 2016
- * \author: jsola
- */
-
-#include "core/common/wolf.h"
-#include "core/problem/problem.h"
-#include "core/map/map_base.h"
-#include "core/landmark/landmark_polyline_2d.h"
-#include "core/landmark/landmark_AHP.h"
-#include "core/state_block/state_block.h"
-#include "core/yaml/yaml_conversion.h"
-
-#include <iostream>
-using namespace wolf;
-
-void print(MapBase& _map)
-{
- for (auto lmk_ptr : _map.getLandmarkList())
- {
- std::cout << "Lmk ID: " << lmk_ptr->id();
- std::cout << "\nLmk type: " << lmk_ptr->getType();
- if (lmk_ptr->getType() == "POLYLINE 2d")
- {
- LandmarkPolyline2dPtr poly_ptr = std::static_pointer_cast<LandmarkPolyline2d>(lmk_ptr);
- std::cout << "\npos: " << poly_ptr->getP()->getState().transpose() << " -- fixed: " << poly_ptr->getP()->isFixed();
- std::cout << "\nori: " << poly_ptr->getO()->getState().transpose() << " -- fixed: " << poly_ptr->getO()->isFixed();
- std::cout << "\nn points: " << poly_ptr->getNPoints();
- std::cout << "\nFirst idx: " << poly_ptr->getFirstId();
- std::cout << "\nFirst def: " << poly_ptr->isFirstDefined();
- std::cout << "\nLast def: " << poly_ptr->isLastDefined();
- for (int idx = poly_ptr->getFirstId(); idx <= poly_ptr->getLastId(); idx++)
- std::cout << "\n point: " << idx << ": " << poly_ptr->getPointStateBlock(idx)->getState().transpose();
- break;
- }
- else if (lmk_ptr->getType() == "AHP")
- {
- LandmarkAHPPtr ahp_ptr = std::static_pointer_cast<LandmarkAHP>(lmk_ptr);
- std::cout << "\npos: " << ahp_ptr->getP()->getState().transpose() << " -- fixed: " << ahp_ptr->getP()->isFixed();
- std::cout << "\ndescript: " << ahp_ptr->getCvDescriptor().t();
- break;
- }
- else
- break;
-
- std::cout << std::endl;
- }
-}
-
-int main()
-{
- using namespace Eigen;
-
- std::cout << "\nTesting Lmk creator and node saving..." << std::endl;
- Vector4d v;
- v << 1, 2, 3, 4;
- cv::Mat d = (cv::Mat_<int>(8,1) << 1, 2, 3, 4, 5, 6, 7, 8);
- LandmarkAHP lmk_1(v, nullptr, nullptr, d);
- std::cout << "Pos 1 = " << lmk_1.getP()->getState().transpose() << std::endl;
- std::cout << "Des 1 = " << lmk_1.getCvDescriptor().t() << std::endl;
-
- YAML::Node n = lmk_1.saveToYaml();
- std::cout << "Pos n = " << n["position"].as<VectorXd>().transpose() << std::endl;
- std::cout << "Des n = " << n["descriptor"].as<VectorXd>().transpose() << std::endl;
-
- LandmarkAHP lmk_2 = *(std::static_pointer_cast<LandmarkAHP>(LandmarkAHP::create(n)));
- std::cout << "Pos 2 = " << lmk_2.getP()->getState().transpose() << std::endl;
- std::cout << "Des 2 = " << lmk_2.getCvDescriptor().t() << std::endl;
-
- std::string filename;
-
- std::string wolf_root = _WOLF_ROOT_DIR;
- std::string wolf_config = wolf_root + "/src/examples";
- std::cout << "\nWolf directory for configuration files: " << wolf_config << std::endl;
-
- ProblemPtr problem = Problem::create("PO", 2);
- filename = wolf_config + "/map_polyline_example.yaml";
- std::cout << "Reading map from file: " << filename << std::endl;
- problem->loadMap(filename);
-
- std::cout << "Printing map..." << std::endl;
- print(*(problem->getMap()));
-
- filename = wolf_config + "/map_polyline_example_write.yaml";
- std::cout << "Writing map to file: " << filename << std::endl;
- std::string thisfile = __FILE__;
- problem->getMap()->save(filename, "Example generated by test file " + thisfile);
-
- std::cout << "Clearing map... " << std::endl;
- problem->getMap()->getLandmarkList().clear();
-
- std::cout << "Re-reading map from file: " << filename << std::endl;
- problem->loadMap(filename);
-
- std::cout << "Printing map..." << std::endl;
- print(*(problem->getMap()));
-
- return 0;
-}
diff --git a/hello_wolf/CMakeLists.txt b/demos/hello_wolf/CMakeLists.txt
similarity index 100%
rename from hello_wolf/CMakeLists.txt
rename to demos/hello_wolf/CMakeLists.txt
diff --git a/hello_wolf/capture_range_bearing.cpp b/demos/hello_wolf/capture_range_bearing.cpp
similarity index 89%
rename from hello_wolf/capture_range_bearing.cpp
rename to demos/hello_wolf/capture_range_bearing.cpp
index a2d44a101085ea0ad3aa4ae7d68af68370ea5024..9ead1c183b8b08a64f709faea5236d5dbb056231 100644
--- a/hello_wolf/capture_range_bearing.cpp
+++ b/demos/hello_wolf/capture_range_bearing.cpp
@@ -11,7 +11,7 @@ namespace wolf
{
CaptureRangeBearing::CaptureRangeBearing(const TimeStamp& _ts, const SensorBasePtr& _scanner, const Eigen::VectorXi& _ids, const Eigen::VectorXd& _ranges, const Eigen::VectorXd& _bearings) :
- CaptureBase("RANGE BEARING", _ts, _scanner),
+ CaptureBase("CaptureRangeBearing", _ts, _scanner),
ids_(_ids),
ranges_(_ranges),
bearings_(_bearings)
diff --git a/hello_wolf/capture_range_bearing.h b/demos/hello_wolf/capture_range_bearing.h
similarity index 100%
rename from hello_wolf/capture_range_bearing.h
rename to demos/hello_wolf/capture_range_bearing.h
diff --git a/hello_wolf/factor_bearing.h b/demos/hello_wolf/factor_bearing.h
similarity index 92%
rename from hello_wolf/factor_bearing.h
rename to demos/hello_wolf/factor_bearing.h
index 6975a798db580e1c127b0215609f49904b29ae40..f5c5dd95887add0562377c360e7087a8f975665d 100644
--- a/hello_wolf/factor_bearing.h
+++ b/demos/hello_wolf/factor_bearing.h
@@ -68,9 +68,8 @@ inline bool FactorBearing::operator ()(const T* const _p1, const T* const _o1,
// 3. Get the expected bearing of the point
T bearing = atan2(point_r(1), point_r(0));
- // 4. Get the measured range-and-bearing to the point, and its covariance
+ // 4. Get the measured range-and-bearing to the point
Matrix<T, 2, 1> range_bearing = getMeasurement();
- Matrix<T, 2, 2> range_bearing_cov = getMeasurementCovariance();
// 5. Get the bearing error by comparing against the bearing measurement
T er = range_bearing(1) - bearing;
@@ -80,8 +79,7 @@ inline bool FactorBearing::operator ()(const T* const _p1, const T* const _o1,
er -= T(2*M_PI);
// 6. Compute the residual by scaling according to the standard deviation of the bearing part
- T sigma = sqrt(range_bearing_cov(1,1));
- *_res = er / sigma;
+ *_res = er * T(getMeasurementSquareRootInformationUpper()(1,1));
return true;
}
diff --git a/hello_wolf/factor_range_bearing.h b/demos/hello_wolf/factor_range_bearing.h
similarity index 98%
rename from hello_wolf/factor_range_bearing.h
rename to demos/hello_wolf/factor_range_bearing.h
index 436b79e4a0eee67aa8ddd97de34ec14127251d01..226a6335e7966a0d3bdd7042c017913bc64b419b 100644
--- a/hello_wolf/factor_range_bearing.h
+++ b/demos/hello_wolf/factor_range_bearing.h
@@ -21,12 +21,14 @@ class FactorRangeBearing : public FactorAutodiff<FactorRangeBearing, 2, 2, 1, 2,
{
public:
FactorRangeBearing(const CaptureBasePtr& _capture_own, // own capture's pointer
+ const FeatureBasePtr& _feature_ptr,
const LandmarkBasePtr& _landmark_other_ptr, // other landmark's pointer
const ProcessorBasePtr& _processor_ptr, // processor having created this
bool _apply_loss_function, // apply loss function to residual?
FactorStatus _status) : // active factor?
FactorAutodiff<FactorRangeBearing, 2, 2, 1, 2, 1, 2>( // sizes of: residual, rob pos, rob ori, sen pos, sen ori, lmk pos
"RANGE BEARING", // factor type enum (see wolf.h)
+ _feature_ptr,
nullptr, // other frame's pointer
nullptr, // other capture's pointer
nullptr, // other feature's pointer
diff --git a/hello_wolf/feature_range_bearing.cpp b/demos/hello_wolf/feature_range_bearing.cpp
similarity index 82%
rename from hello_wolf/feature_range_bearing.cpp
rename to demos/hello_wolf/feature_range_bearing.cpp
index 4d47cc5539f5fcfe2cc011ea4c4e6e11d2685f90..ea584a419aa21c1d7e462824d6872f7d57bd818c 100644
--- a/hello_wolf/feature_range_bearing.cpp
+++ b/demos/hello_wolf/feature_range_bearing.cpp
@@ -11,7 +11,7 @@ namespace wolf
{
FeatureRangeBearing::FeatureRangeBearing(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance) :
- FeatureBase("RANGE BEARING", _measurement, _meas_covariance)
+ FeatureBase("FeatureRangeBearing", _measurement, _meas_covariance)
{
//
}
diff --git a/hello_wolf/feature_range_bearing.h b/demos/hello_wolf/feature_range_bearing.h
similarity index 100%
rename from hello_wolf/feature_range_bearing.h
rename to demos/hello_wolf/feature_range_bearing.h
diff --git a/hello_wolf/hello_wolf.cpp b/demos/hello_wolf/hello_wolf.cpp
similarity index 92%
rename from hello_wolf/hello_wolf.cpp
rename to demos/hello_wolf/hello_wolf.cpp
index ac33da83c3fece5dce946e9c808869edd511e6f4..e51f7a33658bf1c3afb9f2d20e8118825886ebf6 100644
--- a/hello_wolf/hello_wolf.cpp
+++ b/demos/hello_wolf/hello_wolf.cpp
@@ -115,7 +115,7 @@ int main()
SensorBasePtr sensor_odo = problem->installSensor("SensorOdom2d", "sensor odo", Vector3d(0,0,0), intrinsics_odo);
// processor odometer 2d
- ProcessorParamsOdom2dPtr params_odo = std::make_shared<ProcessorParamsOdom2d>();
+ ParamsProcessorOdom2dPtr params_odo = std::make_shared<ParamsProcessorOdom2d>();
params_odo->voting_active = true;
params_odo->time_tolerance = 0.1;
params_odo->max_time_span = 999;
@@ -132,16 +132,19 @@ int main()
SensorBasePtr sensor_rb = problem->installSensor("SensorRangeBearing", "sensor RB", Vector3d(1,1,0), intrinsics_rb);
// processor Range and Bearing
- ProcessorParamsRangeBearingPtr params_rb = std::make_shared<ProcessorParamsRangeBearing>();
+ ParamsProcessorRangeBearingPtr params_rb = std::make_shared<ParamsProcessorRangeBearing>();
params_rb->voting_active = false;
params_rb->time_tolerance = 0.01;
ProcessorBasePtr processor_rb = problem->installProcessor("ProcessorRangeBearing", "processor RB", sensor_rb, params_rb);
// initialize
TimeStamp t(0.0); // t : 0.0
- Vector3d x(0,0,0);
- Matrix3d P = Matrix3d::Identity() * 0.1;
- problem->setPrior(x, P, t, 0.5); // KF1 : (0,0,0)
+ // Vector3d x(0,0,0);
+ VectorComposite x(Vector3d(0,0,0), "PO", {2,1});
+ // Matrix3d P = Matrix3d::Identity() * 0.1;
+ VectorComposite P(Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)), "PO", {2,1});
+ FrameBasePtr KF1 = problem->setPriorFactor(x, P, t, 0.5); // KF1 : (0,0,0)
+ std::static_pointer_cast<ProcessorMotion>(processor)->setOrigin(KF1);
// SELF CALIBRATION ===================================================
@@ -220,19 +223,7 @@ int main()
// PERTURB initial guess
WOLF_TRACE("======== PERTURB PROBLEM PRIORS =======")
- for (auto& sen : problem->getHardware()->getSensorList())
- for (auto& sb : sen->getStateBlockVec())
- if (sb && !sb->isFixed())
- sb->setState(sb->getState() + VectorXd::Random(sb->getSize()) * 0.5); // We perturb A LOT !
- for (auto& kf : problem->getTrajectory()->getFrameList())
- if (kf->isKeyOrAux())
- for (auto& pair_key_sb : kf->getStateBlockMap())
- if (!pair_key_sb.second->isFixed())
- pair_key_sb.second->setState(pair_key_sb.second->getState() + VectorXd::Random(pair_key_sb.second->getSize()) * 0.5); // We perturb A LOT !
- for (auto lmk : problem->getMap()->getLandmarkList())
- for (auto& pair_key_sb : lmk->getStateBlockMap())
- if (!pair_key_sb.second->isFixed())
- pair_key_sb.second->setState(pair_key_sb.second->getState() + VectorXd::Random(pair_key_sb.second->getSize()) * 0.5); // We perturb A LOT !
+ problem->perturb(0.5); // Perturb all state blocks that are not fixed
problem->print(1,0,1,0);
// SOLVE again
@@ -260,7 +251,7 @@ int main()
std::cout << std::endl;
WOLF_TRACE("======== FINAL PRINT FOR INTERPRETATION =======")
- problem->print(4,1,1,1);
+ problem->print(4,0,1,0);
/*
* ============= FIRST COMMENT ON THE RESULTS ==================
diff --git a/hello_wolf/hello_wolf_autoconf.cpp b/demos/hello_wolf/hello_wolf_autoconf.cpp
similarity index 69%
rename from hello_wolf/hello_wolf_autoconf.cpp
rename to demos/hello_wolf/hello_wolf_autoconf.cpp
index a387adfe9656523c37d025bb3a74f0f198281b3d..bff8afd133c97f83f16f35fbcd35ab3db011f17f 100644
--- a/hello_wolf/hello_wolf_autoconf.cpp
+++ b/demos/hello_wolf/hello_wolf_autoconf.cpp
@@ -9,6 +9,7 @@
// wolf core includes
#include "core/common/wolf.h"
#include "core/capture/capture_odom_2d.h"
+#include "core/processor/processor_motion.h"
#include "core/yaml/parser_yaml.h"
#include "core/ceres_wrapper/ceres_manager.h"
@@ -98,7 +99,7 @@ int main()
WOLF_TRACE("======== CONFIGURE PROBLEM =======");
// Config file to parse. Here is where all the problem is defined:
- std::string config_file = "hello_wolf/yaml/hello_wolf_config.yaml";
+ std::string config_file = "demos/hello_wolf/yaml/hello_wolf_config.yaml";
std::string wolf_path = std::string(_WOLF_ROOT_DIR);
// parse file into params server: each param will be retrievable from this params server:
@@ -118,9 +119,11 @@ int main()
// recover sensor pointers and other stuff for later use (access by sensor name)
SensorBasePtr sensor_odo = problem->getSensor("sen odom");
SensorBasePtr sensor_rb = problem->getSensor("sen rb");
- TimeStamp t = problem->getTrajectory()->getFrameList().front()->getTimeStamp();
-
+ // APPLY PRIOR and SET PROCESSOR ODOM ORIGIN ===================================================
+ TimeStamp t(0.0);
+ FrameBasePtr KF1 = problem->applyPriorOptions(t);
+ std::static_pointer_cast<ProcessorMotion>(problem->getProcessorIsMotion())->setOrigin(KF1);
// SELF CALIBRATION ===================================================
// These few lines control whether we calibrate some sensor parameters or not.
@@ -203,19 +206,7 @@ int main()
// PERTURB initial guess
WOLF_TRACE("======== PERTURB PROBLEM PRIORS =======")
- for (auto& sen : problem->getHardware()->getSensorList())
- for (auto& sb : sen->getStateBlockVec())
- if (sb && !sb->isFixed())
- sb->setState(sb->getState() + VectorXd::Random(sb->getSize()) * 0.5); // We perturb A LOT !
- for (auto& kf : problem->getTrajectory()->getFrameList())
- if (kf->isKeyOrAux())
- for (auto& pair_key_sb : kf->getStateBlockMap())
- if (pair_key_sb.second && !pair_key_sb.second->isFixed())
- pair_key_sb.second->setState(pair_key_sb.second->getState() + VectorXd::Random(pair_key_sb.second->getSize()) * 0.5); // We perturb A LOT !
- for (auto lmk : problem->getMap()->getLandmarkList())
- for (auto& pair_key_sb : lmk->getStateBlockMap())
- if (!pair_key_sb.second->isFixed())
- pair_key_sb.second->setState(pair_key_sb.second->getState() + VectorXd::Random(pair_key_sb.second->getSize()) * 0.5); // We perturb A LOT !
+ problem->perturb(0.5); // Perturb all state blocks that are not fixed
problem->print(1,0,1,0);
// SOLVE again
@@ -250,9 +241,8 @@ int main()
*
* IF YOU SEE at the end of the printed problem the estimate for Lmk 3 as:
*
- * L3 POINT 2d <-- c8
- * Est, x = ( 3 2 )
- * sb: Est
+ * Lmk3 LandmarkPoint2d <-- Fac9
+ * P[Est] = ( 1 2 )
*
* it means WOLF SOLVED SUCCESSFULLY (L3 is effectively at location (3,2) ) !
*
@@ -271,73 +261,80 @@ int main()
* The full message is explained below.
*
* P: wolf tree status ---------------------
- Hardware
- S1 ODOM 2d // Sensor 1, type ODOMETRY 2d.
- Extr [Sta] = [ Fix( 0 0 ) Fix( 0 ) ] // Static extrinsics, fixed position, fixed orientation (See notes 1 and 2 below)
- Intr [Sta] = [ ] // Static intrinsics, but no intrinsics anyway
- pm1 ODOM 2d // Processor 1, type ODOMETRY 2d
- o: C7 - F3 // origin at Capture 7, Frame 3
- l: C10 - F4 // last at Capture 10, frame 4
- S2 RANGE BEARING // Sensor 2, type RANGE and BEARING.
- Extr [Sta] = [ Fix( 1 1 ) Est( 0 ) ] // Static extrinsics, fixed position, estimated orientation (See notes 1 and 2 below)
- Intr [Sta] = [ ] // Static intrinsics, but no intrinsics anyway
- pt2 RANGE BEARING // Processor 2: type Range and Bearing
- Trajectory
- KF1 <-- c3 // KeyFrame 1, constrained by Factor 3
- Est, ts=0, x = ( -1.6e-13 9.4e-11 1.4e-10 ) // State is estimated; time stamp and state vector
- sb: Est Est // State's pos and orient are estimated
- C1 FIX -> S- [ <-- // Capture 1, type FIX or Absolute
- f1 FIX <-- // Feature 1, type Fix
- m = ( 0 0 0 ) // The absolute measurement for this frame is (0,0,0) --> origin
- c1 FIX --> A // Factor 1, type FIX, it is Absolute
- CM2 ODOM 2d -> S1 [Sta, Sta] <-- // Capture 2, type ODOM, from Sensor 1 (static extr and intr)
- C5 RANGE BEARING -> S2 [Sta, Sta] <-- // Capture 5, type R+B, from Sensor 2 (static extr and intr)
- f2 RANGE BEARING <-- // Feature 2, type R+B
- m = ( 1 1.57 ) // The feature's measurement is 1m, 1.57rad
- c2 RANGE BEARING --> L1 // Factor 2 against Landmark 1
- KF2 <-- c6
- Est, ts=1, x = ( 1 2.5e-10 1.6e-10 )
- sb: Est Est
- CM3 ODOM 2d -> S1 [Sta, Sta] <--
- f3 ODOM 2d <--
- m = ( 1 0 0 )
- c3 ODOM 2d --> F1 // Factor 3, type ODOM, against Frame 1
- C9 RANGE BEARING -> S2 [Sta, Sta] <--
- f4 RANGE BEARING <--
- m = ( 1.41 2.36 )
- c4 RANGE BEARING --> L1
- f5 RANGE BEARING <--
- m = ( 1 1.57 )
- c5 RANGE BEARING --> L2
- KF3 <--
- Est, ts=2, x = ( 2 4.1e-10 1.7e-10 )
- sb: Est Est
- CM7 ODOM 2d -> S1 [Sta, Sta] <--
- f6 ODOM 2d <--
- m = ( 1 0 0 )
- c6 ODOM 2d --> F2
- C12 RANGE BEARING -> S2 [Sta, Sta] <--
- f7 RANGE BEARING <--
- m = ( 1.41 2.36 )
- c7 RANGE BEARING --> L2
- f8 RANGE BEARING <--
- m = ( 1 1.57 )
- c8 RANGE BEARING --> L3
- F4 <--
- Est, ts=2, x = ( 0.11 -0.045 0.26 )
- sb: Est Est
- CM10 ODOM 2d -> S1 [Sta, Sta] <--
- Map
- L1 POINT 2d <-- c2 c4 // Landmark 1, constrained by Factors 2 and 4
- Est, x = ( 1 2 ) // L4 state is estimated, state vector
- sb: Est // L4 has 1 state block estimated
- L2 POINT 2d <-- c5 c7
- Est, x = ( 2 2 )
- sb: Est
- L3 POINT 2d <-- c8
- Est, x = ( 3 2 )
- sb: Est
- -----------------------------------------
+ Hardware
+ Sen1 SensorOdom2d "sen odom" // Sensor 1, type odometry 2D
+ sb: P[Sta,Fix] = ( 0 0 ); O[Sta,Fix] = ( 0 ); // Static extrinsics, fixed position, fixed orientation (See notes 1 and 2 below)
+ PrcM1 ProcessorOdom2d "prc odom" // Processor 1, type odometry 2D
+ o: Cap6 - KFrm3 // origin at Capture 6, Frame 3
+ l: Cap8 - Frm4 // last at Capture 8, Frame 4
+ Sen2 SensorRangeBearing "sen rb" // Sensor 2, type range and bearing
+ sb: P[Sta,Fix] = ( 1 1 ); O[Sta,Fix] = ( 0 ); // Static extrinsics, fixed position, estimated orientation (See notes 1 and 2 below)
+ Prc2 ProcessorRangeBearing "prc rb" // Processor 2, type range and bearing
+ Trajectory
+ KFrm1 <-- Fac4 // KeyFrame 1, constrained by Factor 4
+ P[Est] = ( -4.4e-12 1.5e-09 ) // Position is estimated
+ O[Est] = ( 2.6e-09 ) // Orientation is estimated
+ Cap1 CaptureVoid -> Sen- <-- // This is an "artificial" capture used to hold the features relative to the prior
+ Ftr1 trk0 prior <-- // Position prior
+ m = ( 0 0 )
+ Fac1 FactorBlockAbsolute --> Abs
+ Ftr2 trk0 prior <-- // Orientation prior
+ m = ( 0 )
+ Fac2 FactorBlockAbsolute --> Abs
+ CapM2 CaptureOdom2d -> Sen1 <-- // Capture 2 of type motion odom 2d from sensor 1.
+ buffer size : 0
+ Cap4 CaptureRangeBearing -> Sen2 <--
+ Ftr3 trk0 FeatureRangeBearing <--
+ m = ( 1 1.6 )
+ Fac3 RANGE BEARING --> Lmk1
+ KFrm2 <-- Fac7
+ P[Est] = ( 1 5.7e-09 )
+ O[Est] = ( 5.7e-09 )
+ CapM3 CaptureOdom2d -> Sen1 -> OCap2 ; OFrm1 <--// Capture 3 of type motion odom2d from sensor 1.
+ // Its origin is at Capture 2; Frame 1
+ buffer size : 2
+ delta preint : (1 0 0)
+ Ftr4 trk0 ProcessorOdom2d <--
+ m = ( 1 0 0 )
+ Fac4 FactorOdom2d --> Frm1
+ Cap7 CaptureRangeBearing -> Sen2 <--
+ Ftr5 trk0 FeatureRangeBearing <--
+ m = ( 1.4 2.4 )
+ Fac5 RANGE BEARING --> Lmk1
+ Ftr6 trk0 FeatureRangeBearing <--
+ m = ( 1 1.6 )
+ Fac6 RANGE BEARING --> Lmk2
+ KFrm3 <--
+ P[Est] = ( 2 1.2e-08 )
+ O[Est] = ( 6.6e-09 )
+ CapM6 CaptureOdom2d -> Sen1 -> OCap3 ; OFrm2 <--
+ buffer size : 2
+ delta preint : (1 0 0)
+ Ftr7 trk0 ProcessorOdom2d <--
+ m = ( 1 0 0 )
+ Fac7 FactorOdom2d --> Frm2
+ Cap9 CaptureRangeBearing -> Sen2 <--
+ Ftr8 trk0 FeatureRangeBearing <--
+ m = ( 1.4 2.4 )
+ Fac8 RANGE BEARING --> Lmk2
+ Ftr9 trk0 FeatureRangeBearing <--
+ m = ( 1 1.6 )
+ Fac9 RANGE BEARING --> Lmk3
+ Frm4 <--
+ P[Est] = ( 2 0 )
+ O[Est] = ( 0 )
+ CapM8 CaptureOdom2d -> Sen1 -> OCap6 ; OFrm3 <--
+ buffer size : 1
+ delta preint : (0 0 0)
+ Map
+ Lmk1 LandmarkPoint2d <-- Fac3 Fac5 // Landmark 1 constrained by factors 3 & 5
+ P[Est] = ( 1 2 )
+ Lmk2 LandmarkPoint2d <-- Fac6 Fac8
+ P[Est] = ( 2 2 )
+ Lmk3 LandmarkPoint2d <-- Fac9
+ P[Est] = ( 3 2 )
+ -----------------------------------------
+
*
* ============= GENERAL WOLF NOTES ==================
*
diff --git a/hello_wolf/landmark_point_2d.cpp b/demos/hello_wolf/landmark_point_2d.cpp
similarity index 80%
rename from hello_wolf/landmark_point_2d.cpp
rename to demos/hello_wolf/landmark_point_2d.cpp
index 835c4fb04e4f7b1e5695958ca9db3cbb810c5ec5..8b2ad4c0e8a9b576f67e9d8d56e41d932abedef8 100644
--- a/hello_wolf/landmark_point_2d.cpp
+++ b/demos/hello_wolf/landmark_point_2d.cpp
@@ -11,7 +11,7 @@ namespace wolf
{
LandmarkPoint2d::LandmarkPoint2d(int _id, const Eigen::Vector2d& _xy) :
- LandmarkBase("POINT 2d", std::make_shared<StateBlock>(_xy))
+ LandmarkBase("LandmarkPoint2d", std::make_shared<StateBlock>(_xy))
{
setId(_id);
}
diff --git a/hello_wolf/landmark_point_2d.h b/demos/hello_wolf/landmark_point_2d.h
similarity index 100%
rename from hello_wolf/landmark_point_2d.h
rename to demos/hello_wolf/landmark_point_2d.h
diff --git a/hello_wolf/processor_range_bearing.cpp b/demos/hello_wolf/processor_range_bearing.cpp
similarity index 92%
rename from hello_wolf/processor_range_bearing.cpp
rename to demos/hello_wolf/processor_range_bearing.cpp
index 89b3d7775e3c5b79dca8de4a0d2f6f5ad91d17e7..08e77f76c8d7d2ec70c9b1ac6689e724a67e5c7c 100644
--- a/hello_wolf/processor_range_bearing.cpp
+++ b/demos/hello_wolf/processor_range_bearing.cpp
@@ -14,7 +14,7 @@
namespace wolf
{
-ProcessorRangeBearing::ProcessorRangeBearing(ProcessorParamsBasePtr _params) :
+ProcessorRangeBearing::ProcessorRangeBearing(ParamsProcessorBasePtr _params) :
ProcessorBase("ProcessorRangeBearing", 2, _params)
{
//
@@ -91,6 +91,7 @@ void ProcessorRangeBearing::processCapture(CaptureBasePtr _capture)
auto prc = shared_from_this();
auto ctr = FactorBase::emplace<FactorRangeBearing>(ftr,
capture_rb,
+ ftr,
lmk,
prc,
false,
@@ -125,14 +126,14 @@ ProcessorRangeBearing::Trf ProcessorRangeBearing::transform(const Eigen::Vector2
Eigen::Vector2d ProcessorRangeBearing::fromSensor(const Eigen::Vector2d& lmk_s) const
{
- Trf H_w_r = transform(getProblem()->getCurrentState());
+ Trf H_w_r = transform(getProblem()->getState().vector("PO"));
return H_w_r * H_r_s * lmk_s;
}
Eigen::Vector2d ProcessorRangeBearing::toSensor(const Eigen::Vector2d& lmk_w) const
{
// Trf H_w_r = transform(getSensor()->getP()->getState(), getSensor()->getO()->getState());
- Trf H_w_r = transform(getProblem()->getCurrentState());
+ Trf H_w_r = transform(getProblem()->getState().vector("PO"));
return (H_w_r * H_r_s).inverse() * lmk_w;
}
@@ -160,10 +161,10 @@ bool ProcessorRangeBearing::storeCapture(CaptureBasePtr _cap_ptr)
} /* namespace wolf */
// Register in the SensorFactory
-#include "core/processor/processor_factory.h"
+#include "core/processor/factory_processor.h"
namespace wolf
{
-WOLF_REGISTER_PROCESSOR("ProcessorRangeBearing", ProcessorRangeBearing)
-WOLF_REGISTER_PROCESSOR_AUTO("ProcessorRangeBearing", ProcessorRangeBearing)
+WOLF_REGISTER_PROCESSOR(ProcessorRangeBearing)
+WOLF_REGISTER_PROCESSOR_AUTO(ProcessorRangeBearing)
} // namespace wolf
diff --git a/hello_wolf/processor_range_bearing.h b/demos/hello_wolf/processor_range_bearing.h
similarity index 87%
rename from hello_wolf/processor_range_bearing.h
rename to demos/hello_wolf/processor_range_bearing.h
index ec1d8619835f1169a192639db09d8be93087cdae..1203b217e4f07c3e02a287f63b0bace4ba46ede2 100644
--- a/hello_wolf/processor_range_bearing.h
+++ b/demos/hello_wolf/processor_range_bearing.h
@@ -17,23 +17,23 @@
namespace wolf
{
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsRangeBearing);
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorRangeBearing);
-struct ProcessorParamsRangeBearing : public ProcessorParamsBase
+struct ParamsProcessorRangeBearing : public ParamsProcessorBase
{
// We do not need special parameters, but in case you need they should be defined here.
- ProcessorParamsRangeBearing()
+ ParamsProcessorRangeBearing()
{
//DEFINED FOR COMPATIBILITY PURPOSES. TO BE REMOVED IN THE FUTURE.
}
- ProcessorParamsRangeBearing(std::string _unique_name, const ParamsServer& _server) :
- ProcessorParamsBase(_unique_name, _server)
+ ParamsProcessorRangeBearing(std::string _unique_name, const ParamsServer& _server) :
+ ParamsProcessorBase(_unique_name, _server)
{
//
}
std::string print() const
{
- return "\n" + ProcessorParamsBase::print();
+ return "\n" + ParamsProcessorBase::print();
}
};
@@ -45,12 +45,12 @@ class ProcessorRangeBearing : public ProcessorBase
public:
typedef Eigen::Transform<double, 2, Eigen::Isometry> Trf;
- ProcessorRangeBearing(ProcessorParamsBasePtr _params);
+ ProcessorRangeBearing(ParamsProcessorBasePtr _params);
virtual ~ProcessorRangeBearing() {/* empty */}
virtual void configure(SensorBasePtr _sensor) override;
// Factory method for high level API
- WOLF_PROCESSOR_CREATE(ProcessorRangeBearing, ProcessorParamsRangeBearing);
+ WOLF_PROCESSOR_CREATE(ProcessorRangeBearing, ParamsProcessorRangeBearing);
protected:
// Implementation of pure virtuals from ProcessorBase
diff --git a/hello_wolf/sensor_range_bearing.cpp b/demos/hello_wolf/sensor_range_bearing.cpp
similarity index 86%
rename from hello_wolf/sensor_range_bearing.cpp
rename to demos/hello_wolf/sensor_range_bearing.cpp
index 3eea032b2ba632cf3343805446a98d0805e56f3c..54507f9ac53771523b9db8716089113686bc1ca0 100644
--- a/hello_wolf/sensor_range_bearing.cpp
+++ b/demos/hello_wolf/sensor_range_bearing.cpp
@@ -38,9 +38,9 @@ SensorRangeBearing::~SensorRangeBearing()
} /* namespace wolf */
// Register in the SensorFactory
-#include "core/sensor/sensor_factory.h"
+#include "core/sensor/factory_sensor.h"
namespace wolf
{
-WOLF_REGISTER_SENSOR("SensorRangeBearing", SensorRangeBearing)
-WOLF_REGISTER_SENSOR_AUTO("SensorRangeBearing", SensorRangeBearing)
+WOLF_REGISTER_SENSOR(SensorRangeBearing)
+WOLF_REGISTER_SENSOR_AUTO(SensorRangeBearing)
} // namespace wolf
diff --git a/hello_wolf/sensor_range_bearing.h b/demos/hello_wolf/sensor_range_bearing.h
similarity index 100%
rename from hello_wolf/sensor_range_bearing.h
rename to demos/hello_wolf/sensor_range_bearing.h
diff --git a/hello_wolf/yaml/hello_wolf_config.yaml b/demos/hello_wolf/yaml/hello_wolf_config.yaml
similarity index 59%
rename from hello_wolf/yaml/hello_wolf_config.yaml
rename to demos/hello_wolf/yaml/hello_wolf_config.yaml
index b84d7776622264f96a4597ade146d64bcec86438..f2e4f8db31f97ef8059b4d59483a747456e6d19c 100644
--- a/hello_wolf/yaml/hello_wolf_config.yaml
+++ b/demos/hello_wolf/yaml/hello_wolf_config.yaml
@@ -2,12 +2,23 @@ config:
problem:
+ tree_manager:
+ type: "TreeManagerSlidingWindow"
+ n_key_frames: -1
+ fix_first_key_frame: false
+ viral_remove_empty_parent: true
frame_structure: "PO" # keyframes have position and orientation
dimension: 2 # space is 2d
prior:
- timestamp: 0.0
- state: [0,0,0]
- cov: [[3,3],.01,0,0,0,.01,0,0,0,.01]
+ mode: "factor"
+ # state: [0,0,0]
+ $state:
+ P: [0,0]
+ O: [0]
+ # cov: [[3,3],.01,0,0,0,.01,0,0,0,.01]
+ $sigma:
+ P: [0.31, 0.31]
+ O: [0.31]
time_tolerance: 0.1
sensors:
@@ -17,7 +28,7 @@ config:
name: "sen odom"
extrinsic:
pose: [0,0, 0]
- follow: "hello_wolf/yaml/sensor_odom_2d.yaml" # config parameters in this file
+ follow: "demos/hello_wolf/yaml/sensor_odom_2d.yaml" # config parameters in this file
- type: "SensorRangeBearing"
plugin: "core"
@@ -25,7 +36,7 @@ config:
extrinsic:
pose: [1,1, 0]
noise_range_metres_std: 0.2 # parser only considers first appearence so the following file parsing will not overwrite this param
- follow: hello_wolf/yaml/sensor_range_bearing.yaml # config parameters in this file
+ follow: demos/hello_wolf/yaml/sensor_range_bearing.yaml # config parameters in this file
processors:
@@ -33,10 +44,10 @@ config:
plugin: "core"
name: "prc odom"
sensor_name: "sen odom" # attach processor to this sensor
- follow: hello_wolf/yaml/processor_odom_2d.yaml # config parameters in this file
+ follow: demos/hello_wolf/yaml/processor_odom_2d.yaml # config parameters in this file
- type: "ProcessorRangeBearing"
plugin: "core"
name: "prc rb"
sensor_name: "sen rb" # attach processor to this sensor
- follow: hello_wolf/yaml/processor_range_bearing.yaml # config parameters in this file
+ follow: demos/hello_wolf/yaml/processor_range_bearing.yaml # config parameters in this file
diff --git a/hello_wolf/yaml/processor_odom_2d.yaml b/demos/hello_wolf/yaml/processor_odom_2d.yaml
similarity index 92%
rename from hello_wolf/yaml/processor_odom_2d.yaml
rename to demos/hello_wolf/yaml/processor_odom_2d.yaml
index bee82ff9ad905a72dc6c1ffd46ac6d7267ae8870..7e345ef211f52897b17267065e28feec0bf4b143 100644
--- a/hello_wolf/yaml/processor_odom_2d.yaml
+++ b/demos/hello_wolf/yaml/processor_odom_2d.yaml
@@ -10,3 +10,4 @@ keyframe_vote:
angle_turned: 999
max_buff_length: 999
cov_det: 999
+apply_loss_function: true
diff --git a/hello_wolf/yaml/processor_range_bearing.yaml b/demos/hello_wolf/yaml/processor_range_bearing.yaml
similarity index 84%
rename from hello_wolf/yaml/processor_range_bearing.yaml
rename to demos/hello_wolf/yaml/processor_range_bearing.yaml
index 61efbf8e31ee32ab9a348133a67037c5fbbdafee..3c257678c9a73eeb3b415e976b55eeddb3a79edd 100644
--- a/hello_wolf/yaml/processor_range_bearing.yaml
+++ b/demos/hello_wolf/yaml/processor_range_bearing.yaml
@@ -5,3 +5,5 @@ time_tolerance: 0.1
keyframe_vote:
voting_active: true
voting_aux_active: false
+
+apply_loss_function: true
diff --git a/hello_wolf/yaml/sensor_odom_2d.yaml b/demos/hello_wolf/yaml/sensor_odom_2d.yaml
similarity index 64%
rename from hello_wolf/yaml/sensor_odom_2d.yaml
rename to demos/hello_wolf/yaml/sensor_odom_2d.yaml
index 3ad7204855cae5c1e8e00cfdc011ff27d2725692..65ac28219471ad65180538e526521d7866391a01 100644
--- a/hello_wolf/yaml/sensor_odom_2d.yaml
+++ b/demos/hello_wolf/yaml/sensor_odom_2d.yaml
@@ -1,4 +1,5 @@
-type: "SensorOdom2d" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+type: "SensorOdom2d" # This must match the KEY used in the FactorySensor. Otherwise it is an error.
k_disp_to_disp: 0.1 # m^2 / m
k_rot_to_rot: 0.1 # rad^2 / rad
+apply_loss_function: true
diff --git a/hello_wolf/yaml/sensor_range_bearing.yaml b/demos/hello_wolf/yaml/sensor_range_bearing.yaml
similarity index 58%
rename from hello_wolf/yaml/sensor_range_bearing.yaml
rename to demos/hello_wolf/yaml/sensor_range_bearing.yaml
index 40a2387a041f8a34a41c5669218dd4a4eb00c0bf..d1625337dfc0b0d323d662a727c1ac298117a455 100644
--- a/hello_wolf/yaml/sensor_range_bearing.yaml
+++ b/demos/hello_wolf/yaml/sensor_range_bearing.yaml
@@ -1,4 +1,5 @@
type: "SensorRangeBearing"
noise_range_metres_std: 0.1
-noise_bearing_degrees_std: 0.5
+noise_bearing_degrees_std: 0.5
+apply_loss_function: true
diff --git a/include/core/capture/capture_base.h b/include/core/capture/capture_base.h
index 2491d5a6a5afbdc4d3b52a3e86003c3794b6cd81..d4e215e47cd576b48797c4e9f028be5a5a64edf1 100644
--- a/include/core/capture/capture_base.h
+++ b/include/core/capture/capture_base.h
@@ -29,7 +29,6 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s
FeatureBasePtrList feature_list_;
FactorBasePtrList constrained_by_list_;
SensorBaseWPtr sensor_ptr_; ///< Pointer to sensor
- SizeEigen calib_size_; ///< size of the calibration parameters (dynamic or static sensor params that are not fixed)
static unsigned int capture_id_count_;
@@ -83,7 +82,7 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s
// State blocks
- const std::string& getStructure() const;
+ const StateStructure& getStructure() const;
StateBlockPtr getStateBlock(const std::string& _key) const;
StateBlockPtr getStateBlock(const char _key) const { return getStateBlock(std::string(1, _key)); }
StateBlockPtr getSensorP() const;
@@ -93,24 +92,31 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s
virtual void fix() override;
virtual void unfix() override;
- bool hasCalibration() {return calib_size_ > 0;}
- SizeEigen getCalibSize() const;
- virtual Eigen::VectorXd getCalibration() const;
- void setCalibration(const Eigen::VectorXd& _calib);
void move(FrameBasePtr);
void link(FrameBasePtr);
template<typename classType, typename... T>
static std::shared_ptr<classType> emplace(FrameBasePtr _frm_ptr, T&&... all);
- protected:
- virtual SizeEigen computeCalibSize() const;
+ virtual void printHeader(int depth, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream ,
+ std::string _tabs = "") const;
+
+ void print(int depth, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream = std::cout,
+ std::string _tabs = "") const;
+
+ virtual CheckLog localCheck(bool _verbose, CaptureBasePtr _cap_ptr, std::ostream& _stream, std::string _tabs = "") const;
+ bool check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
private:
FeatureBasePtr addFeature(FeatureBasePtr _ft_ptr);
void removeFeature(FeatureBasePtr _ft_ptr);
-
- private:
- void updateCalibSize();
};
}
@@ -130,16 +136,6 @@ std::shared_ptr<classType> CaptureBase::emplace(FrameBasePtr _frm_ptr, T&&... al
return cpt;
}
-inline SizeEigen CaptureBase::getCalibSize() const
-{
- return calib_size_;
-}
-
-inline void CaptureBase::updateCalibSize()
-{
- calib_size_ = computeCalibSize();
-}
-
inline StateBlockPtr CaptureBase::getSensorP() const
{
return getStateBlock("P");
diff --git a/include/core/capture/capture_diff_drive.h b/include/core/capture/capture_diff_drive.h
index 2300a65b2a203ccc6bb9ca155fab6dbb6b2bdd96..52e865da7d6d3e441643480d9a5e6acc0014aa89 100644
--- a/include/core/capture/capture_diff_drive.h
+++ b/include/core/capture/capture_diff_drive.h
@@ -38,9 +38,6 @@ public:
StateBlockPtr _intr_ptr = nullptr);
virtual ~CaptureDiffDrive() = default;
-
- virtual Eigen::VectorXd correctDelta(const VectorXd& _delta, const VectorXd& _delta_error) const override;
-
};
} // namespace wolf
diff --git a/include/core/capture/capture_motion.h b/include/core/capture/capture_motion.h
index d78cd5169f3dfc523c9435c11dc65b41ef6b3591..fca231eeb7142aa456171c95438d0b322927a88d 100644
--- a/include/core/capture/capture_motion.h
+++ b/include/core/capture/capture_motion.h
@@ -50,8 +50,6 @@ class CaptureMotion : public CaptureBase
const TimeStamp& _ts,
SensorBasePtr _sensor_ptr,
const Eigen::VectorXd& _data,
- SizeEigen _delta_size,
- SizeEigen _delta_cov_size,
CaptureBasePtr _capture_origin_ptr);
CaptureMotion(const std::string & _type,
@@ -59,8 +57,6 @@ class CaptureMotion : public CaptureBase
SensorBasePtr _sensor_ptr,
const Eigen::VectorXd& _data,
const Eigen::MatrixXd& _data_cov,
- SizeEigen _delta_size,
- SizeEigen _delta_cov_size,
CaptureBasePtr _capture_origin_ptr,
StateBlockPtr _p_ptr = nullptr,
StateBlockPtr _o_ptr = nullptr,
@@ -88,18 +84,25 @@ class CaptureMotion : public CaptureBase
MatrixXd getJacobianCalib(const TimeStamp& _ts) const;
// Get delta preintegrated, and corrected for changes on calibration params
- VectorXd getDeltaCorrected(const VectorXd& _calib_current) const;
- VectorXd getDeltaCorrected(const VectorXd& _calib_current, const TimeStamp& _ts) const;
VectorXd getDeltaPreint() const;
VectorXd getDeltaPreint(const TimeStamp& _ts) const;
MatrixXd getDeltaPreintCov() const;
MatrixXd getDeltaPreintCov(const TimeStamp& _ts) const;
- virtual VectorXd correctDelta(const VectorXd& _delta, const VectorXd& _delta_error) const;
// Origin frame and capture
CaptureBasePtr getOriginCapture();
+ CaptureBasePtr getOriginCapture() const;
void setOriginCapture(CaptureBasePtr _capture_origin_ptr);
+ bool containsTimeStamp(const TimeStamp& _ts, double _time_tolerance);
+
+ virtual void printHeader(int depth, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream ,
+ std::string _tabs = "") const override;
+
// member data:
private:
Eigen::VectorXd data_; ///< Motion data in form of vector mandatory
@@ -144,7 +147,7 @@ inline MotionBuffer& CaptureMotion::getBuffer()
inline Eigen::MatrixXd CaptureMotion::getJacobianCalib() const
{
- return getBuffer().get().back().jacobian_calib_;
+ return getBuffer().back().jacobian_calib_;
}
inline Eigen::MatrixXd CaptureMotion::getJacobianCalib(const TimeStamp& _ts) const
@@ -152,13 +155,12 @@ inline Eigen::MatrixXd CaptureMotion::getJacobianCalib(const TimeStamp& _ts) con
return getBuffer().getMotion(_ts).jacobian_calib_;
}
-inline Eigen::VectorXd CaptureMotion::correctDelta(const VectorXd& _delta, const VectorXd& _delta_error) const
+inline wolf::CaptureBasePtr CaptureMotion::getOriginCapture()
{
- WOLF_DEBUG("WARNING: using Cartesian sum for delta correction. \nIf your deltas lie on a manifold, derive this function and implement the proper delta correction!")
- return _delta + _delta_error;
+ return capture_origin_ptr_.lock();
}
-inline wolf::CaptureBasePtr CaptureMotion::getOriginCapture()
+inline wolf::CaptureBasePtr CaptureMotion::getOriginCapture() const
{
return capture_origin_ptr_.lock();
}
@@ -180,7 +182,7 @@ inline void CaptureMotion::setCalibrationPreint(const VectorXd& _calib_preint)
inline VectorXd CaptureMotion::getDeltaPreint() const
{
- return getBuffer().get().back().delta_integr_;
+ return getBuffer().back().delta_integr_;
}
inline VectorXd CaptureMotion::getDeltaPreint(const TimeStamp& _ts) const
@@ -190,7 +192,7 @@ inline VectorXd CaptureMotion::getDeltaPreint(const TimeStamp& _ts) const
inline MatrixXd CaptureMotion::getDeltaPreintCov() const
{
- return getBuffer().get().back().delta_integr_cov_;
+ return getBuffer().back().delta_integr_cov_;
}
inline MatrixXd CaptureMotion::getDeltaPreintCov(const TimeStamp& _ts) const
diff --git a/include/core/capture/capture_odom_2d.h b/include/core/capture/capture_odom_2d.h
index d18217279601157c9a1b2eb3c4e7f4ba4dae6cbb..8856c363a7f576413a3428fa7b206ab6bd17f824 100644
--- a/include/core/capture/capture_odom_2d.h
+++ b/include/core/capture/capture_odom_2d.h
@@ -33,17 +33,8 @@ class CaptureOdom2d : public CaptureMotion
virtual ~CaptureOdom2d();
- virtual VectorXd correctDelta(const VectorXd& _delta, const VectorXd& _delta_error) const override;
-
};
-inline Eigen::VectorXd CaptureOdom2d::correctDelta(const VectorXd& _delta, const VectorXd& _delta_error) const
-{
- Vector3d delta = _delta + _delta_error;
- delta(2) = pi2pi(delta(2));
- return delta;
-}
-
} /* namespace wolf */
#endif /* CAPTURE_ODOM_2d_H_ */
diff --git a/include/core/capture/capture_odom_3d.h b/include/core/capture/capture_odom_3d.h
index 7e4ee02c8b694cc6ae2b8e8b6d9e6803d7587c65..b4a293f8c4059b9346a334c31e017a967267a902 100644
--- a/include/core/capture/capture_odom_3d.h
+++ b/include/core/capture/capture_odom_3d.h
@@ -33,8 +33,6 @@ class CaptureOdom3d : public CaptureMotion
virtual ~CaptureOdom3d();
- virtual VectorXd correctDelta(const VectorXd& _delta, const VectorXd& _delta_error) const override;
-
};
} /* namespace wolf */
diff --git a/include/core/capture/capture_velocity.h b/include/core/capture/capture_velocity.h
deleted file mode 100644
index d2d1c686dfd84c0d8d9ca3c4fbe0a1b7d17091e1..0000000000000000000000000000000000000000
--- a/include/core/capture/capture_velocity.h
+++ /dev/null
@@ -1,59 +0,0 @@
-/**
- * \file capture_velocity.h
- *
- * Created on: Oct 20, 2016
- * \author: Jeremie Deray
- */
-
-#ifndef _WOLF_CAPTURE_VELOCITY_H_
-#define _WOLF_CAPTURE_VELOCITY_H_
-
-//wolf includes
-#include "core/capture/capture_motion.h"
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(CaptureVelocity);
-
-/**
- * @brief The CaptureVelocity class
- *
- * Represents a velocity.
- */
-class CaptureVelocity : public CaptureMotion
-{
-protected:
-
- using NodeBase::node_type_;
-
-public:
-
- /**
- * \brief Constructor
- **/
- CaptureVelocity(const TimeStamp& _ts,
- const SensorBasePtr& _sensor_ptr,
- const Eigen::VectorXd& _velocity,
- SizeEigen _delta_size, SizeEigen _delta_cov_size,
- CaptureBasePtr _capture_origin_ptr);
-
- CaptureVelocity(const TimeStamp& _ts,
- const SensorBasePtr& _sensor_ptr,
- const Eigen::VectorXd& _velocity,
- const Eigen::MatrixXd& _velocity_cov,
- SizeEigen _delta_size, SizeEigen _delta_cov_size,
- CaptureBasePtr _capture_origin_ptr,
- StateBlockPtr _p_ptr = nullptr,
- StateBlockPtr _o_ptr = nullptr,
- StateBlockPtr _intr_ptr = nullptr);
-
- virtual ~CaptureVelocity() = default;
-
- const Eigen::VectorXd& getVelocity() const;
-
- const Eigen::MatrixXd& getVelocityCov() const;
-};
-
-} // namespace wolf
-
-#endif /* _WOLF_CAPTURE_VELOCITY_H_ */
diff --git a/include/core/ceres_wrapper/ceres_manager.h b/include/core/ceres_wrapper/ceres_manager.h
index 802ab7b49dabf93455f8f93fc8c30745415fe33e..0841f7421ef37f039b2fa77449ffafefd52b7118 100644
--- a/include/core/ceres_wrapper/ceres_manager.h
+++ b/include/core/ceres_wrapper/ceres_manager.h
@@ -62,10 +62,7 @@ class CeresManager : public SolverManager
ceres::Solver::Summary getSummary();
- std::unique_ptr<ceres::Problem>& getCeresProblem()
- {
- return ceres_problem_;
- }
+ std::unique_ptr<ceres::Problem>& getCeresProblem();
virtual void computeCovariances(CovarianceBlocksToBeComputed _blocks
= CovarianceBlocksToBeComputed::ROBOT_LANDMARKS) override;
@@ -82,25 +79,26 @@ class CeresManager : public SolverManager
ceres::Solver::Options& getSolverOptions();
- void check();
const Eigen::SparseMatrixd computeHessian() const;
protected:
- std::string solveImpl(const ReportVerbosity report_level) override;
+ virtual bool checkDerived(std::string prefix="") const override;
- void addFactor(const FactorBasePtr& fac_ptr) override;
+ virtual std::string solveDerived(const ReportVerbosity report_level) override;
- void removeFactor(const FactorBasePtr& fac_ptr) override;
+ virtual void addFactorDerived(const FactorBasePtr& fac_ptr) override;
- void addStateBlock(const StateBlockPtr& state_ptr) override;
+ virtual void removeFactorDerived(const FactorBasePtr& fac_ptr) override;
- void removeStateBlock(const StateBlockPtr& state_ptr) override;
+ virtual void addStateBlockDerived(const StateBlockPtr& state_ptr) override;
- void updateStateBlockStatus(const StateBlockPtr& state_ptr) override;
+ virtual void removeStateBlockDerived(const StateBlockPtr& state_ptr) override;
- void updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr) override;
+ virtual void updateStateBlockStatusDerived(const StateBlockPtr& state_ptr) override;
+
+ virtual void updateStateBlockLocalParametrizationDerived(const StateBlockPtr& state_ptr) override;
ceres::CostFunctionPtr createCostFunction(const FactorBasePtr& _fac_ptr);
@@ -114,6 +112,11 @@ inline ceres::Solver::Summary CeresManager::getSummary()
return summary_;
}
+inline std::unique_ptr<ceres::Problem>& CeresManager::getCeresProblem()
+{
+ return ceres_problem_;
+}
+
inline ceres::Solver::Options& CeresManager::getSolverOptions()
{
return ceres_options_;
diff --git a/include/core/common/factory.h b/include/core/common/factory.h
index 810871e64fd3ed645d3412a21836ea8d3e6b93cc..70f66f6e5f39a7525bcf8d5cccbf762c4ca9f2c0 100644
--- a/include/core/common/factory.h
+++ b/include/core/common/factory.h
@@ -112,9 +112,9 @@ namespace wolf
* The first thing to know is that we have defined typedefs for the templates that we are using. For example:
*
* \code
- * typedef Factory<ParamsSensorBase, std::string> ParamsSensorFactory;
- * typedef Factory<ProcessorParamsBase, std::string> ProcessorParamsFactory;
- * typedef Factory<LandmarkBase, YAML::Node> LandmarkFactory;
+ * typedef Factory<ParamsSensorBase, std::string> FactoryParamsSensor;
+ * typedef Factory<ParamsProcessorBase, std::string> FactoryParamsProcessor;
+ * typedef Factory<LandmarkBase, YAML::Node> FactoryLandmark;
* \endcode
*
* Second to know, the Factory class is a <a href="http://stackoverflow.com/questions/1008019/c-singleton-design-pattern#1008289">singleton</a>: it can only exist once in your application.
@@ -129,7 +129,7 @@ namespace wolf
* You can then call the methods you like, e.g. to create a landmark, you use:
*
* \code
- * LandmarkFactory::get().create(TYPE, args...); // see below for creating objects ...
+ * FactoryLandmark::get().create(TYPE, args...); // see below for creating objects ...
* \endcode
*
* #### Write creator methods (in your derived object classes)
@@ -164,7 +164,7 @@ namespace wolf
* that knows how to create your specific object, e.g.:
*
* \code
- * LandmarkFactory::get().registerCreator("POLYLINE 2d", LandmarkPolyline2d::create);
+ * FactoryLandmark::get().registerCreator("POLYLINE 2d", LandmarkPolyline2d::create);
* \endcode
*
* #### Automatic registration
@@ -172,7 +172,7 @@ namespace wolf
* For example, in sensor_camera_yaml.cpp we find the line:
*
* \code
- * const bool registered_camera_intr = ParamsSensorFactory::get().registerCreator("CAMERA", createParamsSensorCamera);
+ * const bool registered_camera_intr = FactoryParamsSensor::get().registerCreator("CAMERA", createParamsSensorCamera);
* \endcode
*
* which is a static invocation (i.e., it is placed at global scope outside of the ParamsSensorCamera class).
@@ -199,7 +199,7 @@ namespace wolf
* or even better, make use of the convenient typedefs:
*
* \code
- * LandmarkBasePtr lmk_ptr = LandmarkFactory::get().create("POLYLINE 2d", lmk_yaml_node);
+ * LandmarkBasePtr lmk_ptr = FactoryLandmark::get().create("POLYLINE 2d", lmk_yaml_node);
* \endcode
*
* ### Examples
@@ -239,21 +239,21 @@ namespace wolf
* // Register landmark creator (put the register code inside an unnamed namespace):
* namespace
* {
- * const bool registered_lmk_polyline_2d = LandmarkFactory::get().registerCreator("POLYLINE 2d", LandmarkPolyline2d::create);
+ * const bool registered_lmk_polyline_2d = FactoryLandmark::get().registerCreator("POLYLINE 2d", LandmarkPolyline2d::create);
* }
*
* \endcode
*
* ### More information
- * - ParamsSensorFactory: typedef of this template to create intrinsic structs deriving from ParamsSensorBase directly from YAML files.
- * - ProcessorParamsFactory: typedef of this template to create processor params structs deriving from ProcessorParamsBase directly from YAML files.
- * - LandmarkFactory: typedef of this template to create landmarks deriving from LandmarkBase directly from YAML nodes.
+ * - FactoryParamsSensor: typedef of this template to create intrinsic structs deriving from ParamsSensorBase directly from YAML files.
+ * - FactoryParamsProcessor: typedef of this template to create processor params structs deriving from ParamsProcessorBase directly from YAML files.
+ * - FactoryLandmark: typedef of this template to create landmarks deriving from LandmarkBase directly from YAML nodes.
* - Problem::loadMap() : to load a maps directly from YAML files.
* - You can also check the code in the example file ````src/examples/test_map_yaml.cpp````.
*
* #### See also
- * - SensorFactory: to create sensors
- * - ProcessorFactory: to create processors.
+ * - FactorySensor: to create sensors
+ * - FactoryProcessor: to create processors.
* - Problem::installSensor() : to install sensors in WOLF Problem.
* - Problem::installProcessor() : to install processors in WOLF Problem.
*
@@ -275,7 +275,7 @@ class Factory
bool registerCreator(const std::string& _type, CreatorCallback createFn);
bool unregisterCreator(const std::string& _type);
TypeBasePtr create(const std::string& _type, TypeInput... _input);
- std::string getClass();
+ std::string getClass() const;
private:
CallbackMap callbacks_;
@@ -332,7 +332,7 @@ inline Factory<TypeBase, TypeInput...>& Factory<TypeBase, TypeInput...>::get()
}
template<class TypeBase, typename... TypeInput>
-inline std::string Factory<TypeBase, TypeInput...>::getClass()
+inline std::string Factory<TypeBase, TypeInput...>::getClass() const
{
return "Factory<class TypeBase>";
}
@@ -348,11 +348,11 @@ namespace wolf
// Landmarks from YAML
class LandmarkBase;
typedef Factory<LandmarkBase,
- const YAML::Node&> LandmarkFactory;
+ const YAML::Node&> FactoryLandmark;
template<>
-inline std::string LandmarkFactory::getClass()
+inline std::string FactoryLandmark::getClass() const
{
- return "LandmarkFactory";
+ return "FactoryLandmark";
}
diff --git a/include/core/common/node_base.h b/include/core/common/node_base.h
index 4488ab30ec274eeb84e49b7dcd600de9a93acf5e..dcd7d37176c2eb4c36aea7c8b342aa064b43a896 100644
--- a/include/core/common/node_base.h
+++ b/include/core/common/node_base.h
@@ -3,6 +3,7 @@
// Wolf includes
#include "core/common/wolf.h"
+#include "core/utils/check_log.h"
namespace wolf {
@@ -27,8 +28,8 @@ namespace wolf {
* - "LANDMARK"
*
* - A unique type, which is a subcategory of the above. The list here cannot be exhaustive, but a few examples follow:
- * - "SesorCamera"
- * - "SesorLaser2d"
+ * - "SensorCamera"
+ * - "SensorLaser2d"
* - "LandmarkPoint3d"
* - "ProcessorLaser2d"
*
diff --git a/include/core/common/wolf.h b/include/core/common/wolf.h
index 64a4741c518ecd0f75bf029aa4e3267b1e3f8c4e..319b2df0bf0d0ed464ca8a55647a63594a1bb4a3 100644
--- a/include/core/common/wolf.h
+++ b/include/core/common/wolf.h
@@ -212,6 +212,10 @@ WOLF_PTR_TYPEDEFS(NodeBase);
// Problem
WOLF_PTR_TYPEDEFS(Problem);
+// Tree Manager
+WOLF_PTR_TYPEDEFS(TreeManagerBase);
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsTreeManagerBase);
+
// Hardware
WOLF_PTR_TYPEDEFS(HardwareBase);
@@ -230,7 +234,7 @@ WOLF_LIST_TYPEDEFS(ProcessorBase);
WOLF_PTR_TYPEDEFS(ProcessorMotion);
// - - Processor params
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsBase);
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorBase);
// Trajectory
WOLF_PTR_TYPEDEFS(TrajectoryBase);
@@ -279,7 +283,6 @@ inline bool file_exists(const std::string& _name)
inline const Eigen::Vector3d gravity(void) {
return Eigen::Vector3d(0,0,-9.806);
}
-
} // namespace wolf
#endif /* WOLF_H_ */
diff --git a/include/core/factor/factor_analytic.h b/include/core/factor/factor_analytic.h
index f1f9f02e617be2046471b7cecb26959bfe610143..34fd699a7b1461f9ab147c9d5d9f6ef2496b2f97 100644
--- a/include/core/factor/factor_analytic.h
+++ b/include/core/factor/factor_analytic.h
@@ -19,6 +19,7 @@ class FactorAnalytic: public FactorBase
public:
FactorAnalytic(const std::string& _tp,
+ const FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
diff --git a/include/core/factor/factor_autodiff.h b/include/core/factor/factor_autodiff.h
index d5e86d2f168acce03feb9085420de1c6ad901ab8..dce0203c894704953ff3001732f418b2636343a3 100644
--- a/include/core/factor/factor_autodiff.h
+++ b/include/core/factor/factor_autodiff.h
@@ -15,7 +15,7 @@
namespace wolf {
//template class FactorAutodiff
-template <class FacT, unsigned int RES, unsigned int B0, unsigned int B1 = 0, unsigned int B2 = 0, unsigned int B3 = 0, unsigned int B4 = 0, unsigned int B5 = 0, unsigned int B6 = 0, unsigned int B7 = 0, unsigned int B8 = 0, unsigned int B9 = 0>
+template <class FacT, unsigned int RES, unsigned int B0, unsigned int B1 = 0, unsigned int B2 = 0, unsigned int B3 = 0, unsigned int B4 = 0, unsigned int B5 = 0, unsigned int B6 = 0, unsigned int B7 = 0, unsigned int B8 = 0, unsigned int B9 = 0, unsigned int B10 = 0, unsigned int B11 = 0>
class FactorAutodiff : public FactorBase
{
public:
@@ -31,34 +31,40 @@ class FactorAutodiff : public FactorBase
static const unsigned int block7Size = B7;
static const unsigned int block8Size = B8;
static const unsigned int block9Size = B9;
- static const unsigned int n_blocks = 10;
+ static const unsigned int block10Size = B10;
+ static const unsigned int block11Size = B11;
+ static const unsigned int n_blocks = 12;
static const std::vector<unsigned int> state_block_sizes_;
typedef ceres::Jet<double, B0 + B1 + B2 + B3 + B4 +
- B5 + B6 + B7 + B8 + B9> WolfJet;
+ B5 + B6 + B7 + B8 + B9 +
+ B10 + B11> WolfJet;
protected:
std::vector<StateBlockPtr> state_ptrs_;
static const std::vector<unsigned int> jacobian_locations_;
- std::array<WolfJet, RES>* residuals_jets_;
- std::array<WolfJet, B0>* jets_0_;
- std::array<WolfJet, B1>* jets_1_;
- std::array<WolfJet, B2>* jets_2_;
- std::array<WolfJet, B3>* jets_3_;
- std::array<WolfJet, B4>* jets_4_;
- std::array<WolfJet, B5>* jets_5_;
- std::array<WolfJet, B6>* jets_6_;
- std::array<WolfJet, B7>* jets_7_;
- std::array<WolfJet, B8>* jets_8_;
- std::array<WolfJet, B9>* jets_9_;
+ mutable std::array<WolfJet, RES> residuals_jets_;
+ mutable std::array<WolfJet, B0> jets_0_;
+ mutable std::array<WolfJet, B1> jets_1_;
+ mutable std::array<WolfJet, B2> jets_2_;
+ mutable std::array<WolfJet, B3> jets_3_;
+ mutable std::array<WolfJet, B4> jets_4_;
+ mutable std::array<WolfJet, B5> jets_5_;
+ mutable std::array<WolfJet, B6> jets_6_;
+ mutable std::array<WolfJet, B7> jets_7_;
+ mutable std::array<WolfJet, B8> jets_8_;
+ mutable std::array<WolfJet, B9> jets_9_;
+ mutable std::array<WolfJet, B10> jets_10_;
+ mutable std::array<WolfJet, B11> jets_11_;
public:
/** \brief Constructor valid for all categories (ABSOLUTE, FRAME, FEATURE, LANDMARK)
**/
FactorAutodiff(const std::string& _tp,
+ const FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
@@ -75,58 +81,56 @@ class FactorAutodiff : public FactorBase
StateBlockPtr _state6Ptr,
StateBlockPtr _state7Ptr,
StateBlockPtr _state8Ptr,
- StateBlockPtr _state9Ptr) :
- FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
- state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr}),
- residuals_jets_(new std::array<WolfJet, RES>),
- jets_0_(new std::array<WolfJet, B0>),
- jets_1_(new std::array<WolfJet, B1>),
- jets_2_(new std::array<WolfJet, B2>),
- jets_3_(new std::array<WolfJet, B3>),
- jets_4_(new std::array<WolfJet, B4>),
- jets_5_(new std::array<WolfJet, B5>),
- jets_6_(new std::array<WolfJet, B6>),
- jets_7_(new std::array<WolfJet, B7>),
- jets_8_(new std::array<WolfJet, B8>),
- jets_9_(new std::array<WolfJet, B9>)
+ StateBlockPtr _state9Ptr,
+ StateBlockPtr _state10Ptr,
+ StateBlockPtr _state11Ptr) :
+ FactorBase(_tp, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr,_state10Ptr,_state11Ptr})
{
+ // asserts for null states
+ assert(_state0Ptr != nullptr && "nullptr state block");
+ assert(_state1Ptr != nullptr && "nullptr state block");
+ assert(_state2Ptr != nullptr && "nullptr state block");
+ assert(_state3Ptr != nullptr && "nullptr state block");
+ assert(_state4Ptr != nullptr && "nullptr state block");
+ assert(_state5Ptr != nullptr && "nullptr state block");
+ assert(_state6Ptr != nullptr && "nullptr state block");
+ assert(_state7Ptr != nullptr && "nullptr state block");
+ assert(_state8Ptr != nullptr && "nullptr state block");
+ assert(_state9Ptr != nullptr && "nullptr state block");
+ assert(_state10Ptr != nullptr && "nullptr state block");
+ assert(_state11Ptr != nullptr && "nullptr state block");
+
// initialize jets
unsigned int last_jet_idx = 0;
for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
+ jets_1_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i] = WolfJet(0, last_jet_idx++);
+ jets_2_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B3; i++)
- (*jets_3_)[i] = WolfJet(0, last_jet_idx++);
+ jets_3_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B4; i++)
- (*jets_4_)[i] = WolfJet(0, last_jet_idx++);
+ jets_4_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B5; i++)
- (*jets_5_)[i] = WolfJet(0, last_jet_idx++);
+ jets_5_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B6; i++)
- (*jets_6_)[i] = WolfJet(0, last_jet_idx++);
+ jets_6_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B7; i++)
- (*jets_7_)[i] = WolfJet(0, last_jet_idx++);
+ jets_7_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B8; i++)
- (*jets_8_)[i] = WolfJet(0, last_jet_idx++);
+ jets_8_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B9; i++)
- (*jets_9_)[i] = WolfJet(0, last_jet_idx++);
+ jets_9_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B10; i++)
+ jets_10_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B11; i++)
+ jets_11_[i] = WolfJet(0, last_jet_idx++);
}
virtual ~FactorAutodiff()
{
- delete jets_0_;
- delete jets_1_;
- delete jets_2_;
- delete jets_3_;
- delete jets_4_;
- delete jets_5_;
- delete jets_6_;
- delete jets_7_;
- delete jets_8_;
- delete jets_9_;
- delete residuals_jets_;
}
virtual JacobianMethod getJacobianMethod() const
@@ -154,6 +158,8 @@ class FactorAutodiff : public FactorBase
parameters[7],
parameters[8],
parameters[9],
+ parameters[10],
+ parameters[11],
residuals);
}
// also compute jacobians
@@ -165,28 +171,30 @@ class FactorAutodiff : public FactorBase
updateJetsRealPart(param_vec);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_->data(),
- jets_1_->data(),
- jets_2_->data(),
- jets_3_->data(),
- jets_4_->data(),
- jets_5_->data(),
- jets_6_->data(),
- jets_7_->data(),
- jets_8_->data(),
- jets_9_->data(),
- residuals_jets_->data());
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ jets_7_.data(),
+ jets_8_.data(),
+ jets_9_.data(),
+ jets_10_.data(),
+ jets_11_.data(),
+ residuals_jets_.data());
// fill the residual array
for (unsigned int i = 0; i < RES; i++)
- residuals[i] = (*residuals_jets_)[i].a;
+ residuals[i] = residuals_jets_[i].a;
// fill the jacobian matrices
for (unsigned int i = 0; i<n_blocks; i++)
if (jacobians[i] != nullptr)
for (unsigned int row = 0; row < RES; row++)
- std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
- (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
jacobians[i] + row * state_block_sizes_.at(i));
}
return true;
@@ -199,25 +207,29 @@ class FactorAutodiff : public FactorBase
{
// update jets real part
for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i].a = parameters[0][i];
+ jets_0_[i].a = parameters[0][i];
for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i].a = parameters[1][i];
+ jets_1_[i].a = parameters[1][i];
for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i].a = parameters[2][i];
+ jets_2_[i].a = parameters[2][i];
for (unsigned int i = 0; i < B3; i++)
- (*jets_3_)[i].a = parameters[3][i];
+ jets_3_[i].a = parameters[3][i];
for (unsigned int i = 0; i < B4; i++)
- (*jets_4_)[i].a = parameters[4][i];
+ jets_4_[i].a = parameters[4][i];
for (unsigned int i = 0; i < B5; i++)
- (*jets_5_)[i].a = parameters[5][i];
+ jets_5_[i].a = parameters[5][i];
for (unsigned int i = 0; i < B6; i++)
- (*jets_6_)[i].a = parameters[6][i];
+ jets_6_[i].a = parameters[6][i];
for (unsigned int i = 0; i < B7; i++)
- (*jets_7_)[i].a = parameters[7][i];
+ jets_7_[i].a = parameters[7][i];
for (unsigned int i = 0; i < B8; i++)
- (*jets_8_)[i].a = parameters[8][i];
+ jets_8_[i].a = parameters[8][i];
for (unsigned int i = 0; i < B9; i++)
- (*jets_9_)[i].a = parameters[9][i];
+ jets_9_[i].a = parameters[9][i];
+ for (unsigned int i = 0; i < B10; i++)
+ jets_10_[i].a = parameters[10][i];
+ for (unsigned int i = 0; i < B11; i++)
+ jets_11_[i].a = parameters[11][i];
}
/** \brief Returns a vector of Jacobian matrix corresponding to each state block evaluated in the point provided in _states_ptr
@@ -234,21 +246,23 @@ class FactorAutodiff : public FactorBase
updateJetsRealPart(_states_ptr);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_->data(),
- jets_1_->data(),
- jets_2_->data(),
- jets_3_->data(),
- jets_4_->data(),
- jets_5_->data(),
- jets_6_->data(),
- jets_7_->data(),
- jets_8_->data(),
- jets_9_->data(),
- residuals_jets_->data());
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ jets_7_.data(),
+ jets_8_.data(),
+ jets_9_.data(),
+ jets_10_.data(),
+ jets_11_.data(),
+ residuals_jets_.data());
// fill the residual vector
for (unsigned int i = 0; i < RES; i++)
- residual_(i) = (*residuals_jets_)[i].a;
+ residual_(i) = residuals_jets_[i].a;
// fill the jacobian matrices
for (unsigned int i = 0; i<n_blocks; i++)
@@ -258,8 +272,8 @@ class FactorAutodiff : public FactorBase
// Fill Jacobian rows from jets
if (!state_ptrs_[i]->isFixed())
for (unsigned int row = 0; row < RES; row++)
- std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
- (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
Ji.data() + row * state_block_sizes_.at(i));
// add to the Jacobians vector
jacobians_.push_back(Ji);
@@ -299,10 +313,531 @@ class FactorAutodiff : public FactorBase
}
};
+
+////////////////// SPECIALIZATION 11 BLOCKS ////////////////////////////////////////////////////////////////////////
+
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9,unsigned int B10>
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,0> : public FactorBase
+{
+ public:
+
+ static const unsigned int residualSize = RES;
+ static const unsigned int block0Size = B0;
+ static const unsigned int block1Size = B1;
+ static const unsigned int block2Size = B2;
+ static const unsigned int block3Size = B3;
+ static const unsigned int block4Size = B4;
+ static const unsigned int block5Size = B5;
+ static const unsigned int block6Size = B6;
+ static const unsigned int block7Size = B7;
+ static const unsigned int block8Size = B8;
+ static const unsigned int block9Size = B9;
+ static const unsigned int block10Size = B10;
+ static const unsigned int block11Size = 0;
+ static const unsigned int n_blocks = 11;
+
+ static const std::vector<unsigned int> state_block_sizes_;
+
+ typedef ceres::Jet<double, B0 + B1 + B2 + B3 + B4 +
+ B5 + B6 + B7 + B8 + B9 + B10> WolfJet;
+
+ protected:
+
+ std::vector<StateBlockPtr> state_ptrs_;
+
+ static const std::vector<unsigned int> jacobian_locations_;
+ mutable std::array<WolfJet, RES> residuals_jets_;
+ mutable std::array<WolfJet, B0> jets_0_;
+ mutable std::array<WolfJet, B1> jets_1_;
+ mutable std::array<WolfJet, B2> jets_2_;
+ mutable std::array<WolfJet, B3> jets_3_;
+ mutable std::array<WolfJet, B4> jets_4_;
+ mutable std::array<WolfJet, B5> jets_5_;
+ mutable std::array<WolfJet, B6> jets_6_;
+ mutable std::array<WolfJet, B7> jets_7_;
+ mutable std::array<WolfJet, B8> jets_8_;
+ mutable std::array<WolfJet, B9> jets_9_;
+ mutable std::array<WolfJet, B10> jets_10_;
+
+ public:
+
+ FactorAutodiff(const std::string& _tp,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtr& _frame_other_ptr,
+ const CaptureBasePtr& _capture_other_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr,
+ StateBlockPtr _state6Ptr,
+ StateBlockPtr _state7Ptr,
+ StateBlockPtr _state8Ptr,
+ StateBlockPtr _state9Ptr,
+ StateBlockPtr _state10Ptr) :
+ FactorBase(_tp, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr,_state10Ptr})
+ {
+ // asserts for null states
+ assert(_state0Ptr != nullptr && "nullptr state block");
+ assert(_state1Ptr != nullptr && "nullptr state block");
+ assert(_state2Ptr != nullptr && "nullptr state block");
+ assert(_state3Ptr != nullptr && "nullptr state block");
+ assert(_state4Ptr != nullptr && "nullptr state block");
+ assert(_state5Ptr != nullptr && "nullptr state block");
+ assert(_state6Ptr != nullptr && "nullptr state block");
+ assert(_state7Ptr != nullptr && "nullptr state block");
+ assert(_state8Ptr != nullptr && "nullptr state block");
+ assert(_state9Ptr != nullptr && "nullptr state block");
+ assert(_state10Ptr != nullptr && "nullptr state block");
+
+ // initialize jets
+ unsigned int last_jet_idx = 0;
+ for (unsigned int i = 0; i < B0; i++)
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B1; i++)
+ jets_1_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B2; i++)
+ jets_2_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B3; i++)
+ jets_3_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B4; i++)
+ jets_4_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B5; i++)
+ jets_5_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B6; i++)
+ jets_6_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B7; i++)
+ jets_7_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B8; i++)
+ jets_8_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B9; i++)
+ jets_9_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B10; i++)
+ jets_10_[i] = WolfJet(0, last_jet_idx++);
+ state_ptrs_.resize(n_blocks);
+ }
+
+ virtual ~FactorAutodiff()
+ {
+ }
+
+ virtual JacobianMethod getJacobianMethod() const
+ {
+ return JAC_AUTO;
+ }
+
+ virtual bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const
+ {
+ // only residuals
+ if (jacobians == nullptr)
+ {
+ (*static_cast<FacT const*>(this))(parameters[0],
+ parameters[1],
+ parameters[2],
+ parameters[3],
+ parameters[4],
+ parameters[5],
+ parameters[6],
+ parameters[7],
+ parameters[8],
+ parameters[9],
+ parameters[10],
+ residuals);
+ }
+ // also compute jacobians
+ else
+ {
+ // update jets real part
+ std::vector<double const*> param_vec;
+ param_vec.assign(parameters,parameters+n_blocks);
+ updateJetsRealPart(param_vec);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ jets_7_.data(),
+ jets_8_.data(),
+ jets_9_.data(),
+ jets_10_.data(),
+ residuals_jets_.data());
+
+ // fill the residual array
+ for (unsigned int i = 0; i < RES; i++)
+ residuals[i] = residuals_jets_[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ if (jacobians[i] != nullptr)
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ jacobians[i] + row * state_block_sizes_.at(i));
+ }
+ return true;
+ }
+
+ void updateJetsRealPart(const std::vector<double const*>& parameters) const
+ {
+ // update jets real part
+ for (unsigned int i = 0; i < B0; i++)
+ jets_0_[i].a = parameters[0][i];
+ for (unsigned int i = 0; i < B1; i++)
+ jets_1_[i].a = parameters[1][i];
+ for (unsigned int i = 0; i < B2; i++)
+ jets_2_[i].a = parameters[2][i];
+ for (unsigned int i = 0; i < B3; i++)
+ jets_3_[i].a = parameters[3][i];
+ for (unsigned int i = 0; i < B4; i++)
+ jets_4_[i].a = parameters[4][i];
+ for (unsigned int i = 0; i < B5; i++)
+ jets_5_[i].a = parameters[5][i];
+ for (unsigned int i = 0; i < B6; i++)
+ jets_6_[i].a = parameters[6][i];
+ for (unsigned int i = 0; i < B7; i++)
+ jets_7_[i].a = parameters[7][i];
+ for (unsigned int i = 0; i < B8; i++)
+ jets_8_[i].a = parameters[8][i];
+ for (unsigned int i = 0; i < B9; i++)
+ jets_9_[i].a = parameters[9][i];
+ for (unsigned int i = 0; i < B10; i++)
+ jets_10_[i].a = parameters[10][i];
+ }
+
+ virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const
+ {
+ assert(residual_.size() == RES);
+ jacobians_.clear();
+
+ assert(_states_ptr.size() == n_blocks);
+
+ // update jets real part
+ updateJetsRealPart(_states_ptr);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ jets_7_.data(),
+ jets_8_.data(),
+ jets_9_.data(),
+ jets_10_.data(),
+ residuals_jets_.data());
+
+ // fill the residual vector
+ for (unsigned int i = 0; i < RES; i++)
+ residual_(i) = residuals_jets_[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ {
+ // Create a row major Jacobian
+ Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
+ // Fill Jacobian rows from jets
+ if (!state_ptrs_[i]->isFixed())
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ Ji.data() + row * state_block_sizes_.at(i));
+ // add to the Jacobians vector
+ jacobians_.push_back(Ji);
+ }
+
+ // print jacobian matrices
+// for (unsigned int i = 0; i < n_blocks; i++)
+// std::cout << jacobians_[i] << std::endl << std::endl;
+ }
+
+ virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const
+ {
+ return state_ptrs_;
+ }
+
+ virtual std::vector<unsigned int> getStateSizes() const
+ {
+ return state_block_sizes_;
+ }
+
+ virtual unsigned int getSize() const
+ {
+ return RES;
+ }
+};
+
+////////////////// SPECIALIZATION 10 BLOCKS ////////////////////////////////////////////////////////////////////////
+
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9>
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,0,0> : public FactorBase
+{
+ public:
+
+ static const unsigned int residualSize = RES;
+ static const unsigned int block0Size = B0;
+ static const unsigned int block1Size = B1;
+ static const unsigned int block2Size = B2;
+ static const unsigned int block3Size = B3;
+ static const unsigned int block4Size = B4;
+ static const unsigned int block5Size = B5;
+ static const unsigned int block6Size = B6;
+ static const unsigned int block7Size = B7;
+ static const unsigned int block8Size = B8;
+ static const unsigned int block9Size = B9;
+ static const unsigned int block10Size = 0;
+ static const unsigned int block11Size = 0;
+ static const unsigned int n_blocks = 10;
+
+ static const std::vector<unsigned int> state_block_sizes_;
+
+ typedef ceres::Jet<double, B0 + B1 + B2 + B3 + B4 +
+ B5 + B6 + B7 + B8 + B9> WolfJet;
+
+ protected:
+
+ std::vector<StateBlockPtr> state_ptrs_;
+
+ static const std::vector<unsigned int> jacobian_locations_;
+ mutable std::array<WolfJet, RES> residuals_jets_;
+ mutable std::array<WolfJet, B0> jets_0_;
+ mutable std::array<WolfJet, B1> jets_1_;
+ mutable std::array<WolfJet, B2> jets_2_;
+ mutable std::array<WolfJet, B3> jets_3_;
+ mutable std::array<WolfJet, B4> jets_4_;
+ mutable std::array<WolfJet, B5> jets_5_;
+ mutable std::array<WolfJet, B6> jets_6_;
+ mutable std::array<WolfJet, B7> jets_7_;
+ mutable std::array<WolfJet, B8> jets_8_;
+ mutable std::array<WolfJet, B9> jets_9_;
+
+ public:
+
+ FactorAutodiff(const std::string& _tp,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtr& _frame_other_ptr,
+ const CaptureBasePtr& _capture_other_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr,
+ StateBlockPtr _state6Ptr,
+ StateBlockPtr _state7Ptr,
+ StateBlockPtr _state8Ptr,
+ StateBlockPtr _state9Ptr) :
+ FactorBase(_tp, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr})
+ {
+ // asserts for null states
+ assert(_state0Ptr != nullptr && "nullptr state block");
+ assert(_state1Ptr != nullptr && "nullptr state block");
+ assert(_state2Ptr != nullptr && "nullptr state block");
+ assert(_state3Ptr != nullptr && "nullptr state block");
+ assert(_state4Ptr != nullptr && "nullptr state block");
+ assert(_state5Ptr != nullptr && "nullptr state block");
+ assert(_state6Ptr != nullptr && "nullptr state block");
+ assert(_state7Ptr != nullptr && "nullptr state block");
+ assert(_state8Ptr != nullptr && "nullptr state block");
+ assert(_state9Ptr != nullptr && "nullptr state block");
+
+ // initialize jets
+ unsigned int last_jet_idx = 0;
+ for (unsigned int i = 0; i < B0; i++)
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B1; i++)
+ jets_1_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B2; i++)
+ jets_2_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B3; i++)
+ jets_3_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B4; i++)
+ jets_4_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B5; i++)
+ jets_5_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B6; i++)
+ jets_6_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B7; i++)
+ jets_7_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B8; i++)
+ jets_8_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B9; i++)
+ jets_9_[i] = WolfJet(0, last_jet_idx++);
+ state_ptrs_.resize(n_blocks);
+ }
+
+ virtual ~FactorAutodiff()
+ {
+ }
+
+ virtual JacobianMethod getJacobianMethod() const
+ {
+ return JAC_AUTO;
+ }
+
+ virtual bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const
+ {
+ // only residuals
+ if (jacobians == nullptr)
+ {
+ (*static_cast<FacT const*>(this))(parameters[0],
+ parameters[1],
+ parameters[2],
+ parameters[3],
+ parameters[4],
+ parameters[5],
+ parameters[6],
+ parameters[7],
+ parameters[8],
+ parameters[9],
+ residuals);
+ }
+ // also compute jacobians
+ else
+ {
+ // update jets real part
+ std::vector<double const*> param_vec;
+ param_vec.assign(parameters,parameters+n_blocks);
+ updateJetsRealPart(param_vec);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ jets_7_.data(),
+ jets_8_.data(),
+ jets_9_.data(),
+ residuals_jets_.data());
+
+ // fill the residual array
+ for (unsigned int i = 0; i < RES; i++)
+ residuals[i] = residuals_jets_[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ if (jacobians[i] != nullptr)
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ jacobians[i] + row * state_block_sizes_.at(i));
+ }
+ return true;
+ }
+
+ void updateJetsRealPart(const std::vector<double const*>& parameters) const
+ {
+ // update jets real part
+ for (unsigned int i = 0; i < B0; i++)
+ jets_0_[i].a = parameters[0][i];
+ for (unsigned int i = 0; i < B1; i++)
+ jets_1_[i].a = parameters[1][i];
+ for (unsigned int i = 0; i < B2; i++)
+ jets_2_[i].a = parameters[2][i];
+ for (unsigned int i = 0; i < B3; i++)
+ jets_3_[i].a = parameters[3][i];
+ for (unsigned int i = 0; i < B4; i++)
+ jets_4_[i].a = parameters[4][i];
+ for (unsigned int i = 0; i < B5; i++)
+ jets_5_[i].a = parameters[5][i];
+ for (unsigned int i = 0; i < B6; i++)
+ jets_6_[i].a = parameters[6][i];
+ for (unsigned int i = 0; i < B7; i++)
+ jets_7_[i].a = parameters[7][i];
+ for (unsigned int i = 0; i < B8; i++)
+ jets_8_[i].a = parameters[8][i];
+ for (unsigned int i = 0; i < B9; i++)
+ jets_9_[i].a = parameters[9][i];
+ }
+
+ virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const
+ {
+ assert(residual_.size() == RES);
+ jacobians_.clear();
+
+ assert(_states_ptr.size() == n_blocks);
+
+ // update jets real part
+ updateJetsRealPart(_states_ptr);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ jets_7_.data(),
+ jets_8_.data(),
+ jets_9_.data(),
+ residuals_jets_.data());
+
+ // fill the residual vector
+ for (unsigned int i = 0; i < RES; i++)
+ residual_(i) = residuals_jets_[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ {
+ // Create a row major Jacobian
+ Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
+ // Fill Jacobian rows from jets
+ if (!state_ptrs_[i]->isFixed())
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ Ji.data() + row * state_block_sizes_.at(i));
+ // add to the Jacobians vector
+ jacobians_.push_back(Ji);
+ }
+
+ // print jacobian matrices
+// for (unsigned int i = 0; i < n_blocks; i++)
+// std::cout << jacobians_[i] << std::endl << std::endl;
+ }
+
+ virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const
+ {
+ return state_ptrs_;
+ }
+
+ virtual std::vector<unsigned int> getStateSizes() const
+ {
+ return state_block_sizes_;
+ }
+
+ virtual unsigned int getSize() const
+ {
+ return RES;
+ }
+};
+
////////////////// SPECIALIZATION 9 BLOCKS ////////////////////////////////////////////////////////////////////////
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8>
-class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0> : public FactorBase
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0,0,0> : public FactorBase
{
public:
@@ -317,6 +852,8 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0> : public FactorBase
static const unsigned int block7Size = B7;
static const unsigned int block8Size = B8;
static const unsigned int block9Size = 0;
+ static const unsigned int block10Size = 0;
+ static const unsigned int block11Size = 0;
static const unsigned int n_blocks = 9;
static const std::vector<unsigned int> state_block_sizes_;
@@ -329,21 +866,21 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0> : public FactorBase
std::vector<StateBlockPtr> state_ptrs_;
static const std::vector<unsigned int> jacobian_locations_;
- std::array<WolfJet, RES>* residuals_jets_;
-
- std::array<WolfJet, B0>* jets_0_;
- std::array<WolfJet, B1>* jets_1_;
- std::array<WolfJet, B2>* jets_2_;
- std::array<WolfJet, B3>* jets_3_;
- std::array<WolfJet, B4>* jets_4_;
- std::array<WolfJet, B5>* jets_5_;
- std::array<WolfJet, B6>* jets_6_;
- std::array<WolfJet, B7>* jets_7_;
- std::array<WolfJet, B8>* jets_8_;
+ mutable std::array<WolfJet, RES> residuals_jets_;
+ mutable std::array<WolfJet, B0> jets_0_;
+ mutable std::array<WolfJet, B1> jets_1_;
+ mutable std::array<WolfJet, B2> jets_2_;
+ mutable std::array<WolfJet, B3> jets_3_;
+ mutable std::array<WolfJet, B4> jets_4_;
+ mutable std::array<WolfJet, B5> jets_5_;
+ mutable std::array<WolfJet, B6> jets_6_;
+ mutable std::array<WolfJet, B7> jets_7_;
+ mutable std::array<WolfJet, B8> jets_8_;
public:
FactorAutodiff(const std::string& _tp,
+ const FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
@@ -360,54 +897,45 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0> : public FactorBase
StateBlockPtr _state6Ptr,
StateBlockPtr _state7Ptr,
StateBlockPtr _state8Ptr) :
- FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
- state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr}),
- residuals_jets_(new std::array<WolfJet, RES>),
- jets_0_(new std::array<WolfJet, B0>),
- jets_1_(new std::array<WolfJet, B1>),
- jets_2_(new std::array<WolfJet, B2>),
- jets_3_(new std::array<WolfJet, B3>),
- jets_4_(new std::array<WolfJet, B4>),
- jets_5_(new std::array<WolfJet, B5>),
- jets_6_(new std::array<WolfJet, B6>),
- jets_7_(new std::array<WolfJet, B7>),
- jets_8_(new std::array<WolfJet, B8>)
- {
+ FactorBase(_tp, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr})
+ {
+ // asserts for null states
+ assert(_state0Ptr != nullptr && "nullptr state block");
+ assert(_state1Ptr != nullptr && "nullptr state block");
+ assert(_state2Ptr != nullptr && "nullptr state block");
+ assert(_state3Ptr != nullptr && "nullptr state block");
+ assert(_state4Ptr != nullptr && "nullptr state block");
+ assert(_state5Ptr != nullptr && "nullptr state block");
+ assert(_state6Ptr != nullptr && "nullptr state block");
+ assert(_state7Ptr != nullptr && "nullptr state block");
+ assert(_state8Ptr != nullptr && "nullptr state block");
+
// initialize jets
unsigned int last_jet_idx = 0;
for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
+ jets_1_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i] = WolfJet(0, last_jet_idx++);
+ jets_2_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B3; i++)
- (*jets_3_)[i] = WolfJet(0, last_jet_idx++);
+ jets_3_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B4; i++)
- (*jets_4_)[i] = WolfJet(0, last_jet_idx++);
+ jets_4_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B5; i++)
- (*jets_5_)[i] = WolfJet(0, last_jet_idx++);
+ jets_5_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B6; i++)
- (*jets_6_)[i] = WolfJet(0, last_jet_idx++);
+ jets_6_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B7; i++)
- (*jets_7_)[i] = WolfJet(0, last_jet_idx++);
+ jets_7_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B8; i++)
- (*jets_8_)[i] = WolfJet(0, last_jet_idx++);
+ jets_8_[i] = WolfJet(0, last_jet_idx++);
state_ptrs_.resize(n_blocks);
}
virtual ~FactorAutodiff()
{
- delete jets_0_;
- delete jets_1_;
- delete jets_2_;
- delete jets_3_;
- delete jets_4_;
- delete jets_5_;
- delete jets_6_;
- delete jets_7_;
- delete jets_8_;
- delete residuals_jets_;
}
virtual JacobianMethod getJacobianMethod() const
@@ -440,27 +968,27 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0> : public FactorBase
updateJetsRealPart(param_vec);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_->data(),
- jets_1_->data(),
- jets_2_->data(),
- jets_3_->data(),
- jets_4_->data(),
- jets_5_->data(),
- jets_6_->data(),
- jets_7_->data(),
- jets_8_->data(),
- residuals_jets_->data());
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ jets_7_.data(),
+ jets_8_.data(),
+ residuals_jets_.data());
// fill the residual array
for (unsigned int i = 0; i < RES; i++)
- residuals[i] = (*residuals_jets_)[i].a;
+ residuals[i] = residuals_jets_[i].a;
// fill the jacobian matrices
for (unsigned int i = 0; i<n_blocks; i++)
if (jacobians[i] != nullptr)
for (unsigned int row = 0; row < RES; row++)
- std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
- (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
jacobians[i] + row * state_block_sizes_.at(i));
}
return true;
@@ -470,23 +998,23 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0> : public FactorBase
{
// update jets real part
for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i].a = parameters[0][i];
+ jets_0_[i].a = parameters[0][i];
for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i].a = parameters[1][i];
+ jets_1_[i].a = parameters[1][i];
for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i].a = parameters[2][i];
+ jets_2_[i].a = parameters[2][i];
for (unsigned int i = 0; i < B3; i++)
- (*jets_3_)[i].a = parameters[3][i];
+ jets_3_[i].a = parameters[3][i];
for (unsigned int i = 0; i < B4; i++)
- (*jets_4_)[i].a = parameters[4][i];
+ jets_4_[i].a = parameters[4][i];
for (unsigned int i = 0; i < B5; i++)
- (*jets_5_)[i].a = parameters[5][i];
+ jets_5_[i].a = parameters[5][i];
for (unsigned int i = 0; i < B6; i++)
- (*jets_6_)[i].a = parameters[6][i];
+ jets_6_[i].a = parameters[6][i];
for (unsigned int i = 0; i < B7; i++)
- (*jets_7_)[i].a = parameters[7][i];
+ jets_7_[i].a = parameters[7][i];
for (unsigned int i = 0; i < B8; i++)
- (*jets_8_)[i].a = parameters[8][i];
+ jets_8_[i].a = parameters[8][i];
}
virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const
@@ -500,20 +1028,20 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0> : public FactorBase
updateJetsRealPart(_states_ptr);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_->data(),
- jets_1_->data(),
- jets_2_->data(),
- jets_3_->data(),
- jets_4_->data(),
- jets_5_->data(),
- jets_6_->data(),
- jets_7_->data(),
- jets_8_->data(),
- residuals_jets_->data());
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ jets_7_.data(),
+ jets_8_.data(),
+ residuals_jets_.data());
// fill the residual vector
for (unsigned int i = 0; i < RES; i++)
- residual_(i) = (*residuals_jets_)[i].a;
+ residual_(i) = residuals_jets_[i].a;
// fill the jacobian matrices
for (unsigned int i = 0; i<n_blocks; i++)
@@ -523,8 +1051,8 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0> : public FactorBase
// Fill Jacobian rows from jets
if (!state_ptrs_[i]->isFixed())
for (unsigned int row = 0; row < RES; row++)
- std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
- (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
Ji.data() + row * state_block_sizes_.at(i));
// add to the Jacobians vector
jacobians_.push_back(Ji);
@@ -554,7 +1082,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0> : public FactorBase
////////////////// SPECIALIZATION 8 BLOCKS ////////////////////////////////////////////////////////////////////////
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7>
-class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0> : public FactorBase
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0,0,0> : public FactorBase
{
public:
@@ -569,6 +1097,8 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0> : public FactorBase
static const unsigned int block7Size = B7;
static const unsigned int block8Size = 0;
static const unsigned int block9Size = 0;
+ static const unsigned int block10Size = 0;
+ static const unsigned int block11Size = 0;
static const unsigned int n_blocks = 8;
static const std::vector<unsigned int> state_block_sizes_;
@@ -581,20 +1111,20 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0> : public FactorBase
std::vector<StateBlockPtr> state_ptrs_;
static const std::vector<unsigned int> jacobian_locations_;
- std::array<WolfJet, RES>* residuals_jets_;
-
- std::array<WolfJet, B0>* jets_0_;
- std::array<WolfJet, B1>* jets_1_;
- std::array<WolfJet, B2>* jets_2_;
- std::array<WolfJet, B3>* jets_3_;
- std::array<WolfJet, B4>* jets_4_;
- std::array<WolfJet, B5>* jets_5_;
- std::array<WolfJet, B6>* jets_6_;
- std::array<WolfJet, B7>* jets_7_;
+ mutable std::array<WolfJet, RES> residuals_jets_;
+ mutable std::array<WolfJet, B0> jets_0_;
+ mutable std::array<WolfJet, B1> jets_1_;
+ mutable std::array<WolfJet, B2> jets_2_;
+ mutable std::array<WolfJet, B3> jets_3_;
+ mutable std::array<WolfJet, B4> jets_4_;
+ mutable std::array<WolfJet, B5> jets_5_;
+ mutable std::array<WolfJet, B6> jets_6_;
+ mutable std::array<WolfJet, B7> jets_7_;
public:
FactorAutodiff(const std::string& _tp,
+ const FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
@@ -610,50 +1140,42 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0> : public FactorBase
StateBlockPtr _state5Ptr,
StateBlockPtr _state6Ptr,
StateBlockPtr _state7Ptr) :
- FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
- state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr}),
- residuals_jets_(new std::array<WolfJet, RES>),
- jets_0_(new std::array<WolfJet, B0>),
- jets_1_(new std::array<WolfJet, B1>),
- jets_2_(new std::array<WolfJet, B2>),
- jets_3_(new std::array<WolfJet, B3>),
- jets_4_(new std::array<WolfJet, B4>),
- jets_5_(new std::array<WolfJet, B5>),
- jets_6_(new std::array<WolfJet, B6>),
- jets_7_(new std::array<WolfJet, B7>)
- {
+ FactorBase(_tp, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr})
+ {
+ // asserts for null states
+ assert(_state0Ptr != nullptr && "nullptr state block");
+ assert(_state1Ptr != nullptr && "nullptr state block");
+ assert(_state2Ptr != nullptr && "nullptr state block");
+ assert(_state3Ptr != nullptr && "nullptr state block");
+ assert(_state4Ptr != nullptr && "nullptr state block");
+ assert(_state5Ptr != nullptr && "nullptr state block");
+ assert(_state6Ptr != nullptr && "nullptr state block");
+ assert(_state7Ptr != nullptr && "nullptr state block");
+
// initialize jets
unsigned int last_jet_idx = 0;
for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
+ jets_1_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i] = WolfJet(0, last_jet_idx++);
+ jets_2_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B3; i++)
- (*jets_3_)[i] = WolfJet(0, last_jet_idx++);
+ jets_3_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B4; i++)
- (*jets_4_)[i] = WolfJet(0, last_jet_idx++);
+ jets_4_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B5; i++)
- (*jets_5_)[i] = WolfJet(0, last_jet_idx++);
+ jets_5_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B6; i++)
- (*jets_6_)[i] = WolfJet(0, last_jet_idx++);
+ jets_6_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B7; i++)
- (*jets_7_)[i] = WolfJet(0, last_jet_idx++);
+ jets_7_[i] = WolfJet(0, last_jet_idx++);
state_ptrs_.resize(n_blocks);
}
virtual ~FactorAutodiff()
{
- delete jets_0_;
- delete jets_1_;
- delete jets_2_;
- delete jets_3_;
- delete jets_4_;
- delete jets_5_;
- delete jets_6_;
- delete jets_7_;
- delete residuals_jets_;
}
virtual JacobianMethod getJacobianMethod() const
@@ -685,26 +1207,26 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0> : public FactorBase
updateJetsRealPart(param_vec);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_->data(),
- jets_1_->data(),
- jets_2_->data(),
- jets_3_->data(),
- jets_4_->data(),
- jets_5_->data(),
- jets_6_->data(),
- jets_7_->data(),
- residuals_jets_->data());
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ jets_7_.data(),
+ residuals_jets_.data());
// fill the residual array
for (unsigned int i = 0; i < RES; i++)
- residuals[i] = (*residuals_jets_)[i].a;
+ residuals[i] = residuals_jets_[i].a;
// fill the jacobian matrices
for (unsigned int i = 0; i<n_blocks; i++)
if (jacobians[i] != nullptr)
for (unsigned int row = 0; row < RES; row++)
- std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
- (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
jacobians[i] + row * state_block_sizes_.at(i));
}
return true;
@@ -714,21 +1236,21 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0> : public FactorBase
{
// update jets real part
for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i].a = parameters[0][i];
+ jets_0_[i].a = parameters[0][i];
for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i].a = parameters[1][i];
+ jets_1_[i].a = parameters[1][i];
for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i].a = parameters[2][i];
+ jets_2_[i].a = parameters[2][i];
for (unsigned int i = 0; i < B3; i++)
- (*jets_3_)[i].a = parameters[3][i];
+ jets_3_[i].a = parameters[3][i];
for (unsigned int i = 0; i < B4; i++)
- (*jets_4_)[i].a = parameters[4][i];
+ jets_4_[i].a = parameters[4][i];
for (unsigned int i = 0; i < B5; i++)
- (*jets_5_)[i].a = parameters[5][i];
+ jets_5_[i].a = parameters[5][i];
for (unsigned int i = 0; i < B6; i++)
- (*jets_6_)[i].a = parameters[6][i];
+ jets_6_[i].a = parameters[6][i];
for (unsigned int i = 0; i < B7; i++)
- (*jets_7_)[i].a = parameters[7][i];
+ jets_7_[i].a = parameters[7][i];
}
virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const
@@ -742,19 +1264,19 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0> : public FactorBase
updateJetsRealPart(_states_ptr);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_->data(),
- jets_1_->data(),
- jets_2_->data(),
- jets_3_->data(),
- jets_4_->data(),
- jets_5_->data(),
- jets_6_->data(),
- jets_7_->data(),
- residuals_jets_->data());
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ jets_7_.data(),
+ residuals_jets_.data());
// fill the residual vector
for (unsigned int i = 0; i < RES; i++)
- residual_(i) = (*residuals_jets_)[i].a;
+ residual_(i) = residuals_jets_[i].a;
// fill the jacobian matrices
for (unsigned int i = 0; i<n_blocks; i++)
@@ -764,8 +1286,8 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0> : public FactorBase
// Fill Jacobian rows from jets
if (!state_ptrs_[i]->isFixed())
for (unsigned int row = 0; row < RES; row++)
- std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
- (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
Ji.data() + row * state_block_sizes_.at(i));
// add to the Jacobians vector
jacobians_.push_back(Ji);
@@ -795,7 +1317,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0> : public FactorBase
////////////////// SPECIALIZATION 7 BLOCKS ////////////////////////////////////////////////////////////////////////
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6>
-class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0> : public FactorBase
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0,0,0> : public FactorBase
{
public:
@@ -810,6 +1332,8 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0> : public FactorBase
static const unsigned int block7Size = 0;
static const unsigned int block8Size = 0;
static const unsigned int block9Size = 0;
+ static const unsigned int block10Size = 0;
+ static const unsigned int block11Size = 0;
static const unsigned int n_blocks = 7;
static const std::vector<unsigned int> state_block_sizes_;
@@ -822,19 +1346,19 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0> : public FactorBase
std::vector<StateBlockPtr> state_ptrs_;
static const std::vector<unsigned int> jacobian_locations_;
- std::array<WolfJet, RES>* residuals_jets_;
-
- std::array<WolfJet, B0>* jets_0_;
- std::array<WolfJet, B1>* jets_1_;
- std::array<WolfJet, B2>* jets_2_;
- std::array<WolfJet, B3>* jets_3_;
- std::array<WolfJet, B4>* jets_4_;
- std::array<WolfJet, B5>* jets_5_;
- std::array<WolfJet, B6>* jets_6_;
+ mutable std::array<WolfJet, RES> residuals_jets_;
+ mutable std::array<WolfJet, B0> jets_0_;
+ mutable std::array<WolfJet, B1> jets_1_;
+ mutable std::array<WolfJet, B2> jets_2_;
+ mutable std::array<WolfJet, B3> jets_3_;
+ mutable std::array<WolfJet, B4> jets_4_;
+ mutable std::array<WolfJet, B5> jets_5_;
+ mutable std::array<WolfJet, B6> jets_6_;
public:
FactorAutodiff(const std::string& _tp,
+ const FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
@@ -849,46 +1373,39 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0> : public FactorBase
StateBlockPtr _state4Ptr,
StateBlockPtr _state5Ptr,
StateBlockPtr _state6Ptr) :
- FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
- state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr}),
- residuals_jets_(new std::array<WolfJet, RES>),
- jets_0_(new std::array<WolfJet, B0>),
- jets_1_(new std::array<WolfJet, B1>),
- jets_2_(new std::array<WolfJet, B2>),
- jets_3_(new std::array<WolfJet, B3>),
- jets_4_(new std::array<WolfJet, B4>),
- jets_5_(new std::array<WolfJet, B5>),
- jets_6_(new std::array<WolfJet, B6>)
- {
+ FactorBase(_tp, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr})
+ {
+ // asserts for null states
+ assert(_state0Ptr != nullptr && "nullptr state block");
+ assert(_state1Ptr != nullptr && "nullptr state block");
+ assert(_state2Ptr != nullptr && "nullptr state block");
+ assert(_state3Ptr != nullptr && "nullptr state block");
+ assert(_state4Ptr != nullptr && "nullptr state block");
+ assert(_state5Ptr != nullptr && "nullptr state block");
+ assert(_state6Ptr != nullptr && "nullptr state block");
+
// initialize jets
unsigned int last_jet_idx = 0;
for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
+ jets_1_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i] = WolfJet(0, last_jet_idx++);
+ jets_2_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B3; i++)
- (*jets_3_)[i] = WolfJet(0, last_jet_idx++);
+ jets_3_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B4; i++)
- (*jets_4_)[i] = WolfJet(0, last_jet_idx++);
+ jets_4_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B5; i++)
- (*jets_5_)[i] = WolfJet(0, last_jet_idx++);
+ jets_5_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B6; i++)
- (*jets_6_)[i] = WolfJet(0, last_jet_idx++);
+ jets_6_[i] = WolfJet(0, last_jet_idx++);
state_ptrs_.resize(n_blocks);
}
virtual ~FactorAutodiff()
{
- delete jets_0_;
- delete jets_1_;
- delete jets_2_;
- delete jets_3_;
- delete jets_4_;
- delete jets_5_;
- delete jets_6_;
- delete residuals_jets_;
}
virtual JacobianMethod getJacobianMethod() const
@@ -919,25 +1436,25 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0> : public FactorBase
updateJetsRealPart(param_vec);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_->data(),
- jets_1_->data(),
- jets_2_->data(),
- jets_3_->data(),
- jets_4_->data(),
- jets_5_->data(),
- jets_6_->data(),
- residuals_jets_->data());
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ residuals_jets_.data());
// fill the residual array
for (unsigned int i = 0; i < RES; i++)
- residuals[i] = (*residuals_jets_)[i].a;
+ residuals[i] = residuals_jets_[i].a;
// fill the jacobian matrices
for (unsigned int i = 0; i<n_blocks; i++)
if (jacobians[i] != nullptr)
for (unsigned int row = 0; row < RES; row++)
- std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
- (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
jacobians[i] + row * state_block_sizes_.at(i));
}
return true;
@@ -947,19 +1464,19 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0> : public FactorBase
{
// update jets real part
for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i].a = parameters[0][i];
+ jets_0_[i].a = parameters[0][i];
for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i].a = parameters[1][i];
+ jets_1_[i].a = parameters[1][i];
for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i].a = parameters[2][i];
+ jets_2_[i].a = parameters[2][i];
for (unsigned int i = 0; i < B3; i++)
- (*jets_3_)[i].a = parameters[3][i];
+ jets_3_[i].a = parameters[3][i];
for (unsigned int i = 0; i < B4; i++)
- (*jets_4_)[i].a = parameters[4][i];
+ jets_4_[i].a = parameters[4][i];
for (unsigned int i = 0; i < B5; i++)
- (*jets_5_)[i].a = parameters[5][i];
+ jets_5_[i].a = parameters[5][i];
for (unsigned int i = 0; i < B6; i++)
- (*jets_6_)[i].a = parameters[6][i];
+ jets_6_[i].a = parameters[6][i];
}
virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const
@@ -973,18 +1490,18 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0> : public FactorBase
updateJetsRealPart(_states_ptr);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_->data(),
- jets_1_->data(),
- jets_2_->data(),
- jets_3_->data(),
- jets_4_->data(),
- jets_5_->data(),
- jets_6_->data(),
- residuals_jets_->data());
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ residuals_jets_.data());
// fill the residual vector
for (unsigned int i = 0; i < RES; i++)
- residual_(i) = (*residuals_jets_)[i].a;
+ residual_(i) = residuals_jets_[i].a;
// fill the jacobian matrices
for (unsigned int i = 0; i<n_blocks; i++)
@@ -994,8 +1511,8 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0> : public FactorBase
// Fill Jacobian rows from jets
if (!state_ptrs_[i]->isFixed())
for (unsigned int row = 0; row < RES; row++)
- std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
- (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
Ji.data() + row * state_block_sizes_.at(i));
// add to the Jacobians vector
jacobians_.push_back(Ji);
@@ -1025,7 +1542,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0> : public FactorBase
////////////////// SPECIALIZATION 6 BLOCKS ////////////////////////////////////////////////////////////////////////
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5>
-class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0> : public FactorBase
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0,0,0> : public FactorBase
{
public:
@@ -1040,6 +1557,8 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0> : public FactorBase
static const unsigned int block7Size = 0;
static const unsigned int block8Size = 0;
static const unsigned int block9Size = 0;
+ static const unsigned int block10Size = 0;
+ static const unsigned int block11Size = 0;
static const unsigned int n_blocks = 6;
static const std::vector<unsigned int> state_block_sizes_;
@@ -1052,18 +1571,18 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0> : public FactorBase
std::vector<StateBlockPtr> state_ptrs_;
static const std::vector<unsigned int> jacobian_locations_;
- std::array<WolfJet, RES>* residuals_jets_;
-
- std::array<WolfJet, B0>* jets_0_;
- std::array<WolfJet, B1>* jets_1_;
- std::array<WolfJet, B2>* jets_2_;
- std::array<WolfJet, B3>* jets_3_;
- std::array<WolfJet, B4>* jets_4_;
- std::array<WolfJet, B5>* jets_5_;
+ mutable std::array<WolfJet, RES> residuals_jets_;
+ mutable std::array<WolfJet, B0> jets_0_;
+ mutable std::array<WolfJet, B1> jets_1_;
+ mutable std::array<WolfJet, B2> jets_2_;
+ mutable std::array<WolfJet, B3> jets_3_;
+ mutable std::array<WolfJet, B4> jets_4_;
+ mutable std::array<WolfJet, B5> jets_5_;
public:
FactorAutodiff(const std::string& _tp,
+ const FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
@@ -1077,42 +1596,36 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0> : public FactorBase
StateBlockPtr _state3Ptr,
StateBlockPtr _state4Ptr,
StateBlockPtr _state5Ptr) :
- FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
- state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr}),
- residuals_jets_(new std::array<WolfJet, RES>),
- jets_0_(new std::array<WolfJet, B0>),
- jets_1_(new std::array<WolfJet, B1>),
- jets_2_(new std::array<WolfJet, B2>),
- jets_3_(new std::array<WolfJet, B3>),
- jets_4_(new std::array<WolfJet, B4>),
- jets_5_(new std::array<WolfJet, B5>)
+ FactorBase(_tp, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr})
{
+ // asserts for null states
+ assert(_state0Ptr != nullptr && "nullptr state block");
+ assert(_state1Ptr != nullptr && "nullptr state block");
+ assert(_state2Ptr != nullptr && "nullptr state block");
+ assert(_state3Ptr != nullptr && "nullptr state block");
+ assert(_state4Ptr != nullptr && "nullptr state block");
+ assert(_state5Ptr != nullptr && "nullptr state block");
+
// initialize jets
unsigned int last_jet_idx = 0;
for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
+ jets_1_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i] = WolfJet(0, last_jet_idx++);
+ jets_2_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B3; i++)
- (*jets_3_)[i] = WolfJet(0, last_jet_idx++);
+ jets_3_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B4; i++)
- (*jets_4_)[i] = WolfJet(0, last_jet_idx++);
+ jets_4_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B5; i++)
- (*jets_5_)[i] = WolfJet(0, last_jet_idx++);
+ jets_5_[i] = WolfJet(0, last_jet_idx++);
state_ptrs_.resize(n_blocks);
}
virtual ~FactorAutodiff()
{
- delete jets_0_;
- delete jets_1_;
- delete jets_2_;
- delete jets_3_;
- delete jets_4_;
- delete jets_5_;
- delete residuals_jets_;
}
virtual JacobianMethod getJacobianMethod() const
@@ -1142,24 +1655,24 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0> : public FactorBase
updateJetsRealPart(param_vec);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_->data(),
- jets_1_->data(),
- jets_2_->data(),
- jets_3_->data(),
- jets_4_->data(),
- jets_5_->data(),
- residuals_jets_->data());
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ residuals_jets_.data());
// fill the residual array
for (unsigned int i = 0; i < RES; i++)
- residuals[i] = (*residuals_jets_)[i].a;
+ residuals[i] = residuals_jets_[i].a;
// fill the jacobian matrices
for (unsigned int i = 0; i<n_blocks; i++)
if (jacobians[i] != nullptr)
for (unsigned int row = 0; row < RES; row++)
- std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
- (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
jacobians[i] + row * state_block_sizes_.at(i));
}
return true;
@@ -1169,17 +1682,17 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0> : public FactorBase
{
// update jets real part
for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i].a = parameters[0][i];
+ jets_0_[i].a = parameters[0][i];
for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i].a = parameters[1][i];
+ jets_1_[i].a = parameters[1][i];
for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i].a = parameters[2][i];
+ jets_2_[i].a = parameters[2][i];
for (unsigned int i = 0; i < B3; i++)
- (*jets_3_)[i].a = parameters[3][i];
+ jets_3_[i].a = parameters[3][i];
for (unsigned int i = 0; i < B4; i++)
- (*jets_4_)[i].a = parameters[4][i];
+ jets_4_[i].a = parameters[4][i];
for (unsigned int i = 0; i < B5; i++)
- (*jets_5_)[i].a = parameters[5][i];
+ jets_5_[i].a = parameters[5][i];
}
virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const
@@ -1193,17 +1706,17 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0> : public FactorBase
updateJetsRealPart(_states_ptr);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_->data(),
- jets_1_->data(),
- jets_2_->data(),
- jets_3_->data(),
- jets_4_->data(),
- jets_5_->data(),
- residuals_jets_->data());
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ residuals_jets_.data());
// fill the residual vector
for (unsigned int i = 0; i < RES; i++)
- residual_(i) = (*residuals_jets_)[i].a;
+ residual_(i) = residuals_jets_[i].a;
// fill the jacobian matrices
for (unsigned int i = 0; i<n_blocks; i++)
@@ -1213,8 +1726,8 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0> : public FactorBase
// Fill Jacobian rows from jets
if (!state_ptrs_[i]->isFixed())
for (unsigned int row = 0; row < RES; row++)
- std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
- (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
Ji.data() + row * state_block_sizes_.at(i));
// add to the Jacobians vector
jacobians_.push_back(Ji);
@@ -1240,7 +1753,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0> : public FactorBase
////////////////// SPECIALIZATION 5 BLOCKS ////////////////////////////////////////////////////////////////////////
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4>
-class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0> : public FactorBase
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0,0,0> : public FactorBase
{
public:
@@ -1255,6 +1768,8 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0> : public FactorBase
static const unsigned int block7Size = 0;
static const unsigned int block8Size = 0;
static const unsigned int block9Size = 0;
+ static const unsigned int block10Size = 0;
+ static const unsigned int block11Size = 0;
static const unsigned int n_blocks = 5;
static const std::vector<unsigned int> state_block_sizes_;
@@ -1266,17 +1781,17 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0> : public FactorBase
std::vector<StateBlockPtr> state_ptrs_;
static const std::vector<unsigned int> jacobian_locations_;
- std::array<WolfJet, RES>* residuals_jets_;
-
- std::array<WolfJet, B0>* jets_0_;
- std::array<WolfJet, B1>* jets_1_;
- std::array<WolfJet, B2>* jets_2_;
- std::array<WolfJet, B3>* jets_3_;
- std::array<WolfJet, B4>* jets_4_;
+ mutable std::array<WolfJet, RES> residuals_jets_;
+ mutable std::array<WolfJet, B0> jets_0_;
+ mutable std::array<WolfJet, B1> jets_1_;
+ mutable std::array<WolfJet, B2> jets_2_;
+ mutable std::array<WolfJet, B3> jets_3_;
+ mutable std::array<WolfJet, B4> jets_4_;
public:
FactorAutodiff(const std::string& _tp,
+ const FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
@@ -1289,38 +1804,33 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0> : public FactorBase
StateBlockPtr _state2Ptr,
StateBlockPtr _state3Ptr,
StateBlockPtr _state4Ptr) :
- FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
- state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr}),
- residuals_jets_(new std::array<WolfJet, RES>),
- jets_0_(new std::array<WolfJet, B0>),
- jets_1_(new std::array<WolfJet, B1>),
- jets_2_(new std::array<WolfJet, B2>),
- jets_3_(new std::array<WolfJet, B3>),
- jets_4_(new std::array<WolfJet, B4>)
+ FactorBase(_tp, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr})
{
+ // asserts for null states
+ assert(_state0Ptr != nullptr && "nullptr state block");
+ assert(_state1Ptr != nullptr && "nullptr state block");
+ assert(_state2Ptr != nullptr && "nullptr state block");
+ assert(_state3Ptr != nullptr && "nullptr state block");
+ assert(_state4Ptr != nullptr && "nullptr state block");
+
// initialize jets
unsigned int last_jet_idx = 0;
for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
+ jets_1_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i] = WolfJet(0, last_jet_idx++);
+ jets_2_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B3; i++)
- (*jets_3_)[i] = WolfJet(0, last_jet_idx++);
+ jets_3_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B4; i++)
- (*jets_4_)[i] = WolfJet(0, last_jet_idx++);
+ jets_4_[i] = WolfJet(0, last_jet_idx++);
state_ptrs_.resize(n_blocks);
}
virtual ~FactorAutodiff()
{
- delete jets_0_;
- delete jets_1_;
- delete jets_2_;
- delete jets_3_;
- delete jets_4_;
- delete residuals_jets_;
}
virtual JacobianMethod getJacobianMethod() const
@@ -1349,23 +1859,23 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0> : public FactorBase
updateJetsRealPart(param_vec);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_->data(),
- jets_1_->data(),
- jets_2_->data(),
- jets_3_->data(),
- jets_4_->data(),
- residuals_jets_->data());
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ residuals_jets_.data());
// fill the residual array
for (unsigned int i = 0; i < RES; i++)
- residuals[i] = (*residuals_jets_)[i].a;
+ residuals[i] = residuals_jets_[i].a;
// fill the jacobian matrices
for (unsigned int i = 0; i<n_blocks; i++)
if (jacobians[i] != nullptr)
for (unsigned int row = 0; row < RES; row++)
- std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
- (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
jacobians[i] + row * state_block_sizes_.at(i));
}
return true;
@@ -1375,15 +1885,15 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0> : public FactorBase
{
// update jets real part
for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i].a = parameters[0][i];
+ jets_0_[i].a = parameters[0][i];
for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i].a = parameters[1][i];
+ jets_1_[i].a = parameters[1][i];
for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i].a = parameters[2][i];
+ jets_2_[i].a = parameters[2][i];
for (unsigned int i = 0; i < B3; i++)
- (*jets_3_)[i].a = parameters[3][i];
+ jets_3_[i].a = parameters[3][i];
for (unsigned int i = 0; i < B4; i++)
- (*jets_4_)[i].a = parameters[4][i];
+ jets_4_[i].a = parameters[4][i];
}
virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const
@@ -1397,16 +1907,16 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0> : public FactorBase
updateJetsRealPart(_states_ptr);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_->data(),
- jets_1_->data(),
- jets_2_->data(),
- jets_3_->data(),
- jets_4_->data(),
- residuals_jets_->data());
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ residuals_jets_.data());
// fill the residual vector
for (unsigned int i = 0; i < RES; i++)
- residual_(i) = (*residuals_jets_)[i].a;
+ residual_(i) = residuals_jets_[i].a;
// fill the jacobian matrices
for (unsigned int i = 0; i<n_blocks; i++)
@@ -1416,8 +1926,8 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0> : public FactorBase
// Fill Jacobian rows from jets
if (!state_ptrs_[i]->isFixed())
for (unsigned int row = 0; row < RES; row++)
- std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
- (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
Ji.data() + row * state_block_sizes_.at(i));
// add to the Jacobians vector
jacobians_.push_back(Ji);
@@ -1443,7 +1953,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0> : public FactorBase
////////////////// SPECIALIZATION 4 BLOCKS ////////////////////////////////////////////////////////////////////////
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3>
-class FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0> : public FactorBase
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0,0,0> : public FactorBase
{
public:
@@ -1458,6 +1968,8 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0> : public FactorBase
static const unsigned int block7Size = 0;
static const unsigned int block8Size = 0;
static const unsigned int block9Size = 0;
+ static const unsigned int block10Size = 0;
+ static const unsigned int block11Size = 0;
static const unsigned int n_blocks = 4;
static const std::vector<unsigned int> state_block_sizes_;
@@ -1469,16 +1981,16 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0> : public FactorBase
std::vector<StateBlockPtr> state_ptrs_;
static const std::vector<unsigned int> jacobian_locations_;
- std::array<WolfJet, RES>* residuals_jets_;
-
- std::array<WolfJet, B0>* jets_0_;
- std::array<WolfJet, B1>* jets_1_;
- std::array<WolfJet, B2>* jets_2_;
- std::array<WolfJet, B3>* jets_3_;
+ mutable std::array<WolfJet, RES> residuals_jets_;
+ mutable std::array<WolfJet, B0> jets_0_;
+ mutable std::array<WolfJet, B1> jets_1_;
+ mutable std::array<WolfJet, B2> jets_2_;
+ mutable std::array<WolfJet, B3> jets_3_;
public:
FactorAutodiff(const std::string& _tp,
+ const FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
@@ -1490,34 +2002,30 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0> : public FactorBase
StateBlockPtr _state1Ptr,
StateBlockPtr _state2Ptr,
StateBlockPtr _state3Ptr) :
- FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
- state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr}),
- residuals_jets_(new std::array<WolfJet, RES>),
- jets_0_(new std::array<WolfJet, B0>),
- jets_1_(new std::array<WolfJet, B1>),
- jets_2_(new std::array<WolfJet, B2>),
- jets_3_(new std::array<WolfJet, B3>)
+ FactorBase(_tp, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr})
{
+ // asserts for null states
+ assert(_state0Ptr != nullptr && "nullptr state block");
+ assert(_state1Ptr != nullptr && "nullptr state block");
+ assert(_state2Ptr != nullptr && "nullptr state block");
+ assert(_state3Ptr != nullptr && "nullptr state block");
+
// initialize jets
unsigned int last_jet_idx = 0;
for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
+ jets_1_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i] = WolfJet(0, last_jet_idx++);
+ jets_2_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B3; i++)
- (*jets_3_)[i] = WolfJet(0, last_jet_idx++);
+ jets_3_[i] = WolfJet(0, last_jet_idx++);
state_ptrs_.resize(n_blocks);
}
virtual ~FactorAutodiff()
{
- delete jets_0_;
- delete jets_1_;
- delete jets_2_;
- delete jets_3_;
- delete residuals_jets_;
}
virtual JacobianMethod getJacobianMethod() const
@@ -1545,22 +2053,22 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0> : public FactorBase
updateJetsRealPart(param_vec);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_->data(),
- jets_1_->data(),
- jets_2_->data(),
- jets_3_->data(),
- residuals_jets_->data());
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ residuals_jets_.data());
// fill the residual array
for (unsigned int i = 0; i < RES; i++)
- residuals[i] = (*residuals_jets_)[i].a;
+ residuals[i] = residuals_jets_[i].a;
// fill the jacobian matrices
for (unsigned int i = 0; i<n_blocks; i++)
if (jacobians[i] != nullptr)
for (unsigned int row = 0; row < RES; row++)
- std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
- (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
jacobians[i] + row * state_block_sizes_.at(i));
}
return true;
@@ -1570,13 +2078,13 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0> : public FactorBase
{
// update jets real part
for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i].a = parameters[0][i];
+ jets_0_[i].a = parameters[0][i];
for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i].a = parameters[1][i];
+ jets_1_[i].a = parameters[1][i];
for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i].a = parameters[2][i];
+ jets_2_[i].a = parameters[2][i];
for (unsigned int i = 0; i < B3; i++)
- (*jets_3_)[i].a = parameters[3][i];
+ jets_3_[i].a = parameters[3][i];
}
virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const
@@ -1590,15 +2098,15 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0> : public FactorBase
updateJetsRealPart(_states_ptr);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_->data(),
- jets_1_->data(),
- jets_2_->data(),
- jets_3_->data(),
- residuals_jets_->data());
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ residuals_jets_.data());
// fill the residual vector
for (unsigned int i = 0; i < RES; i++)
- residual_(i) = (*residuals_jets_)[i].a;
+ residual_(i) = residuals_jets_[i].a;
// fill the jacobian matrices
for (unsigned int i = 0; i<n_blocks; i++)
@@ -1608,8 +2116,8 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0> : public FactorBase
// Fill Jacobian rows from jets
if (!state_ptrs_[i]->isFixed())
for (unsigned int row = 0; row < RES; row++)
- std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
- (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
Ji.data() + row * state_block_sizes_.at(i));
// add to the Jacobians vector
jacobians_.push_back(Ji);
@@ -1639,7 +2147,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0> : public FactorBase
////////////////// SPECIALIZATION 3 BLOCKS ////////////////////////////////////////////////////////////////////////
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2>
-class FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0> : public FactorBase
+class FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0,0,0> : public FactorBase
{
public:
@@ -1654,6 +2162,8 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0> : public FactorBase
static const unsigned int block7Size = 0;
static const unsigned int block8Size = 0;
static const unsigned int block9Size = 0;
+ static const unsigned int block10Size = 0;
+ static const unsigned int block11Size = 0;
static const unsigned int n_blocks = 3;
static const std::vector<unsigned int> state_block_sizes_;
@@ -1665,15 +2175,15 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0> : public FactorBase
std::vector<StateBlockPtr> state_ptrs_;
static const std::vector<unsigned int> jacobian_locations_;
- std::array<WolfJet, RES>* residuals_jets_;
-
- std::array<WolfJet, B0>* jets_0_;
- std::array<WolfJet, B1>* jets_1_;
- std::array<WolfJet, B2>* jets_2_;
+ mutable std::array<WolfJet, RES> residuals_jets_;
+ mutable std::array<WolfJet, B0> jets_0_;
+ mutable std::array<WolfJet, B1> jets_1_;
+ mutable std::array<WolfJet, B2> jets_2_;
public:
FactorAutodiff(const std::string& _tp,
+ const FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
@@ -1684,30 +2194,27 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0> : public FactorBase
StateBlockPtr _state0Ptr,
StateBlockPtr _state1Ptr,
StateBlockPtr _state2Ptr) :
- FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
- state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr}),
- residuals_jets_(new std::array<WolfJet, RES>),
- jets_0_(new std::array<WolfJet, B0>),
- jets_1_(new std::array<WolfJet, B1>),
- jets_2_(new std::array<WolfJet, B2>)
+ FactorBase(_tp, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr})
{
+ // asserts for null states
+ assert(_state0Ptr != nullptr && "nullptr state block");
+ assert(_state1Ptr != nullptr && "nullptr state block");
+ assert(_state2Ptr != nullptr && "nullptr state block");
+
// initialize jets
unsigned int last_jet_idx = 0;
for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
+ jets_1_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i] = WolfJet(0, last_jet_idx++);
+ jets_2_[i] = WolfJet(0, last_jet_idx++);
state_ptrs_.resize(n_blocks);
}
virtual ~FactorAutodiff()
{
- delete jets_0_;
- delete jets_1_;
- delete jets_2_;
- delete residuals_jets_;
}
virtual JacobianMethod getJacobianMethod() const
@@ -1734,21 +2241,21 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0> : public FactorBase
updateJetsRealPart(param_vec);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_->data(),
- jets_1_->data(),
- jets_2_->data(),
- residuals_jets_->data());
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ residuals_jets_.data());
// fill the residual array
for (unsigned int i = 0; i < RES; i++)
- residuals[i] = (*residuals_jets_)[i].a;
+ residuals[i] = residuals_jets_[i].a;
// fill the jacobian matrices
for (unsigned int i = 0; i<n_blocks; i++)
if (jacobians[i] != nullptr)
for (unsigned int row = 0; row < RES; row++)
- std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
- (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
jacobians[i] + row * state_block_sizes_.at(i));
}
return true;
@@ -1758,11 +2265,11 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0> : public FactorBase
{
// update jets real part
for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i].a = parameters[0][i];
+ jets_0_[i].a = parameters[0][i];
for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i].a = parameters[1][i];
+ jets_1_[i].a = parameters[1][i];
for (unsigned int i = 0; i < B2; i++)
- (*jets_2_)[i].a = parameters[2][i];
+ jets_2_[i].a = parameters[2][i];
}
virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const
@@ -1776,14 +2283,14 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0> : public FactorBase
updateJetsRealPart(_states_ptr);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_->data(),
- jets_1_->data(),
- jets_2_->data(),
- residuals_jets_->data());
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ residuals_jets_.data());
// fill the residual vector
for (unsigned int i = 0; i < RES; i++)
- residual_(i) = (*residuals_jets_)[i].a;
+ residual_(i) = residuals_jets_[i].a;
// fill the jacobian matrices
for (unsigned int i = 0; i<n_blocks; i++)
@@ -1793,8 +2300,8 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0> : public FactorBase
// Fill Jacobian rows from jets
if (!state_ptrs_[i]->isFixed())
for (unsigned int row = 0; row < RES; row++)
- std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
- (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
Ji.data() + row * state_block_sizes_.at(i));
// add to the Jacobians vector
jacobians_.push_back(Ji);
@@ -1824,7 +2331,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0> : public FactorBase
////////////////// SPECIALIZATION 2 BLOCKS ////////////////////////////////////////////////////////////////////////
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1>
-class FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0> : public FactorBase
+class FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0,0,0> : public FactorBase
{
public:
@@ -1839,6 +2346,8 @@ class FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0> : public FactorBase
static const unsigned int block7Size = 0;
static const unsigned int block8Size = 0;
static const unsigned int block9Size = 0;
+ static const unsigned int block10Size = 0;
+ static const unsigned int block11Size = 0;
static const unsigned int n_blocks = 2;
static const std::vector<unsigned int> state_block_sizes_;
@@ -1850,14 +2359,14 @@ class FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0> : public FactorBase
std::vector<StateBlockPtr> state_ptrs_;
static const std::vector<unsigned int> jacobian_locations_;
- std::array<WolfJet, RES>* residuals_jets_;
-
- std::array<WolfJet, B0>* jets_0_;
- std::array<WolfJet, B1>* jets_1_;
+ mutable std::array<WolfJet, RES> residuals_jets_;
+ mutable std::array<WolfJet, B0> jets_0_;
+ mutable std::array<WolfJet, B1> jets_1_;
public:
FactorAutodiff(const std::string& _tp,
+ const FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
@@ -1867,26 +2376,24 @@ class FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0> : public FactorBase
FactorStatus _status,
StateBlockPtr _state0Ptr,
StateBlockPtr _state1Ptr) :
- FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
- state_ptrs_({_state0Ptr,_state1Ptr}),
- residuals_jets_(new std::array<WolfJet, RES>),
- jets_0_(new std::array<WolfJet, B0>),
- jets_1_(new std::array<WolfJet, B1>)
+ FactorBase(_tp, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ state_ptrs_({_state0Ptr,_state1Ptr})
{
+ // asserts for null states
+ assert(_state0Ptr != nullptr && "nullptr state block");
+ assert(_state1Ptr != nullptr && "nullptr state block");
+
// initialize jets
unsigned int last_jet_idx = 0;
for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
+ jets_1_[i] = WolfJet(0, last_jet_idx++);
state_ptrs_.resize(n_blocks);
}
virtual ~FactorAutodiff()
{
- delete jets_0_;
- delete jets_1_;
- delete residuals_jets_;
}
virtual JacobianMethod getJacobianMethod() const
@@ -1912,20 +2419,20 @@ class FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0> : public FactorBase
updateJetsRealPart(param_vec);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_->data(),
- jets_1_->data(),
- residuals_jets_->data());
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ residuals_jets_.data());
// fill the residual array
for (unsigned int i = 0; i < RES; i++)
- residuals[i] = (*residuals_jets_)[i].a;
+ residuals[i] = residuals_jets_[i].a;
// fill the jacobian matrices
for (unsigned int i = 0; i<n_blocks; i++)
if (jacobians[i] != nullptr)
for (unsigned int row = 0; row < RES; row++)
- std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
- (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
jacobians[i] + row * state_block_sizes_.at(i));
}
return true;
@@ -1935,9 +2442,9 @@ class FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0> : public FactorBase
{
// update jets real part
for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i].a = parameters[0][i];
+ jets_0_[i].a = parameters[0][i];
for (unsigned int i = 0; i < B1; i++)
- (*jets_1_)[i].a = parameters[1][i];
+ jets_1_[i].a = parameters[1][i];
}
virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const
@@ -1951,13 +2458,13 @@ class FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0> : public FactorBase
updateJetsRealPart(_states_ptr);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_->data(),
- jets_1_->data(),
- residuals_jets_->data());
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ residuals_jets_.data());
// fill the residual vector
for (unsigned int i = 0; i < RES; i++)
- residual_(i) = (*residuals_jets_)[i].a;
+ residual_(i) = residuals_jets_[i].a;
// fill the jacobian matrices
for (unsigned int i = 0; i<n_blocks; i++)
@@ -1967,8 +2474,8 @@ class FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0> : public FactorBase
// Fill Jacobian rows from jets
if (!state_ptrs_[i]->isFixed())
for (unsigned int row = 0; row < RES; row++)
- std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
- (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
Ji.data() + row * state_block_sizes_.at(i));
// add to the Jacobians vector
jacobians_.push_back(Ji);
@@ -1998,7 +2505,7 @@ class FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0> : public FactorBase
////////////////// SPECIALIZATION 1 BLOCK ////////////////////////////////////////////////////////////////////////
template <class FacT,unsigned int RES,unsigned int B0>
-class FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0> : public FactorBase
+class FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0,0,0> : public FactorBase
{
public:
@@ -2013,6 +2520,8 @@ class FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0> : public FactorBase
static const unsigned int block7Size = 0;
static const unsigned int block8Size = 0;
static const unsigned int block9Size = 0;
+ static const unsigned int block10Size = 0;
+ static const unsigned int block11Size = 0;
static const unsigned int n_blocks = 1;
static const std::vector<unsigned int> state_block_sizes_;
@@ -2024,13 +2533,13 @@ class FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0> : public FactorBase
std::vector<StateBlockPtr> state_ptrs_;
static const std::vector<unsigned int> jacobian_locations_;
- std::array<WolfJet, RES>* residuals_jets_;
-
- std::array<WolfJet, B0>* jets_0_;
+ mutable std::array<WolfJet, RES> residuals_jets_;
+ mutable std::array<WolfJet, B0> jets_0_;
public:
FactorAutodiff(const std::string& _tp,
+ const FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
@@ -2039,22 +2548,21 @@ class FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0> : public FactorBase
bool _apply_loss_function,
FactorStatus _status,
StateBlockPtr _state0Ptr) :
- FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
- state_ptrs_({_state0Ptr}),
- residuals_jets_(new std::array<WolfJet, RES>),
- jets_0_(new std::array<WolfJet, B0>)
+ FactorBase(_tp, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ state_ptrs_({_state0Ptr})
{
+ // asserts for null states
+ assert(_state0Ptr != nullptr && "nullptr state block");
+
// initialize jets
unsigned int last_jet_idx = 0;
for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
state_ptrs_.resize(n_blocks);
}
virtual ~FactorAutodiff()
{
- delete jets_0_;
- delete residuals_jets_;
}
virtual JacobianMethod getJacobianMethod() const
@@ -2079,19 +2587,19 @@ class FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0> : public FactorBase
updateJetsRealPart(param_vec);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_->data(),
- residuals_jets_->data());
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ residuals_jets_.data());
// fill the residual array
for (unsigned int i = 0; i < RES; i++)
- residuals[i] = (*residuals_jets_)[i].a;
+ residuals[i] = residuals_jets_[i].a;
// fill the jacobian matrices
for (unsigned int i = 0; i<n_blocks; i++)
if (jacobians[i] != nullptr)
for (unsigned int row = 0; row < RES; row++)
- std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
- (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
jacobians[i] + row * state_block_sizes_.at(i));
}
return true;
@@ -2101,7 +2609,7 @@ class FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0> : public FactorBase
{
// update jets real part
for (unsigned int i = 0; i < B0; i++)
- (*jets_0_)[i].a = parameters[0][i];
+ jets_0_[i].a = parameters[0][i];
}
virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const
@@ -2115,12 +2623,12 @@ class FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0> : public FactorBase
updateJetsRealPart(_states_ptr);
// call functor
- (*static_cast<FacT const*>(this))(jets_0_->data(),
- residuals_jets_->data());
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ residuals_jets_.data());
// fill the residual vector
for (unsigned int i = 0; i < RES; i++)
- residual_(i) = (*residuals_jets_)[i].a;
+ residual_(i) = residuals_jets_[i].a;
// fill the jacobian matrices
for (unsigned int i = 0; i<n_blocks; i++)
@@ -2130,8 +2638,8 @@ class FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0> : public FactorBase
// Fill Jacobian rows from jets
if (!state_ptrs_[i]->isFixed())
for (unsigned int row = 0; row < RES; row++)
- std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
- (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
Ji.data() + row * state_block_sizes_.at(i));
// add to the Jacobians vector
jacobians_.push_back(Ji);
@@ -2162,113 +2670,146 @@ class FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0> : public FactorBase
// STATIC CONST VECTORS INITIALIZATION
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// state_block_sizes_
+// 12 BLOCKS
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9,unsigned int B10,unsigned int B11>
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,B11>::state_block_sizes_ = {B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,B11};
+// 11 BLOCKS
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9,unsigned int B10>
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,0>::state_block_sizes_ = {B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10};
// 10 BLOCKS
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9>::state_block_sizes_ = {B0,B1,B2,B3,B4,B5,B6,B7,B8,B9};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,0,0>::state_block_sizes_ = {B0,B1,B2,B3,B4,B5,B6,B7,B8,B9};
// 9 BLOCKS
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0>::state_block_sizes_ = {B0,B1,B2,B3,B4,B5,B6,B7,B8};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0,0,0>::state_block_sizes_ = {B0,B1,B2,B3,B4,B5,B6,B7,B8};
// 8 BLOCKS
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0>::state_block_sizes_ = {B0,B1,B2,B3,B4,B5,B6,B7};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0,0,0>::state_block_sizes_ = {B0,B1,B2,B3,B4,B5,B6,B7};
// 7 BLOCKS
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0>::state_block_sizes_ = {B0,B1,B2,B3,B4,B5,B6};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0,0,0>::state_block_sizes_ = {B0,B1,B2,B3,B4,B5,B6};
// 6 BLOCKS
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0>::state_block_sizes_ = {B0,B1,B2,B3,B4,B5};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0,0,0>::state_block_sizes_ = {B0,B1,B2,B3,B4,B5};
// 5 BLOCKS
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0>::state_block_sizes_ = {B0,B1,B2,B3,B4};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0,0,0>::state_block_sizes_ = {B0,B1,B2,B3,B4};
// 4 BLOCKS
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0>::state_block_sizes_ = {B0,B1,B2,B3};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0,0,0>::state_block_sizes_ = {B0,B1,B2,B3};
// 3 BLOCKS
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0>::state_block_sizes_ = {B0,B1,B2};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0,0,0>::state_block_sizes_ = {B0,B1,B2};
// 2 BLOCKS
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0>::state_block_sizes_ = {B0,B1};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0,0,0>::state_block_sizes_ = {B0,B1};
// 1 BLOCK
template <class FacT,unsigned int RES,unsigned int B0>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0>::state_block_sizes_ = {B0};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0,0,0>::state_block_sizes_ = {B0};
// jacobian_locations_
+// 12 BLOCKS
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9,unsigned int B10,unsigned int B11>
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,B11>::jacobian_locations_ = {0,
+ B0,
+ B0+B1,
+ B0+B1+B2,
+ B0+B1+B2+B3,
+ B0+B1+B2+B3+B4,
+ B0+B1+B2+B3+B4+B5,
+ B0+B1+B2+B3+B4+B5+B6,
+ B0+B1+B2+B3+B4+B5+B6+B7,
+ B0+B1+B2+B3+B4+B5+B6+B7+B8,
+ B0+B1+B2+B3+B4+B5+B6+B7+B8+B9,
+ B0+B1+B2+B3+B4+B5+B6+B7+B8+B9+B10};
+// 10 BLOCKS
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9,unsigned int B10>
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,0>::jacobian_locations_ = {0,
+ B0,
+ B0+B1,
+ B0+B1+B2,
+ B0+B1+B2+B3,
+ B0+B1+B2+B3+B4,
+ B0+B1+B2+B3+B4+B5,
+ B0+B1+B2+B3+B4+B5+B6,
+ B0+B1+B2+B3+B4+B5+B6+B7,
+ B0+B1+B2+B3+B4+B5+B6+B7+B8,
+ B0+B1+B2+B3+B4+B5+B6+B7+B8+B9};
// 10 BLOCKS
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9>::jacobian_locations_ = {0,
- B0,
- B0+B1,
- B0+B1+B2,
- B0+B1+B2+B3,
- B0+B1+B2+B3+B4,
- B0+B1+B2+B3+B4+B5,
- B0+B1+B2+B3+B4+B5+B6,
- B0+B1+B2+B3+B4+B5+B6+B7,
- B0+B1+B2+B3+B4+B5+B6+B7+B8};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,0,0>::jacobian_locations_ = {0,
+ B0,
+ B0+B1,
+ B0+B1+B2,
+ B0+B1+B2+B3,
+ B0+B1+B2+B3+B4,
+ B0+B1+B2+B3+B4+B5,
+ B0+B1+B2+B3+B4+B5+B6,
+ B0+B1+B2+B3+B4+B5+B6+B7,
+ B0+B1+B2+B3+B4+B5+B6+B7+B8};
// 9 BLOCKS
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0>::jacobian_locations_ = {0,
- B0,
- B0+B1,
- B0+B1+B2,
- B0+B1+B2+B3,
- B0+B1+B2+B3+B4,
- B0+B1+B2+B3+B4+B5,
- B0+B1+B2+B3+B4+B5+B6,
- B0+B1+B2+B3+B4+B5+B6+B7};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0,0,0>::jacobian_locations_ = {0,
+ B0,
+ B0+B1,
+ B0+B1+B2,
+ B0+B1+B2+B3,
+ B0+B1+B2+B3+B4,
+ B0+B1+B2+B3+B4+B5,
+ B0+B1+B2+B3+B4+B5+B6,
+ B0+B1+B2+B3+B4+B5+B6+B7};
// 8 BLOCKS
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0>::jacobian_locations_ = {0,
- B0,
- B0+B1,
- B0+B1+B2,
- B0+B1+B2+B3,
- B0+B1+B2+B3+B4,
- B0+B1+B2+B3+B4+B5,
- B0+B1+B2+B3+B4+B5+B6};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0,0,0>::jacobian_locations_ = {0,
+ B0,
+ B0+B1,
+ B0+B1+B2,
+ B0+B1+B2+B3,
+ B0+B1+B2+B3+B4,
+ B0+B1+B2+B3+B4+B5,
+ B0+B1+B2+B3+B4+B5+B6};
// 7 BLOCKS
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0>::jacobian_locations_ = {0,
- B0,
- B0+B1,
- B0+B1+B2,
- B0+B1+B2+B3,
- B0+B1+B2+B3+B4,
- B0+B1+B2+B3+B4+B5};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0,0,0>::jacobian_locations_ = {0,
+ B0,
+ B0+B1,
+ B0+B1+B2,
+ B0+B1+B2+B3,
+ B0+B1+B2+B3+B4,
+ B0+B1+B2+B3+B4+B5};
// 6 BLOCKS
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0>::jacobian_locations_ = {0,
- B0,
- B0+B1,
- B0+B1+B2,
- B0+B1+B2+B3,
- B0+B1+B2+B3+B4};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0,0,0>::jacobian_locations_ = {0,
+ B0,
+ B0+B1,
+ B0+B1+B2,
+ B0+B1+B2+B3,
+ B0+B1+B2+B3+B4};
// 5 BLOCKS
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0>::jacobian_locations_ = {0,
- B0,
- B0+B1,
- B0+B1+B2,
- B0+B1+B2+B3};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0,0,0>::jacobian_locations_ = {0,
+ B0,
+ B0+B1,
+ B0+B1+B2,
+ B0+B1+B2+B3};
// 4 BLOCKS
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0>::jacobian_locations_ = {0,
- B0,
- B0+B1,
- B0+B1+B2};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0,0,0>::jacobian_locations_ = {0,
+ B0,
+ B0+B1,
+ B0+B1+B2};
// 3 BLOCKS
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0>::jacobian_locations_ = {0,
- B0,
- B0+B1};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0,0,0>::jacobian_locations_ = {0,
+ B0,
+ B0+B1};
// 2 BLOCKS
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0>::jacobian_locations_ = {0,
- B0};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0,0,0>::jacobian_locations_ = {0,
+ B0};
// 1 BLOCK
template <class FacT,unsigned int RES,unsigned int B0>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0>::jacobian_locations_ = {0};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0,0,0>::jacobian_locations_ = {0};
} // namespace wolf
diff --git a/include/core/factor/factor_autodiff_distance_3d.h b/include/core/factor/factor_autodiff_distance_3d.h
index 8dc182d6f92ec60d7ba310ed1e7f94e27a9281fc..320f7ac270cdd151f078c520561e509f108296d8 100644
--- a/include/core/factor/factor_autodiff_distance_3d.h
+++ b/include/core/factor/factor_autodiff_distance_3d.h
@@ -23,8 +23,9 @@ class FactorAutodiffDistance3d : public FactorAutodiff<FactorAutodiffDistance3d,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status) :
- FactorAutodiff("DISTANCE 3d",
- _frm_other,
+ FactorAutodiff("FactorAutodiffDistance3d",
+ _feat,
+ _frm_other,
nullptr,
nullptr,
nullptr,
diff --git a/include/core/factor/factor_base.h b/include/core/factor/factor_base.h
index cd1439f96e437dc39dde137dd1014f37abee10db..acbe45ec5d827c247376a130d0f5de5591c1bed3 100644
--- a/include/core/factor/factor_base.h
+++ b/include/core/factor/factor_base.h
@@ -52,7 +52,9 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
CaptureBaseWPtrList capture_other_list_; ///< CaptureBase pointer list
FeatureBaseWPtrList feature_other_list_; ///< FeatureBase pointer list
LandmarkBaseWPtrList landmark_other_list_; ///< LandmarkBase pointer list
- ProcessorBaseWPtr processor_ptr_; ///< ProcessorBase pointer list
+ ProcessorBaseWPtr processor_ptr_; ///< Processor pointer
+ Eigen::VectorXd measurement_; ///< the measurement vector
+ Eigen::MatrixXd measurement_sqrt_information_upper_;///< the squared root information matrix ///< ProcessorBase pointer list
virtual void setProblem(ProblemPtr) final;
public:
@@ -64,6 +66,7 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
* that does not located in the same branch.
**/
FactorBase(const std::string& _tp,
+ const FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
@@ -73,6 +76,7 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
FactorStatus _status = FAC_ACTIVE);
FactorBase(const std::string& _tp,
+ const FeatureBasePtr& _feature_ptr,
const FrameBasePtrList& _frame_other_list,
const CaptureBasePtrList& _capture_other_list,
const FeatureBasePtrList& _feature_other_list,
@@ -124,15 +128,11 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
**/
virtual std::vector<unsigned int> getStateSizes() const = 0;
- /** \brief Returns a reference to the feature measurement
+ /** \brief Returns a reference to the measurement
**/
virtual const Eigen::VectorXd& getMeasurement() const;
- /** \brief Returns a reference to the feature measurement covariance
- **/
- virtual const Eigen::MatrixXd& getMeasurementCovariance() const;
-
- /** \brief Returns a reference to the feature measurement square root information
+ /** \brief Returns a reference to the measurement square root information
**/
virtual const Eigen::MatrixXd& getMeasurementSquareRootInformationUpper() const;
@@ -206,6 +206,23 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
template<typename classType, typename... T>
static std::shared_ptr<classType> emplace(FeatureBasePtr _oth_ptr, T&&... all);
+ virtual void printHeader(int depth, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream ,
+ std::string _tabs = "") const;
+
+ void print(int depth, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream = std::cout,
+ std::string _tabs = "") const;
+
+ virtual CheckLog localCheck(bool _verbose, FactorBasePtr _fac_ptr, std::ostream& _stream, std::string _tabs = "") const;
+ bool check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
+
private:
void setFeature(const FeatureBasePtr _ft_ptr){feature_ptr_ = _ft_ptr;}
@@ -237,7 +254,6 @@ std::shared_ptr<classType> FactorBase::emplace(FeatureBasePtr _ftr_ptr, T&&... a
return ctr;
}
-
inline unsigned int FactorBase::id() const
{
return factor_id_;
@@ -290,7 +306,6 @@ inline LandmarkBasePtr FactorBase::getLandmarkOther() const
return landmark_other_list_.front().lock();
}
-
inline ProcessorBasePtr FactorBase::getProcessor() const
{
return processor_ptr_.lock();
@@ -301,5 +316,15 @@ inline void FactorBase::setProcessor(const ProcessorBasePtr& _processor_ptr)
processor_ptr_ = _processor_ptr;
}
+inline const Eigen::VectorXd& FactorBase::getMeasurement() const
+{
+ return measurement_;
+}
+
+inline const Eigen::MatrixXd& FactorBase::getMeasurementSquareRootInformationUpper() const
+{
+ return measurement_sqrt_information_upper_;
+}
+
} // namespace wolf
#endif
diff --git a/include/core/factor/factor_block_absolute.h b/include/core/factor/factor_block_absolute.h
index 0743548b7b4aad0517ecdd2590955c4cdf7c023c..143308af2988a74d754f865c305e1ba089f4f778 100644
--- a/include/core/factor/factor_block_absolute.h
+++ b/include/core/factor/factor_block_absolute.h
@@ -31,17 +31,49 @@ class FactorBlockAbsolute : public FactorAnalytic
/** \brief Constructor
*
* \param _sb_ptr the constrained state block
- * \param _start_idx (default=0) the index of the first state element that is constrained
- * \param _start_idx (default=-1) the size of the state segment that is constrained, -1 = all the
*
*/
- FactorBlockAbsolute(StateBlockPtr _sb_ptr,
- unsigned int _start_idx = 0,
- int _size = -1,
- ProcessorBasePtr _processor_ptr = nullptr,
- bool _apply_loss_function = false,
+ FactorBlockAbsolute(FeatureBasePtr _feature_ptr,
+ StateBlockPtr _sb_ptr,
+ ProcessorBasePtr _processor_ptr,
+ bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE) :
FactorAnalytic("FactorBlockAbsolute",
+ _feature_ptr,
+ nullptr,
+ nullptr,
+ nullptr,
+ nullptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _sb_ptr),
+ sb_size_(_sb_ptr->getSize()),
+ sb_constrained_start_(0),
+ sb_constrained_size_(sb_size_)
+ {
+ assert(sb_constrained_size_+sb_constrained_start_ <= sb_size_);
+
+ // precompute Jacobian (Identity)
+ J_ = Eigen::MatrixXd::Identity(sb_constrained_size_, sb_size_);
+ }
+
+ /** \brief Constructor for segment of state block
+ *
+ * \param _sb_ptr the constrained state block
+ * \param _start_idx the index of the first state element that is constrained
+ * \param _start_idx the size of the state segment that is constrained, -1 = all the
+ *
+ */
+ FactorBlockAbsolute(FeatureBasePtr _feature_ptr,
+ StateBlockPtr _sb_ptr,
+ unsigned int _start_idx,
+ int _size,
+ ProcessorBasePtr _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE) :
+ FactorAnalytic("FactorBlockAbsolute",
+ _feature_ptr,
nullptr,
nullptr,
nullptr,
diff --git a/include/core/factor/factor_block_difference.h b/include/core/factor/factor_block_difference.h
index d3558ef21aa6e6577fc54d26015db3f9e3193312..ebaf8bc7be42c444f74588599e5b18d7135a5a88 100644
--- a/include/core/factor/factor_block_difference.h
+++ b/include/core/factor/factor_block_difference.h
@@ -36,21 +36,22 @@ class FactorBlockDifference : public FactorAnalytic
* \param _sb_ptr the constrained state block
*
*/
- FactorBlockDifference(
- const StateBlockPtr& _sb1_ptr,
- const StateBlockPtr& _sb2_ptr,
- const FrameBasePtr& _frame_other = nullptr,
- const CaptureBasePtr& _cap_other = nullptr,
- const FeatureBasePtr& _feat_other = nullptr,
- const LandmarkBasePtr& _lmk_other = nullptr,
- unsigned int _start_idx1 = 0,
- int _size1 = -1,
- unsigned int _start_idx2 = 0,
- int _size2 = -1,
- ProcessorBasePtr _processor_ptr = nullptr,
- bool _apply_loss_function = false,
- FactorStatus _status = FAC_ACTIVE) :
+ FactorBlockDifference(const FeatureBasePtr& _feature_ptr,
+ const StateBlockPtr& _sb1_ptr,
+ const StateBlockPtr& _sb2_ptr,
+ const FrameBasePtr& _frame_other = nullptr,
+ const CaptureBasePtr& _cap_other = nullptr,
+ const FeatureBasePtr& _feat_other = nullptr,
+ const LandmarkBasePtr& _lmk_other = nullptr,
+ unsigned int _start_idx1 = 0,
+ int _size1 = -1,
+ unsigned int _start_idx2 = 0,
+ int _size2 = -1,
+ ProcessorBasePtr _processor_ptr = nullptr,
+ bool _apply_loss_function = false,
+ FactorStatus _status = FAC_ACTIVE) :
FactorAnalytic("FactorBlockDifference",
+ _feature_ptr,
_frame_other,
_cap_other,
_feat_other,
@@ -129,8 +130,8 @@ inline Eigen::VectorXd FactorBlockDifference::evaluateResiduals(const std::vecto
{
// Check measurement and cov sizes
assert(_st_vector.size() == 2 && "Wrong number of state blocks!");
- assert(getMeasurement().size() == getMeasurementCovariance().cols());
- assert(getMeasurement().size() == getMeasurementCovariance().rows());
+ assert(getMeasurement().size() == getMeasurementSquareRootInformationUpper().cols());
+ assert(getMeasurement().size() == getMeasurementSquareRootInformationUpper().rows());
assert(getMeasurement().size() == sb1_constrained_size_);
assert(getMeasurement().size() == sb2_constrained_size_);
diff --git a/include/core/factor/factor_diff_drive.h b/include/core/factor/factor_diff_drive.h
index d936a99d6c1d64f29fda2c5aeca1806e0065c4d1..e267fb507ebfd811a0c17b1b6446267223510198 100644
--- a/include/core/factor/factor_diff_drive.h
+++ b/include/core/factor/factor_diff_drive.h
@@ -14,6 +14,7 @@
#include "core/factor/factor_autodiff.h"
#include "core/frame/frame_base.h"
#include "core/feature/feature_motion.h"
+#include "core/math/rotations.h"
namespace
{
@@ -31,20 +32,20 @@ namespace wolf
WOLF_PTR_TYPEDEFS(FactorDiffDrive);
class FactorDiffDrive : public FactorAutodiff<FactorDiffDrive,
- RESIDUAL_SIZE,
- POSITION_SIZE,
- ORIENTATION_SIZE,
- POSITION_SIZE,
- ORIENTATION_SIZE,
- INTRINSICS_SIZE>
+ RESIDUAL_SIZE,
+ POSITION_SIZE,
+ ORIENTATION_SIZE,
+ POSITION_SIZE,
+ ORIENTATION_SIZE,
+ INTRINSICS_SIZE>
{
using Base = FactorAutodiff<FactorDiffDrive, // @suppress("Type cannot be resolved")
- RESIDUAL_SIZE,
- POSITION_SIZE,
- ORIENTATION_SIZE,
- POSITION_SIZE,
- ORIENTATION_SIZE,
- INTRINSICS_SIZE>;
+ RESIDUAL_SIZE,
+ POSITION_SIZE,
+ ORIENTATION_SIZE,
+ POSITION_SIZE,
+ ORIENTATION_SIZE,
+ INTRINSICS_SIZE>;
public:
@@ -54,6 +55,7 @@ class FactorDiffDrive : public FactorAutodiff<FactorDiffDrive,
const bool _apply_loss_function,
const FactorStatus _status = FAC_ACTIVE) :
Base("FactorDiffDrive",
+ _ftr_ptr,
_capture_origin_ptr->getFrame(),
_capture_origin_ptr,
nullptr,
@@ -126,10 +128,7 @@ inline bool FactorDiffDrive::operator ()(const T* const _p1, const T* const _o1,
residuals = delta_corrected - delta_predicted;
// angle remapping
- while (residuals(2) > T(M_PI))
- residuals(2) = residuals(2) - T(2. * M_PI);
- while (residuals(2) <= T(-M_PI))
- residuals(2) = residuals(2) + T(2. * M_PI);
+ pi2pi(residuals(2));
// weighted residual
residuals = getMeasurementSquareRootInformationUpper() * residuals;
diff --git a/include/core/factor/factor_odom_2d.h b/include/core/factor/factor_odom_2d.h
index e20f0f50f158b7ed04e2d09054fafee9300a6f29..d8cfa08a9a9bf70c8b580f407f0983f60e791566 100644
--- a/include/core/factor/factor_odom_2d.h
+++ b/include/core/factor/factor_odom_2d.h
@@ -4,6 +4,7 @@
//Wolf includes
#include "core/factor/factor_autodiff.h"
#include "core/frame/frame_base.h"
+#include "core/math/rotations.h"
//#include "ceres/jet.h"
@@ -16,18 +17,19 @@ class FactorOdom2d : public FactorAutodiff<FactorOdom2d, 3, 2, 1, 2, 1>
{
public:
FactorOdom2d(const FeatureBasePtr& _ftr_ptr,
- const FrameBasePtr& _frame_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status = FAC_ACTIVE) :
+ const FrameBasePtr& _frame_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorOdom2d, 3, 2, 1, 2, 1>("FactorOdom2d",
- _frame_other_ptr, nullptr, nullptr, nullptr,
- _processor_ptr,
- _apply_loss_function, _status,
- _frame_other_ptr->getP(),
- _frame_other_ptr->getO(),
- _ftr_ptr->getFrame()->getP(),
- _ftr_ptr->getFrame()->getO())
+ _ftr_ptr,
+ _frame_other_ptr, nullptr, nullptr, nullptr,
+ _processor_ptr,
+ _apply_loss_function, _status,
+ _frame_other_ptr->getP(),
+ _frame_other_ptr->getO(),
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO())
{
//
}
@@ -72,10 +74,7 @@ inline bool FactorOdom2d::operator ()(const T* const _p1, const T* const _o1, co
// Error
er = expected_measurement - getMeasurement();
- while (er(2) > T( M_PI ))
- er(2) -= T( 2 * M_PI );
- while (er(2) < T( -M_PI ))
- er(2) += T( 2 * M_PI );
+ er(2) = pi2pi(er(2));
// Residuals
res = getMeasurementSquareRootInformationUpper() * er;
diff --git a/include/core/factor/factor_odom_2d_analytic.h b/include/core/factor/factor_odom_2d_analytic.h
index c7eec32a1778b9f1ad69da6c3c0087e50509a904..52ea04ccd817f2a6c914d771737c284bca4ce664 100644
--- a/include/core/factor/factor_odom_2d_analytic.h
+++ b/include/core/factor/factor_odom_2d_analytic.h
@@ -18,7 +18,7 @@ class FactorOdom2dAnalytic : public FactorRelative2dAnalytic
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE) :
- FactorRelative2dAnalytic("ODOM_2d",
+ FactorRelative2dAnalytic("FactorOdom2dAnalytic",
_ftr_ptr,
_frame_ptr,
_processor_ptr,
diff --git a/include/core/factor/factor_odom_2d_closeloop.h b/include/core/factor/factor_odom_2d_closeloop.h
index e7d35aefa48b549174d060668cbee540edb6e9e2..a4ea7b1dff8484a89885edd424a6ac899331d3d7 100644
--- a/include/core/factor/factor_odom_2d_closeloop.h
+++ b/include/core/factor/factor_odom_2d_closeloop.h
@@ -4,6 +4,7 @@
//Wolf includes
#include "core/factor/factor_autodiff.h"
#include "core/frame/frame_base.h"
+#include "core/math/rotations.h"
//#include "ceres/jet.h"
@@ -20,13 +21,14 @@ class FactorOdom2dCloseloop : public FactorAutodiff<FactorOdom2dCloseloop, 3, 2,
const ProcessorBasePtr& _processor_ptr = nullptr,
bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorOdom2dCloseloop, 3, 2, 1, 2, 1>("FactorOdom2dCloseloop",
- _frame_other_ptr, nullptr, nullptr, nullptr,
- _processor_ptr,
- _apply_loss_function, _status,
- _frame_other_ptr->getP(),
- _frame_other_ptr->getO(),
- _ftr_ptr->getFrame()->getP(),
- _ftr_ptr->getFrame()->getO())
+ _ftr_ptr,
+ _frame_other_ptr, nullptr, nullptr, nullptr,
+ _processor_ptr,
+ _apply_loss_function, _status,
+ _frame_other_ptr->getP(),
+ _frame_other_ptr->getO(),
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO())
{
//
}
@@ -71,10 +73,7 @@ inline bool FactorOdom2dCloseloop::operator ()(const T* const _p1, const T* cons
// Error
er = expected_measurement - getMeasurement().cast<T>();
- while (er(2) > T( M_PI ))
- er(2) -= T( 2 * M_PI );
- while (er(2) < T( -M_PI ))
- er(2) += T( 2 * M_PI );
+ er(2)=pi2pi(er(2));
// Residuals
res = getMeasurementSquareRootInformationUpper().cast<T>() * er;
diff --git a/include/core/factor/factor_odom_3d.h b/include/core/factor/factor_odom_3d.h
index 2e49daf216906b4e96d92e9c195cbff579fd6545..4e32efe4067d52c23bfe978cea7ef792214e00c9 100644
--- a/include/core/factor/factor_odom_3d.h
+++ b/include/core/factor/factor_odom_3d.h
@@ -21,10 +21,10 @@ class FactorOdom3d : public FactorAutodiff<FactorOdom3d,6,3,4,3,4>
{
public:
FactorOdom3d(const FeatureBasePtr& _ftr_current_ptr,
- const FrameBasePtr& _frame_past_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status = FAC_ACTIVE);
+ const FrameBasePtr& _frame_past_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE);
virtual ~FactorOdom3d() = default;
@@ -81,18 +81,19 @@ inline FactorOdom3d::FactorOdom3d(const FeatureBasePtr& _ftr_current_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status) :
- FactorAutodiff<FactorOdom3d, 6, 3, 4, 3, 4>("FactorOdom3d", // type
- _frame_past_ptr, // frame other
- nullptr, // capture other
- nullptr, // feature other
- nullptr, // landmark other
- _processor_ptr, // processor
- _apply_loss_function,
- _status,
- _ftr_current_ptr->getFrame()->getP(), // current frame P
- _ftr_current_ptr->getFrame()->getO(), // current frame Q
- _frame_past_ptr->getP(), // past frame P
- _frame_past_ptr->getO()) // past frame Q
+ FactorAutodiff<FactorOdom3d, 6, 3, 4, 3, 4>("FactorOdom3d", // type
+ _ftr_current_ptr, // feature
+ _frame_past_ptr, // frame other
+ nullptr, // capture other
+ nullptr, // feature other
+ nullptr, // landmark other
+ _processor_ptr, // processor
+ _apply_loss_function,
+ _status,
+ _ftr_current_ptr->getFrame()->getP(), // current frame P
+ _ftr_current_ptr->getFrame()->getO(), // current frame Q
+ _frame_past_ptr->getP(), // past frame P
+ _frame_past_ptr->getO()) // past frame Q
{
// WOLF_TRACE("Constr ODOM 3d (f", _ftr_current_ptr->id(),
// " F", _ftr_current_ptr->getCapture()->getFrame()->id(),
@@ -105,8 +106,12 @@ inline FactorOdom3d::FactorOdom3d(const FeatureBasePtr& _ftr_current_ptr,
}
template<typename T>
-inline bool FactorOdom3d::expectation(const T* const _p_current, const T* const _q_current, const T* const _p_past,
- const T* const _q_past, T* _expectation_dp, T* _expectation_dq) const
+inline bool FactorOdom3d::expectation(const T* const _p_current,
+ const T* const _q_current,
+ const T* const _p_past,
+ const T* const _q_past,
+ T* _expectation_dp,
+ T* _expectation_dq) const
{
Eigen::Map<const Eigen::Matrix<T,3,1> > p_current(_p_current);
Eigen::Map<const Eigen::Quaternion<T> > q_current(_q_current);
@@ -168,8 +173,11 @@ inline Eigen::VectorXd FactorOdom3d::expectation() const
}
template<typename T>
-inline bool FactorOdom3d::operator ()(const T* const _p_current, const T* const _q_current, const T* const _p_past,
- const T* const _q_past, T* _residuals) const
+inline bool FactorOdom3d::operator ()(const T* const _p_current,
+ const T* const _q_current,
+ const T* const _p_past,
+ const T* const _q_past,
+ T* _residuals) const
{
/* Residual expression
diff --git a/include/core/factor/factor_pose_2d.h b/include/core/factor/factor_pose_2d.h
index ce63c806017117e67d18c2e79c34468e6b16a61c..a054c11673b75e69fe413afa9c5a161ac42d0778 100644
--- a/include/core/factor/factor_pose_2d.h
+++ b/include/core/factor/factor_pose_2d.h
@@ -22,6 +22,7 @@ class FactorPose2d: public FactorAutodiff<FactorPose2d,3,2,1>
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorPose2d, 3, 2, 1>("FactorPose2d",
+ _ftr_ptr,
nullptr, nullptr, nullptr, nullptr,
_processor_ptr,
_apply_loss_function,
@@ -54,15 +55,11 @@ inline bool FactorPose2d::operator ()(const T* const _p, const T* const _o, T* _
Eigen::Matrix<T,3,1> er;
er(0) = meas(0) - _p[0];
er(1) = meas(1) - _p[1];
- er(2) = meas(2) - _o[0];
- while (er[2] > T(M_PI))
- er(2) = er(2) - T(2*M_PI);
- while (er(2) <= T(-M_PI))
- er(2) = er(2) + T(2*M_PI);
+ er(2) = pi2pi(meas(2) - _o[0]);
// residual
Eigen::Map<Eigen::Matrix<T,3,1>> res(_residuals);
- res = getFeature()->getMeasurementSquareRootInformationUpper() * er;
+ res = getMeasurementSquareRootInformationUpper() * er;
////////////////////////////////////////////////////////
// print Jacobian. Uncomment this as you wish (remember to uncomment #include "ceres/jet.h" above):
diff --git a/include/core/factor/factor_pose_3d.h b/include/core/factor/factor_pose_3d.h
index 9a46cbab24101a69faaad78ea190f1823406dd35..0390ab6401f782ed30921e9090bc2e5ed796589a 100644
--- a/include/core/factor/factor_pose_3d.h
+++ b/include/core/factor/factor_pose_3d.h
@@ -21,6 +21,7 @@ class FactorPose3d: public FactorAutodiff<FactorPose3d,6,3,4>
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorPose3d,6,3,4>("FactorPose3d",
+ _ftr_ptr,
nullptr, nullptr, nullptr, nullptr,
_processor_ptr,
_apply_loss_function,
@@ -61,7 +62,7 @@ inline bool FactorPose3d::operator ()(const T* const _p, const T* const _o, T* _
// residual
Eigen::Map<Eigen::Matrix<T, 6, 1>> res(_residuals);
- res = getFeature()->getMeasurementSquareRootInformationUpper() * er;
+ res = getMeasurementSquareRootInformationUpper() * er;
return true;
}
diff --git a/include/core/factor/factor_quaternion_absolute.h b/include/core/factor/factor_quaternion_absolute.h
index 6533bd414d54a9eb45da8c92336ba3d1f0e07739..8cb37c39234d4ae47c05453179f0354380c824e9 100644
--- a/include/core/factor/factor_quaternion_absolute.h
+++ b/include/core/factor/factor_quaternion_absolute.h
@@ -23,11 +23,13 @@ class FactorQuaternionAbsolute: public FactorAutodiff<FactorQuaternionAbsolute,3
{
public:
- FactorQuaternionAbsolute(StateBlockPtr _sb_ptr,
- ProcessorBasePtr _processor_ptr = nullptr,
- bool _apply_loss_function = false,
+ FactorQuaternionAbsolute(FeatureBasePtr _ftr_ptr,
+ StateBlockPtr _sb_ptr,
+ ProcessorBasePtr _processor_ptr,
+ bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE) :
- FactorAutodiff<FactorQuaternionAbsolute,3,4>("QUATERNION ABS",
+ FactorAutodiff<FactorQuaternionAbsolute,3,4>("FactorQuaternionAbsolute",
+ _ftr_ptr,
nullptr,
nullptr,
nullptr,
@@ -78,7 +80,7 @@ inline bool FactorQuaternionAbsolute::operator ()(const T* const _o, T* _residua
// residual
Eigen::Map<Eigen::Matrix<T, 3, 1>> res(_residuals);
- res = getFeature()->getMeasurementSquareRootInformationUpper() * er;
+ res = getMeasurementSquareRootInformationUpper() * er;
return true;
}
diff --git a/include/core/factor/factor_relative_2d_analytic.h b/include/core/factor/factor_relative_2d_analytic.h
index 5a67da1ff6c0e17614902ebc3195dd8124ec0ed6..e4ccf958c00e0e6185e0cf97a20c8072517c2756 100644
--- a/include/core/factor/factor_relative_2d_analytic.h
+++ b/include/core/factor/factor_relative_2d_analytic.h
@@ -4,6 +4,7 @@
//Wolf includes
#include "core/factor/factor_analytic.h"
#include "core/landmark/landmark_base.h"
+#include "core/math/rotations.h"
#include <Eigen/StdVector>
namespace wolf {
@@ -18,12 +19,24 @@ class FactorRelative2dAnalytic : public FactorAnalytic
/** \brief Constructor of category FAC_FRAME
**/
FactorRelative2dAnalytic(const std::string& _tp,
- const FeatureBasePtr& _ftr_ptr,
- const FrameBasePtr& _frame_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status = FAC_ACTIVE) :
- FactorAnalytic(_tp, _frame_ptr, nullptr, nullptr, nullptr, _processor_ptr, _apply_loss_function, _status, _frame_ptr->getP(), _frame_ptr->getO(), _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO())
+ const FeatureBasePtr& _ftr_ptr,
+ const FrameBasePtr& _frame_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE) :
+ FactorAnalytic(_tp,
+ _ftr_ptr,
+ _frame_other_ptr,
+ nullptr,
+ nullptr,
+ nullptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _frame_other_ptr->getP(),
+ _frame_other_ptr->getO(),
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO())
{
//
}
@@ -31,12 +44,24 @@ class FactorRelative2dAnalytic : public FactorAnalytic
/** \brief Constructor of category FAC_FEATURE
**/
FactorRelative2dAnalytic(const std::string& _tp,
- const FeatureBasePtr& _ftr_ptr,
- const FeatureBasePtr& _ftr_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status = FAC_ACTIVE) :
- FactorAnalytic(_tp, nullptr, nullptr, _ftr_other_ptr, nullptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO(), _ftr_other_ptr->getFrame()->getP(), _ftr_other_ptr->getFrame()->getO() )
+ const FeatureBasePtr& _ftr_ptr,
+ const FeatureBasePtr& _ftr_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE) :
+ FactorAnalytic(_tp,
+ _ftr_ptr,
+ nullptr,
+ nullptr,
+ _ftr_other_ptr,
+ nullptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO(),
+ _ftr_other_ptr->getFrame()->getP(),
+ _ftr_other_ptr->getFrame()->getO() )
{
//
}
@@ -44,12 +69,24 @@ class FactorRelative2dAnalytic : public FactorAnalytic
/** \brief Constructor of category FAC_LANDMARK
**/
FactorRelative2dAnalytic(const std::string& _tp,
- const FeatureBasePtr& _ftr_ptr,
- const LandmarkBasePtr& _landmark_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status = FAC_ACTIVE) :
- FactorAnalytic(_tp, nullptr, nullptr, nullptr, _landmark_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO(), _landmark_ptr->getP(), _landmark_ptr->getO())
+ const FeatureBasePtr& _ftr_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE) :
+ FactorAnalytic(_tp,
+ _ftr_ptr,
+ nullptr,
+ nullptr,
+ nullptr,
+ _landmark_other_ptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO(),
+ _landmark_other_ptr->getP(),
+ _landmark_other_ptr->getO())
{
//
}
@@ -113,10 +150,7 @@ inline Eigen::VectorXd FactorRelative2dAnalytic::evaluateResiduals(
expected_measurement(2) = _st_vector[3](0) - _st_vector[1](0);
// Residual
residual = expected_measurement - getMeasurement();
- while (residual(2) > M_PI)
- residual(2) = residual(2) - 2 * M_PI;
- while (residual(2) <= -M_PI)
- residual(2) = residual(2) + 2 * M_PI;
+ residual(2) = pi2pi(residual(2));
residual = getMeasurementSquareRootInformationUpper() * residual;
return residual;
}
diff --git a/include/core/factor/factor_relative_pose_2d_with_extrinsics.h b/include/core/factor/factor_relative_pose_2d_with_extrinsics.h
new file mode 100644
index 0000000000000000000000000000000000000000..704cc3e7bc906a84a6702b6e8fdcad38766562ee
--- /dev/null
+++ b/include/core/factor/factor_relative_pose_2d_with_extrinsics.h
@@ -0,0 +1,130 @@
+#ifndef FACTOR_RELATIVE_POSE_2D_WITH_EXTRINSICS_THETA_H_
+#define FACTOR_RELATIVE_POSE_2D_WITH_EXTRINSICS_THETA_H_
+
+//Wolf includes
+#include "core/factor/factor_autodiff.h"
+#include "core/frame/frame_base.h"
+#include "core/math/rotations.h"
+
+//#include "ceres/jet.h"
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(FactorRelativePose2dWithExtrinsics);
+
+//class
+class FactorRelativePose2dWithExtrinsics : public FactorAutodiff<FactorRelativePose2dWithExtrinsics, 3, 2, 1, 2, 1, 2, 1>
+{
+ public:
+ FactorRelativePose2dWithExtrinsics(const FeatureBasePtr& _ftr_ptr,
+ const FrameBasePtr& _frame_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE) :
+ FactorAutodiff<FactorRelativePose2dWithExtrinsics, 3, 2, 1, 2, 1, 2, 1>("FactorRelativePose2dWithExtrinsics",
+ _ftr_ptr,
+ _frame_other_ptr, nullptr, nullptr, nullptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _frame_other_ptr->getP(),
+ _frame_other_ptr->getO(),
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO(),
+ _ftr_ptr->getCapture()->getSensorP(),
+ _ftr_ptr->getCapture()->getSensorO())
+ {
+ assert(_ftr_ptr->getCapture()->getSensorP() != nullptr && "No extrinsics found!");
+ assert(_ftr_ptr->getCapture()->getSensorO() != nullptr && "No extrinsics found!");
+ //
+ }
+
+ virtual ~FactorRelativePose2dWithExtrinsics() = default;
+
+ virtual std::string getTopology() const override
+ {
+ return std::string("MOTION");
+ }
+
+ template<typename T>
+ bool operator ()(const T* const _p1,
+ const T* const _o1,
+ const T* const _p2,
+ const T* const _o2,
+ const T* const _sp,
+ const T* const _so,
+ T* _residuals) const;
+};
+
+template<typename T>
+inline bool FactorRelativePose2dWithExtrinsics::operator ()(const T* const _p1,
+ const T* const _o1,
+ const T* const _p2,
+ const T* const _o2,
+ const T* const _ps,
+ const T* const _os,
+ T* _residuals) const
+{
+
+ // MAPS
+ Eigen::Map<Eigen::Matrix<T,3,1> > res(_residuals);
+ Eigen::Map<const Eigen::Matrix<T,2,1> > p1(_p1);
+ Eigen::Map<const Eigen::Matrix<T,2,1> > p2(_p2);
+ Eigen::Map<const Eigen::Matrix<T,2,1> > ps(_ps);
+ T o1 = _o1[0];
+ T o2 = _o2[0];
+ T os = _os[0];
+
+ Eigen::Matrix<T, 3, 1> expected_measurement;
+ Eigen::Matrix<T, 3, 1> er; // error
+
+ // Expected measurement
+ // r1_p_r1_s1 = ps
+ // r2_p_r2_s2 = ps
+ // r1_R_s1 = R(os)
+ // r2_R_s2 = R(os)
+ // w_R_r1 = R(o1)
+ // w_R_r2 = R(o2)
+ // ----------------------------------------
+
+ // s1_p_s1_s2 = s1_R_r1*r1_p_s1_r1 +
+ // s1_R_r1*r1_R_w*w_p_r1_w +
+ // s1_R_r1*r1_R_w*w_p_w_r2 +
+ // s1_R_r1*r1_R_w*w_R_r2*r2_p_r2_s2
+
+ // s1_p_s1_s2 = s1_R_r1*(r1_p_s1_r1 + r1_R_w*(w_p_r1_w + w_p_w_r2 + w_R_r2*r2_p_r2_s2))
+
+ expected_measurement.head(2) = Eigen::Rotation2D<T>(-os)*(-ps + Eigen::Rotation2D<T>(-o1)*(-p1 + p2 + Eigen::Rotation2D<T>(o2)*ps));
+ expected_measurement(2) = o2 - o1;
+
+ // Error
+ er = expected_measurement - getMeasurement().cast<T>();
+ er(2) = pi2pi(er(2));
+
+ // Residuals
+ res = getMeasurementSquareRootInformationUpper().cast<T>() * er;
+
+ ////////////////////////////////////////////////////////
+ // print Jacobian. Uncomment this as you wish (remember to uncomment #include "ceres/jet.h" above):
+// using ceres::Jet;
+// Eigen::MatrixXs J(3,6);
+// J.row(0) = ((Jet<Scalar, 6>)(er(0))).v;
+// J.row(1) = ((Jet<Scalar, 6>)(er(1))).v;
+// J.row(2) = ((Jet<Scalar, 6>)(er(2))).v;
+// J.row(0) = ((Jet<Scalar, 6>)(res(0))).v;
+// J.row(1) = ((Jet<Scalar, 6>)(res(1))).v;
+// J.row(2) = ((Jet<Scalar, 6>)(res(2))).v;
+// if (sizeof(er(0)) != sizeof(double))
+// {
+// std::cout << "FactorRelativePose2dWithExtrinsics::Jacobian(c" << id() << ") = \n " << J << std::endl;
+// std::cout << "FactorRelativePose2dWithExtrinsics::Weighted Jacobian(c" << id() << ") = \n " << J << std::endl;
+// std::cout << "FactorRelativePose2dWithExtrinsics::Sqrt Info(c" << id() << ") = \n" << getMeasurementSquareRootInformationUpper() << std::endl;
+// }
+ ////////////////////////////////////////////////////////
+
+ return true;
+}
+
+} // namespace wolf
+
+#endif
diff --git a/include/core/feature/feature_base.h b/include/core/feature/feature_base.h
index a2ba3bc60f5d0db16d6c486099ac5e3e9b3116ec..f412fd3688e114e6116cd17db7de724e0fb7fa85 100644
--- a/include/core/feature/feature_base.h
+++ b/include/core/feature/feature_base.h
@@ -76,9 +76,17 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
*/
double getMeasurement(unsigned int _ii) const;
const Eigen::VectorXd& getMeasurement() const;
+
+ private:
+ /* \brief Set the measurement and its noise
+ *
+ * WARNING: To be used once in the constructor only.
+ */
void setMeasurement(const Eigen::VectorXd& _meas);
void setMeasurementCovariance(const Eigen::MatrixXd & _meas_cov);
void setMeasurementInformation(const Eigen::MatrixXd & _meas_info);
+
+ public:
const Eigen::MatrixXd& getMeasurementCovariance() const;
Eigen::MatrixXd getMeasurementInformation() const;
const Eigen::MatrixXd& getMeasurementSquareRootInformationUpper() const;
@@ -106,6 +114,21 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
template<typename classType, typename... T>
static std::shared_ptr<classType> emplace(CaptureBasePtr _cpt_ptr, T&&... all);
+ virtual void printHeader(int depth, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream ,
+ std::string _tabs = "") const;
+ void print(int depth, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream = std::cout,
+ std::string _tabs = "") const;
+
+ virtual CheckLog localCheck(bool _verbose, FeatureBasePtr _feature_ptr, std::ostream& _stream, std::string _tabs = "") const;
+ bool check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
private:
void setCapture(CaptureBasePtr _cap_ptr){capture_ptr_ = _cap_ptr;}
@@ -176,11 +199,6 @@ inline const Eigen::MatrixXd& FeatureBase::getMeasurementSquareRootInformationUp
return measurement_sqrt_information_upper_;
}
-inline void FeatureBase::setMeasurement(const Eigen::VectorXd& _meas)
-{
- measurement_ = _meas;
-}
-
inline const Eigen::VectorXd& FeatureBase::getExpectation() const
{
return expectation_;
diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h
index eea8873b3e804743f4cd224bffd6442d4c17febf..6c73ad933f4b305fca0c71af6db5a47792cab574 100644
--- a/include/core/frame/frame_base.h
+++ b/include/core/frame/frame_base.h
@@ -48,16 +48,6 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
TimeStamp time_stamp_; ///< frame time stamp
public:
- /** \brief Constructor of non-key Frame with only time stamp
- *
- * Constructor with only time stamp
- * \param _ts is the time stamp associated to this frame, provided in seconds
- * \param _p_ptr StateBlock pointer to the position (default: nullptr)
- * \param _o_ptr StateBlock pointer to the orientation (default: nullptr). Pass a StateQuaternion if needed.
- * \param _v_ptr StateBlock pointer to the velocity (default: nullptr).
- **/
- FrameBase(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr = nullptr, StateBlockPtr _v_ptr = nullptr);
-
/** \brief Constructor with type, time stamp and state pointer
*
* Constructor with type, time stamp and state pointer
@@ -67,9 +57,27 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
* \param _o_ptr StateBlock pointer to the orientation (default: nullptr)
* \param _v_ptr StateBlock pointer to the velocity (default: nullptr).
**/
- FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr = nullptr, StateBlockPtr _v_ptr = nullptr);
-
- FrameBase(const std::string _frame_structure, const SizeEigen _dim, const FrameType & _tp, const TimeStamp& _ts, const Eigen::VectorXd& _x);
+ FrameBase(const FrameType & _tp,
+ const TimeStamp& _ts,
+ StateBlockPtr _p_ptr,
+ StateBlockPtr _o_ptr = nullptr,
+ StateBlockPtr _v_ptr = nullptr);
+
+ FrameBase(const FrameType & _tp,
+ const TimeStamp& _ts,
+ const std::string _frame_structure,
+ const SizeEigen _dim,
+ const Eigen::VectorXd& _x);
+
+ FrameBase(const FrameType & _tp,
+ const TimeStamp& _ts,
+ const std::string _frame_structure,
+ const VectorComposite& _state);
+
+ FrameBase(const FrameType & _tp,
+ const TimeStamp& _ts,
+ const std::string _frame_structure,
+ const std::list<VectorXd>& _vectors);
virtual ~FrameBase();
virtual void remove(bool viral_remove_empty_parent=false);
@@ -131,6 +139,23 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
template<typename classType, typename... T>
static std::shared_ptr<classType> emplace(TrajectoryBasePtr _ptr, T&&... all);
+ virtual void printHeader(int depth, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream ,
+ std::string _tabs = "") const;
+
+ void print(int depth, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream = std::cout,
+ std::string _tabs = "") const;
+
+ virtual CheckLog localCheck(bool _verbose, FrameBasePtr _frm_ptr, std::ostream& _stream, std::string _tabs = "") const;
+ bool check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
+
private:
CaptureBasePtr addCapture(CaptureBasePtr _capt_ptr);
diff --git a/include/core/hardware/hardware_base.h b/include/core/hardware/hardware_base.h
index a309e7a03c8a5e27f459b772ce6bff5b0994f714..fba575b0b8ea55a15b8bbe4972d0bc12984ae62b 100644
--- a/include/core/hardware/hardware_base.h
+++ b/include/core/hardware/hardware_base.h
@@ -27,6 +27,22 @@ class HardwareBase : public NodeBase, public std::enable_shared_from_this<Hardwa
const SensorBasePtrList& getSensorList() const;
+ virtual void printHeader(int depth, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream ,
+ std::string _tabs = "") const;
+ void print(int depth, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream = std::cout,
+ std::string _tabs = "") const;
+
+ virtual CheckLog localCheck(bool _verbose, HardwareBasePtr _hwd_ptr, std::ostream& _stream, std::string _tabs = "") const;
+ bool check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
+
private:
virtual SensorBasePtr addSensor(SensorBasePtr _sensor_ptr);
};
diff --git a/include/core/landmark/landmark_base.h b/include/core/landmark/landmark_base.h
index 254e03e480f8471029c38c8f59626b230dc962f9..3d177d79c972b1e072fb86d5d7790e686cd387ff 100644
--- a/include/core/landmark/landmark_base.h
+++ b/include/core/landmark/landmark_base.h
@@ -85,6 +85,23 @@ class LandmarkBase : public NodeBase, public HasStateBlocks, public std::enable_
*/
static LandmarkBasePtr create(const YAML::Node& _node);
+ virtual void printHeader(int depth, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream ,
+ std::string _tabs = "") const;
+
+ void print(int depth, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream = std::cout,
+ std::string _tabs = "") const;
+
+ virtual CheckLog localCheck(bool _verbose, LandmarkBasePtr _lmk_ptr, std::ostream& _stream, std::string _tabs = "") const;
+ bool check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
+
private:
void setMap(const MapBasePtr _map_ptr);
diff --git a/include/core/map/map_base.h b/include/core/map/map_base.h
index 6d09191e54619d5a00fce39e1ecf002aace66d3a..e609298014d8b68ca9e686a877ea80b041b34214 100644
--- a/include/core/map/map_base.h
+++ b/include/core/map/map_base.h
@@ -38,6 +38,21 @@ class MapBase : public NodeBase, public std::enable_shared_from_this<MapBase>
void load(const std::string& _map_file_yaml);
void save(const std::string& _map_file_yaml, const std::string& _map_name = "Map automatically saved by Wolf");
+ virtual void printHeader(int depth, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream ,
+ std::string _tabs = "") const;
+ void print(int depth, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream = std::cout,
+ std::string _tabs = "") const;
+
+ virtual CheckLog localCheck(bool _verbose, MapBasePtr _map_ptr, std::ostream& _stream, std::string _tabs = "") const;
+ bool check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
private:
std::string dateTimeNow();
};
diff --git a/include/core/math/SE2.h b/include/core/math/SE2.h
index 00f570faed8c0542a1e0312a5ad1bc7c2ab710e3..4ac32d2ded2923f212937cfa88a0994fb0dca5b4 100644
--- a/include/core/math/SE2.h
+++ b/include/core/math/SE2.h
@@ -10,6 +10,7 @@
#include "core/common/wolf.h"
#include "core/math/rotations.h"
+#include "core/state_block/state_composite.h"
#include <Eigen/Geometry>
#include <Eigen/Dense>
@@ -25,17 +26,26 @@ namespace wolf
namespace SO2{
template<typename T>
-Eigen::Matrix<T, 2, 2> exp(const T theta)
+inline Eigen::Matrix<T, 2, 2> exp(const T theta)
{
return Eigen::Rotation2D<T>(theta).matrix();
}
+
} // namespace SO2
namespace SE2{
+/** \brief Compute [1]_x * t = (-t.y ; t.x)
+ */
+template<class D>
+inline Eigen::Matrix<typename D::Scalar, 2, 1> skewed(const MatrixBase<D>& t)
+{
+ return Eigen::Matrix<typename D::Scalar, 2, 1>(-t(1), t(0));
+}
+
template<typename T>
-Eigen::Matrix2d V_helper(const T theta)
+inline Eigen::Matrix2d V_helper(const T theta)
{
T s; // sin(theta) / theta
T c_1; // (1-cos(theta)) / theta
@@ -56,8 +66,17 @@ Eigen::Matrix2d V_helper(const T theta)
return (Matrix<T, 2, 2>() << s, -c_1, c_1, s).finished();
}
+inline VectorComposite identity()
+{
+ VectorComposite v;
+ v["P"] = Vector2d::Zero();
+ v["O"] = Vector1d::Zero();
+
+ return v;
+}
+
template<class D1, class D2>
-void exp(const Eigen::MatrixBase<D1>& _tau, Eigen::MatrixBase<D2>& _delta)
+inline void exp(const Eigen::MatrixBase<D1>& _tau, Eigen::MatrixBase<D2>& _delta)
{
MatrixSizeCheck<3, 1>::check(_tau);
MatrixSizeCheck<3, 1>::check(_delta);
@@ -68,7 +87,7 @@ void exp(const Eigen::MatrixBase<D1>& _tau, Eigen::MatrixBase<D2>& _delta)
}
template<class D, typename T>
-Eigen::Matrix<T, 2, 1> J_Vp_theta(const Eigen::MatrixBase<D>& p, const T theta)
+inline Eigen::Matrix<T, 2, 1> J_Vp_theta(const Eigen::MatrixBase<D>& p, const T theta)
{
MatrixSizeCheck<2, 1>::check (p);
@@ -95,7 +114,7 @@ Eigen::Matrix<T, 2, 1> J_Vp_theta(const Eigen::MatrixBase<D>& p, const T theta)
}
template<class D1, class D2, class D3>
-void exp(const MatrixBase<D1>& _tau, MatrixBase<D2>& _delta, MatrixBase<D3>& _J_delta_tau)
+inline void exp(const MatrixBase<D1>& _tau, MatrixBase<D2>& _delta, MatrixBase<D3>& _J_delta_tau)
{
MatrixSizeCheck<3, 1>::check(_tau);
MatrixSizeCheck<3, 1>::check(_delta);
@@ -116,6 +135,254 @@ void exp(const MatrixBase<D1>& _tau, MatrixBase<D2>& _delta, MatrixBase<D3>& _J_
_J_delta_tau << V, J_Vp_theta(_tau.template head<2>(), theta), 0.0, 0.0, 1.0;
}
+inline void exp(const VectorComposite& _tau, VectorComposite& _delta)
+{
+ // [1] eq. 156
+ const auto &p = _tau.at("P");
+ const auto &theta = pi2pi(_tau.at("O")(0));
+ Matrix2d V = V_helper(theta);
+
+ _delta["P"] = V * p;
+ _delta["O"] = Matrix1d(theta);
+}
+
+inline VectorComposite exp(const VectorComposite& tau)
+{
+ VectorComposite res("PO", {2,1});
+
+ exp(tau, res);
+
+ return res;
+}
+
+inline void exp(const VectorComposite& _tau, VectorComposite& _delta, MatrixComposite& _J_delta_tau)
+{
+ // [1] eq. 156
+ const auto &p = _tau.at("P");
+ const auto &theta = pi2pi(_tau.at("O")(0));
+ Matrix2d V = V_helper(theta);
+
+ _delta["P"] = V * p;
+ _delta["O"] = Matrix1d(theta);
+
+ // Jacobian follows the composite definition in [1] eq. 89, with jacobian blocks:
+ // J_Vp_p = V: see V_helper, eq. 158
+ // J_Vp_theta: see fcn helper
+ // J_theta_theta = 1; eq. 126
+ _J_delta_tau.clear();
+ _J_delta_tau.emplace("P", "P", V);
+ _J_delta_tau.emplace("P", "O", J_Vp_theta(p, theta));
+ _J_delta_tau.emplace("O", "P", RowVector2d(0.0, 0.0));
+ _J_delta_tau.emplace("O", "O", Matrix1d(1));
+}
+
+template<class D1, class D2, class D3>
+inline void compose(const MatrixBase<D1> &_delta1, MatrixBase<D2> &_delta2, MatrixBase<D3> &_delta1_compose_delta2)
+{
+ MatrixSizeCheck<3, 1>::check(_delta1);
+ MatrixSizeCheck<3, 1>::check(_delta2);
+ MatrixSizeCheck<3, 1>::check(_delta1_compose_delta2);
+
+ _delta1_compose_delta2.template head<2>() = _delta1.template head<2>()
+ + SO2::exp(_delta1(2)) * _delta2.template head<2>();
+ _delta1_compose_delta2(2) = pi2pi(_delta1(2) + _delta2(2));
+}
+
+template<class D1, class D2, class D3, class D4, class D5>
+inline void compose(const MatrixBase<D1>& _delta1,
+ const MatrixBase<D2>& _delta2,
+ MatrixBase<D3>& _delta1_compose_delta2,
+ MatrixBase<D4>& _J_compose_delta1,
+ MatrixBase<D5>& _J_compose_delta2)
+{
+ MatrixSizeCheck<3, 1>::check(_delta1);
+ MatrixSizeCheck<3, 1>::check(_delta2);
+ MatrixSizeCheck<3, 1>::check(_delta1_compose_delta2);
+ MatrixSizeCheck<3, 3>::check(_J_compose_delta1);
+ MatrixSizeCheck<3, 3>::check(_J_compose_delta2);
+
+ Matrix2d R1 = SO2::exp(_delta1(2)); // need temporary
+
+ // tc = t1 + R1 t2
+ _delta1_compose_delta2.template head<2>() = _delta1.template head<2>() + R1 * _delta2.template head<2>();
+
+ // Rc = R1 R2 --> theta = theta1 + theta2
+ _delta1_compose_delta2(2) = pi2pi(_delta1(2) + _delta2(2));
+
+ /* Jacobians in composite form [1] see eqs. (89, 125, 129, 130)
+ *
+ * resulting delta is:
+ *
+ * tc = t1 + R1 t2 (*)
+ * Rc = R1 R2 (**)
+ *
+ * Jacobians have the form:
+ *
+ * [ J_t_t J_t_R ]
+ * [ J_r_t J_R_R ]
+ *
+ * Jacobian blocks are:
+ *
+ * J_tc_t1 = I trivial from (*)
+ * J_tc_R1 = R1 [1]x t2 = R1 (-t2.y ; t2.x) see [1]: eq. (129)
+ *
+ * J_Rc_t1 = (0 0) trivial from (**)
+ * J_Rc_R1 = 1 see [1]: eq. (125)
+ *
+ * J_tc_t2 = R1 see [1]: eq. (130)
+ * J_tc_R2 = 0 trivial from (*)
+ *
+ * J_Rc_t2 = 0 trivial from (**)
+ * J_Rc_R2 = 1 see [1]: eq. (125)
+ */
+
+ _J_compose_delta1.setIdentity();
+ _J_compose_delta1.template block<2, 1>(0, 2) = R1 * skewed(_delta2.template head<2>());
+
+ _J_compose_delta2.setIdentity(3, 3);
+ _J_compose_delta2.template block<2, 2>(0, 0) = R1;
+}
+
+inline void compose(const VectorComposite& _x1,
+ const VectorComposite& _x2,
+ VectorComposite& _c)
+{
+ const auto& p1 = _x1.at("P");
+ const auto& a1 = _x1.at("O")(0); // angle
+ const auto& R1 = SO2::exp(a1);
+
+ const auto& p2 = _x2.at("P");
+ const auto& a2 = _x2.at("O")(0); // angle
+
+ // tc = t1 + R1 t2
+ _c["P"] = p1 + R1 * p2;
+
+ // Rc = R1 R2 --> theta = theta1 + theta2
+ _c["O"] = Matrix1d( pi2pi(a1 + a2) ) ;
+}
+
+inline void compose(const VectorComposite& _x1,
+ const VectorComposite& _x2,
+ VectorComposite& _c,
+ MatrixComposite& _J_c_x1,
+ MatrixComposite& _J_c_x2)
+{
+ const auto& p1 = _x1.at("P");
+ const auto& a1 = _x1.at("O")(0); // angle
+ Matrix2d R1 = SO2::exp(a1); // need temporary
+
+ const auto& p2 = _x2.at("P");
+ const auto& a2 = _x2.at("O")(0); // angle
+
+ /* Jacobians in composite form [1] see eqs. (89, 125, 129, 130)
+ *
+ * resulting delta is:
+ *
+ * tc = t1 + R1 t2 (*)
+ * Rc = R1 R2 (**)
+ *
+ * Jacobians have the form:
+ *
+ * [ J_t_t J_t_R ]
+ * [ J_r_t J_R_R ]
+ *
+ * Jacobian blocks are:
+ *
+ * J_tc_t1 = I trivial from (*)
+ * J_tc_R1 = R1 [1]x t2 = R1 (-t2.y ; t2.x) see [1]: eq. (129)
+ *
+ * J_Rc_t1 = (0 0) trivial from (**)
+ * J_Rc_R1 = 1 see [1]: eq. (125)
+ *
+ * J_tc_t2 = R1 see [1]: eq. (130)
+ * J_tc_R2 = 0 trivial from (*)
+ *
+ * J_Rc_t2 = 0 trivial from (**)
+ * J_Rc_R2 = 1 see [1]: eq. (125)
+ */
+
+ _J_c_x1.clear();
+ _J_c_x1.emplace("P", "P", Matrix2d::Identity());
+ _J_c_x1.emplace("P", "O", MatrixXd(R1 * skewed(p2)) );
+ _J_c_x1.emplace("O", "P", RowVector2d(0,0));
+ _J_c_x1.emplace("O", "O", Matrix1d(1));
+
+ _J_c_x2.clear();
+ _J_c_x2.emplace("P", "P", R1);
+ _J_c_x2.emplace("P", "O", Vector2d(0,0));
+ _J_c_x2.emplace("O", "P", RowVector2d(0,0));
+ _J_c_x2.emplace("O", "O", Matrix1d(1));
+
+
+ // Actually compose the vectors here. This avoids aliasing in case this is called with 'c' coinciding with 'x1' or 'x2'.
+
+ // tc = t1 + R1 t2
+ _c["P"] = p1 + R1 * p2;
+
+ // Rc = R1 R2 --> theta = theta1 + theta2
+ _c["O"] = Matrix1d( pi2pi(a1 + a2) ) ;
+
+}
+
+template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
+inline void plus(const MatrixBase<D1>& p1, const MatrixBase<D2>& q1,
+ const MatrixBase<D4>& p2, const MatrixBase<D5>& o2,
+ MatrixBase<D7>& plus_p, MatrixBase<D8>& plus_q)
+{
+ MatrixSizeCheck<2, 1>::check(p1);
+ MatrixSizeCheck<1, 1>::check(q1);
+ MatrixSizeCheck<2, 1>::check(p2);
+ MatrixSizeCheck<1, 1>::check(o2);
+ MatrixSizeCheck<2, 1>::check(plus_p);
+ MatrixSizeCheck<1, 1>::check(plus_q);
+
+ plus_p = p1 + p2;
+ plus_q(0) = pi2pi(q1(0) + o2(0));
+}
+
+template<typename D1, typename D2, typename D3>
+inline void plus(const MatrixBase<D1>& d1,
+ const MatrixBase<D2>& d2,
+ MatrixBase<D3>& d_plus)
+{
+ Map<const Matrix<typename D1::Scalar, 2, 1> > p1 ( & d1(0) );
+ Map<const Matrix<typename D1::Scalar, 1, 1> > q1 ( & d1(2) );
+ Map<const Matrix<typename D2::Scalar, 2, 1> > p2 ( & d2(0) );
+ Map<const Matrix<typename D2::Scalar, 1, 1> > o2 ( & d2(2) );
+ Map<Matrix<typename D3::Scalar, 2, 1> > p_p ( & d_plus(0) );
+ Map<Matrix<typename D1::Scalar, 1, 1> > q_p ( & d_plus(2) );
+
+ plus(p1, q1, p2, o2, p_p, q_p);
+}
+
+template<typename D1, typename D2>
+inline Matrix<typename D1::Scalar, 3, 1> plus(const MatrixBase<D1>& d1,
+ const MatrixBase<D2>& d2)
+{
+ Matrix<typename D1::Scalar, 3, 1> d_plus;
+ plus(d1, d2, d_plus);
+ return d_plus;
+}
+
+inline void plus(const VectorComposite& x, const VectorComposite& tau, VectorComposite& res)
+{
+ plus(x.at("P"), x.at("O"), tau.at("P"), tau.at("O"), res.at("P"), res.at("O"));
+}
+
+inline VectorComposite plus(const VectorComposite& x, const VectorComposite& tau)
+{
+ VectorComposite res;
+
+ res["P"] = Vector2d();
+ res["O"] = Vector1d();
+
+ plus(x, tau, res);
+
+ return res;
+}
+
+
+
} // namespace SE2
} // namespacs wolf
diff --git a/include/core/math/SE3.h b/include/core/math/SE3.h
index 701c5bab7a5ec5ecc6401def52ad9ca6d0f26856..4b000889c59320e8c7e819725c415dd19308d077 100644
--- a/include/core/math/SE3.h
+++ b/include/core/math/SE3.h
@@ -10,6 +10,7 @@
#include "core/common/wolf.h"
#include "core/math/rotations.h"
+#include "core/state_block/state_composite.h"
/*
* The functions in this file are related to manipulations of Delta motion magnitudes used in 3d motion.
@@ -26,11 +27,17 @@
* - identity: I = Delta at the origin, with Dp = [0,0,0]; Dq = [0,0,0,1], so that D (+) I = I (+) D = D
* - inverse: so that D (+) D.inv = D.inv (+) D = I
* - between: Db = D2 (-) D1 := D1.inv (+) D2, so that D2 = D1 (+) Db
- * - log_SE3: go from Delta manifold to tangent space (equivalent to log() in rotations)
- * - exp_SE3: go from tangent space to delta manifold (equivalent to exp() in rotations)
- * - plus: D2 = D1 * exp_SE3(d)
- * - minus: d = log_SE3( D1.inv() * D2 )
+ * - log: go from Delta manifold to tangent space (equivalent to log() in rotations)
+ * - exp: go from tangent space to delta manifold (equivalent to exp() in rotations)
+ * - plus: D2 = D1 * exp(d)
+ * - minus: d = log( D1.inv() * D2 )
* - interpolate: dd = D1 * exp ( log( D1.inv() * D2 ) * t ) = D1 (+) ( (D2 (-) D1) * t)
+ *
+ * ------------------------------------------------------------------------------------------
+ *
+ * Some of the functions below are based on:
+ *
+ * [1] J. Sola et. al. "A micro Lie theory for state estimation in robotics", tech rep. IRI 2018, https://arxiv.org/pdf/1812.01537.pdf
*/
namespace wolf
@@ -57,6 +64,13 @@ inline void identity(MatrixBase<D1>& p, MatrixBase<D2>& q)
q << T2(0), T2(0), T2(0), T2(1);
}
+inline void identity(VectorComposite& v)
+{
+ v.clear();
+ v["P"] = Vector3d::Zero();
+ v["O"] = Quaterniond::Identity().coeffs();
+}
+
template<typename T = double>
inline Matrix<T, 7, 1> identity()
{
@@ -67,14 +81,14 @@ inline Matrix<T, 7, 1> identity()
}
template<typename D1, typename D2, typename D4, typename D5>
-inline void inverse(const MatrixBase<D1>& dp, const QuaternionBase<D2>& dq,
- MatrixBase<D4>& idp, QuaternionBase<D5>& idq)
+inline void inverse(const MatrixBase<D1>& p, const QuaternionBase<D2>& q,
+ MatrixBase<D4>& ip, QuaternionBase<D5>& iq)
{
- MatrixSizeCheck<3, 1>::check(dp);
- MatrixSizeCheck<3, 1>::check(idp);
+ MatrixSizeCheck<3, 1>::check(p);
+ MatrixSizeCheck<3, 1>::check(ip);
- idp = - dq.conjugate() * dp ;
- idq = dq.conjugate() ;
+ ip = - q.conjugate() * p ;
+ iq = q.conjugate() ;
}
template<typename D1, typename D2>
@@ -84,12 +98,12 @@ inline void inverse(const MatrixBase<D1>& d,
MatrixSizeCheck<7, 1>::check(d);
MatrixSizeCheck<7, 1>::check(id);
- Map<const Matrix<typename D1::Scalar, 3, 1> > dp ( & d( 0 ) );
- Map<const Quaternion<typename D1::Scalar> > dq ( & d( 3 ) );
- Map<Matrix<typename D2::Scalar, 3, 1> > idp ( & id( 0 ) );
- Map<Quaternion<typename D2::Scalar> > idq ( & id( 3 ) );
+ Map<const Matrix<typename D1::Scalar, 3, 1> > p ( & d( 0 ) );
+ Map<const Quaternion<typename D1::Scalar> > q ( & d( 3 ) );
+ Map<Matrix<typename D2::Scalar, 3, 1> > ip ( & id( 0 ) );
+ Map<Quaternion<typename D2::Scalar> > iq ( & id( 3 ) );
- inverse(dp, dq, idp, idq);
+ inverse(p, q, ip, iq);
}
template<typename D>
@@ -101,16 +115,35 @@ inline Matrix<typename D::Scalar, 7, 1> inverse(const MatrixBase<D>& d)
}
template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
-inline void compose(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1,
- const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2,
- MatrixBase<D7>& sum_p, QuaternionBase<D8>& sum_q )
+inline void compose(const MatrixBase<D1>& p1, const QuaternionBase<D2>& q1,
+ const MatrixBase<D4>& p2, const QuaternionBase<D5>& q2,
+ MatrixBase<D7>& pc, QuaternionBase<D8>& qc )
{
- MatrixSizeCheck<3, 1>::check(dp1);
- MatrixSizeCheck<3, 1>::check(dp2);
- MatrixSizeCheck<3, 1>::check(sum_p);
+ MatrixSizeCheck<3, 1>::check(p1);
+ MatrixSizeCheck<3, 1>::check(p2);
+ MatrixSizeCheck<3, 1>::check(pc);
- sum_p = dp1 + dq1*dp2;
- sum_q = dq1*dq2; // dq here to avoid possible aliasing between d1 and sum
+ pc = p1 + q1 * p2;
+ qc = q1 * q2; // q here to avoid possible aliasing between d1 and sum
+}
+
+template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
+inline void compose(const MatrixBase<D1>& p1, const MatrixBase<D2>& q1,
+ const MatrixBase<D4>& p2, const MatrixBase<D5>& q2,
+ MatrixBase<D7>& pc, MatrixBase<D8>& qc )
+{
+ MatrixSizeCheck<3, 1>::check(p1);
+ MatrixSizeCheck<4, 1>::check(q1);
+ MatrixSizeCheck<3, 1>::check(p2);
+ MatrixSizeCheck<4, 1>::check(q2);
+ MatrixSizeCheck<3, 1>::check(pc);
+ MatrixSizeCheck<4, 1>::check(qc);
+
+ Map<const Quaternion<typename D2::Scalar> > mq1 ( & q1( 0 ) );
+ Map<const Quaternion<typename D5::Scalar> > mq2 ( & q2( 0 ) );
+ Map< Quaternion<typename D8::Scalar> > mqc ( & qc( 0 ) );
+
+ compose(p1, mq1, p2, mq2, pc, mqc);
}
template<typename D1, typename D2, typename D3>
@@ -122,14 +155,14 @@ inline void compose(const MatrixBase<D1>& d1,
MatrixSizeCheck<7, 1>::check(d2);
MatrixSizeCheck<7, 1>::check(sum);
- Map<const Matrix<typename D1::Scalar, 3, 1> > dp1 ( & d1( 0 ) );
- Map<const Quaternion<typename D1::Scalar> > dq1 ( & d1( 3 ) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > dp2 ( & d2( 0 ) );
- Map<const Quaternion<typename D2::Scalar> > dq2 ( & d2( 3 ) );
- Map<Matrix<typename D3::Scalar, 3, 1> > sum_p ( & sum( 0 ) );
- Map<Quaternion<typename D3::Scalar> > sum_q ( & sum( 3 ) );
+ Map<const Matrix<typename D1::Scalar, 3, 1> > p1 ( & d1( 0 ) );
+ Map<const Quaternion<typename D1::Scalar> > q1 ( & d1( 3 ) );
+ Map<const Matrix<typename D2::Scalar, 3, 1> > p2 ( & d2( 0 ) );
+ Map<const Quaternion<typename D2::Scalar> > q2 ( & d2( 3 ) );
+ Map<Matrix<typename D3::Scalar, 3, 1> > pc ( & sum( 0 ) );
+ Map<Quaternion<typename D3::Scalar> > qc ( & sum( 3 ) );
- compose(dp1, dq1, dp2, dq2, sum_p, sum_q);
+ compose(p1, q1, p2, q2, pc, qc);
}
template<typename D1, typename D2>
@@ -155,8 +188,8 @@ inline void compose(const MatrixBase<D1>& d1,
MatrixSizeCheck< 6, 6>::check(J_sum_d2);
// Some useful temporaries
- Matrix<typename D1::Scalar, 3, 3> dR1 = q2R(d1.segment(3,4)); //dq1.matrix(); // First Delta, DR
- Matrix<typename D2::Scalar, 3, 3> dR2 = q2R(d2.segment(3,4)); //dq2.matrix(); // Second delta, dR
+ Matrix<typename D1::Scalar, 3, 3> dR1 = q2R(d1.segment(3,4)); //q1.matrix(); // First Delta, DR
+ Matrix<typename D2::Scalar, 3, 3> dR2 = q2R(d2.segment(3,4)); //q2.matrix(); // Second delta, dR
// Jac wrt first delta
J_sum_d1.setIdentity(); // dDp'/dDp = dDv'/dDv = I
@@ -165,43 +198,75 @@ inline void compose(const MatrixBase<D1>& d1,
// Jac wrt second delta
J_sum_d2.setIdentity(); //
- J_sum_d2.block(0,0,3,3) = dR1; // dDp'/ddp
+ J_sum_d2.block(0,0,3,3) = dR1; // dDp'/p
// J_sum_d2.block(3,3,3,3) = Matrix3d::Identity(); // dDo'/ddo = I
// compose deltas -- done here to avoid aliasing when calling with input `d1` and result `sum` referencing the same variable
compose(d1, d2, sum);
}
+inline void compose(const VectorComposite& x1, const VectorComposite& x2, VectorComposite& c)
+{
+ compose(x1.at("P"), x1.at("O"), x2.at("P"), x2.at("O"), c["P"], c["O"]);
+}
+
+inline void compose(const VectorComposite& x1,
+ const VectorComposite& x2,
+ VectorComposite& c,
+ MatrixComposite& J_c_x1,
+ MatrixComposite& J_c_x2)
+{
+
+ // Some useful temporaries
+ const auto R1 = q2R(x1.at("O")); //q1.matrix(); // make temporary
+ const auto& R2 = q2R(x2.at("O")); //q2.matrix(); // do not make temporary, only reference
+
+ // Jac wrt first delta
+ J_c_x1.emplace("P", "P", Matrix3d::Identity() ) ;
+ J_c_x1.emplace("P", "O", - R1 * skew(x2.at("P")) ) ;
+ J_c_x1.emplace("O", "P", Matrix3d::Zero() ) ;
+ J_c_x1.emplace("O", "O", R2.transpose() ) ;
+
+ // Jac wrt second delta
+ J_c_x2.emplace("P", "P", R1 );
+ J_c_x2.emplace("P", "O", Matrix3d::Zero() );
+ J_c_x2.emplace("O", "P", Matrix3d::Zero() );
+ J_c_x2.emplace("O", "O", Matrix3d::Identity() );
+
+ // compose deltas -- done here to avoid aliasing when calling with input `x1` and result `c` referencing the same variable
+ compose(x1, x2, c);
+}
+
template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
-inline void between(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1,
- const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2,
- MatrixBase<D7>& dp12, QuaternionBase<D8>& dq12)
+inline void between(const MatrixBase<D1>& p1, const QuaternionBase<D2>& q1,
+ const MatrixBase<D4>& p2, const QuaternionBase<D5>& q2,
+ MatrixBase<D7>& p12, QuaternionBase<D8>& q12)
{
- MatrixSizeCheck<3, 1>::check(dp1);
- MatrixSizeCheck<3, 1>::check(dp2);
- MatrixSizeCheck<3, 1>::check(dp12);
+ MatrixSizeCheck<3, 1>::check(p1);
+ MatrixSizeCheck<3, 1>::check(p2);
+ MatrixSizeCheck<3, 1>::check(p12);
- dp12 = dq1.conjugate() * ( dp2 - dp1 );
- dq12 = dq1.conjugate() * dq2;
+ p12 = q1.conjugate() * ( p2 - p1 );
+ q12 = q1.conjugate() * q2;
}
template<typename D1, typename D2, typename D3>
inline void between(const MatrixBase<D1>& d1,
const MatrixBase<D2>& d2,
- MatrixBase<D3>& d2_minus_d1)
+ MatrixBase<D3>& d12)
{
MatrixSizeCheck<7, 1>::check(d1);
MatrixSizeCheck<7, 1>::check(d2);
- MatrixSizeCheck<7, 1>::check(d2_minus_d1);
+ MatrixSizeCheck<7, 1>::check(d12);
- Map<const Matrix<typename D1::Scalar, 3, 1> > dp1 ( & d1(0) );
- Map<const Quaternion<typename D1::Scalar> > dq1 ( & d1(3) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > dp2 ( & d2(0) );
- Map<const Quaternion<typename D2::Scalar> > dq2 ( & d2(3) );
- Map<Matrix<typename D3::Scalar, 3, 1> > dp12 ( & d2_minus_d1(0) );
- Map<Quaternion<typename D3::Scalar> > dq12 ( & d2_minus_d1(3) );
+ Map<const Matrix<typename D1::Scalar, 3, 1> > p1 ( & d1(0) );
+ Map<const Quaternion<typename D1::Scalar> > q1 ( & d1(3) );
+ Map<const Matrix<typename D2::Scalar, 3, 1> > p2 ( & d2(0) );
+ Map<const Quaternion<typename D2::Scalar> > q2 ( & d2(3) );
+ Map<Matrix<typename D3::Scalar, 3, 1> > p12 ( & d12(0) );
+ Map<Quaternion<typename D3::Scalar> > q12 ( & d12(3) );
- between(dp1, dq1, dp2, dq2, dp12, dq12);
+ between(p1, q1, p2, q2, p12, q12);
}
template<typename D1, typename D2>
@@ -216,28 +281,28 @@ inline Matrix<typename D1::Scalar, 7, 1> between(const MatrixBase<D1>& d1,
}
template<typename Derived>
-Matrix<typename Derived::Scalar, 6, 1> log_SE3(const MatrixBase<Derived>& delta_in)
+inline Matrix<typename Derived::Scalar, 6, 1> log(const MatrixBase<Derived>& delta_in)
{
MatrixSizeCheck<7, 1>::check(delta_in);
Matrix<typename Derived::Scalar, 6, 1> ret;
- Map<const Matrix<typename Derived::Scalar, 3, 1> > dp_in ( & delta_in(0) );
- Map<const Quaternion<typename Derived::Scalar> > dq_in ( & delta_in(3) );
- Map<Matrix<typename Derived::Scalar, 3, 1> > dp_ret ( & ret(0) );
+ Map<const Matrix<typename Derived::Scalar, 3, 1> > p_in ( & delta_in(0) );
+ Map<const Quaternion<typename Derived::Scalar> > q_in ( & delta_in(3) );
+ Map<Matrix<typename Derived::Scalar, 3, 1> > p_ret ( & ret(0) );
Map<Matrix<typename Derived::Scalar, 3, 1> > do_ret ( & ret(3) );
Matrix<typename Derived::Scalar, 3, 3> V_inv;
- do_ret = log_q(dq_in);
+ do_ret = log_q(q_in);
V_inv = jac_SO3_left_inv(do_ret);
- dp_ret = V_inv * dp_in;
+ p_ret = V_inv * p_in;
return ret;
}
template<typename Derived>
-Matrix<typename Derived::Scalar, 7, 1> exp_SE3(const MatrixBase<Derived>& d_in)
+inline Matrix<typename Derived::Scalar, 7, 1> exp(const MatrixBase<Derived>& d_in)
{
MatrixSizeCheck<6, 1>::check(d_in);
@@ -247,27 +312,58 @@ Matrix<typename Derived::Scalar, 7, 1> exp_SE3(const MatrixBase<Derived>& d_in)
V = jac_SO3_left(d_in.template tail<3>());
- Map<const Matrix<typename Derived::Scalar, 3, 1> > dp_in ( & d_in(0) );
- Map<const Matrix<typename Derived::Scalar, 3, 1> > do_in ( & d_in(3) );
- Map<Matrix<typename Derived::Scalar, 3, 1> > dp ( & ret(0) );
- Map<Quaternion<typename Derived::Scalar> > dq ( & ret(3) );
+ Map<const Matrix<typename Derived::Scalar, 3, 1> > p_in ( & d_in(0) );
+ Map<const Matrix<typename Derived::Scalar, 3, 1> > o_in ( & d_in(3) );
+ Map<Matrix<typename Derived::Scalar, 3, 1> > p ( & ret(0) );
+ Map<Quaternion<typename Derived::Scalar> > q ( & ret(3) );
- dp = V * dp_in;
- dq = exp_q(do_in);
+ p = V * p_in;
+ q = exp_q(o_in);
return ret;
}
+inline VectorComposite exp(const VectorComposite& tau)
+{
+ const auto& p = tau.at("P");
+ const auto& theta = tau.at("O");
+
+ Matrix3d V = jac_SO3_left(theta); // see [1] eqs. (174) and (145)
+
+ VectorComposite res;
+
+ res["P"] = V * p ;
+ res["O"] = exp_q(theta).coeffs() ;
+
+ return res;
+}
+
template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
-inline void plus(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1,
- const MatrixBase<D4>& dp2, const MatrixBase<D5>& do2,
+inline void plus(const MatrixBase<D1>& p1, const QuaternionBase<D2>& q1,
+ const MatrixBase<D4>& p2, const MatrixBase<D5>& o2,
MatrixBase<D7>& plus_p, QuaternionBase<D8>& plus_q)
{
- MatrixSizeCheck<3, 1>::check(dp1);
- MatrixSizeCheck<3, 1>::check(dp2);
+ MatrixSizeCheck<3, 1>::check(p1);
+ MatrixSizeCheck<3, 1>::check(p2);
+ MatrixSizeCheck<3, 1>::check(o2);
MatrixSizeCheck<3, 1>::check(plus_p);
- plus_p = dp1 + dp2;
- plus_q = dq1 * exp_q(do2);
+
+ plus_p = p1 + p2;
+ plus_q = q1 * exp_q(o2);
+}
+
+template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
+inline void plus(const MatrixBase<D1>& p1, const MatrixBase<D2>& q1,
+ const MatrixBase<D4>& p2, const MatrixBase<D5>& o2,
+ MatrixBase<D7>& plus_p, MatrixBase<D8>& plus_q)
+{
+ MatrixSizeCheck<4, 1>::check(q1);
+ MatrixSizeCheck<4, 1>::check(plus_q);
+
+ Map<const Quaterniond> q1m ( & q1 (0) );
+ Map<Quaterniond> plus_qm ( & plus_q(0) );
+
+ plus(p1, q1m, p2, o2, plus_p, plus_qm);
}
template<typename D1, typename D2, typename D3>
@@ -275,14 +371,14 @@ inline void plus(const MatrixBase<D1>& d1,
const MatrixBase<D2>& d2,
MatrixBase<D3>& d_plus)
{
- Map<const Matrix<typename D1::Scalar, 3, 1> > dp1 ( & d1(0) );
- Map<const Quaternion<typename D1::Scalar> > dq1 ( & d1(3) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > dp2 ( & d2(0) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > do2 ( & d2(3) );
- Map<Matrix<typename D3::Scalar, 3, 1> > dp_p ( & d_plus(0) );
- Map<Quaternion<typename D3::Scalar> > dq_p ( & d_plus(3) );
-
- plus(dp1, dq1, dp2, do2, dp_p, dq_p);
+ Map<const Matrix<typename D1::Scalar, 3, 1> > p1 ( & d1(0) );
+ Map<const Quaternion<typename D1::Scalar> > q1 ( & d1(3) );
+ Map<const Matrix<typename D2::Scalar, 3, 1> > p2 ( & d2(0) );
+ Map<const Matrix<typename D2::Scalar, 3, 1> > o2 ( & d2(3) );
+ Map<Matrix<typename D3::Scalar, 3, 1> > p_p ( & d_plus(0) );
+ Map<Quaternion<typename D3::Scalar> > q_p ( & d_plus(3) );
+
+ plus(p1, q1, p2, o2, p_p, q_p);
}
template<typename D1, typename D2>
@@ -294,25 +390,42 @@ inline Matrix<typename D1::Scalar, 7, 1> plus(const MatrixBase<D1>& d1,
return d_plus;
}
+inline void plus(const VectorComposite& x, const VectorComposite& tau, VectorComposite& res)
+{
+ plus(x.at("P"), x.at("O"), tau.at("P"), tau.at("O"), res.at("P"), res.at("O"));
+}
+
+inline VectorComposite plus(const VectorComposite& x, const VectorComposite& tau)
+{
+ VectorComposite res;
+
+ res["P"] = Vector3d();
+ res["O"] = Vector4d();
+
+ plus(x, tau, res);
+
+ return res;
+}
+
template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
-inline void minus(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1,
- const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2,
- MatrixBase<D7>& diff_p, MatrixBase<D8>& diff_o )
+inline void minus(const MatrixBase<D1>& p1, const QuaternionBase<D2>& q1,
+ const MatrixBase<D4>& p2, const QuaternionBase<D5>& q2,
+ MatrixBase<D7>& pd, MatrixBase<D8>& od )
{
- diff_p = dp2 - dp1;
- diff_o = log_q(dq1.conjugate() * dq2);
+ pd = p2 - p1;
+ od = log_q(q1.conjugate() * q2);
}
template<typename D1, typename D2, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9>
-inline void minus(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1,
- const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2,
- MatrixBase<D6>& diff_p, MatrixBase<D7>& diff_o,
- MatrixBase<D8>& J_do_dq1, MatrixBase<D9>& J_do_dq2)
+inline void minus(const MatrixBase<D1>& p1, const QuaternionBase<D2>& q1,
+ const MatrixBase<D4>& p2, const QuaternionBase<D5>& q2,
+ MatrixBase<D6>& pd, MatrixBase<D7>& od,
+ MatrixBase<D8>& J_do_q1, MatrixBase<D9>& J_do_q2)
{
- minus(dp1, dq1, dp2, dq2, diff_p, diff_o);
+ minus(p1, q1, p2, q2, pd, od);
- J_do_dq1 = - jac_SO3_left_inv(diff_o);
- J_do_dq2 = jac_SO3_right_inv(diff_o);
+ J_do_q1 = - jac_SO3_left_inv(od);
+ J_do_q2 = jac_SO3_right_inv(od);
}
template<typename D1, typename D2, typename D3>
@@ -320,14 +433,14 @@ inline void minus(const MatrixBase<D1>& d1,
const MatrixBase<D2>& d2,
MatrixBase<D3>& err)
{
- Map<const Matrix<typename D1::Scalar, 3, 1> > dp1 ( & d1(0) );
- Map<const Quaternion<typename D1::Scalar> > dq1 ( & d1(3) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > dp2 ( & d2(0) );
- Map<const Quaternion<typename D2::Scalar> > dq2 ( & d2(3) );
- Map<Matrix<typename D3::Scalar, 3, 1> > diff_p ( & err(0) );
- Map<Matrix<typename D3::Scalar, 3, 1> > diff_o ( & err(3) );
-
- minus(dp1, dq1, dp2, dq2, diff_p, diff_o);
+ Map<const Matrix<typename D1::Scalar, 3, 1> > p1 ( & d1(0) );
+ Map<const Quaternion<typename D1::Scalar> > q1 ( & d1(3) );
+ Map<const Matrix<typename D2::Scalar, 3, 1> > p2 ( & d2(0) );
+ Map<const Quaternion<typename D2::Scalar> > q2 ( & d2(3) );
+ Map<Matrix<typename D3::Scalar, 3, 1> > pd ( & err(0) );
+ Map<Matrix<typename D3::Scalar, 3, 1> > od ( & err(3) );
+
+ minus(p1, q1, p2, q2, pd, od);
}
template<typename D1, typename D2, typename D3, typename D4, typename D5>
@@ -337,35 +450,30 @@ inline void minus(const MatrixBase<D1>& d1,
MatrixBase<D4>& J_diff_d1,
MatrixBase<D5>& J_diff_d2)
{
- Map<const Matrix<typename D1::Scalar, 3, 1> > dp1 ( & d1(0) );
- Map<const Quaternion<typename D1::Scalar> > dq1 ( & d1(3) );
- Map<const Matrix<typename D1::Scalar, 3, 1> > dv1 ( & d1(7) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > dp2 ( & d2(0) );
- Map<const Quaternion<typename D2::Scalar> > dq2 ( & d2(3) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > dv2 ( & d2(7) );
- Map<Matrix<typename D3::Scalar, 3, 1> > diff_p ( & dif(0) );
- Map<Matrix<typename D3::Scalar, 3, 1> > diff_o ( & dif(3) );
- Map<Matrix<typename D3::Scalar, 3, 1> > diff_v ( & dif(6) );
+ Map<const Matrix<typename D1::Scalar, 3, 1> > p1 ( & d1(0) );
+ Map<const Quaternion<typename D1::Scalar> > q1 ( & d1(3) );
+ Map<const Matrix<typename D2::Scalar, 3, 1> > p2 ( & d2(0) );
+ Map<const Quaternion<typename D2::Scalar> > q2 ( & d2(3) );
+ Map<Matrix<typename D3::Scalar, 3, 1> > pd ( & dif(0) );
+ Map<Matrix<typename D3::Scalar, 3, 1> > od ( & dif(3) );
- Matrix<typename D4::Scalar, 3, 3> J_do_dq1, J_do_dq2;
+ Matrix<typename D4::Scalar, 3, 3> J_do_q1, J_do_q2;
- minus(dp1, dq1, dv1, dp2, dq2, dv2, diff_p, diff_o, diff_v, J_do_dq1, J_do_dq2);
+ minus(p1, q1, p2, q2, pd, od, J_do_q1, J_do_q2);
/* d = minus(d1, d2) is
- * dp = dp2 - dp1
- * do = Log(dq1.conj * dq2)
- * dv = dv2 - dv1
+ * pd = p2 - p1
+ * od = Log(q1.conj * q2)
*
- * With trivial Jacobians for dp and dv, and:
- * J_do_dq1 = - J_l_inv(theta)
- * J_do_dq2 = J_r_inv(theta)
+ * With trivial Jacobians for p and dv, and:
+ * J_do_q1 = - J_l_inv(theta)
+ * J_do_q2 = J_r_inv(theta)
*/
-
J_diff_d1 = - Matrix<typename D4::Scalar, 6, 6>::Identity();// d(p2 - p1) / d(p1) = - Identity
- J_diff_d1.block(3,3,3,3) = J_do_dq1; // d(R1.tr*R2) / d(R1) = - J_l_inv(theta)
+ J_diff_d1.block(3,3,3,3) = J_do_q1; // d(R1.tr*R2) / d(R1) = - J_l_inv(theta)
J_diff_d2.setIdentity(6,6); // d(R1.tr*R2) / d(R2) = Identity
- J_diff_d2.block(3,3,3,3) = J_do_dq2; // d(R1.tr*R2) / d(R1) = J_r_inv(theta)
+ J_diff_d2.block(3,3,3,3) = J_do_q2; // d(R1.tr*R2) / d(R1) = J_r_inv(theta)
}
template<typename D1, typename D2>
@@ -385,9 +493,9 @@ inline void interpolate(const MatrixBase<D1>& d1,
{
Matrix<typename D1::Scalar, 7, 1> dd = between(d1, d2);
- Matrix<typename D1::Scalar, 6, 1> tau = t * log_SE3(dd);
+ Matrix<typename D1::Scalar, 6, 1> tau = t * log(dd);
- ret = compose(d1, exp_SE3(tau));
+ ret = compose(d1, exp(tau));
}
template<typename D1, typename D2>
diff --git a/include/core/math/rotations.h b/include/core/math/rotations.h
index 7c2ede927a796adfd35e7dd9240e0128f1192c11..a0a86c6ee4c3ef095725a2df2230c2fdb8de751f 100644
--- a/include/core/math/rotations.h
+++ b/include/core/math/rotations.h
@@ -27,7 +27,7 @@ pi2pi(const T _angle)
T angle = _angle;
while (angle > T( M_PI ))
angle -= T( 2 * M_PI );
- while (angle < T( -M_PI ))
+ while (angle <= T( -M_PI ))
angle += T( 2 * M_PI );
return angle;
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index f2b19ef05c22f7a4c6777f889031dbda45d80a5b..f64f629056e3c5c52abc41818f049dc14b876438 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -11,7 +11,7 @@ class ProcessorMotion;
class StateBlock;
class TimeStamp;
struct ParamsSensorBase;
-struct ProcessorParamsBase;
+struct ParamsProcessorBase;
}
//wolf includes
@@ -19,9 +19,10 @@ struct ProcessorParamsBase;
#include "core/frame/frame_base.h"
#include "core/state_block/state_block.h"
#include "core/utils/params_server.h"
-#include "core/sensor/sensor_factory.h"
-#include "core/processor/processor_factory.h"
+#include "core/sensor/factory_sensor.h"
+#include "core/processor/factory_processor.h"
#include "core/processor/is_motion.h"
+#include "core/state_block/state_composite.h"
// std includes
#include <mutex>
@@ -34,6 +35,15 @@ enum Notification
REMOVE
};
+struct PriorOptions
+{
+ std::string mode = "";
+ VectorComposite state;
+ MatrixComposite cov;
+ double time_tolerance;
+};
+WOLF_STRUCT_PTR_TYPEDEFS(PriorOptions);
+
/** \brief Wolf problem node element in the Wolf Tree
*/
class Problem : public std::enable_shared_from_this<Problem>
@@ -41,8 +51,10 @@ class Problem : public std::enable_shared_from_this<Problem>
friend SolverManager; // Enable SolverManager to acces protected functions (consumeXXXNotificationMap())
friend ProcessorBase;
friend ProcessorMotion;
+ friend IsMotion;
protected:
+ TreeManagerBasePtr tree_manager_;
HardwareBasePtr hardware_ptr_;
TrajectoryBasePtr trajectory_ptr_;
MapBasePtr map_ptr_;
@@ -51,11 +63,10 @@ class Problem : public std::enable_shared_from_this<Problem>
SizeEigen state_size_, state_cov_size_, dim_;
std::map<FactorBasePtr, Notification> factor_notification_map_;
std::map<StateBlockPtr, Notification> state_block_notification_map_;
- mutable std::mutex mut_factor_notifications_;
- mutable std::mutex mut_state_block_notifications_;
+ mutable std::mutex mut_notifications_;
mutable std::mutex mut_covariances_;
- bool prior_is_set_;
- std::string frame_structure_;
+ StateStructure frame_structure_;
+ PriorOptionsPtr prior_options_;
private: // CAUTION: THESE METHODS ARE PRIVATE, DO NOT MAKE THEM PUBLIC !!
Problem(const std::string& _frame_structure); // USE create() below !!
@@ -67,11 +78,26 @@ class Problem : public std::enable_shared_from_this<Problem>
static ProblemPtr autoSetup(ParamsServer &_server);
virtual ~Problem();
+
+
// Properties -----------------------------------------
- SizeEigen getFrameStructureSize() const;
- void getFrameStructureSize(SizeEigen& _x_size, SizeEigen& _cov_size) const;
+ public:
SizeEigen getDim() const;
- std::string getFrameStructure() const;
+ StateStructure getFrameStructure() const;
+
+ protected:
+ void appendToStructure(const StateStructure& _structure);
+
+
+
+
+ // Tree manager --------------------------------------
+ public:
+ void setTreeManager(TreeManagerBasePtr _gm);
+ TreeManagerBasePtr getTreeManager() const;
+
+
+
// Hardware branch ------------------------------------
HardwareBasePtr getHardware() const;
@@ -91,7 +117,7 @@ class Problem : public std::enable_shared_from_this<Problem>
* \param _sen_type type of sensor
* \param _unique_sensor_name unique sensor name, used to identify the particular instance of the sensor
* \param _extrinsics a vector of extrinsic parameters: size 2 for 2d position, 3 for 2d pose, 3 for 3d position, 7 for 3d pose.
- * \param _intrinsics_filename the name of a file containing the intrinsic parameters in a format compatible with the intrinsics creator registered in ParamsSensorFactory under the key _sen_type.
+ * \param _intrinsics_filename the name of a file containing the intrinsic parameters in a format compatible with the intrinsics creator registered in FactoryParamsSensor under the key _sen_type.
*/
SensorBasePtr installSensor(const std::string& _sen_type, //
const std::string& _unique_sensor_name, //
@@ -119,7 +145,7 @@ class Problem : public std::enable_shared_from_this<Problem>
ProcessorBasePtr installProcessor(const std::string& _prc_type, //
const std::string& _unique_processor_name, //
SensorBasePtr _corresponding_sensor_ptr, //
- ProcessorParamsBasePtr _prc_params = nullptr);
+ ParamsProcessorBasePtr _prc_params = nullptr);
/** \brief Factory method to install (create, and add to sensor) processors only from its properties
*
@@ -155,51 +181,91 @@ class Problem : public std::enable_shared_from_this<Problem>
IsMotionPtr getProcessorIsMotion();
std::list<IsMotionPtr> getProcessorIsMotionList();
+
+
+
// Trajectory branch ----------------------------------
TrajectoryBasePtr getTrajectory() const;
- virtual FrameBasePtr setPrior(const Eigen::VectorXd& _prior_state, //
- const Eigen::MatrixXd& _prior_cov, //
- const TimeStamp& _ts,
- const double _time_tolerance);
- /** \brief Emplace frame from string frame_structure
+ // Prior
+ bool isPriorSet() const;
+ void setPriorOptions(const std::string& _mode,
+ const double _time_tolerance = 0,
+ const VectorComposite& _state = VectorComposite(),
+ const VectorComposite& _cov = VectorComposite());
+ FrameBasePtr applyPriorOptions(const TimeStamp& _ts);
+ FrameBasePtr setPriorFactor(const VectorComposite &_state,
+ const VectorComposite &_sigma,
+ const TimeStamp &_ts,
+ const double &_time_tol);
+ FrameBasePtr setPriorFix(const VectorComposite &_state,
+ const TimeStamp &_ts,
+ const double &_time_tol);
+ FrameBasePtr setPriorInitialGuess(const VectorComposite &_state,
+ const TimeStamp &_ts,
+ const double &_time_tol);
+
+ /** \brief Emplace frame from string frame_structure, dimension and vector
+ * \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED
+ * \param _time_stamp Time stamp of the frame
* \param _frame_structure String indicating the frame structure.
* \param _dim variable indicating the dimension of the problem
- * \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED
* \param _frame_state State vector; must match the size and format of the chosen frame structure
- * \param _time_stamp Time stamp of the frame
*
* This acts as a Frame factory, but also takes care to update related lists in WolfProblem:
* - Create a Frame
* - Add it to Trajectory
* - If it is key-frame, update state-block lists in Problem
*/
- FrameBasePtr emplaceFrame(const std::string& _frame_structure, //
+ FrameBasePtr emplaceFrame(FrameType _frame_key_type, //
+ const TimeStamp& _time_stamp, //
+ const StateStructure& _frame_structure, //
const SizeEigen _dim, //
- FrameType _frame_key_type, //
- const Eigen::VectorXd& _frame_state, //
- const TimeStamp& _time_stamp);
+ const Eigen::VectorXd& _frame_state);
- /** \brief Emplace frame from string frame_structure without state
+ /** \brief Emplace frame from string frame_structure and state
+ * \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED
+ * \param _time_stamp Time stamp of the frame
* \param _frame_structure String indicating the frame structure.
- * \param _dim variable indicating the dimension of the problem
+ * \param _frame_state State vector; must match the size and format of the chosen frame structure
+ *
+ * - The dimension is taken from Problem
+ *
+ * This acts as a Frame factory, but also takes care to update related lists in WolfProblem:
+ * - Create a Frame
+ * - 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);
+
+ /** \brief Emplace frame from state
* \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED
* \param _time_stamp Time stamp of the frame
+ * \param _frame_state State; must be part of the problem's frame structure
+ *
+ * - The structure is taken from Problem
+ * - The dimension is taken from Problem
*
* This acts as a Frame factory, but also takes care to update related lists in WolfProblem:
* - Create a Frame
* - Add it to Trajectory
* - If it is key-frame, update state-block lists in Problem
*/
- FrameBasePtr emplaceFrame(const std::string& _frame_structure, //
- const SizeEigen _dim, //
- FrameType _frame_key_type, //
- const TimeStamp& _time_stamp);
+ FrameBasePtr emplaceFrame(FrameType _frame_key_type, //
+ const TimeStamp& _time_stamp, //
+ const VectorComposite& _frame_state);
- /** \brief Emplace frame from string frame_structure without structure
+ /** \brief Emplace frame from string frame_structure and dimension
* \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED
- * \param _frame_state State vector; must match the size and format of the chosen frame structure
* \param _time_stamp Time stamp of the frame
+ * \param _frame_structure String indicating the frame structure.
+ * \param _dim variable indicating the dimension of the problem
+ *
+ * - The dimension is taken from Problem
+ * - The state is taken from Problem
*
* This acts as a Frame factory, but also takes care to update related lists in WolfProblem:
* - Create a Frame
@@ -207,12 +273,34 @@ class Problem : public std::enable_shared_from_this<Problem>
* - If it is key-frame, update state-block lists in Problem
*/
FrameBasePtr emplaceFrame(FrameType _frame_key_type, //
- const Eigen::VectorXd& _frame_state, //
- const TimeStamp& _time_stamp);
+ const TimeStamp& _time_stamp, //
+ const StateStructure& _frame_structure, //
+ const SizeEigen _dim);
- /** \brief Emplace frame from string frame_structure without structure nor state
+ /** \brief Emplace frame from state vector
* \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED
* \param _time_stamp Time stamp of the frame
+ * \param _frame_state State vector; must match the size and format of the chosen frame structure
+ *
+ * - The structure is taken from Problem
+ * - The dimension is taken from Problem
+ *
+ * This acts as a Frame factory, but also takes care to update related lists in WolfProblem:
+ * - Create a Frame
+ * - 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);
+
+ /** \brief Emplace frame, guess all values
+ * \param _frame_key_type Either KEY, AUXILIARY or NON_ESTIMATED
+ * \param _time_stamp Time stamp of the frame
+ *
+ * - The structure is taken from Problem
+ * - The dimension is taken from Problem
+ * - The state is taken from Problem
*
* This acts as a Frame factory, but also takes care to update related lists in WolfProblem:
* - Create a Frame
@@ -262,17 +350,15 @@ class Problem : public std::enable_shared_from_this<Problem>
const double& _time_tolerance);
// State getters
- Eigen::VectorXd getCurrentState ( ) const;
- void getCurrentState (Eigen::VectorXd& _state) const;
- void getCurrentStateAndStamp (Eigen::VectorXd& _state, TimeStamp& _ts) const;
- Eigen::VectorXd getState (const TimeStamp& _ts) const;
- void getState (const TimeStamp& _ts, Eigen::VectorXd& _state) const;
+ TimeStamp getTimeStamp ( ) const;
+ VectorComposite getState ( const StateStructure& _structure = "" ) const;
+ VectorComposite getState ( const TimeStamp& _ts, const StateStructure& _structure = "" ) const;
+
// Zero state provider
- Eigen::VectorXd zeroState ( ) const;
- bool priorIsSet() const;
- void setPriorIsSet(bool _prior_is_set);
- // Perturb state
- void perturb(double amplitude = 0.01);
+ VectorComposite stateZero ( const StateStructure& _structure = "" ) const;
+
+
+
// Map branch -----------------------------------------
MapBasePtr getMap() const;
@@ -280,7 +366,13 @@ class Problem : public std::enable_shared_from_this<Problem>
void saveMap(const std::string& _filename_dot_yaml, //
const std::string& _map_name = "Map automatically saved by Wolf");
- // Covariances -------------------------------------------
+
+
+ // All branches -------------------------------------------
+ // perturb states
+ void perturb(double amplitude = 0.01);
+
+ // Covariances
void clearCovariance();
void addCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, const Eigen::MatrixXd& _cov);
void addCovarianceBlock(StateBlockPtr _state1, const Eigen::MatrixXd& _cov);
@@ -292,6 +384,8 @@ class Problem : public std::enable_shared_from_this<Problem>
bool getLastKeyOrAuxFrameCovariance(Eigen::MatrixXd& _covariance) const;
bool getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::MatrixXd& _covariance) const;
+
+
// Solver management ----------------------------------------
/** \brief Notifies a new/removed state block to update the solver manager
@@ -319,6 +413,11 @@ class Problem : public std::enable_shared_from_this<Problem>
bool getFactorNotification(const FactorBasePtr& fac_ptr, Notification& notif) const;
protected:
+ /** \brief Returns the map of state block notification to be handled by the solver (the map stored in this is emptied)
+ */
+ void consumeNotifications(std::map<StateBlockPtr,Notification>&,
+ std::map<FactorBasePtr,Notification>&);
+
/** \brief Returns the map of state block notification to be handled by the solver (the map stored in this is emptied)
*/
std::map<StateBlockPtr,Notification> consumeStateBlockNotificationMap();
@@ -329,11 +428,6 @@ class Problem : public std::enable_shared_from_this<Problem>
public:
// Print and check ---------------------------------------
- void print(int depth, //
- std::ostream& stream ,
- bool constr_by, //
- bool metric, //
- bool state_blocks) const;
/**
* \brief print wolf tree
* \param depth : levels to show ( 0: H, T, M : 1: H:S:p, T:F, M:L ; 2: T:F:C ; 3: T:F:C:f ; 4: T:F:C:f:c )
@@ -344,11 +438,8 @@ class Problem : public std::enable_shared_from_this<Problem>
void print(int depth = 4, //
bool constr_by = false, //
bool metric = true, //
- bool state_blocks = false) const;
- void print(const std::string& depth, //
- bool constr_by = false, //
- bool metric = true, //
- bool state_blocks = false) const;
+ bool state_blocks = false,
+ std::ostream& stream = std::cout) const;
bool check(int verbose_level = 0) const;
bool check(bool verbose, std::ostream& stream) const;
@@ -361,14 +452,14 @@ class Problem : public std::enable_shared_from_this<Problem>
namespace wolf
{
-inline bool Problem::priorIsSet() const
+inline TreeManagerBasePtr Problem::getTreeManager() const
{
- return prior_is_set_;
+ return tree_manager_;
}
-inline void Problem::setPriorIsSet(bool _prior_is_set)
+inline bool Problem::isPriorSet() const
{
- prior_is_set_ = _prior_is_set;
+ return prior_options_ == nullptr;
}
inline IsMotionPtr Problem::getProcessorIsMotion()
@@ -384,28 +475,25 @@ inline std::list<IsMotionPtr> Problem::getProcessorIsMotionList()
return processor_is_motion_list_;
}
-inline std::map<StateBlockPtr,Notification> Problem::consumeStateBlockNotificationMap()
-{
- std::lock_guard<std::mutex> lock(mut_state_block_notifications_);
- return std::move(state_block_notification_map_);
-}
-
inline SizeStd Problem::getStateBlockNotificationMapSize() const
{
- std::lock_guard<std::mutex> lock(mut_state_block_notifications_);
+ std::lock_guard<std::mutex> lock(mut_notifications_);
return state_block_notification_map_.size();
}
-inline std::map<FactorBasePtr,Notification> Problem::consumeFactorNotificationMap()
+inline wolf::SizeStd Problem::getFactorNotificationMapSize() const
{
- std::lock_guard<std::mutex> lock(mut_factor_notifications_);
- return std::move(factor_notification_map_);
+ std::lock_guard<std::mutex> lock(mut_notifications_);
+ return factor_notification_map_.size();
}
-inline wolf::SizeStd Problem::getFactorNotificationMapSize() const
+inline void Problem::consumeNotifications(std::map<StateBlockPtr,Notification>& _sb_notification_map,
+ std::map<FactorBasePtr,Notification>& _fac_notification_map)
{
- std::lock_guard<std::mutex> lock(mut_factor_notifications_);
- return factor_notification_map_.size();
+ std::lock_guard<std::mutex> lock(mut_notifications_);
+
+ _sb_notification_map = std::move(state_block_notification_map_);
+ _fac_notification_map = std::move(factor_notification_map_);
}
} // namespace wolf
diff --git a/include/core/processor/processor_factory.h b/include/core/processor/factory_processor.h
similarity index 74%
rename from include/core/processor/processor_factory.h
rename to include/core/processor/factory_processor.h
index c14f3daa364678ef6f549fa04f65eddb4885e1fd..5fc2f8f14ccffc52653c6f43fa0c07fdb03be825 100644
--- a/include/core/processor/processor_factory.h
+++ b/include/core/processor/factory_processor.h
@@ -1,17 +1,17 @@
/**
- * \file processor_factory.h
+ * \file factory_processor.h
*
* Created on: May 4, 2016
* \author: jsola
*/
-#ifndef PROCESSOR_FACTORY_H_
-#define PROCESSOR_FACTORY_H_
+#ifndef FACTORY_PROCESSOR_H_
+#define FACTORY_PROCESSOR_H_
namespace wolf
{
class ProcessorBase;
-struct ProcessorParamsBase;
+struct ParamsProcessorBase;
}
// wolf
@@ -47,17 +47,17 @@ namespace wolf
* - Write a processor creator for ProcessorOdom2d (example).
*
* #### Accessing the Factory
- * The ProcessorFactory class is a singleton: it can only exist once in your application.
+ * The FactoryProcessor class is a singleton: it can only exist once in your application.
* To obtain an instance of it, use the static method get(),
*
* \code
- * ProcessorFactory::get()
+ * FactoryProcessor::get()
* \endcode
*
* You can then call the methods you like, e.g. to create a processor, you type:
*
* \code
- * ProcessorFactory::get().create(...); // see below for creating processors ...
+ * FactoryProcessor::get().create(...); // see below for creating processors ...
* \endcode
*
* #### Registering processor creators
@@ -69,21 +69,21 @@ namespace wolf
* that knows how to create your specific processor, e.g.:
*
* \code
- * ProcessorFactory::get().registerCreator("ProcessorOdom2d", ProcessorOdom2d::create);
+ * FactoryProcessor::get().registerCreator("ProcessorOdom2d", ProcessorOdom2d::create);
* \endcode
*
* The method ProcessorOdom2d::create() exists in the ProcessorOdom2d class as a static method.
* All these ProcessorXxx::create() methods need to have exactly the same API, regardless of the processor type.
- * This API includes a processor name, and a pointer to a base struct of parameters, ProcessorParamsBasePtr,
+ * This API includes a processor name, and a pointer to a base struct of parameters, ParamsProcessorBasePtr,
* that can be derived for each derived processor.
*
* Here is an example of ProcessorOdom2d::create() extracted from processor_odom_2d.h:
*
* \code
- * static ProcessorBasePtr create(const std::string& _name, ProcessorParamsBasePtr _params)
+ * static ProcessorBasePtr create(const std::string& _name, ParamsProcessorBasePtr _params)
* {
* // cast _params to good type
- * ProcessorParamsOdom2d* params = (ProcessorParamsOdom2d*)_params;
+ * ParamsProcessorOdom2d* params = (ParamsProcessorOdom2d*)_params;
*
* ProcessorBasePtr prc = new ProcessorOdom2d(params);
* prc->setName(_name); // pass the name to the created ProcessorOdom2d.
@@ -96,7 +96,7 @@ namespace wolf
* For example, in processor_odom_2d.cpp we find the line:
*
* \code
- * const bool registered_odom_2d = ProcessorFactory::get().registerCreator("ProcessorOdom2d", ProcessorOdom2d::create);
+ * const bool registered_odom_2d = FactoryProcessor::get().registerCreator("ProcessorOdom2d", ProcessorOdom2d::create);
* \endcode
*
* which is a static invocation (i.e., it is placed at global scope outside of the ProcessorOdom2d class).
@@ -107,7 +107,7 @@ namespace wolf
* It only needs to be passed the string of the processor type.
*
* \code
- * ProcessorFactory::get().unregisterCreator("ProcessorOdom2d");
+ * FactoryProcessor::get().unregisterCreator("ProcessorOdom2d");
* \endcode
*
* #### Creating processors
@@ -117,7 +117,7 @@ namespace wolf
* To create a ProcessorOdom2d, you type:
*
* \code
- * ProcessorFactory::get().create("ProcessorOdom2d", "main odometry", params_ptr);
+ * FactoryProcessor::get().create("ProcessorOdom2d", "main odometry", params_ptr);
* \endcode
*
* #### Example 1 : using the Factories alone
@@ -125,21 +125,21 @@ namespace wolf
* and bind it to a SensorOdom2d:
*
* \code
- * #include "core/sensor/sensor_odom_2d.h" // provides SensorOdom2d and SensorFactory
- * #include "core/processor/processor_odom_2d.h" // provides ProcessorOdom2d and ProcessorFactory
+ * #include "core/sensor/sensor_odom_2d.h" // provides SensorOdom2d and FactorySensor
+ * #include "core/processor/processor_odom_2d.h" // provides ProcessorOdom2d and FactoryProcessor
*
* // Note: SensorOdom2d::create() is already registered, automatically.
* // Note: ProcessorOdom2d::create() is already registered, automatically.
*
- * // First create the sensor (See SensorFactory for details)
- * SensorBasePtr sensor_ptr = SensorFactory::get().create ( "FactorOdom2d" , "Main odometer" , extrinsics , &intrinsics );
+ * // First create the sensor (See FactorySensor for details)
+ * SensorBasePtr sensor_ptr = FactorySensor::get().create ( "FactorOdom2d" , "Main odometer" , extrinsics , &intrinsics );
*
* // To create a odometry integrator, provide a type="ODOM 2d", a name="main odometry", and a pointer to the parameters struct:
*
- * ProcessorParamsOdom2d params({...}); // fill in the derived struct (note: ProcessorOdom2d actually has no input params)
+ * ParamsProcessorOdom2d params({...}); // fill in the derived struct (note: ProcessorOdom2d actually has no input params)
*
* ProcessorBasePtr processor_ptr =
- * ProcessorFactory::get().create ( "ProcessorOdom2d" , "main odometry" , ¶ms );
+ * FactoryProcessor::get().create ( "ProcessorOdom2d" , "main odometry" , ¶ms );
*
* // Bind processor to sensor
* sensor_ptr->addProcessor(processor_ptr);
@@ -165,43 +165,43 @@ namespace wolf
* You can also check the code in the example file ````src/examples/test_wolf_factories.cpp````.
*/
-// ProcessorParams factory
-struct ProcessorParamsBase;
-typedef Factory<ProcessorParamsBase,
- const std::string&> ProcessorParamsFactory;
+// ParamsProcessor factory
+struct ParamsProcessorBase;
+typedef Factory<ParamsProcessorBase,
+ const std::string&> FactoryParamsProcessor;
template<>
-inline std::string ProcessorParamsFactory::getClass()
+inline std::string FactoryParamsProcessor::getClass() const
{
- return "ProcessorParamsFactory";
+ return "FactoryParamsProcessor";
}
// Processor factory
typedef Factory<ProcessorBase,
const std::string&,
- const ProcessorParamsBasePtr> ProcessorFactory;
+ const ParamsProcessorBasePtr> FactoryProcessor;
template<>
-inline std::string ProcessorFactory::getClass()
+inline std::string FactoryProcessor::getClass() const
{
- return "ProcessorFactory";
+ return "FactoryProcessor";
}
-#define WOLF_REGISTER_PROCESSOR(ProcessorType, ProcessorName) \
- namespace{ const bool WOLF_UNUSED ProcessorName##Registered = \
- wolf::ProcessorFactory::get().registerCreator(ProcessorType, ProcessorName::create); } \
+#define WOLF_REGISTER_PROCESSOR(ProcessorType) \
+ namespace{ const bool WOLF_UNUSED ProcessorType##Registered = \
+ wolf::FactoryProcessor::get().registerCreator(#ProcessorType, ProcessorType::create); } \
typedef Factory<ProcessorBase,
const std::string&,
- const ParamsServer&> AutoConfProcessorFactory;
+ const ParamsServer&> AutoConfFactoryProcessor;
template<>
-inline std::string AutoConfProcessorFactory::getClass()
+inline std::string AutoConfFactoryProcessor::getClass() const
{
- return "AutoConfProcessorFactory";
+ return "AutoConfFactoryProcessor";
}
-#define WOLF_REGISTER_PROCESSOR_AUTO(ProcessorType, ProcessorName) \
- namespace{ const bool WOLF_UNUSED ProcessorName##AutoConfRegistered = \
- wolf::AutoConfProcessorFactory::get().registerCreator(ProcessorType, ProcessorName::create); } \
+#define WOLF_REGISTER_PROCESSOR_AUTO(ProcessorType) \
+ namespace{ const bool WOLF_UNUSED ProcessorType##AutoConfRegistered = \
+ wolf::AutoConfFactoryProcessor::get().registerCreator(#ProcessorType, ProcessorType::create); } \
} /* namespace wolf */
diff --git a/include/core/processor/is_motion.h b/include/core/processor/is_motion.h
index 8c99fe72e37698a2210e35a3af0c14bf9168f65e..2f725e18d723463e810441c83f7fc81acc206785 100644
--- a/include/core/processor/is_motion.h
+++ b/include/core/processor/is_motion.h
@@ -9,6 +9,7 @@
#define PROCESSOR_IS_MOTION_H_
#include "core/common/wolf.h"
+#include "core/state_block/state_composite.h"
namespace wolf
{
@@ -24,28 +25,14 @@ class IsMotion
virtual ~IsMotion();
// Queries to the processor:
-
- /** \brief Fill a reference to the state integrated so far
- * \param _x the returned state vector
- */
- virtual void getCurrentState(Eigen::VectorXd& _x) const = 0;
- virtual void getCurrentTimeStamp(TimeStamp& _ts) const = 0;
- /** \brief Fill the state corresponding to the provided time-stamp
- * \param _ts the time stamp
- * \param _x the returned state
- * \return if state in the provided time-stamp could be resolved
- */
- virtual bool getState(const TimeStamp& _ts, Eigen::VectorXd& _x) const = 0;
-
-
- // Overloaded getters
- Eigen::VectorXd getCurrentState() const;
- TimeStamp getCurrentTimeStamp() const;
- Eigen::VectorXd getState(const TimeStamp& _ts) const;
+ virtual TimeStamp getTimeStamp() const = 0;
+ virtual VectorComposite getState() const = 0;
+ virtual VectorComposite getState(const TimeStamp& _ts) const = 0;
std::string getStateStructure(){return state_structure_;};
void setStateStructure(std::string _state_structure){state_structure_ = _state_structure;};
-
+ void addToProblem(ProblemPtr _prb_ptr, IsMotionPtr _motion_ptr);
+
protected:
std::string state_structure_; ///< The structure of the state vector (to retrieve state blocks from frames)
@@ -62,29 +49,6 @@ inline IsMotion::~IsMotion()
{
}
-inline Eigen::VectorXd IsMotion::getCurrentState() const
-{
- Eigen::VectorXd x;
- getCurrentState(x);
- return x;
-}
-
-inline TimeStamp IsMotion::getCurrentTimeStamp() const
-{
- TimeStamp ts;
- getCurrentTimeStamp(ts);
- return ts;
-}
-
-inline Eigen::VectorXd IsMotion::getState(const TimeStamp& _ts) const
-{
- Eigen::VectorXd x;
- getState(_ts, x);
- return x;
-}
-
-
-
} /* namespace wolf */
#endif /* PROCESSOR_IS_MOTION_H_ */
diff --git a/include/core/processor/motion_buffer.h b/include/core/processor/motion_buffer.h
index a5b41ebfe3ebf77f269148d42781b7dd96cd36a2..548fcf6e1e4f99b9d020784b226f4f1df75e4b98 100644
--- a/include/core/processor/motion_buffer.h
+++ b/include/core/processor/motion_buffer.h
@@ -72,30 +72,16 @@ struct Motion
* - If the query time stamp does not match any time stamp in the buffer,
* the returned motion-integral or delta-integral is the one immediately before the query time stamp.
*/
-class MotionBuffer{
+class MotionBuffer : public std::list<Motion>
+{
public:
MotionBuffer() ;
- std::list<Motion>& get();
- const std::list<Motion>& get() const;
const Motion& getMotion(const TimeStamp& _ts) const;
void getMotion(const TimeStamp& _ts, Motion& _motion) const;
void split(const TimeStamp& _ts, MotionBuffer& _oldest_buffer);
void print(bool show_data = 0, bool show_delta = 0, bool show_delta_int = 0, bool show_jacs = 0);
-
- private:
- std::list<Motion> container_;
};
-inline std::list<Motion>& MotionBuffer::get()
-{
- return container_;
-}
-
-inline const std::list<Motion>& MotionBuffer::get() const
-{
- return container_;
-}
-
} // namespace wolf
#endif /* SRC_MOTIONBUFFER_H_ */
diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h
index dc62abcac8375fc238f7c93617a33ccb8662d246..4182e762db050a3f36e714d453b73fb63568f5bc 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.h
@@ -30,13 +30,13 @@ namespace wolf {
* In order to use this macro, the derived processor class, ProcessorClass,
* must have a constructor available with the API:
*
- * ProcessorClass(const ProcessorParamsClassPtr _params);
+ * ProcessorClass(const ParamsProcessorClassPtr _params);
*/
-#define WOLF_PROCESSOR_CREATE(ProcessorClass, ProcessorParamsClass) \
+#define WOLF_PROCESSOR_CREATE(ProcessorClass, ParamsProcessorClass) \
static ProcessorBasePtr create(const std::string& _unique_name, \
const ParamsServer& _server) \
{ \
- auto params = std::make_shared<ProcessorParamsClass>(_unique_name, _server); \
+ auto params = std::make_shared<ParamsProcessorClass>(_unique_name, _server); \
\
auto processor = std::make_shared<ProcessorClass>(params); \
\
@@ -44,9 +44,9 @@ static ProcessorBasePtr create(const std::string& _unique_name,
\
return processor; \
} \
-static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params) \
+static ProcessorBasePtr create(const std::string& _unique_name, const ParamsProcessorBasePtr _params) \
{ \
- auto params = std::static_pointer_cast<ProcessorParamsClass>(_params); \
+ auto params = std::static_pointer_cast<ParamsProcessorClass>(_params); \
\
auto processor = std::make_shared<ProcessorClass>(params); \
\
@@ -208,11 +208,11 @@ class BufferCapture : public Buffer<CaptureBasePtr> {};
*
* Derive from this struct to create structs of processor parameters.
*/
-struct ProcessorParamsBase : public ParamsBase
+struct ParamsProcessorBase : public ParamsBase
{
std::string prefix = "processor/";
- ProcessorParamsBase() = default;
- ProcessorParamsBase(std::string _unique_name, const ParamsServer& _server):
+ ParamsProcessorBase() = default;
+ ParamsProcessorBase(std::string _unique_name, const ParamsServer& _server):
ParamsBase(_unique_name, _server)
{
time_tolerance = _server.getParam<double>(prefix + _unique_name + "/time_tolerance");
@@ -221,7 +221,7 @@ struct ProcessorParamsBase : public ParamsBase
apply_loss_function = _server.getParam<bool>(prefix + _unique_name + "/apply_loss_function");
}
- virtual ~ProcessorParamsBase() = default;
+ virtual ~ParamsProcessorBase() = default;
/** maximum time difference between a Keyframe time stamp and
* a particular Capture of this processor to allow assigning
@@ -248,7 +248,7 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
friend SensorBase;
protected:
unsigned int processor_id_;
- ProcessorParamsBasePtr params_;
+ ParamsProcessorBasePtr params_;
BufferPackKeyFrame buffer_pack_kf_;
BufferCapture buffer_capture_;
int dim_;
@@ -258,7 +258,7 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
static unsigned int processor_id_count_;
public:
- ProcessorBase(const std::string& _type, int _dim, ProcessorParamsBasePtr _params);
+ ProcessorBase(const std::string& _type, int _dim, ParamsProcessorBasePtr _params);
virtual ~ProcessorBase();
virtual void configure(SensorBasePtr _sensor) = 0;
virtual void remove();
@@ -326,6 +326,8 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
virtual bool permittedAuxFrame() final;
+ virtual void setProblem(ProblemPtr) override;
+
public:
/**\brief notify a new keyframe made by another processor
*
@@ -361,6 +363,23 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
template<typename classType, typename... T>
static std::shared_ptr<classType> emplace(SensorBasePtr _sen_ptr, T&&... all);
void setVotingAuxActive(bool _voting_active = true);
+
+ virtual void printHeader(int depth, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream ,
+ std::string _tabs = "") const;
+
+ void print(int depth, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream = std::cout,
+ std::string _tabs = "") const;
+
+ virtual CheckLog localCheck(bool _verbose, ProcessorBasePtr _prc_ptr, std::ostream& _stream, std::string _tabs = "") const;
+ bool check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
};
inline bool ProcessorBase::isVotingActive() const
diff --git a/include/core/processor/processor_diff_drive.h b/include/core/processor/processor_diff_drive.h
index 48d18acccc0e48f3f5f2373b0ece28e2da607f61..e3e504f36b50d95c69bf13d0a99c0107f5207b27 100644
--- a/include/core/processor/processor_diff_drive.h
+++ b/include/core/processor/processor_diff_drive.h
@@ -13,18 +13,18 @@
namespace wolf
{
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsDiffDrive);
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorDiffDrive);
-struct ProcessorParamsDiffDrive : public ProcessorParamsOdom2d
+struct ParamsProcessorDiffDrive : public ParamsProcessorOdom2d
{
- ProcessorParamsDiffDrive() = default;
- ProcessorParamsDiffDrive(std::string _unique_name, const wolf::ParamsServer & _server) :
- ProcessorParamsOdom2d(_unique_name, _server)
+ ParamsProcessorDiffDrive() = default;
+ ParamsProcessorDiffDrive(std::string _unique_name, const wolf::ParamsServer & _server) :
+ ParamsProcessorOdom2d(_unique_name, _server)
{
}
std::string print() const
{
- return "\n" + ProcessorParamsOdom2d::print();
+ return "\n" + ParamsProcessorOdom2d::print();
}
};
@@ -33,12 +33,12 @@ WOLF_PTR_TYPEDEFS(ProcessorDiffDrive);
class ProcessorDiffDrive : public ProcessorOdom2d
{
public:
- ProcessorDiffDrive(ProcessorParamsDiffDrivePtr _params_motion);
+ ProcessorDiffDrive(ParamsProcessorDiffDrivePtr _params_motion);
virtual ~ProcessorDiffDrive();
virtual void configure(SensorBasePtr _sensor) override;
// Factory method for high level API
- WOLF_PROCESSOR_CREATE(ProcessorDiffDrive, ProcessorParamsDiffDrive);
+ WOLF_PROCESSOR_CREATE(ProcessorDiffDrive, ParamsProcessorDiffDrive);
protected:
// Motion integration
@@ -60,14 +60,26 @@ class ProcessorDiffDrive : public ProcessorOdom2d
virtual FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_own) override;
virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion,
CaptureBasePtr _capture_origin) override;
-
+ virtual VectorXd getCalibration (const CaptureBasePtr _capture) const override;
+ virtual void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
protected:
- ProcessorParamsDiffDrivePtr params_prc_diff_drv_selfcal_;
+ ParamsProcessorDiffDrivePtr params_prc_diff_drv_selfcal_;
double radians_per_tick_;
};
+inline Eigen::VectorXd ProcessorDiffDrive::getCalibration (const CaptureBasePtr _capture) const
+{
+ return _capture->getStateBlock("I")->getState();
+}
+
+inline void ProcessorDiffDrive::setCalibration (const CaptureBasePtr _capture, const VectorXd& _calibration)
+{
+ _capture->getStateBlock("I")->setState(_calibration);
+}
+
+
}
diff --git a/include/core/processor/processor_loopclosure.h b/include/core/processor/processor_loopclosure.h
index b0720f3a47afdfd7878b23929923295964340ea5..4b22ba3985cf04a1c5c1f7658419ea9ed384f390 100644
--- a/include/core/processor/processor_loopclosure.h
+++ b/include/core/processor/processor_loopclosure.h
@@ -6,12 +6,12 @@
namespace wolf{
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsLoopClosure);
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorLoopClosure);
-struct ProcessorParamsLoopClosure : public ProcessorParamsBase
+struct ParamsProcessorLoopClosure : public ParamsProcessorBase
{
- using ProcessorParamsBase::ProcessorParamsBase;
- // virtual ~ProcessorParamsLoopClosure() = default;
+ using ParamsProcessorBase::ParamsProcessorBase;
+ // virtual ~ParamsProcessorLoopClosure() = default;
// add neccesery parameters for loop closure initialisation here and initialize
// them in constructor
@@ -46,12 +46,11 @@ class ProcessorLoopClosure : public ProcessorBase
{
protected:
- ProcessorParamsLoopClosurePtr params_loop_closure_;
- int _dim;
+ ParamsProcessorLoopClosurePtr params_loop_closure_;
public:
- ProcessorLoopClosure(const std::string& _type, int _dim, ProcessorParamsLoopClosurePtr _params_loop_closure);
+ ProcessorLoopClosure(const std::string& _type, int _dim, ParamsProcessorLoopClosurePtr _params_loop_closure);
virtual ~ProcessorLoopClosure() = default;
virtual void configure(SensorBasePtr _sensor) override { };
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index 81a9184ad4798ccbaac1ebd61a194f6e287331b4..8533e66caf5da96a06680098b44236a4fb420482 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -21,9 +21,9 @@
namespace wolf
{
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsMotion);
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorMotion);
-struct ProcessorParamsMotion : public ProcessorParamsBase
+struct ParamsProcessorMotion : public ParamsProcessorBase
{
double max_time_span = 0.5;
unsigned int max_buff_length = 10;
@@ -31,9 +31,9 @@ struct ProcessorParamsMotion : public ProcessorParamsBase
double angle_turned = 0.5;
double unmeasured_perturbation_std = 1e-4;
- ProcessorParamsMotion() = default;
- ProcessorParamsMotion(std::string _unique_name, const ParamsServer& _server):
- ProcessorParamsBase(_unique_name, _server)
+ ParamsProcessorMotion() = default;
+ ParamsProcessorMotion(std::string _unique_name, const ParamsServer& _server):
+ ParamsProcessorBase(_unique_name, _server)
{
max_time_span = _server.getParam<double>(prefix + _unique_name + "/keyframe_vote/max_time_span");
max_buff_length = _server.getParam<unsigned int>(prefix + _unique_name + "/keyframe_vote/max_buff_length");
@@ -43,7 +43,7 @@ struct ProcessorParamsMotion : public ProcessorParamsBase
}
std::string print() const
{
- return "\n" + ProcessorParamsBase::print() + "\n"
+ return "\n" + ParamsProcessorBase::print() + "\n"
+ "max_time_span: " + std::to_string(max_time_span) + "\n"
+ "max_buff_length: " + std::to_string(max_buff_length) + "\n"
+ "dist_traveled: " + std::to_string(dist_traveled) + "\n"
@@ -134,16 +134,19 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
{
public:
typedef enum {
- RUNNING_WITHOUT_PACK,
- RUNNING_WITH_PACK_BEFORE_ORIGIN,
- RUNNING_WITH_PACK_ON_ORIGIN,
- RUNNING_WITH_PACK_AFTER_ORIGIN
+ FIRST_TIME_WITHOUT_KF,
+ FIRST_TIME_WITH_KF_BEFORE_INCOMING,
+ FIRST_TIME_WITH_KF_ON_INCOMING,
+ FIRST_TIME_WITH_KF_AFTER_INCOMING,
+ RUNNING_WITHOUT_KF,
+ RUNNING_WITH_KF_BEFORE_ORIGIN,
+ RUNNING_WITH_KF_ON_ORIGIN,
+ RUNNING_WITH_KF_AFTER_ORIGIN
} ProcessingStep ;
protected:
- ProcessorParamsMotionPtr params_motion_;
+ ParamsProcessorMotionPtr params_motion_;
ProcessingStep processing_step_; ///< State machine controlling the processing step
- virtual void setProblem(ProblemPtr) override;
// This is the main public interface
public:
@@ -155,30 +158,16 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
SizeEigen _delta_cov_size,
SizeEigen _data_size,
SizeEigen _calib_size,
- ProcessorParamsMotionPtr _params_motion);
+ ParamsProcessorMotionPtr _params_motion);
virtual ~ProcessorMotion();
// Instructions to the processor:
virtual void resetDerived();
// Queries to the processor:
-
- /** \brief Fill a reference to the state integrated so far
- * \param _x the returned state vector
- */
- virtual void getCurrentState(Eigen::VectorXd& _x) const override;
- virtual void getCurrentTimeStamp(TimeStamp& _ts) const override { _ts = getBuffer().get().back().ts_; }
- using IsMotion::getCurrentState;
- using IsMotion::getCurrentTimeStamp;
-
-
- /** \brief Fill the state corresponding to the provided time-stamp
- * \param _ts the time stamp
- * \param _x the returned state
- * \return if state in the provided time-stamp could be resolved
- */
- virtual bool getState(const TimeStamp& _ts, Eigen::VectorXd& _x) const override;
- using IsMotion::getState;
+ virtual TimeStamp getTimeStamp() const override;
+ virtual VectorComposite getState() const override;
+ virtual VectorComposite getState(const TimeStamp& _ts) const override;
/** \brief Gets the delta preintegrated covariance from all integrations so far
* \return the delta preintegrated covariance matrix
@@ -209,13 +198,12 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
* \param _x_origin the state at the origin
* \param _ts_origin origin timestamp.
*/
- FrameBasePtr setOrigin(const Eigen::VectorXd& _x_origin, const TimeStamp& _ts_origin);
-
+ FrameBasePtr setOrigin(const VectorComposite& _x_origin, const TimeStamp& _ts_origin);
+ // Helper functions:
MotionBuffer& getBuffer();
const MotionBuffer& getBuffer() const;
- // Helper functions:
protected:
/** \brief process an incoming capture
@@ -393,10 +381,10 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
*
* This function implements the composition (+) so that _x2 = _x1 (+) _delta.
*/
- virtual void statePlusDelta(const Eigen::VectorXd& _x,
+ virtual void statePlusDelta(const VectorComposite& _x,
const Eigen::VectorXd& _delta,
const double _dt,
- Eigen::VectorXd& _x_plus_delta) const = 0;
+ VectorComposite& _x_plus_delta) const = 0;
/** \brief Delta zero
* \return a delta state equivalent to the null motion.
@@ -409,6 +397,25 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
virtual Eigen::VectorXd deltaZero() const = 0;
+ /** \brief correct the preintegrated delta
+ * This produces a delta correction according to the appropriate manifold.
+ * @param delta_preint : the preintegrated delta to correct
+ * @param delta_step : an increment in the tangent space of the manifold
+ *
+ * We want to do
+ *
+ * delta_corr = delta_preint (+) d_step
+ *
+ * where the operator (+) is the right-plus operator on the delta's manifold.
+ *
+ * Note: usually in motion pre-integration we have
+ *
+ * delta_step = Jac_delta_calib * (calib - calib_pre)
+ *
+ */
+ virtual Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint,
+ const Eigen::VectorXd& delta_step) const = 0;
+
/** \brief emplace a CaptureMotion
* \param _ts time stamp
* \param _sensor Sensor that produced the Capture
@@ -437,6 +444,9 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
*/
virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin) = 0;
+ virtual VectorXd getCalibration (const CaptureBasePtr _capture) const = 0;
+ virtual void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) = 0;
+
Motion motionZero(const TimeStamp& _ts) const;
bool hasCalibration() const {return calib_size_ > 0;}
@@ -457,6 +467,13 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
void setDistTraveled(const double& _dist_traveled);
void setAngleTurned(const double& _angle_turned);
+ void printHeader(int depth, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream ,
+ std::string _tabs = "") const override;
+
protected:
// Attributes
SizeEigen x_size_; ///< The size of the state vector
@@ -500,28 +517,14 @@ inline bool ProcessorMotion::voteForKeyFrame() const
return false;
}
-inline void ProcessorMotion::getCurrentState(Eigen::VectorXd& _x) const
-{
- // ensure integrity
- assert(origin_ptr_ && "Trying to access origin_ptr_ but it is nullptr!");
-
- // ensure proper size of the provided reference
- Eigen::VectorXd curr_x = origin_ptr_->getFrame()->getState(state_structure_);
- _x.resize( curr_x.size() );
-
- // do get timestamp and state corrected by possible self-calibrations
- double Dt = getCurrentTimeStamp() - origin_ptr_->getTimeStamp();
- statePlusDelta(curr_x, last_ptr_->getDeltaCorrected(origin_ptr_->getCalibration()), Dt, _x);
-}
-
inline const Eigen::MatrixXd ProcessorMotion::getCurrentDeltaPreintCov() const
{
- return getBuffer().get().back().delta_integr_cov_;
+ return getBuffer().back().delta_integr_cov_;
}
inline Motion ProcessorMotion::getMotion() const
{
- return getBuffer().get().back();
+ return getBuffer().back();
}
inline Motion ProcessorMotion::getMotion(const TimeStamp& _ts) const
@@ -534,7 +537,7 @@ inline Motion ProcessorMotion::getMotion(const TimeStamp& _ts) const
inline double ProcessorMotion::updateDt()
{
- return dt_ = incoming_ptr_->getTimeStamp() - getBuffer().get().back().ts_;
+ return dt_ = incoming_ptr_->getTimeStamp() - getBuffer().back().ts_;
}
inline const MotionBuffer& ProcessorMotion::getBuffer() const
diff --git a/include/core/processor/processor_odom_2d.h b/include/core/processor/processor_odom_2d.h
index b03416837ec1b0a6a86a67ff0cf40fdf820b1370..0b873564bb7339e9acf63f1739076b9baa4ec29a 100644
--- a/include/core/processor/processor_odom_2d.h
+++ b/include/core/processor/processor_odom_2d.h
@@ -13,26 +13,27 @@
#include "core/factor/factor_odom_2d.h"
#include "core/math/rotations.h"
#include "core/utils/params_server.h"
+#include "core/math/SE2.h"
namespace wolf {
WOLF_PTR_TYPEDEFS(ProcessorOdom2d);
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsOdom2d);
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorOdom2d);
-struct ProcessorParamsOdom2d : public ProcessorParamsMotion
+struct ParamsProcessorOdom2d : public ParamsProcessorMotion
{
double cov_det = 1.0;
- ProcessorParamsOdom2d() = default;
- ProcessorParamsOdom2d(std::string _unique_name, const wolf::ParamsServer & _server) :
- ProcessorParamsMotion(_unique_name, _server)
+ ParamsProcessorOdom2d() = default;
+ ParamsProcessorOdom2d(std::string _unique_name, const wolf::ParamsServer & _server) :
+ ParamsProcessorMotion(_unique_name, _server)
{
cov_det = _server.getParam<double>(prefix + _unique_name + "/keyframe_vote/cov_det");
}
std::string print() const
{
- return "\n" + ProcessorParamsMotion::print() + "\n"
+ return "\n" + ParamsProcessorMotion::print() + "\n"
+ "cov_det: " + std::to_string(cov_det) + "\n";
}
};
@@ -40,12 +41,12 @@ struct ProcessorParamsOdom2d : public ProcessorParamsMotion
class ProcessorOdom2d : public ProcessorMotion
{
public:
- ProcessorOdom2d(ProcessorParamsOdom2dPtr _params);
+ ProcessorOdom2d(ParamsProcessorOdom2dPtr _params);
virtual ~ProcessorOdom2d();
- virtual void configure(SensorBasePtr _sensor) override;
+ virtual void configure(SensorBasePtr _sensor) override { };
// Factory method for high level API
- WOLF_PROCESSOR_CREATE(ProcessorOdom2d, ProcessorParamsOdom2d);
+ WOLF_PROCESSOR_CREATE(ProcessorOdom2d, ParamsProcessorOdom2d);
virtual bool voteForKeyFrame() const override;
@@ -67,11 +68,13 @@ class ProcessorOdom2d : public ProcessorMotion
Eigen::VectorXd& _delta1_plus_delta2,
Eigen::MatrixXd& _jacobian1,
Eigen::MatrixXd& _jacobian2) const override;
- virtual void statePlusDelta(const Eigen::VectorXd& _x,
+ virtual void statePlusDelta(const VectorComposite& _x,
const Eigen::VectorXd& _delta,
const double _Dt,
- Eigen::VectorXd& _x_plus_delta) const override;
+ VectorComposite& _x_plus_delta) const override;
virtual Eigen::VectorXd deltaZero() const override;
+ Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint,
+ const Eigen::VectorXd& delta_step) const override;
virtual CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
const SensorBasePtr& _sensor,
@@ -84,9 +87,11 @@ class ProcessorOdom2d : public ProcessorMotion
virtual FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_motion) override;
virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature,
CaptureBasePtr _capture_origin) override;
+ virtual VectorXd getCalibration (const CaptureBasePtr _capture) const override;
+ virtual void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
protected:
- ProcessorParamsOdom2dPtr params_odom_2d_;
+ ParamsProcessorOdom2dPtr params_odom_2d_;
};
@@ -95,6 +100,23 @@ inline Eigen::VectorXd ProcessorOdom2d::deltaZero() const
return Eigen::VectorXd::Zero(delta_size_);
}
+inline Eigen::VectorXd ProcessorOdom2d::correctDelta (const Eigen::VectorXd& delta_preint,
+ const Eigen::VectorXd& delta_step) const
+{
+ VectorXd delta_corrected(3);
+ SE2::plus(delta_preint, delta_step, delta_corrected);
+ return delta_corrected;
+}
+
+inline VectorXd ProcessorOdom2d::getCalibration (const CaptureBasePtr _capture) const
+{
+ return VectorXd::Zero(0);
+}
+
+inline void ProcessorOdom2d::setCalibration (const CaptureBasePtr _capture, const VectorXd& _calibration)
+{
+}
+
} // namespace wolf
#endif /* SRC_PROCESSOR_ODOM_2d_H_ */
diff --git a/include/core/processor/processor_odom_3d.h b/include/core/processor/processor_odom_3d.h
index f8f849b896aad644f998c38b50e8ca198c0738c4..49eaf4fad2788dde0499021ec89fb58d228cb65b 100644
--- a/include/core/processor/processor_odom_3d.h
+++ b/include/core/processor/processor_odom_3d.h
@@ -17,19 +17,19 @@
namespace wolf {
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsOdom3d);
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorOdom3d);
-struct ProcessorParamsOdom3d : public ProcessorParamsMotion
+struct ParamsProcessorOdom3d : public ParamsProcessorMotion
{
- ProcessorParamsOdom3d() = default;
- ProcessorParamsOdom3d(std::string _unique_name, const ParamsServer& _server):
- ProcessorParamsMotion(_unique_name, _server)
+ ParamsProcessorOdom3d() = default;
+ ParamsProcessorOdom3d(std::string _unique_name, const ParamsServer& _server):
+ ParamsProcessorMotion(_unique_name, _server)
{
//
}
std::string print() const
{
- return "\n" + ProcessorParamsMotion::print();
+ return "\n" + ParamsProcessorMotion::print();
}
};
@@ -60,12 +60,12 @@ WOLF_PTR_TYPEDEFS(ProcessorOdom3d);
class ProcessorOdom3d : public ProcessorMotion
{
public:
- ProcessorOdom3d(ProcessorParamsOdom3dPtr _params);
+ ProcessorOdom3d(ParamsProcessorOdom3dPtr _params);
virtual ~ProcessorOdom3d();
virtual void configure(SensorBasePtr _sensor) override;
// Factory method for high level API
- WOLF_PROCESSOR_CREATE(ProcessorOdom3d, ProcessorParamsOdom3d);
+ WOLF_PROCESSOR_CREATE(ProcessorOdom3d, ParamsProcessorOdom3d);
public:
// Motion integration
@@ -86,11 +86,14 @@ class ProcessorOdom3d : public ProcessorMotion
Eigen::VectorXd& _delta1_plus_delta2,
Eigen::MatrixXd& _jacobian1,
Eigen::MatrixXd& _jacobian2) const override;
- void statePlusDelta(const Eigen::VectorXd& _x,
- const Eigen::VectorXd& _delta,
- const double _Dt,
- Eigen::VectorXd& _x_plus_delta) const override;
+ void statePlusDelta(const VectorComposite& _x,
+ const Eigen::VectorXd& _delta,
+ const double _Dt,
+ VectorComposite& _x_plus_delta) const override;
Eigen::VectorXd deltaZero() const override;
+ Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint,
+ const Eigen::VectorXd& delta_step) const override;
+
bool voteForKeyFrame() const override;
virtual CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
const SensorBasePtr& _sensor,
@@ -105,9 +108,11 @@ class ProcessorOdom3d : public ProcessorMotion
virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion,
CaptureBasePtr _capture_origin) override;
+ virtual VectorXd getCalibration (const CaptureBasePtr _capture) const override;
+ virtual void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
protected:
- ProcessorParamsOdom3dPtr params_odom_3d_;
+ ParamsProcessorOdom3dPtr params_odom_3d_;
// noise parameters (stolen from owner SensorOdom3d)
double k_disp_to_disp_; // displacement variance growth per meter of linear motion
diff --git a/include/core/processor/processor_tracker.h b/include/core/processor/processor_tracker.h
index d7ab951643fbc2760b8d81110a9f2338a96dad36..0b7197574ae6a05064792dbabeaea9fd019abd0b 100644
--- a/include/core/processor/processor_tracker.h
+++ b/include/core/processor/processor_tracker.h
@@ -13,23 +13,23 @@
namespace wolf {
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTracker);
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorTracker);
-struct ProcessorParamsTracker : public ProcessorParamsBase
+struct ParamsProcessorTracker : public ParamsProcessorBase
{
unsigned int min_features_for_keyframe; ///< minimum nbr. of features to vote for keyframe
int max_new_features; ///< maximum nbr. of new features to be processed when adding a keyframe (-1: unlimited. 0: none.)
- ProcessorParamsTracker() = default;
- ProcessorParamsTracker(std::string _unique_name, const wolf::ParamsServer & _server):
- ProcessorParamsBase(_unique_name, _server)
+ ParamsProcessorTracker() = default;
+ ParamsProcessorTracker(std::string _unique_name, const wolf::ParamsServer & _server):
+ ParamsProcessorBase(_unique_name, _server)
{
min_features_for_keyframe = _server.getParam<unsigned int>(prefix + _unique_name + "/min_features_for_keyframe");
max_new_features = _server.getParam<int>(prefix + _unique_name + "/max_new_features");
}
std::string print() const
{
- return ProcessorParamsBase::print() + "\n"
+ return ParamsProcessorBase::print() + "\n"
+ "min_features_for_keyframe: " + std::to_string(min_features_for_keyframe) + "\n"
+ "max_new_features: " + std::to_string(max_new_features) + "\n";
}
@@ -96,22 +96,26 @@ class ProcessorTracker : public ProcessorBase
} ProcessingStep ;
protected:
- ProcessorParamsTrackerPtr params_tracker_; ///< parameters for the tracker
+ ParamsProcessorTrackerPtr params_tracker_; ///< parameters for the tracker
ProcessingStep processing_step_; ///< State machine controlling the processing step
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.
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
FeatureBasePtrList known_features_incoming_; ///< list of the known features in \b last successfully tracked in \b incoming
- int _dim;
+ StateStructure state_structure_; ///< Structure of frames created or used by this processor
public:
ProcessorTracker(const std::string& _type,
+ const StateStructure& _structure,
int _dim,
- ProcessorParamsTrackerPtr _params_tracker);
+ ParamsProcessorTrackerPtr _params_tracker);
virtual ~ProcessorTracker();
+ StateStructure getStateStructure() const;
+
bool checkTimeTolerance(const TimeStamp& _ts1, const TimeStamp& _ts2);
bool checkTimeTolerance(const CaptureBasePtr _cap, const TimeStamp& _ts);
bool checkTimeTolerance(const FrameBasePtr _frm, const TimeStamp& _ts);
@@ -246,6 +250,13 @@ class ProcessorTracker : public ProcessorBase
return this->params_tracker_->print();
}
+ void printHeader(int depth, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream ,
+ std::string _tabs = "") const override;
+
protected:
void computeProcessingStep();
@@ -293,6 +304,11 @@ inline bool ProcessorTracker::checkTimeTolerance(const FrameBasePtr _frm, const
return checkTimeTolerance(_frm->getTimeStamp(), _cap->getTimeStamp());
}
+inline StateStructure ProcessorTracker::getStateStructure ( ) const
+{
+ return state_structure_;
+}
+
inline void ProcessorTracker::addNewFeatureIncoming(FeatureBasePtr _feature_ptr)
{
new_features_incoming_.push_back(_feature_ptr);
diff --git a/include/core/processor/processor_tracker_feature.h b/include/core/processor/processor_tracker_feature.h
index b86e041d25e026762ea1413429b1c6e63a594fd7..b69599ef55fce9f4624b6fe9115722aeb86aa3ca 100644
--- a/include/core/processor/processor_tracker_feature.h
+++ b/include/core/processor/processor_tracker_feature.h
@@ -18,11 +18,11 @@
namespace wolf
{
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTrackerFeature);
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorTrackerFeature);
-struct ProcessorParamsTrackerFeature : public ProcessorParamsTracker
+struct ParamsProcessorTrackerFeature : public ParamsProcessorTracker
{
- using ProcessorParamsTracker::ProcessorParamsTracker;
+ using ParamsProcessorTracker::ParamsProcessorTracker;
};
WOLF_PTR_TYPEDEFS(ProcessorTrackerFeature);
@@ -85,12 +85,13 @@ class ProcessorTrackerFeature : public ProcessorTracker
/** \brief Constructor with type
*/
ProcessorTrackerFeature(const std::string& _type,
+ const StateStructure& _structure,
int _dim,
- ProcessorParamsTrackerFeaturePtr _params_tracker_feature);
+ ParamsProcessorTrackerFeaturePtr _params_tracker_feature);
virtual ~ProcessorTrackerFeature();
protected:
- ProcessorParamsTrackerFeaturePtr params_tracker_feature_;
+ ParamsProcessorTrackerFeaturePtr params_tracker_feature_;
TrackMatrix track_matrix_;
FeatureMatchMap matches_last_from_incoming_;
@@ -111,7 +112,7 @@ class ProcessorTrackerFeature : public ProcessorTracker
* - Create the factors, of the correct type, derived from FactorBase
* (through FactorAnalytic or FactorSparse).
*/
- virtual unsigned int processKnown();
+ virtual unsigned int processKnown() override;
/** \brief Track provided features in \b _capture
* \param _features_in input list of features in \b last to track
@@ -147,16 +148,16 @@ class ProcessorTrackerFeature : public ProcessorTracker
*
* WARNING! This function only votes! It does not create KeyFrames!
*/
- virtual bool voteForKeyFrame() const = 0;
+ virtual bool voteForKeyFrame() const override = 0;
// We overload the advance and reset functions to update the lists of matches
- void advanceDerived();
- void resetDerived();
+ virtual void advanceDerived() override;
+ virtual void resetDerived() override;
/**\brief Process new Features
*
*/
- virtual unsigned int processNew(const int& _max_features);
+ virtual unsigned int processNew(const int& _max_features) override;
/** \brief Detect new Features
* \param _max_features maximum number of features detected (-1: unlimited. 0: none)
@@ -190,7 +191,7 @@ class ProcessorTrackerFeature : public ProcessorTracker
/** \brief Emplaces a new factor for each correspondence between a feature in Capture \b last and a feature in Capture \b origin
*/
- virtual void establishFactors();
+ virtual void establishFactors() override;
};
} // namespace wolf
diff --git a/include/core/processor/processor_tracker_landmark.h b/include/core/processor/processor_tracker_landmark.h
index 8809426f8654803fc7012068ff9d89cd1d70ea4c..6181810bde23395d70c8e28cabc2457f4b13f41b 100644
--- a/include/core/processor/processor_tracker_landmark.h
+++ b/include/core/processor/processor_tracker_landmark.h
@@ -16,11 +16,11 @@
namespace wolf
{
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTrackerLandmark);
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorTrackerLandmark);
-struct ProcessorParamsTrackerLandmark : public ProcessorParamsTracker
+struct ParamsProcessorTrackerLandmark : public ParamsProcessorTracker
{
- using ProcessorParamsTracker::ProcessorParamsTracker;
+ using ParamsProcessorTracker::ParamsProcessorTracker;
//
};
@@ -81,12 +81,13 @@ class ProcessorTrackerLandmark : public ProcessorTracker
{
public:
ProcessorTrackerLandmark(const std::string& _type,
- ProcessorParamsTrackerLandmarkPtr _params_tracker_landmark);
+ const StateStructure& _structure,
+ ParamsProcessorTrackerLandmarkPtr _params_tracker_landmark);
virtual ~ProcessorTrackerLandmark();
protected:
- ProcessorParamsTrackerLandmarkPtr params_tracker_landmark_;
+ ParamsProcessorTrackerLandmarkPtr params_tracker_landmark_;
LandmarkBasePtrList new_landmarks_; ///< List of new detected landmarks
LandmarkMatchMap matches_landmark_from_incoming_;
LandmarkMatchMap matches_landmark_from_last_;
diff --git a/include/core/sensor/sensor_factory.h b/include/core/sensor/factory_sensor.h
similarity index 78%
rename from include/core/sensor/sensor_factory.h
rename to include/core/sensor/factory_sensor.h
index daf01499ac759d799517cdd73ab830fb6ccaf4d5..ff0574aaadc6a52015a6c9e4f076a1ec64d968c0 100644
--- a/include/core/sensor/sensor_factory.h
+++ b/include/core/sensor/factory_sensor.h
@@ -1,12 +1,12 @@
/**
- * \file sensor_factory.h
+ * \file factory_sensor.h
*
* Created on: Apr 25, 2016
* \author: jsola
*/
-#ifndef SENSOR_FACTORY_H_
-#define SENSOR_FACTORY_H_
+#ifndef FACTORY_SENSOR_H_
+#define FACTORY_SENSOR_H_
namespace wolf
{
@@ -47,17 +47,17 @@ namespace wolf
* - Write a sensor creator for SensorCamera (example).
*
* #### Accessing the factory
- * The SensorFactory class is a <a href="http://stackoverflow.com/questions/1008019/c-singleton-design-pattern#1008289">singleton</a>: it can only exist once in your application.
+ * The FactorySensor class is a <a href="http://stackoverflow.com/questions/1008019/c-singleton-design-pattern#1008289">singleton</a>: it can only exist once in your application.
* To obtain an instance of it, use the static method get(),
*
* \code
- * SensorFactory::get()
+ * FactorySensor::get()
* \endcode
*
* You can then call the methods you like, e.g. to create a sensor, you type:
*
* \code
- * SensorFactory::get().create(...); // see below for creating sensors ...
+ * FactorySensor::get().create(...); // see below for creating sensors ...
* \endcode
*
* #### Registering sensor creators
@@ -69,7 +69,7 @@ namespace wolf
* that knows how to create your specific sensor, e.g.:
*
* \code
- * SensorFactory::get().registerCreator("CAMERA", SensorCamera::create);
+ * FactorySensor::get().registerCreator("CAMERA", SensorCamera::create);
* \endcode
*
* The method SensorCamera::create() exists in the SensorCamera class as a static method.
@@ -89,7 +89,7 @@ namespace wolf
* For example, in sensor_camera.cpp we find the line:
*
* \code
- * const bool registered_camera = SensorFactory::get().registerCreator("CAMERA", SensorCamera::create);
+ * const bool registered_camera = FactorySensor::get().registerCreator("CAMERA", SensorCamera::create);
* \endcode
*
* which is a static invocation (i.e., it is placed at global scope outside of the SensorCamera class).
@@ -99,7 +99,7 @@ namespace wolf
* The method unregisterCreator() unregisters the SensorXxx::create() method. It only needs to be passed the string of the sensor type.
*
* \code
- * SensorFactory::get().unregisterCreator("CAMERA");
+ * FactorySensor::get().unregisterCreator("CAMERA");
* \endcode
*
* #### Creating sensors
@@ -109,15 +109,15 @@ namespace wolf
* To create e.g. a SensorCamera, you type:
*
* \code
- * SensorFactory::get().create("CAMERA", "Front-left camera", extrinsics, intrinsics_ptr);
+ * FactorySensor::get().create("CAMERA", "Front-left camera", extrinsics, intrinsics_ptr);
* \endcode
*
* where ABSOLUTELY ALL input parameters are important. In particular, the sensor name "Front-left camera" will be used to identify this camera
* and to assign it the appropriate processors. DO NOT USE IT WITH DUMMY PARAMETERS!
*
* #### See also
- * - ParamsSensorFactory: to create intrinsic structs deriving from ParamsSensorBase directly from YAML files.
- * - ProcessorFactory: to create processors that will be bound to sensors.
+ * - FactoryParamsSensor: to create intrinsic structs deriving from ParamsSensorBase directly from YAML files.
+ * - FactoryProcessor: to create processors that will be bound to sensors.
* - Problem::installSensor() : to install sensors in WOLF Problem.
*
* #### Example 1: writing a specific sensor creator
@@ -148,14 +148,14 @@ namespace wolf
* Put the code either at global scope (you must define a dummy variable for this),
* \code
* namespace {
- * const bool registered_camera = SensorFactory::get().registerCreator("CAMERA", SensorCamera::create);
+ * const bool registered_camera = FactorySensor::get().registerCreator("CAMERA", SensorCamera::create);
* }
* main () { ... }
* \endcode
* or inside your main(), where a direct call is possible:
* \code
* main () {
- * SensorFactory::get().registerCreator("CAMERA", SensorCamera::create);
+ * FactorySensor::get().registerCreator("CAMERA", SensorCamera::create);
* ...
* }
* \endcode
@@ -164,7 +164,7 @@ namespace wolf
* Put the code at the last line of the sensor_xxx.cpp file,
* \code
* namespace {
- * const bool registered_camera = SensorFactory::get().registerCreator("CAMERA", SensorCamera::create);
+ * const bool registered_camera = FactorySensor::get().registerCreator("CAMERA", SensorCamera::create);
* }
* \endcode
* Automatic registration is recommended in wolf, and implemented in the classes shipped with it.
@@ -174,7 +174,7 @@ namespace wolf
* We finally provide the necessary steps to create a sensor of class SensorCamera in our application:
*
* \code
- * #include "sensor_factory.h"
+ * #include "factory_sensor.h"
* #include "core/sensor/sensor_camera.h" // provides SensorCamera
*
* // Note: SensorCamera::create() is already registered, automatically.
@@ -189,10 +189,10 @@ namespace wolf
* // a pointer to the intrinsics struct:
*
* Eigen::VectorXd extrinsics_1(7); // give it some values...
- * ParamsSensorCamera intrinsics_1({...}); // see ParamsSensorFactory to fill in the derived struct
+ * ParamsSensorCamera intrinsics_1({...}); // see FactoryParamsSensor to fill in the derived struct
*
* SensorBasePtr camera_1_ptr =
- * SensorFactory::get().create ( "CAMERA" , "Front-left camera" , extrinsics_1 , &intrinsics_1 );
+ * FactorySensor::get().create ( "CAMERA" , "Front-left camera" , extrinsics_1 , &intrinsics_1 );
*
* // A second camera... with a different name!
*
@@ -200,7 +200,7 @@ namespace wolf
* ParamsSensorCamera intrinsics_2({...});
*
* SensorBasePtr camera_2_ptr =
- * SensorFactory::get().create( "CAMERA" , "Front-right camera" , extrinsics_2 , &intrinsics_2 );
+ * FactorySensor::get().create( "CAMERA" , "Front-right camera" , extrinsics_2 , &intrinsics_2 );
*
* return 0;
* }
@@ -212,42 +212,43 @@ namespace wolf
// ParamsSensor factory
struct ParamsSensorBase;
typedef Factory<ParamsSensorBase,
- const std::string&> ParamsSensorFactory;
+ const std::string&> FactoryParamsSensor;
template<>
-inline std::string ParamsSensorFactory::getClass()
+inline std::string FactoryParamsSensor::getClass() const
{
- return "ParamsSensorFactory";
+ return "FactoryParamsSensor";
}
// Sensor factory
typedef Factory<SensorBase,
const std::string&,
- const Eigen::VectorXd&, const ParamsSensorBasePtr> SensorFactory;
+ const Eigen::VectorXd&,
+ const ParamsSensorBasePtr> FactorySensor;
template<>
-inline std::string SensorFactory::getClass()
+inline std::string FactorySensor::getClass() const
{
- return "SensorFactory";
+ return "FactorySensor";
}
-#define WOLF_REGISTER_SENSOR(SensorType, SensorName) \
- namespace{ const bool WOLF_UNUSED SensorName##Registered = \
- SensorFactory::get().registerCreator(SensorType, SensorName::create); } \
+#define WOLF_REGISTER_SENSOR(SensorType) \
+ namespace{ const bool WOLF_UNUSED SensorType##Registered = \
+ FactorySensor::get().registerCreator(#SensorType, SensorType::create); } \
typedef Factory<SensorBase,
const std::string&,
- const ParamsServer&> AutoConfSensorFactory;
+ const ParamsServer&> AutoConfFactorySensor;
template<>
-inline std::string AutoConfSensorFactory::getClass()
+inline std::string AutoConfFactorySensor::getClass() const
{
- return "AutoConfSensorFactory";
+ return "AutoConfFactorySensor";
}
-#define WOLF_REGISTER_SENSOR_AUTO(SensorType, SensorName) \
- namespace{ const bool WOLF_UNUSED SensorName##AutoConfRegistered = \
- AutoConfSensorFactory::get().registerCreator(SensorType, SensorName::create); } \
+#define WOLF_REGISTER_SENSOR_AUTO(SensorType) \
+ namespace{ const bool WOLF_UNUSED SensorType##AutoConfRegistered = \
+ AutoConfFactorySensor::get().registerCreator(#SensorType, SensorType::create); } \
} /* namespace wolf */
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index 1c099cb9be4827705bd43d229714ea1dabf24ed0..e1be8095b90f395477ea72ed0302520fb0ae4bbb 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -242,6 +242,22 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
Eigen::VectorXd getNoiseStd() const;
Eigen::MatrixXd getNoiseCov() const;
+ virtual void printHeader(int depth, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream ,
+ std::string _tabs = "") const;
+ void print(int depth, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream = std::cout,
+ std::string _tabs = "") const;
+
+ virtual CheckLog localCheck(bool _verbose, SensorBasePtr _sen_ptr, std::ostream& _stream, std::string _tabs = "") const;
+ bool check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
+
void link(HardwareBasePtr);
template<typename classType, typename... T>
static std::shared_ptr<classType> emplace(HardwareBasePtr _hwd_ptr, T&&... all);
diff --git a/include/core/solver/solver_factory.h b/include/core/solver/factory_solver.h
similarity index 83%
rename from include/core/solver/solver_factory.h
rename to include/core/solver/factory_solver.h
index d80fe24f78ddaa91a44e7821f2dc0a55117b8dcf..e3cc58a4a0b6d269026312f9c946a490d1045923 100644
--- a/include/core/solver/solver_factory.h
+++ b/include/core/solver/factory_solver.h
@@ -1,12 +1,12 @@
/**
- * \file solver_factory.h
+ * \file factory_solver.h
*
* Created on: Dec 17, 2018
* \author: jcasals
*/
-#ifndef SOLVER_FACTORY_H_
-#define SOLVER_FACTORY_H_
+#ifndef FACTORY_SOLVER_H_
+#define FACTORY_SOLVER_H_
namespace wolf
{
@@ -47,17 +47,17 @@ namespace wolf
* - Write a sensor creator for SensorCamera (example).
*
* #### Accessing the factory
- * The SensorFactory class is a <a href="http://stackoverflow.com/questions/1008019/c-singleton-design-pattern#1008289">singleton</a>: it can only exist once in your application.
+ * The FactorySensor class is a <a href="http://stackoverflow.com/questions/1008019/c-singleton-design-pattern#1008289">singleton</a>: it can only exist once in your application.
* To obtain an instance of it, use the static method get(),
*
* \code
- * SensorFactory::get()
+ * FactorySensor::get()
* \endcode
*
* You can then call the methods you like, e.g. to create a sensor, you type:
*
* \code
- * SensorFactory::get().create(...); // see below for creating sensors ...
+ * FactorySensor::get().create(...); // see below for creating sensors ...
* \endcode
*
* #### Registering sensor creators
@@ -69,7 +69,7 @@ namespace wolf
* that knows how to create your specific sensor, e.g.:
*
* \code
- * SensorFactory::get().registerCreator("CAMERA", SensorCamera::create);
+ * FactorySensor::get().registerCreator("CAMERA", SensorCamera::create);
* \endcode
*
* The method SensorCamera::create() exists in the SensorCamera class as a static method.
@@ -89,7 +89,7 @@ namespace wolf
* For example, in sensor_camera.cpp we find the line:
*
* \code
- * const bool registered_camera = SensorFactory::get().registerCreator("CAMERA", SensorCamera::create);
+ * const bool registered_camera = FactorySensor::get().registerCreator("CAMERA", SensorCamera::create);
* \endcode
*
* which is a static invocation (i.e., it is placed at global scope outside of the SensorCamera class).
@@ -99,7 +99,7 @@ namespace wolf
* The method unregisterCreator() unregisters the SensorXxx::create() method. It only needs to be passed the string of the sensor type.
*
* \code
- * SensorFactory::get().unregisterCreator("CAMERA");
+ * FactorySensor::get().unregisterCreator("CAMERA");
* \endcode
*
* #### Creating sensors
@@ -109,15 +109,15 @@ namespace wolf
* To create e.g. a SensorCamera, you type:
*
* \code
- * SensorFactory::get().create("CAMERA", "Front-left camera", extrinsics, intrinsics_ptr);
+ * FactorySensor::get().create("CAMERA", "Front-left camera", extrinsics, intrinsics_ptr);
* \endcode
*
* where ABSOLUTELY ALL input parameters are important. In particular, the sensor name "Front-left camera" will be used to identify this camera
* and to assign it the appropriate processors. DO NOT USE IT WITH DUMMY PARAMETERS!
*
* #### See also
- * - ParamsSensorFactory: to create intrinsic structs deriving from ParamsSensorBase directly from YAML files.
- * - ProcessorFactory: to create processors that will be bound to sensors.
+ * - FactoryParamsSensor: to create intrinsic structs deriving from ParamsSensorBase directly from YAML files.
+ * - FactoryProcessor: to create processors that will be bound to sensors.
* - Problem::installSensor() : to install sensors in WOLF Problem.
*
* #### Example 1: writing a specific sensor creator
@@ -148,14 +148,14 @@ namespace wolf
* Put the code either at global scope (you must define a dummy variable for this),
* \code
* namespace {
- * const bool registered_camera = SensorFactory::get().registerCreator("CAMERA", SensorCamera::create);
+ * const bool registered_camera = FactorySensor::get().registerCreator("CAMERA", SensorCamera::create);
* }
* main () { ... }
* \endcode
* or inside your main(), where a direct call is possible:
* \code
* main () {
- * SensorFactory::get().registerCreator("CAMERA", SensorCamera::create);
+ * FactorySensor::get().registerCreator("CAMERA", SensorCamera::create);
* ...
* }
* \endcode
@@ -164,7 +164,7 @@ namespace wolf
* Put the code at the last line of the sensor_xxx.cpp file,
* \code
* namespace {
- * const bool registered_camera = SensorFactory::get().registerCreator("CAMERA", SensorCamera::create);
+ * const bool registered_camera = FactorySensor::get().registerCreator("CAMERA", SensorCamera::create);
* }
* \endcode
* Automatic registration is recommended in wolf, and implemented in the classes shipped with it.
@@ -174,7 +174,7 @@ namespace wolf
* We finally provide the necessary steps to create a sensor of class SensorCamera in our application:
*
* \code
- * #include "sensor_factory.h"
+ * #include "factory_sensor.h"
* #include "sensor_camera.h" // provides SensorCamera
*
* // Note: SensorCamera::create() is already registered, automatically.
@@ -189,10 +189,10 @@ namespace wolf
* // a pointer to the intrinsics struct:
*
* Eigen::VectorXd extrinsics_1(7); // give it some values...
- * ParamsSensorCamera intrinsics_1({...}); // see ParamsSensorFactory to fill in the derived struct
+ * ParamsSensorCamera intrinsics_1({...}); // see FactoryParamsSensor to fill in the derived struct
*
* SensorBasePtr camera_1_ptr =
- * SensorFactory::get().create ( "CAMERA" , "Front-left camera" , extrinsics_1 , &intrinsics_1 );
+ * FactorySensor::get().create ( "CAMERA" , "Front-left camera" , extrinsics_1 , &intrinsics_1 );
*
* // A second camera... with a different name!
*
@@ -200,7 +200,7 @@ namespace wolf
* ParamsSensorCamera intrinsics_2({...});
*
* SensorBasePtr camera_2_ptr =
- * SensorFactory::get().create( "CAMERA" , "Front-right camera" , extrinsics_2 , &intrinsics_2 );
+ * FactorySensor::get().create( "CAMERA" , "Front-right camera" , extrinsics_2 , &intrinsics_2 );
*
* return 0;
* }
@@ -211,17 +211,17 @@ namespace wolf
typedef Factory<SolverManager,
const ProblemPtr&,
- const ParamsServer&> SolverFactory;
+ const ParamsServer&> FactorySolver;
template<>
-inline std::string SolverFactory::getClass()
+inline std::string FactorySolver::getClass() const
{
- return "SolverFactory";
+ return "FactorySolver";
}
-#define WOLF_REGISTER_SOLVER(SolverType, SolverName) \
- namespace{ const bool WOLF_UNUSED SolverName##Registered = \
- wolf::SolverFactory::get().registerCreator(SolverType, SolverName::create); } \
+#define WOLF_REGISTER_SOLVER(SolverType) \
+ namespace{ const bool WOLF_UNUSED SolverType##Registered = \
+ wolf::FactorySolver::get().registerCreator(#SolverType, SolverType::create); } \
} /* namespace wolf */
diff --git a/include/core/solver/solver_manager.h b/include/core/solver/solver_manager.h
index c2b2c161caa8c3ee8401e8d1b3426ac8f8f64db0..3a5e613276c432db263cf43a484b2550ec1cfac2 100644
--- a/include/core/solver/solver_manager.h
+++ b/include/core/solver/solver_manager.h
@@ -93,7 +93,7 @@ public:
SolverManager(const ProblemPtr& wolf_problem);
- virtual ~SolverManager() = default;
+ virtual ~SolverManager();
std::string solve(const ReportVerbosity report_level = ReportVerbosity::QUIET);
@@ -117,30 +117,40 @@ public:
virtual bool isFactorRegistered(const FactorBasePtr& fac_ptr) const;
-protected:
-
- std::map<StateBlockPtr, Eigen::VectorXd> state_blocks_;
-
- virtual Eigen::VectorXd& getAssociatedMemBlock(const StateBlockPtr& state_ptr);
- virtual double* getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr);
-
- virtual std::string solveImpl(const ReportVerbosity report_level) = 0;
-
- virtual void addFactor(const FactorBasePtr& fac_ptr) = 0;
+ bool check(std::string prefix="") const;
- virtual void removeFactor(const FactorBasePtr& fac_ptr) = 0;
-
- virtual void addStateBlock(const StateBlockPtr& state_ptr) = 0;
-
- virtual void removeStateBlock(const StateBlockPtr& state_ptr) = 0;
+protected:
- virtual void updateStateBlockStatus(const StateBlockPtr& state_ptr) = 0;
+ std::map<StateBlockPtr, Eigen::VectorXd> state_blocks_;
+ std::map<StateBlockPtr, FactorBasePtrList> state_blocks_2_factors_;
+ std::set<FactorBasePtr> factors_;
- virtual void updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr) = 0;
+ virtual Eigen::VectorXd& getAssociatedMemBlock(const StateBlockPtr& state_ptr);
+ const double* getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr) const;
+ virtual double* getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr);
- virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) = 0;
+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 bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const = 0;
+protected:
+ // Derived virtual functions
+ virtual std::string solveDerived(const ReportVerbosity report_level) = 0;
+ virtual void addFactorDerived(const FactorBasePtr& fac_ptr) = 0;
+ virtual void removeFactorDerived(const FactorBasePtr& fac_ptr) = 0;
+ virtual void addStateBlockDerived(const StateBlockPtr& state_ptr) = 0;
+ 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 isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const = 0;
+ virtual bool checkDerived(std::string prefix="") const = 0;
};
} // namespace wolf
diff --git a/include/core/state_block/factory_state_block.h b/include/core/state_block/factory_state_block.h
new file mode 100644
index 0000000000000000000000000000000000000000..c353121b724011b993942d5a567166963a7440d0
--- /dev/null
+++ b/include/core/state_block/factory_state_block.h
@@ -0,0 +1,51 @@
+/*
+ * factory_state_block.h
+ *
+ * Created on: Apr 27, 2020
+ * Author: jsola
+ */
+
+#ifndef STATE_BLOCK_FACTORY_STATE_BLOCK_H_
+#define STATE_BLOCK_FACTORY_STATE_BLOCK_H_
+
+#include "core/common/factory.h"
+#include "core/state_block/state_block.h"
+
+namespace wolf
+{
+
+// State blocks factory
+typedef Factory<StateBlock, const Eigen::VectorXd&, bool> FactoryStateBlock;
+template<>
+inline std::string FactoryStateBlock::getClass() const
+{
+ return "FactoryStateBlock";
+}
+template<>
+inline StateBlockPtr FactoryStateBlock::create(const std::string& _type, const Eigen::VectorXd& _state, bool _fixed)
+{
+ typename CallbackMap::const_iterator creator_callback_it = callbacks_.find(_type);
+
+ if (creator_callback_it == callbacks_.end())
+ // not found: return StateBlock base
+ return std::make_shared<StateBlock>(_state, _fixed);
+
+ // Invoke the creation function
+ return (creator_callback_it->second)(_state, _fixed);
+}
+
+#define WOLF_REGISTER_STATEBLOCK(StateBlockType) \
+ namespace{ const bool WOLF_UNUSED StateBlockType##Registered = \
+ FactoryStateBlock::get().registerCreator(#StateBlockType, StateBlockType::create); } \
+
+#define WOLF_REGISTER_STATEBLOCK_WITH_KEY(Key, StateBlockType) \
+ namespace{ const bool WOLF_UNUSED StateBlockType##RegisteredWith##Key = \
+ FactoryStateBlock::get().registerCreator(#Key, StateBlockType::create); } \
+
+
+
+
+}
+
+
+#endif /* STATE_BLOCK_FACTORY_STATE_BLOCK_H_ */
diff --git a/include/core/state_block/has_state_blocks.h b/include/core/state_block/has_state_blocks.h
index 07c6d757bc09125c4a4e8a55aefd6d37b480c7d4..9462efe7a6dd314ff2ab94cbf923ee79f5c4e21e 100644
--- a/include/core/state_block/has_state_blocks.h
+++ b/include/core/state_block/has_state_blocks.h
@@ -9,6 +9,7 @@
#define STATE_BLOCK_HAS_STATE_BLOCKS_H_
#include "core/common/wolf.h"
+#include "core/state_block/state_composite.h"
#include <map>
@@ -17,6 +18,7 @@ namespace wolf
/// State of nodes containing several state blocks
typedef std::unordered_map<std::string, Eigen::VectorXd> State;
+//typedef std::string StateStructure;
class HasStateBlocks
@@ -26,7 +28,7 @@ class HasStateBlocks
HasStateBlocks(const std::string& _structure) : structure_(_structure), state_block_map_() {}
virtual ~HasStateBlocks();
- const std::string& getStructure() const { return structure_; }
+ const StateStructure& getStructure() const { return structure_; }
void appendToStructure(const std::string& _frame_type){ structure_ += _frame_type; }
bool isInStructure(const std::string& _sb_type) { return structure_.find(_sb_type) != std::string::npos; }
@@ -62,27 +64,29 @@ class HasStateBlocks
// Emplace base state blocks.
template<typename ... Args>
- inline StateBlockPtr emplaceStateBlock(const std::string& _sb_type, ProblemPtr _problem, Args&&... _args_of_base_state_block_constructor);
+ StateBlockPtr emplaceStateBlock(const std::string& _sb_type, ProblemPtr _problem, Args&&... _args_of_base_state_block_constructor);
// Register/remove state blocks to/from wolf::Problem
void registerNewStateBlocks(ProblemPtr _problem) const;
void removeStateBlocks(ProblemPtr _problem);
// States
- inline void setState(const Eigen::VectorXd& _state, std::string _sub_structure="", const bool _notify = true);
- void getState(Eigen::VectorXd& _state, std::string structure="") const;
- Eigen::VectorXd getState(std::string structure="") const;
- unsigned int getSize(std::string _sub_structure="") const;
- unsigned int getLocalSize(std::string _sub_structure="") const;
+ VectorComposite getState(const StateStructure& structure="") const;
+ void setState(const VectorComposite& _state, const bool _notify = true);
+ void setState(const Eigen::VectorXd& _state, const StateStructure& _structure, const std::list<SizeEigen>& _sizes, const bool _notify = true);
+ void setState(const Eigen::VectorXd& _state, const StateStructure& _sub_structure="", const bool _notify = true);
+ void setState(const StateStructure& _structure, const std::list<VectorXd>& _vectors, const bool _notify = true);
- State getStateComposite();
+ VectorXd getStateVector(const StateStructure& structure="") const;
+ unsigned int getSize(const StateStructure& _sub_structure="") const;
+ unsigned int getLocalSize(const StateStructure& _sub_structure="") const;
// Perturb state
void perturb(double amplitude = 0.01);
private:
- std::string structure_;
+ StateStructure structure_;
std::unordered_map<std::string, StateBlockPtr> state_block_map_;
};
@@ -207,74 +211,125 @@ inline bool HasStateBlocks::isFixed() const
return fixed;
}
-inline void HasStateBlocks::setState(const Eigen::VectorXd& _state, std::string _sub_structure, const bool _notify)
+inline void HasStateBlocks::setState(const VectorComposite& _state, const bool _notify)
{
- if (_sub_structure == ""){
- _sub_structure = structure_;
+ for (const auto& pair_key_vec : _state)
+ {
+ const auto& key = pair_key_vec.first;
+ const auto& vec = pair_key_vec.second;
+ const auto& sb = getStateBlock(key);
+ if (!sb)
+ WOLF_ERROR("Stateblock key ", key, " not in the structure");
+
+ sb->setState(vec, _notify);
}
- int size = getSize(_sub_structure);
+}
+
+inline void HasStateBlocks::setState(const Eigen::VectorXd& _state, const StateStructure& _sub_structure, const bool _notify)
+{
+ StateStructure structure;
+ if (_sub_structure == "")
+ structure = structure_;
+ else
+ structure = _sub_structure;
+
+ int size = getSize(structure);
assert(_state.size() == size && "In FrameBase::setState wrong state size");
unsigned int index = 0;
- for (const char key : _sub_structure)
+ for (const char ckey : structure)
{
- const auto& sb = getStateBlock(key);
- if (!sb){
- WOLF_ERROR("Stateblock key ", key, " not in the structure");
- }
+ const auto& key = string(1,ckey);
+ const auto& sb = getStateBlock(key);
+ assert (sb && "Stateblock key not in the structure");
+
sb->setState(_state.segment(index, sb->getSize()), _notify); // do not notify if state block is not estimated by the solver
index += sb->getSize();
}
}
-// _sub_structure can be either stateblock structure of the node or a subset of this structure
-inline void HasStateBlocks::getState(Eigen::VectorXd& _state, std::string _sub_structure) const
+inline void HasStateBlocks::setState (const Eigen::VectorXd& _state,
+ const StateStructure& _structure,
+ const std::list<SizeEigen>& _sizes,
+ const bool _notify)
{
- if (_sub_structure == ""){
- _sub_structure = structure_;
+ SizeEigen index = 0;
+ auto size_it = _sizes.begin();
+ for (const auto& ckey : _structure)
+ {
+ const auto& key = string(1,ckey);
+ const auto& sb = getStateBlock(key);
+ assert (sb && "Stateblock key not in the structure");
+ assert(*size_it == sb->getSize() && "State block size mismatch");
+
+ sb->setState(_state.segment(index, *size_it), _notify);
+ index += *size_it;
+ size_it ++;
}
- _state.resize(getSize(_sub_structure));
+}
- unsigned int index = 0;
- for (const char key : _sub_structure)
+
+inline void HasStateBlocks::setState(const StateStructure& _structure, const std::list<VectorXd>& _vectors, const bool _notify)
+{
+ auto vec_it = _vectors.begin();
+ for (const auto ckey : _structure)
{
+ const auto key = string(1,ckey);
const auto& sb = getStateBlock(key);
- if (!sb){
- WOLF_ERROR("Stateblock key ", key, " not in the structure");
- }
- _state.segment(index,sb->getSize()) = sb->getState();
- index += sb->getSize();
- }
+ assert (sb && "Stateblock key not in the structure");
+ assert(vec_it->size() == sb->getSize() && "State block size mismatch");
+ sb->setState(*vec_it, _notify);
+ vec_it ++;
+ }
}
-inline Eigen::VectorXd HasStateBlocks::getState(std::string _sub_structure) const
+
+//// _sub_structure can be either stateblock structure of the node or a subset of this structure
+inline VectorXd HasStateBlocks::getStateVector(const StateStructure& _sub_structure) const
{
- Eigen::VectorXd state;
+ StateStructure structure;
+ if (_sub_structure == "")
+ structure = structure_;
+ else
+ structure = _sub_structure;
- getState(state, _sub_structure);
+ VectorXd state(getSize(structure));
+ unsigned int index = 0;
+ for (const char key : structure)
+ {
+ const auto& sb = getStateBlock(key);
+ if (!sb){
+ WOLF_ERROR("Stateblock key ", key, " not in the structure");
+ }
+ state.segment(index,sb->getSize()) = sb->getState();
+ index += sb->getSize();
+ }
return state;
}
-inline State HasStateBlocks::getStateComposite()
+inline VectorComposite HasStateBlocks::getState(const StateStructure& _structure) const
{
- State state;
- for (auto& pair_key_kf : state_block_map_)
+ VectorComposite state;
+ for (auto& pair_key_sb : state_block_map_)
{
- state.emplace(pair_key_kf.first, pair_key_kf.second->getState());
+ state.emplace(pair_key_sb.first, pair_key_sb.second->getState());
}
return state;
}
-inline unsigned int HasStateBlocks::getSize(std::string _sub_structure) const
+inline unsigned int HasStateBlocks::getSize(const StateStructure& _sub_structure) const
{
- if (_sub_structure == ""){
- _sub_structure = structure_;
- }
+ StateStructure structure;
+ if (_sub_structure == "")
+ structure = structure_;
+ else
+ structure = _sub_structure;
+
unsigned int size = 0;
- for (const char key : _sub_structure)
+ for (const char key : structure)
{
const auto& sb = getStateBlock(key);
if (!sb){
@@ -325,16 +380,16 @@ inline bool HasStateBlocks::stateBlockKey(const StateBlockPtr &_sb, std::string&
}
}
-//inline unsigned int HasStateBlocks::getLocalSize() const
-//=======
-inline unsigned int HasStateBlocks::getLocalSize(std::string _sub_structure) const
-//>>>>>>> devel
+inline unsigned int HasStateBlocks::getLocalSize(const StateStructure& _sub_structure) const
{
- if (_sub_structure == ""){
- _sub_structure = structure_;
- }
+ StateStructure structure;
+ if (_sub_structure == "")
+ structure = structure_;
+ else
+ structure = _sub_structure;
+
unsigned int size = 0;
- for (const char key : _sub_structure)
+ for (const char key : structure)
{
const auto& sb = getStateBlock(key);
if (!sb){
diff --git a/include/core/state_block/local_parametrization_base.h b/include/core/state_block/local_parametrization_base.h
index 32599d9dc72a556c06f9ade7df6339f38cc7c8cf..14f2ce38f1dd55b6571884710eb7ed7b7640f985 100644
--- a/include/core/state_block/local_parametrization_base.h
+++ b/include/core/state_block/local_parametrization_base.h
@@ -12,8 +12,6 @@
namespace wolf {
-//WOLF_PTR_TYPEDEFS(LocalParametrizationBase);
-
class LocalParametrizationBase{
protected:
unsigned int global_size_;
@@ -22,13 +20,17 @@ class LocalParametrizationBase{
LocalParametrizationBase(unsigned int _global_size, unsigned int _local_size);
virtual ~LocalParametrizationBase();
+ bool plus(const Eigen::VectorXd& _x,
+ const Eigen::VectorXd& _delta,
+ Eigen::VectorXd& _x_plus_delta) const;
virtual bool plus(Eigen::Map<const Eigen::VectorXd>& _x,
Eigen::Map<const Eigen::VectorXd>& _delta,
- Eigen::Map<Eigen::VectorXd>& _x_plus_delta) const = 0;
- virtual bool computeJacobian(Eigen::Map<const Eigen::VectorXd>& _x, Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const = 0;
+ Eigen::Map<Eigen::VectorXd>& _x_plus_delta) const = 0;
+ virtual bool computeJacobian(Eigen::Map<const Eigen::VectorXd>& _x,
+ Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const = 0;
virtual bool minus(Eigen::Map<const Eigen::VectorXd>& _x1,
Eigen::Map<const Eigen::VectorXd>& _x2,
- Eigen::Map<Eigen::VectorXd>& _x2_minus_x1) = 0;
+ Eigen::Map<Eigen::VectorXd>& _x2_minus_x1) = 0;
unsigned int getLocalSize() const;
unsigned int getGlobalSize() const;
diff --git a/include/core/state_block/state_angle.h b/include/core/state_block/state_angle.h
index 05a87f0f25bb2af1466da5a5a5b4608b64f2a883..8b714b64c8b3bd82699fab1d845887e2bd74e10c 100644
--- a/include/core/state_block/state_angle.h
+++ b/include/core/state_block/state_angle.h
@@ -20,6 +20,8 @@ class StateAngle : public StateBlock
StateAngle(double _angle = 0.0, bool _fixed = false);
virtual ~StateAngle();
+
+ static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed);
};
inline StateAngle::StateAngle(double _angle, bool _fixed) :
@@ -33,6 +35,12 @@ inline StateAngle::~StateAngle()
//
}
+inline StateBlockPtr StateAngle::create (const Eigen::VectorXd& _state, bool _fixed)
+{
+ MatrixSizeCheck<1,1>::check(_state);
+ return std::make_shared<StateAngle>(_state(0), _fixed);
+}
+
} /* namespace wolf */
#endif /* STATE_ANGLE_H_ */
diff --git a/include/core/state_block/state_block.h b/include/core/state_block/state_block.h
index ab3bb7147f3c81f57a3436216412274394acc04a..c8706e2b6d081802d4a3b2590538b2e45df0584b 100644
--- a/include/core/state_block/state_block.h
+++ b/include/core/state_block/state_block.h
@@ -146,17 +146,21 @@ public:
**/
void resetLocalParamUpdated();
+ virtual void setIdentity(bool _notify = true);
+ void setZero (bool _notify = true);
+
+ virtual Eigen::VectorXd identity() const;
+ Eigen::VectorXd zero() const;
+
/** \brief perturb state
*/
void perturb(double amplitude = 0.1);
- /** \brief Add this state_block to the problem
- **/
- //void addToProblem(ProblemPtr _problem_ptr);
- /** \brief Remove this state_block from the problem
- **/
- //void removeFromProblem(ProblemPtr _problem_ptr);
+ void plus(const Eigen::VectorXd& _dv);
+
+ static StateBlockPtr create (const Eigen::VectorXd& _state, bool _fixed = false);
+
};
} // namespace wolf
@@ -292,6 +296,30 @@ inline double* StateBlock::getStateData()
return state_.data();
}
+inline void StateBlock::setIdentity(bool _notify)
+{
+ setState( Eigen::VectorXd::Zero(state_size_), _notify );
+}
+
+inline void StateBlock::setZero(bool _notify)
+{
+ setIdentity(_notify);
+}
+
+inline Eigen::VectorXd StateBlock::identity() const
+{
+ return Eigen::VectorXd::Zero(state_size_);
+}
+inline Eigen::VectorXd StateBlock::zero() const
+{
+ return identity();
+}
+
+inline StateBlockPtr StateBlock::create (const Eigen::VectorXd& _state, bool _fixed)
+{
+ return std::make_shared<StateBlock>(_state, _fixed);
+}
+
}// namespace wolf
#endif
diff --git a/include/core/state_block/state_composite.h b/include/core/state_block/state_composite.h
new file mode 100644
index 0000000000000000000000000000000000000000..44fdc98d588d46cbdf996ff94ec788f38bf7ce9c
--- /dev/null
+++ b/include/core/state_block/state_composite.h
@@ -0,0 +1,370 @@
+/*
+ * state_composite.h
+ *
+ * Created on: Apr 6, 2020
+ * Author: jsola
+ */
+
+#ifndef STATE_BLOCK_STATE_COMPOSITE_H_
+#define STATE_BLOCK_STATE_COMPOSITE_H_
+
+#include "core/common/wolf.h"
+
+#include <unordered_map>
+
+#include <iostream>
+
+
+namespace wolf
+{
+
+using std::string;
+using namespace Eigen;
+
+typedef std::string StateStructure;
+typedef std::unordered_map < std::string, StateBlockPtr > StateBlockMap;
+typedef StateBlockMap::const_iterator StateBlockMapCIter;
+
+class VectorComposite : public std::unordered_map < std::string, Eigen::VectorXd >
+{
+ public:
+ VectorComposite() {};
+ VectorComposite(const StateStructure& _s);
+ VectorComposite(const StateStructure& _s, const std::list<int>& _sizes);
+ VectorComposite(const VectorComposite & v) : unordered_map<string, VectorXd>(v){};
+ /**
+ * \brief Construct from Eigen::VectorXd and structure
+ *
+ * Usage:
+ *
+ * VectorXd vec_eigen(1,2,3,4,5);
+ *
+ * VectorComposite vec_comp( vec_eigen, "ab", {2,3} ); // vec_eigen must be at least size 5 !!
+ *
+ * result:
+ *
+ * vec_comp["a"].transpose(); // = (1,2);
+ * vec_comp["b"].transpose(); // = (3,4,5);
+ */
+ VectorComposite(const VectorXd& _v, const StateStructure& _structure, const std::list<int>& _sizes);
+ VectorComposite(const StateStructure& _structure, const std::list<VectorXd>& _vectors);
+
+ unsigned int size(const StateStructure& _structure) const;
+
+ Eigen::VectorXd vector(const StateStructure& _structure) const;
+
+ /**
+ * \brief set from Eigen::VectorXd and structure
+ *
+ * Usage:
+ *
+ * Eigen::VectorXd vec_eigen;
+ * wolf::VectorComposite vec_comp;
+ *
+ * vec_comp.set( vec_eigen, "ab", {2,3} ); // vec_eigen must be at least size 5 !!
+ *
+ * result:
+ *
+ * vec_comp["a"].transpose(); // = (1,2);
+ * vec_comp["b"].transpose(); // = (3,4,5);
+ */
+ void set(const VectorXd& _v, const StateStructure& _structure, const std::list<int>& _sizes);
+ void setZero();
+
+ friend std::ostream& operator <<(std::ostream &_os, const wolf::VectorComposite &_x);
+ friend wolf::VectorComposite operator +(const wolf::VectorComposite &_x, const wolf::VectorComposite &_y);
+ friend wolf::VectorComposite operator -(const wolf::VectorComposite &_x, const wolf::VectorComposite &_y);
+ friend wolf::VectorComposite operator -(const wolf::VectorComposite &_x);
+};
+
+
+class MatrixComposite : public std::unordered_map < std::string, std::unordered_map < std::string, Eigen::MatrixXd > >
+{
+
+ private:
+ std::unordered_map < string, unsigned int> size_rows_, size_cols_;
+ unsigned int rows() const;
+ unsigned int cols() const;
+ void sizeAndIndices(const StateStructure & _struct_rows,
+ const StateStructure & _struct_cols,
+ std::unordered_map < string, unsigned int>& _indices_rows,
+ std::unordered_map < string, unsigned int>& _indices_cols,
+ unsigned int& _rows,
+ unsigned int& _cols) const;
+
+ public:
+ MatrixComposite() {};
+ MatrixComposite(const StateStructure& _row_structure,
+ const StateStructure& _col_structure);
+ MatrixComposite(const StateStructure& _row_structure,
+ const std::list<int>& _row_sizes,
+ const StateStructure& _col_structure,
+ const std::list<int>& _col_sizes);
+ MatrixComposite (const MatrixComposite& m);
+ /**
+ * \brief Construct from Eigen::VectorXd and structure
+ *
+ * Usage:
+ *
+ * VectorXd vec_eigen(1,2,3,4,5);
+ *
+ * VectorComposite vec_comp( vec_eigen, "ab", {2,3} ); // vec_eigen must be at least size 5 !!
+ *
+ * result:
+ *
+ * vec_comp["a"].transpose(); // = (1,2);
+ * vec_comp["b"].transpose(); // = (3,4,5);
+ */
+ MatrixComposite(const MatrixXd& _m,
+ const StateStructure& _row_structure,
+ const std::list<int>& _row_sizes,
+ const StateStructure& _col_structure,
+ const std::list<int>& _col_sizes);
+ ~MatrixComposite() = default;
+
+ bool check() const;
+ static MatrixComposite zero(const StateStructure& _row_structure,
+ const std::list<int>& _row_sizes,
+ const StateStructure& _col_structure,
+ const std::list<int>& _col_sizes);
+
+ static MatrixComposite identity(const StateStructure& _structure,
+ const std::list<int>& _sizes);
+
+ unsigned int count(const std::string &_row,
+ const std::string &_col) const;
+
+ bool emplace(const std::string &_row,
+ const std::string &_col,
+ const Eigen::MatrixXd &_mat_blk);
+
+ // throw error if queried block is not present
+ bool at(const std::string &_row,
+ const std::string &_col,
+ Eigen::MatrixXd &_mat_blk) const;
+ const MatrixXd& at(const std::string &_row,
+ const std::string &_col) const;
+ MatrixXd& at(const std::string &_row,
+ const std::string &_col);
+
+ // make some shadowed API available
+ using std::unordered_map < std::string, std::unordered_map < std::string, Eigen::MatrixXd > >::at;
+ using std::unordered_map < std::string, std::unordered_map < std::string, Eigen::MatrixXd > >::count;
+
+
+ MatrixXd matrix(const StateStructure & _struct_rows,
+ const StateStructure & _struct_cols) const;
+
+ MatrixComposite operator * (double scalar_right) const;
+ MatrixComposite operator + (const MatrixComposite & _second) const;
+ MatrixComposite operator - (const MatrixComposite & _second) const;
+ MatrixComposite operator - (void) const;
+
+ /**
+ * \brief Matrix product
+ *
+ * This function computes the matrix product M * N
+ *
+ * It assumes that the second matrix's blocks are compatible with the blocks of this matrix,
+ * both in their structure and their individual sizes.
+ *
+ * For example: Let us call 'this' matrix with the name 'M'.
+ *
+ * The matrix 'M' maps the "PO" space into a new space "VW":
+ * M["V"]["P"] M["V"]["O"]
+ * M["W"]["P"] M["W"]["O"]
+ *
+ * The second matrix N has blocks "PO" in vertical arrangement,
+ * and "XY" in horizontal arrangement:
+ * N["P"]["X"] N["P"]["Y"]
+ * N["O"]["X"] N["O"]["Y"]
+ *
+ * Also, assume that the sizes of the blocks "P" and "O" are the same in M and N.
+ *
+ * Then, the result is a matrix S = M * N with block arrangements "VW" and "XY"
+ * S["V"]["X"] S["V"]["Y"]
+ * S["W"]["X"] S["W"]["Y"]
+ */
+ MatrixComposite operator *(const MatrixComposite & _second) const;
+
+ /**
+ * \brief Propagate a given covariances matrix, with 'this' being a Jacobian matrix
+ *
+ * This function computes the product J * Q * J.transpose.
+ *
+ * It considers that this is a Jacobian matrix, and that the provided matrix
+ * is a proper covariance (e.g. symmmetric and semi-positive).
+ * It also considers that the Jacobian blocks are compatible with the blocks
+ * of the provided covariance, both in their structure and their individual sizes.
+ *
+ * If these conditions are not satisfied, the product will fail and throw,
+ * or give aberrant results at best.
+ *
+ * -----
+ *
+ * For example: Let us call 'this' a Jacobian matrix with the name 'J'.
+ *
+ * This Jacobian 'J' maps the "PO" blocks into "VW":
+ *
+ * J["V"]["P"] J["V"]["O"]
+ * J["W"]["P"] J["W"]["O"]
+ *
+ * The provided matrix is a covariances matrix Q.
+ * Q has blocks "P", "O" in both vertical and horizontal arrangements:
+ *
+ * Q["P"]["P"] Q["P"]["O"]
+ * Q["O"]["P"] Q["O"]["O"]
+ *
+ * Also, it is required that the sizes of the blocks "P", "O" are the same in Q and J.
+ *
+ * Now, upon a call to
+ *
+ * MatrixComposite S = J.propagate(Q);
+ *
+ * the result is a covariances matrix S with blocks "V" and "W"
+ *
+ * S["V"]["V"] S["V"]["W"]
+ * S["W"]["V"] S["W"]["W"]
+ */
+ MatrixComposite propagate(const MatrixComposite & _Cov); // This performs Jac * this * Jac.tr
+
+ /**
+ * \brief Matrix-vector product
+ *
+ * This function computes the matrix product M * N
+ *
+ * It assumes that the second matrix's blocks are compatible with the blocks of this matrix,
+ * both in their structure and their individual sizes.
+ *
+ * For example: Let us call 'this' matrix with the name 'M'.
+ *
+ * The matrix 'M' maps the "PO" space into a new space "VW":
+ * M["V"]["P"] M["V"]["O"]
+ * M["W"]["P"] M["W"]["O"]
+ *
+ * The second vector V has blocks "PO" in vertical arrangement,
+ * V["P"]
+ * V["O"]
+ *
+ * Also, assume that the sizes of the blocks "P" and "O" are the same in M and V.
+ *
+ * Then, the result is a vector S = M * V with block arrangement "VW"
+ * S["V"]
+ * S["W"]
+ */
+ VectorComposite operator *(const VectorComposite & _second) const;
+
+ friend std::ostream& operator << (std::ostream & _os, const MatrixComposite & _M);
+
+ friend MatrixComposite operator * (double scalar_left, const MatrixComposite& M);
+};
+
+
+class StateBlockComposite
+{
+ public:
+ StateBlockComposite() = default;
+ ~StateBlockComposite() = default;
+
+ const StateBlockMap& getStateBlockMap() const;
+
+ StateBlockPtr add(const std::string& _sb_type, const StateBlockPtr& _sb);
+
+ // Emplace derived state blocks (angle, quaternion, etc).
+ template<typename SB, typename ... Args>
+ std::shared_ptr<SB> emplace(const std::string& _sb_type, Args&&... _args_of_derived_state_block_constructor);
+
+ // Emplace base state blocks.
+ template<typename ... Args>
+ inline StateBlockPtr emplace(const std::string& _sb_type, Args&&... _args_of_base_state_block_constructor);
+
+ unsigned int remove(const std::string& _sb_type);
+ bool has(const std::string& _sb_type) const { return state_block_map_.count(_sb_type) > 0; }
+ bool has(const StateBlockPtr& _sb) const;
+ StateBlockPtr at(const std::string& _sb_type) const;
+ void set(const std::string& _sb_type, const StateBlockPtr& _sb);
+ void set(const std::string& _sb_type, const VectorXd& _vec);
+ void setVectors(const StateStructure& _structure, const std::list<VectorXd> &_vectors);
+ bool key(const StateBlockPtr& _sb, std::string& _key) const;
+ std::string key(const StateBlockPtr& _sb) const;
+ StateBlockMapCIter find(const StateBlockPtr& _sb) const;
+
+ // identity and zero (they are the same with different names)
+ void setIdentity(bool _notify = true);
+ void setZero(bool _notify = true);
+ VectorComposite identity() const;
+ VectorComposite zero() const;
+
+ // Plus operator
+ void plus(const VectorComposite& _dx);
+
+ // Perturb state with random noise
+ void perturb(double amplitude = 0.01);
+
+ // These act on all state blocks. Per-block action must be done through state_block.fix() or through extended API in derived classes of this.
+ void fix();
+ void unfix();
+ bool isFixed() const;
+
+ // Get composite of state vectors (not blocks)
+ VectorComposite getState() const;
+ bool getState(VectorComposite& _state) const;
+ void setState(const VectorComposite& _state);
+
+ // Get compact Eigen vector and related things
+ SizeEigen stateSize() const;
+ SizeEigen stateSize(const StateStructure& _structure) const;
+ VectorXd stateVector(const StateStructure& _structure) const;
+ void stateVector(const StateStructure& _structure, VectorXd& _vector) const;
+
+ private:
+ StateBlockMap state_block_map_;
+};
+
+//////////// IMPLEMENTATION ////////////
+
+inline unsigned int MatrixComposite::count(const std::string &_row, const std::string &_col) const
+{
+ const auto& rit = this->find(_row);
+
+ if (rit == this->end())
+ return 0;
+
+ else
+ return rit->second.count(_col);
+}
+
+
+template<typename SB, typename ... Args>
+inline std::shared_ptr<SB> wolf::StateBlockComposite::emplace(const std::string &_sb_type,
+ Args &&... _args_of_derived_state_block_constructor)
+{
+ assert(state_block_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
+ std::shared_ptr<SB> sb = std::make_shared<SB>(std::forward<Args>(_args_of_derived_state_block_constructor)...);
+
+ add(_sb_type, sb);
+
+ return sb;
+}
+
+template<typename ... Args>
+inline StateBlockPtr wolf::StateBlockComposite::emplace(const std::string &_sb_type,
+ Args &&... _args_of_base_state_block_constructor)
+{
+ assert(state_block_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
+ auto sb = std::make_shared<StateBlock>(std::forward<Args>(_args_of_base_state_block_constructor)...);
+
+ add(_sb_type, sb);
+
+ return sb;
+}
+
+inline MatrixComposite::MatrixComposite (const MatrixComposite& m)
+ : unordered_map<string, unordered_map<string, MatrixXd> >(m), size_rows_(m.size_rows_), size_cols_(m.size_cols_)
+{
+}
+
+}
+
+
+#endif /* STATE_BLOCK_STATE_COMPOSITE_H_ */
diff --git a/include/core/state_block/state_homogeneous_3d.h b/include/core/state_block/state_homogeneous_3d.h
index 4760a52b7faa2bf188b520534b8512722d209f63..ec42d19e188593b61db2a78b8f6ecbf35680216d 100644
--- a/include/core/state_block/state_homogeneous_3d.h
+++ b/include/core/state_block/state_homogeneous_3d.h
@@ -19,6 +19,11 @@ class StateHomogeneous3d : public StateBlock
StateHomogeneous3d(bool _fixed = false);
StateHomogeneous3d(const Eigen::VectorXd _state, bool _fixed = false);
virtual ~StateHomogeneous3d();
+
+ virtual void setIdentity(bool _notify = true) override;
+ virtual Eigen::VectorXd identity() const override;
+
+ static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed);
};
inline StateHomogeneous3d::StateHomogeneous3d(const Eigen::VectorXd _state, bool _fixed) :
@@ -40,6 +45,23 @@ inline StateHomogeneous3d::~StateHomogeneous3d()
// The local_param_ptr_ pointer is already removed by the base class
}
+
+inline void StateHomogeneous3d::setIdentity(bool _notify)
+{
+ setState(Eigen::Quaterniond::Identity().coeffs(), _notify);
+}
+
+inline Eigen::VectorXd StateHomogeneous3d::identity() const
+{
+ return Eigen::Quaterniond::Identity().coeffs();
+}
+
+inline StateBlockPtr StateHomogeneous3d::create (const Eigen::VectorXd& _state, bool _fixed)
+{
+ MatrixSizeCheck<4,1>::check(_state);
+ return std::make_shared<StateHomogeneous3d>(_state, _fixed);
+}
+
} // namespace wolf
#endif /* SRC_STATE_HOMOGENEOUS_3d_H_ */
diff --git a/include/core/state_block/state_quaternion.h b/include/core/state_block/state_quaternion.h
index 3732201633ca04eeac5815b9948c8e9d7261f852..1533607134e89684674a4eec1d88d7cc83c9e6f0 100644
--- a/include/core/state_block/state_quaternion.h
+++ b/include/core/state_block/state_quaternion.h
@@ -20,6 +20,11 @@ class StateQuaternion : public StateBlock
StateQuaternion(const Eigen::VectorXd& _state, bool _fixed = false);
StateQuaternion(const Eigen::Quaterniond& _quaternion, bool _fixed = false);
virtual ~StateQuaternion();
+
+ virtual void setIdentity(bool _notify = true) override;
+ virtual Eigen::VectorXd identity() const override;
+
+ static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed);
};
inline StateQuaternion::StateQuaternion(const Eigen::Quaterniond& _quaternion, bool _fixed) :
@@ -45,6 +50,23 @@ inline StateQuaternion::~StateQuaternion()
// The local_param_ptr_ pointer is already removed by the base class
}
+
+inline void StateQuaternion::setIdentity(bool _notify)
+{
+ setState(Eigen::Quaterniond::Identity().coeffs(), _notify);
+}
+
+inline Eigen::VectorXd StateQuaternion::identity() const
+{
+ return Eigen::Quaterniond::Identity().coeffs();
+}
+
+inline StateBlockPtr StateQuaternion::create (const Eigen::VectorXd& _state, bool _fixed)
+{
+ MatrixSizeCheck<4,1>::check(_state);
+ return std::make_shared<StateQuaternion>(_state, _fixed);
+}
+
} // namespace wolf
#endif /* SRC_STATE_QUATERNION_H_ */
diff --git a/include/core/trajectory/trajectory_base.h b/include/core/trajectory/trajectory_base.h
index 60f81c58afcb881fe8be35aae225c2ee55ac2c69..5fe9c6c9184cbdca679f5832b910be6b1bc19f99 100644
--- a/include/core/trajectory/trajectory_base.h
+++ b/include/core/trajectory/trajectory_base.h
@@ -26,17 +26,13 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj
std::list<FrameBasePtr> frame_list_;
protected:
- 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(const std::string& _frame_sturcture);
+ TrajectoryBase();
virtual ~TrajectoryBase();
- // Properties
- std::string getFrameStructure() const;
-
// Frames
const FrameBasePtrList& getFrameList() const;
FrameBasePtr getLastFrame() const;
@@ -47,6 +43,21 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj
void sortFrame(FrameBasePtr _frm_ptr);
void updateLastFrames();
+ virtual void printHeader(int depth, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream ,
+ std::string _tabs = "") const;
+ void print(int depth, //
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks,
+ std::ostream& stream = std::cout,
+ std::string _tabs = "") const;
+
+ virtual CheckLog localCheck(bool _verbose, TrajectoryBasePtr _trj_ptr, std::ostream& _stream, std::string _tabs = "") const;
+ bool check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
private:
FrameBasePtr addFrame(FrameBasePtr _frame_ptr);
void removeFrame(FrameBasePtr _frame_ptr);
@@ -80,11 +91,6 @@ inline FrameBasePtr TrajectoryBase::getLastKeyOrAuxFrame() const
return last_key_or_aux_frame_ptr_;
}
-inline std::string TrajectoryBase::getFrameStructure() const
-{
- return frame_structure_;
-}
-
} // namespace wolf
#endif
diff --git a/include/core/tree_manager/factory_tree_manager.h b/include/core/tree_manager/factory_tree_manager.h
new file mode 100644
index 0000000000000000000000000000000000000000..a56dae93e138c0f99cc51f73884a50be9f252707
--- /dev/null
+++ b/include/core/tree_manager/factory_tree_manager.h
@@ -0,0 +1,61 @@
+#ifndef FACTORY_TREE_MANAGER_H_
+#define FACTORY_TREE_MANAGER_H_
+
+namespace wolf
+{
+class TreeManagerBase;
+struct ParamsTreeManagerBase;
+}
+
+// wolf
+#include "core/common/factory.h"
+
+// std
+
+namespace wolf
+{
+/** \brief TreeManager factory class
+ * TODO
+ */
+
+// ParamsTreeManager factory
+struct ParamsTreeManagerBase;
+typedef Factory<ParamsTreeManagerBase,
+ const std::string&> FactoryParamsTreeManager;
+template<>
+inline std::string FactoryParamsTreeManager::getClass() const
+{
+ return "FactoryParamsTreeManager";
+}
+
+// TreeManager factory
+typedef Factory<TreeManagerBase,
+ const std::string&,
+ const ParamsTreeManagerBasePtr> FactoryTreeManager;
+template<>
+inline std::string FactoryTreeManager::getClass() const
+{
+ return "FactoryTreeManager";
+}
+
+#define WOLF_REGISTER_TREE_MANAGER(TreeManagerType) \
+ namespace{ const bool WOLF_UNUSED TreeManagerType##Registered = \
+ wolf::FactoryTreeManager::get().registerCreator(#TreeManagerType, TreeManagerType::create); } \
+
+typedef Factory<TreeManagerBase,
+ const std::string&,
+ const ParamsServer&> AutoConfFactoryTreeManager;
+template<>
+inline std::string AutoConfFactoryTreeManager::getClass() const
+{
+ return "AutoConfFactoryTreeManager";
+}
+
+
+#define WOLF_REGISTER_TREE_MANAGER_AUTO(TreeManagerType) \
+ namespace{ const bool WOLF_UNUSED TreeManagerType##AutoConfRegistered = \
+ wolf::AutoConfFactoryTreeManager::get().registerCreator(#TreeManagerType, TreeManagerType::create); } \
+
+} /* namespace wolf */
+
+#endif /* FACTORY_TREE_MANAGER_H_ */
diff --git a/include/core/tree_manager/tree_manager_base.h b/include/core/tree_manager/tree_manager_base.h
new file mode 100644
index 0000000000000000000000000000000000000000..53ef9825ab029933b8c58e497e792d4f101c0406
--- /dev/null
+++ b/include/core/tree_manager/tree_manager_base.h
@@ -0,0 +1,82 @@
+#ifndef INCLUDE_TREE_MANAGER_BASE_H_
+#define INCLUDE_TREE_MANAGER_BASE_H_
+
+// Wolf includes
+#include "core/common/wolf.h"
+#include "core/common/node_base.h"
+#include "core/common/params_base.h"
+#include "core/problem/problem.h"
+
+namespace wolf
+{
+/*
+ * Macro for defining Autoconf tree manager creator for WOLF's high level API.
+ *
+ * Place a call to this macro inside your class declaration (in the tree_manager_class.h file),
+ * preferably just after the constructors.
+ *
+ * In order to use this macro, the derived processor class, ProcessorClass,
+ * must have a constructor available with the API:
+ *
+ * TreeManagerClass(const ParamsTreeManagerPtr _params);
+ */
+#define WOLF_TREE_MANAGER_CREATE(TreeManagerClass, ParamsTreeManagerClass) \
+static TreeManagerBasePtr create(const std::string& _unique_name, \
+ const ParamsServer& _server) \
+{ \
+ auto params = std::make_shared<ParamsTreeManagerClass>(_unique_name, _server); \
+ \
+ auto tree_manager = std::make_shared<TreeManagerClass>(params); \
+ \
+ tree_manager ->setName(_unique_name); \
+ \
+ return tree_manager; \
+} \
+static TreeManagerBasePtr create(const std::string& _unique_name, \
+ const ParamsTreeManagerBasePtr _params) \
+{ \
+ auto params = std::static_pointer_cast<ParamsTreeManagerClass>(_params); \
+ \
+ auto tree_manager = std::make_shared<TreeManagerClass>(params); \
+ \
+ tree_manager ->setName(_unique_name); \
+ \
+ return tree_manager; \
+} \
+
+struct ParamsTreeManagerBase : public ParamsBase
+{
+ std::string prefix = "problem/tree_manager";
+ ParamsTreeManagerBase() = default;
+ ParamsTreeManagerBase(std::string _unique_name, const ParamsServer& _server):
+ ParamsBase(_unique_name, _server)
+ {
+ }
+
+ virtual ~ParamsTreeManagerBase() = default;
+
+ std::string print() const
+ {
+ return ParamsBase::print() + "\n";
+ }
+};
+
+class TreeManagerBase : public NodeBase
+{
+ public:
+ TreeManagerBase(const std::string& _type, ParamsTreeManagerBasePtr _params) :
+ NodeBase("TREE_MANAGER", _type),
+ params_(_params)
+ {}
+
+ virtual ~TreeManagerBase(){}
+
+ virtual void keyFrameCallback(FrameBasePtr _key_frame) = 0;
+
+ protected:
+ ParamsTreeManagerBasePtr params_;
+};
+
+} /* namespace wolf */
+
+#endif /* INCLUDE_TREE_MANAGER_BASE_H_ */
diff --git a/include/core/tree_manager/tree_manager_sliding_window.h b/include/core/tree_manager/tree_manager_sliding_window.h
new file mode 100644
index 0000000000000000000000000000000000000000..8b2deec3b4fe8de046e1dd6c65c646be1a773727
--- /dev/null
+++ b/include/core/tree_manager/tree_manager_sliding_window.h
@@ -0,0 +1,54 @@
+#ifndef INCLUDE_TREE_MANAGER_SLIDING_WINDOW_H_
+#define INCLUDE_TREE_MANAGER_SLIDING_WINDOW_H_
+
+#include "../tree_manager/tree_manager_base.h"
+
+namespace wolf
+{
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsTreeManagerSlidingWindow)
+WOLF_PTR_TYPEDEFS(TreeManagerSlidingWindow)
+
+struct ParamsTreeManagerSlidingWindow : public ParamsTreeManagerBase
+{
+ ParamsTreeManagerSlidingWindow() = default;
+ ParamsTreeManagerSlidingWindow(std::string _unique_name, const wolf::ParamsServer & _server) :
+ ParamsTreeManagerBase(_unique_name, _server)
+ {
+ n_key_frames = _server.getParam<unsigned int>(prefix + "/n_key_frames");
+ fix_first_key_frame = _server.getParam<bool> (prefix + "/fix_first_key_frame");
+ viral_remove_empty_parent = _server.getParam<bool> (prefix + "/viral_remove_empty_parent");
+ }
+ std::string print() const
+ {
+ return "\n" + ParamsTreeManagerBase::print() + "\n"
+ + "n_key_frames: " + std::to_string(n_key_frames) + "\n"
+ + "fix_first_key_frame: " + std::to_string(fix_first_key_frame) + "\n"
+ + "viral_remove_empty_parent: " + std::to_string(viral_remove_empty_parent) + "\n";
+ }
+
+ unsigned int n_key_frames;
+ bool fix_first_key_frame;
+ bool viral_remove_empty_parent;
+};
+
+class TreeManagerSlidingWindow : public TreeManagerBase
+{
+ public:
+ TreeManagerSlidingWindow(ParamsTreeManagerSlidingWindowPtr _params) :
+ TreeManagerBase("TreeManagerSlidingWindow", _params),
+ params_sw_(_params)
+ {};
+ WOLF_TREE_MANAGER_CREATE(TreeManagerSlidingWindow, ParamsTreeManagerSlidingWindow)
+
+ virtual ~TreeManagerSlidingWindow(){}
+
+ virtual void keyFrameCallback(FrameBasePtr _key_frame) override;
+
+ protected:
+ ParamsTreeManagerSlidingWindowPtr params_sw_;
+};
+
+} /* namespace wolf */
+
+#endif /* INCLUDE_TREE_MANAGER_SLIDING_WINDOW_H_ */
diff --git a/include/core/utils/converter.h b/include/core/utils/converter.h
index a1b4a5a86ee2082b6f44bd3eb8d37e0c3a5c88d3..8c8e34935d728a505537643fa4c51bf001787bb6 100644
--- a/include/core/utils/converter.h
+++ b/include/core/utils/converter.h
@@ -4,6 +4,7 @@
// wolf
#include "core/common/wolf.h"
#include "core/utils/converter_utils.h"
+#include "core/state_block/state_composite.h"
// Eigen
#include <eigen3/Eigen/Dense>
@@ -22,6 +23,15 @@
*/
namespace wolf{
+template<class A, class B>
+struct Wpair : std::pair<A,B>
+{
+ Wpair(A first, B second): std::pair<A,B>(first, second)
+ {
+
+ }
+};
+ //// CONVERTERS ~~~~ STRING ----> TYPE
template<typename T>
struct converter{
static T convert(std::string val){
@@ -120,6 +130,11 @@ struct converter<std::string>{
static std::string convert(std::pair<A,B> val){
return "{" + converter<std::string>::convert(val.first) + ":" + converter<std::string>::convert(val.second) + "}";
}
+ //NEW GUY
+ template<typename A, typename B>
+ static std::string convert(Wpair<A,B> val){
+ return "(" + converter<std::string>::convert(val.first) + "," + converter<std::string>::convert(val.second) + ")";
+ }
template<typename A, typename B>
static std::string convert(std::map<A,B> val){
std::string result = "";
@@ -129,6 +144,15 @@ struct converter<std::string>{
if(result.size() > 0) result = result.substr(1,result.size());
return "[" + result + "]";
}
+ template<typename A, typename B>
+ static std::string convert(std::unordered_map<A,B> val){
+ std::string result = "";
+ for(auto it : val){
+ result += "," + converter<std::string>::convert(it);
+ }
+ if(result.size() > 0) result = result.substr(1,result.size());
+ return "[" + result + "]";
+ }
template<typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
static std::string convert(Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols> val){
std::string result = "";
@@ -143,7 +167,25 @@ struct converter<std::string>{
}
return "[" + result + "]";
}
+ //// WOLF DEFINED TYPES -----> TO STRING
+ static std::string convert(VectorComposite val){
+ //TODO
+ // throw std::runtime_error("TODO! We still haven't got around to implement convert for VectorComposite to String :(");
+ auto helper = std::unordered_map<StateStructure, Eigen::VectorXd>();
+ for(const auto& pair: val)
+ helper.insert(std::pair<StateStructure, Eigen::VectorXd>(pair.first, pair.second));
+ return converter<std::string>::convert(helper);
+ }
+ static std::string convert(MatrixComposite val){
+ //TODO
+ // throw std::runtime_error("TODO! We still haven't got around to implement convert for MatrixComposite to String :(");
+ auto helper = std::unordered_map< StateStructure, std::unordered_map<StateStructure, Eigen::MatrixXd>>();
+ for(const auto& pair: val)
+ helper.insert(std::pair<StateStructure, std::unordered_map<StateStructure, Eigen::MatrixXd>>(pair.first, pair.second));
+ return converter<std::string>::convert(helper);
+ }
};
+ //// CONVERTERS ~~~~ TYPE ----> STRING
template<typename A, typename B>
struct converter<std::pair<A,B>>{
static std::pair<A,B> convert(std::string val){
@@ -153,6 +195,17 @@ struct converter<std::pair<A,B>>{
return std::pair<A,B>(converter<A>::convert(matches[1].str()), converter<B>::convert(matches[2].str()));
} else throw std::runtime_error("Invalid string format representing a pair. Correct format is {identifier:value}. String provided: " + val);
}
+};
+ //NEW GUY
+template<typename A, typename B>
+struct converter<Wpair<A,B>>{
+ static Wpair<A,B> convert(std::string val){
+ std::regex rgxP("\\(([^\\(,]+),([^\\)]+)\\)");
+ std::smatch matches;
+ if(std::regex_match(val, matches, rgxP)) {
+ return Wpair<A,B>(converter<A>::convert(matches[1].str()), converter<B>::convert(matches[2].str()));
+ } else throw std::runtime_error("Invalid string format representing a Wpair. Correct format is (first,second). String provided: " + val);
+ }
};
// TODO: WARNING!! For some reason when trying to specialize converter to std::array
// it defaults to the generic T type, thus causing an error!
@@ -216,5 +269,67 @@ struct converter<std::map<std::string,A>>{
return map;
}
};
+template<typename A, typename B>
+struct converter<std::map<A,B>>{
+ static std::map<A,B> convert(std::string val){
+ std::regex rgxM("\\[((?:(?:\\{[^\\{:]+:[^:\\}]+\\}),?)*)\\]");
+ if(not std::regex_match(val, rgxM))
+ throw std::runtime_error("Invalid string representation of a Map. Correct format is [({id:value})?(,{id:value})*]. String provided: " + val);
+
+ auto v = utils::parseList(val);
+ auto map = std::map<A, B>();
+ for(auto it : v){
+ auto p = converter<std::pair<A,B>>::convert(it);
+ map.insert(std::pair<A, B>(p.first, p.second));
+ }
+ return map;
+ }
+};
+template<typename A, typename B>
+struct converter<std::unordered_map<A,B>>{
+ static std::unordered_map<A,B> convert(std::string val){
+ std::regex rgxM("\\[((?:(?:\\{[^\\{:]+:[^:\\}]+\\}),?)*)\\]");
+ if(not std::regex_match(val, rgxM))
+ throw std::runtime_error("Invalid string representation of an unordered Map. Correct format is [({id:value})?(,{id:value})*]. String provided: " + val);
+
+ auto v = utils::parseList(val);
+ auto map = std::unordered_map<A, B>();
+ for(auto it : v){
+ auto p = converter<std::pair<A,B>>::convert(it);
+ map.insert(std::pair<A, B>(p.first, p.second));
+ }
+ return map;
+ }
+};
+//// FROM STRING -----> WOLF DEFINED TYPES
+template<>
+struct converter<VectorComposite>{
+ static VectorComposite convert(std::string val){
+ auto unordered_map = converter<std::unordered_map<StateStructure, Eigen::VectorXd>>::convert(val);
+ // VectorComposite result = VectorComposite(unordered_map);
+ // return result;
+ auto helper = VectorComposite();
+ for(auto const& it: unordered_map)
+ {
+ helper.insert(std::pair<StateStructure, Eigen::VectorXd>(it.first, it.second));
+ }
+ return helper;
+ }
+};
+template<>
+struct converter<MatrixComposite>{
+ static MatrixComposite convert(std::string val){
+ auto unordered_map = converter<std::unordered_map<StateStructure,
+ std::unordered_map<StateStructure, Eigen::MatrixXd>>>::convert(val);
+ // VectorComposite result = VectorComposite(unordered_map);
+ // return result;
+ auto helper = MatrixComposite();
+ for(auto const& it: unordered_map)
+ {
+ helper.insert(std::pair<StateStructure, std::unordered_map<StateStructure, Eigen::MatrixXd>>(it.first, it.second));
+ }
+ return helper;
+ }
+};
}
#endif
diff --git a/include/core/yaml/parser_yaml.h b/include/core/yaml/parser_yaml.h
index 1b6f659ebd609dffef4f3c472800cf32714f22e1..1e11cb86aeb89932815e38708f3cbf36530e2006 100644
--- a/include/core/yaml/parser_yaml.h
+++ b/include/core/yaml/parser_yaml.h
@@ -30,6 +30,7 @@ class ParserYAML {
YAML::Node n_;
};
struct PublisherManager{
+ std::string package_;
std::string subscriber_;
std::string topic_;
std::string period_;
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index d07b9b055d8197593dbdde99d05f6a4eddd46bfe..c091b5527677fb491fb2b819de8770aae7e497c2 100644
--- a/src/capture/capture_base.cpp
+++ b/src/capture/capture_base.cpp
@@ -17,7 +17,6 @@ CaptureBase::CaptureBase(const std::string& _type,
HasStateBlocks(""),
frame_ptr_(), // nullptr
sensor_ptr_(_sensor_ptr),
- calib_size_(0),
capture_id_(++capture_id_count_),
time_stamp_(_ts)
{
@@ -52,8 +51,6 @@ CaptureBase::CaptureBase(const std::string& _type,
{
WOLF_ERROR("Provided sensor parameters but no sensor pointer");
}
-
- updateCalibSize();
}
CaptureBase::~CaptureBase()
@@ -63,11 +60,26 @@ CaptureBase::~CaptureBase()
void CaptureBase::remove(bool viral_remove_empty_parent)
{
+ /* Order of removing:
+ * Factors are removed first, and afterwards the rest of nodes containing state blocks.
+ * In case multi-threading, solver can be called while removing.
+ * This order avoids SolverManager to ignore notifications (efficiency)
+ */
if (!is_removing_)
{
is_removing_ = true;
CaptureBasePtr this_C = shared_from_this(); // keep this alive while removing it
+ // remove downstream
+ while (!constrained_by_list_.empty())
+ {
+ constrained_by_list_.front()->remove(viral_remove_empty_parent); // remove constrained by
+ }
+ while (!feature_list_.empty())
+ {
+ feature_list_.front()->remove(viral_remove_empty_parent); // remove downstream
+ }
+
// Remove State Blocks
removeStateBlocks(getProblem());
@@ -79,16 +91,6 @@ void CaptureBase::remove(bool viral_remove_empty_parent)
if (viral_remove_empty_parent && F->getCaptureList().empty() && F->getConstrainedByList().empty())
F->remove(viral_remove_empty_parent); // remove upstream
}
-
- // remove downstream
- while (!feature_list_.empty())
- {
- feature_list_.front()->remove(viral_remove_empty_parent); // remove downstream
- }
- while (!constrained_by_list_.empty())
- {
- constrained_by_list_.front()->remove(viral_remove_empty_parent); // remove constrained by
- }
}
}
@@ -146,7 +148,7 @@ bool CaptureBase::isConstrainedBy(const FactorBasePtr &_factor) const
}
-const std::string& CaptureBase::getStructure() const
+const StateStructure& CaptureBase::getStructure() const
{
if (getSensor())
return getSensor()->getStructure();
@@ -170,57 +172,11 @@ StateBlockPtr CaptureBase::getStateBlock(const std::string& _key) const
void CaptureBase::fix()
{
HasStateBlocks::fix();
- updateCalibSize();
}
void CaptureBase::unfix()
{
HasStateBlocks::unfix();
- updateCalibSize();
-}
-
-SizeEigen CaptureBase::computeCalibSize() const
-{
- SizeEigen sz = 0;
- for (const auto& key : getStructure())
- {
- auto sb = getStateBlock(key);
- if (sb && !sb->isFixed())
- sz += sb->getSize();
- }
- return sz;
-}
-
-Eigen::VectorXd CaptureBase::getCalibration() const
-{
- Eigen::VectorXd calib(calib_size_);
- SizeEigen index = 0;
- for (const auto& key : getStructure())
- {
- auto sb = getStateBlock(key);
- if (sb && !sb->isFixed())
- {
- calib.segment(index, sb->getSize()) = sb->getState();
- index += sb->getSize();
- }
- }
- return calib;
-}
-
-void CaptureBase::setCalibration(const VectorXd& _calib)
-{
- updateCalibSize();
- assert(_calib.size() == calib_size_ && "Wrong size of calibration vector");
- SizeEigen index = 0;
- for (const auto& key : getStructure())
- {
- auto sb = getStateBlock(key);
- if (sb && !sb->isFixed())
- {
- sb->setState(_calib.segment(index, sb->getSize()));
- index += sb->getSize();
- }
- }
}
void CaptureBase::move(FrameBasePtr _frm_ptr)
@@ -228,15 +184,12 @@ void CaptureBase::move(FrameBasePtr _frm_ptr)
WOLF_WARN_COND(this->getFrame() == nullptr, "moving a capture not linked to any frame");
WOLF_WARN_COND(_frm_ptr == nullptr, "moving a capture to a null FrameBasePtr");
+ assert((this->getFrame() == nullptr || this->getFrame()->isKey()) && "Forbidden: moving a capture already linked to a KF");
+ assert((_frm_ptr == nullptr || _frm_ptr->isKey()) && "Forbidden: moving a capture to a non-estimated frame");
+
// Unlink
if (this->getFrame())
{
- if (this->getFrame()->isKey())
- {
- WOLF_ERROR("moving a capture linked to a KF");
- return;
- }
-
// unlink from previous non-key frame
this->getFrame()->removeCapture(shared_from_this());
this->setFrame(nullptr);
@@ -277,5 +230,161 @@ void CaptureBase::setProblem(ProblemPtr _problem)
ft->setProblem(_problem);
}
-} // namespace wolf
+void CaptureBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+{
+ _stream << _tabs << "Cap" << id() << " " << getType();
+ if(getSensor() != nullptr)
+ {
+ _stream << " -> Sen" << getSensor()->id();
+ }
+ else
+ _stream << " -> Sen-";
+
+ _stream << ((_depth < 3) ? " -- " + std::to_string(getFeatureList().size()) + "f" : "");
+ if (_constr_by)
+ {
+ _stream << " <-- ";
+ for (auto cby : getConstrainedByList())
+ _stream << "Fac" << cby->id() << " \t";
+ }
+ _stream << std::endl;
+
+ if(getStateBlockMap().size() > 0)
+ {
+ if (_metric && _state_blocks){
+ for (const auto& key : getStructure())
+ {
+ auto sb = getStateBlock(key);
+ _stream << _tabs << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "] = ( " << sb->getState().transpose() << " )" << std::endl;
+ }
+ }
+ else if (_metric)
+ {
+ _stream << _tabs << (isFixed() ? "Fix" : "Est") << ", ts=" << std::setprecision(5)
+ << getTimeStamp();
+ _stream << ",\t x= ( " << std::setprecision(2) << getStateVector() << " )";
+ _stream << std::endl;
+ }
+ else if (_state_blocks)
+ {
+ _stream << " sb:";
+ for (const auto& key : getStructure())
+ {
+ const auto& sb = getStateBlock(key);
+ _stream << " " << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "]; ";
+ }
+ _stream << std::endl;
+ }
+ }
+}
+
+void CaptureBase::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 >= 3)
+ for (auto f : getFeatureList())
+ f->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
+}
+
+CheckLog CaptureBase::localCheck(bool _verbose, CaptureBasePtr _cap_ptr, std::ostream& _stream, std::string _tabs) const
+{
+ CheckLog log;
+ std::stringstream inconsistency_explanation;
+
+ if (_verbose)
+ {
+ _stream << _tabs << "Cap" << id() << " @ " << _cap_ptr.get() << " -> Sen";
+ if (getSensor()) _stream << getSensor()->id();
+ else _stream << "-";
+ _stream << std::endl;
+ _stream << _tabs << " " << "-> Prb @ " << getProblem().get() << std::endl;
+ _stream << _tabs << " " << "-> Frm" << getFrame()->id() << " @ " << getFrame().get() << std::endl;
+ }
+ for (auto pair: getStateBlockMap())
+ {
+ auto sb = pair.second;
+
+ // check for valid state block
+ inconsistency_explanation << "Capture " << id() << " @ " << _cap_ptr.get() << " has State block pointer "
+ << sb.get() << " = 0 \n";
+ log.assertTrue((sb.get() != 0), inconsistency_explanation);
+
+ if (_verbose)
+ {
+ _stream << _tabs << " " << pair.first << " sb @ " << sb.get();
+ if (sb) {
+ auto lp = sb->getLocalParametrization();
+ if (lp)
+ _stream << " (lp @ " << lp.get() << ")";
+ }
+ _stream << std::endl;
+ }
+ }
+
+ auto frm_ptr = getFrame();
+ // check problem and frame pointers
+ inconsistency_explanation << "Capture problem pointer " << getProblem().get()
+ << " different from frame problem pointer " << frm_ptr->getProblem().get() << "\n";
+ log.assertTrue((getProblem() == frm_ptr->getProblem()), inconsistency_explanation);
+
+
+ // check contrained_by
+ for (const auto& cby : getConstrainedByList())
+ {
+ if (_verbose)
+ {
+ _stream << _tabs << " " << "<- Fac" << cby->id() << " -> ";
+ for (const auto& Cow : cby->getCaptureOtherList())
+ _stream << " Cap" << Cow.lock()->id();
+ _stream << std::endl;
+ }
+
+ // check constrained_by pointer to this capture
+ inconsistency_explanation << "constrained by capture " << id() << " @ " << _cap_ptr.get()
+ << " not found among constrained-by factors\n";
+ log.assertTrue((cby->hasCaptureOther(_cap_ptr)), inconsistency_explanation);
+
+ for (auto sb : cby->getStateBlockPtrVector())
+ {
+ if (_verbose)
+ {
+ _stream << _tabs << " " << "sb @ " << sb.get();
+ if (sb)
+ {
+ auto lp = sb->getLocalParametrization();
+ if (lp)
+ _stream << " (lp @ " << lp.get() << ")";
+ }
+ _stream << std::endl;
+ }
+ }
+ }
+
+ auto frm_cap = _cap_ptr->getFrame();
+ inconsistency_explanation << "Cap" << id() << " @ " << _cap_ptr
+ << " ---> Frm" << frm_cap->id() << " @ " << frm_cap
+ << " -X-> Frm" << id();
+ auto frm_cap_list = frm_cap->getCaptureList();
+ auto frame_has_cap = std::find_if(frm_cap_list.begin(), frm_cap_list.end(), [&_cap_ptr](CaptureBasePtr cap){ return cap == _cap_ptr;});
+ log.assertTrue(frame_has_cap != frm_cap_list.end(), inconsistency_explanation);
+
+ for(auto f : getFeatureList())
+ {
+ inconsistency_explanation << "Cap " << id() << " @ " << _cap_ptr
+ << " ---> Ftr" << f->id() << " @ " << f
+ << " -X-> Cap" << id();
+ log.assertTrue((f->getCapture() == _cap_ptr), inconsistency_explanation);
+ }
+ return log;
+}
+bool CaptureBase::check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
+{
+ auto sen_ptr = std::static_pointer_cast<CaptureBase>(_node_ptr);
+ auto local_log = localCheck(_verbose, sen_ptr, _stream, _tabs);
+ _log.compose(local_log);
+
+ for(auto f : getFeatureList()) f->check(_log, f, _verbose, _stream, _tabs + " ");
+ return _log.is_consistent_;
+}
+} // namespace wolf
diff --git a/src/capture/capture_diff_drive.cpp b/src/capture/capture_diff_drive.cpp
index 05f8a5c49fb73e228b135fffc4de398e9e4ba038..7a09336470bfefeaf304cfbb817a8793c4c9a82d 100644
--- a/src/capture/capture_diff_drive.cpp
+++ b/src/capture/capture_diff_drive.cpp
@@ -18,8 +18,6 @@ CaptureDiffDrive::CaptureDiffDrive(const TimeStamp& _ts,
_sensor_ptr,
_data,
_data_cov,
- 3,
- 3,
_capture_origin_ptr,
_p_ptr,
_o_ptr,
@@ -28,12 +26,4 @@ CaptureDiffDrive::CaptureDiffDrive(const TimeStamp& _ts,
//
}
-Eigen::VectorXd CaptureDiffDrive::correctDelta(const VectorXd& _delta,
- const VectorXd& _delta_error) const
-{
- Vector3d delta_corrected = _delta + _delta_error;
- delta_corrected(2) = pi2pi(delta_corrected(2));
- return delta_corrected;
-}
-
} // namespace wolf
diff --git a/src/capture/capture_motion.cpp b/src/capture/capture_motion.cpp
index a736c242e7fb7047498d061d9121ce8fb56f817c..1a61a7f5c763a4508427c5fb2566c3fd53248af7 100644
--- a/src/capture/capture_motion.cpp
+++ b/src/capture/capture_motion.cpp
@@ -14,8 +14,6 @@ CaptureMotion::CaptureMotion(const std::string & _type,
const TimeStamp& _ts,
SensorBasePtr _sensor_ptr,
const Eigen::VectorXd& _data,
- SizeEigen _delta_size,
- SizeEigen _delta_cov_size,
CaptureBasePtr _capture_origin_ptr) :
CaptureBase(_type, _ts, _sensor_ptr),
data_(_data),
@@ -31,8 +29,6 @@ CaptureMotion::CaptureMotion(const std::string & _type,
SensorBasePtr _sensor_ptr,
const Eigen::VectorXd& _data,
const Eigen::MatrixXd& _data_cov,
- SizeEigen _delta_size,
- SizeEigen _delta_cov_size,
CaptureBasePtr _capture_origin_ptr,
StateBlockPtr _p_ptr ,
StateBlockPtr _o_ptr ,
@@ -51,24 +47,80 @@ CaptureMotion::~CaptureMotion()
//
}
-Eigen::VectorXd CaptureMotion::getDeltaCorrected(const VectorXd& _calib_current) const
+bool CaptureMotion::containsTimeStamp (const TimeStamp& _ts, double _time_tolerance)
{
- VectorXd calib_preint = getCalibrationPreint();
- VectorXd delta_preint = getBuffer().get().back().delta_integr_;
- MatrixXd jac_calib = getBuffer().get().back().jacobian_calib_;
- VectorXd delta_error = jac_calib * (_calib_current - calib_preint);
- VectorXd delta_corrected = correctDelta(delta_preint, delta_error);
- return delta_corrected;
+ // the same capture is within tolerance
+ if (this->time_stamp_ - _time_tolerance <= _ts && _ts <= this->time_stamp_ + _time_tolerance)
+ return true;
+
+ // buffer is empty, and the capture is not within tolerance
+ if (this->getBuffer().empty())
+ return false;
+
+ // buffer encloses timestamp, if ts is:
+ // from : origin.tx + tt not included
+ // to : capture.ts + tt included
+ if (this->getOriginCapture()->getTimeStamp() +_time_tolerance < _ts and _ts <= this->getBuffer().back().ts_ + _time_tolerance)
+ return true;
+
+ // not found anywhere
+ return false;
}
-Eigen::VectorXd CaptureMotion::getDeltaCorrected(const VectorXd& _calib_current, const TimeStamp& _ts) const
+
+
+
+void CaptureMotion::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
{
- Motion motion = getBuffer().getMotion(_ts);
- VectorXd delta_preint = motion.delta_integr_;
- MatrixXd jac_calib = motion.jacobian_calib_;
- VectorXd delta_error = jac_calib * (_calib_current - calib_preint_);
- VectorXd delta_corrected = correctDelta(delta_preint, delta_error);
- return delta_corrected;
-}
+ _stream << _tabs << "CapM" << id() << " " << getType();
+
+ if(getSensor() != nullptr)
+ {
+ _stream << " -> Sen" << getSensor()->id();
+ }
+ else
+ _stream << " -> Sen-";
+ if (auto OC = getOriginCapture())
+ {
+ _stream << " -> OCap" << OC->id();
+ if (auto OF = OC->getFrame())
+ _stream << " ; OFrm" << OF->id();
+ }
+
+ _stream << ((_depth < 3) ? " -- " + std::to_string(getFeatureList().size()) + "f" : "");
+ if (_constr_by)
+ {
+ _stream << " <-- ";
+ for (auto cby : getConstrainedByList())
+ _stream << "Fac" << cby->id() << " \t";
+ }
+ _stream << std::endl;
+ if (_state_blocks)
+ for (const auto& sb : getStateBlockVec())
+ {
+ if(sb != nullptr)
+ {
+ _stream << _tabs << " " << "sb: ";
+ _stream << (sb->isFixed() ? "Fix" : "Est");
+ if (_metric)
+ _stream << std::setprecision(2) << " (" << sb->getState().transpose() << " )";
+ _stream << std::endl;
+ }
+ }
+
+ _stream << _tabs << " " << "buffer size : " << getBuffer().size() << std::endl;
+ if ( _metric && ! getBuffer().empty())
+ {
+ _stream << _tabs << " " << "delta preint : (" << getDeltaPreint().transpose() << ")" << std::endl;
+// if (hasCalibration())
+// {
+// _stream << _tabs << " " << "calib preint : (" << getCalibrationPreint().transpose() << ")" << std::endl;
+// _stream << _tabs << " " << "jacob preint : (" << getJacobianCalib().row(0) << ")" << std::endl;
+//// _stream << _tabs << " " << "calib current: (" << getCalibration().transpose() << ")" << std::endl;
+//// _stream << _tabs << " " << "delta correct: (" << getDeltaCorrected(getCalibration()).transpose() << ")" << std::endl;
+// }
+ }
+
+}
}
diff --git a/src/capture/capture_odom_2d.cpp b/src/capture/capture_odom_2d.cpp
index b024294a5df38772bc040495728958aa8cb08c4c..4755b6fe067fcbcd81c3ae66ecb4be6b562078e7 100644
--- a/src/capture/capture_odom_2d.cpp
+++ b/src/capture/capture_odom_2d.cpp
@@ -14,7 +14,7 @@ CaptureOdom2d::CaptureOdom2d(const TimeStamp& _init_ts,
SensorBasePtr _sensor_ptr,
const Eigen::VectorXd& _data,
CaptureBasePtr _capture_origin_ptr):
- CaptureMotion("CaptureOdom2d", _init_ts, _sensor_ptr, _data, 3, 3, _capture_origin_ptr)
+ CaptureMotion("CaptureOdom2d", _init_ts, _sensor_ptr, _data, _capture_origin_ptr)
{
//
}
@@ -24,7 +24,7 @@ CaptureOdom2d::CaptureOdom2d(const TimeStamp& _init_ts,
const Eigen::VectorXd& _data,
const Eigen::MatrixXd& _data_cov,
CaptureBasePtr _capture_origin_ptr):
- CaptureMotion("CaptureOdom2d", _init_ts, _sensor_ptr, _data, _data_cov, 3, 3, _capture_origin_ptr)
+ CaptureMotion("CaptureOdom2d", _init_ts, _sensor_ptr, _data, _data_cov, _capture_origin_ptr)
{
//
}
diff --git a/src/capture/capture_odom_3d.cpp b/src/capture/capture_odom_3d.cpp
index 447e1d500f072e2754de4ed107aeb61e6e6299c0..a692011ffc2b61c0d46e9f024d39b354106feeb7 100644
--- a/src/capture/capture_odom_3d.cpp
+++ b/src/capture/capture_odom_3d.cpp
@@ -14,7 +14,7 @@ CaptureOdom3d::CaptureOdom3d(const TimeStamp& _init_ts,
SensorBasePtr _sensor_ptr,
const Eigen::VectorXd& _data,
CaptureBasePtr _capture_origin_ptr):
- CaptureMotion("CaptureOdom3d", _init_ts, _sensor_ptr, _data, 7, 6, _capture_origin_ptr)
+ CaptureMotion("CaptureOdom3d", _init_ts, _sensor_ptr, _data, _capture_origin_ptr)
{
//
}
@@ -24,7 +24,7 @@ CaptureOdom3d::CaptureOdom3d(const TimeStamp& _init_ts,
const Eigen::VectorXd& _data,
const Eigen::MatrixXd& _data_cov,
CaptureBasePtr _capture_origin_ptr):
- CaptureMotion("CaptureOdom3d", _init_ts, _sensor_ptr, _data, _data_cov, 7, 6, _capture_origin_ptr)
+ CaptureMotion("CaptureOdom3d", _init_ts, _sensor_ptr, _data, _data_cov, _capture_origin_ptr)
{
//
}
@@ -34,13 +34,5 @@ CaptureOdom3d::~CaptureOdom3d()
//
}
-Eigen::VectorXd CaptureOdom3d::correctDelta(const VectorXd& _delta, const VectorXd& _delta_error) const
-{
- VectorXd delta(7);
- delta.head(3) = _delta.head(3) + _delta_error.head(3);
- delta.tail(4) = (Quaterniond(_delta.data()+3) * exp_q(_delta_error.tail(3))).coeffs();
- return delta;
-}
-
} /* namespace wolf */
diff --git a/src/capture/capture_velocity.cpp b/src/capture/capture_velocity.cpp
deleted file mode 100644
index 538121825ba5ca528d4e850d55edb514de90a87f..0000000000000000000000000000000000000000
--- a/src/capture/capture_velocity.cpp
+++ /dev/null
@@ -1,42 +0,0 @@
-#include "core/capture/capture_velocity.h"
-
-namespace wolf {
-
-CaptureVelocity::CaptureVelocity(const TimeStamp& _ts,
- const SensorBasePtr& _sensor_ptr,
- const Eigen::VectorXd& _velocity,
- SizeEigen _delta_size, SizeEigen _delta_cov_size,
- CaptureBasePtr _capture_origin_ptr) :
- CaptureMotion("CaptureVelocity", _ts, _sensor_ptr, _velocity,
- _delta_size, _delta_cov_size, _capture_origin_ptr)
-{
- //
-}
-
-CaptureVelocity::CaptureVelocity(const TimeStamp& _ts,
- const SensorBasePtr& _sensor_ptr,
- const Eigen::VectorXd& _velocity,
- const Eigen::MatrixXd& _velocity_cov,
- SizeEigen _delta_size, SizeEigen _delta_cov_size,
- CaptureBasePtr _capture_origin_ptr,
- StateBlockPtr _p_ptr,
- StateBlockPtr _o_ptr,
- StateBlockPtr _intr_ptr) :
- CaptureMotion("CaptureVelocity", _ts, _sensor_ptr, _velocity, _velocity_cov,
- _delta_size, _delta_cov_size, _capture_origin_ptr,
- _p_ptr, _o_ptr, _intr_ptr)
-{
- //
-}
-
-const Eigen::VectorXd& CaptureVelocity::getVelocity() const
-{
- return getData();
-}
-
-const Eigen::MatrixXd& CaptureVelocity::getVelocityCov() const
-{
- return getDataCovariance();
-}
-
-} // namespace wolf
diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp
index 0d8cba6f97cb7e2c2cdf067e7ebcb65d8ac79c10..b5bcfa4b7b6dc09e990adbecb93685c0294dbef5 100644
--- a/src/ceres_wrapper/ceres_manager.cpp
+++ b/src/ceres_wrapper/ceres_manager.cpp
@@ -37,7 +37,13 @@ CeresManager::CeresManager(const ProblemPtr& _wolf_problem,
CeresManager::~CeresManager()
{
while (!fac_2_residual_idx_.empty())
- removeFactor(fac_2_residual_idx_.begin()->first);
+ removeFactorDerived(fac_2_residual_idx_.begin()->first);
+
+ while (!state_blocks_.empty())
+ {
+ removeStateBlockDerived(state_blocks_.begin()->first);
+ state_blocks_.erase(state_blocks_.begin());
+ }
}
SolverManagerPtr CeresManager::create(const ProblemPtr &_wolf_problem, const ParamsServer& _server)
@@ -48,13 +54,8 @@ CeresManager::~CeresManager()
return std::make_shared<CeresManager>(_wolf_problem, opt);
}
-std::string CeresManager::solveImpl(const ReportVerbosity report_level)
+std::string CeresManager::solveDerived(const ReportVerbosity report_level)
{
- // Check
- #ifdef _WOLF_DEBUG
- check();
- #endif
-
// run Ceres Solver
ceres::Solve(ceres_options_, ceres_problem_.get(), &summary_);
@@ -259,7 +260,7 @@ void CeresManager::computeCovariances(const std::vector<StateBlockPtr>& st_list)
std::cout << "WARNING: Couldn't compute covariances!" << std::endl;
}
-void CeresManager::addFactor(const FactorBasePtr& fac_ptr)
+void CeresManager::addFactorDerived(const FactorBasePtr& fac_ptr)
{
assert(fac_2_costfunction_.find(fac_ptr) == fac_2_costfunction_.end() && "adding a factor that is already in the fac_2_costfunction_ map");
@@ -287,7 +288,7 @@ void CeresManager::addFactor(const FactorBasePtr& fac_ptr)
assert((unsigned int)(ceres_problem_->NumResidualBlocks()) == fac_2_residual_idx_.size() && "ceres residuals different from wrapper residuals");
}
-void CeresManager::removeFactor(const FactorBasePtr& _fac_ptr)
+void CeresManager::removeFactorDerived(const FactorBasePtr& _fac_ptr)
{
assert(fac_2_residual_idx_.find(_fac_ptr) != fac_2_residual_idx_.end() && "removing a factor that is not in the fac_2_residual map");
@@ -298,12 +299,15 @@ void CeresManager::removeFactor(const FactorBasePtr& _fac_ptr)
assert((unsigned int)(ceres_problem_->NumResidualBlocks()) == fac_2_residual_idx_.size() && "ceres residuals different from wrapper residuals");
}
-void CeresManager::addStateBlock(const StateBlockPtr& state_ptr)
+void CeresManager::addStateBlockDerived(const StateBlockPtr& state_ptr)
{
+ assert(state_ptr);
+ assert(!ceres_problem_->HasParameterBlock(getAssociatedMemBlockPtr(state_ptr)) && "adding a state block already added");
+ assert(state_blocks_local_param_.count(state_ptr) == 0 && "local param stored for this state block");
+
ceres::LocalParameterization* local_parametrization_ptr = nullptr;
- if (state_ptr->hasLocalParametrization() &&
- state_blocks_local_param_.find(state_ptr)==state_blocks_local_param_.end())
+ if (state_ptr->hasLocalParametrization())
{
auto p = state_blocks_local_param_.emplace(state_ptr,
std::make_shared<LocalParametrizationWrapper>(state_ptr->getLocalParametrization()));
@@ -318,18 +322,19 @@ void CeresManager::addStateBlock(const StateBlockPtr& state_ptr)
if (state_ptr->isFixed())
ceres_problem_->SetParameterBlockConstant(getAssociatedMemBlockPtr(state_ptr));
- updateStateBlockStatus(state_ptr);
+ updateStateBlockStatusDerived(state_ptr);
}
-void CeresManager::removeStateBlock(const StateBlockPtr& state_ptr)
+void CeresManager::removeStateBlockDerived(const StateBlockPtr& state_ptr)
{
//std::cout << "CeresManager::removeStateBlock " << state_ptr.get() << " - " << getAssociatedMemBlockPtr(state_ptr) << std::endl;
assert(state_ptr);
+ assert(ceres_problem_->HasParameterBlock(getAssociatedMemBlockPtr(state_ptr)) && "removing a state block that is not in ceres");
ceres_problem_->RemoveParameterBlock(getAssociatedMemBlockPtr(state_ptr));
state_blocks_local_param_.erase(state_ptr);
}
-void CeresManager::updateStateBlockStatus(const StateBlockPtr& state_ptr)
+void CeresManager::updateStateBlockStatusDerived(const StateBlockPtr& state_ptr)
{
assert(state_ptr != nullptr);
if (state_ptr->isFixed())
@@ -338,7 +343,7 @@ void CeresManager::updateStateBlockStatus(const StateBlockPtr& state_ptr)
ceres_problem_->SetParameterBlockVariable(getAssociatedMemBlockPtr(state_ptr));
}
-void CeresManager::updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr)
+void CeresManager::updateStateBlockLocalParametrizationDerived(const StateBlockPtr& state_ptr)
{
assert(state_ptr != nullptr);
@@ -361,17 +366,17 @@ void CeresManager::updateStateBlockLocalParametrization(const StateBlockPtr& sta
// Remove all involved factors (it does not remove any parameter block)
for (auto fac : involved_factors)
- removeFactor(fac);
+ removeFactorDerived(fac);
// Remove state block (it removes all involved residual blocks but they just were removed)
- removeStateBlock(state_ptr);
+ removeStateBlockDerived(state_ptr);
// Add state block
- addStateBlock(state_ptr);
+ addStateBlockDerived(state_ptr);
// Add all involved factors
for (auto fac : involved_factors)
- addFactor(fac);
+ addFactorDerived(fac);
}
bool CeresManager::hasConverged()
@@ -410,37 +415,88 @@ ceres::CostFunctionPtr CeresManager::createCostFunction(const FactorBasePtr& _fa
throw std::invalid_argument( "Wrong Jacobian Method!" );
}
-void CeresManager::check()
+bool CeresManager::checkDerived(std::string prefix) const
{
+ bool ok = true;
+
// Check numbers
- assert(ceres_problem_->NumResidualBlocks() == fac_2_costfunction_.size());
- assert(ceres_problem_->NumResidualBlocks() == fac_2_residual_idx_.size());
- assert(ceres_problem_->NumParameterBlocks() == state_blocks_.size());
+ if (ceres_problem_->NumResidualBlocks() != factors_.size() or
+ ceres_problem_->NumResidualBlocks() != fac_2_costfunction_.size() or
+ ceres_problem_->NumResidualBlocks() != fac_2_residual_idx_.size())
+ {
+ WOLF_ERROR("CeresManager::check: number of residuals mismatch - in ", prefix, ":\n\tceres_problem_->NumResidualBlocks(): ", ceres_problem_->NumResidualBlocks(), "\n\tfactors_.size(): ", factors_.size(), "\n\tfac_2_costfunction_.size(): ", fac_2_costfunction_.size(), "\n\tfac_2_residual_idx_.size(): ", fac_2_residual_idx_.size());
+ ok = false;
+ }
+ if (ceres_problem_->NumParameterBlocks() != state_blocks_.size())
+ {
+ WOLF_ERROR("CeresManager::check: number of parameters mismatch ((ceres_problem_->NumParameterBlocks(): ", ceres_problem_->NumParameterBlocks(), " != state_blocks_.size(): ", state_blocks_.size(), ") - in ", prefix);
+ ok = false;
+ }
// Check parameter blocks
- for (auto&& state_block_pair : state_blocks_)
- assert(ceres_problem_->HasParameterBlock(state_block_pair.second.data()));
+ //for (auto&& state_block_pair : state_blocks_)
+ for (const auto& state_block_pair : state_blocks_)
+ {
+ if (!ceres_problem_->HasParameterBlock(state_block_pair.second.data()))
+ {
+ WOLF_ERROR("CeresManager::check: any state block is missing in ceres problem - in ", prefix);
+ ok = false;
+ }
+ }
// Check residual blocks
- for (auto&& fac_res_pair : fac_2_residual_idx_)
+ for (const auto& fac_res_pair : fac_2_residual_idx_)
{
+ // SolverManager::factors_
+ if (factors_.count(fac_res_pair.first) == 0)
+ {
+ WOLF_ERROR("CeresManager::check: factor ", fac_res_pair.first->id(), " (in fac_2_residual_idx_) not in factors_ - in ", prefix);
+ ok = false;
+ }
+
// costfunction - residual
- assert(fac_2_costfunction_.find(fac_res_pair.first) != fac_2_costfunction_.end());
- assert(fac_2_costfunction_[fac_res_pair.first].get() == ceres_problem_->GetCostFunctionForResidualBlock(fac_res_pair.second));
+ if (fac_2_costfunction_.count(fac_res_pair.first) == 0)
+ {
+ WOLF_ERROR("CeresManager::check: any factor in fac_2_residual_idx_ is not in fac_2_costfunction_ - in ", prefix);
+ ok = false;
+ }
+ //if (fac_2_costfunction_[fac_res_pair.first].get() != ceres_problem_->GetCostFunctionForResidualBlock(fac_res_pair.second))
+ if (fac_2_costfunction_.at(fac_res_pair.first).get() != ceres_problem_->GetCostFunctionForResidualBlock(fac_res_pair.second))
+ {
+ WOLF_ERROR("CeresManager::check: fac_2_costfunction_ and ceres mismatch - in ", prefix);
+ ok = false;
+ }
// factor - residual
- assert(fac_res_pair.first == static_cast<const CostFunctionWrapper*>(ceres_problem_->GetCostFunctionForResidualBlock(fac_res_pair.second))->getFactor());
+ if (fac_res_pair.first != static_cast<const CostFunctionWrapper*>(ceres_problem_->GetCostFunctionForResidualBlock(fac_res_pair.second))->getFactor())
+ {
+ WOLF_ERROR("CeresManager::check: fac_2_residual_idx_ and ceres mismatch - in ", prefix);
+ ok = false;
+ }
// parameter blocks - state blocks
std::vector<double*> param_blocks;
ceres_problem_->GetParameterBlocksForResidualBlock(fac_res_pair.second, ¶m_blocks);
- auto i = 0;
- for (const StateBlockPtr& st : fac_res_pair.first->getStateBlockPtrVector())
+ if (param_blocks.size() != fac_res_pair.first->getStateBlockPtrVector().size())
+ {
+ WOLF_ERROR("CeresManager::check: number parameter blocks in ceres residual", param_blocks.size(), " different from number of state blocks in factor ", fac_res_pair.first->getStateBlockPtrVector().size(), " - in ", prefix);
+ ok = false;
+ }
+ else
{
- assert(getAssociatedMemBlockPtr(st) == param_blocks[i]);
- i++;
+ auto i = 0;
+ for (const StateBlockPtr& st : fac_res_pair.first->getStateBlockPtrVector())
+ {
+ if (getAssociatedMemBlockPtr(st) != param_blocks[i])
+ {
+ WOLF_ERROR("CeresManager::check: parameter", i, " mismatch - in ", prefix);
+ ok = false;
+ }
+ i++;
+ }
}
}
+ return ok;
}
void insertSparseBlock(const Eigen::MatrixXd& ins, Eigen::SparseMatrixd& original, const unsigned int& row, const unsigned int& col)
@@ -483,8 +539,8 @@ const Eigen::SparseMatrixd CeresManager::computeHessian() const
StateBlockPtr sb = fac_ptr->getStateBlockPtrVector()[i];
if (!sb->isFixed())
{
- assert((state_blocks_local_param_.find(sb) != state_blocks_local_param_.end()) && "factor involving a state block not added");
- assert((A.cols() >= sb->getLocalSize() + jacobians[i].cols()) - 1 && "bad A number of cols");
+ assert(state_blocks_local_param_.count(sb) != 0 && "factor involving a state block not added");
+ assert((A.cols() >= sb->getLocalSize() + jacobians[i].cols() - 1) && "bad A number of cols");
// insert since A_block_row has just been created so it's empty for sure
if (sb->hasLocalParametrization()){
@@ -515,8 +571,8 @@ const Eigen::SparseMatrixd CeresManager::computeHessian() const
} // namespace wolf
-#include "core/solver/solver_factory.h"
+#include "core/solver/factory_solver.h"
namespace wolf {
- WOLF_REGISTER_SOLVER("CERES", CeresManager)
+ WOLF_REGISTER_SOLVER(CeresManager)
} // namespace wolf
diff --git a/src/common/time_stamp.cpp b/src/common/time_stamp.cpp
index 107bd0eb85917959f0b4fbdaacba3c136deacea7..528d0ad54872bc077ba74d72be0cb2cb79099d13 100644
--- a/src/common/time_stamp.cpp
+++ b/src/common/time_stamp.cpp
@@ -61,7 +61,7 @@ TimeStamp TimeStamp::operator -(const double& dt) const
return ts;
}
-inline void TimeStamp::operator -=(const double& dt)
+void TimeStamp::operator -=(const double& dt)
{
unsigned long int dt_nano = (unsigned long int)(dt*NANOSECS);
time_stamp_nano_ = (dt_nano > time_stamp_nano_ ? 0 : time_stamp_nano_ - dt_nano);
diff --git a/src/factor/factor_analytic.cpp b/src/factor/factor_analytic.cpp
index 73bc391c2c038ec27da7e8b2fc936e054795a415..6d146706065f2ecc0d1ce51bfe1ac290a2f1d2bd 100644
--- a/src/factor/factor_analytic.cpp
+++ b/src/factor/factor_analytic.cpp
@@ -4,6 +4,7 @@
namespace wolf {
FactorAnalytic::FactorAnalytic(const std::string& _tp,
+ const FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
@@ -12,7 +13,7 @@ FactorAnalytic::FactorAnalytic(const std::string& _tp,
bool _apply_loss_function, FactorStatus _status,
StateBlockPtr _state0Ptr, StateBlockPtr _state1Ptr, StateBlockPtr _state2Ptr, StateBlockPtr _state3Ptr, StateBlockPtr _state4Ptr,
StateBlockPtr _state5Ptr, StateBlockPtr _state6Ptr, StateBlockPtr _state7Ptr, StateBlockPtr _state8Ptr, StateBlockPtr _state9Ptr ) :
- FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ FactorBase(_tp, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
state_ptr_vector_({_state0Ptr, _state1Ptr, _state2Ptr, _state3Ptr, _state4Ptr,
_state5Ptr, _state6Ptr, _state7Ptr, _state8Ptr, _state9Ptr}),
state_block_sizes_vector_({_state0Ptr ? (unsigned int) _state0Ptr->getSize() : 0,
diff --git a/src/factor/factor_base.cpp b/src/factor/factor_base.cpp
index 72b36c519a0d8d0ce2cb7ac4748e0dfc110026d0..4e18cb47d8485a7b9ee83853e863ddf20bb7e78a 100644
--- a/src/factor/factor_base.cpp
+++ b/src/factor/factor_base.cpp
@@ -7,6 +7,7 @@ namespace wolf {
unsigned int FactorBase::factor_id_count_ = 0;
FactorBase::FactorBase(const std::string& _tp,
+ const FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
@@ -15,7 +16,7 @@ FactorBase::FactorBase(const std::string& _tp,
bool _apply_loss_function,
FactorStatus _status) :
NodeBase("FACTOR", _tp),
- feature_ptr_(),
+ feature_ptr_(), // will be filled in link()
factor_id_(++factor_id_count_),
status_(_status),
apply_loss_function_(_apply_loss_function),
@@ -33,9 +34,14 @@ FactorBase::FactorBase(const std::string& _tp,
feature_other_list_.push_back(_feature_other_ptr);
if (_landmark_other_ptr)
landmark_other_list_.push_back(_landmark_other_ptr);
+
+ assert(_feature_ptr && "null feature pointer when creating a factor");
+ measurement_ = _feature_ptr->getMeasurement();
+ measurement_sqrt_information_upper_ = _feature_ptr->getMeasurementSquareRootInformationUpper();
}
FactorBase::FactorBase(const std::string& _tp,
+ const FeatureBasePtr& _feature_ptr,
const FrameBasePtrList& _frame_other_list,
const CaptureBasePtrList& _capture_other_list,
const FeatureBasePtrList& _feature_other_list,
@@ -44,7 +50,7 @@ FactorBase::FactorBase(const std::string& _tp,
bool _apply_loss_function,
FactorStatus _status) :
NodeBase("FACTOR", _tp),
- feature_ptr_(),
+ feature_ptr_(), // will be filled in link()
factor_id_(++factor_id_count_),
status_(_status),
apply_loss_function_(_apply_loss_function),
@@ -62,25 +68,37 @@ FactorBase::FactorBase(const std::string& _tp,
feature_other_list_.push_back(fo);
for (auto& Lo : landmark_other_list_)
landmark_other_list_.push_back(Lo);
+
+ assert(_feature_ptr && "null feature pointer when creating a factor");
+ measurement_ = _feature_ptr->getMeasurement();
+ measurement_sqrt_information_upper_ = _feature_ptr->getMeasurementSquareRootInformationUpper();
}
void FactorBase::remove(bool viral_remove_empty_parent)
{
+ /* Order of removing:
+ * Factors are removed first, and afterwards the rest of nodes containing state blocks.
+ * In case multi-threading, solver can be called while removing.
+ * This order avoids SolverManager to ignore notifications (efficiency)
+ */
if (!is_removing_)
{
is_removing_ = true;
FactorBasePtr this_fac = shared_from_this(); // keep this alive while removing it
+
+ // add factor to be removed from solver
+ if (getProblem() != nullptr and this->getStatus() == FAC_ACTIVE)
+ getProblem()->notifyFactor(this_fac,REMOVE);
+
+ // remove from upstream
FeatureBasePtr f = feature_ptr_.lock();
if (f)
{
- f->removeFactor(this_fac); // remove from upstream
+ f->removeFactor(this_fac);
if (viral_remove_empty_parent && f->getFactorList().empty() && f->getConstrainedByList().empty())
f->remove(viral_remove_empty_parent); // remove upstream
}
- // add factor to be removed from solver
- if (getProblem() != nullptr)
- getProblem()->notifyFactor(this_fac,REMOVE);
// remove other: {Frame, Capture, Feature, Landmark}
for (auto& frm_ow : frame_other_list_)
@@ -89,7 +107,7 @@ void FactorBase::remove(bool viral_remove_empty_parent)
if (frm_o)
{
frm_o->removeConstrainedBy(this_fac);
- if (frm_o->getConstrainedByList().empty() && frm_o->getCaptureList().empty())
+ if (viral_remove_empty_parent && frm_o->getConstrainedByList().empty() && frm_o->getCaptureList().empty())
frm_o->remove(viral_remove_empty_parent);
}
}
@@ -100,7 +118,7 @@ void FactorBase::remove(bool viral_remove_empty_parent)
if (cap_o)
{
cap_o->removeConstrainedBy(this_fac);
- if (cap_o->getConstrainedByList().empty() && cap_o->getFeatureList().empty())
+ if (viral_remove_empty_parent && cap_o->getConstrainedByList().empty() && cap_o->getFeatureList().empty())
cap_o->remove(viral_remove_empty_parent);
}
}
@@ -111,7 +129,7 @@ void FactorBase::remove(bool viral_remove_empty_parent)
if (ftr_o)
{
ftr_o->removeConstrainedBy(this_fac);
- if (ftr_o->getConstrainedByList().empty() && ftr_o->getFactorList().empty())
+ if (viral_remove_empty_parent && ftr_o->getConstrainedByList().empty() && ftr_o->getFactorList().empty())
ftr_o->remove(viral_remove_empty_parent);
}
}
@@ -122,7 +140,7 @@ void FactorBase::remove(bool viral_remove_empty_parent)
if (lmk_o)
{
lmk_o->removeConstrainedBy(this_fac);
- if (lmk_o->getConstrainedByList().empty())
+ if (viral_remove_empty_parent && lmk_o->getConstrainedByList().empty())
lmk_o->remove(viral_remove_empty_parent);
}
}
@@ -131,24 +149,6 @@ void FactorBase::remove(bool viral_remove_empty_parent)
}
}
-const Eigen::VectorXd& FactorBase::getMeasurement() const
-{
- assert(getFeature() != nullptr && "calling getMeasurement before linking with a feature");
- return getFeature()->getMeasurement();
-}
-
-const Eigen::MatrixXd& FactorBase::getMeasurementCovariance() const
-{
- assert(getFeature() != nullptr && "calling getMeasurementCovariance before linking with a feature");
- return getFeature()->getMeasurementCovariance();
-}
-
-const Eigen::MatrixXd& FactorBase::getMeasurementSquareRootInformationUpper() const
-{
- assert(getFeature() != nullptr && "calling getMeasurementSquareRootInformationUpper before linking with a feature");
- return getFeature()->getMeasurementSquareRootInformationUpper();
-}
-
CaptureBasePtr FactorBase::getCapture() const
{
assert(getFeature() != nullptr && "calling getCapture before linking with a feature");
@@ -241,11 +241,6 @@ bool FactorBase::hasLandmarkOther(const LandmarkBasePtr &_lmk_other) const
return false;
}
-//void FactorBase::setApplyLossFunction(const bool _apply)
-//{
-// apply_loss_function_ = _apply;
-//}
-
void FactorBase::link(FeatureBasePtr _ftr_ptr)
{
assert(!is_removing_ && "linking a removed factor");
@@ -262,6 +257,10 @@ void FactorBase::link(FeatureBasePtr _ftr_ptr)
_ftr_ptr->addFactor(shared_from_this());
this->setFeature(_ftr_ptr);
+ // if frame, should be key
+ if (getCapture() and getFrame())
+ assert(getFrame()->isKey() && "Forbidden: linking a factor with a non-key frame.");
+
// set problem ( and register factor )
WOLF_WARN_COND(_ftr_ptr->getProblem() == nullptr, "ADDING FACTOR ", this->id(), " TO FEATURE ", _ftr_ptr->id(), " THAT IS NOT CONNECTED WITH PROBLEM.");
this->setProblem(_ftr_ptr->getProblem());
@@ -270,7 +269,11 @@ void FactorBase::link(FeatureBasePtr _ftr_ptr)
for (auto& frm_ow : frame_other_list_)
{
auto frame_other = frm_ow.lock();
- if(frame_other != nullptr) frame_other->addConstrainedBy(shared_from_this());
+ if(frame_other != nullptr)
+ {
+ assert(frame_other->isKey() && "Forbidden: linking a factor with a non-key frame_other.");
+ frame_other->addConstrainedBy(shared_from_this());
+ }
}
for (auto& cap_ow : capture_other_list_)
{
@@ -299,4 +302,312 @@ void FactorBase::setProblem(ProblemPtr _problem)
this->getProblem()->notifyFactor(shared_from_this(),ADD);
}
+void FactorBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+{
+ _stream << _tabs << "Fac" << id() << " " << getType() << " -->";
+ if ( getFrameOtherList() .empty()
+ && getCaptureOtherList() .empty()
+ && getFeatureOtherList() .empty()
+ && getLandmarkOtherList().empty())
+ _stream << " Abs";
+
+ for (const auto& Fow : getFrameOtherList())
+ if (!Fow.expired())
+ _stream << " Frm" << Fow.lock()->id();
+ for (const auto& Cow : getCaptureOtherList())
+ if (!Cow.expired())
+ _stream << " Cap" << Cow.lock()->id();
+ for (const auto& fow : getFeatureOtherList())
+ if (!fow.expired())
+ _stream << " Ftr" << fow.lock()->id();
+ for (const auto& Low : getLandmarkOtherList())
+ if (!Low.expired())
+ _stream << " Lmk" << Low.lock()->id();
+ _stream << std::endl;
+}
+
+void FactorBase::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);
+}
+
+CheckLog FactorBase::localCheck(bool _verbose, FactorBasePtr _fac_ptr, std::ostream& _stream, std::string _tabs) const
+{
+ CheckLog log;
+ std::stringstream inconsistency_explanation;
+
+ if (_verbose)
+ _stream << _tabs << "Fac" << id() << " @ " << _fac_ptr.get();
+
+ if ( getFrameOtherList() .empty()
+ && getCaptureOtherList() .empty()
+ && getFeatureOtherList() .empty()
+ && getLandmarkOtherList().empty() ) // case ABSOLUTE:
+ {
+ if (_verbose)
+ _stream << " --> Abs.";
+ }
+
+ // find constrained_by pointer in constrained frame
+ for (const auto& Fow : getFrameOtherList())
+ {
+ if (!Fow.expired())
+ {
+ const auto& Fo = Fow.lock();
+ if (_verbose)
+ {
+ _stream << " ( --> Frm" << Fo->id() << " <- ";
+ for (auto cby : Fo->getConstrainedByList())
+ _stream << " Fac" << cby->id();
+ }
+
+ // check constrained_by pointer in constrained frame
+ bool found = Fo->isConstrainedBy(_fac_ptr);
+ inconsistency_explanation << "The constrained Feature " << Fo->id() << " @ " << Fo
+ << " not found among constrained-by factors\n";
+ log.assertTrue((found), inconsistency_explanation);
+
+ }
+ }
+ if (_verbose && !getFrameOtherList().empty())
+ _stream << ")";
+ // find constrained_by pointer in constrained capture
+ for (const auto& Cow : getCaptureOtherList())
+ {
+ if (!Cow.expired())
+ {
+ const auto& Co = Cow.lock();
+
+ if (_verbose)
+ {
+ _stream << " ( --> Cap" << Co->id() << " <- ";
+ for (auto cby : Co->getConstrainedByList())
+ _stream << " Fac" << cby->id();
+ }
+
+ // check constrained_by pointer in constrained frame
+ bool found = Co->isConstrainedBy(_fac_ptr);
+ inconsistency_explanation << "The constrained capture " << Co->id() << " @ " << Co
+ << " not found among constrained-by factors\n";
+ log.assertTrue((found), inconsistency_explanation);
+
+ }
+ }
+ if (_verbose && !getCaptureOtherList().empty())
+ _stream << ")";
+ // find constrained_by pointer in constrained feature
+ for (const auto& fow : getFeatureOtherList())
+ {
+ if (!fow.expired())
+ {
+ const auto& fo = fow.lock();
+ if (_verbose)
+ {
+ _stream << " ( --> Ftr" << fo->id() << " <- ";
+ for (auto cby : fo->getConstrainedByList())
+ _stream << " Fac" << cby->id();
+ }
+
+ // check constrained_by pointer in constrained feature
+ bool found = fo->isConstrainedBy(_fac_ptr);
+ inconsistency_explanation << "The constrained feature " << fo->id() << " @ " << fo
+ << " not found among constrained-by factors\n";
+ log.assertTrue((found), inconsistency_explanation);
+ }
+ }
+ if (_verbose && !getFeatureOtherList().empty())
+ _stream << ")";
+
+ // find constrained_by pointer in constrained landmark
+ for (const auto& Low : getLandmarkOtherList())
+ {
+ if (Low.expired())
+ {
+ const auto& Lo = Low.lock();
+
+ if (_verbose)
+ {
+ _stream << " ( --> Lmk" << Lo->id() << " <- ";
+ for (auto cby : Lo->getConstrainedByList())
+ _stream << " Fac" << cby->id();
+ }
+
+ // check constrained_by pointer in constrained landmark
+ bool found = Lo->isConstrainedBy(_fac_ptr);
+ inconsistency_explanation << "The constrained landmark " << Lo->id() << " @ " << Lo
+ << " not found among constrained-by factors\n";
+ log.assertTrue((found), inconsistency_explanation);
+
+ }
+ }
+ if (_verbose && !getLandmarkOtherList().empty())
+ _stream << ")";
+ if (_verbose)
+ _stream << std::endl;
+ //Check Problem and feature ptrs
+ if (_verbose)
+ {
+ _stream << _tabs << " " << "-> Prb @ " << getProblem().get() << std::endl;
+ _stream << _tabs << " " << "-> Ftr" << getFeature()->id() << " @ " << getFeature().get() << std::endl;
+ }
+ auto ftr_ptr = getFeature();
+ // check problem and feature pointers
+ inconsistency_explanation << "The factor " << id() << " @ " << _fac_ptr.get() << " problem ptr " << getProblem().get()
+ << " is different from Feature's problem ptr " << ftr_ptr->getProblem().get() << "\n";
+ log.assertTrue((getProblem() == ftr_ptr->getProblem()), inconsistency_explanation);
+
+
+ inconsistency_explanation << "Fac" << id() << " @ " << _fac_ptr
+ << " ---> Ftr" << ftr_ptr->id() << " @ " << ftr_ptr
+ << " -X-> Fac" << id();
+ auto ftr_fac_list = ftr_ptr->getFactorList();
+ auto ftr_has_fac = std::find_if(ftr_fac_list.begin(), ftr_fac_list.end(), [&_fac_ptr](FactorBasePtr fac){ return fac == _fac_ptr;});
+ log.assertTrue(ftr_has_fac!= ftr_fac_list.end(), inconsistency_explanation);
+
+ // find state block pointers in all constrained nodes
+ SensorBasePtr S = getSensor(); // get own sensor to check sb
+ FrameBasePtr F = getFrame();
+ CaptureBasePtr C = getCapture();
+
+ for (auto sb : getStateBlockPtrVector())
+ {
+ bool found = false;
+ if (_verbose)
+ {
+ _stream << _tabs << " " << "sb @ " << sb.get();
+ if (sb)
+ {
+ auto lp = sb->getLocalParametrization();
+ if (lp)
+ _stream << " (lp @ " << lp.get() << ")";
+ }
+ }
+ bool found_here;
+
+ // find in own Frame
+ found_here = F->hasStateBlock(sb);
+ if (found_here && _verbose) _stream << " Frm" << F->id();
+ found = found || found_here;
+
+ // find in own Capture
+ found_here = C->hasStateBlock(sb);
+ if (found_here && _verbose) _stream << " Cap" << C->id();
+ found = found || found_here;
+
+ // Find in other Captures of the own Frame
+ if (!found_here)
+ for (auto FC : F->getCaptureList())
+ {
+ found_here = FC->hasStateBlock(sb);
+ if (found_here && _verbose) _stream << " Frm" << F->id() << ".Cap" << FC->id();
+ found = found || found_here;
+ }
+
+ // find in own Sensor
+ if (S)
+ {
+ found_here = S->hasStateBlock(sb);
+ if (found_here && _verbose) _stream << " Sen" << S->id();
+ found = found || found_here;
+ }
+
+
+ // find in constrained Frame
+ for (const auto& Fow : getFrameOtherList())
+ {
+ if (!Fow.expired())
+ {
+ const auto& Fo = Fow.lock();
+ found_here = Fo->hasStateBlock(sb);
+ if (found_here && _verbose) _stream << " FrmO" << Fo->id();
+ found = found || found_here;
+
+ // find in feature other's captures
+ for (auto FoC : Fo->getCaptureList())
+ {
+ found_here = FoC->hasStateBlock(sb);
+ if (found_here && _verbose) _stream << " FrmO" << Fo->id() << ".C" << FoC->id();
+ found = found || found_here;
+ }
+
+ }
+ }
+
+ // find in constrained Capture
+ for (const auto& Cow : getCaptureOtherList())
+ {
+ if (!Cow.expired())
+ {
+ const auto& Co = Cow.lock();
+ found_here = Co->hasStateBlock(sb);
+ if (found_here && _verbose) _stream << " CapO" << Co->id();
+ found = found || found_here;
+ }
+ }
+
+ // find in constrained Feature
+ for (const auto& fow : getFeatureOtherList())
+ {
+ if (!fow.expired())
+ {
+ const auto& fo = fow.lock();
+ // find in constrained feature's Frame
+ auto foF = fo->getFrame();
+ found_here = foF->hasStateBlock(sb);
+ if (found_here && _verbose) _stream << " FtrOF" << foF->id();
+ found = found || found_here;
+
+ // find in constrained feature's Capture
+ auto foC = fo->getCapture();
+ found_here = foC->hasStateBlock(sb);
+ if (found_here && _verbose) _stream << " FtrOC" << foC->id();
+ found = found || found_here;
+
+ // find in constrained feature's Sensor
+ auto foS = fo->getCapture()->getSensor();
+ found_here = foS->hasStateBlock(sb);
+ if (found_here && _verbose) _stream << " FtrOS" << foS->id();
+ found = found || found_here;
+ }
+ }
+
+ // find in constrained landmark
+ for (const auto& Low : getLandmarkOtherList())
+ {
+ if (!Low.expired())
+ {
+ const auto& Lo = Low.lock();
+ found_here = Lo->hasStateBlock(sb);
+ if (found_here && _verbose) _stream << " LmkO" << Lo->id();
+ found = found || found_here;
+ }
+ }
+
+ if (_verbose)
+ {
+ if (found)
+ _stream << " found";
+ else
+ _stream << " NOT FOUND !";
+ _stream << std::endl;
+ }
+
+ // check that the state block has been found somewhere
+ inconsistency_explanation << "The stateblock " << sb << " has not been found (is floating!)";
+ log.assertTrue((found), inconsistency_explanation);
+
+ inconsistency_explanation << "The stateblock " << sb << " of factor " << id() << " @ " << _fac_ptr << " is null\n";
+ log.assertTrue((sb.get() != nullptr), inconsistency_explanation);
+ }
+ return log;
+}
+
+bool FactorBase::check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
+{
+ auto fac_ptr = std::static_pointer_cast<FactorBase>(_node_ptr);
+ auto local_log = localCheck(_verbose, fac_ptr, _stream, _tabs);
+ _log.compose(local_log);
+
+ return _log.is_consistent_;
+}
} // namespace wolf
diff --git a/src/feature/feature_base.cpp b/src/feature/feature_base.cpp
index 8dc3801efa5f93e5bd0df593442e4860cb75135e..69133345e0f57c28e3135aa3b50e0d99e179be50 100644
--- a/src/feature/feature_base.cpp
+++ b/src/feature/feature_base.cpp
@@ -39,20 +39,16 @@ FeatureBase::~FeatureBase()
void FeatureBase::remove(bool viral_remove_empty_parent)
{
+ /* Order of removing:
+ * Factors are removed first, and afterwards the rest of nodes containing state blocks.
+ * In case multi-threading, solver can be called while removing.
+ * This order avoids SolverManager to ignore notifications (efficiency)
+ */
if (!is_removing_)
{
is_removing_ = true;
FeatureBasePtr this_f = shared_from_this(); // keep this alive while removing it
- // remove from upstream
- CaptureBasePtr C = capture_ptr_.lock();
- if (C)
- {
- C->removeFeature(this_f); // remove from upstream
- if (viral_remove_empty_parent && C->getFeatureList().empty() && C->getConstrainedByList().empty())
- C->remove(viral_remove_empty_parent); // remove upstream
- }
-
// remove downstream
while (!factor_list_.empty())
{
@@ -62,6 +58,15 @@ void FeatureBase::remove(bool viral_remove_empty_parent)
{
constrained_by_list_.front()->remove(viral_remove_empty_parent); // remove constrained
}
+
+ // remove from upstream
+ CaptureBasePtr C = capture_ptr_.lock();
+ if (C)
+ {
+ C->removeFeature(this_f); // remove from upstream
+ if (viral_remove_empty_parent && C->getFeatureList().empty() && C->getConstrainedByList().empty())
+ C->remove(viral_remove_empty_parent); // remove upstream
+ }
}
}
@@ -142,6 +147,11 @@ void FeatureBase::setMeasurementInformation(const Eigen::MatrixXd & _meas_info)
measurement_sqrt_information_upper_ = computeSqrtUpper(_meas_info);
}
+void FeatureBase::setMeasurement(const Eigen::VectorXd& _meas)
+{
+ measurement_ = _meas;
+}
+
void FeatureBase::setProblem(ProblemPtr _problem)
{
NodeBase::setProblem(_problem);
@@ -165,4 +175,93 @@ void FeatureBase::link(CaptureBasePtr _cpt_ptr)
WOLF_WARN("Linking with nullptr");
}
}
+void FeatureBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+{
+ _stream << _tabs << "Ftr" << id() << " trk" << trackId() << " " << getType() << ((_depth < 4) ? " -- " + std::to_string(getFactorList().size()) + "c " : "");
+ if (_constr_by)
+ {
+ _stream << " <--\t";
+ for (auto cby : getConstrainedByList())
+ _stream << "Fac" << cby->id() << " \t";
+ }
+ _stream << std::endl;
+ if (_metric)
+ _stream << _tabs << " " << "m = ( " << std::setprecision(2) << getMeasurement().transpose()
+ << " )" << std::endl;
+
+}
+
+void FeatureBase::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 >= 4)
+ for (auto c : getFactorList())
+ c->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
+}
+
+CheckLog FeatureBase::localCheck(bool _verbose, FeatureBasePtr _ftr_ptr, std::ostream& _stream, std::string _tabs) const
+{
+ CheckLog log;
+ std::stringstream inconsistency_explanation;
+
+ if (_verbose)
+ {
+ _stream << _tabs << "Ftr" << id() << " @ " << _ftr_ptr.get() << std::endl;
+ _stream << _tabs << " " << "-> Prb @ " << getProblem().get() << std::endl;
+ _stream << _tabs << " " << "-> Cap" << getCapture()->id() << " @ " << getCapture().get()
+ << std::endl;
+ }
+
+ auto cap_ptr = getCapture();
+ // check problem and capture pointers
+ inconsistency_explanation << "Feature frame problem pointer " << getProblem().get()
+ << " different from Capture problem pointer " << cap_ptr->getProblem().get() << "\n";
+ log.assertTrue((getProblem() == cap_ptr->getProblem()), inconsistency_explanation);
+
+
+ // check contrained_by
+ for (auto cby : getConstrainedByList())
+ {
+ if (_verbose)
+ {
+ _stream << _tabs << " " << "<- Fac" << cby->id() << " -> ";
+ for (const auto& fow : cby->getFeatureOtherList())
+ _stream << " Ftr" << fow.lock()->id();
+ _stream << std::endl;
+ }
+
+ // check constrained_by pointer to this feature
+ inconsistency_explanation << "constrained by Feature " << id() << " @ " << _ftr_ptr.get()
+ << " not found among constrained-by factors\n";
+ log.assertTrue((cby->hasFeatureOther(_ftr_ptr)), inconsistency_explanation);
+
+ }
+
+ auto cap_ftr = _ftr_ptr->getCapture();
+ inconsistency_explanation << "Ftr" << id() << " @ " << _ftr_ptr
+ << " ---> Cap" << cap_ftr->id() << " @ " << cap_ftr
+ << " -X-> Ftr" << id();
+ auto cap_ftr_list = cap_ftr->getFeatureList();
+ auto frame_has_cap = std::find_if(cap_ftr_list.begin(), cap_ftr_list.end(), [&_ftr_ptr](FeatureBasePtr ftr){ return ftr == _ftr_ptr;});
+ log.assertTrue(frame_has_cap != cap_ftr_list.end(), inconsistency_explanation);
+
+ for(auto fac : getFactorList())
+ {
+ inconsistency_explanation << "Ftr" << id() << " @ " << _ftr_ptr
+ << " ---> Fac" << fac->id() << " @ " << fac
+ << " -X-> Ftr" << id();
+ log.assertTrue((fac->getFeature() == _ftr_ptr), inconsistency_explanation);
+ }
+ return log;
+}
+bool FeatureBase::check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
+{
+ auto ftr_ptr = std::static_pointer_cast<FeatureBase>(_node_ptr);
+ auto local_log = localCheck(_verbose, ftr_ptr, _stream, _tabs);
+ _log.compose(local_log);
+
+ for(auto f : getFactorList()) f->check(_log, f, _verbose, _stream, _tabs + " ");
+
+ return _log.is_consistent_;
+}
} // namespace wolf
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index c0f4d310e1062c9a68ea94a0ee0b70f912cd9d15..de9fc606991d9f3b9cc73d52f130123c07e3d6e1 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -6,35 +6,69 @@
#include "core/state_block/state_block.h"
#include "core/state_block/state_angle.h"
#include "core/state_block/state_quaternion.h"
+#include "core/state_block/factory_state_block.h"
namespace wolf {
unsigned int FrameBase::frame_id_count_ = 0;
-FrameBase::FrameBase(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _v_ptr) :
- NodeBase("FRAME", "Base"),
- HasStateBlocks(""),
- trajectory_ptr_(),
- frame_id_(++frame_id_count_),
- type_(NON_ESTIMATED),
- time_stamp_(_ts)
+FrameBase::FrameBase(const FrameType & _tp,
+ const TimeStamp& _ts,
+ const std::string _frame_structure,
+ const VectorComposite& _state) :
+ NodeBase("FRAME", "FrameBase"),
+ HasStateBlocks(_frame_structure),
+ trajectory_ptr_(),
+ frame_id_(++frame_id_count_),
+ type_(_tp),
+ time_stamp_(_ts)
{
- if (_p_ptr)
+ for (const auto& pair_key_vec : _state)
{
- addStateBlock("P", _p_ptr);
- }
- if (_o_ptr)
- {
- addStateBlock("O", _o_ptr);
+ const auto& key = pair_key_vec.first;
+ const auto& vec = pair_key_vec.second;
+
+ StateBlockPtr sb = FactoryStateBlock::get().create(key, vec, false); // false = non fixed
+
+ addStateBlock(key, sb);
}
- if (_v_ptr)
+}
+
+
+FrameBase::FrameBase(const FrameType & _tp,
+ const TimeStamp& _ts,
+ const std::string _frame_structure,
+ const std::list<VectorXd>& _vectors) :
+ NodeBase("FRAME", "FrameBase"),
+ HasStateBlocks(_frame_structure),
+ trajectory_ptr_(),
+ frame_id_(++frame_id_count_),
+ type_(_tp),
+ time_stamp_(_ts)
+{
+ assert(_frame_structure.size() == _vectors.size() && "Structure size does not match number of provided vectors!");
+
+ auto vec_it = _vectors.begin();
+ for (const auto ckey : _frame_structure)
{
- addStateBlock("V", _v_ptr);
+ const auto& key = string(1,ckey);
+ const auto& vec = *vec_it;
+
+ StateBlockPtr sb = FactoryStateBlock::get().create(key, vec, false); // false = non fixed
+
+ addStateBlock(key, sb);
+
+ vec_it++;
}
}
-FrameBase::FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _v_ptr) :
- NodeBase("FRAME", "Base"),
+
+FrameBase::FrameBase(const FrameType & _tp,
+ const TimeStamp& _ts,
+ StateBlockPtr _p_ptr,
+ StateBlockPtr _o_ptr,
+ StateBlockPtr _v_ptr) :
+ NodeBase("FRAME", "FrameBase"),
HasStateBlocks(""),
trajectory_ptr_(),
frame_id_(++frame_id_count_),
@@ -55,8 +89,12 @@ FrameBase::FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr
}
}
-FrameBase::FrameBase(const std::string _frame_structure, const SizeEigen _dim, const FrameType & _tp, const TimeStamp& _ts, const Eigen::VectorXd& _x) :
- NodeBase("FRAME", "Base"),
+FrameBase::FrameBase(const FrameType & _tp,
+ const TimeStamp& _ts,
+ const std::string _frame_structure,
+ const SizeEigen _dim,
+ const Eigen::VectorXd& _x) :
+ NodeBase("FRAME", "FrameBase"),
HasStateBlocks(_frame_structure),
trajectory_ptr_(),
frame_id_(++frame_id_count_),
@@ -117,31 +155,36 @@ FrameBase::~FrameBase()
void FrameBase::remove(bool viral_remove_empty_parent)
{
+ /* Order of removing:
+ * Factors are removed first, and afterwards the rest of nodes containing state blocks.
+ * In case multi-threading, solver can be called while removing.
+ * This order avoids SolverManager to ignore notifications (efficiency)
+ */
if (!is_removing_)
{
is_removing_ = true;
FrameBasePtr this_F = shared_from_this(); // keep this alive while removing it
- // remove from upstream
- TrajectoryBasePtr T = trajectory_ptr_.lock();
- if (T)
+ // remove downstream
+ while (!constrained_by_list_.empty())
{
- T->removeFrame(this_F); // remove from upstream
+ constrained_by_list_.front()->remove(viral_remove_empty_parent); // remove constrained
}
-
- // remove downstream
while (!capture_list_.empty())
{
capture_list_.front()->remove(viral_remove_empty_parent); // remove downstream
}
- while (!constrained_by_list_.empty())
- {
- constrained_by_list_.front()->remove(viral_remove_empty_parent); // remove constrained
- }
// Remove Frame State Blocks
if ( isKeyOrAux() )
removeStateBlocks(getProblem());
+
+ // remove from upstream
+ TrajectoryBasePtr T = trajectory_ptr_.lock();
+ if (T)
+ {
+ T->removeFrame(this_F); // remove from upstream
+ }
}
}
@@ -359,4 +402,157 @@ void FrameBase::setProblem(ProblemPtr _problem)
cap->setProblem(_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()
+ << " " << getStructure()
+ << " ts = " << std::setprecision(3) << getTimeStamp()
+ << ((_depth < 2) ? " -- " + std::to_string(getCaptureList().size()) + "C " : "");
+ if (_constr_by)
+ {
+ _stream << " <-- ";
+ for (auto cby : getConstrainedByList())
+ _stream << "Fac" << cby->id() << " \t";
+ }
+ _stream << std::endl;
+
+ if (_metric && _state_blocks)
+ {
+ for (const auto& key : getStructure())
+ {
+ auto sb = getStateBlock(key);
+ _stream << _tabs << " " << key
+ << "[" << (sb->isFixed() ? "Fix" : "Est")
+ << "] = ( " << std::setprecision(3) << sb->getState().transpose() << " )"
+ << std::endl;
+ }
+ }
+ else if (_metric)
+ {
+ _stream << _tabs << " " << (isFixed() ? "Fix" : "Est");
+ _stream << ",\t x = ( " << std::setprecision(3) << getStateVector().transpose() << " )";
+ _stream << std::endl;
+ }
+ else if (_state_blocks)
+ {
+ _stream << _tabs << " " << "sb:";
+ for (const auto& key : getStructure())
+ {
+ const auto& sb = getStateBlock(key);
+ _stream << " " << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "]; ";
+ }
+ _stream << std::endl;
+ }
+}
+
+void FrameBase::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 >= 2)
+ for (auto C : getCaptureList())
+ C->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
+}
+
+CheckLog FrameBase::localCheck(bool _verbose, FrameBasePtr _frm_ptr, std::ostream& _stream, std::string _tabs) const
+{
+ CheckLog log;
+ std::stringstream inconsistency_explanation;
+
+ if (_verbose) {
+ _stream << _tabs << (isKeyOrAux() ? (isKey() ? "KFrm" : "EFrm") : "Frm")
+ << id() << " @ " << _frm_ptr.get() << std::endl;
+ _stream << _tabs << " " << "-> Prb @ " << getProblem().get() << std::endl;
+ _stream << _tabs << " " << "-> Trj @ " << getTrajectory().get() << std::endl;
+ }
+ for (const auto &pair: getStateBlockMap()) {
+
+ auto sb = pair.second;
+
+ // check for valid state block
+ inconsistency_explanation << "Frame " << id() << " @ "<< _frm_ptr.get()
+ << " has State block pointer " << sb.get()
+ << " = 0 \n";
+ log.assertTrue((sb.get() != 0), inconsistency_explanation);
+
+ if (_verbose) {
+ _stream << _tabs << " " << pair.first << " sb @ " << sb.get();
+ if (sb) {
+ auto lp = sb->getLocalParametrization();
+ if (lp)
+ _stream << " (lp @ " << lp.get() << ")";
+ }
+ _stream << std::endl;
+ }
+ }
+
+ // check problem pointer
+ auto trajectory_ptr = getTrajectory();
+ auto trajectory_problem_ptr = trajectory_ptr->getProblem();
+ inconsistency_explanation << "Frame problem pointer " << getProblem().get()
+ << " different from Trajectory problem pointer " << trajectory_problem_ptr.get() << "\n";
+ log.assertTrue((getProblem() == trajectory_problem_ptr), inconsistency_explanation);
+
+ // // check trajectory pointer
+ // inconsistency_explanation << "Frame trajectory pointer is " << getTrajectory()
+ // << " but trajectory pointer is" << trajectory_ptr << "\n";
+ // log.assertTrue((getTrajectory() == T, inconsistency_explanation);
+
+ // check constrained_by
+ for (auto cby : getConstrainedByList())
+ {
+ if (_verbose)
+ {
+ _stream << _tabs << " " << "<- Fac" << cby->id() << " -> ";
+ for (const auto& Fow : cby->getFrameOtherList())
+ _stream << " Frm" << Fow.lock()->id() << std::endl;
+
+
+ // check constrained_by pointer to this frame
+ inconsistency_explanation << "constrained-by frame " << id() << " @ " << _frm_ptr
+ << " not found among constrained-by factors\n";
+ auto F = std::static_pointer_cast<FrameBase>(_frm_ptr);
+ log.assertTrue((cby->hasFrameOther(F)), inconsistency_explanation);
+
+ for (auto sb : cby->getStateBlockPtrVector())
+ {
+ if (_verbose) {
+ _stream << _tabs << " " << "sb @ " << sb.get();
+ if (sb) {
+ auto lp = sb->getLocalParametrization();
+ if (lp)
+ _stream << " (lp @ " << lp.get() << ")";
+ }
+ _stream << std::endl;
+ }
+ }
+ }
+ }
+
+ auto trj_ptr = getTrajectory();
+ 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);
+
+ for(auto C : getCaptureList())
+ {
+ inconsistency_explanation << "Frm " << id() << " @ " << _frm_ptr
+ << " ---> Cap" << C->id() << " @ " << C
+ << " -X-> Frm" << id();
+ log.assertTrue((C->getFrame() == _frm_ptr), inconsistency_explanation);
+ }
+ return log;
+}
+
+bool FrameBase::check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
+{
+ auto frm_ptr = std::static_pointer_cast<FrameBase>(_node_ptr);
+ auto local_log = localCheck(_verbose, frm_ptr, _stream, _tabs);
+ _log.compose(local_log);
+ for(auto C : getCaptureList()) C->check(_log, C, _verbose, _stream, _tabs + " ");
+
+ return _log.is_consistent_;
+}
} // namespace wolf
diff --git a/src/hardware/hardware_base.cpp b/src/hardware/hardware_base.cpp
index adda7984748ebfb2fa6e40517f0d1bedc331efd2..72f07b8e216e6ca830ec9021dbb7ca1144d09860 100644
--- a/src/hardware/hardware_base.cpp
+++ b/src/hardware/hardware_base.cpp
@@ -4,7 +4,7 @@
namespace wolf {
HardwareBase::HardwareBase() :
- NodeBase("HARDWARE", "Base")
+ NodeBase("HARDWARE", "HardwareBase")
{
// std::cout << "constructed H" << std::endl;
}
@@ -20,4 +20,41 @@ SensorBasePtr HardwareBase::addSensor(SensorBasePtr _sensor_ptr)
return _sensor_ptr;
}
+void HardwareBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+{
+ _stream << _tabs << "Hardware" << ((_depth < 1) ? (" -- " + std::to_string(getSensorList().size()) + "S") : "") << std::endl;
+
+}
+void HardwareBase::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 S : getSensorList())
+ S->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
+}
+
+CheckLog HardwareBase::localCheck(bool _verbose, HardwareBasePtr _hwd_ptr, std::ostream& _stream, std::string _tabs) const
+{
+ CheckLog log;
+ std::stringstream inconsistency_explanation;
+ if (_verbose)
+ {
+ _stream << _tabs << "Hrw @ " << _hwd_ptr.get() << std::endl;
+ }
+
+ // check pointer to Problem
+ inconsistency_explanation << "Hwd->getProblem() [" << getProblem().get()
+ << "] -> " << " Prb->getHardware() [" << getProblem()->getHardware().get() << "] -> Hwd [" << _hwd_ptr.get() << "] Mismatch!\n";
+ log.assertTrue((_hwd_ptr->getProblem()->getHardware().get() == _hwd_ptr.get()), inconsistency_explanation);
+ return log;
+}
+bool HardwareBase::check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
+{
+ auto hwd_ptr = std::static_pointer_cast<HardwareBase>(_node_ptr);
+ auto local_log = localCheck(_verbose, hwd_ptr, _stream, _tabs);
+ _log.compose(local_log);
+ for (auto S : getSensorList())
+ S->check(_log, S, _verbose, _stream, _tabs + " ");
+ return _log.is_consistent_;
+}
} // namespace wolf
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index bc57f27294e6a0b05d67cbcf44725dd2b819f63b..ada2bef9648e15ca6a2809d9bf35dc88f2e454ac 100644
--- a/src/landmark/landmark_base.cpp
+++ b/src/landmark/landmark_base.cpp
@@ -37,16 +37,16 @@ LandmarkBase::~LandmarkBase()
void LandmarkBase::remove(bool viral_remove_empty_parent)
{
+ /* Order of removing:
+ * Factors are removed first, and afterwards the rest of nodes containing state blocks.
+ * In case multi-threading, solver can be called while removing.
+ * This order avoids SolverManager to ignore notifications (efficiency)
+ */
if (!is_removing_)
{
is_removing_ = true;
LandmarkBasePtr this_L = shared_from_this(); // keep this alive while removing it
- // remove from upstream
- auto M = map_ptr_.lock();
- if (M)
- M->removeLandmark(this_L);
-
// remove constrained by
while (!constrained_by_list_.empty())
{
@@ -55,6 +55,11 @@ void LandmarkBase::remove(bool viral_remove_empty_parent)
// Remove State Blocks
removeStateBlocks(getProblem());
+
+ // remove from upstream
+ auto M = map_ptr_.lock();
+ if (M)
+ M->removeLandmark(this_L);
}
}
@@ -153,6 +158,46 @@ bool LandmarkBase::isConstrainedBy(const FactorBasePtr &_factor) const
return false;
}
+void LandmarkBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+{
+ _stream << _tabs << "Lmk" << id() << " " << getType();
+ if (_constr_by)
+ {
+ _stream << "\t<-- ";
+ for (auto cby : getConstrainedByList())
+ _stream << "Fac" << cby->id() << " \t";
+ }
+ _stream << std::endl;
+
+ if (_metric && _state_blocks){
+ for (const auto& key : getStructure())
+ {
+ auto sb = getStateBlock(key);
+ _stream << " " << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "] = ( " << sb->getState().transpose() << " )" << std::endl;
+ }
+ }
+ else if (_metric)
+ {
+ _stream << _tabs << " " << (isFixed() ? "Fix" : "Est");
+ _stream << ",\t x = ( " << std::setprecision(2) << getStateVector().transpose() << " )";
+ _stream << std::endl;
+ }
+ else if (_state_blocks)
+ {
+ _stream << _tabs << " " << "sb:";
+ for (const auto& key : getStructure())
+ {
+ const auto& sb = getStateBlock(key);
+ _stream << " " << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "]; ";
+ }
+ _stream << std::endl;
+ }
+}
+
+void LandmarkBase::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);
+}
LandmarkBasePtr LandmarkBase::create(const YAML::Node& _node)
{
unsigned int id = _node["id"] .as< unsigned int >();
@@ -179,10 +224,94 @@ LandmarkBasePtr LandmarkBase::create(const YAML::Node& _node)
return lmk;
}
+CheckLog LandmarkBase::localCheck(bool _verbose, LandmarkBasePtr _lmk_ptr, std::ostream& _stream, std::string _tabs) const
+{
+ CheckLog log;
+ std::stringstream inconsistency_explanation;
+
+ if (_verbose)
+ {
+ _stream << _tabs << "Lmk" << id() << " @ " << _lmk_ptr.get() << std::endl;
+ _stream << _tabs << " -> Prb @ " << getProblem().get() << std::endl;
+ _stream << _tabs << " -> Map @ " << getMap().get() << std::endl;
+ for (const auto& pair : getStateBlockMap())
+ {
+ auto sb = pair.second;
+ _stream << _tabs << " " << pair.first << " sb @ " << sb.get();
+ if (sb)
+ {
+ auto lp = sb->getLocalParametrization();
+ if (lp)
+ _stream << " (lp @ " << lp.get() << ")";
+ }
+ _stream << std::endl;
+ }
+ }
+
+ auto map_ptr = getMap();
+ // check problem and map pointers
+ inconsistency_explanation << "Landmarks' problem ptr "
+ << getProblem().get() << " different from Map's problem ptr "
+ << map_ptr->getProblem().get() << "\n";
+
+ log.assertTrue((getProblem() == map_ptr->getProblem()), inconsistency_explanation);
+
+ for (auto cby : getConstrainedByList())
+ {
+ if (_verbose)
+ {
+ _stream << _tabs << " " << "<- Fac" << cby->id() << " ->";
+
+ for (const auto& Low : cby->getLandmarkOtherList())
+ {
+ if (!Low.expired())
+ {
+ const auto& Lo = Low.lock();
+ _stream << " Lmk" << Lo->id();
+ }
+ }
+ _stream << std::endl;
+ }
+
+ // check constrained-by factors
+ inconsistency_explanation << "constrained-by landmark " << id() << " @ " << _lmk_ptr.get()
+ << " not found among constrained-by factors\n";
+ log.assertTrue((cby->hasLandmarkOther(_lmk_ptr)), inconsistency_explanation);
+
+ for (auto sb : cby->getStateBlockPtrVector()) {
+ if (_verbose) {
+ _stream << _tabs << " " << "sb @ " << sb.get();
+ if (sb) {
+ auto lp = sb->getLocalParametrization();
+ if (lp)
+ _stream << " (lp @ " << lp.get() << ")";
+ }
+ _stream << std::endl;
+ }
+ }
+ }
+
+ inconsistency_explanation << "Lmk" << id() << " @ " << _lmk_ptr
+ << " ---> Map" << map_ptr
+ << " -X-> Lmk" << id();
+ auto map_lmk_list = map_ptr->getLandmarkList();
+ auto map_has_lmk = std::find_if(map_lmk_list.begin(), map_lmk_list.end(), [&_lmk_ptr](LandmarkBasePtr lmk){ return lmk == _lmk_ptr;});
+ log.assertTrue(map_has_lmk != map_lmk_list.end(), inconsistency_explanation);
+
+ return log;
+}
+bool LandmarkBase::check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
+{
+ auto lmk_ptr = std::static_pointer_cast<LandmarkBase>(_node_ptr);
+ auto local_log = localCheck(_verbose, lmk_ptr, _stream, _tabs);
+ _log.compose(local_log);
+
+ return _log.is_consistent_;
+}
// Register landmark creator
namespace
{
-const bool WOLF_UNUSED registered_lmk_base = LandmarkFactory::get().registerCreator("LandmarkBase", LandmarkBase::create);
+const bool WOLF_UNUSED registered_lmk_base = FactoryLandmark::get().registerCreator("LandmarkBase", LandmarkBase::create);
}
} // namespace wolf
diff --git a/src/map/map_base.cpp b/src/map/map_base.cpp
index 3a3d32f8ac073ba88160995f69af8e1c150426bb..4c13360effa052b61b189042a95d2ec92bedfa4d 100644
--- a/src/map/map_base.cpp
+++ b/src/map/map_base.cpp
@@ -49,7 +49,7 @@ void MapBase::load(const std::string& _map_file_dot_yaml)
for (unsigned int i = 0; i < nlandmarks; i++)
{
YAML::Node lmk_node = map["landmarks"][i];
- LandmarkBasePtr lmk_ptr = LandmarkFactory::get().create(lmk_node["type"].as<std::string>(), lmk_node);
+ LandmarkBasePtr lmk_ptr = FactoryLandmark::get().create(lmk_node["type"].as<std::string>(), lmk_node);
lmk_ptr->link(shared_from_this());
}
@@ -90,4 +90,39 @@ std::string MapBase::dateTimeNow()
return date_time;
}
+void MapBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+{
+ _stream << _tabs << "Map" << ((_depth < 1) ? (" -- " + std::to_string(getLandmarkList().size()) + "L") : "") << std::endl;
+}
+void MapBase::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 L : getLandmarkList())
+ L->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
+}
+
+CheckLog MapBase::localCheck(bool _verbose, MapBasePtr _map_ptr, std::ostream& _stream, std::string _tabs) const
+{
+ CheckLog log;
+ std::stringstream inconsistency_explanation;
+
+ if (_verbose)
+ _stream << _tabs << "Map @ " << _map_ptr.get() << std::endl;
+
+ // check pointer to Problem
+ inconsistency_explanation << "Map->getProblem() [" << getProblem().get()
+ << "] -> " << " Prb->getMap() [" << getProblem()->getMap().get() << "] -> Map [" << _map_ptr.get() << "] Mismatch!\n";
+ log.assertTrue((_map_ptr->getProblem()->getMap().get() == _map_ptr.get()), inconsistency_explanation);
+ return log;
+}
+bool MapBase::check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
+{
+ auto map_ptr = std::static_pointer_cast<MapBase>(_node_ptr);
+ auto local_log = localCheck(_verbose, map_ptr, _stream, _tabs);
+ _log.compose(local_log);
+ for (auto L : getLandmarkList())
+ L->check(_log, L, _verbose, _stream, _tabs + " ");
+ return _log.is_consistent_;
+}
} // namespace wolf
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 53acc960ecc87a61d28d7e1567af966d0579b375..225e6f1ed184b3b51959ab4f125c1dbb9066eaec 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -7,15 +7,23 @@
#include "core/processor/processor_motion.h"
#include "core/processor/processor_tracker.h"
#include "core/capture/capture_pose.h"
+#include "core/capture/capture_void.h"
#include "core/factor/factor_base.h"
-#include "core/sensor/sensor_factory.h"
-#include "core/processor/processor_factory.h"
+#include "core/factor/factor_block_absolute.h"
+#include "core/factor/factor_quaternion_absolute.h"
+#include "core/sensor/factory_sensor.h"
+#include "core/processor/factory_processor.h"
#include "core/state_block/state_block.h"
+#include "core/state_block/state_quaternion.h"
+#include "core/state_block/state_angle.h"
+#include "core/tree_manager/factory_tree_manager.h"
+#include "core/tree_manager/tree_manager_base.h"
#include "core/utils/logging.h"
#include "core/utils/params_server.h"
#include "core/utils/loader.h"
#include "core/utils/check_log.h"
-
+#include "core/math/covariance.h"
+#include "core/state_block/factory_state_block.h"
// IRI libs includes
@@ -28,16 +36,18 @@
#include <vector>
#include <unordered_set>
+
namespace wolf
{
Problem::Problem(const std::string& _frame_structure, SizeEigen _dim) :
+ tree_manager_(nullptr),
hardware_ptr_(std::make_shared<HardwareBase>()),
- trajectory_ptr_(std::make_shared<TrajectoryBase>(_frame_structure)),
+ trajectory_ptr_(std::make_shared<TrajectoryBase>()),
map_ptr_(std::make_shared<MapBase>()),
processor_is_motion_list_(std::list<IsMotionPtr>()),
- prior_is_set_(false),
- frame_structure_(_frame_structure)
+ frame_structure_(_frame_structure),
+ prior_options_(std::make_shared<PriorOptions>())
{
dim_ = _dim;
if (_frame_structure == "PO" and _dim == 2)
@@ -59,7 +69,6 @@ Problem::Problem(const std::string& _frame_structure, SizeEigen _dim) :
}
else std::runtime_error(
"Problem::Problem(): Unknown frame structure. Add appropriate frame structure to the switch statement.");
-
}
void Problem::setup()
@@ -119,13 +128,20 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
WOLF_WARN(e.what());
WOLF_WARN("Support for subscribers disabled...");
}
- for (auto it : _server.getParam<std::vector<std::string>>("packages")) {
+ for (auto it : _server.getParam<std::vector<std::string>>("packages_subscriber")) {
std::string subscriber = packages_path + "/libsubscriber_" + it + lib_extension;
WOLF_TRACE("Loading subscriber " + subscriber);
auto l = new LoaderRaw(subscriber);
l->load();
loaders.push_back(l);
}
+ for (auto it : _server.getParam<std::vector<std::string>>("packages_publisher")) {
+ std::string subscriber = packages_path + "/libpublisher_" + it + lib_extension;
+ WOLF_TRACE("Loading publisher " + subscriber);
+ auto l = new LoaderRaw(subscriber);
+ l->load();
+ loaders.push_back(l);
+ }
std::vector<std::string> raw_libs;
try {
raw_libs = _server.getParam<std::vector<std::string>>("raw_libs");
@@ -152,22 +168,40 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
procesorMap.insert(std::pair<std::string, ProcessorBasePtr>(prc["name"], problem->installProcessor(prc["type"], prc["name"], prc["sensor_name"], _server)));
}
+ // Tree manager
+ std::string tree_manager_type = _server.getParam<std::string>("problem/tree_manager/type");
+ WOLF_TRACE("Tree Manager Type: ", tree_manager_type);
+ if (tree_manager_type != "None" and tree_manager_type != "none")
+ problem->setTreeManager(AutoConfFactoryTreeManager::get().create(tree_manager_type, "tree manager", _server));
+
// Prior
- Eigen::VectorXd prior_state = _server.getParam<Eigen::VectorXd>("problem/prior/state");
- Eigen::MatrixXd prior_cov = _server.getParam<Eigen::MatrixXd>("problem/prior/cov");
- double prior_time_tolerance = _server.getParam<double>("problem/prior/time_tolerance");
- double prior_ts = _server.getParam<double>("problem/prior/timestamp");
+ std::string prior_mode = _server.getParam<std::string>("problem/prior/mode");
+ WOLF_TRACE("Prior mode: ", prior_mode);
+ if (prior_mode == "nothing")
+ {
+ problem->setPriorOptions(prior_mode);
+ }
+ else if (prior_mode == "factor")
+ {
+ problem->setPriorOptions(prior_mode,
+ _server.getParam<double>("problem/prior/time_tolerance"),
+ _server.getParam<VectorComposite>("problem/prior/state"),
+ _server.getParam<VectorComposite>("problem/prior/sigma"));
- WOLF_TRACE("prior timestamp:\n" , prior_ts);
- WOLF_TRACE("prior state:\n" , prior_state.transpose());
- WOLF_TRACE("prior covariance:\n" , prior_cov);
- WOLF_TRACE("prior time tolerance:\n", prior_time_tolerance);
- problem->setPrior(prior_state, prior_cov, prior_ts, prior_time_tolerance);
+
+ }
+ else
+ {
+ WOLF_TRACE("Prior mode: ", prior_mode);
+ problem->setPriorOptions(prior_mode,
+ _server.getParam<double>("problem/prior/time_tolerance"),
+ _server.getParam<VectorComposite>("problem/prior/state"));
+ }
// Done
return problem;
- }
+}
Problem::~Problem()
{
@@ -179,7 +213,7 @@ SensorBasePtr Problem::installSensor(const std::string& _sen_type, //
const Eigen::VectorXd& _extrinsics, //
ParamsSensorBasePtr _intrinsics)
{
- SensorBasePtr sen_ptr = SensorFactory::get().create(_sen_type, _unique_sensor_name, _extrinsics, _intrinsics);
+ SensorBasePtr sen_ptr = FactorySensor::get().create(_sen_type, _unique_sensor_name, _extrinsics, _intrinsics);
sen_ptr->link(getHardware());
return sen_ptr;
}
@@ -194,7 +228,7 @@ SensorBasePtr Problem::installSensor(const std::string& _sen_type, //
if (_intrinsics_filename != "")
{
assert(file_exists(_intrinsics_filename) && "Cannot install sensor: intrinsics' YAML file does not exist.");
- ParamsSensorBasePtr intr_ptr = ParamsSensorFactory::get().create(_sen_type, _intrinsics_filename);
+ ParamsSensorBasePtr intr_ptr = FactoryParamsSensor::get().create(_sen_type, _intrinsics_filename);
return installSensor(_sen_type, _unique_sensor_name, _extrinsics, intr_ptr);
}
else
@@ -206,7 +240,7 @@ SensorBasePtr Problem::installSensor(const std::string& _sen_type, //
const std::string& _unique_sensor_name, //
const ParamsServer& _server)
{
- SensorBasePtr sen_ptr = AutoConfSensorFactory::get().create(_sen_type, _unique_sensor_name, _server);
+ SensorBasePtr sen_ptr = AutoConfFactorySensor::get().create(_sen_type, _unique_sensor_name, _server);
sen_ptr->link(getHardware());
return sen_ptr;
}
@@ -214,7 +248,7 @@ SensorBasePtr Problem::installSensor(const std::string& _sen_type, //
ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
const std::string& _unique_processor_name, //
SensorBasePtr _corresponding_sensor_ptr, //
- ProcessorParamsBasePtr _prc_params)
+ ParamsProcessorBasePtr _prc_params)
{
if (_corresponding_sensor_ptr == nullptr)
{
@@ -223,7 +257,7 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
return ProcessorBasePtr();
}
- ProcessorBasePtr prc_ptr = ProcessorFactory::get().create(_prc_type, _unique_processor_name, _prc_params);
+ ProcessorBasePtr prc_ptr = FactoryProcessor::get().create(_prc_type, _unique_processor_name, _prc_params);
//Dimension check
int prc_dim = prc_ptr->getDim();
@@ -233,10 +267,6 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
prc_ptr->configure(_corresponding_sensor_ptr);
prc_ptr->link(_corresponding_sensor_ptr);
- // setting the origin in all processor motion if origin already setted
- if (prc_ptr->isMotion() && prior_is_set_)
- (std::static_pointer_cast<ProcessorMotion>(prc_ptr))->setOrigin(getLastKeyFrame());
-
return prc_ptr;
}
@@ -253,7 +283,7 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
else
{
assert(file_exists(_params_filename) && "Cannot install processor: parameters' YAML file does not exist.");
- ProcessorParamsBasePtr prc_params = ProcessorParamsFactory::get().create(_prc_type, _params_filename);
+ ParamsProcessorBasePtr prc_params = FactoryParamsProcessor::get().create(_prc_type, _params_filename);
return installProcessor(_prc_type, _unique_processor_name, sen_ptr, prc_params);
}
}
@@ -267,7 +297,7 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
if (sen_ptr == nullptr)
throw std::runtime_error("Cannot bind processor. Reason: Sensor \"" + _corresponding_sensor_name + "\" not found. Check sensor name, it must match in sensor and processor!");
- ProcessorBasePtr prc_ptr = AutoConfProcessorFactory::get().create(_prc_type, _unique_processor_name, _server);
+ ProcessorBasePtr prc_ptr = AutoConfFactoryProcessor::get().create(_prc_type, _unique_processor_name, _server);
//Dimension check
int prc_dim = prc_ptr->getDim();
@@ -277,11 +307,6 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
prc_ptr->configure(sen_ptr);
prc_ptr->link(sen_ptr);
-
- // setting the origin in all processor motion if origin already setted
- if (prc_ptr->isMotion() && prior_is_set_)
- (std::static_pointer_cast<ProcessorMotion>(prc_ptr))->setOrigin(getLastKeyFrame());
-
return prc_ptr;
}
@@ -300,196 +325,231 @@ SensorBasePtr Problem::getSensor(const std::string& _sensor_name) const
return (*sen_it);
}
-FrameBasePtr Problem::emplaceFrame(const std::string& _frame_structure, //
+FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, //
+ const TimeStamp& _time_stamp, //
+ const StateStructure& _frame_structure, //
const SizeEigen _dim, //
- FrameType _frame_key_type, //
- const Eigen::VectorXd& _frame_state, //
- const TimeStamp& _time_stamp)
+ const Eigen::VectorXd& _frame_state)
{
- auto frm = FrameBase::emplace<FrameBase>(trajectory_ptr_, _frame_structure, _dim, _frame_key_type, _time_stamp, _frame_state);
+ auto frm = FrameBase::emplace<FrameBase>(trajectory_ptr_,
+ _frame_key_type,
+ _time_stamp,
+ _frame_structure,
+ _dim,
+ _frame_state);
return frm;
}
-FrameBasePtr Problem::emplaceFrame(const std::string& _frame_structure, //
- const SizeEigen _dim, //
- FrameType _frame_key_type, //
- const TimeStamp& _time_stamp)
+FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, //
+ const TimeStamp& _time_stamp, //
+ const StateStructure& _frame_structure, //
+ const SizeEigen _dim)
{
- return emplaceFrame(_frame_structure, _dim, _frame_key_type, getState(_time_stamp), _time_stamp);
+ return emplaceFrame(_frame_key_type,
+ _time_stamp,
+ _frame_structure,
+ getState(_time_stamp));
}
-FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, //
- const Eigen::VectorXd& _frame_state, //
- const TimeStamp& _time_stamp)
+FrameBasePtr Problem::emplaceFrame (FrameType _frame_key_type, //
+ const TimeStamp& _time_stamp, //
+ const StateStructure& _frame_structure, //
+ const VectorComposite& _frame_state)
{
- return emplaceFrame(this->getFrameStructure(), this->getDim(), _frame_key_type, _frame_state, _time_stamp);
+ return FrameBase::emplace<FrameBase>(getTrajectory(),
+ _frame_key_type,
+ _time_stamp,
+ _frame_structure,
+ _frame_state);
}
-FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, //
- const TimeStamp& _time_stamp)
+FrameBasePtr Problem::emplaceFrame (FrameType _frame_key_type, //
+ const TimeStamp& _time_stamp, //
+ const VectorComposite& _frame_state)
{
- return emplaceFrame(this->getFrameStructure(), this->getDim(), _frame_key_type, _time_stamp);
+ return FrameBase::emplace<FrameBase>(getTrajectory(),
+ _frame_key_type,
+ _time_stamp,
+ getFrameStructure(),
+ _frame_state);
}
-Eigen::VectorXd Problem::getCurrentState() const
+FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, //
+ const TimeStamp& _time_stamp, //
+ const Eigen::VectorXd& _frame_state)
{
- Eigen::VectorXd state(getFrameStructureSize());
- getCurrentState(state);
- return state;
+ return emplaceFrame(_frame_key_type,
+ _time_stamp,
+ this->getFrameStructure(),
+ this->getDim(),
+ _frame_state);
}
-void Problem::getCurrentState(Eigen::VectorXd& _state) const
+FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, //
+ const TimeStamp& _time_stamp)
{
- TimeStamp ts; // throwaway timestamp
- getCurrentStateAndStamp(_state, ts);
+ return emplaceFrame(_frame_key_type,
+ _time_stamp,
+ this->getFrameStructure(),
+ this->getDim());
}
-void Problem::getCurrentStateAndStamp(Eigen::VectorXd& _state, TimeStamp& _ts) const
-{
- if (!processor_is_motion_list_.empty())
+TimeStamp Problem::getTimeStamp ( ) const
+{
+ if ( processor_is_motion_list_.empty() ) // Use last estimated frame's state
{
- // retrieve the minimum of the most recent ts in all processor is motion then call getSate(ts, state)
- std::list<TimeStamp> proc_is_motion_current_ts;
- for (auto proc: processor_is_motion_list_){
- proc_is_motion_current_ts.push_back(proc->getCurrentTimeStamp());
- }
- auto min_it = std::min_element(proc_is_motion_current_ts.begin(), proc_is_motion_current_ts.end());
- getState(*min_it, _state);
- _ts = *min_it;
+ auto last_kf_or_aux = trajectory_ptr_->getLastKeyOrAuxFrame();
+
+ assert(last_kf_or_aux != nullptr && "Problem has no Keyframe so no timestamp can be obtained!");
+
+ return last_kf_or_aux->getTimeStamp();
}
- else if (trajectory_ptr_->getLastKeyOrAuxFrame() != nullptr)
+ else
{
- // kind of redundant with getState(_ts, _state)
- trajectory_ptr_->getLastKeyOrAuxFrame()->getTimeStamp(_ts);
- trajectory_ptr_->getLastKeyOrAuxFrame()->getState(_state);
+ TimeStamp ts(0);
+ for (const auto& prc : processor_is_motion_list_)
+ if (prc->getTimeStamp() > ts)
+ ts = prc->getTimeStamp();
+ return ts;
}
- else
- _state = zeroState();
}
-// Problem of this implmentation: if more state
-void Problem::getState(const TimeStamp& _ts, Eigen::VectorXd& _state) const
+VectorComposite Problem::getState(const StateStructure& _structure) const
{
- // if _ts is too recent, for some of the processor is motion, they return the state corresponding to their last frame timestamp
- FrameBasePtr closest_frame = trajectory_ptr_->closestKeyOrAuxFrameToTimeStamp(_ts);
- if (processor_is_motion_list_.empty()){
- if (closest_frame != nullptr)
- _state = closest_frame->getState();
+ StateStructure structure = (_structure == "" ? getFrameStructure() : _structure);
+ VectorComposite state;
+
+ if ( processor_is_motion_list_.empty() ) // Use last estimated frame's state
+ {
+ auto last_kf_or_aux = trajectory_ptr_->getLastKeyOrAuxFrame();
+ if (last_kf_or_aux)
+ state = last_kf_or_aux->getState(structure);
else
- _state = zeroState();
+ state = stateZero(structure);
}
-
- // RETRIEVE FROM PROCESSOR MOTION
- // TODO: current implementation messy, would be much easier with a state being an std::unordered_map
- else {
- // Iterate over the problem state structure and get the corresponding state
- // in the first processor is motion that provides it
- // finally check if the state to concatenate list has the same total size as the problem state_size
-
- // an map is necessary as a temporary structure because we do not know in which order we will find the state blocks in processors
- std::unordered_map<char, Eigen::VectorXd> states_to_concat_map; // not necessary to be ordered
- for (auto proc: processor_is_motion_list_){
- Eigen::VectorXd proc_state = proc->getState(_ts);
-
- int idx = 0;
- for (char sb_name: proc->getStateStructure()){
- // not already there
- if (states_to_concat_map.find(sb_name) == states_to_concat_map.end()){
- if (sb_name == 'O'){
- int size_sb = dim_ == 3 ? 4 : 1; // really bad: should be more transparent
- states_to_concat_map[sb_name] = proc_state.segment(idx, size_sb);
- idx += size_sb;
- }
- else{
- int size_sb = dim_ == 3 ? 3 : 2;
- states_to_concat_map[sb_name] = proc_state.segment(idx, size_sb);
- idx += size_sb;
- }
- }
+ else // Compose from different processor motion
+ {
+ // compose the states of all processor motions into one only state
+ for (const auto& prc : processor_is_motion_list_)
+ {
+ const auto& prc_state = prc->getState();
+ for (const auto& pair_key_vec : prc_state)
+ {
+ if (state.count(pair_key_vec.first) == 0) // only add those keys that do not exist yet
+ state.insert(pair_key_vec);
}
}
-
- int concat_size = 0;
- for (auto state_map_it: states_to_concat_map){
- concat_size += state_map_it.second.size();
+ // check for empty blocks and fill them with zeros
+ for (const auto& ckey : frame_structure_)
+ {
+ const auto& key = string(1,ckey);
+ if (state.count(key) == 0)
+ state.emplace(key, stateZero(key).at(key));
}
- // assert(concat_size == state_size_ && "Problem with the concatenated size: " );
-
- // fill the state value from the state concatenation in the order dictated by frame_structure_
- int idx = 0;
- _state.resize(state_size_);
- for (char sb_name: frame_structure_){
- Eigen::VectorXd sb_state;
- int size_sb; // really bad...
- if (sb_name == 'O'){
- size_sb = dim_ == 3 ? 4 : 1;
- }
- else {
- size_sb = dim_ == 3 ? 3 : 2;
- }
- if (states_to_concat_map.find(sb_name) != states_to_concat_map.end()){
- sb_state = states_to_concat_map[sb_name];
- }
- else {
- // Should be taken from the last state but too messy already
- sb_state.resize(size_sb);
- sb_state.setZero();
- }
+ }
- _state.segment(idx, size_sb) = sb_state;
- idx += size_sb;
+ return state;
+}
+VectorComposite Problem::getState (const TimeStamp& _ts, const StateStructure& _structure) const
+{
+ StateStructure structure = (_structure == "" ? getFrameStructure() : _structure);
+
+ if ( processor_is_motion_list_.empty() ) // Use last estimated frame's state
+ {
+ const auto& last_kf_or_aux = trajectory_ptr_->closestKeyOrAuxFrameToTimeStamp(_ts);
+ if (last_kf_or_aux)
+ {
+ return last_kf_or_aux->getState(structure);
+ }
+ else
+ {
+ return stateZero(structure);
}
}
-}
-Eigen::VectorXd Problem::getState(const TimeStamp& _ts) const
-{
- Eigen::VectorXd state(getFrameStructureSize());
- getState(_ts, state);
+ VectorComposite state;
+ for (const auto& prc : processor_is_motion_list_)
+ {
+ const auto& prc_state = prc->getState(_ts);
+
+ // transfer processor vector blocks to problem state
+ for (const auto& pair_key_vec : prc_state)
+ {
+ const auto& key = pair_key_vec.first;
+
+ if (state.count(key) == 0) // Only write once. This gives priority to processors upfront in the list
+ state.insert(pair_key_vec);
+ }
+ }
return state;
}
-SizeEigen Problem::getFrameStructureSize() const
+SizeEigen Problem::getDim() const
{
- return state_size_;
+ return dim_;
}
-
-void Problem::getFrameStructureSize(SizeEigen& _x_size, SizeEigen& _cov_size) const
+StateStructure Problem::getFrameStructure() const
{
- _x_size = state_size_;
- _cov_size = state_cov_size_;
+ return frame_structure_;
}
-SizeEigen Problem::getDim() const
+void Problem::appendToStructure(const StateStructure& _structure)
{
- return dim_;
+ for (const auto& ckey : _structure)
+ if (frame_structure_.find(ckey) == std::string::npos) // now key not found in problem structure -> append!
+ frame_structure_ += ckey;
}
-std::string Problem::getFrameStructure() const
+
+void Problem::setTreeManager(TreeManagerBasePtr _gm)
{
- return frame_structure_;
+ if (tree_manager_)
+ tree_manager_->setProblem(nullptr);
+ tree_manager_ = _gm;
+ tree_manager_->setProblem(shared_from_this());
+
}
void Problem::addProcessorIsMotion(IsMotionPtr _processor_motion_ptr)
{
processor_is_motion_list_.push_back(_processor_motion_ptr);
+ appendToStructure(_processor_motion_ptr->getStateStructure());
}
-void Problem::clearProcessorIsMotion(IsMotionPtr proc){
+void Problem::clearProcessorIsMotion(IsMotionPtr proc)
+{
auto it = std::find(processor_is_motion_list_.begin(), processor_is_motion_list_.end(), proc);
if (it != processor_is_motion_list_.end()){
processor_is_motion_list_.erase(it);
}
+
+ // rebuild frame structure with remaining motion processors
+ frame_structure_.clear();
+ for (const auto& pm : processor_is_motion_list_)
+ appendToStructure(pm->getStateStructure());
}
-Eigen::VectorXd Problem::zeroState() const
+VectorComposite Problem::stateZero ( const StateStructure& _structure ) const
{
- Eigen::VectorXd state = Eigen::VectorXd::Zero(getFrameStructureSize());
+ StateStructure structure = (_structure == "" ? getFrameStructure() : _structure);
- // Set the quaternion identity for 3d states. Set only the real part to 1:
- if(this->getDim() == 3)
- state(6) = 1.0;
+ VectorComposite state;
+ for (const auto& ckey : structure)
+ {
+ const auto& key = string(1,ckey);
+ VectorXd vec;
+ if (key == "O")
+ {
+ if (dim_ == 2) vec = VectorXd::Zero(1);
+ else if (dim_ == 3) vec = Quaterniond::Identity().coeffs();
+ }
+ else
+ {
+ vec = VectorXd::Zero(dim_);
+ }
+ state.emplace(key, vec);
+ }
return state;
}
@@ -528,6 +588,10 @@ void Problem::keyFrameCallback(FrameBasePtr _keyframe_ptr, ProcessorBasePtr _pro
" microseconds: ", duration.count());
#endif
}
+
+ // notify tree manager
+ if (tree_manager_)
+ tree_manager_->keyFrameCallback(_keyframe_ptr);
}
bool Problem::permitAuxFrame(ProcessorBasePtr _processor_ptr) const
@@ -557,7 +621,7 @@ void Problem::auxFrameCallback(FrameBasePtr _frame_ptr, ProcessorBasePtr _proces
StateBlockPtr Problem::notifyStateBlock(StateBlockPtr _state_ptr, Notification _noti)
{
- std::lock_guard<std::mutex> lock(mut_state_block_notifications_);
+ std::lock_guard<std::mutex> lock(mut_notifications_);
//std::cout << "Problem::addStateBlockPtr " << _state_ptr.get() << std::endl;
// Check if there is already a notification for this state block
@@ -584,7 +648,7 @@ StateBlockPtr Problem::notifyStateBlock(StateBlockPtr _state_ptr, Notification _
bool Problem::getStateBlockNotification(const StateBlockPtr& sb_ptr, Notification& notif) const
{
- std::lock_guard<std::mutex> lock(mut_state_block_notifications_);
+ std::lock_guard<std::mutex> lock(mut_notifications_);
if (state_block_notification_map_.find(sb_ptr) == state_block_notification_map_.end())
return false;
@@ -594,7 +658,7 @@ bool Problem::getStateBlockNotification(const StateBlockPtr& sb_ptr, Notificatio
FactorBasePtr Problem::notifyFactor(FactorBasePtr _factor_ptr, Notification _noti)
{
- std::lock_guard<std::mutex> lock(mut_factor_notifications_);
+ std::lock_guard<std::mutex> lock(mut_notifications_);
//std::cout << "Problem::addFactorPtr " << _factor_ptr->id() << std::endl;
// Check if there is already the same notification for this factor
@@ -609,7 +673,9 @@ FactorBasePtr Problem::notifyFactor(FactorBasePtr _factor_ptr, Notification _not
}
// opposite notification -> cancell out eachother
else
+ {
factor_notification_map_.erase(notification_it);
+ }
}
// Add notification
@@ -621,7 +687,7 @@ FactorBasePtr Problem::notifyFactor(FactorBasePtr _factor_ptr, Notification _not
bool Problem::getFactorNotification(const FactorBasePtr& fac_ptr, Notification& notif) const
{
- std::lock_guard<std::mutex> lock(mut_factor_notifications_);
+ std::lock_guard<std::mutex> lock(mut_notifications_);
if (factor_notification_map_.find(fac_ptr) == factor_notification_map_.end())
return false;
@@ -868,51 +934,141 @@ FrameBasePtr Problem::closestKeyOrAuxFrameToTimeStamp(const TimeStamp& _ts) cons
return trajectory_ptr_->closestKeyOrAuxFrameToTimeStamp(_ts);
}
-FrameBasePtr Problem::setPrior(const Eigen::VectorXd& _prior_state, const Eigen::MatrixXd& _prior_cov, const TimeStamp& _ts, const double _time_tolerance)
+void Problem::setPriorOptions(const std::string& _mode,
+ const double _time_tolerance ,
+ const VectorComposite& _state ,
+ const VectorComposite& _sigma )
{
- if ( ! prior_is_set_ )
+ assert(prior_options_ != nullptr && "prior options have already been applied");
+ assert(prior_options_->mode == "" && "prior options have already been set");
+ assert((_mode == "nothing" || _mode == "initial_guess" || _mode == "fix" || _mode == "factor") && "wrong _mode value, it should be: 'nothing', 'initial_guess', 'fix' or 'factor'");
+
+ // Store options (optionals depending on the mode)
+ WOLF_TRACE("prior mode: ", _mode);
+ prior_options_->mode = _mode;
+
+ if (prior_options_->mode != "nothing")
{
- // Create origin frame
- FrameBasePtr origin_keyframe = emplaceFrame(KEY, _prior_state, _ts);
-
- // create origin capture with the given state as data
- // Capture fix only takes 3d position and Quaternion orientation
- CapturePosePtr init_capture;
- // if (this->getFrameStructure() == "POV" and this->getDim() == 3)
- // init_capture = CaptureBase::emplace<CapturePose>(origin_keyframe, _ts, nullptr, _prior_state.head(7), _prior_cov.topLeftCorner(6,6));
- // else
- // init_capture = CaptureBase::emplace<CapturePose>(origin_keyframe, _ts, nullptr, _prior_state, _prior_cov);
-
- if (this->getDim() == 3)
- init_capture = CaptureBase::emplace<CapturePose>(origin_keyframe, _ts, nullptr, _prior_state.head(7), _prior_cov.topLeftCorner(6,6));
- else
- init_capture = CaptureBase::emplace<CapturePose>(origin_keyframe, _ts, nullptr, _prior_state.head(3), _prior_cov.topLeftCorner(3,3));
+ assert(_time_tolerance > 0 && "time tolerance should be bigger than 0");
+
+ WOLF_TRACE("prior state: ", _state);
+ WOLF_TRACE("prior time tolerance: ", _time_tolerance);
+ prior_options_->state = _state;
+ prior_options_->time_tolerance = _time_tolerance;
+
+ if (prior_options_->mode == "factor")
+ {
+ bool isPositive = true;
+ for(const auto& it: _sigma)
+ isPositive = isPositive and (it.second.array() > Constants::EPS).all();
+
+ assert(isPositive && "sigma is not positive");
+
+ MatrixComposite Q;
+ for (const auto& pair_key_sig : _sigma)
+ {
+ const auto& key = pair_key_sig.first;
+ const auto& sig_blk = pair_key_sig.second;
+
+ const auto& cov_blk = (sig_blk.array() * sig_blk.array()).matrix().asDiagonal();
+ Q.emplace(key,key,cov_blk);
+ }
+ WOLF_TRACE("prior covariance:" , Q);
+ prior_options_->cov = Q;
+ }
+ }
+}
+
+FrameBasePtr Problem::applyPriorOptions(const TimeStamp& _ts)
+{
+ assert(!isPriorSet() && "applyPriorOptions can be called once!");
+
+ FrameBasePtr prior_keyframe(nullptr);
+
+ if (prior_options_->mode != "nothing" and prior_options_->mode != "")
+ {
+ prior_keyframe = emplaceFrame(KEY,
+ _ts,
+ prior_options_->state);
+
+ if (prior_options_->mode == "fix")
+ prior_keyframe->fix();
+ else if (prior_options_->mode == "factor")
+ {
+ // ASSUMPTION: Independent measurements (non-correlated block-diagonal covariance matrix)
+
+ // Emplace a capture
+ auto prior_cap = CaptureBase::emplace<CaptureVoid>(prior_keyframe, _ts, nullptr);
+ // Emplace a feature and a factor for each state block
+ for (const auto& pair_key_sb : prior_keyframe->getStateBlockMap())
+ {
+ const auto& key = pair_key_sb.first;
+ const auto& sb = pair_key_sb.second;
- // emplace feature and factor
- init_capture->emplaceFeatureAndFactor();
+ const auto& state_blk = prior_options_->state.at(key);
+ const auto& covar_blk = prior_options_->cov.at(key,key);
- WOLF_DEBUG("Set prior callback: KF", origin_keyframe->id(), " Callback emitted with ts = ", origin_keyframe->getTimeStamp());
+ assert(sb->getSize() == state_blk.size() && "prior_options state wrong size");
+ assert(sb->getLocalSize() == covar_blk.rows() && "prior_options cov. wrong size");
- // Notify all motion processors about the prior KF
- for (auto sensor : hardware_ptr_->getSensorList())
- for (auto processor : sensor->getProcessorList())
- if (processor->isMotion())
- // Motion processors will set its origin at the KF
- (std::static_pointer_cast<ProcessorMotion>(processor))->setOrigin(origin_keyframe);
+ // feature
+ auto prior_fea = FeatureBase::emplace<FeatureBase>(prior_cap, "Prior " + key, state_blk, covar_blk);
- prior_is_set_ = true;
+ // factor
+ if (sb->hasLocalParametrization())
+ {
+ if (std::dynamic_pointer_cast<StateQuaternion>(sb) != nullptr)
+ auto prior_fac = FactorBase::emplace<FactorQuaternionAbsolute>(prior_fea, prior_fea, sb, nullptr, false);
+ else if (std::dynamic_pointer_cast<StateAngle>(sb) != nullptr)
+ auto prior_fac = FactorBase::emplace<FactorBlockAbsolute>(prior_fea, prior_fea, sb, nullptr, false);
+ else
+ throw std::runtime_error("not implemented...!");
+ }
+ else
+ {
+ auto prior_fac = FactorBase::emplace<FactorBlockAbsolute>(prior_fea, prior_fea, sb, nullptr, false);
+ }
- // Notify all other processors about the origin KF --> they will join it or not depending on their received data
- for (auto sensor : hardware_ptr_->getSensorList())
- for (auto processor : sensor->getProcessorList())
- if ( !processor->isMotion() )
- processor->keyFrameCallback(origin_keyframe, _time_tolerance);
+ }
- return origin_keyframe;
+ }
+ else
+ assert(prior_options_->mode == "initial_guess" && "wrong prior_options->mode");
+
+ // notify all processors
+ keyFrameCallback(prior_keyframe, nullptr, prior_options_->time_tolerance);
}
- else
- throw std::runtime_error("Origin already set!");
+ // remove prior options
+ prior_options_ = nullptr;
+
+ return prior_keyframe;
+}
+
+FrameBasePtr Problem::setPriorFactor(const VectorComposite &_state,
+ const VectorComposite &_sigma,
+ const TimeStamp &_ts,
+ const double &_time_tol)
+{
+ setPriorOptions("factor", _time_tol, _state, _sigma);
+ return applyPriorOptions(_ts);
+}
+
+
+FrameBasePtr Problem::setPriorFix(const VectorComposite &_state,
+ const TimeStamp &_ts,
+ const double &_time_tol)
+{
+ setPriorOptions("fix", _time_tol, _state);
+ return applyPriorOptions(_ts);
+}
+
+FrameBasePtr Problem::setPriorInitialGuess(const VectorComposite &_state,
+ const TimeStamp &_ts,
+ const double &_time_tol)
+{
+ setPriorOptions("initial_guess", _time_tol, _state);
+ return applyPriorOptions(_ts);
}
void Problem::loadMap(const std::string& _filename_dot_yaml)
@@ -925,266 +1081,18 @@ void Problem::saveMap(const std::string& _filename_dot_yaml, const std::string&
getMap()->save(_filename_dot_yaml, _map_name);
}
-void Problem::print(int _depth, std::ostream& _stream, bool _constr_by, bool _metric, bool _state_blocks) const
+void Problem::print(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream) const
{
_stream << std::endl;
_stream << "P: wolf tree status ---------------------" << std::endl;
- _stream << "Hardware" << ((_depth < 1) ? (" -- " + std::to_string(getHardware()->getSensorList().size()) + "S") : "") << std::endl;
- if (_depth >= 1)
- {
- // Sensors =======================================================================================
- for (auto S : getHardware()->getSensorList())
- {
- _stream << " Sen" << S->id() << " " << S->getType() << " \"" << S->getName() << "\"";
- if (_depth < 2)
- _stream << " -- " << S->getProcessorList().size() << "p";
- _stream << std::endl;
- if (_metric && _state_blocks)
- {
- _stream << " sb: ";
- for (auto& _key : S->getStructure())
- {
- auto key = std::string(1,_key);
- auto sb = S->getStateBlock(key);
- _stream << key << "[" << (S->isStateBlockDynamic(key) ? "Dyn" : "Sta") << "," << (sb->isFixed() ? "Fix" : "Est") << "] = ( " << sb->getState().transpose() << " ); ";
- }
- _stream << std::endl;
- }
- else if (_metric)
- {
- _stream << " ( ";
- for (auto& _key : S->getStructure())
- {
- auto key = std::string(1,_key);
- auto sb = S->getStateBlock(key);
- _stream << sb->getState().transpose() << " ";
- }
- _stream << ")" << std::endl;
- }
- else if (_state_blocks)
- {
- _stream << " sb: ";
- for (auto& _key : S->getStructure())
- {
- auto key = std::string(1,_key);
- auto sb = S->getStateBlock(key);
- _stream << key << "[" << (S->isStateBlockDynamic(key) ? "Dyn" : "Sta") << "," << (sb->isFixed() ? "Fix" : "Est") << "]; ";
- }
- _stream << std::endl;
- }
- if (_depth >= 2)
- {
- // Processors =======================================================================================
- for (auto p : S->getProcessorList())
- {
- if (p->isMotion())
- {
- _stream << " PrcM" << p->id() << " " << p->getType() << " \"" << p->getName() << "\"" << std::endl;
- ProcessorMotionPtr pm = std::static_pointer_cast<ProcessorMotion>(p);
- if (pm->getOrigin())
- _stream << " o: Cap" << pm->getOrigin()->id() << " - " << (pm->getOrigin()->getFrame()->isKeyOrAux() ? (pm->getOrigin()->getFrame()->isKey() ? " KFrm" : " AFrm" ) : " Frm")
- << pm->getOrigin()->getFrame()->id() << std::endl;
- if (pm->getLast())
- _stream << " l: Cap" << pm->getLast()->id() << " - " << (pm->getLast()->getFrame()->isKeyOrAux() ? (pm->getLast()->getFrame()->isKey() ? " KFrm" : " AFrm") : " Frm")
- << pm->getLast()->getFrame()->id() << std::endl;
- if (pm->getIncoming())
- _stream << " i: Cap" << pm->getIncoming()->id() << std::endl;
- }
- else
- {
- _stream << " PrcT" << p->id() << " " << p->getType() << " \"" << p->getName() << "\"" << std::endl;
- ProcessorTrackerPtr pt = std::dynamic_pointer_cast<ProcessorTracker>(p);
- if (pt)
- {
- if (pt->getOrigin())
- _stream << " o: Cap" << pt->getOrigin()->id() << " - " << (pt->getOrigin()->getFrame()->isKeyOrAux() ? (pt->getOrigin()->getFrame()->isKey() ? " KFrm" : " AFrm") : " Frm")
- << pt->getOrigin()->getFrame()->id() << std::endl;
- if (pt->getLast())
- _stream << " l: Cap" << pt->getLast()->id() << " - " << (pt->getLast()->getFrame()->isKeyOrAux() ? (pt->getLast()->getFrame()->isKey() ? " KFrm" : " AFrm") : " Frm")
- << pt->getLast()->getFrame()->id() << std::endl;
- if (pt->getIncoming())
- _stream << " i: Cap" << pt->getIncoming()->id() << std::endl;
- }
- }
- } // for p
- }
- } // for S
- }
- _stream << "Trajectory" << ((_depth < 1) ? (" -- " + std::to_string(getTrajectory()->getFrameList().size()) + "F") : "") << std::endl;
- if (_depth >= 1)
- {
- // Frames =======================================================================================
- for (auto F : getTrajectory()->getFrameList())
- {
- _stream << (F->isKeyOrAux() ? (F->isKey() ? " KFrm" : " AFrm") : " Frm") << F->id() << ((_depth < 2) ? " -- " + std::to_string(F->getCaptureList().size()) + "C " : "");
- if (_constr_by)
- {
- _stream << " <-- ";
- for (auto cby : F->getConstrainedByList())
- _stream << "Fac" << cby->id() << " \t";
- }
- _stream << std::endl;
- if (_metric)
- {
- _stream << (F->isFixed() ? " Fix" : " Est") << ", ts=" << std::setprecision(5)
- << F->getTimeStamp();
- _stream << ",\t x = ( " << std::setprecision(2) << F->getState().transpose() << " )";
- _stream << std::endl;
- }
- if (_state_blocks)
- {
- _stream << " sb:";
- for (const auto& sb : F->getStateBlockVec())
- {
- _stream << " " << (sb->isFixed() ? "Fix" : "Est");
- }
- _stream << std::endl;
- }
- if (_depth >= 2)
- {
- // Captures =======================================================================================
- for (auto C : F->getCaptureList())
- {
- _stream << " Cap" << (C->isMotion() ? "M" : "") << C->id() << " " << C->getType();
- if(C->getSensor() != nullptr)
- {
- _stream << " -> Sen" << C->getSensor()->id();
- }
- else
- _stream << " -> Sen-";
- if (C->isMotion())
- {
- auto CM = std::static_pointer_cast<CaptureMotion>(C);
- if (auto OC = CM->getOriginCapture())
- {
- _stream << " -> OCap" << OC->id();
- if (auto OF = OC->getFrame())
- _stream << " ; OFrm" << OF->id();
- }
- }
-
- _stream << ((_depth < 3) ? " -- " + std::to_string(C->getFeatureList().size()) + "f" : "");
- if (_constr_by)
- {
- _stream << " <-- ";
- for (auto cby : C->getConstrainedByList())
- _stream << "Fac" << cby->id() << " \t";
- }
- _stream << std::endl;
-
- if (_state_blocks)
- for (const auto& sb : C->getStateBlockVec())
- {
- if(sb != nullptr)
- {
- _stream << " sb: ";
- _stream << (sb->isFixed() ? "Fix" : "Est");
- if (_metric)
- _stream << std::setprecision(2) << " (" << sb->getState().transpose() << " )";
- _stream << std::endl;
- }
- }
-
- if (C->isMotion() )
- {
- CaptureMotionPtr CM = std::dynamic_pointer_cast<CaptureMotion>(C);
- _stream << " buffer size : " << CM->getBuffer().get().size() << std::endl;
- if ( _metric && ! CM->getBuffer().get().empty())
- {
- _stream << " delta preint : (" << CM->getDeltaPreint().transpose() << ")" << std::endl;
- if (CM->hasCalibration())
- {
- _stream << " calib preint : (" << CM->getCalibrationPreint().transpose() << ")" << std::endl;
- _stream << " jacob preint : (" << CM->getJacobianCalib().row(0) << ")" << std::endl;
- _stream << " calib current: (" << CM->getCalibration().transpose() << ")" << std::endl;
- _stream << " delta correct: (" << CM->getDeltaCorrected(CM->getCalibration()).transpose() << ")" << std::endl;
- }
- }
- }
-
- if (_depth >= 3)
- {
- // Features =======================================================================================
- for (auto f : C->getFeatureList())
- {
- _stream << " Ftr" << f->id() << " trk" << f->trackId() << " " << f->getType() << ((_depth < 4) ? " -- " + std::to_string(f->getFactorList().size()) + "c " : "");
- if (_constr_by)
- {
- _stream << " <--\t";
- for (auto cby : f->getConstrainedByList())
- _stream << "Fac" << cby->id() << " \t";
- }
- _stream << std::endl;
- if (_metric)
- _stream << " m = ( " << std::setprecision(2) << f->getMeasurement().transpose()
- << " )" << std::endl;
- if (_depth >= 4)
- {
- // Factors =======================================================================================
- for (auto c : f->getFactorList())
- {
- _stream << " Fac" << c->id() << " " << c->getType() << " -->";
- if ( c->getFrameOtherList() .empty()
- && c->getCaptureOtherList() .empty()
- && c->getFeatureOtherList() .empty()
- && c->getLandmarkOtherList().empty())
- _stream << " Abs";
-
- for (const auto& Fow : c->getFrameOtherList())
- if (!Fow.expired())
- _stream << " Frm" << Fow.lock()->id();
- for (const auto& Cow : c->getCaptureOtherList())
- if (!Cow.expired())
- _stream << " Cap" << Cow.lock()->id();
- for (const auto& fow : c->getFeatureOtherList())
- if (!fow.expired())
- _stream << " Ftr" << fow.lock()->id();
- for (const auto& Low : c->getLandmarkOtherList())
- if (!Low.expired())
- _stream << " Lmk" << Low.lock()->id();
- _stream << std::endl;
- } // for c
- }
- } // for f
- }
- } // for C
- }
- } // for F
- }
- _stream << "Map" << ((_depth < 1) ? (" -- " + std::to_string(getMap()->getLandmarkList().size()) + "L") : "") << std::endl;
- if (_depth >= 1)
- {
- // Landmarks =======================================================================================
- for (auto L : getMap()->getLandmarkList())
- {
- _stream << " Lmk" << L->id() << " " << L->getType();
- if (_constr_by)
- {
- _stream << "\t<-- ";
- for (auto cby : L->getConstrainedByList())
- _stream << "Fac" << cby->id() << " \t";
- }
- _stream << std::endl;
- if (_metric)
- {
- _stream << (L->isFixed() ? " Fix" : " Est");
- _stream << ",\t x = ( " << std::setprecision(2) << L->getState().transpose() << " )";
- _stream << std::endl;
- }
- if (_state_blocks)
- {
- _stream << " sb:";
- for (const auto& sb : L->getStateBlockVec())
- {
- if (sb != nullptr)
- _stream << (sb->isFixed() ? " Fix" : " Est");
- }
- _stream << std::endl;
- }
- } // for L
- }
+ getHardware()->print(_depth, _constr_by, _metric, _state_blocks, _stream, "");
+
+ getTrajectory()->print(_depth, _constr_by, _metric, _state_blocks, _stream, "");
+
+ getMap()->print(_depth, _constr_by, _metric, _state_blocks, _stream, "");
+
_stream << "-----------------------------------------" << std::endl;
_stream << std::endl;
}
@@ -1195,6 +1103,8 @@ bool Problem::check(bool _verbose, std::ostream& _stream) const
CheckLog log(true, "");
log.explanation_ = "## WOLF::problem inconsistencies list \n ---------------------------------- \n";
+ std::string tabs = "";
+
if (_verbose) _stream << std::endl;
if (_verbose) _stream << "Wolf tree integrity ---------------------" << std::endl;
auto P = shared_from_this();
@@ -1206,624 +1116,19 @@ bool Problem::check(bool _verbose, std::ostream& _stream) const
// HARDWARE branch
// ------------------------
auto H = hardware_ptr_;
- if (_verbose)
- {
- _stream << "Hrw @ " << H.get() << std::endl;
- }
-
- // check pointer to Problem
- std::stringstream inconsistency_explanation;
- inconsistency_explanation << "Hardware problem pointer is " << H->getProblem().get()
- << " but problem pointer is " << P.get() << "\n";
- log.assertTrue((H->getProblem() == P), inconsistency_explanation);
-
-
- // Sensors =======================================================================================
- for (auto S : H->getSensorList())
- {
- if (_verbose)
- {
- _stream << " Sen" << S->id() << " @ " << S.get() << std::endl;
- _stream << " -> Prb @ " << S->getProblem().get() << std::endl;
- _stream << " -> Hrw @ " << S->getHardware().get() << std::endl;
- for (auto pair: S->getStateBlockMap())
- {
- auto sb = pair.second;
- _stream << " " << pair.first << " sb @ " << sb.get();
- if (sb)
- {
- auto lp = sb->getLocalParametrization();
- if (lp)
- _stream << " (lp @ " << lp.get() << ")";
- }
- _stream << std::endl;
- }
- }
-
- // check problem and hardware pointers
- inconsistency_explanation << "Sensor problem pointer is " << S->getProblem().get()
- << " but problem pointer is " << P.get() << "\n";
- log.assertTrue((S->getProblem() == P), inconsistency_explanation);
-
-
- inconsistency_explanation << "Sensor hardware pointer is " << S->getHardware()
- << " but hardware pointer is " << H << "\n";
- log.assertTrue((S->getHardware() == H), inconsistency_explanation);
-
-
- // Processors =======================================================================================
- for (auto p : S->getProcessorList())
- {
- if (_verbose)
- {
- _stream << " Prc" << p->id() << " @ " << p.get() << " -> Sen" << p->getSensor()->id() << std::endl;
- _stream << " -> Prb @ " << p->getProblem().get() << std::endl;
- _stream << " -> Sen" << p->getSensor()->id() << " @ " << p->getSensor().get() << std::endl;
- }
-
- // check problem and sensor pointers
- inconsistency_explanation << "Processor problem pointer is " << p->getProblem().get()
- << " but problem pointer is " << P.get() << "\n";
- log.assertTrue((p->getProblem() == P), inconsistency_explanation);
-
-
- inconsistency_explanation << "Processor sensor pointer is " << p->getSensor()
- << " but sensor pointer is " << P.get() << "\n";
- log.assertTrue((p->getProblem() == P), inconsistency_explanation);
-
- }
- }
+ H->check(log, H, _verbose, _stream, tabs);
// ------------------------
// TRAJECTORY branch
// ------------------------
auto T = trajectory_ptr_;
- if (_verbose)
- {
- _stream << "Trj @ " << T.get() << std::endl;
- }
-
- // check pointer to Problem
- inconsistency_explanation << "Trajectory problem pointer is " << T->getProblem().get()
- << " but problem pointer is" << P.get() << "\n";
- log.assertTrue((T->getProblem() == P), inconsistency_explanation);
-
-
- // Frames =======================================================================================
- for (auto F : T->getFrameList())
- {
- if (_verbose) {
- _stream << (F->isKeyOrAux() ? (F->isKey() ? " KFrm" : " EFrm") : " Frm")
- << F->id() << " @ " << F.get() << std::endl;
- _stream << " -> Prb @ " << F->getProblem().get() << std::endl;
- _stream << " -> Trj @ " << F->getTrajectory().get() << std::endl;
- }
- for (const auto &pair: F->getStateBlockMap()) {
-
- auto sb = pair.second;
-
- // check for valid state block
- inconsistency_explanation << "Frame " << F->id() << " @ "<< F.get()
- << " has State block pointer " << sb.get()
- << " = 0 \n";
- log.assertTrue((sb.get() != 0), inconsistency_explanation);
-
- if (_verbose) {
- _stream << " "<< pair.first << " sb @ " << sb.get();
- if (sb) {
- auto lp = sb->getLocalParametrization();
- if (lp)
- _stream << " (lp @ " << lp.get() << ")";
- }
- _stream << std::endl;
- }
- }
-
- // check problem pointer
- inconsistency_explanation << "Frame problem pointer is " << F->getProblem().get()
- << " but problem pointer is" << P.get() << "\n";
- log.assertTrue((F->getProblem() == P), inconsistency_explanation);
-
- // check trajectory pointer
- inconsistency_explanation << "Frame trajectory pointer is " << F->getTrajectory()
- << " but trajectory pointer is" << T << "\n";
- log.assertTrue((F->getTrajectory() == T), inconsistency_explanation);
-
+ T->check(log, T, _verbose, _stream, tabs);
- // check constrained_by
- for (auto cby : F->getConstrainedByList())
- {
- if (_verbose)
- {
- _stream << " <- c" << cby->id() << " -> ";
- for (const auto& Fow : cby->getFrameOtherList())
- _stream << " F" << Fow.lock()->id() << std::endl;
- }
-
- // check constrained_by pointer to this frame
- inconsistency_explanation << "constrained-by frame " << F->id() << " @ " << F
- << " not found among constrained-by factors\n";
- log.assertTrue((cby->hasFrameOther(F)), inconsistency_explanation);
-
- for (auto sb : cby->getStateBlockPtrVector())
- {
-
- if (_verbose) {
- _stream << " sb @ " << sb.get();
- if (sb) {
- auto lp = sb->getLocalParametrization();
- if (lp)
- _stream << " (lp @ " << lp.get() << ")";
- }
- _stream << std::endl;
- }
- }
- }
-
- // Captures =======================================================================================
- for (auto C : F->getCaptureList())
- {
- if (_verbose)
- {
- _stream << " Cap" << C->id() << " @ " << C.get() << " -> Sen";
- if (C->getSensor()) _stream << C->getSensor()->id();
- else _stream << "-";
- _stream << std::endl;
- _stream << " -> Prb @ " << C->getProblem().get() << std::endl;
- _stream << " -> Frm" << C->getFrame()->id() << " @ " << C->getFrame().get() << std::endl;
- }
- for (auto pair: C->getStateBlockMap())
- {
- auto sb = pair.second;
-
- // check for valid state block
- inconsistency_explanation << "Capture " << C->id() << " @ " << C.get() << " has State block pointer "
- << sb.get() << " = 0 \n";
- log.assertTrue((sb.get() != 0), inconsistency_explanation);
-
- if (_verbose)
- {
- _stream << " " << pair.first << " sb @ " << sb.get();
- if (sb) {
- auto lp = sb->getLocalParametrization();
- if (lp)
- _stream << " (lp @ " << lp.get() << ")";
- }
- _stream << std::endl;
- }
- }
-
- // check problem and frame pointers
- inconsistency_explanation << "Capture problem pointer is " << C->getProblem().get()
- << " but problem pointer is" << P.get() << "\n";
- log.assertTrue((C->getProblem() == P), inconsistency_explanation);
-
-
- inconsistency_explanation << "Capture frame pointer is " << C->getFrame()
- << " but frame pointer is" << F << "\n";
- log.assertTrue((C->getFrame() == F), inconsistency_explanation);
-
-
- // check contrained_by
- for (const auto& cby : C->getConstrainedByList())
- {
- if (_verbose)
- {
- _stream << " <- c" << cby->id() << " -> ";
- for (const auto& Cow : cby->getCaptureOtherList())
- _stream << " C" << Cow.lock()->id();
- _stream << std::endl;
- }
-
- // check constrained_by pointer to this capture
- inconsistency_explanation << "constrained by capture " << C->id() << " @ " << C
- << " not found among constrained-by factors\n";
- log.assertTrue((cby->hasCaptureOther(C)), inconsistency_explanation);
-
- for (auto sb : cby->getStateBlockPtrVector())
- {
- if (_verbose)
- {
- _stream << " sb @ " << sb.get();
- if (sb)
- {
- auto lp = sb->getLocalParametrization();
- if (lp)
- _stream << " (lp @ " << lp.get() << ")";
- }
- _stream << std::endl;
- }
- }
-
- }
-
- // Features =======================================================================================
- for (auto f : C->getFeatureList())
- {
- if (_verbose)
- {
- _stream << " Ftr" << f->id() << " @ " << f.get() << std::endl;
- _stream << " -> Prb @ " << f->getProblem().get() << std::endl;
- _stream << " -> Cap" << f->getCapture()->id() << " @ " << f->getCapture().get()
- << std::endl;
- }
-
- // check problem and capture pointers
- inconsistency_explanation << "Feature frame pointer is " << f->getProblem().get()
- << " but problem pointer is" << P.get() << "\n";
- log.assertTrue((f->getProblem() == P), inconsistency_explanation);
-
-
- inconsistency_explanation << "Feature capture pointer is " << f->getCapture()
- << " but capture pointer is" << C << "\n";
- log.assertTrue((f->getCapture() == C), inconsistency_explanation);
-
-
- // check contrained_by
- for (auto cby : f->getConstrainedByList())
- {
- if (_verbose)
- {
- _stream << " <- c" << cby->id() << " -> ";
- for (const auto& fow : cby->getFeatureOtherList())
- _stream << " f" << fow.lock()->id();
- _stream << std::endl;
- }
-
- // check constrained_by pointer to this feature
- inconsistency_explanation << "constrained by Feature " << f->id() << " @ " << f
- << " not found among constrained-by factors\n";
- log.assertTrue((cby->hasFeatureOther(f)), inconsistency_explanation);
-
- }
-
- // Factors =======================================================================================
- for (auto c : f->getFactorList())
- {
- if (_verbose)
- _stream << " Fac" << c->id() << " @ " << c.get();
-
- if ( c->getFrameOtherList() .empty()
- && c->getCaptureOtherList() .empty()
- && c->getFeatureOtherList() .empty()
- && c->getLandmarkOtherList().empty() ) // case ABSOLUTE:
- {
- if (_verbose)
- _stream << " --> Abs.";
- }
-
- // find constrained_by pointer in constrained frame
- for (const auto& Fow : c->getFrameOtherList())
- {
- if (!Fow.expired())
- {
- const auto& Fo = Fow.lock();
- if (_verbose)
- {
- _stream << " ( --> F" << Fo->id() << " <- ";
- for (auto cby : Fo->getConstrainedByList())
- _stream << " c" << cby->id();
- }
-
- // check constrained_by pointer in constrained frame
- bool found = Fo->isConstrainedBy(c);
- inconsistency_explanation << "The constrained Feature " << Fo->id() << " @ " << Fo
- << " not found among constrained-by factors\n";
- log.assertTrue((found), inconsistency_explanation);
-
- }
- }
- if (_verbose && !c->getFrameOtherList().empty())
- _stream << ")";
-
- // find constrained_by pointer in constrained capture
- for (const auto& Cow : c->getCaptureOtherList())
- {
- if (!Cow.expired())
- {
- const auto& Co = Cow.lock();
-
- if (_verbose)
- {
- _stream << " ( --> C" << Co->id() << " <- ";
- for (auto cby : Co->getConstrainedByList())
- _stream << " c" << cby->id();
- }
-
- // check constrained_by pointer in constrained frame
- bool found = Co->isConstrainedBy(c);
- inconsistency_explanation << "The constrained capture " << Co->id() << " @ " << Co
- << " not found among constrained-by factors\n";
- log.assertTrue((found), inconsistency_explanation);
-
- }
- }
- if (_verbose && !c->getCaptureOtherList().empty())
- _stream << ")";
-
- // find constrained_by pointer in constrained feature
- for (const auto& fow : c->getFeatureOtherList())
- {
- if (!fow.expired())
- {
- const auto& fo = fow.lock();
- if (_verbose)
- {
- _stream << " ( --> f" << fo->id() << " <- ";
- for (auto cby : fo->getConstrainedByList())
- _stream << " c" << cby->id();
- }
-
- // check constrained_by pointer in constrained feature
- bool found = fo->isConstrainedBy(c);
- inconsistency_explanation << "The constrained feature " << fo->id() << " @ " << fo
- << " not found among constrained-by factors\n";
- log.assertTrue((found), inconsistency_explanation);
- }
- }
- if (_verbose && !c->getFeatureOtherList().empty())
- _stream << ")";
-
- // find constrained_by pointer in constrained landmark
- for (const auto& Low : c->getLandmarkOtherList())
- {
- if (Low.expired())
- {
- const auto& Lo = Low.lock();
-
- if (_verbose)
- {
- _stream << " ( --> L" << Lo->id() << " <- ";
- for (auto cby : Lo->getConstrainedByList())
- _stream << " c" << cby->id();
- }
-
- // check constrained_by pointer in constrained landmark
- bool found = Lo->isConstrainedBy(c);
- inconsistency_explanation << "The constrained landmark " << Lo->id() << " @ " << Lo
- << " not found among constrained-by factors\n";
- log.assertTrue((found), inconsistency_explanation);
-
- }
- }
- if (_verbose && !c->getLandmarkOtherList().empty())
- _stream << ")";
-
- if (_verbose)
- _stream << std::endl;
-
- if (_verbose)
- {
- _stream << " -> Prb @ " << c->getProblem().get() << std::endl;
- _stream << " -> Ftr" << c->getFeature()->id() << " @ " << c->getFeature().get() << std::endl;
- }
-
- // check problem and feature pointers
- inconsistency_explanation << "The factor " << c->id() << " @ " << c << " has problem ptr " << c->getProblem().get()
- << " but problem ptr is " << P.get() << "\n";
- log.assertTrue((c->getProblem() == P), inconsistency_explanation);
-
-
- inconsistency_explanation << "The factor " << c->id() << " @ " << c << " has feature ptr " << c->getFeature()
- << " but it should have " << f << "\n";
- log.assertTrue((c->getProblem() == P), inconsistency_explanation);
-
-
- // find state block pointers in all constrained nodes
- SensorBasePtr S = c->getFeature()->getCapture()->getSensor(); // get own sensor to check sb
- for (auto sb : c->getStateBlockPtrVector())
- {
- bool found = false;
- if (_verbose)
- {
- _stream << " sb @ " << sb.get();
- if (sb)
- {
- auto lp = sb->getLocalParametrization();
- if (lp)
- _stream << " (lp @ " << lp.get() << ")";
- }
- }
- bool found_here;
-
- // find in own Frame
- found_here = F->hasStateBlock(sb);
- if (found_here && _verbose) _stream << " F" << F->id();
- found = found || found_here;
-
- // find in own Capture
- found_here = C->hasStateBlock(sb);
- if (found_here && _verbose) _stream << " C" << C->id();
- found = found || found_here;
-
- // Find in other Captures of the own Frame
- if (!found_here)
- for (auto FC : F->getCaptureList())
- {
- found_here = FC->hasStateBlock(sb);
- if (found_here && _verbose) _stream << " F" << F->id() << ".C" << FC->id();
- found = found || found_here;
- }
-
- // find in own Sensor
- if (S)
- {
- found_here = S->hasStateBlock(sb);
- if (found_here && _verbose) _stream << " S" << S->id();
- found = found || found_here;
- }
-
-
- // find in constrained Frame
- for (const auto& Fow : c->getFrameOtherList())
- {
- if (!Fow.expired())
- {
- const auto& Fo = Fow.lock();
- found_here = Fo->hasStateBlock(sb);
- if (found_here && _verbose) _stream << " Fo" << Fo->id();
- found = found || found_here;
-
- // find in feature other's captures
- for (auto FoC : Fo->getCaptureList())
- {
- found_here = FoC->hasStateBlock(sb);
- if (found_here && _verbose) _stream << " Fo" << Fo->id() << ".C" << FoC->id();
- found = found || found_here;
- }
-
- }
- }
-
- // find in constrained Capture
- for (const auto& Cow : c->getCaptureOtherList())
- {
- if (!Cow.expired())
- {
- const auto& Co = Cow.lock();
- found_here = Co->hasStateBlock(sb);
- if (found_here && _verbose) _stream << " Co" << Co->id();
- found = found || found_here;
- }
- }
-
- // find in constrained Feature
- for (const auto& fow : c->getFeatureOtherList())
- {
- if (!fow.expired())
- {
- const auto& fo = fow.lock();
- // find in constrained feature's Frame
- auto foF = fo->getFrame();
- found_here = foF->hasStateBlock(sb);
- if (found_here && _verbose) _stream << " foF" << foF->id();
- found = found || found_here;
-
- // find in constrained feature's Capture
- auto foC = fo->getCapture();
- found_here = foC->hasStateBlock(sb);
- if (found_here && _verbose) _stream << " foC" << foC->id();
- found = found || found_here;
-
- // find in constrained feature's Sensor
- auto foS = fo->getCapture()->getSensor();
- found_here = foS->hasStateBlock(sb);
- if (found_here && _verbose) _stream << " foS" << foS->id();
- found = found || found_here;
- }
- }
-
- // find in constrained landmark
- for (const auto& Low : c->getLandmarkOtherList())
- {
- if (!Low.expired())
- {
- const auto& Lo = Low.lock();
- found_here = Lo->hasStateBlock(sb);
- if (found_here && _verbose) _stream << " Lo" << Lo->id();
- found = found || found_here;
- }
- }
-
- if (_verbose)
- {
- if (found)
- _stream << " found";
- else
- _stream << " NOT FOUND !";
- _stream << std::endl;
- }
-
- // check that the state block has been found somewhere
- inconsistency_explanation << "The stateblock " << sb << " has not been found (is floating!)";
- log.assertTrue((found), inconsistency_explanation);
-
- inconsistency_explanation << "The stateblock " << sb << " of factor " << c->id() << " @ " << c << " is null\n";
- log.assertTrue((sb.get() != nullptr), inconsistency_explanation);
- }
- }
- }
- }
- }
// ------------------------
// MAP branch
// ------------------------
auto M = map_ptr_;
- if (_verbose)
- _stream << "Map @ " << M.get() << std::endl;
+ M->check(log, M, _verbose, _stream, tabs);
- // check pointer to Problem
- inconsistency_explanation << "The map problem ptr is " << M->getProblem().get() << " but problem is " << P.get() << "\n";
- log.assertTrue((M->getProblem() == P), inconsistency_explanation);
-
- // Landmarks =======================================================================================
- for (auto L : M->getLandmarkList())
- {
- if (_verbose)
- {
- _stream << " Lmk" << L->id() << " @ " << L.get() << std::endl;
- _stream << " -> Prb @ " << L->getProblem().get() << std::endl;
- _stream << " -> Map @ " << L->getMap().get() << std::endl;
- for (const auto& pair : L->getStateBlockMap())
- {
- auto sb = pair.second;
- _stream << " " << pair.first << " sb @ " << sb.get();
- if (sb)
- {
- auto lp = sb->getLocalParametrization();
- if (lp)
- _stream << " (lp @ " << lp.get() << ")";
- }
- _stream << std::endl;
- }
- }
-
- // check problem and map pointers
- inconsistency_explanation << "Landmarks' problem ptr is "
- << L->getProblem().get() << " but problem is "
- << P.get() << "\n";
-
- log.assertTrue((L->getProblem() == P), inconsistency_explanation);
-
- inconsistency_explanation << "The Landmarks' map ptr is "
- << L->getMap() << " but map is "
- << M << "\n";
- log.assertTrue((M->getProblem() == P), inconsistency_explanation);
-
- for (auto cby : L->getConstrainedByList())
- {
- if (_verbose)
- {
- _stream << " <- Fac" << cby->id() << " ->";
-
- for (const auto& Low : cby->getLandmarkOtherList())
- {
- if (!Low.expired())
- {
- const auto& Lo = Low.lock();
- _stream << " Lmk" << Lo->id();
- }
- }
- _stream << std::endl;
- }
-
- // check constrained-by factors
- inconsistency_explanation << "constrained-by landmark " << L->id() << " @ " << L
- << " not found among constrained-by factors\n";
- log.assertTrue((cby->hasLandmarkOther(L)), inconsistency_explanation);
-
- for (auto sb : cby->getStateBlockPtrVector()) {
- if (_verbose) {
- _stream << " sb @ " << sb.get();
- if (sb) {
- auto lp = sb->getLocalParametrization();
- if (lp)
- _stream << " (lp @ " << lp.get() << ")";
- }
- _stream << std::endl;
- }
- }
- }
- }
if (_verbose) _stream << "--------------------------- Wolf tree " << (log.is_consistent_ ? " OK" : "Not OK !!") << std::endl;
if (_verbose) _stream << std::endl;
@@ -1836,25 +1141,6 @@ bool Problem::check(int _verbose_level) const
{
return check((_verbose_level > 0), std::cout);
}
-void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks) const
-{
- print(depth, std::cout, constr_by, metric, state_blocks);
-}
-void Problem::print(const std::string& depth, bool constr_by, bool metric, bool state_blocks) const
-{
- if (depth.compare("T") == 0)
- print(0, constr_by, metric, state_blocks);
- else if (depth.compare("F") == 0)
- print(1, constr_by, metric, state_blocks);
- else if (depth.compare("C") == 0)
- print(2, constr_by, metric, state_blocks);
- else if (depth.compare("f") == 0)
- print(3, constr_by, metric, state_blocks);
- else if (depth.compare("c") == 0)
- print(4, constr_by, metric, state_blocks);
- else
- print(0, constr_by, metric, state_blocks);
-}
void Problem::perturb(double amplitude)
{
diff --git a/src/processor/is_motion.cpp b/src/processor/is_motion.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1bfd06a5e9669f76ef48c49a559cade0dc7845f0
--- /dev/null
+++ b/src/processor/is_motion.cpp
@@ -0,0 +1,9 @@
+#include "core/processor/is_motion.h"
+#include "core/problem/problem.h"
+
+using namespace wolf;
+
+void IsMotion::addToProblem(ProblemPtr _prb_ptr, IsMotionPtr _motion_ptr)
+{
+ _prb_ptr->addProcessorIsMotion(_motion_ptr);
+}
diff --git a/src/processor/motion_buffer.cpp b/src/processor/motion_buffer.cpp
index 5d0a595673da23a73efd69d6b1c12d0734f1172b..52383b14cbf0cb7c355f416ef96a20e726bd711b 100644
--- a/src/processor/motion_buffer.cpp
+++ b/src/processor/motion_buffer.cpp
@@ -64,17 +64,17 @@ void Motion::resize(SizeEigen _data_s, SizeEigen _delta_s, SizeEigen _delta_cov_
MotionBuffer::MotionBuffer()
{
- container_.clear();
+ this->clear();
}
const Motion& MotionBuffer::getMotion(const TimeStamp& _ts) const
{
- //assert((container_.front().ts_ <= _ts) && "Query time stamp out of buffer bounds");
- auto previous = std::find_if(container_.rbegin(), container_.rend(), [&](const Motion& m)
+ //assert((this->front().ts_ <= _ts) && "Query time stamp out of buffer bounds");
+ auto previous = std::find_if(this->rbegin(), this->rend(), [&](const Motion& m)
{
return m.ts_ <= _ts;
});
- if (previous == container_.rend())
+ if (previous == this->rend())
// The time stamp is older than the buffer's oldest data.
// We could do something here, and throw an error or something, but by now we'll return the first valid data
previous--;
@@ -84,12 +84,12 @@ const Motion& MotionBuffer::getMotion(const TimeStamp& _ts) const
void MotionBuffer::getMotion(const TimeStamp& _ts, Motion& _motion) const
{
- //assert((container_.front().ts_ <= _ts) && "Query time stamp out of buffer bounds");
- auto previous = std::find_if(container_.rbegin(), container_.rend(), [&](const Motion& m)
+ //assert((this->front().ts_ <= _ts) && "Query time stamp out of buffer bounds");
+ auto previous = std::find_if(this->rbegin(), this->rend(), [&](const Motion& m)
{
return m.ts_ <= _ts;
});
- if (previous == container_.rend())
+ if (previous == this->rend())
// The time stamp is older than the buffer's oldest data.
// We could do something here, but by now we'll return the last valid data
previous--;
@@ -99,25 +99,25 @@ void MotionBuffer::getMotion(const TimeStamp& _ts, Motion& _motion) const
void MotionBuffer::split(const TimeStamp& _ts, MotionBuffer& _buffer_part_before_ts)
{
- assert((container_.front().ts_ <= _ts) && "Error: Query time stamp is smaller than the buffer's first tick");
- assert((container_.back().ts_ >= _ts) && "Error: Query time stamp is greater than the buffer's last tick");
+ assert((this->front().ts_ <= _ts) && "Error: Query time stamp is smaller than the buffer's first tick");
+ assert((this->back().ts_ >= _ts) && "Error: Query time stamp is greater than the buffer's last tick");
- auto previous = std::find_if(container_.rbegin(), container_.rend(), [&](const Motion& m)
+ auto previous = std::find_if(this->rbegin(), this->rend(), [&](const Motion& m)
{
return m.ts_ <= _ts;
});
- if (previous == container_.rend())
+ if (previous == this->rend())
{
// The time stamp is more recent than the buffer's most recent data:
// return an empty buffer as the _oldest_buffer
- _buffer_part_before_ts.get().clear();
+ _buffer_part_before_ts.clear();
}
else
{
// Transfer part of the buffer
- _buffer_part_before_ts.container_.splice(_buffer_part_before_ts.container_.begin(),
- container_,
- container_.begin(),
+ _buffer_part_before_ts.splice(_buffer_part_before_ts.begin(),
+ *this,
+ this->begin(),
(previous--).base());
}
}
@@ -130,15 +130,15 @@ void MotionBuffer::print(bool show_data, bool show_delta, bool show_delta_int, b
if (!show_data && !show_delta && !show_delta_int && !show_jacs)
{
- cout << "Buffer state [" << container_.size() << "] : <";
- for (Motion mot : container_)
+ cout << "Buffer state [" << this->size() << "] : <";
+ for (Motion mot : *this)
cout << " " << mot.ts_;
cout << " >" << endl;
}
else
{
print(0,0,0,0);
- for (Motion mot : container_)
+ for (Motion mot : *this)
{
cout << "-- Motion (" << mot.ts_ << ")" << endl;
if (show_data)
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index f61f861b021e51a08dd92db301aa1bd8de159272..c02228864e1db24fb0b43826736a26c1c6688451 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -7,7 +7,7 @@ namespace wolf {
unsigned int ProcessorBase::processor_id_count_ = 0;
-ProcessorBase::ProcessorBase(const std::string& _type, int _dim, ProcessorParamsBasePtr _params) :
+ProcessorBase::ProcessorBase(const std::string& _type, int _dim, ParamsProcessorBasePtr _params) :
NodeBase("PROCESSOR", _type),
processor_id_(++processor_id_count_),
params_(_params),
@@ -53,6 +53,10 @@ void ProcessorBase::captureCallback(CaptureBasePtr _capture_ptr)
assert(_capture_ptr != nullptr && "captureCallback with a nullptr capture");
WOLF_DEBUG("P", isMotion() ? "M " : "T ", getName(), ": Capture ", _capture_ptr->id(), " callback received with ts = ", _capture_ptr->getTimeStamp());
+ // apply prior in problem if not done (very first capture)
+ if (getProblem() && !getProblem()->isPriorSet())
+ getProblem()->applyPriorOptions(_capture_ptr->getTimeStamp());
+
// asking if capture should be stored
if (storeCapture(_capture_ptr))
buffer_capture_.add(_capture_ptr->getTimeStamp(), _capture_ptr);
@@ -100,6 +104,23 @@ void ProcessorBase::link(SensorBasePtr _sen_ptr)
WOLF_WARN("Linking with a nullptr");
}
}
+
+void ProcessorBase::setProblem(ProblemPtr _problem)
+{
+ std::string str = "Processor works with " + std::to_string(dim_) + "D but problem is " + std::to_string(_problem->getDim()) + "D";
+ assert((dim_ == 0 or dim_ == _problem->getDim()) && str.c_str());
+
+ if (_problem == nullptr or _problem == this->getProblem())
+ return;
+
+ NodeBase::setProblem(_problem);
+
+ // adding processor is motion to the processor is motion vector
+ auto is_motion_ptr = std::dynamic_pointer_cast<IsMotion>(shared_from_this());
+ if (is_motion_ptr)
+ getProblem()->addProcessorIsMotion(is_motion_ptr);
+}
+
/////////////////////////////////////////////////////////////////////////////////////////
void BufferPackKeyFrame::add(const FrameBasePtr& _key_frame, const double& _time_tolerance)
@@ -196,4 +217,48 @@ void BufferPackKeyFrame::print(void) const
std::cout << "]" << std::endl;
}
+void ProcessorBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+{
+ _stream << _tabs << "Prc" << id() << " " << getType() << " \"" << getName() << "\"" << std::endl;
+
+}
+
+void ProcessorBase::print(int _depth, bool _metric, bool _state_blocks, bool _constr_by, std::ostream& _stream, std::string _tabs) const
+{
+ printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
+}
+CheckLog ProcessorBase::localCheck(bool _verbose, ProcessorBasePtr _prc_ptr, std::ostream& _stream, std::string _tabs) const
+{
+ CheckLog log;
+ std::stringstream inconsistency_explanation;
+
+ auto sen_ptr = getSensor();
+ if (_verbose)
+ {
+ _stream << _tabs << "Prc" << id() << " @ " << _prc_ptr.get() << " -> Sen" << sen_ptr->id() << std::endl;
+ _stream << _tabs << " " << "-> Prb @ " << getProblem().get() << std::endl;
+ _stream << _tabs << " " << "-> Sen" << sen_ptr->id() << " @ " << sen_ptr.get() << std::endl;
+ }
+
+ // check problem and sensor pointers
+ inconsistency_explanation << "Processor problem pointer " << getProblem().get()
+ << " is different from Sensor problem pointer " << sen_ptr->getProblem().get() << "\n";
+ log.assertTrue((getProblem() == sen_ptr->getProblem()), inconsistency_explanation);
+
+ inconsistency_explanation << "Prc" << id() << " @ " << _prc_ptr
+ << " ---> Sen" << sen_ptr->id() << " @ " << sen_ptr
+ << " -X-> Prc" << id();
+ auto sen_prc_list = sen_ptr->getProcessorList();
+ auto sen_has_prc = std::find_if(sen_prc_list.begin(), sen_prc_list.end(), [&_prc_ptr](ProcessorBasePtr prc){ return prc == _prc_ptr;});
+ log.assertTrue(sen_has_prc != sen_prc_list.end(), inconsistency_explanation);
+
+ return log;
+}
+bool ProcessorBase::check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
+{
+ auto prc_ptr = std::static_pointer_cast<ProcessorBase>(_node_ptr);
+ auto local_log = localCheck(_verbose, prc_ptr, _stream, _tabs);
+ _log.compose(local_log);
+ return _log.is_consistent_;
+}
} // namespace wolf
diff --git a/src/processor/processor_diff_drive.cpp b/src/processor/processor_diff_drive.cpp
index d80f89ccee5cddff4aab89ec3737c5b518463e7e..43f15c52d8e0eee217a4c824c67bca1e9ae5b9e3 100644
--- a/src/processor/processor_diff_drive.cpp
+++ b/src/processor/processor_diff_drive.cpp
@@ -16,7 +16,7 @@
namespace wolf
{
-ProcessorDiffDrive::ProcessorDiffDrive(const ProcessorParamsDiffDrivePtr _params) :
+ProcessorDiffDrive::ProcessorDiffDrive(const ParamsProcessorDiffDrivePtr _params) :
ProcessorOdom2d(_params),
params_prc_diff_drv_selfcal_(_params),
radians_per_tick_(0.0) // This param needs further initialization. See this->configure(sensor).
@@ -36,8 +36,7 @@ void ProcessorDiffDrive::configure(SensorBasePtr _sensor)
{
assert(_sensor && "Provided sensor is nullptr");
- SensorDiffDriveConstPtr sensor_diff_drive = std::dynamic_pointer_cast<SensorDiffDrive>(_sensor);
- assert(sensor_diff_drive != nullptr && "Sensor is not of type SensorDiffDrive");
+ SensorDiffDriveConstPtr sensor_diff_drive = std::static_pointer_cast<SensorDiffDrive>(_sensor);
radians_per_tick_ = sensor_diff_drive->getRadiansPerTick();
}
@@ -139,7 +138,7 @@ CaptureMotionPtr ProcessorDiffDrive::emplaceCapture(const FrameBasePtr& _frame_o
_data,
_data_cov,
_capture_origin);
- cap_motion->setCalibration (_calib);
+ setCalibration (cap_motion, _calib);
cap_motion->setCalibrationPreint(_calib_preint);
return cap_motion;
@@ -149,10 +148,10 @@ FeatureBasePtr ProcessorDiffDrive::emplaceFeature(CaptureMotionPtr _capture_moti
{
auto key_feature_ptr = FeatureBase::emplace<FeatureMotion>(_capture_motion,
"ProcessorDiffDrive",
- _capture_motion->getBuffer().get().back().delta_integr_,
- _capture_motion->getBuffer().get().back().delta_integr_cov_,
+ _capture_motion->getBuffer().back().delta_integr_,
+ _capture_motion->getBuffer().back().delta_integr_cov_,
_capture_motion->getCalibrationPreint(),
- _capture_motion->getBuffer().get().back().jacobian_calib_);
+ _capture_motion->getBuffer().back().jacobian_calib_);
return key_feature_ptr;
}
@@ -169,14 +168,13 @@ FactorBasePtr ProcessorDiffDrive::emplaceFactor(FeatureBasePtr _feature,
return fac_odom;
}
-
} /* namespace wolf */
-// Register in the ProcessorFactory
-#include "core/processor/processor_factory.h"
+// Register in the FactoryProcessor
+#include "core/processor/factory_processor.h"
namespace wolf {
-WOLF_REGISTER_PROCESSOR("ProcessorDiffDrive", ProcessorDiffDrive);
-WOLF_REGISTER_PROCESSOR_AUTO("ProcessorDiffDrive", ProcessorDiffDrive);
+WOLF_REGISTER_PROCESSOR(ProcessorDiffDrive);
+WOLF_REGISTER_PROCESSOR_AUTO(ProcessorDiffDrive);
} // namespace wolf
diff --git a/src/processor/processor_loopclosure.cpp b/src/processor/processor_loopclosure.cpp
index 3517f9e20d5bb55c40ebde25636411c5be02d9cb..16e037b1c7634c4d0d7509fea51e075fbc8d0066 100644
--- a/src/processor/processor_loopclosure.cpp
+++ b/src/processor/processor_loopclosure.cpp
@@ -11,7 +11,9 @@
namespace wolf
{
-ProcessorLoopClosure::ProcessorLoopClosure(const std::string& _type, int _dim, ProcessorParamsLoopClosurePtr _params_loop_closure):
+ProcessorLoopClosure::ProcessorLoopClosure(const std::string& _type,
+ int _dim,
+ ParamsProcessorLoopClosurePtr _params_loop_closure):
ProcessorBase(_type, _dim, _params_loop_closure),
params_loop_closure_(_params_loop_closure)
{
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index aded1532cc17fc6a0f2e0d48edbbca857a1ab28b..ead3fc9e5d0bcbd56367ef12e1a703e658541478 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -1,3 +1,10 @@
+/**
+ * \file processor_motion.cpp
+ *
+ * Created on: 15/03/2016
+ * \author: jsola
+ */
+
#include "core/processor/processor_motion.h"
@@ -14,10 +21,10 @@ ProcessorMotion::ProcessorMotion(const std::string& _type,
SizeEigen _delta_cov_size,
SizeEigen _data_size,
SizeEigen _calib_size,
- ProcessorParamsMotionPtr _params_motion) :
+ ParamsProcessorMotionPtr _params_motion) :
ProcessorBase(_type, _dim, _params_motion),
params_motion_(_params_motion),
- processing_step_(RUNNING_WITHOUT_PACK),
+ processing_step_(RUNNING_WITHOUT_KF),
x_size_(_state_size),
data_size_(_data_size),
delta_size_(_delta_size),
@@ -56,14 +63,14 @@ void ProcessorMotion::splitBuffer(const wolf::CaptureMotionPtr& _capture_source,
_capture_source->getBuffer().split(_ts_split, _capture_target->getBuffer());
// start with empty motion
- TimeStamp t_zero = _capture_target->getBuffer().get().back().ts_; // last tick of previous buffer is zero tick of current buffer
- _capture_source->getBuffer().get().push_front(motionZero(t_zero));
+ TimeStamp t_zero = _capture_target->getBuffer().back().ts_; // last tick of previous buffer is zero tick of current buffer
+ _capture_source->getBuffer().push_front(motionZero(t_zero));
// Update the existing capture
_capture_source->setOriginCapture(_capture_target);
// Get optimized calibration params from 'origin' keyframe
- VectorXd calib_preint_optim = _capture_source->getOriginCapture()->getCalibration();
+ VectorXd calib_preint_optim = getCalibration(_capture_source->getOriginCapture());
// Write the calib params into the capture before re-integration
_capture_source->setCalibrationPreint(calib_preint_optim);
@@ -83,7 +90,8 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
return;
}
- incoming_ptr_ = std::static_pointer_cast<CaptureMotion>(_incoming_ptr);
+ incoming_ptr_ = std::dynamic_pointer_cast<CaptureMotion>(_incoming_ptr);
+ assert(incoming_ptr_ != nullptr && ("Capture type mismatch. Processor " + getName() + " can only process captures of type CaptureMotion").c_str());
preProcess(); // Derived class operations
@@ -93,11 +101,35 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
switch(processing_step_)
{
- case RUNNING_WITHOUT_PACK :
- case RUNNING_WITH_PACK_ON_ORIGIN :
+ case FIRST_TIME_WITHOUT_KF :
+ {
+ // There is no KF: create own origin
+ setOrigin(getProblem()->stateZero(getStateStructure()), _incoming_ptr->getTimeStamp());
+ break;
+ }
+ case FIRST_TIME_WITH_KF_BEFORE_INCOMING :
+ {
+ // cannot joint to the KF: create own origin
+ setOrigin(getProblem()->getState(getStateStructure()), _incoming_ptr->getTimeStamp());
+ break;
+ }
+ case FIRST_TIME_WITH_KF_ON_INCOMING :
+ {
+ // can joint to the KF
+ setOrigin(pack->key_frame);
+ break;
+ }
+ case FIRST_TIME_WITH_KF_AFTER_INCOMING :
+ {
+ // cannot joint to the KF: create own origin
+ setOrigin(getProblem()->getState(getStateStructure()), _incoming_ptr->getTimeStamp());
+ break;
+ }
+ case RUNNING_WITHOUT_KF :
+ case RUNNING_WITH_KF_ON_ORIGIN :
break;
- case RUNNING_WITH_PACK_BEFORE_ORIGIN :
+ case RUNNING_WITH_KF_BEFORE_ORIGIN :
{
/*
@@ -143,7 +175,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
auto capture_origin = capture_existing->getOriginCapture();
// Get calibration params for preintegration from origin, since it has chances to have optimized values
- VectorXd calib_origin = capture_origin->getCalibration();
+ VectorXd calib_origin = getCalibration(capture_origin);
// emplace a new motion capture to the new keyframe
TimeStamp ts_from_callback = keyframe_from_callback->getTimeStamp();
@@ -169,22 +201,29 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// modify existing feature and factor (if they exist in the existing capture)
if (!capture_existing->getFeatureList().empty())
{
- auto feature_existing = capture_existing->getFeatureList().back(); // there is only one feature!
-
- // Modify existing feature --------
- feature_existing->setMeasurement (capture_existing->getBuffer().get().back().delta_integr_);
- feature_existing->setMeasurementCovariance(capture_existing->getBuffer().get().back().delta_integr_cov_);
-
- // Modify existing factor --------
- // Instead of modifying, we remove one ctr, and create a new one.
- auto fac_to_remove = feature_existing->getFactorList().back(); // there is only one factor!
- auto new_ctr = emplaceFactor(feature_existing, capture_for_keyframe_callback);
- fac_to_remove ->remove(); // remove old factor now (otherwise c->remove() gets propagated to f, C, F, etc.)
+ // setMeasurement is not allowed: feature (and factor) will be removed and a new feature and factor will be emplaced
+ capture_existing->getFeatureList().back()->remove(); // factor is removed automatically
+
+ assert(capture_existing->getFeatureList().empty());// there was only one feature!
+ auto new_feature_existing = emplaceFeature(capture_existing);
+ emplaceFactor(new_feature_existing, capture_for_keyframe_callback);
+
+// auto feature_existing = capture_existing->getFeatureList().back(); // there is only one feature!
+//
+// // Modify existing feature --------
+// feature_existing->setMeasurement (capture_existing->getBuffer().back().delta_integr_);
+// feature_existing->setMeasurementCovariance(capture_existing->getBuffer().back().delta_integr_cov_);
+//
+// // Modify existing factor --------
+// // Instead of modifying, we remove one ctr, and create a new one.
+// auto fac_to_remove = feature_existing->getFactorList().back(); // there is only one factor!
+// auto new_ctr = emplaceFactor(feature_existing, capture_for_keyframe_callback);
+// fac_to_remove ->remove(); // remove old factor now (otherwise c->remove() gets propagated to f, C, F, etc.)
}
break;
}
- case RUNNING_WITH_PACK_AFTER_ORIGIN :
+ case RUNNING_WITH_KF_AFTER_ORIGIN :
{
/*
* Legend:
@@ -228,7 +267,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
auto & capture_origin = origin_ptr_;
// Get calibration params for preintegration from origin, since it has chances to have optimized values
- VectorXd calib_origin = capture_origin->getCalibration();
+ VectorXd calib_origin = getCalibration(capture_origin);
// emplace a new motion capture to the new keyframe
TimeStamp ts_from_callback = keyframe_from_callback->getTimeStamp();
@@ -268,9 +307,10 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
integrateOneStep();
// Update state and time stamps
- last_ptr_->setTimeStamp(getCurrentTimeStamp());
- last_ptr_->getFrame()->setTimeStamp(getCurrentTimeStamp());
- last_ptr_->getFrame()->setState(getProblem()->getState(getCurrentTimeStamp()));
+ const auto& ts = getTimeStamp();
+ last_ptr_->setTimeStamp( ts );
+ last_ptr_->getFrame()->setTimeStamp( ts );
+ last_ptr_->getFrame()->setState( getProblem()->getState( ts ) );
if (permittedKeyFrame() && voteForKeyFrame())
{
@@ -318,19 +358,20 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// create a new frame
auto frame_new = getProblem()->emplaceFrame(NON_ESTIMATED,
- getProblem()->getCurrentState(),
- getCurrentTimeStamp());
+ getTimeStamp(),
+ getStateStructure(),
+ getProblem()->getState());
// create a new capture
auto capture_new = emplaceCapture(frame_new,
getSensor(),
key_frame->getTimeStamp(),
Eigen::VectorXd::Zero(data_size_),
getSensor()->getNoiseCov(),
- last_ptr_->getCalibration(),
- last_ptr_->getCalibration(),
+ getCalibration(last_ptr_),
+ getCalibration(last_ptr_),
last_ptr_);
// reset the new buffer
- capture_new->getBuffer().get().push_back( motionZero(key_frame->getTimeStamp()) ) ;
+ capture_new->getBuffer().push_back( motionZero(key_frame->getTimeStamp()) ) ;
// reset derived things
resetDerived();
@@ -349,51 +390,164 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
postProcess();
}
-// _x needs to have the size of the processor state
-bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXd& _x) const
+VectorComposite ProcessorMotion::getState() const
{
- CaptureMotionPtr capture_motion;
- if (origin_ptr_ && _ts >= origin_ptr_->getTimeStamp())
- // timestamp found in the current processor buffer
- capture_motion = last_ptr_;
- else
- // We need to search in previous keyframes for the capture containing a motion buffer with the queried time stamp
- capture_motion = findCaptureContainingTimeStamp(_ts);
- if (capture_motion) // We found a CaptureMotion whose buffer contains the time stamp
+ if (last_ptr_ == nullptr or last_ptr_->getFrame() == nullptr) // We do not have any info of where to find a valid state
+ // Further checking here for origin_ptr is redundant: if last=null, then origin=null too.
{
- // Get origin state and calibration
- CaptureBasePtr cap_orig = capture_motion->getOriginCapture();
- VectorXd state_0 = cap_orig->getFrame()->getState(state_structure_);
- VectorXd calib = cap_orig->getCalibration();
+ WOLF_ERROR("Processor has no state. Returning an empty VectorComposite with no blocks");
+ return VectorComposite(); // return empty state
+ }
- // Get delta and correct it with new calibration params
- VectorXd calib_preint = capture_motion->getCalibrationPreint();
- Motion motion = capture_motion->getBuffer().getMotion(_ts);
- VectorXd delta_step = motion.jacobian_calib_ * (calib - calib_preint);
- VectorXd delta = capture_motion->correctDelta( motion.delta_integr_, delta_step);
+ // From here on, we do have info to compute a valid state
+
+ // if buffer is empty --> we did not advance from origin!
+ // this may happen when in the very first frame where the capture has no motion info --> empty buffer
+ if (last_ptr_->getBuffer().empty())
+ {
+ return last_ptr_->getFrame()->getState(state_structure_);
+ }
+
+ /* Doing this:
+ *
+ * x_t = x_0 [+] ( D_0t (+) J_D_c * ( c - c_preint ) )
+ *
+ * or, put in code form:
+ *
+ * _state = state_origin [+] (delta_preint (+) Jac_delta_calib * (calib - calib_preint) )
+ *
+ * with
+ * [+] : group composition
+ * (+) : block-wise plus
+ */
+
+ // Get state of origin
+ const auto& x_origin = getOrigin()->getFrame()->getState();
+
+ // Get most rescent motion
+ const auto& motion = last_ptr_->getBuffer().back();
+
+ // Get delta preintegrated up to now
+ const auto& delta_preint = motion.delta_integr_;
+
+ // Get calibration preint -- stored in last capture
+ const auto& calib_preint = last_ptr_->getCalibrationPreint();
+
+ VectorComposite state;
+ if ( hasCalibration() )
+ {
+ // Get current calibration -- from origin capture
+ const auto& calib = getCalibration(origin_ptr_);
- // ensure proper size of the provided reference
- _x.resize( state_0.size() );
+ // get Jacobian of delta wrt calibration
+ const auto& J_delta_calib = motion.jacobian_calib_;
- // Compose on top of origin state using the buffered time stamp, not the query time stamp
- double dt = motion.ts_ - capture_motion->getBuffer().get().front().ts_;
- statePlusDelta(state_0, delta, dt, _x);
+ // compute delta change
+ const auto& delta_step = J_delta_calib * (calib - calib_preint);
+
+ // correct delta // this is (+)
+ const auto& delta_corrected = correctDelta(delta_preint, delta_step);
+
+ // compute current state // this is [+]
+ statePlusDelta(x_origin, delta_corrected, last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp(), state);
}
else
{
- // We could not find any CaptureMotion for the time stamp requested
- WOLF_ERROR("Could not find any Capture for the time stamp requested. ", _ts);
- WOLF_TRACE("Did you forget to call Problem::setPrior() in your application?")
- return false;
+ statePlusDelta(x_origin, delta_preint, last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp(), state);
+ }
+
+ return state;
+}
+
+
+
+// _x needs to have the size of the processor state
+VectorComposite ProcessorMotion::getState(const TimeStamp& _ts) const
+{
+
+ // We need to search for the capture containing a motion buffer with the queried time stamp
+ CaptureMotionPtr capture_motion = findCaptureContainingTimeStamp(_ts);
+
+ if (capture_motion == nullptr) // we do not have any info of where to find a valid state
+ {
+ WOLF_ERROR("Processor has no state. Returning an empty VectorComposite with no blocks");
+ return VectorComposite(); // return empty state
+ }
+
+
+ else // We found a CaptureMotion whose buffer contains the time stamp
+ {
+ // if buffer is empty --> we did not advance from origin!
+ // this may happen when in the very first frame where the capture has no motion info --> empty buffer
+ if (capture_motion->getBuffer().empty())
+ {
+ return capture_motion->getFrame()->getState(state_structure_);
+ }
+
+ /* Doing this:
+ *
+ * x_t = x_0 [+] ( D_0t (+) J_D_c * ( c - c_preint ) )
+ *
+ * or, put in code form:
+ *
+ * _state = state_origin [+] (delta_preint (+) Jac_delta_calib * (calib - calib_preint) )
+ *
+ * with
+ * [+] : group composition
+ * (+) : block-wise plus
+ */
+
+ // Get state of origin
+ CaptureBasePtr cap_orig = capture_motion->getOriginCapture();
+ const auto& x_origin = cap_orig->getFrame()->getState();
+
+ // Get motion at time stamp
+ const auto& motion = capture_motion->getBuffer().getMotion(_ts);
+
+ // Get delta preintegrated up to time stamp
+ const auto& delta_preint = motion.delta_integr_;
+
+ // Get calibration preint -- stored in last capture
+ const auto& calib_preint = capture_motion->getCalibrationPreint();
+
+ VectorComposite state;
+
+ if ( hasCalibration() )
+ {
+ // Get current calibration -- from origin capture
+ const auto& calib = getCalibration(cap_orig);
+
+ // get Jacobian of delta wrt calibration
+ const auto& J_delta_calib = motion.jacobian_calib_;
+
+ // compute delta change
+ const auto& delta_step = J_delta_calib * (calib - calib_preint);
+
+ // correct delta // this is (+)
+ const auto& delta_corrected = correctDelta(delta_preint, delta_step);
+
+ // compute current state // this is [+]
+ statePlusDelta(x_origin, delta_corrected, _ts - cap_orig->getTimeStamp(), state);
+ }
+ else
+ {
+ statePlusDelta(x_origin, delta_preint, _ts - cap_orig->getTimeStamp(), state);
+ }
+
+ // return success
+ return state;
+
}
- return true;
}
-FrameBasePtr ProcessorMotion::setOrigin(const Eigen::VectorXd& _x_origin, const TimeStamp& _ts_origin)
+FrameBasePtr ProcessorMotion::setOrigin(const VectorComposite& _x_origin, const TimeStamp& _ts_origin)
{
- FrameBasePtr key_frame_ptr = getProblem()->emplaceFrame(KEY, _x_origin, _ts_origin);
+ FrameBasePtr key_frame_ptr = getProblem()->emplaceFrame(KEY,
+ _ts_origin,
+ getStateStructure(),
+ _x_origin);
setOrigin(key_frame_ptr);
return key_frame_ptr;
@@ -406,11 +560,13 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
&& "ProcessorMotion::setOrigin: origin frame must be in the trajectory.");
assert(_origin_frame->isKey() && "ProcessorMotion::setOrigin: origin frame must be KEY FRAME.");
+ TimeStamp origin_ts = _origin_frame->getTimeStamp();
+
// -------- ORIGIN ---------
// emplace (empty) origin Capture
origin_ptr_ = emplaceCapture(_origin_frame,
getSensor(),
- _origin_frame->getTimeStamp(),
+ origin_ts,
Eigen::VectorXd::Zero(data_size_),
getSensor()->getNoiseCov(),
getSensor()->getCalibration(),
@@ -419,15 +575,15 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
// ---------- LAST ----------
// Make non-key-frame for last Capture
- TimeStamp origin_ts = _origin_frame->getTimeStamp();
auto new_frame_ptr = getProblem()->emplaceFrame(NON_ESTIMATED,
- _origin_frame->getState(),
- origin_ts);
+ origin_ts,
+ getStateStructure(),
+ _origin_frame->getState());
// emplace (emtpy) last Capture
last_ptr_ = emplaceCapture(new_frame_ptr,
getSensor(),
- _origin_frame->getTimeStamp(),
+ origin_ts,
Eigen::VectorXd::Zero(data_size_),
getSensor()->getNoiseCov(),
getSensor()->getCalibration(),
@@ -435,7 +591,7 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
origin_ptr_);
// clear and reset buffer
- getBuffer().get().push_back(motionZero(_origin_frame->getTimeStamp()));
+ getBuffer().push_back(motionZero(_origin_frame->getTimeStamp()));
// Reset derived things
resetDerived();
@@ -460,7 +616,7 @@ void ProcessorMotion::integrateOneStep()
jacobian_delta_calib_);
// integrate the current delta to pre-integrated measurements, and get Jacobians
- deltaPlusDelta(getBuffer().get().back().delta_integr_,
+ deltaPlusDelta(getBuffer().back().delta_integr_,
delta_,
dt_,
delta_integrated_,
@@ -468,29 +624,29 @@ void ProcessorMotion::integrateOneStep()
jacobian_delta_);
// integrate Jacobian wrt calib
- if ( hasCalibration() )
+ if ( hasCalibration() ) // if no calibration, matrices can have mismatching sizes, and this computation is nevertheless pointless
{
jacobian_calib_.noalias()
- = jacobian_delta_preint_ * getBuffer().get().back().jacobian_calib_
+ = jacobian_delta_preint_ * getBuffer().back().jacobian_calib_
+ jacobian_delta_ * jacobian_delta_calib_;
}
// Integrate covariance
delta_integrated_cov_.noalias()
- = jacobian_delta_preint_ * getBuffer().get().back().delta_integr_cov_ * jacobian_delta_preint_.transpose()
+ = jacobian_delta_preint_ * getBuffer().back().delta_integr_cov_ * jacobian_delta_preint_.transpose()
+ jacobian_delta_ * delta_cov_ * jacobian_delta_.transpose();
// push all into buffer
- getBuffer().get().emplace_back(incoming_ptr_->getTimeStamp(),
- incoming_ptr_->getData(),
- incoming_ptr_->getDataCovariance(),
- delta_,
- delta_cov_,
- delta_integrated_,
- delta_integrated_cov_,
- jacobian_delta_,
- jacobian_delta_preint_,
- jacobian_calib_);
+ getBuffer().emplace_back(incoming_ptr_->getTimeStamp(),
+ incoming_ptr_->getData(),
+ incoming_ptr_->getDataCovariance(),
+ delta_,
+ delta_cov_,
+ delta_integrated_,
+ delta_integrated_cov_,
+ jacobian_delta_,
+ jacobian_delta_preint_,
+ jacobian_calib_);
}
void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr)
@@ -503,15 +659,15 @@ void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr)
jacobian_calib_ .setZero();
// check that first motion in buffer is zero!
- assert(_capture_ptr->getBuffer().get().front().delta_integr_ == delta_integrated_ && "Buffer's first motion is not zero!");
- assert(_capture_ptr->getBuffer().get().front().delta_integr_cov_ == delta_integrated_cov_ && "Buffer's first motion is not zero!");
- assert(_capture_ptr->getBuffer().get().front().jacobian_calib_ == jacobian_calib_ && "Buffer's first motion is not zero!");
+ assert(_capture_ptr->getBuffer().front().delta_integr_ == delta_integrated_ && "Buffer's first motion is not zero!");
+ assert(_capture_ptr->getBuffer().front().delta_integr_cov_ == delta_integrated_cov_ && "Buffer's first motion is not zero!");
+ assert(_capture_ptr->getBuffer().front().jacobian_calib_ == jacobian_calib_ && "Buffer's first motion is not zero!");
// Iterate all the buffer
- auto motion_it = _capture_ptr->getBuffer().get().begin();
+ auto motion_it = _capture_ptr->getBuffer().begin();
auto prev_motion_it = motion_it;
motion_it++;
- while (motion_it != _capture_ptr->getBuffer().get().end())
+ while (motion_it != _capture_ptr->getBuffer().end())
{
// get dt
const double dt = motion_it->ts_ - prev_motion_it->ts_;
@@ -574,7 +730,8 @@ CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp
{
// Rule 1 satisfied! We found a Capture belonging to this processor's Sensor ==> it is a CaptureMotion
capture_motion = std::static_pointer_cast<CaptureMotion>(capture);
- if (_ts < frame->getTimeStamp() and !capture_motion->getBuffer().get().empty() and _ts >= capture_motion->getBuffer().get().front().ts_)
+
+ if (capture_motion->containsTimeStamp(_ts, params_->time_tolerance))
{
// Found time stamp satisfying rule 3 above !! ==> break for loop
break;
@@ -588,60 +745,68 @@ CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp
PackKeyFramePtr ProcessorMotion::computeProcessingStep()
{
- if (!getProblem()->priorIsSet())
+ // Origin not set
+ if (!origin_ptr_)
{
- WOLF_WARN ("||*||");
- WOLF_INFO (" ... It seems you missed something!");
- WOLF_ERROR("ProcessorMotion received data before being initialized.");
- WOLF_INFO ("Did you forget to issue a Problem::setPrior()?");
- throw std::runtime_error("ProcessorMotion received data before being initialized.");
- }
-
- PackKeyFramePtr pack = buffer_pack_kf_.selectFirstPackBefore(last_ptr_, params_motion_->time_tolerance);
-
- // ignore "future" KF to avoid MotionBuffer::split() error
- if (pack && pack->key_frame->getTimeStamp() > last_ptr_->getBuffer().get().back().ts_)
- pack = nullptr;
+ PackKeyFramePtr pack = buffer_pack_kf_.selectFirstPackBefore(incoming_ptr_, params_motion_->time_tolerance);
- if (pack)
- {
- if (buffer_pack_kf_.checkTimeTolerance(pack->key_frame->getTimeStamp(), pack->time_tolerance, origin_ptr_->getTimeStamp(), params_motion_->time_tolerance))
+ if (pack)
{
- WOLF_WARN("||*||");
- WOLF_INFO(" ... It seems you missed something!");
- WOLF_ERROR("Pack's KF and origin's KF have matching time stamps (i.e. below time tolerances)");
- // throw std::runtime_error("Pack's KF and origin's KF have matching time stamps (i.e. below time tolerances)");
- processing_step_ = RUNNING_WITH_PACK_ON_ORIGIN;
+ if (buffer_pack_kf_.checkTimeTolerance(pack->key_frame->getTimeStamp(), pack->time_tolerance, incoming_ptr_->getTimeStamp(), params_motion_->time_tolerance))
+ {
+ WOLF_DEBUG("First time with a KF compatible.")
+ processing_step_ = FIRST_TIME_WITH_KF_ON_INCOMING;
+ }
+ else if (pack->key_frame->getTimeStamp() < incoming_ptr_->getTimeStamp())
+ {
+ WOLF_DEBUG("First time with a KF too old. It seems the prior has been set before receiving the first capture of this processor.")
+ processing_step_ = FIRST_TIME_WITH_KF_BEFORE_INCOMING;
+ }
+ else
+ {
+ WOLF_DEBUG("First time with a KF newer than the first capture. It only can happen if prior mode is 'nothing'")
+ processing_step_ = FIRST_TIME_WITH_KF_AFTER_INCOMING;
+ }
}
- else if (pack->key_frame->getTimeStamp() < origin_ptr_->getTimeStamp())
- processing_step_ = RUNNING_WITH_PACK_BEFORE_ORIGIN;
-
else
- processing_step_ = RUNNING_WITH_PACK_AFTER_ORIGIN;
+ {
+ WOLF_DEBUG("First time with a KF newer than the first capture. It only can happen if prior mode is 'nothing'")
+ processing_step_ = FIRST_TIME_WITHOUT_KF;
+ }
+ return pack;
}
else
- processing_step_ = RUNNING_WITHOUT_PACK;
+ {
+ PackKeyFramePtr pack = buffer_pack_kf_.selectFirstPackBefore(last_ptr_, params_motion_->time_tolerance);
- return pack;
-}
+ // ignore "future" KF to avoid MotionBuffer::split() error
+ if (pack && pack->key_frame->getTimeStamp() > last_ptr_->getBuffer().back().ts_)
+ pack = nullptr;
-void ProcessorMotion::setProblem(ProblemPtr _problem)
-{
- std::string str = "Processor works with " + std::to_string(dim_) + "D but problem is " + std::to_string(_problem->getDim()) + "D";
- assert((dim_ == 0 or dim_ == _problem->getDim()) && str.c_str());
+ if (pack)
+ {
+ if (buffer_pack_kf_.checkTimeTolerance(pack->key_frame->getTimeStamp(), pack->time_tolerance, origin_ptr_->getTimeStamp(), params_motion_->time_tolerance))
- if (_problem == nullptr or _problem == this->getProblem())
- return;
+ processing_step_ = RUNNING_WITH_KF_ON_ORIGIN;
- NodeBase::setProblem(_problem);
+ else if (pack->key_frame->getTimeStamp() < origin_ptr_->getTimeStamp())
- // set the origin
- if (origin_ptr_ == nullptr && this->getProblem()->getLastKeyFrame() != nullptr)
- this->setOrigin(this->getProblem()->getLastKeyFrame());
-
- // adding processor is motion to the processor is motion vector
- getProblem()->addProcessorIsMotion(std::dynamic_pointer_cast<IsMotion>(shared_from_this()));
+ processing_step_ = RUNNING_WITH_KF_BEFORE_ORIGIN;
+
+ else
+
+ processing_step_ = RUNNING_WITH_KF_AFTER_ORIGIN;
+
+ }
+ else
+ processing_step_ = RUNNING_WITHOUT_KF;
+
+ return pack;
+ }
+
+ // not reached
+ return nullptr;
}
bool ProcessorMotion::storeKeyFrame(FrameBasePtr _frame_ptr)
@@ -653,4 +818,32 @@ bool ProcessorMotion::storeCapture(CaptureBasePtr _cap_ptr)
return false;
}
+TimeStamp ProcessorMotion::getTimeStamp ( ) const
+{
+ if (not last_ptr_)
+ {
+ WOLF_ERROR("Processor has no time stamp. Returning a non-valid timestamp equal to 0");
+ return TimeStamp(0);
+ }
+
+ if (getBuffer().empty())
+ return last_ptr_->getTimeStamp();
+
+ return getBuffer().back().ts_;
+}
+
+void ProcessorMotion::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+{
+ _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")
+ << getOrigin()->getFrame()->id() << std::endl;
+ if (getLast())
+ _stream << _tabs << " " << "l: Cap" << getLast()->id() << " - " << (getLast()->getFrame()->isKeyOrAux() ? (getLast()->getFrame()->isKey() ? " KFrm" : " AFrm") : " Frm")
+ << getLast()->getFrame()->id() << std::endl;
+ if (getIncoming())
+ _stream << _tabs << " " << "i: Cap" << getIncoming()->id() << std::endl;
+
}
+
+} // namespace wolf
diff --git a/src/processor/processor_odom_2d.cpp b/src/processor/processor_odom_2d.cpp
index 929c1ed7ceb825fdaa265a1af592fa8125832789..284090e238f3c75327424cb76f3946e277e23bdf 100644
--- a/src/processor/processor_odom_2d.cpp
+++ b/src/processor/processor_odom_2d.cpp
@@ -1,11 +1,12 @@
#include "core/processor/processor_odom_2d.h"
#include "core/sensor/sensor_odom_2d.h"
#include "core/math/covariance.h"
+#include "core/state_block/state_composite.h"
namespace wolf
{
-ProcessorOdom2d::ProcessorOdom2d(ProcessorParamsOdom2dPtr _params) :
+ProcessorOdom2d::ProcessorOdom2d(ParamsProcessorOdom2dPtr _params) :
ProcessorMotion("ProcessorOdom2d", "PO", 2, 3, 3, 3, 2, 0, _params),
params_odom_2d_(_params)
{
@@ -16,11 +17,6 @@ ProcessorOdom2d::~ProcessorOdom2d()
{
}
-void ProcessorOdom2d::configure(SensorBasePtr _sensor)
-{
- auto sensor_ = std::dynamic_pointer_cast<SensorOdom2d>(_sensor);
- assert(sensor_ != nullptr && "Sensor is not of type SensorOdom2d");
-}
void ProcessorOdom2d::computeCurrentDelta(const Eigen::VectorXd& _data, const Eigen::MatrixXd& _data_cov,
const Eigen::VectorXd& _calib, const double _dt, Eigen::VectorXd& _delta,
Eigen::MatrixXd& _delta_cov, Eigen::MatrixXd& _jacobian_calib) const
@@ -89,38 +85,39 @@ void ProcessorOdom2d::deltaPlusDelta(const Eigen::VectorXd& _delta1, const Eigen
_jacobian2.topLeftCorner<2, 2>() = Eigen::Rotation2Dd(_delta1(2)).matrix();
}
-void ProcessorOdom2d::statePlusDelta(const Eigen::VectorXd& _x, const Eigen::VectorXd& _delta, const double _Dt,
- Eigen::VectorXd& _x_plus_delta) const
+void ProcessorOdom2d::statePlusDelta(const VectorComposite& _x,
+ const Eigen::VectorXd& _delta,
+ const double _Dt,
+ VectorComposite& _x_plus_delta) const
{
- assert(_x.size() == x_size_ && "Wrong _x vector size");
- assert(_x_plus_delta.size() == x_size_ && "Wrong _x_plus_delta vector size");
+ assert(_delta.size() == delta_size_ && "Wrong _x_plus_delta vector size");
// This is just a frame composition in 2d
- _x_plus_delta.head<2>() = _x.head<2>() + Eigen::Rotation2Dd(_x(2)).matrix() * _delta.head<2>();
- _x_plus_delta(2) = pi2pi(_x(2) + _delta(2));
+ _x_plus_delta["P"] = _x.at("P") + Eigen::Rotation2Dd(_x.at("O")(0)).matrix() * _delta.head<2>();
+ _x_plus_delta["O"] = Vector1d(pi2pi(_x.at("O")(0) + _delta(2)));
}
bool ProcessorOdom2d::voteForKeyFrame() const
{
// Distance criterion
- if (getBuffer().get().back().delta_integr_.head<2>().norm() > params_odom_2d_->dist_traveled)
+ if (getBuffer().back().delta_integr_.head<2>().norm() > params_odom_2d_->dist_traveled)
{
WOLF_DEBUG("PM", id(), " ", getType(), " votes per distance");
return true;
}
- if (getBuffer().get().back().delta_integr_.tail<1>().norm() > params_odom_2d_->angle_turned)
+ if (getBuffer().back().delta_integr_.tail<1>().norm() > params_odom_2d_->angle_turned)
{
WOLF_DEBUG("PM", id(), " ", getType(), " votes per angle");
return true;
}
// Uncertainty criterion
- if (getBuffer().get().back().delta_integr_cov_.determinant() > params_odom_2d_->cov_det)
+ if (getBuffer().back().delta_integr_cov_.determinant() > params_odom_2d_->cov_det)
{
WOLF_DEBUG("PM", id(), " ", getType(), " votes per state covariance");
return true;
}
// Time criterion
- if (getBuffer().get().back().ts_.get() - origin_ptr_->getFrame()->getTimeStamp().get() > params_odom_2d_->max_time_span)
+ if (getBuffer().back().ts_.get() - origin_ptr_->getFrame()->getTimeStamp().get() > params_odom_2d_->max_time_span)
{
WOLF_DEBUG("PM", id(), " ", getType(), " votes per time");
return true;
@@ -138,7 +135,7 @@ CaptureMotionPtr ProcessorOdom2d::emplaceCapture(const FrameBasePtr& _frame_own,
const CaptureBasePtr& _capture_origin_ptr)
{
auto cap_motion = CaptureBase::emplace<CaptureOdom2d>(_frame_own, _ts, _sensor, _data, _data_cov, _capture_origin_ptr);
- cap_motion->setCalibration(_calib);
+ setCalibration(cap_motion, _calib);
cap_motion->setCalibrationPreint(_calib_preint);
return cap_motion;
@@ -156,12 +153,12 @@ FactorBasePtr ProcessorOdom2d::emplaceFactor(FeatureBasePtr _feature, CaptureBas
FeatureBasePtr ProcessorOdom2d::emplaceFeature(CaptureMotionPtr _capture_motion)
{
- Eigen::MatrixXd covariance = _capture_motion->getBuffer().get().back().delta_integr_cov_;
+ Eigen::MatrixXd covariance = _capture_motion->getBuffer().back().delta_integr_cov_;
makePosDef(covariance);
FeatureBasePtr key_feature_ptr = FeatureBase::emplace<FeatureBase>(_capture_motion,
"ProcessorOdom2d",
- _capture_motion->getBuffer().get().back().delta_integr_,
+ _capture_motion->getBuffer().back().delta_integr_,
covariance);
return key_feature_ptr;
}
@@ -170,9 +167,9 @@ FeatureBasePtr ProcessorOdom2d::emplaceFeature(CaptureMotionPtr _capture_motion)
} /* namespace wolf */
-// Register in the ProcessorFactory
-#include "core/processor/processor_factory.h"
+// Register in the FactoryProcessor
+#include "core/processor/factory_processor.h"
namespace wolf {
-WOLF_REGISTER_PROCESSOR("ProcessorOdom2d", ProcessorOdom2d);
-WOLF_REGISTER_PROCESSOR_AUTO("ProcessorOdom2d", ProcessorOdom2d);
+WOLF_REGISTER_PROCESSOR(ProcessorOdom2d);
+WOLF_REGISTER_PROCESSOR_AUTO(ProcessorOdom2d);
} // namespace wolf
diff --git a/src/processor/processor_odom_3d.cpp b/src/processor/processor_odom_3d.cpp
index 3e1268eb1d705b3a335d9c8d9389e0c9449ebba1..5b01e98b3ad8fbbe6f31bedc4f25b1a9cfe76747 100644
--- a/src/processor/processor_odom_3d.cpp
+++ b/src/processor/processor_odom_3d.cpp
@@ -1,8 +1,10 @@
#include "core/processor/processor_odom_3d.h"
+#include "core/math/SE3.h"
+
namespace wolf
{
-ProcessorOdom3d::ProcessorOdom3d(ProcessorParamsOdom3dPtr _params) :
+ProcessorOdom3d::ProcessorOdom3d(ParamsProcessorOdom3dPtr _params) :
ProcessorMotion("ProcessorOdom3d", "PO", 3, 7, 7, 6, 6, 0, _params),
params_odom_3d_ (_params),
k_disp_to_disp_ (0),
@@ -21,8 +23,8 @@ ProcessorOdom3d::~ProcessorOdom3d()
void ProcessorOdom3d::configure(SensorBasePtr _sensor)
{
assert (_sensor && "Trying to configure processor with a sensor but provided sensor is nullptr.");
- auto sen_ptr = std::dynamic_pointer_cast<SensorOdom3d>(_sensor);
- assert(sen_ptr != nullptr && "Sensor is not of type SensorOdom3d");
+
+ SensorOdom3dPtr sen_ptr = std::static_pointer_cast<SensorOdom3d>(_sensor);
// we steal the parameters from the provided odom3d sensor.
k_disp_to_disp_ = sen_ptr->getDispVarToDispNoiseFactor();
@@ -41,22 +43,17 @@ void ProcessorOdom3d::computeCurrentDelta(const Eigen::VectorXd& _data,
Eigen::MatrixXd& _jacobian_calib) const
{
assert((_data.size() == 6 || _data.size() == 7) && "Wrong data size. Must be 6 or 7 for 3d.");
- double disp, rot; // displacement and rotation of this motion step
if (_data.size() == (long int)6)
{
// rotation in vector form
_delta.head<3>() = _data.head<3>();
Eigen::Map<Eigen::Quaterniond> q(_delta.data() + 3);
q = v2q(_data.tail<3>());
- disp = _data.head<3>().norm();
- rot = _data.tail<3>().norm();
}
else
{
// rotation in quaternion form
_delta = _data;
- disp = _data.head<3>().norm();
- rot = 2.0 * acos(_data(6)); // '6' is the real part of the quaternion
}
/* Jacobians of d = data2delta(data, dt)
* with: d = [Dp Dq]
@@ -72,13 +69,7 @@ void ProcessorOdom3d::computeCurrentDelta(const Eigen::VectorXd& _data,
*
* so, J = I, and delta_cov = _data_cov
*/
- // We discard _data_cov and create a new one from the measured motion
- double disp_var = min_disp_var_ + k_disp_to_disp_ * disp;
- double rot_var = min_rot_var_ + k_disp_to_rot_ * disp + k_rot_to_rot_ * rot;
- Eigen::Matrix6d data_cov(Eigen::Matrix6d::Identity());
- data_cov(0, 0) = data_cov(1, 1) = data_cov(2, 2) = disp_var;
- data_cov(3, 3) = data_cov(4, 4) = data_cov(5, 5) = rot_var;
- _delta_cov = data_cov;
+ _delta_cov = _data_cov;
}
void ProcessorOdom3d::deltaPlusDelta(const Eigen::VectorXd& _delta1, const Eigen::VectorXd& _delta2, const double _Dt2,
@@ -149,43 +140,40 @@ void ProcessorOdom3d::deltaPlusDelta(const Eigen::VectorXd& _delta1, const Eigen
dq_out = dq1 * dq2;
}
-void ProcessorOdom3d::statePlusDelta(const Eigen::VectorXd& _x, const Eigen::VectorXd& _delta, const double _Dt,
- Eigen::VectorXd& _x_plus_delta) const
-{
- assert(_x.size() >= x_size_ && "Wrong _x vector size"); //we need a state vector which size is at least x_size_
+void ProcessorOdom3d::statePlusDelta(const VectorComposite& _x,
+ const Eigen::VectorXd& _delta,
+ const double _Dt,
+ VectorComposite& _x_plus_delta) const
+{
assert(_delta.size() == delta_size_ && "Wrong _delta vector size");
- assert(_x_plus_delta.size() >= x_size_ && "Wrong _x_plus_delta vector size");
-
- Eigen::Map<const Eigen::Vector3d> p (_x.data() );
- Eigen::Map<const Eigen::Quaterniond> q (_x.data() + 3 );
- Eigen::Map<const Eigen::Vector3d> dp (_delta.data() );
- Eigen::Map<const Eigen::Quaterniond> dq (_delta.data() + 3 );
- Eigen::Map<Eigen::Vector3d> p_out (_x_plus_delta.data() );
- Eigen::Map<Eigen::Quaterniond> q_out (_x_plus_delta.data() + 3 );
+ Eigen::Map<const Eigen::Vector3d> p (_x.at("P").data() );
+ Eigen::Map<const Eigen::Quaterniond> q (_x.at("O").data() );
+ Eigen::Map<const Eigen::Vector3d> dp (_delta.data() );
+ Eigen::Map<const Eigen::Quaterniond> dq (_delta.data() + 3 );
- p_out = p + q * dp;
- q_out = q * dq;
+ _x_plus_delta["P"] = p + q * dp;
+ _x_plus_delta["O"] = (q * dq).coeffs();
}
bool ProcessorOdom3d::voteForKeyFrame() const
{
- //WOLF_DEBUG( "Time span : " , getBuffer().get().back().ts_ - getBuffer().get().front().ts_ );
- //WOLF_DEBUG( " last ts : ", getBuffer().get().back().ts_);
- //WOLF_DEBUG( " first ts : ", getBuffer().get().front().ts_);
- //WOLF_DEBUG( "BufferLength: " , getBuffer().get().size() );
+ //WOLF_DEBUG( "Time span : " , getBuffer().back().ts_ - getBuffer().front().ts_ );
+ //WOLF_DEBUG( " last ts : ", getBuffer().back().ts_);
+ //WOLF_DEBUG( " first ts : ", getBuffer().front().ts_);
+ //WOLF_DEBUG( "BufferLength: " , getBuffer().size() );
//WOLF_DEBUG( "DistTraveled: " , delta_integrated_.head(3).norm() );
//WOLF_DEBUG( "AngleTurned : " , 2.0 * acos(delta_integrated_(6)) );
// time span
- if (getBuffer().get().back().ts_ - getBuffer().get().front().ts_ > params_odom_3d_->max_time_span)
+ if (getBuffer().back().ts_ - getBuffer().front().ts_ > params_odom_3d_->max_time_span)
{
WOLF_DEBUG( "PM: vote: time span" );
return true;
}
// buffer length
- if (getBuffer().get().size() > params_odom_3d_->max_buff_length)
+ if (getBuffer().size() > params_odom_3d_->max_buff_length)
{
WOLF_DEBUG( "PM: vote: buffer size" );
return true;
@@ -218,7 +206,7 @@ CaptureMotionPtr ProcessorOdom3d::emplaceCapture(const FrameBasePtr& _frame_own,
const CaptureBasePtr& _capture_origin)
{
auto cap_motion = CaptureBase::emplace<CaptureOdom3d>(_frame_own, _ts, _sensor, _data, _data_cov, _capture_origin);
- cap_motion->setCalibration(_calib);
+ setCalibration(cap_motion, _calib);
cap_motion->setCalibrationPreint(_calib_preint);
return cap_motion;
@@ -228,11 +216,19 @@ FeatureBasePtr ProcessorOdom3d::emplaceFeature(CaptureMotionPtr _capture_motion)
{
FeatureBasePtr key_feature_ptr = FeatureBase::emplace<FeatureBase>(_capture_motion,
"ProcessorOdom3d",
- _capture_motion->getBuffer().get().back().delta_integr_,
- _capture_motion->getBuffer().get().back().delta_integr_cov_);
+ _capture_motion->getBuffer().back().delta_integr_,
+ _capture_motion->getBuffer().back().delta_integr_cov_);
return key_feature_ptr;
}
+Eigen::VectorXd ProcessorOdom3d::correctDelta (const Eigen::VectorXd& delta_preint,
+ const Eigen::VectorXd& delta_step) const
+{
+ VectorXd delta_corrected(7);
+ SE3::plus(delta_preint, delta_step, delta_corrected);
+ return delta_corrected;
+}
+
FactorBasePtr ProcessorOdom3d::emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin)
{
auto fac_odom = FactorBase::emplace<FactorOdom3d>(_feature_motion,
@@ -243,12 +239,21 @@ FactorBasePtr ProcessorOdom3d::emplaceFactor(FeatureBasePtr _feature_motion, Cap
return fac_odom;
}
+VectorXd ProcessorOdom3d::getCalibration (const CaptureBasePtr _capture) const
+{
+ return VectorXd::Zero(0);
+}
+void ProcessorOdom3d::setCalibration (const CaptureBasePtr _capture, const VectorXd& _calibration)
+{
}
-// Register in the SensorFactory
-#include "core/processor/processor_factory.h"
+} // namespace wolf
+
+// Register in the FactorySensor
+#include "core/processor/factory_processor.h"
namespace wolf {
-WOLF_REGISTER_PROCESSOR("ProcessorOdom3d", ProcessorOdom3d);
-WOLF_REGISTER_PROCESSOR_AUTO("ProcessorOdom3d", ProcessorOdom3d);
+WOLF_REGISTER_PROCESSOR(ProcessorOdom3d);
+WOLF_REGISTER_PROCESSOR_AUTO(ProcessorOdom3d);
} // namespace wolf
+
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index 56d5ae4ae45ed40962274292328076087da7fb9f..7b5d5dec09eadb5aa13b84d9340c760adba3f7c4 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -15,14 +15,16 @@ namespace wolf
{
ProcessorTracker::ProcessorTracker(const std::string& _type,
+ const StateStructure& _structure,
int _dim,
- ProcessorParamsTrackerPtr _params_tracker) :
+ ParamsProcessorTrackerPtr _params_tracker) :
ProcessorBase(_type, _dim, _params_tracker),
params_tracker_(_params_tracker),
processing_step_(FIRST_TIME_WITHOUT_PACK),
origin_ptr_(nullptr),
last_ptr_(nullptr),
- incoming_ptr_(nullptr)
+ incoming_ptr_(nullptr),
+ state_structure_(_structure)
{
//
}
@@ -78,7 +80,10 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
{
WOLF_DEBUG( "PT ", getName(), " FIRST_TIME_WITHOUT_PACK" );
- FrameBasePtr kfrm = getProblem()->emplaceFrame(KEY, incoming_ptr_->getTimeStamp());
+ FrameBasePtr kfrm = getProblem()->emplaceFrame(KEY,
+ incoming_ptr_->getTimeStamp(),
+ getStateStructure(),
+ getProblem()->getState(getStateStructure()));
incoming_ptr_->link(kfrm);
// Process info
@@ -102,12 +107,14 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
// No-break case only for debug. Next case will be executed too.
PackKeyFramePtr pack = buffer_pack_kf_.selectPack( incoming_ptr_, params_tracker_->time_tolerance);
WOLF_DEBUG( "PT ", getName(), " SECOND_TIME_WITH_PACK: KF" , pack->key_frame->id() , " callback unpacked with ts= " , pack->key_frame->getTimeStamp() );
- } // @suppress("No break at end of case")
+ }
+ // Fall through
case SECOND_TIME_WITHOUT_PACK :
{
WOLF_DEBUG( "PT ", getName(), " SECOND_TIME_WITHOUT_PACK" );
- FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, incoming_ptr_->getTimeStamp());
+ FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED,
+ incoming_ptr_->getTimeStamp());
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.
@@ -142,7 +149,8 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
last_old_frame->remove();
// Create new frame
- FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, incoming_ptr_->getTimeStamp());
+ FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED,
+ incoming_ptr_->getTimeStamp());
incoming_ptr_->link(frm);
// Detect new Features, initialize Landmarks, create Factors, ...
@@ -170,7 +178,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
// process
processNew(params_tracker_->max_new_features);
- //TODO abort KF if last_ptr_->getFeatureList().size() < params_tracker_->min_features_for_keyframe
+ //TODO abort KF if known_features_last_.size() < params_tracker_->min_features_for_keyframe
// We create a KF
// set KF on last
@@ -191,7 +199,9 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
resetDerived();
// make F; append incoming to new F
- FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, last_ptr_->getFrame()->getState(), incoming_ptr_->getTimeStamp());
+ FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED,
+ incoming_ptr_->getTimeStamp(),
+ last_ptr_->getFrame()->getStateVector());
incoming_ptr_->link(frm);
origin_ptr_ = last_ptr_;
@@ -238,7 +248,9 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
advanceDerived();
// Replace last frame for a new one in incoming
- FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, last_ptr_->getFrame()->getState(), incoming_ptr_->getTimeStamp());
+ FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED,
+ incoming_ptr_->getTimeStamp(),
+ last_ptr_->getFrame()->getStateVector());
incoming_ptr_->link(frm);
last_ptr_->getFrame()->remove(); // implicitly calling last_ptr_->remove();
@@ -297,7 +309,6 @@ void ProcessorTracker::computeProcessingStep()
WOLF_INFO("Pack's KF and last's KF have matching time stamps (i.e. below time tolerances)");
WOLF_INFO("Check the following for correctness:");
WOLF_INFO(" - You have all processors installed before starting receiving any data");
- WOLF_INFO(" - You issued a problem->setPrior() after all processors are installed ---> ", (getProblem()->priorIsSet() ? "OK" : "NOK"));
WOLF_INFO(" - You have configured all your processors with compatible time tolerances");
WOLF_ERROR("Pack's KF and last's KF have matching time stamps (i.e. below time tolerances).");
}
@@ -322,5 +333,17 @@ bool ProcessorTracker::storeCapture(CaptureBasePtr _cap_ptr)
return false;
}
+void ProcessorTracker::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+{
+ _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")
+ << getOrigin()->getFrame()->id() << std::endl;
+ if (getLast())
+ _stream << _tabs << " " << "l: Cap" << getLast()->id() << " - " << (getLast()->getFrame()->isKeyOrAux() ? (getLast()->getFrame()->isKey() ? " KFrm" : " AFrm") : " Frm")
+ << getLast()->getFrame()->id() << std::endl;
+ if (getIncoming())
+ _stream << _tabs << " " << "i: Cap" << getIncoming()->id() << std::endl;
+}
} // namespace wolf
diff --git a/src/processor/processor_tracker_feature.cpp b/src/processor/processor_tracker_feature.cpp
index 211a1aba12b12ecfb10afea00041cf82e07493be..3804962f62fcbfba13b56e7c64866be7497eeaa8 100644
--- a/src/processor/processor_tracker_feature.cpp
+++ b/src/processor/processor_tracker_feature.cpp
@@ -11,9 +11,10 @@ namespace wolf
{
ProcessorTrackerFeature::ProcessorTrackerFeature(const std::string& _type,
+ const StateStructure& _structure,
int _dim,
- ProcessorParamsTrackerFeaturePtr _params_tracker_feature) :
- ProcessorTracker(_type, _dim, _params_tracker_feature),
+ ParamsProcessorTrackerFeaturePtr _params_tracker_feature) :
+ ProcessorTracker(_type, _structure, _dim, _params_tracker_feature),
params_tracker_feature_(_params_tracker_feature)
{
}
@@ -47,7 +48,10 @@ unsigned int ProcessorTrackerFeature::processNew(const int& _max_new_features)
// fill the track matrix
for (auto ftr : new_features_last_)
+ {
+ assert(std::find(known_features_last_.begin(), known_features_last_.end(), ftr) == known_features_last_.end() && "detectNewFeatures() provided a new feature that is already in known_features_last_`");
track_matrix_.newTrack(ftr);
+ }
// Track new features from last to incoming. This will append new correspondences to matches_last_incoming
trackFeatures(new_features_last_,
@@ -72,21 +76,27 @@ unsigned int ProcessorTrackerFeature::processNew(const int& _max_new_features)
unsigned int ProcessorTrackerFeature::processKnown()
{
- assert(incoming_ptr_->getFeatureList().size() == 0
- && "In ProcessorTrackerFeature::processKnown(): incoming_ptr_ feature list must be empty before processKnown()");
+ //assert(incoming_ptr_->getFeatureList().size() == 0
+ // && "In ProcessorTrackerFeature::processKnown(): incoming_ptr_ feature list must be empty before processKnown()");
// clear list of known features
matches_last_from_incoming_.clear();
known_features_incoming_.clear();
- if (!last_ptr_ || last_ptr_->getFeatureList().empty())
+ if (!last_ptr_)
+ {
+ WOLF_TRACE("null last, returning...");
+ return 0;
+ }
+
+ if (known_features_last_.empty())
{
WOLF_TRACE("Empty last feature list, returning...");
return 0;
}
// Track features from last_ptr_ to incoming_ptr_
- trackFeatures(last_ptr_->getFeatureList(),
+ trackFeatures(known_features_last_,
incoming_ptr_,
known_features_incoming_,
matches_last_from_incoming_);
@@ -132,6 +142,9 @@ void ProcessorTrackerFeature::advanceDerived()
{
// Reset here the list of correspondences.
matches_last_from_incoming_.clear();
+ known_features_last_ = std::move(known_features_incoming_);
+ //new_features_incoming should be zero!
+ //known_features_last_.splice(new_features_incoming_);
// // remove last from track matrix in case you want to have only KF in the track matrix
// track_matrix_.remove(last_ptr_);
@@ -141,6 +154,8 @@ void ProcessorTrackerFeature::resetDerived()
{
// Reset here the list of correspondences.
matches_last_from_incoming_.clear();
+ known_features_last_ = std::move(known_features_incoming_);
+ known_features_last_.splice(known_features_last_.end(), new_features_incoming_);
// Debug
//for (auto const & pair_trkid_pair : track_matrix_.matches(origin_ptr_, last_ptr_))
diff --git a/src/processor/processor_tracker_landmark.cpp b/src/processor/processor_tracker_landmark.cpp
index 7035dd38de2fcfb177deef2f4ed30d28fcdd92cf..ec16f76dc095de7cf01ebf45400d0eacd7733516 100644
--- a/src/processor/processor_tracker_landmark.cpp
+++ b/src/processor/processor_tracker_landmark.cpp
@@ -14,8 +14,12 @@ namespace wolf
{
ProcessorTrackerLandmark::ProcessorTrackerLandmark(const std::string& _type,
- ProcessorParamsTrackerLandmarkPtr _params_tracker_landmark) :
- ProcessorTracker(_type, 0, _params_tracker_landmark),
+ const StateStructure& _structure,
+ ParamsProcessorTrackerLandmarkPtr _params_tracker_landmark) :
+ ProcessorTracker(_type,
+ _structure,
+ 0,
+ _params_tracker_landmark),
params_tracker_landmark_(_params_tracker_landmark)
{
//
@@ -30,6 +34,8 @@ void ProcessorTrackerLandmark::advanceDerived()
{
matches_landmark_from_last_ = std::move(matches_landmark_from_incoming_);
new_features_last_ = std::move(new_features_incoming_);
+ known_features_last_ = std::move(known_features_incoming_);
+ known_features_last_.splice(known_features_last_.end(), new_features_incoming_);
matches_landmark_from_incoming_.clear();
new_features_incoming_.clear();
@@ -40,6 +46,8 @@ void ProcessorTrackerLandmark::resetDerived()
{
matches_landmark_from_last_ = std::move(matches_landmark_from_incoming_);
new_features_last_ = std::move(new_features_incoming_);
+ known_features_last_ = std::move(known_features_incoming_);
+ known_features_last_.splice(known_features_last_.end(), new_features_incoming_);
matches_landmark_from_incoming_.clear();
new_features_incoming_.clear();
@@ -129,15 +137,17 @@ void ProcessorTrackerLandmark::emplaceNewLandmarks()
void ProcessorTrackerLandmark::establishFactors()
{
- // Loop all features in last_ptr_
- for (auto last_feature : last_ptr_->getFeatureList())
- {
- assert(matches_landmark_from_last_.count(last_feature) == 1);
+ // Loop all features tracked in last_ptr_ (two lists known_features_last_ and new_features_last_)
+ std::list<FeatureBasePtrList> both_lists{known_features_last_, new_features_last_};
+ for (auto feature_list : both_lists)
+ for (auto last_feature : feature_list)
+ {
+ assert(matches_landmark_from_last_.count(last_feature) == 1);
- FactorBasePtr fac_ptr = emplaceFactor(last_feature, matches_landmark_from_last_[last_feature]->landmark_ptr_);
+ FactorBasePtr fac_ptr = emplaceFactor(last_feature, matches_landmark_from_last_[last_feature]->landmark_ptr_);
- assert(fac_ptr->getFeature() != nullptr && "not linked factor returned by emplaceFactor()");
- }
+ assert(fac_ptr->getFeature() != nullptr && "not linked factor returned by emplaceFactor()");
+ }
}
} // namespace wolf
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index 09afa700e79e66b058df609ea004334bd0830e4b..f8c3c76c7e5865d241846516ce9989226d9985cd 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -184,9 +184,9 @@ void SensorBase::addPriorParameter(const std::string& _key, const Eigen::VectorX
// create & add factor absolute
if (is_quaternion)
- FactorBase::emplace<FactorQuaternionAbsolute>(ftr_prior, _sb);
+ FactorBase::emplace<FactorQuaternionAbsolute>(ftr_prior, ftr_prior, _sb, nullptr, false);
else
- FactorBase::emplace<FactorBlockAbsolute>(ftr_prior, _sb, _start_idx, _size);
+ FactorBase::emplace<FactorBlockAbsolute>(ftr_prior, ftr_prior, _sb, _start_idx, _size, nullptr, false);
// store feature in params_prior_map_
params_prior_map_[_key] = ftr_prior;
@@ -467,4 +467,111 @@ void SensorBase::link(HardwareBasePtr _hw_ptr)
}
}
+void SensorBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+{
+ _stream << _tabs << "Sen" << id() << " " << getType() << " \"" << getName() << "\"";
+ if (_depth < 2)
+ _stream << " -- " << getProcessorList().size() << "p";
+ _stream << std::endl;
+
+ if (_metric && _state_blocks)
+ {
+ _stream << _tabs << " " << "sb: ";
+ for (auto& _key : getStructure())
+ {
+ auto key = std::string(1,_key);
+ auto sb = getStateBlock(key);
+ _stream << key << "[" << (isStateBlockDynamic(key) ? "Dyn" : "Sta") << "," << (sb->isFixed() ? "Fix" : "Est") << "] = ( " << sb->getState().transpose() << " ); ";
+ }
+ _stream << std::endl;
+ }
+ else if (_metric)
+ {
+ _stream << _tabs << " " << "( ";
+ for (auto& _key : getStructure())
+ {
+ auto key = std::string(1,_key);
+ auto sb = getStateBlock(key);
+ _stream << sb->getState().transpose() << " ";
+ }
+ _stream << ")" << std::endl;
+ }
+ else if (_state_blocks)
+ {
+ _stream << _tabs << " " << "sb: ";
+ for (auto& _key : getStructure())
+ {
+ auto key = std::string(1,_key);
+ auto sb = getStateBlock(key);
+ _stream << key << "[" << (isStateBlockDynamic(key) ? "Dyn" : "Sta") << "," << (sb->isFixed() ? "Fix" : "Est") << "]; ";
+ }
+ _stream << std::endl;
+ }
+}
+
+void SensorBase::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 >= 2)
+ for (auto p : getProcessorList())
+ p->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
+}
+
+CheckLog SensorBase::localCheck(bool _verbose, SensorBasePtr _sen_ptr, std::ostream& _stream, std::string _tabs) const
+{
+ CheckLog log;
+ if (_verbose)
+ {
+ _stream << _tabs << "Sen" << id() << " @ " << _sen_ptr.get() << std::endl;
+ _stream << _tabs << " " << "-> Prb @ " << getProblem().get() << std::endl;
+ _stream << _tabs << " " << "-> Hrw @ " << getHardware().get() << std::endl;
+ for (auto pair: getStateBlockMap())
+ {
+ auto sb = pair.second;
+ _stream << _tabs << " " << pair.first << " sb @ " << sb.get();
+ if (sb)
+ {
+ auto lp = sb->getLocalParametrization();
+ if (lp)
+ _stream << " (lp @ " << lp.get() << ")";
+ }
+ _stream << std::endl;
+ }
+ }
+
+ std::stringstream inconsistency_explanation;
+ auto hwd_ptr = getHardware();
+ // check problem and hardware pointers
+ inconsistency_explanation << "Sensor problem pointer " << getProblem().get()
+ << " different from Hardware problem pointer " << hwd_ptr->getProblem().get() << "\n";
+ log.assertTrue((getProblem() == hwd_ptr->getProblem()), inconsistency_explanation);
+
+ inconsistency_explanation << "Sen" << id() << " @ " << _sen_ptr
+ << " ---> Hwd" << " @ " << hwd_ptr
+ << " -X-> Sen" << id();
+ auto hwd_sen_list = hwd_ptr->getSensorList();
+ auto hwd_has_sen = std::find_if(hwd_sen_list.begin(), hwd_sen_list.end(), [&_sen_ptr](SensorBasePtr sen){ return sen == _sen_ptr;});
+ log.assertTrue(hwd_has_sen != hwd_sen_list.end(), inconsistency_explanation);
+
+ for(auto prc : getProcessorList())
+ {
+ inconsistency_explanation << "Sen" << id() << " @ " << _sen_ptr
+ << " ---> Prc" << prc->id() << " @ " << prc
+ << " -X-> Sen" << id();
+ log.assertTrue((prc->getSensor() == _sen_ptr), inconsistency_explanation);
+ }
+ return log;
+}
+
+bool SensorBase::check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
+{
+ auto sen_ptr = std::static_pointer_cast<SensorBase>(_node_ptr);
+ auto local_log = localCheck(_verbose, sen_ptr, _stream, _tabs);
+ _log.compose(local_log);
+
+ for (auto p : getProcessorList()) p->check(_log, p, _verbose, _stream, _tabs + " ");
+ return _log.is_consistent_;
+}
+
} // namespace wolf
diff --git a/src/sensor/sensor_diff_drive.cpp b/src/sensor/sensor_diff_drive.cpp
index ae48fcceb261da67fa2b940aa06248b3d5006e48..afdc43e02a8085feb99c4ae0ac1a9a4cdefe2162 100644
--- a/src/sensor/sensor_diff_drive.cpp
+++ b/src/sensor/sensor_diff_drive.cpp
@@ -22,7 +22,7 @@ SensorDiffDrive::SensorDiffDrive(const Eigen::VectorXd& _extrinsics,
radians_per_tick = 2.0*M_PI / params_diff_drive_->ticks_per_wheel_revolution;
getIntrinsic()->setState(Eigen::Vector3d(_intrinsics->radius_left,_intrinsics->radius_right,_intrinsics->wheel_separation));
unfixIntrinsics();
- std::cout << "Prior cov diag " << params_diff_drive_->prior_cov_diag.transpose() << std::endl;
+
if(params_diff_drive_->set_intrinsics_prior)
addPriorIntrinsics(getIntrinsic()->getState(), params_diff_drive_->prior_cov_diag.asDiagonal());
@@ -31,26 +31,22 @@ SensorDiffDrive::SensorDiffDrive(const Eigen::VectorXd& _extrinsics,
double sigma_rev = 2*radians_per_tick;
Eigen::Vector2d noise_sigma; noise_sigma << sigma_rev, sigma_rev;
-// // 2. unmeasured lateral wheel slippage: sigma = 1mm
-// double sigma_slippage = 0.001;
-// Eigen::Vector3d noise_sigma; noise_sigma << sigma_rev, sigma_rev, sigma_slippage;
-
setNoiseStd(noise_sigma);
}
SensorDiffDrive::~SensorDiffDrive()
{
- // TODO Auto-generated destructor stub
+ // Auto-generated destructor stub
}
} /* namespace wolf */
-// Register in the SensorFactory
-#include "core/sensor/sensor_factory.h"
+// Register in the FactorySensor
+#include "core/sensor/factory_sensor.h"
namespace wolf {
-WOLF_REGISTER_SENSOR("SensorDiffDrive", SensorDiffDrive);
-WOLF_REGISTER_SENSOR_AUTO("SensorDiffDrive", SensorDiffDrive);
+WOLF_REGISTER_SENSOR(SensorDiffDrive);
+WOLF_REGISTER_SENSOR_AUTO(SensorDiffDrive);
} // namespace wolf
diff --git a/src/sensor/sensor_odom_2d.cpp b/src/sensor/sensor_odom_2d.cpp
index 57af930f6724980d49209aaf1ece6b6762556adc..ec7a3b54d3f8163f44276bf8120cb482a3a2fdd7 100644
--- a/src/sensor/sensor_odom_2d.cpp
+++ b/src/sensor/sensor_odom_2d.cpp
@@ -36,9 +36,9 @@ double SensorOdom2d::getRotVarToRotNoiseFactor() const
}
-// Register in the SensorFactory
-#include "core/sensor/sensor_factory.h"
+// Register in the FactorySensor
+#include "core/sensor/factory_sensor.h"
namespace wolf {
-WOLF_REGISTER_SENSOR("SensorOdom2d", SensorOdom2d);
-WOLF_REGISTER_SENSOR_AUTO("SensorOdom2d", SensorOdom2d);
+WOLF_REGISTER_SENSOR(SensorOdom2d);
+WOLF_REGISTER_SENSOR_AUTO(SensorOdom2d);
} // namespace wolf
diff --git a/src/sensor/sensor_odom_3d.cpp b/src/sensor/sensor_odom_3d.cpp
index a79995b85e11a3c248be1f19d4c69514cd6483d8..58b9b62ee9e111ce84887ba16e728a3494e40b7a 100644
--- a/src/sensor/sensor_odom_3d.cpp
+++ b/src/sensor/sensor_odom_3d.cpp
@@ -39,9 +39,9 @@ SensorOdom3d::~SensorOdom3d()
} // namespace wolf
-// Register in the SensorFactory
-#include "core/sensor/sensor_factory.h"
+// Register in the FactorySensor
+#include "core/sensor/factory_sensor.h"
namespace wolf {
-WOLF_REGISTER_SENSOR("SensorOdom3d", SensorOdom3d);
-WOLF_REGISTER_SENSOR_AUTO("SensorOdom3d", SensorOdom3d);
+WOLF_REGISTER_SENSOR(SensorOdom3d);
+WOLF_REGISTER_SENSOR_AUTO(SensorOdom3d);
}
diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp
index 6532dfc6a2b95d43451e00f8fad4797e7c53edf9..24a674f5f517e7bbd5ac8448c8678a7517632b90 100644
--- a/src/solver/solver_manager.cpp
+++ b/src/solver/solver_manager.cpp
@@ -11,11 +11,20 @@ SolverManager::SolverManager(const ProblemPtr& _wolf_problem) :
assert(_wolf_problem != nullptr && "Passed a nullptr ProblemPtr.");
}
+SolverManager::~SolverManager()
+{
+}
+
void SolverManager::update()
{
// Consume notification maps
- auto fac_notification_map = wolf_problem_->consumeFactorNotificationMap();
- auto sb_notification_map = wolf_problem_->consumeStateBlockNotificationMap();
+ std::map<StateBlockPtr,Notification> sb_notification_map;
+ std::map<FactorBasePtr,Notification> fac_notification_map;
+ wolf_problem_->consumeNotifications(sb_notification_map, fac_notification_map);
+
+ #ifdef _WOLF_DEBUG
+ assert(check("before update()"));
+ #endif
// REMOVE FACTORS
for (auto fac_notification_it = fac_notification_map.begin();
@@ -34,39 +43,15 @@ void SolverManager::update()
// ADD/REMOVE STATE BLOCS
while ( !sb_notification_map.empty() )
{
- StateBlockPtr state_ptr = sb_notification_map.begin()->first;
-
+ // add
if (sb_notification_map.begin()->second == ADD)
- {
- // only add if not added
- if (state_blocks_.find(state_ptr) == state_blocks_.end())
- {
- state_blocks_.emplace(state_ptr, state_ptr->getState());
- addStateBlock(state_ptr);
- // A state_block is added with its last state_ptr, status and local_param, thus, no update is needed for any of those things -> reset flags
- state_ptr->resetStateUpdated();
- state_ptr->resetFixUpdated();
- state_ptr->resetLocalParamUpdated();
- }
- else
- {
- WOLF_DEBUG("Tried to add a StateBlock that was already added !");
- }
- }
+ addStateBlock(sb_notification_map.begin()->first);
+
+ // remove
else
- {
- // only remove if it exists
- if (state_blocks_.find(state_ptr)!=state_blocks_.end())
- {
- removeStateBlock(state_ptr);
- state_blocks_.erase(state_ptr);
- }
- else
- {
- WOLF_DEBUG("Tried to remove a StateBlock that was not added !");
- }
- }
- // next notification
+ removeStateBlock(sb_notification_map.begin()->first);
+
+ // remove notification
sb_notification_map.erase(sb_notification_map.begin());
}
@@ -77,6 +62,7 @@ void SolverManager::update()
// add factor
addFactor(fac_notification_map.begin()->first);
+
// remove notification
fac_notification_map.erase(fac_notification_map.begin());
}
@@ -88,28 +74,20 @@ void SolverManager::update()
// state update
if (state_ptr->stateUpdated())
- {
- Eigen::VectorXd new_state = state_ptr->getState();
- // We assume the same size for the states in both WOLF and the solver.
- std::copy(new_state.data(),new_state.data()+new_state.size(),getAssociatedMemBlockPtr(state_ptr));
- // reset flag
- state_ptr->resetStateUpdated();
- }
+ updateStateBlockState(state_ptr);
+
// fix update
if (state_ptr->fixUpdated())
- {
updateStateBlockStatus(state_ptr);
- // reset flag
- state_ptr->resetFixUpdated();
- }
+
// local parameterization update
if (state_ptr->localParamUpdated())
- {
updateStateBlockLocalParametrization(state_ptr);
- // reset flag
- state_ptr->resetLocalParamUpdated();
- }
}
+
+ #ifdef _WOLF_DEBUG
+ assert(check("after update()"));
+ #endif
}
wolf::ProblemPtr SolverManager::getProblem()
@@ -122,24 +100,179 @@ std::string SolverManager::solve(const ReportVerbosity report_level)
// update problem
update();
- std::string report = solveImpl(report_level);
+ std::string report = solveDerived(report_level);
// update StateBlocks with optimized state value.
/// @todo whatif someone has changed the state notification during opti ??
/// JV: I do not see a problem here, the solver provides the optimal state given the factors, if someone changed the state during optimization, it will be overwritten by the optimal one.
- std::map<StateBlockPtr, Eigen::VectorXd>::iterator it = state_blocks_.begin(),
- it_end = state_blocks_.end();
- for (; it != it_end; ++it)
+ //std::map<StateBlockPtr, Eigen::VectorXd>::iterator it = state_blocks_.begin(),
+ // it_end = state_blocks_.end();
+ for (auto& stateblock_statevector : state_blocks_)
{
// Avoid usuless copies
- if (!it->first->isFixed())
- it->first->setState(it->second, false); // false = do not raise the flag state_updated_
+ if (!stateblock_statevector.first->isFixed())
+ stateblock_statevector.first->setState(stateblock_statevector.second, false); // false = do not raise the flag state_updated_
}
return report;
}
+void SolverManager::addFactor(const FactorBasePtr& fac_ptr)
+{
+ // Warning if adding an already added
+ if (factors_.count(fac_ptr) != 0)
+ {
+ WOLF_WARN("Tried to add a factor that was already added !");
+ return;
+ }
+
+ /* HANDLING 'TRUNCATED' NOTIFICATION
+ * This happens in multi-threading if update() is called while emplacing/removing nodes
+ *
+ * ADD factor without ADD state block constrained by the factor
+ *
+ * Notification is put back on problem notifications, it will be added next update() call.
+ */
+ for (const auto& st : fac_ptr->getStateBlockPtrVector())
+ if (state_blocks_.count(st) == 0)
+ {
+ WOLF_WARN("SolverManager::addFactor(): Factor ", fac_ptr->id(), " is notified to ADD but the involved state block ", st, " is not. Skipping, will be added later.");
+ // put back notification in problem (will be added next update() call) and do nothing
+ wolf_problem_->notifyFactor(fac_ptr, ADD);
+ return;
+ }
+
+ // factors
+ factors_.insert(fac_ptr);
+
+ // state-factor map
+ for (const auto& st : fac_ptr->getStateBlockPtrVector())
+ {
+ assert(state_blocks_.count(st) != 0 && "SolverManager::addFactor before adding any involved state block");
+ assert(state_blocks_2_factors_.count(st) != 0 && "SolverManager::addFactor before adding any involved state block");
+ state_blocks_2_factors_.at(st).push_back(fac_ptr);
+ }
+
+ // derived
+ addFactorDerived(fac_ptr);
+}
+
+void SolverManager::removeFactor(const FactorBasePtr& fac_ptr)
+{
+ // Warning if removing a missing factor
+ if (factors_.count(fac_ptr) == 0)
+ {
+ WOLF_WARN("Tried to remove a factor that is missing !");
+ return;
+ }
+
+ // derived
+ removeFactorDerived(fac_ptr);
+
+ // factors
+ factors_.erase(fac_ptr);
+
+ // state-factor map
+ for (const auto& st : fac_ptr->getStateBlockPtrVector())
+ {
+ assert(state_blocks_.count(st) != 0 && "SolverManager::removeFactor missing any involved state block");
+ assert(state_blocks_2_factors_.count(st) != 0 && "SolverManager::removeFactor missing any involved state block");
+ state_blocks_2_factors_.at(st).remove(fac_ptr);
+ }
+}
+
+void SolverManager::addStateBlock(const StateBlockPtr& state_ptr)
+{
+ // Warning if adding an already added state block
+ if (state_blocks_.count(state_ptr) != 0)
+ {
+ WOLF_WARN("Tried to add a StateBlock that was already added !");
+ return;
+ }
+
+ assert(state_blocks_.count(state_ptr) == 0 && "SolverManager::addStateBlock state block already added");
+ assert(state_blocks_2_factors_.count(state_ptr) == 0 && "SolverManager::addStateBlock state block already added");
+
+ // stateblock maps
+ state_blocks_.emplace(state_ptr, state_ptr->getState());
+ state_blocks_2_factors_.emplace(state_ptr, FactorBasePtrList());
+
+ // derived
+ addStateBlockDerived(state_ptr);
+
+ // A state_block is added with its last state_ptr, status and local_param, thus, no update is needed for any of those things -> reset flags
+ state_ptr->resetStateUpdated();
+ state_ptr->resetFixUpdated();
+ state_ptr->resetLocalParamUpdated();
+}
+
+void SolverManager::removeStateBlock(const StateBlockPtr& state_ptr)
+{
+ // Warning if removing a missing state block
+ if (state_blocks_.count(state_ptr) == 0)
+ {
+ WOLF_WARN("Tried to remove a StateBlock that was not added !");
+ return;
+ }
+
+ assert(state_blocks_.count(state_ptr) != 0 && "SolverManager::removeStateBlock missing state block");
+ assert(state_blocks_2_factors_.count(state_ptr) != 0 && "SolverManager::removeStateBlock missing state block");
+
+ /* HANDLING 'TRUNCATED' NOTIFICATION
+ * This happens in multi-threading if update() is called while emplacing/removing nodes
+ *
+ * REMOVE state block without REMOVE factor that constrains it
+ *
+ * Notification is put back on problem notifications, it will be removed next update() call.
+ */
+ if (!state_blocks_2_factors_.at(state_ptr).empty())
+ {
+ WOLF_WARN("SolverManager::removeStateBlock(): StateBlock ", state_ptr, " is notified to REMOVE but ", state_blocks_2_factors_.at(state_ptr).size(), " involved factors still not removed. Skipping, will be removed later.");
+ // put back notification in problem (will be removed next update() call) and do nothing
+ for (auto fac : state_blocks_2_factors_.at(state_ptr))
+ WOLF_INFO(fac->id());
+ wolf_problem_->notifyStateBlock(state_ptr, REMOVE);
+ return;
+ }
+
+ assert(state_blocks_2_factors_.at(state_ptr).empty() && "SolverManager::removeStateBlock removing state block before removing all factors involved");
+
+ // derived
+ removeStateBlockDerived(state_ptr);
+
+ // stateblock maps
+ state_blocks_.erase(state_ptr);
+ state_blocks_2_factors_.erase(state_ptr);
+}
+
+void SolverManager::updateStateBlockStatus(const StateBlockPtr& state_ptr)
+{
+ // derived
+ updateStateBlockStatusDerived(state_ptr);
+
+ // reset flag
+ state_ptr->resetFixUpdated();
+}
+
+void SolverManager::updateStateBlockState(const StateBlockPtr& state_ptr)
+{
+ Eigen::VectorXd new_state = state_ptr->getState();
+ // We assume the same size for the states in both WOLF and the solver.
+ std::copy(new_state.data(),new_state.data()+new_state.size(),getAssociatedMemBlockPtr(state_ptr));
+ // reset flag
+ state_ptr->resetStateUpdated();
+}
+
+void SolverManager::updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr)
+{
+ // derived
+ updateStateBlockLocalParametrizationDerived(state_ptr);
+
+ // reset flag
+ state_ptr->resetLocalParamUpdated();
+}
+
Eigen::VectorXd& SolverManager::getAssociatedMemBlock(const StateBlockPtr& state_ptr)
{
auto it = state_blocks_.find(state_ptr);
@@ -150,6 +283,16 @@ Eigen::VectorXd& SolverManager::getAssociatedMemBlock(const StateBlockPtr& state
return it->second;
}
+const double* SolverManager::getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr) const
+{
+ auto it = state_blocks_.find(state_ptr);
+
+ if (it == state_blocks_.end())
+ throw std::runtime_error("Tried to retrieve the memory block of an unregistered StateBlock !");
+
+ return it->second.data();
+}
+
double* SolverManager::getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr)
{
auto it = state_blocks_.find(state_ptr);
@@ -162,7 +305,7 @@ double* SolverManager::getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr)
bool SolverManager::isStateBlockRegistered(const StateBlockPtr& state_ptr)
{
- return state_blocks_.find(state_ptr) != state_blocks_.end() && isStateBlockRegisteredDerived(state_ptr);
+ return state_blocks_.count(state_ptr) ==1 && isStateBlockRegisteredDerived(state_ptr);
}
bool SolverManager::isFactorRegistered(const FactorBasePtr& fac_ptr) const
@@ -170,4 +313,59 @@ bool SolverManager::isFactorRegistered(const FactorBasePtr& fac_ptr) const
return isFactorRegisteredDerived(fac_ptr);
}
+
+bool SolverManager::check(std::string prefix) const
+{
+ bool ok = true;
+
+ // state blocks
+ if (state_blocks_.size() != state_blocks_2_factors_.size())
+ {
+ WOLF_ERROR("SolverManager::check: mismatching number of state blocks (state_blocks_, state_blocks_2_factors_) - in ", prefix);
+ ok = false;
+ }
+ auto sb_vec_it = state_blocks_.begin();
+ auto sb_fac_it = state_blocks_2_factors_.begin();
+ while (sb_vec_it != state_blocks_.end())
+ {
+ // same state block in both maps
+ if (sb_vec_it->first != sb_fac_it->first)
+ {
+ WOLF_ERROR("SolverManager::check: mismatching state blocks ", sb_vec_it->first, " and ", sb_fac_it->first, " - in ", prefix);
+ ok = false;
+ }
+
+ // factor involving state block in factors_
+ for (auto fac : sb_fac_it->second)
+ {
+ 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;
+ }
+ }
+ sb_vec_it++;
+ sb_fac_it++;
+ }
+
+ // factors
+ for (auto fac : factors_)
+ {
+ // involved sb stored
+ for (auto sb : fac->getStateBlockPtrVector())
+ {
+ if (state_blocks_.count(sb) == 0)
+ {
+ WOLF_ERROR("SolverManager::check: state block ", sb, " inolved in factor ", fac->id(), " missing in state_blocks_ map - in ", prefix);
+ ok = false;
+ }
+ }
+ }
+
+ // checkDerived
+ ok &= checkDerived(prefix);
+
+ return ok;
+}
+
} // namespace wolf
diff --git a/src/state_block/local_parametrization_base.cpp b/src/state_block/local_parametrization_base.cpp
index f46a93f9c488d0b899e94df156f3faff04a63554..aee3f704126bf4ba81e9658b3c337ded088c78a5 100644
--- a/src/state_block/local_parametrization_base.cpp
+++ b/src/state_block/local_parametrization_base.cpp
@@ -22,3 +22,14 @@ unsigned int LocalParametrizationBase::getGlobalSize() const
}
} // namespace wolf
+
+bool wolf::LocalParametrizationBase::plus(const Eigen::VectorXd &_x,
+ const Eigen::VectorXd &_delta,
+ Eigen::VectorXd &_x_plus_delta) const
+{
+ Eigen::Map<const Eigen::VectorXd> x(_x.data(), _x.size());
+ Eigen::Map<const Eigen::VectorXd> delta(_delta.data(), _delta.size());
+ Eigen::Map<Eigen::VectorXd> x_plus_delta(_x_plus_delta.data(), _x_plus_delta.size());
+
+ return plus(x, delta, x_plus_delta);
+}
diff --git a/src/state_block/state_block.cpp b/src/state_block/state_block.cpp
index 783a56a5e8cb52fe0ae79e2ad14ca4b50976a48c..ea2c9a4cd4f32b4a1c49e1bfec6ad28efb397cbe 100644
--- a/src/state_block/state_block.cpp
+++ b/src/state_block/state_block.cpp
@@ -26,32 +26,52 @@ void StateBlock::setFixed(bool _fixed)
fixed_.store(_fixed);
}
-//void StateBlock::addToProblem(ProblemPtr _problem_ptr)
-//{
-// _problem_ptr->addStateBlock(shared_from_this());
-//}
-//
-//void StateBlock::removeFromProblem(ProblemPtr _problem_ptr)
-//{
-// _problem_ptr->removeStateBlock(shared_from_this());
-//}
-
void StateBlock::perturb(double amplitude)
{
using namespace Eigen;
VectorXd perturbation(VectorXd::Random(getLocalSize()) * amplitude);
+ this->plus(perturbation);
+}
+
+void StateBlock::plus(const Eigen::VectorXd &_dv)
+{
if (local_param_ptr_ == nullptr)
- state_ += perturbation;
+ setState(getState() + _dv);
else
{
- VectorXd state_perturbed(getSize());
- // Note: LocalParametrizationBase::plus() works with Eigen::Map only. Build all necessary maps:
- Map<const VectorXd> state_map(state_.data(), getSize());
- Map<const VectorXd> perturbation_map(perturbation.data(), getLocalSize());
- Map<VectorXd> state_perturbed_map(state_perturbed.data(), getSize());
- local_param_ptr_->plus(state_map, perturbation_map, state_perturbed_map);
- state_ = state_perturbed;
+ Eigen::VectorXd state(getSize());
+ local_param_ptr_->plus(getState(), _dv, state);
+ setState(state);
}
}
}
+
+#include "core/state_block/factory_state_block.h"
+#include "core/state_block/state_quaternion.h"
+#include "core/state_block/state_angle.h"
+#include "core/state_block/state_homogeneous_3d.h"
+
+namespace wolf{
+WOLF_REGISTER_STATEBLOCK(StateBlock);
+WOLF_REGISTER_STATEBLOCK(StateQuaternion);
+WOLF_REGISTER_STATEBLOCK(StateAngle);
+WOLF_REGISTER_STATEBLOCK(StateHomogeneous3d);
+WOLF_REGISTER_STATEBLOCK_WITH_KEY(H, StateHomogeneous3d);
+
+StateBlockPtr create_orientation(const Eigen::VectorXd& _state, bool _fixed)
+{
+ if (_state.size() == 1)
+ return StateAngle::create(_state, _fixed);
+ if (_state.size() == 4)
+ return StateQuaternion::create(_state, _fixed);
+
+ throw std::length_error("Wrong vector size for orientation. Must be 4 for a quaternion in 3D, or 1 for an angle in 2D.");
+
+ return nullptr;
+}
+
+//WOLF_REGISTER_STATEBLOCK_WITH_KEY(O, wolf::create_orientation);
+namespace{ const bool __attribute__((used)) create_orientationRegisteredWithO = \
+ FactoryStateBlock::get().registerCreator("O", wolf::create_orientation); }
+}
diff --git a/src/state_block/state_composite.cpp b/src/state_block/state_composite.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..612b8e53ec55d7e5b787a5aa0c49312d21dfe4a9
--- /dev/null
+++ b/src/state_block/state_composite.cpp
@@ -0,0 +1,1010 @@
+
+
+
+#include "core/state_block/state_composite.h"
+#include "core/state_block/state_block.h"
+
+namespace wolf{
+
+////// VECTOR COMPOSITE //////////
+
+VectorComposite::VectorComposite(const StateStructure& _structure, const std::list<int>& _sizes)
+{
+ auto size_it = _sizes.begin();
+ for ( const auto& ckey : _structure)
+ {
+ const auto& key = string(1,ckey);
+ const auto& size = *size_it;
+
+ this->emplace(key, VectorXd(size).setZero());
+
+ size_it ++;
+ }
+}
+
+VectorComposite::VectorComposite(const VectorXd& _v, const StateStructure& _structure, const std::list<int>& _sizes)
+{
+ int index = 0;
+ auto size_it = _sizes.begin();
+ for ( const auto& ckey : _structure)
+ {
+ const auto& key = string(1,ckey);
+ const auto& size = *size_it;
+
+ (*this)[key] = _v.segment(index,size);
+
+ index += size;
+ size_it ++;
+ }
+}
+
+VectorComposite::VectorComposite (const StateStructure& _s)
+{
+ for (const auto& ckey : _s)
+ {
+ const auto& key = string(1,ckey); // ckey is char
+ this->emplace(key,VectorXd(0));
+ }
+}
+
+VectorComposite::VectorComposite (const StateStructure& _structure, const std::list<VectorXd>& _vectors)
+{
+ assert(_structure.size() == _vectors.size() && "Structure and vector list size mismatch");
+
+ auto vector_it = _vectors.begin();
+
+ for (const auto& ckey : _structure)
+ {
+ string key(1,ckey);
+ this->emplace(key, *vector_it);
+ vector_it++;
+ }
+}
+
+
+unsigned int VectorComposite::size(const StateStructure &_structure) const
+{
+ unsigned int size = 0;
+ for (const auto& ckey : _structure)
+ {
+ std::string key(1,ckey); // ckey is char
+ const VectorXd& v = this->at(key);
+ size += v.size();
+ }
+ return size;
+}
+
+Eigen::VectorXd VectorComposite::vector(const StateStructure &_structure) const
+{
+ // traverse once with unordered_map access
+ std::vector<const VectorXd*> vp;
+ unsigned int size = 0;
+ for (const auto& ckey : _structure)
+ {
+ std::string key(1,ckey); // ckey is char
+ vp.push_back(&(this->at(key)));
+ size += vp.back()->size();
+ }
+
+ Eigen::VectorXd x(size);
+
+ // traverse once linearly
+ unsigned int index = 0;
+ for (const auto& blkp : vp)
+ {
+ x.segment(index, blkp->size()) = *blkp;
+ index += blkp->size();
+ }
+ return x;
+}
+
+std::ostream& operator <<(std::ostream &_os, const wolf::VectorComposite &_x)
+{
+ for (const auto &pair_key_vec : _x)
+ {
+ const auto &key = pair_key_vec.first;
+ const auto &vec = pair_key_vec.second;
+ _os << key << ": (" << vec.transpose() << "); ";
+ }
+ return _os;
+}
+
+wolf::VectorComposite operator +(const wolf::VectorComposite &_x, const wolf::VectorComposite &_y)
+{
+ wolf::VectorComposite xpy;
+ for (const auto& pair_i_xi : _x)
+ {
+ const auto& i = pair_i_xi.first;
+
+ xpy.emplace(i, _x.at(i) + _y.at(i));
+ }
+ return xpy;
+}
+
+wolf::VectorComposite operator -(const wolf::VectorComposite &_x, const wolf::VectorComposite &_y)
+{
+ wolf::VectorComposite xpy;
+ for (const auto& pair_i_xi : _x)
+ {
+ const auto& i = pair_i_xi.first;
+
+ xpy.emplace(i, _x.at(i) - _y.at(i));
+ }
+ return xpy;
+}
+
+void VectorComposite::set (const VectorXd& _v, const StateStructure& _structure, const std::list<int>& _sizes)
+{
+ int index = 0;
+ auto size_it = _sizes.begin();
+ for ( const auto& ckey : _structure)
+ {
+ const auto& key = string(1,ckey); // ckey is char
+ const auto& size = *size_it;
+
+ (*this)[key] = _v.segment(index,size);
+
+ index += size;
+ size_it ++;
+ }
+}
+
+void VectorComposite::setZero()
+{
+ for (auto& pair_key_vec : *this)
+ pair_key_vec.second.setZero();
+}
+
+wolf::VectorComposite operator -(const wolf::VectorComposite &_x)
+{
+ wolf::VectorComposite m;
+ for (const auto& pair_i_xi : _x)
+ {
+ const auto& i = pair_i_xi.first;
+ m.emplace(i, - _x.at(i));
+ }
+ return m;
+}
+
+////// MATRIX COMPOSITE //////////
+
+bool MatrixComposite::emplace(const std::string &_row, const std::string &_col, const Eigen::MatrixXd &_mat_blk)
+{
+ // check rows
+ if (size_rows_.count(_row) == 0)
+ size_rows_[_row] = _mat_blk.rows();
+ else
+ assert(size_rows_.at(_row) == _mat_blk.rows() && "Provided matrix block has wrong number of rows!");
+
+ // check cols
+ if (size_cols_.count(_col) == 0)
+ size_cols_[_col] = _mat_blk.cols();
+ else
+ assert(size_rows_.at(_row) == _mat_blk.rows() && "Provided matrix block has wrong number of rows!");
+
+ // now it's safe to use [] operator. the first one is a function-like call to [] -- just weird for this->[]
+ this->operator[](_row)[_col] = _mat_blk;
+ return true;
+}
+
+bool MatrixComposite::at(const std::string &_row, const std::string &_col, Eigen::MatrixXd &_mat_blk) const
+{
+ const auto &row_it = this->find(_row);
+ if(row_it != this->end())
+ return false;
+
+ const auto &col_it = row_it->second.find(_col);
+ if(col_it != row_it->second.end())
+ return false;
+
+ _mat_blk = col_it->second;
+
+ return true;
+}
+
+Eigen::MatrixXd& MatrixComposite::at(const std::string &_row, const std::string &_col)
+{
+ const auto &row_it = this->find(_row);
+ assert(row_it != this->end() && "Requested matrix block does not exist in matrix composite.");
+
+ const auto &col_it = row_it->second.find(_col);
+ assert(col_it != row_it->second.end() && "Requested matrix block does not exist in matrix composite.");
+
+ return col_it->second;
+}
+
+const Eigen::MatrixXd& MatrixComposite::at(const std::string &_row, const std::string &_col) const
+{
+ const auto &row_it = this->find(_row);
+ assert(row_it != this->end() && "Requested matrix block does not exist in matrix composite.");
+
+ const auto &col_it = row_it->second.find(_col);
+ assert(col_it != row_it->second.end() && "Requested matrix block does not exist in matrix composite.");
+
+ return col_it->second;
+}
+
+wolf::MatrixComposite MatrixComposite::operator *(const MatrixComposite &_N) const
+{
+ MatrixComposite MN;
+ for (const auto &pair_i_Mi : (*this))
+ {
+ const auto &i = pair_i_Mi.first;
+ const auto &Mi = pair_i_Mi.second;
+
+ for (const auto &pair_k_Nk : _N)
+ {
+ const auto &k = pair_k_Nk.first;
+ const auto &Nk = pair_k_Nk.second;
+
+ for (const auto &pair_j_Nkj : Nk)
+ {
+ const auto &j = pair_j_Nkj.first;
+ const auto &Nkj = pair_j_Nkj.second;
+ const auto &Mik = Mi.at(k);
+
+ if (MN.count(i, j) == 0)
+ MN.emplace(i, j, Mik * Nkj);
+ else
+ MN.at(i, j) += Mik * Nkj;
+ }
+ }
+ }
+ return MN;
+}
+
+wolf::MatrixComposite MatrixComposite::operator +(const MatrixComposite &_N) const
+{
+ MatrixComposite MpN;
+ for (const auto &pair_i_Mi : *this)
+ {
+ const auto &i = pair_i_Mi.first;
+ const auto &Mi = pair_i_Mi.second;
+
+ for (const auto& pair_j_Mij : Mi)
+ {
+ const auto& j = pair_j_Mij.first;
+ const auto& Mij = pair_j_Mij.second;
+ const auto& Nij = _N.at(i,j);
+
+ MpN.emplace(i, j, Mij + Nij);
+ }
+ }
+ return MpN;
+}
+
+wolf::MatrixComposite MatrixComposite::operator -(const MatrixComposite &_N) const
+{
+ MatrixComposite MpN;
+ for (const auto &pair_i_Mi : *this)
+ {
+ const auto &i = pair_i_Mi.first;
+ const auto &Mi = pair_i_Mi.second;
+
+ for (const auto& pair_j_Mij : Mi)
+ {
+ const auto& j = pair_j_Mij.first;
+ const auto& Mij = pair_j_Mij.second;
+ const auto& Nij = _N.at(i,j);
+
+ MpN.emplace(i, j, Mij - Nij);
+ }
+ }
+ return MpN;
+}
+
+MatrixComposite MatrixComposite::operator - (void) const
+{
+ MatrixComposite m;
+
+ for (const auto& pair_rkey_row : *this)
+ {
+ m.unordered_map<string,unordered_map<string, MatrixXd>>::emplace(pair_rkey_row.first, unordered_map<string, MatrixXd>());
+ for (const auto& pair_ckey_blk : pair_rkey_row.second)
+ {
+ m[pair_rkey_row.first].emplace(pair_ckey_blk.first, - pair_ckey_blk.second);
+ }
+ }
+ return m;
+}
+
+
+wolf::VectorComposite MatrixComposite::operator *(const VectorComposite &_x) const
+{
+ VectorComposite y;
+ for (const auto &pair_rkey_row : *this)
+ {
+ const auto &rkey = pair_rkey_row.first;
+ const auto &row = pair_rkey_row.second;
+
+ for (const auto &pair_ckey_mat : row)
+ {
+ const auto &ckey = pair_ckey_mat.first;
+ const auto &J_r_c = pair_ckey_mat.second;
+
+ const auto& it = y.find(rkey);
+ if (it != y.end())
+ it->second += J_r_c * _x.at(ckey);
+ else
+ y.emplace(rkey, J_r_c * _x.at(ckey));
+ }
+ }
+ return y;
+}
+
+MatrixComposite MatrixComposite::propagate(const MatrixComposite& _Cov)
+{
+
+
+ // simplify names of operands
+ const auto& J = *this;
+ const auto& Q = _Cov;
+
+ MatrixComposite S; // S = J * Q * J.tr
+
+
+ /* Covariance propagation
+ *
+ * 1. General formula for the output block S(i,j)
+ *
+ * S_ij = sum_n (J_in * sum_k(Q_nk * J_jk^T)) (A)
+ *
+ * which develops as
+ *
+ * S_ij = sum_n (J_in * QJt_nj)
+ *
+ * with:
+ *
+ * QJt_nj = sum_k (Q_nk * J_jk^T) (B)
+ *
+ *
+ * 2. Algorithm to accomplish the above:
+ *
+ * for i = 1 : S.rows = J.rows (1) // iterate for i and j
+ * {
+ * J_i = J[i]
+ * for j = i : S.cols = J.rows (2) // j starts at i: do not loop over the symmetrical blocks
+ * {
+ * S_ij = 0 (3) // start formula (A) for S_ij
+ * for n = 1 : Q.rows (4)
+ * {
+ * J_in = J[i][n]
+ * QJt_nj = 0 (5) // start formula (B) for QJt_nj
+ * for k = 1 : Q.cols (6)
+ * {
+ * J_jk = J[j][k]
+ * QJt_nj += Q_nk * J_jk^T (7) // sum on QJt_nj
+ * }
+ * S_ij += J_in * QJt_nj (8) // sum on S_ij
+ * }
+ * S[i][j] = S_ij // write block in output composite
+ * if (i != j)
+ * S[j][i] = S_ij^T (9) // write symmetrical block in output composite
+ * }
+ * }
+ */
+
+ // Iterate on the output matrix S first, row i, col j.
+ for (const auto& pair_i_Si : J) // (1)
+ {
+ const auto& i = pair_i_Si.first;
+ const auto& J_i = pair_i_Si.second;
+
+ double i_size = J_i.begin()->second.rows();
+
+ for (const auto& pair_j_Sj : J) // (2)
+ {
+ const auto& j = pair_j_Sj.first;
+ const auto& J_j = pair_j_Sj.second;
+
+ double j_size = J_j.begin()->second.rows();
+
+ MatrixXd S_ij(MatrixXd::Zero(i_size, j_size)); // (3)
+
+ for (const auto& pair_n_Qn : Q) // (4)
+ {
+ const auto& n = pair_n_Qn.first;
+ const auto& Q_n = pair_n_Qn.second;
+
+ double n_size = Q_n.begin()->second.rows();
+
+ const auto& J_in = J_i.at(n);
+
+ MatrixXd QJt_nj(MatrixXd::Zero(n_size, j_size)); // (5)
+
+ for (const auto& pair_k_Qnk : Q_n) // (6)
+ {
+ const auto& k = pair_k_Qnk.first;
+ const auto& Q_nk = pair_k_Qnk.second;
+
+ const auto& J_jk = J_j.at(k);
+
+ QJt_nj += Q_nk * J_jk.transpose(); // (7)
+ }
+
+ S_ij += J_in * QJt_nj; // (8)
+ }
+
+ S.emplace(i,j,S_ij);
+ if (j == i)
+ break; // avoid computing the symmetrical block!
+ else
+ S.emplace(j, i, S_ij.transpose()); // (9)
+ }
+ }
+
+ return S;
+}
+
+MatrixComposite MatrixComposite::operator * (double scalar_right) const
+{
+ MatrixComposite S(*this);
+ for (const auto& pair_rkey_row : *this)
+ {
+ const auto& rkey = pair_rkey_row.first;
+ for (const auto& pair_ckey_block : pair_rkey_row.second)
+ {
+ const auto ckey = pair_ckey_block.first;
+ S[rkey][ckey] *= scalar_right;
+ }
+ }
+ return S;
+}
+
+MatrixComposite operator * (double scalar_left, const MatrixComposite& M)
+{
+ MatrixComposite S;
+ S = M * scalar_left;
+ return S;
+}
+
+MatrixXd MatrixComposite::matrix(const StateStructure &_struct_rows, const StateStructure &_struct_cols) const
+{
+
+ std::unordered_map < string, unsigned int> indices_rows;
+ std::unordered_map < string, unsigned int> indices_cols;
+ unsigned int rows, cols;
+
+ sizeAndIndices(_struct_rows, _struct_cols, indices_rows, indices_cols, rows, cols);
+
+ MatrixXd M ( MatrixXd::Zero(rows, cols) );
+
+ for (const auto& pair_row_rband : (*this))
+ {
+ const auto& row = pair_row_rband.first;
+ const auto& rband = pair_row_rband.second;
+ for (const auto& pair_col_blk : rband)
+ {
+ const auto& col = pair_col_blk.first;
+ const auto& blk = pair_col_blk.second;
+
+ const unsigned int & blk_rows = size_rows_.at(row);
+ const unsigned int & blk_cols = size_cols_.at(col);
+
+ assert(blk.rows() == blk_rows && "MatrixComposite block size mismatch! Wrong number of rows.");
+ assert(blk.cols() == blk_cols && "MatrixComposite block size mismatch! Wrong number of cols.");
+
+ M.block(indices_rows.at(row), indices_cols.at(col), blk_rows, blk_cols) = blk;
+
+ }
+ }
+
+ return M;
+}
+
+unsigned int MatrixComposite::rows() const
+{
+ unsigned int rows = 0;
+ for (const auto& pair_row_size : this->size_rows_)
+ rows += pair_row_size.second;
+ return rows;
+}
+
+unsigned int MatrixComposite::cols() const
+{
+ unsigned int cols = 0;
+ for (const auto& pair_col_size : this->size_rows_)
+ cols += pair_col_size.second;
+ return cols;
+}
+
+void MatrixComposite::sizeAndIndices(const StateStructure &_struct_rows,
+ const StateStructure &_struct_cols,
+ std::unordered_map<string, unsigned int> &_indices_rows,
+ std::unordered_map<string, unsigned int> &_indices_cols,
+ unsigned int &_rows,
+ unsigned int &_cols) const
+{
+ _rows = 0;
+ _cols = 0;
+ for (const auto& crow : _struct_rows)
+ {
+ string row = string(1,crow); // crow is char
+ _indices_rows[row] = _rows;
+ _rows += size_rows_.at(row);
+ }
+ for (const auto& ccol : _struct_cols)
+ {
+ string col = string(1,ccol); // ccol is char
+ _indices_cols[col] = _cols;
+ _cols += size_cols_.at(col);
+ }
+}
+
+MatrixComposite::MatrixComposite (const StateStructure& _row_structure, const StateStructure& _col_structure)
+{
+ for (const auto& rkey_char : _row_structure)
+ {
+ string rkey(1,rkey_char); // key was char
+ for (const auto& ckey_char : _col_structure)
+ {
+ string ckey(1,ckey_char); // key was char
+
+ this->emplace(rkey, ckey, MatrixXd(0,0));
+ }
+ }
+}
+
+MatrixComposite::MatrixComposite (const StateStructure& _row_structure,
+ const std::list<int>& _row_sizes,
+ const StateStructure& _col_structure,
+ const std::list<int>& _col_sizes)
+{
+ assert (_row_structure.size() == _row_sizes.size() && "Row structure and sizes have different number of elements!");
+ assert (_col_structure.size() == _col_sizes.size() && "Column structure and sizes have different number of elements!");
+
+ auto rsize_it = _row_sizes.begin();
+ for (const auto& rkey_char : _row_structure)
+ {
+ string rkey(1,rkey_char); // key was char
+
+ auto csize_it = _col_sizes.begin();
+ for (const auto& ckey_char : _col_structure)
+ {
+ string ckey(1,ckey_char); // key was char
+
+ this->emplace(rkey, ckey, MatrixXd(*rsize_it, *csize_it)); // caution: blocks non initialized.
+
+ csize_it ++;
+ }
+ rsize_it++;
+ }
+}
+
+MatrixComposite::MatrixComposite (const MatrixXd& _m,
+ const StateStructure& _row_structure,
+ const std::list<int>& _row_sizes,
+ const StateStructure& _col_structure,
+ const std::list<int>& _col_sizes)
+{
+ assert (_row_structure.size() == _row_sizes.size() && "Row structure and sizes have different number of elements!");
+ assert (_col_structure.size() == _col_sizes.size() && "Column structure and sizes have different number of elements!");
+
+ SizeEigen rindex = 0, cindex;
+ auto rsize_it = _row_sizes.begin();
+ for (const auto& rkey_char : _row_structure)
+ {
+ assert(_m.rows() >= rindex + *rsize_it && "Provided matrix has insufficient number of rows");
+
+ string rkey(1,rkey_char); // key was char
+
+ cindex = 0;
+ auto csize_it = _col_sizes.begin();
+
+ for (const auto& ckey_char : _col_structure)
+ {
+ assert(_m.cols() >= cindex + *csize_it && "Provided matrix has insufficient number of columns");
+
+ string ckey(1,ckey_char); // key was char
+
+ this->emplace(rkey, ckey, _m.block(rindex, cindex, *rsize_it, *csize_it));
+
+ cindex += *csize_it;
+ csize_it ++;
+ }
+
+ assert(_m.cols() == cindex && "Provided matrix has too many columns");
+
+ rindex += *rsize_it;
+ rsize_it++;
+ }
+
+ assert(_m.rows() == rindex && "Provided matrix has too many rows");
+}
+
+MatrixComposite MatrixComposite::zero (const StateStructure& _row_structure, const std::list<int>& _row_sizes,
+ const StateStructure& _col_structure, const std::list<int>& _col_sizes)
+{
+ MatrixComposite m;
+
+ assert (_row_structure.size() == _row_sizes.size() && "Row structure and sizes have different number of elements!");
+ assert (_col_structure.size() == _col_sizes.size() && "Column structure and sizes have different number of elements!");
+
+ auto rsize_it = _row_sizes.begin();
+ for (const auto& rkey_char : _row_structure)
+ {
+ string rkey(1,rkey_char); // key was char
+
+ auto csize_it = _col_sizes.begin();
+ for (const auto& ckey_char : _col_structure)
+ {
+ string ckey(1,ckey_char); // key was char
+
+ m.emplace(rkey, ckey, MatrixXd::Zero(*rsize_it, *csize_it));
+
+ csize_it ++;
+ }
+ rsize_it++;
+ }
+ return m;
+}
+
+MatrixComposite MatrixComposite::identity (const StateStructure& _structure, const std::list<int>& _sizes)
+{
+ MatrixComposite m;
+
+ assert (_structure.size() == _sizes.size() && "Structure and sizes have different number of elements!");
+
+ auto rsize_it = _sizes.begin();
+ auto rkey_it = _structure.begin();
+
+ while (rkey_it != _structure.end())
+ {
+
+ const auto& rkey = string(1,*rkey_it);
+ const auto rsize = *rsize_it;
+
+ m.emplace(rkey, rkey, MatrixXd::Identity(rsize,rsize)); // diagonal block
+
+ auto csize_it = rsize_it;
+ auto ckey_it = rkey_it;
+
+ csize_it ++;
+ ckey_it++;
+
+ while (ckey_it != _structure.end())
+ {
+ const auto& ckey = string(1,*ckey_it);
+ const auto& csize = *csize_it;
+
+ m.emplace(rkey, ckey, MatrixXd::Zero(rsize, csize)); // above-diagonal block
+ m.emplace(ckey, rkey, MatrixXd::Zero(csize, rsize)); // symmetric block
+
+ csize_it ++;
+ ckey_it++;
+ }
+
+ rsize_it ++;
+ rkey_it++;
+ }
+ return m;
+}
+
+bool MatrixComposite::check ( ) const
+{
+ bool size_ok = true;
+
+ // first see matrix itself, check that sizes are OK
+ for (const auto& pair_rkey_row : *this)
+ {
+ const auto& rkey = pair_rkey_row.first;
+ const auto& row = pair_rkey_row.second;
+ for (const auto& pair_ckey_blk : row)
+ {
+ const auto& ckey = pair_ckey_blk.first;
+ const auto& blk = pair_ckey_blk.second;
+
+ if (size_rows_.count(rkey) != 0)
+ {
+ if( size_rows_.at(rkey) != blk.rows())
+ {
+ WOLF_ERROR("row size for row ", rkey, " has wrong size");
+ size_ok = false;
+ }
+ }
+ else
+ {
+ WOLF_ERROR("row size for row ", rkey, " does not exist");
+ size_ok = false;
+ }
+ if (size_cols_.count(ckey) != 0)
+ {
+ if( size_cols_.at(ckey) != blk.cols())
+ {
+ WOLF_ERROR("col size for col ", rkey, " has wrong size");
+ size_ok = false;
+ }
+ }
+ else
+ {
+ WOLF_ERROR("col size for col ", ckey, " does not exist");
+ size_ok = false;
+ }
+ }
+ }
+ return size_ok;
+}
+
+std::ostream& operator <<(std::ostream &_os, const MatrixComposite &_M)
+{
+ for (const auto &pair_rkey_row : _M)
+ {
+ const auto rkey = pair_rkey_row.first;
+
+ for (const auto &pair_ckey_mat : pair_rkey_row.second)
+ {
+ const auto &ckey = pair_ckey_mat.first;
+
+ _os << "\n[" << rkey << "," << ckey << "]: \n" << pair_ckey_mat.second;
+ }
+ }
+ return _os;
+}
+
+
+
+////// STATE BLOCK COMPOSITE //////////
+
+const StateBlockMap& StateBlockComposite::getStateBlockMap() const
+{
+ return state_block_map_;
+}
+
+void StateBlockComposite::fix()
+{
+ for (auto pair_key_sbp : state_block_map_)
+ if (pair_key_sbp.second != nullptr)
+ pair_key_sbp.second->fix();
+}
+
+void StateBlockComposite::unfix()
+{
+ for (auto pair_key_sbp : state_block_map_)
+ if (pair_key_sbp.second != nullptr)
+ pair_key_sbp.second->unfix();
+}
+
+bool StateBlockComposite::isFixed() const
+{
+ bool fixed = true;
+ for (auto pair_key_sbp : state_block_map_)
+ {
+ if (pair_key_sbp.second)
+ fixed &= pair_key_sbp.second->isFixed();
+ }
+ return fixed;
+}
+
+unsigned int StateBlockComposite::remove(const std::string &_sb_type)
+{
+ return state_block_map_.erase(_sb_type);
+}
+
+bool StateBlockComposite::has(const StateBlockPtr &_sb) const
+{
+ bool found = false;
+ for (const auto& pair_key_sb : state_block_map_)
+ {
+ found = found || (pair_key_sb.second == _sb);
+ }
+ return found;
+}
+
+StateBlockPtr StateBlockComposite::at(const std::string &_sb_type) const
+{
+ auto it = state_block_map_.find(_sb_type);
+ if (it != state_block_map_.end())
+ return it->second;
+ else
+ return nullptr;
+}
+
+void StateBlockComposite::set(const std::string& _sb_type, const StateBlockPtr &_sb)
+{
+ auto it = state_block_map_.find(_sb_type);
+ assert ( it != state_block_map_.end() && "Cannot set an inexistent state block! Use addStateBlock instead.");
+
+ it->second = _sb;
+}
+
+void StateBlockComposite::set(const std::string& _key, const VectorXd &_vec)
+{
+ auto it = state_block_map_.find(_key);
+ assert ( it != state_block_map_.end() && "Cannot set an inexistent state block! Use addStateBlock instead.");
+
+ assert ( it->second->getSize() == _vec.size() && "Provided vector size mismatch with associated state block");
+
+ it->second->setState(_vec);
+}
+
+void StateBlockComposite::setVectors(const StateStructure& _structure, const std::list<VectorXd> &_vectors)
+{
+ assert(_structure.size() == _vectors.size() && "Sizes of structure and vector list do not match");
+
+ auto vec_it = _vectors.begin();
+ for (const auto ckey : _structure)
+ {
+ string key(1,ckey);
+ set (key, *vec_it);
+ vec_it++;
+ }
+}
+
+bool StateBlockComposite::key(const StateBlockPtr &_sb, std::string &_key) const
+{
+ const auto& it = this->find(_sb);
+
+ bool found = (it != state_block_map_.end());
+
+ if (found)
+ {
+ _key = it->first;
+ return true;
+ }
+ else
+ {
+ _key = "";
+ return false;
+ }
+}
+
+std::string StateBlockComposite::key(const StateBlockPtr& _sb) const
+{
+ const auto& it = this->find(_sb);
+
+ bool found = (it != state_block_map_.end());
+
+ if (found)
+ return it->first;
+ else
+ return "";
+}
+
+
+StateBlockMapCIter StateBlockComposite::find(const StateBlockPtr &_sb) const
+{
+ const auto& it = std::find_if(state_block_map_.begin(),
+ state_block_map_.end(),
+ [_sb](const std::pair<std::string, StateBlockPtr>& pair)
+ {
+ return pair.second == _sb;
+ }
+ );
+
+ return it;
+}
+
+StateBlockPtr StateBlockComposite::add(const std::string &_sb_type, const StateBlockPtr &_sb)
+{
+ assert(state_block_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
+
+ state_block_map_.emplace(_sb_type, _sb);
+
+ return _sb;
+}
+
+void StateBlockComposite::perturb(double amplitude)
+{
+ for (const auto& pair_key_sb : state_block_map_)
+ {
+ auto& sb = pair_key_sb.second;
+ if (!sb->isFixed())
+ sb->perturb(amplitude);
+ }
+}
+
+void StateBlockComposite::plus(const VectorComposite &_dx)
+{
+ for (const auto& pair_key_sb : state_block_map_)
+ {
+ const auto& key = pair_key_sb.first;
+ const auto& sb = pair_key_sb.second;
+ const auto& dv = _dx.at(key);
+
+ sb->plus(dv);
+ }
+}
+
+VectorComposite StateBlockComposite::getState() const
+{
+ VectorComposite v;
+ getState(v);
+ return v;
+}
+
+bool StateBlockComposite::getState(VectorComposite &_state) const
+{
+ for (auto &pair_key_sb : state_block_map_)
+ {
+ _state.emplace(pair_key_sb.first, pair_key_sb.second->getState());
+ }
+ return true;
+}
+
+void StateBlockComposite::setState(const VectorComposite &_state)
+{
+ for (const auto &pair_key_sb : _state)
+ {
+ state_block_map_[pair_key_sb.first]->setState(pair_key_sb.second);
+ }
+}
+
+void StateBlockComposite::setIdentity(bool _notify)
+{
+ for (const auto& pair_key_sb : getStateBlockMap())
+ {
+ pair_key_sb.second->setIdentity(_notify);
+ }
+}
+
+void StateBlockComposite::setZero(bool _notify)
+{
+ setIdentity(_notify);
+}
+
+VectorComposite StateBlockComposite::identity() const
+{
+ VectorComposite x;
+ for (const auto& pair_key_sb : getStateBlockMap())
+ x.emplace(pair_key_sb.first, pair_key_sb.second->identity());
+ return x;
+}
+
+VectorComposite StateBlockComposite::zero() const
+{
+ return identity();
+}
+
+
+SizeEigen StateBlockComposite::stateSize() const
+{
+ SizeEigen size = 0;
+ for (const auto& pair_key_sb : state_block_map_)
+ size += pair_key_sb.second->getSize();
+ return size;
+}
+
+SizeEigen StateBlockComposite::stateSize(const StateStructure &_structure) const
+{
+ SizeEigen size = 0;
+ for (const auto& ckey : _structure)
+ {
+ string key(1,ckey);
+ size += state_block_map_.at(key)->getSize();
+ }
+ return size;
+}
+
+VectorXd StateBlockComposite::stateVector(const StateStructure &_structure) const
+{
+ VectorXd x(stateSize(_structure));
+ SizeEigen index = 0;
+ SizeEigen size;
+ for (const auto& ckey : _structure)
+ {
+ string key(1,ckey);
+ const auto& sb = state_block_map_.at(key);
+ size = sb->getSize();
+ x.segment(index,size) = sb->getState();
+ index += size;
+ }
+ return x;
+}
+
+void StateBlockComposite::stateVector(const StateStructure &_structure, VectorXd &_vector) const
+{
+ _vector.resize(stateSize(_structure));
+ SizeEigen index = 0;
+ SizeEigen size;
+ for (const auto& ckey : _structure)
+ {
+ string key(1,ckey);
+ const auto& sb = state_block_map_.at(key);
+ size = sb->getSize();
+ _vector.segment(index,size) = sb->getState();
+ index += size;
+ }
+}
+
+} // namespace wolf
+
diff --git a/src/trajectory/trajectory_base.cpp b/src/trajectory/trajectory_base.cpp
index 428faf954d89ffc8f0d7a88b182cd04a92850c4a..ad760c8c3ccf2942f734c256e7b04ed3d59aecf7 100644
--- a/src/trajectory/trajectory_base.cpp
+++ b/src/trajectory/trajectory_base.cpp
@@ -3,9 +3,8 @@
namespace wolf {
-TrajectoryBase::TrajectoryBase(const std::string& _frame_structure) :
- NodeBase("TRAJECTORY", "Base"),
- frame_structure_(_frame_structure),
+TrajectoryBase::TrajectoryBase() :
+ NodeBase("TRAJECTORY", "TrajectoryBase"),
last_key_frame_ptr_(nullptr),
last_key_or_aux_frame_ptr_(nullptr)
{
@@ -136,4 +135,42 @@ FrameBasePtr TrajectoryBase::closestKeyOrAuxFrameToTimeStamp(const TimeStamp& _t
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;
+}
+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())
+ F->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
+
+}
+
+CheckLog TrajectoryBase::localCheck(bool _verbose, TrajectoryBasePtr _trj_ptr, std::ostream& _stream, std::string _tabs) const
+{
+ CheckLog log;
+ std::stringstream inconsistency_explanation;
+
+ if (_verbose)
+ {
+ _stream << _tabs << "Trj @ " << _trj_ptr.get() << std::endl;
+ }
+
+ // check pointer to Problem
+ inconsistency_explanation << "Trj->getProblem() [" << getProblem().get()
+ << "] -> " << " Prb->getTrajectory() [" << getProblem()->getTrajectory().get() << "] -> Trj [" << _trj_ptr.get() << "] Mismatch!\n";
+ log.assertTrue((_trj_ptr->getProblem()->getTrajectory().get() == _trj_ptr.get()), inconsistency_explanation);
+ return log;
+}
+bool TrajectoryBase::check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
+{
+ 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())
+ F->check(_log, F, _verbose, _stream, _tabs + " ");
+ return _log.is_consistent_;
+}
} // namespace wolf
diff --git a/src/tree_manager/tree_manager_sliding_window.cpp b/src/tree_manager/tree_manager_sliding_window.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5816567557bdd2ca5779615834a3f48a418bbabe
--- /dev/null
+++ b/src/tree_manager/tree_manager_sliding_window.cpp
@@ -0,0 +1,43 @@
+#include "core/tree_manager/tree_manager_sliding_window.h"
+
+namespace wolf
+{
+
+void TreeManagerSlidingWindow::keyFrameCallback(FrameBasePtr _key_frame)
+{
+ int n_kf(0);
+ FrameBasePtr first_KF(nullptr), second_KF(nullptr);
+ for (auto frm : getProblem()->getTrajectory()->getFrameList())
+ {
+ if (frm->isKey())
+ {
+ n_kf++;
+ if (first_KF == nullptr)
+ first_KF = frm;
+ else if (second_KF == nullptr)
+ second_KF = frm;
+ }
+ }
+
+ // remove first KF if too many KF
+ if (n_kf > params_sw_->n_key_frames)
+ {
+ WOLF_DEBUG("TreeManagerSlidingWindow removing first frame");
+ first_KF->remove(params_sw_->viral_remove_empty_parent);
+ if (params_sw_->fix_first_key_frame)
+ {
+ WOLF_DEBUG("TreeManagerSlidingWindow fixing new first frame");
+ second_KF->fix();
+ }
+ }
+}
+
+} /* namespace wolf */
+
+// Register in the FactoryTreeManager
+#include "core/tree_manager/factory_tree_manager.h"
+namespace wolf {
+WOLF_REGISTER_TREE_MANAGER(TreeManagerSlidingWindow);
+WOLF_REGISTER_TREE_MANAGER_AUTO(TreeManagerSlidingWindow);
+} // namespace wolf
+
diff --git a/src/yaml/parser_yaml.cpp b/src/yaml/parser_yaml.cpp
index 267d0c9da2024abd59ed5b529d13de7680d14472..2d1aa8200790d5d5484b7c24c1f8dc78304a3218 100644
--- a/src/yaml/parser_yaml.cpp
+++ b/src/yaml/parser_yaml.cpp
@@ -430,12 +430,12 @@ void ParserYAML::parseFirstLevel(std::string _file)
{
for (const auto& kv : n_config["ROS publisher"])
{
- WOLF_DEBUG("WHAT");
PublisherManager pPublisherManager = {
- kv["type"].Scalar(), kv["topic"].Scalar(), kv["period"].Scalar(), kv
+ kv["package"].Scalar(), kv["type"].Scalar(), kv["topic"].Scalar(), kv["period"].Scalar(), kv
};
publisher_managers_.push_back(pPublisherManager);
- map_container.push_back(std::map<std::string, std::string>({ { "type", kv["type"].Scalar() },
+ map_container.push_back(std::map<std::string, std::string>({ { "package", kv["package"].Scalar() },
+ { "type", kv["type"].Scalar() },
{ "topic", kv["topic"].Scalar() },
{ "period", kv["period"].Scalar() } }));
}
@@ -445,7 +445,7 @@ void ParserYAML::parseFirstLevel(std::string _file)
catch (YAML::InvalidNode& e)
{
throw std::runtime_error("Error parsing publisher @" + generatePath(_file) +
- ". Please make sure that each manager has 'type', 'topic' and 'period' entries.");
+ ". Please make sure that each manager has 'package', 'type', 'topic' and 'period' entries.");
}
}
std::map<std::string, std::string> ParserYAML::getParams()
@@ -475,19 +475,25 @@ void ParserYAML::parse()
tags.push_back("processor");
walkTreeR(it.n_, tags, "processor/" + it.name_);
}
- std::list<std::string> plugins, packages;
+ std::list<std::string> plugins;
+ std::list<std::string> packages_subscriber, packages_publisher;
for (const auto& it : paramsSens_)
plugins.push_back(it.plugin_);
for (const auto& it : paramsProc_)
plugins.push_back(it.plugin_);
for (const auto& it : subscriber_managers_)
- packages.push_back(it.package_);
+ packages_subscriber.push_back(it.package_);
+ for (const auto& it : publisher_managers_)
+ packages_publisher.push_back(it.package_);
plugins.sort();
plugins.unique();
- packages.sort();
- packages.unique();
+ packages_subscriber.sort();
+ packages_subscriber.unique();
+ packages_publisher.sort();
+ packages_publisher.unique();
insert_register("plugins", wolf::converter<std::string>::convert(plugins));
- insert_register("packages", wolf::converter<std::string>::convert(packages));
+ insert_register("packages_subscriber", wolf::converter<std::string>::convert(packages_subscriber));
+ insert_register("packages_publisher", wolf::converter<std::string>::convert(packages_publisher));
YAML::Node n;
n = loadYAML(generatePath(file_));
diff --git a/src/yaml/processor_odom_3d_yaml.cpp b/src/yaml/processor_odom_3d_yaml.cpp
index 1047ba5fd2482ab1f89c5a7256eb54b86f6181d6..0a0884454f126a46e169c5edc25d26b3bb04ef64 100644
--- a/src/yaml/processor_odom_3d_yaml.cpp
+++ b/src/yaml/processor_odom_3d_yaml.cpp
@@ -20,13 +20,13 @@ namespace wolf
namespace
{
-static ProcessorParamsBasePtr createProcessorOdom3dParams(const std::string & _filename_dot_yaml)
+static ParamsProcessorBasePtr createProcessorOdom3dParams(const std::string & _filename_dot_yaml)
{
YAML::Node config = YAML::LoadFile(_filename_dot_yaml);
if (config["type"].as<std::string>() == "ProcessorOdom3d")
{
- ProcessorParamsOdom3dPtr params = std::make_shared<ProcessorParamsOdom3d>();
+ ParamsProcessorOdom3dPtr params = std::make_shared<ParamsProcessorOdom3d>();
params->time_tolerance = config["time_tolerance"] .as<double>();
params->unmeasured_perturbation_std = config["unmeasured_perturbation_std"] .as<double>();
@@ -47,8 +47,8 @@ static ProcessorParamsBasePtr createProcessorOdom3dParams(const std::string & _f
return nullptr;
}
-// Register in the SensorFactory
-const bool WOLF_UNUSED registered_prc_odom_3d = ProcessorParamsFactory::get().registerCreator("ProcessorOdom3d", createProcessorOdom3dParams);
+// Register in the FactorySensor
+const bool WOLF_UNUSED registered_prc_odom_3d = FactoryParamsProcessor::get().registerCreator("ProcessorOdom3d", createProcessorOdom3dParams);
} // namespace [unnamed]
diff --git a/src/yaml/sensor_odom_2d_yaml.cpp b/src/yaml/sensor_odom_2d_yaml.cpp
index d59cd4decfb77338a0b3bb4ba17c607e03f847c1..b8ef1a083111a877b5b283c13d751df88a5af20d 100644
--- a/src/yaml/sensor_odom_2d_yaml.cpp
+++ b/src/yaml/sensor_odom_2d_yaml.cpp
@@ -39,8 +39,8 @@ static ParamsSensorBasePtr createParamsSensorOdom2d(const std::string & _filenam
return nullptr;
}
-// Register in the SensorFactory
-const bool WOLF_UNUSED registered_odom_2d_intr = ParamsSensorFactory::get().registerCreator("SensorOdom2d", createParamsSensorOdom2d);
+// Register in the FactorySensor
+const bool WOLF_UNUSED registered_odom_2d_intr = FactoryParamsSensor::get().registerCreator("SensorOdom2d", createParamsSensorOdom2d);
} // namespace [unnamed]
diff --git a/src/yaml/sensor_odom_3d_yaml.cpp b/src/yaml/sensor_odom_3d_yaml.cpp
index e437bedcbf00257068bfea07830caa9f8224e7c9..5b700eb8210b337d59f1186be90b869fe123874f 100644
--- a/src/yaml/sensor_odom_3d_yaml.cpp
+++ b/src/yaml/sensor_odom_3d_yaml.cpp
@@ -41,8 +41,8 @@ static ParamsSensorBasePtr createParamsSensorOdom3d(const std::string & _filenam
return nullptr;
}
-// Register in the SensorFactory
-const bool WOLF_UNUSED registered_odom_3d_intr = ParamsSensorFactory::get().registerCreator("SensorOdom3d", createParamsSensorOdom3d);
+// Register in the FactorySensor
+const bool WOLF_UNUSED registered_odom_3d_intr = FactoryParamsSensor::get().registerCreator("SensorOdom3d", createParamsSensorOdom3d);
} // namespace [unnamed]
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index db82caf1fc34b83b2509fe7d4f3ac4d6cc9e8c82..05717bad197c33e241146a8cf4d08fc337d99ea2 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -35,7 +35,8 @@ set(SRC_DUMMY
dummy/processor_tracker_feature_dummy.cpp
dummy/processor_tracker_landmark_dummy.cpp
)
-add_library(dummy ${SRC_DUMMY})
+add_library(dummy SHARED ${SRC_DUMMY})
+target_link_libraries(dummy ${PROJECT_NAME})
################# ADD YOUR TESTS BELOW ####################
# #
# ==== IN ALPHABETICAL ORDER! ==== #
@@ -47,6 +48,10 @@ add_library(dummy ${SRC_DUMMY})
wolf_add_gtest(gtest_capture_base gtest_capture_base.cpp)
target_link_libraries(gtest_capture_base ${PROJECT_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})
+
# FactorBase class test
wolf_add_gtest(gtest_factor_base gtest_factor_base.cpp)
target_link_libraries(gtest_factor_base ${PROJECT_NAME})
@@ -55,9 +60,9 @@ target_link_libraries(gtest_factor_base ${PROJECT_NAME})
wolf_add_gtest(gtest_factor_autodiff gtest_factor_autodiff.cpp)
target_link_libraries(gtest_factor_autodiff ${PROJECT_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})
+# FactoryStateBlock factory test
+wolf_add_gtest(gtest_factory_state_block gtest_factory_state_block.cpp)
+target_link_libraries(gtest_factory_state_block ${PROJECT_NAME})
# FeatureBase classes test
wolf_add_gtest(gtest_eigen_predicates gtest_eigen_predicates.cpp)
@@ -120,6 +125,7 @@ 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})
# SensorBase test
wolf_add_gtest(gtest_sensor_base gtest_sensor_base.cpp)
@@ -137,6 +143,10 @@ target_link_libraries(gtest_solver_manager ${PROJECT_NAME})
wolf_add_gtest(gtest_state_block gtest_state_block.cpp)
target_link_libraries(gtest_state_block ${PROJECT_NAME})
+# StateBlock class test
+wolf_add_gtest(gtest_state_composite gtest_state_composite.cpp)
+target_link_libraries(gtest_state_composite ${PROJECT_NAME})
+
# TimeStamp class test
wolf_add_gtest(gtest_time_stamp gtest_time_stamp.cpp)
target_link_libraries(gtest_time_stamp ${PROJECT_NAME})
@@ -149,6 +159,10 @@ target_link_libraries(gtest_track_matrix ${PROJECT_NAME})
wolf_add_gtest(gtest_trajectory gtest_trajectory.cpp)
target_link_libraries(gtest_trajectory ${PROJECT_NAME})
+# TreeManager class test
+wolf_add_gtest(gtest_tree_manager gtest_tree_manager.cpp)
+target_link_libraries(gtest_tree_manager ${PROJECT_NAME})
+
# ------- Now Derived classes ----------
IF (Ceres_FOUND)
@@ -173,6 +187,10 @@ target_link_libraries(gtest_factor_block_difference ${PROJECT_NAME})
wolf_add_gtest(gtest_factor_diff_drive gtest_factor_diff_drive.cpp)
target_link_libraries(gtest_factor_diff_drive ${PROJECT_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})
+
# FactorOdom3d class test
wolf_add_gtest(gtest_factor_odom_3d gtest_factor_odom_3d.cpp)
target_link_libraries(gtest_factor_odom_3d ${PROJECT_NAME})
@@ -185,6 +203,14 @@ target_link_libraries(gtest_factor_pose_2d ${PROJECT_NAME})
wolf_add_gtest(gtest_factor_pose_3d gtest_factor_pose_3d.cpp)
target_link_libraries(gtest_factor_pose_3d ${PROJECT_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})
+
+# 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})
+
# MakePosDef function test
wolf_add_gtest(gtest_make_posdef gtest_make_posdef.cpp)
target_link_libraries(gtest_make_posdef ${PROJECT_NAME})
@@ -229,6 +255,10 @@ target_link_libraries(gtest_processor_tracker_landmark_dummy ${PROJECT_NAME} dum
wolf_add_gtest(gtest_sensor_diff_drive gtest_sensor_diff_drive.cpp)
target_link_libraries(gtest_sensor_diff_drive ${PROJECT_NAME})
+# 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})
+
# yaml conversions
wolf_add_gtest(gtest_yaml_conversions gtest_yaml_conversions.cpp)
target_link_libraries(gtest_yaml_conversions ${PROJECT_NAME})
diff --git a/test/dummy/factor_dummy_zero_1.h b/test/dummy/factor_dummy_zero_1.h
new file mode 100644
index 0000000000000000000000000000000000000000..1ebe1b8fe2e4eba79fe2c9fbf236da5844327189
--- /dev/null
+++ b/test/dummy/factor_dummy_zero_1.h
@@ -0,0 +1,49 @@
+#ifndef FACTOR_DUMMY_ZERO_1_H_
+#define FACTOR_DUMMY_ZERO_1_H_
+
+//Wolf includes
+#include "core/factor/factor_autodiff.h"
+
+//#include "ceres/jet.h"
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(FactorDummyZero1);
+
+//class
+class FactorDummyZero1 : public FactorAutodiff<FactorDummyZero1, 1, 1>
+{
+ using Base = FactorAutodiff<FactorDummyZero1, 1, 1>;
+ public:
+ FactorDummyZero1(const FeatureBasePtr& _ftr_ptr,
+ const StateBlockPtr& _sb_ptr) :
+ Base("FactorDummy1",
+ _ftr_ptr,
+ nullptr, nullptr, nullptr, nullptr,
+ nullptr,
+ false,
+ FAC_ACTIVE,
+ _sb_ptr)
+ {
+ //
+ }
+
+ virtual ~FactorDummyZero1() = default;
+
+ virtual std::string getTopology() const override
+ {
+ return std::string("MOTION");
+ }
+
+ template<typename T>
+ bool operator ()(const T* const _st1,
+ T* _residuals) const
+ {
+ _residuals[0] = _st1[0];
+ return true;
+ }
+};
+
+} // namespace wolf
+
+#endif
diff --git a/test/dummy/factor_dummy_zero_12.h b/test/dummy/factor_dummy_zero_12.h
new file mode 100644
index 0000000000000000000000000000000000000000..c60b9ce3232aa5c18807e80fbfb766cde275c953
--- /dev/null
+++ b/test/dummy/factor_dummy_zero_12.h
@@ -0,0 +1,159 @@
+#ifndef FACTOR_DUMMY_ZERO_12_H_
+#define FACTOR_DUMMY_ZERO_12_H_
+
+//Wolf includes
+#include "core/factor/factor_autodiff.h"
+
+//#include "ceres/jet.h"
+
+namespace wolf {
+
+//class
+template <unsigned int ID>
+class FactorDummyZero12 : public FactorAutodiff<FactorDummyZero12<ID>, ID, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12>
+{
+ using Base = FactorAutodiff<FactorDummyZero12<ID>, ID, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12>;
+ static const unsigned int id = ID;
+ public:
+ FactorDummyZero12(const FeatureBasePtr& _ftr_ptr,
+ const StateBlockPtr& _sb1_ptr,
+ const StateBlockPtr& _sb2_ptr,
+ const StateBlockPtr& _sb3_ptr,
+ const StateBlockPtr& _sb4_ptr,
+ const StateBlockPtr& _sb5_ptr,
+ const StateBlockPtr& _sb6_ptr,
+ const StateBlockPtr& _sb7_ptr,
+ const StateBlockPtr& _sb8_ptr,
+ const StateBlockPtr& _sb9_ptr,
+ const StateBlockPtr& _sb10_ptr,
+ const StateBlockPtr& _sb11_ptr,
+ const StateBlockPtr& _sb12_ptr) :
+ Base("FactorDummy12",
+ _ftr_ptr,
+ nullptr, nullptr, nullptr, nullptr,
+ nullptr,
+ false,
+ FAC_ACTIVE,
+ _sb1_ptr,
+ _sb2_ptr,
+ _sb3_ptr,
+ _sb4_ptr,
+ _sb5_ptr,
+ _sb6_ptr,
+ _sb7_ptr,
+ _sb8_ptr,
+ _sb9_ptr,
+ _sb10_ptr,
+ _sb11_ptr,
+ _sb12_ptr)
+ {
+ assert(id > 0 && id <= 12);
+ }
+
+ virtual ~FactorDummyZero12() = default;
+
+ virtual std::string getTopology() const override
+ {
+ return std::string("MOTION");
+ }
+
+ template<typename T>
+ bool operator ()(const T* const _st1,
+ const T* const _st2,
+ const T* const _st3,
+ const T* const _st4,
+ const T* const _st5,
+ const T* const _st6,
+ const T* const _st7,
+ const T* const _st8,
+ const T* const _st9,
+ const T* const _st10,
+ const T* const _st11,
+ const T* const _st12,
+ T* _residuals) const
+ {
+ Eigen::Map<Eigen::Matrix<T,ID,1>> residuals(_residuals);
+ switch (id)
+ {
+ case 1:
+ {
+ Eigen::Map<const Eigen::Matrix<T,ID,1>> st1(_st1);
+ residuals = st1;
+ break;
+ }
+ case 2:
+ {
+ Eigen::Map<const Eigen::Matrix<T,ID,1>> st2(_st2);
+ residuals = st2;
+ break;
+ }
+ case 3:
+ {
+ Eigen::Map<const Eigen::Matrix<T,ID,1>> st3(_st3);
+ residuals = st3;
+ break;
+ }
+ case 4:
+ {
+ Eigen::Map<const Eigen::Matrix<T,ID,1>> st4(_st4);
+ residuals = st4;
+ break;
+ }
+ case 5:
+ {
+ Eigen::Map<const Eigen::Matrix<T,ID,1>> st5(_st5);
+ residuals = st5;
+ break;
+ }
+ case 6:
+ {
+ Eigen::Map<const Eigen::Matrix<T,ID,1>> st6(_st6);
+ residuals = st6;
+ break;
+ }
+ case 7:
+ {
+ Eigen::Map<const Eigen::Matrix<T,ID,1>> st7(_st7);
+ residuals = st7;
+ break;
+ }
+ case 8:
+ {
+ Eigen::Map<const Eigen::Matrix<T,ID,1>> st8(_st8);
+ residuals = st8;
+ break;
+ }
+ case 9:
+ {
+ Eigen::Map<const Eigen::Matrix<T,ID,1>> st9(_st9);
+ residuals = st9;
+ break;
+ }
+ case 10:
+ {
+ Eigen::Map<const Eigen::Matrix<T,ID,1>> st10(_st10);
+ residuals = st10;
+ break;
+ }
+ case 11:
+ {
+ Eigen::Map<const Eigen::Matrix<T,ID,1>> st11(_st11);
+ residuals = st11;
+ break;
+ }
+ case 12:
+ {
+ Eigen::Map<const Eigen::Matrix<T,ID,1>> st12(_st12);
+ residuals = st12;
+ break;
+ }
+ default:
+ throw std::runtime_error("ID not found");
+ }
+ return true;
+ }
+};
+
+} // namespace wolf
+
+#endif
diff --git a/test/dummy/factor_feature_dummy.h b/test/dummy/factor_feature_dummy.h
index 6b31ffa63a398dc166a4e51fc552352f05a28390..48c7d66ff9482ed55df9f166374948a0d07ef672 100644
--- a/test/dummy/factor_feature_dummy.h
+++ b/test/dummy/factor_feature_dummy.h
@@ -49,23 +49,36 @@ class FactorFeatureDummy : public FactorBase
public:
static FactorBasePtr create(const FeatureBasePtr& _feature_ptr,
- const NodeBasePtr& _correspondant_ptr,
- const ProcessorBasePtr& _processor_ptr = nullptr);
+ const NodeBasePtr& _correspondant_ptr,
+ const ProcessorBasePtr& _processor_ptr = nullptr);
};
-inline FactorFeatureDummy::FactorFeatureDummy(const FeatureBasePtr& /*_feature_ptr*/, const FeatureBasePtr& _feature_other_ptr,
+inline FactorFeatureDummy::FactorFeatureDummy(const FeatureBasePtr& _feature_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function, FactorStatus _status) :
- FactorBase("FactorFeatureDummy", nullptr, nullptr, _feature_other_ptr, nullptr, _processor_ptr, _apply_loss_function, _status)
+ bool _apply_loss_function,
+ FactorStatus _status) :
+ FactorBase("FactorFeatureDummy",
+ _feature_ptr,
+ nullptr,
+ nullptr,
+ _feature_other_ptr,
+ nullptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status)
{
//
}
-inline FactorBasePtr FactorFeatureDummy::create(const FeatureBasePtr& _feature_ptr, const NodeBasePtr& _correspondant_ptr,
- const ProcessorBasePtr& _processor_ptr)
+inline FactorBasePtr FactorFeatureDummy::create(const FeatureBasePtr& _feature_ptr,
+ const NodeBasePtr& _correspondant_ptr,
+ const ProcessorBasePtr& _processor_ptr)
{
- return std::make_shared<FactorFeatureDummy>(_feature_ptr, std::static_pointer_cast<FeatureBase>(_correspondant_ptr), _processor_ptr);
+ return std::make_shared<FactorFeatureDummy>(_feature_ptr,
+ std::static_pointer_cast<FeatureBase>(_correspondant_ptr),
+ _processor_ptr);
}
} // namespace wolf
diff --git a/test/dummy/factor_landmark_dummy.h b/test/dummy/factor_landmark_dummy.h
index b4db44b7890f73289d54cb2708c85edb8a1b8b46..8adcfff4ec7891cee87da0fe0b70fd0bb15e0065 100644
--- a/test/dummy/factor_landmark_dummy.h
+++ b/test/dummy/factor_landmark_dummy.h
@@ -25,11 +25,15 @@ class FactorLandmarkDummy : public FactorBase
/** \brief Evaluate the factor given the input parameters and returning the residuals and jacobians
**/
- virtual bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override {return true;};
+ virtual bool evaluate(double const* const* parameters,
+ double* residuals,
+ double** jacobians) const override {return true;};
/** Returns a residual vector and a vector of Jacobian matrix corresponding to each state block evaluated in the point provided in _states_ptr
**/
- virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override {};
+ virtual void evaluate(const std::vector<const double*>& _states_ptr,
+ Eigen::VectorXd& residual_,
+ std::vector<Eigen::MatrixXd>& jacobians_) const override {};
/** \brief Returns the jacobians computation method
**/
@@ -37,7 +41,10 @@ class FactorLandmarkDummy : public FactorBase
/** \brief Returns a vector of pointers to the states in which this factor depends
**/
- virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const override {return std::vector<StateBlockPtr>(0);}
+ virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const override
+ {
+ return std::vector<StateBlockPtr>(0);
+ }
/** \brief Returns the factor residual size
**/
@@ -45,7 +52,10 @@ class FactorLandmarkDummy : public FactorBase
/** \brief Returns the factor states sizes
**/
- virtual std::vector<unsigned int> getStateSizes() const override {return std::vector<unsigned int>({1});}
+ virtual std::vector<unsigned int> getStateSizes() const override
+ {
+ return std::vector<unsigned int>({1});
+ }
public:
static FactorBasePtr create(const FeatureBasePtr& _feature_ptr,
@@ -54,18 +64,31 @@ class FactorLandmarkDummy : public FactorBase
};
-inline FactorLandmarkDummy::FactorLandmarkDummy(const FeatureBasePtr& /*_feature_ptr*/, const LandmarkBasePtr& _landmark_other_ptr,
+inline FactorLandmarkDummy::FactorLandmarkDummy(const FeatureBasePtr& _feature_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function, FactorStatus _status) :
- FactorBase("FactorFeatureDummy", nullptr, nullptr, nullptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status)
+ bool _apply_loss_function,
+ FactorStatus _status) :
+ FactorBase("FactorFeatureDummy",
+ _feature_ptr,
+ nullptr,
+ nullptr,
+ nullptr,
+ _landmark_other_ptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status)
{
//
}
-inline FactorBasePtr FactorLandmarkDummy::create(const FeatureBasePtr& _feature_ptr, const NodeBasePtr& _correspondant_ptr,
+inline FactorBasePtr FactorLandmarkDummy::create(const FeatureBasePtr& _feature_ptr,
+ const NodeBasePtr& _correspondant_ptr,
const ProcessorBasePtr& _processor_ptr)
{
- return std::make_shared<FactorLandmarkDummy>(_feature_ptr, std::static_pointer_cast<LandmarkBase>(_correspondant_ptr), _processor_ptr);
+ return std::make_shared<FactorLandmarkDummy>(_feature_ptr,
+ std::static_pointer_cast<LandmarkBase>(_correspondant_ptr),
+ _processor_ptr);
}
} // namespace wolf
diff --git a/test/dummy/processor_tracker_feature_dummy.cpp b/test/dummy/processor_tracker_feature_dummy.cpp
index 3a30e98af0e409fb0fc8604231d7bee16a0630e6..8485d267034febde4f08911108c0773fd67ab892 100644
--- a/test/dummy/processor_tracker_feature_dummy.cpp
+++ b/test/dummy/processor_tracker_feature_dummy.cpp
@@ -58,6 +58,7 @@ unsigned int ProcessorTrackerFeatureDummy::detectNewFeatures(const int& _max_new
FeatureBasePtrList& _features_out)
{
unsigned int max_features = _max_new_features;
+ WOLF_INFO("Detecting " , _max_new_features , " new features..." );
if (max_features == -1)
{
@@ -70,7 +71,7 @@ unsigned int ProcessorTrackerFeatureDummy::detectNewFeatures(const int& _max_new
for (unsigned int i = 0; i < max_features; i++)
{
FeatureBasePtr ftr = FeatureBase::emplace<FeatureBase>(_capture,
- "DUMMY FEATURE",
+ "FeatureDummy",
Eigen::Vector1d::Ones(),
Eigen::MatrixXd::Ones(1, 1));
_features_out.push_back(ftr);
@@ -92,26 +93,11 @@ FactorBasePtr ProcessorTrackerFeatureDummy::emplaceFactor(FeatureBasePtr _featur
return FactorBase::emplace<FactorFeatureDummy>(_feature_ptr, _feature_ptr, _feature_other_ptr, shared_from_this());
}
-ProcessorBasePtr ProcessorTrackerFeatureDummy::create(const std::string& _unique_name,
- const ProcessorParamsBasePtr _params)
-{
- auto params = std::static_pointer_cast<ProcessorParamsTrackerFeatureDummy>(_params);
-
- // if cast failed use default value
- if (params == nullptr)
- params = std::make_shared<ProcessorParamsTrackerFeatureDummy>();
-
- auto prc_ptr = std::make_shared<ProcessorTrackerFeatureDummy>(params);
-
- prc_ptr->setName(_unique_name);
-
- return prc_ptr;
-}
-
} // namespace wolf
-// Register in the ProcessorFactory
-#include "core/processor/processor_factory.h"
+// Register in the FactoryProcessor
+#include "core/processor/factory_processor.h"
namespace wolf {
-WOLF_REGISTER_PROCESSOR("ProcessorTrackerFeatureDummy", ProcessorTrackerFeatureDummy)
+WOLF_REGISTER_PROCESSOR(ProcessorTrackerFeatureDummy)
+WOLF_REGISTER_PROCESSOR_AUTO(ProcessorTrackerFeatureDummy)
} // namespace wolf
diff --git a/test/dummy/processor_tracker_feature_dummy.h b/test/dummy/processor_tracker_feature_dummy.h
index 3328edf08c0234d6b587680b9b215bc91a7ae43f..d06ac67503acfda0a55d9418bac339359b3d9625 100644
--- a/test/dummy/processor_tracker_feature_dummy.h
+++ b/test/dummy/processor_tracker_feature_dummy.h
@@ -14,16 +14,17 @@
namespace wolf
{
-
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTrackerFeatureDummy);
-struct ProcessorParamsTrackerFeatureDummy : public ProcessorParamsTrackerFeature
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorTrackerFeatureDummy);
+
+struct ParamsProcessorTrackerFeatureDummy : public ParamsProcessorTrackerFeature
{
unsigned int n_tracks_lost; ///< number of tracks lost each time track is called (the first ones)
- ProcessorParamsTrackerFeatureDummy() = default;
- ProcessorParamsTrackerFeatureDummy(std::string _unique_name, const wolf::ParamsServer & _server):
- ProcessorParamsTrackerFeature(_unique_name, _server)
+ ParamsProcessorTrackerFeatureDummy() = default;
+ ParamsProcessorTrackerFeatureDummy(std::string _unique_name,
+ const wolf::ParamsServer & _server):
+ ParamsProcessorTrackerFeature(_unique_name, _server)
{
n_tracks_lost = _server.getParam<unsigned int>(prefix + _unique_name + "/n_tracks_lost");
}
@@ -31,19 +32,23 @@ struct ProcessorParamsTrackerFeatureDummy : public ProcessorParamsTrackerFeature
WOLF_PTR_TYPEDEFS(ProcessorTrackerFeatureDummy);
-
+
//Class
class ProcessorTrackerFeatureDummy : public ProcessorTrackerFeature
{
public:
- ProcessorTrackerFeatureDummy(ProcessorParamsTrackerFeatureDummyPtr _params_tracker_feature);
+ ProcessorTrackerFeatureDummy(ParamsProcessorTrackerFeatureDummyPtr _params_tracker_feature);
virtual ~ProcessorTrackerFeatureDummy();
- virtual void configure(SensorBasePtr _sensor) { };
+
+ // Factory method for high level API
+ WOLF_PROCESSOR_CREATE(ProcessorTrackerFeatureDummy, ParamsProcessorTrackerFeatureDummy);
+
+ virtual void configure(SensorBasePtr _sensor) override { };
protected:
- ProcessorParamsTrackerFeatureDummyPtr params_tracker_feature_dummy_;
+ ParamsProcessorTrackerFeatureDummyPtr params_tracker_feature_dummy_;
/** \brief Track provided features in \b _capture
* \param _features_in input list of features in \b last to track
@@ -59,14 +64,16 @@ class ProcessorTrackerFeatureDummy : public ProcessorTrackerFeature
virtual unsigned int trackFeatures(const FeatureBasePtrList& _features_in,
const CaptureBasePtr& _capture,
FeatureBasePtrList& _features_out,
- FeatureMatchMap& _feature_correspondences);
+ FeatureMatchMap& _feature_correspondences) override;
/** \brief Correct the drift in incoming feature by re-comparing against the corresponding feature in origin.
* \param _last_feature input feature in last capture tracked
* \param _incoming_feature input/output feature in incoming capture to be corrected
* \return false if the the process discards the correspondence with origin's feature
*/
- virtual bool correctFeatureDrift(const FeatureBasePtr _origin_feature, const FeatureBasePtr _last_feature, FeatureBasePtr _incoming_feature);
+ virtual bool correctFeatureDrift(const FeatureBasePtr _origin_feature,
+ const FeatureBasePtr _last_feature,
+ FeatureBasePtr _incoming_feature) override;
/** \brief Vote for KeyFrame generation
*
@@ -75,7 +82,7 @@ class ProcessorTrackerFeatureDummy : public ProcessorTrackerFeature
*
* WARNING! This function only votes! It does not create KeyFrames!
*/
- virtual bool voteForKeyFrame() const;
+ virtual bool voteForKeyFrame() const override;
/** \brief Detect new Features
* \param _max_features maximum number of features detected (-1: unlimited. 0: none)
@@ -93,24 +100,19 @@ class ProcessorTrackerFeatureDummy : public ProcessorTrackerFeature
*/
virtual unsigned int detectNewFeatures(const int& _max_new_features,
const CaptureBasePtr& _capture,
- FeatureBasePtrList& _features_out);
+ FeatureBasePtrList& _features_out) override;
/** \brief Emplaces a new factor
* \param _feature_ptr pointer to the parent Feature
* \param _feature_other_ptr pointer to the other feature constrained.
*
* This function emplaces a factor of the appropriate type for the derived processor.
*/
- virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr);
-
- public:
-
- static ProcessorBasePtr create(const std::string& _unique_name,
- const ProcessorParamsBasePtr _params);
-
+ virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_ptr,
+ FeatureBasePtr _feature_other_ptr) override;
};
-inline ProcessorTrackerFeatureDummy::ProcessorTrackerFeatureDummy(ProcessorParamsTrackerFeatureDummyPtr _params_tracker_feature_dummy) :
- ProcessorTrackerFeature("ProcessorTrackerFeatureDummy", 0, _params_tracker_feature_dummy),
+inline ProcessorTrackerFeatureDummy::ProcessorTrackerFeatureDummy(ParamsProcessorTrackerFeatureDummyPtr _params_tracker_feature_dummy) :
+ ProcessorTrackerFeature("ProcessorTrackerFeatureDummy", "PO", 0, _params_tracker_feature_dummy),
params_tracker_feature_dummy_(_params_tracker_feature_dummy)
{
//
diff --git a/test/dummy/processor_tracker_landmark_dummy.cpp b/test/dummy/processor_tracker_landmark_dummy.cpp
index 72fc32f9baa1ce71216e6457d63185e057bcdc40..d36efd3f804cd71ff38fc7b835dc1b5230c59885 100644
--- a/test/dummy/processor_tracker_landmark_dummy.cpp
+++ b/test/dummy/processor_tracker_landmark_dummy.cpp
@@ -12,8 +12,8 @@
namespace wolf
{
-ProcessorTrackerLandmarkDummy::ProcessorTrackerLandmarkDummy(ProcessorParamsTrackerLandmarkDummyPtr _params_tracker_landmark_dummy) :
- ProcessorTrackerLandmark("ProcessorTrackerLandmarkDummy", _params_tracker_landmark_dummy),
+ProcessorTrackerLandmarkDummy::ProcessorTrackerLandmarkDummy(ParamsProcessorTrackerLandmarkDummyPtr _params_tracker_landmark_dummy) :
+ ProcessorTrackerLandmark("ProcessorTrackerLandmarkDummy", "PO", _params_tracker_landmark_dummy),
params_tracker_landmark_dummy_(_params_tracker_landmark_dummy)
{
//
@@ -101,10 +101,14 @@ unsigned int ProcessorTrackerLandmarkDummy::detectNewFeatures(const int& _max_fe
LandmarkBasePtr ProcessorTrackerLandmarkDummy::emplaceLandmark(FeatureBasePtr _feature_ptr)
{
//std::cout << "ProcessorTrackerLandmarkDummy::emplaceLandmark" << std::endl;
- return LandmarkBase::emplace<LandmarkBase>(getProblem()->getMap(), "LandmarkBase", std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1));
+ return LandmarkBase::emplace<LandmarkBase>(getProblem()->getMap(),
+ "LandmarkBase",
+ std::make_shared<StateBlock>(2),
+ std::make_shared<StateBlock>(1));
}
-FactorBasePtr ProcessorTrackerLandmarkDummy::emplaceFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr)
+FactorBasePtr ProcessorTrackerLandmarkDummy::emplaceFactor(FeatureBasePtr _feature_ptr,
+ LandmarkBasePtr _landmark_ptr)
{
std::cout << "\tProcessorTrackerLandmarkDummy::emplaceFactor" << std::endl;
std::cout << "\t\tfeature " << _feature_ptr->id() << std::endl;
@@ -112,4 +116,12 @@ FactorBasePtr ProcessorTrackerLandmarkDummy::emplaceFactor(FeatureBasePtr _featu
return FactorBase::emplace<FactorLandmarkDummy>(_feature_ptr, _feature_ptr, _landmark_ptr, shared_from_this());
}
-} //namespace wolf
+} // namespace wolf
+
+// Register in the FactoryProcessor
+#include "core/processor/factory_processor.h"
+namespace wolf {
+WOLF_REGISTER_PROCESSOR(ProcessorTrackerLandmarkDummy)
+WOLF_REGISTER_PROCESSOR_AUTO(ProcessorTrackerLandmarkDummy)
+} // namespace wolf
+
diff --git a/test/dummy/processor_tracker_landmark_dummy.h b/test/dummy/processor_tracker_landmark_dummy.h
index a9531c8db7a0372636c1d0cb605c21c38aec8446..5140a2b2762db740944019ac1595e7d37bf07a2c 100644
--- a/test/dummy/processor_tracker_landmark_dummy.h
+++ b/test/dummy/processor_tracker_landmark_dummy.h
@@ -13,15 +13,15 @@
namespace wolf
{
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTrackerLandmarkDummy);
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorTrackerLandmarkDummy);
-struct ProcessorParamsTrackerLandmarkDummy : public ProcessorParamsTrackerLandmark
+struct ParamsProcessorTrackerLandmarkDummy : public ParamsProcessorTrackerLandmark
{
unsigned int n_landmarks_lost; ///< number of landmarks lost each time findLandmarks is called (the last ones)
- ProcessorParamsTrackerLandmarkDummy() = default;
- ProcessorParamsTrackerLandmarkDummy(std::string _unique_name, const wolf::ParamsServer & _server):
- ProcessorParamsTrackerLandmark(_unique_name, _server)
+ ParamsProcessorTrackerLandmarkDummy() = default;
+ ParamsProcessorTrackerLandmarkDummy(std::string _unique_name, const wolf::ParamsServer & _server):
+ ParamsProcessorTrackerLandmark(_unique_name, _server)
{
n_landmarks_lost = _server.getParam<unsigned int>(prefix + _unique_name + "/n_landmarks_lost");
}
@@ -32,13 +32,17 @@ WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmarkDummy);
class ProcessorTrackerLandmarkDummy : public ProcessorTrackerLandmark
{
public:
- ProcessorTrackerLandmarkDummy(ProcessorParamsTrackerLandmarkDummyPtr _params_tracker_landmark_dummy);
+ ProcessorTrackerLandmarkDummy(ParamsProcessorTrackerLandmarkDummyPtr _params_tracker_landmark_dummy);
virtual ~ProcessorTrackerLandmarkDummy();
- virtual void configure(SensorBasePtr _sensor) { };
+
+ // Factory method for high level API
+ WOLF_PROCESSOR_CREATE(ProcessorTrackerLandmarkDummy, ParamsProcessorTrackerLandmarkDummy);
+
+ virtual void configure(SensorBasePtr _sensor) override { };
protected:
- ProcessorParamsTrackerLandmarkDummyPtr params_tracker_landmark_dummy_;
+ ParamsProcessorTrackerLandmarkDummyPtr params_tracker_landmark_dummy_;
/** \brief Find provided landmarks as features in the provided capture
* \param _landmarks_in input list of landmarks to be found
@@ -54,7 +58,7 @@ class ProcessorTrackerLandmarkDummy : public ProcessorTrackerLandmark
virtual unsigned int findLandmarks(const LandmarkBasePtrList& _landmarks_in,
const CaptureBasePtr& _capture,
FeatureBasePtrList& _features_out,
- LandmarkMatchMap& _feature_landmark_correspondences);
+ LandmarkMatchMap& _feature_landmark_correspondences) override;
/** \brief Vote for KeyFrame generation
*
@@ -63,7 +67,7 @@ class ProcessorTrackerLandmarkDummy : public ProcessorTrackerLandmark
*
* WARNING! This function only votes! It does not create KeyFrames!
*/
- virtual bool voteForKeyFrame() const;
+ virtual bool voteForKeyFrame() const override;
/** \brief Detect new Features
* \param _max_features maximum number of features detected (-1: unlimited. 0: none)
@@ -81,13 +85,13 @@ class ProcessorTrackerLandmarkDummy : public ProcessorTrackerLandmark
*/
virtual unsigned int detectNewFeatures(const int& _max_new_features,
const CaptureBasePtr& _capture,
- FeatureBasePtrList& _features_out);
+ FeatureBasePtrList& _features_out) override;
/** \brief Emplace one landmark
*
* Implement in derived classes to build the type of landmark you need for this tracker.
*/
- virtual LandmarkBasePtr emplaceLandmark(FeatureBasePtr _feature_ptr);
+ virtual LandmarkBasePtr emplaceLandmark(FeatureBasePtr _feature_ptr) override;
/** \brief Emplace a new factor
* \param _feature_ptr pointer to the Feature to constrain
@@ -95,7 +99,8 @@ class ProcessorTrackerLandmarkDummy : public ProcessorTrackerLandmark
*
* Implement this method in derived classes.
*/
- virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr);
+ virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_ptr,
+ LandmarkBasePtr _landmark_ptr) override;
};
} // namespace wolf
diff --git a/test/dummy/solver_manager_dummy.h b/test/dummy/solver_manager_dummy.h
new file mode 100644
index 0000000000000000000000000000000000000000..fe62d065c64fbf2f4c37f58b00ff4f51ea2a0ef1
--- /dev/null
+++ b/test/dummy/solver_manager_dummy.h
@@ -0,0 +1,104 @@
+/*
+ * solver_manager_dummy.h
+ *
+ * Created on: May 27, 2020
+ * Author: joanvallve
+ */
+
+#ifndef TEST_DUMMY_SOLVER_MANAGER_DUMMY_H_
+#define TEST_DUMMY_SOLVER_MANAGER_DUMMY_H_
+
+#include "core/solver/solver_manager.h"
+
+namespace wolf
+{
+
+WOLF_PTR_TYPEDEFS(SolverManagerDummy);
+class SolverManagerDummy : public SolverManager
+{
+ public:
+ std::list<FactorBasePtr> factors_;
+ std::map<StateBlockPtr,bool> state_block_fixed_;
+ std::map<StateBlockPtr,LocalParametrizationBasePtr> state_block_local_param_;
+
+ SolverManagerDummy(const ProblemPtr& wolf_problem) :
+ SolverManager(wolf_problem)
+ {
+ };
+
+ bool isStateBlockRegistered(const StateBlockPtr& st) override
+ {
+ return state_blocks_.find(st)!=state_blocks_.end();
+ };
+
+ bool isStateBlockFixed(const StateBlockPtr& st) const
+ {
+ return state_block_fixed_.at(st);
+ };
+
+ bool isFactorRegistered(const FactorBasePtr& fac_ptr) const override
+ {
+ return std::find(factors_.begin(), factors_.end(), fac_ptr) != factors_.end();
+ };
+
+ bool hasThisLocalParametrization(const StateBlockPtr& st, const LocalParametrizationBasePtr& local_param) const
+ {
+ return state_block_local_param_.find(st) != state_block_local_param_.end() && state_block_local_param_.at(st) == local_param;
+ };
+
+ bool hasLocalParametrization(const StateBlockPtr& st) const
+ {
+ return state_block_local_param_.find(st) != state_block_local_param_.end();
+ };
+
+ virtual void computeCovariances(const CovarianceBlocksToBeComputed blocks) override {};
+ virtual void computeCovariances(const std::vector<StateBlockPtr>& st_list) override {};
+ virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) override {return true;};
+ virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const override {return true;};
+
+ // The following are dummy implementations
+ bool hasConverged() override { return true; }
+ SizeStd iterations() override { return 1; }
+ double initialCost() override { return double(1); }
+ double finalCost() override { return double(0); }
+
+
+
+ protected:
+
+ virtual bool checkDerived(std::string prefix="") const override {return true;}
+ virtual std::string solveDerived(const ReportVerbosity report_level) override { return std::string("");};
+ virtual void addFactorDerived(const FactorBasePtr& fac_ptr) override
+ {
+ factors_.push_back(fac_ptr);
+ };
+ virtual void removeFactorDerived(const FactorBasePtr& fac_ptr) override
+ {
+ factors_.remove(fac_ptr);
+ };
+ virtual void addStateBlockDerived(const StateBlockPtr& state_ptr) override
+ {
+ state_block_fixed_[state_ptr] = state_ptr->isFixed();
+ state_block_local_param_[state_ptr] = state_ptr->getLocalParametrization();
+ };
+ virtual void removeStateBlockDerived(const StateBlockPtr& state_ptr) override
+ {
+ state_block_fixed_.erase(state_ptr);
+ state_block_local_param_.erase(state_ptr);
+ };
+ virtual void updateStateBlockStatusDerived(const StateBlockPtr& state_ptr) override
+ {
+ state_block_fixed_[state_ptr] = state_ptr->isFixed();
+ };
+ virtual 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();
+ };
+};
+
+}
+
+#endif /* TEST_DUMMY_SOLVER_MANAGER_DUMMY_H_ */
diff --git a/test/dummy/tree_manager_dummy.h b/test/dummy/tree_manager_dummy.h
new file mode 100644
index 0000000000000000000000000000000000000000..ce8e6379ece21e6a5422445b183f0576a88cc720
--- /dev/null
+++ b/test/dummy/tree_manager_dummy.h
@@ -0,0 +1,61 @@
+#ifndef INCLUDE_TREE_MANAGER_DUMMY_H_
+#define INCLUDE_TREE_MANAGER_DUMMY_H_
+
+#include "core/tree_manager/tree_manager_base.h"
+
+namespace wolf
+{
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsTreeManagerDummy)
+struct ParamsTreeManagerDummy : public ParamsTreeManagerBase
+{
+ ParamsTreeManagerDummy() = default;
+ ParamsTreeManagerDummy(std::string _unique_name, const ParamsServer& _server):
+ ParamsTreeManagerBase(_unique_name, _server)
+ {
+ toy_param = _server.getParam<double>(prefix + "/toy_param");
+ }
+
+ virtual ~ParamsTreeManagerDummy() = default;
+
+ bool toy_param;
+
+ std::string print() const
+ {
+ return ParamsTreeManagerBase::print() + "\n"
+ + "toy_param: " + std::to_string(toy_param) + "\n";
+ }
+};
+
+WOLF_PTR_TYPEDEFS(TreeManagerDummy)
+
+class TreeManagerDummy : public TreeManagerBase
+{
+ public:
+ TreeManagerDummy(ParamsTreeManagerBasePtr _params) :
+ TreeManagerBase("TreeManagerDummy", _params),
+ n_KF_(0)
+ {};
+ WOLF_TREE_MANAGER_CREATE(TreeManagerDummy, ParamsTreeManagerBase)
+
+ virtual ~TreeManagerDummy(){}
+
+ virtual void keyFrameCallback(FrameBasePtr _KF) override
+ {
+ n_KF_++;
+ };
+
+ int n_KF_;
+};
+
+} /* namespace wolf */
+
+// Register in the FactoryTreeManager
+#include "core/tree_manager/factory_tree_manager.h"
+namespace wolf {
+WOLF_REGISTER_TREE_MANAGER(TreeManagerDummy)
+WOLF_REGISTER_TREE_MANAGER_AUTO(TreeManagerDummy)
+
+} /* namespace wolf */
+
+#endif /* INCLUDE_TREE_MANAGER_DUMMY_H_ */
diff --git a/test/gtest_SE3.cpp b/test/gtest_SE3.cpp
index 329feed0d0aad5565204f4b746cc1781ec872c9d..27acfea3069b92076bd30e4abd4ac28d787147bf 100644
--- a/test/gtest_SE3.cpp
+++ b/test/gtest_SE3.cpp
@@ -20,7 +20,7 @@ TEST(SE3, exp_0)
Vector6d tau = Vector6d::Zero();
Vector7d pose; pose << 0,0,0, 0,0,0,1;
- ASSERT_MATRIX_APPROX(pose, exp_SE3(tau), 1e-8);
+ ASSERT_MATRIX_APPROX(pose, exp(tau), 1e-8);
}
TEST(SE3, log_I)
@@ -28,16 +28,16 @@ TEST(SE3, log_I)
Vector7d pose; pose << 0,0,0, 0,0,0,1;
Vector6d tau = Vector6d::Zero();
- ASSERT_MATRIX_APPROX(tau, log_SE3(pose), 1e-8);
+ ASSERT_MATRIX_APPROX(tau, log(pose), 1e-8);
}
TEST(SE3, exp_log)
{
Vector6d tau = Vector6d::Random() / 5.0;
- Vector7d pose = exp_SE3(tau);
+ Vector7d pose = exp(tau);
- ASSERT_MATRIX_APPROX(tau, log_SE3(exp_SE3(tau)), 1e-8);
- ASSERT_MATRIX_APPROX(pose, exp_SE3(log_SE3(pose)), 1e-8);
+ ASSERT_MATRIX_APPROX(tau, log(exp(tau)), 1e-8);
+ ASSERT_MATRIX_APPROX(pose, exp(log(pose)), 1e-8);
}
TEST(SE3, exp_of_multiple)
@@ -45,20 +45,20 @@ TEST(SE3, exp_of_multiple)
Vector6d tau, tau2, tau3;
Vector7d pose, pose2, pose3;
tau = Vector6d::Random() / 5;
- pose << exp_SE3(tau);
+ pose << exp(tau);
tau2 = 2*tau;
tau3 = 3*tau;
pose2 = compose(pose, pose);
pose3 = compose(pose2, pose);
// 1
- ASSERT_MATRIX_APPROX(exp_SE3(tau), pose, 1e-8);
+ ASSERT_MATRIX_APPROX(exp(tau), pose, 1e-8);
// 2
- ASSERT_MATRIX_APPROX(exp_SE3(tau2), pose2, 1e-8);
+ ASSERT_MATRIX_APPROX(exp(tau2), pose2, 1e-8);
// 3
- ASSERT_MATRIX_APPROX(exp_SE3(tau3), pose3, 1e-8);
+ ASSERT_MATRIX_APPROX(exp(tau3), pose3, 1e-8);
}
TEST(SE3, log_of_power)
@@ -66,20 +66,177 @@ TEST(SE3, log_of_power)
Vector6d tau, tau2, tau3;
Vector7d pose, pose2, pose3;
tau = Vector6d::Random() / 5;
- pose << exp_SE3(tau);
+ pose << exp(tau);
tau2 = 2*tau;
tau3 = 3*tau;
pose2 = compose(pose, pose);
pose3 = compose(pose2, pose);
// 1
- ASSERT_MATRIX_APPROX(tau, log_SE3(pose), 1e-8);
+ ASSERT_MATRIX_APPROX(tau, log(pose), 1e-8);
// 2
- ASSERT_MATRIX_APPROX(tau2, log_SE3(pose2), 1e-8);
+ ASSERT_MATRIX_APPROX(tau2, log(pose2), 1e-8);
// 3
- ASSERT_MATRIX_APPROX(tau3, log_SE3(pose3), 1e-8);
+ ASSERT_MATRIX_APPROX(tau3, log(pose3), 1e-8);
+}
+
+TEST(SE3, composeBlocks)
+{
+ Vector3d p1, p2, pc;
+ Quaterniond q1, q2, qc;
+ p1.setRandom();
+ q1 = exp_q(Vector3d::Random()*100);
+ p2.setRandom();
+ q2 = exp_q(Vector3d::Random()*100);
+
+ compose(p1, q1, p2, q2, pc, qc);
+
+ ASSERT_MATRIX_APPROX(pc, p1 + q1*p2, 1e-8);
+ ASSERT_QUATERNION_APPROX(qc, q1*q2, 1e-8);
+}
+
+TEST(SE3, composeVectorBlocks)
+{
+ Vector3d p1, p2, pc;
+ Quaterniond q1, q2, qc;
+ p1.setRandom();
+ q1 = exp_q(Vector3d::Random()*100);
+ p2.setRandom();
+ q2 = exp_q(Vector3d::Random()*100);
+
+ compose(p1, q1.coeffs(), p2, q2.coeffs(), pc, qc.coeffs());
+
+ ASSERT_MATRIX_APPROX(pc, p1 + q1*p2, 1e-8);
+ ASSERT_QUATERNION_APPROX(qc, q1*q2, 1e-8);
+}
+
+TEST(SE3, composeEigenVectors)
+{
+ Vector3d p1, p2, pc;
+ Quaterniond q1, q2, qc;
+ p1.setRandom();
+ q1 = exp_q(Vector3d::Random()*100);
+ p2.setRandom();
+ q2 = exp_q(Vector3d::Random()*100);
+
+ compose(p1, q1, p2, q2, pc, qc); // tested in composeVectorBlocks
+
+
+ Vector7d x1; x1 << p1, q1.coeffs();
+ Vector7d x2; x2 << p2, q2.coeffs();
+ Vector7d xc, xc_true; xc_true << pc, qc.coeffs();
+
+ compose(x1, x2, xc);
+
+ ASSERT_MATRIX_APPROX(xc, xc_true, 1e-8);
+}
+
+TEST(SE3, composeVectorComposite)
+{
+ Vector3d p1, p2, pc;
+ Quaterniond q1, q2, qc;
+ p1.setRandom();
+ q1 = exp_q(Vector3d::Random()*100);
+ p2.setRandom();
+ q2 = exp_q(Vector3d::Random()*100);
+
+ compose(p1, q1, p2, q2, pc, qc); // tested in composeVectorBlocks
+
+ VectorComposite x1, x2, xc("PO", {3,4});
+
+ x1.emplace("P", p1);
+ x1.emplace("O", q1.coeffs());
+ x2.emplace("P", p2);
+ x2.emplace("O", q2.coeffs());
+
+ compose(x1, x2, xc);
+
+ ASSERT_MATRIX_APPROX(xc.at("P"), pc, 1e-8);
+ ASSERT_MATRIX_APPROX(xc.at("O"), qc.coeffs(), 1e-8);
+}
+
+TEST(SE3, composeVectorComposite_Jacobians)
+{
+ Vector3d p1, p2, pc;
+ Quaterniond q1, q2, qc;
+ p1.setRandom();
+ q1 = exp_q(Vector3d::Random()*100);
+ p2.setRandom();
+ q2 = exp_q(Vector3d::Random()*100);
+
+ Matrix3d R1 = q1.matrix();
+ Matrix3d R2 = q2.matrix();
+
+ compose(p1, q1, p2, q2, pc, qc); // tested in composeVectorBlocks
+
+ VectorComposite x1, x2, xc("PO", {3,4});
+ MatrixComposite J_xc_x1, J_xc_x2;
+
+ x1.emplace("P", p1);
+ x1.emplace("O", q1.coeffs());
+ x2.emplace("P", p2);
+ x2.emplace("O", q2.coeffs());
+
+ // we'll do xc = x1 * x2 and obtain Jacobians
+ compose(x1, x2, xc, J_xc_x1, J_xc_x2);
+
+ ASSERT_MATRIX_APPROX(xc.at("P"), pc, 1e-8);
+ ASSERT_MATRIX_APPROX(xc.at("O"), qc.coeffs(), 1e-8);
+
+ ASSERT_MATRIX_APPROX(J_xc_x1.at("P", "P"), Matrix3d::Identity() , 1e-8);
+ ASSERT_MATRIX_APPROX(J_xc_x1.at("P", "O"), - R1 * skew(p2) , 1e-8);
+ ASSERT_MATRIX_APPROX(J_xc_x1.at("O", "P"), Matrix3d::Zero() , 1e-8);
+ ASSERT_MATRIX_APPROX(J_xc_x1.at("O", "O"), R2.transpose() , 1e-8);
+
+ ASSERT_MATRIX_APPROX(J_xc_x2.at("P", "P"), R1 , 1e-8);
+ ASSERT_MATRIX_APPROX(J_xc_x2.at("P", "O"), Matrix3d::Zero() , 1e-8);
+ ASSERT_MATRIX_APPROX(J_xc_x2.at("O", "P"), Matrix3d::Zero() , 1e-8);
+ ASSERT_MATRIX_APPROX(J_xc_x2.at("O", "O"), Matrix3d::Identity() , 1e-8);
+}
+
+TEST(SE3, exp_0_Composite)
+{
+ // we check that exp(zero) = identity
+
+ Vector3d p; p.setZero();
+ Vector3d theta; theta.setZero();
+
+ VectorComposite tau;
+
+ tau.emplace("P", p);
+ tau.emplace("O", theta);
+
+ VectorComposite x = SE3::exp(tau);
+
+ ASSERT_MATRIX_APPROX(x.at("P"), Vector3d::Zero(), 1e-8);
+ ASSERT_MATRIX_APPROX(x.at("O"), Quaterniond::Identity().coeffs(), 1e-8);
+
+}
+
+TEST(SE3, plus_0_Composite)
+{
+ // we check that x plus zero = x
+
+ Vector3d p; p.setRandom();
+ Vector4d q; q.setRandom().normalized();
+
+ VectorComposite x;
+ x.emplace("P", p);
+ x.emplace("O", q);
+
+ Vector3d dp; dp.setZero();
+ Vector3d dtheta; dtheta.setZero();
+
+ VectorComposite tau;
+ tau.emplace("P", dp);
+ tau.emplace("O", dtheta);
+
+ VectorComposite res = plus(x, tau);
+
+ ASSERT_MATRIX_APPROX(res.at("P"), p, 1e-8);
+ ASSERT_MATRIX_APPROX(res.at("O"), q, 1e-8);
}
TEST(SE3, interpolate_I_xyz)
@@ -183,7 +340,7 @@ TEST(SE3, interpolate_half)
p1.setRandom(); p1.tail(4).normalize();
da.setRandom(); da /= 5; // small rotation
- dp << exp_SE3(da);
+ dp << exp(da);
// compose double, then interpolate 1/2
@@ -204,7 +361,7 @@ TEST(SE3, interpolate_third)
p1.setRandom(); p1.tail(4).normalize();
da.setRandom(); da /= 5; // small rotation
- dp << exp_SE3(da);
+ dp << exp(da);
dp2 = compose(dp, dp);
dp3 = compose(dp2, dp);
diff --git a/test/gtest_capture_base.cpp b/test/gtest_capture_base.cpp
index f0c1cbfcb2cee9f676700846dbb93fe0cd9e3521..4c004cd72e14e9e69bb343e288ab86e3a5b9354e 100644
--- a/test/gtest_capture_base.cpp
+++ b/test/gtest_capture_base.cpp
@@ -82,6 +82,15 @@ TEST(CaptureBase, addFeature)
ASSERT_EQ(C->getFeatureList().front(), f);
}
+TEST(CaptureBase, print)
+{
+ CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.2)); // timestamp = 1.2
+ // FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("DUMMY", Vector2d::Zero(), Matrix2d::Identity()));
+ auto f = FeatureBase::emplace<FeatureBase>(C, "DUMMY", Vector2d::Zero(), Matrix2d::Identity());
+
+ C->print(4, 1, 1, 1);
+}
+
TEST(CaptureBase, process)
{
SensorBasePtr S(std::make_shared<SensorBase>("DUMMY", nullptr, nullptr, nullptr, 2));
diff --git a/test/gtest_ceres_manager.cpp b/test/gtest_ceres_manager.cpp
index 7ead2b8f801602e664cae1e2ec4249f21ef916a5..d2b5c7e01b70fa65472f3e18bbfa7b38d8989679 100644
--- a/test/gtest_ceres_manager.cpp
+++ b/test/gtest_ceres_manager.cpp
@@ -88,7 +88,7 @@ TEST(CeresManager, Create)
ASSERT_EQ(P, ceres_manager_ptr->getProblem());
// run ceres manager check
- ceres_manager_ptr->check();
+ ASSERT_TRUE(ceres_manager_ptr->check());
}
TEST(CeresManager, AddStateBlock)
@@ -111,7 +111,7 @@ TEST(CeresManager, AddStateBlock)
ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredCeresManager(sb_ptr));
// run ceres manager check
- ceres_manager_ptr->check();
+ ASSERT_TRUE(ceres_manager_ptr->check());
}
TEST(CeresManager, DoubleAddStateBlock)
@@ -140,7 +140,7 @@ TEST(CeresManager, DoubleAddStateBlock)
ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredCeresManager(sb_ptr));
// run ceres manager check
- ceres_manager_ptr->check();
+ ASSERT_TRUE(ceres_manager_ptr->check());
}
TEST(CeresManager, UpdateStateBlock)
@@ -171,7 +171,7 @@ TEST(CeresManager, UpdateStateBlock)
ASSERT_TRUE(ceres_manager_ptr->isStateBlockFixed(sb_ptr));
// run ceres manager check
- ceres_manager_ptr->check();
+ ASSERT_TRUE(ceres_manager_ptr->check());
}
TEST(CeresManager, AddUpdateStateBlock)
@@ -198,7 +198,7 @@ TEST(CeresManager, AddUpdateStateBlock)
ASSERT_TRUE(ceres_manager_ptr->isStateBlockFixed(sb_ptr));
// run ceres manager check
- ceres_manager_ptr->check();
+ ASSERT_TRUE(ceres_manager_ptr->check());
}
TEST(CeresManager, RemoveStateBlock)
@@ -230,7 +230,7 @@ TEST(CeresManager, RemoveStateBlock)
ASSERT_EQ(ceres_manager_ptr->numStateBlocks(), 0);
// run ceres manager check
- ceres_manager_ptr->check();
+ ASSERT_TRUE(ceres_manager_ptr->check());
}
TEST(CeresManager, AddRemoveStateBlock)
@@ -256,7 +256,7 @@ TEST(CeresManager, AddRemoveStateBlock)
ASSERT_EQ(ceres_manager_ptr->numStateBlocks(), 0);
// run ceres manager check
- ceres_manager_ptr->check();
+ ASSERT_TRUE(ceres_manager_ptr->check());
}
TEST(CeresManager, RemoveUpdateStateBlock)
@@ -281,7 +281,7 @@ TEST(CeresManager, RemoveUpdateStateBlock)
ceres_manager_ptr->update();
// run ceres manager check
- ceres_manager_ptr->check();
+ ASSERT_TRUE(ceres_manager_ptr->check());
}
TEST(CeresManager, DoubleRemoveStateBlock)
@@ -309,7 +309,7 @@ TEST(CeresManager, DoubleRemoveStateBlock)
ceres_manager_ptr->update();
// run ceres manager check
- ceres_manager_ptr->check();
+ ASSERT_TRUE(ceres_manager_ptr->check());
}
TEST(CeresManager, AddFactor)
@@ -318,7 +318,7 @@ TEST(CeresManager, AddFactor)
CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
// Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
+ 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);
@@ -331,7 +331,7 @@ TEST(CeresManager, AddFactor)
ASSERT_EQ(ceres_manager_ptr->numFactors(), 1);
// run ceres manager check
- ceres_manager_ptr->check();
+ ASSERT_TRUE(ceres_manager_ptr->check());
}
TEST(CeresManager, DoubleAddFactor)
@@ -340,7 +340,7 @@ TEST(CeresManager, DoubleAddFactor)
CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
// Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
+ 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);
@@ -356,7 +356,7 @@ TEST(CeresManager, DoubleAddFactor)
ASSERT_EQ(ceres_manager_ptr->numFactors(), 1);
// run ceres manager check
- ceres_manager_ptr->check();
+ ASSERT_TRUE(ceres_manager_ptr->check());
}
TEST(CeresManager, RemoveFactor)
@@ -365,7 +365,7 @@ TEST(CeresManager, RemoveFactor)
CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
// Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
+ 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);
@@ -384,7 +384,7 @@ TEST(CeresManager, RemoveFactor)
ASSERT_EQ(ceres_manager_ptr->numFactors(), 0);
// run ceres manager check
- ceres_manager_ptr->check();
+ ASSERT_TRUE(ceres_manager_ptr->check());
}
TEST(CeresManager, AddRemoveFactor)
@@ -393,7 +393,7 @@ TEST(CeresManager, AddRemoveFactor)
CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
// Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
+ 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);
@@ -411,7 +411,7 @@ TEST(CeresManager, AddRemoveFactor)
ASSERT_EQ(ceres_manager_ptr->numFactors(), 0);
// run ceres manager check
- ceres_manager_ptr->check();
+ ASSERT_TRUE(ceres_manager_ptr->check());
}
TEST(CeresManager, DoubleRemoveFactor)
@@ -420,7 +420,7 @@ TEST(CeresManager, DoubleRemoveFactor)
CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
// Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
+ 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);
@@ -437,16 +437,15 @@ TEST(CeresManager, DoubleRemoveFactor)
// remove factor
P->notifyFactor(c,REMOVE);
- ASSERT_DEATH({
// update solver
- ceres_manager_ptr->update();},"");
+ ceres_manager_ptr->update();
// check factor
ASSERT_FALSE(ceres_manager_ptr->isFactorRegistered(c));
ASSERT_EQ(ceres_manager_ptr->numFactors(), 0);
// run ceres manager check
- ceres_manager_ptr->check();
+ ASSERT_TRUE(ceres_manager_ptr->check());
}
TEST(CeresManager, AddStateBlockLocalParam)
@@ -473,7 +472,7 @@ TEST(CeresManager, AddStateBlockLocalParam)
ASSERT_TRUE(ceres_manager_ptr->hasThisLocalParametrization(sb_ptr,local_param_ptr));
// run ceres manager check
- ceres_manager_ptr->check();
+ ASSERT_TRUE(ceres_manager_ptr->check());
}
TEST(CeresManager, RemoveLocalParam)
@@ -505,7 +504,7 @@ TEST(CeresManager, RemoveLocalParam)
ASSERT_FALSE(ceres_manager_ptr->hasLocalParametrization(sb_ptr));
// run ceres manager check
- ceres_manager_ptr->check();
+ ASSERT_TRUE(ceres_manager_ptr->check());
}
TEST(CeresManager, AddLocalParam)
@@ -538,7 +537,7 @@ TEST(CeresManager, AddLocalParam)
ASSERT_TRUE(ceres_manager_ptr->hasThisLocalParametrization(sb_ptr,local_param_ptr));
// run ceres manager check
- ceres_manager_ptr->check();
+ ASSERT_TRUE(ceres_manager_ptr->check());
}
TEST(CeresManager, FactorsRemoveLocalParam)
@@ -547,11 +546,11 @@ TEST(CeresManager, FactorsRemoveLocalParam)
CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
// Create (and add) 2 factors quaternion
- FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
+ 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->getO(),nullptr,false);
- FactorQuaternionAbsolutePtr c2 = FactorBase::emplace<FactorQuaternionAbsolute>(f, F->getO(),nullptr,false);
+ FactorQuaternionAbsolutePtr c1 = FactorBase::emplace<FactorQuaternionAbsolute>(f, f, F->getO(),nullptr,false);
+ FactorQuaternionAbsolutePtr c2 = FactorBase::emplace<FactorQuaternionAbsolute>(f, f, F->getO(),nullptr,false);
// update solver
ceres_manager_ptr->update();
@@ -580,7 +579,7 @@ TEST(CeresManager, FactorsRemoveLocalParam)
ASSERT_EQ(ceres_manager_ptr->numFactors(), 2);
// run ceres manager check
- ceres_manager_ptr->check();
+ ASSERT_TRUE(ceres_manager_ptr->check());
}
TEST(CeresManager, FactorsUpdateLocalParam)
@@ -589,11 +588,11 @@ TEST(CeresManager, FactorsUpdateLocalParam)
CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
// Create (and add) 2 factors quaternion
- FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
+ 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->getO(),nullptr,false);
- FactorQuaternionAbsolutePtr c2 = FactorBase::emplace<FactorQuaternionAbsolute>(f, F->getO(),nullptr,false);
+ FactorQuaternionAbsolutePtr c1 = FactorBase::emplace<FactorQuaternionAbsolute>(f, f, F->getO(),nullptr,false);
+ FactorQuaternionAbsolutePtr c2 = FactorBase::emplace<FactorQuaternionAbsolute>(f, f, F->getO(),nullptr,false);
// update solver
ceres_manager_ptr->update();
@@ -624,7 +623,7 @@ TEST(CeresManager, FactorsUpdateLocalParam)
ASSERT_EQ(ceres_manager_ptr->numFactors(), 2);
// run ceres manager check
- ceres_manager_ptr->check();
+ ASSERT_TRUE(ceres_manager_ptr->check());
}
int main(int argc, char **argv)
diff --git a/test/gtest_emplace.cpp b/test/gtest_emplace.cpp
index e60531daf9ab855e03b074384452d8e61c78d52a..457d41fe43f1adb01345065130f416f8ec785185 100644
--- a/test/gtest_emplace.cpp
+++ b/test/gtest_emplace.cpp
@@ -51,13 +51,13 @@ TEST(Emplace, Processor)
ProblemPtr P = Problem::create("PO", 2);
auto sen = SensorBase::emplace<SensorBase>(P->getHardware(), "Dummy", nullptr, nullptr, nullptr, 2, false);
- auto prc = ProcessorOdom2d::emplace<ProcessorOdom2d>(sen, std::make_shared<ProcessorParamsOdom2d>());
+ auto prc = ProcessorOdom2d::emplace<ProcessorOdom2d>(sen, std::make_shared<ParamsProcessorOdom2d>());
ASSERT_EQ(P, P->getHardware()->getSensorList().front()->getProcessorList().front()->getSensor()->getProblem());
ASSERT_EQ(sen, sen->getProcessorList().front()->getSensor());
ASSERT_EQ(prc, sen->getProcessorList().front());
SensorBasePtr sen2 = SensorBase::emplace<SensorBase>(P->getHardware(), "Dummy", nullptr, nullptr, nullptr, 2, false);
- ProcessorOdom2dPtr prc2 = ProcessorOdom2d::emplace<ProcessorOdom2d>(sen2, std::make_shared<ProcessorParamsOdom2d>());
+ ProcessorOdom2dPtr prc2 = ProcessorOdom2d::emplace<ProcessorOdom2d>(sen2, std::make_shared<ParamsProcessorOdom2d>());
ASSERT_EQ(sen2, sen2->getProcessorList().front()->getSensor());
ASSERT_EQ(prc2, sen2->getProcessorList().front());
}
diff --git a/test/gtest_factor_absolute.cpp b/test/gtest_factor_absolute.cpp
index c0f75e3e8753a568fafb772805910642031494d4..82b56da523c70dff134908ba3e63adf97493d914 100644
--- a/test/gtest_factor_absolute.cpp
+++ b/test/gtest_factor_absolute.cpp
@@ -35,10 +35,11 @@ ProblemPtr problem_ptr = Problem::create("POV", 3);
CeresManager ceres_mgr(problem_ptr);
// Two frames
-FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, problem_ptr->zeroState(), TimeStamp(0));
+FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, 0, problem_ptr->stateZero() );
-// Capture, feature and factor
-auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "IMU ABS", 0, nullptr, pose10, 10, 9, nullptr);
+// Capture
+// auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "IMU ABS", 0, nullptr, pose10, 10, 9, nullptr);
+auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "IMU ABS", 0, nullptr, pose10, nullptr);
////////////////////////////////////////////////////////
/* In the tests below, we check that FactorBlockAbsolute and FactorQuaternionAbsolute are working fine
@@ -49,7 +50,7 @@ auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "IMU ABS", 0, nullptr, pos
TEST(FactorBlockAbs, fac_block_abs_p)
{
auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "POSITION", pose10.head<3>(), data_cov.topLeftCorner<3,3>());
- FactorBase::emplace<FactorBlockAbsolute>(fea0, fea0->getFrame()->getP());
+ FactorBase::emplace<FactorBlockAbsolute>(fea0, fea0, fea0->getFrame()->getP(), nullptr, false);
ASSERT_TRUE(problem_ptr->check(0));
@@ -67,7 +68,7 @@ TEST(FactorBlockAbs, fac_block_abs_p)
TEST(FactorBlockAbs, fac_block_abs_p_tail2)
{
auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "POSITION TAIL 2", pose10.segment<2>(1), data_cov.bottomRightCorner<2,2>());
- FactorBase::emplace<FactorBlockAbsolute>(fea0, fea0->getFrame()->getP(),1,2);
+ FactorBase::emplace<FactorBlockAbsolute>(fea0, fea0, fea0->getFrame()->getP(),1,2, nullptr, false);
// Unfix frame 0, perturb frm0
frm0->unfix();
@@ -83,7 +84,7 @@ TEST(FactorBlockAbs, fac_block_abs_p_tail2)
TEST(FactorBlockAbs, fac_block_abs_v)
{
auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "FeatureVelocity", pose10.tail<3>(), data_cov.bottomRightCorner<3,3>());
- FactorBase::emplace<FactorBlockAbsolute>(fea0, fea0->getFrame()->getV());
+ FactorBase::emplace<FactorBlockAbsolute>(fea0, fea0, fea0->getFrame()->getV(), nullptr, false);
ASSERT_TRUE(problem_ptr->check(0));
@@ -101,7 +102,7 @@ TEST(FactorBlockAbs, fac_block_abs_v)
TEST(FactorQuatAbs, fac_block_abs_o)
{
auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "FeatureQuaternion", pose10.segment<4>(3), data_cov.block<3,3>(3,3));
- FactorBase::emplace<FactorQuaternionAbsolute>(fea0, fea0->getFrame()->getO());
+ FactorBase::emplace<FactorQuaternionAbsolute>(fea0, fea0, fea0->getFrame()->getO(), nullptr, false);
ASSERT_TRUE(problem_ptr->check(0));
diff --git a/test/gtest_factor_autodiff.cpp b/test/gtest_factor_autodiff.cpp
index 993908461d6b37cae1b24d8701a3612d4dc9c432..0acd77891bf1bae60fe41575edec629e9864999b 100644
--- a/test/gtest_factor_autodiff.cpp
+++ b/test/gtest_factor_autodiff.cpp
@@ -13,14 +13,345 @@
#include "core/factor/factor_odom_2d.h"
#include "core/factor/factor_odom_2d_analytic.h"
#include "core/factor/factor_autodiff.h"
+#include "dummy/factor_dummy_zero_1.h"
+#include "dummy/factor_dummy_zero_12.h"
using namespace wolf;
using namespace Eigen;
-TEST(CaptureAutodiff, ConstructorOdom2d)
+
+TEST(FactorAutodiff, AutodiffDummy1)
{
- FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(TimeStamp(0), std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1)));
- FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(TimeStamp(0), std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1)));
+ StateBlockPtr sb = std::make_shared<StateBlock>(Eigen::Vector1d::Random());
+ FeatureBasePtr ftr = std::make_shared<FeatureBase>("Feature", Vector2d(1,2), Matrix2d::Identity());
+
+ auto fac = std::make_shared<FactorDummyZero1>(ftr, sb);
+
+ // COMPUTE JACOBIANS
+ std::vector<const double*> states_ptr({sb->getStateData()});
+
+ std::vector<Eigen::MatrixXd> J;
+ Eigen::VectorXd residuals(fac->getSize());
+ fac->evaluate(states_ptr, residuals, J);
+
+ ASSERT_MATRIX_APPROX(J[0], Eigen::Matrix1d::Ones(), wolf::Constants::EPS);
+}
+
+TEST(FactorAutodiff, AutodiffDummy12)
+{
+ StateBlockPtr sb1 = std::make_shared<StateBlock>(Eigen::Vector1d::Random());
+ StateBlockPtr sb2 = std::make_shared<StateBlock>(Eigen::Vector2d::Random());
+ StateBlockPtr sb3 = std::make_shared<StateBlock>(Eigen::Vector3d::Random());
+ StateBlockPtr sb4 = std::make_shared<StateBlock>(Eigen::Vector4d::Random());
+ StateBlockPtr sb5 = std::make_shared<StateBlock>(Eigen::Vector5d::Random());
+ StateBlockPtr sb6 = std::make_shared<StateBlock>(Eigen::Vector6d::Random());
+ StateBlockPtr sb7 = std::make_shared<StateBlock>(Eigen::Vector7d::Random());
+ StateBlockPtr sb8 = std::make_shared<StateBlock>(Eigen::Vector8d::Random());
+ StateBlockPtr sb9 = std::make_shared<StateBlock>(Eigen::Vector9d::Random());
+ StateBlockPtr sb10 = std::make_shared<StateBlock>(Eigen::Vector10d::Random());
+ StateBlockPtr sb11 = std::make_shared<StateBlock>(Eigen::VectorXd::Random(11));
+ StateBlockPtr sb12 = std::make_shared<StateBlock>(Eigen::VectorXd::Random(12));
+
+ std::vector<const double*> states_ptr({sb1->getStateData(),sb2->getStateData(),sb3->getStateData(),sb4->getStateData(),sb5->getStateData(),sb6->getStateData(),sb7->getStateData(),sb8->getStateData(),sb9->getStateData(),sb10->getStateData(),sb11->getStateData(),sb12->getStateData()});
+ std::vector<Eigen::MatrixXd> J;
+ Eigen::VectorXd residuals;
+ unsigned int i;
+ FactorBasePtr fac = nullptr;
+ FeatureBasePtr ftr = std::make_shared<FeatureBase>("Feature", Vector2d(1,2), Matrix2d::Identity());
+
+ // 1 ------------------------------------------------------------------------
+ fac = std::make_shared<FactorDummyZero12<1>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+ i = fac->getSize();
+
+ // Jacobian
+ J.clear();
+ residuals.resize(i);
+ fac->evaluate(states_ptr, residuals, J);
+
+ std::cout << "i = " << i << std::endl;
+ for (unsigned int j = 0; j < 12; j++)
+ {
+ std::cout << "j = " << j << std::endl;
+ std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
+ if (j == i-1)
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
+ }
+ else
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
+ }
+ }
+
+ // 2 ------------------------------------------------------------------------
+ fac = std::make_shared<FactorDummyZero12<2>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+ i = fac->getSize();
+
+ // Jacobian
+ J.clear();
+ residuals.resize(i);
+ fac->evaluate(states_ptr, residuals, J);
+
+ std::cout << "i = " << i << std::endl;
+ for (unsigned int j = 0; j < 12; j++)
+ {
+ std::cout << "j = " << j << std::endl;
+ std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
+ if (j == i-1)
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
+ }
+ else
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
+ }
+ }
+
+ // 3 ------------------------------------------------------------------------
+ fac = std::make_shared<FactorDummyZero12<3>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+ i = fac->getSize();
+
+ // Jacobian
+ J.clear();
+ residuals.resize(i);
+ fac->evaluate(states_ptr, residuals, J);
+
+ std::cout << "i = " << i << std::endl;
+ for (unsigned int j = 0; j < 12; j++)
+ {
+ std::cout << "j = " << j << std::endl;
+ std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
+ if (j == i-1)
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
+ }
+ else
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
+ }
+ }
+
+ // 4 ------------------------------------------------------------------------
+ fac = std::make_shared<FactorDummyZero12<4>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+ i = fac->getSize();
+
+ // Jacobian
+ J.clear();
+ residuals.resize(i);
+ fac->evaluate(states_ptr, residuals, J);
+
+ std::cout << "i = " << i << std::endl;
+ for (unsigned int j = 0; j < 12; j++)
+ {
+ std::cout << "j = " << j << std::endl;
+ std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
+ if (j == i-1)
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
+ }
+ else
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
+ }
+ }
+
+ // 5 ------------------------------------------------------------------------
+ fac = std::make_shared<FactorDummyZero12<5>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+ i = fac->getSize();
+
+ // Jacobian
+ J.clear();
+ residuals.resize(i);
+ fac->evaluate(states_ptr, residuals, J);
+
+ std::cout << "i = " << i << std::endl;
+ for (unsigned int j = 0; j < 12; j++)
+ {
+ std::cout << "j = " << j << std::endl;
+ std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
+ if (j == i-1)
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
+ }
+ else
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
+ }
+ }
+
+ // 6 ------------------------------------------------------------------------
+ fac = std::make_shared<FactorDummyZero12<6>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+ i = fac->getSize();
+
+ // Jacobian
+ J.clear();
+ residuals.resize(i);
+ fac->evaluate(states_ptr, residuals, J);
+
+ std::cout << "i = " << i << std::endl;
+ for (unsigned int j = 0; j < 12; j++)
+ {
+ std::cout << "j = " << j << std::endl;
+ std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
+ if (j == i-1)
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
+ }
+ else
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
+ }
+ }
+
+ // 7 ------------------------------------------------------------------------
+ fac = std::make_shared<FactorDummyZero12<7>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+ i = fac->getSize();
+
+ // Jacobian
+ J.clear();
+ residuals.resize(i);
+ fac->evaluate(states_ptr, residuals, J);
+
+ std::cout << "i = " << i << std::endl;
+ for (unsigned int j = 0; j < 12; j++)
+ {
+ std::cout << "j = " << j << std::endl;
+ std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
+ if (j == i-1)
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
+ }
+ else
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
+ }
+ }
+
+ // 8 ------------------------------------------------------------------------
+ fac = std::make_shared<FactorDummyZero12<8>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+ i = fac->getSize();
+
+ // Jacobian
+ J.clear();
+ residuals.resize(i);
+ fac->evaluate(states_ptr, residuals, J);
+
+ std::cout << "i = " << i << std::endl;
+ for (unsigned int j = 0; j < 12; j++)
+ {
+ std::cout << "j = " << j << std::endl;
+ std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
+ if (j == i-1)
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
+ }
+ else
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
+ }
+ }
+
+ // 9 ------------------------------------------------------------------------
+ fac = std::make_shared<FactorDummyZero12<9>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+ i = fac->getSize();
+
+ // Jacobian
+ J.clear();
+ residuals.resize(i);
+ fac->evaluate(states_ptr, residuals, J);
+
+ std::cout << "i = " << i << std::endl;
+ for (unsigned int j = 0; j < 12; j++)
+ {
+ std::cout << "j = " << j << std::endl;
+ std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
+ if (j == i-1)
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
+ }
+ else
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
+ }
+ }
+
+ // 10 ------------------------------------------------------------------------
+ fac = std::make_shared<FactorDummyZero12<10>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+ i = fac->getSize();
+
+ // Jacobian
+ J.clear();
+ residuals.resize(i);
+ fac->evaluate(states_ptr, residuals, J);
+
+ std::cout << "i = " << i << std::endl;
+ for (unsigned int j = 0; j < 12; j++)
+ {
+ std::cout << "j = " << j << std::endl;
+ std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
+ if (j == i-1)
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
+ }
+ else
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
+ }
+ }
+
+ // 11 ------------------------------------------------------------------------
+ fac = std::make_shared<FactorDummyZero12<11>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+ i = fac->getSize();
+
+ // Jacobian
+ J.clear();
+ residuals.resize(i);
+ fac->evaluate(states_ptr, residuals, J);
+
+ std::cout << "i = " << i << std::endl;
+ for (unsigned int j = 0; j < 12; j++)
+ {
+ std::cout << "j = " << j << std::endl;
+ std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
+ if (j == i-1)
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
+ }
+ else
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
+ }
+ }
+
+ // 12 ------------------------------------------------------------------------
+ fac = std::make_shared<FactorDummyZero12<12>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
+ i = fac->getSize();
+
+ // Jacobian
+ J.clear();
+ residuals.resize(i);
+ fac->evaluate(states_ptr, residuals, J);
+
+ std::cout << "i = " << i << std::endl;
+ for (unsigned int j = 0; j < 12; j++)
+ {
+ std::cout << "j = " << j << std::endl;
+ std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
+ if (j == i-1)
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
+ }
+ else
+ {
+ ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
+ }
+ }
+}
+
+TEST(FactorAutodiff, EmplaceOdom2d)
+{
+ FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(KEY, TimeStamp(0), std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1)));
+ FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(KEY, TimeStamp(0), std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1)));
// SENSOR
ParamsSensorOdom2d intrinsics_odo;
@@ -44,14 +375,14 @@ TEST(CaptureAutodiff, ConstructorOdom2d)
ASSERT_TRUE(factor_ptr->getFrameOther());
}
-TEST(CaptureAutodiff, ResidualOdom2d)
+TEST(FactorAutodiff, ResidualOdom2d)
{
Eigen::Vector3d f1_pose, f2_pose;
f1_pose << 2,1,M_PI;
f2_pose << 3,5,3*M_PI/2;
- FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(TimeStamp(0), std::make_shared<StateBlock>(f1_pose.head<2>()), std::make_shared<StateBlock>(f1_pose.tail<1>())));
- FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(TimeStamp(0), std::make_shared<StateBlock>(f2_pose.head<2>()), std::make_shared<StateBlock>(f2_pose.tail<1>())));
+ FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(KEY, TimeStamp(0), std::make_shared<StateBlock>(f1_pose.head<2>()), std::make_shared<StateBlock>(f1_pose.tail<1>())));
+ FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(KEY, TimeStamp(0), std::make_shared<StateBlock>(f2_pose.head<2>()), std::make_shared<StateBlock>(f2_pose.tail<1>())));
// SENSOR
ParamsSensorOdom2d intrinsics_odo;
@@ -88,14 +419,14 @@ TEST(CaptureAutodiff, ResidualOdom2d)
ASSERT_TRUE(residuals.minCoeff() > -1e-9);
}
-TEST(CaptureAutodiff, JacobianOdom2d)
+TEST(FactorAutodiff, JacobianOdom2d)
{
Eigen::Vector3d f1_pose, f2_pose;
f1_pose << 2,1,M_PI;
f2_pose << 3,5,3*M_PI/2;
- FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(TimeStamp(0), std::make_shared<StateBlock>(f1_pose.head<2>()), std::make_shared<StateBlock>(f1_pose.tail<1>())));
- FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(TimeStamp(0), std::make_shared<StateBlock>(f2_pose.head<2>()), std::make_shared<StateBlock>(f2_pose.tail<1>())));
+ FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(KEY, TimeStamp(0), std::make_shared<StateBlock>(f1_pose.head<2>()), std::make_shared<StateBlock>(f1_pose.tail<1>())));
+ FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(KEY, TimeStamp(0), std::make_shared<StateBlock>(f2_pose.head<2>()), std::make_shared<StateBlock>(f2_pose.tail<1>())));
// SENSOR
ParamsSensorOdom2d intrinsics_odo;
@@ -166,14 +497,14 @@ TEST(CaptureAutodiff, JacobianOdom2d)
// std::cout << "analytic J " << 3 << ":\n" << J3 << std::endl;
}
-TEST(CaptureAutodiff, AutodiffVsAnalytic)
+TEST(FactorAutodiff, AutodiffVsAnalytic)
{
Eigen::Vector3d f1_pose, f2_pose;
f1_pose << 2,1,M_PI;
f2_pose << 3,5,3*M_PI/2;
- FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(TimeStamp(0), std::make_shared<StateBlock>(f1_pose.head<2>()), std::make_shared<StateBlock>(f1_pose.tail<1>())));
- FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(TimeStamp(0), std::make_shared<StateBlock>(f2_pose.head<2>()), std::make_shared<StateBlock>(f2_pose.tail<1>())));
+ FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(KEY, TimeStamp(0), std::make_shared<StateBlock>(f1_pose.head<2>()), std::make_shared<StateBlock>(f1_pose.tail<1>())));
+ FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(KEY, TimeStamp(0), std::make_shared<StateBlock>(f2_pose.head<2>()), std::make_shared<StateBlock>(f2_pose.tail<1>())));
// SENSOR
ParamsSensorOdom2d intrinsics_odo;
@@ -194,7 +525,6 @@ TEST(CaptureAutodiff, AutodiffVsAnalytic)
auto fac_analytic_ptr = FactorBase::emplace<FactorOdom2dAnalytic>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false);
// COMPUTE JACOBIANS
-
const Eigen::VectorXd fr1_pstate = fr1_ptr->getP()->getState();
const Eigen::VectorXd fr1_ostate = fr1_ptr->getO()->getState();
const Eigen::VectorXd fr2_pstate = fr2_ptr->getP()->getState();
diff --git a/test/gtest_factor_autodiff_distance_3d.cpp b/test/gtest_factor_autodiff_distance_3d.cpp
index 985e25a23de5f1c6b137d1b477bb9a22b219fb2a..e25e1505a3ddaa15ecfa6152fc007bc6009696f9 100644
--- a/test/gtest_factor_autodiff_distance_3d.cpp
+++ b/test/gtest_factor_autodiff_distance_3d.cpp
@@ -55,13 +55,10 @@ class FactorAutodiffDistance3d_Test : public testing::Test
problem = Problem::create("PO", 3);
ceres_manager = std::make_shared<CeresManager>(problem);
- F1 = problem->emplaceFrame (KEY, pose1, 1.0);
+ F1 = problem->emplaceFrame (KEY, 1.0, pose1);
- F2 = problem->emplaceFrame (KEY, pose2, 2.0);
+ F2 = problem->emplaceFrame (KEY, 2.0, pose2);
C2 = CaptureBase::emplace<CaptureBase>(F2, "Base", 1.0);
- f2 = FeatureBase::emplace<FeatureBase>(C2, "Dist", dist, dist_cov);
- c2 = FactorBase::emplace<FactorAutodiffDistance3d>(f2, f2, F1, nullptr, false, FAC_ACTIVE);
-
}
};
@@ -70,6 +67,8 @@ TEST_F(FactorAutodiffDistance3d_Test, ground_truth)
{
double res;
+ f2 = FeatureBase::emplace<FeatureBase>(C2, "Dist", dist, dist_cov);
+ c2 = FactorBase::emplace<FactorAutodiffDistance3d>(f2, f2, F1, nullptr, false, FAC_ACTIVE);
c2->operator ()(pos1.data(), pos2.data(), &res);
ASSERT_NEAR(res, 0.0, 1e-5);
@@ -79,7 +78,8 @@ TEST_F(FactorAutodiffDistance3d_Test, expected_residual)
{
double measurement = 1.400;
- f2->setMeasurement(Vector1d(measurement));
+ f2 = FeatureBase::emplace<FeatureBase>(C2, "Dist", Vector1d(measurement), dist_cov);
+ c2 = FactorBase::emplace<FactorAutodiffDistance3d>(f2, f2, F1, nullptr, false, FAC_ACTIVE);
double res_expected = (measurement - (pos2-pos1).norm()) * c2->getMeasurementSquareRootInformationUpper()(0,0);
@@ -92,7 +92,9 @@ TEST_F(FactorAutodiffDistance3d_Test, expected_residual)
TEST_F(FactorAutodiffDistance3d_Test, solve)
{
double measurement = 1.400;
- f2->setMeasurement(Vector1d(measurement));
+
+ f2 = FeatureBase::emplace<FeatureBase>(C2, "Dist", Vector1d(measurement), dist_cov);
+ c2 = FactorBase::emplace<FactorAutodiffDistance3d>(f2, f2, F1, nullptr, false, FAC_ACTIVE);
std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::QUIET);
diff --git a/test/gtest_factor_base.cpp b/test/gtest_factor_base.cpp
index 9e4f2ef10e8bf2fc1fb9a47d48468e254e46d0af..3237da062337d5a26c3c320e63df62c7d2798fb8 100644
--- a/test/gtest_factor_base.cpp
+++ b/test/gtest_factor_base.cpp
@@ -24,8 +24,8 @@ class FactorBaseTest : public testing::Test
virtual void SetUp()
{
- F0 = std::make_shared<FrameBase>(0.0, nullptr);
- F1 = std::make_shared<FrameBase>(1.0, nullptr);
+ F0 = std::make_shared<FrameBase>(KEY, 0.0, nullptr);
+ F1 = std::make_shared<FrameBase>(KEY, 1.0, nullptr);
C0 = std::make_shared<CaptureBase>("Capture", 0.0, nullptr);
C1 = std::make_shared<CaptureBase>("Capture", 1.0, nullptr);
f0 = std::make_shared<FeatureBase>("Feature", Vector2d(1,2), Matrix2d::Identity(), FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
@@ -39,11 +39,13 @@ class FactorBaseTest : public testing::Test
class FactorDummy : public FactorBase
{
public:
- FactorDummy(const FrameBasePtr& _frame_other,
+ FactorDummy(const FeatureBasePtr& _feature,
+ const FrameBasePtr& _frame_other,
const CaptureBasePtr& _capture_other,
const FeatureBasePtr& _feature_other,
const LandmarkBasePtr& _landmark_other) :
FactorBase("Dummy",
+ _feature,
_frame_other,
_capture_other,
_feature_other,
@@ -53,11 +55,13 @@ class FactorDummy : public FactorBase
{
//
}
- FactorDummy(const FrameBasePtrList& _frame_other_list,
+ FactorDummy(const FeatureBasePtr& _feature,
+ const FrameBasePtrList& _frame_other_list,
const CaptureBasePtrList& _capture_other_list,
const FeatureBasePtrList& _feature_other_list,
const LandmarkBasePtrList& _landmark_other_list) :
FactorBase("Dummy",
+ _feature,
_frame_other_list,
_capture_other_list,
_feature_other_list,
@@ -70,8 +74,12 @@ class FactorDummy : public FactorBase
virtual ~FactorDummy() = default;
virtual std::string getTopology() const override {return "DUMMY";}
- virtual bool evaluate(double const* const* _parameters, double* _residuals, double** _jacobians) const override {return true;}
- virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& _residual, std::vector<Eigen::MatrixXd>& _jacobians) const override {}
+ virtual bool evaluate(double const* const* _parameters,
+ double* _residuals,
+ double** _jacobians) const override {return true;}
+ virtual void evaluate(const std::vector<const double*>& _states_ptr,
+ Eigen::VectorXd& _residual,
+ std::vector<Eigen::MatrixXd>& _jacobians) const override {}
virtual JacobianMethod getJacobianMethod() const override {return JacobianMethod::JAC_ANALYTIC;}
virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const override {std::vector<StateBlockPtr> v; return v;}
virtual std::vector<unsigned int> getStateSizes() const override {std::vector<unsigned int> v; return v;}
@@ -81,7 +89,7 @@ class FactorDummy : public FactorBase
TEST_F(FactorBaseTest, constructor_from_pointers)
{
- FactorDummy fac(nullptr,C0,f0,nullptr);
+ FactorDummy fac(f0,nullptr,C0,f1,nullptr);
ASSERT_TRUE(fac.getFrameOtherList().empty());
@@ -90,11 +98,15 @@ TEST_F(FactorBaseTest, constructor_from_pointers)
ASSERT_EQ(fac.getFeatureOtherList().size(), 1);
ASSERT_TRUE(fac.getLandmarkOtherList().empty());
+
+ ASSERT_MATRIX_APPROX(fac.getMeasurement(), f0->getMeasurement(), 1e-12);
+
+ ASSERT_MATRIX_APPROX(fac.getMeasurementSquareRootInformationUpper(), f0->getMeasurementSquareRootInformationUpper(), 1e-12);
}
TEST_F(FactorBaseTest, constructor_from_lists)
{
- FactorDummy fac({},{C0},{f0,f1},{});
+ FactorDummy fac(f0,{},{C0},{f0,f1},{});
ASSERT_TRUE(fac.getFrameOtherList().empty());
@@ -103,6 +115,10 @@ TEST_F(FactorBaseTest, constructor_from_lists)
ASSERT_EQ(fac.getFeatureOtherList().size(), 2);
ASSERT_TRUE(fac.getLandmarkOtherList().empty());
+
+ ASSERT_MATRIX_APPROX(fac.getMeasurement(), f0->getMeasurement(), 1e-12);
+
+ ASSERT_MATRIX_APPROX(fac.getMeasurementSquareRootInformationUpper(), f0->getMeasurementSquareRootInformationUpper(), 1e-12);
}
diff --git a/test/gtest_factor_block_difference.cpp b/test/gtest_factor_block_difference.cpp
index 1e06c7a80377b99197a721e85473319f9bf34862..bee51d78e417f1f446a5890fa1ead4834dc366bc 100644
--- a/test/gtest_factor_block_difference.cpp
+++ b/test/gtest_factor_block_difference.cpp
@@ -24,7 +24,7 @@ using namespace wolf;
const Vector6d zero6 = Vector6d::Zero();
const Vector3d zero3 = zero6.head<3>();
-
+const Matrix3d idty02 = Matrix3d::Identity() * 0.02;
class FixtureFactorBlockDifference : public testing::Test
{
@@ -35,6 +35,7 @@ class FixtureFactorBlockDifference : public testing::Test
FrameBasePtr KF1_;
CaptureBasePtr Cap_;
FeatureBasePtr Feat_;
+ FactorBlockDifferencePtr Fac_;
protected:
@@ -48,31 +49,36 @@ class FixtureFactorBlockDifference : public testing::Test
TimeStamp t0(0);
TimeStamp t1(1);
- Vector10d x_origin = problem_->zeroState();
- Eigen::Matrix9d cov_prior = 1e-3 * Eigen::Matrix9d::Identity();
- KF0_ = problem_->setPrior(x_origin, cov_prior, t0, 0.1);
+ // Vector10d x_origin = problem_->stateZero().vector("POV");
+ VectorComposite x_origin(problem_->stateZero().vector("POV"), "POV", {3, 4, 3});
+ // Eigen::Matrix9d cov_prior = 1e-3 * Eigen::Matrix9d::Identity();
+ VectorComposite cov_prior("POV", {Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)),Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)),Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1))});
+ KF0_ = problem_->setPriorFactor(x_origin, cov_prior, t0, 0.1);
- CaptureBasePtr capV0 = CaptureBase::emplace<CaptureBase>(KF0_, "Vel0", t0);
- FeatureBasePtr featV0 = FeatureBase::emplace<FeatureBase>(capV0, "Vel0", x_origin.tail(3), cov_prior.bottomRightCorner<3,3>());
- FactorBasePtr facV0 = FactorBase::emplace<FactorBlockAbsolute>(featV0, KF0_->getV());
+ //CaptureBasePtr capV0 = CaptureBase::emplace<CaptureBase>(KF0_, "Vel0", t0);
+ //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, problem_->zeroState(), t1);
+ KF1_ = problem_->emplaceFrame(KEY, t1, problem_->stateZero());
Cap_ = CaptureBase::emplace<CaptureBase>(KF1_, "BlockDifference", t1);
- Eigen::Matrix3d cov = 0.2 * Eigen::Matrix3d::Identity();
- Feat_ = FeatureBase::emplace<FeatureBase>(Cap_, "BlockDifference", zero3, cov);
}
virtual void TearDown() override {}
};
+TEST_F(FixtureFactorBlockDifference, CheckFactorType)
+{
+ Feat_ = FeatureBase::emplace<FeatureBase>(Cap_, "BlockDifference", zero3, idty02);
+ Fac_ = FactorBase::emplace<FactorBlockDifference>(Feat_, Feat_, KF0_->getP(), KF1_->getP());
+ ASSERT_EQ(Fac_->getType(), "FactorBlockDifference");
+}
+
TEST_F(FixtureFactorBlockDifference, EqualP)
{
- // Feat_->setMeasurement()
- FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>(
- Feat_, KF0_->getP(), KF1_->getP()
- );
+ Feat_ = FeatureBase::emplace<FeatureBase>(Cap_, "BlockDifference", zero3, idty02);
+ Fac_ = FactorBase::emplace<FactorBlockDifference>(Feat_, Feat_, KF0_->getP(), KF1_->getP());
// perturbate
KF0_->getP()->setState(Vector3d::Random());
@@ -81,14 +87,12 @@ TEST_F(FixtureFactorBlockDifference, EqualP)
std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF);
ASSERT_MATRIX_APPROX(KF1_->getP()->getState() - KF0_->getP()->getState(), zero3, 1e-8);
-
}
TEST_F(FixtureFactorBlockDifference, EqualV)
{
- FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>(
- Feat_, KF0_->getV(), KF1_->getV()
- );
+ Feat_ = FeatureBase::emplace<FeatureBase>(Cap_, "BlockDifference", zero3, idty02);
+ Fac_ = FactorBase::emplace<FactorBlockDifference>(Feat_, Feat_, KF0_->getV(), KF1_->getV());
// perturbate
KF0_->getV()->setState(Vector3d::Random());
@@ -97,18 +101,14 @@ TEST_F(FixtureFactorBlockDifference, EqualV)
std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF);
ASSERT_MATRIX_APPROX(KF1_->getV()->getState() - KF0_->getV()->getState(), zero3, 1e-8);
-
}
-
-
TEST_F(FixtureFactorBlockDifference, DiffP)
{
Vector3d diff = Vector3d::Random();
- Feat_->setMeasurement(diff);
- FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>(
- Feat_, KF0_->getP(), KF1_->getP()
- );
+
+ Feat_ = FeatureBase::emplace<FeatureBase>(Cap_, "BlockDifference", diff, idty02);
+ Fac_ = FactorBase::emplace<FactorBlockDifference>(Feat_, Feat_, KF0_->getP(), KF1_->getP());
// perturbate
KF0_->getP()->setState(Vector3d::Random());
@@ -117,18 +117,14 @@ TEST_F(FixtureFactorBlockDifference, DiffP)
std::string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF);
ASSERT_MATRIX_APPROX(KF1_->getP()->getState() - KF0_->getP()->getState(), diff, 1e-8);
-
}
-
-
TEST_F(FixtureFactorBlockDifference, DiffV)
{
Vector3d diff = Vector3d::Random();
- Feat_->setMeasurement(diff);
- FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>(
- Feat_, KF0_->getV(), KF1_->getV()
- );
+
+ Feat_ = FeatureBase::emplace<FeatureBase>(Cap_, "BlockDifference", diff, idty02);
+ Fac_ = FactorBase::emplace<FactorBlockDifference>(Feat_, Feat_, KF0_->getV(), KF1_->getV());
// perturbate
KF0_->getV()->setState(Vector3d::Random());
@@ -145,13 +141,11 @@ TEST_F(FixtureFactorBlockDifference, DiffPx)
// Vector3d diff = Vector3d::Random();
Vector1d diff; diff << 1; // measurement still of the same size as the constrained state
Matrix1d cov_diff = 1e-4 * Matrix1d::Identity(); // measurement still of the same size as the constrained state
- Feat_->setMeasurement(diff);
- Feat_->setMeasurementCovariance(cov_diff);
- FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>(
- Feat_, KF0_->getP(), KF1_->getP(),
- nullptr, nullptr, nullptr, nullptr,
- 0, 1, 0, 1
- );
+
+ Feat_ = FeatureBase::emplace<FeatureBase>(Cap_, "BlockDifference", diff, cov_diff);
+ Fac_ = FactorBase::emplace<FactorBlockDifference>(Feat_, Feat_, KF0_->getP(), KF1_->getP(),
+ nullptr, nullptr, nullptr, nullptr,
+ 0, 1, 0, 1);
// perturbate
KF0_->getP()->setState(Vector3d::Random());
@@ -162,21 +156,16 @@ TEST_F(FixtureFactorBlockDifference, DiffPx)
ASSERT_MATRIX_APPROX(KF1_->getP()->getState().head<1>() - KF0_->getP()->getState().head<1>(), diff.head<1>(), 1e-8);
}
-
-
-
TEST_F(FixtureFactorBlockDifference, DiffPxy)
{
// Vector3d diff = Vector3d::Random();
Vector2d diff; diff << 1, 2; // measurement still of the same size as the constrained state
Matrix2d cov_diff = 1e-4 * Matrix2d::Identity(); // measurement still of the same size as the constrained state
- Feat_->setMeasurement(diff);
- Feat_->setMeasurementCovariance(cov_diff);
- FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>(
- Feat_, KF0_->getP(), KF1_->getP(),
- nullptr, nullptr, nullptr, nullptr,
- 0, 2, 0, 2
- );
+
+ Feat_ = FeatureBase::emplace<FeatureBase>(Cap_, "BlockDifference", diff, cov_diff);
+ Fac_ = FactorBase::emplace<FactorBlockDifference>(Feat_, Feat_, KF0_->getP(), KF1_->getP(),
+ nullptr, nullptr, nullptr, nullptr,
+ 0, 2, 0, 2);
// perturbate
KF0_->getP()->setState(Vector3d::Random());
@@ -192,13 +181,11 @@ TEST_F(FixtureFactorBlockDifference, DiffPyz)
// Vector3d diff = Vector3d::Random();
Vector2d diff; diff << 1, 2; // measurement still of the same size as the constrained state
Matrix2d cov_diff = 1e-4 * Matrix2d::Identity(); // measurement still of the same size as the constrained state
- Feat_->setMeasurement(diff);
- Feat_->setMeasurementCovariance(cov_diff);
- FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>(
- Feat_, KF0_->getP(), KF1_->getP(),
- nullptr, nullptr, nullptr, nullptr,
- 1, 2, 1, 2
- );
+
+ Feat_ = FeatureBase::emplace<FeatureBase>(Cap_, "BlockDifference", diff, cov_diff);
+ Fac_ = FactorBase::emplace<FactorBlockDifference>(Feat_, Feat_, KF0_->getP(), KF1_->getP(),
+ nullptr, nullptr, nullptr, nullptr,
+ 1, 2, 1, 2);
// perturbate
KF0_->getP()->setState(Vector3d::Random());
diff --git a/test/gtest_factor_diff_drive.cpp b/test/gtest_factor_diff_drive.cpp
index 67ef0974061dfa88339bbf3be362794ec6af060d..0413c21cb2c30bb91b1a03dabac0b595d1843349 100644
--- a/test/gtest_factor_diff_drive.cpp
+++ b/test/gtest_factor_diff_drive.cpp
@@ -25,7 +25,7 @@ class ProcessorDiffDrivePublic : public ProcessorDiffDrive
{
using Base = ProcessorDiffDrive;
public:
- ProcessorDiffDrivePublic(ProcessorParamsDiffDrivePtr _params_motion) :
+ ProcessorDiffDrivePublic(ParamsProcessorDiffDrivePtr _params_motion) :
ProcessorDiffDrive(_params_motion)
{
//
@@ -63,12 +63,9 @@ class ProcessorDiffDrivePublic : public ProcessorDiffDrive
Base::deltaPlusDelta(_delta1, _delta2, _dt2, _delta1_plus_delta2, _jacobian1, _jacobian2);
}
- virtual void statePlusDelta(const Eigen::VectorXd& _x,
- const Eigen::VectorXd& _delta,
- const double _dt,
- Eigen::VectorXd& _x_plus_delta) const
+ virtual VectorXd getCalibration (const CaptureBasePtr _capture) const
{
- Base::statePlusDelta(_x, _delta, _dt, _x_plus_delta);
+ return Base::getCalibration(_capture);
}
virtual Eigen::VectorXd deltaZero() const
@@ -99,7 +96,7 @@ class ProcessorDiffDrivePublic : public ProcessorDiffDrive
return Base::emplaceFactor(_feature_motion, _capture_origin);
}
- ProcessorParamsDiffDrivePtr getParams()
+ ParamsProcessorDiffDrivePtr getParams()
{
return params_prc_diff_drv_selfcal_;
}
@@ -121,7 +118,7 @@ class FactorDiffDriveTest : public testing::Test
TrajectoryBasePtr trajectory;
ParamsSensorDiffDrivePtr intr;
SensorDiffDrivePtr sensor;
- ProcessorParamsDiffDrivePtr params;
+ ParamsProcessorDiffDrivePtr params;
ProcessorDiffDrivePublicPtr processor;
FrameBasePtr F0, F1;
CaptureMotionPtr C0, C1;
@@ -154,7 +151,7 @@ class FactorDiffDriveTest : public testing::Test
calib = sensor->getIntrinsic()->getState();
// params and processor
- params = std::make_shared<ProcessorParamsDiffDrive>();
+ params = std::make_shared<ParamsProcessorDiffDrive>();
params->angle_turned = 1;
params->dist_traveled = 2;
params->max_buff_length = 3;
@@ -163,16 +160,16 @@ class FactorDiffDriveTest : public testing::Test
// frames
F0 = FrameBase::emplace<FrameBase>(trajectory,
- "PO",
- 2,
KEY,
0.0,
- Vector3d(0,0,0));
- F1 = FrameBase::emplace<FrameBase>(trajectory,
"PO",
2,
+ Vector3d(0,0,0));
+ F1 = FrameBase::emplace<FrameBase>(trajectory,
KEY,
1.0,
+ "PO",
+ 2,
Vector3d(1,0,0));
// captures
@@ -201,7 +198,7 @@ class FactorDiffDriveTest : public testing::Test
"FeatureDiffDrive",
delta1,
delta1_cov,
- C0->getCalibration(),
+ processor->getCalibration(C0),
Matrix3d::Identity()); // TODO Jacobian?
@@ -290,7 +287,15 @@ TEST_F(FactorDiffDriveTest, residual_right_turn_90_deg)
ASSERT_MATRIX_APPROX(delta2, Vector3d(1.5, -1.5, -M_PI_2), 1e-6);
- f1->setMeasurement(delta2);
+ // modify f1 measurement (remove and emplace - FeatureBase::setMeasurement is private)
+ f1 ->remove();
+ f1 = FeatureBase::emplace<FeatureMotion>(C1,
+ "FeatureDiffDrive",
+ delta2,
+ delta1_cov,
+ processor->getCalibration(C0),
+ Matrix3d::Identity()); // TODO Jacobian?
+
F1->setState(Vector3d(1.5, -1.5, -M_PI_2));
// wolf: emplace
@@ -317,7 +322,15 @@ TEST_F(FactorDiffDriveTest, solve_F1)
delta1 .setZero();
processor->deltaPlusDelta(delta1, delta, dt, delta2); // 90deg
- f1->setMeasurement(delta2);
+ // modify f1 measurement (remove and emplace - FeatureBase::setMeasurement is private)
+ f1 ->remove();
+ f1 = FeatureBase::emplace<FeatureMotion>(C1,
+ "FeatureDiffDrive",
+ delta2,
+ delta1_cov,
+ processor->getCalibration(C0),
+ Matrix3d::Identity()); // TODO Jacobian?
+
F1->setState(Vector3d(1.5, -1.5, -M_PI_2));
// wolf: emplace
@@ -328,14 +341,14 @@ TEST_F(FactorDiffDriveTest, solve_F1)
// Fix all but F1 ; perturb F1 ; solve ; print and assert values of F1
F0->fix();
sensor->fixIntrinsics();
- F1->setState(F1->getState() + Vector3d::Random() * 0.1);
+ F1->perturb(0.1);
std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
// WOLF_TRACE("\n", report);
problem->print(1,0,1,1);
- ASSERT_MATRIX_APPROX(F1->getState(), delta, 1e-6);
+ ASSERT_MATRIX_APPROX(F1->getStateVector(), delta, 1e-6);
}
@@ -355,7 +368,15 @@ TEST_F(FactorDiffDriveTest, solve_sensor_intrinsics)
delta1 .setZero();
processor->deltaPlusDelta(delta1, delta, dt, delta2); // 90deg
- f1->setMeasurement(delta2);
+ // modify f1 measurement (remove and emplace - FeatureBase::setMeasurement is private)
+ f1 ->remove();
+ f1 = FeatureBase::emplace<FeatureMotion>(C1,
+ "FeatureDiffDrive",
+ delta2,
+ delta1_cov,
+ processor->getCalibration(C0),
+ Matrix3d::Identity()); // TODO Jacobian?
+
F1->setState(Vector3d(1.5, -1.5, -M_PI_2));
// wolf: emplace
@@ -367,7 +388,7 @@ TEST_F(FactorDiffDriveTest, solve_sensor_intrinsics)
F0->fix();
F1->fix();
sensor->unfixIntrinsics();
- sensor->getIntrinsic()->setState(calib + Vector3d::Random() * 0.1);
+ sensor->perturb(0.1);
std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
@@ -432,7 +453,7 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt)
Vector3d calib_gt(1,1,1);
// params and processor
- ProcessorParamsDiffDrivePtr params = std::make_shared<ProcessorParamsDiffDrive>();
+ ParamsProcessorDiffDrivePtr params = std::make_shared<ParamsProcessorDiffDrive>();
params->angle_turned = 99;
params->dist_traveled = 99;
params->max_buff_length = 99;
@@ -441,15 +462,17 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt)
auto prc = problem->installProcessor("ProcessorDiffDrive", "diff drive", sensor, params);
auto processor = std::static_pointer_cast<ProcessorDiffDrivePublic>(prc);
-
TimeStamp t(0.0);
double dt = 1.0;
- Vector3d x0(0,0,0);
+ VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
Vector3d x1(1.5, -1.5, -M_PI/2.0);
Vector3d x2(3.0, -3.0, 0.0);
- Matrix3d P0; P0.setIdentity();
+ // Matrix3d P0; P0.setIdentity();
+ VectorComposite s0(Vector3d(0.1,0.1,0.1), "PO", {2,1});
- auto F0 = problem->setPrior(x0, P0, t, 0.1);
+ // set prior at t=0 and processor origin
+ auto F0 = problem->setPriorFactor(x0, s0, t, 0.1);
+ processor->setOrigin(F0);
// right turn 90 deg in N steps --> ends up in (1.5, -1.5, -pi/2)
int N = 6;
@@ -459,7 +482,7 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt)
data(0) = 0.50*intr->ticks_per_wheel_revolution / N;
data(1) = 0.25*intr->ticks_per_wheel_revolution / N;
- auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, F0->getCaptureList().front());
+ auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, nullptr);
for (int n = 0; n < N; n++)
{
@@ -481,29 +504,28 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt)
auto F2 = problem->getLastKeyFrame();
-
// Fix boundaries
F0->fix();
F2->fix();
// Perturb S
- Vector3d calib_pert = calib_gt + Vector3d::Random()*0.2;
- sensor->getIntrinsic()->setState(calib_pert);
+ sensor->getIntrinsic()->perturb(0.2);
+ Vector3d calib_pert = sensor->getIntrinsic()->getState();
WOLF_TRACE("\n ========== SOLVE =========");
std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
WOLF_TRACE("\n", report);
- WOLF_TRACE("x1 : ", problem->getState(N*dt).transpose());
- WOLF_TRACE("x2 : ", F2->getState().transpose());
+ WOLF_TRACE("x1 : ", problem->getState(N*dt).vector("PO").transpose());
+ WOLF_TRACE("x2 : ", F2->getStateVector().transpose());
WOLF_TRACE("calib_preint : ", calib_preint.transpose());
WOLF_TRACE("calib_pert : ", calib_pert.transpose());
WOLF_TRACE("calib_est : ", sensor->getIntrinsic()->getState().transpose());
WOLF_TRACE("calib_gt : ", calib_gt.transpose());
- ASSERT_MATRIX_APPROX(sensor->getIntrinsic()->getState(), calib_gt, 1e-6);
- ASSERT_MATRIX_APPROX(problem->getState( N*dt), x1, 1e-6);
- ASSERT_MATRIX_APPROX(problem->getState(2*N*dt), x2, 1e-6);
+ ASSERT_MATRIX_APPROX(sensor->getIntrinsic()->getState() , calib_gt , 1e-6 );
+ ASSERT_MATRIX_APPROX(problem->getState( N*dt).vector("PO") , x1 , 1e-6 );
+ ASSERT_MATRIX_APPROX(problem->getState(2*N*dt).vector("PO") , x2 , 1e-6 );
std::cout << "\n\n" << std::endl;
@@ -564,7 +586,7 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
Vector3d calib_gt(1.0, 1.0, 1.0); // ground truth calib
// params and processor
- ProcessorParamsDiffDrivePtr params = std::make_shared<ProcessorParamsDiffDrive>();
+ ParamsProcessorDiffDrivePtr params = std::make_shared<ParamsProcessorDiffDrive>();
params->angle_turned = 99;
params->dist_traveled = 99;
params->max_buff_length = 99;
@@ -573,15 +595,18 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
auto prc = problem->installProcessor("ProcessorDiffDrive", "diff drive", sensor, params);
auto processor = std::static_pointer_cast<ProcessorDiffDrivePublic>(prc);
-
TimeStamp t(0.0);
double dt = 1.0;
- Vector3d x0(0,0,0);
+ // Vector3d x0(0,0,0);
+ VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
Vector3d x1(1.5, -1.5, -M_PI/2.0);
Vector3d x2(3.0, -3.0, 0.0);
- Matrix3d P0; P0.setIdentity();
+ // Matrix3d P0; P0.setIdentity();
+ VectorComposite s0(Vector3d(1,1,1), "PO", {2,1});
- auto F0 = problem->setPrior(x0, P0, t, 0.1);
+ // set prior at t=0 and processor origin
+ auto F0 = problem->setPriorFactor(x0, s0, t, 0.1);
+ processor->setOrigin(F0);
// right turn 90 deg in N steps --> ends up in (1.5, -1.5, -pi/2)
int N = 6;
@@ -591,7 +616,7 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
data(0) = 0.50*intr->ticks_per_wheel_revolution / N;
data(1) = 0.25*intr->ticks_per_wheel_revolution / N;
- auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, F0->getCaptureList().front());
+ auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, nullptr);
for (int n = 0; n < N; n++)
{
@@ -615,27 +640,27 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
auto F2 = problem->getLastKeyFrame();
- F2->setState(x2); // Impose known final state regardless of integrated value.
+// VectorComposite x2c(x2, "PO", {2,1});
+ F2->setState(x2, "PO"); // Impose known final state regardless of integrated value.
// Fix boundaries
F0->fix();
F2->fix();
-
WOLF_TRACE("\n ========== SOLVE =========");
std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
WOLF_TRACE("\n", report);
- WOLF_TRACE("x1 : ", problem->getState(N*dt).transpose());
- WOLF_TRACE("x2 : ", F2->getState().transpose());
+ WOLF_TRACE("x1 : ", problem->getState(N*dt).vector("PO").transpose());
+ WOLF_TRACE("x2 : ", F2->getStateVector().transpose());
WOLF_TRACE("calib_preint : ", calib_preint.transpose());
WOLF_TRACE("calib_est : ", sensor->getIntrinsic()->getState().transpose());
WOLF_TRACE("calib_GT : ", calib_gt.transpose());
ASSERT_MATRIX_APPROX(sensor->getIntrinsic()->getState(), calib_gt, 1e-2);
- ASSERT_MATRIX_APPROX(problem->getState( N*dt), x1, 1e-2);
- ASSERT_MATRIX_APPROX(problem->getState(2*N*dt), x2, 1e-6);
+ ASSERT_MATRIX_APPROX(problem->getState( N*dt).vector("PO"), x1, 1e-2);
+ ASSERT_MATRIX_APPROX(problem->getState(2*N*dt).vector("PO"), x2, 1e-6);
}
diff --git a/test/gtest_factor_odom_2d.cpp b/test/gtest_factor_odom_2d.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8c0b11e3b428cfbfa99c91c29bffe53ceeeb1d92
--- /dev/null
+++ b/test/gtest_factor_odom_2d.cpp
@@ -0,0 +1,119 @@
+#include "core/utils/utils_gtest.h"
+
+#include "core/factor/factor_odom_2d.h"
+#include "core/capture/capture_odom_2d.h"
+#include "core/math/rotations.h"
+
+#include "core/ceres_wrapper/ceres_manager.h"
+
+using namespace Eigen;
+using namespace wolf;
+using std::cout;
+using std::endl;
+
+// Input odometry data and covariance
+Matrix3d data_cov = 0.1*Matrix3d::Identity();
+
+// Problem and solver
+ProblemPtr problem_ptr = Problem::create("PO", 2);
+CeresManager ceres_mgr(problem_ptr);
+
+// Two frames
+FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, TimeStamp(0), Vector3d::Zero());
+FrameBasePtr frm1 = problem_ptr->emplaceFrame(KEY, TimeStamp(1), Vector3d::Zero());
+
+// Capture from frm1 to frm0
+auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, nullptr, Vector3d::Zero(), data_cov);
+
+TEST(FactorOdom2d, check_tree)
+{
+ ASSERT_TRUE(problem_ptr->check(0));
+}
+
+TEST(FactorOdom2d, fix_0_solve)
+{
+ for (int i = 0; i < 1e3; i++)
+ {
+ // Random delta
+ Vector3d delta = Vector3d::Random() * 10;
+ pi2pi(delta(2));
+
+ // Random initial pose
+ Vector3d x0 = Vector3d::Random() * 10;
+ pi2pi(x0(2));
+
+ // x1 groundtruth
+ Vector3d x1;
+ x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * delta.head<2>();
+ x1(2) = pi2pi(x0(2) + delta(2));
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+ cap1->setData(delta);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
+ FactorBase::emplace<FactorOdom2d>(fea1, fea1, frm0, nullptr, false);
+
+ // Fix frm0, perturb frm1
+ frm0->fix();
+ frm1->unfix();
+ frm1->perturb(5);
+
+ // solve for frm1
+ std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
+}
+
+TEST(FactorOdom2d, fix_1_solve)
+{
+ for (int i = 0; i < 1e3; i++)
+ {
+ // Random delta
+ Vector3d delta = Vector3d::Random() * 10;
+ pi2pi(delta(2));
+
+ // Random initial pose
+ Vector3d x0 = Vector3d::Random() * 10;
+ pi2pi(x0(2));
+
+ // x1 groundtruth
+ Vector3d x1;
+ x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * delta.head<2>();
+ x1(2) = pi2pi(x0(2) + delta(2));
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+ cap1->setData(delta);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
+ FactorBase::emplace<FactorOdom2d>(fea1, fea1, frm0, nullptr, false);
+
+ // Fix frm1, perturb frm0
+ frm1->fix();
+ frm0->unfix();
+ frm0->perturb(5);
+
+ // solve for frm0
+ std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_factor_odom_3d.cpp b/test/gtest_factor_odom_3d.cpp
index 1315118016ebe3319a04e07b37d0d8438b714ebe..8001bbe70f25f774a55cf0c834950529934bdb36 100644
--- a/test/gtest_factor_odom_3d.cpp
+++ b/test/gtest_factor_odom_3d.cpp
@@ -37,11 +37,11 @@ ProblemPtr problem_ptr = Problem::create("PO", 3);
CeresManager ceres_mgr(problem_ptr);
// Two frames
-FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, problem_ptr->zeroState(), TimeStamp(0));
-FrameBasePtr frm1 = problem_ptr->emplaceFrame(KEY, delta, TimeStamp(1));
+FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, TimeStamp(0), problem_ptr->stateZero());
+FrameBasePtr frm1 = problem_ptr->emplaceFrame(KEY, TimeStamp(1), delta);
// Capture, feature and factor from frm1 to frm0
-auto cap1 = CaptureBase::emplace<CaptureMotion>(frm1, "CaptureOdom3d", 1, nullptr, delta, 7, 6, nullptr);
+auto cap1 = CaptureBase::emplace<CaptureMotion>(frm1, "CaptureOdom3d", 1, nullptr, delta, nullptr);
auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom3d", delta, data_cov);
FactorOdom3dPtr ctr1 = FactorBase::emplace<FactorOdom3d>(fea1, fea1, frm0, nullptr, false); // create and add
@@ -68,7 +68,7 @@ TEST(FactorOdom3d, fix_0_solve)
// solve for frm1
std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF);
- ASSERT_MATRIX_APPROX(frm1->getState(), delta, 1e-6);
+ ASSERT_MATRIX_APPROX(frm1->getStateVector(), delta, 1e-6);
}
@@ -82,7 +82,7 @@ TEST(FactorOdom3d, fix_1_solve)
// solve for frm0
std::string brief_report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF);
- ASSERT_MATRIX_APPROX(frm0->getState(), problem_ptr->zeroState(), 1e-6);
+ ASSERT_MATRIX_APPROX(frm0->getStateVector(), problem_ptr->stateZero().vector("PO"), 1e-6);
}
int main(int argc, char **argv)
diff --git a/test/gtest_factor_pose_2d.cpp b/test/gtest_factor_pose_2d.cpp
index 5d9cb7f406beef3b95ce1da4add8f2041a20c685..e3107b7a5f976f5626f22016f64f60695e1fc982 100644
--- a/test/gtest_factor_pose_2d.cpp
+++ b/test/gtest_factor_pose_2d.cpp
@@ -30,10 +30,10 @@ ProblemPtr problem = Problem::create("PO", 2);
CeresManager ceres_mgr(problem);
// Two frames
-FrameBasePtr frm0 = problem->emplaceFrame(KEY, problem->zeroState(), TimeStamp(0));
+FrameBasePtr frm0 = problem->emplaceFrame(KEY, TimeStamp(0), problem->stateZero());
// Capture, feature and factor from frm1 to frm0
-auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "CaptureOdom2d", 0, nullptr, pose, 3, 3, nullptr);
+auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "CaptureOdom2d", 0, nullptr, pose, nullptr);
auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "FeatureOdom2d", pose, data_cov);
auto ctr0 = FactorBase::emplace<FactorPose2d>(fea0, fea0, nullptr, false);
@@ -59,7 +59,7 @@ TEST(FactorPose2d, solve)
// solve for frm0
std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport
- ASSERT_MATRIX_APPROX(frm0->getState(), pose, 1e-6);
+ ASSERT_MATRIX_APPROX(frm0->getStateVector(), pose, 1e-6);
}
diff --git a/test/gtest_factor_pose_3d.cpp b/test/gtest_factor_pose_3d.cpp
index 9eeb5dfc14bbc6b185c598a8a17489a44794206e..d57aee85c27b47e41f9610040c3b8bcceb57ad35 100644
--- a/test/gtest_factor_pose_3d.cpp
+++ b/test/gtest_factor_pose_3d.cpp
@@ -36,10 +36,10 @@ ProblemPtr problem = Problem::create("PO", 3);
CeresManager ceres_mgr(problem);
// Two frames
-FrameBasePtr frm0 = problem->emplaceFrame(KEY, problem->zeroState(), TimeStamp(0));
+FrameBasePtr frm0 = problem->emplaceFrame(KEY, 0, problem->stateZero() );
// Capture, feature and factor
-auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "CaptureOdom3d", 0, nullptr, pose7, 7, 6, nullptr);
+auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "CaptureOdom3d", 0, nullptr, pose7, nullptr);
auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "FeatureOdom3d", pose7, data_cov);
FactorPose3dPtr ctr0 = FactorBase::emplace<FactorPose3d>(fea0, fea0, nullptr, false);
@@ -65,7 +65,7 @@ TEST(FactorPose3d, solve)
// solve for frm0
std::string brief_report = ceres_mgr.solve(SolverManager::ReportVerbosity::FULL);
- ASSERT_MATRIX_APPROX(frm0->getState(), pose7, 1e-6);
+ ASSERT_MATRIX_APPROX(frm0->getStateVector(), pose7, 1e-6);
}
diff --git a/test/gtest_factor_relative_2d_analytic.cpp b/test/gtest_factor_relative_2d_analytic.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c461bbb94743c9ee8b3665650fb00686d979fc20
--- /dev/null
+++ b/test/gtest_factor_relative_2d_analytic.cpp
@@ -0,0 +1,119 @@
+#include "core/utils/utils_gtest.h"
+
+#include "core/factor/factor_relative_2d_analytic.h"
+#include "core/capture/capture_odom_2d.h"
+#include "core/math/rotations.h"
+
+#include "core/ceres_wrapper/ceres_manager.h"
+
+using namespace Eigen;
+using namespace wolf;
+using std::cout;
+using std::endl;
+
+// Input odometry data and covariance
+Matrix3d data_cov = 0.1*Matrix3d::Identity();
+
+// Problem and solver
+ProblemPtr problem_ptr = Problem::create("PO", 2);
+CeresManager ceres_mgr(problem_ptr);
+
+// Two frames
+FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, 0.0, Vector3d::Zero());
+FrameBasePtr frm1 = problem_ptr->emplaceFrame(KEY, 1.0, Vector3d::Zero());
+
+// Capture from frm1 to frm0
+auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, nullptr, Vector3d::Zero(), data_cov);
+
+TEST(FactorRelative2dAnalytic, check_tree)
+{
+ ASSERT_TRUE(problem_ptr->check(0));
+}
+
+TEST(FactorRelative2dAnalytic, fix_0_solve)
+{
+ for (int i = 0; i < 1e3; i++)
+ {
+ // Random delta
+ Vector3d delta = Vector3d::Random() * 10;
+ pi2pi(delta(2));
+
+ // Random initial pose
+ Vector3d x0 = Vector3d::Random() * 10;
+ pi2pi(x0(2));
+
+ // x1 groundtruth
+ Vector3d x1;
+ x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * delta.head<2>();
+ x1(2) = pi2pi(x0(2) + delta(2));
+
+ // Set states and measurement
+ frm0->setState(x0);
+ frm1->setState(x1);
+ cap1->setData(delta);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
+ FactorBase::emplace<FactorRelative2dAnalytic>(fea1, "odom2d", fea1, frm0, nullptr, false);
+
+ // Fix frm0, perturb frm1
+ frm0->fix();
+ frm1->unfix();
+ frm1->perturb(5);
+
+ // solve for frm1
+ std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
+}
+
+TEST(FactorRelative2dAnalytic, fix_1_solve)
+{
+ for (int i = 0; i < 1e3; i++)
+ {
+ // Random delta
+ Vector3d delta = Vector3d::Random() * 10;
+ pi2pi(delta(2));
+
+ // Random initial pose
+ Vector3d x0 = Vector3d::Random() * 10;
+ pi2pi(x0(2));
+
+ // x1 groundtruth
+ Vector3d x1;
+ x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * delta.head<2>();
+ x1(2) = pi2pi(x0(2) + delta(2));
+
+ // Set states and measurement
+ frm0->setState(x0);
+ frm1->setState(x1);
+ cap1->setData(delta);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
+ FactorBase::emplace<FactorRelative2dAnalytic>(fea1, "odom2d", fea1, frm0, nullptr, false);
+
+ // Fix frm1, perturb frm0
+ frm1->fix();
+ frm0->unfix();
+ frm0->perturb(5);
+
+ // solve for frm0
+ std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp b/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b9ad5bbe208d981a4ca69aa3eab8eac806f62e14
--- /dev/null
+++ b/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
@@ -0,0 +1,239 @@
+#include "core/utils/utils_gtest.h"
+
+#include "core/factor/factor_relative_pose_2d_with_extrinsics.h"
+#include "core/capture/capture_odom_2d.h"
+#include "core/sensor/sensor_odom_2d.h"
+#include "core/processor/processor_odom_2d.h"
+#include "core/math/rotations.h"
+
+#include "core/ceres_wrapper/ceres_manager.h"
+
+using namespace Eigen;
+using namespace wolf;
+using std::cout;
+using std::endl;
+
+std::string wolf_root = _WOLF_ROOT_DIR;
+
+// Input odometry data and covariance
+Matrix3d data_cov = 0.1*Matrix3d::Identity();
+
+// Problem and solver
+ProblemPtr problem_ptr = Problem::create("PO", 2);
+CeresManager ceres_mgr(problem_ptr);
+
+// Sensor
+auto sensor_odom2d = problem_ptr->installSensor("SensorOdom2d", "odom", Vector3d(0,0,0), wolf_root + "/test/yaml/sensor_odom_2d.yaml");
+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() );
+
+// Capture from frm1 to frm0
+auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, sensor_odom2d, Vector3d::Zero(), data_cov);
+
+TEST(FactorRelativePose2dWithExtrinsics, check_tree)
+{
+ ASSERT_TRUE(problem_ptr->check(0));
+}
+
+TEST(FactorRelativePose2dWithExtrinsics, fix_0_solve)
+{
+ for (int i = 0; i < 1e3; i++)
+ {
+ // Random delta
+ Vector3d delta = Vector3d::Random() * 10;
+ pi2pi(delta(2));
+
+ // Random initial pose
+ Vector3d x0 = Vector3d::Random() * 10;
+ pi2pi(x0(2));
+
+ // Random extrinsics
+ Vector3d xs = Vector3d::Random();
+ pi2pi(xs(2));
+
+ // x1 groundtruth
+ Vector3d x1;
+ x1(2) = pi2pi(x0(2) + delta(2));
+ x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * (xs.head<2>() + Rotation2Dd(xs(2)) * delta.head<2>()) - Rotation2Dd(x1(2)) * xs.head<2>();
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+ cap1->setData(delta);
+ sensor_odom2d->setState(xs);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
+ FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(fea1, fea1, frm0, processor_odom2d, false); // create and add
+
+ // Perturb frm1, fix the rest
+ frm0->fix();
+ frm1->unfix();
+ sensor_odom2d->getP()->fix();
+ sensor_odom2d->getO()->fix();
+ frm1->perturb(5);
+
+ // solve for frm1
+ std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
+}
+
+TEST(FactorRelativePose2dWithExtrinsics, fix_1_solve)
+{
+ for (int i = 0; i < 1e3; i++)
+ {
+ // Random delta
+ Vector3d delta = Vector3d::Random() * 10;
+ pi2pi(delta(2));
+
+ // Random initial pose
+ Vector3d x0 = Vector3d::Random() * 10;
+ pi2pi(x0(2));
+
+ // Random extrinsics
+ Vector3d xs = Vector3d::Random();
+ pi2pi(xs(2));
+
+ // x1 groundtruth
+ Vector3d x1;
+ x1(2) = pi2pi(x0(2) + delta(2));
+ x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * (xs.head<2>() + Rotation2Dd(xs(2)) * delta.head<2>()) - Rotation2Dd(x1(2)) * xs.head<2>();
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+ cap1->setData(delta);
+ sensor_odom2d->setState(xs);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
+ FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(fea1, fea1, frm0, processor_odom2d, false); // create and add
+
+ // Perturb frm0, fix the rest
+ frm1->fix();
+ frm0->unfix();
+ sensor_odom2d->getP()->fix();
+ sensor_odom2d->getO()->fix();
+ frm0->perturb(5);
+
+ // solve for frm0
+ std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
+}
+
+TEST(FactorRelativePose2dWithExtrinsics, extrinsics_p_solve)
+{
+ for (int i = 0; i < 1e3; i++)
+ {
+ // Random delta
+ Vector3d delta = Vector3d::Random() * 10;
+ pi2pi(delta(2));
+
+ // Random initial pose
+ Vector3d x0 = Vector3d::Random() * 10;
+ pi2pi(x0(2));
+
+ // Random extrinsics
+ Vector3d xs = Vector3d::Random();
+ pi2pi(xs(2));
+
+ // x1 groundtruth
+ Vector3d x1;
+ x1(2) = pi2pi(x0(2) + delta(2));
+ x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * (xs.head<2>() + Rotation2Dd(xs(2)) * delta.head<2>()) - Rotation2Dd(x1(2)) * xs.head<2>();
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+ cap1->setData(delta);
+ sensor_odom2d->setState(xs);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
+ FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(fea1, fea1, frm0, processor_odom2d, false); // create and add
+
+ // Perturb sensor P, fix the rest
+ frm1->fix();
+ frm0->fix();
+ sensor_odom2d->getP()->unfix();
+ sensor_odom2d->getO()->fix();
+ sensor_odom2d->getP()->perturb(1);
+
+ // solve for frm0
+ std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(sensor_odom2d->getStateVector(), xs, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
+}
+
+TEST(FactorRelativePose2dWithExtrinsics, extrinsics_o_solve)
+{
+ for (int i = 0; i < 1e3; i++)
+ {
+ // Random delta
+ Vector3d delta = Vector3d::Random() * 10;
+ pi2pi(delta(2));
+
+ // Random initial pose
+ Vector3d x0 = Vector3d::Random() * 10;
+ pi2pi(x0(2));
+
+ // Random extrinsics
+ Vector3d xs = Vector3d::Random();
+ pi2pi(xs(2));
+
+ // x1 groundtruth
+ Vector3d x1;
+ x1(2) = pi2pi(x0(2) + delta(2));
+ x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * (xs.head<2>() + Rotation2Dd(xs(2)) * delta.head<2>()) - Rotation2Dd(x1(2)) * xs.head<2>();
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+ cap1->setData(delta);
+ sensor_odom2d->setState(xs);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
+ FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(fea1, fea1, frm0, processor_odom2d, false); // create and add
+
+ // Perturb sensor O, fix the rest
+ frm1->fix();
+ frm0->fix();
+ sensor_odom2d->getP()->fix();
+ sensor_odom2d->getO()->unfix();
+ sensor_odom2d->getO()->perturb(.2);
+
+ //std::cout << "Sensor O (perturbed): " << sensor_odom2d->getO()->getState().transpose() << std::endl;
+
+ // solve for frm0
+ std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(sensor_odom2d->getStateVector(), xs, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_factory_state_block.cpp b/test/gtest_factory_state_block.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fbded073c2e9a226c0f66b1feb34388bd5c06dfc
--- /dev/null
+++ b/test/gtest_factory_state_block.cpp
@@ -0,0 +1,157 @@
+/*
+ * gtest_factory_state_block.cpp
+ *
+ * Created on: Apr 28, 2020
+ * Author: jsola
+ */
+
+#include "core/common/wolf.h"
+#include "core/state_block/factory_state_block.h"
+#include "core/sensor/factory_sensor.h"
+
+#include "core/utils/utils_gtest.h"
+#include "core/utils/logging.h"
+
+using namespace wolf;
+
+/*
+// You may use this to make some methods of Foo public
+WOLF_PTR_TYPEDEFS(FooPublic);
+class FooPublic : public Foo
+{
+ // You may use this to make some methods of Foo public
+}
+
+class TestInit : public testing::Test
+{
+ public:
+ // You may use this to initialize stuff
+ // Combine it with TEST_F(FooTest, testName) { }
+ void SetUp() override
+ {
+ // Init all you want here
+ // e.g. FooPublic foo;
+ }
+ void TearDown() override {} // you can delete this if unused
+};
+
+TEST_F(TestInit, testName)
+{
+ // Use class TestInit
+}
+*/
+
+TEST(FactoryStateBlock, get_getClass)
+{
+ const auto& f = FactoryStateBlock::get();
+
+ const std::string& s = f.getClass();
+
+ ASSERT_EQ(s, "FactoryStateBlock");
+}
+
+TEST(FactoryStateBlock, creator_default)
+{
+ auto sbp = FactoryStateBlock::get().create("P", Eigen::Vector3d(1,2,3), false);
+ auto sbv = FactoryStateBlock::get().create("V", Eigen::Vector2d(4,5), false);
+ auto sbw = FactoryStateBlock::get().create("W", Eigen::Vector1d(6), false);
+
+ ASSERT_MATRIX_APPROX(Eigen::Vector3d(1,2,3) , sbp->getState(), 1e-20);
+ ASSERT_MATRIX_APPROX(Eigen::Vector2d(4,5) , sbv->getState(), 1e-20);
+ ASSERT_MATRIX_APPROX(Eigen::Vector1d(6) , sbw->getState(), 1e-20);
+
+ ASSERT_FALSE(sbp->hasLocalParametrization());
+ ASSERT_FALSE(sbv->hasLocalParametrization());
+ ASSERT_FALSE(sbw->hasLocalParametrization());
+}
+
+TEST(FactoryStateBlock, creator_StateBlock)
+{
+ auto sbp = FactoryStateBlock::get().create("StateBlock", Eigen::Vector3d(1,2,3), false);
+ auto sbv = FactoryStateBlock::get().create("StateBlock", Eigen::Vector2d(4,5), true);
+ auto sbw = FactoryStateBlock::get().create("StateBlock", Eigen::Vector1d(6), false);
+
+ ASSERT_MATRIX_APPROX(Eigen::Vector3d(1,2,3) , sbp->getState(), 1e-20);
+ ASSERT_MATRIX_APPROX(Eigen::Vector2d(4,5) , sbv->getState(), 1e-20);
+ ASSERT_MATRIX_APPROX(Eigen::Vector1d(6) , sbw->getState(), 1e-20);
+
+ ASSERT_FALSE(sbp->isFixed());
+ ASSERT_TRUE (sbv->isFixed());
+ ASSERT_FALSE(sbw->isFixed());
+
+ ASSERT_FALSE(sbp->hasLocalParametrization());
+ ASSERT_FALSE(sbv->hasLocalParametrization());
+ ASSERT_FALSE(sbw->hasLocalParametrization());
+}
+
+TEST(FactoryStateBlock, creator_Quaternion)
+{
+ auto sbq = FactoryStateBlock::get().create("StateQuaternion", Eigen::Vector4d(1,2,3,4), false);
+
+ ASSERT_EQ(sbq->getSize() , 4);
+ ASSERT_EQ(sbq->getLocalSize(), 3);
+ ASSERT_TRUE(sbq->hasLocalParametrization());
+}
+
+TEST(FactoryStateBlock, creator_Angle)
+{
+ auto sba = FactoryStateBlock::get().create("StateAngle", Eigen::Vector1d(1), false);
+
+ ASSERT_EQ(sba->getSize() , 1);
+ ASSERT_EQ(sba->getLocalSize(), 1);
+ ASSERT_TRUE(sba->hasLocalParametrization());
+}
+
+TEST(FactoryStateBlock, creator_Homogeneous3d)
+{
+ auto sbh = FactoryStateBlock::get().create("StateHomogeneous3d", Eigen::Vector4d(1,2,3,4), false);
+
+ ASSERT_EQ(sbh->getSize() , 4);
+ ASSERT_EQ(sbh->getLocalSize(), 3);
+ ASSERT_TRUE(sbh->hasLocalParametrization());
+}
+
+TEST(FactoryStateBlock, creator_H)
+{
+ auto sbh = FactoryStateBlock::get().create("H", Eigen::Vector4d(1,2,3,4), false);
+
+ ASSERT_EQ(sbh->getSize() , 4);
+ ASSERT_EQ(sbh->getLocalSize(), 3);
+ ASSERT_TRUE(sbh->hasLocalParametrization());
+}
+
+TEST(FactoryStateBlock, creator_O_is_quaternion)
+{
+ auto sbq = FactoryStateBlock::get().create("O", Eigen::Vector4d(1,2,3,4), false);
+
+ ASSERT_EQ(sbq->getSize() , 4);
+ ASSERT_EQ(sbq->getLocalSize(), 3);
+ ASSERT_TRUE(sbq->hasLocalParametrization());
+}
+
+TEST(FactoryStateBlock, creator_O_is_angle)
+{
+ auto sba = FactoryStateBlock::get().create("O", Eigen::Vector1d(1), false);
+
+ ASSERT_EQ(sba->getSize() , 1);
+ ASSERT_EQ(sba->getLocalSize(), 1);
+ ASSERT_TRUE(sba->hasLocalParametrization());
+}
+
+TEST(FactoryStateBlock, creator_O_is_wrong_size)
+{
+ ASSERT_THROW(auto sba = FactoryStateBlock::get().create("O", Eigen::Vector2d(1,2), false) , std::length_error);
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+
+ // restrict to a group of tests only
+ //::testing::GTEST_FLAG(filter) = "TestInit.*";
+
+ // restrict to this test only
+ //::testing::GTEST_FLAG(filter) = "TestInit.testName";
+
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_feature_base.cpp b/test/gtest_feature_base.cpp
index 2824fe179096eaca65136b5fadb31b4053bd99cc..bbb978af9c69260bb409ee28e0c9da88214609f5 100644
--- a/test/gtest_feature_base.cpp
+++ b/test/gtest_feature_base.cpp
@@ -12,7 +12,7 @@
using namespace wolf;
using namespace Eigen;
-TEST(FeatureBase, Constructor)
+TEST(FeatureBase, ConstructorCov)
{
Vector3d m; m << 1,2,3;
Matrix3d M; M.setRandom(); M = M*M.transpose();
@@ -29,7 +29,7 @@ TEST(FeatureBase, Constructor)
ASSERT_MATRIX_APPROX(f->getMeasurementSquareRootInformationUpper(), U, 1e-6);
}
-TEST(FeatureBase, SetMeasurement)
+TEST(FeatureBase, ConstructorInfo)
{
Vector3d m; m << 1,2,3;
Matrix3d M; M.setRandom(); M = M*M.transpose();
@@ -38,26 +38,9 @@ TEST(FeatureBase, SetMeasurement)
Eigen::MatrixXd U = llt_of_info.matrixU();
Eigen::MatrixXd L = llt_of_info.matrixL();
- FeatureBasePtr f(std::make_shared<FeatureBase>("DUMMY", Vector3d::Zero(), Matrix3d::Identity()));
-
- f->setMeasurement(m);
+ FeatureBasePtr f(std::make_shared<FeatureBase>("DUMMY", m, I, FeatureBase::UncertaintyType::UNCERTAINTY_IS_INFO));
ASSERT_MATRIX_APPROX(f->getMeasurement(), m, 1e-6);
-}
-
-TEST(FeatureBase, SetMeasurementCovariance)
-{
- Vector3d m; m << 1,2,3;
- Matrix3d M; M.setRandom(); M = M*M.transpose();
- Matrix3d I = M.inverse();
- Eigen::LLT<Eigen::MatrixXd> llt_of_info(I); // compute the Cholesky decomposition of A
- Eigen::MatrixXd U = llt_of_info.matrixU();
- Eigen::MatrixXd L = llt_of_info.matrixL();
-
- FeatureBasePtr f(std::make_shared<FeatureBase>("DUMMY", Vector3d::Zero(), Matrix3d::Identity()));
-
- f->setMeasurementCovariance(M);
-
ASSERT_MATRIX_APPROX(f->getMeasurementCovariance(), M, 1e-6);
ASSERT_MATRIX_APPROX(f->getMeasurementSquareRootInformationUpper().transpose() * f->getMeasurementSquareRootInformationUpper(), U.transpose()*U, 1e-6);
ASSERT_MATRIX_APPROX(f->getMeasurementSquareRootInformationUpper(), U, 1e-6);
diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp
index 77fa1ace2c4bf876b0c318fe79dc476688e92af0..9d1368d3b673bb1d00baf11c8100273cbc813d1d 100644
--- a/test/gtest_frame_base.cpp
+++ b/test/gtest_frame_base.cpp
@@ -22,7 +22,7 @@ using namespace wolf;
TEST(FrameBase, GettersAndSetters)
{
- FrameBasePtr F = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+ FrameBasePtr F = make_shared<FrameBase>(NON_ESTIMATED, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
// getters
ASSERT_EQ(F->id(), (unsigned int) 1);
@@ -37,7 +37,7 @@ TEST(FrameBase, GettersAndSetters)
TEST(FrameBase, StateBlocks)
{
- FrameBasePtr F = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+ FrameBasePtr F = make_shared<FrameBase>(NON_ESTIMATED, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
ASSERT_EQ(F->getStateBlockMap().size(),(unsigned int) 2);
ASSERT_EQ(F->getP()->getState().size(),(unsigned int) 2);
@@ -47,7 +47,7 @@ TEST(FrameBase, StateBlocks)
TEST(FrameBase, LinksBasic)
{
- FrameBasePtr F = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+ FrameBasePtr F = make_shared<FrameBase>(NON_ESTIMATED, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
ASSERT_FALSE(F->getTrajectory());
ASSERT_FALSE(F->getProblem());
@@ -69,10 +69,15 @@ 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, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
- auto F2 = FrameBase::emplace<FrameBase>(T, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
- auto C = CaptureBase::emplace<CaptureMotion>(F1, "CaptureMotion", 1, S, Vector3d::Zero(), 3, 3, nullptr);
- auto p = ProcessorBase::emplace<ProcessorOdom2d>(S, std::make_shared<ProcessorParamsOdom2d>());
+ 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 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>());
+ WOLF_INFO("F2->getCaptureList().size() ", F2->getCaptureList().size());
+ p->link(S);
+ //auto p = ProcessorBase::emplace<ProcessorOdom2d>(S, std::make_shared<ParamsProcessorOdom2d>());
+ WOLF_INFO("F2->getCaptureList().size() ", F2->getCaptureList().size());
auto f = FeatureBase::emplace<FeatureBase>(C, "f", Vector1d(1), Matrix<double,1,1>::Identity()*.01);
auto c = FactorBase::emplace<FactorOdom2d>(f, f, F2, p, false);
@@ -126,7 +131,7 @@ TEST(FrameBase, GetSetState)
StateQuaternionPtr sbq = make_shared<StateQuaternion>();
// Create frame
- FrameBase F(1, sbp, sbq, sbv);
+ FrameBase F(NON_ESTIMATED, 1, sbp, sbq, sbv);
// Give values to vectors and vector blocks
VectorXd x(10), x1(10), x2(10);
@@ -143,15 +148,39 @@ TEST(FrameBase, GetSetState)
ASSERT_MATRIX_APPROX(q, F.getO()->getState(), Constants::EPS_SMALL);
ASSERT_MATRIX_APPROX(v, F.getV()->getState(), Constants::EPS_SMALL);
- // Get the state, form 1 by reference
- F.getState(x1);
- ASSERT_MATRIX_APPROX(x1 , x, Constants::EPS_SMALL);
-
// get the state, form 2 by return value
- x2 = F.getState();
+ x2 = F.getStateVector();
ASSERT_MATRIX_APPROX(x2, x, Constants::EPS_SMALL);
}
+TEST(FrameBase, Constructor_composite)
+{
+ FrameBase F = FrameBase(KEY,
+ 0.0,
+ "POV",
+ VectorComposite("POV", {Vector3d(1,2,3), Vector4d(1,2,3,4).normalized(), Vector3d(1,2,3)}));
+
+ ASSERT_TRUE (F.isKey());
+ ASSERT_MATRIX_APPROX(F.getP()->getState(), Vector3d(1,2,3), 1e-15);
+ ASSERT_TRUE (F.getO()->hasLocalParametrization());
+ ASSERT_EQ (F.getStateBlock("V")->getSize(), 3);
+}
+
+TEST(FrameBase, Constructor_vectors)
+{
+ FrameBase F = FrameBase(KEY,
+ 0.0,
+ "POV",
+ {Vector3d(1,2,3), Vector4d(1,2,3,4).normalized(), Vector3d(1,2,3)});
+
+ ASSERT_TRUE (F.isKey());
+ ASSERT_MATRIX_APPROX(F.getP()->getState(), Vector3d(1,2,3), 1e-15);
+ ASSERT_TRUE (F.getO()->hasLocalParametrization());
+ ASSERT_EQ (F.getStateBlock("V")->getSize(), 3);
+}
+
+
+
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
diff --git a/test/gtest_has_state_blocks.cpp b/test/gtest_has_state_blocks.cpp
index df90859ba57d46382d97e7fa29fa66b23b137580..111ccd08d5653eab1bb73a71a9711dc426a2ab08 100644
--- a/test/gtest_has_state_blocks.cpp
+++ b/test/gtest_has_state_blocks.cpp
@@ -41,8 +41,8 @@ class HasStateBlocksTest : public testing::Test
sbo1 = std::make_shared<StateQuaternion>();
sbv1 = std::make_shared<StateBlock>(3); // size 3
- F0 = std::make_shared<FrameBase>(0.0, sbp0, sbo0); // non KF
- F1 = std::make_shared<FrameBase>(1.0, nullptr); // non KF
+ F0 = std::make_shared<FrameBase>(NON_ESTIMATED, 0.0, sbp0, sbo0); // non KF
+ F1 = std::make_shared<FrameBase>(NON_ESTIMATED, 1.0, nullptr); // non KF
}
virtual void TearDown(){}
diff --git a/test/gtest_motion_buffer.cpp b/test/gtest_motion_buffer.cpp
index 987c713a245df58dba2c32e9edde9505bcd15d56..3c74ede52ce7ec57a153cdcf416b9c84597cadd9 100644
--- a/test/gtest_motion_buffer.cpp
+++ b/test/gtest_motion_buffer.cpp
@@ -42,8 +42,8 @@ TEST(MotionBuffer, QueryTimeStamps)
{
MotionBuffer MB;
- MB.get().push_back(m0);
- MB.get().push_back(m1);
+ MB.push_back(m0);
+ MB.push_back(m1);
TimeStamp t;
t.set(-1); // t is older than m0.ts_ -> return m0
@@ -66,10 +66,10 @@ TEST(MotionBuffer, getMotion)
{
MotionBuffer MB;
- MB.get().push_back(m0);
+ MB.push_back(m0);
ASSERT_EQ(MB.getMotion(t0).delta_, m0.delta_);
- MB.get().push_back(m1);
+ MB.push_back(m1);
ASSERT_EQ(MB.getMotion(t0).delta_, m0.delta_);
ASSERT_EQ(MB.getMotion(t1).delta_, m1.delta_);
ASSERT_EQ(MB.getMotion(t0).delta_integr_, m0.delta_integr_);
@@ -80,11 +80,11 @@ TEST(MotionBuffer, getDelta)
{
MotionBuffer MB;
- MB.get().push_back(m0);
+ MB.push_back(m0);
ASSERT_EQ(MB.getMotion(t0).delta_integr_, m0.delta_integr_);
- MB.get().push_back(m1);
+ MB.push_back(m1);
ASSERT_EQ(MB.getMotion(t0).delta_integr_, m0.delta_integr_);
ASSERT_EQ(MB.getMotion(t1).delta_integr_, m1.delta_integr_);
@@ -94,19 +94,19 @@ TEST(MotionBuffer, Split)
{
MotionBuffer MB;
- MB.get().push_back(m0);
- MB.get().push_back(m1);
- MB.get().push_back(m2);
- MB.get().push_back(m3);
- MB.get().push_back(m4); // put 5 motions
+ MB.push_back(m0);
+ MB.push_back(m1);
+ MB.push_back(m2);
+ MB.push_back(m3);
+ MB.push_back(m4); // put 5 motions
MotionBuffer MB_old;
TimeStamp t = 1.5; // between m1 and m2
MB.split(t, MB_old);
- ASSERT_EQ(MB_old.get().size(), (unsigned int) 2);
- ASSERT_EQ(MB .get().size(), (unsigned int) 3);
+ ASSERT_EQ(MB_old.size(), (unsigned int) 2);
+ ASSERT_EQ(MB .size(), (unsigned int) 3);
ASSERT_EQ(MB_old.getMotion(t1).ts_, t1);
ASSERT_EQ(MB_old.getMotion(t2).ts_, t1); // last ts is t1
@@ -118,11 +118,11 @@ TEST(MotionBuffer, Split)
// {
// MotionBuffer MB;
//
-// MB.get().push_back(m0);
-// MB.get().push_back(m1);
-// MB.get().push_back(m2);
-// MB.get().push_back(m3);
-// MB.get().push_back(m4); // put 5 motions
+// MB.push_back(m0);
+// MB.push_back(m1);
+// MB.push_back(m2);
+// MB.push_back(m3);
+// MB.push_back(m4); // put 5 motions
//
// Eigen::MatrixXd cov = MB.integrateCovariance();
// ASSERT_NEAR(cov(0), 0.04, 1e-8);
@@ -133,11 +133,11 @@ TEST(MotionBuffer, Split)
// {
// MotionBuffer MB;
//
-// MB.get().push_back(m0);
-// MB.get().push_back(m1);
-// MB.get().push_back(m2);
-// MB.get().push_back(m3);
-// MB.get().push_back(m4); // put 5 motions
+// MB.push_back(m0);
+// MB.push_back(m1);
+// MB.push_back(m2);
+// MB.push_back(m3);
+// MB.push_back(m4); // put 5 motions
//
// Eigen::MatrixXd cov = MB.integrateCovariance(t2);
// ASSERT_NEAR(cov(0), 0.02, 1e-8);
@@ -147,11 +147,11 @@ TEST(MotionBuffer, Split)
// {
// MotionBuffer MB;
//
-// MB.get().push_back(m0);
-// MB.get().push_back(m1);
-// MB.get().push_back(m2);
-// MB.get().push_back(m3);
-// MB.get().push_back(m4); // put 5 motions
+// MB.push_back(m0);
+// MB.push_back(m1);
+// MB.push_back(m2);
+// MB.push_back(m3);
+// MB.push_back(m4); // put 5 motions
//
// Eigen::MatrixXd cov = MB.integrateCovariance(t1, t3);
// ASSERT_NEAR(cov(0), 0.03, 1e-8);
@@ -164,9 +164,9 @@ TEST(MotionBuffer, print)
{
MotionBuffer MB;
- MB.get().push_back(m0);
- MB.get().push_back(m1);
- MB.get().push_back(m2);
+ MB.push_back(m0);
+ MB.push_back(m1);
+ MB.push_back(m2);
MB.print();
MB.print(0,0,0,0);
diff --git a/test/gtest_odom_2d.cpp b/test/gtest_odom_2d.cpp
index 1176783c855954afba8bf5e0a7566fb37ecbc8b5..8d97fc8d063ed1a08c36b25f259353cc74852f76 100644
--- a/test/gtest_odom_2d.cpp
+++ b/test/gtest_odom_2d.cpp
@@ -92,7 +92,7 @@ void show(const ProblemPtr& problem)
<< F->getCaptureList().front()->getFeatureList().front()->getMeasurementCovariance() << endl;
}
}
- cout << " state: \n" << F->getState().transpose() << endl;
+ cout << " state: \n" << F->getStateVector().transpose() << endl;
Eigen::MatrixXd cov;
problem->getFrameCovariance(F,cov);
cout << " covariance: \n" << cov << endl;
@@ -119,8 +119,8 @@ TEST(Odom2d, FactorFix_and_FactorOdom2d)
TimeStamp t0(0.0), t = t0;
double dt = .01;
- Vector3d x0 (0,0,0);
- Eigen::Matrix3d P0 = Eigen::Matrix3d::Identity() * 0.1;
+ VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
+ VectorComposite s0(Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)), "PO", {2,1});
Vector3d delta (2,0,0);
Matrix3d delta_cov; delta_cov << .02, 0, 0, 0, .025, .02, 0, .02, .02;
@@ -128,18 +128,18 @@ TEST(Odom2d, FactorFix_and_FactorOdom2d)
CeresManager ceres_manager(Pr);
// KF0 and absolute prior
- FrameBasePtr F0 = Pr->setPrior(x0, P0,t0, dt/2);
+ FrameBasePtr F0 = Pr->setPriorFactor(x0, s0,t0, dt/2);
// KF1 and motion from KF0
t += dt;
- FrameBasePtr F1 = Pr->emplaceFrame(KEY, Vector3d::Zero(), t);
+ FrameBasePtr F1 = Pr->emplaceFrame(KEY, 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, Vector3d::Zero(), t);
+ FrameBasePtr F2 = Pr->emplaceFrame(KEY, 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);
@@ -170,11 +170,12 @@ TEST(Odom2d, FactorFix_and_FactorOdom2d)
ASSERT_TRUE(Pr->getFrameCovariance(F1, P1_solver));
ASSERT_TRUE(Pr->getFrameCovariance(F2, P2_solver));
- ASSERT_POSE2d_APPROX(F0->getState(), Vector3d(0,0,0), 1e-6);
- ASSERT_MATRIX_APPROX(P0_solver, P0, 1e-6);
- ASSERT_POSE2d_APPROX(F1->getState(), Vector3d(2,0,0), 1e-6);
+ ASSERT_POSE2d_APPROX(F0->getStateVector(), Vector3d(0,0,0), 1e-6);
+ auto s0_vector = s0.vector("PO");
+ ASSERT_MATRIX_APPROX(P0_solver, MatrixXd((s0_vector.array() * s0_vector.array()).matrix().asDiagonal()), 1e-6);
+ ASSERT_POSE2d_APPROX(F1->getStateVector(), Vector3d(2,0,0), 1e-6);
ASSERT_MATRIX_APPROX(P1_solver, P1, 1e-6);
- ASSERT_POSE2d_APPROX(F2->getState(), Vector3d(4,0,0), 1e-6);
+ ASSERT_POSE2d_APPROX(F2->getStateVector(), Vector3d(4,0,0), 1e-6);
ASSERT_MATRIX_APPROX(P2_solver, P2, 1e-6);
}
@@ -187,8 +188,10 @@ TEST(Odom2d, VoteForKfAndSolve)
TimeStamp t0(0.0), t = t0;
double dt = .01;
// Origin frame:
- Vector3d x0(0,0,0);
- Eigen::Matrix3d P0 = Eigen::Matrix3d::Identity() * 0.1;
+ // Vector3d x0(0,0,0);
+ VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
+ // Eigen::Matrix3d P0 = Eigen::Matrix3d::Identity() * 0.1;
+ VectorComposite s0(Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)), "PO", {2,1});
// motion data
VectorXd data(Vector2d(1, M_PI_4) ); // advance 1m turn pi/4 rad (45 deg). Need 8 steps for a complete turn
Eigen::MatrixXd data_cov = Eigen::MatrixXd::Identity(2, 2) * 0.01;
@@ -197,12 +200,13 @@ TEST(Odom2d, VoteForKfAndSolve)
// Create Wolf tree nodes
ProblemPtr problem = Problem::create("PO", 2);
SensorBasePtr sensor_odom2d = problem->installSensor("SensorOdom2d", "odom", Vector3d(0,0,0), wolf_root + "/test/yaml/sensor_odom_2d.yaml");
- ProcessorParamsOdom2dPtr params(std::make_shared<ProcessorParamsOdom2d>());
+ ParamsProcessorOdom2dPtr params(std::make_shared<ParamsProcessorOdom2d>());
params->voting_active = true;
params->dist_traveled = 100;
- params->angle_turned = 6.28;
- params->max_time_span = 2.5*dt; // force KF at every third process()
+ params->angle_turned = data(1)*2.5; // force KF at every third process()
+ params->max_time_span = 100;
params->cov_det = 100;
+ params->time_tolerance = dt/2;
params->unmeasured_perturbation_std = 0.00;
Matrix3d unmeasured_cov = params->unmeasured_perturbation_std*params->unmeasured_perturbation_std*Matrix3d::Identity();
ProcessorBasePtr prc_base = problem->installProcessor("ProcessorOdom2d", "odom", sensor_odom2d, params);
@@ -211,13 +215,15 @@ TEST(Odom2d, VoteForKfAndSolve)
// NOTE: We integrate and create KFs as follows:
// i= 0 1 2 3 4 5 6
- // KF -- * -- * -- KF - * -- * -- KF - *
+ // KF - * -- * -- KF - * -- * -- KF - *
// Ceres wrapper
CeresManager ceres_manager(problem);
// Origin Key Frame
- FrameBasePtr origin_frame = problem->setPrior(x0, P0, t0, dt/2);
+ auto KF = problem->setPriorFactor(x0, s0, t, dt/2);
+ processor_odom2d->setOrigin(KF);
+
ceres_manager.solve(SolverManager::ReportVerbosity::BRIEF);
ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
@@ -226,8 +232,8 @@ TEST(Odom2d, VoteForKfAndSolve)
// std::cout << "Motion data : " << data.transpose() << std::endl;
// Check covariance values
- Eigen::Vector3d integrated_pose = x0;
- Eigen::Matrix3d integrated_cov = P0;
+ Eigen::Vector3d integrated_pose = Vector3d(0,0,0);
+ Eigen::Matrix3d integrated_cov = Eigen::Matrix3d::Identity() * 0.1;
Eigen::Vector3d integrated_delta = Eigen::Vector3d::Zero();
Eigen::Matrix3d integrated_delta_cov = Eigen::Matrix3d::Zero();
Eigen::MatrixXd Ju(3, 2);
@@ -246,7 +252,7 @@ TEST(Odom2d, VoteForKfAndSolve)
for (int n=1; n<=N; n++)
{
- // std::cout << "-------------------\nStep " << i << " at time " << t << std::endl;
+ std::cout << "-------------------\nStep " << n << " at time " << t << std::endl;
// re-use capture with updated timestamp
capture->setTimeStamp(t);
@@ -269,9 +275,9 @@ TEST(Odom2d, VoteForKfAndSolve)
integrated_delta_cov = Jx * integrated_delta_cov * Jx.transpose() + Ju * data_cov * Ju.transpose() + unmeasured_cov;
}
- WOLF_DEBUG("n: ", n, " t:", t);
- WOLF_DEBUG("wolf delta: ", processor_odom2d->getMotion().delta_integr_.transpose());
- WOLF_DEBUG("test delta: ", integrated_delta .transpose());
+ WOLF_INFO("n: ", n, " t:", t);
+ WOLF_INFO("wolf delta: ", processor_odom2d->getMotion().delta_integr_.transpose());
+ WOLF_INFO("test delta: ", integrated_delta .transpose());
ASSERT_POSE2d_APPROX(processor_odom2d->getMotion().delta_integr_, integrated_delta, 1e-6);
ASSERT_MATRIX_APPROX(odom2d_delta_cov, integrated_delta_cov, 1e-6);
@@ -282,7 +288,7 @@ TEST(Odom2d, VoteForKfAndSolve)
integrated_pose = plus(integrated_pose, data);
integrated_cov = Jx * integrated_cov * Jx.transpose() + Ju * data_cov * Ju.transpose() + unmeasured_cov;
- ASSERT_POSE2d_APPROX(processor_odom2d->getCurrentState(), integrated_pose, 1e-6);
+ ASSERT_POSE2d_APPROX(processor_odom2d->getState().vector("PO"), integrated_pose, 1e-6);
integrated_pose_vector.push_back(integrated_pose);
integrated_cov_vector.push_back(integrated_cov);
@@ -295,6 +301,8 @@ TEST(Odom2d, VoteForKfAndSolve)
ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
+ problem->print(4,1,1,1);
+
// Check the 3 KFs' states and covariances
MatrixXd computed_cov;
int n = 0;
@@ -302,7 +310,7 @@ TEST(Odom2d, VoteForKfAndSolve)
{
if (!F->isKey()) break;
- ASSERT_POSE2d_APPROX(F->getState(), integrated_pose_vector[n] , 1e-6);
+ ASSERT_POSE2d_APPROX(F->getStateVector("PO"), integrated_pose_vector[n] , 1e-6);
ASSERT_TRUE (F->getCovariance(computed_cov));
ASSERT_MATRIX_APPROX(computed_cov , integrated_cov_vector[n] , 1e-6);
n += 3;
@@ -320,8 +328,8 @@ TEST(Odom2d, KF_callback)
// time
TimeStamp t0(0.0), t = t0;
double dt = .01;
- Vector3d x0(0,0,0);
- Eigen::Matrix3d x0_cov = Eigen::Matrix3d::Identity() * 0.1;
+ VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
+ VectorComposite x0_cov(Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)), "PO", {2,1});
VectorXd data(Vector2d(1, M_PI/4) ); // advance 1m
Eigen::MatrixXd data_cov = Eigen::MatrixXd::Identity(2, 2) * 0.01;
int N = 8; // number of process() steps
@@ -337,11 +345,12 @@ TEST(Odom2d, KF_callback)
// Create Wolf tree nodes
ProblemPtr problem = Problem::create("PO", 2);
SensorBasePtr sensor_odom2d = problem->installSensor("SensorOdom2d", "odom", Vector3d(0,0,0), wolf_root + "/test/yaml/sensor_odom_2d.yaml");
- ProcessorParamsOdom2dPtr params(std::make_shared<ProcessorParamsOdom2d>());
+ ParamsProcessorOdom2dPtr params(std::make_shared<ParamsProcessorOdom2d>());
params->dist_traveled = 100;
- params->angle_turned = 6.28;
+ params->angle_turned = data(1)*2.5; // force KF at every third process()
params->max_time_span = 100;
params->cov_det = 100;
+ params->time_tolerance = dt/2;
params->unmeasured_perturbation_std = 0.000001;
Matrix3d unmeasured_cov = params->unmeasured_perturbation_std*params->unmeasured_perturbation_std*Matrix3d::Identity();
ProcessorBasePtr prc_base = problem->installProcessor("ProcessorOdom2d", "odom", sensor_odom2d, params);
@@ -352,11 +361,12 @@ TEST(Odom2d, KF_callback)
CeresManager ceres_manager(problem);
// Origin Key Frame
- FrameBasePtr keyframe_0 = problem->setPrior(x0, x0_cov, t0, dt/2);
+ FrameBasePtr keyframe_0 = problem->setPriorFactor(x0, x0_cov, t0, dt/2);
+ processor_odom2d->setOrigin(keyframe_0);
// Check covariance values
- Eigen::Vector3d integrated_pose = x0;
- Eigen::Matrix3d integrated_cov = x0_cov;
+ Eigen::Vector3d integrated_pose = Vector3d(0,0,0);
+ Eigen::Matrix3d integrated_cov = Eigen::Matrix3d::Identity() * 0.1;
Eigen::Vector3d integrated_delta = Eigen::Vector3d::Zero();
Eigen::Matrix3d integrated_delta_cov = Eigen::Matrix3d::Zero();
Eigen::MatrixXd Ju(3, 2);
@@ -412,8 +422,8 @@ TEST(Odom2d, KF_callback)
std::cout << "-----------------------------\nSplit after last KF; time: " << t_split << std::endl;
- Vector3d x_split = processor_odom2d->getState(t_split);
- FrameBasePtr keyframe_2 = problem->emplaceFrame(KEY, x_split, t_split);
+ Vector3d x_split = processor_odom2d->getState(t_split).vector("PO");
+ FrameBasePtr keyframe_2 = problem->emplaceFrame(KEY, t_split, x_split);
// there must be 2KF and one F
ASSERT_EQ(problem->getTrajectory()->getFrameList().size(), 3);
@@ -436,7 +446,7 @@ TEST(Odom2d, KF_callback)
// std::cout << report << std::endl;
ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
- ASSERT_POSE2d_APPROX(problem->getLastKeyFrame()->getState() , integrated_pose_vector[n_split], 1e-6);
+ ASSERT_POSE2d_APPROX(problem->getLastKeyFrame()->getStateVector() , integrated_pose_vector[n_split], 1e-6);
MatrixXd computed_cov;
ASSERT_TRUE(problem->getLastKeyFrameCovariance(computed_cov));
ASSERT_MATRIX_APPROX(computed_cov, integrated_cov_vector [n_split], 1e-6);
@@ -449,8 +459,8 @@ TEST(Odom2d, KF_callback)
problem->print(4,1,1,1);
- x_split = processor_odom2d->getState(t_split);
- FrameBasePtr keyframe_1 = problem->emplaceFrame(KEY, x_split, t_split);
+ x_split = processor_odom2d->getState(t_split).vector("PO");
+ FrameBasePtr keyframe_1 = problem->emplaceFrame(KEY, t_split, x_split);
ASSERT_TRUE(problem->check(0));
processor_odom2d->keyFrameCallback(keyframe_1, dt/2);
@@ -477,13 +487,13 @@ TEST(Odom2d, KF_callback)
// check the split KF
MatrixXd KF1_cov;
ASSERT_TRUE(problem->getFrameCovariance(keyframe_1, KF1_cov));
- ASSERT_POSE2d_APPROX(keyframe_1->getState(), integrated_pose_vector[m_split], 1e-6);
- ASSERT_MATRIX_APPROX(KF1_cov, integrated_cov_vector [m_split], 1e-6);
+ ASSERT_POSE2d_APPROX(keyframe_1->getStateVector(), integrated_pose_vector[m_split], 1e-6);
+ ASSERT_MATRIX_APPROX(KF1_cov, integrated_cov_vector [m_split], 1e-6);
// 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()->getState(), integrated_pose_vector[n_split], 1e-6);
+ ASSERT_POSE2d_APPROX(problem->getLastKeyFrame()->getStateVector(), integrated_pose_vector[n_split], 1e-6);
ASSERT_MATRIX_APPROX(KF2_cov, integrated_cov_vector [n_split], 1e-6);
// Check full trajectory
@@ -491,9 +501,9 @@ TEST(Odom2d, KF_callback)
for (int n=1; n<=N; n++)
{
t += dt;
- WOLF_DEBUG(" estimated(", t, ") = ", problem->getState(t).transpose());
+ WOLF_DEBUG(" estimated(", t, ") = ", problem->getState(t).vector("PO").transpose());
WOLF_DEBUG("ground truth(", t, ") = ", integrated_pose_vector[n].transpose());
- ASSERT_POSE2d_APPROX(problem->getState(t), integrated_pose_vector[n], 1e-6);
+ ASSERT_POSE2d_APPROX(problem->getState(t).vector("PO"), integrated_pose_vector[n], 1e-6);
}
}
diff --git a/test/gtest_pack_KF_buffer.cpp b/test/gtest_pack_KF_buffer.cpp
index 3862e263cb747b443e77ef113e9d85efa529f3a5..32bcd10d01a3604d16548cfd738d555af6b7e019 100644
--- a/test/gtest_pack_KF_buffer.cpp
+++ b/test/gtest_pack_KF_buffer.cpp
@@ -34,10 +34,10 @@ class BufferPackKeyFrameTest : public testing::Test
void SetUp(void)
{
- f10 = std::make_shared<FrameBase>(TimeStamp(10),nullptr,nullptr,nullptr);
- f20 = std::make_shared<FrameBase>(TimeStamp(20),nullptr,nullptr,nullptr);
- f21 = std::make_shared<FrameBase>(TimeStamp(21),nullptr,nullptr,nullptr);
- f28 = std::make_shared<FrameBase>(TimeStamp(28),nullptr,nullptr,nullptr);
+ f10 = std::make_shared<FrameBase>(NON_ESTIMATED, TimeStamp(10),nullptr,nullptr,nullptr);
+ f20 = std::make_shared<FrameBase>(NON_ESTIMATED, TimeStamp(20),nullptr,nullptr,nullptr);
+ f21 = std::make_shared<FrameBase>(NON_ESTIMATED, TimeStamp(21),nullptr,nullptr,nullptr);
+ f28 = std::make_shared<FrameBase>(NON_ESTIMATED, TimeStamp(28),nullptr,nullptr,nullptr);
tt10 = 1.0;
tt20 = 1.0;
@@ -137,7 +137,9 @@ TEST_F(BufferPackKeyFrameTest, selectPack)
{
PackKeyFramePtr packQ = pack_kf_buffer.selectPack(16, q[iq]);
if (packQ!=nullptr)
+ {
ASSERT_EQ(packQ->key_frame->getTimeStamp(),res(ip1*6+ip2*3+iq));
+ }
}
pack_kf_buffer.clear();
}
@@ -228,7 +230,7 @@ TEST_F(BufferPackKeyFrameTest, removeUpTo)
// Specifically, only f28 should remain
pack_kf_buffer.add(f28, tt28);
ASSERT_EQ(pack_kf_buffer.size(), (unsigned int) 2);
- FrameBasePtr f22 = std::make_shared<FrameBase>(TimeStamp(22),nullptr,nullptr,nullptr);
+ FrameBasePtr f22 = std::make_shared<FrameBase>(NON_ESTIMATED, TimeStamp(22),nullptr,nullptr,nullptr);
PackKeyFramePtr pack22 = std::make_shared<PackKeyFrame>(f22,5);
pack_kf_buffer.removeUpTo( pack22->key_frame->getTimeStamp() );
ASSERT_EQ(pack_kf_buffer.size(), (unsigned int) 1);
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index 6d262ba06b81438499b29ab3d45fd4bb101c89db..8ec6ebcae42990451909902ee8f245c35fbf16c4 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -6,7 +6,7 @@
*/
#include "core/utils/utils_gtest.h"
-#include "core/utils/logging.h"
+//#include "core/utils/logging.h"
#include "core/problem/problem.h"
#include "core/sensor/sensor_base.h"
@@ -15,6 +15,7 @@
#include "core/processor/processor_odom_3d.h"
#include "dummy/processor_tracker_feature_dummy.h"
#include "core/solver/solver_manager.h"
+#include "dummy/solver_manager_dummy.h"
#include "core/sensor/sensor_diff_drive.h"
#include "core/processor/processor_diff_drive.h"
@@ -31,41 +32,8 @@ using namespace wolf;
using namespace Eigen;
-// Register in the ProcessorFactory
-#include "core/processor/processor_factory.h"
-namespace wolf {
-WOLF_REGISTER_PROCESSOR("TRACKER FEATURE DUMMY", ProcessorTrackerFeatureDummy)
-} // namespace wolf
-
-
-WOLF_PTR_TYPEDEFS(DummySolverManager);
-
-class DummySolverManager : public SolverManager
-{
-public:
- DummySolverManager(const ProblemPtr& _problem)
- : SolverManager(_problem)
- {
- //
- }
- virtual void computeCovariances(const CovarianceBlocksToBeComputed blocks){};
- virtual void computeCovariances(const std::vector<StateBlockPtr>& st_list){};
- virtual bool hasConverged(){return true;};
- virtual SizeStd iterations(){return 0;};
- virtual double initialCost(){return 0;};
- virtual double finalCost(){return 0;};
- virtual std::string solveImpl(const ReportVerbosity report_level){return std::string("");};
- virtual void addFactor(const FactorBasePtr& fac_ptr){};
- virtual void removeFactor(const FactorBasePtr& fac_ptr){};
- virtual void addStateBlock(const StateBlockPtr& state_ptr){};
- virtual void removeStateBlock(const StateBlockPtr& state_ptr){};
- virtual void updateStateBlockStatus(const StateBlockPtr& state_ptr){};
- virtual void updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr){};
- virtual bool isStateBlockRegistered(const StateBlockPtr& state_ptr){return true;};
- virtual bool isFactorRegistered(const FactorBasePtr& fac_ptr) const {return true;};
- virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr){return true;};
- virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const {return true;};
-};
+// Register in the FactoryProcessor
+#include "core/processor/factory_processor.h"
TEST(Problem, create)
{
@@ -77,7 +45,7 @@ TEST(Problem, create)
ASSERT_EQ(P, P->getMap()->getProblem());
// check frame structure through the state size
- ASSERT_EQ(P->getFrameStructureSize(), 10);
+ ASSERT_EQ(P->getFrameStructure(), "POV");
}
TEST(Problem, Sensors)
@@ -104,7 +72,7 @@ TEST(Problem, Processor)
auto Sm = SensorBase::emplace<SensorOdom3d>(P->getHardware(), (Eigen::Vector7d()<<0,0,0, 0,0,0,1).finished(), ParamsSensorOdom3d());
// add motion processor
- auto Pm = ProcessorBase::emplace<ProcessorOdom3d>(Sm, std::make_shared<ProcessorParamsOdom3d>());
+ auto Pm = ProcessorBase::emplace<ProcessorOdom3d>(Sm, std::make_shared<ParamsProcessorOdom3d>());
// check motion processor IS NOT by emplace
ASSERT_EQ(P->getProcessorIsMotion(), Pm);
@@ -138,35 +106,56 @@ TEST(Problem, SetOrigin_PO_2d)
{
ProblemPtr P = Problem::create("PO", 2);
TimeStamp t0(0);
- Eigen::VectorXd x0(3); x0 << 1,2,3;
- Eigen::MatrixXd P0(3,3); P0.setIdentity(); P0 *= 0.1; // P0 is 0.1*Id
+ // Eigen::VectorXd x0(3); x0 << 1,2,3;
+ VectorComposite x0(Vector3d(1,2,3), "PO", {2,1});
+ // Eigen::MatrixXd P0(Eigen::MatrixXd::Identity(3,3) * 0.1); // P0 is 0.1*Id
+ VectorComposite s0(Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)), "PO", {2,1});
+
+ P->setPriorFactor(x0, s0, t0, 1.0);
- P->setPrior(x0, P0, t0, 1.0);
+ P->print(4,1,1,1);
// check that no sensor has been added
ASSERT_EQ(P->getHardware()->getSensorList().size(), (SizeStd) 0);
// check that the state is correct
- ASSERT_MATRIX_APPROX(x0, P->getCurrentState(), Constants::EPS_SMALL );
+ ASSERT_MATRIX_APPROX(x0.vector("PO"), P->getState().vector("PO"), Constants::EPS_SMALL );
// check that we have one frame, one capture, one feature, one factor
TrajectoryBasePtr T = P->getTrajectory();
- ASSERT_EQ(T->getFrameList().size(), (SizeStd) 1);
FrameBasePtr F = P->getLastFrame();
- ASSERT_EQ(F->getCaptureList().size(), (SizeStd) 1);
CaptureBasePtr C = F->getCaptureList().front();
- ASSERT_EQ(C->getFeatureList().size(), (SizeStd) 1);
- FeatureBasePtr f = C->getFeatureList().front();
- ASSERT_EQ(f->getFactorList().size(), (SizeStd) 1);
+ FeatureBasePtr fo = (*std::find_if(C->getFeatureList().begin(), C->getFeatureList().end(), [](FeatureBasePtr ftr){ return ftr->getType() == "Prior O";}));
+ FeatureBasePtr fp = (*std::find_if(C->getFeatureList().begin(), C->getFeatureList().end(), [](FeatureBasePtr ftr){ return ftr->getType() == "Prior P";}));
+ FactorBasePtrList fac_list;
+ F->getFactorList(fac_list);
+
+ // check that we have one frame (prior)
+ ASSERT_EQ(T->getFrameList().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)
+ ASSERT_EQ(C->getFeatureList().size(), (SizeStd) 2);
+ // check that we have two factors (prior: one per state block)
+ ASSERT_EQ(fac_list.size(), (SizeStd) 2);
- // check that the factor is absolute (no pointers to other F, f, or L)
- FactorBasePtr c = f->getFactorList().front();
- ASSERT_FALSE(c->getFrameOther());
- ASSERT_FALSE(c->getLandmarkOther());
+ // check that the factors are absolute (no pointers to other F, f, or L)
+ for (auto fac : fac_list)
+ {
+ ASSERT_FALSE(fac->getFrameOther());
+ ASSERT_FALSE(fac->getLandmarkOther());
+ ASSERT_FALSE(fac->getCaptureOther());
+ ASSERT_FALSE(fac->getFeatureOther());
+ }
+ auto x0_vector = x0.vector("PO");
+ auto s0_vector = s0.vector("PO");
+ MatrixXd P0_matrix = (s0_vector.array() * s0_vector.array()).matrix().asDiagonal();
// check that the Feature measurement and covariance are the ones provided
- ASSERT_MATRIX_APPROX(x0, f->getMeasurement(), Constants::EPS_SMALL );
- ASSERT_MATRIX_APPROX(P0, f->getMeasurementCovariance(), Constants::EPS_SMALL );
+ ASSERT_MATRIX_APPROX(x0_vector.head<2>(), fp->getMeasurement(), Constants::EPS_SMALL );
+ ASSERT_MATRIX_APPROX(x0_vector.tail<1>(), fo->getMeasurement(), Constants::EPS_SMALL );
+ ASSERT_MATRIX_APPROX(P0_matrix.topLeftCorner(2,2), fp->getMeasurementCovariance(), Constants::EPS_SMALL );
+ ASSERT_MATRIX_APPROX(P0_matrix.bottomRightCorner(1,1), fo->getMeasurementCovariance(), Constants::EPS_SMALL );
// P->print(4,1,1,1);
}
@@ -175,35 +164,57 @@ TEST(Problem, SetOrigin_PO_3d)
{
ProblemPtr P = Problem::create("PO", 3);
TimeStamp t0(0);
- Eigen::VectorXd x0(7); x0 << 1,2,3,4,5,6,7;
- Eigen::MatrixXd P0(6,6); P0.setIdentity(); P0 *= 0.1; // P0 is 0.1*Id
+ Eigen::VectorXd vec7(7); vec7 << 1,2,3,4,5,6,7; // not normalized quaternion, this is not what's tested
+ VectorComposite x0(vec7, "PO", {3,4});
+ // Eigen::MatrixXd P0(6,6); P0.setIdentity(); P0 *= 0.1; // P0 is 0.1*Id
+ Eigen::VectorXd vec6(6); vec6 << sqrt(0.1), sqrt(0.1), sqrt(0.1), sqrt(0.1), sqrt(0.1), sqrt(0.1);
+ VectorComposite s0(vec6, "PO", {3,3});
- P->setPrior(x0, P0, t0, 1.0);
+ P->setPriorFactor(x0, s0, t0, 1.0);
// check that no sensor has been added
ASSERT_EQ(P->getHardware()->getSensorList().size(), (SizeStd) 0);
// check that the state is correct
- ASSERT_TRUE((x0 - P->getCurrentState()).isMuchSmallerThan(1, Constants::EPS_SMALL));
+ ASSERT_TRUE((x0.vector("PO") - P->getState().vector("PO")).isMuchSmallerThan(1, Constants::EPS_SMALL));
// check that we have one frame, one capture, one feature, one factor
TrajectoryBasePtr T = P->getTrajectory();
- ASSERT_EQ(T->getFrameList().size(), (SizeStd) 1);
FrameBasePtr F = P->getLastFrame();
- ASSERT_EQ(F->getCaptureList().size(), (SizeStd) 1);
CaptureBasePtr C = F->getCaptureList().front();
- ASSERT_EQ(C->getFeatureList().size(), (SizeStd) 1);
- FeatureBasePtr f = C->getFeatureList().front();
- ASSERT_EQ(f->getFactorList().size(), (SizeStd) 1);
+ // FeatureBasePtr fo = C->getFeatureList().front();
+ // FeatureBasePtr fp = C->getFeatureList().back();
+ FeatureBasePtr fo = (*std::find_if(C->getFeatureList().begin(), C->getFeatureList().end(), [](FeatureBasePtr ftr){ return ftr->getType() == "Prior O";}));
+ FeatureBasePtr fp = (*std::find_if(C->getFeatureList().begin(), C->getFeatureList().end(), [](FeatureBasePtr ftr){ return ftr->getType() == "Prior P";}));
+ FactorBasePtrList fac_list;
+ F->getFactorList(fac_list);
+
+ // check that we have one frame (prior)
+ ASSERT_EQ(T->getFrameList().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)
+ ASSERT_EQ(C->getFeatureList().size(), (SizeStd) 2);
+ // check that we have two factors (prior: one per state block)
+ ASSERT_EQ(fac_list.size(), (SizeStd) 2);
- // check that the factor is absolute (no pointers to other F, f, or L)
- FactorBasePtr c = f->getFactorList().front();
- ASSERT_FALSE(c->getFrameOther());
- ASSERT_FALSE(c->getLandmarkOther());
+ // check that the factors are absolute (no pointers to other F, f, or L)
+ for (auto fac : fac_list)
+ {
+ ASSERT_FALSE(fac->getFrameOther());
+ ASSERT_FALSE(fac->getLandmarkOther());
+ ASSERT_FALSE(fac->getCaptureOther());
+ ASSERT_FALSE(fac->getFeatureOther());
+ }
+ auto x0_vector = x0.vector("PO");
+ auto s0_vector = s0.vector("PO");
+ MatrixXd P0_matrix = (s0_vector.array() * s0_vector.array()).matrix().asDiagonal();
// check that the Feature measurement and covariance are the ones provided
- ASSERT_TRUE((x0 - f->getMeasurement()).isMuchSmallerThan(1, Constants::EPS_SMALL));
- ASSERT_TRUE((P0 - f->getMeasurementCovariance()).isMuchSmallerThan(1, Constants::EPS_SMALL));
+ ASSERT_MATRIX_APPROX(x0_vector.head<3>(), fp->getMeasurement(), Constants::EPS_SMALL );
+ ASSERT_MATRIX_APPROX(x0_vector.tail<4>(), fo->getMeasurement(), Constants::EPS_SMALL );
+ ASSERT_MATRIX_APPROX(P0_matrix.topLeftCorner(3,3), fp->getMeasurementCovariance(), Constants::EPS_SMALL );
+ ASSERT_MATRIX_APPROX(P0_matrix.bottomRightCorner(3,3), fo->getMeasurementCovariance(), Constants::EPS_SMALL );
// P->print(4,1,1,1);
}
@@ -212,9 +223,9 @@ TEST(Problem, emplaceFrame_factory)
{
ProblemPtr P = Problem::create("PO", 2);
- FrameBasePtr f0 = P->emplaceFrame("PO", 2, KEY, VectorXd(3), TimeStamp(0.0));
- FrameBasePtr f1 = P->emplaceFrame("PO", 3, KEY, VectorXd(7), TimeStamp(1.0));
- FrameBasePtr f2 = P->emplaceFrame("POV", 3, KEY, VectorXd(10), TimeStamp(2.0));
+ 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) );
// std::cout << "f0: type PO 2d? " << f0->getType() << std::endl;
// std::cout << "f1: type PO 3d? " << f1->getType() << std::endl;
@@ -253,7 +264,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("PO", 3, KEY, xs3d, 0);
+ auto KF = P->emplaceFrame(KEY, 0, "PO", 3, xs3d );
ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)(2 + 2));
// Notifications
@@ -268,24 +279,26 @@ TEST(Problem, StateBlocks)
Sm->unfixExtrinsics();
ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)(2 + 2)); // changes in state_blocks status (fix/state/localparam) does not raise a notification in problem, only ADD/REMOVE
- // P->print(4,1,1,1);
+ P->print(4,1,1,1);
// consume notifications
- DummySolverManagerPtr SM = std::make_shared<DummySolverManager>(P);
+ SolverManagerDummyPtr SM = std::make_shared<SolverManagerDummy>(P);
ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)(2 + 2));
- SM->update(); // calls P->consumeStateBlockNotificationMap();
- ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) (0)); // consume empties the notification map
+ SM->update(); // calls P->consumeNotifications();
+ ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) (0)); // consumeNotifications empties the notification map
// remove frame
auto SB_P = KF->getP();
auto SB_O = KF->getO();
+ WOLF_INFO("removing frame...");
KF->remove();
+ WOLF_INFO("removed");
ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)(2));
ASSERT_TRUE(P->getStateBlockNotification(SB_P, notif));
ASSERT_EQ(notif, REMOVE);
ASSERT_TRUE(P->getStateBlockNotification(SB_O, notif));
ASSERT_EQ(notif, REMOVE);
-
+ WOLF_INFO("test end");
}
TEST(Problem, Covariances)
@@ -300,7 +313,7 @@ TEST(Problem, Covariances)
// SensorBasePtr St = P->installSensor ("SensorOdom2d", "other odometer", xs2d, wolf_root + "/test/yaml/sensor_odom_2d.yaml");
SensorBasePtr St = P->installSensor ("SensorOdom3d", "other odometer", xs3d, wolf_root + "/test/yaml/sensor_odom_3d.yaml");
- ProcessorParamsTrackerFeaturePtr params = std::make_shared<ProcessorParamsTrackerFeature>();
+ ParamsProcessorTrackerFeaturePtr params = std::make_shared<ParamsProcessorTrackerFeature>();
params->time_tolerance = 0.1;
params->max_new_features = 5;
params->min_features_for_keyframe = 10;
@@ -310,7 +323,7 @@ TEST(Problem, Covariances)
// 4 state blocks, estimated
St->unfixExtrinsics();
- FrameBasePtr F = P->emplaceFrame("PO", 3, KEY, xs3d, 0);
+ FrameBasePtr F = P->emplaceFrame(KEY, 0, "PO", 3, xs3d );
// set covariance (they are not computed without a solver)
P->addCovarianceBlock(F->getP(), Eigen::Matrix3d::Identity());
@@ -346,7 +359,7 @@ TEST(Problem, perturb)
int i = 0;
for (TimeStamp t = 0.0; t < 2.0; t += 1.0)
{
- auto F = problem->emplaceFrame(KEY, pose, t);
+ auto F = problem->emplaceFrame(KEY, t, pose );
if (i==0) F->fix();
for (int j = 0; j< 2 ; j++)
@@ -436,7 +449,7 @@ TEST(Problem, check)
int i = 0;
for (TimeStamp t = 0.0; t < 2.0; t += 1.0)
{
- auto F = problem->emplaceFrame(KEY, pose, t);
+ auto F = problem->emplaceFrame(KEY, t, pose);
if (i==0) F->fix();
for (int j = 0; j< 2 ; j++)
diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp
index c4a1c4b0470c92bf09df1a950a1c19e57d35642d..ed01245b29c903a7584e932196fb93a2c01c5c20 100644
--- a/test/gtest_processor_base.cpp
+++ b/test/gtest_processor_base.cpp
@@ -23,14 +23,6 @@
using namespace wolf;
using namespace Eigen;
-
-// Register in the ProcessorFactory
-#include "core/processor/processor_factory.h"
-namespace wolf {
-WOLF_REGISTER_PROCESSOR("TRACKER FEATURE DUMMY", ProcessorTrackerFeatureDummy)
-} // namespace wolf
-
-
TEST(ProcessorBase, IsMotion)
{
using namespace wolf;
@@ -57,7 +49,7 @@ TEST(ProcessorBase, IsMotion)
// Install odometer (sensor and processor)
SensorBasePtr sens_odo = problem->installSensor("SensorOdom2d", "odometer", Vector3d(0,0,0), wolf_root + "/test/yaml/sensor_odom_2d.yaml");
- ProcessorParamsOdom2dPtr proc_odo_params = make_shared<ProcessorParamsOdom2d>();
+ ParamsProcessorOdom2dPtr proc_odo_params = make_shared<ParamsProcessorOdom2d>();
proc_odo_params->time_tolerance = dt/2;
ProcessorBasePtr proc_odo = problem->installProcessor("ProcessorOdom2d", "odom processor", sens_odo, proc_odo_params);
@@ -84,15 +76,17 @@ TEST(ProcessorBase, KeyFrameCallback)
// Install tracker (sensor and processor)
auto sens_trk = SensorBase::emplace<SensorBase>(problem->getHardware(),
- "SensorOdom2d",
+ "SensorTrackerDummy",
std::make_shared<StateBlock>(Eigen::VectorXd::Zero(2)),
std::make_shared<StateBlock>(Eigen::VectorXd::Zero(1)),
std::make_shared<StateBlock>(Eigen::VectorXd::Zero(2)), 2);
- auto proc_trk = problem->installProcessor("ProcessorTrackerFeatureDummy", "dummy", sens_trk);
+ auto proc_trk_params = make_shared<ParamsProcessorTrackerFeatureDummy>();
+ proc_trk_params->time_tolerance = dt/2;
+ auto proc_trk = problem->installProcessor("ProcessorTrackerFeatureDummy", "dummy", sens_trk, proc_trk_params);
// Install odometer (sensor and processor)
SensorBasePtr sens_odo = problem->installSensor("SensorOdom2d", "odometer", Vector3d(0,0,0), wolf_root + "/test/yaml/sensor_odom_2d.yaml");
- ProcessorParamsOdom2dPtr proc_odo_params = make_shared<ProcessorParamsOdom2d>();
+ ParamsProcessorOdom2dPtr proc_odo_params = make_shared<ParamsProcessorOdom2d>();
proc_odo_params->time_tolerance = dt/2;
ProcessorBasePtr proc_odo = problem->installProcessor("ProcessorOdom2d", "odom processor", sens_odo, proc_odo_params);
@@ -102,9 +96,11 @@ TEST(ProcessorBase, KeyFrameCallback)
// initialize
TimeStamp t(0.0);
- Vector3d x(0,0,0);
- Matrix3d P = Matrix3d::Identity() * 0.1;
- problem->setPrior(x, P, t, dt/2); // KF1
+ // Vector3d x(0,0,0);
+ VectorComposite x(Vector3d(0,0,0), "PO", {2,1});
+ // Matrix3d P = Matrix3d::Identity() * 0.1;
+ VectorComposite P(Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)), "PO", {2,1});
+ problem->setPriorFactor(x, P, t, dt/2); // KF1
CaptureOdom2dPtr capt_odo = make_shared<CaptureOdom2d>(t, sens_odo, Vector2d(0.5,0));
@@ -117,18 +113,24 @@ TEST(ProcessorBase, KeyFrameCallback)
// Move
t = t+dt;
WOLF_INFO("----------------------- ts: ", t , " --------------------------");
+ std::cout << "1\n";
capt_odo->setTimeStamp(t);
+ std::cout << "2\n";
proc_odo->captureCallback(capt_odo);
+ std::cout << "3\n";
// Track
capt_trk = make_shared<CaptureVoid>(t, sens_trk);
+ std::cout << "4\n";
proc_trk->captureCallback(capt_trk);
+ std::cout << "5\n";
- problem->print(4,1,1,0);
+ problem->print(4,1,1,0);
+ std::cout << "6\n";
// Only odom creating KFs
- ASSERT_TRUE( problem->getLastKeyFrame()->getType().compare("PO 2d")==0 );
+ ASSERT_TRUE( problem->getLastKeyFrame()->getStructure().compare("PO")==0 );
}
}
@@ -137,4 +139,3 @@ int main(int argc, char **argv)
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
-
diff --git a/test/gtest_processor_diff_drive.cpp b/test/gtest_processor_diff_drive.cpp
index 612f66019cd62b892a300df15da4a809b0d51e9a..ee908013b9eadd216363d528d7f60757ea331f29 100644
--- a/test/gtest_processor_diff_drive.cpp
+++ b/test/gtest_processor_diff_drive.cpp
@@ -20,7 +20,7 @@ class ProcessorDiffDrivePublic : public ProcessorDiffDrive
{
using Base = ProcessorDiffDrive;
public:
- ProcessorDiffDrivePublic(ProcessorParamsDiffDrivePtr _params_motion) :
+ ProcessorDiffDrivePublic(ParamsProcessorDiffDrivePtr _params_motion) :
ProcessorDiffDrive(_params_motion)
{
//
@@ -58,10 +58,10 @@ class ProcessorDiffDrivePublic : public ProcessorDiffDrive
Base::deltaPlusDelta(_delta1, _delta2, _dt2, _delta1_plus_delta2, _jacobian1, _jacobian2);
}
- virtual void statePlusDelta(const Eigen::VectorXd& _x,
+ virtual void statePlusDelta(const VectorComposite& _x,
const Eigen::VectorXd& _delta,
const double _dt,
- Eigen::VectorXd& _x_plus_delta) const
+ VectorComposite& _x_plus_delta) const
{
Base::statePlusDelta(_x, _delta, _dt, _x_plus_delta);
}
@@ -94,7 +94,7 @@ class ProcessorDiffDrivePublic : public ProcessorDiffDrive
return Base::emplaceFactor(_feature_motion, _capture_origin);
}
- ProcessorParamsDiffDrivePtr getParams()
+ ParamsProcessorDiffDrivePtr getParams()
{
return params_prc_diff_drv_selfcal_;
}
@@ -111,7 +111,7 @@ class ProcessorDiffDriveTest : public testing::Test
public:
ParamsSensorDiffDrivePtr intr;
SensorDiffDrivePtr sensor;
- ProcessorParamsDiffDrivePtr params;
+ ParamsProcessorDiffDrivePtr params;
ProcessorDiffDrivePublicPtr processor;
ProblemPtr problem;
@@ -129,7 +129,7 @@ class ProcessorDiffDriveTest : public testing::Test
sensor = std::static_pointer_cast<SensorDiffDrive>(problem->installSensor("SensorDiffDrive", "sensor diff drive", extr, intr));
// params and processor
- params = std::make_shared<ProcessorParamsDiffDrive>();
+ params = std::make_shared<ParamsProcessorDiffDrive>();
params->voting_active = true;
params->angle_turned = 1;
params->dist_traveled = 2;
@@ -145,7 +145,7 @@ class ProcessorDiffDriveTest : public testing::Test
TEST(ProcessorDiffDrive, constructor)
{
- auto params = std::make_shared<ProcessorParamsDiffDrive>();
+ auto params = std::make_shared<ParamsProcessorDiffDrive>();
ASSERT_NE(params, nullptr);
@@ -164,7 +164,7 @@ TEST(ProcessorDiffDrive, create)
auto sen = std::make_shared<SensorDiffDrive>(extr, intr);
// params
- auto params = std::make_shared<ProcessorParamsDiffDrive>();
+ auto params = std::make_shared<ParamsProcessorDiffDrive>();
// processor
auto prc_base = ProcessorDiffDrive::create("prc", params);
@@ -268,7 +268,8 @@ TEST_F(ProcessorDiffDriveTest, statePlusDelta)
Matrix2d data_cov; data_cov . setIdentity();
Vector3d calib(1,1,1);
double dt = 1.0;
- VectorXd delta(3), x1(3), x2(3);
+ VectorXd delta(3);
+ VectorComposite x1("PO", {2,1}), x2("PO", {2,1});
MatrixXd delta_cov(3,3), J_delta_calib(3,3);
// 1. left turn 90 deg in 3 steps of 30 deg --> ends up in (1.5, 1.5, pi/2)
@@ -283,7 +284,7 @@ TEST_F(ProcessorDiffDriveTest, statePlusDelta)
x1 = x2;
processor->statePlusDelta(x1, delta, dt, x2); // 90
- ASSERT_MATRIX_APPROX(x2, Vector3d(1.5, 1.5, M_PI_2), 1e-6);
+ ASSERT_MATRIX_APPROX(x2.vector("PO"), Vector3d(1.5, 1.5, M_PI_2), 1e-6);
// 2. right turn 90 deg in 4 steps of 22.5 deg --> ends up in (1.5, -1.5, -pi/2)
data(0) = 0.50*intr->ticks_per_wheel_revolution / 4;
@@ -299,7 +300,7 @@ TEST_F(ProcessorDiffDriveTest, statePlusDelta)
x1 = x2;
processor->statePlusDelta(x1, delta, dt, x2); // 90
- ASSERT_MATRIX_APPROX(x2, Vector3d(1.5, -1.5, -M_PI_2), 1e-6);
+ ASSERT_MATRIX_APPROX(x2.vector("PO"), Vector3d(1.5, -1.5, -M_PI_2), 1e-6);
}
TEST_F(ProcessorDiffDriveTest, process)
@@ -308,10 +309,13 @@ TEST_F(ProcessorDiffDriveTest, process)
Matrix2d data_cov; data_cov . setIdentity();
TimeStamp t = 0.0;
double dt = 1.0;
- Vector3d x(0,0,0);
- Matrix3d P; P.setIdentity();
+ // Vector3d x(0,0,0);
+ VectorComposite x(Vector3d(0,0,0), "PO", {2,1});
+ // Matrix3d P; P.setIdentity();
+ VectorComposite P(Vector3d(1,1,1), "PO", {2,1});
- auto F0 = problem->setPrior(x, P, t, 0.1);
+ auto F0 = problem->setPriorFactor(x, P, t, 0.1);
+ processor->setOrigin(F0);
// 1. left turn 90 deg in N steps of 90/N deg --> ends up in (1.5, 1.5, pi/2)
int N = 9;
@@ -320,14 +324,12 @@ TEST_F(ProcessorDiffDriveTest, process)
auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, F0->getCaptureList().front());
- WOLF_TRACE("t = ", t, "; x = ", processor->getCurrentState().transpose());
for (int n = 1; n <= N; n++)
{
- C->process();
- WOLF_TRACE("t = ", t, "; x = ", processor->getCurrentState().transpose());
-
t += dt;
C->setTimeStamp(t);
+ C->process();
+ WOLF_TRACE("t = ", t, "; x = ", processor->getState().vector("PO").transpose());
}
problem->print(4,1,1,1);
@@ -338,10 +340,13 @@ TEST_F(ProcessorDiffDriveTest, linear)
Vector2d data;
Matrix2d data_cov; data_cov . setIdentity();
TimeStamp t = 0.0;
- Vector3d x(0,0,0);
- Matrix3d P; P.setIdentity();
+ // Vector3d x(0,0,0);
+ VectorComposite x(Vector3d(0,0,0), "PO", {2,1});
+ // Matrix3d P; P.setIdentity();
+ VectorComposite P(Vector3d(1,1,1), "PO", {2,1});
- auto F0 = problem->setPrior(x, P, t, 0.1);
+ auto F0 = problem->setPriorFactor(x, P, t, 0.1);
+ processor->setOrigin(F0);
// Straight one turn of the wheels, in one go
data(0) = 100.0 ; // one turn of the wheels
@@ -350,12 +355,12 @@ TEST_F(ProcessorDiffDriveTest, linear)
auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, F0->getCaptureList().front());
C->process();
- WOLF_TRACE("t = ", t, "; x = ", processor->getCurrentState().transpose());
+ WOLF_TRACE("t = ", t, "; x = ", processor->getState().vector("PO").transpose());
// radius is 1.0m, 100 ticks per revolution, so advanced distance is
double distance = 2 * M_PI * 1.0;
- ASSERT_MATRIX_APPROX(processor->getCurrentState(), Vector3d(distance,0,0), 1e-6)
+ ASSERT_MATRIX_APPROX(processor->getState().vector("PO"), Vector3d(distance,0,0), 1e-6)
}
TEST_F(ProcessorDiffDriveTest, angular)
@@ -363,10 +368,13 @@ TEST_F(ProcessorDiffDriveTest, angular)
Vector2d data;
Matrix2d data_cov; data_cov . setIdentity();
TimeStamp t = 0.0;
- Vector3d x(0,0,0);
- Matrix3d P; P.setIdentity();
+ // Vector3d x(0,0,0);
+ VectorComposite x(Vector3d(0,0,0), "PO", {2,1});
+ // Matrix3d P; P.setIdentity();
+ VectorComposite P(Vector3d(1,1,1), "PO", {2,1});
- auto F0 = problem->setPrior(x, P, t, 0.1);
+ auto F0 = problem->setPriorFactor(x, P, t, 0.1);
+ processor->setOrigin(F0);
// Straight one turn of the wheels, in one go
data(0) = -20.0 ; // one fifth of a turn of the left wheel, in reverse
@@ -375,7 +383,7 @@ TEST_F(ProcessorDiffDriveTest, angular)
auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, F0->getCaptureList().front());
C->process();
- WOLF_TRACE("t = ", t, "; x = ", processor->getCurrentState().transpose());
+ WOLF_TRACE("t = ", t, "; x = ", processor->getState().vector("PO").transpose());
// this is a turn in place, so distance = 0;
double distance = 0.0;
@@ -383,7 +391,7 @@ TEST_F(ProcessorDiffDriveTest, angular)
// radius is 1.0m, 100 ticks per revolution, and wheel separation is 1m, so turn angle is
double angle = pi2pi(2 * M_PI * 1.0 / 0.5 / 5);
- ASSERT_MATRIX_APPROX(processor->getCurrentState(), Vector3d(distance,0,angle), 1e-6)
+ ASSERT_MATRIX_APPROX(processor->getState().vector("PO"), Vector3d(distance,0,angle), 1e-6)
}
diff --git a/test/gtest_processor_loopclosure.cpp b/test/gtest_processor_loopclosure.cpp
index d00fb93407ab75bb150a576bc434640f1c75fe12..9b8683f8b74ee566370d4630a248d039257ebb17 100644
--- a/test/gtest_processor_loopclosure.cpp
+++ b/test/gtest_processor_loopclosure.cpp
@@ -23,16 +23,16 @@ private:
bool* factor_created;
public:
- ProcessorLoopClosureDummy(ProcessorParamsLoopClosurePtr _params_loop_closure, bool& factor_created):
+ ProcessorLoopClosureDummy(ParamsProcessorLoopClosurePtr _params_loop_closure, bool& factor_created):
ProcessorLoopClosure("LOOP CLOSURE DUMMY", 0, _params_loop_closure),
factor_created(&factor_created){};
std::pair<FrameBasePtr,CaptureBasePtr> public_selectPairKC(){ return selectPairKC();};
protected:
- virtual bool voteComputeFeatures() { return true;};
- virtual bool voteSearchLoopClosure() { return true;};
- virtual bool detectFeatures(CaptureBasePtr cap) { return true;};
- virtual CaptureBasePtr findLoopCandidate(CaptureBasePtr _capture)
+ virtual bool voteComputeFeatures() override { return true;};
+ virtual bool voteSearchLoopClosure() override { return true;};
+ virtual bool detectFeatures(CaptureBasePtr cap) override { return true;};
+ virtual CaptureBasePtr findLoopCandidate(CaptureBasePtr _capture) override
{
for (FrameBasePtr kf : getProblem()->getTrajectory()->getFrameList())
if (kf->isKey())
@@ -41,7 +41,8 @@ protected:
return cap;
return nullptr;
};
- virtual void emplaceFactors(CaptureBasePtr _capture_1, CaptureBasePtr _capture_2) {
+ virtual void emplaceFactors(CaptureBasePtr _capture_1, CaptureBasePtr _capture_2) override
+ {
std::cout << "factor created\n";
*factor_created = true;
};
@@ -71,20 +72,22 @@ TEST(ProcessorLoopClosure, installProcessor)
std::make_shared<StateBlock>(Eigen::VectorXd::Zero(2)),
std::make_shared<StateBlock>(Eigen::VectorXd::Zero(1)),
std::make_shared<StateBlock>(Eigen::VectorXd::Zero(2)), 2);
- ProcessorParamsLoopClosurePtr params = std::make_shared<ProcessorParamsLoopClosure>();
+ ParamsProcessorLoopClosurePtr params = std::make_shared<ParamsProcessorLoopClosure>();
auto proc_lc = ProcessorBase::emplace<ProcessorLoopClosureDummy>(sens_lc, params, factor_created);
std::cout << "sensor & processor created and added to wolf problem" << std::endl;
// initialize
TimeStamp t(0.0);
- Vector3d x(0,0,0);
- Matrix3d P = Matrix3d::Identity() * 0.1;
- problem->setPrior(x, P, t, dt/2); // KF1
+ // Vector3d x(0,0,0);
+ VectorComposite x(Vector3d(0,0,0), "PO", {2,1});
+ // Matrix3d P = Matrix3d::Identity() * 0.1;
+ VectorComposite P(Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)), "PO", {2,1});
+ problem->setPriorFactor(x, P, t, dt/2); // KF1
// new KF
t += dt;
- auto kf = problem->emplaceFrame(KEY, x, t); //KF2
+ auto kf = problem->emplaceFrame(KEY, 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 076118ba3d7836dbf61cdb2ed1d38d81d46b9084..509f495c1bd4c56be08f2d70279e6fba9a5b6ccc 100644
--- a/test/gtest_processor_motion.cpp
+++ b/test/gtest_processor_motion.cpp
@@ -21,7 +21,7 @@ using std::static_pointer_cast;
class ProcessorOdom2dPublic : public ProcessorOdom2d
{
public:
- ProcessorOdom2dPublic(ProcessorParamsOdom2dPtr params) : ProcessorOdom2d(params)
+ ProcessorOdom2dPublic(ParamsProcessorOdom2dPtr params) : ProcessorOdom2d(params)
{
//
}
@@ -63,7 +63,7 @@ class ProcessorMotion_test : public testing::Test{
dt = 1.0;
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"));
- ProcessorParamsOdom2dPtr params(std::make_shared<ProcessorParamsOdom2d>());
+ ParamsProcessorOdom2dPtr params(std::make_shared<ParamsProcessorOdom2d>());
params->time_tolerance = 0.25;
params->dist_traveled = 100;
params->angle_turned = 6.28;
@@ -71,19 +71,19 @@ class ProcessorMotion_test : public testing::Test{
params->cov_det = 100;
params->unmeasured_perturbation_std = 0.001;
processor = ProcessorBase::emplace<ProcessorOdom2dPublic>(sensor, params);
- capture = std::make_shared<CaptureMotion>("CaptureOdom2d", 0.0, sensor, data, data_cov, 3, 3, nullptr);
-
- Vector3d x0; x0 << 0, 0, 0;
- Matrix3d P0; P0.setIdentity();
- problem->setPrior(x0, P0, 0.0, 0.01);
+ capture = std::make_shared<CaptureMotion>("CaptureOdom2d", 0.0, sensor, data, data_cov, nullptr);
}
virtual void TearDown(){}
};
-TEST_F(ProcessorMotion_test, IntegrateStraight)
+TEST_F(ProcessorMotion_test, IntegrateStraightAutoPrior)
{
+ // Prior
+ Vector3d x0; x0 << 0, 0, 0;
+ Matrix3d P0; P0.setIdentity();
+
data << 1, 0; // advance straight
data_cov.setIdentity();
TimeStamp t(0.0);
@@ -95,17 +95,130 @@ TEST_F(ProcessorMotion_test, IntegrateStraight)
capture->setData(data);
capture->setDataCovariance(data_cov);
processor->captureCallback(capture);
- WOLF_DEBUG("t: ", t, " x: ", problem->getCurrentState().transpose());
+ WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
+ }
+
+ ASSERT_MATRIX_APPROX(problem->getState().vector("PO"), (Vector3d()<<9,0,0).finished(), 1e-8);
+}
+
+
+TEST_F(ProcessorMotion_test, IntegrateStraightFactorPrior)
+{
+ // Prior
+ TimeStamp t(0.0);
+ // Vector3d x0; x0 << 0, 0, 0;
+ VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
+ // Matrix3d P0; P0.setIdentity();
+ VectorComposite s0(Vector3d(1,1,1), "PO", {2,1});
+ auto KF_0 = problem->setPriorFactor(x0, s0, t, 0.01);
+ processor->setOrigin(KF_0);
+
+ data << 1, 0; // advance straight
+ data_cov.setIdentity();
+
+ for (int i = 0; i<9; i++)
+ {
+ t += dt;
+ capture->setTimeStamp(t);
+ capture->setData(data);
+ capture->setDataCovariance(data_cov);
+ processor->captureCallback(capture);
+ WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
+ }
+
+ ASSERT_MATRIX_APPROX(problem->getState().vector("PO"), (Vector3d()<<9,0,0).finished(), 1e-8);
+}
+
+TEST_F(ProcessorMotion_test, IntegrateStraightFixPrior)
+{
+ // Prior
+ TimeStamp t(0.0);
+ // Vector3d x0; x0 << 0, 0, 0;
+ // Matrix3d P0; P0.setIdentity();
+ VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
+ VectorComposite s0(Vector3d(1,1,1), "PO", {2,1});
+ auto KF_0 = problem->setPriorFix(x0, t, 0.01);
+ processor->setOrigin(KF_0);
+
+ data << 1, 0; // advance straight
+ data_cov.setIdentity();
+
+ for (int i = 0; i<9; i++)
+ {
+ t += dt;
+ capture->setTimeStamp(t);
+ capture->setData(data);
+ capture->setDataCovariance(data_cov);
+ processor->captureCallback(capture);
+ WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
}
- ASSERT_MATRIX_APPROX(problem->getCurrentState(), (Vector3d()<<9,0,0).finished(), 1e-8);
+ ASSERT_MATRIX_APPROX(problem->getState().vector("PO"), (Vector3d()<<9,0,0).finished(), 1e-8);
}
-TEST_F(ProcessorMotion_test, IntegrateCircle)
+TEST_F(ProcessorMotion_test, IntegrateCircleAutoPrior)
{
+ // Prior
+ Vector3d x0; x0 << 0, 0, 0;
+ Matrix3d P0; P0.setIdentity();
+
+ data << 1, 2*M_PI/10; // advance in circle
+ data_cov.setIdentity();
+ TimeStamp t(0.0);
+
+ for (int i = 0; i<10; i++) // one full turn exactly
+ {
+ t += dt;
+ capture->setTimeStamp(t);
+ capture->setData(data);
+ capture->setDataCovariance(data_cov);
+ processor->captureCallback(capture);
+ WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
+ }
+
+ ASSERT_MATRIX_APPROX(problem->getState().vector("PO"), (Vector3d()<<0,0,0).finished(), 1e-8);
+}
+
+TEST_F(ProcessorMotion_test, IntegrateCircleFactorPrior)
+{
+ // Prior
+ TimeStamp t(0.0);
+ // Vector3d x0; x0 << 0, 0, 0;
+ VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
+ // Matrix3d P0; P0.setIdentity();
+ VectorComposite s0(Vector3d(1,1,1), "PO", {2,1});
+ auto KF_0 = problem->setPriorFactor(x0, s0, t, 0.01);
+ processor->setOrigin(KF_0);
+
data << 1, 2*M_PI/10; // advance in circle
data_cov.setIdentity();
+
+ for (int i = 0; i<10; i++) // one full turn exactly
+ {
+ t += dt;
+ capture->setTimeStamp(t);
+ capture->setData(data);
+ capture->setDataCovariance(data_cov);
+ processor->captureCallback(capture);
+ WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
+ }
+
+ ASSERT_MATRIX_APPROX(problem->getState().vector("PO"), (Vector3d()<<0,0,0).finished(), 1e-8);
+}
+
+TEST_F(ProcessorMotion_test, IntegrateCircleFixPrior)
+{
+ // Prior
TimeStamp t(0.0);
+ // Vector3d x0; x0 << 0, 0, 0;
+ // Matrix3d P0; P0.setIdentity();
+ VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
+ VectorComposite s0(Vector3d(1,1,1), "PO", {2,1});
+ auto KF_0 = problem->setPriorFix(x0, t, 0.01);
+ processor->setOrigin(KF_0);
+
+ data << 1, 2*M_PI/10; // advance in circle
+ data_cov.setIdentity();
for (int i = 0; i<10; i++) // one full turn exactly
{
@@ -114,14 +227,18 @@ TEST_F(ProcessorMotion_test, IntegrateCircle)
capture->setData(data);
capture->setDataCovariance(data_cov);
processor->captureCallback(capture);
- WOLF_DEBUG("t: ", t, " x: ", problem->getCurrentState().transpose());
+ WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
}
- ASSERT_MATRIX_APPROX(problem->getCurrentState(), (Vector3d()<<0,0,0).finished(), 1e-8);
+ ASSERT_MATRIX_APPROX(problem->getState().vector("PO"), (Vector3d()<<0,0,0).finished(), 1e-8);
}
-TEST_F(ProcessorMotion_test, splitBuffer)
+TEST_F(ProcessorMotion_test, splitBufferAutoPrior)
{
+ // Prior
+ Vector3d x0; x0 << 0, 0, 0;
+ Matrix3d P0; P0.setIdentity();
+
data << 1, 2*M_PI/10; // advance in circle
data_cov.setIdentity();
TimeStamp t(0.0);
@@ -133,7 +250,97 @@ TEST_F(ProcessorMotion_test, splitBuffer)
capture->setData(data);
capture->setDataCovariance(data_cov);
processor->captureCallback(capture);
- WOLF_DEBUG("t: ", t, " x: ", problem->getCurrentState().transpose());
+ WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
+ }
+
+ SensorBasePtr S = processor->getSensor();
+
+ TimeStamp t_target = 8.5;
+ FrameBasePtr F_target = problem->emplaceFrame(KEY, t_target);
+ CaptureMotionPtr C_source = std::dynamic_pointer_cast<CaptureMotion>(processor->getLast());
+ CaptureMotionPtr C_target = CaptureBase::emplace<CaptureMotion>(F_target,
+ "ODOM 2d",
+ t_target,
+ sensor,
+ data,
+ nullptr);
+
+ processor->splitBuffer(C_source,
+ t_target,
+ F_target,
+ C_target);
+
+ C_target->getBuffer().print(1,1,1,0);
+ C_source->getBuffer().print(1,1,1,0);
+}
+
+TEST_F(ProcessorMotion_test, splitBufferFactorPrior)
+{
+ // Prior
+ TimeStamp t(0.0);
+ // Vector3d x0; x0 << 0, 0, 0;
+ VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
+ // Matrix3d P0; P0.setIdentity();
+ VectorComposite s0(Vector3d(1,1,1), "PO", {2,1});
+ auto KF_0 = problem->setPriorFactor(x0, s0, t, 0.01);
+ processor->setOrigin(KF_0);
+
+ data << 1, 2*M_PI/10; // advance in circle
+ data_cov.setIdentity();
+
+ for (int i = 0; i<10; i++) // one full turn exactly
+ {
+ t += dt;
+ capture->setTimeStamp(t);
+ capture->setData(data);
+ capture->setDataCovariance(data_cov);
+ processor->captureCallback(capture);
+ WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
+ }
+
+ SensorBasePtr S = processor->getSensor();
+
+ TimeStamp t_target = 8.5;
+ FrameBasePtr F_target = problem->emplaceFrame(KEY, t_target);
+ CaptureMotionPtr C_source = std::dynamic_pointer_cast<CaptureMotion>(processor->getLast());
+ CaptureMotionPtr C_target = CaptureBase::emplace<CaptureMotion>(F_target,
+ "ODOM 2d",
+ t_target,
+ sensor,
+ data,
+ nullptr);
+
+ processor->splitBuffer(C_source,
+ t_target,
+ F_target,
+ C_target);
+
+ C_target->getBuffer().print(1,1,1,0);
+ C_source->getBuffer().print(1,1,1,0);
+}
+
+TEST_F(ProcessorMotion_test, splitBufferFixPrior)
+{
+ // Prior
+ TimeStamp t(0.0);
+ // Vector3d x0; x0 << 0, 0, 0;
+ // Matrix3d P0; P0.setIdentity();
+ VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
+ VectorComposite s0(Vector3d(1,1,1), "PO", {2,1});
+ auto KF_0 = problem->setPriorFix(x0, t, 0.01);
+ processor->setOrigin(KF_0);
+
+ data << 1, 2*M_PI/10; // advance in circle
+ data_cov.setIdentity();
+
+ for (int i = 0; i<10; i++) // one full turn exactly
+ {
+ t += dt;
+ capture->setTimeStamp(t);
+ capture->setData(data);
+ capture->setDataCovariance(data_cov);
+ processor->captureCallback(capture);
+ WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
}
SensorBasePtr S = processor->getSensor();
@@ -146,8 +353,6 @@ TEST_F(ProcessorMotion_test, splitBuffer)
t_target,
sensor,
data,
- 3,
- 3,
nullptr);
processor->splitBuffer(C_source,
diff --git a/test/gtest_processor_odom_3d.cpp b/test/gtest_processor_odom_3d.cpp
index 24e5e875ea0b5775747c7325239884a586f0a0c4..4a94d3e1b88d1afc89ef80a39b9fad1baa67a1f7 100644
--- a/test/gtest_processor_odom_3d.cpp
+++ b/test/gtest_processor_odom_3d.cpp
@@ -52,7 +52,7 @@ class ProcessorOdom3dTest : public ProcessorOdom3d
double& dvar_min() {return min_disp_var_;}
double& rvar_min() {return min_rot_var_;}
};
-ProcessorOdom3dTest::ProcessorOdom3dTest() : ProcessorOdom3d(std::make_shared<ProcessorParamsOdom3d>())
+ProcessorOdom3dTest::ProcessorOdom3dTest() : ProcessorOdom3d(std::make_shared<ParamsProcessorOdom3d>())
{
dvar_min() = 0.5;
rvar_min() = 0.25;
diff --git a/test/gtest_processor_tracker_feature_dummy.cpp b/test/gtest_processor_tracker_feature_dummy.cpp
index c8bf458bbcdc5058862accd76beedcb13b1d011f..604cd09b4f20ed68e1c44273f85a93c8c1a88c1a 100644
--- a/test/gtest_processor_tracker_feature_dummy.cpp
+++ b/test/gtest_processor_tracker_feature_dummy.cpp
@@ -1,7 +1,7 @@
// wolf includes
#include "core/utils/utils_gtest.h"
-#include "core/sensor/sensor_factory.h"
+#include "core/sensor/factory_sensor.h"
#include "dummy/processor_tracker_feature_dummy.h"
#include "core/capture/capture_void.h"
@@ -15,7 +15,7 @@ class ProcessorTrackerFeatureDummyDummy : public ProcessorTrackerFeatureDummy
{
public:
- ProcessorTrackerFeatureDummyDummy(ProcessorParamsTrackerFeatureDummyPtr& _params) :
+ ProcessorTrackerFeatureDummyDummy(ParamsProcessorTrackerFeatureDummyPtr& _params) :
ProcessorTrackerFeatureDummy(_params){}
void setLast(CaptureBasePtr _last_ptr){ last_ptr_ = _last_ptr; }
@@ -71,7 +71,7 @@ class ProcessorTrackerFeatureDummyTest : public testing::Test
public:
ProblemPtr problem;
SensorBasePtr sensor;
- ProcessorParamsTrackerFeatureDummyPtr params;
+ ParamsProcessorTrackerFeatureDummyPtr params;
ProcessorTrackerFeatureDummyDummyPtr processor;
virtual ~ProcessorTrackerFeatureDummyTest(){}
@@ -85,7 +85,7 @@ class ProcessorTrackerFeatureDummyTest : public testing::Test
sensor = problem->installSensor("SensorOdom2d", "auxiliar sensor", (Eigen::Vector3d() << 0,0,0).finished(), std::make_shared<ParamsSensorBase>());
// Install processor
- params = std::make_shared<ProcessorParamsTrackerFeatureDummy>();
+ params = std::make_shared<ParamsProcessorTrackerFeatureDummy>();
params->max_new_features = 10;
params->min_features_for_keyframe = 7;
params->time_tolerance = 0.25;
@@ -283,7 +283,7 @@ TEST_F(ProcessorTrackerFeatureDummyTest, process)
{
//1ST TIME -> KF (origin)
- WOLF_DEBUG("First time...");
+ WOLF_INFO("First time...");
CaptureBasePtr cap1 = std::make_shared<CaptureVoid>(0, sensor);
cap1->process();
@@ -292,7 +292,7 @@ TEST_F(ProcessorTrackerFeatureDummyTest, process)
ASSERT_TRUE(problem->check(0));
//2ND TIME
- WOLF_DEBUG("Second time...");
+ WOLF_INFO("Second time...");
CaptureBasePtr cap2 = std::make_shared<CaptureVoid>(1, sensor);
cap2->process();
@@ -301,7 +301,7 @@ TEST_F(ProcessorTrackerFeatureDummyTest, process)
ASSERT_TRUE(problem->check(0));
//3RD TIME
- WOLF_DEBUG("Third time...");
+ WOLF_INFO("Third time...");
CaptureBasePtr cap3 = std::make_shared<CaptureVoid>(2, sensor);
cap3->process();
@@ -309,7 +309,7 @@ TEST_F(ProcessorTrackerFeatureDummyTest, process)
ASSERT_TRUE(problem->check(0));
//4TH TIME
- WOLF_DEBUG("Forth time...");
+ WOLF_INFO("Forth time...");
CaptureBasePtr cap4 = std::make_shared<CaptureVoid>(3, sensor);
cap4->process();
@@ -317,7 +317,7 @@ TEST_F(ProcessorTrackerFeatureDummyTest, process)
ASSERT_TRUE(problem->check(0));
//5TH TIME -> KF in cap4 (tracked features < 7 (params->min_features_for_keyframe))
- WOLF_DEBUG("Fifth time...");
+ WOLF_INFO("Fifth time...");
CaptureBasePtr cap5 = std::make_shared<CaptureVoid>(4, sensor);
cap5->process();
@@ -326,7 +326,7 @@ TEST_F(ProcessorTrackerFeatureDummyTest, process)
ASSERT_TRUE(problem->check(0));
// check factors
- WOLF_DEBUG("checking factors...");
+ WOLF_INFO("checking factors...");
TrackMatrix track_matrix = processor->getTrackMatrix();
ASSERT_EQ(cap4->getFeatureList().size(), params->min_features_for_keyframe + params->max_new_features);
ASSERT_TRUE(problem->check(0));
diff --git a/test/gtest_processor_tracker_landmark_dummy.cpp b/test/gtest_processor_tracker_landmark_dummy.cpp
index a75ba7ffb962509468343f66e894a1b527610c04..8963211c45c292341d3754e83579f8bf9738a752 100644
--- a/test/gtest_processor_tracker_landmark_dummy.cpp
+++ b/test/gtest_processor_tracker_landmark_dummy.cpp
@@ -1,7 +1,7 @@
// wolf includes
#include "core/utils/utils_gtest.h"
-#include "core/sensor/sensor_factory.h"
+#include "core/sensor/factory_sensor.h"
#include "dummy/processor_tracker_landmark_dummy.h"
#include "core/capture/capture_void.h"
@@ -15,7 +15,7 @@ class ProcessorTrackerLandmarkDummyDummy : public ProcessorTrackerLandmarkDummy
{
public:
- ProcessorTrackerLandmarkDummyDummy(ProcessorParamsTrackerLandmarkDummyPtr& _params) :
+ ProcessorTrackerLandmarkDummyDummy(ParamsProcessorTrackerLandmarkDummyPtr& _params) :
ProcessorTrackerLandmarkDummy(_params){}
void setLast(CaptureBasePtr _last_ptr){ last_ptr_ = _last_ptr; }
@@ -89,7 +89,7 @@ class ProcessorTrackerLandmarkDummyTest : public testing::Test
public:
ProblemPtr problem;
SensorBasePtr sensor;
- ProcessorParamsTrackerLandmarkDummyPtr params;
+ ParamsProcessorTrackerLandmarkDummyPtr params;
ProcessorTrackerLandmarkDummyDummyPtr processor;
virtual ~ProcessorTrackerLandmarkDummyTest(){}
@@ -103,7 +103,7 @@ class ProcessorTrackerLandmarkDummyTest : public testing::Test
sensor = problem->installSensor("SensorOdom2d", "auxiliar sensor", (Eigen::Vector3d() << 0,0,0).finished(), std::make_shared<ParamsSensorBase>());
// Install processor
- params = std::make_shared<ProcessorParamsTrackerLandmarkDummy>();
+ params = std::make_shared<ParamsProcessorTrackerLandmarkDummy>();
params->max_new_features = 10;
params->min_features_for_keyframe = 7;
params->time_tolerance = 0.25;
diff --git a/test/gtest_solver_manager.cpp b/test/gtest_solver_manager.cpp
index 640dc71faefcdfcaa937bdc928c904ef618f05d9..e8afad2af2283b3a0aaa2fdb7f9290fc9e633981 100644
--- a/test/gtest_solver_manager.cpp
+++ b/test/gtest_solver_manager.cpp
@@ -13,104 +13,20 @@
#include "core/state_block/state_block.h"
#include "core/capture/capture_void.h"
#include "core/factor/factor_pose_2d.h"
-#include "core/solver/solver_manager.h"
#include "core/state_block/local_parametrization_base.h"
#include "core/state_block/local_parametrization_angle.h"
+#include "dummy/solver_manager_dummy.h"
#include <iostream>
+#include <thread>
using namespace wolf;
using namespace Eigen;
-WOLF_PTR_TYPEDEFS(SolverManagerWrapper);
-class SolverManagerWrapper : public SolverManager
-{
- public:
- std::list<FactorBasePtr> factors_;
- std::map<StateBlockPtr,bool> state_block_fixed_;
- std::map<StateBlockPtr,LocalParametrizationBasePtr> state_block_local_param_;
-
- SolverManagerWrapper(const ProblemPtr& wolf_problem) :
- SolverManager(wolf_problem)
- {
- };
-
- bool isStateBlockRegistered(const StateBlockPtr& st)
- {
- return state_blocks_.find(st)!=state_blocks_.end();
- };
-
- bool isStateBlockFixed(const StateBlockPtr& st) const
- {
- return state_block_fixed_.at(st);
- };
-
- bool isFactorRegistered(const FactorBasePtr& fac_ptr) const
- {
- return std::find(factors_.begin(), factors_.end(), fac_ptr) != factors_.end();
- };
-
- bool hasThisLocalParametrization(const StateBlockPtr& st, const LocalParametrizationBasePtr& local_param) const
- {
- return state_block_local_param_.find(st) != state_block_local_param_.end() && state_block_local_param_.at(st) == local_param;
- };
-
- bool hasLocalParametrization(const StateBlockPtr& st) const
- {
- return state_block_local_param_.find(st) != state_block_local_param_.end();
- };
-
- virtual void computeCovariances(const CovarianceBlocksToBeComputed blocks){};
- virtual void computeCovariances(const std::vector<StateBlockPtr>& st_list){};
- virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr){return true;};
- virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const {return true;};
-
- // The following are dummy implementations
- bool hasConverged() { return true; }
- SizeStd iterations() { return 1; }
- double initialCost() { return double(1); }
- double finalCost() { return double(0); }
-
-
-
- protected:
-
- virtual std::string solveImpl(const ReportVerbosity report_level){ return std::string("");};
- virtual void addFactor(const FactorBasePtr& fac_ptr)
- {
- factors_.push_back(fac_ptr);
- };
- virtual void removeFactor(const FactorBasePtr& fac_ptr)
- {
- factors_.remove(fac_ptr);
- };
- virtual void addStateBlock(const StateBlockPtr& state_ptr)
- {
- state_block_fixed_[state_ptr] = state_ptr->isFixed();
- state_block_local_param_[state_ptr] = state_ptr->getLocalParametrization();
- };
- virtual void removeStateBlock(const StateBlockPtr& state_ptr)
- {
- state_block_fixed_.erase(state_ptr);
- state_block_local_param_.erase(state_ptr);
- };
- virtual void updateStateBlockStatus(const StateBlockPtr& state_ptr)
- {
- state_block_fixed_[state_ptr] = state_ptr->isFixed();
- };
- virtual void updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr)
- {
- if (state_ptr->getLocalParametrization() == nullptr)
- state_block_local_param_.erase(state_ptr);
- else
- state_block_local_param_[state_ptr] = state_ptr->getLocalParametrization();
- };
-};
-
TEST(SolverManager, Create)
{
ProblemPtr P = Problem::create("PO", 2);
- SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+ SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
// check double pointers to branches
ASSERT_EQ(P, solver_manager_ptr->getProblem());
@@ -119,7 +35,7 @@ TEST(SolverManager, Create)
TEST(SolverManager, AddStateBlock)
{
ProblemPtr P = Problem::create("PO", 2);
- SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+ SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
Vector2d state; state << 1, 2;
@@ -138,7 +54,7 @@ TEST(SolverManager, AddStateBlock)
TEST(SolverManager, DoubleAddStateBlock)
{
ProblemPtr P = Problem::create("PO", 2);
- SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+ SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
Vector2d state; state << 1, 2;
@@ -163,7 +79,7 @@ TEST(SolverManager, DoubleAddStateBlock)
TEST(SolverManager, UpdateStateBlock)
{
ProblemPtr P = Problem::create("PO", 2);
- SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+ SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
Vector2d state; state << 1, 2;
@@ -211,7 +127,7 @@ TEST(SolverManager, UpdateStateBlock)
TEST(SolverManager, AddUpdateStateBlock)
{
ProblemPtr P = Problem::create("PO", 2);
- SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+ SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
Vector2d state; state << 1, 2;
@@ -249,7 +165,7 @@ TEST(SolverManager, AddUpdateStateBlock)
TEST(SolverManager, AddUpdateLocalParamStateBlock)
{
ProblemPtr P = Problem::create("PO", 2);
- SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+ SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
Vector2d state; state << 1, 2;
@@ -292,7 +208,7 @@ TEST(SolverManager, AddUpdateLocalParamStateBlock)
TEST(SolverManager, AddLocalParamRemoveLocalParamStateBlock)
{
ProblemPtr P = Problem::create("PO", 2);
- SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+ SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
Vector2d state; state << 1, 2;
@@ -339,7 +255,7 @@ TEST(SolverManager, AddLocalParamRemoveLocalParamStateBlock)
TEST(SolverManager, RemoveStateBlock)
{
ProblemPtr P = Problem::create("PO", 2);
- SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+ SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
Vector2d state; state << 1, 2;
@@ -364,7 +280,7 @@ TEST(SolverManager, RemoveStateBlock)
TEST(SolverManager, AddRemoveStateBlock)
{
ProblemPtr P = Problem::create("PO", 2);
- SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+ SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
Vector2d state; state << 1, 2;
@@ -386,7 +302,7 @@ TEST(SolverManager, AddRemoveStateBlock)
TEST(SolverManager, RemoveUpdateStateBlock)
{
ProblemPtr P = Problem::create("PO", 2);
- SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+ SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
Vector2d state; state << 1, 2;
@@ -405,7 +321,7 @@ TEST(SolverManager, RemoveUpdateStateBlock)
TEST(SolverManager, DoubleRemoveStateBlock)
{
ProblemPtr P = Problem::create("PO", 2);
- SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+ SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
Vector2d state; state << 1, 2;
@@ -430,7 +346,7 @@ TEST(SolverManager, DoubleRemoveStateBlock)
TEST(SolverManager, AddUpdatedStateBlock)
{
ProblemPtr P = Problem::create("PO", 2);
- SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+ SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
Vector2d state; state << 1, 2;
@@ -508,17 +424,17 @@ TEST(SolverManager, AddUpdatedStateBlock)
TEST(SolverManager, AddFactor)
{
ProblemPtr P = Problem::create("PO", 2);
- SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+ SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
Vector2d state; state << 1, 2;
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
// Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
+ 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);
+ auto c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false);
// notification
Notification notif;
@@ -535,17 +451,17 @@ TEST(SolverManager, AddFactor)
TEST(SolverManager, RemoveFactor)
{
ProblemPtr P = Problem::create("PO", 2);
- SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+ SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
Vector2d state; state << 1, 2;
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
// Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
+ 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);
+ auto c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false);
// update solver
solver_manager_ptr->update();
@@ -568,18 +484,18 @@ TEST(SolverManager, RemoveFactor)
TEST(SolverManager, AddRemoveFactor)
{
ProblemPtr P = Problem::create("PO", 2);
- SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+ SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
Vector2d state; state << 1, 2;
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
// Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
+ 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);
+ auto c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false);
// notification
Notification notif;
@@ -603,18 +519,18 @@ TEST(SolverManager, AddRemoveFactor)
TEST(SolverManager, DoubleRemoveFactor)
{
ProblemPtr P = Problem::create("PO", 2);
- SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+ SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
Vector2d state; state << 1, 2;
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
// Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
+ FrameBasePtr F = P->emplaceFrame(KEY, TimeStamp(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);
+ auto c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false);
// update solver
solver_manager_ptr->update();
@@ -635,9 +551,44 @@ TEST(SolverManager, DoubleRemoveFactor)
ASSERT_FALSE(solver_manager_ptr->isFactorRegistered(c));
}
+TEST(SolverManager, MultiThreadingTruncatedNotifications)
+{
+ double Dt = 5.0;
+ ProblemPtr P = Problem::create("PO", 2);
+ SolverManagerDummyPtr solver_manager_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();
+ 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();
+ 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);
+ 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();
+
+ 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)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
-
diff --git a/test/gtest_state_composite.cpp b/test/gtest_state_composite.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4d5a420119526c2fc1984aecfc22e01b40d0084b
--- /dev/null
+++ b/test/gtest_state_composite.cpp
@@ -0,0 +1,886 @@
+/*
+ * gtest_state_composite.cpp
+ *
+ * Created on: Apr 6, 2020
+ * Author: jsola
+ */
+
+#include "core/state_block/state_composite.h"
+#include "core/state_block/state_quaternion.h"
+
+#include "core/utils/utils_gtest.h"
+
+using namespace wolf;
+using namespace std;
+
+
+class StateBlockCompositeInit : public testing::Test
+{
+ public:
+
+ StateBlockPtr sbp, sbv, sbx;
+ StateQuaternionPtr sbq;
+
+ StateBlockComposite states;
+
+ void SetUp() override
+ {
+ sbp = states.emplace("P", Vector3d(1,2,3));
+ sbv = states.emplace("V", Vector3d(4,5,6));
+ sbq = states.emplace<StateQuaternion>("Q", Vector4d(.5,.5,.5,.5));
+
+ sbx = std::make_shared<StateBlock>(Vector3d(7,8,9));
+ }
+};
+
+TEST_F(StateBlockCompositeInit, stateSize)
+{
+ ASSERT_EQ( states.stateSize() , 10 );
+ ASSERT_EQ( states.stateSize("PQ"), 7 );
+ ASSERT_EQ( states.stateSize("PV"), 6 );
+}
+
+TEST_F(StateBlockCompositeInit, stateVector)
+{
+ ASSERT_EQ( states.stateVector("PQ") , (VectorXd( 7) << 1,2,3,.5,.5,.5,.5) .finished() );
+ ASSERT_EQ( states.stateVector("PVQ"), (VectorXd(10) << 1,2,3,4,5,6,.5,.5,.5,.5).finished() );
+ ASSERT_EQ( states.stateVector("PQV"), (VectorXd(10) << 1,2,3,.5,.5,.5,.5,4,5,6).finished() );
+}
+
+TEST_F(StateBlockCompositeInit, emplace)
+{
+ // Emplaces called in SetUp()
+
+ // check 3 elements
+ ASSERT_EQ(states.getStateBlockMap().size(), 3);
+}
+
+TEST_F(StateBlockCompositeInit, has_key)
+{
+ ASSERT_TRUE(states.has("P"));
+ ASSERT_FALSE(states.has("X"));
+}
+
+TEST_F(StateBlockCompositeInit, has_sb)
+{
+ ASSERT_TRUE(states.has(sbp));
+ ASSERT_FALSE(states.has(sbx));
+}
+
+TEST_F(StateBlockCompositeInit, at)
+{
+ ASSERT_EQ(states.at("P"), sbp);
+
+ ASSERT_EQ(states.at("X"), nullptr);
+}
+
+TEST_F(StateBlockCompositeInit, set_sb)
+{
+ states.set("P", sbx);
+
+ ASSERT_EQ(states.at("P"), sbx);
+
+ states.set("P", sbp);
+
+ ASSERT_DEATH(states.set("X", sbx), "");
+
+ ASSERT_EQ(states.at("X"), nullptr);
+}
+
+TEST_F(StateBlockCompositeInit, set_vec)
+{
+ Vector3d p(11,22,33);
+ Vector3d x(55,66,77);
+
+ states.set("P", p);
+
+ ASSERT_MATRIX_APPROX(states.at("P")->getState(), p, 1e-20);
+
+ ASSERT_DEATH(states.set("X", x), "");
+
+ ASSERT_EQ(states.at("X"), nullptr);
+}
+
+TEST_F(StateBlockCompositeInit, set_vectors)
+{
+ Vector3d p(11,22,33);
+ Vector4d q(11,22,33,44); q.normalize();
+ Vector3d x(55,66,77);
+
+ states.setVectors("PQ", {p, q});
+
+ ASSERT_MATRIX_APPROX(states.at("P")->getState(), p, 1e-20);
+ ASSERT_MATRIX_APPROX(states.at("Q")->getState(), q, 1e-20);
+
+ ASSERT_DEATH(states.setVectors("PX", {p,x}), "");
+
+ ASSERT_EQ(states.at("X"), nullptr);
+}
+
+TEST_F(StateBlockCompositeInit, key_ref)
+{
+ std::string key;
+ ASSERT_TRUE(states.key(sbp, key));
+ ASSERT_EQ(key, "P");
+
+ ASSERT_FALSE(states.key(sbx, key));
+ ASSERT_EQ(key, "");
+}
+
+TEST_F(StateBlockCompositeInit, key_return)
+{
+ // existing key
+ ASSERT_EQ(states.key(sbp), "P");
+
+ // non existing key returns empty string
+ ASSERT_EQ(states.key(sbx), "");
+}
+
+TEST_F(StateBlockCompositeInit, find)
+{
+ auto it = states.find(sbp);
+ ASSERT_NE(it, states.getStateBlockMap().end());
+
+ it = states.find(sbx);
+ ASSERT_EQ(it, states.getStateBlockMap().end());
+}
+
+TEST_F(StateBlockCompositeInit, add)
+{
+ states.add("X", sbx);
+
+ ASSERT_EQ(states.at("X"), sbx);
+}
+
+TEST_F(StateBlockCompositeInit, remove)
+{
+ // remove existing block
+ states.remove("V");
+ ASSERT_EQ(states.getStateBlockMap().size(), 2);
+
+ // remove non existing block -- no effect
+ states.remove("X");
+ ASSERT_EQ(states.getStateBlockMap().size(), 2);
+}
+
+TEST_F(StateBlockCompositeInit, perturb)
+{
+ ASSERT_TRUE(states.at("P")->getState().isApprox(sbp->getState(), 1e-3));
+ ASSERT_TRUE(states.at("V")->getState().isApprox(sbv->getState(), 1e-3));
+ ASSERT_TRUE(states.at("Q")->getState().isApprox(sbq->getState(), 1e-3));
+
+ states.perturb(0.1);
+
+ // values have moved wrt original
+ ASSERT_FALSE(states.at("P")->getState().isApprox(Vector3d(1,2,3), 1e-3));
+ ASSERT_FALSE(states.at("V")->getState().isApprox(Vector3d(4,5,6), 1e-3));
+ ASSERT_FALSE(states.at("Q")->getState().isApprox(Vector4d(.5,.5,.5,.5), 1e-3));
+}
+
+TEST_F(StateBlockCompositeInit, setIdentity)
+{
+ ASSERT_TRUE(states.at("P")->getState().isApprox(sbp->getState(), 1e-3));
+ ASSERT_TRUE(states.at("V")->getState().isApprox(sbv->getState(), 1e-3));
+ ASSERT_TRUE(states.at("Q")->getState().isApprox(sbq->getState(), 1e-3));
+
+ states.setIdentity();
+
+ // values have moved wrt original
+ ASSERT_TRUE(states.at("P")->getState().isApprox(Vector3d(0,0,0), 1e-10));
+ ASSERT_TRUE(states.at("V")->getState().isApprox(Vector3d(0,0,0), 1e-10));
+ ASSERT_TRUE(states.at("Q")->getState().isApprox(Vector4d(0,0,0,1), 1e-10));
+}
+
+TEST_F(StateBlockCompositeInit, identity)
+{
+ VectorComposite v = states.identity();
+
+ ASSERT_TRUE(v.at("P").isApprox(Vector3d(0,0,0), 1e-10));
+ ASSERT_TRUE(v.at("V").isApprox(Vector3d(0,0,0), 1e-10));
+ ASSERT_TRUE(v.at("Q").isApprox(Vector4d(0,0,0,1), 1e-10));
+}
+
+TEST_F(StateBlockCompositeInit, fix)
+{
+ states.fix();
+
+ ASSERT_TRUE(states.at("P")->isFixed());
+ ASSERT_TRUE(states.at("V")->isFixed());
+ ASSERT_TRUE(states.at("Q")->isFixed());
+}
+
+TEST_F(StateBlockCompositeInit, unfix)
+{
+ states.fix();
+
+ ASSERT_TRUE(states.at("P")->isFixed());
+ ASSERT_TRUE(states.at("V")->isFixed());
+ ASSERT_TRUE(states.at("Q")->isFixed());
+
+ states.unfix();
+
+ ASSERT_FALSE(states.at("P")->isFixed());
+ ASSERT_FALSE(states.at("V")->isFixed());
+ ASSERT_FALSE(states.at("Q")->isFixed());
+}
+
+TEST_F(StateBlockCompositeInit, isFixed)
+{
+ states.fix();
+
+ ASSERT_TRUE(states.isFixed());
+
+ states.at("P")->unfix();
+
+ ASSERT_FALSE(states.isFixed());
+}
+
+
+TEST(VectorComposite, constructor_empty)
+{
+ VectorComposite v;
+ ASSERT_TRUE(v.empty());
+}
+
+TEST(VectorComposite, constructor_copy)
+{
+ VectorComposite u;
+ u.emplace("a", Vector2d(1,2));
+ u.emplace("b", Vector3d(3,4,5));
+
+ VectorComposite v(u);
+
+ ASSERT_FALSE(v.empty());
+
+ ASSERT_MATRIX_APPROX(u["a"], v["a"], 1e-20);
+ ASSERT_MATRIX_APPROX(u["b"], v["b"], 1e-20);
+}
+
+TEST(VectorComposite, constructor_from_list)
+{
+ VectorComposite v(Vector4d(1,2,3,4), "ab", {3,1});
+
+ ASSERT_MATRIX_APPROX(v.at("a"), Vector3d(1,2,3), 1e-20);
+ ASSERT_MATRIX_APPROX(v.at("b"), Vector1d(4), 1e-20);
+}
+
+TEST(VectorComposite, set)
+{
+ VectorComposite v;
+
+ v.set(Vector4d(1,2,3,4), "ab", {3,1});
+
+ ASSERT_MATRIX_APPROX(v.at("a"), Vector3d(1,2,3), 1e-20);
+ ASSERT_MATRIX_APPROX(v.at("b"), Vector1d(4), 1e-20);
+}
+
+TEST(VectorComposite, operatorStream)
+{
+ using namespace Eigen;
+
+ VectorComposite x;
+
+ x.emplace("P", Vector2d(1,1));
+ x.emplace("O", Vector3d(2,2,2));
+
+ WOLF_DEBUG("X = ", x);
+}
+
+TEST(VectorComposite, operatorPlus)
+{
+ VectorComposite x, y;
+
+ x.emplace("P", Vector2d(1,1));
+ x.emplace("O", Vector3d(2,2,2));
+ y.emplace("P", Vector2d(1,1));
+ y.emplace("O", Vector3d(2,2,2));
+
+ WOLF_DEBUG("x + y = ", x + y);
+
+ ASSERT_MATRIX_APPROX((x+y).at("P"), Vector2d(2,2), 1e-20);
+ ASSERT_MATRIX_APPROX((x+y).at("O"), Vector3d(4,4,4), 1e-20);
+}
+
+TEST(VectorComposite, operatorMinus)
+{
+ VectorComposite x, y;
+
+ x.emplace("P", Vector2d(3,3));
+ x.emplace("O", Vector3d(6,6,6));
+ y.emplace("P", Vector2d(1,1));
+ y.emplace("O", Vector3d(2,2,2));
+
+ WOLF_DEBUG("x = ", x);
+ WOLF_DEBUG("y = ", y);
+ WOLF_DEBUG("x - y = ", x - y);
+
+ ASSERT_MATRIX_APPROX((x-y).at("P"), Vector2d(2,2), 1e-20);
+ ASSERT_MATRIX_APPROX((x-y).at("O"), Vector3d(4,4,4), 1e-20);
+}
+
+TEST(VectorComposite, unary_Minus)
+{
+ VectorComposite x, y;
+
+ x.emplace("P", Vector2d(1,1));
+ x.emplace("O", Vector3d(2,2,2));
+
+ WOLF_DEBUG("-x = ", -x);
+
+ ASSERT_MATRIX_APPROX((-x).at("P"), Vector2d(-1,-1), 1e-20);
+ ASSERT_MATRIX_APPROX((-x).at("O"), Vector3d(-2,-2,-2), 1e-20);
+}
+
+TEST(VectorComposite, size)
+{
+ VectorComposite x;
+
+ x.emplace("P", Vector2d(1,1));
+ x.emplace("O", Vector3d(2,2,2));
+ x.emplace("V", Vector4d(3,3,3,3));
+
+ ASSERT_EQ(x.size("PO"), 5);
+ ASSERT_EQ(x.size("VO"), 7);
+ ASSERT_EQ(x.size("PVO"), 9);
+ ASSERT_EQ(x.size("OPV"), 9);
+}
+
+TEST(VectorComposite, stateVector)
+{
+ VectorComposite x;
+
+ x.emplace("P", Vector2d(1,1));
+ x.emplace("O", Vector3d(2,2,2));
+ x.emplace("V", Vector4d(3,3,3,3));
+
+ VectorXd y(5); y<<1,1,2,2,2;
+ ASSERT_MATRIX_APPROX(x.vector("PO"), y, 1e-20);
+
+ y.resize(7);
+ y << 3,3,3,3,2,2,2;
+ ASSERT_MATRIX_APPROX(x.vector("VO"), y, 1e-20);
+
+ y.resize(9);
+ y << 1,1,3,3,3,3,2,2,2;
+ ASSERT_MATRIX_APPROX(x.vector("PVO"), y, 1e-20);
+}
+
+TEST(MatrixComposite, Constructor_empty)
+{
+ MatrixComposite M;
+
+ ASSERT_EQ(M.size() , 0);
+
+ ASSERT_TRUE(M.check());
+}
+
+TEST(MatrixComposite, Constructor_structure)
+{
+ MatrixComposite M("PO", "POV");
+
+ ASSERT_EQ(M.size() , 2);
+ ASSERT_EQ(M.at("P").size() , 3);
+
+ ASSERT_TRUE(M.check());
+}
+
+TEST(MatrixComposite, Constructor_structure_sizes)
+{
+ MatrixComposite M("PO", {3,4}, "POV", {3,4,3});
+
+ ASSERT_EQ(M.size() , 2);
+ ASSERT_EQ(M.at("P").size() , 3);
+
+ ASSERT_EQ(M.at("P","O").rows() , 3);
+ ASSERT_EQ(M.at("P","O").cols() , 4);
+
+ ASSERT_EQ(M.matrix("PO","POV").rows() , 7);
+ ASSERT_EQ(M.matrix("PO","POV").cols() , 10);
+
+ ASSERT_TRUE(M.check());
+}
+
+TEST(MatrixComposite, Constructor_eigenMatrix_structure_sizes)
+{
+ MatrixXd m ( MatrixXd::Random(7,10) );
+
+ MatrixComposite M(m, "PO", {3,4}, "POV", {3,4,3});
+
+ ASSERT_EQ(M.size() , 2);
+ ASSERT_EQ(M.at("P").size() , 3);
+
+ ASSERT_EQ(M.at("P","O").rows() , 3);
+ ASSERT_EQ(M.at("P","O").cols() , 4);
+
+ ASSERT_EQ(M.matrix("PO","POV").rows() , 7);
+ ASSERT_EQ(M.matrix("PO","POV").cols() , 10);
+
+ ASSERT_MATRIX_APPROX(M.at("P","O"), m.block(0,3,3,4), 1e-20);
+ ASSERT_MATRIX_APPROX(M.at("O","V"), m.block(3,7,4,3), 1e-20);
+
+ ASSERT_TRUE(M.check());
+}
+
+TEST(MatrixComposite, Constructor_eigenMatrix_structure_sizes_mismatches)
+{
+ MatrixXd m;
+
+// // input m has too few rows
+// m.setRandom(6,10);
+// MatrixComposite M1(m, "PO", {3,4}, "POV", {3,4,3});
+
+// // input m has too many rows
+// m.setRandom(8,10);
+// MatrixComposite M2(m, "PO", {3,4}, "POV", {3,4,3});
+
+// // input m has too few cols
+// m.setRandom(7,9) ;
+// MatrixComposite M3(m, "PO", {3,4}, "POV", {3,4,3});
+
+// // input m has too many cols
+// m.setRandom(7,11) ;
+// MatrixComposite M4(m, "PO", {3,4}, "POV", {3,4,3});
+
+}
+
+TEST(MatrixComposite, Zero)
+{
+ MatrixComposite M = MatrixComposite::zero("PO", {3,4}, "POV", {3,4,3});
+
+ ASSERT_MATRIX_APPROX(M.matrix("PO", "POV"), MatrixXd::Zero(7,10), 1e-20);
+
+ ASSERT_TRUE(M.check());
+}
+
+TEST(MatrixComposite, Identity)
+{
+ MatrixComposite M = MatrixComposite::identity("POV", {3,4,3});
+
+ ASSERT_MATRIX_APPROX(M.matrix("POV", "POV"), MatrixXd::Identity(10,10), 1e-20);
+
+ ASSERT_TRUE(M.check());
+}
+
+TEST(MatrixComposite, emplace_operatorStream)
+{
+ MatrixComposite M;
+
+ unsigned int psize, osize;
+ psize = 2;
+ osize = 3;
+
+ MatrixXd Mpp(psize,psize), Mpo(psize, osize), Mop(osize,psize), Moo(osize,osize);
+
+ Mpp.setOnes();
+ Mpo.setOnes(); Mpo *= 2;
+ Mop.setOnes(); Mop *= 3;
+ Moo.setOnes(); Moo *= 4;
+
+ ASSERT_TRUE(M.emplace("P", "P", Mpp));
+ ASSERT_TRUE(M.emplace("P", "O", Mpo));
+ ASSERT_TRUE(M.emplace("O", "P", Mop));
+ ASSERT_TRUE(M.emplace("O", "O", Moo));
+
+ cout << "M = " << M << endl;
+}
+
+//TEST(MatrixComposite, operatorBrackets)
+//{
+// MatrixComposite M;
+//
+// unsigned int psize, osize;
+// psize = 2;
+// osize = 3;
+//
+// MatrixXd Mpp(psize,psize), Mpo(psize, osize), Mop(osize,psize), Moo(osize,osize);
+//
+// Mpp.setOnes();
+// Mpo.setOnes(); Mpo *= 2;
+// Mop.setOnes(); Mop *= 3;
+// Moo.setOnes(); Moo *= 4;
+//
+// M.emplace("P", "P", Mpp);
+// ASSERT_MATRIX_APPROX( M["P"]["P"], Mpp, 1e-20);
+//
+// M.emplace("P", "O", Mpo);
+// ASSERT_MATRIX_APPROX( M["P"]["O"], Mpo, 1e-20);
+//
+// // return default empty matrix if block not present
+// MatrixXd N = M["O"]["O"];
+// ASSERT_EQ(N.rows(), 0);
+// ASSERT_EQ(N.cols(), 0);
+//}
+
+//TEST(MatrixComposite, operatorParenthesis)
+//{
+// MatrixComposite M;
+//
+// unsigned int psize, osize;
+// psize = 2;
+// osize = 3;
+//
+// MatrixXd Mpp(psize,psize), Mpo(psize, osize), Mop(osize,psize), Moo(osize,osize);
+//
+// Mpp.setOnes();
+// Mpo.setOnes(); Mpo *= 2;
+// Mop.setOnes(); Mop *= 3;
+// Moo.setOnes(); Moo *= 4;
+//
+// M.emplace("P", "P", Mpp);
+// ASSERT_MATRIX_APPROX( M("P", "P"), Mpp, 1e-20);
+//
+// M.emplace("P", "O", Mpo);
+// ASSERT_MATRIX_APPROX( M("P", "O"), Mpo, 1e-20);
+//
+// // return default empty matrix if block not present
+// MatrixXd N = M("O", "O");
+// ASSERT_EQ(N.rows(), 0);
+// ASSERT_EQ(N.cols(), 0);
+//}
+
+TEST(MatrixComposite, operatorAt)
+{
+ MatrixComposite M;
+
+ unsigned int psize, osize;
+ psize = 2;
+ osize = 3;
+
+ MatrixXd Mpp(psize,psize), Mpo(psize, osize), Mop(osize,psize), Moo(osize,osize);
+
+ Mpp.setOnes();
+ Mpo.setOnes(); Mpo *= 2;
+ Mop.setOnes(); Mop *= 3;
+ Moo.setOnes(); Moo *= 4;
+
+ M.emplace("P", "P", Mpp);
+ ASSERT_MATRIX_APPROX( M.at("P", "P"), Mpp, 1e-20);
+
+ M.emplace("P", "O", Mpo);
+ ASSERT_MATRIX_APPROX( M.at("P", "O"), Mpo, 1e-20);
+
+ // error if block not present
+ ASSERT_DEATH(MatrixXd N = M.at("O", "O"), "");
+}
+
+TEST(MatrixComposite, productScalar)
+{
+ unsigned int psize, osize;
+ psize = 2;
+ osize = 3;
+
+ MatrixComposite M;
+
+ MatrixXd Mpp(psize,psize), Mpo(psize, osize), Mop(osize,psize), Moo(osize,osize);
+ Mpp.setOnes();
+ Mpo.setOnes(); Mpo *= 2;
+ Mop.setOnes(); Mop *= 3;
+ Moo.setOnes(); Moo *= 4;
+
+ M.emplace("P", "P", Mpp);
+ M.emplace("P", "O", Mpo);
+ M.emplace("O", "P", Mop);
+ M.emplace("O", "O", Moo);
+ WOLF_DEBUG("M = " , M);
+
+
+ // right-multiply by scalar
+ MatrixComposite R = M * 1.2;
+ ASSERT_MATRIX_APPROX(R.matrix("PO","PO"), 1.2 * M.matrix("PO","PO"), 1e-20);
+
+
+ // left-multiply by scalar
+ MatrixComposite L = 1.2 * M;
+ ASSERT_MATRIX_APPROX(L.matrix("PO","PO"), 1.2 * M.matrix("PO","PO"), 1e-20);
+
+
+}
+
+TEST(MatrixComposite, productVector)
+{
+ unsigned int psize, osize;
+ psize = 2;
+ osize = 3;
+
+ VectorComposite x;
+ x.emplace("P", Vector2d(1,1));
+ x.emplace("O", Vector3d(2,2,2));
+
+ cout << "x= " << x << endl;
+
+// WOLF_DEBUG("x = " , x);
+
+ MatrixComposite M;
+
+ MatrixXd Mpp(psize,psize), Mpo(psize, osize), Mop(osize,psize), Moo(osize,osize);
+ Mpp.setOnes();
+ Mpo.setOnes(); Mpo *= 2;
+ Mop.setOnes(); Mop *= 3;
+ Moo.setOnes(); Moo *= 4;
+
+ M.emplace("P", "P", Mpp);
+ M.emplace("P", "O", Mpo);
+ M.emplace("O", "P", Mop);
+ M.emplace("O", "O", Moo);
+ WOLF_DEBUG("M = " , M);
+
+ VectorComposite y;
+
+ y = M * x;
+
+// WOLF_DEBUG("y = M * x = " , y);
+
+ /* M * x = y
+ * p o
+ * p [1 1 2 2 2] p [1] p [14]
+ * [1 1 2 2 2] [1] [14]
+ * [3 3 4 4 4] * [2] = [30]
+ * o [3 3 4 4 4] o [2] o [30]
+ * [3 3 4 4 4] [2] [30]
+ */
+
+ Vector2d yp(14,14);
+ Vector3d yo(30,30,30);
+
+ ASSERT_MATRIX_APPROX(y.at("P"), yp, 1e-20);
+ ASSERT_MATRIX_APPROX(y.at("O"), yo, 1e-20);
+
+ // throw if x has extra blocks
+ // x.emplace("V", Vector2d(3,3));
+ // ASSERT_DEATH(y = M * x , ""); // M * x --> does not die if x has extra blocks wrt M
+
+ // throw if x has missing blocks
+ // x.erase("O");
+ // cout << "x = " << x << endl;
+ // ASSERT_DEATH(y = M * x , ""); // M * x --> exception if x has missing blocks wrt M, not caught by ASSERT_DEATH
+
+}
+
+TEST(MatrixComposite, product)
+{
+ unsigned int psize, osize, vsize;
+ psize = 2;
+ osize = 1;
+ vsize = 2;
+
+ MatrixComposite M, N;
+
+ MatrixXd Mpp(psize,psize), Mpo(psize, osize), Mop(osize,psize), Moo(osize,osize);
+ Mpp.setOnes();
+ Mpo.setOnes(); Mpo *= 2;
+ Mop.setOnes(); Mop *= 3;
+ Moo.setOnes(); Moo *= 4;
+
+ M.emplace("P", "P", Mpp);
+ M.emplace("P", "O", Mpo);
+ M.emplace("O", "P", Mop);
+ M.emplace("O", "O", Moo);
+ WOLF_DEBUG("M = " , M);
+
+ MatrixXd Noo(osize,osize), Nov(osize, vsize), Npo(psize,osize), Npv(psize,vsize);
+ Noo.setOnes();
+ Nov.setOnes(); Nov *= 2;
+ Npo.setOnes(); Npo *= 3;
+ Npv.setOnes(); Npv *= 4;
+
+ N.emplace("O", "O", Noo);
+ N.emplace("O", "V", Nov);
+ N.emplace("P", "O", Npo);
+ N.emplace("P", "V", Npv);
+ WOLF_DEBUG("N = " , N);
+
+ MatrixComposite MN;
+
+ MN = M * N;
+
+ WOLF_DEBUG("MN = M * N = " , MN);
+
+ /* M * N = MN
+ * p o o v o v
+ * p [1 1 2] p [3 4 4] p [ 8 12 12]
+ * [1 1 2] * [3 4 4] = [ 8 12 12]
+ * o [3 3 4] o [1 2 2] o [22 32 32]
+ */
+
+ MatrixXd MNpo(MatrixXd::Ones(psize,osize) * 8);
+ MatrixXd MNpv(MatrixXd::Ones(psize,vsize) * 12);
+ MatrixXd MNoo(MatrixXd::Ones(osize,osize) * 22);
+ MatrixXd MNov(MatrixXd::Ones(osize,vsize) * 32);
+
+ ASSERT_MATRIX_APPROX(MN.at("P","O"), MNpo, 1e-20);
+ ASSERT_MATRIX_APPROX(MN.at("P","V"), MNpv, 1e-20);
+ ASSERT_MATRIX_APPROX(MN.at("O","O"), MNoo, 1e-20);
+ ASSERT_MATRIX_APPROX(MN.at("O","V"), MNov, 1e-20);
+
+ ASSERT_TRUE(MN.check());
+}
+
+TEST(MatrixComposite, propagate)
+{
+ unsigned int psize, osize, vsize, wsize;
+ psize = 2;
+ osize = 3;
+ vsize = 4;
+ wsize = 1;
+
+ MatrixComposite Q, J;
+
+ MatrixXd Qpp(psize,psize), Qpo(psize, osize), Qop(osize,psize), Qoo(osize,osize);
+ Qpp.setOnes();
+ Qpo.setOnes(); Qpo *= 2;
+ Qop.setOnes(); Qop *= 2;
+ Qoo.setOnes(); Qoo *= 3;
+
+ Q.emplace("P", "P", Qpp);
+ Q.emplace("P", "O", Qpo);
+ Q.emplace("O", "P", Qop);
+ Q.emplace("O", "O", Qoo);
+ WOLF_DEBUG("Q = " , Q);
+
+ MatrixXd Jvp(vsize,psize), Jvo(vsize, osize), Jwp(wsize,psize), Jwo(wsize,osize);
+ Jvp.setOnes();
+ Jvo.setOnes(); Jvo *= 2;
+ Jwp.setOnes(); Jwp *= 3;
+ Jwo.setOnes(); Jwo *= 4;
+
+ J.emplace("V", "P", Jvp);
+ J.emplace("V", "O", Jvo);
+ J.emplace("W", "P", Jwp);
+ J.emplace("W", "O", Jwo);
+ WOLF_DEBUG("J = " , J);
+
+ MatrixComposite JQJt;
+
+ JQJt = J.propagate(Q);
+
+ WOLF_DEBUG("JQJt = J * Q * J.tr = " , JQJt);
+
+ WOLF_DEBUG("JQJt = J * Q * J.tr = \n" , JQJt.matrix("VW", "VW"));
+
+ ASSERT_MATRIX_APPROX(JQJt.matrix("VW", "VW"), J.matrix("VW","PO") * Q.matrix("PO","PO") * J.matrix("VW","PO").transpose(), 1e-8);
+
+ ASSERT_TRUE(JQJt.check());
+
+}
+
+TEST(MatrixComposite, sum)
+{
+ unsigned int psize, osize;
+ psize = 2;
+ osize = 1;
+
+ MatrixComposite M, N;
+
+ MatrixXd Mpp(psize,psize), Mpo(psize, osize), Mop(osize,psize), Moo(osize,osize);
+ Mpp.setOnes();
+ Mpo.setOnes(); Mpo *= 2;
+ Mop.setOnes(); Mop *= 3;
+ Moo.setOnes(); Moo *= 4;
+
+ M.emplace("P", "P", Mpp);
+ M.emplace("P", "O", Mpo);
+ M.emplace("O", "P", Mop);
+ M.emplace("O", "O", Moo);
+ WOLF_DEBUG("M = " , M);
+ N.emplace("P", "P", Mpp);
+ N.emplace("P", "O", Mpo);
+ N.emplace("O", "P", Mop);
+ N.emplace("O", "O", Moo);
+ WOLF_DEBUG("N = " , N);
+
+ MatrixComposite MpN;
+
+ MpN = M + N;
+
+ WOLF_DEBUG("MpN = M + N = " , MpN);
+
+ ASSERT_MATRIX_APPROX(MpN.at("P","P"), 2 * Mpp, 1e-10);
+ ASSERT_MATRIX_APPROX(MpN.at("P","O"), 2 * Mpo, 1e-10);
+ ASSERT_MATRIX_APPROX(MpN.at("O","P"), 2 * Mop, 1e-10);
+ ASSERT_MATRIX_APPROX(MpN.at("O","O"), 2 * Moo, 1e-10);
+
+}
+
+TEST(MatrixComposite, difference)
+{
+ unsigned int psize, osize;
+ psize = 2;
+ osize = 1;
+
+ MatrixComposite M, N;
+
+ MatrixXd Mpp(psize,psize), Mpo(psize, osize), Mop(osize,psize), Moo(osize,osize);
+ Mpp.setOnes();
+ Mpo.setOnes(); Mpo *= 2;
+ Mop.setOnes(); Mop *= 3;
+ Moo.setOnes(); Moo *= 4;
+
+ M.emplace("P", "P", Mpp);
+ M.emplace("P", "O", Mpo);
+ M.emplace("O", "P", Mop);
+ M.emplace("O", "O", Moo);
+ WOLF_DEBUG("M = " , M);
+ N.emplace("P", "P", Mpp);
+ N.emplace("P", "O", Mpo);
+ N.emplace("O", "P", Mop);
+ N.emplace("O", "O", Moo);
+ WOLF_DEBUG("N = " , N);
+
+ MatrixComposite MmN;
+
+ MmN = M - N;
+
+ WOLF_DEBUG("MmN = M - N = " , MmN);
+
+ ASSERT_MATRIX_APPROX(MmN.at("P","P"), Mpp * 0, 1e-10);
+ ASSERT_MATRIX_APPROX(MmN.at("P","O"), Mpo * 0, 1e-10);
+ ASSERT_MATRIX_APPROX(MmN.at("O","P"), Mop * 0, 1e-10);
+ ASSERT_MATRIX_APPROX(MmN.at("O","O"), Moo * 0, 1e-10);
+
+}
+
+TEST(MatrixComposite, unary_minus)
+{
+ unsigned int psize, osize;
+ psize = 2;
+ osize = 1;
+
+ MatrixComposite M, N;
+
+ MatrixXd Mpp(psize,psize), Mpo(psize, osize), Mop(osize,psize), Moo(osize,osize);
+ Mpp.setOnes();
+ Mpo.setOnes(); Mpo *= 2;
+ Mop.setOnes(); Mop *= 3;
+ Moo.setOnes(); Moo *= 4;
+
+ M.emplace("P", "P", Mpp);
+ M.emplace("P", "O", Mpo);
+ M.emplace("O", "P", Mop);
+ M.emplace("O", "O", Moo);
+ WOLF_DEBUG("M = " , M);
+
+ MatrixComposite m;
+
+ m = - M;
+
+ WOLF_DEBUG("m = - M = " , m);
+
+ ASSERT_MATRIX_APPROX(m.at("P","P"), - M.at("P","P"), 1e-10);
+ ASSERT_MATRIX_APPROX(m.at("P","O"), - M.at("P","O"), 1e-10);
+ ASSERT_MATRIX_APPROX(m.at("O","P"), - M.at("O","P"), 1e-10);
+ ASSERT_MATRIX_APPROX(m.at("O","O"), - M.at("O","O"), 1e-10);
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+
+ // restrict to a group of tests only
+ // ::testing::GTEST_FLAG(filter) = "VectorComposite.*";
+
+ // restrict to this test only
+ // ::testing::GTEST_FLAG(filter) = "MatrixComposite.propagate";
+// ::testing::GTEST_FLAG(filter) = "MatrixComposite.*";
+
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_track_matrix.cpp b/test/gtest_track_matrix.cpp
index 1f641dcf0da9765ddd155b5d35eaf87041879828..5b333f26c6cb097957a6963e0602314ba3eef481 100644
--- a/test/gtest_track_matrix.cpp
+++ b/test/gtest_track_matrix.cpp
@@ -35,11 +35,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, 0.0, nullptr);
- F1 = FrameBase::emplace<FrameBase>(nullptr, 1.0, nullptr);
- F2 = FrameBase::emplace<FrameBase>(nullptr, 2.0, nullptr);
- F3 = FrameBase::emplace<FrameBase>(nullptr, 3.0, nullptr);
- F4 = FrameBase::emplace<FrameBase>(nullptr, 4.0, nullptr);
+ 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);
// unlinked features
// Some warnings will be thrown "linking with nullptr" for emplacing without providing capture pointer
diff --git a/test/gtest_trajectory.cpp b/test/gtest_trajectory.cpp
index 82c65c284c5f06b518943679ac70ae3f2803bc42..9be1e82fafac833ca90d000e487072d5488ff240 100644
--- a/test/gtest_trajectory.cpp
+++ b/test/gtest_trajectory.cpp
@@ -12,37 +12,12 @@
#include "core/trajectory/trajectory_base.h"
#include "core/frame/frame_base.h"
#include "core/solver/solver_manager.h"
+#include "dummy/solver_manager_dummy.h"
#include <iostream>
using namespace wolf;
-struct DummySolverManager : public SolverManager
-{
- DummySolverManager(const ProblemPtr& _problem)
- : SolverManager(_problem)
- {
- //
- }
- virtual void computeCovariances(const CovarianceBlocksToBeComputed blocks){};
- virtual void computeCovariances(const std::vector<StateBlockPtr>& st_list){};
- virtual bool hasConverged(){return true;};
- virtual SizeStd iterations(){return 0;};
- virtual double initialCost(){return 0;};
- virtual double finalCost(){return 0;};
- virtual std::string solveImpl(const ReportVerbosity report_level){return std::string("");};
- virtual void addFactor(const FactorBasePtr& fac_ptr){};
- virtual void removeFactor(const FactorBasePtr& fac_ptr){};
- virtual void addStateBlock(const StateBlockPtr& state_ptr){};
- virtual void removeStateBlock(const StateBlockPtr& state_ptr){};
- virtual void updateStateBlockStatus(const StateBlockPtr& state_ptr){};
- virtual void updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr){};
- virtual bool isStateBlockRegistered(const StateBlockPtr& state_ptr){return true;};
- virtual bool isFactorRegistered(const FactorBasePtr& fac_ptr) const {return true;};
- virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr){return true;};
- virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const {return true;};
-};
-
/// Set to true if you want debug info
bool debug = false;
@@ -57,10 +32,10 @@ TEST(TrajectoryBase, ClosestKeyFrame)
// 1 2 3 4 time stamps
// --+-----+-----+-----+---> time
- FrameBasePtr F1 = P->emplaceFrame(KEY, Eigen::Vector3d::Zero(), 1);
- FrameBasePtr F2 = P->emplaceFrame(KEY, Eigen::Vector3d::Zero(), 2);
- FrameBasePtr F3 = P->emplaceFrame(AUXILIARY, Eigen::Vector3d::Zero(), 3);
- FrameBasePtr F4 = P->emplaceFrame(NON_ESTIMATED, Eigen::Vector3d::Zero(), 4);
+ 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 KF; // closest key-frame queried
@@ -94,9 +69,9 @@ TEST(TrajectoryBase, ClosestKeyOrAuxFrame)
// 1 2 3 time stamps
// --+-----+-----+---> time
- FrameBasePtr F1 = P->emplaceFrame(KEY, Eigen::Vector3d::Zero(), 1);
- FrameBasePtr F2 = P->emplaceFrame(AUXILIARY, Eigen::Vector3d::Zero(), 2);
- FrameBasePtr F3 = P->emplaceFrame(NON_ESTIMATED, Eigen::Vector3d::Zero(), 3);
+ 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
@@ -123,7 +98,7 @@ TEST(TrajectoryBase, Add_Remove_Frame)
ProblemPtr P = Problem::create("PO", 2);
TrajectoryBasePtr T = P->getTrajectory();
- DummySolverManager N(P);
+ SolverManagerDummy N(P);
// Trajectory status:
// KF1 KF2 F3 frames
@@ -133,21 +108,21 @@ TEST(TrajectoryBase, Add_Remove_Frame)
std::cout << __LINE__ << std::endl;
// add F1
- FrameBasePtr F1 = P->emplaceFrame(KEY, Eigen::Vector3d::Zero(), 1); // 2 non-fixed
+ FrameBasePtr F1 = P->emplaceFrame(KEY, 1, Eigen::Vector3d::Zero()); // 2 non-fixed
if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getFrameList(). size(), (SizeStd) 1);
ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 2);
std::cout << __LINE__ << std::endl;
// add F2
- FrameBasePtr F2 = P->emplaceFrame(KEY, Eigen::Vector3d::Zero(), 2); // 1 fixed, 1 not
+ FrameBasePtr F2 = P->emplaceFrame(KEY, 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(P->getStateBlockNotificationMapSize(), (SizeStd) 4);
std::cout << __LINE__ << std::endl;
// add F3
- FrameBasePtr F3 = P->emplaceFrame(NON_ESTIMATED, Eigen::Vector3d::Zero(), 3);
+ FrameBasePtr F3 = P->emplaceFrame(NON_ESTIMATED, 3, Eigen::Vector3d::Zero());
if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getFrameList(). size(), (SizeStd) 3);
ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 4);
@@ -203,19 +178,19 @@ TEST(TrajectoryBase, KeyFramesAreSorted)
// --+-----+-----+---> time
// add frames and keyframes in random order --> keyframes must be sorted after that
- FrameBasePtr F2 = P->emplaceFrame(KEY, Eigen::Vector3d::Zero(), 2);
+ 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, Eigen::Vector3d::Zero(), 3); // non-key-frame
+ 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, Eigen::Vector3d::Zero(), 1);
+ 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());
@@ -227,7 +202,7 @@ TEST(TrajectoryBase, KeyFramesAreSorted)
ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F3->id());
ASSERT_EQ(T->getLastKeyFrame() ->id(), F3->id());
- auto F4 = P->emplaceFrame(NON_ESTIMATED, Eigen::Vector3d::Zero(), 1.5);
+ 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
@@ -265,7 +240,7 @@ TEST(TrajectoryBase, KeyFramesAreSorted)
if (debug) P->print(2,0,1,0);
ASSERT_EQ(T->getFrameList().front()->id(), F4->id());
- auto F5 = P->emplaceFrame(AUXILIARY, Eigen::Vector3d::Zero(), 1.5);
+ 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
@@ -285,7 +260,7 @@ TEST(TrajectoryBase, KeyFramesAreSorted)
ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F5->id());
ASSERT_EQ(T->getLastKeyFrame() ->id(), F2->id());
- auto F6 = P->emplaceFrame(NON_ESTIMATED, Eigen::Vector3d::Zero(), 6);
+ 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
@@ -295,7 +270,7 @@ TEST(TrajectoryBase, KeyFramesAreSorted)
ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F5->id());
ASSERT_EQ(T->getLastKeyFrame() ->id(), F2->id());
- auto F7 = P->emplaceFrame(NON_ESTIMATED, Eigen::Vector3d::Zero(), 5.5);
+ 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
diff --git a/test/gtest_tree_manager.cpp b/test/gtest_tree_manager.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..507d2104e14bf43f57d563dc4873481322f57090
--- /dev/null
+++ b/test/gtest_tree_manager.cpp
@@ -0,0 +1,107 @@
+#include "core/utils/utils_gtest.h"
+#include "core/utils/logging.h"
+
+#include "core/problem/problem.h"
+#include "dummy/tree_manager_dummy.h"
+#include "core/yaml/parser_yaml.h"
+
+using namespace wolf;
+using namespace Eigen;
+
+std::string wolf_root = _WOLF_ROOT_DIR;
+
+TEST(TreeManager, make_shared)
+{
+ ProblemPtr P = Problem::create("PO", 2);
+
+ auto ParamsGM = std::make_shared<ParamsTreeManagerBase>();
+
+ auto GM = std::make_shared<TreeManagerDummy>(ParamsGM);
+
+ P->setTreeManager(GM);
+
+ ASSERT_EQ(std::static_pointer_cast<TreeManagerDummy>(P->getTreeManager())->n_KF_, 0);
+ ASSERT_EQ(P->getTreeManager(), GM);
+}
+
+TEST(TreeManager, createParams)
+{
+ ProblemPtr P = Problem::create("PO", 2);
+
+ auto ParamsGM = std::make_shared<ParamsTreeManagerBase>();
+
+ auto GM = TreeManagerDummy::create("tree_manager", ParamsGM);
+
+ ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerDummy>(GM) != nullptr);
+
+ P->setTreeManager(GM);
+
+ ASSERT_EQ(std::static_pointer_cast<TreeManagerDummy>(P->getTreeManager())->n_KF_, 0);
+ ASSERT_EQ(P->getTreeManager(), GM);
+}
+
+TEST(TreeManager, createParamServer)
+{
+ ProblemPtr P = Problem::create("PO", 2);
+
+ ParserYAML parser = ParserYAML("test/yaml/params_tree_manager1.yaml", wolf_root);
+ ParamsServer server = ParamsServer(parser.getParams());
+
+ auto GM = TreeManagerDummy::create("tree_manager", server);
+
+ ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerDummy>(GM) != nullptr);
+
+ P->setTreeManager(GM);
+
+ ASSERT_EQ(std::static_pointer_cast<TreeManagerDummy>(P->getTreeManager())->n_KF_, 0);
+ ASSERT_EQ(P->getTreeManager(), GM);
+}
+
+TEST(TreeManager, autoConf)
+{
+
+ ParserYAML parser = ParserYAML("test/yaml/params_tree_manager1.yaml", wolf_root);
+ ParamsServer server = ParamsServer(parser.getParams());
+
+ ProblemPtr P = Problem::autoSetup(server);
+ P->applyPriorOptions(0);
+
+ ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerDummy>(P->getTreeManager()) != nullptr);
+ ASSERT_EQ(std::static_pointer_cast<TreeManagerDummy>(P->getTreeManager())->n_KF_, 1); // prior KF
+}
+
+TEST(TreeManager, autoConfNone)
+{
+
+ ParserYAML parser = ParserYAML("test/yaml/params_tree_manager2.yaml", wolf_root);
+ ParamsServer server = ParamsServer(parser.getParams());
+
+ ProblemPtr P = Problem::autoSetup(server);
+ P->applyPriorOptions(0);
+
+ ASSERT_TRUE(P->getTreeManager() == nullptr); // params_tree_manager2.yaml problem/tree_manager/type: None
+}
+
+TEST(TreeManager, keyFrameCallback)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+
+ auto ParamsGM = std::make_shared<ParamsTreeManagerBase>();
+
+ auto GM = std::make_shared<TreeManagerDummy>(ParamsGM);
+
+ P->setTreeManager(GM);
+
+ ASSERT_EQ(GM->n_KF_, 0);
+
+ auto F0 = P->emplaceFrame(KEY, 0, "PO", 3, VectorXd(7) );
+ P->keyFrameCallback(F0, nullptr, 0);
+
+ ASSERT_EQ(GM->n_KF_, 1);
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_tree_manager_sliding_window.cpp b/test/gtest_tree_manager_sliding_window.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8909987f091831fddc686c585e827035ce629ebd
--- /dev/null
+++ b/test/gtest_tree_manager_sliding_window.cpp
@@ -0,0 +1,269 @@
+#include "core/utils/utils_gtest.h"
+#include "core/utils/logging.h"
+
+#include "core/problem/problem.h"
+#include "core/tree_manager/tree_manager_sliding_window.h"
+#include "core/yaml/parser_yaml.h"
+#include "core/capture/capture_void.h"
+#include "core/feature/feature_base.h"
+#include "core/factor/factor_odom_3d.h"
+#include "core/factor/factor_pose_3d.h"
+
+using namespace wolf;
+using namespace Eigen;
+
+std::string wolf_root = _WOLF_ROOT_DIR;
+
+TEST(TreeManagerSlidingWindow, make_shared)
+{
+ ProblemPtr P = Problem::create("PO", 2);
+
+ auto ParamsGM = std::make_shared<ParamsTreeManagerSlidingWindow>();
+
+ auto GM = std::make_shared<TreeManagerSlidingWindow>(ParamsGM);
+
+ P->setTreeManager(GM);
+
+ ASSERT_EQ(P->getTreeManager(), GM);
+}
+
+TEST(TreeManagerSlidingWindow, createParams)
+{
+ ProblemPtr P = Problem::create("PO", 2);
+
+ auto ParamsGM = std::make_shared<ParamsTreeManagerSlidingWindow>();
+
+ auto GM = TreeManagerSlidingWindow::create("tree_manager", ParamsGM);
+
+ ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(GM) != nullptr);
+
+ P->setTreeManager(GM);
+
+ ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(P->getTreeManager()) != nullptr);
+ ASSERT_EQ(P->getTreeManager(), GM);
+}
+
+TEST(TreeManagerSlidingWindow, createParamServer)
+{
+ ProblemPtr P = Problem::create("PO", 2);
+
+ ParserYAML parser = ParserYAML("test/yaml/params_tree_manager_sliding_window1.yaml", wolf_root);
+ ParamsServer server = ParamsServer(parser.getParams());
+
+ auto GM = TreeManagerSlidingWindow::create("tree_manager", server);
+
+ ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(GM) != nullptr);
+
+ P->setTreeManager(GM);
+
+ ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(P->getTreeManager()) != nullptr);
+ ASSERT_EQ(P->getTreeManager(), GM);
+}
+
+TEST(TreeManagerSlidingWindow, autoConf)
+{
+ ParserYAML parser = ParserYAML("test/yaml/params_tree_manager_sliding_window1.yaml", wolf_root);
+ ParamsServer server = ParamsServer(parser.getParams());
+
+ ProblemPtr P = Problem::autoSetup(server);
+
+ ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(P->getTreeManager()) != nullptr);
+}
+
+TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
+{
+ ParserYAML parser = ParserYAML("test/yaml/params_tree_manager_sliding_window1.yaml", wolf_root);
+ ParamsServer server = ParamsServer(parser.getParams());
+
+ ProblemPtr P = Problem::autoSetup(server);
+ P->applyPriorOptions(0);
+
+ // FRAME 1 ----------------------------------------------------------
+ auto F1 = P->getTrajectory()->getLastKeyFrame();
+ ASSERT_TRUE(F1 != nullptr);
+
+ Vector7d state = F1->getStateVector();
+ Vector7d zero_disp(state);
+ Matrix6d cov = Matrix6d::Identity();
+
+ // FRAME 2 ----------------------------------------------------------
+ auto F2 = P->emplaceFrame(KEY, TimeStamp(2), "PO", 3, state);
+ P->keyFrameCallback(F2, nullptr, 0);
+
+ // absolute factor
+ auto C2 = CaptureBase::emplace<CaptureVoid>(F2, TimeStamp(2), nullptr);
+ auto f2 = FeatureBase::emplace<FeatureBase>(C2, "absolute", state, cov);
+ auto c2 = FactorBase::emplace<FactorPose3d>(f2, f2, nullptr, false);
+ // displacement
+ auto C12 = CaptureBase::emplace<CaptureVoid>(F2, TimeStamp(2), nullptr);
+ auto f12 = FeatureBase::emplace<FeatureBase>(C12, "odom", zero_disp, cov);
+ auto c12 = FactorBase::emplace<FactorOdom3d>(f12, f12, F1, nullptr, false);
+
+ // Check no frame removed
+ ASSERT_FALSE(F1->isRemoving());
+
+ // FRAME 3 ----------------------------------------------------------
+ auto F3 = P->emplaceFrame(KEY, TimeStamp(3), "PO", 3, state);
+ P->keyFrameCallback(F3, nullptr, 0);
+
+ // absolute factor
+ auto C3 = CaptureBase::emplace<CaptureVoid>(F3, TimeStamp(3), nullptr);
+ auto f3 = FeatureBase::emplace<FeatureBase>(C3, "absolute", state, cov);
+ auto c3 = FactorBase::emplace<FactorPose3d>(f3, f3, nullptr, false);
+ // displacement
+ auto C23 = CaptureBase::emplace<CaptureVoid>(F3, TimeStamp(3), nullptr);
+ auto f23 = FeatureBase::emplace<FeatureBase>(C23, "odom", zero_disp, cov);
+ auto c23 = FactorBase::emplace<FactorOdom3d>(f23, f23, F2, nullptr, false);
+
+ // Check no frame removed
+ ASSERT_FALSE(F1->isRemoving());
+
+ // FRAME 4 ----------------------------------------------------------
+ auto F4 = P->emplaceFrame(KEY, TimeStamp(4), "PO", 3, state);
+ P->keyFrameCallback(F4, nullptr, 0);
+
+ // absolute factor
+ auto C4 = CaptureBase::emplace<CaptureVoid>(F4, TimeStamp(4), nullptr);
+ auto f4 = FeatureBase::emplace<FeatureBase>(C4, "absolute", state, cov);
+ auto c4 = FactorBase::emplace<FactorPose3d>(f4, f4, nullptr, false);
+ // displacement
+ auto C34 = CaptureBase::emplace<CaptureVoid>(F4, TimeStamp(4), nullptr);
+ auto f34 = FeatureBase::emplace<FeatureBase>(C34, "odom", zero_disp, cov);
+ auto c34 = FactorBase::emplace<FactorOdom3d>(f34, f34, F3, nullptr, false);
+
+ // Checks
+ ASSERT_TRUE(F1->isRemoving());
+ ASSERT_TRUE(c12->isRemoving());
+ ASSERT_TRUE(C12->isRemoving()); //Virally removed
+ ASSERT_TRUE(f12->isRemoving()); //Virally removed
+ ASSERT_TRUE(F2->isFixed()); //Fixed
+
+ // FRAME 5 ----------------------------------------------------------
+ auto F5 = P->emplaceFrame(KEY, TimeStamp(5), "PO", 3, state);
+ P->keyFrameCallback(F5, nullptr, 0);
+
+ // absolute factor
+ auto C5 = CaptureBase::emplace<CaptureVoid>(F5, TimeStamp(5), nullptr);
+ auto f5 = FeatureBase::emplace<FeatureBase>(C5, "absolute", state, cov);
+ auto c5 = FactorBase::emplace<FactorPose3d>(f5, f5, nullptr, false);
+ // displacement
+ auto C45 = CaptureBase::emplace<CaptureVoid>(F5, TimeStamp(5), nullptr);
+ auto f45 = FeatureBase::emplace<FeatureBase>(C45, "odom", zero_disp, cov);
+ auto c45 = FactorBase::emplace<FactorOdom3d>(f45, f45, F4, nullptr, false);
+
+ // Checks
+ ASSERT_TRUE(F1->isRemoving());
+ ASSERT_TRUE(c12->isRemoving());
+ ASSERT_TRUE(C12->isRemoving()); //Virally removed
+ ASSERT_TRUE(f12->isRemoving()); //Virally removed
+ ASSERT_TRUE(F2->isRemoving());
+ ASSERT_TRUE(c2->isRemoving());
+ ASSERT_TRUE(C2->isRemoving()); //Virally removed
+ ASSERT_TRUE(f2->isRemoving()); //Virally removed
+ ASSERT_TRUE(c23->isRemoving());
+ ASSERT_TRUE(C23->isRemoving()); //Virally removed
+ ASSERT_TRUE(f23->isRemoving()); //Virally removed
+ ASSERT_TRUE(F3->isFixed()); //Fixed
+}
+
+TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
+{
+ ParserYAML parser = ParserYAML("test/yaml/params_tree_manager_sliding_window2.yaml", wolf_root);
+ ParamsServer server = ParamsServer(parser.getParams());
+
+ ProblemPtr P = Problem::autoSetup(server);
+ P->applyPriorOptions(0);
+
+ // FRAME 1 (prior) ----------------------------------------------------------
+ auto F1 = P->getTrajectory()->getLastKeyFrame();
+ ASSERT_TRUE(F1 != nullptr);
+
+ Vector7d state = F1->getStateVector();
+ Vector7d zero_disp(state);
+ Matrix6d cov = Matrix6d::Identity();
+
+ // FRAME 2 ----------------------------------------------------------
+ auto F2 = P->emplaceFrame(KEY, TimeStamp(2), "PO", 3, state);
+ P->keyFrameCallback(F2, nullptr, 0);
+
+ // absolute factor
+ auto C2 = CaptureBase::emplace<CaptureVoid>(F2, TimeStamp(2), nullptr);
+ auto f2 = FeatureBase::emplace<FeatureBase>(C2, "absolute", state, cov);
+ auto c2 = FactorBase::emplace<FactorPose3d>(f2, f2, nullptr, false);
+ // displacement
+ auto C12 = CaptureBase::emplace<CaptureVoid>(F2, TimeStamp(2), nullptr);
+ auto f12 = FeatureBase::emplace<FeatureBase>(C12, "odom", zero_disp, cov);
+ auto c12 = FactorBase::emplace<FactorOdom3d>(f12, f12, F1, nullptr, false);
+
+ // Check no frame removed
+ ASSERT_FALSE(F1->isRemoving());
+
+ // FRAME 3 ----------------------------------------------------------
+ auto F3 = P->emplaceFrame(KEY, TimeStamp(3), "PO", 3, state);
+ P->keyFrameCallback(F3, nullptr, 0);
+
+ // absolute factor
+ auto C3 = CaptureBase::emplace<CaptureVoid>(F3, TimeStamp(3), nullptr);
+ auto f3 = FeatureBase::emplace<FeatureBase>(C3, "absolute", state, cov);
+ auto c3 = FactorBase::emplace<FactorPose3d>(f3, f3, nullptr, false);
+ // displacement
+ auto C23 = CaptureBase::emplace<CaptureVoid>(F3, TimeStamp(3), nullptr);
+ auto f23 = FeatureBase::emplace<FeatureBase>(C23, "odom", zero_disp, cov);
+ auto c23 = FactorBase::emplace<FactorOdom3d>(f23, f23, F2, nullptr, false);
+
+ // Check no frame removed
+ ASSERT_FALSE(F1->isRemoving());
+
+ // FRAME 4 ----------------------------------------------------------
+ auto F4 = P->emplaceFrame(KEY, TimeStamp(4), "PO", 3, state);
+ P->keyFrameCallback(F4, nullptr, 0);
+
+ // absolute factor
+ auto C4 = CaptureBase::emplace<CaptureVoid>(F4, TimeStamp(4), nullptr);
+ auto f4 = FeatureBase::emplace<FeatureBase>(C4, "absolute", state, cov);
+ auto c4 = FactorBase::emplace<FactorPose3d>(f4, f4, nullptr, false);
+ // displacement
+ auto C34 = CaptureBase::emplace<CaptureVoid>(F4, TimeStamp(4), nullptr);
+ auto f34 = FeatureBase::emplace<FeatureBase>(C34, "odom", zero_disp, cov);
+ auto c34 = FactorBase::emplace<FactorOdom3d>(f34, f34, F3, nullptr, false);
+
+ // Checks
+ ASSERT_TRUE(F1->isRemoving());
+ ASSERT_TRUE(c12->isRemoving());
+ ASSERT_FALSE(C12->isRemoving()); //Not virally removed
+ ASSERT_FALSE(f12->isRemoving()); //Not virally removed
+ ASSERT_FALSE(F2->isFixed()); //Not fixed
+
+ // FRAME 5 ----------------------------------------------------------
+ auto F5 = P->emplaceFrame(KEY, TimeStamp(5), "PO", 3, state);
+ P->keyFrameCallback(F5, nullptr, 0);
+
+ // absolute factor
+ auto C5 = CaptureBase::emplace<CaptureVoid>(F5, TimeStamp(5), nullptr);
+ auto f5 = FeatureBase::emplace<FeatureBase>(C5, "absolute", state, cov);
+ auto c5 = FactorBase::emplace<FactorPose3d>(f5, f5, nullptr, false);
+ // displacement
+ auto C45 = CaptureBase::emplace<CaptureVoid>(F5, TimeStamp(5), nullptr);
+ auto f45 = FeatureBase::emplace<FeatureBase>(C45, "odom", zero_disp, cov);
+ auto c45 = FactorBase::emplace<FactorOdom3d>(f45, f45, F4, nullptr, false);
+
+ // Checks
+ ASSERT_TRUE(F1->isRemoving());
+ ASSERT_TRUE(c12->isRemoving());
+ ASSERT_TRUE(C12->isRemoving());
+ ASSERT_TRUE(f12->isRemoving());
+ ASSERT_TRUE(F2->isRemoving());
+ ASSERT_TRUE(c2->isRemoving());
+ ASSERT_TRUE(C2->isRemoving());
+ ASSERT_TRUE(f2->isRemoving());
+ ASSERT_TRUE(c23->isRemoving());
+ ASSERT_FALSE(C23->isRemoving()); //Not virally removed
+ ASSERT_FALSE(f23->isRemoving()); //Not virally removed
+ ASSERT_FALSE(F3->isFixed()); //Not fixed
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/yaml/params_tree_manager1.yaml b/test/yaml/params_tree_manager1.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..d8f443220bf71786c1e95ca77ae9e68705d1d576
--- /dev/null
+++ b/test/yaml/params_tree_manager1.yaml
@@ -0,0 +1,49 @@
+config:
+ problem:
+ frame_structure: "POV"
+ dimension: 3
+ prior:
+ mode: "factor"
+ # state: [0,0,0,0,0,0,1,0,0,0]
+ # cov: [[9,9],.1,0,0,0,0,0,0,0,0, 0,.1,0,0,0,0,0,0,0, 0,0,.1,0,0,0,0,0,0, 0,0,0,.1,0,0,0,0,0, 0,0,0,0,.1,0,0,0,0, 0,0,0,0,0,.1,0,0,0, 0,0,0,0,0,0,.1,0,0, 0,0,0,0,0,0,0,.1,0, 0,0,0,0,0,0,0,0,.1]
+ $state:
+ P: [0,0,0]
+ O: [0,0,0,1]
+ V: [0,0,0]
+ $sigma:
+ P: [0.31, 0.31, 0.31]
+ O: [0.31, 0.31, 0.31]
+ V: [0.31, 0.31, 0.31]
+ time_tolerance: 0.1
+ tree_manager:
+ type: "TreeManagerDummy"
+ toy_param: 0
+ sensors:
+ -
+ type: "SensorOdom3d"
+ name: "odom"
+ plugin: "core"
+ k_disp_to_disp: 0.1
+ k_disp_to_rot: 0.1
+ k_rot_to_rot: 0.1
+ min_disp_var: 0.1
+ min_rot_var: 0.1
+ extrinsic:
+ pose: [1,2,3,0,0,0,1]
+ processors:
+ -
+ type: "ProcessorOdom3d"
+ name: "my_proc_odom3d"
+ sensor_name: "odom"
+ plugin: "core"
+ apply_loss_function: false
+ time_tolerance: 0.01 # seconds
+ keyframe_vote:
+ voting_active: false
+ voting_aux_active: false
+ max_time_span: 0.2 # seconds
+ max_buff_length: 10 # motion deltas
+ dist_traveled: 0.5 # meters
+ angle_turned: 0.1 # radians (1 rad approx 57 deg, approx 60 deg)
+
+ unmeasured_perturbation_std: 0.00111
diff --git a/test/yaml/params_tree_manager2.yaml b/test/yaml/params_tree_manager2.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..b61f9936d4ccb8fc705d2ca92717f2a2ad67c402
--- /dev/null
+++ b/test/yaml/params_tree_manager2.yaml
@@ -0,0 +1,48 @@
+config:
+ problem:
+ frame_structure: "POV"
+ dimension: 3
+ prior:
+ mode: "factor"
+ # state: [0,0,0,0,0,0,1,0,0,0]
+ # cov: [[9,9],.1,0,0,0,0,0,0,0,0, 0,.1,0,0,0,0,0,0,0, 0,0,.1,0,0,0,0,0,0, 0,0,0,.1,0,0,0,0,0, 0,0,0,0,.1,0,0,0,0, 0,0,0,0,0,.1,0,0,0, 0,0,0,0,0,0,.1,0,0, 0,0,0,0,0,0,0,.1,0, 0,0,0,0,0,0,0,0,.1]
+ $state:
+ P: [0,0,0]
+ O: [0,0,0,1]
+ V: [0,0,0]
+ $sigma:
+ P: [0.31, 0.31, 0.31]
+ O: [0.31, 0.31, 0.31]
+ V: [0.31, 0.31, 0.31]
+ time_tolerance: 0.1
+ tree_manager:
+ type: "None"
+ sensors:
+ -
+ type: "SensorOdom3d"
+ name: "odom"
+ plugin: "core"
+ k_disp_to_disp: 0.1
+ k_disp_to_rot: 0.1
+ k_rot_to_rot: 0.1
+ min_disp_var: 0.1
+ min_rot_var: 0.1
+ extrinsic:
+ pose: [1,2,3,0,0,0,1]
+ processors:
+ -
+ type: "ProcessorOdom3d"
+ name: "my_proc_odom3d"
+ sensor_name: "odom"
+ plugin: "core"
+ apply_loss_function: false
+ time_tolerance: 0.01 # seconds
+ keyframe_vote:
+ voting_active: false
+ voting_aux_active: false
+ max_time_span: 0.2 # seconds
+ max_buff_length: 10 # motion deltas
+ dist_traveled: 0.5 # meters
+ angle_turned: 0.1 # radians (1 rad approx 57 deg, approx 60 deg)
+
+ unmeasured_perturbation_std: 0.00111
diff --git a/test/yaml/params_tree_manager_sliding_window1.yaml b/test/yaml/params_tree_manager_sliding_window1.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..96ad94f72af7e8de1b9200a8e5ea44e3bc952a85
--- /dev/null
+++ b/test/yaml/params_tree_manager_sliding_window1.yaml
@@ -0,0 +1,49 @@
+config:
+ problem:
+ frame_structure: "PO"
+ dimension: 3
+ prior:
+ mode: "factor"
+ # state: [0,0,0,0,0,0,1]
+ # cov: [[6,6],.1,0,0,0,0,0, 0,.1,0,0,0,0, 0,0,.1,0,0,0, 0,0,0,.1,0,0, 0,0,0,0,.1,0, 0,0,0,0,0,.1]
+ $state:
+ P: [0,0,0]
+ O: [0,0,0,1]
+ $sigma:
+ P: [0.31, 0.31, 0.31]
+ O: [0.31, 0.31, 0.31]
+ time_tolerance: 0.1
+ tree_manager:
+ type: "TreeManagerSlidingWindow"
+ n_key_frames: 3
+ fix_first_key_frame: true
+ viral_remove_empty_parent: true
+ sensors:
+ -
+ type: "SensorOdom3d"
+ name: "odom"
+ plugin: "core"
+ k_disp_to_disp: 0.1
+ k_disp_to_rot: 0.1
+ k_rot_to_rot: 0.1
+ min_disp_var: 0.1
+ min_rot_var: 0.1
+ extrinsic:
+ pose: [1,2,3,0,0,0,1]
+ processors:
+ -
+ type: "ProcessorOdom3d"
+ name: "my_proc_odom3d"
+ sensor_name: "odom"
+ plugin: "core"
+ apply_loss_function: false
+ time_tolerance: 0.01 # seconds
+ keyframe_vote:
+ voting_active: false
+ voting_aux_active: false
+ max_time_span: 0.2 # seconds
+ max_buff_length: 10 # motion deltas
+ dist_traveled: 0.5 # meters
+ angle_turned: 0.1 # radians (1 rad approx 57 deg, approx 60 deg)
+
+ unmeasured_perturbation_std: 0.00111
diff --git a/test/yaml/params_tree_manager_sliding_window2.yaml b/test/yaml/params_tree_manager_sliding_window2.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..9ae33a2af5ab0695bd44948d33d2f9aa104cbc74
--- /dev/null
+++ b/test/yaml/params_tree_manager_sliding_window2.yaml
@@ -0,0 +1,49 @@
+config:
+ problem:
+ frame_structure: "PO"
+ dimension: 3
+ prior:
+ mode: "factor"
+ # state: [0,0,0,0,0,0,1]
+ # cov: [[6,6],.1,0,0,0,0,0, 0,.1,0,0,0,0, 0,0,.1,0,0,0, 0,0,0,.1,0,0, 0,0,0,0,.1,0, 0,0,0,0,0,.1]
+ $state:
+ P: [0,0,0]
+ O: [0,0,0,1]
+ $sigma:
+ P: [0.31, 0.31, 0.31]
+ O: [0.31, 0.31, 0.31]
+ time_tolerance: 0.1
+ tree_manager:
+ type: "TreeManagerSlidingWindow"
+ n_key_frames: 3
+ fix_first_key_frame: false
+ viral_remove_empty_parent: false
+ sensors:
+ -
+ type: "SensorOdom3d"
+ name: "odom"
+ plugin: "core"
+ k_disp_to_disp: 0.1
+ k_disp_to_rot: 0.1
+ k_rot_to_rot: 0.1
+ min_disp_var: 0.1
+ min_rot_var: 0.1
+ extrinsic:
+ pose: [1,2,3,0,0,0,1]
+ processors:
+ -
+ type: "ProcessorOdom3d"
+ name: "my_proc_odom3d"
+ sensor_name: "odom"
+ plugin: "core"
+ apply_loss_function: false
+ time_tolerance: 0.01 # seconds
+ keyframe_vote:
+ voting_active: false
+ voting_aux_active: false
+ max_time_span: 0.2 # seconds
+ max_buff_length: 10 # motion deltas
+ dist_traveled: 0.5 # meters
+ angle_turned: 0.1 # radians (1 rad approx 57 deg, approx 60 deg)
+
+ unmeasured_perturbation_std: 0.00111
diff --git a/test/yaml/processor_odom_3d.yaml b/test/yaml/processor_odom_3d.yaml
index 65b00b967a1b3c1ed870f979479db930caf78b5e..672421b5d074b7fd627bf955a879e3f10a3544e0 100644
--- a/test/yaml/processor_odom_3d.yaml
+++ b/test/yaml/processor_odom_3d.yaml
@@ -1,4 +1,4 @@
-type: "ProcessorOdom3d" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+type: "ProcessorOdom3d" # This must match the KEY used in the FactorySensor. Otherwise it is an error.
time_tolerance: 0.01 # seconds
diff --git a/test/yaml/sensor_odom_2d.yaml b/test/yaml/sensor_odom_2d.yaml
index 0841d40b310d90064de6131e32ccaee189ab104c..2d26af901956a324988ac86a67a78dacb5ae8a67 100644
--- a/test/yaml/sensor_odom_2d.yaml
+++ b/test/yaml/sensor_odom_2d.yaml
@@ -1,4 +1,4 @@
-type: "SensorOdom2d" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+type: "SensorOdom2d" # This must match the KEY used in the FactorySensor. Otherwise it is an error.
k_disp_to_disp: 0.02 # m^2 / m
k_rot_to_rot: 0.01 # rad^2 / rad
diff --git a/test/yaml/sensor_odom_3d.yaml b/test/yaml/sensor_odom_3d.yaml
index 58db1c088fbc80339a78ba815fddbaf69674d3b6..8eb2b235011cb3cfe4f35b1f73da6344991af0da 100644
--- a/test/yaml/sensor_odom_3d.yaml
+++ b/test/yaml/sensor_odom_3d.yaml
@@ -1,4 +1,4 @@
-type: "SensorOdom3d" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+type: "SensorOdom3d" # This must match the KEY used in the FactorySensor. Otherwise it is an error.
k_disp_to_disp: 0.02 # m^2 / m
k_disp_to_rot: 0.02 # rad^2 / m
diff --git a/wolf_scripts/sed_rename.sh b/wolf_scripts/sed_rename.sh
new file mode 100755
index 0000000000000000000000000000000000000000..53d20f2bba1f57a7c4569d1dd27b8d5ec09974ea
--- /dev/null
+++ b/wolf_scripts/sed_rename.sh
@@ -0,0 +1,28 @@
+#!/bin/bash
+#$1 flag telling whether we are in test mode or not
+
+modify=$1
+apply=false
+
+if [[ "$modify" = true || "$modify" = t ]]; then
+ read -p "Are you sure? " -n 1 -r
+ echo # (optional) move to a new line
+ if [[ $REPLY =~ ^[Yy]$ ]]
+ then
+ apply=true
+ echo "Modifying..."
+ fi
+fi
+
+string="StringToBeReplaced"
+replace="ReplacementString"
+
+for file in $(find include/ src/ test/ demos/ -type f); do
+ if [ "$apply" = true ]; then
+ # echo "APPLYING"
+ sed -Ei 's%'$string'%'$replace'%g' $file
+ else
+ # echo "NOT APPLYING"
+ sed -En 's%'$string'%'$replace'%gp' $file
+ fi
+done