diff --git a/CMakeLists.txt b/CMakeLists.txt
index 9ae5db6ed39c099a74bbd8c5e8de2ebd4c7ec3e9..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
@@ -285,7 +286,11 @@ SET(HDRS_SOLVER
include/core/solver/solver_manager.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 100%
rename from hello_wolf/hello_wolf.cpp
rename to demos/hello_wolf/hello_wolf.cpp
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 100%
rename from hello_wolf/processor_range_bearing.cpp
rename to demos/hello_wolf/processor_range_bearing.cpp
diff --git a/hello_wolf/processor_range_bearing.h b/demos/hello_wolf/processor_range_bearing.h
similarity index 100%
rename from hello_wolf/processor_range_bearing.h
rename to demos/hello_wolf/processor_range_bearing.h
diff --git a/hello_wolf/sensor_range_bearing.cpp b/demos/hello_wolf/sensor_range_bearing.cpp
similarity index 100%
rename from hello_wolf/sensor_range_bearing.cpp
rename to demos/hello_wolf/sensor_range_bearing.cpp
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 87%
rename from hello_wolf/yaml/sensor_odom_2d.yaml
rename to demos/hello_wolf/yaml/sensor_odom_2d.yaml
index 2149405a784f637087572df559287f3f36942d7c..65ac28219471ad65180538e526521d7866391a01 100644
--- a/hello_wolf/yaml/sensor_odom_2d.yaml
+++ b/demos/hello_wolf/yaml/sensor_odom_2d.yaml
@@ -2,3 +2,4 @@ type: "SensorOdom2d" # This must match the KEY used in the FactoryS
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/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 93b753b77cbe1560b1eef3a50f9e299e188da2ff..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);
diff --git a/include/core/factor/factor_autodiff.h b/include/core/factor/factor_autodiff.h
index d5e86d2f168acce03feb9085420de1c6ad901ab8..71e09826bbc8f37b5b594e3c93d1041c7f83dcb5 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,7 +95,9 @@ 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>)
{
// initialize jets
unsigned int last_jet_idx = 0;
@@ -111,7 +120,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 +139,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 +169,8 @@ class FactorAutodiff : public FactorBase
parameters[7],
parameters[8],
parameters[9],
+ parameters[10],
+ parameters[11],
residuals);
}
// also compute jacobians
@@ -175,6 +192,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 +237,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 +267,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 +324,552 @@ 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>)
+ {
+ // 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>)
+ {
+ // 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 +884,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_;
@@ -554,7 +1123,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 +1138,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_;
@@ -795,7 +1366,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 +1381,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_;
@@ -1025,7 +1598,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 +1613,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_;
@@ -1240,7 +1815,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 +1830,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_;
@@ -1443,7 +2020,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 +2035,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_;
@@ -1639,7 +2218,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 +2233,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_;
@@ -1824,7 +2405,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 +2420,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_;
@@ -1998,7 +2581,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 +2596,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_;
@@ -2162,113 +2747,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 6533bd414d54a9eb45da8c92336ba3d1f0e07739..402615a5e5c4f11d1f306d57119cb7668031c459 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 = nullptr,
bool _apply_loss_function = false,
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 dc485c52754078c53751c537fcfe72c674f645e0..3da679fe15b63d7a11c011e85855ea06ef564fdf 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -43,6 +43,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_;
@@ -73,6 +74,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;
@@ -361,6 +366,11 @@ class Problem : public std::enable_shared_from_this<Problem>
namespace wolf
{
+inline TreeManagerBasePtr Problem::getTreeManager() const
+{
+ return tree_manager_;
+}
+
inline bool Problem::priorIsSet() const
{
return prior_is_set_;
diff --git a/include/core/processor/processor_tracker.h b/include/core/processor/processor_tracker.h
index f07d906bab503f097330a1d5203f34347a2a6c4e..efe8ef33837df852499491c5f524bcfed0765129 100644
--- a/include/core/processor/processor_tracker.h
+++ b/include/core/processor/processor_tracker.h
@@ -103,6 +103,7 @@ class ProcessorTracker : public ProcessorBase
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;
diff --git a/include/core/processor/processor_tracker_feature.h b/include/core/processor/processor_tracker_feature.h
index 54c4168e88e33e35d6c854a5053982087a2dfaec..b16f2b6c18c03084f1808c2a6a2df275e98fd60b 100644
--- a/include/core/processor/processor_tracker_feature.h
+++ b/include/core/processor/processor_tracker_feature.h
@@ -150,8 +150,8 @@ class ProcessorTrackerFeature : public ProcessorTracker
virtual bool voteForKeyFrame() const = 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
*
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..7b421461c4c636a5a87ccc35b87fc83213d5b4d0
--- /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, TreeManagerName) \
+ namespace{ const bool WOLF_UNUSED TreeManagerName##Registered = \
+ wolf::FactoryTreeManager::get().registerCreator(TreeManagerType, TreeManagerName::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, TreeManagerName) \
+ namespace{ const bool WOLF_UNUSED TreeManagerName##AutoConfRegistered = \
+ wolf::AutoConfFactoryTreeManager::get().registerCreator(TreeManagerType, TreeManagerName::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/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 785d95f2f66293ea72202103852733da111b7365..2b605444d58ec497c044f5910af4174f31d7a06c 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -11,12 +11,13 @@
#include "core/sensor/factory_sensor.h"
#include "core/processor/factory_processor.h"
#include "core/state_block/state_block.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"
-
// IRI libs includes
// C++ includes
@@ -28,10 +29,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>()),
@@ -152,6 +155,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
Eigen::VectorXd prior_state = _server.getParam<Eigen::VectorXd>("problem/prior/state");
Eigen::MatrixXd prior_cov = _server.getParam<Eigen::MatrixXd>("problem/prior/cov");
@@ -471,12 +480,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);
@@ -528,6 +547,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
@@ -909,6 +932,10 @@ FrameBasePtr Problem::setPrior(const Eigen::VectorXd& _prior_state, const Eigen:
if ( !processor->isMotion() )
processor->keyFrameCallback(origin_keyframe, _time_tolerance);
+ // Notify tree manager
+ if (tree_manager_)
+ tree_manager_->keyFrameCallback(origin_keyframe);
+
return origin_keyframe;
}
else
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index e2db67fcd9271ce4cd87652f1c33363be4c34bab..3d4cb43942b60b1bf9103593483d4ca8d394eb48 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"
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index 97f3f6c0b0b7f6a14afdffbe2cb460ce4c94fbdc..6119b6c487b56f2a7c48af70131e0570fa54d1dd 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -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 a0f48d7c771527402b7adffd688e837c0cef7e3f..878f4e24dc31b64627cc3f5959c9cdf80c73f96f 100644
--- a/src/processor/processor_tracker_feature.cpp
+++ b/src/processor/processor_tracker_feature.cpp
@@ -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_,
@@ -79,14 +82,14 @@ unsigned int ProcessorTrackerFeature::processKnown()
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 f6425f4048c0830bc05bfb1d1dc5ac18f9210dec..0e593a917d9c7715cb16a62a43154cbd53992d58 100644
--- a/src/processor/processor_tracker_landmark.cpp
+++ b/src/processor/processor_tracker_landmark.cpp
@@ -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/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..7e59cd114f691e24ea04c1a7dc218501c65d31ca
--- /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", TreeManagerSlidingWindow);
+WOLF_REGISTER_TREE_MANAGER_AUTO("TreeManagerSlidingWindow", TreeManagerSlidingWindow);
+} // namespace wolf
+
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 4693ff76fb3bf38ad5a52d6a6754383e9601dbe0..d7eb625c2717769099d4de48dea2b5c30e43cf7c 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 ParamsProcessorBasePtr _params)
-{
- auto params = std::static_pointer_cast<ParamsProcessorTrackerFeatureDummy>(_params);
-
- // if cast failed use default value
- if (params == nullptr)
- params = std::make_shared<ParamsProcessorTrackerFeatureDummy>();
-
- 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/factory_processor.h"
namespace wolf {
WOLF_REGISTER_PROCESSOR("ProcessorTrackerFeatureDummy", ProcessorTrackerFeatureDummy)
+WOLF_REGISTER_PROCESSOR_AUTO("ProcessorTrackerFeatureDummy", ProcessorTrackerFeatureDummy)
} // namespace wolf
diff --git a/test/dummy/processor_tracker_feature_dummy.h b/test/dummy/processor_tracker_feature_dummy.h
index 4b42d372942b276ae1784486fe823ff5234f26e1..ee8fa4c5bcf8765004c3eb07f940708832249817 100644
--- a/test/dummy/processor_tracker_feature_dummy.h
+++ b/test/dummy/processor_tracker_feature_dummy.h
@@ -14,7 +14,7 @@
namespace wolf
{
-
+
WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorTrackerFeatureDummy);
struct ParamsProcessorTrackerFeatureDummy : public ParamsProcessorTrackerFeature
@@ -31,7 +31,7 @@ struct ParamsProcessorTrackerFeatureDummy : public ParamsProcessorTrackerFeature
WOLF_PTR_TYPEDEFS(ProcessorTrackerFeatureDummy);
-
+
//Class
class ProcessorTrackerFeatureDummy : public ProcessorTrackerFeature
{
@@ -39,6 +39,10 @@ class ProcessorTrackerFeatureDummy : public ProcessorTrackerFeature
public:
ProcessorTrackerFeatureDummy(ParamsProcessorTrackerFeatureDummyPtr _params_tracker_feature);
virtual ~ProcessorTrackerFeatureDummy();
+
+ // Factory method for high level API
+ WOLF_PROCESSOR_CREATE(ProcessorTrackerFeatureDummy, ProcessorParamsTrackerFeatureDummy);
+
virtual void configure(SensorBasePtr _sensor) { };
protected:
@@ -101,12 +105,6 @@ 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 ParamsProcessorBasePtr _params);
-
};
inline ProcessorTrackerFeatureDummy::ProcessorTrackerFeatureDummy(ParamsProcessorTrackerFeatureDummyPtr _params_tracker_feature_dummy) :
diff --git a/test/dummy/processor_tracker_landmark_dummy.cpp b/test/dummy/processor_tracker_landmark_dummy.cpp
index 58519f3d085d67233378ebc813569f933a338d5f..43ba3d8d407b741b5089935c1df37e5bdee41c03 100644
--- a/test/dummy/processor_tracker_landmark_dummy.cpp
+++ b/test/dummy/processor_tracker_landmark_dummy.cpp
@@ -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 ProcessorFactory
+#include "core/processor/processor_factory.h"
+namespace wolf {
+WOLF_REGISTER_PROCESSOR("ProcessorTrackerLandmarkDummy", ProcessorTrackerLandmarkDummy)
+WOLF_REGISTER_PROCESSOR_AUTO("ProcessorTrackerLandmarkDummy", ProcessorTrackerLandmarkDummy)
+} // namespace wolf
+
diff --git a/test/dummy/processor_tracker_landmark_dummy.h b/test/dummy/processor_tracker_landmark_dummy.h
index fa9c11dcffd6e7d78e1bc7e4e0cc988ce827f1fe..3261481365ebbdba04957d149a165be62368bd41 100644
--- a/test/dummy/processor_tracker_landmark_dummy.h
+++ b/test/dummy/processor_tracker_landmark_dummy.h
@@ -34,6 +34,10 @@ class ProcessorTrackerLandmarkDummy : public ProcessorTrackerLandmark
public:
ProcessorTrackerLandmarkDummy(ParamsProcessorTrackerLandmarkDummyPtr _params_tracker_landmark_dummy);
virtual ~ProcessorTrackerLandmarkDummy();
+
+ // Factory method for high level API
+ WOLF_PROCESSOR_CREATE(ProcessorTrackerLandmarkDummy, ProcessorParamsTrackerLandmarkDummy);
+
virtual void configure(SensorBasePtr _sensor) { };
protected:
diff --git a/test/dummy/tree_manager_dummy.h b/test/dummy/tree_manager_dummy.h
new file mode 100644
index 0000000000000000000000000000000000000000..379fdd23902daaa5e3ba9d29a3f6a445f9e21373
--- /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", TreeManagerDummy)
+WOLF_REGISTER_TREE_MANAGER_AUTO("TreeManagerDummy", TreeManagerDummy)
+
+} /* namespace wolf */
+
+#endif /* INCLUDE_TREE_MANAGER_DUMMY_H_ */
diff --git a/test/gtest_factor_autodiff.cpp b/test/gtest_factor_autodiff.cpp
index 993908461d6b37cae1b24d8701a3612d4dc9c432..0ca75836e54c0661be560a623f484bee0e5c592a 100644
--- a/test/gtest_factor_autodiff.cpp
+++ b/test/gtest_factor_autodiff.cpp
@@ -13,11 +13,360 @@
#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)
+template <int First, int Last>
+struct static_for
+{
+ template <typename Lambda>
+ static inline constexpr void apply(Lambda const& f)
+ {
+ if (First < Last)
+ {
+ f(std::integral_constant<int, First>{});
+ static_for<First + 1, Last>::apply(f);
+ }
+ }
+};
+template <int N>
+struct static_for<N, N>
+{
+ template <typename Lambda>
+ static inline constexpr void apply(Lambda const& f) {}
+};
+
+
+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 +393,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 +437,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 +515,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 1e06c7a80377b99197a721e85473319f9bf34862..f661fb37fd65f4f99ecd7015d4e8474688521a18 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_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..547b4c9b7402a9b17eb695ea0e555869fb748a58
--- /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<ProcessorParamsOdom2d>());
+
+// 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_processor_base.cpp b/test/gtest_processor_base.cpp
index f41c0dec436396a0dafffe3a4e549bfcd739c2b0..cf7cd9d0998b278a74d6f29fc8fcd9d6998fb37a 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/factory_processor.h"
-namespace wolf {
-WOLF_REGISTER_PROCESSOR("TRACKER FEATURE DUMMY", ProcessorTrackerFeatureDummy)
-} // namespace wolf
-
-
TEST(ProcessorBase, IsMotion)
{
using namespace wolf;
@@ -84,11 +76,13 @@ 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<ProcessorParamsTrackerFeatureDummy>();
+ 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");
@@ -137,4 +131,3 @@ int main(int argc, char **argv)
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
-
diff --git a/test/gtest_tree_manager.cpp b/test/gtest_tree_manager.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5799117ffa523a9cbf5132ed504fa1525001c0ae
--- /dev/null
+++ b/test/gtest_tree_manager.cpp
@@ -0,0 +1,105 @@
+#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);
+
+ 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);
+
+ 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..ed3a8052bfb4a73b4395cc885904d8f752af91d2
--- /dev/null
+++ b/test/gtest_tree_manager_sliding_window.cpp
@@ -0,0 +1,267 @@
+#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);
+
+ // 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);
+
+ // 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_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..25e8ac8a4417ffec910fee4cac96669a00ebdc4f
--- /dev/null
+++ b/test/yaml/params_tree_manager1.yaml
@@ -0,0 +1,41 @@
+config:
+ problem:
+ frame_structure: "POV"
+ dimension: 3
+ prior:
+ 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
+ timestamp: 0
+ 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..913f5875bfd09750e024fcfeab910ff45d6058ee
--- /dev/null
+++ b/test/yaml/params_tree_manager2.yaml
@@ -0,0 +1,40 @@
+config:
+ problem:
+ frame_structure: "POV"
+ dimension: 3
+ prior:
+ 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
+ timestamp: 0
+ 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..5cbb9e5a787b4ad2e70f9cf7f283ab1747e1279b
--- /dev/null
+++ b/test/yaml/params_tree_manager_sliding_window1.yaml
@@ -0,0 +1,43 @@
+config:
+ problem:
+ frame_structure: "PO"
+ dimension: 3
+ prior:
+ 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
+ timestamp: 0
+ 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..01f41eea97c7cca943d4aa12a143736668c2673f
--- /dev/null
+++ b/test/yaml/params_tree_manager_sliding_window2.yaml
@@ -0,0 +1,43 @@
+config:
+ problem:
+ frame_structure: "PO"
+ dimension: 3
+ prior:
+ 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
+ timestamp: 0
+ 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