diff --git a/demos/demo_projection_points.cpp b/demos/demo_projection_points.cpp
deleted file mode 100644
index 6d6f943ab4b3ee11ea03965f8a872fc3454a9f7b..0000000000000000000000000000000000000000
--- a/demos/demo_projection_points.cpp
+++ /dev/null
@@ -1,468 +0,0 @@
-
-// Vision utils
-#include <vision_utils/vision_utils.h>
-
-//std includes
-#include <iostream>
-
-//wolf includes
-#include "core/math/pinhole_tools.h"
-
-int main(int argc, char** argv)
-{
- using namespace wolf;
-
- std::cout << std::endl << " ========= ProjectPoints test ===========" << std::endl << std::endl;
-
- Eigen::MatrixXs points3D(2,3);
- Eigen::Vector3s point3D;
- point3D[0] = 2.0;
- point3D[1] = 5.0;
- point3D[2] = 6.0;
- points3D.row(0)= point3D;
- point3D[0] = 4.0;
- point3D[1] = 2.0;
- point3D[2] = 1.0;
- points3D.row(1)= point3D;
-
- Eigen::Vector3s cam_ext_rot_mat = Eigen::Vector3s::Zero();
- Eigen::Vector3s cam_ext_trans_mat = Eigen::Vector3s::Ones();
-
- Eigen::Matrix3s cam_intr_mat;
- cam_intr_mat = Eigen::Matrix3s::Identity();
- cam_intr_mat(0,2)=2;
- cam_intr_mat(1,2)=2;
-
- Eigen::VectorXs dist_coef(5);
- dist_coef << 0,0,0,0,0;
-
- Eigen::MatrixXs points2D = vision_utils::projectPoints(points3D, cam_ext_rot_mat, cam_ext_trans_mat, cam_intr_mat, dist_coef);
-
- for (int ii = 0; ii < points3D.rows(); ++ii)
- std::cout << "points2D- X: " << points2D(ii,0) << "; Y: " << points2D(ii,1) << std::endl;
-
- std::cout << std::endl << " ========= PinholeTools DUALITY TEST ===========" << std::endl << std::endl;
-
- //================================= projectPoint and backprojectPoint to/from NormalizedPlane
-
- Eigen::Vector3s project_point_normalized_test;
- project_point_normalized_test[0] = 1.06065;
- project_point_normalized_test[1] = 1.06065;
- project_point_normalized_test[2] = 3;
- Eigen::Vector2s project_point_normalized_output;
- Eigen::Vector2s project_point_normalized_output2;
- Scalar project_point_normalized_dist;
-
- Scalar backproject_point_normalized_depth = 3;
- Eigen::Vector3s backproject_point_normalized_output;
-
- project_point_normalized_output = pinhole::projectPointToNormalizedPlane(project_point_normalized_test);
-
- pinhole::projectPointToNormalizedPlane(project_point_normalized_test,
- project_point_normalized_output2,
- project_point_normalized_dist);
-
- backproject_point_normalized_output =
- pinhole::backprojectPointFromNormalizedPlane(project_point_normalized_output,
- backproject_point_normalized_depth);
-
- std::cout << "TEST project and backproject PointToNormalizedPlane" << std::endl;
- std::cout << std:: endl << "Original " << project_point_normalized_test.transpose() << std::endl;
- std::cout << std:: endl << "Project " << project_point_normalized_output.transpose() << std::endl;
- std::cout << std:: endl << "Alternate project" << project_point_normalized_output.transpose() << std::endl;
- std::cout << std:: endl << "Backproject " << backproject_point_normalized_output.transpose() << std::endl;
-
- //================================= projectPoint and backprojectPoint to/from NormalizedPlane WITH JACOBIANS
-
- Eigen::Vector3s pp_normalized_test;
- pp_normalized_test[0] = 3;
- pp_normalized_test[1] = 3;
- pp_normalized_test[2] = 3;
- Eigen::Vector2s pp_normalized_output;
- Eigen::Vector2s pp_normalized_output2;
- Eigen::Matrix<Scalar, 2, 3> pp_normalized_jacobian;
- Eigen::Matrix<Scalar, 2, 3> pp_normalized_jacobian2;
- Scalar pp_normalized_distance;
-
- Scalar bpp_normalized_depth = 3;
- Eigen::Vector3s bpp_normalized_output;
- Eigen::Matrix<Scalar, 3, 2> bpp_normalized_jacobian;
- Eigen::Vector3s bpp_normalized_jacobian_depth;
-
- pinhole::projectPointToNormalizedPlane(pp_normalized_test,
- pp_normalized_output,
- pp_normalized_jacobian);
- pinhole::projectPointToNormalizedPlane(pp_normalized_test,
- pp_normalized_output2,
- pp_normalized_distance,
- pp_normalized_jacobian2);
-
- pinhole::backprojectPointFromNormalizedPlane(pp_normalized_output,
- bpp_normalized_depth,
- bpp_normalized_output,
- bpp_normalized_jacobian,
- bpp_normalized_jacobian_depth);
-
- std::cout << "\n--------------------------------------------------------" << std::endl;
- std::cout << "\nTEST project and backproject PointToNormalizedPlane with JACOBIAN" << std::endl;
-
- std::cout << std:: endl << "Original" << pp_normalized_test.transpose() << std::endl;
- std::cout << std:: endl << "Project" << pp_normalized_output.transpose() << std::endl;
- std::cout << "\n--> Jacobian" << std::endl << pp_normalized_jacobian << std::endl;
-
- std::cout << std:: endl << "Alternate project" << pp_normalized_output2.transpose() << "; distance: "
- << pp_normalized_distance << std::endl;
- std::cout << "\n--> Jacobian" << std::endl << pp_normalized_jacobian2 << std::endl;
-
- std::cout << std:: endl << "Backproject" << bpp_normalized_output.transpose()
- << "; depth: " << bpp_normalized_depth << std::endl;
- std::cout << "\n--> Jacobian" << std::endl << bpp_normalized_jacobian << std::endl;
- std::cout << "\n--> Jacobian - depth" << bpp_normalized_jacobian_depth.transpose() << std::endl;
-
- Eigen::Matrix2s test_jacobian; // (2x3) * (3x2) = (2x2)
- test_jacobian = pp_normalized_jacobian * bpp_normalized_jacobian;
-
- std::cout << "\n\n\n==> Jacobian Testing" << std::endl << test_jacobian << std::endl;
-
- //================================= IsInRoi / IsInImage
-
- Eigen::Vector2s pix;
- pix[0] = 40; // x
- pix[1] = 40; // y
-
- int roi_x = 30;
- int roi_y = 30;
- int roi_width = 20;
- int roi_height = 20;
-
- int image_width = 640;
- int image_height = 480;
-
- bool is_in_roi;
- bool is_in_image;
- is_in_roi = pinhole::isInRoi(pix,
- roi_x,
- roi_y,
- roi_width,
- roi_height);
- is_in_image = pinhole::isInImage(pix,
- image_width,
- image_height);
-
- std::cout << "\n--------------------------------------------------------" << std::endl;
- std::cout << "\nTEST isInRoi/isInImage" << std::endl;
- std::cout << std::endl << "Pixel " << std::endl;
- std::cout << "x: " << pix[0] << "; y: " << pix[1] << std::endl;
- std::cout << std::endl << "ROI " << std::endl;
- std::cout << "x: " << roi_x << "; y: " << roi_y << "; width: " << roi_width << "; height: " << roi_height << std::endl;
- std::cout << "is_in_roi: " << is_in_roi << std::endl;
- std::cout << std::endl << "Image " << std::endl;
- std::cout << "width: " << image_width << "; height: " << image_height << std::endl;
- std::cout << "is_in_image: " << is_in_image << std::endl;
-
- //================================= computeCorrectionModel
-
- Eigen::Vector2s distortion2;
- distortion2[0] = -0.301701;
- distortion2[1] = 0.0963189;
- Eigen::Vector4s k_test2;
- //k = [u0, v0, au, av]
- k_test2[0] = 516.686; //u0
- k_test2[1] = 355.129; //v0
- k_test2[2] = 991.852; //au
- k_test2[3] = 995.269; //av
-
- Eigen::Vector2s correction_test2;
- pinhole::computeCorrectionModel(k_test2,
- distortion2,
- correction_test2);
-
- std::cout << "\n--------------------------------------------------------" << std::endl;
- std::cout << "\nTEST computeCorrectionModel" << std::endl;
- std::cout << std::endl << "distortion" << std::endl;
- std::cout << "d1: " << distortion2[0] << "; d2: " << distortion2[1] << std::endl;
- std::cout << std::endl << "k values" << std::endl;
- std::cout << "u0: " << k_test2[0] << "; v0: " << k_test2[1] << "; au: " << k_test2[2] << "; av: " << k_test2[3] << std::endl;
- std::cout << std::endl << "correction" << std::endl;
- std::cout << "c1: " << correction_test2[0] << "; c2: " << correction_test2[1] << std::endl;
-
- //================================= distortPoint
-
- Eigen::Vector2s distorting_point;
- distorting_point[0] = 0.35355;
- distorting_point[1] = 0.35355;
-
- Eigen::Vector2s distored_point3;
- distored_point3 = pinhole::distortPoint(distortion2,
- distorting_point);
-
- std::cout << "\n--------------------------------------------------------" << std::endl;
- std::cout << "\nTEST distortPoint" << std::endl;
- std::cout << std::endl << "Point to be distorted" << std::endl;
- std::cout << "x: " << distorting_point[0] << "; y: " << distorting_point[1] << std::endl;
- std::cout << std::endl << "Distorted point" << std::endl;
- std::cout << "x: " << distored_point3[0] << "; y: " << distored_point3[1] << std::endl;
-
- Eigen::Vector2s corrected_point4;
- corrected_point4 = pinhole::undistortPoint(correction_test2,
- distored_point3);
- std::cout << std::endl << "Corrected point" << std::endl;
- std::cout << "x: " << corrected_point4[0] << "; y: " << corrected_point4[1] << std::endl;
-
- ////
-
- Eigen::Vector2s distored_point4;
- Eigen::Matrix2s distortion_jacobian2;
- pinhole::distortPoint(distortion2,
- distorting_point,
- distored_point4,
- distortion_jacobian2);
-
- std::cout << "\n\nTEST distortPoint, jacobian" << std::endl;
- std::cout << std::endl << "Point to be distorted" << std::endl;
- std::cout << "x: " << distorting_point[0] << "; y: " << distorting_point[1] << std::endl;
- std::cout << std::endl << "Distorted point" << std::endl;
- std::cout << "x: " << distored_point4[0] << "; y: " << distored_point4[1] << std::endl;
- std::cout << "\n--> Jacobian" << std::endl <<
- distortion_jacobian2 << std::endl;
-
- Eigen::Vector2s corrected_point5;
- Eigen::Matrix2s corrected_jacobian2;
- pinhole::undistortPoint(correction_test2,
- distored_point4,
- corrected_point5,
- corrected_jacobian2);
-
- std::cout << std::endl << "Corrected point" << std::endl;
- std::cout << "x: " << corrected_point5[0] << "; y: " << corrected_point5[1] << std::endl;
- std::cout << "\n--> Jacobian" << std::endl <<
- corrected_jacobian2 << std::endl;
-
- Eigen::Matrix2s test_jacobian_distortion;
- test_jacobian_distortion = distortion_jacobian2 * corrected_jacobian2;
-
- std::cout << "\n\n\n==> Jacobian Testing" << std::endl <<
- test_jacobian_distortion << std::endl;
-
- //================================= PixelizePoint
-
- Eigen::Vector2s pixelize_ud;
- pixelize_ud[0] = 45;
- pixelize_ud[1] = 28;
-
- Eigen::Vector2s pixelize_output3;
- pixelize_output3 = pinhole::pixellizePoint(k_test2,
- pixelize_ud);
-
- std::cout << "\n--------------------------------------------------------" << std::endl;
- std::cout << "\nTEST pixelizePoint; Eigen::Vector2s" << std::endl;
- std::cout << std::endl << "Original" << std::endl;
- std::cout << "x: " << pixelize_ud[0] << "; y: " << pixelize_ud[1] << std::endl;
- std::cout << std::endl << "Pixelized" << std::endl;
- std::cout << "x: " << pixelize_output3[0] << "; y: " << pixelize_output3[1] << std::endl;
-
- Eigen::Vector2s depixelize_output3;
- depixelize_output3 = pinhole::depixellizePoint(k_test2,
- pixelize_output3);
- std::cout << std::endl << "Depixelized" << std::endl;
- std::cout << "x: " << depixelize_output3[0] << "; y: " << depixelize_output3[1] << std::endl;
-
- ////
-
- Eigen::Vector2s pixelize_output4;
- Eigen::Matrix2s pixelize_jacobian2;
- pinhole::pixellizePoint(k_test2,
- pixelize_ud,
- pixelize_output4,
- pixelize_jacobian2);
-
- std::cout << std::endl << "TEST pixelizePoint; Jacobians" << std::endl;
- std::cout << std::endl << "Original" << std::endl;
- std::cout << "x: " << pixelize_ud[0] << "; y: " << pixelize_ud[1] << std::endl;
- std::cout << std::endl << "Pixelized" << std::endl;
- std::cout << "x: " << pixelize_output4[0] << "; y: " << pixelize_output4[1] << std::endl;
- std::cout << "\n--> Jacobian" << std::endl <<
- pixelize_jacobian2 << std::endl;
-
- Eigen::Vector2s depixelize_output4;
- Eigen::Matrix2s depixelize_jacobian2;
- pinhole::depixellizePoint(k_test2,
- pixelize_output4,
- depixelize_output4,
- depixelize_jacobian2);
-
- std::cout << std::endl << "Depixelized" << std::endl;
- std::cout << "x: " << depixelize_output4[0] << "; y: " << depixelize_output4[1] << std::endl;
- std::cout << "\n--> Jacobian" << std::endl <<
- depixelize_jacobian2 << std::endl;
-
- Eigen::Matrix2s test_jacobian_pix;
- test_jacobian_pix = pixelize_jacobian2 * depixelize_jacobian2;
-
- std::cout << "\n\n\n==> Jacobian Testing" << std::endl <<
- test_jacobian_pix << std::endl;
-
- //================================= projectPoint Complete
-
-// //distortion
-// distortion2;
-
-// //k
-// k_test2;
-
-// //3Dpoint
-// project_point_normalized_test;
-
- Eigen::Vector2s point2D_test5;
- point2D_test5 = pinhole::projectPoint(k_test2,
- distortion2,
- project_point_normalized_test);
-
- std::cout << "\n--------------------------------------------------------" << std::endl;
- std::cout << std::endl << "TEST projectPoint Complete" << std::endl;
- std::cout << "\nPARAMS" << std::endl;
- std::cout << "\nDistortion:" << std::endl;
- std::cout << "d1: " << distortion2[0] << "; d2: " << distortion2[1] << std::endl << std::endl;
- std::cout << "k values:" << std::endl;
- std::cout << "u0: " << k_test2[0] << "; v0: " << k_test2[1] << "; au: " << k_test2[2] << "; av: " << k_test2[3] << std::endl << std::endl;
- std::cout << "3D Point" << std::endl;
- std::cout << "x: " << project_point_normalized_test[0] << "; y: " << project_point_normalized_test[1]
- << "; z: " << project_point_normalized_test[2] << std::endl;
- std::cout << "\n\n\nFirst function output" << std::endl;
- std::cout << "x: " << point2D_test5[0] << "; y: " << point2D_test5[1] << std::endl;
-
- //distance
- Eigen::Vector2s point2D_test6;
- Scalar distance_test4;
- pinhole::projectPoint(k_test2,
- distortion2,
- project_point_normalized_test,
- point2D_test6,
- distance_test4);
-
- std::cout << std::endl << "Second function output" << std::endl;
- std::cout << "x: " << point2D_test6[0] << "; y: " << point2D_test6[1] << "; dist: " << distance_test4 << std::endl;
-
- //jacobian
- Eigen::Vector2s point2D_test7;
- Eigen::MatrixXs jacobian_test3(2,3);
- pinhole::projectPoint(k_test2,
- distortion2,
- project_point_normalized_test,
- point2D_test7,
- jacobian_test3);
-
- std::cout << std::endl << "Third function output" << std::endl;
- std::cout << "x: " << point2D_test7[0] << "; y: " << point2D_test7[1] << std::endl;
- std::cout << "\n-->Jacobian" << std::endl <<
- jacobian_test3 << std::endl;
-
- //jacobian and distance
- Eigen::Vector2s point2D_test8;
- Eigen::MatrixXs jacobian_test4(2,3);
- Scalar distance_test3;
- pinhole::projectPoint(k_test2,
- distortion2,
- project_point_normalized_test,
- point2D_test8,
- distance_test3,
- jacobian_test4);
-
- std::cout << std::endl << "Fourth function output" << std::endl;
- std::cout << "x: " << point2D_test8[0] << "; y: " << point2D_test8[1] << "; dist: " << distance_test3 << std::endl;
- std::cout << "\n-->Jacobian" << std::endl <<
- jacobian_test4 << std::endl;
-
- /////////////////////////////
-
-// //correction
-// correction_test2
-
-// //2Dpoint
-// point2D_test5
-
- Scalar depth3 = project_point_normalized_test[2];
-
- Eigen::Vector3s point3D_backproj5;
- point3D_backproj5 = pinhole::backprojectPoint(k_test2,
- correction_test2,
- point2D_test5,
- depth3);
-
- std::cout << "\n\nTEST backprojectPoint Complete" << std::endl;
- std::cout << std::endl << "First function output" << std::endl;
- std::cout << "x: " << point3D_backproj5[0] << "; y: " << point3D_backproj5[1] << "; z: " << point3D_backproj5[2] << std::endl;
-
- //jacobian
- Eigen::Vector3s point3D_backproj4;
- Eigen::MatrixXs jacobian_backproj2(3,2);
- Eigen::Vector3s depth_jacobian2;
- pinhole::backprojectPoint(k_test2,
- correction_test2,
- point2D_test7,
- depth3,
- point3D_backproj4,
- jacobian_backproj2,
- depth_jacobian2);
-
- std::cout << std::endl << "Second function output" << std::endl;
- std::cout << "x: " << point3D_backproj4[0] << "; y: " << point3D_backproj4[1] << "; z: " << point3D_backproj4[2] << std::endl;
- std::cout << "\n--> Jacobian" << std::endl <<
- jacobian_backproj2 << std::endl;
- std::cout << "\n--> Jacobian - depth" << std::endl <<
- depth_jacobian2[0] << " " << depth_jacobian2[1] << " " << depth_jacobian2[2] << " " << std::endl;
-
- Eigen::Matrix2s test_jacobian_complete;
- test_jacobian_complete = jacobian_test4 * jacobian_backproj2;
-
- std::cout << "\n\n\n==> Jacobian Testing" << std::endl <<
- test_jacobian_complete << std::endl;
-
- /* Test */
- std::cout << "\n\n\n\nTEST\n" << std::endl;
-
- Eigen::Matrix3s K;
- K(0,0) = 830.748734; K(0,1) = 0; K(0,2) = 327.219132;
- K(1,0) = 0; K(1,1) = 831.18208; K(1,2) = 234.720244;
- K(2,0) = 0; K(2,1) = 0; K(2,2) = 1;
-
- Eigen::Vector4s K_params = {327.219132,234.720244, 830.748734,831.18208};
-
- std::cout << "K:\n" << K << std::endl;
-
- Eigen::Vector4s distortion_vector = {0.0006579999999999999, 0.023847, -0.001878, 0.007706999999999999};
-
- std::cout << "\nDistortion vector:\n" << distortion_vector << std::endl;
-
- Eigen::Vector4s correction_vector;
- pinhole::computeCorrectionModel(K_params,
- distortion_vector,
- correction_vector);
-
- std::cout << "\nCorrection vector:\n" << correction_vector << std::endl;
-
- Eigen::Vector3s test_point3D;
- Eigen::Vector2s test_point2D = {123,321};
- std::cout << "\ntest_point2D ORIGINAL:\n" << test_point2D << std::endl;
-
- test_point2D = pinhole::depixellizePoint(K_params,
- test_point2D);
- std::cout << "\ntest_point2D DEPIXELIZED:\n" << test_point2D << std::endl;
- test_point2D = pinhole::undistortPoint(correction_vector,
- test_point2D);
- std::cout << "\ntest_point2D UNDISTORTED:\n" << test_point2D << std::endl;
- test_point3D = pinhole::backprojectPointFromNormalizedPlane(test_point2D,
- 2);
- std::cout << "\ntest_point3D BACKPROJECTED:\n" << test_point3D << std::endl;
-
- test_point2D = pinhole::projectPointToNormalizedPlane(test_point3D);
- std::cout << "\ntest_point2D NORMALIZED:\n" << test_point2D << std::endl;
- test_point2D = pinhole::distortPoint(distortion_vector,
- test_point2D);
- std::cout << "\ntest_point2D DISTORTED:\n" << test_point2D << std::endl;
- test_point2D = pinhole::pixellizePoint(K_params,
- test_point2D);
- std::cout << "\ntest_point2D PIXELIZED:\n" << test_point2D << std::endl;
-
-}
-
diff --git a/demos/processor_odom_3D.yaml b/demos/processor_odom_3D.yaml
index f501e333800ec1bbb4b7c751a32aa67a99bec74c..82fdecf637f5a14431fc8ff5b66e7e7415ffcfd2 100644
--- a/demos/processor_odom_3D.yaml
+++ b/demos/processor_odom_3D.yaml
@@ -1,8 +1,11 @@
processor type: "ODOM 3D" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
processor name: "Main odometer" # This is ignored. The name provided to the SensorFactory prevails
+
time tolerance: 0.01 # seconds
-keyframe vote:
- max time span: 0.2 # seconds
- max buffer length: 10 # motion deltas
- dist traveled: 0.5 # meters
- angle turned: 0.1 # radians (1 rad approx 57 deg, approx 60 deg)
\ No newline at end of file
+
+voting_active: false
+voting_aux_active: false
+k_max_time span: 0.2 # seconds
+k_max_buff_length: 10 # motion deltas
+k_dist_traveled: 0.5 # meters
+k_angle_turned: 0.1 # radians (1 rad approx 57 deg, approx 60 deg)
\ No newline at end of file
diff --git a/demos/sensor_odom_3D.yaml b/demos/sensor_odom_3D.yaml
index 9ea77803548cfd9d033f54e40b6d92a72710afb8..d0c1f0b0e4d025090170cbbd85ea3ae4cfc9f62b 100644
--- a/demos/sensor_odom_3D.yaml
+++ b/demos/sensor_odom_3D.yaml
@@ -1,8 +1,8 @@
-sensor type: "ODOM 3D" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
-sensor name: "Main odometer" # This is ignored. The name provided to the SensorFactory prevails
-motion variances:
- disp_to_disp: 0.02 # m^2 / m
- disp_to_rot: 0.02 # rad^2 / m
- rot_to_rot: 0.01 # rad^2 / rad
- min_disp_var: 0.01 # m^2
- min_rot_var: 0.01 # rad^2
+type: "ODOM 3D" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+name: "Main odometer" # This is ignored. The name provided to the SensorFactory prevails
+
+k_disp_to_disp: 0.02 # m^2 / m
+k_disp_to_rot: 0.02 # rad^2 / m
+k_rot_to_rot: 0.01 # rad^2 / rad
+min_disp_var: 0.01 # m^2
+min_rot_var: 0.01 # rad^2
diff --git a/demos/sensor_odom_3D_HQ.yaml b/demos/sensor_odom_3D_HQ.yaml
index 08945ef842e46f28642f1be63ca95850b618a35e..359ada0dfed9bbb4debc6a11e83dad494ecf258c 100644
--- a/demos/sensor_odom_3D_HQ.yaml
+++ b/demos/sensor_odom_3D_HQ.yaml
@@ -1,8 +1,8 @@
-sensor type: "ODOM 3D" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
-sensor name: "Main odometer" # This is ignored. The name provided to the SensorFactory prevails
-motion variances:
- disp_to_disp: 0.000001 # m^2 / m
- disp_to_rot: 0.000001 # rad^2 / m
- 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
+type: "ODOM 3D" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+name: "Main odometer" # This is ignored. The name provided to the SensorFactory prevails
+
+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
+k_min_disp_var: 0.00000001 # m^2
+k_min_rot_var: 0.00000001 # rad^2
\ No newline at end of file
diff --git a/hello_wolf/hello_wolf_autoconf.cpp b/hello_wolf/hello_wolf_autoconf.cpp
index 8921a4698485119155fb9ac59699ec3abef2687f..a4500bf3b441ee640a74c58a67197eb4b168aff7 100644
--- a/hello_wolf/hello_wolf_autoconf.cpp
+++ b/hello_wolf/hello_wolf_autoconf.cpp
@@ -101,7 +101,7 @@ int main()
using namespace wolf;
- WOLF_TRACE("======== CONFIGURE PROBLEM =======")
+ WOLF_TRACE("======== CONFIGURE PROBLEM =======");
// Config file to parse. Here is where all the problem is defined:
std::string file = std::string(_WOLF_ROOT_DIR) + "/hello_wolf/hello_wolf_config.yaml";
@@ -151,7 +151,7 @@ int main()
// SET OF EVENTS: make things happen =======================================================
std::cout << std::endl;
- WOLF_TRACE("======== BUILD PROBLEM =======")
+ WOLF_TRACE("======== START ROBOT MOVE AND SLAM =======")
// We'll do 3 steps of motion and landmark observations.
diff --git a/hello_wolf/hello_wolf_config.yaml b/hello_wolf/hello_wolf_config.yaml
index 13472f71bde49c1e5f46272ea248274f8e77a4aa..da25853a6a6222a181560fecfa1fe828a542a0b3 100644
--- a/hello_wolf/hello_wolf_config.yaml
+++ b/hello_wolf/hello_wolf_config.yaml
@@ -1,45 +1,54 @@
config:
problem:
- frame structure: "PO"
+ frame_structure: "PO"
dimension: 2
- prior timestamp: 0.0
- prior state: [0,0,0]
- prior cov: [[3,3],.01,0,0,0,.01,0,0,0,.01]
- prior time tolerance: 0.1
+ prior:
+ timestamp: 0.0
+ state: [0,0,0]
+ cov: [[3,3],.01,0,0,0,.01,0,0,0,.01]
+ time_tolerance: 0.1
-
sensors:
-
type: "ODOM 2D"
name: "odom"
+ k_disp_to_disp: 0.1
+ k_rot_to_rot: 0.1
+ extrinsic:
+ pose: [0,0,0]
-
type: "RANGE BEARING"
name: "rb"
- noise range metres std: 0.1
- noise bearing degrees std: 0.5
-
+ noise_range_metres_std: 0.1
+ noise_bearing_degrees_std: 0.5
+
processors:
-
- type: "ODOM 2D"
- name: "odom"
- sensor name: "odom"
- voting active: true
- time tolerance: 0.1
- max time span: 999
- dist_traveled: 0.95
- angle_turned: 999
- cov_det: 999
+ type: "ODOM 2D"
+ name: "odom"
+ sensor_name: "odom"
+ time_tolerance: 0.1
+ voting_active: true
+ voting_aux_active: false
+ max_time_span: 999
+ dist_traveled: 0.95
+ angle_turned: 999
+ max_buff_length: 999
+ cov_det: 999
unmeasured_perturbation_std: 0.001
-
type: "RANGE BEARING"
name: "rb"
- sensor name: "rb"
+ sensor_name: "rb"
+ voting_active: false
+ voting_aux_active: false
+ time_tolerance: 0.1
files:
-# - "/Users/jsola/dev/wolf_lib/core/lib/libhellowolf.dylib"
+# - "/Users/jsola/dev/wolf lib/core/lib/libhellowolf.dylib"
\ No newline at end of file
diff --git a/hello_wolf/processor_range_bearing.cpp b/hello_wolf/processor_range_bearing.cpp
index dbd86a30e2a5c3499c4aca126f12f875d98b0a48..5685c411998415689aa8d795b4f8c61417382f3d 100644
--- a/hello_wolf/processor_range_bearing.cpp
+++ b/hello_wolf/processor_range_bearing.cpp
@@ -120,9 +120,7 @@ ProcessorBasePtr ProcessorRangeBearing::createAutoConf(const std::string& _uniqu
const SensorBasePtr _sensor_ptr)
{
SensorRangeBearingPtr sensor_rb = std::static_pointer_cast<SensorRangeBearing>(_sensor_ptr);
- ProcessorParamsRangeBearingPtr params = std::make_shared<ProcessorParamsRangeBearing>();
- params->voting_active = _server.getParam<bool>(_unique_name + "/voting_active", "false");
- params->time_tolerance = _server.getParam<double>(_unique_name + "/time_tolerance", "0.01");
+ ProcessorParamsRangeBearingPtr params = std::make_shared<ProcessorParamsRangeBearing>(_unique_name, _server);
// construct processor
ProcessorRangeBearingPtr prc = std::make_shared<ProcessorRangeBearing>(sensor_rb, params);
diff --git a/hello_wolf/sensor_range_bearing.h b/hello_wolf/sensor_range_bearing.h
index 161818df65a56f5b2df02d9a66327a71e80a63a0..2c33482365adf14e1b5ec468cc8cc4ca51c65154 100644
--- a/hello_wolf/sensor_range_bearing.h
+++ b/hello_wolf/sensor_range_bearing.h
@@ -26,8 +26,8 @@ struct IntrinsicsRangeBearing : public IntrinsicsBase
IntrinsicsRangeBearing(std::string _unique_name, const ParamsServer& _server):
IntrinsicsBase(_unique_name, _server)
{
- noise_range_metres_std = _server.getParam<Scalar>(_unique_name + "/noise range metres std", "0.05");
- noise_bearing_degrees_std = _server.getParam<Scalar>(_unique_name + "/noise bearing degrees std", "0.5");
+ noise_range_metres_std = _server.getParam<Scalar>(_unique_name + "/noise_range_metres_std");
+ noise_bearing_degrees_std = _server.getParam<Scalar>(_unique_name + "/noise_bearing_degrees_std");
}
std::string print()
{
diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h
index 95d96f22fe0db0dfcb2a8bf804367831c9c45147..3537f96032ba64bbbb901f722bd64bdf10bbe0ab 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.h
@@ -179,9 +179,9 @@ struct ProcessorParamsBase : public ParamsBase
ProcessorParamsBase(std::string _unique_name, const ParamsServer& _server):
ParamsBase(_unique_name, _server)
{
- voting_active = _server.getParam<bool>(_unique_name + "/voting_active", "false");
- voting_aux_active = _server.getParam<bool>(_unique_name + "/voting_aux_active", "false");
- time_tolerance = _server.getParam<Scalar>(_unique_name + "/time_tolerance", "0");
+ voting_active = _server.getParam<bool>(_unique_name + "/voting_active");
+ voting_aux_active = _server.getParam<bool>(_unique_name + "/voting_aux_active");
+ time_tolerance = _server.getParam<Scalar>(_unique_name + "/time_tolerance");
}
virtual ~ProcessorParamsBase() = default;
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index b06cf3390b6175ac989df88d9265542759955407..6a96be5ef0c13a299e687bafb77bf38fbd48ede1 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -34,15 +34,16 @@ struct ProcessorParamsMotion : public ProcessorParamsBase
ProcessorParamsMotion(std::string _unique_name, const ParamsServer& _server):
ProcessorParamsBase(_unique_name, _server)
{
- max_time_span = _server.getParam<Scalar>(_unique_name + "/max_time_span", "0.5");
- max_buff_length = _server.getParam<unsigned int>(_unique_name + "/max_buff_length", "10");
- dist_traveled = _server.getParam<Scalar>(_unique_name + "/dist_traveled", "5");
- angle_turned = _server.getParam<Scalar>(_unique_name + "/angle_turned", "0.5");
- unmeasured_perturbation_std = _server.getParam<Scalar>(_unique_name + "/unmeasured_perturbation_std", "1e-4");
+ max_time_span = _server.getParam<Scalar>(_unique_name + "/max_time_span");
+ max_buff_length = _server.getParam<unsigned int>(_unique_name + "/max_buff_length");
+ dist_traveled = _server.getParam<Scalar>(_unique_name + "/dist_traveled");
+ angle_turned = _server.getParam<Scalar>(_unique_name + "/angle_turned");
+ unmeasured_perturbation_std = _server.getParam<Scalar>(_unique_name + "/unmeasured_perturbation_std");
}
std::string print()
{
- return "\n" + ProcessorParamsBase::print() + "max_time_span: " + std::to_string(max_time_span) + "\n"
+ return "\n" + ProcessorParamsBase::print()
+ + "max_time_span: " + std::to_string(max_time_span) + "\n"
+ "max_buff_length: " + std::to_string(max_buff_length) + "\n"
+ "dist_traveled: " + std::to_string(dist_traveled) + "\n"
+ "angle_turned: " +std::to_string(angle_turned) + "\n"
diff --git a/include/core/processor/processor_tracker.h b/include/core/processor/processor_tracker.h
index 3915ba33f8b9899deb99b6436b66a5b5c9ef9f4f..1c9277989d11fee7e0ea568e8cbccdb1bec58b0e 100644
--- a/include/core/processor/processor_tracker.h
+++ b/include/core/processor/processor_tracker.h
@@ -24,8 +24,8 @@ struct ProcessorParamsTracker : public ProcessorParamsBase
ProcessorParamsTracker(std::string _unique_name, const wolf::ParamsServer & _server):
ProcessorParamsBase(_unique_name, _server)
{
- min_features_for_keyframe = _server.getParam<unsigned int>(_unique_name + "/min_features_for_keyframe", "1");
- max_new_features = _server.getParam<int>(_unique_name + "/max_new_features", "-1");
+ min_features_for_keyframe = _server.getParam<unsigned int>(_unique_name + "/min_features_for_keyframe");
+ max_new_features = _server.getParam<int>(_unique_name + "/max_new_features");
}
std::string print()
{
diff --git a/include/core/sensor/sensor_diff_drive.h b/include/core/sensor/sensor_diff_drive.h
index c7465756a1cbb347f3e877e15056d8e45082931c..20b09ae4995461b34811e79cd5c53e66ffb08a66 100644
--- a/include/core/sensor/sensor_diff_drive.h
+++ b/include/core/sensor/sensor_diff_drive.h
@@ -20,20 +20,19 @@ struct IntrinsicsDiffDrive : public IntrinsicsBase
Scalar radius_left;
Scalar radius_right;
Scalar wheel_separation;
- unsigned int ticks_per_wheel_revolution;
+ Scalar ticks_per_wheel_revolution;
Scalar radians_per_tick; ///< Not user-definable -- DO NOT PRETEND TO USE YAML TO SET THIS PARAM.
IntrinsicsDiffDrive() = default;
- IntrinsicsDiffDrive(std::string _unique_name,
- const wolf::ParamsServer & _server) :
- IntrinsicsBase(_unique_name, _server)
+ IntrinsicsDiffDrive(std::string _unique_name, const wolf::ParamsServer & _server) :
+ IntrinsicsBase(_unique_name, _server)
{
- radius_left = _server.getParam<Scalar>(_unique_name + "/radius_left");
- radius_right = _server.getParam<Scalar>(_unique_name + "/radius_right");
- wheel_separation = _server.getParam<Scalar>(_unique_name + "/wheel_separation");
- ticks_per_wheel_revolution = _server.getParam<unsigned int>(_unique_name + "/ticks_per_wheel_revolution");
+ radius_left = _server.getParam<Scalar>(_unique_name + "/radius_left");
+ radius_right = _server.getParam<Scalar>(_unique_name + "/radius_right");
+ wheel_separation = _server.getParam<Scalar>(_unique_name + "/wheel_separation");
+ ticks_per_wheel_revolution = _server.getParam<Scalar>(_unique_name + "/ticks_per_wheel_revolution");
radians_per_tick = 2.0 * M_PI / ticks_per_wheel_revolution;
}
std::string print()
diff --git a/include/core/sensor/sensor_odom_2D.h b/include/core/sensor/sensor_odom_2D.h
index efe8d7e4daa8705ebe2e9ecb07be0f1d4a377d65..80936cfcdcbcfc8427c74336dc8052358b89b0f0 100644
--- a/include/core/sensor/sensor_odom_2D.h
+++ b/include/core/sensor/sensor_odom_2D.h
@@ -23,7 +23,7 @@ struct IntrinsicsOdom2D : public IntrinsicsBase
IntrinsicsBase(_unique_name, _server)
{
k_disp_to_disp = _server.getParam<Scalar>(_unique_name + "/k_disp_to_disp");
- k_rot_to_rot = _server.getParam<Scalar>(_unique_name + "/k_rot_to_rot");
+ k_rot_to_rot = _server.getParam<Scalar>(_unique_name + "/k_rot_to_rot");
}
std::string print()
{
diff --git a/include/core/sensor/sensor_odom_3D.h b/include/core/sensor/sensor_odom_3D.h
index 2fab5f065909452622a8760e5f9a976c63327e89..a6d7296a9aa44a711060ae3fc6aae394e2c0e2da 100644
--- a/include/core/sensor/sensor_odom_3D.h
+++ b/include/core/sensor/sensor_odom_3D.h
@@ -31,18 +31,19 @@ struct IntrinsicsOdom3D : public IntrinsicsBase
IntrinsicsBase(_unique_name, _server)
{
k_disp_to_disp = _server.getParam<Scalar>(_unique_name + "/k_disp_to_disp");
- k_disp_to_rot = _server.getParam<Scalar>(_unique_name + "/k_disp_to_rot");
- k_rot_to_rot = _server.getParam<Scalar>(_unique_name + "/k_rot_to_rot");
- min_disp_var = _server.getParam<Scalar>(_unique_name + "/min_disp_var");
- min_rot_var = _server.getParam<Scalar>(_unique_name + "/min_rot_var");
+ k_disp_to_rot = _server.getParam<Scalar>(_unique_name + "/k_disp_to_rot");
+ k_rot_to_rot = _server.getParam<Scalar>(_unique_name + "/k_rot_to_rot");
+ min_disp_var = _server.getParam<Scalar>(_unique_name + "/min_disp_var");
+ min_rot_var = _server.getParam<Scalar>(_unique_name + "/min_rot_var");
}
std::string print()
{
- return "\n" + IntrinsicsBase::print() + "k_disp_to_disp: " + std::to_string(k_disp_to_disp) + "\n"
- + "k_disp_to_rot: " + std::to_string(k_disp_to_rot) + "\n"
- + "k_rot_to_rot: " + std::to_string(k_rot_to_rot) + "\n"
- + "min_disp_var: " + std::to_string(min_disp_var) + "\n"
- + "min_rot_var: " + std::to_string(min_rot_var) + "\n";
+ return "\n" + IntrinsicsBase::print()
+ + "k_disp_to_disp: " + std::to_string(k_disp_to_disp) + "\n"
+ + "k_disp_to_rot: " + std::to_string(k_disp_to_rot) + "\n"
+ + "k_rot_to_rot: " + std::to_string(k_rot_to_rot) + "\n"
+ + "min_disp_var: " + std::to_string(min_disp_var) + "\n"
+ + "min_rot_var: " + std::to_string(min_rot_var) + "\n";
}
virtual ~IntrinsicsOdom3D() = default;
};
diff --git a/include/core/utils/params_server.hpp b/include/core/utils/params_server.hpp
index 94e8f1a8468a89749685ad471ee9b234e392f84d..c8d74c4df793b8f16696a5674150af505651e7bd 100644
--- a/include/core/utils/params_server.hpp
+++ b/include/core/utils/params_server.hpp
@@ -68,14 +68,14 @@ public:
_params.insert(std::pair<std::string, std::string>(key, value));
}
- template<typename T>
- T getParam(std::string key, std::string def_value) const {
- if(_params.find(key) != _params.end()){
- return converter<T>::convert(_params.find(key)->second);
- }else{
- return converter<T>::convert(def_value);
- }
- }
+// template<typename T>
+// T getParam(std::string key, std::string def_value) const {
+// if(_params.find(key) != _params.end()){
+// return converter<T>::convert(_params.find(key)->second);
+// }else{
+// return converter<T>::convert(def_value);
+// }
+// }
template<typename T>
T getParam(std::string key) const {
diff --git a/include/core/yaml/parser_yaml.hpp b/include/core/yaml/parser_yaml.hpp
index 1ec675756ec1b3d59946a21f1be5c01eef16979a..beb5a589a87aa99dec1996712190c146ddd8d504 100644
--- a/include/core/yaml/parser_yaml.hpp
+++ b/include/core/yaml/parser_yaml.hpp
@@ -373,7 +373,7 @@ void ParserYAML::parseFirstLevel(std::string file){
_paramsSens.push_back(pSensor);
}
for(const auto& kv : n_config["processors"]){
- ParamsInitProcessor pProc = {kv["type"].Scalar(), kv["name"].Scalar(), kv["sensor name"].Scalar(), kv};
+ ParamsInitProcessor pProc = {kv["type"].Scalar(), kv["name"].Scalar(), kv["sensor_name"].Scalar(), kv};
_paramsProc.push_back(pProc);
}
for(const auto& kv : n_config["callbacks"]){
diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp
index 1528c15e708d61f13ed8a5d1084207287722ba6b..b64e86ab9008296a57e01c7ed8a3a8061fa88b2b 100644
--- a/src/ceres_wrapper/ceres_manager.cpp
+++ b/src/ceres_wrapper/ceres_manager.cpp
@@ -43,7 +43,7 @@ CeresManager::~CeresManager()
SolverManagerPtr CeresManager::create(const ProblemPtr &wolf_problem, const ParamsServer &_server)
{
ceres::Solver::Options opt;
- opt.max_num_iterations = _server.getParam<int>("max_num_iterations","1000");
+ opt.max_num_iterations = _server.getParam<int>("max_num_iterations");
// CeresManager m = CeresManager(wolf_problem, opt);
return std::make_shared<CeresManager>(wolf_problem, opt);
}
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 60b395cf8719bb2f10ce1e481888afe1c8c240f9..b0052f8066b04250051d912727797fdb20569d40 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -77,8 +77,8 @@ ProblemPtr Problem::create(const std::string& _frame_structure, SizeEigen _dim)
ProblemPtr Problem::autoSetup(ParamsServer &_server)
{
// Problem structure and dimension
- std::string frame_structure = _server.getParam<std::string> ("problem/frame structure", "PO");
- int dim = _server.getParam<int> ("problem/dimension", "2");
+ std::string frame_structure = _server.getParam<std::string> ("problem/frame_structure");
+ int dim = _server.getParam<int> ("problem/dimension");
auto problem = Problem::create(frame_structure, dim);
// cout << "PRINTING SERVER MAP" << endl;
// _server.print();
@@ -107,10 +107,10 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
}
// Prior
- Eigen::VectorXs prior_state = _server.getParam<Eigen::VectorXs>("problem/prior state");
- Eigen::MatrixXs prior_cov = _server.getParam<Eigen::MatrixXs>("problem/prior cov");
- Scalar prior_time_tolerance = _server.getParam<Scalar>("problem/prior time tolerance");
- Scalar prior_ts = _server.getParam<Scalar>("problem/prior timestamp");
+ Eigen::VectorXs prior_state = _server.getParam<Eigen::VectorXs>("problem/prior/state");
+ Eigen::MatrixXs prior_cov = _server.getParam<Eigen::MatrixXs>("problem/prior/cov");
+ Scalar prior_time_tolerance = _server.getParam<Scalar>("problem/prior/time_tolerance");
+ Scalar prior_ts = _server.getParam<Scalar>("problem/prior/timestamp");
WOLF_TRACE("prior timestamp:\n" , prior_ts);
WOLF_TRACE("prior state:\n" , prior_state.transpose());
diff --git a/src/processor/processor_diff_drive.cpp b/src/processor/processor_diff_drive.cpp
index ffdce5bc9886d7421219d611b4d1f9f16dea175c..ac2e9e7d58c5a05104559591338020d26a6146f7 100644
--- a/src/processor/processor_diff_drive.cpp
+++ b/src/processor/processor_diff_drive.cpp
@@ -187,19 +187,9 @@ FactorBasePtr ProcessorDiffDrive::emplaceFactor(FeatureBasePtr _feature,
ProcessorBasePtr ProcessorDiffDrive::createAutoConf(const std::string& _unique_name, const ParamsServer& _server, const SensorBasePtr sensor_ptr)
{
+ auto params = std::make_shared<ProcessorParamsDiffDrive>(_unique_name, _server);
- ProcessorDiffDrivePtr prc_ptr;
-
- std::shared_ptr<ProcessorParamsDiffDrive> params;
- params = std::make_shared<ProcessorParamsDiffDrive>();
- params->voting_active = _server.getParam<bool>(_unique_name + "/voting_active");
- params->time_tolerance = _server.getParam<double>(_unique_name + "/time_tolerance");
- params->max_time_span = _server.getParam<double>(_unique_name + "/max_time_span");
- params->dist_traveled = _server.getParam<double>(_unique_name + "/dist_traveled"); // Will make KFs automatically every 1m displacement
- params->angle_turned = _server.getParam<double>(_unique_name + "/angle_turned");
- params->unmeasured_perturbation_std = _server.getParam<double>(_unique_name + "/unmeasured_perturbation_std");
-
- prc_ptr = std::make_shared<ProcessorDiffDrive>(params);
+ auto prc_ptr = std::make_shared<ProcessorDiffDrive>(params);
prc_ptr->setName(_unique_name);
return prc_ptr;
diff --git a/src/processor/processor_odom_2D.cpp b/src/processor/processor_odom_2D.cpp
index 664ee4ae943248b81140b6663a3987f3a80dbf24..9253e23c08ca3b12e296c7c92a825f93ce6ee599 100644
--- a/src/processor/processor_odom_2D.cpp
+++ b/src/processor/processor_odom_2D.cpp
@@ -183,22 +183,12 @@ ProcessorBasePtr ProcessorOdom2D::create(const std::string& _unique_name, const
return prc_ptr;
}
+
ProcessorBasePtr ProcessorOdom2D::createAutoConf(const std::string& _unique_name, const ParamsServer& _server, const SensorBasePtr sensor_ptr)
{
+ auto params = std::make_shared<ProcessorParamsOdom2D>(_unique_name, _server);
- ProcessorOdom2DPtr prc_ptr;
-
- std::shared_ptr<ProcessorParamsOdom2D> params;
- params = std::make_shared<ProcessorParamsOdom2D>();
- params->voting_active = _server.getParam<bool>(_unique_name + "/voting_active", "true");
- params->time_tolerance = _server.getParam<double>(_unique_name + "/time_tolerance", "0.1");
- params->max_time_span = _server.getParam<double>(_unique_name + "/max_time_span", "999");
- params->dist_traveled = _server.getParam<double>(_unique_name + "/dist_traveled", "0.95"); // Will make KFs automatically every 1m displacement
- params->angle_turned = _server.getParam<double>(_unique_name + "/angle_turned", "999");
- params->cov_det = _server.getParam<double>(_unique_name + "/cov_det", "999");
- params->unmeasured_perturbation_std = _server.getParam<double>(_unique_name + "/unmeasured_perturbation_std", "0.0001");
-
- prc_ptr = std::make_shared<ProcessorOdom2D>(params);
+ auto prc_ptr = std::make_shared<ProcessorOdom2D>(params);
prc_ptr->setName(_unique_name);
return prc_ptr;
diff --git a/src/sensor/sensor_odom_2D.cpp b/src/sensor/sensor_odom_2D.cpp
index fd4427abec8211ddebf95b2172409692f807e72d..c96059e58b2c40358f75e0b520f9166944bd08ee 100644
--- a/src/sensor/sensor_odom_2D.cpp
+++ b/src/sensor/sensor_odom_2D.cpp
@@ -61,14 +61,15 @@ SensorBasePtr SensorOdom2D::create(const std::string& _unique_name, const Eigen:
SensorBasePtr SensorOdom2D::createAutoConf(const std::string& _unique_name, const ParamsServer& _server)
{
// Eigen::VectorXs _extrinsics_po = Eigen::Vector3s(0,0,0);
- Eigen::VectorXs _extrinsics_po = _server.getParam<Eigen::VectorXs>(_unique_name + "/extrinsic/pos", "[0,0,0]");
+ Eigen::VectorXs _extrinsics_po = _server.getParam<Eigen::VectorXs>(_unique_name + "/extrinsic/pose");
+
assert(_extrinsics_po.size() == 3 && "Bad extrinsics vector length. Should be 3 for 2D.");
- SensorOdom2DPtr odo;
- IntrinsicsOdom2D params;
- params.k_disp_to_disp = _server.getParam<double>(_unique_name + "/intrinsic/k_disp_to_disp", "1");
- params.k_rot_to_rot = _server.getParam<double>(_unique_name + "/intrinsic/k_rot_to_rot", "1");
- odo = std::make_shared<SensorOdom2D>(_extrinsics_po, params);
+
+ IntrinsicsOdom2DPtr params = std::make_shared<IntrinsicsOdom2D>(_unique_name, _server);
+
+ SensorOdom2DPtr odo = std::make_shared<SensorOdom2D>(_extrinsics_po, params);
odo->setName(_unique_name);
+
return odo;
}
diff --git a/src/yaml/processor_odom_3D_yaml.cpp b/src/yaml/processor_odom_3D_yaml.cpp
index ff564779517aa946365510c4a046c1cbc91de053..6b6f53a64d655f77127ddf398add56246b2a55ed 100644
--- a/src/yaml/processor_odom_3D_yaml.cpp
+++ b/src/yaml/processor_odom_3D_yaml.cpp
@@ -24,17 +24,18 @@ static ProcessorParamsBasePtr createProcessorOdom3DParams(const std::string & _f
{
YAML::Node config = YAML::LoadFile(_filename_dot_yaml);
- if (config["processor type"].as<std::string>() == "ODOM 3D")
+ if (config["type"].as<std::string>() == "ODOM 3D")
{
- YAML::Node kf_vote = config["keyframe vote"];
-
ProcessorParamsOdom3DPtr params = std::make_shared<ProcessorParamsOdom3D>();
- params->time_tolerance = config["time tolerance"] .as<Scalar>();
- params->max_time_span = kf_vote["max time span"] .as<Scalar>();
- params->max_buff_length = kf_vote["max buffer length"] .as<SizeEigen >();
- params->dist_traveled = kf_vote["dist traveled"] .as<Scalar>();
- params->angle_turned = kf_vote["angle turned"] .as<Scalar>();
+ params->voting_active = config["voting_active"] .as<bool>();
+ params->voting_aux_active = config["voting_aux_active"] .as<bool>();
+ params->time_tolerance = config["time_tolerance"] .as<Scalar>();
+ params->max_time_span = config["max_time_span"] .as<Scalar>();
+ params->max_buff_length = config["max_buff_length"] .as<SizeEigen >();
+ params->dist_traveled = config["dist_traveled"] .as<Scalar>();
+ params->angle_turned = config["angle_turned"] .as<Scalar>();
+ params->unmeasured_perturbation_std = config["unmeasured_perturbation_std"].as<Scalar>();
return params;
}
diff --git a/src/yaml/sensor_odom_3D_yaml.cpp b/src/yaml/sensor_odom_3D_yaml.cpp
index 3756953b46cf952c900dca37569f8d51118509e4..51ceb2f6344d440fec93bf3417654a9f47bf3333 100644
--- a/src/yaml/sensor_odom_3D_yaml.cpp
+++ b/src/yaml/sensor_odom_3D_yaml.cpp
@@ -24,17 +24,15 @@ static IntrinsicsBasePtr createIntrinsicsOdom3D(const std::string & _filename_do
{
YAML::Node config = YAML::LoadFile(_filename_dot_yaml);
- if (config["sensor type"].as<std::string>() == "ODOM 3D")
+ if (config["type"].as<std::string>() == "ODOM 3D")
{
- YAML::Node variances = config["motion variances"];
-
IntrinsicsOdom3DPtr params = std::make_shared<IntrinsicsOdom3D>();
- params->k_disp_to_disp = variances["disp_to_disp"] .as<Scalar>();
- params->k_disp_to_rot = variances["disp_to_rot"] .as<Scalar>();
- params->k_rot_to_rot = variances["rot_to_rot"] .as<Scalar>();
- params->min_disp_var = variances["min_disp_var"] .as<Scalar>();
- params->min_rot_var = variances["min_rot_var"] .as<Scalar>();
+ params->k_disp_to_disp = config["k_disp_to_disp"] .as<Scalar>();
+ params->k_disp_to_rot = config["k_disp_to_rot"] .as<Scalar>();
+ params->k_rot_to_rot = config["k_rot_to_rot"] .as<Scalar>();
+ params->min_disp_var = config["min_disp_var"] .as<Scalar>();
+ params->min_rot_var = config["min_rot_var"] .as<Scalar>();
return params;
}
diff --git a/test/dummy/processor_tracker_feature_dummy.h b/test/dummy/processor_tracker_feature_dummy.h
index 217196b7e26542f39d60fbfb1f53f01214e20ff9..d4f6dfbcde8fda17e75806ae9b4353afd27497bd 100644
--- a/test/dummy/processor_tracker_feature_dummy.h
+++ b/test/dummy/processor_tracker_feature_dummy.h
@@ -25,7 +25,7 @@ struct ProcessorParamsTrackerFeatureDummy : public ProcessorParamsTrackerFeature
ProcessorParamsTrackerFeatureDummy(std::string _unique_name, const wolf::ParamsServer & _server):
ProcessorParamsTrackerFeature(_unique_name, _server)
{
- n_tracks_lost = _server.getParam<unsigned int>(_unique_name + "/n_tracks_lost", "1");
+ n_tracks_lost = _server.getParam<unsigned int>(_unique_name + "/n_tracks_lost");
}
};
diff --git a/test/dummy/processor_tracker_landmark_dummy.h b/test/dummy/processor_tracker_landmark_dummy.h
index f79b2da401c1f925d7f8629f1e4b9f1cc4c14b3b..9977abc480ab2c1cff695b732286cd7857f5276d 100644
--- a/test/dummy/processor_tracker_landmark_dummy.h
+++ b/test/dummy/processor_tracker_landmark_dummy.h
@@ -23,7 +23,7 @@ struct ProcessorParamsTrackerLandmarkDummy : public ProcessorParamsTrackerLandma
ProcessorParamsTrackerLandmarkDummy(std::string _unique_name, const wolf::ParamsServer & _server):
ProcessorParamsTrackerLandmark(_unique_name, _server)
{
- n_landmarks_lost = _server.getParam<unsigned int>(_unique_name + "/n_landmarks_lost", "1");
+ n_landmarks_lost = _server.getParam<unsigned int>(_unique_name + "/n_landmarks_lost");
}
};
diff --git a/test/gtest_param_server.cpp b/test/gtest_param_server.cpp
index 00c6c099f6542e6e1f08dc9e2e451d8d2d055e81..bfa888fa7fc415c1689eb6b66a30b2919b599dbd 100644
--- a/test/gtest_param_server.cpp
+++ b/test/gtest_param_server.cpp
@@ -16,18 +16,19 @@ ParserYAML parse(string _file, string _path_root)
return parser;
}
-TEST(ParamsServer, Default)
-{
- auto parser = parse("test/yaml/params1.yaml", wolf_root);
- auto params = parser.getParams();
- ParamsServer server = ParamsServer(params, parser.sensorsSerialization(), parser.processorsSerialization());
- EXPECT_EQ(server.getParam<double>("should_not_exist", "2.6"), 2.6);
- EXPECT_EQ(server.getParam<bool>("my_proc_test/voting_active", "true"), false);
- EXPECT_NE(server.getParam<unsigned int>("my_proc_test/time_tolerance", "23"), 23);
- EXPECT_THROW({ server.getParam<unsigned int>("test error"); }, std::runtime_error);
- EXPECT_NE(server.getParam<unsigned int>("my_proc_test/time_tolerance"), 23);
- EXPECT_EQ(server.getParam<bool>("my_proc_test/voting_active"), false);
-}
+//TEST(ParamsServer, Default)
+//{
+// auto parser = parse("test/yaml/params1.yaml", wolf_root);
+// auto params = parser.getParams();
+// ParamsServer server = ParamsServer(params, parser.sensorsSerialization(), parser.processorsSerialization());
+// EXPECT_EQ(server.getParam<double>("should_not_exist", "2.6"), 2.6);
+// EXPECT_EQ(server.getParam<bool>("my_proc_test/voting_active", "true"), false);
+// EXPECT_NE(server.getParam<unsigned int>("my_proc_test/time_tolerance", "23"), 23);
+// EXPECT_THROW({ server.getParam<unsigned int>("test error"); }, std::runtime_error);
+// EXPECT_NE(server.getParam<unsigned int>("my_proc_test/time_tolerance"), 23);
+// EXPECT_EQ(server.getParam<bool>("my_proc_test/voting_active"), false);
+//}
+
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
diff --git a/test/gtest_parser_yaml.cpp b/test/gtest_parser_yaml.cpp
index 985657de83d5bc1381a488d2ed5b6986c12f8f64..de005340e981a6d62a6b24cec3e190f52d09882f 100644
--- a/test/gtest_parser_yaml.cpp
+++ b/test/gtest_parser_yaml.cpp
@@ -22,7 +22,7 @@ TEST(ParserYAML, RegularParse)
// for(auto it : params)
// cout << it.first << " %% " << it.second << endl;
EXPECT_EQ(params["odom/intrinsic/k_rot_to_rot"], "0.1");
- EXPECT_EQ(params["processor1/sensor name"], "odom");
+ EXPECT_EQ(params["processor1/sensor_name"], "odom");
}
TEST(ParserYAML, ParseMap)
{
@@ -37,14 +37,14 @@ TEST(ParserYAML, JumpFile)
{
auto parser = parse("test/yaml/params3.yaml", wolf_root);
auto params = parser.getParams();
- EXPECT_EQ(params["my_proc_test/extern params/max_buff_length"], "100");
- EXPECT_EQ(params["my_proc_test/extern params/voting_active"], "false");
+ EXPECT_EQ(params["my_proc_test/extern_params/max_buff_length"], "100");
+ EXPECT_EQ(params["my_proc_test/extern_params/voting_active"], "false");
}
TEST(ParserYAML, ProblemConfig)
{
auto parser = parse("test/yaml/params2.yaml", wolf_root);
auto params = parser.getParams();
- EXPECT_EQ(params["problem/frame structure"], "POV");
+ EXPECT_EQ(params["problem/frame_structure"], "POV");
EXPECT_EQ(params["problem/dimension"], "2");
}
int main(int argc, char **argv)
diff --git a/test/yaml/params1.yaml b/test/yaml/params1.yaml
index d7d066b53a333d6beb27b20d7eef7468b29f505e..e23b1a484986615060eff7a8d3ad05c2d7116d6d 100644
--- a/test/yaml/params1.yaml
+++ b/test/yaml/params1.yaml
@@ -1,6 +1,6 @@
config:
problem:
- frame structure: "POV"
+ frame_structure: "POV"
dimension: 3
sensors:
-
@@ -10,7 +10,7 @@ config:
k_disp_to_disp: 0.1
k_rot_to_rot: 0.1
extrinsic:
- pos: [1,2,3]
+ pose: [1,2,3]
-
type: "RANGE BEARING"
name: "rb"
@@ -18,15 +18,15 @@ config:
-
type: "ODOM 2D"
name: "processor1"
- sensor name: "odom"
+ sensor_name: "odom"
-
type: "RANGE BEARING"
name: "rb_processor"
- sensor name: "rb"
+ sensor_name: "rb"
-
type: "ODOM 2D"
name: "my_proc_test"
- sensor name: "odom"
+ sensor_name: "odom"
follow: "test/yaml/params3.1.yaml"
files:
- "/home/jcasals/workspace/wip/wolf/lib/libsensor_odo.so"
diff --git a/test/yaml/params2.yaml b/test/yaml/params2.yaml
index 7830e8cb85f9fef0acea46e81fa293ed7528fc5c..5825166a88c5383b3d7cc4e3264cf51e9605aa5e 100644
--- a/test/yaml/params2.yaml
+++ b/test/yaml/params2.yaml
@@ -1,6 +1,6 @@
config:
problem:
- frame structure: "POV"
+ frame_structure: "POV"
dimension: 2
sensors:
-
@@ -10,7 +10,7 @@ config:
k_disp_to_disp: 0.1
k_rot_to_rot: 0.1
extrinsic:
- pos: [1,2,3]
+ pose: [1,2,3]
-
type: "RANGE BEARING"
name: "rb"
@@ -18,7 +18,7 @@ config:
-
type: "ODOM 2D"
name: "processor1"
- sensor name: "odom"
+ sensor_name: "odom"
$mymap:
k1: v1
k2: v2
@@ -26,11 +26,11 @@ config:
-
type: "RANGE BEARING"
name: "rb_processor"
- sensor name: "rb"
+ sensor_name: "rb"
-
type: "ODOM 2D"
name: "my_proc_test"
- sensor name: "odom"
+ sensor_name: "odom"
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/params3.yaml b/test/yaml/params3.yaml
index 3e0d7a40f9fce04d9f6976518fe3bb900dabdbf9..222fd91b45a9846d0f6b571db36f2f8b680591ae 100644
--- a/test/yaml/params3.yaml
+++ b/test/yaml/params3.yaml
@@ -1,6 +1,6 @@
config:
# problem:
- # frame structure: "POV"
+ # frame_structure: "POV"
# dimension: 2
sensors:
-
@@ -10,10 +10,10 @@ config:
k_disp_to_disp: 0.1
k_rot_to_rot: 0.1
extrinsic:
- pos: [1,2,3]
+ pose: [1,2,3]
processors:
-
type: "ODOM 2D"
name: "my_proc_test"
- sensor name: "odom"
- extern params: "@test/yaml/params3.1.yaml"
\ No newline at end of file
+ sensor_name: "odom"
+ extern_params: "@test/yaml/params3.1.yaml"
\ No newline at end of file
diff --git a/test/yaml/processor_odom_3D.yaml b/test/yaml/processor_odom_3D.yaml
index f501e333800ec1bbb4b7c751a32aa67a99bec74c..6e81ac08b2c815a81f079df37bb38dec0fe11fa1 100644
--- a/test/yaml/processor_odom_3D.yaml
+++ b/test/yaml/processor_odom_3D.yaml
@@ -1,8 +1,11 @@
-processor type: "ODOM 3D" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
-processor name: "Main odometer" # This is ignored. The name provided to the SensorFactory prevails
-time tolerance: 0.01 # seconds
-keyframe vote:
- max time span: 0.2 # seconds
- max buffer length: 10 # motion deltas
- dist traveled: 0.5 # meters
- angle turned: 0.1 # radians (1 rad approx 57 deg, approx 60 deg)
\ No newline at end of file
+type: "ODOM 3D" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+name: "Main odometer" # This is ignored. The name provided to the SensorFactory prevails
+time_tolerance: 0.01 # seconds
+
+voting_active: false
+voting_aux_active: false
+max_time_span: 0.2 # seconds
+max_buff_length: 10 # motion deltas
+dist_traveled: 0.5 # meters
+angle_turned: 0.1 # radians (1 rad approx 57 deg, approx 60 deg)
+unmeasured_perturbation_std: 0.001
\ No newline at end of file
diff --git a/test/yaml/sensor_odom_3D.yaml b/test/yaml/sensor_odom_3D.yaml
index 9ea77803548cfd9d033f54e40b6d92a72710afb8..d555856890c58947df326e514c5a60695cd49f6e 100644
--- a/test/yaml/sensor_odom_3D.yaml
+++ b/test/yaml/sensor_odom_3D.yaml
@@ -1,8 +1,8 @@
-sensor type: "ODOM 3D" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
-sensor name: "Main odometer" # This is ignored. The name provided to the SensorFactory prevails
-motion variances:
- disp_to_disp: 0.02 # m^2 / m
- disp_to_rot: 0.02 # rad^2 / m
- rot_to_rot: 0.01 # rad^2 / rad
- min_disp_var: 0.01 # m^2
- min_rot_var: 0.01 # rad^2
+type: "ODOM 3D" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+name: "Main odometer" # This is ignored. The name provided to the SensorFactory prevails
+
+k_disp_to_disp: 0.02 # m^2 / m
+k_disp_to_rot: 0.02 # rad^2 / m
+k_rot_to_rot: 0.01 # rad^2 / rad
+min_disp_var: 0.01 # m^2
+min_rot_var: 0.01 # rad^2