diff --git a/CMakeLists.txt b/CMakeLists.txt
index fdc81d7a6ed05ce6f166e14f198009c63ad64dad..98de6dbd9bfb7521238c7c784b077456b35b5629 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -181,13 +181,14 @@ SET(HDRS_MATH
include/core/math/covariance.h
)
SET(HDRS_UTILS
+ include/core/utils/check_log.h
include/core/utils/converter.h
include/core/utils/eigen_assert.h
include/core/utils/eigen_predicates.h
- include/core/utils/loader.hpp
+ include/core/utils/loader.h
include/core/utils/logging.h
include/core/utils/make_unique.h
- include/core/utils/params_server.hpp
+ include/core/utils/params_server.h
include/core/utils/singleton.h
include/core/utils/utils_gtest.h
include/core/utils/converter_utils.h
@@ -215,15 +216,15 @@ SET(HDRS_STATE_BLOCK
include/core/state_block/local_parametrization_quaternion.h
include/core/state_block/state_angle.h
include/core/state_block/state_block.h
- include/core/state_block/state_homogeneous_3D.h
+ include/core/state_block/state_homogeneous_3d.h
include/core/state_block/state_quaternion.h
)
SET(HDRS_CAPTURE
include/core/capture/capture_base.h
include/core/capture/capture_motion.h
- include/core/capture/capture_odom_2D.h
- include/core/capture/capture_odom_3D.h
+ include/core/capture/capture_odom_2d.h
+ include/core/capture/capture_odom_3d.h
include/core/capture/capture_pose.h
include/core/capture/capture_velocity.h
include/core/capture/capture_void.h
@@ -232,25 +233,25 @@ SET(HDRS_CAPTURE
SET(HDRS_FACTOR
include/core/factor/factor_analytic.h
include/core/factor/factor_autodiff.h
- include/core/factor/factor_autodiff_distance_3D.h
+ include/core/factor/factor_autodiff_distance_3d.h
include/core/factor/factor_base.h
include/core/factor/factor_block_absolute.h
include/core/factor/factor_block_difference.h
include/core/factor/factor_diff_drive.h
- include/core/factor/factor_odom_2D.h
- include/core/factor/factor_odom_2D_closeloop.h
- include/core/factor/factor_odom_2D_analytic.h
- include/core/factor/factor_odom_3D.h
- include/core/factor/factor_pose_2D.h
- include/core/factor/factor_pose_3D.h
+ include/core/factor/factor_odom_2d.h
+ include/core/factor/factor_odom_2d_closeloop.h
+ include/core/factor/factor_odom_2d_analytic.h
+ include/core/factor/factor_odom_3d.h
+ include/core/factor/factor_pose_2d.h
+ 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_2d_analytic.h
)
SET(HDRS_FEATURE
include/core/feature/feature_base.h
include/core/feature/feature_match.h
include/core/feature/feature_motion.h
- include/core/feature/feature_odom_2D.h
+ include/core/feature/feature_odom_2d.h
include/core/feature/feature_pose.h
)
SET(HDRS_LANDMARK
@@ -266,8 +267,8 @@ SET(HDRS_PROCESSOR
include/core/processor/processor_logging.h
include/core/processor/processor_loopclosure.h
include/core/processor/processor_motion.h
- include/core/processor/processor_odom_2D.h
- include/core/processor/processor_odom_3D.h
+ include/core/processor/processor_odom_2d.h
+ include/core/processor/processor_odom_3d.h
include/core/processor/processor_tracker.h
include/core/processor/processor_tracker_feature.h
include/core/processor/processor_tracker_landmark.h
@@ -277,16 +278,20 @@ SET(HDRS_SENSOR
include/core/sensor/sensor_base.h
include/core/sensor/sensor_diff_drive.h
include/core/sensor/sensor_factory.h
- include/core/sensor/sensor_odom_2D.h
- include/core/sensor/sensor_odom_3D.h
+ include/core/sensor/sensor_odom_2d.h
+ include/core/sensor/sensor_odom_3d.h
)
SET(HDRS_SOLVER
include/core/solver/solver_manager.h
include/core/solver/solver_factory.h
)
-
+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.hpp
+ include/core/yaml/parser_yaml.h
include/core/yaml/yaml_conversion.h
)
#SOURCES
@@ -320,13 +325,16 @@ SET(SRCS_MATH
)
SET(SRCS_UTILS
src/utils/converter_utils.cpp
+ src/utils/params_server.cpp
+ src/utils/loader.cpp
+ src/utils/check_log.cpp
)
SET(SRCS_CAPTURE
src/capture/capture_base.cpp
src/capture/capture_motion.cpp
- src/capture/capture_odom_2D.cpp
- src/capture/capture_odom_3D.cpp
+ src/capture/capture_odom_2d.cpp
+ src/capture/capture_odom_3d.cpp
src/capture/capture_pose.cpp
src/capture/capture_velocity.cpp
src/capture/capture_void.cpp
@@ -339,7 +347,7 @@ SET(SRCS_FACTOR
SET(SRCS_FEATURE
src/feature/feature_base.cpp
src/feature/feature_motion.cpp
- src/feature/feature_odom_2D.cpp
+ src/feature/feature_odom_2d.cpp
src/feature/feature_pose.cpp
)
SET(SRCS_LANDMARK
@@ -351,8 +359,8 @@ SET(SRCS_PROCESSOR
src/processor/processor_diff_drive.cpp
src/processor/processor_loopclosure.cpp
src/processor/processor_motion.cpp
- src/processor/processor_odom_2D.cpp
- src/processor/processor_odom_3D.cpp
+ src/processor/processor_odom_2d.cpp
+ src/processor/processor_odom_3d.cpp
src/processor/processor_tracker.cpp
src/processor/processor_tracker_feature.cpp
src/processor/processor_tracker_landmark.cpp
@@ -361,16 +369,20 @@ SET(SRCS_PROCESSOR
SET(SRCS_SENSOR
src/sensor/sensor_base.cpp
src/sensor/sensor_diff_drive.cpp
- src/sensor/sensor_odom_2D.cpp
- src/sensor/sensor_odom_3D.cpp
+ src/sensor/sensor_odom_2d.cpp
+ src/sensor/sensor_odom_3d.cpp
)
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/processor_odom_3D_yaml.cpp
- src/yaml/sensor_odom_2D_yaml.cpp
- src/yaml/sensor_odom_3D_yaml.cpp
+ src/yaml/parser_yaml.cpp
+ src/yaml/processor_odom_3d_yaml.cpp
+ src/yaml/sensor_odom_2d_yaml.cpp
+ src/yaml/sensor_odom_3d_yaml.cpp
)
#OPTIONALS
#optional HDRS and SRCS
@@ -426,6 +438,7 @@ ADD_LIBRARY(${PROJECT_NAME}
${SRCS_SOLVER}
${SRCS_STATE_BLOCK}
${SRCS_TRAJECTORY}
+ ${SRCS_TREE_MANAGER}
${SRCS_UTILS}
${SRCS_WRAPPER}
${SRCS_YAML}
@@ -466,44 +479,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/demo_analytic_autodiff_factor.cpp b/demos/demo_analytic_autodiff_factor.cpp
index 7d0fdbec689cba9f912e8e26c3c823bb6c29cb23..6325f785bd2cbfab046f31b73e419c01adf3d7ff 100644
--- a/demos/demo_analytic_autodiff_factor.cpp
+++ b/demos/demo_analytic_autodiff_factor.cpp
@@ -57,9 +57,9 @@ int main(int argc, char** argv)
std::map<unsigned int, FrameBasePtr> index_2_frame_ptr_analytic;
// Wolf problem
- ProblemPtr wolf_problem_autodiff = new Problem(FRM_PO_2D);
- ProblemPtr wolf_problem_analytic = new Problem(FRM_PO_2D);
- SensorBasePtr sensor = new SensorBase("ODOM 2D", 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);
+ ProblemPtr wolf_problem_autodiff = new Problem(FRM_PO_2d);
+ ProblemPtr wolf_problem_analytic = new Problem(FRM_PO_2d);
+ SensorBasePtr sensor = new SensorBase("ODOM 2d", 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);
// Ceres wrapper
ceres::Solver::Options ceres_options;
@@ -242,7 +242,7 @@ int main(int argc, char** argv)
FrameBasePtr frame_new_ptr_autodiff = index_2_frame_ptr_autodiff[edge_new];
frame_new_ptr_autodiff->addCapture(capture_ptr_autodiff);
capture_ptr_autodiff->addFeature(feature_ptr_autodiff);
- FactorOdom2D* factor_ptr_autodiff = new FactorOdom2D(feature_ptr_autodiff, frame_old_ptr_autodiff);
+ FactorOdom2d* factor_ptr_autodiff = new FactorOdom2d(feature_ptr_autodiff, frame_old_ptr_autodiff);
feature_ptr_autodiff->addFactor(factor_ptr_autodiff);
//std::cout << "Added autodiff edge! " << factor_ptr_autodiff->id() << " from vertex " << factor_ptr_autodiff->getCapture()->getFrame()->id() << " to " << factor_ptr_autodiff->getFrameOther()->id() << std::endl;
@@ -255,7 +255,7 @@ int main(int argc, char** argv)
FrameBasePtr frame_new_ptr_analytic = index_2_frame_ptr_analytic[edge_new];
frame_new_ptr_analytic->addCapture(capture_ptr_analytic);
capture_ptr_analytic->addFeature(feature_ptr_analytic);
- FactorOdom2DAnalytic* factor_ptr_analytic = new FactorOdom2DAnalytic(feature_ptr_analytic, frame_old_ptr_analytic);
+ FactorOdom2dAnalytic* factor_ptr_analytic = new FactorOdom2dAnalytic(feature_ptr_analytic, frame_old_ptr_analytic);
feature_ptr_analytic->addFactor(factor_ptr_analytic);
//std::cout << "Added analytic edge! " << factor_ptr_analytic->id() << " from vertex " << factor_ptr_analytic->getCapture()->getFrame()->id() << " to " << factor_ptr_analytic->getFrameOther()->id() << std::endl;
//std::cout << "vector " << factor_ptr_analytic->getMeasurement().transpose() << std::endl;
diff --git a/demos/demo_map_yaml.cpp b/demos/demo_map_yaml.cpp
index c56eba791c994cacc04577154f5720f0b81ce486..87023e52856c9e3ba937882cd5311e05260b507b 100644
--- a/demos/demo_map_yaml.cpp
+++ b/demos/demo_map_yaml.cpp
@@ -8,7 +8,7 @@
#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_polyline_2d.h"
#include "core/landmark/landmark_AHP.h"
#include "core/state_block/state_block.h"
#include "core/yaml/yaml_conversion.h"
@@ -22,9 +22,9 @@ void print(MapBase& _map)
{
std::cout << "Lmk ID: " << lmk_ptr->id();
std::cout << "\nLmk type: " << lmk_ptr->getType();
- if (lmk_ptr->getType() == "POLYLINE 2D")
+ if (lmk_ptr->getType() == "POLYLINE 2d")
{
- LandmarkPolyline2DPtr poly_ptr = std::static_pointer_cast<LandmarkPolyline2D>(lmk_ptr);
+ 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();
diff --git a/demos/demo_wolf_imported_graph.cpp b/demos/demo_wolf_imported_graph.cpp
index 40646846123e3fcd0bf09b27e20d0c03f27d1165..f19095d3400f9c08b9633e6486398a0d059bc707 100644
--- a/demos/demo_wolf_imported_graph.cpp
+++ b/demos/demo_wolf_imported_graph.cpp
@@ -61,9 +61,9 @@ int main(int argc, char** argv)
std::map<FrameBasePtr, unsigned int> frame_ptr_2_index_prun;
// Wolf problem
- ProblemPtr wolf_problem_full = new Problem(FRM_PO_2D);
- ProblemPtr wolf_problem_prun = new Problem(FRM_PO_2D);
- SensorBasePtr sensor = new SensorBase("ODOM 2D", 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);
+ ProblemPtr wolf_problem_full = new Problem(FRM_PO_2d);
+ ProblemPtr wolf_problem_prun = new Problem(FRM_PO_2d);
+ SensorBasePtr sensor = new SensorBase("ODOM 2d", 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);
Eigen::SparseMatrix<double> Lambda(0,0);
@@ -259,8 +259,8 @@ int main(int argc, char** argv)
frame_new_ptr_prun->addCapture(capture_ptr_prun);
capture_ptr_full->addFeature(feature_ptr_full);
capture_ptr_prun->addFeature(feature_ptr_prun);
- FactorOdom2D* factor_ptr_full = new FactorOdom2D(feature_ptr_full, frame_old_ptr_full);
- FactorOdom2D* factor_ptr_prun = new FactorOdom2D(feature_ptr_prun, frame_old_ptr_prun);
+ FactorOdom2d* factor_ptr_full = new FactorOdom2d(feature_ptr_full, frame_old_ptr_full);
+ FactorOdom2d* factor_ptr_prun = new FactorOdom2d(feature_ptr_prun, frame_old_ptr_prun);
feature_ptr_full->addFactor(factor_ptr_full);
feature_ptr_prun->addFactor(factor_ptr_prun);
//std::cout << "Added edge! " << factor_ptr_prun->id() << " from vertex " << factor_ptr_prun->getCapture()->getFrame()->id() << " to " << factor_ptr_prun->getFrameTo()->id() << std::endl;
diff --git a/demos/processor_odom_3D.yaml b/demos/processor_odom_3d.yaml
similarity index 75%
rename from demos/processor_odom_3D.yaml
rename to demos/processor_odom_3d.yaml
index 89e9d00f268fd341368f1c4063e4685985072750..2526e5dde7b256431a3f2fa5735b398a51faadb7 100644
--- a/demos/processor_odom_3D.yaml
+++ b/demos/processor_odom_3d.yaml
@@ -1,4 +1,4 @@
-type: "ODOM 3D" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+type: "ProcessorOdom3d" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
time_tolerance: 0.01 # seconds
unmeasured_perturbation_std: 0.001
diff --git a/test/yaml/sensor_odom_3D.yaml b/demos/sensor_odom_3d.yaml
similarity index 77%
rename from test/yaml/sensor_odom_3D.yaml
rename to demos/sensor_odom_3d.yaml
index 9fb43d4c30c0f5c01757362f6122d0cba98b14c5..58db1c088fbc80339a78ba815fddbaf69674d3b6 100644
--- a/test/yaml/sensor_odom_3D.yaml
+++ b/demos/sensor_odom_3d.yaml
@@ -1,4 +1,4 @@
-type: "SensorOdom3D" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+type: "SensorOdom3d" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
k_disp_to_disp: 0.02 # m^2 / m
k_disp_to_rot: 0.02 # rad^2 / m
diff --git a/demos/sensor_odom_3D_HQ.yaml b/demos/sensor_odom_3d_HQ.yaml
similarity index 51%
rename from demos/sensor_odom_3D_HQ.yaml
rename to demos/sensor_odom_3d_HQ.yaml
index 0d6b65cab555e1ced3c6443faf8ade4c3ab95ea0..8515eb0cb24f711b29b88196a7f764895c0b6ff6 100644
--- a/demos/sensor_odom_3D_HQ.yaml
+++ b/demos/sensor_odom_3d_HQ.yaml
@@ -1,7 +1,7 @@
-type: "ODOM 3D" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+type: "SensorOdom3d" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
k_disp_to_disp: 0.000001 # m^2 / m
k_disp_to_rot: 0.000001 # rad^2 / m
k_rot_to_rot: 0.000001 # rad^2 / rad
min_disp_var: 0.00000001 # m^2
-min_rot_var: 0.00000001 # rad^2
\ No newline at end of file
+min_rot_var: 0.00000001 # rad^2
diff --git a/doc/interpolate.rtf b/doc/interpolate.rtf
index f64c7d77940675ec18b62d6428fe42eaaa376c58..4aea11adf1deb3f7a4e1093555ff799a6c1eb929 100644
--- a/doc/interpolate.rtf
+++ b/doc/interpolate.rtf
@@ -179,12 +179,12 @@
\cf2 *\cf0 \
\cf2 * For simple pose increments, we can use a local implementation:\cf0 \
\cf2 *\cf0 \
-\cf2 * - for 2D\cf0 \
+\cf2 * - for 2d\cf0 \
\cf2 *\cf0 \
\cf2 * dp_S = dR_I.tr * (1-tau)*dp_F // dR is the rotation matrix of the angle delta '\ul da\ulnone '; '\ul tr\ulnone ' is transposed\cf0 \
\cf2 * da_S = dR_I.tr * (1-tau)*da_F\cf0 \
\cf2 *\cf0 \
-\cf2 * - for 3D\cf0 \
+\cf2 * - for 3d\cf0 \
\cf2 *\cf0 \
\cf2 * dp_S = dq_I.conj * (1-tau)*dp_F // \ul dq\ulnone is a \ul quaternion\ulnone ; '\ul conj\ulnone ' is the \ul conjugate\ulnone \ul quaternion\ulnone .\cf0 \
\cf2 * dq_S = dq_I.conj * dq_F\cf0 \
@@ -206,14 +206,14 @@
\cf2 *\cf0 \
\cf2 * ### Examples\cf0 \
\cf2 *\cf0 \
-\cf2 * #### Example 1: For 2D poses\cf0 \
+\cf2 * #### Example 1: For 2d poses\cf0 \
\cf2 *\cf0 \
\cf2 *\cf0 \
\cf2 * t_I = _ts // time stamp of the interpolated motion\cf0 \
\cf2 * tau = (t_I - t_R) / (t_F - t_R) // interpolation factor\cf0 \
\cf2 *\cf0 \
\cf2 * dp_I = tau*dp_F // \ul dp\ulnone is a 2-vector\cf0 \
-\cf2 * da_I = tau*da_F // \ul da\ulnone is an angle, for 2D poses\cf0 \
+\cf2 * da_I = tau*da_F // \ul da\ulnone is an angle, for 2d poses\cf0 \
\cf2 *\cf0 \
\cf2 * D_I = deltaPlusDelta(D_R, d_I)\cf0 \
\cf2 *\cf0 \
@@ -222,7 +222,7 @@
\cf2 *\cf0 \
\cf2 * D_S = D_F\cf0 \
\cf2 *\cf0 \
-\cf2 * #### Example 2: For 3D poses\cf0 \
+\cf2 * #### Example 2: For 3d poses\cf0 \
\cf2 *\cf0 \
\cf2 * Orientation is in \ul quaternion\ulnone form, which is the best for interpolation using `\ul slerp\ulnone ()` :\cf0 \
\cf2 *\cf0 \
@@ -230,7 +230,7 @@
\cf2 * tau = (t_I - t_R) / (t_F - t_R) // interpolation factor\cf0 \
\cf2 *\cf0 \
\cf2 * dp_I = tau*dp_F // \ul dp\ulnone is a 3-vector\cf0 \
-\cf2 * dq_I = 1.\ul slerp\ulnone (tau, dq_F) // '1' is the identity \ul quaternion\ulnone ; \ul slerp\ulnone () interpolates 3D rotation.\cf0 \
+\cf2 * dq_I = 1.\ul slerp\ulnone (tau, dq_F) // '1' is the identity \ul quaternion\ulnone ; \ul slerp\ulnone () interpolates 3d rotation.\cf0 \
\cf2 *\cf0 \
\cf2 * D_I = deltaPlusDelta(D_R, d_I)\cf0 \
\cf2 *\cf0 \
@@ -260,4 +260,4 @@
\cf2 * DC_S = DC_F\cf0 \
\cf2 *\cf0 \
\cf2 */\cf0 \
-}
\ No newline at end of file
+}
diff --git a/hello_wolf/CMakeLists.txt b/hello_wolf/CMakeLists.txt
index 8d1114b65c1f81eb7fecf443abeeff16c723d6d3..bf223af61a3358cfc458d8360c1b07e7ab1aff01 100644
--- a/hello_wolf/CMakeLists.txt
+++ b/hello_wolf/CMakeLists.txt
@@ -7,7 +7,7 @@ SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
${CMAKE_CURRENT_SOURCE_DIR}/factor_bearing.h
${CMAKE_CURRENT_SOURCE_DIR}/factor_range_bearing.h
${CMAKE_CURRENT_SOURCE_DIR}/feature_range_bearing.h
- ${CMAKE_CURRENT_SOURCE_DIR}/landmark_point_2D.h
+ ${CMAKE_CURRENT_SOURCE_DIR}/landmark_point_2d.h
${CMAKE_CURRENT_SOURCE_DIR}/processor_range_bearing.h
${CMAKE_CURRENT_SOURCE_DIR}/sensor_range_bearing.h
)
@@ -15,7 +15,7 @@ SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
SET(SRCS_HELLOWOLF
${CMAKE_CURRENT_SOURCE_DIR}/capture_range_bearing.cpp
${CMAKE_CURRENT_SOURCE_DIR}/feature_range_bearing.cpp
- ${CMAKE_CURRENT_SOURCE_DIR}/landmark_point_2D.cpp
+ ${CMAKE_CURRENT_SOURCE_DIR}/landmark_point_2d.cpp
${CMAKE_CURRENT_SOURCE_DIR}/processor_range_bearing.cpp
${CMAKE_CURRENT_SOURCE_DIR}/sensor_range_bearing.cpp
)
@@ -31,7 +31,7 @@ ADD_EXECUTABLE(hello_wolf_autoconf hello_wolf_autoconf.cpp)
#TARGET_LINK_LIBRARIES(hello_wolf_autoconf ${PROJECT_NAME})
TARGET_LINK_LIBRARIES(hello_wolf_autoconf ${PROJECT_NAME} hellowolf)
-#add_library(sensor_odom SHARED ../src/sensor/sensor_odom_2D.cpp ../src/processor/processor_odom_2D.cpp)
+#add_library(sensor_odom SHARED ../src/sensor/sensor_odom_2d.cpp ../src/processor/processor_odom_2d.cpp)
#TARGET_LINK_LIBRARIES(sensor_odom ${PROJECT_NAME} hellowolf)
#
#add_library(range_bearing SHARED sensor_range_bearing.cpp processor_range_bearing.cpp)
diff --git a/hello_wolf/capture_range_bearing.cpp b/hello_wolf/capture_range_bearing.cpp
index 81322d73d2e592b0bf07db7100b75f6167b46020..a2d44a101085ea0ad3aa4ae7d68af68370ea5024 100644
--- a/hello_wolf/capture_range_bearing.cpp
+++ b/hello_wolf/capture_range_bearing.cpp
@@ -1,5 +1,5 @@
/*
- * CaptureRangeBearing2D.cpp
+ * CaptureRangeBearing2d.cpp
*
* Created on: Nov 30, 2017
* Author: jsola
diff --git a/hello_wolf/feature_range_bearing.cpp b/hello_wolf/feature_range_bearing.cpp
index 40967163a3a5ab643cd18fc068f3c0ab55a2b1d9..4d47cc5539f5fcfe2cc011ea4c4e6e11d2685f90 100644
--- a/hello_wolf/feature_range_bearing.cpp
+++ b/hello_wolf/feature_range_bearing.cpp
@@ -1,5 +1,5 @@
/*
- * FeatureRangeBearing2D.cpp
+ * FeatureRangeBearing2d.cpp
*
* Created on: Nov 30, 2017
* Author: jsola
diff --git a/hello_wolf/feature_range_bearing.h b/hello_wolf/feature_range_bearing.h
index 6a8dada9cd8bb9f53f8a4386a687c4655c8d14bf..8571a8fb933976c740440c9d4d9ba8a2fb3d8f20 100644
--- a/hello_wolf/feature_range_bearing.h
+++ b/hello_wolf/feature_range_bearing.h
@@ -1,5 +1,5 @@
/*
- * FeatureRangeBearing2D.h
+ * FeatureRangeBearing2d.h
*
* Created on: Nov 30, 2017
* Author: jsola
diff --git a/hello_wolf/hello_wolf.cpp b/hello_wolf/hello_wolf.cpp
index b1531f45173b200d2c10bb448996ec4c87095263..ac33da83c3fece5dce946e9c808869edd511e6f4 100644
--- a/hello_wolf/hello_wolf.cpp
+++ b/hello_wolf/hello_wolf.cpp
@@ -15,9 +15,9 @@
// wolf core includes
#include "core/common/wolf.h"
-#include "core/sensor/sensor_odom_2D.h"
-#include "core/processor/processor_odom_2D.h"
-#include "core/capture/capture_odom_2D.h"
+#include "core/sensor/sensor_odom_2d.h"
+#include "core/processor/processor_odom_2d.h"
+#include "core/capture/capture_odom_2d.h"
#include "core/ceres_wrapper/ceres_manager.h"
// hello wolf local includes
@@ -110,12 +110,12 @@ int main()
options.max_num_iterations = 1000; // We depart far from solution, need a lot of iterations
CeresManagerPtr ceres = std::make_shared<CeresManager>(problem, options);
- // sensor odometer 2D
- ParamsSensorOdom2DPtr intrinsics_odo = std::make_shared<ParamsSensorOdom2D>();
- SensorBasePtr sensor_odo = problem->installSensor("SensorOdom2D", "sensor odo", Vector3d(0,0,0), intrinsics_odo);
+ // sensor odometer 2d
+ ParamsSensorOdom2dPtr intrinsics_odo = std::make_shared<ParamsSensorOdom2d>();
+ SensorBasePtr sensor_odo = problem->installSensor("SensorOdom2d", "sensor odo", Vector3d(0,0,0), intrinsics_odo);
- // processor odometer 2D
- ProcessorParamsOdom2DPtr params_odo = std::make_shared<ProcessorParamsOdom2D>();
+ // processor odometer 2d
+ ProcessorParamsOdom2dPtr params_odo = std::make_shared<ProcessorParamsOdom2d>();
params_odo->voting_active = true;
params_odo->time_tolerance = 0.1;
params_odo->max_time_span = 999;
@@ -123,7 +123,7 @@ int main()
params_odo->angle_turned = 999;
params_odo->cov_det = 999;
params_odo->unmeasured_perturbation_std = 0.001;
- ProcessorBasePtr processor = problem->installProcessor("ProcessorOdom2D", "processor odo", sensor_odo, params_odo);
+ ProcessorBasePtr processor = problem->installProcessor("ProcessorOdom2d", "processor odo", sensor_odo, params_odo);
// sensor Range and Bearing
ParamsSensorRangeBearingPtr intrinsics_rb = std::make_shared<ParamsSensorRangeBearing>();
@@ -183,7 +183,7 @@ int main()
t += 1.0; // t : 1.0
// motion
- CaptureOdom2DPtr cap_motion = std::make_shared<CaptureOdom2D>(t, sensor_odo, motion_data, motion_cov);
+ CaptureOdom2dPtr cap_motion = std::make_shared<CaptureOdom2d>(t, sensor_odo, motion_data, motion_cov);
sensor_odo ->process(cap_motion); // KF2 : (1,0,0)
// observe lmks
@@ -267,7 +267,7 @@ int main()
*
* IF YOU SEE at the end of the printed problem the estimate for Lmk 3 as:
*
- * L3 POINT 2D <-- c8
+ * L3 POINT 2d <-- c8
* Est, x = ( 3 2 )
* sb: Est
*
@@ -289,10 +289,10 @@ int main()
*
* P: wolf tree status ---------------------
Hardware
- S1 ODOM 2D // Sensor 1, type ODOMETRY 2D.
+ S1 ODOM 2d // Sensor 1, type ODOMETRY 2d.
Extr [Sta] = [ Fix( 0 0 ) Fix( 0 ) ] // Static extrinsics, fixed position, fixed orientation (See notes 1 and 2 below)
Intr [Sta] = [ ] // Static intrinsics, but no intrinsics anyway
- pm1 ODOM 2D // Processor 1, type ODOMETRY 2D
+ pm1 ODOM 2d // Processor 1, type ODOMETRY 2d
o: C7 - F3 // origin at Capture 7, Frame 3
l: C10 - F4 // last at Capture 10, frame 4
S2 RANGE BEARING // Sensor 2, type RANGE and BEARING.
@@ -307,7 +307,7 @@ int main()
f1 FIX <-- // Feature 1, type Fix
m = ( 0 0 0 ) // The absolute measurement for this frame is (0,0,0) --> origin
c1 FIX --> A // Factor 1, type FIX, it is Absolute
- CM2 ODOM 2D -> S1 [Sta, Sta] <-- // Capture 2, type ODOM, from Sensor 1 (static extr and intr)
+ CM2 ODOM 2d -> S1 [Sta, Sta] <-- // Capture 2, type ODOM, from Sensor 1 (static extr and intr)
C5 RANGE BEARING -> S2 [Sta, Sta] <-- // Capture 5, type R+B, from Sensor 2 (static extr and intr)
f2 RANGE BEARING <-- // Feature 2, type R+B
m = ( 1 1.57 ) // The feature's measurement is 1m, 1.57rad
@@ -315,10 +315,10 @@ int main()
KF2 <-- c6
Est, ts=1, x = ( 1 2.5e-10 1.6e-10 )
sb: Est Est
- CM3 ODOM 2D -> S1 [Sta, Sta] <--
- f3 ODOM 2D <--
+ CM3 ODOM 2d -> S1 [Sta, Sta] <--
+ f3 ODOM 2d <--
m = ( 1 0 0 )
- c3 ODOM 2D --> F1 // Factor 3, type ODOM, against Frame 1
+ c3 ODOM 2d --> F1 // Factor 3, type ODOM, against Frame 1
C9 RANGE BEARING -> S2 [Sta, Sta] <--
f4 RANGE BEARING <--
m = ( 1.41 2.36 )
@@ -329,10 +329,10 @@ int main()
KF3 <--
Est, ts=2, x = ( 2 4.1e-10 1.7e-10 )
sb: Est Est
- CM7 ODOM 2D -> S1 [Sta, Sta] <--
- f6 ODOM 2D <--
+ CM7 ODOM 2d -> S1 [Sta, Sta] <--
+ f6 ODOM 2d <--
m = ( 1 0 0 )
- c6 ODOM 2D --> F2
+ c6 ODOM 2d --> F2
C12 RANGE BEARING -> S2 [Sta, Sta] <--
f7 RANGE BEARING <--
m = ( 1.41 2.36 )
@@ -343,15 +343,15 @@ int main()
F4 <--
Est, ts=2, x = ( 0.11 -0.045 0.26 )
sb: Est Est
- CM10 ODOM 2D -> S1 [Sta, Sta] <--
+ CM10 ODOM 2d -> S1 [Sta, Sta] <--
Map
- L1 POINT 2D <-- c2 c4 // Landmark 1, constrained by Factors 2 and 4
+ L1 POINT 2d <-- c2 c4 // Landmark 1, constrained by Factors 2 and 4
Est, x = ( 1 2 ) // L4 state is estimated, state vector
sb: Est // L4 has 1 state block estimated
- L2 POINT 2D <-- c5 c7
+ L2 POINT 2d <-- c5 c7
Est, x = ( 2 2 )
sb: Est
- L3 POINT 2D <-- c8
+ L3 POINT 2d <-- c8
Est, x = ( 3 2 )
sb: Est
-----------------------------------------
diff --git a/hello_wolf/hello_wolf_autoconf.cpp b/hello_wolf/hello_wolf_autoconf.cpp
index 62c62402e1134e059f688f9fbf961a7e3fd1b5c6..a387adfe9656523c37d025bb3a74f0f198281b3d 100644
--- a/hello_wolf/hello_wolf_autoconf.cpp
+++ b/hello_wolf/hello_wolf_autoconf.cpp
@@ -8,8 +8,8 @@
// wolf core includes
#include "core/common/wolf.h"
-#include "core/capture/capture_odom_2D.h"
-#include "core/yaml/parser_yaml.hpp"
+#include "core/capture/capture_odom_2d.h"
+#include "core/yaml/parser_yaml.h"
#include "core/ceres_wrapper/ceres_manager.h"
// hello wolf local includes
@@ -163,7 +163,7 @@ int main()
t += 1.0; // t : 1.0
// motion
- CaptureOdom2DPtr cap_motion = std::make_shared<CaptureOdom2D>(t, sensor_odo, motion_data, motion_cov);
+ CaptureOdom2dPtr cap_motion = std::make_shared<CaptureOdom2d>(t, sensor_odo, motion_data, motion_cov);
cap_motion ->process(); // KF2 : (1,0,0)
// observe lmks
@@ -250,7 +250,7 @@ int main()
*
* IF YOU SEE at the end of the printed problem the estimate for Lmk 3 as:
*
- * L3 POINT 2D <-- c8
+ * L3 POINT 2d <-- c8
* Est, x = ( 3 2 )
* sb: Est
*
@@ -272,10 +272,10 @@ int main()
*
* P: wolf tree status ---------------------
Hardware
- S1 ODOM 2D // Sensor 1, type ODOMETRY 2D.
+ S1 ODOM 2d // Sensor 1, type ODOMETRY 2d.
Extr [Sta] = [ Fix( 0 0 ) Fix( 0 ) ] // Static extrinsics, fixed position, fixed orientation (See notes 1 and 2 below)
Intr [Sta] = [ ] // Static intrinsics, but no intrinsics anyway
- pm1 ODOM 2D // Processor 1, type ODOMETRY 2D
+ pm1 ODOM 2d // Processor 1, type ODOMETRY 2d
o: C7 - F3 // origin at Capture 7, Frame 3
l: C10 - F4 // last at Capture 10, frame 4
S2 RANGE BEARING // Sensor 2, type RANGE and BEARING.
@@ -290,7 +290,7 @@ int main()
f1 FIX <-- // Feature 1, type Fix
m = ( 0 0 0 ) // The absolute measurement for this frame is (0,0,0) --> origin
c1 FIX --> A // Factor 1, type FIX, it is Absolute
- CM2 ODOM 2D -> S1 [Sta, Sta] <-- // Capture 2, type ODOM, from Sensor 1 (static extr and intr)
+ CM2 ODOM 2d -> S1 [Sta, Sta] <-- // Capture 2, type ODOM, from Sensor 1 (static extr and intr)
C5 RANGE BEARING -> S2 [Sta, Sta] <-- // Capture 5, type R+B, from Sensor 2 (static extr and intr)
f2 RANGE BEARING <-- // Feature 2, type R+B
m = ( 1 1.57 ) // The feature's measurement is 1m, 1.57rad
@@ -298,10 +298,10 @@ int main()
KF2 <-- c6
Est, ts=1, x = ( 1 2.5e-10 1.6e-10 )
sb: Est Est
- CM3 ODOM 2D -> S1 [Sta, Sta] <--
- f3 ODOM 2D <--
+ CM3 ODOM 2d -> S1 [Sta, Sta] <--
+ f3 ODOM 2d <--
m = ( 1 0 0 )
- c3 ODOM 2D --> F1 // Factor 3, type ODOM, against Frame 1
+ c3 ODOM 2d --> F1 // Factor 3, type ODOM, against Frame 1
C9 RANGE BEARING -> S2 [Sta, Sta] <--
f4 RANGE BEARING <--
m = ( 1.41 2.36 )
@@ -312,10 +312,10 @@ int main()
KF3 <--
Est, ts=2, x = ( 2 4.1e-10 1.7e-10 )
sb: Est Est
- CM7 ODOM 2D -> S1 [Sta, Sta] <--
- f6 ODOM 2D <--
+ CM7 ODOM 2d -> S1 [Sta, Sta] <--
+ f6 ODOM 2d <--
m = ( 1 0 0 )
- c6 ODOM 2D --> F2
+ c6 ODOM 2d --> F2
C12 RANGE BEARING -> S2 [Sta, Sta] <--
f7 RANGE BEARING <--
m = ( 1.41 2.36 )
@@ -326,15 +326,15 @@ int main()
F4 <--
Est, ts=2, x = ( 0.11 -0.045 0.26 )
sb: Est Est
- CM10 ODOM 2D -> S1 [Sta, Sta] <--
+ CM10 ODOM 2d -> S1 [Sta, Sta] <--
Map
- L1 POINT 2D <-- c2 c4 // Landmark 1, constrained by Factors 2 and 4
+ L1 POINT 2d <-- c2 c4 // Landmark 1, constrained by Factors 2 and 4
Est, x = ( 1 2 ) // L4 state is estimated, state vector
sb: Est // L4 has 1 state block estimated
- L2 POINT 2D <-- c5 c7
+ L2 POINT 2d <-- c5 c7
Est, x = ( 2 2 )
sb: Est
- L3 POINT 2D <-- c8
+ L3 POINT 2d <-- c8
Est, x = ( 3 2 )
sb: Est
-----------------------------------------
diff --git a/hello_wolf/landmark_point_2D.cpp b/hello_wolf/landmark_point_2D.cpp
deleted file mode 100644
index 2e74de9065542dde024989342087fa8dbdcee659..0000000000000000000000000000000000000000
--- a/hello_wolf/landmark_point_2D.cpp
+++ /dev/null
@@ -1,24 +0,0 @@
-/**
- * \file landmark_point_2D.cpp
- *
- * Created on: Dec 4, 2017
- * \author: jsola
- */
-
-#include "landmark_point_2D.h"
-
-namespace wolf
-{
-
-LandmarkPoint2D::LandmarkPoint2D(int _id, const Eigen::Vector2d& _xy) :
- LandmarkBase("POINT 2D", std::make_shared<StateBlock>(_xy))
-{
- setId(_id);
-}
-
-LandmarkPoint2D::~LandmarkPoint2D()
-{
- //
-}
-
-} /* namespace wolf */
diff --git a/hello_wolf/landmark_point_2D.h b/hello_wolf/landmark_point_2D.h
deleted file mode 100644
index 371db93ee80e991b2981307a97b6a6a54d466491..0000000000000000000000000000000000000000
--- a/hello_wolf/landmark_point_2D.h
+++ /dev/null
@@ -1,27 +0,0 @@
-/**
- * \file landmark_point_2D.h
- *
- * Created on: Dec 4, 2017
- * \author: jsola
- */
-
-#ifndef HELLO_WOLF_LANDMARK_POINT_2D_H_
-#define HELLO_WOLF_LANDMARK_POINT_2D_H_
-
-#include "core/landmark/landmark_base.h"
-
-namespace wolf
-{
-
-WOLF_PTR_TYPEDEFS(LandmarkPoint2D);
-
-class LandmarkPoint2D : public LandmarkBase
-{
- public:
- LandmarkPoint2D(int _id, const Eigen::Vector2d& _xy);
- virtual ~LandmarkPoint2D();
-};
-
-} /* namespace wolf */
-
-#endif /* HELLO_WOLF_LANDMARK_POINT_2D_H_ */
diff --git a/hello_wolf/landmark_point_2d.cpp b/hello_wolf/landmark_point_2d.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..835c4fb04e4f7b1e5695958ca9db3cbb810c5ec5
--- /dev/null
+++ b/hello_wolf/landmark_point_2d.cpp
@@ -0,0 +1,24 @@
+/**
+ * \file landmark_point_2d.cpp
+ *
+ * Created on: Dec 4, 2017
+ * \author: jsola
+ */
+
+#include "landmark_point_2d.h"
+
+namespace wolf
+{
+
+LandmarkPoint2d::LandmarkPoint2d(int _id, const Eigen::Vector2d& _xy) :
+ LandmarkBase("POINT 2d", std::make_shared<StateBlock>(_xy))
+{
+ setId(_id);
+}
+
+LandmarkPoint2d::~LandmarkPoint2d()
+{
+ //
+}
+
+} /* namespace wolf */
diff --git a/hello_wolf/landmark_point_2d.h b/hello_wolf/landmark_point_2d.h
new file mode 100644
index 0000000000000000000000000000000000000000..0ec660c9e70944a8de9dd152649dc78d8a85b86b
--- /dev/null
+++ b/hello_wolf/landmark_point_2d.h
@@ -0,0 +1,27 @@
+/**
+ * \file landmark_point_2d.h
+ *
+ * Created on: Dec 4, 2017
+ * \author: jsola
+ */
+
+#ifndef HELLO_WOLF_LANDMARK_POINT_2d_H_
+#define HELLO_WOLF_LANDMARK_POINT_2d_H_
+
+#include "core/landmark/landmark_base.h"
+
+namespace wolf
+{
+
+WOLF_PTR_TYPEDEFS(LandmarkPoint2d);
+
+class LandmarkPoint2d : public LandmarkBase
+{
+ public:
+ LandmarkPoint2d(int _id, const Eigen::Vector2d& _xy);
+ virtual ~LandmarkPoint2d();
+};
+
+} /* namespace wolf */
+
+#endif /* HELLO_WOLF_LANDMARK_POINT_2d_H_ */
diff --git a/hello_wolf/processor_range_bearing.cpp b/hello_wolf/processor_range_bearing.cpp
index 0ce5d610105a6984ed4a0beb4474f06a35d2ba67..89b3d7775e3c5b79dca8de4a0d2f6f5ad91d17e7 100644
--- a/hello_wolf/processor_range_bearing.cpp
+++ b/hello_wolf/processor_range_bearing.cpp
@@ -7,7 +7,7 @@
#include "processor_range_bearing.h"
#include "capture_range_bearing.h"
-#include "landmark_point_2D.h"
+#include "landmark_point_2d.h"
#include "feature_range_bearing.h"
#include "factor_range_bearing.h"
@@ -15,7 +15,7 @@ namespace wolf
{
ProcessorRangeBearing::ProcessorRangeBearing(ProcessorParamsBasePtr _params) :
- ProcessorBase("ProcessorRangeBearing", _params)
+ ProcessorBase("ProcessorRangeBearing", 2, _params)
{
//
}
@@ -63,19 +63,19 @@ void ProcessorRangeBearing::processCapture(CaptureBasePtr _capture)
double bearing = capture_rb->getBearing(i);
// 4. create or recover landmark
- LandmarkPoint2DPtr lmk;
+ LandmarkPoint2dPtr lmk;
auto lmk_it = known_lmks.find(id);
if ( lmk_it != known_lmks.end() )
{
// known landmarks : recover landmark
- lmk = std::static_pointer_cast<LandmarkPoint2D>(lmk_it->second);
+ lmk = std::static_pointer_cast<LandmarkPoint2d>(lmk_it->second);
WOLF_TRACE("known lmk(", id, "): ", lmk->getP()->getState().transpose());
}
else
{
// new landmark:
// - create landmark
- lmk = LandmarkBase::emplace<LandmarkPoint2D>(getProblem()->getMap(), id, invObserve(range, bearing));
+ lmk = LandmarkBase::emplace<LandmarkPoint2d>(getProblem()->getMap(), id, invObserve(range, bearing));
WOLF_TRACE("new lmk(", id, "): ", lmk->getP()->getState().transpose());
// - add to known landmarks
known_lmks.emplace(id, lmk);
diff --git a/hello_wolf/sensor_range_bearing.cpp b/hello_wolf/sensor_range_bearing.cpp
index f2348928a25a2bc8835ea61f3bad7c1139424d7c..3eea032b2ba632cf3343805446a98d0805e56f3c 100644
--- a/hello_wolf/sensor_range_bearing.cpp
+++ b/hello_wolf/sensor_range_bearing.cpp
@@ -20,7 +20,7 @@ SensorRangeBearing::SensorRangeBearing(const Eigen::VectorXd& _extrinsics, const
nullptr,
_noise_std)
{
- assert(_extrinsics.size() == 3 && "Bad extrinsics vector size. Must be 3 for 2D");
+ assert(_extrinsics.size() == 3 && "Bad extrinsics vector size. Must be 3 for 2d");
}
SensorRangeBearing::SensorRangeBearing(const Eigen::VectorXd& _extrinsics, const ParamsSensorRangeBearingPtr _intr) :
diff --git a/hello_wolf/sensor_range_bearing.h b/hello_wolf/sensor_range_bearing.h
index e62bfc1a5cd4ff7115cac06f9738204c71017f2b..491efff4f533edc993051482de7d0e84562a790a 100644
--- a/hello_wolf/sensor_range_bearing.h
+++ b/hello_wolf/sensor_range_bearing.h
@@ -9,7 +9,7 @@
#define HELLO_WOLF_SENSOR_RANGE_BEARING_H_
#include "core/sensor/sensor_base.h"
-#include "core/utils/params_server.hpp"
+#include "core/utils/params_server.h"
namespace wolf
{
diff --git a/hello_wolf/yaml/hello_wolf_config.yaml b/hello_wolf/yaml/hello_wolf_config.yaml
index 167dea95d3c6d49f7eca2f8e1a682c7473d415c2..b84d7776622264f96a4597ade146d64bcec86438 100644
--- a/hello_wolf/yaml/hello_wolf_config.yaml
+++ b/hello_wolf/yaml/hello_wolf_config.yaml
@@ -3,7 +3,7 @@ config:
problem:
frame_structure: "PO" # keyframes have position and orientation
- dimension: 2 # space is 2D
+ dimension: 2 # space is 2d
prior:
timestamp: 0.0
state: [0,0,0]
@@ -12,12 +12,12 @@ config:
sensors:
- - type: "SensorOdom2D"
+ - type: "SensorOdom2d"
plugin: "core"
name: "sen odom"
extrinsic:
pose: [0,0, 0]
- follow: "hello_wolf/yaml/sensor_odom_2D.yaml" # config parameters in this file
+ follow: "hello_wolf/yaml/sensor_odom_2d.yaml" # config parameters in this file
- type: "SensorRangeBearing"
plugin: "core"
@@ -29,19 +29,14 @@ config:
processors:
- - type: "ProcessorOdom2D"
+ - type: "ProcessorOdom2d"
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: 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
-
-
-
-
-
\ No newline at end of file
diff --git a/hello_wolf/yaml/processor_odom_2D.yaml b/hello_wolf/yaml/processor_odom_2d.yaml
similarity index 87%
rename from hello_wolf/yaml/processor_odom_2D.yaml
rename to hello_wolf/yaml/processor_odom_2d.yaml
index b14f39bbe25ae1f6a2a2785417f5f1fe3c9fc08d..bee82ff9ad905a72dc6c1ffd46ac6d7267ae8870 100644
--- a/hello_wolf/yaml/processor_odom_2D.yaml
+++ b/hello_wolf/yaml/processor_odom_2d.yaml
@@ -1,4 +1,4 @@
-type: "ProcessorOdom2D"
+type: "ProcessorOdom2d"
time_tolerance: 0.1
unmeasured_perturbation_std: 0.001
diff --git a/hello_wolf/yaml/sensor_odom_2D.yaml b/hello_wolf/yaml/sensor_odom_2d.yaml
similarity index 65%
rename from hello_wolf/yaml/sensor_odom_2D.yaml
rename to hello_wolf/yaml/sensor_odom_2d.yaml
index 4755665d896210c733a26ac5ab210d107c5ed528..3ad7204855cae5c1e8e00cfdc011ff27d2725692 100644
--- a/hello_wolf/yaml/sensor_odom_2D.yaml
+++ b/hello_wolf/yaml/sensor_odom_2d.yaml
@@ -1,4 +1,4 @@
-type: "SensorOdom2D" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+type: "SensorOdom2d" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
k_disp_to_disp: 0.1 # m^2 / m
k_rot_to_rot: 0.1 # rad^2 / rad
diff --git a/include/core/capture/capture_base.h b/include/core/capture/capture_base.h
index 0a52b61933bc8a0daf48cf201d085da54c1ce306..99dc9a25ec2875115901eabf2cfe9b39fc42890d 100644
--- a/include/core/capture/capture_base.h
+++ b/include/core/capture/capture_base.h
@@ -79,6 +79,8 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s
public:
unsigned int getHits() const;
const FactorBasePtrList& getConstrainedByList() const;
+ bool isConstrainedBy(const FactorBasePtr &_factor) const;
+
// State blocks
const StateStructure& getStructure() const;
@@ -183,6 +185,7 @@ inline const FactorBasePtrList& CaptureBase::getConstrainedByList() const
return constrained_by_list_;
}
+
inline TimeStamp CaptureBase::getTimeStamp() const
{
return time_stamp_;
diff --git a/include/core/capture/capture_odom_2D.h b/include/core/capture/capture_odom_2d.h
similarity index 69%
rename from include/core/capture/capture_odom_2D.h
rename to include/core/capture/capture_odom_2d.h
index ac242ee1ec64c81c82787b9a383f7ffa7d7444d8..d18217279601157c9a1b2eb3c4e7f4ba4dae6cbb 100644
--- a/include/core/capture/capture_odom_2D.h
+++ b/include/core/capture/capture_odom_2d.h
@@ -1,12 +1,12 @@
/*
- * capture_odom_2D.h
+ * capture_odom_2d.h
*
* Created on: Oct 16, 2017
* Author: jsola
*/
-#ifndef CAPTURE_ODOM_2D_H_
-#define CAPTURE_ODOM_2D_H_
+#ifndef CAPTURE_ODOM_2d_H_
+#define CAPTURE_ODOM_2d_H_
#include "core/capture/capture_motion.h"
@@ -15,29 +15,29 @@
namespace wolf
{
-WOLF_PTR_TYPEDEFS(CaptureOdom2D);
+WOLF_PTR_TYPEDEFS(CaptureOdom2d);
-class CaptureOdom2D : public CaptureMotion
+class CaptureOdom2d : public CaptureMotion
{
public:
- CaptureOdom2D(const TimeStamp& _init_ts,
+ CaptureOdom2d(const TimeStamp& _init_ts,
SensorBasePtr _sensor_ptr,
const Eigen::VectorXd& _data,
CaptureBasePtr _capture_origin_ptr = nullptr);
- CaptureOdom2D(const TimeStamp& _init_ts,
+ CaptureOdom2d(const TimeStamp& _init_ts,
SensorBasePtr _sensor_ptr,
const Eigen::VectorXd& _data,
const Eigen::MatrixXd& _data_cov,
CaptureBasePtr _capture_origin_ptr = nullptr);
- virtual ~CaptureOdom2D();
+ virtual ~CaptureOdom2d();
virtual VectorXd correctDelta(const VectorXd& _delta, const VectorXd& _delta_error) const override;
};
-inline Eigen::VectorXd CaptureOdom2D::correctDelta(const VectorXd& _delta, const VectorXd& _delta_error) const
+inline Eigen::VectorXd CaptureOdom2d::correctDelta(const VectorXd& _delta, const VectorXd& _delta_error) const
{
Vector3d delta = _delta + _delta_error;
delta(2) = pi2pi(delta(2));
@@ -46,4 +46,4 @@ inline Eigen::VectorXd CaptureOdom2D::correctDelta(const VectorXd& _delta, const
} /* namespace wolf */
-#endif /* CAPTURE_ODOM_2D_H_ */
+#endif /* CAPTURE_ODOM_2d_H_ */
diff --git a/include/core/capture/capture_odom_3D.h b/include/core/capture/capture_odom_3d.h
similarity index 69%
rename from include/core/capture/capture_odom_3D.h
rename to include/core/capture/capture_odom_3d.h
index c9990f38513c63809d7cb396790ba388ef0d5383..7e4ee02c8b694cc6ae2b8e8b6d9e6803d7587c65 100644
--- a/include/core/capture/capture_odom_3D.h
+++ b/include/core/capture/capture_odom_3d.h
@@ -1,12 +1,12 @@
/*
- * capture_odom_3D.h
+ * capture_odom_3d.h
*
* Created on: Oct 16, 2017
* Author: jsola
*/
-#ifndef CAPTURE_ODOM_3D_H_
-#define CAPTURE_ODOM_3D_H_
+#ifndef CAPTURE_ODOM_3d_H_
+#define CAPTURE_ODOM_3d_H_
#include "core/capture/capture_motion.h"
@@ -15,23 +15,23 @@
namespace wolf
{
-WOLF_PTR_TYPEDEFS(CaptureOdom3D);
+WOLF_PTR_TYPEDEFS(CaptureOdom3d);
-class CaptureOdom3D : public CaptureMotion
+class CaptureOdom3d : public CaptureMotion
{
public:
- CaptureOdom3D(const TimeStamp& _init_ts,
+ CaptureOdom3d(const TimeStamp& _init_ts,
SensorBasePtr _sensor_ptr,
const Eigen::VectorXd& _data,
CaptureBasePtr _capture_origin_ptr = nullptr);
- CaptureOdom3D(const TimeStamp& _init_ts,
+ CaptureOdom3d(const TimeStamp& _init_ts,
SensorBasePtr _sensor_ptr,
const Eigen::VectorXd& _data,
const Eigen::MatrixXd& _data_cov,
CaptureBasePtr _capture_origin_ptr = nullptr);
- virtual ~CaptureOdom3D();
+ virtual ~CaptureOdom3d();
virtual VectorXd correctDelta(const VectorXd& _delta, const VectorXd& _delta_error) const override;
@@ -39,4 +39,4 @@ class CaptureOdom3D : public CaptureMotion
} /* namespace wolf */
-#endif /* CAPTURE_ODOM_3D_H_ */
+#endif /* CAPTURE_ODOM_3d_H_ */
diff --git a/include/core/capture/capture_pose.h b/include/core/capture/capture_pose.h
index 4073cb1e2681b4024029146d83372961085db646..56ce0ea48c5aadd9d3b189353bae75b83088d792 100644
--- a/include/core/capture/capture_pose.h
+++ b/include/core/capture/capture_pose.h
@@ -3,8 +3,8 @@
//Wolf includes
#include "core/capture/capture_base.h"
-#include "core/factor/factor_pose_2D.h"
-#include "core/factor/factor_pose_3D.h"
+#include "core/factor/factor_pose_2d.h"
+#include "core/factor/factor_pose_3d.h"
#include "core/feature/feature_pose.h"
//std includes
diff --git a/include/core/ceres_wrapper/ceres_manager.h b/include/core/ceres_wrapper/ceres_manager.h
index f124325950a06f74642cff38b6d0c053958cab8e..802ab7b49dabf93455f8f93fc8c30745415fe33e 100644
--- a/include/core/ceres_wrapper/ceres_manager.h
+++ b/include/core/ceres_wrapper/ceres_manager.h
@@ -8,7 +8,7 @@
//wolf includes
#include "core/solver/solver_manager.h"
-#include "core/utils/params_server.hpp"
+#include "core/utils/params_server.h"
#include "core/ceres_wrapper/cost_function_wrapper.h"
#include "core/ceres_wrapper/local_parametrization_wrapper.h"
#include "core/ceres_wrapper/create_numeric_diff_cost_function.h"
@@ -84,6 +84,8 @@ class CeresManager : public SolverManager
void check();
+ const Eigen::SparseMatrixd computeHessian() const;
+
protected:
std::string solveImpl(const ReportVerbosity report_level) override;
diff --git a/include/core/ceres_wrapper/create_numeric_diff_cost_function.h b/include/core/ceres_wrapper/create_numeric_diff_cost_function.h
index ff60fea02729c791b569c0585f47e5da34bfca19..203c38ef635cf02d6ded7aa0a0491169ef67a04d 100644
--- a/include/core/ceres_wrapper/create_numeric_diff_cost_function.h
+++ b/include/core/ceres_wrapper/create_numeric_diff_cost_function.h
@@ -12,7 +12,7 @@
#include "ceres/numeric_diff_cost_function.h"
// Factors
-#include "core/factor/factor_odom_2D.h"
+#include "core/factor/factor_odom_2d.h"
#include "core/factor/factor_base.h"
namespace wolf {
@@ -33,8 +33,8 @@ inline std::shared_ptr<ceres::CostFunction> createNumericDiffCostFunction(Factor
// switch (_fac_ptr->getTypeId())
// {
// // just for testing
-// case FAC_ODOM_2D:
-// return createNumericDiffCostFunctionCeres<FactorOdom2D>(_fac_ptr);
+// case FAC_ODOM_2d:
+// return createNumericDiffCostFunctionCeres<FactorOdom2d>(_fac_ptr);
//
// /* For adding a new factor, add the #include and a case:
// case FAC_ENUM:
diff --git a/include/core/common/factory.h b/include/core/common/factory.h
index 88820c8b382fed793ba0cdc0db73a60eafcd44ca..f03cabec9a7527801464d9e4e025464a7d23f0ad 100644
--- a/include/core/common/factory.h
+++ b/include/core/common/factory.h
@@ -57,7 +57,7 @@ namespace wolf
*
* > For example, if you want to make a Landmark factory, set TypeBase = LandmarkBase.\n
* > Then, the factory will create specific landmarks deriving from LandmarkBase.\n
- * > The specific type of landmark (e.g. LandmarkCorner2D, LandmarkAHP, LandmarkPolyline2D, etc) is indicated by a string that we call TYPE in this documentation.
+ * > The specific type of landmark (e.g. LandmarkCorner2d, LandmarkAHP, LandmarkPolyline2d, etc) is indicated by a string that we call TYPE in this documentation.
*
* Specific object creation is invoked by the method ````create(TYPE, params ... )````, where
* - the TYPE of object to create is identified with a string
@@ -72,14 +72,14 @@ namespace wolf
* - Write object creators
* - Register and unregister object creators to the factory
* - Create objects using the factory
- * - Examples: Write and register a landmark creator for LandmarkPolyline2D.
+ * - Examples: Write and register a landmark creator for LandmarkPolyline2d.
*
* #### Define correct TYPE names
* The rule to make new TYPE strings unique is that you skip the generic 'Type' prefix from your class name,
* and you build a string in CAPITALS with space separators, e.g.:
* - ParamsSensorCamera -> ````"CAMERA"````
- * - ParamsSensorLaser2D -> ````"LASER 2D"````
- * - LandmarkPolyline2D -> ````"POLYLINE 2D"````
+ * - ParamsSensorLaser2d -> ````"LASER 2d"````
+ * - LandmarkPolyline2d -> ````"POLYLINE 2d"````
* - etc.
*
* #### Access the factory
@@ -107,7 +107,7 @@ namespace wolf
* \endcode
*
* #### Write creator methods (in your derived object classes)
- * The method LandmarkPolyline2D::create(...) exists in the LandmarkPolyline2D class as a static method.
+ * The method LandmarkPolyline2d::create(...) exists in the LandmarkPolyline2d class as a static method.
* All these ````XxxXxx::create()```` methods need to have exactly the same API, regardless of the object type.
* The API puts into play the two template parameters:
*
@@ -137,7 +137,7 @@ namespace wolf
* that knows how to create your specific object, e.g.:
*
* \code
- * LandmarkFactory::get().registerCreator("POLYLINE 2D", LandmarkPolyline2D::create);
+ * LandmarkFactory::get().registerCreator("POLYLINE 2d", LandmarkPolyline2d::create);
* \endcode
*
* #### Automatic registration
@@ -163,26 +163,26 @@ namespace wolf
* Note: Prior to invoking the creation of a object of a particular type,
* you must register the creator for this type into the factory.
*
- * To create e.g. a LandmarkPolyline2D from a YAML node you type:
+ * To create e.g. a LandmarkPolyline2d from a YAML node you type:
*
* \code
- * LandmarkBasePtr lmk_ptr = Factory<LandmarkBasePtr, YAML::Node>::get().create("POLYLINE 2D", lmk_yaml_node);
+ * LandmarkBasePtr lmk_ptr = Factory<LandmarkBasePtr, YAML::Node>::get().create("POLYLINE 2d", lmk_yaml_node);
* \endcode
*
* or even better, make use of the convenient typedefs:
*
* \code
- * LandmarkBasePtr lmk_ptr = LandmarkFactory::get().create("POLYLINE 2D", lmk_yaml_node);
+ * LandmarkBasePtr lmk_ptr = LandmarkFactory::get().create("POLYLINE 2d", lmk_yaml_node);
* \endcode
*
* ### Examples
- * #### Example 1: Writing the creator of LandmarkPolyline2D from a YAML node
+ * #### Example 1: Writing the creator of LandmarkPolyline2d from a YAML node
*
- * You can find this code in the landmark_polyline_2D.cpp file.
+ * You can find this code in the landmark_polyline_2d.cpp file.
*
* \code
* // Creator (this method is static):
- * LandmarkBasePtr LandmarkPolyline2D::create(const YAML::Node& _lmk_node)
+ * LandmarkBasePtr LandmarkPolyline2d::create(const YAML::Node& _lmk_node)
* {
* // Parse YAML node with lmk info and data
* unsigned int id = _lmk_node["id"].as<unsigned int>();
@@ -197,22 +197,22 @@ namespace wolf
* }
*
* // Create a new landmark
- * LandmarkBasePtr lmk_ptr = new LandmarkPolyline2D(points, first_defined, last_defined, first_id);
+ * LandmarkBasePtr lmk_ptr = new LandmarkPolyline2d(points, first_defined, last_defined, first_id);
* lmk_ptr->setId(id);
*
* return lmk_ptr;
* }
* \endcode
*
- * #### Example 2: Registering the creator of LandmarkPolyline2D from a YAML node
+ * #### Example 2: Registering the creator of LandmarkPolyline2d from a YAML node
*
- * You can find this code in the landmark_polyline_2D.cpp file.
+ * You can find this code in the landmark_polyline_2d.cpp file.
*
* \code
* // Register landmark creator (put the register code inside an unnamed namespace):
* namespace
* {
- * const bool registered_lmk_polyline_2D = LandmarkFactory::get().registerCreator("POLYLINE 2D", LandmarkPolyline2D::create);
+ * const bool registered_lmk_polyline_2d = LandmarkFactory::get().registerCreator("POLYLINE 2d", LandmarkPolyline2d::create);
* }
*
* \endcode
diff --git a/include/core/common/node_base.h b/include/core/common/node_base.h
index b771859e5114fa372cce9d48428d36549202b230..4488ab30ec274eeb84e49b7dcd600de9a93acf5e 100644
--- a/include/core/common/node_base.h
+++ b/include/core/common/node_base.h
@@ -28,9 +28,9 @@ namespace wolf {
*
* - A unique type, which is a subcategory of the above. The list here cannot be exhaustive, but a few examples follow:
* - "SesorCamera"
- * - "SesorLaser2D"
- * - "LandmarkPoint3D"
- * - "ProcessorLaser2D"
+ * - "SesorLaser2d"
+ * - "LandmarkPoint3d"
+ * - "ProcessorLaser2d"
*
* please refer to each base class derived from NodeLinked for better examples of their types.
*
@@ -64,7 +64,7 @@ class NodeBase
unsigned int node_id_; ///< Node id. It is unique over the whole Wolf Tree
std::string node_category_; ///< Text label identifying the category of node ("SENSOR", "FEATURE", etc)
- std::string node_type_; ///< Text label identifying the type or subcategory of node ("Pin Hole", "Point 2D", etc)
+ std::string node_type_; ///< Text label identifying the type or subcategory of node ("Pin Hole", "Point 2d", etc)
std::string node_name_; ///< Text label identifying each specific object ("left camera", "LIDAR 1", "PointGrey", "Andrew", etc)
bool is_removing_; ///< A flag for safely removing nodes from the Wolf tree. See remove().
diff --git a/include/core/common/params_base.h b/include/core/common/params_base.h
index 5295bc0ac17f941503a8f28e462962290a9ee82b..80f6516415e28ddce47ab272415604014ffb0a66 100644
--- a/include/core/common/params_base.h
+++ b/include/core/common/params_base.h
@@ -1,7 +1,7 @@
#ifndef PARAMS_BASE_H_
#define PARAMS_BASE_H_
-#include "core/utils/params_server.hpp"
+#include "core/utils/params_server.h"
namespace wolf {
struct ParamsBase
diff --git a/include/core/common/wolf.h b/include/core/common/wolf.h
index b06c4e7daca81209c5302b7998a188a144b9716e..d4976b53ee5060fedaee9432f8243187fe71db6e 100644
--- a/include/core/common/wolf.h
+++ b/include/core/common/wolf.h
@@ -200,7 +200,11 @@ struct MatrixSizeCheck
typedef std::list<ClassName##Ptr> ClassName##PtrList; \
typedef ClassName##PtrList::iterator ClassName##Iter; \
typedef ClassName##PtrList::const_iterator ClassName##ConstIter; \
- typedef ClassName##PtrList::reverse_iterator ClassName##RevIter;
+ typedef ClassName##PtrList::reverse_iterator ClassName##RevIter; \
+ typedef std::list<ClassName##WPtr> ClassName##WPtrList; \
+ typedef ClassName##WPtrList::iterator ClassName##WIter; \
+ typedef ClassName##WPtrList::const_iterator ClassName##WConstIter; \
+ typedef ClassName##WPtrList::reverse_iterator ClassName##WRevIter;
#define WOLF_STRUCT_PTR_TYPEDEFS(StructName) \
struct StructName; \
@@ -213,6 +217,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_distance_3D.h b/include/core/factor/factor_autodiff_distance_3d.h
similarity index 81%
rename from include/core/factor/factor_autodiff_distance_3D.h
rename to include/core/factor/factor_autodiff_distance_3d.h
index 624112000fe6f019e6d86fc9e310e4da9a4a2b3f..8dc182d6f92ec60d7ba310ed1e7f94e27a9281fc 100644
--- a/include/core/factor/factor_autodiff_distance_3D.h
+++ b/include/core/factor/factor_autodiff_distance_3d.h
@@ -1,29 +1,29 @@
/**
- * \file factor_autodiff_distance_3D.h
+ * \file factor_autodiff_distance_3d.h
*
* Created on: Apr 10, 2018
* \author: jsola
*/
-#ifndef FACTOR_AUTODIFF_DISTANCE_3D_H_
-#define FACTOR_AUTODIFF_DISTANCE_3D_H_
+#ifndef FACTOR_AUTODIFF_DISTANCE_3d_H_
+#define FACTOR_AUTODIFF_DISTANCE_3d_H_
#include "core/factor/factor_autodiff.h"
namespace wolf
{
-WOLF_PTR_TYPEDEFS(FactorAutodiffDistance3D);
+WOLF_PTR_TYPEDEFS(FactorAutodiffDistance3d);
-class FactorAutodiffDistance3D : public FactorAutodiff<FactorAutodiffDistance3D, 1, 3, 3>
+class FactorAutodiffDistance3d : public FactorAutodiff<FactorAutodiffDistance3d, 1, 3, 3>
{
public:
- FactorAutodiffDistance3D(const FeatureBasePtr& _feat,
+ FactorAutodiffDistance3d(const FeatureBasePtr& _feat,
const FrameBasePtr& _frm_other,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status) :
- FactorAutodiff("DISTANCE 3D",
+ FactorAutodiff("DISTANCE 3d",
_frm_other,
nullptr,
nullptr,
@@ -36,7 +36,7 @@ class FactorAutodiffDistance3D : public FactorAutodiff<FactorAutodiffDistance3D,
{
}
- virtual ~FactorAutodiffDistance3D() { /* nothing */ }
+ virtual ~FactorAutodiffDistance3d() { /* nothing */ }
virtual std::string getTopology() const override
{
@@ -73,4 +73,4 @@ class FactorAutodiffDistance3D : public FactorAutodiff<FactorAutodiffDistance3D,
} /* namespace wolf */
-#endif /* FACTOR_AUTODIFF_DISTANCE_3D_H_ */
+#endif /* FACTOR_AUTODIFF_DISTANCE_3d_H_ */
diff --git a/include/core/factor/factor_base.h b/include/core/factor/factor_base.h
index 6f215396071b757b980faac31be8d0250716ea71..cd1439f96e437dc39dde137dd1014f37abee10db 100644
--- a/include/core/factor/factor_base.h
+++ b/include/core/factor/factor_base.h
@@ -40,19 +40,19 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
{
friend FeatureBase;
private:
- FeatureBaseWPtr feature_ptr_; ///< FeatureBase pointer (upper node)
+ FeatureBaseWPtr feature_ptr_; ///< FeatureBase pointer (upper node)
static unsigned int factor_id_count_;
protected:
unsigned int factor_id_;
- FactorStatus status_; ///< status of factor
- bool apply_loss_function_; ///< flag for applying loss function to this factor
- FrameBaseWPtr frame_other_ptr_; ///< FrameBase pointer
- CaptureBaseWPtr capture_other_ptr_; ///< CaptureBase pointer
- FeatureBaseWPtr feature_other_ptr_; ///< FeatureBase pointer
- LandmarkBaseWPtr landmark_other_ptr_; ///< LandmarkBase pointer
- ProcessorBaseWPtr processor_ptr_; ///< ProcessorBase pointer
+ FactorStatus status_; ///< status of factor
+ bool apply_loss_function_; ///< flag for applying loss function to this factor
+ FrameBaseWPtrList frame_other_list_; ///< FrameBase pointer list
+ CaptureBaseWPtrList capture_other_list_; ///< CaptureBase pointer list
+ FeatureBaseWPtrList feature_other_list_; ///< FeatureBase pointer list
+ LandmarkBaseWPtrList landmark_other_list_; ///< LandmarkBase pointer list
+ ProcessorBaseWPtr processor_ptr_; ///< ProcessorBase pointer list
virtual void setProblem(ProblemPtr) final;
public:
@@ -72,6 +72,17 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE);
+ FactorBase(const std::string& _tp,
+ const FrameBasePtrList& _frame_other_list,
+ const CaptureBasePtrList& _capture_other_list,
+ const FeatureBasePtrList& _feature_other_list,
+ const LandmarkBasePtrList& _landmark_other_list,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE);
+
+
+
virtual ~FactorBase() = default;
virtual void remove(bool viral_remove_empty_parent=false);
@@ -133,6 +144,14 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
**/
CaptureBasePtr getCapture() const;
+ /** \brief Returns a pointer to its frame
+ **/
+ FrameBasePtr getFrame() const;
+
+ /** \brief Returns a pointer to its capture's sensor
+ **/
+ SensorBasePtr getSensor() const;
+
/** \brief Returns the factor residual size
**/
virtual unsigned int getSize() const = 0;
@@ -149,21 +168,32 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
*/
bool getApplyLossFunction() const;
- /** \brief Returns a pointer to the frame constrained to
+ /** \brief Returns a pointer to the first frame constrained to
**/
- FrameBasePtr getFrameOther() const { return frame_other_ptr_.lock(); }
+ FrameBasePtr getFrameOther() const;
- /** \brief Returns a pointer to the capture constrained to
+ /** \brief Returns a pointer to the first capture constrained to
**/
- CaptureBasePtr getCaptureOther() const { return capture_other_ptr_.lock(); }
+ CaptureBasePtr getCaptureOther() const;
- /** \brief Returns a pointer to the feature constrained to
+ /** \brief Returns a pointer to the first feature constrained to
**/
- FeatureBasePtr getFeatureOther() const { return feature_other_ptr_.lock(); }
+ FeatureBasePtr getFeatureOther() const;
- /** \brief Returns a pointer to the landmark constrained to
+ /** \brief Returns a pointer to the first landmark constrained to
**/
- LandmarkBasePtr getLandmarkOther() const { return landmark_other_ptr_.lock(); }
+ LandmarkBasePtr getLandmarkOther() const;
+
+ // get pointer lists to other nodes
+ FrameBaseWPtrList getFrameOtherList() const { return frame_other_list_; }
+ CaptureBaseWPtrList getCaptureOtherList() const { return capture_other_list_; }
+ FeatureBaseWPtrList getFeatureOtherList() const { return feature_other_list_; }
+ LandmarkBaseWPtrList getLandmarkOtherList() const { return landmark_other_list_; }
+
+ bool hasFrameOther(const FrameBasePtr& _frm_other) const;
+ bool hasCaptureOther(const CaptureBasePtr& _cap_other) const;
+ bool hasFeatureOther(const FeatureBasePtr& _ftr_other) const;
+ bool hasLandmarkOther(const LandmarkBasePtr& _lmk_other) const;
public:
/**
@@ -228,6 +258,39 @@ inline bool FactorBase::getApplyLossFunction() const
return apply_loss_function_;
}
+inline FrameBasePtr FactorBase::getFrameOther() const
+{
+ if (frame_other_list_.empty()) return nullptr;
+ if (frame_other_list_.front().expired()) return nullptr;
+
+ return frame_other_list_.front().lock();
+}
+
+inline CaptureBasePtr FactorBase::getCaptureOther() const
+{
+ if (capture_other_list_.empty()) return nullptr;
+ if (capture_other_list_.front().expired()) return nullptr;
+
+ return capture_other_list_.front().lock();
+}
+
+inline FeatureBasePtr FactorBase::getFeatureOther() const
+{
+ if (feature_other_list_.empty()) return nullptr;
+ if (feature_other_list_.front().expired()) return nullptr;
+
+ return feature_other_list_.front().lock();
+}
+
+inline LandmarkBasePtr FactorBase::getLandmarkOther() const
+{
+ if (landmark_other_list_.empty()) return nullptr;
+ if (landmark_other_list_.front().expired()) return nullptr;
+
+ return landmark_other_list_.front().lock();
+}
+
+
inline ProcessorBasePtr FactorBase::getProcessor() const
{
return processor_ptr_.lock();
diff --git a/include/core/factor/factor_block_absolute.h b/include/core/factor/factor_block_absolute.h
index eb55faae757f7fe1b02d2fe29c75a049e0b04299..0743548b7b4aad0517ecdd2590955c4cdf7c023c 100644
--- a/include/core/factor/factor_block_absolute.h
+++ b/include/core/factor/factor_block_absolute.h
@@ -41,7 +41,7 @@ class FactorBlockAbsolute : public FactorAnalytic
ProcessorBasePtr _processor_ptr = nullptr,
bool _apply_loss_function = false,
FactorStatus _status = FAC_ACTIVE) :
- FactorAnalytic("BLOCK ABS",
+ FactorAnalytic("FactorBlockAbsolute",
nullptr,
nullptr,
nullptr,
diff --git a/include/core/factor/factor_block_difference.h b/include/core/factor/factor_block_difference.h
index 8f62b63838d79c6a654c80120bff4e4a144f4098..d3558ef21aa6e6577fc54d26015db3f9e3193312 100644
--- a/include/core/factor/factor_block_difference.h
+++ b/include/core/factor/factor_block_difference.h
@@ -37,8 +37,12 @@ class FactorBlockDifference : public FactorAnalytic
*
*/
FactorBlockDifference(
- StateBlockPtr _sb1_ptr,
- StateBlockPtr _sb2_ptr,
+ const StateBlockPtr& _sb1_ptr,
+ const StateBlockPtr& _sb2_ptr,
+ const FrameBasePtr& _frame_other = nullptr,
+ const CaptureBasePtr& _cap_other = nullptr,
+ const FeatureBasePtr& _feat_other = nullptr,
+ const LandmarkBasePtr& _lmk_other = nullptr,
unsigned int _start_idx1 = 0,
int _size1 = -1,
unsigned int _start_idx2 = 0,
@@ -46,11 +50,11 @@ class FactorBlockDifference : public FactorAnalytic
ProcessorBasePtr _processor_ptr = nullptr,
bool _apply_loss_function = false,
FactorStatus _status = FAC_ACTIVE) :
- FactorAnalytic("BLOCK ABS",
- nullptr,
- nullptr,
- nullptr,
- nullptr,
+ FactorAnalytic("FactorBlockDifference",
+ _frame_other,
+ _cap_other,
+ _feat_other,
+ _lmk_other,
_processor_ptr,
_apply_loss_function,
_status,
@@ -146,10 +150,16 @@ inline void FactorBlockDifference::evaluateJacobians(const std::vector<Eigen::Ma
assert(_st_vector[1].size() == sb2_size_ && "Wrong StateBlock size");
assert(_st_vector[0].size() >= getMeasurement().size() && "StateBlock size and measurement size should match");
assert(_st_vector[1].size() >= getMeasurement().size() && "StateBlock size and measurement size should match");
-
- // normalized jacobian
- _jacobians[0] = getMeasurementSquareRootInformationUpper() * J_res_sb1_;
- _jacobians[1] = getMeasurementSquareRootInformationUpper() * J_res_sb2_;
+ assert(getMeasurementSquareRootInformationUpper().cols() == J_res_sb1_.rows() && "Wrong jacobian sb1 or covariance size");
+ assert(getMeasurementSquareRootInformationUpper().cols() == J_res_sb2_.rows() && "Wrong jacobian sb2 or covariance size");
+
+ // normalized jacobian, computed according to the _compute_jacobian flag
+ if (_compute_jacobian[0]){
+ _jacobians[0] = getMeasurementSquareRootInformationUpper() * J_res_sb1_;
+ }
+ if (_compute_jacobian[1]){
+ _jacobians[1] = getMeasurementSquareRootInformationUpper() * J_res_sb2_;
+ }
}
inline void FactorBlockDifference::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
@@ -163,10 +173,16 @@ inline void FactorBlockDifference::evaluateJacobians(const std::vector<Eigen::Ma
assert(_st_vector[1].size() == sb2_size_ && "Wrong StateBlock size");
assert(_st_vector[0].size() >= getMeasurement().size() && "StateBlock size and measurement size should match");
assert(_st_vector[1].size() >= getMeasurement().size() && "StateBlock size and measurement size should match");
-
- // normalized jacobian
- _jacobians[0] = getMeasurementSquareRootInformationUpper() * J_res_sb1_;
- _jacobians[1] = getMeasurementSquareRootInformationUpper() * J_res_sb2_;
+ assert(getMeasurementSquareRootInformationUpper().cols() == J_res_sb1_.rows() && "Wrong jacobian sb1 or covariance size");
+ assert(getMeasurementSquareRootInformationUpper().cols() == J_res_sb2_.rows() && "Wrong jacobian sb2 or covariance size");
+
+ // normalized jacobian, computed according to the _compute_jacobian flag
+ if (_compute_jacobian[0]){
+ _jacobians[0] = getMeasurementSquareRootInformationUpper() * J_res_sb1_;
+ }
+ if (_compute_jacobian[1]){
+ _jacobians[1] = getMeasurementSquareRootInformationUpper() * J_res_sb2_;
+ }
}
inline void FactorBlockDifference::evaluatePureJacobians(std::vector<Eigen::MatrixXd>& _pure_jacobians) const
diff --git a/include/core/factor/factor_diff_drive.h b/include/core/factor/factor_diff_drive.h
index 2338f699de73d38c74ce13509cdc0b70a1a82130..d936a99d6c1d64f29fda2c5aeca1806e0065c4d1 100644
--- a/include/core/factor/factor_diff_drive.h
+++ b/include/core/factor/factor_diff_drive.h
@@ -140,10 +140,12 @@ inline bool FactorDiffDrive::operator ()(const T* const _p1, const T* const _o1,
inline Eigen::VectorXd FactorDiffDrive::residual()
{
VectorXd residual(3);
- operator ()(getFrameOther()->getP()->getState().data(), getFrameOther()->getO()->getState().data(),
- getCapture()->getFrame()->getP()->getState().data(),
- getCapture()->getFrame()->getO()->getState().data(),
- getCaptureOther()->getSensorIntrinsic()->getState().data(), residual.data());
+ operator ()(getFrameOther() ->getP() ->getState() .data(),
+ getFrameOther() ->getO() ->getState() .data(),
+ getFrame() ->getP() ->getState() .data(),
+ getFrame() ->getO() ->getState() .data(),
+ getCaptureOther() ->getSensorIntrinsic() ->getState() .data(),
+ residual.data());
return residual;
}
diff --git a/include/core/factor/factor_odom_2D.h b/include/core/factor/factor_odom_2d.h
similarity index 83%
rename from include/core/factor/factor_odom_2D.h
rename to include/core/factor/factor_odom_2d.h
index 64d09368e97d0f5e394672de4218d7e608c49445..e20f0f50f158b7ed04e2d09054fafee9300a6f29 100644
--- a/include/core/factor/factor_odom_2D.h
+++ b/include/core/factor/factor_odom_2d.h
@@ -1,5 +1,5 @@
-#ifndef FACTOR_ODOM_2D_THETA_H_
-#define FACTOR_ODOM_2D_THETA_H_
+#ifndef FACTOR_ODOM_2d_THETA_H_
+#define FACTOR_ODOM_2d_THETA_H_
//Wolf includes
#include "core/factor/factor_autodiff.h"
@@ -9,18 +9,18 @@
namespace wolf {
-WOLF_PTR_TYPEDEFS(FactorOdom2D);
+WOLF_PTR_TYPEDEFS(FactorOdom2d);
//class
-class FactorOdom2D : public FactorAutodiff<FactorOdom2D, 3, 2, 1, 2, 1>
+class FactorOdom2d : public FactorAutodiff<FactorOdom2d, 3, 2, 1, 2, 1>
{
public:
- FactorOdom2D(const FeatureBasePtr& _ftr_ptr,
+ FactorOdom2d(const FeatureBasePtr& _ftr_ptr,
const FrameBasePtr& _frame_other_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE) :
- FactorAutodiff<FactorOdom2D, 3, 2, 1, 2, 1>("FactorOdom2D",
+ FactorAutodiff<FactorOdom2d, 3, 2, 1, 2, 1>("FactorOdom2d",
_frame_other_ptr, nullptr, nullptr, nullptr,
_processor_ptr,
_apply_loss_function, _status,
@@ -32,7 +32,7 @@ class FactorOdom2D : public FactorAutodiff<FactorOdom2D, 3, 2, 1, 2, 1>
//
}
- virtual ~FactorOdom2D() = default;
+ virtual ~FactorOdom2d() = default;
virtual std::string getTopology() const override
{
@@ -47,13 +47,13 @@ class FactorOdom2D : public FactorAutodiff<FactorOdom2D, 3, 2, 1, 2, 1>
// public:
// static FactorBasePtr create(const FeatureBasePtr& _feature_ptr, const NodeBasePtr& _correspondant_ptr, const ProcessorBasePtr& _processor_ptr)
// {
-// return std::make_shared<FactorOdom2D>(_feature_ptr, std::static_pointer_cast<FrameBase>(_correspondant_ptr), _processor_ptr);
+// return std::make_shared<FactorOdom2d>(_feature_ptr, std::static_pointer_cast<FrameBase>(_correspondant_ptr), _processor_ptr);
// }
};
template<typename T>
-inline bool FactorOdom2D::operator ()(const T* const _p1, const T* const _o1, const T* const _p2,
+inline bool FactorOdom2d::operator ()(const T* const _p1, const T* const _o1, const T* const _p2,
const T* const _o2, T* _residuals) const
{
@@ -92,9 +92,9 @@ inline bool FactorOdom2D::operator ()(const T* const _p1, const T* const _o1, co
// J.row(2) = ((Jet<double, 6>)(res(2))).v;
// if (sizeof(er(0)) != sizeof(double))
// {
-// std::cout << "FactorOdom2D::Jacobian(c" << id() << ") = \n " << J << std::endl;
-// std::cout << "FactorOdom2D::Weighted Jacobian(c" << id() << ") = \n " << J << std::endl;
-// std::cout << "FactorOdom2D::Sqrt Info(c" << id() << ") = \n" << getMeasurementSquareRootInformationUpper() << std::endl;
+// std::cout << "FactorOdom2d::Jacobian(c" << id() << ") = \n " << J << std::endl;
+// std::cout << "FactorOdom2d::Weighted Jacobian(c" << id() << ") = \n " << J << std::endl;
+// std::cout << "FactorOdom2d::Sqrt Info(c" << id() << ") = \n" << getMeasurementSquareRootInformationUpper() << std::endl;
// }
////////////////////////////////////////////////////////
diff --git a/include/core/factor/factor_odom_2D_analytic.h b/include/core/factor/factor_odom_2d_analytic.h
similarity index 72%
rename from include/core/factor/factor_odom_2D_analytic.h
rename to include/core/factor/factor_odom_2d_analytic.h
index 96b3688fd01dada34da94ac21f7bef178c2e5d42..c7eec32a1778b9f1ad69da6c3c0087e50509a904 100644
--- a/include/core/factor/factor_odom_2D_analytic.h
+++ b/include/core/factor/factor_odom_2d_analytic.h
@@ -1,24 +1,24 @@
-#ifndef FACTOR_ODOM_2D_ANALYTIC_H_
-#define FACTOR_ODOM_2D_ANALYTIC_H_
+#ifndef FACTOR_ODOM_2d_ANALYTIC_H_
+#define FACTOR_ODOM_2d_ANALYTIC_H_
//Wolf includes
-#include "core/factor/factor_relative_2D_analytic.h"
+#include "core/factor/factor_relative_2d_analytic.h"
#include <Eigen/StdVector>
namespace wolf {
-WOLF_PTR_TYPEDEFS(FactorOdom2DAnalytic);
+WOLF_PTR_TYPEDEFS(FactorOdom2dAnalytic);
//class
-class FactorOdom2DAnalytic : public FactorRelative2DAnalytic
+class FactorOdom2dAnalytic : public FactorRelative2dAnalytic
{
public:
- FactorOdom2DAnalytic(const FeatureBasePtr& _ftr_ptr,
+ FactorOdom2dAnalytic(const FeatureBasePtr& _ftr_ptr,
const FrameBasePtr& _frame_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE) :
- FactorRelative2DAnalytic("ODOM_2D",
+ FactorRelative2dAnalytic("ODOM_2d",
_ftr_ptr,
_frame_ptr,
_processor_ptr,
@@ -28,7 +28,7 @@ class FactorOdom2DAnalytic : public FactorRelative2DAnalytic
//
}
- virtual ~FactorOdom2DAnalytic() = default;
+ virtual ~FactorOdom2dAnalytic() = default;
virtual std::string getTopology() const override
{
@@ -40,7 +40,7 @@ class FactorOdom2DAnalytic : public FactorRelative2DAnalytic
// const NodeBasePtr& _correspondant_ptr,
// const ProcessorBasePtr& _processor_ptr = nullptr)
// {
-// return std::make_shared<FactorOdom2DAnalytic>(_feature_ptr,
+// return std::make_shared<FactorOdom2dAnalytic>(_feature_ptr,
// std::static_pointer_cast<FrameBase>(_correspondant_ptr), _processor_ptr);
// }
diff --git a/include/core/factor/factor_odom_2D_closeloop.h b/include/core/factor/factor_odom_2d_closeloop.h
similarity index 82%
rename from include/core/factor/factor_odom_2D_closeloop.h
rename to include/core/factor/factor_odom_2d_closeloop.h
index 8a016afab4bbcdeb03adce41267c0ad14f6bc620..e7d35aefa48b549174d060668cbee540edb6e9e2 100644
--- a/include/core/factor/factor_odom_2D_closeloop.h
+++ b/include/core/factor/factor_odom_2d_closeloop.h
@@ -1,5 +1,5 @@
-#ifndef FACTOR_ODOM_2D_CLOSELOOP_H_
-#define FACTOR_ODOM_2D_CLOSELOOP_H_
+#ifndef FACTOR_ODOM_2d_CLOSELOOP_H_
+#define FACTOR_ODOM_2d_CLOSELOOP_H_
//Wolf includes
#include "core/factor/factor_autodiff.h"
@@ -9,17 +9,17 @@
namespace wolf {
-WOLF_PTR_TYPEDEFS(FactorOdom2DCloseloop);
+WOLF_PTR_TYPEDEFS(FactorOdom2dCloseloop);
//class
-class FactorOdom2DCloseloop : public FactorAutodiff<FactorOdom2DCloseloop, 3, 2, 1, 2, 1>
+class FactorOdom2dCloseloop : public FactorAutodiff<FactorOdom2dCloseloop, 3, 2, 1, 2, 1>
{
public:
- FactorOdom2DCloseloop(const FeatureBasePtr& _ftr_ptr,
+ FactorOdom2dCloseloop(const FeatureBasePtr& _ftr_ptr,
const FrameBasePtr& _frame_other_ptr,
const ProcessorBasePtr& _processor_ptr = nullptr,
bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
- FactorAutodiff<FactorOdom2DCloseloop, 3, 2, 1, 2, 1>("FactorOdom2DCloseloop",
+ FactorAutodiff<FactorOdom2dCloseloop, 3, 2, 1, 2, 1>("FactorOdom2dCloseloop",
_frame_other_ptr, nullptr, nullptr, nullptr,
_processor_ptr,
_apply_loss_function, _status,
@@ -31,7 +31,7 @@ class FactorOdom2DCloseloop : public FactorAutodiff<FactorOdom2DCloseloop, 3, 2,
//
}
- virtual ~FactorOdom2DCloseloop() = default;
+ virtual ~FactorOdom2dCloseloop() = default;
virtual std::string getTopology() const override
{
@@ -46,13 +46,13 @@ class FactorOdom2DCloseloop : public FactorAutodiff<FactorOdom2DCloseloop, 3, 2,
public:
static FactorBasePtr create(const FeatureBasePtr& _feature_ptr, const NodeBasePtr& _correspondant_ptr, const ProcessorBasePtr& _processor_ptr)
{
- return std::make_shared<FactorOdom2DCloseloop>(_feature_ptr, std::static_pointer_cast<FrameBase>(_correspondant_ptr), _processor_ptr);
+ return std::make_shared<FactorOdom2dCloseloop>(_feature_ptr, std::static_pointer_cast<FrameBase>(_correspondant_ptr), _processor_ptr);
}
};
template<typename T>
-inline bool FactorOdom2DCloseloop::operator ()(const T* const _p1, const T* const _o1, const T* const _p2,
+inline bool FactorOdom2dCloseloop::operator ()(const T* const _p1, const T* const _o1, const T* const _p2,
const T* const _o2, T* _residuals) const
{
@@ -91,9 +91,9 @@ inline bool FactorOdom2DCloseloop::operator ()(const T* const _p1, const T* cons
// J.row(2) = ((Jet<double, 6>)(res(2))).v;
// if (sizeof(er(0)) != sizeof(double))
// {
-// std::cout << "FactorOdom2DCloseloop::Jacobian(c" << id() << ") = \n " << J << std::endl;
-// std::cout << "FactorOdom2DCloseloop::Weighted Jacobian(c" << id() << ") = \n " << J << std::endl;
-// std::cout << "FactorOdom2DCloseloop::Sqrt Info(c" << id() << ") = \n" << getMeasurementSquareRootInformationUpper() << std::endl;
+// std::cout << "FactorOdom2dCloseloop::Jacobian(c" << id() << ") = \n " << J << std::endl;
+// std::cout << "FactorOdom2dCloseloop::Weighted Jacobian(c" << id() << ") = \n " << J << std::endl;
+// std::cout << "FactorOdom2dCloseloop::Sqrt Info(c" << id() << ") = \n" << getMeasurementSquareRootInformationUpper() << std::endl;
// }
////////////////////////////////////////////////////////
diff --git a/include/core/factor/factor_odom_3D.h b/include/core/factor/factor_odom_3d.h
similarity index 90%
rename from include/core/factor/factor_odom_3D.h
rename to include/core/factor/factor_odom_3d.h
index 1b53915402fe5adb6a1f1481fefc06add006c291..2e49daf216906b4e96d92e9c195cbff579fd6545 100644
--- a/include/core/factor/factor_odom_3D.h
+++ b/include/core/factor/factor_odom_3d.h
@@ -1,12 +1,12 @@
/*
- * factor_odom_3D.h
+ * factor_odom_3d.h
*
* Created on: Oct 7, 2016
* Author: jsola
*/
-#ifndef FACTOR_ODOM_3D_H_
-#define FACTOR_ODOM_3D_H_
+#ifndef FACTOR_ODOM_3d_H_
+#define FACTOR_ODOM_3d_H_
#include "core/factor/factor_autodiff.h"
#include "core/math/rotations.h"
@@ -14,19 +14,19 @@
namespace wolf
{
-WOLF_PTR_TYPEDEFS(FactorOdom3D);
+WOLF_PTR_TYPEDEFS(FactorOdom3d);
//class
-class FactorOdom3D : public FactorAutodiff<FactorOdom3D,6,3,4,3,4>
+class FactorOdom3d : public FactorAutodiff<FactorOdom3d,6,3,4,3,4>
{
public:
- FactorOdom3D(const FeatureBasePtr& _ftr_current_ptr,
+ FactorOdom3d(const FeatureBasePtr& _ftr_current_ptr,
const FrameBasePtr& _frame_past_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE);
- virtual ~FactorOdom3D() = default;
+ virtual ~FactorOdom3d() = default;
virtual std::string getTopology() const override
{
@@ -58,7 +58,7 @@ class FactorOdom3D : public FactorAutodiff<FactorOdom3D,6,3,4,3,4>
};
template<typename T>
-inline void FactorOdom3D::printRes(const Eigen::Matrix<T, 6, 1>& r) const
+inline void FactorOdom3d::printRes(const Eigen::Matrix<T, 6, 1>& r) const
{
std::cout << "Jet residual = " << std::endl;
std::cout << r(0) << std::endl;
@@ -70,18 +70,18 @@ inline void FactorOdom3D::printRes(const Eigen::Matrix<T, 6, 1>& r) const
}
template<>
-inline void FactorOdom3D::printRes (const Eigen::Matrix<double,6,1> & r) const
+inline void FactorOdom3d::printRes (const Eigen::Matrix<double,6,1> & r) const
{
std::cout << "Scalar residual = " << std::endl;
std::cout << r.transpose() << std::endl;
}
-inline FactorOdom3D::FactorOdom3D(const FeatureBasePtr& _ftr_current_ptr,
+inline FactorOdom3d::FactorOdom3d(const FeatureBasePtr& _ftr_current_ptr,
const FrameBasePtr& _frame_past_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status) :
- FactorAutodiff<FactorOdom3D, 6, 3, 4, 3, 4>("FactorOdom3D", // type
+ FactorAutodiff<FactorOdom3d, 6, 3, 4, 3, 4>("FactorOdom3d", // type
_frame_past_ptr, // frame other
nullptr, // capture other
nullptr, // feature other
@@ -94,7 +94,7 @@ inline FactorOdom3D::FactorOdom3D(const FeatureBasePtr& _ftr_current_ptr,
_frame_past_ptr->getP(), // past frame P
_frame_past_ptr->getO()) // past frame Q
{
-// WOLF_TRACE("Constr ODOM 3D (f", _ftr_current_ptr->id(),
+// WOLF_TRACE("Constr ODOM 3d (f", _ftr_current_ptr->id(),
// " F", _ftr_current_ptr->getCapture()->getFrame()->id(),
// ") (Fo", _frame_past_ptr->id(), ")");
//
@@ -105,7 +105,7 @@ inline FactorOdom3D::FactorOdom3D(const FeatureBasePtr& _ftr_current_ptr,
}
template<typename T>
-inline bool FactorOdom3D::expectation(const T* const _p_current, const T* const _q_current, const T* const _p_past,
+inline bool FactorOdom3d::expectation(const T* const _p_current, const T* const _q_current, const T* const _p_past,
const T* const _q_past, T* _expectation_dp, T* _expectation_dq) const
{
Eigen::Map<const Eigen::Matrix<T,3,1> > p_current(_p_current);
@@ -145,7 +145,7 @@ inline bool FactorOdom3D::expectation(const T* const _p_current, const T* const
return true;
}
-inline Eigen::VectorXd FactorOdom3D::expectation() const
+inline Eigen::VectorXd FactorOdom3d::expectation() const
{
Eigen::VectorXd exp(7);
FrameBasePtr frm_current = getFeature()->getFrame();
@@ -168,7 +168,7 @@ inline Eigen::VectorXd FactorOdom3D::expectation() const
}
template<typename T>
-inline bool FactorOdom3D::operator ()(const T* const _p_current, const T* const _q_current, const T* const _p_past,
+inline bool FactorOdom3d::operator ()(const T* const _p_current, const T* const _q_current, const T* const _p_past,
const T* const _q_past, T* _residuals) const
{
@@ -235,4 +235,4 @@ inline bool FactorOdom3D::operator ()(const T* const _p_current, const T* const
} /* namespace wolf */
-#endif /* FACTOR_ODOM_3D_H_ */
+#endif /* FACTOR_ODOM_3d_H_ */
diff --git a/include/core/factor/factor_pose_2D.h b/include/core/factor/factor_pose_2d.h
similarity index 81%
rename from include/core/factor/factor_pose_2D.h
rename to include/core/factor/factor_pose_2d.h
index fece6f0bc06d0352319ceaa30e614200a6d254e9..ce63c806017117e67d18c2e79c34468e6b16a61c 100644
--- a/include/core/factor/factor_pose_2D.h
+++ b/include/core/factor/factor_pose_2d.h
@@ -1,6 +1,6 @@
-#ifndef FACTOR_POSE_2D_H_
-#define FACTOR_POSE_2D_H_
+#ifndef FACTOR_POSE_2d_H_
+#define FACTOR_POSE_2d_H_
//Wolf includes
#include "core/factor/factor_autodiff.h"
@@ -11,27 +11,27 @@
namespace wolf {
-WOLF_PTR_TYPEDEFS(FactorPose2D);
+WOLF_PTR_TYPEDEFS(FactorPose2d);
//class
-class FactorPose2D: public FactorAutodiff<FactorPose2D,3,2,1>
+class FactorPose2d: public FactorAutodiff<FactorPose2d,3,2,1>
{
public:
- FactorPose2D(FeatureBasePtr _ftr_ptr,
+ FactorPose2d(FeatureBasePtr _ftr_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE) :
- FactorAutodiff<FactorPose2D, 3, 2, 1>("FactorPose2D",
+ FactorAutodiff<FactorPose2d, 3, 2, 1>("FactorPose2d",
nullptr, nullptr, nullptr, nullptr,
_processor_ptr,
_apply_loss_function,
_status,
_ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO())
{
-// std::cout << "created FactorPose2D " << std::endl;
+// std::cout << "created FactorPose2d " << std::endl;
}
- virtual ~FactorPose2D() = default;
+ virtual ~FactorPose2d() = default;
virtual std::string getTopology() const override
{
@@ -44,7 +44,7 @@ class FactorPose2D: public FactorAutodiff<FactorPose2D,3,2,1>
};
template<typename T>
-inline bool FactorPose2D::operator ()(const T* const _p, const T* const _o, T* _residuals) const
+inline bool FactorPose2d::operator ()(const T* const _p, const T* const _o, T* _residuals) const
{
// measurement
// Eigen::Matrix<T,3,1> meas = getMeasurement().cast<T>();
@@ -76,8 +76,8 @@ inline bool FactorPose2D::operator ()(const T* const _p, const T* const _o, T* _
// J.row(2) = ((Jet<double, 3>)(res(2))).v;
// if (sizeof(er(0)) != sizeof(double))
// {
-// std::cout << "FactorPose2D::Jacobian(c" << id() << ") = \n " << J << std::endl;
-// std::cout << "FactorPose2D::Weighted Jacobian(c" << id() << ") = \n " << J << std::endl;
+// std::cout << "FactorPose2d::Jacobian(c" << id() << ") = \n " << J << std::endl;
+// std::cout << "FactorPose2d::Weighted Jacobian(c" << id() << ") = \n " << J << std::endl;
// std::cout << "Sqrt Info(c" << id() << ") = \n " << getMeasurementSquareRootInformationUpper() << std::endl;
// }
////////////////////////////////////////////////////////
diff --git a/include/core/factor/factor_pose_3D.h b/include/core/factor/factor_pose_3d.h
similarity index 82%
rename from include/core/factor/factor_pose_3D.h
rename to include/core/factor/factor_pose_3d.h
index c82a1788f6919fd66844e3790c616c6afd54b7b8..9a46cbab24101a69faaad78ea190f1823406dd35 100644
--- a/include/core/factor/factor_pose_3D.h
+++ b/include/core/factor/factor_pose_3d.h
@@ -1,6 +1,6 @@
-#ifndef FACTOR_POSE_3D_H_
-#define FACTOR_POSE_3D_H_
+#ifndef FACTOR_POSE_3d_H_
+#define FACTOR_POSE_3d_H_
//Wolf includes
#include "core/factor/factor_autodiff.h"
@@ -9,18 +9,18 @@
namespace wolf {
-WOLF_PTR_TYPEDEFS(FactorPose3D);
+WOLF_PTR_TYPEDEFS(FactorPose3d);
//class
-class FactorPose3D: public FactorAutodiff<FactorPose3D,6,3,4>
+class FactorPose3d: public FactorAutodiff<FactorPose3d,6,3,4>
{
public:
- FactorPose3D(FeatureBasePtr _ftr_ptr,
+ FactorPose3d(FeatureBasePtr _ftr_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE) :
- FactorAutodiff<FactorPose3D,6,3,4>("FactorPose3D",
+ FactorAutodiff<FactorPose3d,6,3,4>("FactorPose3d",
nullptr, nullptr, nullptr, nullptr,
_processor_ptr,
_apply_loss_function,
@@ -30,7 +30,7 @@ class FactorPose3D: public FactorAutodiff<FactorPose3D,6,3,4>
//
}
- virtual ~FactorPose3D() = default;
+ virtual ~FactorPose3d() = default;
virtual std::string getTopology() const override
{
@@ -43,7 +43,7 @@ class FactorPose3D: public FactorAutodiff<FactorPose3D,6,3,4>
};
template<typename T>
-inline bool FactorPose3D::operator ()(const T* const _p, const T* const _o, T* _residuals) const
+inline bool FactorPose3d::operator ()(const T* const _p, const T* const _o, T* _residuals) const
{
// states
diff --git a/include/core/factor/factor_relative_2D_analytic.h b/include/core/factor/factor_relative_2d_analytic.h
similarity index 94%
rename from include/core/factor/factor_relative_2D_analytic.h
rename to include/core/factor/factor_relative_2d_analytic.h
index be2b7f16a3d5ce01f4c15ea9f4dbd52d7848a956..5a67da1ff6c0e17614902ebc3195dd8124ec0ed6 100644
--- a/include/core/factor/factor_relative_2D_analytic.h
+++ b/include/core/factor/factor_relative_2d_analytic.h
@@ -1,5 +1,5 @@
-#ifndef FACTOR_RELATIVE_2D_ANALYTIC_H_
-#define FACTOR_RELATIVE_2D_ANALYTIC_H_
+#ifndef FACTOR_RELATIVE_2d_ANALYTIC_H_
+#define FACTOR_RELATIVE_2d_ANALYTIC_H_
//Wolf includes
#include "core/factor/factor_analytic.h"
@@ -8,16 +8,16 @@
namespace wolf {
-WOLF_PTR_TYPEDEFS(FactorRelative2DAnalytic);
+WOLF_PTR_TYPEDEFS(FactorRelative2dAnalytic);
//class
-class FactorRelative2DAnalytic : public FactorAnalytic
+class FactorRelative2dAnalytic : public FactorAnalytic
{
public:
/** \brief Constructor of category FAC_FRAME
**/
- FactorRelative2DAnalytic(const std::string& _tp,
+ FactorRelative2dAnalytic(const std::string& _tp,
const FeatureBasePtr& _ftr_ptr,
const FrameBasePtr& _frame_ptr,
const ProcessorBasePtr& _processor_ptr,
@@ -30,7 +30,7 @@ class FactorRelative2DAnalytic : public FactorAnalytic
/** \brief Constructor of category FAC_FEATURE
**/
- FactorRelative2DAnalytic(const std::string& _tp,
+ FactorRelative2dAnalytic(const std::string& _tp,
const FeatureBasePtr& _ftr_ptr,
const FeatureBasePtr& _ftr_other_ptr,
const ProcessorBasePtr& _processor_ptr,
@@ -43,7 +43,7 @@ class FactorRelative2DAnalytic : public FactorAnalytic
/** \brief Constructor of category FAC_LANDMARK
**/
- FactorRelative2DAnalytic(const std::string& _tp,
+ FactorRelative2dAnalytic(const std::string& _tp,
const FeatureBasePtr& _ftr_ptr,
const LandmarkBasePtr& _landmark_ptr,
const ProcessorBasePtr& _processor_ptr,
@@ -59,7 +59,7 @@ class FactorRelative2DAnalytic : public FactorAnalytic
return std::string("GEOM");
}
- virtual ~FactorRelative2DAnalytic() = default;
+ virtual ~FactorRelative2dAnalytic() = default;
/** \brief Returns the factor residual size
**/
@@ -102,7 +102,7 @@ class FactorRelative2DAnalytic : public FactorAnalytic
/// IMPLEMENTATION ///
-inline Eigen::VectorXd FactorRelative2DAnalytic::evaluateResiduals(
+inline Eigen::VectorXd FactorRelative2dAnalytic::evaluateResiduals(
const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector) const
{
Eigen::VectorXd residual(3);
@@ -121,7 +121,7 @@ inline Eigen::VectorXd FactorRelative2DAnalytic::evaluateResiduals(
return residual;
}
-inline void FactorRelative2DAnalytic::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector,
+inline void FactorRelative2dAnalytic::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector,
std::vector<Eigen::Map<Eigen::MatrixRowXd> >& jacobians,
const std::vector<bool>& _compute_jacobian) const
{
@@ -157,7 +157,7 @@ inline void FactorRelative2DAnalytic::evaluateJacobians(const std::vector<Eigen:
}
}
-inline void FactorRelative2DAnalytic::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
+inline void FactorRelative2dAnalytic::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
std::vector<Eigen::MatrixXd>& jacobians,
const std::vector<bool>& _compute_jacobian) const
{
@@ -193,7 +193,7 @@ inline void FactorRelative2DAnalytic::evaluateJacobians(const std::vector<Eigen:
}
}
-inline void FactorRelative2DAnalytic::evaluatePureJacobians(std::vector<Eigen::MatrixXd>& jacobians) const
+inline void FactorRelative2dAnalytic::evaluatePureJacobians(std::vector<Eigen::MatrixXd>& jacobians) const
{
assert(jacobians.size() == 4);
diff --git a/include/core/feature/feature_base.h b/include/core/feature/feature_base.h
index 7203425913de8bb5a3ff5a0fb267b4aef98f9fd5..a2ba3bc60f5d0db16d6c486099ac5e3e9b3116ec 100644
--- a/include/core/feature/feature_base.h
+++ b/include/core/feature/feature_base.h
@@ -95,6 +95,9 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
unsigned int getHits() const;
const FactorBasePtrList& getConstrainedByList() const;
+ bool isConstrainedBy(const FactorBasePtr &_factor) const;
+
+
// all factors
void getFactorList(FactorBasePtrList & _fac_list) const;
diff --git a/include/core/feature/feature_odom_2D.h b/include/core/feature/feature_odom_2d.h
similarity index 65%
rename from include/core/feature/feature_odom_2D.h
rename to include/core/feature/feature_odom_2d.h
index 3a50759802870400e19b3851959297295cc2aa55..9ef3059cd8afb11ea7cdf994b46b2f66ad344723 100644
--- a/include/core/feature/feature_odom_2D.h
+++ b/include/core/feature/feature_odom_2d.h
@@ -1,19 +1,19 @@
-#ifndef FEATURE_ODOM_2D_H_
-#define FEATURE_ODOM_2D_H_
+#ifndef FEATURE_ODOM_2d_H_
+#define FEATURE_ODOM_2d_H_
//Wolf includes
#include "core/feature/feature_base.h"
-#include "core/factor/factor_odom_2D.h"
-#include "core/factor/factor_odom_2D_analytic.h"
+#include "core/factor/factor_odom_2d.h"
+#include "core/factor/factor_odom_2d_analytic.h"
//std includes
namespace wolf {
-WOLF_PTR_TYPEDEFS(FeatureOdom2D);
+WOLF_PTR_TYPEDEFS(FeatureOdom2d);
-//class FeatureOdom2D
-class FeatureOdom2D : public FeatureBase
+//class FeatureOdom2d
+class FeatureOdom2d : public FeatureBase
{
public:
@@ -23,9 +23,9 @@ class FeatureOdom2D : public FeatureBase
* \param _meas_covariance the noise of the measurement
*
*/
- FeatureOdom2D(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance);
+ FeatureOdom2d(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance);
- virtual ~FeatureOdom2D();
+ virtual ~FeatureOdom2d();
/** \brief Generic interface to find factors
*
diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h
index 74c2b059a54c60bc1f5e74619d58f69e3a7775fd..0793828d6f6de6ab38b965c811e29387a9bc0cf9 100644
--- a/include/core/frame/frame_base.h
+++ b/include/core/frame/frame_base.h
@@ -96,8 +96,10 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
// State blocks ----------------------------------------------------
public:
+ using HasStateBlocks::addStateBlock;
+ StateBlockPtr addStateBlock(const std::string& _sb_type, const StateBlockPtr& _sb);
StateBlockPtr getV() const;
- void setV(const StateBlockPtr _v_ptr);
+
protected:
virtual void setProblem(ProblemPtr _problem) final;
@@ -124,6 +126,7 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
void getFactorList(FactorBasePtrList& _fac_list) const;
unsigned int getHits() const;
const FactorBasePtrList& getConstrainedByList() const;
+ bool isConstrainedBy(const FactorBasePtr& _factor) const;
void link(TrajectoryBasePtr);
template<typename classType, typename... T>
static std::shared_ptr<classType> emplace(TrajectoryBasePtr _ptr, T&&... all);
@@ -192,11 +195,6 @@ inline StateBlockPtr FrameBase::getV() const
return getStateBlock("V");
}
-inline void FrameBase::setV(const StateBlockPtr _v_ptr)
-{
- addStateBlock("V", _v_ptr);
-}
-
inline TrajectoryBasePtr FrameBase::getTrajectory() const
{
return trajectory_ptr_.lock();
@@ -217,6 +215,17 @@ inline const FactorBasePtrList& FrameBase::getConstrainedByList() const
return constrained_by_list_;
}
+inline StateBlockPtr FrameBase::addStateBlock(const std::string& _sb_type,
+ const StateBlockPtr& _sb)
+{
+ if (isKeyOrAux())
+ HasStateBlocks::addStateBlock(_sb_type, _sb, getProblem());
+ else
+ HasStateBlocks::addStateBlock(_sb_type, _sb, nullptr);
+
+ return _sb;
+}
+
inline void FrameBase::setTrajectory(TrajectoryBasePtr _trj_ptr)
{
trajectory_ptr_ = _trj_ptr;
diff --git a/include/core/landmark/landmark_base.h b/include/core/landmark/landmark_base.h
index 2f80a3d6fbe26d171126a82d280e65a3acaf4349..254e03e480f8471029c38c8f59626b230dc962f9 100644
--- a/include/core/landmark/landmark_base.h
+++ b/include/core/landmark/landmark_base.h
@@ -71,6 +71,8 @@ class LandmarkBase : public NodeBase, public HasStateBlocks, public std::enable_
unsigned int getHits() const;
const FactorBasePtrList& getConstrainedByList() const;
+ bool isConstrainedBy(const FactorBasePtr &_factor) const;
+
MapBasePtr getMap() const;
void link(MapBasePtr);
diff --git a/include/core/math/SE3.h b/include/core/math/SE3.h
index 9acfc971027378db119d4c96e78d94793614b631..701c5bab7a5ec5ecc6401def52ad9ca6d0f26856 100644
--- a/include/core/math/SE3.h
+++ b/include/core/math/SE3.h
@@ -12,9 +12,9 @@
#include "core/math/rotations.h"
/*
- * The functions in this file are related to manipulations of Delta motion magnitudes used in 3D motion.
+ * The functions in this file are related to manipulations of Delta motion magnitudes used in 3d motion.
*
- * The Delta is defined as a simple 3D pose with the rotation expressed in quaternion form,
+ * The Delta is defined as a simple 3d pose with the rotation expressed in quaternion form,
* Delta = [Dp, Dq]
* with
* Dp : position delta
diff --git a/include/core/math/rotations.h b/include/core/math/rotations.h
index 4196b9b35bba96995dddcd7156c6b3d47617f93a..7c2ede927a796adfd35e7dd9240e0128f1192c11 100644
--- a/include/core/math/rotations.h
+++ b/include/core/math/rotations.h
@@ -62,7 +62,7 @@ toDeg(const T rad)
/** \brief Skew symmetric matrix
*
- * @param _v a 3D vector
+ * @param _v a 3d vector
* @return the skew-symmetric matrix V so that V*u = _v.cross(u), for u in R^3
*/
template<typename Derived>
@@ -346,7 +346,7 @@ exp_R(const Eigen::MatrixBase<Derived>& _v)
/** \brief Rotation matrix logarithmic map
*
- * @param _R a 3D rotation matrix
+ * @param _R a 3d rotation matrix
* @return the rotation vector v such that _R = exp_R(v)
*/
template<typename Derived>
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 470d9fe07d56e973314cf5e18d35d40cd3007310..edda0b3b59fa839cdb6c5156abf61e1f2c338174 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -18,7 +18,7 @@ struct ProcessorParamsBase;
#include "core/common/wolf.h"
#include "core/frame/frame_base.h"
#include "core/state_block/state_block.h"
-#include "core/utils/params_server.hpp"
+#include "core/utils/params_server.h"
#include "core/sensor/sensor_factory.h"
#include "core/processor/processor_factory.h"
#include "core/processor/is_motion.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,13 +74,17 @@ 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;
/** \brief Factory method to install (create and add) sensors only from its properties
* \param _sen_type type of sensor
* \param _unique_sensor_name unique sensor name, used to identify the particular instance of the sensor
- * \param _extrinsics a vector of extrinsic parameters: size 2 for 2D position, 3 for 2D pose, 3 for 3D position, 7 for 3D pose.
+ * \param _extrinsics a vector of extrinsic parameters: size 2 for 2d position, 3 for 2d pose, 3 for 3d position, 7 for 3d pose.
* \param _intrinsics a base-pointer to a derived struct defining the intrinsic parameters.
*/
SensorBasePtr installSensor(const std::string& _sen_type, //
@@ -90,7 +95,7 @@ class Problem : public std::enable_shared_from_this<Problem>
/** \brief Factory method to install (create and add) sensors only from its properties -- Helper method loading parameters from file
* \param _sen_type type of sensor
* \param _unique_sensor_name unique sensor name, used to identify the particular instance of the sensor
- * \param _extrinsics a vector of extrinsic parameters: size 2 for 2D position, 3 for 2D pose, 3 for 3D position, 7 for 3D pose.
+ * \param _extrinsics a vector of extrinsic parameters: size 2 for 2d position, 3 for 2d pose, 3 for 3d position, 7 for 3d pose.
* \param _intrinsics_filename the name of a file containing the intrinsic parameters in a format compatible with the intrinsics creator registered in ParamsSensorFactory under the key _sen_type.
*/
SensorBasePtr installSensor(const std::string& _sen_type, //
@@ -270,6 +275,9 @@ class Problem : public std::enable_shared_from_this<Problem>
// Zero state provider
Eigen::VectorXd zeroState ( ) const;
bool priorIsSet() const;
+ void setPriorIsSet(bool _prior_is_set);
+ // Perturb state
+ void perturb(double amplitude = 0.01);
// Map branch -----------------------------------------
MapBasePtr getMap() const;
@@ -326,6 +334,11 @@ class Problem : public std::enable_shared_from_this<Problem>
public:
// Print and check ---------------------------------------
+ void print(int depth, //
+ std::ostream& stream ,
+ bool constr_by, //
+ bool metric, //
+ bool state_blocks) const;
/**
* \brief print wolf tree
* \param depth : levels to show ( 0: H, T, M : 1: H:S:p, T:F, M:L ; 2: T:F:C ; 3: T:F:C:f ; 4: T:F:C:f:c )
@@ -341,11 +354,8 @@ class Problem : public std::enable_shared_from_this<Problem>
bool constr_by = false, //
bool metric = true, //
bool state_blocks = false) const;
- std::string printToString(int depth = 4, //
- bool constr_by = false, //
- bool metric = true, //
- bool state_blocks = false) const;
bool check(int verbose_level = 0) const;
+ bool check(bool verbose, std::ostream& stream) const;
};
@@ -356,11 +366,21 @@ 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_;
}
+inline void Problem::setPriorIsSet(bool _prior_is_set)
+{
+ prior_is_set_ = _prior_is_set;
+}
+
inline IsMotionPtr Problem::getProcessorIsMotion()
{
if (!processor_is_motion_list_.empty())
diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h
index 8da9334e4106d9405d1d3c4f3a99ca4f5a9b1434..dc62abcac8375fc238f7c93617a33ccb8662d246 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.h
@@ -251,13 +251,14 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
ProcessorParamsBasePtr params_;
BufferPackKeyFrame buffer_pack_kf_;
BufferCapture buffer_capture_;
+ int dim_;
private:
SensorBaseWPtr sensor_ptr_;
static unsigned int processor_id_count_;
public:
- ProcessorBase(const std::string& _type, ProcessorParamsBasePtr _params);
+ ProcessorBase(const std::string& _type, int _dim, ProcessorParamsBasePtr _params);
virtual ~ProcessorBase();
virtual void configure(SensorBasePtr _sensor) = 0;
virtual void remove();
@@ -354,6 +355,8 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
void setVotingActive(bool _voting_active = true);
+ int getDim() const;
+
void link(SensorBasePtr);
template<typename classType, typename... T>
static std::shared_ptr<classType> emplace(SensorBasePtr _sen_ptr, T&&... all);
@@ -400,7 +403,10 @@ inline void ProcessorBase::setTimeTolerance(double _time_tolerance)
{
params_->time_tolerance = _time_tolerance;
}
-
+inline int ProcessorBase::getDim() const
+{
+ return dim_;
+}
template<typename classType, typename... T>
std::shared_ptr<classType> ProcessorBase::emplace(SensorBasePtr _sen_ptr, T&&... all)
diff --git a/include/core/processor/processor_diff_drive.h b/include/core/processor/processor_diff_drive.h
index 05b98e4b517e25cd4aa476238bde867c2196de23..48d18acccc0e48f3f5f2373b0ece28e2da607f61 100644
--- a/include/core/processor/processor_diff_drive.h
+++ b/include/core/processor/processor_diff_drive.h
@@ -8,29 +8,29 @@
#ifndef PROCESSOR_PROCESSOR_DIFF_DRIVE_H_
#define PROCESSOR_PROCESSOR_DIFF_DRIVE_H_
-#include "core/processor/processor_odom_2D.h"
+#include "core/processor/processor_odom_2d.h"
namespace wolf
{
WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsDiffDrive);
-struct ProcessorParamsDiffDrive : public ProcessorParamsOdom2D
+struct ProcessorParamsDiffDrive : public ProcessorParamsOdom2d
{
ProcessorParamsDiffDrive() = default;
ProcessorParamsDiffDrive(std::string _unique_name, const wolf::ParamsServer & _server) :
- ProcessorParamsOdom2D(_unique_name, _server)
+ ProcessorParamsOdom2d(_unique_name, _server)
{
}
std::string print() const
{
- return "\n" + ProcessorParamsOdom2D::print();
+ return "\n" + ProcessorParamsOdom2d::print();
}
};
WOLF_PTR_TYPEDEFS(ProcessorDiffDrive);
-class ProcessorDiffDrive : public ProcessorOdom2D
+class ProcessorDiffDrive : public ProcessorOdom2d
{
public:
ProcessorDiffDrive(ProcessorParamsDiffDrivePtr _params_motion);
diff --git a/include/core/processor/processor_factory.h b/include/core/processor/processor_factory.h
index 24f074902f56999949d83e9b7b8a412c14e3aa41..c14f3daa364678ef6f549fa04f65eddb4885e1fd 100644
--- a/include/core/processor/processor_factory.h
+++ b/include/core/processor/processor_factory.h
@@ -27,14 +27,14 @@ namespace wolf
*
* Specific object creation is invoked by create(TYPE, params), and the TYPE of processor is identified with a string.
* For example, the following processor types are implemented,
- * - "ODOM 3D" for ProcessorOdom3D
- * - "ODOM 2D" for ProcessorOdom2D
+ * - "ODOM 3d" for ProcessorOdom3d
+ * - "ODOM 2d" for ProcessorOdom2d
* - "GPS" for ProcessorGPS
*
* The rule to make new TYPE strings unique is that you skip the prefix 'Processor' from your class name,
* and you build a string in CAPITALS with space separators.
* - ProcessorImageFeature -> ````"IMAGE"````
- * - ProcessorLaser2D -> ````"LASER 2D"````
+ * - ProcessorLaser2d -> ````"LASER 2d"````
* - etc.
*
* The methods to create specific processors are called __creators__.
@@ -44,7 +44,7 @@ namespace wolf
* - Access the Factory
* - Register and unregister creators
* - Create processors
- * - Write a processor creator for ProcessorOdom2D (example).
+ * - Write a processor creator for ProcessorOdom2d (example).
*
* #### Accessing the Factory
* The ProcessorFactory class is a singleton: it can only exist once in your application.
@@ -69,37 +69,37 @@ namespace wolf
* that knows how to create your specific processor, e.g.:
*
* \code
- * ProcessorFactory::get().registerCreator("ProcessorOdom2D", ProcessorOdom2D::create);
+ * ProcessorFactory::get().registerCreator("ProcessorOdom2d", ProcessorOdom2d::create);
* \endcode
*
- * The method ProcessorOdom2D::create() exists in the ProcessorOdom2D class as a static method.
+ * The method ProcessorOdom2d::create() exists in the ProcessorOdom2d class as a static method.
* All these ProcessorXxx::create() methods need to have exactly the same API, regardless of the processor type.
* This API includes a processor name, and a pointer to a base struct of parameters, ProcessorParamsBasePtr,
* that can be derived for each derived processor.
*
- * Here is an example of ProcessorOdom2D::create() extracted from processor_odom_2D.h:
+ * Here is an example of ProcessorOdom2d::create() extracted from processor_odom_2d.h:
*
* \code
* static ProcessorBasePtr create(const std::string& _name, ProcessorParamsBasePtr _params)
* {
* // cast _params to good type
- * ProcessorParamsOdom2D* params = (ProcessorParamsOdom2D*)_params;
+ * ProcessorParamsOdom2d* params = (ProcessorParamsOdom2d*)_params;
*
- * ProcessorBasePtr prc = new ProcessorOdom2D(params);
- * prc->setName(_name); // pass the name to the created ProcessorOdom2D.
+ * ProcessorBasePtr prc = new ProcessorOdom2d(params);
+ * prc->setName(_name); // pass the name to the created ProcessorOdom2d.
* return prc;
* }
* \endcode
*
* #### Achieving automatic registration
* Currently, registering is performed in each specific ProcessorXxxx source file, processor_xxxx.cpp.
- * For example, in processor_odom_2D.cpp we find the line:
+ * For example, in processor_odom_2d.cpp we find the line:
*
* \code
- * const bool registered_odom_2D = ProcessorFactory::get().registerCreator("ProcessorOdom2D", ProcessorOdom2D::create);
+ * const bool registered_odom_2d = ProcessorFactory::get().registerCreator("ProcessorOdom2d", ProcessorOdom2d::create);
* \endcode
*
- * which is a static invocation (i.e., it is placed at global scope outside of the ProcessorOdom2D class).
+ * which is a static invocation (i.e., it is placed at global scope outside of the ProcessorOdom2d class).
* Therefore, at application level, all processors that have a .cpp file compiled are automatically registered.
*
* #### Unregister processor creators
@@ -107,39 +107,39 @@ namespace wolf
* It only needs to be passed the string of the processor type.
*
* \code
- * ProcessorFactory::get().unregisterCreator("ProcessorOdom2D");
+ * ProcessorFactory::get().unregisterCreator("ProcessorOdom2d");
* \endcode
*
* #### Creating processors
* Prior to invoking the creation of a processor of a particular type,
* you must register the creator for this type into the factory.
*
- * To create a ProcessorOdom2D, you type:
+ * To create a ProcessorOdom2d, you type:
*
* \code
- * ProcessorFactory::get().create("ProcessorOdom2D", "main odometry", params_ptr);
+ * ProcessorFactory::get().create("ProcessorOdom2d", "main odometry", params_ptr);
* \endcode
*
* #### Example 1 : using the Factories alone
- * We provide the necessary steps to create a processor of class ProcessorOdom2D in our application,
- * and bind it to a SensorOdom2D:
+ * We provide the necessary steps to create a processor of class ProcessorOdom2d in our application,
+ * and bind it to a SensorOdom2d:
*
* \code
- * #include "core/sensor/sensor_odom_2D.h" // provides SensorOdom2D and SensorFactory
- * #include "core/processor/processor_odom_2D.h" // provides ProcessorOdom2D and ProcessorFactory
+ * #include "core/sensor/sensor_odom_2d.h" // provides SensorOdom2d and SensorFactory
+ * #include "core/processor/processor_odom_2d.h" // provides ProcessorOdom2d and ProcessorFactory
*
- * // Note: SensorOdom2D::create() is already registered, automatically.
- * // Note: ProcessorOdom2D::create() is already registered, automatically.
+ * // Note: SensorOdom2d::create() is already registered, automatically.
+ * // Note: ProcessorOdom2d::create() is already registered, automatically.
*
* // First create the sensor (See SensorFactory for details)
- * SensorBasePtr sensor_ptr = SensorFactory::get().create ( "FactorOdom2D" , "Main odometer" , extrinsics , &intrinsics );
+ * SensorBasePtr sensor_ptr = SensorFactory::get().create ( "FactorOdom2d" , "Main odometer" , extrinsics , &intrinsics );
*
- * // To create a odometry integrator, provide a type="ODOM 2D", a name="main odometry", and a pointer to the parameters struct:
+ * // To create a odometry integrator, provide a type="ODOM 2d", a name="main odometry", and a pointer to the parameters struct:
*
- * ProcessorParamsOdom2D params({...}); // fill in the derived struct (note: ProcessorOdom2D actually has no input params)
+ * ProcessorParamsOdom2d params({...}); // fill in the derived struct (note: ProcessorOdom2d actually has no input params)
*
* ProcessorBasePtr processor_ptr =
- * ProcessorFactory::get().create ( "ProcessorOdom2D" , "main odometry" , ¶ms );
+ * ProcessorFactory::get().create ( "ProcessorOdom2d" , "main odometry" , ¶ms );
*
* // Bind processor to sensor
* sensor_ptr->addProcessor(processor_ptr);
@@ -153,13 +153,13 @@ namespace wolf
* The example 1 above can be accomplished as follows (we obviated for simplicity all the parameter creation),
*
* \code
- * #include "core/sensor/sensor_odom_2D.h"
- * #include "core/processor/processor_odom_2D.h"
+ * #include "core/sensor/sensor_odom_2d.h"
+ * #include "core/processor/processor_odom_2d.h"
* #include "core/problem/problem.h"
*
- * Problem problem(FRM_PO_2D);
- * problem.installSensor ( "SensorOdom2D" , "Main odometer" , extrinsics , &intrinsics );
- * problem.installProcessor ( "ProcessorOdom2D" , "Odometry" , "Main odometer" , ¶ms );
+ * Problem problem(FRM_PO_2d);
+ * problem.installSensor ( "SensorOdom2d" , "Main odometer" , extrinsics , &intrinsics );
+ * problem.installProcessor ( "ProcessorOdom2d" , "Odometry" , "Main odometer" , ¶ms );
* \endcode
*
* You can also check the code in the example file ````src/examples/test_wolf_factories.cpp````.
diff --git a/include/core/processor/processor_loopclosure.h b/include/core/processor/processor_loopclosure.h
index a1f9f67dd14e28c617ab476608e96924d6b3e78e..b0720f3a47afdfd7878b23929923295964340ea5 100644
--- a/include/core/processor/processor_loopclosure.h
+++ b/include/core/processor/processor_loopclosure.h
@@ -47,10 +47,11 @@ class ProcessorLoopClosure : public ProcessorBase
protected:
ProcessorParamsLoopClosurePtr params_loop_closure_;
+ int _dim;
public:
- ProcessorLoopClosure(const std::string& _type, ProcessorParamsLoopClosurePtr _params_loop_closure);
+ ProcessorLoopClosure(const std::string& _type, int _dim, ProcessorParamsLoopClosurePtr _params_loop_closure);
virtual ~ProcessorLoopClosure() = default;
virtual void configure(SensorBasePtr _sensor) override { };
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index d883202026e095e3574e5483951d9c185be54c08..81a9184ad4798ccbaac1ebd61a194f6e287331b4 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -13,7 +13,7 @@
#include "core/processor/processor_base.h"
#include "core/processor/is_motion.h"
#include "core/common/time_stamp.h"
-#include "core/utils/params_server.hpp"
+#include "core/utils/params_server.h"
// std
#include <iomanip>
@@ -149,6 +149,7 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
public:
ProcessorMotion(const std::string& _type,
std::string _state_structure,
+ int _dim,
SizeEigen _state_size,
SizeEigen _delta_size,
SizeEigen _delta_cov_size,
@@ -401,8 +402,8 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
* \return a delta state equivalent to the null motion.
*
* Examples (see documentation of the the class for info on Eigen::VectorXd):
- * - 2D odometry: a 3-vector with all zeros, e.g. VectorXd::Zero(3)
- * - 3D odometry: delta type is a PQ vector: 7-vector with [0,0,0, 0,0,0,1]
+ * - 2d odometry: a 3-vector with all zeros, e.g. VectorXd::Zero(3)
+ * - 3d odometry: delta type is a PQ vector: 7-vector with [0,0,0, 0,0,0,1]
* - IMU: PQVBB 10-vector with [0,0,0, 0,0,0,1, 0,0,0] // No biases in the delta !!
*/
virtual Eigen::VectorXd deltaZero() const = 0;
diff --git a/include/core/processor/processor_odom_2D.h b/include/core/processor/processor_odom_2d.h
similarity index 78%
rename from include/core/processor/processor_odom_2D.h
rename to include/core/processor/processor_odom_2d.h
index 491df7ada9f4b9c540740b4a9f06bb209e74f31c..b03416837ec1b0a6a86a67ff0cf40fdf820b1370 100644
--- a/include/core/processor/processor_odom_2D.h
+++ b/include/core/processor/processor_odom_2d.h
@@ -1,30 +1,30 @@
/**
- * \file processor_odom_2D.h
+ * \file processor_odom_2d.h
*
* Created on: Apr 15, 2016
* \author: jvallve
*/
-#ifndef SRC_PROCESSOR_ODOM_2D_H_
-#define SRC_PROCESSOR_ODOM_2D_H_
+#ifndef SRC_PROCESSOR_ODOM_2d_H_
+#define SRC_PROCESSOR_ODOM_2d_H_
#include "core/processor/processor_motion.h"
-#include "core/capture/capture_odom_2D.h"
-#include "core/factor/factor_odom_2D.h"
+#include "core/capture/capture_odom_2d.h"
+#include "core/factor/factor_odom_2d.h"
#include "core/math/rotations.h"
-#include "core/utils/params_server.hpp"
+#include "core/utils/params_server.h"
namespace wolf {
-WOLF_PTR_TYPEDEFS(ProcessorOdom2D);
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsOdom2D);
+WOLF_PTR_TYPEDEFS(ProcessorOdom2d);
+WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsOdom2d);
-struct ProcessorParamsOdom2D : public ProcessorParamsMotion
+struct ProcessorParamsOdom2d : public ProcessorParamsMotion
{
double cov_det = 1.0;
- ProcessorParamsOdom2D() = default;
- ProcessorParamsOdom2D(std::string _unique_name, const wolf::ParamsServer & _server) :
+ ProcessorParamsOdom2d() = default;
+ ProcessorParamsOdom2d(std::string _unique_name, const wolf::ParamsServer & _server) :
ProcessorParamsMotion(_unique_name, _server)
{
cov_det = _server.getParam<double>(prefix + _unique_name + "/keyframe_vote/cov_det");
@@ -37,15 +37,15 @@ struct ProcessorParamsOdom2D : public ProcessorParamsMotion
}
};
-class ProcessorOdom2D : public ProcessorMotion
+class ProcessorOdom2d : public ProcessorMotion
{
public:
- ProcessorOdom2D(ProcessorParamsOdom2DPtr _params);
- virtual ~ProcessorOdom2D();
- virtual void configure(SensorBasePtr _sensor) override { };
+ ProcessorOdom2d(ProcessorParamsOdom2dPtr _params);
+ virtual ~ProcessorOdom2d();
+ virtual void configure(SensorBasePtr _sensor) override;
// Factory method for high level API
- WOLF_PROCESSOR_CREATE(ProcessorOdom2D, ProcessorParamsOdom2D);
+ WOLF_PROCESSOR_CREATE(ProcessorOdom2d, ProcessorParamsOdom2d);
virtual bool voteForKeyFrame() const override;
@@ -86,15 +86,15 @@ class ProcessorOdom2D : public ProcessorMotion
CaptureBasePtr _capture_origin) override;
protected:
- ProcessorParamsOdom2DPtr params_odom_2D_;
+ ProcessorParamsOdom2dPtr params_odom_2d_;
};
-inline Eigen::VectorXd ProcessorOdom2D::deltaZero() const
+inline Eigen::VectorXd ProcessorOdom2d::deltaZero() const
{
return Eigen::VectorXd::Zero(delta_size_);
}
} // namespace wolf
-#endif /* SRC_PROCESSOR_ODOM_2D_H_ */
+#endif /* SRC_PROCESSOR_ODOM_2d_H_ */
diff --git a/include/core/processor/processor_odom_3D.h b/include/core/processor/processor_odom_3d.h
similarity index 82%
rename from include/core/processor/processor_odom_3D.h
rename to include/core/processor/processor_odom_3d.h
index 9c487a8ea061fa42466fb6bf62fbc9868c3094de..f8f849b896aad644f998c38b50e8ca198c0738c4 100644
--- a/include/core/processor/processor_odom_3D.h
+++ b/include/core/processor/processor_odom_3d.h
@@ -1,28 +1,28 @@
/**
- * \file processor_odom_3D.h
+ * \file processor_odom_3d.h
*
* Created on: Mar 18, 2016
* \author: jsola
*/
-#ifndef SRC_PROCESSOR_ODOM_3D_H_
-#define SRC_PROCESSOR_ODOM_3D_H_
+#ifndef SRC_PROCESSOR_ODOM_3d_H_
+#define SRC_PROCESSOR_ODOM_3d_H_
#include "core/processor/processor_motion.h"
-#include "core/sensor/sensor_odom_3D.h"
-#include "core/capture/capture_odom_3D.h"
-#include "core/factor/factor_odom_3D.h"
+#include "core/sensor/sensor_odom_3d.h"
+#include "core/capture/capture_odom_3d.h"
+#include "core/factor/factor_odom_3d.h"
#include "core/math/rotations.h"
#include <cmath>
namespace wolf {
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsOdom3D);
+WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsOdom3d);
-struct ProcessorParamsOdom3D : public ProcessorParamsMotion
+struct ProcessorParamsOdom3d : public ProcessorParamsMotion
{
- ProcessorParamsOdom3D() = default;
- ProcessorParamsOdom3D(std::string _unique_name, const ParamsServer& _server):
+ ProcessorParamsOdom3d() = default;
+ ProcessorParamsOdom3d(std::string _unique_name, const ParamsServer& _server):
ProcessorParamsMotion(_unique_name, _server)
{
//
@@ -33,11 +33,11 @@ struct ProcessorParamsOdom3D : public ProcessorParamsMotion
}
};
-WOLF_PTR_TYPEDEFS(ProcessorOdom3D);
+WOLF_PTR_TYPEDEFS(ProcessorOdom3d);
/** \brief Processor for 3d odometry integration.
*
- * This processor integrates motion data in the form of 3D odometry.
+ * This processor integrates motion data in the form of 3d odometry.
*
* The odometry data is extracted from Captures of the type CaptureOdometry3d.
* This data comes in the form of a 6-vector, or a 7-vector, containing the following components:
@@ -57,15 +57,15 @@ WOLF_PTR_TYPEDEFS(ProcessorOdom3D);
*
* All frames are assumed FLU ( x: Front, y: Left, z: Up ).
*/
-class ProcessorOdom3D : public ProcessorMotion
+class ProcessorOdom3d : public ProcessorMotion
{
public:
- ProcessorOdom3D(ProcessorParamsOdom3DPtr _params);
- virtual ~ProcessorOdom3D();
+ ProcessorOdom3d(ProcessorParamsOdom3dPtr _params);
+ virtual ~ProcessorOdom3d();
virtual void configure(SensorBasePtr _sensor) override;
// Factory method for high level API
- WOLF_PROCESSOR_CREATE(ProcessorOdom3D, ProcessorParamsOdom3D);
+ WOLF_PROCESSOR_CREATE(ProcessorOdom3d, ProcessorParamsOdom3d);
public:
// Motion integration
@@ -107,9 +107,9 @@ class ProcessorOdom3D : public ProcessorMotion
CaptureBasePtr _capture_origin) override;
protected:
- ProcessorParamsOdom3DPtr params_odom_3D_;
+ ProcessorParamsOdom3dPtr params_odom_3d_;
- // noise parameters (stolen from owner SensorOdom3D)
+ // noise parameters (stolen from owner SensorOdom3d)
double k_disp_to_disp_; // displacement variance growth per meter of linear motion
double k_disp_to_rot_; // orientation variance growth per meter of linear motion
double k_rot_to_rot_; // orientation variance growth per radian of rotational motion
@@ -118,11 +118,11 @@ class ProcessorOdom3D : public ProcessorMotion
};
-inline Eigen::VectorXd ProcessorOdom3D::deltaZero() const
+inline Eigen::VectorXd ProcessorOdom3d::deltaZero() const
{
return (Eigen::VectorXd(7) << 0,0,0, 0,0,0,1).finished(); // p, q
}
} // namespace wolf
-#endif /* SRC_PROCESSOR_ODOM_3D_H_ */
+#endif /* SRC_PROCESSOR_ODOM_3d_H_ */
diff --git a/include/core/processor/processor_tracker.h b/include/core/processor/processor_tracker.h
index ed07d3de55a3ec8e4b9181cb8c9b3af85ba4f1cb..d7ab951643fbc2760b8d81110a9f2338a96dad36 100644
--- a/include/core/processor/processor_tracker.h
+++ b/include/core/processor/processor_tracker.h
@@ -104,9 +104,11 @@ class ProcessorTracker : public ProcessorBase
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_incoming_; ///< list of the known features in \b last successfully tracked in \b incoming
+ int _dim;
public:
ProcessorTracker(const std::string& _type,
+ int _dim,
ProcessorParamsTrackerPtr _params_tracker);
virtual ~ProcessorTracker();
diff --git a/include/core/processor/processor_tracker_feature.h b/include/core/processor/processor_tracker_feature.h
index f05fb89762d6efef528c100502523b41a869af9f..b86e041d25e026762ea1413429b1c6e63a594fd7 100644
--- a/include/core/processor/processor_tracker_feature.h
+++ b/include/core/processor/processor_tracker_feature.h
@@ -85,6 +85,7 @@ class ProcessorTrackerFeature : public ProcessorTracker
/** \brief Constructor with type
*/
ProcessorTrackerFeature(const std::string& _type,
+ int _dim,
ProcessorParamsTrackerFeaturePtr _params_tracker_feature);
virtual ~ProcessorTrackerFeature();
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index a5afc4664a826c19b3c58272164b8e3849d1c15c..1c099cb9be4827705bd43d229714ea1dabf24ed0 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -173,15 +173,17 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
bool process(const CaptureBasePtr capture_ptr);
// State blocks
- virtual StateBlockPtr addStateBlock(const std::string& _key, const StateBlockPtr& _sb_ptr) override;
- virtual StateBlockPtr addStateBlock(const char _key, const StateBlockPtr& _sb_ptr) override { return this->addStateBlock(std::string(1, _key), _sb_ptr); }
- StateBlockPtr addStateBlock(const std::string& _key, const StateBlockPtr& _sb_ptr, const bool _dynamic);
- StateBlockPtr addStateBlock(const char _key, const StateBlockPtr& _sb_ptr, const bool _dynamic) { return this->addStateBlock(std::string(1, _key), _sb_ptr, _dynamic); }
+ using HasStateBlocks::addStateBlock;
+ StateBlockPtr addStateBlock(const std::string& _key, const StateBlockPtr& _sb_ptr, const bool _dynamic = false);
+ StateBlockPtr addStateBlock(const char _key, const StateBlockPtr& _sb_ptr, const bool _dynamic = false) { return this->addStateBlock(std::string(1, _key), _sb_ptr, _dynamic); }
StateBlockPtr getStateBlockDynamic(const std::string& _key) const;
StateBlockPtr getStateBlockDynamic(const std::string& _key, const TimeStamp& _ts) const;
StateBlockPtr getStateBlockDynamic(const char _key) const { return getStateBlockDynamic(std::string(1,_key)); }
StateBlockPtr getStateBlockDynamic(const char _key, const TimeStamp& _ts) const { return getStateBlockDynamic(std::string(1,_key), _ts); }
+ // Declare a state block as dynamic or static (with _dymanic = false)
+ void setStateBlockDynamic(const std::string& _key, bool _dynamic = true);
+
bool isStateBlockDynamic(const std::string& _key) const;
bool isStateBlockInCapture(const std::string& _key, const TimeStamp& _ts, CaptureBasePtr& _cap) const;
bool isStateBlockInCapture(const std::string& _key, CaptureBasePtr& _cap) const;
@@ -198,7 +200,6 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
protected:
void removeStateBlocks();
virtual void registerNewStateBlocks() const;
- void setStateBlockDynamic(const std::string& _key, bool _dynamic = true);
public:
@@ -279,18 +280,10 @@ inline const ProcessorBasePtrList& SensorBase::getProcessorList() const
return processor_list_;
}
-inline StateBlockPtr SensorBase::addStateBlock(const std::string& _key, const StateBlockPtr& _sb_ptr)
-{
- assert((params_prior_map_.find(_key) == params_prior_map_.end() || _sb_ptr == nullptr) && "overwriting a state block that has an absolute factor");
- HasStateBlocks::addStateBlock(_key, _sb_ptr);
- setStateBlockDynamic(_key, false);
- return _sb_ptr;
-}
-
inline StateBlockPtr SensorBase::addStateBlock(const std::string& _key, const StateBlockPtr& _sb_ptr, const bool _dynamic)
{
assert((params_prior_map_.find(_key) == params_prior_map_.end() || _sb_ptr == nullptr) && "overwriting a state block that has an absolute factor");
- HasStateBlocks::addStateBlock(_key, _sb_ptr);
+ HasStateBlocks::addStateBlock(_key, _sb_ptr, getProblem());
setStateBlockDynamic(_key, _dynamic);
return _sb_ptr;
}
diff --git a/include/core/sensor/sensor_factory.h b/include/core/sensor/sensor_factory.h
index 1f0b803df4febb6c3b4f71b1f77f60a4909d76ca..daf01499ac759d799517cdd73ab830fb6ccaf4d5 100644
--- a/include/core/sensor/sensor_factory.h
+++ b/include/core/sensor/sensor_factory.h
@@ -16,7 +16,7 @@ struct ParamsSensorBase;
// wolf
#include "core/common/factory.h"
-#include "core/utils/params_server.hpp"
+#include "core/utils/params_server.h"
namespace wolf
{
@@ -28,13 +28,13 @@ namespace wolf
* Specific object creation is invoked by ````create(TYPE, params ... )````, and the TYPE of sensor is identified with a string.
* Currently, the following sensor types are implemented,
* - "CAMERA" for SensorCamera
- * - "ODOM 2D" for SensorOdom2D
+ * - "ODOM 2d" for SensorOdom2d
* - "GPS FIX" for SensorGPSFix
*
* The rule to make new TYPE strings unique is that you skip the prefix 'Sensor' from your class name,
* and you build a string in CAPITALS with space separators, e.g.:
* - SensorCamera -> ````"CAMERA"````
- * - SensorLaser2D -> ````"LASER 2D"````
+ * - SensorLaser2d -> ````"LASER 2d"````
* - etc.
*
* The methods to create specific sensors are called __creators__.
@@ -127,7 +127,7 @@ namespace wolf
* static SensorBasePtr create(const std::string& _name, Eigen::VectorXd& _extrinsics_pq, ParamsSensorBasePtr _intrinsics)
* {
* // check extrinsics vector
- * assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector length. Should be 7 for 3D.");
+ * assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector length. Should be 7 for 3d.");
*
* // cast instrinsics to good type
* ParamsSensorCamera* intrinsics_ptr = (ParamsSensorCamera*) _intrinsics;
diff --git a/include/core/sensor/sensor_odom_2D.h b/include/core/sensor/sensor_odom_2d.h
similarity index 69%
rename from include/core/sensor/sensor_odom_2D.h
rename to include/core/sensor/sensor_odom_2d.h
index 6f16e71fef41d774539464c2c001d719fd9d50e0..45499f5c56fb342ed8a82b8521e6c2e156d06031 100644
--- a/include/core/sensor/sensor_odom_2D.h
+++ b/include/core/sensor/sensor_odom_2d.h
@@ -1,25 +1,25 @@
-#ifndef SENSOR_ODOM_2D_H_
-#define SENSOR_ODOM_2D_H_
+#ifndef SENSOR_ODOM_2d_H_
+#define SENSOR_ODOM_2d_H_
//wolf includes
#include "core/sensor/sensor_base.h"
-#include "core/utils/params_server.hpp"
+#include "core/utils/params_server.h"
namespace wolf {
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorOdom2D);
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorOdom2d);
-struct ParamsSensorOdom2D : public ParamsSensorBase
+struct ParamsSensorOdom2d : public ParamsSensorBase
{
- virtual ~ParamsSensorOdom2D() = default;
+ virtual ~ParamsSensorOdom2d() = default;
double k_disp_to_disp; ///< ratio of displacement variance to displacement, for odometry noise calculation
double k_rot_to_rot; ///< ratio of rotation variance to rotation, for odometry noise calculation
- ParamsSensorOdom2D()
+ ParamsSensorOdom2d()
{
//DEFINED FOR COMPATIBILITY PURPOSES. TO BE REMOVED IN THE FUTURE.
}
- ParamsSensorOdom2D(std::string _unique_name, const ParamsServer& _server):
+ ParamsSensorOdom2d(std::string _unique_name, const ParamsServer& _server):
ParamsSensorBase(_unique_name, _server)
{
k_disp_to_disp = _server.getParam<double>(prefix + _unique_name + "/k_disp_to_disp");
@@ -33,20 +33,20 @@ struct ParamsSensorOdom2D : public ParamsSensorBase
}
};
-WOLF_PTR_TYPEDEFS(SensorOdom2D);
+WOLF_PTR_TYPEDEFS(SensorOdom2d);
-class SensorOdom2D : public SensorBase
+class SensorOdom2d : public SensorBase
{
protected:
double k_disp_to_disp_; ///< ratio of displacement variance to displacement, for odometry noise calculation
double k_rot_to_rot_; ///< ratio of rotation variance to rotation, for odometry noise calculation
public:
- SensorOdom2D(Eigen::VectorXd _extrinsics, const ParamsSensorOdom2D& _intrinsics);
- SensorOdom2D(Eigen::VectorXd _extrinsics, ParamsSensorOdom2DPtr _intrinsics);
- WOLF_SENSOR_CREATE(SensorOdom2D, ParamsSensorOdom2D, 3);
+ SensorOdom2d(Eigen::VectorXd _extrinsics, const ParamsSensorOdom2d& _intrinsics);
+ SensorOdom2d(Eigen::VectorXd _extrinsics, ParamsSensorOdom2dPtr _intrinsics);
+ WOLF_SENSOR_CREATE(SensorOdom2d, ParamsSensorOdom2d, 3);
- virtual ~SensorOdom2D();
+ virtual ~SensorOdom2d();
/** \brief Returns displacement noise factor
*
@@ -66,4 +66,4 @@ class SensorOdom2D : public SensorBase
} // namespace wolf
-#endif // SENSOR_ODOM_2D_H_
+#endif // SENSOR_ODOM_2d_H_
diff --git a/include/core/sensor/sensor_odom_3D.h b/include/core/sensor/sensor_odom_3d.h
similarity index 70%
rename from include/core/sensor/sensor_odom_3D.h
rename to include/core/sensor/sensor_odom_3d.h
index b02fa3c38cfc3bdcc66063d264b67107984e5cf8..3d08375b4fc30e43f07f201a5653d5e83a258381 100644
--- a/include/core/sensor/sensor_odom_3D.h
+++ b/include/core/sensor/sensor_odom_3d.h
@@ -1,33 +1,33 @@
/**
- * \file sensor_odom_3D.h
+ * \file sensor_odom_3d.h
*
* Created on: Oct 7, 2016
* \author: jsola
*/
-#ifndef SRC_SENSOR_ODOM_3D_H_
-#define SRC_SENSOR_ODOM_3D_H_
+#ifndef SRC_SENSOR_ODOM_3d_H_
+#define SRC_SENSOR_ODOM_3d_H_
//wolf includes
#include "core/sensor/sensor_base.h"
-#include "core/utils/params_server.hpp"
+#include "core/utils/params_server.h"
namespace wolf {
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorOdom3D);
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorOdom3d);
-struct ParamsSensorOdom3D : public ParamsSensorBase
+struct ParamsSensorOdom3d : public ParamsSensorBase
{
double k_disp_to_disp; ///< ratio of displacement variance to displacement, for odometry noise calculation
double k_disp_to_rot; ///< ratio of displacement variance to rotation, for odometry noise calculation
double k_rot_to_rot; ///< ratio of rotation variance to rotation, for odometry noise calculation
double min_disp_var;
double min_rot_var;
- ParamsSensorOdom3D()
+ ParamsSensorOdom3d()
{
//DEFINED FOR COMPATIBILITY PURPOSES. TO BE REMOVED IN THE FUTURE.
}
- ParamsSensorOdom3D(std::string _unique_name, const ParamsServer& _server):
+ ParamsSensorOdom3d(std::string _unique_name, const ParamsServer& _server):
ParamsSensorBase(_unique_name, _server)
{
k_disp_to_disp = _server.getParam<double>(prefix + _unique_name + "/k_disp_to_disp");
@@ -45,12 +45,12 @@ struct ParamsSensorOdom3D : public ParamsSensorBase
+ "min_disp_var: " + std::to_string(min_disp_var) + "\n"
+ "min_rot_var: " + std::to_string(min_rot_var) + "\n";
}
- virtual ~ParamsSensorOdom3D() = default;
+ virtual ~ParamsSensorOdom3d() = default;
};
-WOLF_PTR_TYPEDEFS(SensorOdom3D);
+WOLF_PTR_TYPEDEFS(SensorOdom3d);
-class SensorOdom3D : public SensorBase
+class SensorOdom3d : public SensorBase
{
protected:
double k_disp_to_disp_; ///< ratio of displacement variance to displacement, for odometry noise calculation
@@ -60,11 +60,11 @@ class SensorOdom3D : public SensorBase
double min_rot_var_;
public:
- SensorOdom3D(const Eigen::VectorXd& _extrinsics_pq, const ParamsSensorOdom3D& params);
- SensorOdom3D(const Eigen::VectorXd& _extrinsics_pq, ParamsSensorOdom3DPtr params);
- WOLF_SENSOR_CREATE(SensorOdom3D, ParamsSensorOdom3D, 7);
+ SensorOdom3d(const Eigen::VectorXd& _extrinsics_pq, const ParamsSensorOdom3d& params);
+ SensorOdom3d(const Eigen::VectorXd& _extrinsics_pq, ParamsSensorOdom3dPtr params);
+ WOLF_SENSOR_CREATE(SensorOdom3d, ParamsSensorOdom3d, 7);
- virtual ~SensorOdom3D();
+ virtual ~SensorOdom3d();
double getDispVarToDispNoiseFactor() const;
double getDispVarToRotNoiseFactor() const;
@@ -74,31 +74,31 @@ class SensorOdom3D : public SensorBase
};
-inline double SensorOdom3D::getDispVarToDispNoiseFactor() const
+inline double SensorOdom3d::getDispVarToDispNoiseFactor() const
{
return k_disp_to_disp_;
}
-inline double SensorOdom3D::getDispVarToRotNoiseFactor() const
+inline double SensorOdom3d::getDispVarToRotNoiseFactor() const
{
return k_disp_to_rot_;
}
-inline double SensorOdom3D::getRotVarToRotNoiseFactor() const
+inline double SensorOdom3d::getRotVarToRotNoiseFactor() const
{
return k_rot_to_rot_;
}
-inline double SensorOdom3D::getMinDispVar() const
+inline double SensorOdom3d::getMinDispVar() const
{
return min_disp_var_;
}
-inline double SensorOdom3D::getMinRotVar() const
+inline double SensorOdom3d::getMinRotVar() const
{
return min_rot_var_;
}
} /* namespace wolf */
-#endif /* SRC_SENSOR_ODOM_3D_H_ */
+#endif /* SRC_SENSOR_ODOM_3d_H_ */
diff --git a/include/core/solver/solver_factory.h b/include/core/solver/solver_factory.h
index 40caeaf666c95f7369e5dd2e8328a46d22bef259..d80fe24f78ddaa91a44e7821f2dc0a55117b8dcf 100644
--- a/include/core/solver/solver_factory.h
+++ b/include/core/solver/solver_factory.h
@@ -28,13 +28,13 @@ namespace wolf
* Specific object creation is invoked by ````create(TYPE, params ... )````, and the TYPE of sensor is identified with a string.
* Currently, the following sensor types are implemented,
* - "CAMERA" for SensorCamera
- * - "ODOM 2D" for SensorOdom2D
+ * - "ODOM 2d" for SensorOdom2d
* - "GPS FIX" for SensorGPSFix
*
* The rule to make new TYPE strings unique is that you skip the prefix 'Sensor' from your class name,
* and you build a string in CAPITALS with space separators, e.g.:
* - SensorCamera -> ````"CAMERA"````
- * - SensorLaser2D -> ````"LASER 2D"````
+ * - SensorLaser2d -> ````"LASER 2d"````
* - etc.
*
* The methods to create specific sensors are called __creators__.
@@ -127,7 +127,7 @@ namespace wolf
* static SensorBasePtr create(const std::string& _name, Eigen::VectorXd& _extrinsics_pq, ParamsSensorBasePtr _intrinsics)
* {
* // check extrinsics vector
- * assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector length. Should be 7 for 3D.");
+ * assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector length. Should be 7 for 3d.");
*
* // cast instrinsics to good type
* ParamsSensorCamera* intrinsics_ptr = (ParamsSensorCamera*) _intrinsics;
diff --git a/include/core/solver_suitesparse/qr_solver.h b/include/core/solver_suitesparse/qr_solver.h
index 53ea301cff2f9506c6978dc401bc2b65c14df961..b586278c7e7f5a7cb949b78d2e90f58323031109 100644
--- a/include/core/solver_suitesparse/qr_solver.h
+++ b/include/core/solver_suitesparse/qr_solver.h
@@ -15,8 +15,8 @@
//Wolf includes
#include "core/state_block/state_block.h"
#include "../factor_sparse.h"
-#include "core/factor/factor_odom_2D.h"
-#include "core/factor/factor_corner_2D.h"
+#include "core/factor/factor_odom_2d.h"
+#include "core/factor/factor_corner_2d.h"
#include "core/factor/factor_container.h"
#include "core/solver_suitesparse/sparse_utils.h"
@@ -28,7 +28,7 @@
#include <eigen3/Eigen/OrderingMethods>
#include <eigen3/Eigen/SparseQR>
#include <Eigen/StdVector>
-#include "core/factor/factor_pose_2D.h"
+#include "core/factor/factor_pose_2d.h"
namespace wolf
{
@@ -535,30 +535,30 @@ class SolverQR
switch (_fac_ptr->getTypeId())
{
- case FAC_GPS_FIX_2D:
+ case FAC_GPS_FIX_2d:
{
- FactorGPS2D* specific_ptr = (FactorGPS2D*)(_fac_ptr);
- return (CostFunctionBasePtr)(new CostFunctionSparse<FactorGPS2D, specific_ptr->residualSize,
+ FactorGPS2d* specific_ptr = (FactorGPS2d*)(_fac_ptr);
+ return (CostFunctionBasePtr)(new CostFunctionSparse<FactorGPS2d, specific_ptr->residualSize,
specific_ptr->block0Size, specific_ptr->block1Size, specific_ptr->block2Size,
specific_ptr->block3Size, specific_ptr->block4Size, specific_ptr->block5Size,
specific_ptr->block6Size, specific_ptr->block7Size, specific_ptr->block8Size,
specific_ptr->block9Size>(specific_ptr));
break;
}
- case FAC_ODOM_2D:
+ case FAC_ODOM_2d:
{
- FactorOdom2D* specific_ptr = (FactorOdom2D*)(_fac_ptr);
- return (CostFunctionBasePtr)new CostFunctionSparse<FactorOdom2D, specific_ptr->residualSize,
+ FactorOdom2d* specific_ptr = (FactorOdom2d*)(_fac_ptr);
+ return (CostFunctionBasePtr)new CostFunctionSparse<FactorOdom2d, specific_ptr->residualSize,
specific_ptr->block0Size, specific_ptr->block1Size, specific_ptr->block2Size,
specific_ptr->block3Size, specific_ptr->block4Size, specific_ptr->block5Size,
specific_ptr->block6Size, specific_ptr->block7Size, specific_ptr->block8Size,
specific_ptr->block9Size>(specific_ptr);
break;
}
- case FAC_CORNER_2D:
+ case FAC_CORNER_2d:
{
- FactorCorner2D* specific_ptr = (FactorCorner2D*)(_fac_ptr);
- return (CostFunctionBasePtr)new CostFunctionSparse<FactorCorner2D, specific_ptr->residualSize,
+ FactorCorner2d* specific_ptr = (FactorCorner2d*)(_fac_ptr);
+ return (CostFunctionBasePtr)new CostFunctionSparse<FactorCorner2d, specific_ptr->residualSize,
specific_ptr->block0Size, specific_ptr->block1Size, specific_ptr->block2Size,
specific_ptr->block3Size, specific_ptr->block4Size, specific_ptr->block5Size,
specific_ptr->block6Size, specific_ptr->block7Size, specific_ptr->block8Size,
diff --git a/include/core/solver_suitesparse/solver_manager.h b/include/core/solver_suitesparse/solver_manager.h
index bb3863e11d20719704c7acae1bbc88bebf597eb2..449c8eee3a7b1d7ce54782300017604c51d1843c 100644
--- a/include/core/solver_suitesparse/solver_manager.h
+++ b/include/core/solver_suitesparse/solver_manager.h
@@ -2,16 +2,16 @@
#define CERES_MANAGER_H_
//wolf includes
-#include "core/factor/factor_GPS_2D.h"
+#include "core/factor/factor_GPS_2d.h"
#include "core/common/wolf.h"
#include "core/state_block/state_block.h"
#include "../state_point.h"
#include "../state_complex_angle.h"
#include "../state_theta.h"
#include "../factor_sparse.h"
-#include "../factor_odom_2D_theta.h"
-#include "../factor_odom_2D_complex_angle.h"
-#include "../factor_corner_2D_theta.h"
+#include "../factor_odom_2d_theta.h"
+#include "../factor_odom_2d_complex_angle.h"
+#include "../factor_corner_2d_theta.h"
/** \brief solver manager for WOLF
*
diff --git a/include/core/state_block/has_state_blocks.h b/include/core/state_block/has_state_blocks.h
index b1f59dc71ff2ed30a4dc98e829e7040137903277..815149b3e914bc48ec9df794a0b0716019eead1f 100644
--- a/include/core/state_block/has_state_blocks.h
+++ b/include/core/state_block/has_state_blocks.h
@@ -28,58 +28,65 @@ class HasStateBlocks
virtual ~HasStateBlocks();
const StateStructure& getStructure() const { return structure_; }
- void appendToStructure(const std::string& _frame_type){ structure_.push_back(_frame_type); }
+ void appendToStructure(const std::string& _frame_type){ structure_ += _frame_type; }
bool isInStructure(const std::string& _sb_type) { return std::find(structure_.begin(), structure_.end(), _sb_type) != structure_.end(); }
- const std::map<std::string, StateBlockPtr>& getStateBlockMap() const;
+ const std::unordered_map<std::string, StateBlockPtr>& getStateBlockMap() const;
std::vector<StateBlockPtr> getStateBlockVec() const;
// Some typical shortcuts -- not all should be coded here, see notes below.
StateBlockPtr getP() const;
StateBlockPtr getO() const;
- void setP(const StateBlockPtr _p_ptr);
- void setO(const StateBlockPtr _o_ptr);
// These act on all state blocks. Per-block action must be done through state_block.fix() or through extended API in derived classes of this.
virtual void fix();
virtual void unfix();
virtual bool isFixed() const;
- virtual StateBlockPtr addStateBlock(const std::string& _sb_type, const StateBlockPtr& _sb);
- virtual StateBlockPtr addStateBlock(const char _sb_type, const StateBlockPtr& _sb) { return addStateBlock(std::string(1,_sb_type), _sb); }
+ virtual StateBlockPtr addStateBlock(const std::string& _sb_type, const StateBlockPtr& _sb, ProblemPtr _problem);
+ virtual StateBlockPtr addStateBlock(const char _sb_type, const StateBlockPtr& _sb, ProblemPtr _problem) { return addStateBlock(std::string(1,_sb_type), _sb, _problem); }
virtual unsigned int removeStateBlock(const std::string& _sb_type);
virtual unsigned int removeStateBlock(const char _sb_type);
bool hasStateBlock(const std::string& _sb_type) const { return state_block_map_.count(_sb_type) > 0; }
bool hasStateBlock(const char _sb_type) const { return hasStateBlock(std::string(1, _sb_type)); }
+ bool hasStateBlock(const StateBlockPtr& _sb) const;
StateBlockPtr getStateBlock(const std::string& _sb_type) const;
StateBlockPtr getStateBlock(const char _sb_type) const { return getStateBlock(std::string(1,_sb_type)); }
bool setStateBlock(const std::string _sb_type, const StateBlockPtr& _sb);
bool setStateBlock(const char _sb_type, const StateBlockPtr& _sb) { return setStateBlock(std::string(1, _sb_type), _sb); }
+ bool stateBlockKey(const StateBlockPtr& _sb, std::string& _key) const;
+ std::unordered_map<std::string, StateBlockPtr>::const_iterator find(const StateBlockPtr& _sb) const;
// Emplace derived state blocks (angle, quaternion, etc).
template<typename SB, typename ... Args>
- std::shared_ptr<SB> emplaceStateBlock(const std::string& _sb_type, Args&&... _args_of_derived_state_block_constructor);
+ std::shared_ptr<SB> emplaceStateBlock(const std::string& _sb_type, ProblemPtr _problem, Args&&... _args_of_derived_state_block_constructor);
// Emplace base state blocks.
template<typename ... Args>
- inline StateBlockPtr emplaceStateBlock(const std::string& _sb_type, Args&&... _args_of_base_state_block_constructor);
+ inline StateBlockPtr emplaceStateBlock(const std::string& _sb_type, ProblemPtr _problem, Args&&... _args_of_base_state_block_constructor);
// Register/remove state blocks to/from wolf::Problem
void registerNewStateBlocks(ProblemPtr _problem) const;
void removeStateBlocks(ProblemPtr _problem);
// States
- inline void setState(const Eigen::VectorXd& _state, const StateStructure& _sub_structure = StateStructure(), const bool _notify = true);
- void getState(Eigen::VectorXd& _state, const StateStructure& _sub_structure = StateStructure()) const;
+ inline void setState(const Eigen::VectorXd& _state, const StateStructure& _sub_structure = StateStructure(), const bool _notify = true);
+ void getState(Eigen::VectorXd& _state, const StateStructure& _sub_structure = StateStructure()) const;
Eigen::VectorXd getState(const StateStructure& structure = StateStructure()) const;
- unsigned int getSize(const StateStructure& _sub_structure = StateStructure()) const;
- unsigned int getLocalSize(const StateStructure& _sub_structure = StateStructure()) const;
+ unsigned int getSize(const StateStructure& _sub_structure = StateStructure()) const;
+ unsigned int getLocalSize(const StateStructure& _sub_structure = StateStructure()) const;
+
+ inline unsigned int getLocalSize(StateStructure _sub_structure = "") const;
+
+ State getStateComposite();
+
+ // Perturb state
+ void perturb(double amplitude = 0.01);
- State getStateComposite();
private:
- StateStructure structure_;
- std::map<std::string, StateBlockPtr> state_block_map_;
+ StateStructure structure_; // TBR
+ std::unordered_map<std::string, StateBlockPtr> state_block_map_;
};
@@ -104,7 +111,7 @@ inline HasStateBlocks::~HasStateBlocks()
//
}
-inline const std::map<std::string, StateBlockPtr>& HasStateBlocks::getStateBlockMap() const
+inline const std::unordered_map<std::string, StateBlockPtr>& HasStateBlocks::getStateBlockMap() const
{
return state_block_map_;
}
@@ -119,15 +126,6 @@ inline std::vector<StateBlockPtr> HasStateBlocks::getStateBlockVec() const
return sbv;
}
-inline wolf::StateBlockPtr HasStateBlocks::addStateBlock(const std::string& _sb_type, const StateBlockPtr& _sb)
-{
- assert(state_block_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
- state_block_map_.emplace(_sb_type, _sb);
- if (!isInStructure(_sb_type))
- appendToStructure(_sb_type);
- return _sb;
-}
-
inline unsigned int HasStateBlocks::removeStateBlock(const char _sb_type)
{
return removeStateBlock(std::string(1, _sb_type));
@@ -139,24 +137,24 @@ inline unsigned int HasStateBlocks::removeStateBlock(const std::string& _sb_type
}
template<typename SB, typename ... Args>
-inline std::shared_ptr<SB> HasStateBlocks::emplaceStateBlock(const std::string& _sb_type, Args&&... _args_of_derived_state_block_constructor)
+inline std::shared_ptr<SB> HasStateBlocks::emplaceStateBlock(const std::string& _sb_type, ProblemPtr _problem, Args&&... _args_of_derived_state_block_constructor)
{
assert(state_block_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
std::shared_ptr<SB> sb = std::make_shared<SB>(std::forward<Args>(_args_of_derived_state_block_constructor)...);
- state_block_map_.emplace(_sb_type, sb);
- if (!isInStructure(_sb_type))
- appendToStructure(_sb_type);
+
+ addStateBlock(_sb_type, sb, _problem);
+
return sb;
}
template<typename ... Args>
-inline StateBlockPtr HasStateBlocks::emplaceStateBlock(const std::string& _sb_type, Args&&... _args_of_base_state_block_constructor)
+inline StateBlockPtr HasStateBlocks::emplaceStateBlock(const std::string& _sb_type, ProblemPtr _problem, Args&&... _args_of_base_state_block_constructor)
{
assert(state_block_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
- std::shared_ptr<StateBlock> sb = std::make_shared<StateBlock>(std::forward<Args>(_args_of_base_state_block_constructor)...);
- state_block_map_.emplace(_sb_type, sb);
- if (!isInStructure(_sb_type))
- appendToStructure(_sb_type);
+ auto sb = std::make_shared<StateBlock>(std::forward<Args>(_args_of_base_state_block_constructor)...);
+
+ addStateBlock(_sb_type, sb, _problem);
+
return sb;
}
@@ -187,16 +185,6 @@ inline wolf::StateBlockPtr HasStateBlocks::getO() const
return getStateBlock("O");
}
-inline void HasStateBlocks::setP(const StateBlockPtr _p_ptr)
-{
- addStateBlock("P", _p_ptr);
-}
-
-inline void HasStateBlocks::setO(const StateBlockPtr _o_ptr)
-{
- addStateBlock("O", _o_ptr);
-}
-
inline void HasStateBlocks::fix()
{
for (auto pair_key_sbp : state_block_map_)
@@ -309,7 +297,45 @@ inline unsigned int HasStateBlocks::getSize(const StateStructure& _sub_structure
return size;
}
-inline unsigned int HasStateBlocks::getLocalSize(const StateStructure& _sub_structure) const
+inline std::unordered_map<std::string, StateBlockPtr>::const_iterator HasStateBlocks::find(const StateBlockPtr& _sb) const
+{
+ const auto& it = std::find_if(state_block_map_.begin(),
+ state_block_map_.end(),
+ [_sb](const std::pair<std::string, StateBlockPtr>& pair)
+ {
+ return pair.second == _sb;
+ }
+ );
+
+ return it;
+}
+
+inline bool HasStateBlocks::hasStateBlock(const StateBlockPtr &_sb) const
+{
+ const auto& it = this->find(_sb);
+
+ return it != state_block_map_.end();
+}
+
+inline bool HasStateBlocks::stateBlockKey(const StateBlockPtr &_sb, std::string& _key) const
+{
+ const auto& it = this->find(_sb);
+
+ bool found = (it != state_block_map_.end());
+
+ if (found)
+ {
+ _key = it->first;
+ return true;
+ }
+ else
+ {
+ _key = "";
+ return false;
+ }
+}
+
+inline unsigned int HasStateBlocks::getLocalSize(StateStructure _sub_structure) const
{
StateStructure sub_structure;
if (_sub_structure.empty())
@@ -330,4 +356,5 @@ inline unsigned int HasStateBlocks::getLocalSize(const StateStructure& _sub_stru
}
} // namespace wolf
+
#endif /* STATE_BLOCK_HAS_STATE_BLOCKS_H_ */
diff --git a/include/core/state_block/state_block.h b/include/core/state_block/state_block.h
index b68b4ad7dd482f071faf04339656879635232dd2..ab3bb7147f3c81f57a3436216412274394acc04a 100644
--- a/include/core/state_block/state_block.h
+++ b/include/core/state_block/state_block.h
@@ -74,6 +74,10 @@ public:
**/
Eigen::VectorXd getState() const;
+ /** \brief Returns the state vector data array
+ **/
+ double* getStateData();
+
/** \brief Sets the state vector
**/
void setState(const Eigen::VectorXd& _state, const bool _notify = true);
@@ -142,6 +146,10 @@ public:
**/
void resetLocalParamUpdated();
+ /** \brief perturb state
+ */
+ void perturb(double amplitude = 0.1);
+
/** \brief Add this state_block to the problem
**/
//void addToProblem(ProblemPtr _problem_ptr);
@@ -279,6 +287,11 @@ inline void StateBlock::resetLocalParamUpdated()
local_param_updated_.store(false);
}
+inline double* StateBlock::getStateData()
+{
+ return state_.data();
+}
+
}// namespace wolf
#endif
diff --git a/include/core/state_block/state_homogeneous_3D.h b/include/core/state_block/state_homogeneous_3d.h
similarity index 51%
rename from include/core/state_block/state_homogeneous_3D.h
rename to include/core/state_block/state_homogeneous_3d.h
index 9ae7e8da01e5d1544dd887682b7f164e1ddc1e99..4760a52b7faa2bf188b520534b8512722d209f63 100644
--- a/include/core/state_block/state_homogeneous_3D.h
+++ b/include/core/state_block/state_homogeneous_3d.h
@@ -1,45 +1,45 @@
/*
- * \file state_homogeneous_3D.h
+ * \file state_homogeneous_3d.h
*
* Created on: Mar 7, 2016
* \author: jsola
*/
-#ifndef SRC_STATE_HOMOGENEOUS_3D_H_
-#define SRC_STATE_HOMOGENEOUS_3D_H_
+#ifndef SRC_STATE_HOMOGENEOUS_3d_H_
+#define SRC_STATE_HOMOGENEOUS_3d_H_
#include "core/state_block/state_block.h"
#include "core/state_block/local_parametrization_homogeneous.h"
namespace wolf {
-class StateHomogeneous3D : public StateBlock
+class StateHomogeneous3d : public StateBlock
{
public:
- StateHomogeneous3D(bool _fixed = false);
- StateHomogeneous3D(const Eigen::VectorXd _state, bool _fixed = false);
- virtual ~StateHomogeneous3D();
+ StateHomogeneous3d(bool _fixed = false);
+ StateHomogeneous3d(const Eigen::VectorXd _state, bool _fixed = false);
+ virtual ~StateHomogeneous3d();
};
-inline StateHomogeneous3D::StateHomogeneous3D(const Eigen::VectorXd _state, bool _fixed) :
+inline StateHomogeneous3d::StateHomogeneous3d(const Eigen::VectorXd _state, bool _fixed) :
StateBlock(_state, _fixed)
{
- assert(_state.size() == 4 && "Homogeneous 3D must be size 4.");
+ assert(_state.size() == 4 && "Homogeneous 3d must be size 4.");
local_param_ptr_ = std::make_shared<LocalParametrizationHomogeneous>();
}
-inline StateHomogeneous3D::StateHomogeneous3D(bool _fixed) :
+inline StateHomogeneous3d::StateHomogeneous3d(bool _fixed) :
StateBlock(4, _fixed)
{
local_param_ptr_ = std::make_shared<LocalParametrizationHomogeneous>();
state_ << 0, 0, 0, 1; // Set origin
}
-inline StateHomogeneous3D::~StateHomogeneous3D()
+inline StateHomogeneous3d::~StateHomogeneous3d()
{
// The local_param_ptr_ pointer is already removed by the base class
}
} // namespace wolf
-#endif /* SRC_STATE_HOMOGENEOUS_3D_H_ */
+#endif /* SRC_STATE_HOMOGENEOUS_3d_H_ */
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/include/core/utils/check_log.h b/include/core/utils/check_log.h
new file mode 100644
index 0000000000000000000000000000000000000000..035bd0aa0e2e9fa4989c95509800d1fee517bd46
--- /dev/null
+++ b/include/core/utils/check_log.h
@@ -0,0 +1,22 @@
+#ifndef CHECK_LOG_H
+#define CHECK_LOG_H
+#include <iostream>
+#include <string>
+#include <sstream>
+
+namespace wolf
+{
+class CheckLog
+{
+ public:
+ bool is_consistent_;
+ std::string explanation_;
+
+ CheckLog();
+ CheckLog(bool _consistent, std::string _explanation);
+ ~CheckLog(){};
+ void compose(CheckLog l);
+ void assertTrue(bool _condition, std::stringstream& _stream);
+};
+} // namespace wolf
+#endif
diff --git a/include/core/utils/check_log.hpp b/include/core/utils/check_log.hpp
deleted file mode 100644
index db3ab2dc9dd6e165c3541544bf7770022568f2fe..0000000000000000000000000000000000000000
--- a/include/core/utils/check_log.hpp
+++ /dev/null
@@ -1,32 +0,0 @@
-#ifndef CHECK_LOG_HPP
-#define CHECK_LOG_HPP
-#include <iostream>
-#include <string>
-
-class CheckLog {
-
-public:
-
- bool is_consistent_;
- std::string explanation_;
-
- CheckLog() {
- is_consistent_ = true;
- explanation_ = "";
- }
- CheckLog(bool consistent, std::string explanation) {
- is_consistent_ = consistent;
- if (not consistent)
- explanation_ = explanation;
- else
- explanation_ = "";
- }
- ~CheckLog(){};
- void compose(CheckLog l) {
-
- CheckLog result_log;
- is_consistent_ = is_consistent_ and l.is_consistent_;
- explanation_ = explanation_ + l.explanation_;
- }
-};
-#endif
diff --git a/include/core/utils/loader.h b/include/core/utils/loader.h
new file mode 100644
index 0000000000000000000000000000000000000000..6f1a4cc68c2d0dcb20a6ef18f53ec259507fc176
--- /dev/null
+++ b/include/core/utils/loader.h
@@ -0,0 +1,32 @@
+#ifndef LOADER_H
+#define LOADER_H
+
+#include <string>
+
+class Loader{
+protected:
+ std::string path_;
+public:
+ Loader(std::string _file);
+ virtual void load() = 0;
+ virtual void close() = 0;
+};
+
+class LoaderRaw: public Loader{
+ void* resource_;
+public:
+ LoaderRaw(std::string _file);
+ ~LoaderRaw();
+ void load();
+ void close();
+};
+// class LoaderPlugin: public Loader{
+// ClassLoader* resource_;
+// void load(){
+// this->resource_ = new ClassLoader(this->path_);
+// }
+// void close(){
+// delete this->resource_;
+// }
+// };
+#endif
diff --git a/include/core/utils/loader.hpp b/include/core/utils/loader.hpp
deleted file mode 100644
index 3d867d799b36bfd53fb42e93577554caf1b050c1..0000000000000000000000000000000000000000
--- a/include/core/utils/loader.hpp
+++ /dev/null
@@ -1,45 +0,0 @@
-#ifndef LOADER_HPP
-#define LOADER_HPP
-#include <dlfcn.h>
-#include <string>
-#include <stdexcept>
-class Loader{
-protected:
- std::string path_;
-public:
- Loader(std::string _file){
- this->path_ = _file;
- }
- virtual void load() = 0;
- virtual void close() = 0;
-};
-class LoaderRaw: public Loader{
- void* resource_;
-public:
- LoaderRaw(std::string _file):
- Loader(_file)
- {
- //
- }
- ~LoaderRaw(){
- this->close();
- }
- void load(){
- this->resource_ = dlopen(this->path_.c_str(), RTLD_LAZY);
- if(not this->resource_)
- throw std::runtime_error("Couldn't load resource with path " + this->path_ + "\n" + "Error info: " + dlerror());
- }
- void close(){
- if(this->resource_) dlclose(this->resource_);
- }
-};
-// class LoaderPlugin: public Loader{
-// ClassLoader* resource_;
-// void load(){
-// this->resource_ = new ClassLoader(this->path_);
-// }
-// void close(){
-// delete this->resource_;
-// }
-// };
-#endif
\ No newline at end of file
diff --git a/include/core/utils/params_server.h b/include/core/utils/params_server.h
new file mode 100644
index 0000000000000000000000000000000000000000..e8b473ca26703f17ea932cca4d1e913717108026
--- /dev/null
+++ b/include/core/utils/params_server.h
@@ -0,0 +1,55 @@
+#ifndef PARAMS_SERVER_H
+#define PARAMS_SERVER_H
+
+#include "core/utils/converter.h"
+
+#include <map>
+#include <exception>
+
+namespace wolf{
+
+class MissingValueException : public std::runtime_error
+{
+public:
+ MissingValueException(std::string _msg) : std::runtime_error(_msg) {}
+};
+
+class ParamsServer{
+ std::map<std::string, std::string> params_;
+public:
+ ParamsServer();
+ ParamsServer(std::map<std::string, std::string> _params);
+ ~ParamsServer(){
+ //
+ }
+
+ void print();
+
+
+ void addParam(std::string _key, std::string _value);
+
+ void addParams(std::map<std::string, std::string> _params);
+
+ // template<typename T>
+ // T getParam(std::string key, std::string def_value) const {
+ // if(params_.find(key) != params_.end()){
+ // return converter<T>::convert(params_.find(key)->second);
+ // }else{
+ // return converter<T>::convert(def_value);
+ // }
+ // }
+
+ template<typename T>
+ T getParam(std::string _key) const {
+ if(params_.find(_key) != params_.end()){
+ return converter<T>::convert(params_.find(_key)->second);
+ }else{
+ throw MissingValueException("The following key: '" + _key + "' has not been found in the parameters server.");
+ }
+ }
+
+};
+
+}
+
+#endif
diff --git a/include/core/utils/params_server.hpp b/include/core/utils/params_server.hpp
deleted file mode 100644
index a750eab79f4ee90b39e4e21d1926129897c29d92..0000000000000000000000000000000000000000
--- a/include/core/utils/params_server.hpp
+++ /dev/null
@@ -1,70 +0,0 @@
-#ifndef PARAMS_SERVER_HPP
-#define PARAMS_SERVER_HPP
-
-#include "core/utils/converter.h"
-//#include "core/yaml/parser_yaml.hpp"
-
-#include <vector>
-#include <regex>
-#include <map>
-#include <exception>
-
-namespace wolf{
-
-class MissingValueException : public std::runtime_error
-{
-public:
- MissingValueException(std::string msg) : std::runtime_error(msg) {}
-};
-
-class ParamsServer{
- std::map<std::string, std::string> _params;
-public:
- ParamsServer(){
- _params = std::map<std::string, std::string>();
- }
- ParamsServer(std::map<std::string, std::string> params){
- _params = params;
- }
- ~ParamsServer(){
- //
- }
-
- void print(){
- for(auto it : _params)
- std::cout << it.first << "~~" << it.second << std::endl;
- }
-
-
- void addParam(std::string key, std::string value){
- _params.insert(std::pair<std::string, std::string>(key, value));
- }
-
- void addParams(std::map<std::string, std::string> params)
- {
- _params.insert(params.begin(), params.end());
- }
-
- // template<typename T>
- // T getParam(std::string key, std::string def_value) const {
- // if(_params.find(key) != _params.end()){
- // return converter<T>::convert(_params.find(key)->second);
- // }else{
- // return converter<T>::convert(def_value);
- // }
- // }
-
- template<typename T>
- T getParam(std::string key) const {
- if(_params.find(key) != _params.end()){
- return converter<T>::convert(_params.find(key)->second);
- }else{
- throw MissingValueException("The following key: '" + key + "' has not been found in the parameters server.");
- }
- }
-
-};
-
-}
-
-#endif
diff --git a/include/core/utils/utils_gtest.h b/include/core/utils/utils_gtest.h
index e20659233e26c021129f4e72253632e726a08794..53358effaeeb707c7db55dbde51568d68e5efdb6 100644
--- a/include/core/utils/utils_gtest.h
+++ b/include/core/utils/utils_gtest.h
@@ -127,14 +127,14 @@ TEST(Test, Foo)
#define ASSERT_QUATERNION_APPROX(C_expect, C_actual, precision) ASSERT_MATRIX_APPROX((C_expect).coeffs(), (C_actual).coeffs(), precision)
-#define EXPECT_POSE2D_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::MatrixXd lhs, const Eigen::MatrixXd rhs) { \
+#define EXPECT_POSE2d_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::MatrixXd lhs, const Eigen::MatrixXd rhs) { \
MatrixXd er = lhs - rhs; \
er(2) = pi2pi((double)er(2)); \
return er.isMuchSmallerThan(1, precision); \
}, \
C_expect, C_actual);
-#define ASSERT_POSE2D_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::MatrixXd lhs, const Eigen::MatrixXd rhs) { \
+#define ASSERT_POSE2d_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::MatrixXd lhs, const Eigen::MatrixXd rhs) { \
MatrixXd er = lhs - rhs; \
er(2) = pi2pi((double)er(2)); \
return er.isMuchSmallerThan(1, precision); \
diff --git a/include/core/yaml/parser_yaml.h b/include/core/yaml/parser_yaml.h
new file mode 100644
index 0000000000000000000000000000000000000000..1b6f659ebd609dffef4f3c472800cf32714f22e1
--- /dev/null
+++ b/include/core/yaml/parser_yaml.h
@@ -0,0 +1,80 @@
+#ifndef PARSER_YAML_H
+#define PARSER_YAML_H
+
+#include "core/utils/converter.h"
+#include "core/common/wolf.h"
+
+#include "yaml-cpp/yaml.h"
+
+namespace wolf
+{
+class ParserYAML {
+ struct ParamsInitSensor{
+ std::string type_;
+ std::string name_;
+ std::string plugin_;
+ YAML::Node n_;
+ };
+ struct ParamsInitProcessor{
+ std::string type_;
+ std::string name_;
+ std::string name_assoc_sensor_;
+ std::string plugin_;
+ YAML::Node n_;
+ };
+ struct SubscriberManager{
+ std::string package_;
+ std::string subscriber_;
+ std::string topic_;
+ std::string sensor_name_;
+ YAML::Node n_;
+ };
+ struct PublisherManager{
+ std::string subscriber_;
+ std::string topic_;
+ std::string period_;
+ YAML::Node n_;
+ };
+ std::map<std::string, std::string> params_;
+ std::string active_name_;
+ std::vector<ParamsInitSensor> paramsSens_;
+ std::vector<ParamsInitProcessor> paramsProc_;
+ std::stack<std::string> parsing_file_;
+ std::string file_;
+ std::string path_root_;
+ std::vector<SubscriberManager> subscriber_managers_;
+ std::vector<PublisherManager> publisher_managers_;
+ YAML::Node problem;
+ std::string generatePath(std::string);
+ YAML::Node loadYAML(std::string);
+ void insert_register(std::string, std::string);
+public:
+ ParserYAML(std::string _file, std::string _path_root = "",
+ bool _freely_parse = false);
+ ~ParserYAML()
+ {
+ //
+ }
+ void parse_freely();
+ std::map<std::string, std::string> getParams();
+
+ private:
+ void walkTree(std::string _file);
+ void walkTree(std::string _file, std::vector<std::string>& _tags);
+ void walkTree(std::string _file, std::vector<std::string>& _tags, std::string _hdr);
+/** @Brief Recursively walks the YAML tree while filling a map with the values parsed from the file
+ * @param YAML node to be parsed
+ * @param tags represents the path from the root of the YAML tree to the current node
+ * @param hdr is the name of the current YAML node
+ */
+ void walkTreeR(YAML::Node _n, std::vector<std::string>& _tags, std::string _hdr);
+ void updateActiveName(std::string _tag);
+/** @Brief Parse the sensors, processors, callbacks and files elements of the YAML file. We assume that these elements
+ are defined at the top level of the YAML file.
+ * @param file is the path to the YAML file */
+ void parseFirstLevel(std::string _file);
+ std::string tagsToString(std::vector<std::string>& _tags);
+ void parse();
+};
+}
+#endif
diff --git a/include/core/yaml/parser_yaml.hpp b/include/core/yaml/parser_yaml.hpp
deleted file mode 100644
index 22f584d76bab2aa2c3a6bd6ca30edbeb4873a4ea..0000000000000000000000000000000000000000
--- a/include/core/yaml/parser_yaml.hpp
+++ /dev/null
@@ -1,532 +0,0 @@
-#ifndef PARSER_YAML_HPP
-#define PARSER_YAML_HPP
-
-#include "core/utils/converter.h"
-#include "core/common/wolf.h"
-
-#include "yaml-cpp/yaml.h"
-
-#include <string>
-#include <vector>
-#include <list>
-#include <stack>
-#include <regex>
-#include <map>
-#include <iostream>
-#include <algorithm>
-#include <numeric>
-
-namespace {
- //====== START OF FORWARD DECLARATION ========
- std::string parseAtomicNode(YAML::Node);
- std::string fetchMapEntry(YAML::Node);
- std::string mapToString(std::map<std::string,std::string>);
- //====== END OF FORWARD DECLARATION ========
-
-/** @Brief Interprets a map as being atomic and thus parses it as a single entity. We assume that the map has as values only scalars and sequences.
- * @param n the node representing a map
- * @return std::map<std::string, std::string> populated with the key,value pairs in n
- */
-std::map<std::string, std::string> fetchAsMap(YAML::Node n){
- assert(n.Type() == YAML::NodeType::Map && "trying to fetch as Map a non-Map node");
- auto m = std::map<std::string, std::string>();
- for(const auto& kv : n){
- std::string key = kv.first.as<std::string>();
- switch (kv.second.Type()) {
- case YAML::NodeType::Scalar : {
- std::string value = kv.second.Scalar();
- m.insert(std::pair<std::string,std::string>(key, value));
- break;
- }
- case YAML::NodeType::Sequence : {
- std::string aux = parseAtomicNode(kv.second);
- m.insert(std::pair<std::string,std::string>(key, aux));
- break;
- }
- case YAML::NodeType::Map : {
- std::string value = fetchMapEntry(kv.second);
- std::regex r("^\\$.*");
- if (std::regex_match(key, r)) key = key.substr(1,key.size()-1);
- m.insert(std::pair<std::string,std::string>(key, value));
- break;
- }
- default:
- assert(1 == 0 && "Unsupported node Type at fetchAsMap");
- break;
- }
- }
- return m;
-}
- std::string fetchMapEntry(YAML::Node n){
- switch (n.Type()) {
- case YAML::NodeType::Scalar: {
- return n.Scalar();
- break;
- }
- case YAML::NodeType::Sequence: {
- return parseAtomicNode(n);
- break;
- }
- case YAML::NodeType::Map: {
- return mapToString(fetchAsMap(n));
- break;
- }
- default: {
- assert(1 == 0 && "Unsupported node Type at fetchMapEntry");
- return "";
- break;
- }
- }
- }
- /** @Brief Transforms a std::map<std::string,std::string> to its std::string representation [{k1:v1},{k2:v2},{k3:v3},...]
- * @param _map just a std::map<std::string,std::string>
- * @return <b>{std::string}</b> [{k1:v1},{k2:v2},{k3:v3},...]
- */
- std::string mapToString(std::map<std::string,std::string> _map){
- std::string result = "";
- auto v = std::vector<std::string>();
- std::transform(_map.begin(), _map.end(), back_inserter(v), [](const std::pair<std::string,std::string> p){return "{" + p.first + ":" + p.second + "}";});
- auto concat = [](std::string ac, std::string str)-> std::string {
- return ac + str + ",";
- };
- std::string aux = "";
- std::string accumulated = std::accumulate(v.begin(), v.end(), aux, concat);
- if(accumulated.size() > 1) accumulated = accumulated.substr(0,accumulated.size() - 1);
- else accumulated = "";
- return "[" + accumulated + "]";
- }
- /** @Brief Generates a std::string representing a YAML sequence. The sequence is assumed to be scalar or at most be a sequence of sequences of scalars.
- * @param n a vector of YAML::Node that represents a YAML::Sequence
- * @return <b>{std::string}</b> representing the YAML sequence
- */
- std::string parseAtomicNode(YAML::Node n){
- std::string aux = "";
- bool first = true;
- std::string separator = "";
- switch(n.Type()){
- case YAML::NodeType::Scalar:
- return n.Scalar();
- break;
- case YAML::NodeType::Sequence:
- for(auto it : n){
- aux += separator + parseAtomicNode(it);
- if(first){
- separator = ",";
- first = false;
- }
- }
- return "[" + aux + "]";
- break;
- case YAML::NodeType::Map:
- return mapToString(fetchAsMap(n));
- break;
- default:
- return "";
- break;
- }
- }
-
- /** @Brief checks if a node of the YAML tree is atomic. Only works if the nodes are of type
- * Scalar, Sequence or Map.
- * @param key is the key associated to the node n if n.Type() == YAML::NodeType::Map
- * @param n node to be test for atomicity
- */
- bool isAtomic(std::string key, YAML::Node n){
- assert(n.Type() != YAML::NodeType::Undefined && n.Type() != YAML::NodeType::Null && "Cannot determine atomicity of Undefined/Null node");
- std::regex r("^\\$.*");
- bool is_atomic = true;
- switch(n.Type()){
- case YAML::NodeType::Scalar:
- return true;
- break;
- case YAML::NodeType::Sequence:
- for(auto it : n) {
- switch(it.Type()){
- case YAML::NodeType::Map:
- for(const auto& kv : it){
- is_atomic = is_atomic and isAtomic(kv.first.as<std::string>(), it);
- }
- break;
- default:
- is_atomic = is_atomic and isAtomic("", it);
- break;
- }
- }
- return is_atomic;
- break;
- case YAML::NodeType::Map:
- is_atomic = std::regex_match(key, r);
- return is_atomic;
- break;
- default:
- throw std::runtime_error("Cannot determine atomicity of node type " + std::to_string(n.Type()));
- return false;
- break;
- }
- return false;
- }
-}
-class ParserYAML {
- struct ParamsInitSensor{
- std::string _type;
- std::string _name;
- std::string _plugin;
- YAML::Node n;
- };
- struct ParamsInitProcessor{
- std::string _type;
- std::string _name;
- std::string _name_assoc_sensor;
- std::string _plugin;
- YAML::Node n;
- };
- struct SubscriberManager{
- std::string _package;
- std::string _subscriber;
- std::string _topic;
- std::string _sensor_name;
- YAML::Node n;
- };
- struct PublisherManager{
- std::string _subscriber;
- std::string _topic;
- std::string _period;
- YAML::Node n;
- };
- std::map<std::string, std::string> _params;
- std::string _active_name;
- std::vector<ParamsInitSensor> _paramsSens;
- std::vector<ParamsInitProcessor> _paramsProc;
- std::stack<std::string> _parsing_file;
- std::string _file;
- std::string _path_root;
- std::vector<SubscriberManager> _subscriber_managers;
- std::vector<PublisherManager> _publisher_managers;
- YAML::Node problem;
- std::string generatePath(std::string);
- YAML::Node loadYAML(std::string);
- void insert_register(std::string, std::string);
-public:
- ParserYAML(std::string file, std::string path_root = "",
- bool freely_parse = false) {
- _params = std::map<std::string, std::string>();
- _active_name = "";
- _paramsSens = std::vector<ParamsInitSensor>();
- _paramsProc = std::vector<ParamsInitProcessor>();
- _subscriber_managers = std::vector<SubscriberManager>();
- _publisher_managers = std::vector<PublisherManager>();
- _parsing_file = std::stack<std::string>();
- _file = file;
- if (path_root != "") {
- std::regex r(".*/ *$");
- if (not std::regex_match(path_root, r))
- _path_root = path_root + "/";
- else
- _path_root = path_root;
- }
- if(not freely_parse) this->parse();
- else this->parse_freely();
- }
- ~ParserYAML()
- {
- //
- }
- void parse_freely();
- std::map<std::string, std::string> getParams();
-
- private:
- void walkTree(std::string file);
- void walkTree(std::string file, std::vector<std::string>& tags);
- void walkTree(std::string file, std::vector<std::string>& tags, std::string hdr);
- void walkTreeR(YAML::Node n, std::vector<std::string>& tags, std::string hdr);
- void updateActiveName(std::string tag);
- void parseFirstLevel(std::string file);
- std::string tagsToString(std::vector<std::string>& tags);
- void parse();
-};
-std::string ParserYAML::generatePath(std::string file){
- std::regex r("^/.*");
- if(std::regex_match(file, r)){
- return file;
- }else{
- return _path_root + file;
- }
-}
-YAML::Node ParserYAML::loadYAML(std::string file){
- try{
- // std::cout << "Parsing " << generatePath(file) << std::endl;
- WOLF_INFO("Parsing file: ", generatePath(file));
- return YAML::LoadFile(generatePath(file));
- }catch (YAML::BadFile& e){
- throw std::runtime_error("Couldn't load file " + generatePath(file) + ". Tried to open it from " + this->_parsing_file.top());
- }
-}
-std::string ParserYAML::tagsToString(std::vector<std::string> &tags){
- std::string hdr = "";
- for(auto it : tags){
- hdr = hdr + "/" + it;
- }
- return hdr;
-}
-void ParserYAML::walkTree(std::string file){
- YAML::Node n;
- n = loadYAML(generatePath(file));
- this->_parsing_file.push(generatePath(file));
- std::vector<std::string> hdrs = std::vector<std::string>();
- walkTreeR(n, hdrs, "");
- this->_parsing_file.pop();
-}
-void ParserYAML::walkTree(std::string file, std::vector<std::string>& tags){
- YAML::Node n;
- n = loadYAML(generatePath(file));
- this->_parsing_file.push(generatePath(file));
- walkTreeR(n, tags, "");
- this->_parsing_file.pop();
-}
-void ParserYAML::walkTree(std::string file, std::vector<std::string>& tags, std::string hdr){
- YAML::Node n;
- n = loadYAML(generatePath(file));
- this->_parsing_file.push(generatePath(file));
- walkTreeR(n, tags, hdr);
- this->_parsing_file.pop();
-}
-/** @Brief Recursively walks the YAML tree while filling a map with the values parsed from the file
-* @param YAML node to be parsed
-* @param tags represents the path from the root of the YAML tree to the current node
-* @param hdr is the name of the current YAML node
-*/
-void ParserYAML::walkTreeR(YAML::Node n, std::vector<std::string>& tags, std::string hdr){
- switch (n.Type()) {
- case YAML::NodeType::Scalar : {
- std::regex r("^@.*");
- if(std::regex_match(n.Scalar(), r)){
- std::string str = n.Scalar();
- walkTree(str.substr(1,str.size() - 1), tags, hdr);
- }else{
- insert_register(hdr, n.Scalar());
- }
- break;
- }
- case YAML::NodeType::Sequence : {
- if(isAtomic("", n)){
- insert_register(hdr, parseAtomicNode(n));
- }else{
- for(const auto& kv : n){
- walkTreeR(kv, tags, hdr);
- }
- }
- break;
- }
- case YAML::NodeType::Map : {
- for(const auto& kv : n){
- if(isAtomic(kv.first.as<std::string>(), n)){
- std::string key = kv.first.as<std::string>();
- //WOLF_DEBUG("KEY IN MAP ATOMIC ", hdr + "/" + key);
- key = key.substr(1,key.size() - 1);
- insert_register(hdr + "/" + key, parseAtomicNode(kv.second));
- }else{
- /*
- If key=="follow" then the parser will assume that the value is a path and will parse
- the (expected) yaml file at the specified path. Note that this does not increase the header depth.
- The following example shows how the header remains unafected:
- @my_main_config | @some_path
- - cov_det: 1 | - my_value : 23
- - follow: "@some_path" |
- - var: 1.2 |
- Resulting map:
- cov_det -> 1
- my_value-> 23
- var: 1.2
- Instead of:
- cov_det -> 1
- follow/my_value-> 23
- var: 1.2
- Which would result from the following yaml files
- @my_main_config | @some_path
- - cov_det: 1 | - my_value : 23
- - $follow: "@some_path" |
- - var: 1.2 |
- */
- std::string key = kv.first.as<std::string>();
- //WOLF_DEBUG("KEY IN MAP NON ATOMIC ", key);
- std::regex rr("follow");
- if(not std::regex_match(kv.first.as<std::string>(), rr)) {
- tags.push_back(kv.first.as<std::string>());
- if(tags.size() == 2) this->updateActiveName(kv.first.as<std::string>());
- walkTreeR(kv.second, tags, hdr +"/"+ kv.first.as<std::string>());
- tags.pop_back();
- if(tags.size() == 1) this->updateActiveName("");
- }else{
- walkTree(kv.second.as<std::string>(), tags, hdr);
- }
- }
- }
- break;
- }
- case YAML::NodeType::Null :
- break;
- default:
- assert(1 == 0 && "Unsupported node Type at walkTreeR.");
- break;
- }
-}
-void ParserYAML::updateActiveName(std::string tag){
- this->_active_name = tag;
-}
-/** @Brief Parse the sensors, processors, callbacks and files elements of the YAML file. We assume that these elements
- are defined at the top level of the YAML file.
- * @param file is the path to the YAML file */
-void ParserYAML::parseFirstLevel(std::string file){
- YAML::Node n;
- n = loadYAML(generatePath(file));
-
- YAML::Node n_config = n["config"];
- // assert(n_config.Type() == YAML::NodeType::Map && "trying to parse config node but found a non-Map node");
- if(n_config.Type() != YAML::NodeType::Map) throw std::runtime_error("Could not find config node. Please make sure that your YAML file " + generatePath(file) + " starts with 'config:'");
- if(n_config["problem"].Type() != YAML::NodeType::Map) throw std::runtime_error("Could not find problem node. Please make sure that the 'config' node in YAML file " + generatePath(file) + " has a 'problem' entry");
- this->problem = n_config["problem"];
- std::vector<std::map<std::string, std::string>> map_container;
- try{
- for(const auto& kv : n_config["sensors"]){
- ParamsInitSensor pSensor = {kv["type"].Scalar(), kv["name"].Scalar(), kv["plugin"].Scalar(), kv};
- _paramsSens.push_back(pSensor);
- map_container.push_back(std::map<std::string, std::string>({
- {"type", kv["type"].Scalar()},
- {"name", kv["name"].Scalar()},
- {"plugin", kv["plugin"].Scalar()}
- }));
- }
- insert_register("sensors", wolf::converter<std::string>::convert(map_container));
- map_container.clear();
- } catch(YAML::InvalidNode& e){
- throw std::runtime_error("Error parsing sensors @" + generatePath(file) + ". Please make sure that each sensor entry has 'type', 'name' and 'plugin' entries.");
- }
-
- try{
- for(const auto& kv : n_config["processors"]){
- ParamsInitProcessor pProc = {kv["type"].Scalar(), kv["name"].Scalar(), kv["sensor_name"].Scalar(), kv["plugin"].Scalar(), kv};
- _paramsProc.push_back(pProc);
- map_container.push_back(std::map<std::string, std::string>({
- {"type", kv["type"].Scalar()},
- {"name", kv["name"].Scalar()},
- {"plugin", kv["plugin"].Scalar()},
- {"sensor_name", kv["sensor_name"].Scalar()}}));
- }
- insert_register("processors",
- wolf::converter<std::string>::convert(map_container));
- map_container.clear();
- } catch(YAML::InvalidNode& e){
- throw std::runtime_error("Error parsing processors @" + generatePath(file) + ". Please make sure that each processor has 'type', 'name', 'plugin' and 'sensor_name' entries.");
- }
- try {
- for (const auto &kv : n_config["ROS subscriber"]) {
- SubscriberManager pSubscriberManager = {kv["package"].Scalar(), kv["type"].Scalar(), kv["topic"].Scalar(), kv["sensor_name"].Scalar(), kv};
- _subscriber_managers.push_back(pSubscriberManager);
- map_container.push_back(std::map<std::string, std::string>({
- {"package", kv["package"].Scalar()},
- {"type", kv["type"].Scalar()},
- {"topic", kv["topic"].Scalar()},
- {"sensor_name", kv["sensor_name"].Scalar()}}));
- }
- insert_register("ROS subscriber", wolf::converter<std::string>::convert(map_container));
- map_container.clear();
- } catch (YAML::InvalidNode &e) {
- throw std::runtime_error("Error parsing subscriber @" + generatePath(file) + ". Please make sure that each manager has 'package', 'type', 'topic' and 'sensor_name' entries.");
- }
-
- try {
- for (const auto &kv : n_config["ROS publisher"]) {
- WOLF_DEBUG("WHAT");
- PublisherManager pPublisherManager = {kv["type"].Scalar(), kv["topic"].Scalar(), kv["period"].Scalar(), kv};
- _publisher_managers.push_back(pPublisherManager);
- map_container.push_back(std::map<std::string, std::string>({
- {"type", kv["type"].Scalar()},
- {"topic", kv["topic"].Scalar()},
- {"period", kv["period"].Scalar()}}));
- }
- insert_register("ROS publisher", wolf::converter<std::string>::convert(map_container));
- map_container.clear();
- } catch (YAML::InvalidNode &e) {
- throw std::runtime_error("Error parsing publisher @" + generatePath(file) + ". Please make sure that each manager has 'type', 'topic' and 'period' entries.");
- }
-}
-std::map<std::string,std::string> ParserYAML::getParams(){
- std::map<std::string,std::string> rtn = _params;
- return rtn;
-}
-void ParserYAML::parse(){
- this->_parsing_file.push(generatePath(this->_file));
- this->parseFirstLevel(this->_file);
-
- if(this->problem.Type() != YAML::NodeType::Undefined){
- std::vector<std::string> tags = std::vector<std::string>();
- this->walkTreeR(this->problem, tags , "problem");
- }
- for(auto it : _paramsSens){
- std::vector<std::string> tags = std::vector<std::string>();
- tags.push_back("sensor");
- this->walkTreeR(it.n , tags , "sensor/" + it._name);
- }
- for(auto it : _paramsProc){
- std::vector<std::string> tags = std::vector<std::string>();
- tags.push_back("processor");
- this->walkTreeR(it.n , tags , "processor/" + it._name);
- }
- std::list<std::string> plugins, packages;
- for (const auto& it : this->_paramsSens)
- plugins.push_back(it._plugin);
- for (const auto& it : this->_paramsProc)
- plugins.push_back(it._plugin);
- for (const auto& it : this->_subscriber_managers)
- packages.push_back(it._package);
- plugins.sort();
- plugins.unique();
- packages.sort();
- packages.unique();
- insert_register("plugins", wolf::converter<std::string>::convert(plugins));
- insert_register("packages", wolf::converter<std::string>::convert(packages));
-
- YAML::Node n;
- n = loadYAML(generatePath(this->_file));
-
- if (n.Type() == YAML::NodeType::Map)
- {
- for (auto it : n)
- {
- auto node = it.second;
- // WOLF_INFO("WUT ", it.first);
- std::vector<std::string> tags = std::vector<std::string>();
- if(it.first.as<std::string>() != "config")
- this->walkTreeR(node, tags, it.first.as<std::string>());
- else
- {
- for (auto itt : node)
- {
- std::string node_key = itt.first.as<std::string>();
- if (node_key != "problem" and node_key != "sensors" and node_key != "processors" and
- node_key != "ROS subscriber" and node_key != "ROS publisher")
- {
- this->walkTreeR(itt.second, tags, node_key);
- }
- }
- }
- }
- }else
- {
- std::vector<std::string> tags = std::vector<std::string>();
- this->walkTreeR(n, tags, "");
- }
- this->_parsing_file.pop();
- }
-void ParserYAML::parse_freely(){
- this->_parsing_file.push(generatePath(this->_file));
- std::vector<std::string> tags = std::vector<std::string>();
- this->walkTreeR(loadYAML(this->_file), tags, "");
- this->_parsing_file.pop();
-}
-void ParserYAML::insert_register(std::string key, std::string value){
- if(key.substr(0,1) == "/") key = key.substr(1,key.size()-1);
- auto inserted_it = _params.insert(std::pair<std::string, std::string>(key, value));
- if(not inserted_it.second) WOLF_WARN("Skipping key '", key, "' with value '", value, "'. There already exists the register: (", inserted_it.first->first, ",", inserted_it.first->second, ")");
-}
-#endif
diff --git a/scilab/corner_detector.sce b/scilab/corner_detector.sce
index 18035de71787e22dd3d53adc8002cbbac8f873f6..dfeae1945084531266c5ffd2019a17ec09e6c809 100644
--- a/scilab/corner_detector.sce
+++ b/scilab/corner_detector.sce
@@ -1,4 +1,4 @@
-//info about 2D homogeneous lines and points: http://homepages.inf.ed.ac.uk/rbf/CVonline/LOCAL_COPIES/BEARDSLEY/node2.html
+//info about 2d homogeneous lines and points: http://homepages.inf.ed.ac.uk/rbf/CVonline/LOCAL_COPIES/BEARDSLEY/node2.html
// clear all
xdel(winsid());
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index aacaec893b224975fb99f5d79fd03a87d1740d2d..949bf1740e13096a5ceafc2bb301a7f39d061fc4 100644
--- a/src/capture/capture_base.cpp
+++ b/src/capture/capture_base.cpp
@@ -26,7 +26,7 @@ CaptureBase::CaptureBase(const std::string& _type,
if (_sensor_ptr->getP() != nullptr and _sensor_ptr->isStateBlockDynamic("P"))
{
assert(_p_ptr && "Pointer to dynamic position params is null!");
- addStateBlock("P", _p_ptr);
+ addStateBlock("P", _p_ptr, nullptr);
}
else
assert(_p_ptr == nullptr && "Provided dynamic sensor position but the sensor position is static or doesn't exist");
@@ -34,7 +34,7 @@ CaptureBase::CaptureBase(const std::string& _type,
if (_sensor_ptr->getO() != nullptr and _sensor_ptr->isStateBlockDynamic("O"))
{
assert(_o_ptr && "Pointer to dynamic orientation params is null!");
- addStateBlock("O", _o_ptr);
+ addStateBlock("O", _o_ptr, nullptr);
}
else
assert(_o_ptr == nullptr && "Provided dynamic sensor orientation but the sensor orientation is static or doesn't exist");
@@ -42,7 +42,7 @@ CaptureBase::CaptureBase(const std::string& _type,
if (_sensor_ptr->getIntrinsic() != nullptr and _sensor_ptr->isStateBlockDynamic("I"))
{
assert(_intr_ptr && "Pointer to dynamic intrinsic params is null!");
- addStateBlock("I", _intr_ptr);
+ addStateBlock("I", _intr_ptr, nullptr);
}
else
assert(_intr_ptr == nullptr && "Provided dynamic sensor intrinsics but the sensor intrinsics are static or don't exist");
@@ -130,6 +130,22 @@ void CaptureBase::removeConstrainedBy(FactorBasePtr _fac_ptr)
constrained_by_list_.remove(_fac_ptr);
}
+bool CaptureBase::isConstrainedBy(const FactorBasePtr &_factor) const
+{
+ FactorBaseConstIter cby_it = std::find_if(constrained_by_list_.begin(),
+ constrained_by_list_.end(),
+ [_factor](const FactorBasePtr & cby)
+ {
+ return cby == _factor;
+ }
+ );
+ if (cby_it != constrained_by_list_.end())
+ return true;
+ else
+ return false;
+}
+
+
const StateStructure& CaptureBase::getStructure() const
{
if (getSensor())
@@ -140,7 +156,7 @@ const StateStructure& CaptureBase::getStructure() const
StateBlockPtr CaptureBase::getStateBlock(const std::string& _key) const
{
- if (getSensor() and getSensor()->getStateBlock(_key))
+ if (getSensor() and getSensor()->hasStateBlock(_key))
{
if (getSensor()->isStateBlockDynamic(_key))
return HasStateBlocks::getStateBlock(_key);
diff --git a/src/capture/capture_odom_2D.cpp b/src/capture/capture_odom_2d.cpp
similarity index 65%
rename from src/capture/capture_odom_2D.cpp
rename to src/capture/capture_odom_2d.cpp
index d443135aa8f8cd3705e143d9a1f38399a32bd056..b024294a5df38772bc040495728958aa8cb08c4c 100644
--- a/src/capture/capture_odom_2D.cpp
+++ b/src/capture/capture_odom_2d.cpp
@@ -1,35 +1,35 @@
/*
- * capture_odom_2D.cpp
+ * capture_odom_2d.cpp
*
* Created on: Oct 16, 2017
* Author: jsola
*/
-#include "core/capture/capture_odom_2D.h"
+#include "core/capture/capture_odom_2d.h"
namespace wolf
{
-CaptureOdom2D::CaptureOdom2D(const TimeStamp& _init_ts,
+CaptureOdom2d::CaptureOdom2d(const TimeStamp& _init_ts,
SensorBasePtr _sensor_ptr,
const Eigen::VectorXd& _data,
CaptureBasePtr _capture_origin_ptr):
- CaptureMotion("CaptureOdom2D", _init_ts, _sensor_ptr, _data, 3, 3, _capture_origin_ptr)
+ CaptureMotion("CaptureOdom2d", _init_ts, _sensor_ptr, _data, 3, 3, _capture_origin_ptr)
{
//
}
-CaptureOdom2D::CaptureOdom2D(const TimeStamp& _init_ts,
+CaptureOdom2d::CaptureOdom2d(const TimeStamp& _init_ts,
SensorBasePtr _sensor_ptr,
const Eigen::VectorXd& _data,
const Eigen::MatrixXd& _data_cov,
CaptureBasePtr _capture_origin_ptr):
- CaptureMotion("CaptureOdom2D", _init_ts, _sensor_ptr, _data, _data_cov, 3, 3, _capture_origin_ptr)
+ CaptureMotion("CaptureOdom2d", _init_ts, _sensor_ptr, _data, _data_cov, 3, 3, _capture_origin_ptr)
{
//
}
-CaptureOdom2D::~CaptureOdom2D()
+CaptureOdom2d::~CaptureOdom2d()
{
//
}
diff --git a/src/capture/capture_odom_3D.cpp b/src/capture/capture_odom_3d.cpp
similarity index 68%
rename from src/capture/capture_odom_3D.cpp
rename to src/capture/capture_odom_3d.cpp
index 252885c51f394d235751ed4b25496682107ed8b0..447e1d500f072e2754de4ed107aeb61e6e6299c0 100644
--- a/src/capture/capture_odom_3D.cpp
+++ b/src/capture/capture_odom_3d.cpp
@@ -1,40 +1,40 @@
/*
- * capture_odom_3D.cpp
+ * capture_odom_3d.cpp
*
* Created on: Oct 16, 2017
* Author: jsola
*/
-#include "core/capture/capture_odom_3D.h"
+#include "core/capture/capture_odom_3d.h"
namespace wolf
{
-CaptureOdom3D::CaptureOdom3D(const TimeStamp& _init_ts,
+CaptureOdom3d::CaptureOdom3d(const TimeStamp& _init_ts,
SensorBasePtr _sensor_ptr,
const Eigen::VectorXd& _data,
CaptureBasePtr _capture_origin_ptr):
- CaptureMotion("CaptureOdom3D", _init_ts, _sensor_ptr, _data, 7, 6, _capture_origin_ptr)
+ CaptureMotion("CaptureOdom3d", _init_ts, _sensor_ptr, _data, 7, 6, _capture_origin_ptr)
{
//
}
-CaptureOdom3D::CaptureOdom3D(const TimeStamp& _init_ts,
+CaptureOdom3d::CaptureOdom3d(const TimeStamp& _init_ts,
SensorBasePtr _sensor_ptr,
const Eigen::VectorXd& _data,
const Eigen::MatrixXd& _data_cov,
CaptureBasePtr _capture_origin_ptr):
- CaptureMotion("CaptureOdom3D", _init_ts, _sensor_ptr, _data, _data_cov, 7, 6, _capture_origin_ptr)
+ CaptureMotion("CaptureOdom3d", _init_ts, _sensor_ptr, _data, _data_cov, 7, 6, _capture_origin_ptr)
{
//
}
-CaptureOdom3D::~CaptureOdom3D()
+CaptureOdom3d::~CaptureOdom3d()
{
//
}
-Eigen::VectorXd CaptureOdom3D::correctDelta(const VectorXd& _delta, const VectorXd& _delta_error) const
+Eigen::VectorXd CaptureOdom3d::correctDelta(const VectorXd& _delta, const VectorXd& _delta_error) const
{
VectorXd delta(7);
delta.head(3) = _delta.head(3) + _delta_error.head(3);
diff --git a/src/capture/capture_pose.cpp b/src/capture/capture_pose.cpp
index 4f2a85c751a5a7945fcdcfe12aa84a6f37265e10..190fa7090c698b4a613d0ebba2594e391043f925 100644
--- a/src/capture/capture_pose.cpp
+++ b/src/capture/capture_pose.cpp
@@ -25,11 +25,11 @@ void CapturePose::emplaceFeatureAndFactor()
// std::cout << data_.size() << " ~~ " << data_covariance_.rows() << "x" << data_covariance_.cols() << std::endl;
// Emplace factor
if (data_.size() == 3 && data_covariance_.rows() == 3 && data_covariance_.cols() == 3 )
- FactorBase::emplace<FactorPose2D>(feature_pose, feature_pose, nullptr, false);
+ FactorBase::emplace<FactorPose2d>(feature_pose, feature_pose, nullptr, false);
else if (data_.size() == 7 && data_covariance_.rows() == 6 && data_covariance_.cols() == 6 )
- FactorBase::emplace<FactorPose3D>(feature_pose, feature_pose, nullptr, false);
+ FactorBase::emplace<FactorPose3d>(feature_pose, feature_pose, nullptr, false);
else
- throw std::runtime_error("Wrong data size in CapturePose. Use 3 for 2D. Use 7 for 3D.");
+ throw std::runtime_error("Wrong data size in CapturePose. Use 3 for 2d. Use 7 for 3d.");
}
} // namespace wolf
diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp
index ecf57e7d7e29b4b8ccfecf09f8c39d8eacdafdbb..0d8cba6f97cb7e2c2cdf067e7ebcb65d8ac79c10 100644
--- a/src/ceres_wrapper/ceres_manager.cpp
+++ b/src/ceres_wrapper/ceres_manager.cpp
@@ -443,6 +443,77 @@ void CeresManager::check()
}
}
+void insertSparseBlock(const Eigen::MatrixXd& ins, Eigen::SparseMatrixd& original, const unsigned int& row, const unsigned int& col)
+{
+ for (auto ins_row = 0; ins_row < ins.rows(); ins_row++)
+ for (auto ins_col = 0; ins_col < ins.cols(); ins_col++)
+ original.insert(row+ins_row, col+ins_col) = ins(ins_row,ins_col);
+
+ original.makeCompressed();
+}
+
+const Eigen::SparseMatrixd CeresManager::computeHessian() const
+{
+ Eigen::SparseMatrixd H;
+ Eigen::SparseMatrixd A;
+ // fac_2_residual_idx_
+ // fac_2_costfunction_
+ // state_blocks_local_param_
+ int ix_row = 0; // index of the factor/measurement in the
+ for (auto fac_res_pair: fac_2_residual_idx_){
+ FactorBasePtr fac_ptr = fac_res_pair.first;
+ ix_row += fac_ptr->getSize();
+
+ // retrieve all stateblocks data related to this factor
+ std::vector<const double*> fac_states_ptr;
+ for (auto sb : fac_res_pair.first->getStateBlockPtrVector()){
+ fac_states_ptr.push_back(sb->getStateData());
+ }
+
+ // retrieve residual value, not used afterwards in this case since we just care about jacobians
+ Eigen::VectorXd residual(fac_ptr->getSize());
+ // retrieve jacobian in group size, not local size
+ std::vector<Eigen::MatrixXd> jacobians;
+ fac_ptr->evaluate(fac_states_ptr, residual, jacobians);
+
+ // Retrieve the block row sparse matrix of this factor
+ Eigen::SparseMatrixd A_block_row(fac_ptr->getSize(), A.cols());
+ for (auto i = 0; i < jacobians.size(); i++)
+ {
+ StateBlockPtr sb = fac_ptr->getStateBlockPtrVector()[i];
+ if (!sb->isFixed())
+ {
+ assert((state_blocks_local_param_.find(sb) != state_blocks_local_param_.end()) && "factor involving a state block not added");
+ assert((A.cols() >= sb->getLocalSize() + jacobians[i].cols()) - 1 && "bad A number of cols");
+
+ // insert since A_block_row has just been created so it's empty for sure
+ if (sb->hasLocalParametrization()){
+ // if the state block has a local parameterization, we need to right multiply by the manifold element / tangent element jacobian
+ Eigen::MatrixXd J_manif_tang(sb->getSize(), sb->getLocalSize());
+ Eigen::Map<Eigen::MatrixRowXd> J_manif_tang_map(J_manif_tang.data(), sb->getSize(), sb->getLocalSize());
+ Eigen::Map<const Eigen::VectorXd> sb_state_map(sb->getStateData(), sb->getSize());
+
+ sb->getLocalParametrization()->computeJacobian(sb_state_map, J_manif_tang_map);
+ insertSparseBlock(jacobians[i] * J_manif_tang, A_block_row, 0, sb->getLocalSize()); // (to_insert, matrix_to_fill, row, col)
+ }
+ else {
+ insertSparseBlock(jacobians[i], A_block_row, 0, sb->getLocalSize());
+ }
+ }
+ }
+
+ // fill the weighted jacobian matrix block row
+ A.block(ix_row, 0, fac_ptr->getSize(), A.cols());
+
+ }
+
+ // compute the hessian matrix from the weighted jacobian
+ H = A.transpose() * A;
+
+ return H;
+}
+
+
} // namespace wolf
#include "core/solver/solver_factory.h"
namespace wolf {
diff --git a/src/factor/factor_base.cpp b/src/factor/factor_base.cpp
index 46b244d9e83f9c01f4fca0bf84af35d81d36d53f..72b36c519a0d8d0ce2cb7ac4748e0dfc110026d0 100644
--- a/src/factor/factor_base.cpp
+++ b/src/factor/factor_base.cpp
@@ -19,15 +19,52 @@ FactorBase::FactorBase(const std::string& _tp,
factor_id_(++factor_id_count_),
status_(_status),
apply_loss_function_(_apply_loss_function),
- frame_other_ptr_(_frame_other_ptr),
- capture_other_ptr_(_capture_other_ptr),
- feature_other_ptr_(_feature_other_ptr),
- landmark_other_ptr_(_landmark_other_ptr),
+ frame_other_list_(),
+ capture_other_list_(),
+ feature_other_list_(),
+ landmark_other_list_(),
processor_ptr_(_processor_ptr)
{
-// std::cout << "constructed +c" << id() << std::endl;
+ if (_frame_other_ptr)
+ frame_other_list_.push_back(_frame_other_ptr);
+ if (_capture_other_ptr)
+ capture_other_list_.push_back(_capture_other_ptr);
+ if (_feature_other_ptr)
+ feature_other_list_.push_back(_feature_other_ptr);
+ if (_landmark_other_ptr)
+ landmark_other_list_.push_back(_landmark_other_ptr);
}
+FactorBase::FactorBase(const std::string& _tp,
+ const FrameBasePtrList& _frame_other_list,
+ const CaptureBasePtrList& _capture_other_list,
+ const FeatureBasePtrList& _feature_other_list,
+ const LandmarkBasePtrList& _landmark_other_list,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status) :
+ NodeBase("FACTOR", _tp),
+ feature_ptr_(),
+ factor_id_(++factor_id_count_),
+ status_(_status),
+ apply_loss_function_(_apply_loss_function),
+ frame_other_list_(),
+ capture_other_list_(),
+ feature_other_list_(),
+ landmark_other_list_(),
+ processor_ptr_(_processor_ptr)
+{
+ for (auto& Fo : _frame_other_list)
+ frame_other_list_.push_back(Fo);
+ for (auto& Co : _capture_other_list)
+ capture_other_list_.push_back(Co);
+ for (auto& fo : _feature_other_list)
+ feature_other_list_.push_back(fo);
+ for (auto& Lo : landmark_other_list_)
+ landmark_other_list_.push_back(Lo);
+}
+
+
void FactorBase::remove(bool viral_remove_empty_parent)
{
if (!is_removing_)
@@ -46,36 +83,48 @@ void FactorBase::remove(bool viral_remove_empty_parent)
getProblem()->notifyFactor(this_fac,REMOVE);
// remove other: {Frame, Capture, Feature, Landmark}
- FrameBasePtr frm_o = frame_other_ptr_.lock();
- if (frm_o)
+ for (auto& frm_ow : frame_other_list_)
{
- frm_o->removeConstrainedBy(this_fac);
- if (frm_o->getConstrainedByList().empty() && frm_o->getCaptureList().empty())
- frm_o->remove(viral_remove_empty_parent);
+ auto frm_o = frm_ow.lock();
+ if (frm_o)
+ {
+ frm_o->removeConstrainedBy(this_fac);
+ if (frm_o->getConstrainedByList().empty() && frm_o->getCaptureList().empty())
+ frm_o->remove(viral_remove_empty_parent);
+ }
}
- CaptureBasePtr cap_o = capture_other_ptr_.lock();
- if (cap_o)
+ for (auto& cap_ow : capture_other_list_)
{
- cap_o->removeConstrainedBy(this_fac);
- if (cap_o->getConstrainedByList().empty() && cap_o->getFeatureList().empty())
- cap_o->remove(viral_remove_empty_parent);
+ auto cap_o = cap_ow.lock();
+ if (cap_o)
+ {
+ cap_o->removeConstrainedBy(this_fac);
+ if (cap_o->getConstrainedByList().empty() && cap_o->getFeatureList().empty())
+ cap_o->remove(viral_remove_empty_parent);
+ }
}
- FeatureBasePtr ftr_o = feature_other_ptr_.lock();
- if (ftr_o)
+ for (auto& ftr_ow : feature_other_list_)
{
- ftr_o->removeConstrainedBy(this_fac);
- if (ftr_o->getConstrainedByList().empty() && ftr_o->getFactorList().empty())
- ftr_o->remove(viral_remove_empty_parent);
+ auto ftr_o = ftr_ow.lock();
+ if (ftr_o)
+ {
+ ftr_o->removeConstrainedBy(this_fac);
+ if (ftr_o->getConstrainedByList().empty() && ftr_o->getFactorList().empty())
+ ftr_o->remove(viral_remove_empty_parent);
+ }
}
- LandmarkBasePtr lmk_o = landmark_other_ptr_.lock();
- if (lmk_o)
+ for (auto& lmk_ow : landmark_other_list_)
{
- lmk_o->removeConstrainedBy(this_fac);
- if (lmk_o->getConstrainedByList().empty())
- lmk_o->remove(viral_remove_empty_parent);
+ auto lmk_o = lmk_ow.lock();
+ if (lmk_o)
+ {
+ lmk_o->removeConstrainedBy(this_fac);
+ if (lmk_o->getConstrainedByList().empty())
+ lmk_o->remove(viral_remove_empty_parent);
+ }
}
// std::cout << "Removed c" << id() << std::endl;
@@ -106,6 +155,18 @@ CaptureBasePtr FactorBase::getCapture() const
return getFeature()->getCapture();
}
+FrameBasePtr FactorBase::getFrame() const
+{
+ assert(getCapture() != nullptr && "calling getFrame before linking with a capture");
+ return getCapture()->getFrame();
+}
+
+SensorBasePtr FactorBase::getSensor() const
+{
+ assert(getCapture() != nullptr && "calling getSensor before linking with a capture");
+ return getCapture()->getSensor();
+}
+
void FactorBase::setStatus(FactorStatus _status)
{
if (getProblem() == nullptr)
@@ -120,6 +181,66 @@ void FactorBase::setStatus(FactorStatus _status)
status_ = _status;
}
+bool FactorBase::hasFrameOther(const FrameBasePtr &_frm_other) const
+{
+ FrameBaseWConstIter frm_it = find_if(frame_other_list_.begin(),
+ frame_other_list_.end(),
+ [_frm_other](const FrameBaseWPtr &frm_ow)
+ {
+ return frm_ow.lock() == _frm_other;
+ }
+ );
+ if (frm_it != frame_other_list_.end())
+ return true;
+
+ return false;
+}
+
+bool FactorBase::hasCaptureOther(const CaptureBasePtr &_cap_other) const
+{
+ CaptureBaseWConstIter cap_it = find_if(capture_other_list_.begin(),
+ capture_other_list_.end(),
+ [_cap_other](const CaptureBaseWPtr &cap_ow)
+ {
+ return cap_ow.lock() == _cap_other;
+ }
+ );
+ if (cap_it != capture_other_list_.end())
+ return true;
+
+ return false;
+}
+
+bool FactorBase::hasFeatureOther(const FeatureBasePtr &_ftr_other) const
+{
+ FeatureBaseWConstIter ftr_it = find_if(feature_other_list_.begin(),
+ feature_other_list_.end(),
+ [_ftr_other](const FeatureBaseWPtr &ftr_ow)
+ {
+ return ftr_ow.lock() == _ftr_other;
+ }
+ );
+ if (ftr_it != feature_other_list_.end())
+ return true;
+
+ return false;
+}
+
+bool FactorBase::hasLandmarkOther(const LandmarkBasePtr &_lmk_other) const
+{
+ LandmarkBaseWConstIter lmk_it = find_if(landmark_other_list_.begin(),
+ landmark_other_list_.end(),
+ [_lmk_other](const LandmarkBaseWPtr &lmk_ow)
+ {
+ return lmk_ow.lock() == _lmk_other;
+ }
+ );
+ if (lmk_it != landmark_other_list_.end())
+ return true;
+
+ return false;
+}
+
//void FactorBase::setApplyLossFunction(const bool _apply)
//{
// apply_loss_function_ = _apply;
@@ -146,14 +267,26 @@ void FactorBase::link(FeatureBasePtr _ftr_ptr)
this->setProblem(_ftr_ptr->getProblem());
// constrained by
- auto frame_other = this->frame_other_ptr_.lock();
- if(frame_other != nullptr) frame_other->addConstrainedBy(shared_from_this());
- auto capture_other = this->capture_other_ptr_.lock();
- if(capture_other != nullptr) capture_other->addConstrainedBy(shared_from_this());
- auto feature_other = this->feature_other_ptr_.lock();
- if(feature_other != nullptr) feature_other->addConstrainedBy(shared_from_this());
- auto landmark_other = this->landmark_other_ptr_.lock();
- if(landmark_other != nullptr) landmark_other->addConstrainedBy(shared_from_this());
+ for (auto& frm_ow : frame_other_list_)
+ {
+ auto frame_other = frm_ow.lock();
+ if(frame_other != nullptr) frame_other->addConstrainedBy(shared_from_this());
+ }
+ for (auto& cap_ow : capture_other_list_)
+ {
+ auto capture_other = cap_ow.lock();
+ if(capture_other != nullptr) capture_other->addConstrainedBy(shared_from_this());
+ }
+ for (auto& ftr_ow : feature_other_list_)
+ {
+ auto feature_other = ftr_ow.lock();
+ if(feature_other != nullptr) feature_other->addConstrainedBy(shared_from_this());
+ }
+ for (auto& lmk_ow : landmark_other_list_)
+ {
+ auto landmark_other = lmk_ow.lock();
+ if(landmark_other != nullptr) landmark_other->addConstrainedBy(shared_from_this());
+ }
}
void FactorBase::setProblem(ProblemPtr _problem)
diff --git a/src/feature/feature_base.cpp b/src/feature/feature_base.cpp
index df5d1f1e7aa0f214adab2e0f74dc496623e7cf48..8dc3801efa5f93e5bd0df593442e4860cb75135e 100644
--- a/src/feature/feature_base.cpp
+++ b/src/feature/feature_base.cpp
@@ -92,6 +92,21 @@ void FeatureBase::removeConstrainedBy(FactorBasePtr _fac_ptr)
constrained_by_list_.remove(_fac_ptr);
}
+bool FeatureBase::isConstrainedBy(const FactorBasePtr &_factor) const
+{
+ FactorBaseConstIter cby_it = std::find_if(constrained_by_list_.begin(),
+ constrained_by_list_.end(),
+ [_factor](const FactorBasePtr & cby)
+ {
+ return cby == _factor;
+ }
+ );
+ if (cby_it != constrained_by_list_.end())
+ return true;
+ else
+ return false;
+}
+
const FactorBasePtrList& FeatureBase::getFactorList() const
{
return factor_list_;
diff --git a/src/feature/feature_odom_2D.cpp b/src/feature/feature_odom_2D.cpp
deleted file mode 100644
index f8ae20d7f72bff405903e6f623402b8e350176d7..0000000000000000000000000000000000000000
--- a/src/feature/feature_odom_2D.cpp
+++ /dev/null
@@ -1,21 +0,0 @@
-#include "core/feature/feature_odom_2D.h"
-
-namespace wolf {
-
-FeatureOdom2D::FeatureOdom2D(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance) :
- FeatureBase("FeatureOdom2D", _measurement, _meas_covariance)
-{
- //std::cout << "New FeatureOdom2D: measurement " << _measurement.transpose() << std::endl << "covariance" << std::endl << _meas_covariance << std::endl;
-}
-
-FeatureOdom2D::~FeatureOdom2D()
-{
- //
-}
-
-void FeatureOdom2D::findFactors()
-{
-
-}
-
-} // namespace wolf
diff --git a/src/feature/feature_odom_2d.cpp b/src/feature/feature_odom_2d.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..169031037244dc724846efc0f90a603056945c4a
--- /dev/null
+++ b/src/feature/feature_odom_2d.cpp
@@ -0,0 +1,21 @@
+#include "core/feature/feature_odom_2d.h"
+
+namespace wolf {
+
+FeatureOdom2d::FeatureOdom2d(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance) :
+ FeatureBase("FeatureOdom2d", _measurement, _meas_covariance)
+{
+ //std::cout << "New FeatureOdom2d: measurement " << _measurement.transpose() << std::endl << "covariance" << std::endl << _meas_covariance << std::endl;
+}
+
+FeatureOdom2d::~FeatureOdom2d()
+{
+ //
+}
+
+void FeatureOdom2d::findFactors()
+{
+
+}
+
+} // namespace wolf
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index 74131253fff528d98ae9fbcaed725d250d4daa65..e449ce1604fef9dbdafb1ed45d7dfd982f35845e 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -69,14 +69,14 @@ FrameBase::FrameBase(const StateStructure& _frame_structure, const SizeEigen _di
StateBlockPtr o_ptr ( std::make_shared<StateAngle> ((double) _x(2) ) );
addStateBlock("P", p_ptr);
addStateBlock("O", o_ptr);
- this->setType("PO 2D");
+ this->setType("PO 2d");
}else if(_frame_structure.compare("PO") == 0 and _dim == 3){
assert(_x.size() == 7 && "Wrong state vector size. Should be 7 for PO 3D!");
StateBlockPtr p_ptr ( std::make_shared<StateBlock> (_x.head <3> ( ) ) );
StateBlockPtr o_ptr ( std::make_shared<StateQuaternion> (_x.tail <4> ( ) ) );
addStateBlock("P", p_ptr);
addStateBlock("O", o_ptr);
- this->setType("PO 3D");
+ this->setType("PO 3d");
}else if(_frame_structure.compare("POV") == 0 and _dim == 3){
// auto _x = Eigen::VectorXd::Zero(10);
assert(_x.size() == 10 && "Wrong state vector size. Should be 10 for POV 3D!");
@@ -86,7 +86,7 @@ FrameBase::FrameBase(const StateStructure& _frame_structure, const SizeEigen _di
addStateBlock("P", p_ptr);
addStateBlock("O", o_ptr);
addStateBlock("V", v_ptr);
- this->setType("POV 3D");
+ this->setType("POV 3d");
}else if(_frame_structure.compare("POVCDL") == 0 and _dim == 3){
// auto _x = Eigen::VectorXd::Zero(10);
assert(_x.size() == 19 && "Wrong state vector size. Should be 19 for POVCDL 3D!");
@@ -102,7 +102,7 @@ FrameBase::FrameBase(const StateStructure& _frame_structure, const SizeEigen _di
addStateBlock("C", c_ptr);
addStateBlock("D", cd_ptr);
addStateBlock("L", Lc_ptr);
- this->setType("POVCDL 3D");
+ this->setType("POVCDL 3d");
}else{
std::cout << _frame_structure << " ^^ " << _dim << std::endl;
throw std::runtime_error("Unknown frame structure");
@@ -156,7 +156,8 @@ void FrameBase::setNonEstimated()
{
// unregister if previously estimated
if (isKeyOrAux())
- removeStateBlocks(getProblem());
+ for (const auto& sb : getStateBlockVec())
+ getProblem()->notifyStateBlock(sb, REMOVE);
type_ = NON_ESTIMATED;
if (getTrajectory())
@@ -172,8 +173,8 @@ void FrameBase::setKey()
if (!isKeyOrAux())
registerNewStateBlocks(getProblem());
- // WOLF_DEBUG("Set Key", this->id());
type_ = KEY;
+
if (getTrajectory())
{
getTrajectory()->sortFrame(shared_from_this());
@@ -186,8 +187,8 @@ void FrameBase::setAux()
if (!isKeyOrAux())
registerNewStateBlocks(getProblem());
- // WOLF_DEBUG("Set Auxiliary", this->id());
type_ = AUXILIARY;
+
if (getTrajectory())
{
getTrajectory()->sortFrame(shared_from_this());
@@ -313,6 +314,21 @@ void FrameBase::removeConstrainedBy(FactorBasePtr _fac_ptr)
constrained_by_list_.remove(_fac_ptr);
}
+bool FrameBase::isConstrainedBy(const FactorBasePtr &_factor) const
+{
+ FactorBaseConstIter cby_it = std::find_if(constrained_by_list_.begin(),
+ constrained_by_list_.end(),
+ [_factor](const FactorBasePtr & cby)
+ {
+ return cby == _factor;
+ }
+ );
+ if (cby_it != constrained_by_list_.end())
+ return true;
+ else
+ return false;
+}
+
void FrameBase::link(TrajectoryBasePtr _trj_ptr)
{
assert(!is_removing_ && "linking a removed frame");
@@ -337,7 +353,7 @@ void FrameBase::setProblem(ProblemPtr _problem)
NodeBase::setProblem(_problem);
if (this->isKey())
- registerNewStateBlocks(getProblem());
+ registerNewStateBlocks(_problem);
for (auto cap : capture_list_)
cap->setProblem(_problem);
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index 1a1d785a306cf3bb23c28e66201a3193fb0cb2c6..093714dd4a1f137ef75a41f745e26cd7223b95e2 100644
--- a/src/landmark/landmark_base.cpp
+++ b/src/landmark/landmark_base.cpp
@@ -21,11 +21,11 @@ LandmarkBase::LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, State
{
if (_p_ptr)
{
- addStateBlock("P", _p_ptr);
+ addStateBlock("P", _p_ptr, nullptr);
}
if (_o_ptr)
{
- addStateBlock("O", _o_ptr);
+ addStateBlock("O", _o_ptr, nullptr);
}
}
@@ -138,6 +138,21 @@ void LandmarkBase::removeConstrainedBy(FactorBasePtr _fac_ptr)
constrained_by_list_.remove(_fac_ptr);
}
+bool LandmarkBase::isConstrainedBy(const FactorBasePtr &_factor) const
+{
+ FactorBaseConstIter cby_it = std::find_if(constrained_by_list_.begin(),
+ constrained_by_list_.end(),
+ [_factor](const FactorBasePtr & cby)
+ {
+ return cby == _factor;
+ }
+ );
+ if (cby_it != constrained_by_list_.end())
+ return true;
+ else
+ return false;
+}
+
LandmarkBasePtr LandmarkBase::create(const YAML::Node& _node)
{
unsigned int id = _node["id"] .as< unsigned int >();
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index d3302513c4abf6b4b5b6356cb913dfa66e8422b5..747737dc1ff0ea4b742a97017065d229889ea7d6 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -11,11 +11,12 @@
#include "core/sensor/sensor_factory.h"
#include "core/processor/processor_factory.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.hpp"
-#include "core/utils/loader.hpp"
-#include "core/utils/check_log.hpp"
-
+#include "core/utils/params_server.h"
+#include "core/utils/loader.h"
+#include "core/utils/check_log.h"
// IRI libs 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>()),
@@ -120,7 +123,7 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
WOLF_WARN("Support for subscribers disabled...");
}
for (auto it : _server.getParam<std::vector<std::string>>("packages")) {
- std::string subscriber = packages_path + "/libwolf_subscriber_" + it + lib_extension;
+ std::string subscriber = packages_path + "/libsubscriber_" + it + lib_extension;
WOLF_TRACE("Loading subscriber " + subscriber);
auto l = new LoaderRaw(subscriber);
l->load();
@@ -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");
@@ -224,6 +233,12 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
}
ProcessorBasePtr prc_ptr = ProcessorFactory::get().create(_prc_type, _unique_processor_name, _prc_params);
+
+ //Dimension check
+ int prc_dim = prc_ptr->getDim();
+ auto prb = this;
+ assert(((prc_dim == 0) or (prc_dim == prb->getDim())) && "Processor and Problem do not agree on dimension");
+
prc_ptr->configure(_corresponding_sensor_ptr);
prc_ptr->link(_corresponding_sensor_ptr);
@@ -262,9 +277,16 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
throw std::runtime_error("Cannot bind processor. Reason: Sensor \"" + _corresponding_sensor_name + "\" not found. Check sensor name, it must match in sensor and processor!");
ProcessorBasePtr prc_ptr = AutoConfProcessorFactory::get().create(_prc_type, _unique_processor_name, _server);
+
+ //Dimension check
+ int prc_dim = prc_ptr->getDim();
+ auto prb = this;
+ assert(((prc_dim == 0) or (prc_dim == prb->getDim())) && "Processor and Problem do not agree on dimension");
+
prc_ptr->configure(sen_ptr);
prc_ptr->link(sen_ptr);
+
// setting the origin in all processor motion if origin already setted
if (prc_ptr->isMotion() && prior_is_set_)
(std::static_pointer_cast<ProcessorMotion>(prc_ptr))->setOrigin(getLastKeyFrame());
@@ -458,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);
@@ -474,7 +506,7 @@ Eigen::VectorXd Problem::zeroState() const
{
Eigen::VectorXd state = Eigen::VectorXd::Zero(getFrameStructureSize());
- // Set the quaternion identity for 3D states. Set only the real part to 1:
+ // Set the quaternion identity for 3d states. Set only the real part to 1:
if(this->getDim() == 3)
state(6) = 1.0;
return state;
@@ -515,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
@@ -549,6 +585,7 @@ StateBlockPtr Problem::notifyStateBlock(StateBlockPtr _state_ptr, Notification _
// Check if there is already a notification for this state block
auto notification_it = state_block_notification_map_.find(_state_ptr);
+
// exsiting notification for this state block
if (notification_it != state_block_notification_map_.end())
{
@@ -862,7 +899,7 @@ FrameBasePtr Problem::setPrior(const Eigen::VectorXd& _prior_state, const Eigen:
FrameBasePtr origin_keyframe = emplaceFrame(KEY, _prior_state, _ts);
// create origin capture with the given state as data
- // Capture fix only takes 3D position and Quaternion orientation
+ // Capture fix only takes 3d position and Quaternion orientation
CapturePosePtr init_capture;
// if (this->getFrameStructure() == "POV" and this->getDim() == 3)
// init_capture = CaptureBase::emplace<CapturePose>(origin_keyframe, _ts, nullptr, _prior_state.head(7), _prior_cov.topLeftCorner(6,6));
@@ -895,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
@@ -911,230 +952,226 @@ void Problem::saveMap(const std::string& _filename_dot_yaml, const std::string&
getMap()->save(_filename_dot_yaml, _map_name);
}
-void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks) const
+void Problem::print(int _depth, std::ostream& _stream, bool _constr_by, bool _metric, bool _state_blocks) const
{
- using std::cout;
- using std::endl;
- cout << endl;
- cout << "P: wolf tree status ---------------------" << endl;
- cout << "Hardware" << ((depth < 1) ? (" -- " + std::to_string(getHardware()->getSensorList().size()) + "S") : "") << endl;
- if (depth >= 1)
+ _stream << std::endl;
+ _stream << "P: wolf tree status ---------------------" << std::endl;
+ _stream << "Hardware" << ((_depth < 1) ? (" -- " + std::to_string(getHardware()->getSensorList().size()) + "S") : "") << std::endl;
+ if (_depth >= 1)
{
// Sensors =======================================================================================
for (auto S : getHardware()->getSensorList())
{
- cout << " S" << S->id() << " " << S->getType() << " \"" << S->getName() << "\"";
- if (depth < 2)
- cout << " -- " << S->getProcessorList().size() << "p";
- cout << endl;
- if (metric && state_blocks)
+ _stream << " Sen" << S->id() << " " << S->getType() << " \"" << S->getName() << "\"";
+ if (_depth < 2)
+ _stream << " -- " << S->getProcessorList().size() << "p";
+ _stream << std::endl;
+ if (_metric && _state_blocks)
{
- cout << " sb: ";
+ _stream << " sb: ";
for (auto& _key : S->getStructure())
{
auto key = std::string(1,_key);
auto sb = S->getStateBlock(key);
- cout << key << "[" << (S->isStateBlockDynamic(key) ? "Dyn" : "Sta") << "," << (sb->isFixed() ? "Fix" : "Est") << "] = ( " << sb->getState().transpose() << " ); ";
+ _stream << key << "[" << (S->isStateBlockDynamic(key) ? "Dyn" : "Sta") << "," << (sb->isFixed() ? "Fix" : "Est") << "] = ( " << sb->getState().transpose() << " ); ";
}
- cout << endl;
+ _stream << std::endl;
}
- else if (metric)
+ else if (_metric)
{
- cout << " ( ";
+ _stream << " ( ";
for (auto& _key : S->getStructure())
{
auto key = std::string(1,_key);
auto sb = S->getStateBlock(key);
- cout << sb->getState().transpose() << " ";
+ _stream << sb->getState().transpose() << " ";
}
- cout << ")" << endl;
+ _stream << ")" << std::endl;
}
- else if (state_blocks)
+ else if (_state_blocks)
{
- cout << " sb: ";
+ _stream << " sb: ";
for (auto& _key : S->getStructure())
{
auto key = std::string(1,_key);
auto sb = S->getStateBlock(key);
- cout << key << "[" << (S->isStateBlockDynamic(key) ? "Dyn" : "Sta") << "," << (sb->isFixed() ? "Fix" : "Est") << "]; ";
+ _stream << key << "[" << (S->isStateBlockDynamic(key) ? "Dyn" : "Sta") << "," << (sb->isFixed() ? "Fix" : "Est") << "]; ";
}
- cout << endl;
+ _stream << std::endl;
}
- if (depth >= 2)
+ if (_depth >= 2)
{
// Processors =======================================================================================
for (auto p : S->getProcessorList())
{
if (p->isMotion())
{
- std::cout << " pm" << p->id() << " " << p->getType() << " \"" << p->getName() << "\"" << endl;
+ _stream << " PrcM" << p->id() << " " << p->getType() << " \"" << p->getName() << "\"" << std::endl;
ProcessorMotionPtr pm = std::static_pointer_cast<ProcessorMotion>(p);
if (pm->getOrigin())
- cout << " o: C" << pm->getOrigin()->id() << " - " << (pm->getOrigin()->getFrame()->isKeyOrAux() ? (pm->getOrigin()->getFrame()->isKey() ? " KF" : " AuxF" ) : " F")
- << pm->getOrigin()->getFrame()->id() << endl;
+ _stream << " o: Cap" << pm->getOrigin()->id() << " - " << (pm->getOrigin()->getFrame()->isKeyOrAux() ? (pm->getOrigin()->getFrame()->isKey() ? " KFrm" : " AFrm" ) : " Frm")
+ << pm->getOrigin()->getFrame()->id() << std::endl;
if (pm->getLast())
- cout << " l: C" << pm->getLast()->id() << " - " << (pm->getLast()->getFrame()->isKeyOrAux() ? (pm->getLast()->getFrame()->isKey() ? " KF" : " AuxF") : " F")
- << pm->getLast()->getFrame()->id() << endl;
+ _stream << " l: Cap" << pm->getLast()->id() << " - " << (pm->getLast()->getFrame()->isKeyOrAux() ? (pm->getLast()->getFrame()->isKey() ? " KFrm" : " AFrm") : " Frm")
+ << pm->getLast()->getFrame()->id() << std::endl;
if (pm->getIncoming())
- cout << " i: C" << pm->getIncoming()->id() << endl;
+ _stream << " i: Cap" << pm->getIncoming()->id() << std::endl;
}
else
{
- cout << " pt" << p->id() << " " << p->getType() << " \"" << p->getName() << "\"" << endl;
+ _stream << " PrcT" << p->id() << " " << p->getType() << " \"" << p->getName() << "\"" << std::endl;
ProcessorTrackerPtr pt = std::dynamic_pointer_cast<ProcessorTracker>(p);
if (pt)
{
if (pt->getOrigin())
- cout << " o: C" << pt->getOrigin()->id() << " - " << (pt->getOrigin()->getFrame()->isKeyOrAux() ? (pt->getOrigin()->getFrame()->isKey() ? " KF" : " AuxF") : " F")
- << pt->getOrigin()->getFrame()->id() << endl;
+ _stream << " o: Cap" << pt->getOrigin()->id() << " - " << (pt->getOrigin()->getFrame()->isKeyOrAux() ? (pt->getOrigin()->getFrame()->isKey() ? " KFrm" : " AFrm") : " Frm")
+ << pt->getOrigin()->getFrame()->id() << std::endl;
if (pt->getLast())
- cout << " l: C" << pt->getLast()->id() << " - " << (pt->getLast()->getFrame()->isKeyOrAux() ? (pt->getLast()->getFrame()->isKey() ? " KF" : " AuxF") : " F")
- << pt->getLast()->getFrame()->id() << endl;
+ _stream << " l: Cap" << pt->getLast()->id() << " - " << (pt->getLast()->getFrame()->isKeyOrAux() ? (pt->getLast()->getFrame()->isKey() ? " KFrm" : " AFrm") : " Frm")
+ << pt->getLast()->getFrame()->id() << std::endl;
if (pt->getIncoming())
- cout << " i: C" << pt->getIncoming()->id() << endl;
+ _stream << " i: Cap" << pt->getIncoming()->id() << std::endl;
}
}
} // for p
}
} // for S
}
- cout << "Trajectory" << ((depth < 1) ? (" -- " + std::to_string(getTrajectory()->getFrameList().size()) + "F") : "") << endl;
- if (depth >= 1)
+ _stream << "Trajectory" << ((_depth < 1) ? (" -- " + std::to_string(getTrajectory()->getFrameList().size()) + "F") : "") << std::endl;
+ if (_depth >= 1)
{
// Frames =======================================================================================
for (auto F : getTrajectory()->getFrameList())
{
- cout << (F->isKeyOrAux() ? (F->isKey() ? " KF" : " AuxF") : " F") << F->id() << ((depth < 2) ? " -- " + std::to_string(F->getCaptureList().size()) + "C " : "");
- if (constr_by)
+ _stream << (F->isKeyOrAux() ? (F->isKey() ? " KFrm" : " AFrm") : " Frm") << F->id() << ((_depth < 2) ? " -- " + std::to_string(F->getCaptureList().size()) + "C " : "");
+ if (_constr_by)
{
- cout << " <-- ";
+ _stream << " <-- ";
for (auto cby : F->getConstrainedByList())
- cout << "c" << cby->id() << " \t";
+ _stream << "Fac" << cby->id() << " \t";
}
- cout << endl;
- if (metric)
+ _stream << std::endl;
+ if (_metric)
{
- cout << (F->isFixed() ? " Fix" : " Est") << ", ts=" << std::setprecision(5)
- << F->getTimeStamp().get();
- cout << ",\t x = ( " << std::setprecision(2) << endl;
- for (auto sb_name: F->getStructure()){
- auto sb = F->getStateBlock(sb_name);
- cout << " " << sb_name;
- if (state_blocks){
- cout << "," << (sb->isFixed() ? "Fix" : "Est");
- }
- cout << ": " << sb->getState().transpose() << "\n";
+ _stream << (F->isFixed() ? " Fix" : " Est") << ", ts=" << std::setprecision(5)
+ << F->getTimeStamp();
+ _stream << ",\t x = ( " << std::setprecision(2) << F->getState().transpose() << " )";
+ _stream << std::endl;
+ }
+ if (_state_blocks)
+ {
+ _stream << " sb:";
+ for (const auto& sb : F->getStateBlockVec())
+ {
+ _stream << " " << (sb->isFixed() ? "Fix" : "Est");
}
- cout << " )" << endl;
- // cout << ",\t x = ( " << std::setprecision(2) << F->getState().transpose() << " )";
- // cout << endl;
+ _stream << std::endl;
}
- // if (state_blocks)
- // {
- // cout << " sb:";
- // for (const auto& sb : F->getStateBlockVec())
- // {
- // cout << " " << (sb->isFixed() ? "Fix" : "Est");
- // }
- // cout << endl;
- // }
- if (depth >= 2)
+ if (_depth >= 2)
{
// Captures =======================================================================================
for (auto C : F->getCaptureList())
{
- cout << " C" << (C->isMotion() ? "M" : "") << C->id() << " " << C->getType();
+ _stream << " Cap" << (C->isMotion() ? "M" : "") << C->id() << " " << C->getType();
if(C->getSensor() != nullptr)
{
- cout << " -> S" << C->getSensor()->id();
+ _stream << " -> Sen" << C->getSensor()->id();
}
else
- cout << " -> S-";
+ _stream << " -> Sen-";
if (C->isMotion())
{
auto CM = std::static_pointer_cast<CaptureMotion>(C);
if (auto OC = CM->getOriginCapture())
{
- cout << " -> OC" << OC->id();
+ _stream << " -> OCap" << OC->id();
if (auto OF = OC->getFrame())
- cout << " ; OF" << OF->id();
+ _stream << " ; OFrm" << OF->id();
}
}
- cout << ((depth < 3) ? " -- " + std::to_string(C->getFeatureList().size()) + "f" : "");
- if (constr_by)
+ _stream << ((_depth < 3) ? " -- " + std::to_string(C->getFeatureList().size()) + "f" : "");
+ if (_constr_by)
{
- cout << " <-- ";
+ _stream << " <-- ";
for (auto cby : C->getConstrainedByList())
- cout << "c" << cby->id() << " \t";
+ _stream << "Fac" << cby->id() << " \t";
}
- cout << endl;
+ _stream << std::endl;
- if (state_blocks)
+ if (_state_blocks)
for (const auto& sb : C->getStateBlockVec())
{
if(sb != nullptr)
{
- cout << " sb: ";
- cout << (sb->isFixed() ? "Fix" : "Est");
- if (metric)
- cout << std::setprecision(2) << " (" << sb->getState().transpose() << " )";
- cout << endl;
+ _stream << " sb: ";
+ _stream << (sb->isFixed() ? "Fix" : "Est");
+ if (_metric)
+ _stream << std::setprecision(2) << " (" << sb->getState().transpose() << " )";
+ _stream << std::endl;
}
}
if (C->isMotion() )
{
CaptureMotionPtr CM = std::dynamic_pointer_cast<CaptureMotion>(C);
- cout << " buffer size : " << CM->getBuffer().get().size() << endl;
- if ( metric && ! CM->getBuffer().get().empty())
+ _stream << " buffer size : " << CM->getBuffer().get().size() << std::endl;
+ if ( _metric && ! CM->getBuffer().get().empty())
{
- cout << " delta preint : (" << CM->getDeltaPreint().transpose() << ")" << endl;
+ _stream << " delta preint : (" << CM->getDeltaPreint().transpose() << ")" << std::endl;
if (CM->hasCalibration())
{
- cout << " calib preint : (" << CM->getCalibrationPreint().transpose() << ")" << endl;
- cout << " jacob preint : (" << CM->getJacobianCalib().row(0) << ")" << endl;
- cout << " calib current: (" << CM->getCalibration().transpose() << ")" << endl;
- cout << " delta correct: (" << CM->getDeltaCorrected(CM->getCalibration()).transpose() << ")" << endl;
+ _stream << " calib preint : (" << CM->getCalibrationPreint().transpose() << ")" << std::endl;
+ _stream << " jacob preint : (" << CM->getJacobianCalib().row(0) << ")" << std::endl;
+ _stream << " calib current: (" << CM->getCalibration().transpose() << ")" << std::endl;
+ _stream << " delta correct: (" << CM->getDeltaCorrected(CM->getCalibration()).transpose() << ")" << std::endl;
}
}
}
- if (depth >= 3)
+ if (_depth >= 3)
{
// Features =======================================================================================
for (auto f : C->getFeatureList())
{
- cout << " f" << f->id() << " trk" << f->trackId() << " " << f->getType() << ((depth < 4) ? " -- " + std::to_string(f->getFactorList().size()) + "c " : "");
- if (constr_by)
+ _stream << " Ftr" << f->id() << " trk" << f->trackId() << " " << f->getType() << ((_depth < 4) ? " -- " + std::to_string(f->getFactorList().size()) + "c " : "");
+ if (_constr_by)
{
- cout << " <--\t";
+ _stream << " <--\t";
for (auto cby : f->getConstrainedByList())
- cout << "c" << cby->id() << " \t";
+ _stream << "Fac" << cby->id() << " \t";
}
- cout << endl;
- if (metric)
- cout << " m = ( " << std::setprecision(2) << f->getMeasurement().transpose()
- << " )" << endl;
- if (depth >= 4)
+ _stream << std::endl;
+ if (_metric)
+ _stream << " m = ( " << std::setprecision(2) << f->getMeasurement().transpose()
+ << " )" << std::endl;
+ if (_depth >= 4)
{
// Factors =======================================================================================
for (auto c : f->getFactorList())
{
- cout << " c" << c->id() << " " << c->getType() << " -->";
- if (c->getFrameOther() == nullptr && c->getCaptureOther() == nullptr && c->getFeatureOther() == nullptr && c->getLandmarkOther() == nullptr)
- cout << " A";
- if (c->getFrameOther() != nullptr)
- cout << " F" << c->getFrameOther()->id();
- if (c->getCaptureOther() != nullptr)
- cout << " C" << c->getCaptureOther()->id();
- if (c->getFeatureOther() != nullptr)
- cout << " f" << c->getFeatureOther()->id();
- if (c->getLandmarkOther() != nullptr)
- cout << " L" << c->getLandmarkOther()->id();
- cout << endl;
+ _stream << " Fac" << c->id() << " " << c->getType() << " -->";
+ if ( c->getFrameOtherList() .empty()
+ && c->getCaptureOtherList() .empty()
+ && c->getFeatureOtherList() .empty()
+ && c->getLandmarkOtherList().empty())
+ _stream << " Abs";
+
+ for (const auto& Fow : c->getFrameOtherList())
+ if (!Fow.expired())
+ _stream << " Frm" << Fow.lock()->id();
+ for (const auto& Cow : c->getCaptureOtherList())
+ if (!Cow.expired())
+ _stream << " Cap" << Cow.lock()->id();
+ for (const auto& fow : c->getFeatureOtherList())
+ if (!fow.expired())
+ _stream << " Ftr" << fow.lock()->id();
+ for (const auto& Low : c->getLandmarkOtherList())
+ if (!Low.expired())
+ _stream << " Lmk" << Low.lock()->id();
+ _stream << std::endl;
} // for c
}
} // for f
@@ -1143,490 +1180,208 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks) c
}
} // for F
}
- cout << "Map" << ((depth < 1) ? (" -- " + std::to_string(getMap()->getLandmarkList().size()) + "L") : "") << endl;
- if (depth >= 1)
+ _stream << "Map" << ((_depth < 1) ? (" -- " + std::to_string(getMap()->getLandmarkList().size()) + "L") : "") << std::endl;
+ if (_depth >= 1)
{
// Landmarks =======================================================================================
for (auto L : getMap()->getLandmarkList())
{
- cout << " L" << L->id() << " " << L->getType();
- if (constr_by)
+ _stream << " Lmk" << L->id() << " " << L->getType();
+ if (_constr_by)
{
- cout << "\t<-- ";
+ _stream << "\t<-- ";
for (auto cby : L->getConstrainedByList())
- cout << "c" << cby->id() << " \t";
+ _stream << "Fac" << cby->id() << " \t";
}
- cout << endl;
- if (metric)
+ _stream << std::endl;
+ if (_metric)
{
- cout << (L->isFixed() ? " Fix" : " Est");
- cout << ",\t x = ( " << std::setprecision(2) << L->getState().transpose() << " )";
- cout << endl;
+ _stream << (L->isFixed() ? " Fix" : " Est");
+ _stream << ",\t x = ( " << std::setprecision(2) << L->getState().transpose() << " )";
+ _stream << std::endl;
}
- if (state_blocks)
+ if (_state_blocks)
{
- cout << " sb:";
+ _stream << " sb:";
for (const auto& sb : L->getStateBlockVec())
{
if (sb != nullptr)
- cout << (sb->isFixed() ? " Fix" : " Est");
+ _stream << (sb->isFixed() ? " Fix" : " Est");
}
- cout << endl;
+ _stream << std::endl;
}
} // for L
}
- cout << "-----------------------------------------" << endl;
- cout << endl;
+ _stream << "-----------------------------------------" << std::endl;
+ _stream << std::endl;
}
-std::string Problem::printToString(int depth, bool constr_by, bool metric, bool state_blocks) const
+bool Problem::check(bool _verbose, std::ostream& _stream) const
{
- using std::cout;
- using std::endl;
- std::stringstream result;
-
- result << endl;
- result << "P: wolf tree status ---------------------" << endl;
- result << "Hardware" << ((depth < 1) ? (" -- " + std::to_string(getHardware()->getSensorList().size()) + "S") : "") << endl;
- if (depth >= 1)
- {
- // Sensors =======================================================================================
- for (auto S : getHardware()->getSensorList())
- {
- result << " S" << S->id() << " " << S->getType() << " \"" << S->getName() << "\"";
- if (depth < 2)
- result << " -- " << S->getProcessorList().size() << "p";
- result << endl;
- if (metric && state_blocks)
- {
- result << " sb: ";
- for (auto& _key : S->getStructure())
- {
- auto key = std::string(1,_key);
- auto sb = S->getStateBlock(key);
- result << key << "[" << (S->isStateBlockDynamic(key) ? "Dyn" : "Sta") << "," << (sb->isFixed() ? "Fix" : "Est") << "] = ( " << sb->getState().transpose() << " ); ";
- }
- result << endl;
- }
- else if (metric)
- {
- result << " ( ";
- for (auto& _key : S->getStructure())
- {
- auto key = std::string(1,_key);
- auto sb = S->getStateBlock(key);
- result << sb->getState().transpose() << " ";
- }
- result << ")" << endl;
- }
- else if (state_blocks)
- {
- result << " sb: ";
- for (auto& _key : S->getStructure())
- {
- auto key = std::string(1,_key);
- auto sb = S->getStateBlock(key);
- result << key << "[" << (S->isStateBlockDynamic(key) ? "Dyn" : "Sta") << "," << (sb->isFixed() ? "Fix" : "Est") << "]; ";
- }
- result << endl;
- }
-
-
- if (depth >= 2)
- {
- // Processors =======================================================================================
- for (auto p : S->getProcessorList())
- {
- if (p->isMotion())
- {
- result << " pm" << p->id() << " " << p->getType() << " \"" << p->getName() << "\"" << endl;
- ProcessorMotionPtr pm = std::static_pointer_cast<ProcessorMotion>(p);
- if (pm->getOrigin())
- result << " o: C" << pm->getOrigin()->id() << " - " << (pm->getOrigin()->getFrame()->isKeyOrAux() ? (pm->getOrigin()->getFrame()->isKey() ? " KF" : " AuxF" ) : " F")
- << pm->getOrigin()->getFrame()->id() << endl;
- if (pm->getLast())
- result << " l: C" << pm->getLast()->id() << " - " << (pm->getLast()->getFrame()->isKeyOrAux() ? (pm->getLast()->getFrame()->isKey() ? " KF" : " AuxF") : " F")
- << pm->getLast()->getFrame()->id() << endl;
- if (pm->getIncoming())
- result << " i: C" << pm->getIncoming()->id() << endl;
- }
- else
- {
- result << " pt" << p->id() << " " << p->getType() << " \"" << p->getName() << "\"" << endl;
- ProcessorTrackerPtr pt = std::dynamic_pointer_cast<ProcessorTracker>(p);
- if (pt)
- {
- if (pt->getOrigin())
- result << " o: C" << pt->getOrigin()->id() << " - " << (pt->getOrigin()->getFrame()->isKeyOrAux() ? (pt->getOrigin()->getFrame()->isKey() ? " KF" : " AuxF") : " F")
- << pt->getOrigin()->getFrame()->id() << endl;
- if (pt->getLast())
- result << " l: C" << pt->getLast()->id() << " - " << (pt->getLast()->getFrame()->isKeyOrAux() ? (pt->getLast()->getFrame()->isKey() ? " KF" : " AuxF") : " F")
- << pt->getLast()->getFrame()->id() << endl;
- if (pt->getIncoming())
- result << " i: C" << pt->getIncoming()->id() << endl;
- }
- }
- } // for p
- }
- } // for S
- }
- result << "Trajectory" << ((depth < 1) ? (" -- " + std::to_string(getTrajectory()->getFrameList().size()) + "F") : "") << endl;
- if (depth >= 1)
- {
- // Frames =======================================================================================
- for (auto F : getTrajectory()->getFrameList())
- {
- result << (F->isKeyOrAux() ? (F->isKey() ? " KF" : " AuxF") : " F") << F->id() << ((depth < 2) ? " -- " + std::to_string(F->getCaptureList().size()) + "C " : "");
- if (constr_by)
- {
- result << " <-- ";
- for (auto cby : F->getConstrainedByList())
- result << "c" << cby->id() << " \t";
- }
- result << endl;
- if (metric)
- {
- result << (F->isFixed() ? " Fix" : " Est") << ", ts=" << std::setprecision(5)
- << F->getTimeStamp().get();
- result << ",\t x = ( " << std::setprecision(2) << F->getState().transpose() << " )";
- result << endl;
- }
- if (state_blocks)
- {
- result << " sb:";
- for (auto sb : F->getStateBlockVec())
- if (sb != nullptr)
- result << " " << (sb->isFixed() ? "Fix" : "Est");
- result << endl;
- }
- if (depth >= 2)
- {
- // Captures =======================================================================================
- for (auto C : F->getCaptureList())
- {
- result << " C" << (C->isMotion() ? "M" : "") << C->id() << " " << C->getType();
- if(C->getSensor() != nullptr)
- {
- result << " -> S" << C->getSensor()->id();
- }
- else
- result << " -> S-";
- if (C->isMotion())
- {
- auto CM = std::static_pointer_cast<CaptureMotion>(C);
- if (auto OC = CM->getOriginCapture())
- {
- result << " -> OC" << OC->id();
- if (auto OF = OC->getFrame())
- result << " ; OF" << OF->id();
- }
- }
-
- result << ((depth < 3) ? " -- " + std::to_string(C->getFeatureList().size()) + "f" : "");
- if (constr_by)
- {
- result << " <-- ";
- for (auto cby : C->getConstrainedByList())
- result << "c" << cby->id() << " \t";
- }
- result << endl;
-
- if (state_blocks)
- for(auto sb : C->getStateBlockVec())
- if(sb != nullptr)
- {
- result << " sb: ";
- result << (sb->isFixed() ? "Fix" : "Est");
- if (metric)
- result << std::setprecision(2) << " (" << sb->getState().transpose() << " )";
- result << endl;
- }
-
- if (C->isMotion() )
- {
- CaptureMotionPtr CM = std::dynamic_pointer_cast<CaptureMotion>(C);
- result << " buffer size : " << CM->getBuffer().get().size() << endl;
- if ( metric && ! CM->getBuffer().get().empty())
- {
- result << " delta preint : (" << CM->getDeltaPreint().transpose() << ")" << endl;
- if (CM->hasCalibration())
- {
- result << " calib preint : (" << CM->getCalibrationPreint().transpose() << ")" << endl;
- result << " jacob preint : (" << CM->getJacobianCalib().row(0) << ")" << endl;
- result << " calib current: (" << CM->getCalibration().transpose() << ")" << endl;
- result << " delta correct: (" << CM->getDeltaCorrected(CM->getCalibration()).transpose() << ")" << endl;
- }
- }
- }
-
- if (depth >= 3)
- {
- // Features =======================================================================================
- for (auto f : C->getFeatureList())
- {
- result << " f" << f->id() << " trk" << f->trackId() << " " << f->getType() << ((depth < 4) ? " -- " + std::to_string(f->getFactorList().size()) + "c " : "");
- if (constr_by)
- {
- result << " <--\t";
- for (auto cby : f->getConstrainedByList())
- result << "c" << cby->id() << " \t";
- }
- result << endl;
- if (metric)
- result << " m = ( " << std::setprecision(2) << f->getMeasurement().transpose()
- << " )" << endl;
- if (depth >= 4)
- {
- // Factors =======================================================================================
- for (auto c : f->getFactorList())
- {
- result << " c" << c->id() << " " << c->getType() << " -->";
- if (c->getFrameOther() == nullptr && c->getCaptureOther() == nullptr && c->getFeatureOther() == nullptr && c->getLandmarkOther() == nullptr)
- result << " A";
- if (c->getFrameOther() != nullptr)
- result << " F" << c->getFrameOther()->id();
- if (c->getCaptureOther() != nullptr)
- result << " C" << c->getCaptureOther()->id();
- if (c->getFeatureOther() != nullptr)
- result << " f" << c->getFeatureOther()->id();
- if (c->getLandmarkOther() != nullptr)
- result << " L" << c->getLandmarkOther()->id();
- result << endl;
- } // for c
- }
- } // for f
- }
- } // for C
- }
- } // for F
- }
- result << "Map" << ((depth < 1) ? (" -- " + std::to_string(getMap()->getLandmarkList().size()) + "L") : "") << endl;
- if (depth >= 1)
- {
- // Landmarks =======================================================================================
- for (auto L : getMap()->getLandmarkList())
- {
- result << " L" << L->id() << " " << L->getType();
- if (constr_by)
- {
- result << "\t<-- ";
- for (auto cby : L->getConstrainedByList())
- result << "c" << cby->id() << " \t";
- }
- result << endl;
- if (metric)
- {
- result << (L->isFixed() ? " Fix" : " Est");
- result << ",\t x = ( " << std::setprecision(2) << L->getState().transpose() << " )";
- result << endl;
- }
- if (state_blocks)
- {
- result << " sb:";
- for (auto sb : L->getStateBlockVec())
- if (sb != nullptr)
- result << (sb->isFixed() ? " Fix" : " Est");
- result << endl;
- }
- } // for L
- }
- result << "-----------------------------------------" << endl;
- result << endl;
- return result.str();
-}
-bool Problem::check(int verbose_level) const
-{
- using std::cout;
- using std::endl;
+ CheckLog log(true, "");
+ log.explanation_ = "## WOLF::problem inconsistencies list \n ---------------------------------- \n";
- CheckLog log(true, "WOLF problem inconsistencies\n");
-
- if (verbose_level) cout << endl;
- if (verbose_level) cout << "Wolf tree integrity ---------------------" << endl;
- auto P_raw = this;
- if (verbose_level > 0)
+ if (_verbose) _stream << std::endl;
+ if (_verbose) _stream << "Wolf tree integrity ---------------------" << std::endl;
+ auto P = shared_from_this();
+ if (_verbose)
{
- cout << "P @ " << P_raw << endl;
+ _stream << "Prb @ " << P.get() << std::endl;
}
// ------------------------
// HARDWARE branch
// ------------------------
auto H = hardware_ptr_;
- if (verbose_level > 0)
+ if (_verbose)
{
- cout << "H @ " << H.get() << endl;
+ _stream << "Hrw @ " << H.get() << std::endl;
}
- // check pointer to Problem
- // is_consistent = is_consistent && (H->getProblem().get() == P_raw);
+ // check pointer to Problem
std::stringstream inconsistency_explanation;
inconsistency_explanation << "Hardware problem pointer is " << H->getProblem().get()
- << " but problem pointer is " << P_raw << "\n";
- log.compose(CheckLog((H->getProblem().get() == P_raw),inconsistency_explanation.str()));
- //Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
+ << " but problem pointer is " << P.get() << "\n";
+ log.assertTrue((H->getProblem() == P), inconsistency_explanation);
+
// Sensors =======================================================================================
for (auto S : H->getSensorList())
{
- if (verbose_level > 0)
+ if (_verbose)
{
- cout << " S" << S->id() << " @ " << S.get() << endl;
- cout << " -> P @ " << S->getProblem().get() << endl;
- cout << " -> H @ " << S->getHardware().get() << endl;
- for (auto sb : S->getStateBlockVec())
+ _stream << " Sen" << S->id() << " @ " << S.get() << std::endl;
+ _stream << " -> Prb @ " << S->getProblem().get() << std::endl;
+ _stream << " -> Hrw @ " << S->getHardware().get() << std::endl;
+ for (auto pair: S->getStateBlockMap())
{
- cout << " sb @ " << sb.get();
+ auto sb = pair.second;
+ _stream << " " << pair.first << " sb @ " << sb.get();
if (sb)
{
auto lp = sb->getLocalParametrization();
if (lp)
- cout << " (lp @ " << lp.get() << ")";
+ _stream << " (lp @ " << lp.get() << ")";
}
- cout << endl;
+ _stream << std::endl;
}
}
+
// check problem and hardware pointers
- // is_consistent = is_consistent && (S->getProblem().get() == P_raw);
inconsistency_explanation << "Sensor problem pointer is " << S->getProblem().get()
- << " but problem pointer is " << P_raw << "\n";
- log.compose(CheckLog((S->getProblem().get() == P_raw), inconsistency_explanation.str()));
- //Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
+ << " but problem pointer is " << P.get() << "\n";
+ log.assertTrue((S->getProblem() == P), inconsistency_explanation);
+
- // is_consistent = is_consistent && (S->getHardware() == H);
inconsistency_explanation << "Sensor hardware pointer is " << S->getHardware()
<< " but hardware pointer is " << H << "\n";
- log.compose(CheckLog((S->getHardware() == H), inconsistency_explanation.str()));
- //Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
+ log.assertTrue((S->getHardware() == H), inconsistency_explanation);
+
// Processors =======================================================================================
for (auto p : S->getProcessorList())
{
- if (verbose_level > 0)
+ if (_verbose)
{
- cout << " p" << p->id() << " @ " << p.get() << " -> S" << p->getSensor()->id() << endl;
- cout << " -> P @ " << p->getProblem().get() << endl;
- cout << " -> S" << p->getSensor()->id() << " @ " << p->getSensor().get() << endl;
+ _stream << " Prc" << p->id() << " @ " << p.get() << " -> Sen" << p->getSensor()->id() << std::endl;
+ _stream << " -> Prb @ " << p->getProblem().get() << std::endl;
+ _stream << " -> Sen" << p->getSensor()->id() << " @ " << p->getSensor().get() << std::endl;
}
- // check problem and sensor pointers
-
- // is_consistent = is_consistent && (p->getProblem().get() == P_raw);
+ // check problem and sensor pointers
inconsistency_explanation << "Processor problem pointer is " << p->getProblem().get()
- << " but problem pointer is " << P_raw << "\n";
- log.compose(CheckLog((p->getProblem().get() == P_raw), inconsistency_explanation.str()));
- //Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
+ << " but problem pointer is " << P.get() << "\n";
+ log.assertTrue((p->getProblem() == P), inconsistency_explanation);
- // is_consistent = is_consistent && (p->getSensor() == S);
inconsistency_explanation << "Processor sensor pointer is " << p->getSensor()
- << " but sensor pointer is " << P_raw << "\n";
- log.compose(CheckLog((p->getProblem().get() == P_raw), inconsistency_explanation.str()));
- //Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
+ << " but sensor pointer is " << P.get() << "\n";
+ log.assertTrue((p->getProblem() == P), inconsistency_explanation);
+
}
}
// ------------------------
// TRAJECTORY branch
// ------------------------
auto T = trajectory_ptr_;
- if (verbose_level > 0)
+ if (_verbose)
{
- cout << "T @ " << T.get() << endl;
+ _stream << "Trj @ " << T.get() << std::endl;
}
- // check pointer to Problem
- // is_consistent = is_consistent && (T->getProblem().get() == P_raw);
-
+ // check pointer to Problem
inconsistency_explanation << "Trajectory problem pointer is " << T->getProblem().get()
- << " but problem pointer is" << P_raw << "\n";
- log.compose(CheckLog((T->getProblem().get() == P_raw), inconsistency_explanation.str()));
- //Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
+ << " but problem pointer is" << P.get() << "\n";
+ log.assertTrue((T->getProblem() == P), inconsistency_explanation);
+
// Frames =======================================================================================
for (auto F : T->getFrameList())
{
- if (verbose_level > 0) {
- cout << (F->isKeyOrAux() ? (F->isKey() ? " KF" : " EF") : " F")
- << F->id() << " @ " << F.get() << endl;
- cout << " -> P @ " << F->getProblem().get() << endl;
- cout << " -> T @ " << F->getTrajectory().get() << endl;
+ if (_verbose) {
+ _stream << (F->isKeyOrAux() ? (F->isKey() ? " KFrm" : " EFrm") : " Frm")
+ << F->id() << " @ " << F.get() << std::endl;
+ _stream << " -> Prb @ " << F->getProblem().get() << std::endl;
+ _stream << " -> Trj @ " << F->getTrajectory().get() << std::endl;
}
- for (const auto &sb : F->getStateBlockVec()) {
- inconsistency_explanation << "Frame's " << F.get()
+ for (const auto &pair: F->getStateBlockMap()) {
+
+ auto sb = pair.second;
+
+ // check for valid state block
+ inconsistency_explanation << "Frame " << F->id() << " @ "<< F.get()
<< " has State block pointer " << sb.get()
- << " null! \n";
- log.compose(CheckLog((sb.get() != 0), inconsistency_explanation.str()));
- // Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
- if (verbose_level > 0) {
- cout << " sb @ " << sb.get();
- if (sb) {
- auto lp = sb->getLocalParametrization();
- if (lp)
- cout << " (lp @ " << lp.get() << ")";
+ << " = 0 \n";
+ log.assertTrue((sb.get() != 0), inconsistency_explanation);
+
+ if (_verbose) {
+ _stream << " "<< pair.first << " sb @ " << sb.get();
+ if (sb) {
+ auto lp = sb->getLocalParametrization();
+ if (lp)
+ _stream << " (lp @ " << lp.get() << ")";
+ }
+ _stream << std::endl;
}
- cout << endl;
- }
}
- // check problem and trajectory pointers
- // is_consistent = is_consistent && (F->getProblem().get() == P_raw);
-
+ // check problem pointer
inconsistency_explanation << "Frame problem pointer is " << F->getProblem().get()
- << " but problem pointer is" << P_raw << "\n";
- log.compose(CheckLog((F->getProblem().get() == P_raw), inconsistency_explanation.str()));
- //Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
-
- // is_consistent = is_consistent && (F->getTrajectory() == T);
+ << " but problem pointer is" << P.get() << "\n";
+ log.assertTrue((F->getProblem() == P), inconsistency_explanation);
+ // check trajectory pointer
inconsistency_explanation << "Frame trajectory pointer is " << F->getTrajectory()
- << " but trajectory pointer is" << T << "\n";
- log.compose(CheckLog((F->getTrajectory() == T), inconsistency_explanation.str()));
- //Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
+ << " but trajectory pointer is" << T << "\n";
+ log.assertTrue((F->getTrajectory() == T), inconsistency_explanation);
+
+ // check constrained_by
for (auto cby : F->getConstrainedByList())
{
- for (auto sb : cby->getStateBlockPtrVector()) {
- inconsistency_explanation
- << "Factor " << cby.get() << " constraining " << F.get()
- << " has State block pointer " << sb.get() << " null! \n";
- log.compose(
- CheckLog((sb.get() != 0), inconsistency_explanation.str()));
- // Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
- }
- if (verbose_level > 0) {
- cout << " <- c" << cby->id() << " -> F"
- << cby->getFrameOther()->id() << endl;
- }
- // check constrained_by pointer to this frame
- // is_consistent = is_consistent && (cby->getFrameOther() == F);
+ if (_verbose)
+ {
+ _stream << " <- c" << cby->id() << " -> ";
+ for (const auto& Fow : cby->getFrameOtherList())
+ _stream << " F" << Fow.lock()->id() << std::endl;
+ }
- inconsistency_explanation << "constrained-by frame pointer is " << cby->getFrameOther()
- << " but frame pointer is" << F << "\n";
- log.compose(CheckLog((cby->getFrameOther() == F), inconsistency_explanation.str()));
- //Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
+ // check constrained_by pointer to this frame
+ inconsistency_explanation << "constrained-by frame " << F->id() << " @ " << F
+ << " not found among constrained-by factors\n";
+ log.assertTrue((cby->hasFrameOther(F)), inconsistency_explanation);
for (auto sb : cby->getStateBlockPtrVector())
{
- if (verbose_level > 0) {
- cout << " sb @ " << sb.get();
+ if (_verbose) {
+ _stream << " sb @ " << sb.get();
if (sb) {
auto lp = sb->getLocalParametrization();
if (lp)
- cout << " (lp @ " << lp.get() << ")";
+ _stream << " (lp @ " << lp.get() << ")";
}
- cout << endl;
+ _stream << std::endl;
}
}
}
@@ -1634,293 +1389,383 @@ bool Problem::check(int verbose_level) const
// Captures =======================================================================================
for (auto C : F->getCaptureList())
{
- if (verbose_level > 0)
+ if (_verbose)
{
- cout << " C" << C->id() << " @ " << C.get() << " -> S";
- if (C->getSensor()) cout << C->getSensor()->id();
- else cout << "-";
- cout << endl;
- cout << " -> P @ " << C->getProblem().get() << endl;
- cout << " -> F" << C->getFrame()->id() << " @ " << C->getFrame().get() << endl;
+ _stream << " Cap" << C->id() << " @ " << C.get() << " -> Sen";
+ if (C->getSensor()) _stream << C->getSensor()->id();
+ else _stream << "-";
+ _stream << std::endl;
+ _stream << " -> Prb @ " << C->getProblem().get() << std::endl;
+ _stream << " -> Frm" << C->getFrame()->id() << " @ " << C->getFrame().get() << std::endl;
}
- for (auto sb : C->getStateBlockVec())
+ for (auto pair: C->getStateBlockMap())
+ {
+ auto sb = pair.second;
+
+ // check for valid state block
+ inconsistency_explanation << "Capture " << C->id() << " @ " << C.get() << " has State block pointer "
+ << sb.get() << " = 0 \n";
+ log.assertTrue((sb.get() != 0), inconsistency_explanation);
+
+ if (_verbose)
{
- if (verbose_level > 0)
- {
- cout << " sb @ " << sb.get();
+ _stream << " " << pair.first << " sb @ " << sb.get();
if (sb) {
- auto lp = sb->getLocalParametrization();
- if (lp)
- cout << " (lp @ " << lp.get() << ")";
- }
- cout << endl;
+ auto lp = sb->getLocalParametrization();
+ if (lp)
+ _stream << " (lp @ " << lp.get() << ")";
}
+ _stream << std::endl;
}
+ }
// check problem and frame pointers
- // is_consistent = is_consistent && (C->getProblem().get() == P_raw);
-
inconsistency_explanation << "Capture problem pointer is " << C->getProblem().get()
- << " but problem pointer is" << P_raw << "\n";
- log.compose(CheckLog((C->getProblem().get() == P_raw), inconsistency_explanation.str()));
- //Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
+ << " but problem pointer is" << P.get() << "\n";
+ log.assertTrue((C->getProblem() == P), inconsistency_explanation);
- // is_consistent = is_consistent && (C->getFrame() == F);
inconsistency_explanation << "Capture frame pointer is " << C->getFrame()
<< " but frame pointer is" << F << "\n";
- log.compose(CheckLog((C->getFrame() == F), inconsistency_explanation.str()));
- //Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
+ log.assertTrue((C->getFrame() == F), inconsistency_explanation);
+
+
+ // check contrained_by
+ for (const auto& cby : C->getConstrainedByList())
+ {
+ if (_verbose)
+ {
+ _stream << " <- c" << cby->id() << " -> ";
+ for (const auto& Cow : cby->getCaptureOtherList())
+ _stream << " C" << Cow.lock()->id();
+ _stream << std::endl;
+ }
+
+ // check constrained_by pointer to this capture
+ inconsistency_explanation << "constrained by capture " << C->id() << " @ " << C
+ << " not found among constrained-by factors\n";
+ log.assertTrue((cby->hasCaptureOther(C)), inconsistency_explanation);
+
+ for (auto sb : cby->getStateBlockPtrVector())
+ {
+ if (_verbose)
+ {
+ _stream << " sb @ " << sb.get();
+ if (sb)
+ {
+ auto lp = sb->getLocalParametrization();
+ if (lp)
+ _stream << " (lp @ " << lp.get() << ")";
+ }
+ _stream << std::endl;
+ }
+ }
+
+ }
// Features =======================================================================================
for (auto f : C->getFeatureList())
{
- if (verbose_level > 0)
+ if (_verbose)
{
- cout << " f" << f->id() << " @ " << f.get() << endl;
- cout << " -> P @ " << f->getProblem().get() << endl;
- cout << " -> C" << f->getCapture()->id() << " @ " << f->getCapture().get()
- << endl;
+ _stream << " Ftr" << f->id() << " @ " << f.get() << std::endl;
+ _stream << " -> Prb @ " << f->getProblem().get() << std::endl;
+ _stream << " -> Cap" << f->getCapture()->id() << " @ " << f->getCapture().get()
+ << std::endl;
}
- // check problem and capture pointers
- // is_consistent = is_consistent && (f->getProblem().get() == P_raw);
+ // check problem and capture pointers
inconsistency_explanation << "Feature frame pointer is " << f->getProblem().get()
- << " but problem pointer is" << P_raw << "\n";
- log.compose(CheckLog((f->getProblem().get() == P_raw), inconsistency_explanation.str()));
- //Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
+ << " but problem pointer is" << P.get() << "\n";
+ log.assertTrue((f->getProblem() == P), inconsistency_explanation);
- // is_consistent = is_consistent && (f->getCapture() == C);
inconsistency_explanation << "Feature capture pointer is " << f->getCapture()
<< " but capture pointer is" << C << "\n";
- log.compose(CheckLog((f->getCapture() == C), inconsistency_explanation.str()));
- //Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
+ log.assertTrue((f->getCapture() == C), inconsistency_explanation);
+
+ // check contrained_by
for (auto cby : f->getConstrainedByList())
{
- if (verbose_level > 0)
+ if (_verbose)
{
- cout << " <- c" << cby->id() << " -> f" << cby->getFeatureOther()->id() << endl;
+ _stream << " <- c" << cby->id() << " -> ";
+ for (const auto& fow : cby->getFeatureOtherList())
+ _stream << " f" << fow.lock()->id();
+ _stream << std::endl;
}
+
// check constrained_by pointer to this feature
- // is_consistent = is_consistent && (cby->getFeatureOther() == f);
+ inconsistency_explanation << "constrained by Feature " << f->id() << " @ " << f
+ << " not found among constrained-by factors\n";
+ log.assertTrue((cby->hasFeatureOther(f)), inconsistency_explanation);
- inconsistency_explanation << "constrained by Feature pointer is " << cby->getFeatureOther()
- << " but feature pointer is" << f << "\n";
- log.compose(CheckLog((cby->getFeatureOther() == f), inconsistency_explanation.str()));
- //Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
}
// Factors =======================================================================================
for (auto c : f->getFactorList())
{
- if (verbose_level > 0)
- cout << " c" << c->id() << " @ " << c.get();
-
- auto Fo = c->getFrameOther();
- auto Co = c->getCaptureOther();
- auto fo = c->getFeatureOther();
- auto Lo = c->getLandmarkOther();
+ if (_verbose)
+ _stream << " Fac" << c->id() << " @ " << c.get();
- if ( !Fo && !Co && !fo && !Lo ) // case ABSOLUTE:
+ if ( c->getFrameOtherList() .empty()
+ && c->getCaptureOtherList() .empty()
+ && c->getFeatureOtherList() .empty()
+ && c->getLandmarkOtherList().empty() ) // case ABSOLUTE:
{
- if (verbose_level > 0)
- cout << " --> Abs." << endl;
+ if (_verbose)
+ _stream << " --> Abs.";
}
// find constrained_by pointer in constrained frame
- if ( Fo ) // case FRAME:
+ for (const auto& Fow : c->getFrameOtherList())
{
- if (verbose_level > 0)
- cout << " --> F" << Fo->id() << " <- ";
- bool found = false;
- for (auto cby : Fo->getConstrainedByList())
+ if (!Fow.expired())
{
- if (verbose_level > 0)
- cout << " c" << cby->id();
- found = found || (c == cby);
- }
- if (verbose_level > 0)
- cout << endl;
- // check constrained_by pointer in constrained frame
- // is_consistent = is_consistent && found;
+ const auto& Fo = Fow.lock();
+ if (_verbose)
+ {
+ _stream << " ( --> F" << Fo->id() << " <- ";
+ for (auto cby : Fo->getConstrainedByList())
+ _stream << " c" << cby->id();
+ }
- inconsistency_explanation << "The constrained Feature " << Fo
- << " does not close the constrained-by loop start = " << c << "\n";
- log.compose(CheckLog((found), inconsistency_explanation.str()));
- //Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
+ // check constrained_by pointer in constrained frame
+ bool found = Fo->isConstrainedBy(c);
+ inconsistency_explanation << "The constrained Feature " << Fo->id() << " @ " << Fo
+ << " not found among constrained-by factors\n";
+ log.assertTrue((found), inconsistency_explanation);
+ }
}
+ if (_verbose && !c->getFrameOtherList().empty())
+ _stream << ")";
// find constrained_by pointer in constrained capture
- if ( Co ) // case CAPTURE:
+ for (const auto& Cow : c->getCaptureOtherList())
{
- if (verbose_level > 0)
- cout << " --> C" << Co->id() << " <- ";
- bool found = false;
- for (auto cby : Co->getConstrainedByList())
+ if (!Cow.expired())
{
- if (verbose_level > 0)
- cout << " c" << cby->id();
- found = found || (c == cby);
- }
- if (verbose_level > 0)
- cout << endl;
- // check constrained_by pointer in constrained frame
- // is_consistent = is_consistent && found;
+ const auto& Co = Cow.lock();
- inconsistency_explanation << "The constrained capture " << Co
- << " does not close the constrained-by loop start = " << c << "\n";
- log.compose(CheckLog((found), inconsistency_explanation.str()));
- //Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
+ if (_verbose)
+ {
+ _stream << " ( --> C" << Co->id() << " <- ";
+ for (auto cby : Co->getConstrainedByList())
+ _stream << " c" << cby->id();
+ }
+
+ // check constrained_by pointer in constrained frame
+ bool found = Co->isConstrainedBy(c);
+ inconsistency_explanation << "The constrained capture " << Co->id() << " @ " << Co
+ << " not found among constrained-by factors\n";
+ log.assertTrue((found), inconsistency_explanation);
+ }
}
+ if (_verbose && !c->getCaptureOtherList().empty())
+ _stream << ")";
// find constrained_by pointer in constrained feature
- if ( fo ) // case FEATURE:
+ for (const auto& fow : c->getFeatureOtherList())
{
- if (verbose_level > 0)
- cout << " --> f" << fo->id() << " <- ";
- bool found = false;
- for (auto cby : fo->getConstrainedByList())
+ if (!fow.expired())
{
- if (verbose_level > 0)
- cout << " c" << cby->id();
- found = found || (c == cby);
+ const auto& fo = fow.lock();
+ if (_verbose)
+ {
+ _stream << " ( --> f" << fo->id() << " <- ";
+ for (auto cby : fo->getConstrainedByList())
+ _stream << " c" << cby->id();
+ }
+
+ // check constrained_by pointer in constrained feature
+ bool found = fo->isConstrainedBy(c);
+ inconsistency_explanation << "The constrained feature " << fo->id() << " @ " << fo
+ << " not found among constrained-by factors\n";
+ log.assertTrue((found), inconsistency_explanation);
}
- if (verbose_level > 0)
- cout << endl;
- // check constrained_by pointer in constrained feature
- // is_consistent = is_consistent && found;
-
- inconsistency_explanation << "The constrained feature" << fo
- << " does not close the constrained-by loop start = " << c << "\n";
- log.compose(CheckLog((found), inconsistency_explanation.str()));
- //Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
}
+ if (_verbose && !c->getFeatureOtherList().empty())
+ _stream << ")";
// find constrained_by pointer in constrained landmark
- if ( Lo ) // case LANDMARK:
+ for (const auto& Low : c->getLandmarkOtherList())
{
- if (verbose_level > 0)
- cout << " --> L" << Lo->id() << " <- ";
- bool found = false;
- for (auto cby : Lo->getConstrainedByList())
+ if (Low.expired())
{
- if (verbose_level > 0)
- cout << " c" << cby->id();
- found = found || (c == cby);
+ const auto& Lo = Low.lock();
+
+ if (_verbose)
+ {
+ _stream << " ( --> L" << Lo->id() << " <- ";
+ for (auto cby : Lo->getConstrainedByList())
+ _stream << " c" << cby->id();
+ }
+
+ // check constrained_by pointer in constrained landmark
+ bool found = Lo->isConstrainedBy(c);
+ inconsistency_explanation << "The constrained landmark " << Lo->id() << " @ " << Lo
+ << " not found among constrained-by factors\n";
+ log.assertTrue((found), inconsistency_explanation);
+
}
- if (verbose_level > 0)
- cout << endl;
- // check constrained_by pointer in constrained landmark
- // is_consistent = is_consistent && found;
-
- inconsistency_explanation << "The constrained landmark " << Lo
- << " does not close the constrained-by loop start = " << c << "\n";
- log.compose(CheckLog((found), inconsistency_explanation.str()));
- //Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
}
- if (verbose_level > 0)
+ if (_verbose && !c->getLandmarkOtherList().empty())
+ _stream << ")";
+
+ if (_verbose)
+ _stream << std::endl;
+
+ if (_verbose)
{
- cout << " -> P @ " << c->getProblem().get() << endl;
- cout << " -> f" << c->getFeature()->id() << " @ " << c->getFeature().get() << endl;
+ _stream << " -> Prb @ " << c->getProblem().get() << std::endl;
+ _stream << " -> Ftr" << c->getFeature()->id() << " @ " << c->getFeature().get() << std::endl;
}
- // check problem and feature pointers
- // is_consistent = is_consistent && (c->getProblem().get() == P_raw);
- inconsistency_explanation << "The factor " << c << " has problem ptr " << c->getProblem().get()
- << " but problem ptr is " << P_raw << "\n";
- log.compose(CheckLog((c->getProblem().get() == P_raw), inconsistency_explanation.str()));
- //Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
+ // check problem and feature pointers
+ inconsistency_explanation << "The factor " << c->id() << " @ " << c << " has problem ptr " << c->getProblem().get()
+ << " but problem ptr is " << P.get() << "\n";
+ log.assertTrue((c->getProblem() == P), inconsistency_explanation);
- // is_consistent = is_consistent && (c->getFeature() == f);
- inconsistency_explanation << "The factor " << c << " has feature ptr " << c->getFeature()
+ inconsistency_explanation << "The factor " << c->id() << " @ " << c << " has feature ptr " << c->getFeature()
<< " but it should have " << f << "\n";
- log.compose(CheckLog((c->getProblem().get() == P_raw), inconsistency_explanation.str()));
- //Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
+ log.assertTrue((c->getProblem() == P), inconsistency_explanation);
+
// find state block pointers in all constrained nodes
SensorBasePtr S = c->getFeature()->getCapture()->getSensor(); // get own sensor to check sb
for (auto sb : c->getStateBlockPtrVector())
{
bool found = false;
- if (verbose_level > 0)
+ if (_verbose)
{
- cout << " sb @ " << sb.get();
+ _stream << " sb @ " << sb.get();
if (sb)
{
auto lp = sb->getLocalParametrization();
if (lp)
- cout << " (lp @ " << lp.get() << ")";
+ _stream << " (lp @ " << lp.get() << ")";
}
}
+ bool found_here;
+
// find in own Frame
- found = found || (std::find_if(F->getStateBlockMap().begin(), F->getStateBlockMap().end(), [sb](const std::pair<std::string, StateBlockPtr> & t)->bool {return t.second == sb;}) != F->getStateBlockMap().end());
+ found_here = F->hasStateBlock(sb);
+ if (found_here && _verbose) _stream << " F" << F->id();
+ found = found || found_here;
+
// find in own Capture
- found = found || (std::find(C->getStateBlockVec().begin(), C->getStateBlockVec().end(), sb) != C->getStateBlockVec().end());
+ found_here = C->hasStateBlock(sb);
+ if (found_here && _verbose) _stream << " C" << C->id();
+ found = found || found_here;
+
+ // Find in other Captures of the own Frame
+ if (!found_here)
+ for (auto FC : F->getCaptureList())
+ {
+ found_here = FC->hasStateBlock(sb);
+ if (found_here && _verbose) _stream << " F" << F->id() << ".C" << FC->id();
+ found = found || found_here;
+ }
+
// find in own Sensor
if (S)
{
- auto sb_vec = S->getStateBlockVec();
- found = found || (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
+ found_here = S->hasStateBlock(sb);
+ if (found_here && _verbose) _stream << " S" << S->id();
+ found = found || found_here;
}
+
+
// find in constrained Frame
- if (Fo)
- found = found || (std::find_if(Fo->getStateBlockMap().begin(), Fo->getStateBlockMap().end(), [sb](const std::pair<std::string, StateBlockPtr> & t)->bool {return t.second == sb;}) != Fo->getStateBlockMap().end());
+ for (const auto& Fow : c->getFrameOtherList())
+ {
+ if (!Fow.expired())
+ {
+ const auto& Fo = Fow.lock();
+ found_here = Fo->hasStateBlock(sb);
+ if (found_here && _verbose) _stream << " Fo" << Fo->id();
+ found = found || found_here;
+
+ // find in feature other's captures
+ for (auto FoC : Fo->getCaptureList())
+ {
+ found_here = FoC->hasStateBlock(sb);
+ if (found_here && _verbose) _stream << " Fo" << Fo->id() << ".C" << FoC->id();
+ found = found || found_here;
+ }
+
+ }
+ }
+
// find in constrained Capture
- if (Co)
- found = found || (std::find(Co->getStateBlockVec().begin(), Co->getStateBlockVec().end(), sb) != Co->getStateBlockVec().end());
+ for (const auto& Cow : c->getCaptureOtherList())
+ {
+ if (!Cow.expired())
+ {
+ const auto& Co = Cow.lock();
+ found_here = Co->hasStateBlock(sb);
+ if (found_here && _verbose) _stream << " Co" << Co->id();
+ found = found || found_here;
+ }
+ }
+
// find in constrained Feature
- if (fo)
+ for (const auto& fow : c->getFeatureOtherList())
{
- // find in constrained feature's Frame
- FrameBasePtr foF = fo->getFrame();
- found = found || (std::find_if(foF->getStateBlockMap().begin(), foF->getStateBlockMap().end(), [sb](const std::pair<std::string, StateBlockPtr> & t)->bool {return t.second == sb;}) != foF->getStateBlockMap().end());
- // find in constrained feature's Capture
- CaptureBasePtr foC = fo->getCapture();
- found = found || (std::find(foC->getStateBlockVec().begin(), foC->getStateBlockVec().end(), sb) != foC->getStateBlockVec().end());
- // find in constrained feature's Sensor
- SensorBasePtr foS = fo->getCapture()->getSensor();
- found = found || (std::find(foS->getStateBlockVec().begin(), foS->getStateBlockVec().end(), sb) != foS->getStateBlockVec().end());
+ if (!fow.expired())
+ {
+ const auto& fo = fow.lock();
+ // find in constrained feature's Frame
+ auto foF = fo->getFrame();
+ found_here = foF->hasStateBlock(sb);
+ if (found_here && _verbose) _stream << " foF" << foF->id();
+ found = found || found_here;
+
+ // find in constrained feature's Capture
+ auto foC = fo->getCapture();
+ found_here = foC->hasStateBlock(sb);
+ if (found_here && _verbose) _stream << " foC" << foC->id();
+ found = found || found_here;
+
+ // find in constrained feature's Sensor
+ auto foS = fo->getCapture()->getSensor();
+ found_here = foS->hasStateBlock(sb);
+ if (found_here && _verbose) _stream << " foS" << foS->id();
+ found = found || found_here;
+ }
}
+
// find in constrained landmark
- if (Lo)
- found = found || (std::find_if(Lo->getStateBlockMap().begin(), Lo->getStateBlockMap().end(), [sb](const std::pair<std::string, StateBlockPtr> & t)->bool {return t.second == sb;}) != Lo->getStateBlockMap().end());
- if (verbose_level > 0)
+ for (const auto& Low : c->getLandmarkOtherList())
+ {
+ if (!Low.expired())
+ {
+ const auto& Lo = Low.lock();
+ found_here = Lo->hasStateBlock(sb);
+ if (found_here && _verbose) _stream << " Lo" << Lo->id();
+ found = found || found_here;
+ }
+ }
+
+ if (_verbose)
{
if (found)
- cout << " found";
+ _stream << " found";
else
- cout << " NOT FOUND !";
- cout << endl;
+ _stream << " NOT FOUND !";
+ _stream << std::endl;
}
+ // check that the state block has been found somewhere
inconsistency_explanation << "The stateblock " << sb << " has not been found (is floating!)";
- log.compose(CheckLog((found), inconsistency_explanation.str()));
- //Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
-
- inconsistency_explanation << "The stateblock " << sb << " of factor " << c << " is null\n";
- log.compose(CheckLog((sb.get() != nullptr), inconsistency_explanation.str()));
- //Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
+ log.assertTrue((found), inconsistency_explanation);
- // check that all state block pointers were found
- // is_consistent = is_consistent && found;
+ inconsistency_explanation << "The stateblock " << sb << " of factor " << c->id() << " @ " << c << " is null\n";
+ log.assertTrue((sb.get() != nullptr), inconsistency_explanation);
}
}
}
@@ -1930,94 +1775,98 @@ bool Problem::check(int verbose_level) const
// MAP branch
// ------------------------
auto M = map_ptr_;
- if (verbose_level > 0)
- cout << "M @ " << M.get() << endl;
- // check pointer to Problem
- // is_consistent = is_consistent && (M->getProblem().get() == P_raw);
+ if (_verbose)
+ _stream << "Map @ " << M.get() << std::endl;
- inconsistency_explanation << "The map problem ptr is " << M->getProblem().get() << " but problem is " << P_raw << "\n";
- log.compose(CheckLog((M->getProblem().get() == P_raw), inconsistency_explanation.str()));
- //Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
+ // check pointer to Problem
+ inconsistency_explanation << "The map problem ptr is " << M->getProblem().get() << " but problem is " << P.get() << "\n";
+ log.assertTrue((M->getProblem() == P), inconsistency_explanation);
// Landmarks =======================================================================================
for (auto L : M->getLandmarkList())
{
- if (verbose_level > 0)
+ if (_verbose)
{
- cout << " L" << L->id() << " @ " << L.get() << endl;
- cout << " -> P @ " << L->getProblem().get() << endl;
- cout << " -> M @ " << L->getMap().get() << endl;
- for (const auto& sb : L->getStateBlockVec())
+ _stream << " Lmk" << L->id() << " @ " << L.get() << std::endl;
+ _stream << " -> Prb @ " << L->getProblem().get() << std::endl;
+ _stream << " -> Map @ " << L->getMap().get() << std::endl;
+ for (const auto& pair : L->getStateBlockMap())
{
- cout << " sb @ " << sb.get();
+ auto sb = pair.second;
+ _stream << " " << pair.first << " sb @ " << sb.get();
if (sb)
{
auto lp = sb->getLocalParametrization();
if (lp)
- cout << " (lp @ " << lp.get() << ")";
+ _stream << " (lp @ " << lp.get() << ")";
}
- cout << endl;
+ _stream << std::endl;
}
}
- // check problem and map pointers
- // is_consistent = is_consistent && (L->getProblem().get() == P_raw);
- inconsistency_explanation << "The landmarks problem ptr is "
+ // check problem and map pointers
+ inconsistency_explanation << "Landmarks' problem ptr is "
<< L->getProblem().get() << " but problem is "
- << P_raw << "\n";
+ << P.get() << "\n";
- log.compose(CheckLog((L->getProblem().get() == P_raw), inconsistency_explanation.str()));
- // Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
+ log.assertTrue((L->getProblem() == P), inconsistency_explanation);
- // is_consistent = is_consistent && (L->getMap() == M);
-
- inconsistency_explanation << "The landmarks map ptr is "
+ inconsistency_explanation << "The Landmarks' map ptr is "
<< L->getMap() << " but map is "
<< M << "\n";
- log.compose(CheckLog((M->getProblem().get() == P_raw), inconsistency_explanation.str()));
- // Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
-
- for (auto cby : L->getConstrainedByList()) {
- if (verbose_level > 0)
- cout << " <- c" << cby->id() << " -> L"
- << cby->getLandmarkOther()->id() << endl;
- // check constrained_by pointer to this landmark
- // is_consistent =
- // is_consistent && (cby->getLandmarkOther() &&
- // L->id() == cby->getLandmarkOther()->id());
-
- inconsistency_explanation << "The landmark constrained-by loop started at "
- << cby
- << " is not closed\n";
- log.compose(CheckLog((cby->getLandmarkOther() &&
- L->id() == cby->getLandmarkOther()->id()), inconsistency_explanation.str()));
- // Clear inconsistency_explanation
- std::stringstream().swap(inconsistency_explanation);
+ log.assertTrue((M->getProblem() == P), inconsistency_explanation);
+
+ for (auto cby : L->getConstrainedByList())
+ {
+ if (_verbose)
+ {
+ _stream << " <- Fac" << cby->id() << " ->";
+
+ for (const auto& Low : cby->getLandmarkOtherList())
+ {
+ if (!Low.expired())
+ {
+ const auto& Lo = Low.lock();
+ _stream << " Lmk" << Lo->id();
+ }
+ }
+ _stream << std::endl;
+ }
+
+ // check constrained-by factors
+ inconsistency_explanation << "constrained-by landmark " << L->id() << " @ " << L
+ << " not found among constrained-by factors\n";
+ log.assertTrue((cby->hasLandmarkOther(L)), inconsistency_explanation);
for (auto sb : cby->getStateBlockPtrVector()) {
- if (verbose_level > 0) {
- cout << " sb @ " << sb.get();
+ if (_verbose) {
+ _stream << " sb @ " << sb.get();
if (sb) {
auto lp = sb->getLocalParametrization();
if (lp)
- cout << " (lp @ " << lp.get() << ")";
+ _stream << " (lp @ " << lp.get() << ")";
}
- cout << endl;
+ _stream << std::endl;
}
}
}
}
- if (verbose_level) cout << "--------------------------- Wolf tree " << (log.is_consistent_ ? " OK" : "Not OK !!") << endl;
- if (verbose_level) cout << endl;
- if (verbose_level and not log.is_consistent_) cout << log.explanation_ << endl;
+ if (_verbose) _stream << "--------------------------- Wolf tree " << (log.is_consistent_ ? " OK" : "Not OK !!") << std::endl;
+ if (_verbose) _stream << std::endl;
+ if (_verbose and not log.is_consistent_) _stream << log.explanation_ << std::endl;
return log.is_consistent_;
}
+bool Problem::check(int _verbose_level) const
+{
+ return check((_verbose_level > 0), std::cout);
+}
+void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks) const
+{
+ print(depth, std::cout, constr_by, metric, state_blocks);
+}
void Problem::print(const std::string& depth, bool constr_by, bool metric, bool state_blocks) const
{
if (depth.compare("T") == 0)
@@ -2034,4 +1883,26 @@ void Problem::print(const std::string& depth, bool constr_by, bool metric, bool
print(0, constr_by, metric, state_blocks);
}
+void Problem::perturb(double amplitude)
+{
+ // Sensors
+ for (auto& S : getHardware()->getSensorList())
+ S->perturb(amplitude);
+
+ // Frames and Captures
+ for (auto& F : getTrajectory()->getFrameList())
+ {
+ if (F->isKeyOrAux())
+ {
+ F->perturb(amplitude);
+ for (auto& C : F->getCaptureList())
+ C->perturb(amplitude);
+ }
+ }
+
+ // Landmarks
+ for (auto& L : getMap()->getLandmarkList())
+ L->perturb(amplitude);
+}
+
} // namespace wolf
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index d2ead9ce8ad15391d155f3f3a5bf4f96d276cbcf..f61f861b021e51a08dd92db301aa1bd8de159272 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -7,10 +7,11 @@ namespace wolf {
unsigned int ProcessorBase::processor_id_count_ = 0;
-ProcessorBase::ProcessorBase(const std::string& _type, ProcessorParamsBasePtr _params) :
+ProcessorBase::ProcessorBase(const std::string& _type, int _dim, ProcessorParamsBasePtr _params) :
NodeBase("PROCESSOR", _type),
processor_id_(++processor_id_count_),
params_(_params),
+ dim_(_dim),
sensor_ptr_()
{
// WOLF_DEBUG("constructed +p" , id());
diff --git a/src/processor/processor_diff_drive.cpp b/src/processor/processor_diff_drive.cpp
index e8318e8cdd00696ec87dcf9cf5451ca57b98e754..d80f89ccee5cddff4aab89ec3737c5b518463e7e 100644
--- a/src/processor/processor_diff_drive.cpp
+++ b/src/processor/processor_diff_drive.cpp
@@ -17,13 +17,13 @@ namespace wolf
{
ProcessorDiffDrive::ProcessorDiffDrive(const ProcessorParamsDiffDrivePtr _params) :
- ProcessorOdom2D(_params),
+ ProcessorOdom2d(_params),
params_prc_diff_drv_selfcal_(_params),
radians_per_tick_(0.0) // This param needs further initialization. See this->configure(sensor).
{
- setType("ProcessorDiffDrive"); // overwrite odom2D setting
- calib_size_ = 3; // overwrite odom2D setting
- unmeasured_perturbation_cov_ = Matrix1d(_params->unmeasured_perturbation_std * _params->unmeasured_perturbation_std); // overwrite odom2D setting
+ setType("ProcessorDiffDrive"); // overwrite odom2d setting
+ calib_size_ = 3; // overwrite odom2d setting
+ unmeasured_perturbation_cov_ = Matrix1d(_params->unmeasured_perturbation_std * _params->unmeasured_perturbation_std); // overwrite odom2d setting
}
ProcessorDiffDrive::~ProcessorDiffDrive()
@@ -36,7 +36,8 @@ void ProcessorDiffDrive::configure(SensorBasePtr _sensor)
{
assert(_sensor && "Provided sensor is nullptr");
- SensorDiffDriveConstPtr sensor_diff_drive = std::static_pointer_cast<SensorDiffDrive>(_sensor);
+ SensorDiffDriveConstPtr sensor_diff_drive = std::dynamic_pointer_cast<SensorDiffDrive>(_sensor);
+ assert(sensor_diff_drive != nullptr && "Sensor is not of type SensorDiffDrive");
radians_per_tick_ = sensor_diff_drive->getRadiansPerTick();
}
diff --git a/src/processor/processor_loopclosure.cpp b/src/processor/processor_loopclosure.cpp
index 86fb02bf67e309071c9190b839ffcfc638422e22..3517f9e20d5bb55c40ebde25636411c5be02d9cb 100644
--- a/src/processor/processor_loopclosure.cpp
+++ b/src/processor/processor_loopclosure.cpp
@@ -11,8 +11,8 @@
namespace wolf
{
-ProcessorLoopClosure::ProcessorLoopClosure(const std::string& _type, ProcessorParamsLoopClosurePtr _params_loop_closure):
- ProcessorBase(_type, _params_loop_closure),
+ProcessorLoopClosure::ProcessorLoopClosure(const std::string& _type, int _dim, ProcessorParamsLoopClosurePtr _params_loop_closure):
+ ProcessorBase(_type, _dim, _params_loop_closure),
params_loop_closure_(_params_loop_closure)
{
//
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 9a0cbb90d8284e1fb88a86fa2dd821d0d839b208..11db736b56699ff204f8ff262d63236a625297a7 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"
@@ -8,13 +15,14 @@ namespace wolf
ProcessorMotion::ProcessorMotion(const std::string& _type,
std::string _state_structure,
+ int _dim,
SizeEigen _state_size,
SizeEigen _delta_size,
SizeEigen _delta_cov_size,
SizeEigen _data_size,
SizeEigen _calib_size,
ProcessorParamsMotionPtr _params_motion) :
- ProcessorBase(_type, _params_motion),
+ ProcessorBase(_type, _dim, _params_motion),
params_motion_(_params_motion),
processing_step_(RUNNING_WITHOUT_PACK),
x_size_(_state_size),
@@ -369,7 +377,7 @@ bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXd& _x) const
// Get delta and correct it with new calibration params
VectorXd calib_preint = capture_motion->getCalibrationPreint();
Motion motion = capture_motion->getBuffer().getMotion(_ts);
-
+
VectorXd delta_step = motion.jacobian_calib_ * (calib - calib_preint);
VectorXd delta = capture_motion->correctDelta( motion.delta_integr_, delta_step);
@@ -627,7 +635,10 @@ PackKeyFramePtr ProcessorMotion::computeProcessingStep()
void ProcessorMotion::setProblem(ProblemPtr _problem)
{
- if (_problem == nullptr || _problem == this->getProblem())
+ std::string str = "Processor works with " + std::to_string(dim_) + "D but problem is " + std::to_string(_problem->getDim()) + "D";
+ assert((dim_ == 0 or dim_ == _problem->getDim()) && str.c_str());
+
+ if (_problem == nullptr or _problem == this->getProblem())
return;
NodeBase::setProblem(_problem);
diff --git a/src/processor/processor_odom_2D.cpp b/src/processor/processor_odom_2d.cpp
similarity index 80%
rename from src/processor/processor_odom_2D.cpp
rename to src/processor/processor_odom_2d.cpp
index 7841bfbbf4244eba7b559b889022446439bb117e..929c1ed7ceb825fdaa265a1af592fa8125832789 100644
--- a/src/processor/processor_odom_2D.cpp
+++ b/src/processor/processor_odom_2d.cpp
@@ -1,21 +1,27 @@
-#include "core/processor/processor_odom_2D.h"
+#include "core/processor/processor_odom_2d.h"
+#include "core/sensor/sensor_odom_2d.h"
#include "core/math/covariance.h"
namespace wolf
{
-ProcessorOdom2D::ProcessorOdom2D(ProcessorParamsOdom2DPtr _params) :
- ProcessorMotion("ProcessorOdom2D", "PO", 3, 3, 3, 2, 0, _params),
- params_odom_2D_(_params)
+ProcessorOdom2d::ProcessorOdom2d(ProcessorParamsOdom2dPtr _params) :
+ ProcessorMotion("ProcessorOdom2d", "PO", 2, 3, 3, 3, 2, 0, _params),
+ params_odom_2d_(_params)
{
unmeasured_perturbation_cov_ = _params->unmeasured_perturbation_std * _params->unmeasured_perturbation_std * Matrix3d::Identity();
}
-ProcessorOdom2D::~ProcessorOdom2D()
+ProcessorOdom2d::~ProcessorOdom2d()
{
}
-void ProcessorOdom2D::computeCurrentDelta(const Eigen::VectorXd& _data, const Eigen::MatrixXd& _data_cov,
+void ProcessorOdom2d::configure(SensorBasePtr _sensor)
+{
+ auto sensor_ = std::dynamic_pointer_cast<SensorOdom2d>(_sensor);
+ assert(sensor_ != nullptr && "Sensor is not of type SensorOdom2d");
+}
+void ProcessorOdom2d::computeCurrentDelta(const Eigen::VectorXd& _data, const Eigen::MatrixXd& _data_cov,
const Eigen::VectorXd& _calib, const double _dt, Eigen::VectorXd& _delta,
Eigen::MatrixXd& _delta_cov, Eigen::MatrixXd& _jacobian_calib) const
{
@@ -45,19 +51,19 @@ void ProcessorOdom2D::computeCurrentDelta(const Eigen::VectorXd& _data, const Ei
_delta_cov = J_delta_data * _data_cov * J_delta_data.transpose() + unmeasured_perturbation_cov_;
}
-void ProcessorOdom2D::deltaPlusDelta(const Eigen::VectorXd& _delta1, const Eigen::VectorXd& _delta2, const double _Dt2,
+void ProcessorOdom2d::deltaPlusDelta(const Eigen::VectorXd& _delta1, const Eigen::VectorXd& _delta2, const double _Dt2,
Eigen::VectorXd& _delta1_plus_delta2) const
{
assert(_delta1.size() == delta_size_ && "Wrong _delta1 vector size");
assert(_delta2.size() == delta_size_ && "Wrong _delta2 vector size");
assert(_delta1_plus_delta2.size() == delta_size_ && "Wrong _delta1_plus_delta2 vector size");
- // This is just a frame composition in 2D
+ // This is just a frame composition in 2d
_delta1_plus_delta2.head<2>() = _delta1.head<2>() + Eigen::Rotation2Dd(_delta1(2)).matrix() * _delta2.head<2>();
_delta1_plus_delta2(2) = pi2pi(_delta1(2) + _delta2(2));
}
-void ProcessorOdom2D::deltaPlusDelta(const Eigen::VectorXd& _delta1, const Eigen::VectorXd& _delta2, const double _Dt2,
+void ProcessorOdom2d::deltaPlusDelta(const Eigen::VectorXd& _delta1, const Eigen::VectorXd& _delta2, const double _Dt2,
Eigen::VectorXd& _delta1_plus_delta2, Eigen::MatrixXd& _jacobian1,
Eigen::MatrixXd& _jacobian2) const
{
@@ -69,7 +75,7 @@ void ProcessorOdom2D::deltaPlusDelta(const Eigen::VectorXd& _delta1, const Eigen
assert(_jacobian2.rows() == delta_cov_size_ && "Wrong _jacobian2 size");
assert(_jacobian2.cols() == delta_cov_size_ && "Wrong _jacobian2 size");
- // This is just a frame composition in 2D
+ // This is just a frame composition in 2d
_delta1_plus_delta2.head<2>() = _delta1.head<2>() + Eigen::Rotation2Dd(_delta1(2)).matrix() * _delta2.head<2>();
_delta1_plus_delta2(2) = pi2pi(_delta1(2) + _delta2(2));
@@ -83,38 +89,38 @@ void ProcessorOdom2D::deltaPlusDelta(const Eigen::VectorXd& _delta1, const Eigen
_jacobian2.topLeftCorner<2, 2>() = Eigen::Rotation2Dd(_delta1(2)).matrix();
}
-void ProcessorOdom2D::statePlusDelta(const Eigen::VectorXd& _x, const Eigen::VectorXd& _delta, const double _Dt,
+void ProcessorOdom2d::statePlusDelta(const Eigen::VectorXd& _x, const Eigen::VectorXd& _delta, const double _Dt,
Eigen::VectorXd& _x_plus_delta) const
{
assert(_x.size() == x_size_ && "Wrong _x vector size");
assert(_x_plus_delta.size() == x_size_ && "Wrong _x_plus_delta vector size");
- // This is just a frame composition in 2D
+ // This is just a frame composition in 2d
_x_plus_delta.head<2>() = _x.head<2>() + Eigen::Rotation2Dd(_x(2)).matrix() * _delta.head<2>();
_x_plus_delta(2) = pi2pi(_x(2) + _delta(2));
}
-bool ProcessorOdom2D::voteForKeyFrame() const
+bool ProcessorOdom2d::voteForKeyFrame() const
{
// Distance criterion
- if (getBuffer().get().back().delta_integr_.head<2>().norm() > params_odom_2D_->dist_traveled)
+ if (getBuffer().get().back().delta_integr_.head<2>().norm() > params_odom_2d_->dist_traveled)
{
WOLF_DEBUG("PM", id(), " ", getType(), " votes per distance");
return true;
}
- if (getBuffer().get().back().delta_integr_.tail<1>().norm() > params_odom_2D_->angle_turned)
+ if (getBuffer().get().back().delta_integr_.tail<1>().norm() > params_odom_2d_->angle_turned)
{
WOLF_DEBUG("PM", id(), " ", getType(), " votes per angle");
return true;
}
// Uncertainty criterion
- if (getBuffer().get().back().delta_integr_cov_.determinant() > params_odom_2D_->cov_det)
+ if (getBuffer().get().back().delta_integr_cov_.determinant() > params_odom_2d_->cov_det)
{
WOLF_DEBUG("PM", id(), " ", getType(), " votes per state covariance");
return true;
}
// Time criterion
- if (getBuffer().get().back().ts_.get() - origin_ptr_->getFrame()->getTimeStamp().get() > params_odom_2D_->max_time_span)
+ if (getBuffer().get().back().ts_.get() - origin_ptr_->getFrame()->getTimeStamp().get() > params_odom_2d_->max_time_span)
{
WOLF_DEBUG("PM", id(), " ", getType(), " votes per time");
return true;
@@ -122,7 +128,7 @@ bool ProcessorOdom2D::voteForKeyFrame() const
return false;
}
-CaptureMotionPtr ProcessorOdom2D::emplaceCapture(const FrameBasePtr& _frame_own,
+CaptureMotionPtr ProcessorOdom2d::emplaceCapture(const FrameBasePtr& _frame_own,
const SensorBasePtr& _sensor,
const TimeStamp& _ts,
const VectorXd& _data,
@@ -131,16 +137,16 @@ CaptureMotionPtr ProcessorOdom2D::emplaceCapture(const FrameBasePtr& _frame_own,
const VectorXd& _calib_preint,
const CaptureBasePtr& _capture_origin_ptr)
{
- auto cap_motion = CaptureBase::emplace<CaptureOdom2D>(_frame_own, _ts, _sensor, _data, _data_cov, _capture_origin_ptr);
+ auto cap_motion = CaptureBase::emplace<CaptureOdom2d>(_frame_own, _ts, _sensor, _data, _data_cov, _capture_origin_ptr);
cap_motion->setCalibration(_calib);
cap_motion->setCalibrationPreint(_calib_preint);
return cap_motion;
}
-FactorBasePtr ProcessorOdom2D::emplaceFactor(FeatureBasePtr _feature, CaptureBasePtr _capture_origin)
+FactorBasePtr ProcessorOdom2d::emplaceFactor(FeatureBasePtr _feature, CaptureBasePtr _capture_origin)
{
- auto fac_odom = FactorBase::emplace<FactorOdom2D>(_feature,
+ auto fac_odom = FactorBase::emplace<FactorOdom2d>(_feature,
_feature,
_capture_origin->getFrame(),
shared_from_this(),
@@ -148,13 +154,13 @@ FactorBasePtr ProcessorOdom2D::emplaceFactor(FeatureBasePtr _feature, CaptureBas
return fac_odom;
}
-FeatureBasePtr ProcessorOdom2D::emplaceFeature(CaptureMotionPtr _capture_motion)
+FeatureBasePtr ProcessorOdom2d::emplaceFeature(CaptureMotionPtr _capture_motion)
{
Eigen::MatrixXd covariance = _capture_motion->getBuffer().get().back().delta_integr_cov_;
makePosDef(covariance);
FeatureBasePtr key_feature_ptr = FeatureBase::emplace<FeatureBase>(_capture_motion,
- "ProcessorOdom2D",
+ "ProcessorOdom2d",
_capture_motion->getBuffer().get().back().delta_integr_,
covariance);
return key_feature_ptr;
@@ -167,6 +173,6 @@ FeatureBasePtr ProcessorOdom2D::emplaceFeature(CaptureMotionPtr _capture_motion)
// Register in the ProcessorFactory
#include "core/processor/processor_factory.h"
namespace wolf {
-WOLF_REGISTER_PROCESSOR("ProcessorOdom2D", ProcessorOdom2D);
-WOLF_REGISTER_PROCESSOR_AUTO("ProcessorOdom2D", ProcessorOdom2D);
+WOLF_REGISTER_PROCESSOR("ProcessorOdom2d", ProcessorOdom2d);
+WOLF_REGISTER_PROCESSOR_AUTO("ProcessorOdom2d", ProcessorOdom2d);
} // namespace wolf
diff --git a/src/processor/processor_odom_3D.cpp b/src/processor/processor_odom_3d.cpp
similarity index 85%
rename from src/processor/processor_odom_3D.cpp
rename to src/processor/processor_odom_3d.cpp
index 4282c09ef402a1d51b2086b049b2a9a6974fa0ff..3e1268eb1d705b3a335d9c8d9389e0c9449ebba1 100644
--- a/src/processor/processor_odom_3D.cpp
+++ b/src/processor/processor_odom_3d.cpp
@@ -1,10 +1,10 @@
-#include "core/processor/processor_odom_3D.h"
+#include "core/processor/processor_odom_3d.h"
namespace wolf
{
-ProcessorOdom3D::ProcessorOdom3D(ProcessorParamsOdom3DPtr _params) :
- ProcessorMotion("ProcessorOdom3D", "PO", 7, 7, 6, 6, 0, _params),
- params_odom_3D_ (_params),
+ProcessorOdom3d::ProcessorOdom3d(ProcessorParamsOdom3dPtr _params) :
+ ProcessorMotion("ProcessorOdom3d", "PO", 3, 7, 7, 6, 6, 0, _params),
+ params_odom_3d_ (_params),
k_disp_to_disp_ (0),
k_disp_to_rot_ (0),
k_rot_to_rot_ (0),
@@ -14,17 +14,17 @@ ProcessorOdom3D::ProcessorOdom3D(ProcessorParamsOdom3DPtr _params) :
//
}
-ProcessorOdom3D::~ProcessorOdom3D()
+ProcessorOdom3d::~ProcessorOdom3d()
{
}
-void ProcessorOdom3D::configure(SensorBasePtr _sensor)
+void ProcessorOdom3d::configure(SensorBasePtr _sensor)
{
assert (_sensor && "Trying to configure processor with a sensor but provided sensor is nullptr.");
+ auto sen_ptr = std::dynamic_pointer_cast<SensorOdom3d>(_sensor);
+ assert(sen_ptr != nullptr && "Sensor is not of type SensorOdom3d");
- SensorOdom3DPtr sen_ptr = std::static_pointer_cast<SensorOdom3D>(_sensor);
-
- // we steal the parameters from the provided odom3D sensor.
+ // we steal the parameters from the provided odom3d sensor.
k_disp_to_disp_ = sen_ptr->getDispVarToDispNoiseFactor();
k_disp_to_rot_ = sen_ptr->getDispVarToRotNoiseFactor();
k_rot_to_rot_ = sen_ptr->getRotVarToRotNoiseFactor();
@@ -32,7 +32,7 @@ void ProcessorOdom3D::configure(SensorBasePtr _sensor)
min_rot_var_ = sen_ptr->getMinRotVar();
}
-void ProcessorOdom3D::computeCurrentDelta(const Eigen::VectorXd& _data,
+void ProcessorOdom3d::computeCurrentDelta(const Eigen::VectorXd& _data,
const Eigen::MatrixXd& _data_cov,
const Eigen::VectorXd& _calib,
const double _dt,
@@ -40,7 +40,7 @@ void ProcessorOdom3D::computeCurrentDelta(const Eigen::VectorXd& _data,
Eigen::MatrixXd& _delta_cov,
Eigen::MatrixXd& _jacobian_calib) const
{
- assert((_data.size() == 6 || _data.size() == 7) && "Wrong data size. Must be 6 or 7 for 3D.");
+ assert((_data.size() == 6 || _data.size() == 7) && "Wrong data size. Must be 6 or 7 for 3d.");
double disp, rot; // displacement and rotation of this motion step
if (_data.size() == (long int)6)
{
@@ -81,7 +81,7 @@ void ProcessorOdom3D::computeCurrentDelta(const Eigen::VectorXd& _data,
_delta_cov = data_cov;
}
-void ProcessorOdom3D::deltaPlusDelta(const Eigen::VectorXd& _delta1, const Eigen::VectorXd& _delta2, const double _Dt2,
+void ProcessorOdom3d::deltaPlusDelta(const Eigen::VectorXd& _delta1, const Eigen::VectorXd& _delta2, const double _Dt2,
Eigen::VectorXd& _delta1_plus_delta2, Eigen::MatrixXd& _jacobian1,
Eigen::MatrixXd& _jacobian2) const
{
@@ -130,7 +130,7 @@ void ProcessorOdom3D::deltaPlusDelta(const Eigen::VectorXd& _delta1, const Eigen
dq_out = dq1 * dq2;
}
-void ProcessorOdom3D::deltaPlusDelta(const Eigen::VectorXd& _delta1, const Eigen::VectorXd& _delta2, const double _Dt2,
+void ProcessorOdom3d::deltaPlusDelta(const Eigen::VectorXd& _delta1, const Eigen::VectorXd& _delta2, const double _Dt2,
Eigen::VectorXd& _delta1_plus_delta2) const
{
assert(_delta1.size() == delta_size_ && "Wrong _delta1 vector size");
@@ -149,9 +149,9 @@ void ProcessorOdom3D::deltaPlusDelta(const Eigen::VectorXd& _delta1, const Eigen
dq_out = dq1 * dq2;
}
-void ProcessorOdom3D::statePlusDelta(const Eigen::VectorXd& _x, const Eigen::VectorXd& _delta, const double _Dt,
+void ProcessorOdom3d::statePlusDelta(const Eigen::VectorXd& _x, const Eigen::VectorXd& _delta, const double _Dt,
Eigen::VectorXd& _x_plus_delta) const
-{
+{
assert(_x.size() >= x_size_ && "Wrong _x vector size"); //we need a state vector which size is at least x_size_
assert(_delta.size() == delta_size_ && "Wrong _delta vector size");
assert(_x_plus_delta.size() >= x_size_ && "Wrong _x_plus_delta vector size");
@@ -170,7 +170,7 @@ void ProcessorOdom3D::statePlusDelta(const Eigen::VectorXd& _x, const Eigen::Vec
-bool ProcessorOdom3D::voteForKeyFrame() const
+bool ProcessorOdom3d::voteForKeyFrame() const
{
//WOLF_DEBUG( "Time span : " , getBuffer().get().back().ts_ - getBuffer().get().front().ts_ );
//WOLF_DEBUG( " last ts : ", getBuffer().get().back().ts_);
@@ -179,27 +179,27 @@ bool ProcessorOdom3D::voteForKeyFrame() const
//WOLF_DEBUG( "DistTraveled: " , delta_integrated_.head(3).norm() );
//WOLF_DEBUG( "AngleTurned : " , 2.0 * acos(delta_integrated_(6)) );
// time span
- if (getBuffer().get().back().ts_ - getBuffer().get().front().ts_ > params_odom_3D_->max_time_span)
+ if (getBuffer().get().back().ts_ - getBuffer().get().front().ts_ > params_odom_3d_->max_time_span)
{
WOLF_DEBUG( "PM: vote: time span" );
return true;
}
// buffer length
- if (getBuffer().get().size() > params_odom_3D_->max_buff_length)
+ if (getBuffer().get().size() > params_odom_3d_->max_buff_length)
{
WOLF_DEBUG( "PM: vote: buffer size" );
return true;
}
// distance traveled
double dist = getMotion().delta_integr_.head(3).norm();
- if (dist > params_odom_3D_->dist_traveled)
+ if (dist > params_odom_3d_->dist_traveled)
{
WOLF_DEBUG( "PM: vote: distance traveled" );
return true;
}
// angle turned
double angle = q2v(Quaterniond(getMotion().delta_integr_.data()+3)).norm(); // 2.0 * acos(getMotion().delta_integr_(6));
- if (angle > params_odom_3D_->angle_turned)
+ if (angle > params_odom_3d_->angle_turned)
{
WOLF_DEBUG( "PM: vote: angle turned" );
return true;
@@ -208,7 +208,7 @@ bool ProcessorOdom3D::voteForKeyFrame() const
return false;
}
-CaptureMotionPtr ProcessorOdom3D::emplaceCapture(const FrameBasePtr& _frame_own,
+CaptureMotionPtr ProcessorOdom3d::emplaceCapture(const FrameBasePtr& _frame_own,
const SensorBasePtr& _sensor,
const TimeStamp& _ts,
const VectorXd& _data,
@@ -217,25 +217,25 @@ CaptureMotionPtr ProcessorOdom3D::emplaceCapture(const FrameBasePtr& _frame_own,
const VectorXd& _calib_preint,
const CaptureBasePtr& _capture_origin)
{
- auto cap_motion = CaptureBase::emplace<CaptureOdom3D>(_frame_own, _ts, _sensor, _data, _data_cov, _capture_origin);
+ auto cap_motion = CaptureBase::emplace<CaptureOdom3d>(_frame_own, _ts, _sensor, _data, _data_cov, _capture_origin);
cap_motion->setCalibration(_calib);
cap_motion->setCalibrationPreint(_calib_preint);
return cap_motion;
}
-FeatureBasePtr ProcessorOdom3D::emplaceFeature(CaptureMotionPtr _capture_motion)
+FeatureBasePtr ProcessorOdom3d::emplaceFeature(CaptureMotionPtr _capture_motion)
{
FeatureBasePtr key_feature_ptr = FeatureBase::emplace<FeatureBase>(_capture_motion,
- "ProcessorOdom3D",
+ "ProcessorOdom3d",
_capture_motion->getBuffer().get().back().delta_integr_,
_capture_motion->getBuffer().get().back().delta_integr_cov_);
return key_feature_ptr;
}
-FactorBasePtr ProcessorOdom3D::emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin)
+FactorBasePtr ProcessorOdom3d::emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin)
{
- auto fac_odom = FactorBase::emplace<FactorOdom3D>(_feature_motion,
+ auto fac_odom = FactorBase::emplace<FactorOdom3d>(_feature_motion,
_feature_motion,
_capture_origin->getFrame(),
shared_from_this(),
@@ -249,6 +249,6 @@ FactorBasePtr ProcessorOdom3D::emplaceFactor(FeatureBasePtr _feature_motion, Cap
// Register in the SensorFactory
#include "core/processor/processor_factory.h"
namespace wolf {
-WOLF_REGISTER_PROCESSOR("ProcessorOdom3D", ProcessorOdom3D);
-WOLF_REGISTER_PROCESSOR_AUTO("ProcessorOdom3D", ProcessorOdom3D);
+WOLF_REGISTER_PROCESSOR("ProcessorOdom3d", ProcessorOdom3d);
+WOLF_REGISTER_PROCESSOR_AUTO("ProcessorOdom3d", ProcessorOdom3d);
} // namespace wolf
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index 227f819941fb886e657cb03c0f0cf1422218738b..56d5ae4ae45ed40962274292328076087da7fb9f 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -15,8 +15,9 @@ namespace wolf
{
ProcessorTracker::ProcessorTracker(const std::string& _type,
+ int _dim,
ProcessorParamsTrackerPtr _params_tracker) :
- ProcessorBase(_type, _params_tracker),
+ ProcessorBase(_type, _dim, _params_tracker),
params_tracker_(_params_tracker),
processing_step_(FIRST_TIME_WITHOUT_PACK),
origin_ptr_(nullptr),
diff --git a/src/processor/processor_tracker_feature.cpp b/src/processor/processor_tracker_feature.cpp
index d6ce9c065e8c0611f6e1190acf1fe4555d83cdf6..211a1aba12b12ecfb10afea00041cf82e07493be 100644
--- a/src/processor/processor_tracker_feature.cpp
+++ b/src/processor/processor_tracker_feature.cpp
@@ -11,8 +11,9 @@ namespace wolf
{
ProcessorTrackerFeature::ProcessorTrackerFeature(const std::string& _type,
+ int _dim,
ProcessorParamsTrackerFeaturePtr _params_tracker_feature) :
- ProcessorTracker(_type, _params_tracker_feature),
+ ProcessorTracker(_type, _dim, _params_tracker_feature),
params_tracker_feature_(_params_tracker_feature)
{
}
diff --git a/src/processor/processor_tracker_landmark.cpp b/src/processor/processor_tracker_landmark.cpp
index c0114ace1a52790c72c084f3133ced6c3f94acb8..7035dd38de2fcfb177deef2f4ed30d28fcdd92cf 100644
--- a/src/processor/processor_tracker_landmark.cpp
+++ b/src/processor/processor_tracker_landmark.cpp
@@ -15,7 +15,7 @@ namespace wolf
ProcessorTrackerLandmark::ProcessorTrackerLandmark(const std::string& _type,
ProcessorParamsTrackerLandmarkPtr _params_tracker_landmark) :
- ProcessorTracker(_type, _params_tracker_landmark),
+ ProcessorTracker(_type, 0, _params_tracker_landmark),
params_tracker_landmark_(_params_tracker_landmark)
{
//
diff --git a/src/sensor/sensor_odom_2D.cpp b/src/sensor/sensor_odom_2D.cpp
deleted file mode 100644
index 626b96a86c3957cc8f9dcacf536152d1f28376dd..0000000000000000000000000000000000000000
--- a/src/sensor/sensor_odom_2D.cpp
+++ /dev/null
@@ -1,44 +0,0 @@
-#include "core/sensor/sensor_odom_2D.h"
-#include "core/state_block/state_block.h"
-#include "core/state_block/state_angle.h"
-
-namespace wolf {
-
-SensorOdom2D::SensorOdom2D(Eigen::VectorXd _extrinsics, const ParamsSensorOdom2D& _intrinsics) :
- SensorBase("SensorOdom2D", std::make_shared<StateBlock>(_extrinsics.head(2), true), std::make_shared<StateAngle>(_extrinsics(2), true), nullptr, 2),
- k_disp_to_disp_(_intrinsics.k_disp_to_disp),
- k_rot_to_rot_(_intrinsics.k_rot_to_rot)
-{
- assert(_extrinsics.size() == 3 && "Wrong extrinsics vector size! Should be 3 for 2D.");
- //
-}
-
-SensorOdom2D::SensorOdom2D(Eigen::VectorXd _extrinsics, ParamsSensorOdom2DPtr _intrinsics) :
- SensorOdom2D(_extrinsics, *_intrinsics)
-{
- //
-}
-
-SensorOdom2D::~SensorOdom2D()
-{
- //
-}
-
-double SensorOdom2D::getDispVarToDispNoiseFactor() const
-{
- return k_disp_to_disp_;
-}
-
-double SensorOdom2D::getRotVarToRotNoiseFactor() const
-{
- return k_rot_to_rot_;
-}
-
-}
-
-// Register in the SensorFactory
-#include "core/sensor/sensor_factory.h"
-namespace wolf {
-WOLF_REGISTER_SENSOR("SensorOdom2D", SensorOdom2D);
-WOLF_REGISTER_SENSOR_AUTO("SensorOdom2D", SensorOdom2D);
-} // namespace wolf
diff --git a/src/sensor/sensor_odom_2d.cpp b/src/sensor/sensor_odom_2d.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..57af930f6724980d49209aaf1ece6b6762556adc
--- /dev/null
+++ b/src/sensor/sensor_odom_2d.cpp
@@ -0,0 +1,44 @@
+#include "core/sensor/sensor_odom_2d.h"
+#include "core/state_block/state_block.h"
+#include "core/state_block/state_angle.h"
+
+namespace wolf {
+
+SensorOdom2d::SensorOdom2d(Eigen::VectorXd _extrinsics, const ParamsSensorOdom2d& _intrinsics) :
+ SensorBase("SensorOdom2d", std::make_shared<StateBlock>(_extrinsics.head(2), true), std::make_shared<StateAngle>(_extrinsics(2), true), nullptr, 2),
+ k_disp_to_disp_(_intrinsics.k_disp_to_disp),
+ k_rot_to_rot_(_intrinsics.k_rot_to_rot)
+{
+ assert(_extrinsics.size() == 3 && "Wrong extrinsics vector size! Should be 3 for 2d.");
+ //
+}
+
+SensorOdom2d::SensorOdom2d(Eigen::VectorXd _extrinsics, ParamsSensorOdom2dPtr _intrinsics) :
+ SensorOdom2d(_extrinsics, *_intrinsics)
+{
+ //
+}
+
+SensorOdom2d::~SensorOdom2d()
+{
+ //
+}
+
+double SensorOdom2d::getDispVarToDispNoiseFactor() const
+{
+ return k_disp_to_disp_;
+}
+
+double SensorOdom2d::getRotVarToRotNoiseFactor() const
+{
+ return k_rot_to_rot_;
+}
+
+}
+
+// Register in the SensorFactory
+#include "core/sensor/sensor_factory.h"
+namespace wolf {
+WOLF_REGISTER_SENSOR("SensorOdom2d", SensorOdom2d);
+WOLF_REGISTER_SENSOR_AUTO("SensorOdom2d", SensorOdom2d);
+} // namespace wolf
diff --git a/src/sensor/sensor_odom_3D.cpp b/src/sensor/sensor_odom_3d.cpp
similarity index 64%
rename from src/sensor/sensor_odom_3D.cpp
rename to src/sensor/sensor_odom_3d.cpp
index 181e9326631c21409b35dd0232773576aca5c72c..a79995b85e11a3c248be1f19d4c69514cd6483d8 100644
--- a/src/sensor/sensor_odom_3D.cpp
+++ b/src/sensor/sensor_odom_3d.cpp
@@ -1,38 +1,38 @@
/**
- * \file sensor_odom_3D.cpp
+ * \file sensor_odom_3d.cpp
*
* Created on: Oct 7, 2016
* \author: jsola
*/
-#include "core/sensor/sensor_odom_3D.h"
+#include "core/sensor/sensor_odom_3d.h"
#include "core/state_block/state_block.h"
#include "core/state_block/state_quaternion.h"
namespace wolf {
-SensorOdom3D::SensorOdom3D(const Eigen::VectorXd& _extrinsics_pq, const ParamsSensorOdom3D& _intrinsics) :
- SensorBase("SensorOdom3D", std::make_shared<StateBlock>(_extrinsics_pq.head(3), true), std::make_shared<StateQuaternion>(_extrinsics_pq.tail(4), true), nullptr, 6),
+SensorOdom3d::SensorOdom3d(const Eigen::VectorXd& _extrinsics_pq, const ParamsSensorOdom3d& _intrinsics) :
+ SensorBase("SensorOdom3d", std::make_shared<StateBlock>(_extrinsics_pq.head(3), true), std::make_shared<StateQuaternion>(_extrinsics_pq.tail(4), true), nullptr, 6),
k_disp_to_disp_(_intrinsics.k_disp_to_disp),
k_disp_to_rot_(_intrinsics.k_disp_to_rot),
k_rot_to_rot_(_intrinsics.k_rot_to_rot),
min_disp_var_(_intrinsics.min_disp_var),
min_rot_var_(_intrinsics.min_rot_var)
{
- assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector size! Should be 7 for 3D.");
+ assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector size! Should be 7 for 3d.");
noise_cov_ = (Eigen::Vector6d() << min_disp_var_, min_disp_var_, min_disp_var_, min_rot_var_, min_rot_var_, min_rot_var_).finished().asDiagonal();
setNoiseCov(noise_cov_); // sets also noise_std_
}
-SensorOdom3D::SensorOdom3D(const Eigen::VectorXd& _extrinsics_pq, ParamsSensorOdom3DPtr _intrinsics) :
- SensorOdom3D(_extrinsics_pq, *_intrinsics)
+SensorOdom3d::SensorOdom3d(const Eigen::VectorXd& _extrinsics_pq, ParamsSensorOdom3dPtr _intrinsics) :
+ SensorOdom3d(_extrinsics_pq, *_intrinsics)
{
//
}
-SensorOdom3D::~SensorOdom3D()
+SensorOdom3d::~SensorOdom3d()
{
//
}
@@ -42,6 +42,6 @@ SensorOdom3D::~SensorOdom3D()
// Register in the SensorFactory
#include "core/sensor/sensor_factory.h"
namespace wolf {
-WOLF_REGISTER_SENSOR("SensorOdom3D", SensorOdom3D);
-WOLF_REGISTER_SENSOR_AUTO("SensorOdom3D", SensorOdom3D);
+WOLF_REGISTER_SENSOR("SensorOdom3d", SensorOdom3d);
+WOLF_REGISTER_SENSOR_AUTO("SensorOdom3d", SensorOdom3d);
}
diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp
index 0d47cd191f51c51126d38a591fea7de3614f53ae..6532dfc6a2b95d43451e00f8fad4797e7c53edf9 100644
--- a/src/solver/solver_manager.cpp
+++ b/src/solver/solver_manager.cpp
@@ -50,7 +50,7 @@ void SolverManager::update()
}
else
{
- WOLF_DEBUG("Tried to add an already added !");
+ WOLF_DEBUG("Tried to add a StateBlock that was already added !");
}
}
else
diff --git a/src/solver_suitesparse/solver_manager.cpp b/src/solver_suitesparse/solver_manager.cpp
index 805307fcd4ba78aee0b7d73ceba8a700c67a59cf..908a2f4aabcddc90248ab99261c7d5824c54c826 100644
--- a/src/solver_suitesparse/solver_manager.cpp
+++ b/src/solver_suitesparse/solver_manager.cpp
@@ -94,10 +94,10 @@ void SolverManager::addStateUnit(StateBlockPtr _st_ptr)
ceres_problem_->AddParameterBlock(_st_ptr->get(), ((StateBlockPtr)_st_ptr)->BLOCK_SIZE, nullptr);
break;
}
- case ST_POINT_1D:
+ case ST_POINT_1d:
{
//std::cout << "No Local Parametrization to be added" << std::endl;
- ceres_problem_->AddParameterBlock(_st_ptr->get(), ((StatePoint1D*)_st_ptr)->BLOCK_SIZE, nullptr);
+ ceres_problem_->AddParameterBlock(_st_ptr->get(), ((StatePoint1d*)_st_ptr)->BLOCK_SIZE, nullptr);
break;
}
case ST_VECTOR:
@@ -106,7 +106,7 @@ void SolverManager::addStateUnit(StateBlockPtr _st_ptr)
ceres_problem_->AddParameterBlock(_st_ptr->get(), ((StateBlockPtr)_st_ptr)->BLOCK_SIZE, nullptr);
break;
}
- case ST_POINT_3D:
+ case ST_POINT_3d:
{
//std::cout << "No Local Parametrization to be added" << std::endl;
ceres_problem_->AddParameterBlock(_st_ptr->get(), ((StateBlockPtr)_st_ptr)->BLOCK_SIZE, nullptr);
@@ -152,10 +152,10 @@ ceres::CostFunction* SolverManager::createCostFunction(FactorBasePtr _corrPtr)
switch (_corrPtr->getFactorType())
{
- case FAC_GPS_FIX_2D:
+ case FAC_GPS_FIX_2d:
{
- FactorGPS2D* specific_ptr = (FactorGPS2D*)(_corrPtr);
- return new ceres::AutoDiffCostFunction<FactorGPS2D,
+ FactorGPS2d* specific_ptr = (FactorGPS2d*)(_corrPtr);
+ return new ceres::AutoDiffCostFunction<FactorGPS2d,
specific_ptr->residualSize,
specific_ptr->block0Size,
specific_ptr->block1Size,
@@ -169,10 +169,10 @@ ceres::CostFunction* SolverManager::createCostFunction(FactorBasePtr _corrPtr)
specific_ptr->block9Size>(specific_ptr);
break;
}
- case FAC_ODOM_2D_COMPLEX_ANGLE:
+ case FAC_ODOM_2d_COMPLEX_ANGLE:
{
- FactorOdom2DComplexAngle* specific_ptr = (FactorOdom2DComplexAngle*)(_corrPtr);
- return new ceres::AutoDiffCostFunction<FactorOdom2DComplexAngle,
+ FactorOdom2dComplexAngle* specific_ptr = (FactorOdom2dComplexAngle*)(_corrPtr);
+ return new ceres::AutoDiffCostFunction<FactorOdom2dComplexAngle,
specific_ptr->residualSize,
specific_ptr->block0Size,
specific_ptr->block1Size,
@@ -186,10 +186,10 @@ ceres::CostFunction* SolverManager::createCostFunction(FactorBasePtr _corrPtr)
specific_ptr->block9Size>(specific_ptr);
break;
}
- case FAC_ODOM_2D:
+ case FAC_ODOM_2d:
{
- FactorOdom2D* specific_ptr = (FactorOdom2D*)(_corrPtr);
- return new ceres::AutoDiffCostFunction<FactorOdom2D,
+ FactorOdom2d* specific_ptr = (FactorOdom2d*)(_corrPtr);
+ return new ceres::AutoDiffCostFunction<FactorOdom2d,
specific_ptr->residualSize,
specific_ptr->block0Size,
specific_ptr->block1Size,
@@ -203,10 +203,10 @@ ceres::CostFunction* SolverManager::createCostFunction(FactorBasePtr _corrPtr)
specific_ptr->block9Size>(specific_ptr);
break;
}
- case FAC_CORNER_2D:
+ case FAC_CORNER_2d:
{
- FactorCorner2D* specific_ptr = (FactorCorner2D*)(_corrPtr);
- return new ceres::AutoDiffCostFunction<FactorCorner2D,
+ FactorCorner2d* specific_ptr = (FactorCorner2d*)(_corrPtr);
+ return new ceres::AutoDiffCostFunction<FactorCorner2d,
specific_ptr->residualSize,
specific_ptr->block0Size,
specific_ptr->block1Size,
diff --git a/src/state_block/has_state_blocks.cpp b/src/state_block/has_state_blocks.cpp
index b90e9188c1e1cefbec01f02ca33c99da6511bf89..83cf44b26bdfcad7e415633ef6d8fe783564a75c 100644
--- a/src/state_block/has_state_blocks.cpp
+++ b/src/state_block/has_state_blocks.cpp
@@ -4,6 +4,20 @@
namespace wolf
{
+StateBlockPtr HasStateBlocks::addStateBlock(const std::string& _sb_type, const StateBlockPtr& _sb, ProblemPtr _problem)
+{
+ assert(state_block_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
+ state_block_map_.emplace(_sb_type, _sb);
+ if (!isInStructure(_sb_type))
+ appendToStructure(_sb_type);
+
+ // conditionally register to problem
+ if(_problem)
+ _problem->notifyStateBlock(_sb, ADD);
+
+ return _sb;
+}
+
void HasStateBlocks::registerNewStateBlocks(ProblemPtr _problem) const
{
if (_problem != nullptr)
@@ -30,5 +44,14 @@ void HasStateBlocks::removeStateBlocks(ProblemPtr _problem)
}
}
+void HasStateBlocks::perturb(double amplitude)
+{
+ for (const auto& pair_key_sb : state_block_map_)
+ {
+ auto& sb = pair_key_sb.second;
+ if (!sb->isFixed())
+ sb->perturb(amplitude);
+ }
+}
}
diff --git a/src/state_block/state_block.cpp b/src/state_block/state_block.cpp
index f582ae97787b4a3df8d1e98177385427580a90c0..783a56a5e8cb52fe0ae79e2ad14ca4b50976a48c 100644
--- a/src/state_block/state_block.cpp
+++ b/src/state_block/state_block.cpp
@@ -36,4 +36,22 @@ void StateBlock::setFixed(bool _fixed)
// _problem_ptr->removeStateBlock(shared_from_this());
//}
+void StateBlock::perturb(double amplitude)
+{
+ using namespace Eigen;
+ VectorXd perturbation(VectorXd::Random(getLocalSize()) * amplitude);
+ if (local_param_ptr_ == nullptr)
+ state_ += perturbation;
+ else
+ {
+ VectorXd state_perturbed(getSize());
+ // Note: LocalParametrizationBase::plus() works with Eigen::Map only. Build all necessary maps:
+ Map<const VectorXd> state_map(state_.data(), getSize());
+ Map<const VectorXd> perturbation_map(perturbation.data(), getLocalSize());
+ Map<VectorXd> state_perturbed_map(state_perturbed.data(), getSize());
+ local_param_ptr_->plus(state_map, perturbation_map, state_perturbed_map);
+ state_ = state_perturbed;
+ }
+}
+
}
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..710cd3566cd477e2dd8f9c02c0a71070c20e07d5
--- /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_INFO("TreeManagerSlidingWindow removing first frame");
+ first_KF->remove(params_sw_->viral_remove_empty_parent);
+ if (params_sw_->fix_first_key_frame)
+ {
+ WOLF_INFO("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/src/utils/check_log.cpp b/src/utils/check_log.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..06e58733aa3d99b9cffa312a9a88bdb223d15eb5
--- /dev/null
+++ b/src/utils/check_log.cpp
@@ -0,0 +1,29 @@
+#include "core/utils/check_log.h"
+
+using namespace wolf;
+
+CheckLog::CheckLog()
+{
+ is_consistent_ = true;
+ explanation_ = "";
+}
+CheckLog::CheckLog(bool _consistent, std::string _explanation)
+{
+ is_consistent_ = _consistent;
+ if (not _consistent)
+ explanation_ = _explanation;
+ else
+ explanation_ = "";
+}
+void CheckLog::compose(CheckLog l)
+{
+ is_consistent_ = is_consistent_ and l.is_consistent_;
+ explanation_ = explanation_ + l.explanation_;
+}
+void CheckLog::assertTrue(bool _condition, std::stringstream& _stream)
+{
+ auto cl = CheckLog(_condition, _stream.str());
+ this->compose(cl);
+ // Clear inconsistency_explanation
+ std::stringstream().swap(_stream);
+}
diff --git a/src/utils/loader.cpp b/src/utils/loader.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9fdf6984b6a3de57fa8a8ae062232a685897209f
--- /dev/null
+++ b/src/utils/loader.cpp
@@ -0,0 +1,37 @@
+#include "core/utils/loader.h"
+
+#include <dlfcn.h>
+#include <stdexcept>
+
+Loader::Loader(std::string _file)
+{
+ path_ = _file;
+}
+LoaderRaw::LoaderRaw(std::string _file) : Loader(_file)
+{
+ //
+}
+LoaderRaw::~LoaderRaw()
+{
+ close();
+}
+void LoaderRaw::load()
+{
+ resource_ = dlopen(path_.c_str(), RTLD_LAZY);
+ if (not resource_)
+ throw std::runtime_error("Couldn't load resource with path " + path_ + "\n" + "Error info: " + dlerror());
+}
+void LoaderRaw::close()
+{
+ if (resource_)
+ dlclose(resource_);
+}
+// class LoaderPlugin: public Loader{
+// ClassLoader* resource_;
+// void load(){
+// resource_ = new ClassLoader(path_);
+// }
+// void close(){
+// delete resource_;
+// }
+// };
diff --git a/src/utils/params_server.cpp b/src/utils/params_server.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6344466792706c9e499695721f95e781089ec07e
--- /dev/null
+++ b/src/utils/params_server.cpp
@@ -0,0 +1,28 @@
+#include "core/utils/params_server.h"
+
+using namespace wolf;
+
+ParamsServer::ParamsServer()
+{
+ params_ = std::map<std::string, std::string>();
+}
+ParamsServer::ParamsServer(std::map<std::string, std::string> _params)
+{
+ params_ = _params;
+}
+
+void ParamsServer::print()
+{
+ for (auto it : params_)
+ std::cout << it.first << "~~" << it.second << std::endl;
+}
+
+void ParamsServer::addParam(std::string _key, std::string _value)
+{
+ params_.insert(std::pair<std::string, std::string>(_key, _value));
+}
+
+void ParamsServer::addParams(std::map<std::string, std::string> _params)
+{
+ params_.insert(_params.begin(), _params.end());
+}
diff --git a/src/yaml/parser_yaml.cpp b/src/yaml/parser_yaml.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..267d0c9da2024abd59ed5b529d13de7680d14472
--- /dev/null
+++ b/src/yaml/parser_yaml.cpp
@@ -0,0 +1,547 @@
+#include "core/yaml/parser_yaml.h"
+
+#include <string>
+#include <vector>
+#include <list>
+#include <stack>
+#include <regex>
+#include <map>
+#include <iostream>
+#include <algorithm>
+#include <numeric>
+
+using namespace wolf;
+namespace {
+ //====== START OF FORWARD DECLARATION ========
+ std::string parseAtomicNode(YAML::Node);
+ std::string fetchMapEntry(YAML::Node);
+ std::string mapToString(std::map<std::string,std::string>);
+ //====== END OF FORWARD DECLARATION ========
+
+/** @Brief Interprets a map as being atomic and thus parses it as a single entity. We assume that the map has as values only scalars and sequences.
+ * @param n the node representing a map
+ * @return std::map<std::string, std::string> populated with the key,value pairs in n
+ */
+std::map<std::string, std::string> fetchAsMap(YAML::Node _n){
+ assert(_n.Type() == YAML::NodeType::Map && "trying to fetch as Map a non-Map node");
+ auto m = std::map<std::string, std::string>();
+ for(const auto& kv : _n){
+ std::string key = kv.first.as<std::string>();
+ switch (kv.second.Type()) {
+ case YAML::NodeType::Scalar : {
+ std::string value = kv.second.Scalar();
+ m.insert(std::pair<std::string,std::string>(key, value));
+ break;
+ }
+ case YAML::NodeType::Sequence : {
+ std::string aux = parseAtomicNode(kv.second);
+ m.insert(std::pair<std::string,std::string>(key, aux));
+ break;
+ }
+ case YAML::NodeType::Map : {
+ std::string value = fetchMapEntry(kv.second);
+ std::regex r("^\\$.*");
+ if (std::regex_match(key, r)) key = key.substr(1,key.size()-1);
+ m.insert(std::pair<std::string,std::string>(key, value));
+ break;
+ }
+ default:
+ assert(1 == 0 && "Unsupported node Type at fetchAsMap");
+ break;
+ }
+ }
+ return m;
+}
+ std::string fetchMapEntry(YAML::Node _n){
+ switch (_n.Type()) {
+ case YAML::NodeType::Scalar: {
+ return _n.Scalar();
+ break;
+ }
+ case YAML::NodeType::Sequence: {
+ return parseAtomicNode(_n);
+ break;
+ }
+ case YAML::NodeType::Map: {
+ return mapToString(fetchAsMap(_n));
+ break;
+ }
+ default: {
+ assert(1 == 0 && "Unsupported node Type at fetchMapEntry");
+ return "";
+ break;
+ }
+ }
+ }
+ /** @Brief Transforms a std::map<std::string,std::string> to its std::string representation [{k1:v1},{k2:v2},{k3:v3},...]
+ * @param map_ just a std::map<std::string,std::string>
+ * @return <b>{std::string}</b> [{k1:v1},{k2:v2},{k3:v3},...]
+ */
+ std::string mapToString(std::map<std::string,std::string> _map){
+ std::string result = "";
+ auto v = std::vector<std::string>();
+ std::transform(_map.begin(), _map.end(), back_inserter(v), [](const std::pair<std::string,std::string> p){return "{" + p.first + ":" + p.second + "}";});
+ auto concat = [](std::string ac, std::string str)-> std::string {
+ return ac + str + ",";
+ };
+ std::string aux = "";
+ std::string accumulated = std::accumulate(v.begin(), v.end(), aux, concat);
+ if(accumulated.size() > 1) accumulated = accumulated.substr(0,accumulated.size() - 1);
+ else accumulated = "";
+ return "[" + accumulated + "]";
+ }
+ /** @Brief Generates a std::string representing a YAML sequence. The sequence is assumed to be scalar or at most be a sequence of sequences of scalars.
+ * @param n a vector of YAML::Node that represents a YAML::Sequence
+ * @return <b>{std::string}</b> representing the YAML sequence
+ */
+ std::string parseAtomicNode(YAML::Node _n){
+ std::string aux = "";
+ bool first = true;
+ std::string separator = "";
+ switch(_n.Type()){
+ case YAML::NodeType::Scalar:
+ return _n.Scalar();
+ break;
+ case YAML::NodeType::Sequence:
+ for(auto it : _n){
+ aux += separator + parseAtomicNode(it);
+ if(first){
+ separator = ",";
+ first = false;
+ }
+ }
+ return "[" + aux + "]";
+ break;
+ case YAML::NodeType::Map:
+ return mapToString(fetchAsMap(_n));
+ break;
+ default:
+ return "";
+ break;
+ }
+ }
+
+ /** @Brief checks if a node of the YAML tree is atomic. Only works if the nodes are of type
+ * Scalar, Sequence or Map.
+ * @param key is the key associated to the node n if n.Type() == YAML::NodeType::Map
+ * @param n node to be test for atomicity
+ */
+ bool isAtomic(std::string _key, YAML::Node _n){
+ assert(_n.Type() != YAML::NodeType::Undefined && _n.Type() != YAML::NodeType::Null && "Cannot determine atomicity of Undefined/Null node");
+ std::regex r("^\\$.*");
+ bool is_atomic = true;
+ switch(_n.Type()){
+ case YAML::NodeType::Scalar:
+ return true;
+ break;
+ case YAML::NodeType::Sequence:
+ for(auto it : _n) {
+ switch(it.Type()){
+ case YAML::NodeType::Map:
+ for(const auto& kv : it){
+ is_atomic = is_atomic and isAtomic(kv.first.as<std::string>(), it);
+ }
+ break;
+ default:
+ is_atomic = is_atomic and isAtomic("", it);
+ break;
+ }
+ }
+ return is_atomic;
+ break;
+ case YAML::NodeType::Map:
+ is_atomic = std::regex_match(_key, r);
+ return is_atomic;
+ break;
+ default:
+ throw std::runtime_error("Cannot determine atomicity of node type " + std::to_string(_n.Type()));
+ return false;
+ break;
+ }
+ return false;
+ }
+}
+ParserYAML::ParserYAML(std::string _file, std::string path_root, bool freely_parse)
+{
+ params_ = std::map<std::string, std::string>();
+ active_name_ = "";
+ paramsSens_ = std::vector<ParamsInitSensor>();
+ paramsProc_ = std::vector<ParamsInitProcessor>();
+ subscriber_managers_ = std::vector<SubscriberManager>();
+ publisher_managers_ = std::vector<PublisherManager>();
+ parsing_file_ = std::stack<std::string>();
+ file_ = _file;
+ if (path_root != "")
+ {
+ std::regex r(".*/ *$");
+ if (not std::regex_match(path_root, r))
+ path_root_ = path_root + "/";
+ else
+ path_root_ = path_root;
+ }
+ if (not freely_parse)
+ parse();
+ else
+ parse_freely();
+}
+
+std::string ParserYAML::generatePath(std::string _file)
+{
+ std::regex r("^/.*");
+ if (std::regex_match(_file, r))
+ {
+ return _file;
+ }
+ else
+ {
+ return path_root_ + _file;
+ }
+}
+YAML::Node ParserYAML::loadYAML(std::string _file)
+{
+ try
+ {
+ // std::cout << "Parsing " << generatePath(_file) << std::endl;
+ WOLF_INFO("Parsing file: ", generatePath(_file));
+ return YAML::LoadFile(generatePath(_file));
+ }
+ catch (YAML::BadFile& e)
+ {
+ throw std::runtime_error("Couldn't load file " + generatePath(_file) + ". Tried to open it from " +
+ parsing_file_.top());
+ }
+}
+std::string ParserYAML::tagsToString(std::vector<std::string>& _tags)
+{
+ std::string hdr = "";
+ for (auto it : _tags)
+ {
+ hdr = hdr + "/" + it;
+ }
+ return hdr;
+}
+void ParserYAML::walkTree(std::string _file)
+{
+ YAML::Node n;
+ n = loadYAML(generatePath(_file));
+ parsing_file_.push(generatePath(_file));
+ std::vector<std::string> hdrs = std::vector<std::string>();
+ walkTreeR(n, hdrs, "");
+ parsing_file_.pop();
+}
+void ParserYAML::walkTree(std::string _file, std::vector<std::string>& _tags)
+{
+ YAML::Node n;
+ n = loadYAML(generatePath(_file));
+ parsing_file_.push(generatePath(_file));
+ walkTreeR(n, _tags, "");
+ parsing_file_.pop();
+}
+void ParserYAML::walkTree(std::string _file, std::vector<std::string>& _tags, std::string hdr)
+{
+ YAML::Node n;
+ n = loadYAML(generatePath(_file));
+ parsing_file_.push(generatePath(_file));
+ walkTreeR(n, _tags, hdr);
+ parsing_file_.pop();
+}
+void ParserYAML::walkTreeR(YAML::Node _n, std::vector<std::string>& _tags, std::string hdr)
+{
+ switch (_n.Type())
+ {
+ case YAML::NodeType::Scalar:
+ {
+ std::regex r("^@.*");
+ if (std::regex_match(_n.Scalar(), r))
+ {
+ std::string str = _n.Scalar();
+ walkTree(str.substr(1, str.size() - 1), _tags, hdr);
+ }
+ else
+ {
+ insert_register(hdr, _n.Scalar());
+ }
+ break;
+ }
+ case YAML::NodeType::Sequence:
+ {
+ if (isAtomic("", _n))
+ {
+ insert_register(hdr, parseAtomicNode(_n));
+ }
+ else
+ {
+ for (const auto& kv : _n)
+ {
+ walkTreeR(kv, _tags, hdr);
+ }
+ }
+ break;
+ }
+ case YAML::NodeType::Map:
+ {
+ for (const auto& kv : _n)
+ {
+ if (isAtomic(kv.first.as<std::string>(), _n))
+ {
+ std::string key = kv.first.as<std::string>();
+ // WOLF_DEBUG("KEY IN MAP ATOMIC ", hdr + "/" + key);
+ key = key.substr(1, key.size() - 1);
+ insert_register(hdr + "/" + key, parseAtomicNode(kv.second));
+ }
+ else
+ {
+ /*
+ If key=="follow" then the parser will assume that the value is a path and will parse
+ the (expected) yaml file at the specified path. Note that this does not increase the header depth.
+ The following example shows how the header remains unafected:
+ @my_main_config | @some_path
+ - cov_det: 1 | - my_value : 23
+ - follow: "@some_path" |
+ - var: 1.2 |
+ Resulting map:
+ cov_det -> 1
+ my_value-> 23
+ var: 1.2
+ Instead of:
+ cov_det -> 1
+ follow/my_value-> 23
+ var: 1.2
+ Which would result from the following yaml files
+ @my_main_config | @some_path
+ - cov_det: 1 | - my_value : 23
+ - $follow: "@some_path" |
+ - var: 1.2 |
+ */
+ std::string key = kv.first.as<std::string>();
+ // WOLF_DEBUG("KEY IN MAP NON ATOMIC ", key);
+ std::regex rr("follow");
+ if (not std::regex_match(kv.first.as<std::string>(), rr))
+ {
+ _tags.push_back(kv.first.as<std::string>());
+ if (_tags.size() == 2)
+ updateActiveName(kv.first.as<std::string>());
+ walkTreeR(kv.second, _tags, hdr + "/" + kv.first.as<std::string>());
+ _tags.pop_back();
+ if (_tags.size() == 1)
+ updateActiveName("");
+ }
+ else
+ {
+ walkTree(kv.second.as<std::string>(), _tags, hdr);
+ }
+ }
+ }
+ break;
+ }
+ case YAML::NodeType::Null:
+ break;
+ default:
+ assert(1 == 0 && "Unsupported node Type at walkTreeR.");
+ break;
+ }
+}
+void ParserYAML::updateActiveName(std::string _tag)
+{
+ active_name_ = _tag;
+}
+void ParserYAML::parseFirstLevel(std::string _file)
+{
+ YAML::Node n;
+ n = loadYAML(generatePath(_file));
+
+ YAML::Node n_config = n["config"];
+ // assert(n_config.Type() == YAML::NodeType::Map && "trying to parse config node but found a non-Map node");
+ if (n_config.Type() != YAML::NodeType::Map)
+ throw std::runtime_error("Could not find config node. Please make sure that your YAML file " +
+ generatePath(_file) + " starts with 'config:'");
+ if (n_config["problem"].Type() != YAML::NodeType::Map)
+ throw std::runtime_error("Could not find problem node. Please make sure that the 'config' node in YAML file " +
+ generatePath(_file) + " has a 'problem' entry");
+ problem = n_config["problem"];
+ std::vector<std::map<std::string, std::string>> map_container;
+ try
+ {
+ for (const auto& kv : n_config["sensors"])
+ {
+ ParamsInitSensor pSensor = { kv["type"].Scalar(), kv["name"].Scalar(), kv["plugin"].Scalar(), kv };
+ paramsSens_.push_back(pSensor);
+ map_container.push_back(std::map<std::string, std::string>({ { "type", kv["type"].Scalar() },
+ { "name", kv["name"].Scalar() },
+ { "plugin", kv["plugin"].Scalar() } }));
+ }
+ insert_register("sensors", wolf::converter<std::string>::convert(map_container));
+ map_container.clear();
+ }
+ catch (YAML::InvalidNode& e)
+ {
+ throw std::runtime_error("Error parsing sensors @" + generatePath(_file) +
+ ". Please make sure that each sensor entry has 'type', 'name' and 'plugin' entries.");
+ }
+
+ try
+ {
+ for (const auto& kv : n_config["processors"])
+ {
+ ParamsInitProcessor pProc = {
+ kv["type"].Scalar(), kv["name"].Scalar(), kv["sensor_name"].Scalar(), kv["plugin"].Scalar(), kv
+ };
+ paramsProc_.push_back(pProc);
+ map_container.push_back(
+ std::map<std::string, std::string>({ { "type", kv["type"].Scalar() },
+ { "name", kv["name"].Scalar() },
+ { "plugin", kv["plugin"].Scalar() },
+ { "sensor_name", kv["sensor_name"].Scalar() } }));
+ }
+ insert_register("processors", wolf::converter<std::string>::convert(map_container));
+ map_container.clear();
+ }
+ catch (YAML::InvalidNode& e)
+ {
+ throw std::runtime_error("Error parsing processors @" + generatePath(_file) +
+ ". Please make sure that each processor has 'type', 'name', 'plugin' and "
+ "'sensor_name' entries.");
+ }
+ try
+ {
+ for (const auto& kv : n_config["ROS subscriber"])
+ {
+ SubscriberManager pSubscriberManager = {
+ kv["package"].Scalar(), kv["type"].Scalar(), kv["topic"].Scalar(), kv["sensor_name"].Scalar(), kv
+ };
+ subscriber_managers_.push_back(pSubscriberManager);
+ map_container.push_back(
+ std::map<std::string, std::string>({ { "package", kv["package"].Scalar() },
+ { "type", kv["type"].Scalar() },
+ { "topic", kv["topic"].Scalar() },
+ { "sensor_name", kv["sensor_name"].Scalar() } }));
+ }
+ insert_register("ROS subscriber", wolf::converter<std::string>::convert(map_container));
+ map_container.clear();
+ }
+ catch (YAML::InvalidNode& e)
+ {
+ throw std::runtime_error("Error parsing subscriber @" + generatePath(_file) +
+ ". Please make sure that each manager has 'package', 'type', 'topic' and "
+ "'sensor_name' entries.");
+ }
+
+ try
+ {
+ for (const auto& kv : n_config["ROS publisher"])
+ {
+ WOLF_DEBUG("WHAT");
+ PublisherManager pPublisherManager = {
+ kv["type"].Scalar(), kv["topic"].Scalar(), kv["period"].Scalar(), kv
+ };
+ publisher_managers_.push_back(pPublisherManager);
+ map_container.push_back(std::map<std::string, std::string>({ { "type", kv["type"].Scalar() },
+ { "topic", kv["topic"].Scalar() },
+ { "period", kv["period"].Scalar() } }));
+ }
+ insert_register("ROS publisher", wolf::converter<std::string>::convert(map_container));
+ map_container.clear();
+ }
+ catch (YAML::InvalidNode& e)
+ {
+ throw std::runtime_error("Error parsing publisher @" + generatePath(_file) +
+ ". Please make sure that each manager has 'type', 'topic' and 'period' entries.");
+ }
+}
+std::map<std::string, std::string> ParserYAML::getParams()
+{
+ std::map<std::string, std::string> rtn = params_;
+ return rtn;
+}
+void ParserYAML::parse()
+{
+ parsing_file_.push(generatePath(file_));
+ parseFirstLevel(file_);
+
+ if (problem.Type() != YAML::NodeType::Undefined)
+ {
+ std::vector<std::string> tags = std::vector<std::string>();
+ walkTreeR(problem, tags, "problem");
+ }
+ for (auto it : paramsSens_)
+ {
+ std::vector<std::string> tags = std::vector<std::string>();
+ tags.push_back("sensor");
+ walkTreeR(it.n_, tags, "sensor/" + it.name_);
+ }
+ for (auto it : paramsProc_)
+ {
+ std::vector<std::string> tags = std::vector<std::string>();
+ tags.push_back("processor");
+ walkTreeR(it.n_, tags, "processor/" + it.name_);
+ }
+ std::list<std::string> plugins, packages;
+ for (const auto& it : paramsSens_)
+ plugins.push_back(it.plugin_);
+ for (const auto& it : paramsProc_)
+ plugins.push_back(it.plugin_);
+ for (const auto& it : subscriber_managers_)
+ packages.push_back(it.package_);
+ plugins.sort();
+ plugins.unique();
+ packages.sort();
+ packages.unique();
+ insert_register("plugins", wolf::converter<std::string>::convert(plugins));
+ insert_register("packages", wolf::converter<std::string>::convert(packages));
+
+ YAML::Node n;
+ n = loadYAML(generatePath(file_));
+
+ if (n.Type() == YAML::NodeType::Map)
+ {
+ for (auto it : n)
+ {
+ auto node = it.second;
+ // WOLF_INFO("WUT ", it.first);
+ std::vector<std::string> tags = std::vector<std::string>();
+ if (it.first.as<std::string>() != "config")
+ walkTreeR(node, tags, it.first.as<std::string>());
+ else
+ {
+ for (auto itt : node)
+ {
+ std::string node_key = itt.first.as<std::string>();
+ if (node_key != "problem" and node_key != "sensors" and node_key != "processors" and
+ node_key != "ROS subscriber" and node_key != "ROS publisher")
+ {
+ walkTreeR(itt.second, tags, node_key);
+ }
+ }
+ }
+ }
+ }
+ else
+ {
+ std::vector<std::string> tags = std::vector<std::string>();
+ walkTreeR(n, tags, "");
+ }
+ parsing_file_.pop();
+}
+void ParserYAML::parse_freely()
+{
+ parsing_file_.push(generatePath(file_));
+ std::vector<std::string> tags = std::vector<std::string>();
+ walkTreeR(loadYAML(file_), tags, "");
+ parsing_file_.pop();
+}
+void ParserYAML::insert_register(std::string _key, std::string _value)
+{
+ if (_key.substr(0, 1) == "/")
+ _key = _key.substr(1, _key.size() - 1);
+ auto inserted_it = params_.insert(std::pair<std::string, std::string>(_key, _value));
+ if (not inserted_it.second)
+ WOLF_WARN("Skipping key '",
+ _key,
+ "' with value '",
+ _value,
+ "'. There already exists the register: (",
+ inserted_it.first->first,
+ ",",
+ inserted_it.first->second,
+ ")");
+}
diff --git a/src/yaml/processor_odom_3D_yaml.cpp b/src/yaml/processor_odom_3d_yaml.cpp
similarity index 75%
rename from src/yaml/processor_odom_3D_yaml.cpp
rename to src/yaml/processor_odom_3d_yaml.cpp
index ef18be90644eac4aeed4c085a83a203b95ab685e..1047ba5fd2482ab1f89c5a7256eb54b86f6181d6 100644
--- a/src/yaml/processor_odom_3D_yaml.cpp
+++ b/src/yaml/processor_odom_3d_yaml.cpp
@@ -1,5 +1,5 @@
/**
- * \file processor_odom_3D_yaml.cpp
+ * \file processor_odom_3d_yaml.cpp
*
* Created on: Oct 25, 2016
* \author: jsola
@@ -9,7 +9,7 @@
#include "core/yaml/yaml_conversion.h"
// wolf
-#include "core/processor/processor_odom_3D.h"
+#include "core/processor/processor_odom_3d.h"
#include "core/common/factory.h"
// yaml-cpp library
@@ -20,13 +20,13 @@ namespace wolf
namespace
{
-static ProcessorParamsBasePtr createProcessorOdom3DParams(const std::string & _filename_dot_yaml)
+static ProcessorParamsBasePtr createProcessorOdom3dParams(const std::string & _filename_dot_yaml)
{
YAML::Node config = YAML::LoadFile(_filename_dot_yaml);
- if (config["type"].as<std::string>() == "ProcessorOdom3D")
+ if (config["type"].as<std::string>() == "ProcessorOdom3d")
{
- ProcessorParamsOdom3DPtr params = std::make_shared<ProcessorParamsOdom3D>();
+ ProcessorParamsOdom3dPtr params = std::make_shared<ProcessorParamsOdom3d>();
params->time_tolerance = config["time_tolerance"] .as<double>();
params->unmeasured_perturbation_std = config["unmeasured_perturbation_std"] .as<double>();
@@ -48,7 +48,7 @@ static ProcessorParamsBasePtr createProcessorOdom3DParams(const std::string & _f
}
// Register in the SensorFactory
-const bool WOLF_UNUSED registered_prc_odom_3D = ProcessorParamsFactory::get().registerCreator("ProcessorOdom3D", createProcessorOdom3DParams);
+const bool WOLF_UNUSED registered_prc_odom_3d = ProcessorParamsFactory::get().registerCreator("ProcessorOdom3d", createProcessorOdom3dParams);
} // namespace [unnamed]
diff --git a/src/yaml/sensor_odom_2D_yaml.cpp b/src/yaml/sensor_odom_2d_yaml.cpp
similarity index 62%
rename from src/yaml/sensor_odom_2D_yaml.cpp
rename to src/yaml/sensor_odom_2d_yaml.cpp
index f157c221b223ecd813613b12c3f9460980af264a..d59cd4decfb77338a0b3bb4ba17c607e03f847c1 100644
--- a/src/yaml/sensor_odom_2D_yaml.cpp
+++ b/src/yaml/sensor_odom_2d_yaml.cpp
@@ -1,5 +1,5 @@
/**
- * \file sensor_odom_2D_yaml.cpp
+ * \file sensor_odom_2d_yaml.cpp
*
* Created on: Aug 3, 2019
* \author: jsola
@@ -11,7 +11,7 @@
#include "core/yaml/yaml_conversion.h"
// wolf
-#include "core/sensor/sensor_odom_2D.h"
+#include "core/sensor/sensor_odom_2d.h"
// yaml-cpp library
#include <yaml-cpp/yaml.h>
@@ -21,13 +21,13 @@ namespace wolf
namespace
{
-static ParamsSensorBasePtr createParamsSensorOdom2D(const std::string & _filename_dot_yaml)
+static ParamsSensorBasePtr createParamsSensorOdom2d(const std::string & _filename_dot_yaml)
{
YAML::Node config = YAML::LoadFile(_filename_dot_yaml);
- if (config["type"].as<std::string>() == "SensorOdom2D")
+ if (config["type"].as<std::string>() == "SensorOdom2d")
{
- auto params = std::make_shared<ParamsSensorOdom2D>();
+ auto params = std::make_shared<ParamsSensorOdom2d>();
params->k_disp_to_disp = config["k_disp_to_disp"] .as<double>();
params->k_rot_to_rot = config["k_rot_to_rot"] .as<double>();
@@ -40,7 +40,7 @@ static ParamsSensorBasePtr createParamsSensorOdom2D(const std::string & _filenam
}
// Register in the SensorFactory
-const bool WOLF_UNUSED registered_odom_2D_intr = ParamsSensorFactory::get().registerCreator("SensorOdom2D", createParamsSensorOdom2D);
+const bool WOLF_UNUSED registered_odom_2d_intr = ParamsSensorFactory::get().registerCreator("SensorOdom2d", createParamsSensorOdom2d);
} // namespace [unnamed]
diff --git a/src/yaml/sensor_odom_3D_yaml.cpp b/src/yaml/sensor_odom_3d_yaml.cpp
similarity index 69%
rename from src/yaml/sensor_odom_3D_yaml.cpp
rename to src/yaml/sensor_odom_3d_yaml.cpp
index 61abfae1056ff03eed1421ef3221062c2c875281..e437bedcbf00257068bfea07830caa9f8224e7c9 100644
--- a/src/yaml/sensor_odom_3D_yaml.cpp
+++ b/src/yaml/sensor_odom_3d_yaml.cpp
@@ -1,5 +1,5 @@
/**
- * \file sensor_odom_3D_yaml.cpp
+ * \file sensor_odom_3d_yaml.cpp
*
* Created on: Oct 25, 2016
* \author: jsola
@@ -9,7 +9,7 @@
#include "core/yaml/yaml_conversion.h"
// wolf
-#include "core/sensor/sensor_odom_3D.h"
+#include "core/sensor/sensor_odom_3d.h"
#include "core/common/factory.h"
// yaml-cpp library
@@ -20,13 +20,13 @@ namespace wolf
namespace
{
-static ParamsSensorBasePtr createParamsSensorOdom3D(const std::string & _filename_dot_yaml)
+static ParamsSensorBasePtr createParamsSensorOdom3d(const std::string & _filename_dot_yaml)
{
YAML::Node config = YAML::LoadFile(_filename_dot_yaml);
- if (config["type"].as<std::string>() == "SensorOdom3D")
+ if (config["type"].as<std::string>() == "SensorOdom3d")
{
- ParamsSensorOdom3DPtr params = std::make_shared<ParamsSensorOdom3D>();
+ ParamsSensorOdom3dPtr params = std::make_shared<ParamsSensorOdom3d>();
params->k_disp_to_disp = config["k_disp_to_disp"] .as<double>();
params->k_disp_to_rot = config["k_disp_to_rot"] .as<double>();
@@ -42,7 +42,7 @@ static ParamsSensorBasePtr createParamsSensorOdom3D(const std::string & _filenam
}
// Register in the SensorFactory
-const bool WOLF_UNUSED registered_odom_3D_intr = ParamsSensorFactory::get().registerCreator("SensorOdom3D", createParamsSensorOdom3D);
+const bool WOLF_UNUSED registered_odom_3d_intr = ParamsSensorFactory::get().registerCreator("SensorOdom3d", createParamsSensorOdom3d);
} // namespace [unnamed]
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 79fc2c60a55ef6b0c9df2700e47adafcf27c6d23..501965cdff67102f807e482f87ca7eafb7109e54 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -47,13 +47,9 @@ add_library(dummy ${SRC_DUMMY})
wolf_add_gtest(gtest_capture_base gtest_capture_base.cpp)
target_link_libraries(gtest_capture_base ${PROJECT_NAME})
-# CaptureBase class test
-#wolf_add_gtest(gtest_factor_sparse gtest_factor_sparse.cpp)
-#target_link_libraries(gtest_factor_sparse ${PROJECT_NAME})
-
-# FactorBlockDifference class test
-wolf_add_gtest(gtest_factor_block_difference gtest_factor_block_difference.cpp)
-target_link_libraries(gtest_factor_block_difference ${PROJECT_NAME})
+# FactorBase class test
+wolf_add_gtest(gtest_factor_base gtest_factor_base.cpp)
+target_link_libraries(gtest_factor_base ${PROJECT_NAME})
# FactorAutodiff class test
wolf_add_gtest(gtest_factor_autodiff gtest_factor_autodiff.cpp)
@@ -79,6 +75,10 @@ target_link_libraries(gtest_feature_base ${PROJECT_NAME})
wolf_add_gtest(gtest_frame_base gtest_frame_base.cpp)
target_link_libraries(gtest_frame_base ${PROJECT_NAME})
+# HasStateBlocks classes test
+wolf_add_gtest(gtest_has_state_blocks gtest_has_state_blocks.cpp)
+target_link_libraries(gtest_has_state_blocks ${PROJECT_NAME})
+
# LocalParametrizationXxx classes test
wolf_add_gtest(gtest_local_param gtest_local_param.cpp)
target_link_libraries(gtest_local_param ${PROJECT_NAME})
@@ -133,6 +133,10 @@ target_link_libraries(gtest_shared_from_this ${PROJECT_NAME})
wolf_add_gtest(gtest_solver_manager gtest_solver_manager.cpp)
target_link_libraries(gtest_solver_manager ${PROJECT_NAME})
+# StateBlock class test
+wolf_add_gtest(gtest_state_block gtest_state_block.cpp)
+target_link_libraries(gtest_state_block ${PROJECT_NAME})
+
# TimeStamp class test
wolf_add_gtest(gtest_time_stamp gtest_time_stamp.cpp)
target_link_libraries(gtest_time_stamp ${PROJECT_NAME})
@@ -145,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)
@@ -157,25 +165,29 @@ ENDIF(Ceres_FOUND)
wolf_add_gtest(gtest_factor_absolute gtest_factor_absolute.cpp)
target_link_libraries(gtest_factor_absolute ${PROJECT_NAME})
-# FactorAutodiffDistance3D test
-wolf_add_gtest(gtest_factor_autodiff_distance_3D gtest_factor_autodiff_distance_3D.cpp)
-target_link_libraries(gtest_factor_autodiff_distance_3D ${PROJECT_NAME})
+# FactorAutodiffDistance3d test
+wolf_add_gtest(gtest_factor_autodiff_distance_3d gtest_factor_autodiff_distance_3d.cpp)
+target_link_libraries(gtest_factor_autodiff_distance_3d ${PROJECT_NAME})
+
+# FactorBlockDifference class test
+wolf_add_gtest(gtest_factor_block_difference gtest_factor_block_difference.cpp)
+target_link_libraries(gtest_factor_block_difference ${PROJECT_NAME})
-# FactorOdom3D class test
+# FactorOdom3d class test
wolf_add_gtest(gtest_factor_diff_drive gtest_factor_diff_drive.cpp)
target_link_libraries(gtest_factor_diff_drive ${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})
+# FactorOdom3d class test
+wolf_add_gtest(gtest_factor_odom_3d gtest_factor_odom_3d.cpp)
+target_link_libraries(gtest_factor_odom_3d ${PROJECT_NAME})
-# FactorPose2D class test
-wolf_add_gtest(gtest_factor_pose_2D gtest_factor_pose_2D.cpp)
-target_link_libraries(gtest_factor_pose_2D ${PROJECT_NAME})
+# FactorPose2d class test
+wolf_add_gtest(gtest_factor_pose_2d gtest_factor_pose_2d.cpp)
+target_link_libraries(gtest_factor_pose_2d ${PROJECT_NAME})
-# FactorPose3D class test
-wolf_add_gtest(gtest_factor_pose_3D gtest_factor_pose_3D.cpp)
-target_link_libraries(gtest_factor_pose_3D ${PROJECT_NAME})
+# FactorPose3d class test
+wolf_add_gtest(gtest_factor_pose_3d gtest_factor_pose_3d.cpp)
+target_link_libraries(gtest_factor_pose_3d ${PROJECT_NAME})
# MakePosDef function test
wolf_add_gtest(gtest_make_posdef gtest_make_posdef.cpp)
@@ -198,16 +210,16 @@ wolf_add_gtest(gtest_processor_loopclosure gtest_processor_loopclosure.cpp)
target_link_libraries(gtest_processor_loopclosure ${PROJECT_NAME})
# ProcessorFrameNearestNeighborFilter class test
-# wolf_add_gtest(gtest_processor_frame_nearest_neighbor_filter_2D gtest_processor_frame_nearest_neighbor_filter_2D.cpp)
-# target_link_libraries(gtest_processor_frame_nearest_neighbor_filter_2D ${PROJECT_NAME})
+# wolf_add_gtest(gtest_processor_frame_nearest_neighbor_filter_2d gtest_processor_frame_nearest_neighbor_filter_2d.cpp)
+# target_link_libraries(gtest_processor_frame_nearest_neighbor_filter_2d ${PROJECT_NAME})
-# ProcessorMotion in 2D
-wolf_add_gtest(gtest_odom_2D gtest_odom_2D.cpp)
-target_link_libraries(gtest_odom_2D ${PROJECT_NAME})
+# ProcessorMotion in 2d
+wolf_add_gtest(gtest_odom_2d gtest_odom_2d.cpp)
+target_link_libraries(gtest_odom_2d ${PROJECT_NAME})
-# ProcessorOdom3D class test
-wolf_add_gtest(gtest_processor_odom_3D gtest_processor_odom_3D.cpp)
-target_link_libraries(gtest_processor_odom_3D ${PROJECT_NAME})
+# ProcessorOdom3d class test
+wolf_add_gtest(gtest_processor_odom_3d gtest_processor_odom_3d.cpp)
+target_link_libraries(gtest_processor_odom_3d ${PROJECT_NAME})
# ProcessorTrackerFeatureDummy class test
wolf_add_gtest(gtest_processor_tracker_feature_dummy gtest_processor_tracker_feature_dummy.cpp)
@@ -221,6 +233,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/processor_tracker_feature_dummy.h b/test/dummy/processor_tracker_feature_dummy.h
index 4f5205d152ad4b0916d5c4968e754eb8da84c023..3328edf08c0234d6b587680b9b215bc91a7ae43f 100644
--- a/test/dummy/processor_tracker_feature_dummy.h
+++ b/test/dummy/processor_tracker_feature_dummy.h
@@ -110,7 +110,7 @@ class ProcessorTrackerFeatureDummy : public ProcessorTrackerFeature
};
inline ProcessorTrackerFeatureDummy::ProcessorTrackerFeatureDummy(ProcessorParamsTrackerFeatureDummyPtr _params_tracker_feature_dummy) :
- ProcessorTrackerFeature("ProcessorTrackerFeatureDummy", _params_tracker_feature_dummy),
+ ProcessorTrackerFeature("ProcessorTrackerFeatureDummy", 0, _params_tracker_feature_dummy),
params_tracker_feature_dummy_(_params_tracker_feature_dummy)
{
//
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_ceres_manager.cpp b/test/gtest_ceres_manager.cpp
index 4ac66fb4b90181dde9a369c1be7f8b9e46e03c76..7ead2b8f801602e664cae1e2ec4249f21ef916a5 100644
--- a/test/gtest_ceres_manager.cpp
+++ b/test/gtest_ceres_manager.cpp
@@ -12,7 +12,7 @@
#include "core/sensor/sensor_base.h"
#include "core/state_block/state_block.h"
#include "core/capture/capture_void.h"
-#include "core/factor/factor_pose_2D.h"
+#include "core/factor/factor_pose_2d.h"
#include "core/factor/factor_quaternion_absolute.h"
#include "core/solver/solver_manager.h"
#include "core/ceres_wrapper/ceres_manager.h"
@@ -320,8 +320,8 @@ TEST(CeresManager, AddFactor)
// Create (and add) factor point 2d
FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2D", Vector3d::Zero(), Matrix3d::Identity());
- FactorPose2DPtr c = FactorBase::emplace<FactorPose2D>(f,f,nullptr,false);
+ auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
+ FactorPose2dPtr c = FactorBase::emplace<FactorPose2d>(f,f,nullptr,false);
// update solver
ceres_manager_ptr->update();
@@ -342,8 +342,8 @@ TEST(CeresManager, DoubleAddFactor)
// Create (and add) factor point 2d
FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2D", Vector3d::Zero(), Matrix3d::Identity());
- FactorPose2DPtr c = FactorBase::emplace<FactorPose2D>(f,f,nullptr,false);
+ auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
+ FactorPose2dPtr c = FactorBase::emplace<FactorPose2d>(f,f,nullptr,false);
// add factor again
P->notifyFactor(c,ADD);
@@ -367,8 +367,8 @@ TEST(CeresManager, RemoveFactor)
// Create (and add) factor point 2d
FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2D", Vector3d::Zero(), Matrix3d::Identity());
- FactorPose2DPtr c = FactorBase::emplace<FactorPose2D>(f,f,nullptr,false);
+ auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
+ FactorPose2dPtr c = FactorBase::emplace<FactorPose2d>(f,f,nullptr,false);
// update solver
ceres_manager_ptr->update();
@@ -395,8 +395,8 @@ TEST(CeresManager, AddRemoveFactor)
// Create (and add) factor point 2d
FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2D", Vector3d::Zero(), Matrix3d::Identity());
- FactorPose2DPtr c = FactorBase::emplace<FactorPose2D>(f,f,nullptr,false);
+ auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
+ FactorPose2dPtr c = FactorBase::emplace<FactorPose2d>(f,f,nullptr,false);
// remove factor
P->notifyFactor(c,REMOVE);
@@ -422,8 +422,8 @@ TEST(CeresManager, DoubleRemoveFactor)
// Create (and add) factor point 2d
FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2D", Vector3d::Zero(), Matrix3d::Identity());
- FactorPose2DPtr c = FactorBase::emplace<FactorPose2D>(f,f,nullptr,false);
+ auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
+ FactorPose2dPtr c = FactorBase::emplace<FactorPose2d>(f,f,nullptr,false);
// update solver
ceres_manager_ptr->update();
@@ -549,7 +549,7 @@ TEST(CeresManager, FactorsRemoveLocalParam)
// Create (and add) 2 factors quaternion
FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2D", Vector3d::Zero(), Matrix3d::Identity());
+ auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
FactorQuaternionAbsolutePtr c1 = FactorBase::emplace<FactorQuaternionAbsolute>(f, F->getO(),nullptr,false);
FactorQuaternionAbsolutePtr c2 = FactorBase::emplace<FactorQuaternionAbsolute>(f, F->getO(),nullptr,false);
@@ -591,7 +591,7 @@ TEST(CeresManager, FactorsUpdateLocalParam)
// Create (and add) 2 factors quaternion
FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2D", Vector3d::Zero(), Matrix3d::Identity());
+ auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
FactorQuaternionAbsolutePtr c1 = FactorBase::emplace<FactorQuaternionAbsolute>(f, F->getO(),nullptr,false);
FactorQuaternionAbsolutePtr c2 = FactorBase::emplace<FactorQuaternionAbsolute>(f, F->getO(),nullptr,false);
diff --git a/test/gtest_emplace.cpp b/test/gtest_emplace.cpp
index e2def5779a6d2dab385df5cbfb599fbd3c7a944c..e60531daf9ab855e03b074384452d8e61c78d52a 100644
--- a/test/gtest_emplace.cpp
+++ b/test/gtest_emplace.cpp
@@ -10,11 +10,11 @@
#include "core/problem/problem.h"
#include "core/sensor/sensor_base.h"
-#include "core/sensor/sensor_odom_2D.h"
-#include "core/sensor/sensor_odom_3D.h"
-#include "core/processor/processor_odom_3D.h"
-#include "core/processor/processor_odom_2D.h"
-#include "core/feature/feature_odom_2D.h"
+#include "core/sensor/sensor_odom_2d.h"
+#include "core/sensor/sensor_odom_3d.h"
+#include "core/processor/processor_odom_3d.h"
+#include "core/processor/processor_odom_2d.h"
+#include "core/feature/feature_odom_2d.h"
#include "dummy/processor_tracker_feature_dummy.h"
#include <iostream>
@@ -48,16 +48,16 @@ TEST(Emplace, Frame)
TEST(Emplace, Processor)
{
- ProblemPtr P = Problem::create("POV", 3);
+ ProblemPtr P = Problem::create("PO", 2);
auto sen = SensorBase::emplace<SensorBase>(P->getHardware(), "Dummy", nullptr, nullptr, nullptr, 2, false);
- auto prc = ProcessorOdom2D::emplace<ProcessorOdom2D>(sen, std::make_shared<ProcessorParamsOdom2D>());
+ auto prc = ProcessorOdom2d::emplace<ProcessorOdom2d>(sen, std::make_shared<ProcessorParamsOdom2d>());
ASSERT_EQ(P, P->getHardware()->getSensorList().front()->getProcessorList().front()->getSensor()->getProblem());
ASSERT_EQ(sen, sen->getProcessorList().front()->getSensor());
ASSERT_EQ(prc, sen->getProcessorList().front());
SensorBasePtr sen2 = SensorBase::emplace<SensorBase>(P->getHardware(), "Dummy", nullptr, nullptr, nullptr, 2, false);
- ProcessorOdom2DPtr prc2 = ProcessorOdom2D::emplace<ProcessorOdom2D>(sen2, std::make_shared<ProcessorParamsOdom2D>());
+ ProcessorOdom2dPtr prc2 = ProcessorOdom2d::emplace<ProcessorOdom2d>(sen2, std::make_shared<ProcessorParamsOdom2d>());
ASSERT_EQ(sen2, sen2->getProcessorList().front()->getSensor());
ASSERT_EQ(prc2, sen2->getProcessorList().front());
}
@@ -107,11 +107,11 @@ TEST(Emplace, Factor)
ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getProblem());
ASSERT_EQ(frm, frm->getCaptureList().front()->getFrame());
auto cov = Eigen::MatrixXd::Identity(2,2);
- auto ftr = FeatureBase::emplace<FeatureOdom2D>(cpt, Eigen::VectorXd(2), cov);
+ auto ftr = FeatureBase::emplace<FeatureOdom2d>(cpt, Eigen::VectorXd(2), cov);
ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFeatureList().front()->getCapture()->getFrame()->getTrajectory()->getProblem());
ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFeatureList().front()->getProblem());
ASSERT_EQ(cpt, cpt->getFeatureList().front()->getCapture());
- auto cnt = FactorBase::emplace<FactorOdom2D>(ftr, ftr, frm, nullptr, false);
+ auto cnt = FactorBase::emplace<FactorOdom2d>(ftr, ftr, frm, nullptr, false);
ASSERT_NE(nullptr, ftr->getFactorList().front().get());
}
@@ -120,15 +120,15 @@ TEST(Emplace, EmplaceDerived)
ProblemPtr P = Problem::create("POV", 3);
auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), KEY, TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
- auto sen = SensorBase::emplace<SensorOdom2D>(P->getHardware(), Eigen::VectorXd(3), ParamsSensorOdom2D());
+ auto sen = SensorBase::emplace<SensorOdom2d>(P->getHardware(), Eigen::VectorXd(3), ParamsSensorOdom2d());
auto cov = Eigen::MatrixXd::Identity(2,2);
- auto cpt = CaptureBase::emplace<CaptureOdom2D>(frm, TimeStamp(0), sen, Eigen::VectorXd(2), cov, nullptr);
+ auto cpt = CaptureBase::emplace<CaptureOdom2d>(frm, TimeStamp(0), sen, Eigen::VectorXd(2), cov, nullptr);
auto m = Eigen::Matrix<double,9,6>();
for(int i = 0; i < 9; i++)
for(int j = 0; j < 6; j++)
m(i,j) = 1;
- auto ftr = FeatureBase::emplace<FeatureOdom2D>(cpt, Eigen::VectorXd(2), cov);
+ auto ftr = FeatureBase::emplace<FeatureOdom2d>(cpt, Eigen::VectorXd(2), cov);
ASSERT_EQ(sen, P->getHardware()->getSensorList().front());
ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFeatureList().front()->getProblem());
}
@@ -144,10 +144,10 @@ TEST(Emplace, ReturnDerived)
auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), KEY, TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
auto cpt = CaptureBase::emplace<CaptureBase>(frm, "Dummy", TimeStamp(0), nullptr, nullptr, nullptr, nullptr);
auto cov = Eigen::MatrixXd::Identity(2,2);
- auto ftr = FeatureBase::emplace<FeatureOdom2D>(cpt, Eigen::VectorXd(2), cov);
- auto cnt = FactorBase::emplace<FactorOdom2D>(ftr, ftr, frm, nullptr, false);
+ auto ftr = FeatureBase::emplace<FeatureOdom2d>(cpt, Eigen::VectorXd(2), cov);
+ auto cnt = FactorBase::emplace<FactorOdom2d>(ftr, ftr, frm, nullptr, false);
- FactorOdom2DPtr fac = FactorBase::emplace<FactorOdom2D>(ftr, ftr, frm, nullptr, false);
+ FactorOdom2dPtr fac = FactorBase::emplace<FactorOdom2d>(ftr, ftr, frm, nullptr, false);
}
int main(int argc, char **argv)
diff --git a/test/gtest_factor_autodiff.cpp b/test/gtest_factor_autodiff.cpp
index d03a9d3a5c728682d0cdef0b609ebb52edeff394..993908461d6b37cae1b24d8701a3612d4dc9c432 100644
--- a/test/gtest_factor_autodiff.cpp
+++ b/test/gtest_factor_autodiff.cpp
@@ -7,11 +7,11 @@
#include "core/utils/utils_gtest.h"
-#include "core/sensor/sensor_odom_2D.h"
+#include "core/sensor/sensor_odom_2d.h"
#include "core/capture/capture_void.h"
-#include "core/feature/feature_odom_2D.h"
-#include "core/factor/factor_odom_2D.h"
-#include "core/factor/factor_odom_2D_analytic.h"
+#include "core/feature/feature_odom_2d.h"
+#include "core/factor/factor_odom_2d.h"
+#include "core/factor/factor_odom_2d_analytic.h"
#include "core/factor/factor_autodiff.h"
using namespace wolf;
@@ -23,19 +23,19 @@ TEST(CaptureAutodiff, ConstructorOdom2d)
FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(TimeStamp(0), std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1)));
// SENSOR
- ParamsSensorOdom2D intrinsics_odo;
+ ParamsSensorOdom2d intrinsics_odo;
intrinsics_odo.k_disp_to_disp = 0.1;
intrinsics_odo.k_rot_to_rot = 0.1;
- SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(Vector3d::Zero(), intrinsics_odo);
+ SensorOdom2dPtr sensor_ptr = std::make_shared<SensorOdom2d>(Vector3d::Zero(), intrinsics_odo);
// CAPTURE
auto capture_ptr = CaptureBase::emplace<CaptureVoid>(fr2_ptr, TimeStamp(0), sensor_ptr);
// FEATURE
- auto feature_ptr = FeatureBase::emplace<FeatureOdom2D>(capture_ptr, Eigen::Vector3d::Zero(), Eigen::Matrix3d::Identity());
+ auto feature_ptr = FeatureBase::emplace<FeatureOdom2d>(capture_ptr, Eigen::Vector3d::Zero(), Eigen::Matrix3d::Identity());
// FACTOR
- auto factor_ptr = FactorBase::emplace<FactorOdom2D>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false);
+ auto factor_ptr = FactorBase::emplace<FactorOdom2d>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false);
ASSERT_TRUE(factor_ptr->getFeature());
ASSERT_TRUE(factor_ptr->getFeature()->getCapture());
@@ -54,10 +54,10 @@ TEST(CaptureAutodiff, ResidualOdom2d)
FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(TimeStamp(0), std::make_shared<StateBlock>(f2_pose.head<2>()), std::make_shared<StateBlock>(f2_pose.tail<1>())));
// SENSOR
- ParamsSensorOdom2D intrinsics_odo;
+ ParamsSensorOdom2d intrinsics_odo;
intrinsics_odo.k_disp_to_disp = 0.1;
intrinsics_odo.k_rot_to_rot = 0.1;
- SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(Vector3d::Zero(), intrinsics_odo);
+ SensorOdom2dPtr sensor_ptr = std::make_shared<SensorOdom2d>(Vector3d::Zero(), intrinsics_odo);
// CAPTURE
auto capture_ptr = CaptureBase::emplace<CaptureVoid>(fr2_ptr, TimeStamp(0), sensor_ptr);
@@ -66,10 +66,10 @@ TEST(CaptureAutodiff, ResidualOdom2d)
// FEATURE
Eigen::Vector3d d;
d << -1, -4, M_PI/2;
- auto feature_ptr = FeatureBase::emplace<FeatureOdom2D>(capture_ptr, d, Eigen::Matrix3d::Identity());
+ auto feature_ptr = FeatureBase::emplace<FeatureOdom2d>(capture_ptr, d, Eigen::Matrix3d::Identity());
// FACTOR
- auto factor_ptr = FactorBase::emplace<FactorOdom2D>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false);
+ auto factor_ptr = FactorBase::emplace<FactorOdom2d>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false);
// EVALUATE
@@ -98,10 +98,10 @@ TEST(CaptureAutodiff, JacobianOdom2d)
FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(TimeStamp(0), std::make_shared<StateBlock>(f2_pose.head<2>()), std::make_shared<StateBlock>(f2_pose.tail<1>())));
// SENSOR
- ParamsSensorOdom2D intrinsics_odo;
+ ParamsSensorOdom2d intrinsics_odo;
intrinsics_odo.k_disp_to_disp = 0.1;
intrinsics_odo.k_rot_to_rot = 0.1;
- SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(Vector3d::Zero(), intrinsics_odo);
+ SensorOdom2dPtr sensor_ptr = std::make_shared<SensorOdom2d>(Vector3d::Zero(), intrinsics_odo);
// CAPTURE
auto capture_ptr = CaptureBase::emplace<CaptureVoid>(fr2_ptr, TimeStamp(0), sensor_ptr);
@@ -109,10 +109,10 @@ TEST(CaptureAutodiff, JacobianOdom2d)
// FEATURE
Eigen::Vector3d d;
d << -1, -4, M_PI/2;
- auto feature_ptr = FeatureBase::emplace<FeatureOdom2D>(capture_ptr, d, Eigen::Matrix3d::Identity());
+ auto feature_ptr = FeatureBase::emplace<FeatureOdom2d>(capture_ptr, d, Eigen::Matrix3d::Identity());
// FACTOR
- auto factor_ptr = FactorBase::emplace<FactorOdom2D>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false);
+ auto factor_ptr = FactorBase::emplace<FactorOdom2d>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false);
// COMPUTE JACOBIANS
@@ -176,10 +176,10 @@ TEST(CaptureAutodiff, AutodiffVsAnalytic)
FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(TimeStamp(0), std::make_shared<StateBlock>(f2_pose.head<2>()), std::make_shared<StateBlock>(f2_pose.tail<1>())));
// SENSOR
- ParamsSensorOdom2D intrinsics_odo;
+ ParamsSensorOdom2d intrinsics_odo;
intrinsics_odo.k_disp_to_disp = 0.1;
intrinsics_odo.k_rot_to_rot = 0.1;
- SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(Vector3d::Zero(), intrinsics_odo);
+ SensorOdom2dPtr sensor_ptr = std::make_shared<SensorOdom2d>(Vector3d::Zero(), intrinsics_odo);
// CAPTURE
auto capture_ptr = CaptureBase::emplace<CaptureVoid>(fr2_ptr, TimeStamp(0), sensor_ptr);
@@ -187,11 +187,11 @@ TEST(CaptureAutodiff, AutodiffVsAnalytic)
// FEATURE
Eigen::Vector3d d;
d << -1, -4, M_PI/2;
- auto feature_ptr = FeatureBase::emplace<FeatureOdom2D>(capture_ptr, d, Eigen::Matrix3d::Identity());
+ auto feature_ptr = FeatureBase::emplace<FeatureOdom2d>(capture_ptr, d, Eigen::Matrix3d::Identity());
// FACTOR
- auto fac_autodiff_ptr = FactorBase::emplace<FactorOdom2D>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false);
- auto fac_analytic_ptr = FactorBase::emplace<FactorOdom2DAnalytic>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false);
+ auto fac_autodiff_ptr = FactorBase::emplace<FactorOdom2d>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false);
+ auto fac_analytic_ptr = FactorBase::emplace<FactorOdom2dAnalytic>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false);
// COMPUTE JACOBIANS
diff --git a/test/gtest_factor_autodiff_distance_3D.cpp b/test/gtest_factor_autodiff_distance_3d.cpp
similarity index 85%
rename from test/gtest_factor_autodiff_distance_3D.cpp
rename to test/gtest_factor_autodiff_distance_3d.cpp
index 0e4bf7a2dd6729fee7cdef688b261af3ba3f4a95..985e25a23de5f1c6b137d1b477bb9a22b219fb2a 100644
--- a/test/gtest_factor_autodiff_distance_3D.cpp
+++ b/test/gtest_factor_autodiff_distance_3d.cpp
@@ -1,11 +1,11 @@
/**
- * \file gtest_factor_autodiff_distance_3D.cpp
+ * \file gtest_factor_autodiff_distance_3d.cpp
*
* Created on: Apr 10, 2018
* \author: jsola
*/
-#include "core/factor/factor_autodiff_distance_3D.h"
+#include "core/factor/factor_autodiff_distance_3d.h"
#include "core/problem/problem.h"
#include "core/utils/logging.h"
#include "core/ceres_wrapper/ceres_manager.h"
@@ -16,7 +16,7 @@
using namespace wolf;
using namespace Eigen;
-class FactorAutodiffDistance3D_Test : public testing::Test
+class FactorAutodiffDistance3d_Test : public testing::Test
{
public:
Vector3d pos1, pos2;
@@ -28,7 +28,7 @@ class FactorAutodiffDistance3D_Test : public testing::Test
FrameBasePtr F1, F2;
CaptureBasePtr C2;
FeatureBasePtr f2;
- FactorAutodiffDistance3DPtr c2;
+ FactorAutodiffDistance3dPtr c2;
Vector1d dist;
Matrix1d dist_cov;
@@ -60,13 +60,13 @@ class FactorAutodiffDistance3D_Test : public testing::Test
F2 = problem->emplaceFrame (KEY, pose2, 2.0);
C2 = CaptureBase::emplace<CaptureBase>(F2, "Base", 1.0);
f2 = FeatureBase::emplace<FeatureBase>(C2, "Dist", dist, dist_cov);
- c2 = FactorBase::emplace<FactorAutodiffDistance3D>(f2, f2, F1, nullptr, false, FAC_ACTIVE);
+ c2 = FactorBase::emplace<FactorAutodiffDistance3d>(f2, f2, F1, nullptr, false, FAC_ACTIVE);
}
};
-TEST_F(FactorAutodiffDistance3D_Test, ground_truth)
+TEST_F(FactorAutodiffDistance3d_Test, ground_truth)
{
double res;
@@ -75,7 +75,7 @@ TEST_F(FactorAutodiffDistance3D_Test, ground_truth)
ASSERT_NEAR(res, 0.0, 1e-5);
}
-TEST_F(FactorAutodiffDistance3D_Test, expected_residual)
+TEST_F(FactorAutodiffDistance3d_Test, expected_residual)
{
double measurement = 1.400;
@@ -89,7 +89,7 @@ TEST_F(FactorAutodiffDistance3D_Test, expected_residual)
ASSERT_NEAR(res, res_expected, 1e-5);
}
-TEST_F(FactorAutodiffDistance3D_Test, solve)
+TEST_F(FactorAutodiffDistance3d_Test, solve)
{
double measurement = 1.400;
f2->setMeasurement(Vector1d(measurement));
diff --git a/test/gtest_factor_base.cpp b/test/gtest_factor_base.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9e4f2ef10e8bf2fc1fb9a47d48468e254e46d0af
--- /dev/null
+++ b/test/gtest_factor_base.cpp
@@ -0,0 +1,120 @@
+/*
+ * gtest_factor_base.cpp
+ *
+ * Created on: Apr 2, 2020
+ * Author: jsola
+ */
+
+
+#include "core/utils/utils_gtest.h"
+#include "core/utils/logging.h"
+
+#include "core/factor/factor_base.h"
+
+using namespace wolf;
+using namespace Eigen;
+
+class FactorBaseTest : public testing::Test
+{
+ public:
+ FrameBasePtr F0,F1;
+ CaptureBasePtr C0,C1;
+ FeatureBasePtr f0,f1;
+ LandmarkBasePtr L0,L1;
+
+ virtual void SetUp()
+ {
+ F0 = std::make_shared<FrameBase>(0.0, nullptr);
+ F1 = std::make_shared<FrameBase>(1.0, nullptr);
+ C0 = std::make_shared<CaptureBase>("Capture", 0.0, nullptr);
+ C1 = std::make_shared<CaptureBase>("Capture", 1.0, nullptr);
+ f0 = std::make_shared<FeatureBase>("Feature", Vector2d(1,2), Matrix2d::Identity(), FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
+ f1 = std::make_shared<FeatureBase>("Feature", Vector2d(1,2), Matrix2d::Identity(), FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
+ L0 = std::make_shared<LandmarkBase>("Lmk", nullptr);
+ L1 = std::make_shared<LandmarkBase>("Lmk", nullptr);
+ }
+// virtual void TearDown(){}
+};
+
+class FactorDummy : public FactorBase
+{
+ public:
+ FactorDummy(const FrameBasePtr& _frame_other,
+ const CaptureBasePtr& _capture_other,
+ const FeatureBasePtr& _feature_other,
+ const LandmarkBasePtr& _landmark_other) :
+ FactorBase("Dummy",
+ _frame_other,
+ _capture_other,
+ _feature_other,
+ _landmark_other,
+ nullptr,
+ false)
+ {
+ //
+ }
+ FactorDummy(const FrameBasePtrList& _frame_other_list,
+ const CaptureBasePtrList& _capture_other_list,
+ const FeatureBasePtrList& _feature_other_list,
+ const LandmarkBasePtrList& _landmark_other_list) :
+ FactorBase("Dummy",
+ _frame_other_list,
+ _capture_other_list,
+ _feature_other_list,
+ _landmark_other_list,
+ nullptr,
+ false)
+ {
+ //
+ }
+ virtual ~FactorDummy() = default;
+
+ virtual std::string getTopology() const override {return "DUMMY";}
+ virtual bool evaluate(double const* const* _parameters, double* _residuals, double** _jacobians) const override {return true;}
+ virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& _residual, std::vector<Eigen::MatrixXd>& _jacobians) const override {}
+ virtual JacobianMethod getJacobianMethod() const override {return JacobianMethod::JAC_ANALYTIC;}
+ virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const override {std::vector<StateBlockPtr> v; return v;}
+ virtual std::vector<unsigned int> getStateSizes() const override {std::vector<unsigned int> v; return v;}
+ virtual unsigned int getSize() const override {return 0;}
+
+};
+
+TEST_F(FactorBaseTest, constructor_from_pointers)
+{
+ FactorDummy fac(nullptr,C0,f0,nullptr);
+
+ ASSERT_TRUE(fac.getFrameOtherList().empty());
+
+ ASSERT_EQ(fac.getCaptureOtherList().size(), 1);
+
+ ASSERT_EQ(fac.getFeatureOtherList().size(), 1);
+
+ ASSERT_TRUE(fac.getLandmarkOtherList().empty());
+}
+
+TEST_F(FactorBaseTest, constructor_from_lists)
+{
+ FactorDummy fac({},{C0},{f0,f1},{});
+
+ ASSERT_TRUE(fac.getFrameOtherList().empty());
+
+ ASSERT_EQ(fac.getCaptureOtherList().size(), 1);
+
+ ASSERT_EQ(fac.getFeatureOtherList().size(), 2);
+
+ ASSERT_TRUE(fac.getLandmarkOtherList().empty());
+}
+
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+
+ // restrict to a group of tests only
+ //::testing::GTEST_FLAG(filter) = "TestInit.*";
+
+ // restrict to this test only
+ //::testing::GTEST_FLAG(filter) = "TestInit.testName";
+
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_factor_block_difference.cpp b/test/gtest_factor_block_difference.cpp
index 20bbe2c42ed27688820569a3da345596da3903f1..1e06c7a80377b99197a721e85473319f9bf34862 100644
--- a/test/gtest_factor_block_difference.cpp
+++ b/test/gtest_factor_block_difference.cpp
@@ -50,18 +50,17 @@ class FixtureFactorBlockDifference : public testing::Test
Vector10d x_origin = problem_->zeroState();
Eigen::Matrix9d cov_prior = 1e-3 * Eigen::Matrix9d::Identity();
- KF0_ =problem_->setPrior(x_origin, cov_prior, t0, 0.1);
+ KF0_ = problem_->setPrior(x_origin, cov_prior, t0, 0.1);
CaptureBasePtr capV0 = CaptureBase::emplace<CaptureBase>(KF0_, "Vel0", t0);
FeatureBasePtr featV0 = FeatureBase::emplace<FeatureBase>(capV0, "Vel0", x_origin.tail(3), cov_prior.bottomRightCorner<3,3>());
FactorBasePtr facV0 = FactorBase::emplace<FactorBlockAbsolute>(featV0, KF0_->getV());
- // KF0_ = problem_->emplaceFrame(KEY, problem_->zeroState(), t0);
KF1_ = problem_->emplaceFrame(KEY, problem_->zeroState(), t1);
- Cap_ = CaptureBase::emplace<CaptureBase>(KF1_, "DIFF", t1);
+ Cap_ = CaptureBase::emplace<CaptureBase>(KF1_, "BlockDifference", t1);
Eigen::Matrix3d cov = 0.2 * Eigen::Matrix3d::Identity();
- Feat_ = FeatureBase::emplace<FeatureBase>(Cap_, "DIFF", zero3, cov);
+ Feat_ = FeatureBase::emplace<FeatureBase>(Cap_, "BlockDifference", zero3, cov);
}
virtual void TearDown() override {}
@@ -150,6 +149,7 @@ TEST_F(FixtureFactorBlockDifference, DiffPx)
Feat_->setMeasurementCovariance(cov_diff);
FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>(
Feat_, KF0_->getP(), KF1_->getP(),
+ nullptr, nullptr, nullptr, nullptr,
0, 1, 0, 1
);
@@ -174,6 +174,7 @@ TEST_F(FixtureFactorBlockDifference, DiffPxy)
Feat_->setMeasurementCovariance(cov_diff);
FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>(
Feat_, KF0_->getP(), KF1_->getP(),
+ nullptr, nullptr, nullptr, nullptr,
0, 2, 0, 2
);
@@ -195,6 +196,7 @@ TEST_F(FixtureFactorBlockDifference, DiffPyz)
Feat_->setMeasurementCovariance(cov_diff);
FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>(
Feat_, KF0_->getP(), KF1_->getP(),
+ nullptr, nullptr, nullptr, nullptr,
1, 2, 1, 2
);
diff --git a/test/gtest_factor_odom_3D.cpp b/test/gtest_factor_odom_3d.cpp
similarity index 86%
rename from test/gtest_factor_odom_3D.cpp
rename to test/gtest_factor_odom_3d.cpp
index d1b88599bfee17681e8e8187f6ed4eac982b40e1..1315118016ebe3319a04e07b37d0d8438b714ebe 100644
--- a/test/gtest_factor_odom_3D.cpp
+++ b/test/gtest_factor_odom_3d.cpp
@@ -1,5 +1,5 @@
/**
- * \file gtest_factor_odom_3D.cpp
+ * \file gtest_factor_odom_3d.cpp
*
* Created on: Mar 30, 2017
* \author: jsola
@@ -7,7 +7,7 @@
#include "core/utils/utils_gtest.h"
-#include "core/factor/factor_odom_3D.h"
+#include "core/factor/factor_odom_3d.h"
#include "core/capture/capture_motion.h"
#include "core/ceres_wrapper/ceres_manager.h"
@@ -41,23 +41,23 @@ FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, problem_ptr->zeroState(), Tim
FrameBasePtr frm1 = problem_ptr->emplaceFrame(KEY, delta, TimeStamp(1));
// Capture, feature and factor from frm1 to frm0
-auto cap1 = CaptureBase::emplace<CaptureMotion>(frm1, "CaptureOdom3D", 1, nullptr, delta, 7, 6, nullptr);
-auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom3D", delta, data_cov);
-FactorOdom3DPtr ctr1 = FactorBase::emplace<FactorOdom3D>(fea1, fea1, frm0, nullptr, false); // create and add
+auto cap1 = CaptureBase::emplace<CaptureMotion>(frm1, "CaptureOdom3d", 1, nullptr, delta, 7, 6, nullptr);
+auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom3d", delta, data_cov);
+FactorOdom3dPtr ctr1 = FactorBase::emplace<FactorOdom3d>(fea1, fea1, frm0, nullptr, false); // create and add
////////////////////////////////////////////////////////
-TEST(FactorOdom3D, check_tree)
+TEST(FactorOdom3d, check_tree)
{
ASSERT_TRUE(problem_ptr->check(0));
}
-TEST(FactorOdom3D, expectation)
+TEST(FactorOdom3d, expectation)
{
ASSERT_MATRIX_APPROX(ctr1->expectation() , delta, 1e-6);
}
-TEST(FactorOdom3D, fix_0_solve)
+TEST(FactorOdom3d, fix_0_solve)
{
// Fix frame 0, perturb frm1
@@ -72,7 +72,7 @@ TEST(FactorOdom3D, fix_0_solve)
}
-TEST(FactorOdom3D, fix_1_solve)
+TEST(FactorOdom3d, fix_1_solve)
{
// Fix frame 1, perturb frm0
frm0->unfix();
diff --git a/test/gtest_factor_pose_2D.cpp b/test/gtest_factor_pose_2d.cpp
similarity index 85%
rename from test/gtest_factor_pose_2D.cpp
rename to test/gtest_factor_pose_2d.cpp
index 71c7b7d5ba9169be5c433f714be1d7c97f44bcb8..5d9cb7f406beef3b95ce1da4add8f2041a20c685 100644
--- a/test/gtest_factor_pose_2D.cpp
+++ b/test/gtest_factor_pose_2d.cpp
@@ -1,11 +1,11 @@
/**
- * \file gtest_factor_pose_2D.cpp
+ * \file gtest_factor_pose_2d.cpp
*
* Created on: Mar 30, 2017
* \author: jsola
*/
-#include "core/factor/factor_pose_2D.h"
+#include "core/factor/factor_pose_2d.h"
#include "core/utils/utils_gtest.h"
#include "core/capture/capture_motion.h"
@@ -33,13 +33,13 @@ CeresManager ceres_mgr(problem);
FrameBasePtr frm0 = problem->emplaceFrame(KEY, problem->zeroState(), TimeStamp(0));
// Capture, feature and factor from frm1 to frm0
-auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "CaptureOdom2D", 0, nullptr, pose, 3, 3, nullptr);
-auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "FeatureOdom2D", pose, data_cov);
-auto ctr0 = FactorBase::emplace<FactorPose2D>(fea0, fea0, nullptr, false);
+auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "CaptureOdom2d", 0, nullptr, pose, 3, 3, nullptr);
+auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "FeatureOdom2d", pose, data_cov);
+auto ctr0 = FactorBase::emplace<FactorPose2d>(fea0, fea0, nullptr, false);
////////////////////////////////////////////////////////
-TEST(FactorPose2D, check_tree)
+TEST(FactorPose2d, check_tree)
{
ASSERT_TRUE(problem->check(0));
}
@@ -49,7 +49,7 @@ TEST(FactorPose2D, check_tree)
// ASSERT_EIGEN_APPROX(ctr0->expectation() , delta);
//}
-TEST(FactorPose2D, solve)
+TEST(FactorPose2d, solve)
{
// Fix frame 0, perturb frm1
diff --git a/test/gtest_factor_pose_3D.cpp b/test/gtest_factor_pose_3d.cpp
similarity index 84%
rename from test/gtest_factor_pose_3D.cpp
rename to test/gtest_factor_pose_3d.cpp
index d0a6258a01f834b05c91771986740baa2a65b743..9eeb5dfc14bbc6b185c598a8a17489a44794206e 100644
--- a/test/gtest_factor_pose_3D.cpp
+++ b/test/gtest_factor_pose_3d.cpp
@@ -1,11 +1,11 @@
/**
- * \file gtest_factor_fix_3D.cpp
+ * \file gtest_factor_fix_3d.cpp
*
* Created on: Mar 30, 2017
* \author: jsola
*/
-#include "core/factor/factor_pose_3D.h"
+#include "core/factor/factor_pose_3d.h"
#include "core/utils/utils_gtest.h"
#include "core/capture/capture_motion.h"
@@ -39,23 +39,23 @@ CeresManager ceres_mgr(problem);
FrameBasePtr frm0 = problem->emplaceFrame(KEY, problem->zeroState(), TimeStamp(0));
// Capture, feature and factor
-auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "CaptureOdom3D", 0, nullptr, pose7, 7, 6, nullptr);
-auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "FeatureOdom3D", pose7, data_cov);
-FactorPose3DPtr ctr0 = FactorBase::emplace<FactorPose3D>(fea0, fea0, nullptr, false);
+auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "CaptureOdom3d", 0, nullptr, pose7, 7, 6, nullptr);
+auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "FeatureOdom3d", pose7, data_cov);
+FactorPose3dPtr ctr0 = FactorBase::emplace<FactorPose3d>(fea0, fea0, nullptr, false);
////////////////////////////////////////////////////////
-TEST(FactorPose3D, check_tree)
+TEST(FactorPose3d, check_tree)
{
ASSERT_TRUE(problem->check(0));
}
-//TEST(FactorFix3D, expectation)
+//TEST(FactorFix3d, expectation)
//{
// ASSERT_EIGEN_APPROX(ctr0->expectation() , delta);
//}
-TEST(FactorPose3D, solve)
+TEST(FactorPose3d, solve)
{
// Fix frame 0, perturb frm1
diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp
index e37ef4935c3deb1e2c1e4449a6b7a294ea9a000a..77fa1ace2c4bf876b0c318fe79dc476688e92af0 100644
--- a/test/gtest_frame_base.cpp
+++ b/test/gtest_frame_base.cpp
@@ -9,9 +9,9 @@
#include "core/utils/logging.h"
#include "core/frame/frame_base.h"
-#include "core/sensor/sensor_odom_2D.h"
-#include "core/processor/processor_odom_2D.h"
-#include "core/factor/factor_odom_2D.h"
+#include "core/sensor/sensor_odom_2d.h"
+#include "core/processor/processor_odom_2d.h"
+#include "core/factor/factor_odom_2d.h"
#include "core/capture/capture_motion.h"
#include <iostream>
@@ -53,7 +53,7 @@ TEST(FrameBase, LinksBasic)
ASSERT_FALSE(F->getProblem());
// ASSERT_THROW(f->getPreviousFrame(), std::runtime_error); // protected by assert()
// ASSERT_EQ(f->getStatus(), ST_ESTIMATED); // protected
- SensorOdom2DPtr S = make_shared<SensorOdom2D>(Vector3d::Zero(), ParamsSensorOdom2D());
+ SensorOdom2dPtr S = make_shared<SensorOdom2d>(Vector3d::Zero(), ParamsSensorOdom2d());
ASSERT_FALSE(F->getCaptureOf(S));
ASSERT_TRUE(F->getCaptureList().empty());
ASSERT_TRUE(F->getConstrainedByList().empty());
@@ -65,16 +65,16 @@ TEST(FrameBase, LinksToTree)
// Problem with 2 frames and one motion factor between them
ProblemPtr P = Problem::create("PO", 2);
TrajectoryBasePtr T = P->getTrajectory();
- ParamsSensorOdom2D intrinsics_odo;
+ ParamsSensorOdom2d intrinsics_odo;
intrinsics_odo.k_disp_to_disp = 1;
intrinsics_odo.k_rot_to_rot = 1;
- auto S = SensorBase::emplace<SensorOdom2D>(P->getHardware(), Vector3d::Zero(), intrinsics_odo);
+ auto S = SensorBase::emplace<SensorOdom2d>(P->getHardware(), Vector3d::Zero(), intrinsics_odo);
auto F1 = FrameBase::emplace<FrameBase>(T, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
auto F2 = FrameBase::emplace<FrameBase>(T, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
auto C = CaptureBase::emplace<CaptureMotion>(F1, "CaptureMotion", 1, S, Vector3d::Zero(), 3, 3, nullptr);
- auto p = ProcessorBase::emplace<ProcessorOdom2D>(S, std::make_shared<ProcessorParamsOdom2D>());
+ auto p = ProcessorBase::emplace<ProcessorOdom2d>(S, std::make_shared<ProcessorParamsOdom2d>());
auto f = FeatureBase::emplace<FeatureBase>(C, "f", Vector1d(1), Matrix<double,1,1>::Identity()*.01);
- auto c = FactorBase::emplace<FactorOdom2D>(f, f, F2, p, false);
+ auto c = FactorBase::emplace<FactorOdom2d>(f, f, F2, p, false);
//TODO: WARNING! I dropped this comprovations since the emplacing operation is now atomic.
ASSERT_FALSE(F2->getConstrainedByList().empty());
@@ -120,7 +120,7 @@ TEST(FrameBase, LinksToTree)
#include "core/state_block/state_quaternion.h"
TEST(FrameBase, GetSetState)
{
- // Create PQV_3D state blocks
+ // Create PQV_3d state blocks
StateBlockPtr sbp = make_shared<StateBlock>(3);
StateBlockPtr sbv = make_shared<StateBlock>(3);
StateQuaternionPtr sbq = make_shared<StateQuaternion>();
diff --git a/test/gtest_has_state_blocks.cpp b/test/gtest_has_state_blocks.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..df90859ba57d46382d97e7fa29fa66b23b137580
--- /dev/null
+++ b/test/gtest_has_state_blocks.cpp
@@ -0,0 +1,188 @@
+/**
+ * \file gtest_has_state_blocks.cpp
+ *
+ * Created on: Mar 24, 2020
+ * \author: jsola
+ */
+
+
+#include "core/utils/utils_gtest.h"
+
+#include "core/frame/frame_base.h"
+#include "core/sensor/sensor_base.h"
+#include "core/landmark/landmark_base.h"
+#include "core/state_block/state_quaternion.h"
+#include "core/ceres_wrapper/ceres_manager.h"
+
+using namespace wolf;
+
+class HasStateBlocksTest : public testing::Test
+{
+
+ public:
+
+ ProblemPtr problem;
+ SensorBasePtr S0, S1;
+ FrameBasePtr F0, F1;
+ CaptureBasePtr C0, C1;
+ LandmarkBasePtr L0, L1;
+ StateBlockPtr sbp0, sbv0, sbp1, sbv1;
+ StateQuaternionPtr sbo0, sbo1;
+
+
+ virtual void SetUp()
+ {
+ problem = Problem::create("POV", 3);
+
+ sbp0 = std::make_shared<StateBlock>(3); // size 3
+ sbo0 = std::make_shared<StateQuaternion>();
+ sbv0 = std::make_shared<StateBlock>(3); // size 3
+ sbp1 = std::make_shared<StateBlock>(3); // size 3
+ sbo1 = std::make_shared<StateQuaternion>();
+ sbv1 = std::make_shared<StateBlock>(3); // size 3
+
+ F0 = std::make_shared<FrameBase>(0.0, sbp0, sbo0); // non KF
+ F1 = std::make_shared<FrameBase>(1.0, nullptr); // non KF
+
+ }
+ virtual void TearDown(){}
+
+};
+
+
+TEST_F(HasStateBlocksTest, Notifications_setKey_add)
+{
+ Notification n;
+ ASSERT_FALSE(problem->getStateBlockNotification(sbp0, n));
+
+ F0->link(problem->getTrajectory());
+
+ ASSERT_FALSE(problem->getStateBlockNotification(sbp0, n));
+
+ F0->setKey();
+
+ ASSERT_TRUE(problem->getStateBlockNotification(sbp0, n));
+ ASSERT_EQ(n, ADD);
+}
+
+TEST_F(HasStateBlocksTest, Notifications_add_makeKF)
+{
+ Notification n;
+
+ // First add SB, than make KF
+
+ ASSERT_FALSE(problem->getStateBlockNotification(sbp0, n));
+
+ F0->link(problem->getTrajectory());
+
+ ASSERT_FALSE(problem->getStateBlockNotification(sbp0, n));
+
+ ASSERT_FALSE(problem->getStateBlockNotification(sbv0, n));
+
+ F0->addStateBlock("V", sbv0);
+
+ ASSERT_FALSE(problem->getStateBlockNotification(sbv0, n));
+
+ F0->setKey();
+
+ ASSERT_TRUE(problem->getStateBlockNotification(sbp0, n));
+ ASSERT_EQ(n, ADD);
+
+ ASSERT_TRUE(problem->getStateBlockNotification(sbv0, n));
+ ASSERT_EQ(n, ADD);
+
+}
+
+TEST_F(HasStateBlocksTest, Notifications_makeKF_add)
+{
+ Notification n;
+
+ // first make KF, then add SB
+
+ F1->link(problem->getTrajectory());
+ F1->setKey();
+
+ F1->addStateBlock("P", sbp1);
+
+ ASSERT_TRUE(problem->getStateBlockNotification(sbp1, n));
+ ASSERT_EQ(n, ADD);
+
+ // add another SB
+
+ ASSERT_FALSE(problem->getStateBlockNotification(sbv1, n));
+
+ F1->addStateBlock("V", sbv1);
+
+ ASSERT_TRUE(problem->getStateBlockNotification(sbv1, n));
+ ASSERT_EQ(n, ADD);
+
+}
+
+TEST_F(HasStateBlocksTest, Add_solve_notify_solve_add)
+{
+
+ CeresManagerPtr solver = std::make_shared<CeresManager>(problem);
+
+
+ Notification n;
+
+ // Add SB, make KF
+
+ ASSERT_FALSE(problem->getStateBlockNotification(sbp0, n));
+
+ F0->link(problem->getTrajectory());
+ F0->addStateBlock("V", sbv0);
+ F0->setKey();
+
+ ASSERT_TRUE(problem->getStateBlockNotification(sbv0, n));
+ ASSERT_EQ(n, ADD);
+
+ // solve. This will clear all notifications
+
+ std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
+
+ ASSERT_FALSE(problem->getStateBlockNotification(sbv0, n));
+
+ // Notify again the same SB
+
+ problem->notifyStateBlock(sbv0, ADD);
+
+ ASSERT_TRUE(problem->getStateBlockNotification(sbv0, n));
+ ASSERT_EQ(n, ADD);
+
+ // solve again
+
+ report = solver->solve(SolverManager::ReportVerbosity::FULL);
+
+ ASSERT_FALSE(problem->getStateBlockNotification(sbv0, n));
+
+ // Add again the same SB. This should crash
+
+ ASSERT_DEATH( F0->addStateBlock("V", sbv0) , "" );
+
+}
+
+TEST_F(HasStateBlocksTest, hasStateBlock)
+{
+ ASSERT_TRUE(F0->hasStateBlock(sbp0));
+ ASSERT_FALSE(F0->hasStateBlock(sbv1));
+}
+
+TEST_F(HasStateBlocksTest, stateBlockKey)
+{
+ std::string key;
+ ASSERT_TRUE(F0->stateBlockKey(sbp0, key));
+ ASSERT_TRUE(key == "P");
+
+ ASSERT_FALSE(F0->stateBlockKey(sbp1, key));
+ ASSERT_TRUE(key == "");
+}
+
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+//::testing::GTEST_FLAG(filter) = "TestGroup.DummyTestExample";
+ return RUN_ALL_TESTS();
+}
+
diff --git a/test/gtest_map_yaml.cpp b/test/gtest_map_yaml.cpp
index b738e5e77a3d8926f4c22b6c5b2c7e8d25d4965a..dd3f75031499911304c36a1e8a4f5fa9f7c10e04 100644
--- a/test/gtest_map_yaml.cpp
+++ b/test/gtest_map_yaml.cpp
@@ -18,7 +18,7 @@
#include <iostream>
using namespace wolf;
-TEST(MapYaml, save_2D)
+TEST(MapYaml, save_2d)
{
ProblemPtr problem = Problem::create("PO", 2);
@@ -44,17 +44,17 @@ TEST(MapYaml, save_2D)
std::string map_path = wolf_root + "/test/yaml";
// Check Problem functions
- std::string filename = map_path + "/map_2D_problem.yaml";
+ std::string filename = map_path + "/map_2d_problem.yaml";
std::cout << "Saving map to file: " << filename << std::endl;
- problem->saveMap(filename, "2D map saved from Problem::saveMap()");
+ problem->saveMap(filename, "2d map saved from Problem::saveMap()");
// Check Map functions
- filename = map_path + "/map_2D_map.yaml";
+ filename = map_path + "/map_2d_map.yaml";
std::cout << "Saving map to file: " << filename << std::endl;
- problem->getMap()->save(filename, "2D map saved from Map::save()");
+ problem->getMap()->save(filename, "2d map saved from Map::save()");
}
-TEST(MapYaml, load_2D)
+TEST(MapYaml, load_2d)
{
ProblemPtr problem = Problem::create("PO", 2);
@@ -62,7 +62,7 @@ TEST(MapYaml, load_2D)
std::string map_path = wolf_root + "/test/yaml";
// Check Problem functions
- std::string filename = map_path + "/map_2D_problem.yaml";
+ std::string filename = map_path + "/map_2d_problem.yaml";
std::cout << "Loading map to file: " << filename << std::endl;
problem->loadMap(filename);
@@ -106,7 +106,7 @@ TEST(MapYaml, load_2D)
ASSERT_TRUE(problem->getMap()->getLandmarkList().empty());
// Check Map functions
- filename = map_path + "/map_2D_map.yaml";
+ filename = map_path + "/map_2d_map.yaml";
std::cout << "Loading map to file: " << filename << std::endl;
problem->getMap()->load(filename);
@@ -141,7 +141,7 @@ TEST(MapYaml, load_2D)
}
}
}
-TEST(MapYaml, save_3D)
+TEST(MapYaml, save_3d)
{
ProblemPtr problem = Problem::create("PO", 3);
@@ -167,17 +167,17 @@ TEST(MapYaml, save_3D)
std::string map_path = wolf_root + "/test/yaml";
// Check Problem functions
- std::string filename = map_path + "/map_3D_problem.yaml";
+ std::string filename = map_path + "/map_3d_problem.yaml";
std::cout << "Saving map to file: " << filename << std::endl;
- problem->saveMap(filename, "3D map saved from Problem::saveMap()");
+ problem->saveMap(filename, "3d map saved from Problem::saveMap()");
// Check Map functions
- filename = map_path + "/map_3D_map.yaml";
+ filename = map_path + "/map_3d_map.yaml";
std::cout << "Saving map to file: " << filename << std::endl;
- problem->getMap()->save(filename, "3D map saved from Map::save()");
+ problem->getMap()->save(filename, "3d map saved from Map::save()");
}
-TEST(MapYaml, load_3D)
+TEST(MapYaml, load_3d)
{
ProblemPtr problem = Problem::create("PO", 3);
@@ -185,7 +185,7 @@ TEST(MapYaml, load_3D)
std::string map_path = wolf_root + "/test/yaml";
// Check Problem functions
- std::string filename = map_path + "/map_3D_problem.yaml";
+ std::string filename = map_path + "/map_3d_problem.yaml";
std::cout << "Loading map to file: " << filename << std::endl;
problem->loadMap(filename);
@@ -231,7 +231,7 @@ TEST(MapYaml, load_3D)
ASSERT_TRUE(problem->getMap()->getLandmarkList().empty());
// Check Map functions
- filename = map_path + "/map_3D_map.yaml";
+ filename = map_path + "/map_3d_map.yaml";
std::cout << "Loading map to file: " << filename << std::endl;
problem->getMap()->load(filename);
@@ -274,13 +274,13 @@ TEST(MapYaml, remove_map_files)
std::string wolf_root = _WOLF_ROOT_DIR;
std::string map_path = wolf_root + "/test/yaml";
- std::string filename = map_path + "/map_2D_problem.yaml";
+ std::string filename = map_path + "/map_2d_problem.yaml";
ASSERT_TRUE(std::remove(filename.c_str()) == 0);
- filename = map_path + "/map_2D_map.yaml";
+ filename = map_path + "/map_2d_map.yaml";
ASSERT_TRUE(std::remove(filename.c_str()) == 0);
- filename = map_path + "/map_3D_problem.yaml";
+ filename = map_path + "/map_3d_problem.yaml";
ASSERT_TRUE(std::remove(filename.c_str()) == 0);
- filename = map_path + "/map_3D_map.yaml";
+ filename = map_path + "/map_3d_map.yaml";
ASSERT_TRUE(std::remove(filename.c_str()) == 0);
}
diff --git a/test/gtest_odom_2D.cpp b/test/gtest_odom_2d.cpp
similarity index 88%
rename from test/gtest_odom_2D.cpp
rename to test/gtest_odom_2d.cpp
index 8679baccf74f02221917063f91cb685af48f3566..1176783c855954afba8bf5e0a7566fb37ecbc8b5 100644
--- a/test/gtest_odom_2D.cpp
+++ b/test/gtest_odom_2d.cpp
@@ -1,5 +1,5 @@
/**
- * \file test_odom_2D.cpp
+ * \file test_odom_2d.cpp
*
* Created on: Mar 15, 2016
* \author: jsola
@@ -8,10 +8,10 @@
#include "core/utils/utils_gtest.h"
// Classes under test
-#include "core/sensor/sensor_odom_2D.h"
-#include "core/processor/processor_odom_2D.h"
-#include "core/factor/factor_odom_2D.h"
-#include "core/capture/capture_odom_2D.h"
+#include "core/sensor/sensor_odom_2d.h"
+#include "core/processor/processor_odom_2d.h"
+#include "core/factor/factor_odom_2d.h"
+#include "core/capture/capture_odom_2d.h"
// Wolf includes
#include "core/state_block/state_block.h"
@@ -100,7 +100,7 @@ void show(const ProblemPtr& problem)
}
}
-TEST(Odom2D, FactorFix_and_FactorOdom2D)
+TEST(Odom2d, FactorFix_and_FactorOdom2d)
{
using std::cout;
using std::endl;
@@ -113,7 +113,7 @@ TEST(Odom2D, FactorFix_and_FactorOdom2D)
// |
// GND
// `absolute` is made with FactorFix
- // `motion` is made with FactorOdom2D
+ // `motion` is made with FactorOdom2d
std::cout << std::setprecision(4);
@@ -133,16 +133,16 @@ TEST(Odom2D, FactorFix_and_FactorOdom2D)
// KF1 and motion from KF0
t += dt;
FrameBasePtr F1 = Pr->emplaceFrame(KEY, Vector3d::Zero(), t);
- auto C1 = CaptureBase::emplace<CaptureBase>(F1, "CaptureOdom2D", t);
- auto f1 = FeatureBase::emplace<FeatureBase>(C1, "FeatureOdom2D", delta, delta_cov);
- auto c1 = FactorBase::emplace<FactorOdom2D>(f1, f1, F0, nullptr, false);
+ auto C1 = CaptureBase::emplace<CaptureBase>(F1, "CaptureOdom2d", t);
+ auto f1 = FeatureBase::emplace<FeatureBase>(C1, "FeatureOdom2d", delta, delta_cov);
+ auto c1 = FactorBase::emplace<FactorOdom2d>(f1, f1, F0, nullptr, false);
// KF2 and motion from KF1
t += dt;
FrameBasePtr F2 = Pr->emplaceFrame(KEY, Vector3d::Zero(), t);
- auto C2 = CaptureBase::emplace<CaptureBase>(F2, "CaptureOdom2D", t);
- auto f2 = FeatureBase::emplace<FeatureBase>(C2, "FeatureOdom2D", delta, delta_cov);
- auto c2 = FactorBase::emplace<FactorOdom2D>(f2, f2, F1, nullptr, false);
+ auto C2 = CaptureBase::emplace<CaptureBase>(F2, "CaptureOdom2d", t);
+ auto f2 = FeatureBase::emplace<FeatureBase>(C2, "FeatureOdom2d", delta, delta_cov);
+ auto c2 = FactorBase::emplace<FactorOdom2d>(f2, f2, F1, nullptr, false);
ASSERT_TRUE(Pr->check(0));
@@ -170,15 +170,15 @@ TEST(Odom2D, FactorFix_and_FactorOdom2D)
ASSERT_TRUE(Pr->getFrameCovariance(F1, P1_solver));
ASSERT_TRUE(Pr->getFrameCovariance(F2, P2_solver));
- ASSERT_POSE2D_APPROX(F0->getState(), Vector3d(0,0,0), 1e-6);
+ ASSERT_POSE2d_APPROX(F0->getState(), Vector3d(0,0,0), 1e-6);
ASSERT_MATRIX_APPROX(P0_solver, P0, 1e-6);
- ASSERT_POSE2D_APPROX(F1->getState(), Vector3d(2,0,0), 1e-6);
+ ASSERT_POSE2d_APPROX(F1->getState(), Vector3d(2,0,0), 1e-6);
ASSERT_MATRIX_APPROX(P1_solver, P1, 1e-6);
- ASSERT_POSE2D_APPROX(F2->getState(), Vector3d(4,0,0), 1e-6);
+ ASSERT_POSE2d_APPROX(F2->getState(), Vector3d(4,0,0), 1e-6);
ASSERT_MATRIX_APPROX(P2_solver, P2, 1e-6);
}
-TEST(Odom2D, VoteForKfAndSolve)
+TEST(Odom2d, VoteForKfAndSolve)
{
std::string wolf_root = _WOLF_ROOT_DIR;
@@ -196,8 +196,8 @@ TEST(Odom2D, VoteForKfAndSolve)
// Create Wolf tree nodes
ProblemPtr problem = Problem::create("PO", 2);
- SensorBasePtr sensor_odom2d = problem->installSensor("SensorOdom2D", "odom", Vector3d(0,0,0), wolf_root + "/test/yaml/sensor_odom_2D.yaml");
- ProcessorParamsOdom2DPtr params(std::make_shared<ProcessorParamsOdom2D>());
+ SensorBasePtr sensor_odom2d = problem->installSensor("SensorOdom2d", "odom", Vector3d(0,0,0), wolf_root + "/test/yaml/sensor_odom_2d.yaml");
+ ProcessorParamsOdom2dPtr params(std::make_shared<ProcessorParamsOdom2d>());
params->voting_active = true;
params->dist_traveled = 100;
params->angle_turned = 6.28;
@@ -205,8 +205,8 @@ TEST(Odom2D, VoteForKfAndSolve)
params->cov_det = 100;
params->unmeasured_perturbation_std = 0.00;
Matrix3d unmeasured_cov = params->unmeasured_perturbation_std*params->unmeasured_perturbation_std*Matrix3d::Identity();
- ProcessorBasePtr prc_base = problem->installProcessor("ProcessorOdom2D", "odom", sensor_odom2d, params);
- ProcessorOdom2DPtr processor_odom2d = std::static_pointer_cast<ProcessorOdom2D>(prc_base);
+ ProcessorBasePtr prc_base = problem->installProcessor("ProcessorOdom2d", "odom", sensor_odom2d, params);
+ ProcessorOdom2dPtr processor_odom2d = std::static_pointer_cast<ProcessorOdom2d>(prc_base);
// NOTE: We integrate and create KFs as follows:
@@ -242,7 +242,7 @@ TEST(Odom2D, VoteForKfAndSolve)
t += dt;
// Capture to use as container for all incoming data
- CaptureMotionPtr capture = std::make_shared<CaptureOdom2D>(t, sensor_odom2d, data, data_cov, nullptr);
+ CaptureMotionPtr capture = std::make_shared<CaptureOdom2d>(t, sensor_odom2d, data, data_cov, nullptr);
for (int n=1; n<=N; n++)
{
@@ -273,7 +273,7 @@ TEST(Odom2D, VoteForKfAndSolve)
WOLF_DEBUG("wolf delta: ", processor_odom2d->getMotion().delta_integr_.transpose());
WOLF_DEBUG("test delta: ", integrated_delta .transpose());
- ASSERT_POSE2D_APPROX(processor_odom2d->getMotion().delta_integr_, integrated_delta, 1e-6);
+ ASSERT_POSE2d_APPROX(processor_odom2d->getMotion().delta_integr_, integrated_delta, 1e-6);
ASSERT_MATRIX_APPROX(odom2d_delta_cov, integrated_delta_cov, 1e-6);
// Integrate pose
@@ -282,7 +282,7 @@ TEST(Odom2D, VoteForKfAndSolve)
integrated_pose = plus(integrated_pose, data);
integrated_cov = Jx * integrated_cov * Jx.transpose() + Ju * data_cov * Ju.transpose() + unmeasured_cov;
- ASSERT_POSE2D_APPROX(processor_odom2d->getCurrentState(), integrated_pose, 1e-6);
+ ASSERT_POSE2d_APPROX(processor_odom2d->getCurrentState(), integrated_pose, 1e-6);
integrated_pose_vector.push_back(integrated_pose);
integrated_cov_vector.push_back(integrated_cov);
@@ -302,14 +302,14 @@ TEST(Odom2D, VoteForKfAndSolve)
{
if (!F->isKey()) break;
- ASSERT_POSE2D_APPROX(F->getState(), integrated_pose_vector[n] , 1e-6);
+ ASSERT_POSE2d_APPROX(F->getState(), integrated_pose_vector[n] , 1e-6);
ASSERT_TRUE (F->getCovariance(computed_cov));
ASSERT_MATRIX_APPROX(computed_cov , integrated_cov_vector[n] , 1e-6);
n += 3;
}
}
-TEST(Odom2D, KF_callback)
+TEST(Odom2d, KF_callback)
{
using std::cout;
using std::endl;
@@ -336,16 +336,16 @@ TEST(Odom2D, KF_callback)
// Create Wolf tree nodes
ProblemPtr problem = Problem::create("PO", 2);
- SensorBasePtr sensor_odom2d = problem->installSensor("SensorOdom2D", "odom", Vector3d(0,0,0), wolf_root + "/test/yaml/sensor_odom_2D.yaml");
- ProcessorParamsOdom2DPtr params(std::make_shared<ProcessorParamsOdom2D>());
+ SensorBasePtr sensor_odom2d = problem->installSensor("SensorOdom2d", "odom", Vector3d(0,0,0), wolf_root + "/test/yaml/sensor_odom_2d.yaml");
+ ProcessorParamsOdom2dPtr params(std::make_shared<ProcessorParamsOdom2d>());
params->dist_traveled = 100;
params->angle_turned = 6.28;
params->max_time_span = 100;
params->cov_det = 100;
params->unmeasured_perturbation_std = 0.000001;
Matrix3d unmeasured_cov = params->unmeasured_perturbation_std*params->unmeasured_perturbation_std*Matrix3d::Identity();
- ProcessorBasePtr prc_base = problem->installProcessor("ProcessorOdom2D", "odom", sensor_odom2d, params);
- ProcessorOdom2DPtr processor_odom2d = std::static_pointer_cast<ProcessorOdom2D>(prc_base);
+ ProcessorBasePtr prc_base = problem->installProcessor("ProcessorOdom2d", "odom", sensor_odom2d, params);
+ ProcessorOdom2dPtr processor_odom2d = std::static_pointer_cast<ProcessorOdom2d>(prc_base);
processor_odom2d->setTimeTolerance(dt/2);
// Ceres wrapper
@@ -371,7 +371,7 @@ TEST(Odom2D, KF_callback)
// std::cout << "\nIntegrating data..." << std::endl;
// Capture to use as container for all incoming data
- auto capture = std::make_shared<CaptureOdom2D>(t, sensor_odom2d, data, data_cov, nullptr);
+ auto capture = std::make_shared<CaptureOdom2d>(t, sensor_odom2d, data, data_cov, nullptr);
std::cout << "t: " << t << std::endl;
for (int n=1; n<=N; n++)
@@ -424,7 +424,7 @@ TEST(Odom2D, KF_callback)
processor_odom2d->keyFrameCallback(keyframe_2, dt/2);
ASSERT_TRUE(problem->check(0));
t += dt;
- capture = std::make_shared<CaptureOdom2D>(t, sensor_odom2d, data, data_cov, nullptr);
+ capture = std::make_shared<CaptureOdom2d>(t, sensor_odom2d, data, data_cov, nullptr);
capture->process();
ASSERT_TRUE(problem->check(0));
@@ -436,7 +436,7 @@ TEST(Odom2D, KF_callback)
// std::cout << report << std::endl;
ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
- ASSERT_POSE2D_APPROX(problem->getLastKeyFrame()->getState() , integrated_pose_vector[n_split], 1e-6);
+ ASSERT_POSE2d_APPROX(problem->getLastKeyFrame()->getState() , integrated_pose_vector[n_split], 1e-6);
MatrixXd computed_cov;
ASSERT_TRUE(problem->getLastKeyFrameCovariance(computed_cov));
ASSERT_MATRIX_APPROX(computed_cov, integrated_cov_vector [n_split], 1e-6);
@@ -456,7 +456,7 @@ TEST(Odom2D, KF_callback)
processor_odom2d->keyFrameCallback(keyframe_1, dt/2);
ASSERT_TRUE(problem->check(0));
t += dt;
- capture = std::make_shared<CaptureOdom2D>(t, sensor_odom2d, data, data_cov, nullptr);
+ capture = std::make_shared<CaptureOdom2d>(t, sensor_odom2d, data, data_cov, nullptr);
capture->process();
ASSERT_TRUE(problem->check(0));
@@ -477,13 +477,13 @@ TEST(Odom2D, KF_callback)
// check the split KF
MatrixXd KF1_cov;
ASSERT_TRUE(problem->getFrameCovariance(keyframe_1, KF1_cov));
- ASSERT_POSE2D_APPROX(keyframe_1->getState(), integrated_pose_vector[m_split], 1e-6);
+ ASSERT_POSE2d_APPROX(keyframe_1->getState(), integrated_pose_vector[m_split], 1e-6);
ASSERT_MATRIX_APPROX(KF1_cov, integrated_cov_vector [m_split], 1e-6);
// check other KF in the future of the split KF
MatrixXd KF2_cov;
ASSERT_TRUE(problem->getFrameCovariance(keyframe_2, KF2_cov));
- ASSERT_POSE2D_APPROX(problem->getLastKeyFrame()->getState(), integrated_pose_vector[n_split], 1e-6);
+ ASSERT_POSE2d_APPROX(problem->getLastKeyFrame()->getState(), integrated_pose_vector[n_split], 1e-6);
ASSERT_MATRIX_APPROX(KF2_cov, integrated_cov_vector [n_split], 1e-6);
// Check full trajectory
@@ -493,14 +493,14 @@ TEST(Odom2D, KF_callback)
t += dt;
WOLF_DEBUG(" estimated(", t, ") = ", problem->getState(t).transpose());
WOLF_DEBUG("ground truth(", t, ") = ", integrated_pose_vector[n].transpose());
- ASSERT_POSE2D_APPROX(problem->getState(t), integrated_pose_vector[n], 1e-6);
+ ASSERT_POSE2d_APPROX(problem->getState(t), integrated_pose_vector[n], 1e-6);
}
}
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
-// testing::GTEST_FLAG(filter) = "Odom2D.VoteForKfAndSolve";
+// testing::GTEST_FLAG(filter) = "Odom2d.VoteForKfAndSolve";
return RUN_ALL_TESTS();
}
diff --git a/test/gtest_pack_KF_buffer.cpp b/test/gtest_pack_KF_buffer.cpp
index 48302e8dd7543a6d20259ddce519800b5d4e9c3d..3862e263cb747b443e77ef113e9d85efa529f3a5 100644
--- a/test/gtest_pack_KF_buffer.cpp
+++ b/test/gtest_pack_KF_buffer.cpp
@@ -7,8 +7,8 @@
//Wolf
#include "core/utils/utils_gtest.h"
-#include "core/processor/processor_odom_2D.h"
-#include "core/sensor/sensor_odom_2D.h"
+#include "core/processor/processor_odom_2d.h"
+#include "core/sensor/sensor_odom_2d.h"
#include "dummy/processor_tracker_feature_dummy.h"
#include "core/capture/capture_void.h"
diff --git a/test/gtest_param_prior.cpp b/test/gtest_param_prior.cpp
index 2c5bcbf1852a48baff40d2cf2f9464aa9157f906..6faf7e424fe24d64bdad85c64c05e0df490da426 100644
--- a/test/gtest_param_prior.cpp
+++ b/test/gtest_param_prior.cpp
@@ -10,7 +10,7 @@
#include "core/problem/problem.h"
#include "core/ceres_wrapper/ceres_manager.h"
-#include "core/sensor/sensor_odom_3D.h"
+#include "core/sensor/sensor_odom_3d.h"
#include <iostream>
@@ -22,7 +22,7 @@ Eigen::Vector7d initial_extrinsics((Eigen::Vector7d() << 1, 2, 3, 1, 0, 0, 0).fi
Eigen::Vector7d prior_extrinsics((Eigen::Vector7d() << 10, 20, 30, 0, 0, 0, 1).finished());
Eigen::Vector7d prior2_extrinsics((Eigen::Vector7d() << 100, 200, 300, 0, 0, 0, 1).finished());
-SensorOdom3DPtr odom_sensor_ptr_ = std::static_pointer_cast<SensorOdom3D>(problem_ptr->installSensor("SensorOdom3D", "odometer", initial_extrinsics, std::make_shared<ParamsSensorOdom3D>()));
+SensorOdom3dPtr odom_sensor_ptr_ = std::static_pointer_cast<SensorOdom3d>(problem_ptr->installSensor("SensorOdom3d", "odometer", initial_extrinsics, std::make_shared<ParamsSensorOdom3d>()));
TEST(ParameterPrior, initial_extrinsics)
{
diff --git a/test/gtest_param_server.cpp b/test/gtest_param_server.cpp
index 18f6b1eded0955faf7c6b823e25bbde62894203d..1f1f468360e1aa5073606f8ea32e7d82cafbc438 100644
--- a/test/gtest_param_server.cpp
+++ b/test/gtest_param_server.cpp
@@ -1,8 +1,8 @@
#include "core/utils/utils_gtest.h"
#include "core/utils/converter.h"
#include "core/common/wolf.h"
-#include "core/yaml/parser_yaml.hpp"
-#include "core/utils/params_server.hpp"
+#include "core/yaml/parser_yaml.h"
+#include "core/utils/params_server.h"
using namespace std;
using namespace wolf;
diff --git a/test/gtest_parser_yaml.cpp b/test/gtest_parser_yaml.cpp
index d464c1120e6830c9912d0f16472c53f74b4f9bac..de5164ec6a9db61ad8a0344e8e6ee6d6e52dec16 100644
--- a/test/gtest_parser_yaml.cpp
+++ b/test/gtest_parser_yaml.cpp
@@ -1,7 +1,7 @@
#include "core/utils/utils_gtest.h"
#include "core/utils/converter.h"
#include "core/common/wolf.h"
-#include "core/yaml/parser_yaml.hpp"
+#include "core/yaml/parser_yaml.h"
using namespace std;
using namespace wolf;
@@ -35,7 +35,7 @@ TEST(ParserYAML, FollowFile)
EXPECT_EQ(params[processor_prefix + "my_proc_test/max_buff_length"], "100");
EXPECT_EQ(params[processor_prefix + "my_proc_test/voting_active"], "false");
}
-TEST(ParserYAML, FollowOdom3D)
+TEST(ParserYAML, FollowOdom3d)
{
auto parser = ParserYAML("test/yaml/params1.yaml", wolf_root);
auto params = parser.getParams();
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index a7cae940ef8b6524396bb7d5ae3e5c8388b0cb50..6d262ba06b81438499b29ab3d45fd4bb101c89db 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -10,12 +10,21 @@
#include "core/problem/problem.h"
#include "core/sensor/sensor_base.h"
-#include "core/sensor/sensor_odom_2D.h"
-#include "core/sensor/sensor_odom_3D.h"
-#include "core/processor/processor_odom_3D.h"
+#include "core/sensor/sensor_odom_2d.h"
+#include "core/sensor/sensor_odom_3d.h"
+#include "core/processor/processor_odom_3d.h"
#include "dummy/processor_tracker_feature_dummy.h"
#include "core/solver/solver_manager.h"
+#include "core/sensor/sensor_diff_drive.h"
+#include "core/processor/processor_diff_drive.h"
+#include "core/capture/capture_diff_drive.h"
+//#include "core/feature/feature_diff_drive.h"
+#include "core/factor/factor_diff_drive.h"
+
+#include "core/state_block/state_quaternion.h"
+
+
#include <iostream>
using namespace wolf;
@@ -92,10 +101,10 @@ TEST(Problem, Processor)
ASSERT_FALSE(P->getProcessorIsMotion());
// add a motion sensor and processor
- auto Sm = SensorBase::emplace<SensorOdom3D>(P->getHardware(), (Eigen::Vector7d()<<0,0,0, 0,0,0,1).finished(), ParamsSensorOdom3D());
+ auto Sm = SensorBase::emplace<SensorOdom3d>(P->getHardware(), (Eigen::Vector7d()<<0,0,0, 0,0,0,1).finished(), ParamsSensorOdom3d());
// add motion processor
- auto Pm = ProcessorBase::emplace<ProcessorOdom3D>(Sm, std::make_shared<ProcessorParamsOdom3D>());
+ auto Pm = ProcessorBase::emplace<ProcessorOdom3d>(Sm, std::make_shared<ProcessorParamsOdom3d>());
// check motion processor IS NOT by emplace
ASSERT_EQ(P->getProcessorIsMotion(), Pm);
@@ -107,7 +116,7 @@ TEST(Problem, Installers)
ProblemPtr P = Problem::create("PO", 3);
Eigen::Vector7d xs;
- SensorBasePtr S = P->installSensor ("SensorOdom3D", "odometer", xs, wolf_root + "/test/yaml/sensor_odom_3D.yaml");
+ SensorBasePtr S = P->installSensor ("SensorOdom3d", "odometer", xs, wolf_root + "/test/yaml/sensor_odom_3d.yaml");
// install processor tracker (dummy installation under an Odometry sensor -- it's OK for this test)
auto pt = P->installProcessor("ProcessorTrackerFeatureDummy", "dummy", "odometer");
@@ -116,7 +125,7 @@ TEST(Problem, Installers)
ASSERT_FALSE(P->getProcessorIsMotion());
// install processor motion
- ProcessorBasePtr pm = P->installProcessor("ProcessorOdom3D", "odom integrator", "odometer", wolf_root + "/test/yaml/processor_odom_3D.yaml");
+ ProcessorBasePtr pm = P->installProcessor("ProcessorOdom3d", "odom integrator", "odometer", wolf_root + "/test/yaml/processor_odom_3d.yaml");
// check motion processor is set
ASSERT_TRUE(P->getProcessorIsMotion() != nullptr);
@@ -125,7 +134,7 @@ TEST(Problem, Installers)
ASSERT_EQ(std::dynamic_pointer_cast<ProcessorMotion>(P->getProcessorIsMotion()), pm);
}
-TEST(Problem, SetOrigin_PO_2D)
+TEST(Problem, SetOrigin_PO_2d)
{
ProblemPtr P = Problem::create("PO", 2);
TimeStamp t0(0);
@@ -162,7 +171,7 @@ TEST(Problem, SetOrigin_PO_2D)
// P->print(4,1,1,1);
}
-TEST(Problem, SetOrigin_PO_3D)
+TEST(Problem, SetOrigin_PO_3d)
{
ProblemPtr P = Problem::create("PO", 3);
TimeStamp t0(0);
@@ -207,13 +216,13 @@ TEST(Problem, emplaceFrame_factory)
FrameBasePtr f1 = P->emplaceFrame("PO", 3, KEY, VectorXd(7), TimeStamp(1.0));
FrameBasePtr f2 = P->emplaceFrame("POV", 3, KEY, VectorXd(10), TimeStamp(2.0));
- // std::cout << "f0: type PO 2D? " << f0->getType() << std::endl;
- // std::cout << "f1: type PO 3D? " << f1->getType() << std::endl;
- // std::cout << "f2: type POV 3D? " << f2->getType() << std::endl;
+ // std::cout << "f0: type PO 2d? " << f0->getType() << std::endl;
+ // std::cout << "f1: type PO 3d? " << f1->getType() << std::endl;
+ // std::cout << "f2: type POV 3d? " << f2->getType() << std::endl;
- ASSERT_EQ(f0->getType().compare("PO 2D"), 0);
- ASSERT_EQ(f1->getType().compare("PO 3D"), 0);
- ASSERT_EQ(f2->getType().compare("POV 3D"), 0);
+ ASSERT_EQ(f0->getType().compare("PO 2d"), 0);
+ ASSERT_EQ(f1->getType().compare("PO 3d"), 0);
+ ASSERT_EQ(f2->getType().compare("POV 3d"), 0);
// check that all frames are effectively in the trajectory
ASSERT_EQ(P->getTrajectory()->getFrameList().size(), (SizeStd) 3);
@@ -233,15 +242,15 @@ TEST(Problem, StateBlocks)
Eigen::Vector3d xs2d;
// 2 state blocks, fixed
- SensorBasePtr Sm = P->installSensor ("SensorOdom3D", "odometer",xs3d, wolf_root + "/test/yaml/sensor_odom_3D.yaml");
+ SensorBasePtr Sm = P->installSensor ("SensorOdom3d", "odometer",xs3d, wolf_root + "/test/yaml/sensor_odom_3d.yaml");
ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 2);
// // 2 state blocks, fixed
-// SensorBasePtr St = P->installSensor ("SensorOdom2D", "other odometer", xs2d, "");
+// SensorBasePtr St = P->installSensor ("SensorOdom2d", "other odometer", xs2d, "");
// ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) (2 + 2));
auto pt = P->installProcessor("ProcessorTrackerFeatureDummy", "dummy", "odometer");
- auto pm = P->installProcessor("ProcessorOdom3D", "odom integrator", "odometer", wolf_root + "/test/yaml/processor_odom_3D.yaml");
+ auto pm = P->installProcessor("ProcessorOdom3d", "odom integrator", "odometer", wolf_root + "/test/yaml/processor_odom_3d.yaml");
// 2 state blocks, estimated
auto KF = P->emplaceFrame("PO", 3, KEY, xs3d, 0);
@@ -287,8 +296,9 @@ TEST(Problem, Covariances)
Eigen::Vector7d xs3d;
Eigen::Vector3d xs2d;
- SensorBasePtr Sm = P->installSensor ("SensorOdom3D", "odometer", xs3d, wolf_root + "/test/yaml/sensor_odom_3D.yaml");
- SensorBasePtr St = P->installSensor ("SensorOdom2D", "other odometer", xs2d, wolf_root + "/test/yaml/sensor_odom_2D.yaml");
+ SensorBasePtr Sm = P->installSensor ("SensorOdom3d", "odometer", xs3d, wolf_root + "/test/yaml/sensor_odom_3d.yaml");
+ // SensorBasePtr St = P->installSensor ("SensorOdom2d", "other odometer", xs2d, wolf_root + "/test/yaml/sensor_odom_2d.yaml");
+ SensorBasePtr St = P->installSensor ("SensorOdom3d", "other odometer", xs3d, wolf_root + "/test/yaml/sensor_odom_3d.yaml");
ProcessorParamsTrackerFeaturePtr params = std::make_shared<ProcessorParamsTrackerFeature>();
params->time_tolerance = 0.1;
@@ -296,7 +306,7 @@ TEST(Problem, Covariances)
params->min_features_for_keyframe = 10;
auto pt = P->installProcessor("ProcessorTrackerFeatureDummy", "dummy", Sm, params);
- auto pm = P->installProcessor("ProcessorOdom3D", "odom integrator", "other odometer", wolf_root + "/test/yaml/processor_odom_3D.yaml");
+ auto pm = P->installProcessor("ProcessorOdom3d", "odom integrator", "other odometer", wolf_root + "/test/yaml/processor_odom_3d.yaml");
// 4 state blocks, estimated
St->unfixExtrinsics();
@@ -317,6 +327,160 @@ TEST(Problem, Covariances)
}
+TEST(Problem, perturb)
+{
+ auto problem = Problem::create("PO", 2);
+
+ // make a sensor first
+ auto intr = std::make_shared<ParamsSensorDiffDrive>();
+ intr->radius_left = 1.0;
+ intr->radius_right = 1.0;
+ intr->wheel_separation = 1.0;
+ intr->ticks_per_wheel_revolution = 100;
+ Vector3d extr(0,0,0);
+ auto sensor = std::static_pointer_cast<SensorDiffDrive>(problem->installSensor("SensorDiffDrive", "sensor diff drive", extr, intr));
+ sensor->setStateBlockDynamic("I", true);
+
+ Vector3d pose; pose << 0,0,0;
+
+ int i = 0;
+ for (TimeStamp t = 0.0; t < 2.0; t += 1.0)
+ {
+ auto F = problem->emplaceFrame(KEY, pose, t);
+ if (i==0) F->fix();
+
+ for (int j = 0; j< 2 ; j++)
+ {
+ auto sb = std::make_shared<StateBlock>(Vector3d(1,1,1));
+ auto cap = CaptureBase::emplace<CaptureBase>(F, "CaptureBase", t, sensor, nullptr, nullptr, sb);
+ }
+ i++;
+ }
+
+ for (int l = 0; l < 2; l++)
+ {
+ auto sb1 = std::make_shared<StateBlock>(Vector2d(1,2));
+ auto sb2 = std::make_shared<StateBlock>(Vector1d(3));
+ auto L = LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkBase", sb1, sb2);
+ if (l==0) L->fix();
+ }
+
+ /// PRINT, PERTURB, AND PRINT AGAIN
+ problem->print(2,0,1,1);
+
+ problem->perturb(0.2);
+
+ problem->print(2,0,1,1);
+
+
+ //// CHECK ALL STATE BLOCKS ///
+
+ double prec = 1e-2;
+
+ for (auto S : problem->getHardware()->getSensorList())
+ {
+ ASSERT_TRUE (S->getP()->getState().isApprox(Vector2d(0,0), prec) );
+ ASSERT_TRUE (S->getO()->getState().isApprox(Vector1d(0), prec) );
+ ASSERT_FALSE(S->getIntrinsic()->getState().isApprox(Vector3d(1,1,1), prec) );
+ }
+
+ for (auto F : problem->getTrajectory()->getFrameList())
+ {
+ if (F->isFixed()) // fixed
+ {
+ ASSERT_TRUE (F->getP()->getState().isApprox(Vector2d(0,0), prec) );
+ ASSERT_TRUE (F->getO()->getState().isApprox(Vector1d(0), prec) );
+ }
+ else
+ {
+ ASSERT_FALSE(F->getP()->getState().isApprox(Vector2d(0,0), prec) );
+ ASSERT_FALSE(F->getO()->getState().isApprox(Vector1d(0), prec) );
+ }
+ for (auto C : F->getCaptureList())
+ {
+ // all are estimated
+ ASSERT_FALSE(C->getSensorIntrinsic()->getState().isApprox(Vector3d(1,1,1), prec) );
+ }
+ }
+ for (auto L : problem->getMap()->getLandmarkList())
+ {
+ if ( L->isFixed() ) // fixed
+ {
+ ASSERT_TRUE (L->getP()->getState().isApprox(Vector2d(1,2), prec) );
+ ASSERT_TRUE (L->getO()->getState().isApprox(Vector1d(3), prec) );
+ }
+ else
+ {
+ ASSERT_FALSE(L->getP()->getState().isApprox(Vector2d(1,2), prec) );
+ ASSERT_FALSE(L->getO()->getState().isApprox(Vector1d(3), prec) );
+ }
+ }
+}
+
+TEST(Problem, check)
+{
+ auto problem = Problem::create("PO", 2);
+
+ // make a sensor first
+ auto intr = std::make_shared<ParamsSensorDiffDrive>();
+ intr->radius_left = 1.0;
+ intr->radius_right = 1.0;
+ intr->wheel_separation = 1.0;
+ intr->ticks_per_wheel_revolution = 100;
+ Vector3d extr(0,0,0);
+ auto sensor = std::static_pointer_cast<SensorDiffDrive>(problem->installSensor("SensorDiffDrive", "sensor diff drive", extr, intr));
+ sensor->setStateBlockDynamic("I", true);
+
+ Vector3d pose; pose << 0,0,0;
+
+ int i = 0;
+ for (TimeStamp t = 0.0; t < 2.0; t += 1.0)
+ {
+ auto F = problem->emplaceFrame(KEY, pose, t);
+ if (i==0) F->fix();
+
+ for (int j = 0; j< 2 ; j++)
+ {
+ auto sb = std::make_shared<StateBlock>(Vector3d(1,1,1));
+ auto cap = CaptureBase::emplace<CaptureDiffDrive>(F, t, sensor, Vector2d(1,2), Matrix2d::Identity(), nullptr, nullptr, nullptr, sb);
+
+ for (int k = 0; k<2; k++)
+ {
+ Vector3d d(1,2,3), c(1,1,1);
+ Matrix3d D = Matrix3d::Identity(), J=Matrix3d::Zero();
+ auto fea = FeatureBase::emplace<FeatureMotion>(cap, "FeatureDiffDrive", d, D, c, J);
+ auto fac = FactorBase::emplace<FactorDiffDrive>(fea, fea, cap, nullptr, false);
+ }
+ }
+ i++;
+ }
+
+ for (int l = 0; l < 2; l++)
+ {
+ auto sb1 = std::make_shared<StateBlock>(Vector2d(1,2));
+ auto sb2 = std::make_shared<StateBlock>(Vector1d(3));
+ auto L = LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkBase", sb1, sb2);
+ if (l==0) L->fix();
+ }
+
+ ASSERT_TRUE(problem->check(true, std::cout));
+
+ // remove stuff from problem, and re-check
+
+ auto F = problem->getLastKeyFrame();
+
+ auto cby = F->getConstrainedByList().front();
+
+ cby->remove(true);
+
+ ASSERT_TRUE(problem->check(true, std::cout));
+
+ F->remove();
+
+ ASSERT_TRUE(problem->check(true, std::cout));
+}
+
+
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp
index 983e2e127d32e46cd990440865050aed6f8dfe7e..c4a1c4b0470c92bf09df1a950a1c19e57d35642d 100644
--- a/test/gtest_processor_base.cpp
+++ b/test/gtest_processor_base.cpp
@@ -8,8 +8,8 @@
//Wolf
#include "core/utils/utils_gtest.h"
-#include "core/processor/processor_odom_2D.h"
-#include "core/sensor/sensor_odom_2D.h"
+#include "core/processor/processor_odom_2d.h"
+#include "core/sensor/sensor_odom_2d.h"
#include "dummy/processor_tracker_feature_dummy.h"
#include "core/capture/capture_void.h"
@@ -56,10 +56,10 @@ TEST(ProcessorBase, IsMotion)
auto proc_trk = problem->installProcessor("ProcessorTrackerFeatureDummy", "dummy", sens_trk);
// Install odometer (sensor and processor)
- SensorBasePtr sens_odo = problem->installSensor("SensorOdom2D", "odometer", Vector3d(0,0,0), wolf_root + "/test/yaml/sensor_odom_2D.yaml");
- ProcessorParamsOdom2DPtr proc_odo_params = make_shared<ProcessorParamsOdom2D>();
+ SensorBasePtr sens_odo = problem->installSensor("SensorOdom2d", "odometer", Vector3d(0,0,0), wolf_root + "/test/yaml/sensor_odom_2d.yaml");
+ ProcessorParamsOdom2dPtr proc_odo_params = make_shared<ProcessorParamsOdom2d>();
proc_odo_params->time_tolerance = dt/2;
- ProcessorBasePtr proc_odo = problem->installProcessor("ProcessorOdom2D", "odom processor", sens_odo, proc_odo_params);
+ ProcessorBasePtr proc_odo = problem->installProcessor("ProcessorOdom2d", "odom processor", sens_odo, proc_odo_params);
ASSERT_FALSE(proc_trk->isMotion());
ASSERT_TRUE (proc_odo->isMotion());
@@ -84,17 +84,17 @@ TEST(ProcessorBase, KeyFrameCallback)
// Install tracker (sensor and processor)
auto sens_trk = SensorBase::emplace<SensorBase>(problem->getHardware(),
- "SensorOdom2D",
+ "SensorOdom2d",
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);
// Install odometer (sensor and processor)
- SensorBasePtr sens_odo = problem->installSensor("SensorOdom2D", "odometer", Vector3d(0,0,0), wolf_root + "/test/yaml/sensor_odom_2D.yaml");
- ProcessorParamsOdom2DPtr proc_odo_params = make_shared<ProcessorParamsOdom2D>();
+ SensorBasePtr sens_odo = problem->installSensor("SensorOdom2d", "odometer", Vector3d(0,0,0), wolf_root + "/test/yaml/sensor_odom_2d.yaml");
+ ProcessorParamsOdom2dPtr proc_odo_params = make_shared<ProcessorParamsOdom2d>();
proc_odo_params->time_tolerance = dt/2;
- ProcessorBasePtr proc_odo = problem->installProcessor("ProcessorOdom2D", "odom processor", sens_odo, proc_odo_params);
+ ProcessorBasePtr proc_odo = problem->installProcessor("ProcessorOdom2d", "odom processor", sens_odo, proc_odo_params);
std::cout << "sensor & processor created and added to wolf problem" << std::endl;
@@ -106,7 +106,7 @@ TEST(ProcessorBase, KeyFrameCallback)
Matrix3d P = Matrix3d::Identity() * 0.1;
problem->setPrior(x, P, t, dt/2); // KF1
- CaptureOdom2DPtr capt_odo = make_shared<CaptureOdom2D>(t, sens_odo, Vector2d(0.5,0));
+ CaptureOdom2dPtr capt_odo = make_shared<CaptureOdom2d>(t, sens_odo, Vector2d(0.5,0));
// Track
CaptureVoidPtr capt_trk(make_shared<CaptureVoid>(t, sens_trk));
@@ -128,7 +128,7 @@ TEST(ProcessorBase, KeyFrameCallback)
problem->print(4,1,1,0);
// Only odom creating KFs
- ASSERT_TRUE( problem->getLastKeyFrame()->getType().compare("PO 2D")==0 );
+ ASSERT_TRUE( problem->getLastKeyFrame()->getType().compare("PO 2d")==0 );
}
}
diff --git a/test/gtest_processor_loopclosure.cpp b/test/gtest_processor_loopclosure.cpp
index 01ead38abbb2e208964ca0957625a8dfd9a22eac..d00fb93407ab75bb150a576bc434640f1c75fe12 100644
--- a/test/gtest_processor_loopclosure.cpp
+++ b/test/gtest_processor_loopclosure.cpp
@@ -24,7 +24,7 @@ private:
public:
ProcessorLoopClosureDummy(ProcessorParamsLoopClosurePtr _params_loop_closure, bool& factor_created):
- ProcessorLoopClosure("LOOP CLOSURE DUMMY", _params_loop_closure),
+ ProcessorLoopClosure("LOOP CLOSURE DUMMY", 0, _params_loop_closure),
factor_created(&factor_created){};
std::pair<FrameBasePtr,CaptureBasePtr> public_selectPairKC(){ return selectPairKC();};
diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp
index 3689babf376af854b35f9c836f9b4e804c2504e9..076118ba3d7836dbf61cdb2ed1d38d81d46b9084 100644
--- a/test/gtest_processor_motion.cpp
+++ b/test/gtest_processor_motion.cpp
@@ -10,49 +10,49 @@
#include "core/common/wolf.h"
#include "core/utils/logging.h"
-#include "core/sensor/sensor_odom_2D.h"
-#include "core/processor/processor_odom_2D.h"
+#include "core/sensor/sensor_odom_2d.h"
+#include "core/processor/processor_odom_2d.h"
#include "core/ceres_wrapper/ceres_manager.h"
using namespace Eigen;
using namespace wolf;
using std::static_pointer_cast;
-class ProcessorOdom2DPublic : public ProcessorOdom2D
+class ProcessorOdom2dPublic : public ProcessorOdom2d
{
public:
- ProcessorOdom2DPublic(ProcessorParamsOdom2DPtr params) : ProcessorOdom2D(params)
+ ProcessorOdom2dPublic(ProcessorParamsOdom2dPtr params) : ProcessorOdom2d(params)
{
//
}
- virtual ~ProcessorOdom2DPublic(){}
+ virtual ~ProcessorOdom2dPublic(){}
void splitBuffer(const wolf::CaptureMotionPtr& capture_source,
TimeStamp ts_split,
const FrameBasePtr& keyframe_target,
const wolf::CaptureMotionPtr& capture_target)
{
- ProcessorOdom2D::splitBuffer(capture_source,
+ ProcessorOdom2d::splitBuffer(capture_source,
ts_split,
keyframe_target,
capture_target);
}
};
-WOLF_PTR_TYPEDEFS(ProcessorOdom2DPublic);
+WOLF_PTR_TYPEDEFS(ProcessorOdom2dPublic);
class ProcessorMotion_test : public testing::Test{
public:
double dt;
ProblemPtr problem;
- SensorOdom2DPtr sensor;
- ProcessorOdom2DPublicPtr processor;
+ SensorOdom2dPtr sensor;
+ ProcessorOdom2dPublicPtr processor;
CaptureMotionPtr capture;
Vector2d data;
Matrix2d data_cov;
// ProcessorMotion_test() :
-// ProcessorOdom2D(std::make_shared<ProcessorParamsOdom2D>()),
+// ProcessorOdom2d(std::make_shared<ProcessorParamsOdom2d>()),
// dt(0)
// { }
@@ -62,16 +62,16 @@ class ProcessorMotion_test : public testing::Test{
dt = 1.0;
problem = Problem::create("PO", 2);
- sensor = static_pointer_cast<SensorOdom2D>(problem->installSensor("SensorOdom2D", "odom", Vector3d(0,0,0), wolf_root + "/test/yaml/sensor_odom_2D.yaml"));
- ProcessorParamsOdom2DPtr params(std::make_shared<ProcessorParamsOdom2D>());
+ sensor = static_pointer_cast<SensorOdom2d>(problem->installSensor("SensorOdom2d", "odom", Vector3d(0,0,0), wolf_root + "/test/yaml/sensor_odom_2d.yaml"));
+ ProcessorParamsOdom2dPtr params(std::make_shared<ProcessorParamsOdom2d>());
params->time_tolerance = 0.25;
params->dist_traveled = 100;
params->angle_turned = 6.28;
params->max_time_span = 2.5*dt; // force KF at every third process()
params->cov_det = 100;
params->unmeasured_perturbation_std = 0.001;
- processor = ProcessorBase::emplace<ProcessorOdom2DPublic>(sensor, params);
- capture = std::make_shared<CaptureMotion>("CaptureOdom2D", 0.0, sensor, data, data_cov, 3, 3, nullptr);
+ processor = ProcessorBase::emplace<ProcessorOdom2dPublic>(sensor, params);
+ capture = std::make_shared<CaptureMotion>("CaptureOdom2d", 0.0, sensor, data, data_cov, 3, 3, nullptr);
Vector3d x0; x0 << 0, 0, 0;
Matrix3d P0; P0.setIdentity();
@@ -142,7 +142,7 @@ TEST_F(ProcessorMotion_test, splitBuffer)
FrameBasePtr F_target = problem->emplaceFrame(KEY, t_target);
CaptureMotionPtr C_source = std::dynamic_pointer_cast<CaptureMotion>(processor->getLast());
CaptureMotionPtr C_target = CaptureBase::emplace<CaptureMotion>(F_target,
- "ODOM 2D",
+ "ODOM 2d",
t_target,
sensor,
data,
diff --git a/test/gtest_processor_odom_3D.cpp b/test/gtest_processor_odom_3d.cpp
similarity index 85%
rename from test/gtest_processor_odom_3D.cpp
rename to test/gtest_processor_odom_3d.cpp
index c683105f70502a30fbadf15dbe40203388a4f44b..24e5e875ea0b5775747c7325239884a586f0a0c4 100644
--- a/test/gtest_processor_odom_3D.cpp
+++ b/test/gtest_processor_odom_3d.cpp
@@ -1,5 +1,5 @@
/**
- * \file gtest_odom_3D.cpp
+ * \file gtest_odom_3d.cpp
*
* Created on: Nov 11, 2016
* \author: jsola
@@ -10,7 +10,7 @@
#include "core/common/wolf.h"
#include "core/utils/logging.h"
-#include "core/processor/processor_odom_3D.h"
+#include "core/processor/processor_odom_3d.h"
#include <iostream>
@@ -38,12 +38,12 @@ using namespace Eigen;
using namespace std;
using namespace wolf;
-/** Gain access to members of ProcessorOdom3D
+/** Gain access to members of ProcessorOdom3d
*/
-class ProcessorOdom3DTest : public ProcessorOdom3D
+class ProcessorOdom3dTest : public ProcessorOdom3d
{
public:
- ProcessorOdom3DTest();
+ ProcessorOdom3dTest();
// getters :-D !!
double& kdd() {return k_disp_to_disp_;}
@@ -52,16 +52,16 @@ class ProcessorOdom3DTest : public ProcessorOdom3D
double& dvar_min() {return min_disp_var_;}
double& rvar_min() {return min_rot_var_;}
};
-ProcessorOdom3DTest::ProcessorOdom3DTest() : ProcessorOdom3D(std::make_shared<ProcessorParamsOdom3D>())
+ProcessorOdom3dTest::ProcessorOdom3dTest() : ProcessorOdom3d(std::make_shared<ProcessorParamsOdom3d>())
{
dvar_min() = 0.5;
rvar_min() = 0.25;
}
-TEST(ProcessorOdom3D, computeCurrentDelta)
+TEST(ProcessorOdom3d, computeCurrentDelta)
{
// One instance of the processor to test
- ProcessorOdom3DTest prc;
+ ProcessorOdom3dTest prc;
// input data
Vector6d data; data.setRandom();
@@ -98,9 +98,9 @@ TEST(ProcessorOdom3D, computeCurrentDelta)
}
-TEST(ProcessorOdom3D, deltaPlusDelta)
+TEST(ProcessorOdom3d, deltaPlusDelta)
{
- ProcessorOdom3DTest prc;
+ ProcessorOdom3dTest prc;
VectorXd D(7); D.setRandom(); D.tail<4>().normalize();
VectorXd d(7); d.setRandom(); d *= 1; d.tail<4>().normalize();
@@ -112,7 +112,7 @@ TEST(ProcessorOdom3D, deltaPlusDelta)
D_int_check.head<3>() = D.head<3>() + Quaterniond(D.data()+3) * d.head<3>();
D_int_check.tail<4>() = (Quaterniond(D.data()+3) * Quaterniond(d.data()+3)).coeffs();
- double dt = 1; // dummy, not used in Odom3D
+ double dt = 1; // dummy, not used in Odom3d
VectorXd D_int(7);
@@ -123,9 +123,9 @@ TEST(ProcessorOdom3D, deltaPlusDelta)
// << "\ncheck: " << D_int_check.transpose();
}
-TEST(ProcessorOdom3D, deltaPlusDelta_Jac)
+TEST(ProcessorOdom3d, deltaPlusDelta_Jac)
{
- std::shared_ptr<ProcessorOdom3DTest> prc_ptr = std::make_shared<ProcessorOdom3DTest>();
+ std::shared_ptr<ProcessorOdom3dTest> prc_ptr = std::make_shared<ProcessorOdom3dTest>();
VectorXd D(7); D.setRandom(); D.tail<4>().normalize();
VectorXd d(7); d.setRandom(); d *= 1; d.tail<4>().normalize();
diff --git a/test/gtest_processor_tracker_feature_dummy.cpp b/test/gtest_processor_tracker_feature_dummy.cpp
index 8e1b0d8c7f6c6a16fbe4648dc035ee65c6c845b5..c8bf458bbcdc5058862accd76beedcb13b1d011f 100644
--- a/test/gtest_processor_tracker_feature_dummy.cpp
+++ b/test/gtest_processor_tracker_feature_dummy.cpp
@@ -82,7 +82,7 @@ class ProcessorTrackerFeatureDummyTest : public testing::Test
problem = Problem::create("PO", 2);
// Install camera
- sensor = problem->installSensor("SensorOdom2D", "auxiliar sensor", (Eigen::Vector3d() << 0,0,0).finished(), std::make_shared<ParamsSensorBase>());
+ sensor = problem->installSensor("SensorOdom2d", "auxiliar sensor", (Eigen::Vector3d() << 0,0,0).finished(), std::make_shared<ParamsSensorBase>());
// Install processor
params = std::make_shared<ProcessorParamsTrackerFeatureDummy>();
diff --git a/test/gtest_processor_tracker_landmark_dummy.cpp b/test/gtest_processor_tracker_landmark_dummy.cpp
index b397a512415c6643985c57ac551bceb6e5400994..a75ba7ffb962509468343f66e894a1b527610c04 100644
--- a/test/gtest_processor_tracker_landmark_dummy.cpp
+++ b/test/gtest_processor_tracker_landmark_dummy.cpp
@@ -100,7 +100,7 @@ class ProcessorTrackerLandmarkDummyTest : public testing::Test
problem = Problem::create("PO", 2);
// Install camera
- sensor = problem->installSensor("SensorOdom2D", "auxiliar sensor", (Eigen::Vector3d() << 0,0,0).finished(), std::make_shared<ParamsSensorBase>());
+ sensor = problem->installSensor("SensorOdom2d", "auxiliar sensor", (Eigen::Vector3d() << 0,0,0).finished(), std::make_shared<ParamsSensorBase>());
// Install processor
params = std::make_shared<ProcessorParamsTrackerLandmarkDummy>();
diff --git a/test/gtest_rotation.cpp b/test/gtest_rotation.cpp
index d370d52874b269d0828b34524a10947694806f52..a8a0efd094462347f379bf4507d8dfa38e591038 100644
--- a/test/gtest_rotation.cpp
+++ b/test/gtest_rotation.cpp
@@ -687,6 +687,30 @@ TEST(Conversions, q2R_R2q)
ASSERT_MATRIX_APPROX(R, qq_R.matrix(), wolf::Constants::EPS);
}
+TEST(Conversions, R2q_norm_of_q)
+{
+ Isometry3d T = Translation3d(1,2,3) * AngleAxisd(M_PI_4, Vector3d(1,2,3).normalized());
+ Eigen::Vector4d cqt = R2q(T.linear()).coeffs();
+ Eigen::Matrix3d R = T.linear();
+ Eigen::Vector4d cqt2 = R2q(R).coeffs();
+ std::cout << "cqt: " << cqt.transpose() << std::endl;
+ std::cout << "cqt2: " << cqt2.transpose() << std::endl;
+ std::cout << "cqt.norm(): " << cqt.norm() << std::endl;
+ std::cout << "cqt2.norm(): " << cqt2.norm() << std::endl;
+
+ ASSERT_NEAR(cqt.norm(), 1, 1e-8);
+ ASSERT_NEAR(cqt2.norm(), 1, 1e-8);
+
+ // Theck T.linear() vs T.rotation()
+ T.linear().setRandom();
+
+ Matrix3d TL = T.linear();
+ Matrix3d TR = T.rotation();
+
+ WOLF_INFO("TL = ", TL);
+ WOLF_INFO("TR = ", TR);
+}
+
TEST(Conversions, e2q_q2e)
{
Vector3d e, eo;
@@ -768,5 +792,6 @@ int main(int argc, char **argv)
*/
testing::InitGoogleTest(&argc, argv);
+ ::testing::GTEST_FLAG(filter) = "Conversions.R2q_norm_of_q";
return RUN_ALL_TESTS();
}
diff --git a/test/gtest_solver_manager.cpp b/test/gtest_solver_manager.cpp
index b9472ba1995d7e60444048942c48f5085fcf253e..640dc71faefcdfcaa937bdc928c904ef618f05d9 100644
--- a/test/gtest_solver_manager.cpp
+++ b/test/gtest_solver_manager.cpp
@@ -12,7 +12,7 @@
#include "core/sensor/sensor_base.h"
#include "core/state_block/state_block.h"
#include "core/capture/capture_void.h"
-#include "core/factor/factor_pose_2D.h"
+#include "core/factor/factor_pose_2d.h"
#include "core/solver/solver_manager.h"
#include "core/state_block/local_parametrization_base.h"
#include "core/state_block/local_parametrization_angle.h"
@@ -517,8 +517,8 @@ TEST(SolverManager, AddFactor)
// Create (and add) factor point 2d
FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2D", Vector3d::Zero(), Matrix3d::Identity());
- FactorPose2DPtr c = FactorBase::emplace<FactorPose2D>(f, f, nullptr, false);
+ auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
+ FactorPose2dPtr c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false);
// notification
Notification notif;
@@ -544,8 +544,8 @@ TEST(SolverManager, RemoveFactor)
// Create (and add) factor point 2d
FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2D", Vector3d::Zero(), Matrix3d::Identity());
- FactorPose2DPtr c = FactorBase::emplace<FactorPose2D>(f, f, nullptr, false);
+ auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
+ FactorPose2dPtr c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false);
// update solver
solver_manager_ptr->update();
@@ -578,8 +578,8 @@ TEST(SolverManager, AddRemoveFactor)
FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2D", Vector3d::Zero(), Matrix3d::Identity());
- FactorPose2DPtr c = FactorBase::emplace<FactorPose2D>(f, f, nullptr, false);
+ auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
+ FactorPose2dPtr c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false);
// notification
Notification notif;
@@ -613,8 +613,8 @@ TEST(SolverManager, DoubleRemoveFactor)
FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2D", Vector3d::Zero(), Matrix3d::Identity());
- FactorPose2DPtr c = FactorBase::emplace<FactorPose2D>(f, f, nullptr, false);
+ auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
+ FactorPose2dPtr c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false);
// update solver
solver_manager_ptr->update();
diff --git a/test/gtest_state_block.cpp b/test/gtest_state_block.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..19d9e3cf63623ef275ee5f8fc8111f4e88ece4d1
--- /dev/null
+++ b/test/gtest_state_block.cpp
@@ -0,0 +1,69 @@
+/*
+ * gtest_state_block.cpp
+ *
+ * Created on: Mar 31, 2020
+ * Author: jsola
+ */
+
+#include "core/utils/utils_gtest.h"
+#include "core/utils/logging.h"
+
+#include "core/state_block/state_block.h"
+#include "core/state_block/state_quaternion.h"
+#include "core/state_block/state_angle.h"
+
+#include <iostream>
+
+
+using namespace Eigen;
+using namespace std;
+using namespace wolf;
+
+TEST(StateBlock, block_perturb)
+{
+ Vector3d x(10,20,30);
+ StateBlock sb(x);
+
+ sb.perturb(0.5);
+
+ WOLF_INFO("original ", x.transpose(), ", perturbed = ", sb.getState().transpose());
+
+ ASSERT_NE(x.norm(), sb.getState().norm());
+ ASSERT_MATRIX_APPROX(x , sb.getState() , 0.5 * 4); // 4-sigma test...
+}
+
+TEST(StateBlock, angle_perturb)
+{
+ Vector1d x(1.0);
+ StateAngle sb(x(0));
+
+ sb.perturb(0.1);
+
+ WOLF_INFO("original ", x.transpose(), ", perturbed = ", sb.getState().transpose());
+
+ ASSERT_NE(x.norm(), sb.getState().norm());
+ ASSERT_MATRIX_APPROX(x , sb.getState() , 0.1 * 4); // 4-sigma test...
+}
+
+TEST(StateBlock, quaternion_perturb)
+{
+ Vector4d x(0.5,0.5,0.5,0.5);
+ StateQuaternion sb(x);
+
+ sb.perturb(0.01);
+
+ WOLF_INFO("original ", x.transpose(), ", perturbed = ", sb.getState().transpose());
+
+ ASSERT_LT((sb.getState().transpose() * x).norm() , 1.0); // quaternions are not parallel ==> not equal
+ ASSERT_MATRIX_APPROX(x , sb.getState() , 0.01 * 4); // 4-sigma test...
+}
+
+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/gtest_yaml_conversions.cpp b/test/gtest_yaml_conversions.cpp
index eb42585bd47b6217416d6741a2053bff582f2eda..04acb34ee69f1da8f368966ddef0acaa6c93a7c8 100644
--- a/test/gtest_yaml_conversions.cpp
+++ b/test/gtest_yaml_conversions.cpp
@@ -16,7 +16,7 @@
using namespace Eigen;
using namespace wolf;
-TEST(MapYaml, save_2D)
+TEST(MapYaml, save_2d)
{
MatrixXd M23(2,3);
MatrixXd M33(3,3);
@@ -37,9 +37,9 @@ TEST(MapYaml, save_2D)
std::cout << "Dyn-Dyn [[2,3] ,[1, 2, 3, 4, 5, 6] ] = \n" << Mx << std::endl;
ASSERT_MATRIX_APPROX(Mx, M23, 1e-12);
- Matrix<double, 2, Dynamic> M2D = mat_sized_23.as<Matrix<double, 2, Dynamic>>();
- std::cout << "2-Dyn [[2,3] ,[1, 2, 3, 4, 5, 6] ] = \n" << M2D << std::endl;
- ASSERT_MATRIX_APPROX(M2D, M23, 1e-12);
+ Matrix<double, 2, Dynamic> M2d = mat_sized_23.as<Matrix<double, 2, Dynamic>>();
+ std::cout << "2-Dyn [[2,3] ,[1, 2, 3, 4, 5, 6] ] = \n" << M2d << std::endl;
+ ASSERT_MATRIX_APPROX(M2d, M23, 1e-12);
Matrix<double, Dynamic, 3> MD3 = mat_sized_23.as<Matrix<double, Dynamic, 3>>();
std::cout << "Dyn-3 [[2,3] ,[1, 2, 3, 4, 5, 6] ] = \n" << MD3 << std::endl;
@@ -49,9 +49,9 @@ TEST(MapYaml, save_2D)
std::cout << "3-3 [[3,3] ,[1, 2, 3, 4, 5, 6, 7, 8, 9] ] = \n" << M3 << std::endl;
ASSERT_MATRIX_APPROX(M3, M33, 1e-12);
- M2D = mat_23.as<Matrix<double, 2, Dynamic>>();
- std::cout << "2-Dyn [1, 2, 3, 4, 5, 6] = \n" << M2D << std::endl;
- ASSERT_MATRIX_APPROX(M2D, M23, 1e-12);
+ M2d = mat_23.as<Matrix<double, 2, Dynamic>>();
+ std::cout << "2-Dyn [1, 2, 3, 4, 5, 6] = \n" << M2d << std::endl;
+ ASSERT_MATRIX_APPROX(M2d, M23, 1e-12);
MD3 = mat_23.as<Matrix<double, Dynamic, 3>>();
std::cout << "Dyn-3 [1, 2, 3, 4, 5, 6] = \n" << MD3 << std::endl;
diff --git a/test/yaml/params1.yaml b/test/yaml/params1.yaml
index 70a379caf099773905033ac7f6f4b0e88d467f3e..de23e1f71055b731663efcc424b05fcebe37f5ea 100644
--- a/test/yaml/params1.yaml
+++ b/test/yaml/params1.yaml
@@ -4,7 +4,7 @@ config:
dimension: 3
sensors:
-
- type: "ODOM 2D"
+ type: "ODOM 2d"
name: "odom"
plugin: "core"
intrinsic:
@@ -18,7 +18,7 @@ config:
plugin: "core"
processors:
-
- type: "ODOM 2D"
+ type: "ODOM 2d"
name: "processor1"
sensor_name: "odom"
plugin: "core"
@@ -28,17 +28,17 @@ config:
sensor_name: "rb"
plugin: "core"
-
- type: "ODOM 2D"
+ type: "ODOM 2d"
name: "my_proc_test"
sensor_name: "odom"
plugin: "core"
follow: "test/yaml/params3.1.yaml"
-
- type: "ODOM 3D"
+ type: "ODOM 3d"
name: "my_proc_odom3d"
sensor_name: "odom"
plugin: "core"
- follow: "test/yaml/processor_odom_3D.yaml"
+ follow: "test/yaml/processor_odom_3d.yaml"
files:
- "/home/jcasals/workspace/wip/wolf/lib/libsensor_odo.so"
- "/home/jcasals/workspace/wip/wolf/lib/librange_bearing.so"
\ No newline at end of file
diff --git a/test/yaml/params2.yaml b/test/yaml/params2.yaml
index 289488fc0e4ff0bd0d79bf26835ee72875308129..6829e2d53212689e08d8c9b90290abad6d57631b 100644
--- a/test/yaml/params2.yaml
+++ b/test/yaml/params2.yaml
@@ -4,7 +4,7 @@ config:
dimension: 2
sensors:
-
- type: "ODOM 2D"
+ type: "ODOM 2d"
name: "odom"
plugin: "core"
intrinsic:
@@ -18,7 +18,7 @@ config:
plugin: "core"
processors:
-
- type: "ODOM 2D"
+ type: "ODOM 2d"
name: "processor1"
sensor_name: "odom"
plugin: "core"
@@ -32,7 +32,7 @@ config:
sensor_name: "rb"
plugin: "core"
-
- type: "ODOM 2D"
+ type: "ODOM 2d"
name: "my_proc_test"
sensor_name: "odom"
plugin: "core"
diff --git a/test/yaml/params3.yaml b/test/yaml/params3.yaml
index ec7006053c7e27d79789630f1cd161a4905aa714..e23b24ea4d148394dc87a1789514f3e1c7e71e75 100644
--- a/test/yaml/params3.yaml
+++ b/test/yaml/params3.yaml
@@ -4,7 +4,7 @@ config:
dimension: 2
sensors:
-
- type: "ODOM 2D"
+ type: "ODOM 2d"
name: "odom"
plugin: "core"
intrinsic:
@@ -14,7 +14,7 @@ config:
pose: [1,2,3]
processors:
-
- type: "ODOM 2D"
+ type: "ODOM 2d"
name: "my_proc_test"
plugin: "core"
sensor_name: "odom"
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
diff --git a/test/yaml/processor_odom_3D.yaml b/test/yaml/processor_odom_3d.yaml
similarity index 86%
rename from test/yaml/processor_odom_3D.yaml
rename to test/yaml/processor_odom_3d.yaml
index 25ef29e83065acfe37cff57babc06b25b3b5d57b..65b00b967a1b3c1ed870f979479db930caf78b5e 100644
--- a/test/yaml/processor_odom_3D.yaml
+++ b/test/yaml/processor_odom_3d.yaml
@@ -1,4 +1,4 @@
-type: "ProcessorOdom3D" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+type: "ProcessorOdom3d" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
time_tolerance: 0.01 # seconds
diff --git a/test/yaml/sensor_odom_2D.yaml b/test/yaml/sensor_odom_2d.yaml
similarity index 65%
rename from test/yaml/sensor_odom_2D.yaml
rename to test/yaml/sensor_odom_2d.yaml
index 45271b18cfff1235f05d945c6e5f124ff33e064e..0841d40b310d90064de6131e32ccaee189ab104c 100644
--- a/test/yaml/sensor_odom_2D.yaml
+++ b/test/yaml/sensor_odom_2d.yaml
@@ -1,4 +1,4 @@
-type: "SensorOdom2D" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+type: "SensorOdom2d" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
k_disp_to_disp: 0.02 # m^2 / m
k_rot_to_rot: 0.01 # rad^2 / rad
diff --git a/demos/sensor_odom_3D.yaml b/test/yaml/sensor_odom_3d.yaml
similarity index 60%
rename from demos/sensor_odom_3D.yaml
rename to test/yaml/sensor_odom_3d.yaml
index c45adb8ca809770ff3320ec81637e7a7ea556758..58db1c088fbc80339a78ba815fddbaf69674d3b6 100644
--- a/demos/sensor_odom_3D.yaml
+++ b/test/yaml/sensor_odom_3d.yaml
@@ -1,4 +1,4 @@
-type: "ODOM 3D" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+type: "SensorOdom3d" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
k_disp_to_disp: 0.02 # m^2 / m
k_disp_to_rot: 0.02 # rad^2 / m
diff --git a/wolf_scripts/out b/wolf_scripts/out
index 2b7857b6ba924b96ce256a0a7661b7079c429a2a..493b8dbcb5aaec3863d3634eff897e820206f404 100644
--- a/wolf_scripts/out
+++ b/wolf_scripts/out
@@ -2,7 +2,7 @@
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_wheel_joint_position.h ==================================
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_3D.h ==================================
++====================== /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_3d.h ==================================
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_image.h ==================================
-===============================================================
@@ -10,13 +10,13 @@
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_GPS.h ==================================
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_2D.h ==================================
++====================== /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_2d.h ==================================
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_motion.h ==================================
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_GPS_fix.h ==================================
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_laser_2D.h ==================================
++====================== /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_laser_2d.h ==================================
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/capture/src/capture_image.cpp ==================================
#include "capture_image.h"
@@ -24,8 +24,8 @@
+====================== /home/jcasals/workspace/wip/wolf/src/capture/src/capture_GPS.cpp ==================================
#include "capture_GPS.h"
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_3D.cpp ==================================
-#include "capture_odom_3D.h"
++====================== /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_3d.cpp ==================================
+#include "capture_odom_3d.h"
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/capture/src/capture_velocity.cpp ==================================
#include "capture_velocity.h"
@@ -39,18 +39,18 @@
+====================== /home/jcasals/workspace/wip/wolf/src/capture/src/capture_GPS_fix.cpp ==================================
#include "capture_GPS_fix.h"
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_2D.cpp ==================================
-#include "capture_odom_2D.h"
++====================== /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_2d.cpp ==================================
+#include "capture_odom_2d.h"
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/capture/src/capture_wheel_joint_position.cpp ==================================
#include "capture_wheel_joint_position.h"
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/capture/src/capture_laser_2D.cpp ==================================
-#include "capture_laser_2D.h"
++====================== /home/jcasals/workspace/wip/wolf/src/capture/src/capture_laser_2d.cpp ==================================
+#include "capture_laser_2d.h"
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_IMU.h ==================================
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2D.h ==================================
++====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2d.h ==================================
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_quaternion_absolute.h ==================================
-===============================================================
@@ -58,39 +58,39 @@
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_block_absolute.h ==================================
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2D_analytic.h ==================================
++====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2d_analytic.h ==================================
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_epipolar.h ==================================
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_2D.h ==================================
++====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_2d.h ==================================
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_fix_bias.h ==================================
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_relative_2D_analytic.h ==================================
++====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_relative_2d_analytic.h ==================================
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_2D.h ==================================
++====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_2d.h ==================================
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_autodiff_trifocal.h ==================================
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_2D.h ==================================
++====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_2d.h ==================================
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_corner_2D.h ==================================
++====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_corner_2d.h ==================================
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_AHP.h ==================================
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_3D.h ==================================
++====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_3d.h ==================================
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_3D.h ==================================
++====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_3d.h ==================================
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_autodiff_distance_3D.h ==================================
++====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_autodiff_distance_3d.h ==================================
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_diff_drive.h ==================================
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_to_line_2D.h ==================================
++====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_to_line_2d.h ==================================
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_2D.h ==================================
++====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_2d.h ==================================
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_3D.h ==================================
++====================== /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_3d.h ==================================
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/frame_base.h ==================================
-===============================================================
@@ -134,7 +134,7 @@
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/autoconf_processor_factory.h ==================================
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/state_homogeneous_3D.h ==================================
++====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/state_homogeneous_3d.h ==================================
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/wolf.h ==================================
-===============================================================
@@ -307,15 +307,15 @@
+====================== /home/jcasals/workspace/wip/wolf/src/core/src/local_parametrization_base.cpp ==================================
#include "local_parametrization_base.h"
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_odom_2D.h ==================================
++====================== /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_odom_2d.h ==================================
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_line_2D.h ==================================
++====================== /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_line_2d.h ==================================
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_motion.h ==================================
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_IMU.h ==================================
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_corner_2D.h ==================================
++====================== /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_corner_2d.h ==================================
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_GPS_pseudorange.h ==================================
-===============================================================
@@ -323,7 +323,7 @@
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_GPS_fix.h ==================================
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_polyline_2D.h ==================================
++====================== /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_polyline_2d.h ==================================
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_diff_drive.h ==================================
-===============================================================
@@ -333,11 +333,11 @@
+====================== /home/jcasals/workspace/wip/wolf/src/feature/src/feature_GPS_pseudorange.cpp ==================================
#include "feature_GPS_pseudorange.h"
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/feature/src/feature_odom_2D.cpp ==================================
-#include "feature_odom_2D.h"
++====================== /home/jcasals/workspace/wip/wolf/src/feature/src/feature_odom_2d.cpp ==================================
+#include "feature_odom_2d.h"
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/feature/src/feature_polyline_2D.cpp ==================================
-#include "feature_polyline_2D.h"
++====================== /home/jcasals/workspace/wip/wolf/src/feature/src/feature_polyline_2d.cpp ==================================
+#include "feature_polyline_2d.h"
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/feature/src/feature_diff_drive.cpp ==================================
#include "feature_diff_drive.h"
@@ -348,16 +348,16 @@
+====================== /home/jcasals/workspace/wip/wolf/src/feature/src/feature_motion.cpp ==================================
#include "feature_motion.h"
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/feature/src/feature_corner_2D.cpp ==================================
-#include "feature_corner_2D.h"
++====================== /home/jcasals/workspace/wip/wolf/src/feature/src/feature_corner_2d.cpp ==================================
+#include "feature_corner_2d.h"
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/feature/src/feature_GPS_fix.cpp ==================================
#include "feature_GPS_fix.h"
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/feature/src/feature_line_2D.cpp ==================================
-#include "feature_corner_2D.h"
++====================== /home/jcasals/workspace/wip/wolf/src/feature/src/feature_line_2d.cpp ==================================
+#include "feature_corner_2d.h"
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_point_3D.h ==================================
++====================== /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_point_3d.h ==================================
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_container.h ==================================
-===============================================================
@@ -365,26 +365,26 @@
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_AHP.h ==================================
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_polyline_2D.h ==================================
++====================== /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_polyline_2d.h ==================================
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_corner_2D.h ==================================
++====================== /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_corner_2d.h ==================================
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_line_2D.h ==================================
++====================== /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_line_2d.h ==================================
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_corner_2D.cpp ==================================
-#include "landmark_corner_2D.h"
++====================== /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_corner_2d.cpp ==================================
+#include "landmark_corner_2d.h"
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_line_2D.cpp ==================================
-#include "landmark_line_2D.h"
++====================== /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_line_2d.cpp ==================================
+#include "landmark_line_2d.h"
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_container.cpp ==================================
#include "landmark_container.h"
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_polyline_2D.cpp ==================================
-#include "landmark_polyline_2D.h"
++====================== /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_polyline_2d.cpp ==================================
+#include "landmark_polyline_2d.h"
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_point_3D.cpp ==================================
-#include "landmark_point_3D.h"
++====================== /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_point_3d.cpp ==================================
+#include "landmark_point_3d.h"
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_AHP.cpp ==================================
#include "landmark_AHP.h"
@@ -395,7 +395,7 @@
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_frame_nearest_neighbor_filter.h ==================================
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_2D.h ==================================
++====================== /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_2d.h ==================================
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_landmark_corner.h ==================================
-===============================================================
@@ -415,7 +415,7 @@
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_feature_image.h ==================================
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_3D.h ==================================
++====================== /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_3d.h ==================================
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_IMU.h ==================================
-===============================================================
@@ -443,11 +443,11 @@
+====================== /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_landmark.cpp ==================================
#include "processor_tracker_landmark.h"
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_3D.cpp ==================================
-#include "processor_odom_3D.h"
++====================== /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_3d.cpp ==================================
+#include "processor_odom_3d.h"
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_2D.cpp ==================================
-#include "processor_odom_2D.h"
++====================== /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_2d.cpp ==================================
+#include "processor_odom_2d.h"
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_landmark_image.cpp ==================================
#include "processor_tracker_landmark_image.h"
@@ -473,11 +473,11 @@
+====================== /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_landmark_polyline.cpp ==================================
#include "processor_tracker_landmark_polyline.h"
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_3D.h ==================================
++====================== /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_3d.h ==================================
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_IMU.h ==================================
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_2D.h ==================================
++====================== /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_2d.h ==================================
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_GPS_fix.h ==================================
-===============================================================
@@ -487,7 +487,7 @@
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_GPS.h ==================================
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_laser_2D.h ==================================
++====================== /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_laser_2d.h ==================================
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_diff_drive.cpp ==================================
#include "sensor_diff_drive.h"
@@ -495,17 +495,17 @@
+====================== /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_GPS.cpp ==================================
#include "sensor_GPS.h"
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_2D.cpp ==================================
-#include "sensor_odom_2D.h"
++====================== /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_2d.cpp ==================================
+#include "sensor_odom_2d.h"
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_IMU.cpp ==================================
#include "sensor_IMU.h"
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_3D.cpp ==================================
-#include "sensor_odom_3D.h"
++====================== /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_3d.cpp ==================================
+#include "sensor_odom_3d.h"
-===============================================================
-+====================== /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_laser_2D.cpp ==================================
-#include "sensor_laser_2D.h"
++====================== /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_laser_2d.cpp ==================================
+#include "sensor_laser_2d.h"
-===============================================================
+====================== /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_GPS_fix.cpp ==================================
#include "sensor_GPS_fix.h"
diff --git a/wolf_scripts/output b/wolf_scripts/output
index 565ac41005f746c4bc411e7776ba78adb2c3024a..2652d6fd005dfc9eaad13d4b8bcf7f5e582eb84c 100644
--- a/wolf_scripts/output
+++ b/wolf_scripts/output
@@ -22,18 +22,18 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_wheel_joint_position.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_3d.h ==================================
#include <wolf/core/rotations.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_3d.h ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_image.h ==================================
================================================================
@@ -75,18 +75,18 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_GPS.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_2d.h ==================================
#include <wolf/core/rotations.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_odom_2d.h ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_motion.h ==================================
================================================================
@@ -116,19 +116,19 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_GPS_fix.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_laser_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_laser_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_laser_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_laser_2d.h ==================================
#include <wolf/core/capture_base.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_laser_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_laser_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_laser_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_laser_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_laser_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_laser_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_laser_2D.h ==================================
-#include <wolf/sensor/sensor_laser_2D.h>
+======================= /home/jcasals/workspace/wip/wolf/src/capture/include/capture/capture_laser_2d.h ==================================
+#include <wolf/sensor/sensor_laser_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_image.cpp ==================================
================================================================
@@ -154,17 +154,17 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_GPS.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_3d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_3d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_3d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_3d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_3d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_3d.cpp ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_velocity.cpp ==================================
================================================================
@@ -216,17 +216,17 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_GPS_fix.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_odom_2d.cpp ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_wheel_joint_position.cpp ==================================
================================================================
@@ -242,17 +242,17 @@
======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_wheel_joint_position.cpp ==================================
#include <wolf/sensor/sensor_diff_drive.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_laser_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_laser_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_laser_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_laser_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_laser_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_laser_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_laser_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_laser_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_laser_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_laser_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_laser_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/capture/src/capture_laser_2d.cpp ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_IMU.h ==================================
================================================================
@@ -270,19 +270,19 @@
======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_IMU.h ==================================
#include <wolf/sensor/sensor_IMU.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2d.h ==================================
#include <wolf/core/constraint_autodiff.h>
#include <wolf/core/frame_base.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2d.h ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_quaternion_absolute.h ==================================
================================================================
@@ -329,17 +329,17 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_block_absolute.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2D_analytic.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2d_analytic.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2D_analytic.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2d_analytic.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2D_analytic.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2d_analytic.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2D_analytic.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2d_analytic.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2D_analytic.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2d_analytic.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2D_analytic.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_2d_analytic.h ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_epipolar.h ==================================
================================================================
@@ -354,20 +354,20 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_epipolar.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_2d.h ==================================
#include <wolf/core/wolf.h>
#include <wolf/core/constraint_autodiff.h>
#include <wolf/core/frame_base.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_2d.h ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_fix_bias.h ==================================
#include <wolf/capture/capture_IMU.h>
@@ -386,34 +386,34 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_fix_bias.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_relative_2D_analytic.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_relative_2d_analytic.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_relative_2D_analytic.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_relative_2d_analytic.h ==================================
#include <wolf/core/constraint_analytic.h>
#include <wolf/core/landmark_base.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_relative_2D_analytic.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_relative_2d_analytic.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_relative_2D_analytic.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_relative_2d_analytic.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_relative_2D_analytic.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_relative_2d_analytic.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_relative_2D_analytic.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_relative_2d_analytic.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_2d.h ==================================
#include <wolf/core/constraint_autodiff.h>
#include <wolf/core/frame_base.h>
#include <wolf/core/rotations.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_2d.h ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_autodiff_trifocal.h ==================================
================================================================
@@ -431,34 +431,34 @@
======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_autodiff_trifocal.h ==================================
#include <wolf/sensor/sensor_camera.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_2d.h ==================================
#include <wolf/core/constraint_autodiff.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_2d.h ==================================
#include <wolf/feature/feature_GPS_pseudorange.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_2d.h ==================================
#include <wolf/sensor/sensor_GPS.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_corner_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_corner_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_corner_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_corner_2d.h ==================================
#include <wolf/core/constraint_autodiff.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_corner_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_corner_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_corner_2D.h ==================================
-#include <wolf/landmark/landmark_corner_2D.h>
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_corner_2d.h ==================================
+#include <wolf/landmark/landmark_corner_2d.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_corner_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_corner_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_corner_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_corner_2d.h ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_AHP.h ==================================
================================================================
@@ -476,47 +476,47 @@
======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_AHP.h ==================================
#include <wolf/sensor/sensor_camera.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_3d.h ==================================
#include <wolf/core/constraint_autodiff.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_3d.h ==================================
#include <wolf/feature/feature_GPS_pseudorange.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_GPS_pseudorange_3d.h ==================================
#include <wolf/sensor/sensor_GPS.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_3d.h ==================================
#include <wolf/core/constraint_autodiff.h>
#include <wolf/core/rotations.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_odom_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_autodiff_distance_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_autodiff_distance_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_autodiff_distance_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_autodiff_distance_3d.h ==================================
#include <wolf/core/constraint_autodiff.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_autodiff_distance_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_autodiff_distance_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_autodiff_distance_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_autodiff_distance_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_autodiff_distance_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_autodiff_distance_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_autodiff_distance_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_autodiff_distance_3d.h ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_diff_drive.h ==================================
#include <wolf/capture/capture_wheel_joint_position.h>
@@ -534,50 +534,50 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_diff_drive.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_to_line_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_to_line_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_to_line_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_to_line_2d.h ==================================
#include <wolf/core/constraint_autodiff.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_to_line_2D.h ==================================
-#include <wolf/feature/feature_polyline_2D.h>
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_to_line_2d.h ==================================
+#include <wolf/feature/feature_polyline_2d.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_to_line_2D.h ==================================
-#include <wolf/landmark/landmark_polyline_2D.h>
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_to_line_2d.h ==================================
+#include <wolf/landmark/landmark_polyline_2d.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_to_line_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_to_line_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_to_line_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_to_line_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_2d.h ==================================
#include <wolf/core/constraint_autodiff.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_2D.h ==================================
-#include <wolf/feature/feature_polyline_2D.h>
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_2d.h ==================================
+#include <wolf/feature/feature_polyline_2d.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_2D.h ==================================
-#include <wolf/landmark/landmark_polyline_2D.h>
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_2d.h ==================================
+#include <wolf/landmark/landmark_polyline_2d.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_point_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_3d.h ==================================
#include <wolf/core/constraint_autodiff.h>
#include <wolf/core/frame_base.h>
#include <wolf/core/rotations.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/constraint/include/constraint/constraint_pose_3d.h ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/frame_base.h ==================================
================================================================
@@ -630,8 +630,8 @@
======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/capture_pose.h ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/capture_pose.h ==================================
-#include <wolf/constraint/constraint_pose_2D.h>
-#include <wolf/constraint/constraint_pose_3D.h>
+#include <wolf/constraint/constraint_pose_2d.h>
+#include <wolf/constraint/constraint_pose_3d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/capture_pose.h ==================================
================================================================
@@ -798,17 +798,17 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/problem.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/state_homogeneous_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/state_homogeneous_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/state_homogeneous_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/state_homogeneous_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/state_homogeneous_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/state_homogeneous_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/state_homogeneous_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/state_homogeneous_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/state_homogeneous_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/state_homogeneous_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/state_homogeneous_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/state_homogeneous_3d.h ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/wolf.h ==================================
================================================================
@@ -1073,12 +1073,12 @@
======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/processor_factory.h ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/processor_factory.h ==================================
- * #include <wolf/processor/processor_odom_2D.h> // provides ProcessorOdom2D and ProcessorFactory
- * #include <wolf/processor/processor_odom_2D.h>
+ * #include <wolf/processor/processor_odom_2d.h> // provides ProcessorOdom2d and ProcessorFactory
+ * #include <wolf/processor/processor_odom_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/processor_factory.h ==================================
- * #include <wolf/sensor/sensor_odom_2D.h> // provides SensorOdom2D and SensorFactory
- * #include <wolf/sensor/sensor_odom_2D.h>
+ * #include <wolf/sensor/sensor_odom_2d.h> // provides SensorOdom2d and SensorFactory
+ * #include <wolf/sensor/sensor_odom_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/core/include/core/local_parametrization_quaternion.h ==================================
================================================================
@@ -1471,7 +1471,7 @@
#include <wolf/capture/capture_IMU.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_imuPlateform_Offline.cpp ==================================
-#include <wolf/constraint/constraint_odom_3D.h>
+#include <wolf/constraint/constraint_odom_3d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_imuPlateform_Offline.cpp ==================================
#include <wolf/core/wolf.h>
@@ -1485,17 +1485,17 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_imuPlateform_Offline.cpp ==================================
#include <wolf/processor/processor_IMU.h>
-#include <wolf/processor/processor_odom_3D.h>
+#include <wolf/processor/processor_odom_3d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_imuPlateform_Offline.cpp ==================================
#include <wolf/sensor/sensor_IMU.h>
-#include <wolf/sensor/sensor_odom_3D.h>
+#include <wolf/sensor/sensor_odom_3d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_constraint_imu.cpp ==================================
#include <wolf/capture/capture_IMU.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_constraint_imu.cpp ==================================
-#include <wolf/constraint/constraint_odom_3D.h>
+#include <wolf/constraint/constraint_odom_3d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_constraint_imu.cpp ==================================
#include <wolf/core/capture_pose.h>
@@ -1618,7 +1618,7 @@
======================= /home/jcasals/workspace/wip/wolf/src/examples/solver/test_iQR_wolf2.cpp ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/solver/test_iQR_wolf2.cpp ==================================
-#include <wolf/sensor/sensor_laser_2D.h>
+#include <wolf/sensor/sensor_laser_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/solver/test_permutations.cpp ==================================
================================================================
@@ -1842,12 +1842,12 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_ceres_2_lasers_polylines.cpp ==================================
#include <wolf/processor/processor_tracker_landmark_polyline.h>
-#include <wolf/processor/processor_odom_2D.h>
+#include <wolf/processor/processor_odom_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_ceres_2_lasers_polylines.cpp ==================================
#include <wolf/sensor/sensor_GPS_fix.h>
-#include <wolf/sensor/sensor_laser_2D.h>
-#include <wolf/sensor/sensor_odom_2D.h>
+#include <wolf/sensor/sensor_laser_2d.h>
+#include <wolf/sensor/sensor_odom_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_imu_no_vote.yaml ==================================
================================================================
@@ -1891,20 +1891,20 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_imu_t1.yaml ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_capture_laser_2D.cpp ==================================
-#include <wolf/capture/capture_laser_2D.h>
+======================= /home/jcasals/workspace/wip/wolf/src/examples/test_capture_laser_2d.cpp ==================================
+#include <wolf/capture/capture_laser_2d.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_capture_laser_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/test_capture_laser_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_capture_laser_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/test_capture_laser_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_capture_laser_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/test_capture_laser_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_capture_laser_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/test_capture_laser_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_capture_laser_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/test_capture_laser_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_capture_laser_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/test_capture_laser_2d.cpp ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/ACTIVESEARCH.yaml ==================================
================================================================
@@ -1933,7 +1933,7 @@
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_map_yaml.cpp ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_map_yaml.cpp ==================================
-#include <wolf/landmark/landmark_polyline_2D.h>
+#include <wolf/landmark/landmark_polyline_2d.h>
#include <wolf/landmark/landmark_AHP.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_map_yaml.cpp ==================================
@@ -2047,7 +2047,7 @@
#include <wolf/core/constraint_base.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_sparsification.cpp ==================================
-#include <wolf/feature/feature_odom_2D.h>
+#include <wolf/feature/feature_odom_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_sparsification.cpp ==================================
================================================================
@@ -2070,12 +2070,12 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_ceres_2_lasers.cpp ==================================
#include <wolf/processor/processor_tracker_landmark_corner.h>
-#include <wolf/processor/processor_odom_2D.h>
+#include <wolf/processor/processor_odom_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_ceres_2_lasers.cpp ==================================
#include <wolf/sensor/sensor_GPS_fix.h>
-#include <wolf/sensor/sensor_laser_2D.h>
-#include <wolf/sensor/sensor_odom_2D.h>
+#include <wolf/sensor/sensor_laser_2d.h>
+#include <wolf/sensor/sensor_odom_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_eigen_quaternion.cpp ==================================
================================================================
@@ -2120,7 +2120,7 @@
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_autodiff.cpp ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_autodiff.cpp ==================================
-#include <wolf/sensor/sensor_laser_2D.h>
+#include <wolf/sensor/sensor_laser_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_tracker_ORB.cpp ==================================
================================================================
@@ -2152,13 +2152,13 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_wolf_factories.cpp ==================================
#include <wolf/processor/processor_IMU.h>
-#include <wolf/processor/processor_odom_2D.h>
-#include <wolf/processor/processor_odom_3D.h>
+#include <wolf/processor/processor_odom_2d.h>
+#include <wolf/processor/processor_odom_3d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_wolf_factories.cpp ==================================
#include <wolf/sensor/sensor_GPS_fix.h>
#include <wolf/sensor/sensor_camera.h>
-#include <wolf/sensor/sensor_odom_2D.h>
+#include <wolf/sensor/sensor_odom_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_sh_ptr.cpp ==================================
================================================================
@@ -2178,7 +2178,7 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_imuDock.cpp ==================================
#include <wolf/constraint/constraint_fix_bias.h>
-#include <wolf/constraint/constraint_pose_3D.h>
+#include <wolf/constraint/constraint_pose_3d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_imuDock.cpp ==================================
#include <wolf/core/wolf.h>
@@ -2190,11 +2190,11 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_imuDock.cpp ==================================
#include <wolf/processor/processor_IMU.h>
-#include <wolf/processor/processor_odom_3D.h>
+#include <wolf/processor/processor_odom_3d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_imuDock.cpp ==================================
#include <wolf/sensor/sensor_IMU.h>
-#include <wolf/sensor/sensor_odom_3D.h>
+#include <wolf/sensor/sensor_odom_3d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_2_lasers_offline.cpp ==================================
================================================================
@@ -2211,12 +2211,12 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_2_lasers_offline.cpp ==================================
#include <wolf/processor/processor_tracker_landmark_corner.h>
-#include <wolf/processor/processor_odom_2D.h>
+#include <wolf/processor/processor_odom_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_2_lasers_offline.cpp ==================================
#include <wolf/sensor/sensor_GPS_fix.h>
-#include <wolf/sensor/sensor_laser_2D.h>
-#include <wolf/sensor/sensor_odom_2D.h>
+#include <wolf/sensor/sensor_laser_2d.h>
+#include <wolf/sensor/sensor_odom_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_yaml_conversions.cpp ==================================
================================================================
@@ -2249,10 +2249,10 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_tracker_landmark_image.cpp ==================================
#include <wolf/processor/processor_tracker_landmark_image.h>
-#include <wolf/processor/processor_odom_3D.h>
+#include <wolf/processor/processor_odom_3d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_tracker_landmark_image.cpp ==================================
-#include <wolf/sensor/sensor_odom_3D.h>
+#include <wolf/sensor/sensor_odom_3d.h>
#include <wolf/sensor/sensor_camera.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_eigen_template.cpp ==================================
@@ -2269,33 +2269,33 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_eigen_template.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_odom_3D.yaml ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_odom_3d.yaml ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_odom_3D.yaml ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_odom_3d.yaml ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_odom_3D.yaml ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_odom_3d.yaml ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_odom_3D.yaml ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_odom_3d.yaml ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_odom_3D.yaml ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_odom_3d.yaml ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_odom_3D.yaml ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_odom_3d.yaml ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_odom_3D.yaml ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_odom_3d.yaml ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3D_HQ.yaml ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3d_HQ.yaml ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3D_HQ.yaml ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3d_HQ.yaml ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3D_HQ.yaml ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3d_HQ.yaml ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3D_HQ.yaml ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3d_HQ.yaml ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3D_HQ.yaml ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3d_HQ.yaml ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3D_HQ.yaml ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3d_HQ.yaml ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3D_HQ.yaml ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3d_HQ.yaml ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_matrix_prod.cpp ==================================
================================================================
@@ -2431,11 +2431,11 @@
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_kf_callback.cpp ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_kf_callback.cpp ==================================
-#include <wolf/processor/processor_odom_2D.h>
+#include <wolf/processor/processor_odom_2d.h>
#include <wolf/processor/processor_tracker_feature_dummy.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_kf_callback.cpp ==================================
-#include <wolf/sensor/sensor_odom_2D.h>
+#include <wolf/sensor/sensor_odom_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_faramotics_simulation.cpp ==================================
================================================================
@@ -2459,7 +2459,7 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_imuDock_autoKFs.cpp ==================================
#include <wolf/constraint/constraint_fix_bias.h>
-#include <wolf/constraint/constraint_pose_3D.h>
+#include <wolf/constraint/constraint_pose_3d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_imuDock_autoKFs.cpp ==================================
#include <wolf/core/wolf.h>
@@ -2471,11 +2471,11 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_imuDock_autoKFs.cpp ==================================
#include <wolf/processor/processor_IMU.h>
-#include <wolf/processor/processor_odom_3D.h>
+#include <wolf/processor/processor_odom_3d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_imuDock_autoKFs.cpp ==================================
#include <wolf/sensor/sensor_IMU.h>
-#include <wolf/sensor/sensor_odom_3D.h>
+#include <wolf/sensor/sensor_odom_3d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_analytic_odom_constraint.cpp ==================================
================================================================
@@ -2506,40 +2506,40 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/processor_image_feature.yaml ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_constraint_odom_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/test_constraint_odom_3d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_constraint_odom_3D.cpp ==================================
-#include <wolf/constraint/constraint_odom_3D.h>
+======================= /home/jcasals/workspace/wip/wolf/src/examples/test_constraint_odom_3d.cpp ==================================
+#include <wolf/constraint/constraint_odom_3d.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_constraint_odom_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/test_constraint_odom_3d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_constraint_odom_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/test_constraint_odom_3d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_constraint_odom_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/test_constraint_odom_3d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_constraint_odom_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/test_constraint_odom_3d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_constraint_odom_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/test_constraint_odom_3d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_odom_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_odom_3d.cpp ==================================
#include <wolf/capture/capture_IMU.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_odom_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_odom_3d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_odom_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_odom_3d.cpp ==================================
#include <wolf/core/problem.h>
#include <wolf/core/map_base.h>
#include <wolf/core/landmark_base.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_odom_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_odom_3d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_odom_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_odom_3d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_odom_3D.cpp ==================================
-#include <wolf/processor/processor_odom_3D.h>
+======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_odom_3d.cpp ==================================
+#include <wolf/processor/processor_odom_3d.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_odom_3D.cpp ==================================
-#include <wolf/sensor/sensor_odom_2D.h>
+======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_odom_3d.cpp ==================================
+#include <wolf/sensor/sensor_odom_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_wolf_root.cpp ==================================
================================================================
@@ -2560,7 +2560,7 @@
#include <wolf/capture/capture_IMU.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_imu_constrained0.cpp ==================================
-#include <wolf/constraint/constraint_odom_3D.h>
+#include <wolf/constraint/constraint_odom_3d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_imu_constrained0.cpp ==================================
#include <wolf/core/wolf.h>
@@ -2574,11 +2574,11 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_imu_constrained0.cpp ==================================
#include <wolf/processor/processor_IMU.h>
-#include <wolf/processor/processor_odom_3D.h>
+#include <wolf/processor/processor_odom_3d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_imu_constrained0.cpp ==================================
#include <wolf/sensor/sensor_IMU.h>
-#include <wolf/sensor/sensor_odom_3D.h>
+#include <wolf/sensor/sensor_odom_3d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_constraint_AHP.cpp ==================================
#include <wolf/capture/capture_image.h>
@@ -2600,19 +2600,19 @@
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_constraint_AHP.cpp ==================================
#include <wolf/sensor/sensor_camera.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3D.yaml ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3d.yaml ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3D.yaml ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3d.yaml ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3D.yaml ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3d.yaml ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3D.yaml ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3d.yaml ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3D.yaml ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3d.yaml ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3D.yaml ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3d.yaml ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3D.yaml ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/examples/sensor_odom_3d.yaml ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/test_processor_imu_jacobians.cpp ==================================
#include <wolf/capture/capture_IMU.h>
@@ -2715,33 +2715,33 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/examples/vision_utils_active_search.yaml ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_odom_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_odom_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_odom_2D.h ==================================
-#include <wolf/constraint/constraint_odom_2D.h>
-#include <wolf/constraint/constraint_odom_2D_analytic.h>
+======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_odom_2d.h ==================================
+#include <wolf/constraint/constraint_odom_2d.h>
+#include <wolf/constraint/constraint_odom_2d_analytic.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_odom_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_odom_2d.h ==================================
#include <wolf/core/feature_base.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_odom_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_odom_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_odom_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_odom_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_odom_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_odom_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_line_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_line_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_line_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_line_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_line_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_line_2d.h ==================================
#include <wolf/core/feature_base.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_line_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_line_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_line_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_line_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_line_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_line_2d.h ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_motion.h ==================================
#include <wolf/capture/capture_motion.h>
@@ -2772,18 +2772,18 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_IMU.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_corner_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_corner_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_corner_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_corner_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_corner_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_corner_2d.h ==================================
#include <wolf/core/feature_base.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_corner_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_corner_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_corner_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_corner_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_corner_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_corner_2d.h ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_GPS_pseudorange.h ==================================
================================================================
@@ -2814,7 +2814,7 @@
======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_GPS_fix.h ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_GPS_fix.h ==================================
-#include <wolf/constraint/constraint_GPS_2D.h>
+#include <wolf/constraint/constraint_GPS_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_GPS_fix.h ==================================
#include <wolf/core/feature_base.h>
@@ -2825,18 +2825,18 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_GPS_fix.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_polyline_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_polyline_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_polyline_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_polyline_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_polyline_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_polyline_2d.h ==================================
#include <wolf/core/feature_base.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_polyline_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_polyline_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_polyline_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_polyline_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_polyline_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_polyline_2d.h ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/feature/include/feature/feature_diff_drive.h ==================================
================================================================
@@ -2875,29 +2875,29 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_GPS_pseudorange.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_odom_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_odom_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_odom_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_odom_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_odom_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_odom_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_odom_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_odom_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_odom_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_odom_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_odom_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_odom_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_polyline_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_polyline_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_polyline_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_polyline_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_polyline_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_polyline_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_polyline_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_polyline_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_polyline_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_polyline_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_polyline_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_polyline_2d.cpp ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_diff_drive.cpp ==================================
================================================================
@@ -2935,17 +2935,17 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_motion.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_corner_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_corner_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_corner_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_corner_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_corner_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_corner_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_corner_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_corner_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_corner_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_corner_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_corner_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_corner_2d.cpp ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_GPS_fix.cpp ==================================
================================================================
@@ -2959,30 +2959,30 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_GPS_fix.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_line_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_line_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_line_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_line_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_line_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_line_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_line_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_line_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_line_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_line_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_line_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/feature/src/feature_line_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_point_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_point_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_point_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_point_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_point_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_point_3d.h ==================================
#include <wolf/core/landmark_base.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_point_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_point_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_point_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_point_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_point_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_point_3d.h ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_container.h ==================================
================================================================
@@ -3024,69 +3024,69 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_AHP.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_polyline_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_polyline_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_polyline_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_polyline_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_polyline_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_polyline_2d.h ==================================
#include <wolf/core/landmark_base.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_polyline_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_polyline_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_polyline_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_polyline_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_polyline_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_polyline_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_corner_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_corner_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_corner_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_corner_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_corner_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_corner_2d.h ==================================
#include <wolf/core/landmark_base.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_corner_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_corner_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_corner_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_corner_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_corner_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_corner_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_line_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_line_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_line_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_line_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_line_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_line_2d.h ==================================
#include <wolf/core/landmark_base.h>
#include <wolf/core/wolf.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_line_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_line_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_line_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_line_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_line_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/include/landmark/landmark_line_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_corner_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_corner_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_corner_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_corner_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_corner_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_corner_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_corner_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_corner_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_corner_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_corner_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_corner_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_corner_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_line_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_line_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_line_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_line_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_line_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_line_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_line_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_line_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_line_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_line_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_line_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_line_2d.cpp ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_container.cpp ==================================
================================================================
@@ -3101,41 +3101,41 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_container.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_polyline_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_polyline_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_polyline_2D.cpp ==================================
-#include <wolf/constraint/constraint_point_2D.h>
-#include <wolf/constraint/constraint_point_to_line_2D.h>
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_polyline_2d.cpp ==================================
+#include <wolf/constraint/constraint_point_2d.h>
+#include <wolf/constraint/constraint_point_to_line_2d.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_polyline_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_polyline_2d.cpp ==================================
#include <wolf/core/state_block.h>
#include <wolf/core/factory.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_polyline_2D.cpp ==================================
-#include <wolf/feature/feature_polyline_2D.h>
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_polyline_2d.cpp ==================================
+#include <wolf/feature/feature_polyline_2d.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_polyline_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_polyline_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_polyline_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_polyline_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_point_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_point_3d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_point_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_point_3d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_point_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_point_3d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_point_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_point_3d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_point_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_point_3d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_point_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_point_3d.cpp ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_AHP.cpp ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_AHP.cpp ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_AHP.cpp ==================================
-#include <wolf/core/state_homogeneous_3D.h>
+#include <wolf/core/state_homogeneous_3d.h>
#include <wolf/core/factory.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/landmark/src/landmark_AHP.cpp ==================================
@@ -3187,58 +3187,58 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_frame_nearest_neighbor_filter.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_2D.h ==================================
-#include <wolf/capture/capture_odom_2D.h>
+======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_2d.h ==================================
+#include <wolf/capture/capture_odom_2d.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_2D.h ==================================
-#include <wolf/constraint/constraint_odom_2D.h>
+======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_2d.h ==================================
+#include <wolf/constraint/constraint_odom_2d.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_2d.h ==================================
#include <wolf/core/processor_motion.h>
#include <wolf/core/rotations.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_2d.h ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_landmark_corner.h ==================================
-#include <wolf/capture/capture_laser_2D.h>
+#include <wolf/capture/capture_laser_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_landmark_corner.h ==================================
-#include <wolf/constraint/constraint_corner_2D.h>
+#include <wolf/constraint/constraint_corner_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_landmark_corner.h ==================================
#include <wolf/core/state_block.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_landmark_corner.h ==================================
-#include <wolf/feature/feature_corner_2D.h>
+#include <wolf/feature/feature_corner_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_landmark_corner.h ==================================
-#include <wolf/landmark/landmark_corner_2D.h>
+#include <wolf/landmark/landmark_corner_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_landmark_corner.h ==================================
-#include <wolf/sensor/sensor_laser_2D.h>
+#include <wolf/sensor/sensor_laser_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_feature_corner.h ==================================
-#include <wolf/capture/capture_laser_2D.h>
+#include <wolf/capture/capture_laser_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_feature_corner.h ==================================
-#include <wolf/constraint/constraint_corner_2D.h>
+#include <wolf/constraint/constraint_corner_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_feature_corner.h ==================================
#include <wolf/core/state_block.h>
#include <wolf/core/processor_tracker_feature.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_feature_corner.h ==================================
-#include <wolf/feature/feature_corner_2D.h>
+#include <wolf/feature/feature_corner_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_feature_corner.h ==================================
-#include <wolf/landmark/landmark_corner_2D.h>
+#include <wolf/landmark/landmark_corner_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_feature_corner.h ==================================
-#include <wolf/sensor/sensor_laser_2D.h>
+#include <wolf/sensor/sensor_laser_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_capture_holder.h ==================================
================================================================
@@ -3285,23 +3285,23 @@
======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_landmark.h ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_landmark_polyline.h ==================================
-#include <wolf/capture/capture_laser_2D.h>
+#include <wolf/capture/capture_laser_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_landmark_polyline.h ==================================
-#include <wolf/constraint/constraint_point_2D.h>
-#include <wolf/constraint/constraint_point_to_line_2D.h>
+#include <wolf/constraint/constraint_point_2d.h>
+#include <wolf/constraint/constraint_point_to_line_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_landmark_polyline.h ==================================
#include <wolf/core/state_block.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_landmark_polyline.h ==================================
-#include <wolf/feature/feature_polyline_2D.h>
+#include <wolf/feature/feature_polyline_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_landmark_polyline.h ==================================
-#include <wolf/landmark/landmark_polyline_2D.h>
+#include <wolf/landmark/landmark_polyline_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_landmark_polyline.h ==================================
-#include <wolf/sensor/sensor_laser_2D.h>
+#include <wolf/sensor/sensor_laser_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_logging.h ==================================
================================================================
@@ -3349,22 +3349,22 @@
======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_tracker_feature_image.h ==================================
#include <wolf/sensor/sensor_camera.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_3D.h ==================================
-#include <wolf/capture/capture_odom_3D.h>
+======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_3d.h ==================================
+#include <wolf/capture/capture_odom_3d.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_3D.h ==================================
-#include <wolf/constraint/constraint_odom_3D.h>
+======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_3d.h ==================================
+#include <wolf/constraint/constraint_odom_3d.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_3d.h ==================================
#include <wolf/core/processor_motion.h>
#include <wolf/core/rotations.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_3D.h ==================================
-#include <wolf/sensor/sensor_odom_3D.h>
+======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_odom_3d.h ==================================
+#include <wolf/sensor/sensor_odom_3d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/include/processor/processor_IMU.h ==================================
#include <wolf/capture/capture_IMU.h>
@@ -3439,7 +3439,7 @@
#include <wolf/capture/capture_GPS.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_GPS.cpp ==================================
-#include <wolf/constraint/constraint_GPS_pseudorange_2D.h>
+#include <wolf/constraint/constraint_GPS_pseudorange_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_GPS.cpp ==================================
#include <wolf/core/processor_factory.h>
@@ -3502,31 +3502,31 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_landmark.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_3d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_3d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_3d.cpp ==================================
#include <wolf/core/processor_factory.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_3d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_3d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_3d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_2d.cpp ==================================
#include <wolf/core/processor_factory.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_odom_2d.cpp ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_landmark_image.cpp ==================================
#include <wolf/capture/capture_image.h>
@@ -3555,14 +3555,14 @@
======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_landmark_dummy.cpp ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_landmark_dummy.cpp ==================================
-#include <wolf/constraint/constraint_corner_2D.h>
+#include <wolf/constraint/constraint_corner_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_landmark_dummy.cpp ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_landmark_dummy.cpp ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_landmark_dummy.cpp ==================================
-#include <wolf/landmark/landmark_corner_2D.h>
+#include <wolf/landmark/landmark_corner_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_landmark_dummy.cpp ==================================
================================================================
@@ -3615,7 +3615,7 @@
#include <wolf/capture/capture_velocity.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_diff_drive.cpp ==================================
-#include <wolf/constraint/constraint_odom_2D.h>
+#include <wolf/constraint/constraint_odom_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_diff_drive.cpp ==================================
#include <wolf/core/rotations.h>
@@ -3636,7 +3636,7 @@
======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_feature_corner.cpp ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_feature_corner.cpp ==================================
-#include <wolf/feature/feature_corner_2D.h>
+#include <wolf/feature/feature_corner_2d.h>
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_feature_corner.cpp ==================================
================================================================
@@ -3655,18 +3655,18 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/processor/src/processor_tracker_landmark_polyline.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_3d.h ==================================
#include <wolf/core/sensor_base.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_3d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_3D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_3d.h ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_IMU.h ==================================
================================================================
@@ -3681,18 +3681,18 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_IMU.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_2d.h ==================================
#include <wolf/core/sensor_base.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_odom_2d.h ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_GPS_fix.h ==================================
================================================================
@@ -3748,18 +3748,18 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_GPS.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_laser_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_laser_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_laser_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_laser_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_laser_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_laser_2d.h ==================================
#include <wolf/core/sensor_base.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_laser_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_laser_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_laser_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_laser_2d.h ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_laser_2D.h ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/include/sensor/sensor_laser_2d.h ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_diff_drive.cpp ==================================
#include <wolf/capture/capture_motion.h>
@@ -3791,20 +3791,20 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_GPS.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_2d.cpp ==================================
#include <wolf/core/state_block.h>
#include <wolf/core/state_angle.h>
#include <wolf/core/sensor_factory.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_2d.cpp ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_IMU.cpp ==================================
================================================================
@@ -3821,34 +3821,34 @@
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_IMU.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_3d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_3d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_3d.cpp ==================================
#include <wolf/core/state_block.h>
#include <wolf/core/state_quaternion.h>
#include <wolf/core/sensor_factory.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_3d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_3d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_3D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_odom_3d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_laser_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_laser_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_laser_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_laser_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_laser_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_laser_2d.cpp ==================================
#include <wolf/core/state_block.h>
#include <wolf/core/sensor_factory.h>
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_laser_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_laser_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_laser_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_laser_2d.cpp ==================================
================================================================
-======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_laser_2D.cpp ==================================
+======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_laser_2d.cpp ==================================
================================================================
======================= /home/jcasals/workspace/wip/wolf/src/sensor/src/sensor_GPS_fix.cpp ==================================
================================================================
diff --git a/wolf_scripts/rename.sh b/wolf_scripts/rename.sh
index 27c83e736e3b2b29a0033932f5c134f1bceb8ad0..7d778f475f19523e8cd868c72f0349e4f78dcf30 100755
--- a/wolf_scripts/rename.sh
+++ b/wolf_scripts/rename.sh
@@ -33,7 +33,7 @@ getTypes ()
# echo "VVV "$type
# sed -rn "s/(Feature|Capture|Landmark|Processor|Factor)(.*)\"${type}\"/\1\2\"${camel_file}\"/p" $file
# echo sed -rn "s/(Capture)(.*)\"${type}\"/\1\2\"${camel_file}\"/p" $file
- # type="ODOM 3D"
+ # type="ODOM 3d"
matching=$(ag -l "\"${type}\"" --ignore "demos" . | wc -l)
if [ $matching -gt "0" ]; then
echo ${type}