diff --git a/CMakeLists.txt b/CMakeLists.txt
index cab800a231b74a05eb9c466e0ec5d508ca561918..3cfdcb44eff879f3ea3934c3a79fc9ec6c4f4fd5 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -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
@@ -371,6 +376,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 +408,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,6 +437,7 @@ ADD_LIBRARY(${PROJECT_NAME}
${SRCS_SOLVER}
${SRCS_STATE_BLOCK}
${SRCS_TRAJECTORY}
+ ${SRCS_TREE_MANAGER}
${SRCS_UTILS}
${SRCS_WRAPPER}
${SRCS_YAML}
@@ -471,44 +478,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 100%
rename from hello_wolf/factor_bearing.h
rename to demos/hello_wolf/factor_bearing.h
diff --git a/hello_wolf/factor_range_bearing.h b/demos/hello_wolf/factor_range_bearing.h
similarity index 100%
rename from hello_wolf/factor_range_bearing.h
rename to demos/hello_wolf/factor_range_bearing.h
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 98%
rename from hello_wolf/hello_wolf.cpp
rename to demos/hello_wolf/hello_wolf.cpp
index a705a480cef464c49c0ec6b2f463ecf527c825ec..8cb2ea566e8682395cd20dbc278ac93ad41c2060 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,7 +132,7 @@ 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);
@@ -141,7 +141,7 @@ int main()
TimeStamp t(0.0); // t : 0.0
Vector3d x(0,0,0);
Matrix3d P = Matrix3d::Identity() * 0.1;
- problem->setPriorFactor(x, P, t, 0.5); // KF1 : (0,0,0)
+ problem->setPrior(x, P, t, 0.5); // KF1 : (0,0,0)
// SELF CALIBRATION ===================================================
diff --git a/hello_wolf/hello_wolf_autoconf.cpp b/demos/hello_wolf/hello_wolf_autoconf.cpp
similarity index 99%
rename from hello_wolf/hello_wolf_autoconf.cpp
rename to demos/hello_wolf/hello_wolf_autoconf.cpp
index a387adfe9656523c37d025bb3a74f0f198281b3d..5ab1ad1dd20a6b03411ea5cc458bde99b2b1fca8 100644
--- a/hello_wolf/hello_wolf_autoconf.cpp
+++ b/demos/hello_wolf/hello_wolf_autoconf.cpp
@@ -98,7 +98,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:
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 95%
rename from hello_wolf/processor_range_bearing.cpp
rename to demos/hello_wolf/processor_range_bearing.cpp
index 89b3d7775e3c5b79dca8de4a0d2f6f5ad91d17e7..9510807d443da2458299fe3522fa8c5f49cf0bf5 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)
{
//
@@ -160,10 +160,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 69%
rename from hello_wolf/yaml/hello_wolf_config.yaml
rename to demos/hello_wolf/yaml/hello_wolf_config.yaml
index b84d7776622264f96a4597ade146d64bcec86438..1077fe00cb442147b03a36ffddba5b3b37016e8d 100644
--- a/hello_wolf/yaml/hello_wolf_config.yaml
+++ b/demos/hello_wolf/yaml/hello_wolf_config.yaml
@@ -2,6 +2,11 @@ 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:
@@ -17,7 +22,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 +30,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 +38,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/common/factory.h b/include/core/common/factory.h
index f03cabec9a7527801464d9e4e025464a7d23f0ad..34c214bd674157eefe04433b3406918ae8e0cba0 100644
--- a/include/core/common/factory.h
+++ b/include/core/common/factory.h
@@ -28,8 +28,8 @@ namespace wolf
* This class implements a generic factory as a singleton.
*
* > IMPORTANT: This template factory can be used to construct many different objects except:
- * > - Objects deriving from SensorBase --> see SensorFactory
- * > - Objects deriving from ProcessorBase --> see ProcessorFactory
+ * > - Objects deriving from SensorBase --> see FactorySensor
+ * > - Objects deriving from ProcessorBase --> see FactoryProcessor
* >
* > The reason for this is that the two cases above need a more elaborated API than the one in this template class.
*
@@ -86,8 +86,8 @@ 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<ParamsSensorBase, std::string> FactoryParamsSensor;
+ * typedef Factory<ParamsProcessorBase, std::string> FactoryParamsProcessor;
* typedef Factory<LandmarkBase, YAML::Node> LandmarkFactory;
* \endcode
*
@@ -145,7 +145,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).
@@ -218,15 +218,15 @@ namespace wolf
* \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.
+ * - 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.
* - LandmarkFactory: 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.
*
diff --git a/include/core/common/node_base.h b/include/core/common/node_base.h
index 4488ab30ec274eeb84e49b7dcd600de9a93acf5e..2a6120408718df85ebb2945c9800d71b0060b6fd 100644
--- a/include/core/common/node_base.h
+++ b/include/core/common/node_base.h
@@ -27,8 +27,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..ca7da8dbe679b5bf5f0b3ca8518a6cd045e0aafa 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);
diff --git a/include/core/factor/factor_autodiff.h b/include/core/factor/factor_autodiff.h
index d5e86d2f168acce03feb9085420de1c6ad901ab8..14e3660fa43bfb8c7892a4f225b5edf38592bb4d 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,12 +31,15 @@ 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:
@@ -54,6 +57,8 @@ class FactorAutodiff : public FactorBase
std::array<WolfJet, B7>* jets_7_;
std::array<WolfJet, B8>* jets_8_;
std::array<WolfJet, B9>* jets_9_;
+ std::array<WolfJet, B10>* jets_10_;
+ std::array<WolfJet, B11>* jets_11_;
public:
/** \brief Constructor valid for all categories (ABSOLUTE, FRAME, FEATURE, LANDMARK)
@@ -75,9 +80,11 @@ class FactorAutodiff : public FactorBase
StateBlockPtr _state6Ptr,
StateBlockPtr _state7Ptr,
StateBlockPtr _state8Ptr,
- StateBlockPtr _state9Ptr) :
+ StateBlockPtr _state9Ptr,
+ StateBlockPtr _state10Ptr,
+ StateBlockPtr _state11Ptr) :
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}),
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr,_state10Ptr,_state11Ptr}),
residuals_jets_(new std::array<WolfJet, RES>),
jets_0_(new std::array<WolfJet, B0>),
jets_1_(new std::array<WolfJet, B1>),
@@ -88,8 +95,24 @@ class FactorAutodiff : public FactorBase
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>)
+ jets_9_(new std::array<WolfJet, B9>),
+ jets_10_(new std::array<WolfJet, B10>),
+ jets_11_(new std::array<WolfJet, B11>)
{
+ // 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++)
@@ -111,7 +134,11 @@ class FactorAutodiff : public FactorBase
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++);
+ (*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()
@@ -126,6 +153,8 @@ class FactorAutodiff : public FactorBase
delete jets_7_;
delete jets_8_;
delete jets_9_;
+ delete jets_10_;
+ delete jets_11_;
delete residuals_jets_;
}
@@ -154,6 +183,8 @@ class FactorAutodiff : public FactorBase
parameters[7],
parameters[8],
parameters[9],
+ parameters[10],
+ parameters[11],
residuals);
}
// also compute jacobians
@@ -175,6 +206,8 @@ class FactorAutodiff : public FactorBase
jets_7_->data(),
jets_8_->data(),
jets_9_->data(),
+ jets_10_->data(),
+ jets_11_->data(),
residuals_jets_->data());
// fill the residual array
@@ -218,6 +251,10 @@ class FactorAutodiff : public FactorBase
(*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];
+ 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
@@ -244,6 +281,8 @@ class FactorAutodiff : public FactorBase
jets_7_->data(),
jets_8_->data(),
jets_9_->data(),
+ jets_10_->data(),
+ jets_11_->data(),
residuals_jets_->data());
// fill the residual vector
@@ -299,10 +338,577 @@ 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_;
+ 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_;
+ std::array<WolfJet, B10>* jets_10_;
+
+ public:
+
+ FactorAutodiff(const std::string& _tp,
+ 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, _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}),
+ 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>),
+ jets_10_(new std::array<WolfJet, B10>)
+ {
+ // 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()
+ {
+ 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 jets_10_;
+ delete residuals_jets_;
+ }
+
+ 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_;
+ 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_;
+
+ public:
+
+ FactorAutodiff(const std::string& _tp,
+ 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, _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>)
+ {
+ // 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()
+ {
+ 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
+ {
+ 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 +923,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_;
@@ -373,6 +981,17 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0> : public FactorBase
jets_7_(new std::array<WolfJet, B7>),
jets_8_(new std::array<WolfJet, B8>)
{
+ // 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++)
@@ -554,7 +1173,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 +1188,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_;
@@ -622,6 +1243,16 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0> : public FactorBase
jets_6_(new std::array<WolfJet, B6>),
jets_7_(new std::array<WolfJet, B7>)
{
+ // 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++)
@@ -795,7 +1426,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 +1441,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_;
@@ -860,6 +1493,15 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0> : public FactorBase
jets_5_(new std::array<WolfJet, B5>),
jets_6_(new std::array<WolfJet, B6>)
{
+ // 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++)
@@ -1025,7 +1667,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 +1682,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_;
@@ -1087,6 +1731,14 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0> : public FactorBase
jets_4_(new std::array<WolfJet, B4>),
jets_5_(new std::array<WolfJet, B5>)
{
+ // 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++)
@@ -1240,7 +1892,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 +1907,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_;
@@ -1298,6 +1952,13 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0> : public FactorBase
jets_3_(new std::array<WolfJet, B3>),
jets_4_(new std::array<WolfJet, B4>)
{
+ // 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++)
@@ -1443,7 +2104,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 +2119,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_;
@@ -1498,6 +2161,12 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0> : public FactorBase
jets_2_(new std::array<WolfJet, B2>),
jets_3_(new std::array<WolfJet, B3>)
{
+ // 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++)
@@ -1639,7 +2308,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 +2323,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_;
@@ -1691,6 +2362,11 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0> : public FactorBase
jets_1_(new std::array<WolfJet, B1>),
jets_2_(new std::array<WolfJet, B2>)
{
+ // 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++)
@@ -1824,7 +2500,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 +2515,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_;
@@ -1873,6 +2551,10 @@ class FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0> : public FactorBase
jets_0_(new std::array<WolfJet, B0>),
jets_1_(new std::array<WolfJet, B1>)
{
+ // 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++)
@@ -1998,7 +2680,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 +2695,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_;
@@ -2044,6 +2728,9 @@ class FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0> : public FactorBase
residuals_jets_(new std::array<WolfJet, RES>),
jets_0_(new std::array<WolfJet, B0>)
{
+ // 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++)
@@ -2162,113 +2849,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..1ef90ba493d3d3922685f7f42ff2b14402d7a546 100644
--- a/include/core/factor/factor_autodiff_distance_3d.h
+++ b/include/core/factor/factor_autodiff_distance_3d.h
@@ -23,7 +23,7 @@ class FactorAutodiffDistance3d : public FactorAutodiff<FactorAutodiffDistance3d,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status) :
- FactorAutodiff("DISTANCE 3d",
+ FactorAutodiff("FactorAutodiffDistance3d",
_frm_other,
nullptr,
nullptr,
diff --git a/include/core/factor/factor_diff_drive.h b/include/core/factor/factor_diff_drive.h
index d936a99d6c1d64f29fda2c5aeca1806e0065c4d1..7b189e4424b419b655833020ff9a94c71952748e 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
{
@@ -126,10 +127,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..1ce2a375c131cea5234a5e603cdf2808764bc4cc 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"
@@ -72,10 +73,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..85a1785440a03a15a899117b619d3d07aaa3eb39 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"
@@ -71,10 +72,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_pose_2d.h b/include/core/factor/factor_pose_2d.h
index ce63c806017117e67d18c2e79c34468e6b16a61c..f11b674e7775c78c0f0f306be69c85c832e328de 100644
--- a/include/core/factor/factor_pose_2d.h
+++ b/include/core/factor/factor_pose_2d.h
@@ -54,11 +54,7 @@ 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);
diff --git a/include/core/factor/factor_quaternion_absolute.h b/include/core/factor/factor_quaternion_absolute.h
index 8b399f668a8a4763c35186a309b1cfbf76f2a86b..aca1f7f966aa33e3f970e630b984b7fe145781ed 100644
--- a/include/core/factor/factor_quaternion_absolute.h
+++ b/include/core/factor/factor_quaternion_absolute.h
@@ -27,7 +27,7 @@ class FactorQuaternionAbsolute: public FactorAutodiff<FactorQuaternionAbsolute,3
ProcessorBasePtr _processor_ptr,
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE) :
- FactorAutodiff<FactorQuaternionAbsolute,3,4>("QUATERNION ABS",
+ FactorAutodiff<FactorQuaternionAbsolute,3,4>("FactorQuaternionAbsolute",
nullptr,
nullptr,
nullptr,
diff --git a/include/core/factor/factor_relative_2d_analytic.h b/include/core/factor/factor_relative_2d_analytic.h
index 5a67da1ff6c0e17614902ebc3195dd8124ec0ed6..2dba90bc6d4ead00634f601bf232d4471860c418 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 {
@@ -19,11 +20,11 @@ class FactorRelative2dAnalytic : public FactorAnalytic
**/
FactorRelative2dAnalytic(const std::string& _tp,
const FeatureBasePtr& _ftr_ptr,
- const FrameBasePtr& _frame_ptr,
+ const FrameBasePtr& _frame_other_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())
+ FactorAnalytic(_tp, _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())
{
//
}
@@ -45,11 +46,11 @@ class FactorRelative2dAnalytic : public FactorAnalytic
**/
FactorRelative2dAnalytic(const std::string& _tp,
const FeatureBasePtr& _ftr_ptr,
- const LandmarkBasePtr& _landmark_ptr,
+ const LandmarkBasePtr& _landmark_other_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())
+ FactorAnalytic(_tp, 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 +114,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..be13135c9c7e3e0899ac2bb9043f4dd1b7b60ef2
--- /dev/null
+++ b/include/core/factor/factor_relative_pose_2d_with_extrinsics.h
@@ -0,0 +1,119 @@
+#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",
+ _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/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 8e5d7736eb0bd79c2e77948ae1e73d8a193f4d7f..7dc2caca9fdbe5fc8527cab6814a03665b0ac89e 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,8 +19,8 @@ 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"
// std includes
@@ -52,6 +52,7 @@ class Problem : public std::enable_shared_from_this<Problem>
friend ProcessorMotion;
protected:
+ TreeManagerBasePtr tree_manager_;
HardwareBasePtr hardware_ptr_;
TrajectoryBasePtr trajectory_ptr_;
MapBasePtr map_ptr_;
@@ -82,6 +83,10 @@ class Problem : public std::enable_shared_from_this<Problem>
SizeEigen getDim() const;
std::string getFrameStructure() const;
+ // Tree manager --------------------------------------
+ void setTreeManager(TreeManagerBasePtr _gm);
+ TreeManagerBasePtr getTreeManager() const;
+
// Hardware branch ------------------------------------
HardwareBasePtr getHardware() const;
@@ -100,7 +105,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, //
@@ -128,7 +133,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
*
@@ -382,6 +387,11 @@ class Problem : public std::enable_shared_from_this<Problem>
namespace wolf
{
+inline TreeManagerBasePtr Problem::getTreeManager() const
+{
+ return tree_manager_;
+}
+
inline bool Problem::isPriorSet() const
{
return prior_options_ == nullptr;
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..738d3558a41124377dffa39731ed1c859d09f873 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()
{
- 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()
{
- 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()
{
- 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/processor_base.h b/include/core/processor/processor_base.h
index dc62abcac8375fc238f7c93617a33ccb8662d246..0d0e54eef5217c037dabbf00eb4f3b87f100e283 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();
diff --git a/include/core/processor/processor_diff_drive.h b/include/core/processor/processor_diff_drive.h
index 48d18acccc0e48f3f5f2373b0ece28e2da607f61..bcbf3f6034250a3ffd0ac3e00b73501643d1b69b 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
@@ -63,7 +63,7 @@ class ProcessorDiffDrive : public ProcessorOdom2d
protected:
- ProcessorParamsDiffDrivePtr params_prc_diff_drv_selfcal_;
+ ParamsProcessorDiffDrivePtr params_prc_diff_drv_selfcal_;
double radians_per_tick_;
};
diff --git a/include/core/processor/processor_loopclosure.h b/include/core/processor/processor_loopclosure.h
index b0720f3a47afdfd7878b23929923295964340ea5..cbd8fa378738ad55d1072d63b7a6162a169077fa 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,12 @@ class ProcessorLoopClosure : public ProcessorBase
{
protected:
- ProcessorParamsLoopClosurePtr params_loop_closure_;
+ ParamsProcessorLoopClosurePtr params_loop_closure_;
int _dim;
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 402c590326567b5f4c0c564404c09b331f8ba674..69e346ac755a4db3086b878fcc0ee61cf8979811 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"
@@ -145,7 +145,7 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
} ProcessingStep ;
protected:
- ProcessorParamsMotionPtr params_motion_;
+ ParamsProcessorMotionPtr params_motion_;
ProcessingStep processing_step_; ///< State machine controlling the processing step
virtual void setProblem(ProblemPtr) override;
@@ -159,7 +159,7 @@ 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:
diff --git a/include/core/processor/processor_odom_2d.h b/include/core/processor/processor_odom_2d.h
index b03416837ec1b0a6a86a67ff0cf40fdf820b1370..a626833c99d6b5e6987fab4259f68bf466bd6570 100644
--- a/include/core/processor/processor_odom_2d.h
+++ b/include/core/processor/processor_odom_2d.h
@@ -17,22 +17,22 @@
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 +40,12 @@ struct ProcessorParamsOdom2d : public ProcessorParamsMotion
class ProcessorOdom2d : public ProcessorMotion
{
public:
- ProcessorOdom2d(ProcessorParamsOdom2dPtr _params);
+ ProcessorOdom2d(ParamsProcessorOdom2dPtr _params);
virtual ~ProcessorOdom2d();
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;
@@ -86,7 +86,7 @@ class ProcessorOdom2d : public ProcessorMotion
CaptureBasePtr _capture_origin) override;
protected:
- ProcessorParamsOdom2dPtr params_odom_2d_;
+ ParamsProcessorOdom2dPtr params_odom_2d_;
};
diff --git a/include/core/processor/processor_odom_3d.h b/include/core/processor/processor_odom_3d.h
index f8f849b896aad644f998c38b50e8ca198c0738c4..ca0a9619d06dbd285bb9bf8da88943286d87e0bb 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
@@ -107,7 +107,7 @@ class ProcessorOdom3d : public ProcessorMotion
CaptureBasePtr _capture_origin) 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..efe8ef33837df852499491c5f524bcfed0765129 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,20 +96,21 @@ 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;
public:
ProcessorTracker(const std::string& _type,
int _dim,
- ProcessorParamsTrackerPtr _params_tracker);
+ ParamsProcessorTrackerPtr _params_tracker);
virtual ~ProcessorTracker();
bool checkTimeTolerance(const TimeStamp& _ts1, const TimeStamp& _ts2);
diff --git a/include/core/processor/processor_tracker_feature.h b/include/core/processor/processor_tracker_feature.h
index b86e041d25e026762ea1413429b1c6e63a594fd7..4cb706c5c3de26da86f3653ef6234251bafb3f7c 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);
@@ -86,11 +86,11 @@ class ProcessorTrackerFeature : public ProcessorTracker
*/
ProcessorTrackerFeature(const std::string& _type,
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 +111,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 +147,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 +190,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..b4e7f7a41ae8bbe68d53da08288b85cda87edeab 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,12 @@ class ProcessorTrackerLandmark : public ProcessorTracker
{
public:
ProcessorTrackerLandmark(const std::string& _type,
- ProcessorParamsTrackerLandmarkPtr _params_tracker_landmark);
+ 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 79%
rename from include/core/sensor/sensor_factory.h
rename to include/core/sensor/factory_sensor.h
index daf01499ac759d799517cdd73ab830fb6ccaf4d5..67ef5f597108f7ddc3bc264fb1c0cbc218bdba0a 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,42 @@ 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()
{
- 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()
{
- 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()
{
- 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/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..b055217828d887169f8e732138b43a4f2067c62a 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()
{
- 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/tree_manager/factory_tree_manager.h b/include/core/tree_manager/factory_tree_manager.h
new file mode 100644
index 0000000000000000000000000000000000000000..91f2b9531d91bdad952760a0670ceece4240e86a
--- /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()
+{
+ return "FactoryParamsTreeManager";
+}
+
+// TreeManager factory
+typedef Factory<TreeManagerBase,
+ const std::string&,
+ const ParamsTreeManagerBasePtr> FactoryTreeManager;
+template<>
+inline std::string FactoryTreeManager::getClass()
+{
+ 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()
+{
+ 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/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp
index 0d8cba6f97cb7e2c2cdf067e7ebcb65d8ac79c10..0d0c6537fedfc21dec63760d877fb56386885adf 100644
--- a/src/ceres_wrapper/ceres_manager.cpp
+++ b/src/ceres_wrapper/ceres_manager.cpp
@@ -515,8 +515,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/factor/factor_base.cpp b/src/factor/factor_base.cpp
index 72b36c519a0d8d0ce2cb7ac4748e0dfc110026d0..2b6fb54a6710fed79e8c608dafab1f7caaaab3af 100644
--- a/src/factor/factor_base.cpp
+++ b/src/factor/factor_base.cpp
@@ -79,7 +79,7 @@ void FactorBase::remove(bool viral_remove_empty_parent)
f->remove(viral_remove_empty_parent); // remove upstream
}
// add factor to be removed from solver
- if (getProblem() != nullptr)
+ if (getProblem() != nullptr and this->getStatus() == FAC_ACTIVE)
getProblem()->notifyFactor(this_fac,REMOVE);
// remove other: {Frame, Capture, Feature, Landmark}
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index c0f4d310e1062c9a68ea94a0ee0b70f912cd9d15..217bfb8d4232a40af88670fac91a52b161377d32 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -12,7 +12,7 @@ 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"),
+ NodeBase("FRAME", "FrameBase"),
HasStateBlocks(""),
trajectory_ptr_(),
frame_id_(++frame_id_count_),
@@ -34,7 +34,7 @@ FrameBase::FrameBase(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _
}
FrameBase::FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _v_ptr) :
- NodeBase("FRAME", "Base"),
+ NodeBase("FRAME", "FrameBase"),
HasStateBlocks(""),
trajectory_ptr_(),
frame_id_(++frame_id_count_),
@@ -56,7 +56,7 @@ 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"),
+ NodeBase("FRAME", "FrameBase"),
HasStateBlocks(_frame_structure),
trajectory_ptr_(),
frame_id_(++frame_id_count_),
diff --git a/src/hardware/hardware_base.cpp b/src/hardware/hardware_base.cpp
index adda7984748ebfb2fa6e40517f0d1bedc331efd2..8995fc7830b1b540c055ee1bc6aa4ca2c01c33ed 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;
}
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 263f205c1724fef3569116ce31b07e63214b166e..847c01e36d9986827e8fd16a1a4ba8d32ebb9338 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -11,18 +11,19 @@
#include "core/factor/factor_base.h"
#include "core/factor/factor_block_absolute.h"
#include "core/factor/factor_quaternion_absolute.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/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"
-
// IRI libs includes
// C++ includes
@@ -34,10 +35,12 @@
#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)),
map_ptr_(std::make_shared<MapBase>()),
@@ -157,6 +160,12 @@ 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
std::string prior_mode = _server.getParam<std::string>("problem/prior/mode");
if (prior_mode == "nothing")
@@ -191,7 +200,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;
}
@@ -206,7 +215,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
@@ -218,7 +227,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;
}
@@ -226,7 +235,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)
{
@@ -235,7 +244,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();
@@ -261,7 +270,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);
}
}
@@ -275,7 +284,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();
@@ -474,12 +483,22 @@ std::string Problem::getFrameStructure() const
return frame_structure_;
}
+void Problem::setTreeManager(TreeManagerBasePtr _gm)
+{
+ 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);
}
-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);
@@ -532,6 +551,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
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index 8c3ba79053e98cc77e9c80c2cb6bebc64915cc91..364dc699740595f3f36a8af86e4d8ea290af585c 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),
diff --git a/src/processor/processor_diff_drive.cpp b/src/processor/processor_diff_drive.cpp
index d80f89ccee5cddff4aab89ec3737c5b518463e7e..1bd0ca0ef6cacb1c90d0a7cdf843593bb38ac5ab 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).
@@ -173,10 +173,10 @@ FactorBasePtr ProcessorDiffDrive::emplaceFactor(FeatureBasePtr _feature,
} /* 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..9d8589a239466f2ba7bbd30473186c7dbdba52f7 100644
--- a/src/processor/processor_loopclosure.cpp
+++ b/src/processor/processor_loopclosure.cpp
@@ -11,7 +11,7 @@
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 b9c5182f9bbe1abd2f00ed4340b603f8233931bc..6dfb753acd6d4e2693a6c99be5e14be8239bdd06 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,7 +21,7 @@ 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_KF),
diff --git a/src/processor/processor_odom_2d.cpp b/src/processor/processor_odom_2d.cpp
index 929c1ed7ceb825fdaa265a1af592fa8125832789..4a126d901eb291a9d5fc8537b5429b294bfdc025 100644
--- a/src/processor/processor_odom_2d.cpp
+++ b/src/processor/processor_odom_2d.cpp
@@ -5,7 +5,7 @@
namespace wolf
{
-ProcessorOdom2d::ProcessorOdom2d(ProcessorParamsOdom2dPtr _params) :
+ProcessorOdom2d::ProcessorOdom2d(ParamsProcessorOdom2dPtr _params) :
ProcessorMotion("ProcessorOdom2d", "PO", 2, 3, 3, 3, 2, 0, _params),
params_odom_2d_(_params)
{
@@ -170,9 +170,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..71c77e1e16f9cc3b5b5b76885b64cd67b891bb7b 100644
--- a/src/processor/processor_odom_3d.cpp
+++ b/src/processor/processor_odom_3d.cpp
@@ -2,7 +2,7 @@
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),
@@ -246,9 +246,9 @@ FactorBasePtr ProcessorOdom3d::emplaceFactor(FeatureBasePtr _feature_motion, Cap
}
-// Register in the SensorFactory
-#include "core/processor/processor_factory.h"
+// 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 59671c2308468052d364f3e12bff8aa4b828c625..9750daa633b6a12a95ddf8e4034a792b816104f3 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -16,7 +16,7 @@ namespace wolf
ProcessorTracker::ProcessorTracker(const std::string& _type,
int _dim,
- ProcessorParamsTrackerPtr _params_tracker) :
+ ParamsProcessorTrackerPtr _params_tracker) :
ProcessorBase(_type, _dim, _params_tracker),
params_tracker_(_params_tracker),
processing_step_(FIRST_TIME_WITHOUT_PACK),
@@ -170,7 +170,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
diff --git a/src/processor/processor_tracker_feature.cpp b/src/processor/processor_tracker_feature.cpp
index 211a1aba12b12ecfb10afea00041cf82e07493be..bfe2c6cd1ac780d058421d96c64e745b378c3b88 100644
--- a/src/processor/processor_tracker_feature.cpp
+++ b/src/processor/processor_tracker_feature.cpp
@@ -12,7 +12,7 @@ namespace wolf
ProcessorTrackerFeature::ProcessorTrackerFeature(const std::string& _type,
int _dim,
- ProcessorParamsTrackerFeaturePtr _params_tracker_feature) :
+ ParamsProcessorTrackerFeaturePtr _params_tracker_feature) :
ProcessorTracker(_type, _dim, _params_tracker_feature),
params_tracker_feature_(_params_tracker_feature)
{
@@ -47,7 +47,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 +75,21 @@ 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_ || 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 +135,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 +147,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..0e593a917d9c7715cb16a62a43154cbd53992d58 100644
--- a/src/processor/processor_tracker_landmark.cpp
+++ b/src/processor/processor_tracker_landmark.cpp
@@ -14,7 +14,7 @@ namespace wolf
{
ProcessorTrackerLandmark::ProcessorTrackerLandmark(const std::string& _type,
- ProcessorParamsTrackerLandmarkPtr _params_tracker_landmark) :
+ ParamsProcessorTrackerLandmarkPtr _params_tracker_landmark) :
ProcessorTracker(_type, 0, _params_tracker_landmark),
params_tracker_landmark_(_params_tracker_landmark)
{
@@ -30,6 +30,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 +42,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 +133,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_diff_drive.cpp b/src/sensor/sensor_diff_drive.cpp
index ae48fcceb261da67fa2b940aa06248b3d5006e48..03634eb0938d9ae7c6a20b5d5a5d11a201989101 100644
--- a/src/sensor/sensor_diff_drive.cpp
+++ b/src/sensor/sensor_diff_drive.cpp
@@ -47,10 +47,10 @@ SensorDiffDrive::~SensorDiffDrive()
} /* 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/trajectory/trajectory_base.cpp b/src/trajectory/trajectory_base.cpp
index 428faf954d89ffc8f0d7a88b182cd04a92850c4a..7620c9fa29132561cb0324d60399da97355f7f67 100644
--- a/src/trajectory/trajectory_base.cpp
+++ b/src/trajectory/trajectory_base.cpp
@@ -4,7 +4,7 @@
namespace wolf {
TrajectoryBase::TrajectoryBase(const std::string& _frame_structure) :
- NodeBase("TRAJECTORY", "Base"),
+ NodeBase("TRAJECTORY", "TrajectoryBase"),
frame_structure_(_frame_structure),
last_key_frame_ptr_(nullptr),
last_key_or_aux_frame_ptr_(nullptr)
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/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..dc7cfae429f38b83a414229f78a70b01cc5ba53f 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -149,6 +149,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 +177,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 +193,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 +245,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..ffe372f0cb9f5671cc491f295d38ec24b305da3b
--- /dev/null
+++ b/test/dummy/factor_dummy_zero_1.h
@@ -0,0 +1,45 @@
+#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>
+{
+ public:
+ FactorDummyZero1(const StateBlockPtr& _sb_ptr) :
+ FactorAutodiff<FactorDummyZero1, 1, 1>("FactorDummy1",
+ 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..3a57aa386d20524fbbec30aa154f555af0fbb9a1
--- /dev/null
+++ b/test/dummy/factor_dummy_zero_12.h
@@ -0,0 +1,155 @@
+#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>
+{
+ static const unsigned int id = ID;
+ public:
+ FactorDummyZero12(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) :
+ FactorAutodiff<FactorDummyZero12<ID>, ID, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12>("FactorDummy12",
+ 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/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..c6f651ea013d2fac02164e0d3ea2cb96182fb6b2 100644
--- a/test/dummy/processor_tracker_feature_dummy.h
+++ b/test/dummy/processor_tracker_feature_dummy.h
@@ -14,16 +14,16 @@
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 +31,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();
+
+ // Factory method for high level API
+ WOLF_PROCESSOR_CREATE(ProcessorTrackerFeatureDummy, ParamsProcessorTrackerFeatureDummy);
+
virtual void configure(SensorBasePtr _sensor) { };
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
@@ -101,15 +105,9 @@ class ProcessorTrackerFeatureDummy : public ProcessorTrackerFeature
* 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);
-
};
-inline ProcessorTrackerFeatureDummy::ProcessorTrackerFeatureDummy(ProcessorParamsTrackerFeatureDummyPtr _params_tracker_feature_dummy) :
+inline ProcessorTrackerFeatureDummy::ProcessorTrackerFeatureDummy(ParamsProcessorTrackerFeatureDummyPtr _params_tracker_feature_dummy) :
ProcessorTrackerFeature("ProcessorTrackerFeatureDummy", 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..472b21ab150a7945987d0d64d5cd171bdf9975eb 100644
--- a/test/dummy/processor_tracker_landmark_dummy.cpp
+++ b/test/dummy/processor_tracker_landmark_dummy.cpp
@@ -12,7 +12,7 @@
namespace wolf
{
-ProcessorTrackerLandmarkDummy::ProcessorTrackerLandmarkDummy(ProcessorParamsTrackerLandmarkDummyPtr _params_tracker_landmark_dummy) :
+ProcessorTrackerLandmarkDummy::ProcessorTrackerLandmarkDummy(ParamsProcessorTrackerLandmarkDummyPtr _params_tracker_landmark_dummy) :
ProcessorTrackerLandmark("ProcessorTrackerLandmarkDummy", _params_tracker_landmark_dummy),
params_tracker_landmark_dummy_(_params_tracker_landmark_dummy)
{
@@ -112,4 +112,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..b9485b3378e7644aa9206213dc3466fd41b83340 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();
+
+ // Factory method for high level API
+ WOLF_PROCESSOR_CREATE(ProcessorTrackerLandmarkDummy, ParamsProcessorTrackerLandmarkDummy);
+
virtual void configure(SensorBasePtr _sensor) { };
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
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_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_autodiff.cpp b/test/gtest_factor_autodiff.cpp
index 993908461d6b37cae1b24d8701a3612d4dc9c432..b2b28b32d89412e3afbd2f3466946bed6cc1932b 100644
--- a/test/gtest_factor_autodiff.cpp
+++ b/test/gtest_factor_autodiff.cpp
@@ -13,11 +13,340 @@
#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)
+{
+ StateBlockPtr sb = std::make_shared<StateBlock>(Eigen::Vector1d::Random());
+
+ auto fac = std::make_shared<FactorDummyZero1>(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;
+
+ // 1 ------------------------------------------------------------------------
+ fac = std::make_shared<FactorDummyZero12<1>>(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>>(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>>(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>>(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>>(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>>(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>>(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>>(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>>(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>>(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>>(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>>(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>(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)));
@@ -44,7 +373,7 @@ 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;
@@ -88,7 +417,7 @@ 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;
@@ -166,7 +495,7 @@ 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;
diff --git a/test/gtest_factor_block_difference.cpp b/test/gtest_factor_block_difference.cpp
index cded07e0a3d882710f6b42e16a9460afb5e77611..594eb169754b47fb3304536742ea445051e4da85 100644
--- a/test/gtest_factor_block_difference.cpp
+++ b/test/gtest_factor_block_difference.cpp
@@ -66,6 +66,15 @@ class FixtureFactorBlockDifference : public testing::Test
virtual void TearDown() override {}
};
+TEST_F(FixtureFactorBlockDifference, CheckFactorType)
+{
+ // Feat_->setMeasurement()
+ FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>(
+ Feat_, KF0_->getP(), KF1_->getP()
+ );
+ ASSERT_EQ(Fac->getType(), "FactorBlockDifference");
+}
+
TEST_F(FixtureFactorBlockDifference, EqualP)
{
diff --git a/test/gtest_factor_diff_drive.cpp b/test/gtest_factor_diff_drive.cpp
index 0c88c19c7a719754f82abb964a2f8818aaeacb35..72f8d2529751a2ae634d93203120d5a14e766490 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)
{
//
@@ -99,7 +99,7 @@ class ProcessorDiffDrivePublic : public ProcessorDiffDrive
return Base::emplaceFactor(_feature_motion, _capture_origin);
}
- ProcessorParamsDiffDrivePtr getParams()
+ ParamsProcessorDiffDrivePtr getParams()
{
return params_prc_diff_drv_selfcal_;
}
@@ -121,7 +121,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 +154,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;
@@ -432,7 +432,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;
@@ -568,7 +568,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;
diff --git a/test/gtest_factor_odom_2d.cpp b/test/gtest_factor_odom_2d.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..76d559a767f962ef7e82c41bffdded012dd4d04e
--- /dev/null
+++ b/test/gtest_factor_odom_2d.cpp
@@ -0,0 +1,109 @@
+#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, Vector3d::Zero(), TimeStamp(0));
+FrameBasePtr frm1 = problem_ptr->emplaceFrame(KEY, Vector3d::Zero(), TimeStamp(1));
+
+// Capture, feature and factor from frm1 to frm0
+auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, nullptr, Vector3d::Zero(), data_cov);
+auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", Vector3d::Zero(), data_cov);
+auto fac1 = FactorBase::emplace<FactorOdom2d>(fea1, fea1, frm0, nullptr, false); // create and add
+
+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 and measurement
+ frm0->setState(x0);
+ frm1->setState(x1);
+ cap1->setData(delta);
+ fea1->setMeasurement(delta);
+
+ // 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->getState(), x1, 1e-6);
+ }
+}
+
+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 and measurement
+ frm0->setState(x0);
+ frm1->setState(x1);
+ cap1->setData(delta);
+ fea1->setMeasurement(delta);
+
+ // 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->getState(), x0, 1e-6);
+ }
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_factor_relative_2d_analytic.cpp b/test/gtest_factor_relative_2d_analytic.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fd8e2296c887aeb7ce6a07579eec939964d29a92
--- /dev/null
+++ b/test/gtest_factor_relative_2d_analytic.cpp
@@ -0,0 +1,109 @@
+#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, Vector3d::Zero(), TimeStamp(0));
+FrameBasePtr frm1 = problem_ptr->emplaceFrame(KEY, Vector3d::Zero(), TimeStamp(1));
+
+// Capture, feature and factor from frm1 to frm0
+auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, nullptr, Vector3d::Zero(), data_cov);
+auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", Vector3d::Zero(), data_cov);
+auto fac1 = FactorBase::emplace<FactorRelative2dAnalytic>(fea1, "odom2d", fea1, frm0, nullptr, false); // create and add
+
+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);
+ fea1->setMeasurement(delta);
+
+ // 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->getState(), x1, 1e-6);
+ }
+}
+
+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);
+ fea1->setMeasurement(delta);
+
+ // 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->getState(), x0, 1e-6);
+ }
+}
+
+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..32cff2bd79a00fdcf18850c5f871493584e27865
--- /dev/null
+++ b/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
@@ -0,0 +1,219 @@
+#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, Vector3d::Zero(), TimeStamp(0));
+FrameBasePtr frm1 = problem_ptr->emplaceFrame(KEY, Vector3d::Zero(), TimeStamp(1));
+
+// Capture, feature and factor from frm1 to frm0
+auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, sensor_odom2d, Vector3d::Zero(), data_cov);
+auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", Vector3d::Zero(), data_cov);
+auto fac1 = FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(fea1, fea1, frm0, processor_odom2d, false); // create and add
+
+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 and measurement
+ frm0->setState(x0);
+ frm1->setState(x1);
+ cap1->setData(delta);
+ fea1->setMeasurement(delta);
+ sensor_odom2d->setState(xs);
+
+ // 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->getState(), x1, 1e-6);
+ }
+}
+
+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 and measurement
+ frm0->setState(x0);
+ frm1->setState(x1);
+ cap1->setData(delta);
+ fea1->setMeasurement(delta);
+ sensor_odom2d->setState(xs);
+
+ // 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->getState(), x0, 1e-6);
+ }
+}
+
+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 and measurement
+ frm0->setState(x0);
+ frm1->setState(x1);
+ cap1->setData(delta);
+ fea1->setMeasurement(delta);
+ sensor_odom2d->setState(xs);
+
+ // 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->getState(), xs, 1e-6);
+ }
+}
+
+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 and measurement
+ frm0->setState(x0);
+ frm1->setState(x1);
+ cap1->setData(delta);
+ fea1->setMeasurement(delta);
+ sensor_odom2d->setState(xs);
+
+ //std::cout << "Sensor O: " << sensor_odom2d->getO()->getState().transpose() << std::endl;
+
+ // 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->getState(), xs, 1e-6);
+ }
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp
index 77fa1ace2c4bf876b0c318fe79dc476688e92af0..90ba45c62add57f2f5d222579810d39f69b0cf6b 100644
--- a/test/gtest_frame_base.cpp
+++ b/test/gtest_frame_base.cpp
@@ -72,7 +72,7 @@ TEST(FrameBase, LinksToTree)
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 p = ProcessorBase::emplace<ProcessorOdom2d>(S, std::make_shared<ParamsProcessorOdom2d>());
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);
diff --git a/test/gtest_odom_2d.cpp b/test/gtest_odom_2d.cpp
index a17ea2d4dc50b36dcab596fcb5a970f56e4b5c1d..93b1ed685f976e09dd467305e7d2c2472fc37617 100644
--- a/test/gtest_odom_2d.cpp
+++ b/test/gtest_odom_2d.cpp
@@ -197,7 +197,7 @@ 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 = data(1)*2.5; // force KF at every third process()
@@ -338,7 +338,7 @@ 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->max_time_span = 100;
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index 794d2aebaabdbe02214f2dde17bdf8118437985c..bf878e87c1cb5a277225ac20be11fce7f1b12c5d 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -31,11 +31,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
+// Register in the FactoryProcessor
+#include "core/processor/factory_processor.h"
WOLF_PTR_TYPEDEFS(DummySolverManager);
@@ -104,7 +101,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);
@@ -330,7 +327,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;
diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp
index 96db133bba8e3efa62e6dd68fc0c1331d24623fd..4ac7acf50f3fc066901858db2b34843c58634af3 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);
@@ -143,4 +137,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 bcabf20437590cda0472777cd0e0b4e21bb381a3..92d985bfdd05576be50f862de33f13df5056eb23 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)
{
//
@@ -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);
diff --git a/test/gtest_processor_loopclosure.cpp b/test/gtest_processor_loopclosure.cpp
index 5fb84268d1c376955e8cb2c7a6ed4edbb7536c11..5d5bb796004cb66a817a5fb546eee22da35e0ba5 100644
--- a/test/gtest_processor_loopclosure.cpp
+++ b/test/gtest_processor_loopclosure.cpp
@@ -23,7 +23,7 @@ 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();};
@@ -71,7 +71,7 @@ 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;
diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp
index 4e449f781ba1c8091806fa7634d8b3c87bd4ff1e..8b80be06846217ed28016b1c827b31aa5c81dfb7 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)
{
//
}
@@ -52,7 +52,7 @@ class ProcessorMotion_test : public testing::Test{
Matrix2d data_cov;
// ProcessorMotion_test() :
-// ProcessorOdom2d(std::make_shared<ProcessorParamsOdom2d>()),
+// ProcessorOdom2d(std::make_shared<ParamsProcessorOdom2d>()),
// dt(0)
// { }
@@ -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;
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 e42943aeca2ef0f7359c226f20a16200da6cfdc6..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;
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_tree_manager.cpp b/test/gtest_tree_manager.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4f609da767afb85bbc6cc818906524612aff3ded
--- /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("PO", 3, KEY, VectorXd(7), TimeStamp(0.0));
+ 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..4cf8cf524ef114f357919661a3aee62a65e34234
--- /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->getState();
+ Vector7d zero_disp(state);
+ Matrix6d cov = Matrix6d::Identity();
+
+ // FRAME 2 ----------------------------------------------------------
+ auto F2 = P->emplaceFrame("PO", 3, KEY, state, TimeStamp(2));
+ 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("PO", 3, KEY, state, TimeStamp(3));
+ 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("PO", 3, KEY, state, TimeStamp(4));
+ 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("PO", 3, KEY, state, TimeStamp(5));
+ 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->getState();
+ Vector7d zero_disp(state);
+ Matrix6d cov = Matrix6d::Identity();
+
+ // FRAME 2 ----------------------------------------------------------
+ auto F2 = P->emplaceFrame("PO", 3, KEY, state, TimeStamp(2));
+ 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("PO", 3, KEY, state, TimeStamp(3));
+ 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("PO", 3, KEY, state, TimeStamp(4));
+ 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("PO", 3, KEY, state, TimeStamp(5));
+ 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..085015b89c62f913f7bbf6ab3d6241ddde132dff
--- /dev/null
+++ b/test/yaml/params_tree_manager1.yaml
@@ -0,0 +1,41 @@
+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]
+ 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
\ No newline at end of file
diff --git a/test/yaml/params_tree_manager2.yaml b/test/yaml/params_tree_manager2.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..80b5af47ac509b0746ffb8f66775d87742e1e927
--- /dev/null
+++ b/test/yaml/params_tree_manager2.yaml
@@ -0,0 +1,40 @@
+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]
+ 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
\ No newline at end of file
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..3a0f421c5c0048125d0a8afa742e873f79e703f6
--- /dev/null
+++ b/test/yaml/params_tree_manager_sliding_window1.yaml
@@ -0,0 +1,43 @@
+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]
+ 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
\ No newline at end of file
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..894dadf651dfe18919881ca44a296f3df447246a
--- /dev/null
+++ b/test/yaml/params_tree_manager_sliding_window2.yaml
@@ -0,0 +1,43 @@
+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]
+ 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
\ No newline at end of file
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