diff --git a/CMakeLists.txt b/CMakeLists.txt
index 932c337910234b4d68a58142d549dcd88241abd3..fb83ccf5399baef48a2267f0657cf68d8bbd60db 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -216,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
@@ -233,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
@@ -259,6 +259,7 @@ SET(HDRS_LANDMARK
include/core/landmark/landmark_match.h
)
SET(HDRS_PROCESSOR
+ include/core/processor/is_motion.h
include/core/processor/motion_buffer.h
include/core/processor/processor_base.h
include/core/processor/processor_diff_drive.h
@@ -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,8 +278,8 @@ 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
@@ -325,8 +326,8 @@ SET(SRCS_UTILS
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 +340,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 +352,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 +362,16 @@ 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_YAML
- src/yaml/processor_odom_3D_yaml.cpp
- src/yaml/sensor_odom_2D_yaml.cpp
- src/yaml/sensor_odom_3D_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
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..a97d8d2f9c5c7fb8c7f1f37c12330038beb3ba98 100644
--- a/hello_wolf/hello_wolf_autoconf.cpp
+++ b/hello_wolf/hello_wolf_autoconf.cpp
@@ -8,7 +8,7 @@
// wolf core includes
#include "core/common/wolf.h"
-#include "core/capture/capture_odom_2D.h"
+#include "core/capture/capture_odom_2d.h"
#include "core/yaml/parser_yaml.hpp"
#include "core/ceres_wrapper/ceres_manager.h"
@@ -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/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_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/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/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_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_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 14bcc2139d66765f8a3c78f2358d2c4606b91221..3745de04806c78f65463c39da49bcf9ba42cf0f8 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;
@@ -192,11 +194,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 +214,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/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 d7133cddc09f99aad05d0ada7c35ffbe88513c25..63674826d08a026767aeb19a62803b46625b0d2b 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -21,6 +21,7 @@ struct ProcessorParamsBase;
#include "core/utils/params_server.hpp"
#include "core/sensor/sensor_factory.h"
#include "core/processor/processor_factory.h"
+#include "core/processor/is_motion.h"
// std includes
#include <mutex>
@@ -45,7 +46,8 @@ class Problem : public std::enable_shared_from_this<Problem>
HardwareBasePtr hardware_ptr_;
TrajectoryBasePtr trajectory_ptr_;
MapBasePtr map_ptr_;
- ProcessorMotionPtr processor_motion_ptr_;
+ IsMotionPtr processor_motion_ptr_;
+// IsMotionPtr is_motion_ptr_;
std::map<std::pair<StateBlockPtr, StateBlockPtr>, Eigen::MatrixXd> covariances_;
SizeEigen state_size_, state_cov_size_, dim_;
std::map<FactorBasePtr, Notification> factor_notification_map_;
@@ -78,7 +80,7 @@ class Problem : public std::enable_shared_from_this<Problem>
/** \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, //
@@ -89,7 +91,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, //
@@ -147,12 +149,12 @@ class Problem : public std::enable_shared_from_this<Problem>
*
* Set the processor motion.
*/
- void setProcessorMotion(ProcessorMotionPtr _processor_motion_ptr);
- ProcessorMotionPtr setProcessorMotion(const std::string& _unique_processor_name);
+ void setProcessorMotion(IsMotionPtr _processor_motion_ptr);
+ IsMotionPtr setProcessorMotion(const std::string& _unique_processor_name);
void clearProcessorMotion();
public:
- ProcessorMotionPtr& getProcessorMotion();
+ IsMotionPtr& getProcessorMotion();
// Trajectory branch ----------------------------------
TrajectoryBasePtr getTrajectory() const;
@@ -269,6 +271,9 @@ class Problem : public std::enable_shared_from_this<Problem>
// Zero state provider
Eigen::VectorXd zeroState ( ) const;
bool priorIsSet() const;
+ // Perturb state
+ void perturb(double amplitude = 0.01);
+
// Map branch -----------------------------------------
MapBasePtr getMap() const;
@@ -362,7 +367,7 @@ inline bool Problem::priorIsSet() const
return prior_is_set_;
}
-inline ProcessorMotionPtr& Problem::getProcessorMotion()
+inline IsMotionPtr& Problem::getProcessorMotion()
{
return processor_motion_ptr_;
}
diff --git a/include/core/processor/is_motion.h b/include/core/processor/is_motion.h
new file mode 100644
index 0000000000000000000000000000000000000000..f3cf0d781f39c9fda6ffb9e64817ceed0acfa162
--- /dev/null
+++ b/include/core/processor/is_motion.h
@@ -0,0 +1,84 @@
+/**
+ * \file is_motion.h
+ *
+ * Created on: Mar 10, 2020
+ * \author: jsola
+ */
+
+#ifndef PROCESSOR_IS_MOTION_H_
+#define PROCESSOR_IS_MOTION_H_
+
+#include "core/common/wolf.h"
+
+namespace wolf
+{
+
+class TimeStamp;
+
+WOLF_PTR_TYPEDEFS(IsMotion);
+
+class IsMotion
+{
+ public:
+
+ virtual ~IsMotion();
+
+ // Queries to the processor:
+
+ /** \brief Fill a reference to the state integrated so far
+ * \param _x the returned state vector
+ */
+ virtual void getCurrentState(Eigen::VectorXd& _x) const = 0;
+ virtual void getCurrentTimeStamp(TimeStamp& _ts) const = 0;
+ /** \brief Fill the state corresponding to the provided time-stamp
+ * \param _ts the time stamp
+ * \param _x the returned state
+ * \return if state in the provided time-stamp could be resolved
+ */
+ virtual bool getState(const TimeStamp& _ts, Eigen::VectorXd& _x) const = 0;
+
+
+ // Overloaded getters
+ Eigen::VectorXd getCurrentState() const;
+ TimeStamp getCurrentTimeStamp() const;
+ Eigen::VectorXd getState(const TimeStamp& _ts) const;
+
+};
+
+}
+
+///// IMPLEMENTATION ///////
+#include "core/common/time_stamp.h"
+
+namespace wolf{
+
+inline IsMotion::~IsMotion()
+{
+}
+
+inline Eigen::VectorXd IsMotion::getCurrentState() const
+{
+ Eigen::VectorXd x;
+ getCurrentState(x);
+ return x;
+}
+
+inline TimeStamp IsMotion::getCurrentTimeStamp() const
+{
+ TimeStamp ts;
+ getCurrentTimeStamp(ts);
+ return ts;
+}
+
+inline Eigen::VectorXd IsMotion::getState(const TimeStamp& _ts) const
+{
+ Eigen::VectorXd x;
+ getState(_ts, x);
+ return x;
+}
+
+
+
+} /* namespace wolf */
+
+#endif /* PROCESSOR_IS_MOTION_H_ */
diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h
index c4c325aac18b0435357c73af53e44f7a208186ae..f3feeb56a3bcc6787d8efeb1355175b989dd3f3e 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.h
@@ -9,6 +9,7 @@ class SensorBase;
// Wolf includes
#include "core/common/wolf.h"
#include "core/common/node_base.h"
+#include "core/processor/is_motion.h"
#include "core/sensor/sensor_base.h"
#include "core/frame/frame_base.h"
#include "core/common/time_stamp.h"
@@ -245,13 +246,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();
@@ -338,7 +340,7 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
void setSensor(SensorBasePtr _sen_ptr){sensor_ptr_ = _sen_ptr;}
public:
- virtual bool isMotion() const;
+ bool isMotion() const;
void setTimeTolerance(double _time_tolerance);
@@ -348,6 +350,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);
@@ -376,7 +380,8 @@ inline void ProcessorBase::setVotingAuxActive(bool _voting_active)
inline bool ProcessorBase::isMotion() const
{
- return false;
+ // check if this inherits from IsMotion
+ return (std::dynamic_pointer_cast<const IsMotion>(shared_from_this()) != nullptr);
}
inline unsigned int ProcessorBase::id() const
@@ -393,7 +398,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 88ee84a24470cc715adc8bf512db0d8004e827b0..2ac6c8d5949ad07c1529fd06739c0ce80211f243 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -11,6 +11,7 @@
// Wolf
#include "core/capture/capture_motion.h"
#include "core/processor/processor_base.h"
+#include "core/processor/is_motion.h"
#include "core/common/time_stamp.h"
#include "core/utils/params_server.hpp"
@@ -129,7 +130,7 @@ struct ProcessorParamsMotion : public ProcessorParamsBase
* // TODO: JS: review instructions up to here
*
*/
-class ProcessorMotion : public ProcessorBase
+class ProcessorMotion : public ProcessorBase, public IsMotion
{
public:
typedef enum {
@@ -147,6 +148,7 @@ class ProcessorMotion : public ProcessorBase
// This is the main public interface
public:
ProcessorMotion(const std::string& _type,
+ int _dim,
SizeEigen _state_size,
SizeEigen _delta_size,
SizeEigen _delta_cov_size,
@@ -159,32 +161,23 @@ class ProcessorMotion : public ProcessorBase
virtual void resetDerived();
// Queries to the processor:
- virtual bool isMotion() const override;
/** \brief Fill a reference to the state integrated so far
* \param _x the returned state vector
*/
- void getCurrentState(Eigen::VectorXd& _x) const;
- void getCurrentTimeStamp(TimeStamp& _ts) const { _ts = getBuffer().get().back().ts_; }
+ virtual void getCurrentState(Eigen::VectorXd& _x) const override;
+ virtual void getCurrentTimeStamp(TimeStamp& _ts) const override { _ts = getBuffer().get().back().ts_; }
+ using IsMotion::getCurrentState;
+ using IsMotion::getCurrentTimeStamp;
- /** \brief Get the state integrated so far
- * \return the state vector
- */
- Eigen::VectorXd getCurrentState() const;
- TimeStamp getCurrentTimeStamp() const;
/** \brief Fill the state corresponding to the provided time-stamp
* \param _ts the time stamp
* \param _x the returned state
* \return if state in the provided time-stamp could be resolved
*/
- bool getState(const TimeStamp& _ts, Eigen::VectorXd& _x) const;
-
- /** \brief Get the state corresponding to the provided time-stamp
- * \param _ts the time stamp
- * \return the state vector
- */
- Eigen::VectorXd getState(const TimeStamp& _ts) const;
+ virtual bool getState(const TimeStamp& _ts, Eigen::VectorXd& _x) const override;
+ using IsMotion::getState;
/** \brief Gets the delta preintegrated covariance from all integrations so far
* \return the delta preintegrated covariance matrix
@@ -408,8 +401,8 @@ class ProcessorMotion : public ProcessorBase
* \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;
@@ -506,25 +499,6 @@ inline bool ProcessorMotion::voteForKeyFrame() const
return false;
}
-inline Eigen::VectorXd ProcessorMotion::getState(const TimeStamp& _ts) const
-{
- Eigen::VectorXd x(getProblem()->getFrameStructureSize());
- getState(_ts, x);
- return x;
-}
-
-inline TimeStamp ProcessorMotion::getCurrentTimeStamp() const
-{
- return getBuffer().get().back().ts_;
-}
-
-inline Eigen::VectorXd ProcessorMotion::getCurrentState() const
-{
- Eigen::VectorXd x(getProblem()->getFrameStructureSize());
- getCurrentState(x);
- return x;
-}
-
inline void ProcessorMotion::getCurrentState(Eigen::VectorXd& _x) const
{
// ensure integrity
@@ -556,11 +530,6 @@ inline Motion ProcessorMotion::getMotion(const TimeStamp& _ts) const
return capture_ptr->getBuffer().getMotion(_ts);
}
-inline bool ProcessorMotion::isMotion() const
-{
- return true;
-}
-
inline double ProcessorMotion::updateDt()
{
return dt_ = incoming_ptr_->getTimeStamp() - getBuffer().get().back().ts_;
diff --git a/include/core/processor/processor_odom_2D.h b/include/core/processor/processor_odom_2d.h
similarity index 79%
rename from include/core/processor/processor_odom_2D.h
rename to include/core/processor/processor_odom_2d.h
index 491df7ada9f4b9c540740b4a9f06bb209e74f31c..6d46ab61a32c1b9573f386c97f1ae671086478c6 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"
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 e5e61d3f097779e48a179a37b51c7be5b87038c8..d7ab951643fbc2760b8d81110a9f2338a96dad36 100644
--- a/include/core/processor/processor_tracker.h
+++ b/include/core/processor/processor_tracker.h
@@ -104,15 +104,14 @@ 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();
- virtual bool isMotion() const final override {return false; }
-
-
bool checkTimeTolerance(const TimeStamp& _ts1, const TimeStamp& _ts2);
bool checkTimeTolerance(const CaptureBasePtr _cap, const TimeStamp& _ts);
bool checkTimeTolerance(const FrameBasePtr _frm, const TimeStamp& _ts);
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..3930176443a22cebb8c9591253af7f58b0a40a2c 100644
--- a/include/core/sensor/sensor_factory.h
+++ b/include/core/sensor/sensor_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/sensor/sensor_odom_2D.h b/include/core/sensor/sensor_odom_2d.h
similarity index 71%
rename from include/core/sensor/sensor_odom_2D.h
rename to include/core/sensor/sensor_odom_2d.h
index 6f16e71fef41d774539464c2c001d719fd9d50e0..d6a431d3102b0a82a218ee71247552066c1ef8eb 100644
--- a/include/core/sensor/sensor_odom_2D.h
+++ b/include/core/sensor/sensor_odom_2d.h
@@ -1,5 +1,5 @@
-#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"
@@ -7,19 +7,19 @@
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 71%
rename from include/core/sensor/sensor_odom_3D.h
rename to include/core/sensor/sensor_odom_3d.h
index b02fa3c38cfc3bdcc66063d264b67107984e5cf8..67efe1e9af27f8456401c400766dcbf32f44057b 100644
--- a/include/core/sensor/sensor_odom_3D.h
+++ b/include/core/sensor/sensor_odom_3d.h
@@ -1,12 +1,12 @@
/**
- * \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"
@@ -14,20 +14,20 @@
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 ade367fb88e7e0e987b087d31130dee0e78736ff..ef55d163be4764393c4e1b08ac0003ae67b1d223 100644
--- a/include/core/state_block/has_state_blocks.h
+++ b/include/core/state_block/has_state_blocks.h
@@ -26,22 +26,20 @@ class HasStateBlocks
void appendToStructure(const std::string& _frame_type){ structure_ += _frame_type; }
bool isInStructure(const std::string& _sb_type) { return structure_.find(_sb_type) != std::string::npos; }
- 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; }
@@ -53,26 +51,30 @@ class HasStateBlocks
// 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
- virtual void setState(const Eigen::VectorXd& _state, const bool _notify = true);
+ virtual void setState(const Eigen::VectorXd& _state, const bool _notify = true);
Eigen::VectorXd getState() const;
- void getState(Eigen::VectorXd& _state) const;
- unsigned int getSize() const;
- unsigned int getLocalSize() const;
+ void getState(Eigen::VectorXd& _state) const;
+ unsigned int getSize() const;
+ unsigned int getLocalSize() const;
+
+ // Perturb state
+ void perturb(double amplitude = 0.01);
+
private:
std::string structure_;
- std::map<std::string, StateBlockPtr> state_block_map_;
+ std::unordered_map<std::string, StateBlockPtr> state_block_map_;
};
@@ -97,7 +99,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_;
}
@@ -112,15 +114,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));
@@ -132,24 +125,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;
}
@@ -180,16 +173,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_)
@@ -272,6 +255,5 @@ inline unsigned int HasStateBlocks::getLocalSize() const
return size;
}
-
} // 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..33aa6fcb4b221ea1dc23f24b21f4a0fb12cbfc0d 100644
--- a/include/core/state_block/state_block.h
+++ b/include/core/state_block/state_block.h
@@ -142,6 +142,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);
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/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/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 6e78e69a888482381e870a0a00fef5d1aeaf2514..54e5d2a3cdedb301a7580e691cad9543df3c24c2 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");
@@ -140,7 +140,7 @@ const std::string& 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/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 ca5918624d01bb1d0d23481d4685f9d4006dd028..85f94bd2a97af8a68e4b1551d3352daa8e8906d8 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -64,29 +64,29 @@ FrameBase::FrameBase(const std::string _frame_structure, const SizeEigen _dim, c
time_stamp_(_ts)
{
if(_frame_structure.compare("PO") == 0 and _dim == 2){
- assert(_x.size() == 3 && "Wrong state vector size. Should be 3 for 2D!");
+ assert(_x.size() == 3 && "Wrong state vector size. Should be 3 for 2d!");
StateBlockPtr p_ptr ( std::make_shared<StateBlock> (_x.head <2> ( ) ) );
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 3D!");
+ assert(_x.size() == 7 && "Wrong state vector size. Should be 7 for 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 3D!");
+ assert(_x.size() == 10 && "Wrong state vector size. Should be 10 for 3d!");
StateBlockPtr p_ptr ( std::make_shared<StateBlock> (_x.head <3> ( ) ) );
StateBlockPtr o_ptr ( std::make_shared<StateQuaternion> (_x.segment <4> (3) ) );
StateBlockPtr v_ptr ( std::make_shared<StateBlock> (_x.tail <3> ( ) ) );
addStateBlock("P", p_ptr);
addStateBlock("O", o_ptr);
addStateBlock("V", v_ptr);
- this->setType("POV 3D");
+ this->setType("POV 3d");
}else{
std::cout << _frame_structure << " ^^ " << _dim << std::endl;
throw std::runtime_error("Unknown frame structure");
@@ -140,7 +140,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())
@@ -156,8 +157,8 @@ void FrameBase::setKey()
if (!isKeyOrAux())
registerNewStateBlocks(getProblem());
- // WOLF_DEBUG("Set Key", this->id());
type_ = KEY;
+
if (getTrajectory())
{
getTrajectory()->sortFrame(shared_from_this());
@@ -170,8 +171,8 @@ void FrameBase::setAux()
if (!isKeyOrAux())
registerNewStateBlocks(getProblem());
- // WOLF_DEBUG("Set Auxiliary", this->id());
type_ = AUXILIARY;
+
if (getTrajectory())
{
getTrajectory()->sortFrame(shared_from_this());
@@ -321,7 +322,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 1e24627be754f7bebdf9cf2945fa0fb6bc502bee..7c9b8b664573bd296902ad420c1e5a161605e635 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);
}
}
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 8a694beb4c8145d5d7253f0c8687ce5f6e45d82c..51c3ccd8e40ddcc2df2bfa35ed22e7a9af27e973 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -118,7 +118,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();
@@ -222,6 +222,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);
@@ -231,7 +237,7 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
// setting the main processor motion
if (prc_ptr->isMotion() && processor_motion_ptr_ == nullptr)
- processor_motion_ptr_ = std::static_pointer_cast<ProcessorMotion>(prc_ptr);
+ processor_motion_ptr_ = std::dynamic_pointer_cast<IsMotion>(prc_ptr);
return prc_ptr;
}
@@ -264,9 +270,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());
@@ -293,7 +306,7 @@ SensorBasePtr Problem::getSensor(const std::string& _sensor_name) const
return (*sen_it);
}
-ProcessorMotionPtr Problem::setProcessorMotion(const std::string& _processor_name)
+IsMotionPtr Problem::setProcessorMotion(const std::string& _processor_name)
{
for (auto sen : getHardware()->getSensorList()) // loop all sensors
{
@@ -309,7 +322,7 @@ ProcessorMotionPtr Problem::setProcessorMotion(const std::string& _processor_nam
if ((*prc_it)->isMotion()) // found, and it's motion!
{
std::cout << "Found processor '" << _processor_name << "', of type Motion, and set as the main motion processor." << std::endl;
- processor_motion_ptr_ = std::static_pointer_cast<ProcessorMotion>(*prc_it);
+ processor_motion_ptr_ = std::dynamic_pointer_cast<IsMotion>(*prc_it);
return processor_motion_ptr_;
}
else // found, but it's not motion!
@@ -324,7 +337,7 @@ ProcessorMotionPtr Problem::setProcessorMotion(const std::string& _processor_nam
return nullptr;
}
-void Problem::setProcessorMotion(ProcessorMotionPtr _processor_motion_ptr)
+void Problem::setProcessorMotion(IsMotionPtr _processor_motion_ptr)
{
processor_motion_ptr_ = _processor_motion_ptr;
}
@@ -442,7 +455,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;
@@ -497,16 +510,17 @@ bool Problem::permitAuxFrame(ProcessorBasePtr _processor_ptr) const
void Problem::auxFrameCallback(FrameBasePtr _frame_ptr, ProcessorBasePtr _processor_ptr, const double& _time_tolerance)
{
- if (_processor_ptr)
- {
- WOLF_DEBUG((_processor_ptr->isMotion() ? "PM " : "PT "), _processor_ptr->getName(), ": AuxF", _frame_ptr->id(), " Callback emitted with ts = ", _frame_ptr->getTimeStamp());
- }
- else
- {
- WOLF_DEBUG("External callback: AuxF", _frame_ptr->id(), " Callback emitted with ts = ", _frame_ptr->getTimeStamp());
- }
-
- processor_motion_ptr_->keyFrameCallback(_frame_ptr, _time_tolerance);
+ // TODO
+// if (_processor_ptr)
+// {
+// WOLF_DEBUG((_processor_ptr->isMotion() ? "PM " : "PT "), _processor_ptr->getName(), ": AuxF", _frame_ptr->id(), " Callback emitted with ts = ", _frame_ptr->getTimeStamp());
+// }
+// else
+// {
+// WOLF_DEBUG("External callback: AuxF", _frame_ptr->id(), " Callback emitted with ts = ", _frame_ptr->getTimeStamp());
+// }
+//
+// processor_motion_ptr_->keyFrameCallback(_frame_ptr, _time_tolerance);
}
StateBlockPtr Problem::notifyStateBlock(StateBlockPtr _state_ptr, Notification _noti)
@@ -516,6 +530,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())
{
@@ -829,7 +844,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));
@@ -1806,4 +1821,23 @@ 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())
+ {
+ 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 39b77e59f6980f1a2047db5527a017358572159b..a35097f2c70d3b047117ae6d0c9bf81a5fe2f5da 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());
@@ -71,7 +72,7 @@ void ProcessorBase::remove()
if (isMotion())
{
ProblemPtr P = getProblem();
- if(P && P->getProcessorMotion()->id() == this->id())
+ if(P && (P->getProcessorMotion() == std::dynamic_pointer_cast<IsMotion>( shared_from_this() ) ) )
P->clearProcessorMotion();
}
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 c667b7193bc3b566782351371c7d4f314799a93b..634d1f6d1284997573ef763bd329dcdf37cc94d3 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -1,15 +1,20 @@
+
+
#include "core/processor/processor_motion.h"
+
+
namespace wolf
{
ProcessorMotion::ProcessorMotion(const std::string& _type,
+ 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),
@@ -361,10 +366,13 @@ 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);
+ // ensure proper size of the provided reference
+ _x.resize( getProblem()->getFrameStructureSize() );
+
// Compose on top of origin state using the buffered time stamp, not the query time stamp
double dt = motion.ts_ - capture_motion->getBuffer().get().front().ts_;
statePlusDelta(state_0, delta, dt, _x);
@@ -613,7 +621,7 @@ PackKeyFramePtr ProcessorMotion::computeProcessingStep()
void ProcessorMotion::setProblem(ProblemPtr _problem)
{
- if (_problem == nullptr || _problem == this->getProblem())
+ 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 5ad5e732d52937857a50e4ee3c4826ebfcf4f6d8..c986ebf7cf9cd7fb83df1417ea19c42beed66c74 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", 3, 3, 3, 2, 0, _params),
- params_odom_2D_(_params)
+ProcessorOdom2d::ProcessorOdom2d(ProcessorParamsOdom2dPtr _params) :
+ ProcessorMotion("ProcessorOdom2d", 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 86%
rename from src/processor/processor_odom_3D.cpp
rename to src/processor/processor_odom_3d.cpp
index f0241f7e1ce62ee203948fb6f37394b4bcd4e3ce..967031e9ccd1a68fcee41567be0f7dbf7523ef2c 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", 7, 7, 6, 6, 0, _params),
- params_odom_3D_ (_params),
+ProcessorOdom3d::ProcessorOdom3d(ProcessorParamsOdom3dPtr _params) :
+ ProcessorMotion("ProcessorOdom3d", 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/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..d32d5c6216c857ad703c34aa1a10f4b29d6c4dc0 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -79,6 +79,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 +137,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})
@@ -157,25 +165,25 @@ 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})
-# 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 +206,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)
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/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_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..d8b46e9b721fa4f9947138eee2655ed3ad25bff0
--- /dev/null
+++ b/test/gtest_has_state_blocks.cpp
@@ -0,0 +1,171 @@
+/**
+ * \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) , "" );
+
+}
+
+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_parser_yaml.cpp b/test/gtest_parser_yaml.cpp
index d464c1120e6830c9912d0f16472c53f74b4f9bac..c8702619bc5150eb688bcb7dccfa11f71ab909e5 100644
--- a/test/gtest_parser_yaml.cpp
+++ b/test/gtest_parser_yaml.cpp
@@ -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 1b74256d81808822dd1de31eed99e83098511c4d..e0ba9a42a17936b62ec329c1fb022052d5ae6c30 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -10,12 +10,17 @@
#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/state_block/state_quaternion.h"
+
#include <iostream>
using namespace wolf;
@@ -92,10 +97,10 @@ TEST(Problem, Processor)
ASSERT_FALSE(P->getProcessorMotion());
// 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->getProcessorMotion(), Pm);
@@ -107,7 +112,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,16 +121,16 @@ TEST(Problem, Installers)
ASSERT_FALSE(P->getProcessorMotion());
// 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->getProcessorMotion());
// check motion processor is correct
- ASSERT_EQ(P->getProcessorMotion(), pm);
+ ASSERT_EQ(std::dynamic_pointer_cast<ProcessorMotion>(P->getProcessorMotion()), pm);
}
-TEST(Problem, SetOrigin_PO_2D)
+TEST(Problem, SetOrigin_PO_2d)
{
ProblemPtr P = Problem::create("PO", 2);
TimeStamp t0(0);
@@ -162,7 +167,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 +212,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 +238,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 +292,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 +302,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 +323,96 @@ 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) );
+ }
+ }
+}
+
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 2aef08f59cfcb594a34dce910a9f7f54eb389671..1933da8599234e61c534e9f05008710ac4396661 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"
@@ -31,6 +31,41 @@ WOLF_REGISTER_PROCESSOR("TRACKER FEATURE DUMMY", ProcessorTrackerFeatureDummy)
} // namespace wolf
+TEST(ProcessorBase, IsMotion)
+{
+ using namespace wolf;
+ using std::shared_ptr;
+ using std::make_shared;
+ using std::static_pointer_cast;
+ using Eigen::Vector2d;
+
+ std::string wolf_root = _WOLF_ROOT_DIR;
+
+ double dt = 0.01;
+
+ // Wolf problem
+ ProblemPtr problem = Problem::create("PO", 2);
+
+ // Install tracker (sensor and processor)
+ auto sens_trk = SensorBase::emplace<SensorBase>(problem->getHardware(),
+ "SensorDummy",
+ nullptr,
+ nullptr,
+ nullptr,
+ 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>();
+ proc_odo_params->time_tolerance = dt/2;
+ ProcessorBasePtr proc_odo = problem->installProcessor("ProcessorOdom2d", "odom processor", sens_odo, proc_odo_params);
+
+ ASSERT_FALSE(proc_trk->isMotion());
+ ASSERT_TRUE (proc_odo->isMotion());
+}
+
+
TEST(ProcessorBase, KeyFrameCallback)
{
@@ -49,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;
@@ -71,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));
@@ -93,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_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/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}