diff --git a/CMakeLists.txt b/CMakeLists.txt
index 6e84d54740a220bd58f3db13a9e2c32f9ac935fc..c43cbdafb634820e93163754c9a77b6514bdc24a 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -146,12 +146,14 @@ include/vision/capture/capture_image.h
SET(HDRS_FACTOR
include/vision/factor/factor_autodiff_trifocal.h
include/vision/factor/factor_AHP.h
+include/vision/factor/factor_pixelHP.h
)
SET(HDRS_FEATURE
include/vision/feature/feature_point_image.h
)
SET(HDRS_LANDMARK
include/vision/landmark/landmark_AHP.h
+ include/vision/landmark/landmark_HP.h
include/vision/landmark/landmark_point_3D.h
)
SET(HDRS_PROCESSOR
@@ -159,6 +161,7 @@ include/vision/processor/processor_tracker_feature_trifocal.h
include/vision/processor/processor_params_image.h
include/vision/processor/processor_tracker_landmark_image.h
include/vision/processor/processor_tracker_feature_image.h
+include/vision/processor/processor_bundle_adjustment.h
)
SET(HDRS_SENSOR
include/vision/sensor/sensor_camera.h
@@ -185,12 +188,14 @@ src/feature/feature_point_image.cpp
)
SET(SRCS_LANDMARK
src/landmark/landmark_AHP.cpp
+src/landmark/landmark_HP.cpp
src/landmark/landmark_point_3D.cpp
)
SET(SRCS_PROCESSOR
src/processor/processor_tracker_feature_trifocal.cpp
src/processor/processor_tracker_feature_image.cpp
src/processor/processor_tracker_landmark_image.cpp
+src/processor/processor_bundle_adjustment.cpp
)
SET(SRCS_SENSOR
src/sensor/sensor_camera.cpp
@@ -203,6 +208,7 @@ SET(SRCS_YAML
src/yaml/processor_tracker_feature_trifocal_yaml.cpp
src/yaml/sensor_camera_yaml.cpp
src/yaml/processor_image_yaml.cpp
+src/yaml/processor_bundle_adjustment_yaml.cpp
)
#OPTIONALS
#optional HDRS and SRCS
@@ -253,7 +259,7 @@ TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${vision_utils_LIBRARY})
# TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${CERES_LIBRARIES})
# ENDIF(Ceres_FOUND)
-
+add_subdirectory(demos)
#Build tests
#===============EXAMPLE=========================
IF(BUILD_TESTS)
diff --git a/demos/CMakeLists.txt b/demos/CMakeLists.txt
index beae7b87e0d592e553ab29d51df6cf5f7c154d02..42a4d30481c7dd797954559258312e19d6204af1 100644
--- a/demos/CMakeLists.txt
+++ b/demos/CMakeLists.txt
@@ -1,232 +1,18 @@
-# Dynamically remove objects from list
-# add_executable(test_list_remove test_list_remove.cpp)
-# Matrix product test
-#ADD_EXECUTABLE(test_matrix_prod test_matrix_prod.cpp)
+if (OpenCV_FOUND)
+ if (${OpenCV_VERSION_MAJOR} GREATER 1)
+ message("-- [INFO] Found OpenCV support")
+ ADD_DEFINITIONS(-DHAVE_OPENCV_H)
+ SET(USE_CV true)
+ else(${OpenCV_VERSION_MAJOR} GREATER 1)
+ message("[WARN] OpenCV support not installed. Minimum 2.4 version required.")
+ message("[WARN] Current version ${OpenCV_VERSION_MAJOR}")
+ endif(${OpenCV_VERSION_MAJOR} GREATER 1)
+else(OpenCV_FOUND)
+ message("[WARN] OpenCV support not installed. Minimum 2.4 version required.")
+endif(OpenCV_FOUND)
-#ADD_EXECUTABLE(test_eigen_template test_eigen_template.cpp)
+ADD_EXECUTABLE(demo_processor_bundle_adjustment demo_processor_bundle_adjustment.cpp)
+TARGET_LINK_LIBRARIES(demo_processor_bundle_adjustment ${PLUGIN_NAME} ${OpenCV_LIBS})
-ADD_EXECUTABLE(test_fcn_ptr test_fcn_ptr.cpp)
-ADD_EXECUTABLE(test_kf_callback test_kf_callback.cpp)
-TARGET_LINK_LIBRARIES(test_kf_callback ${PROJECT_NAME})
-
-ADD_EXECUTABLE(test_wolf_logging test_wolf_logging.cpp)
-TARGET_LINK_LIBRARIES(test_wolf_logging ${PROJECT_NAME})
-
-IF(Ceres_FOUND)
- # test_processor_odom_3D
- ADD_EXECUTABLE(test_processor_odom_3D test_processor_odom_3D.cpp)
- TARGET_LINK_LIBRARIES(test_processor_odom_3D ${PROJECT_NAME})
-
-# ADD_EXECUTABLE(test_motion_2d test_motion_2d.cpp)
-# TARGET_LINK_LIBRARIES(test_motion_2d ${PROJECT_NAME})
-ENDIF(Ceres_FOUND)
-
-
-
-
-# State blocks with local parametrizations test
-#ADD_EXECUTABLE(test_state_quaternion test_state_quaternion.cpp)
-#TARGET_LINK_LIBRARIES(test_state_quaternion ${PROJECT_NAME})
-
-# Testing Eigen Permutations
-#ADD_EXECUTABLE(test_permutations solver/test_permutations.cpp)
-#TARGET_LINK_LIBRARIES(test_permutations ${PROJECT_NAME})
-
-# Enable Yaml config files
-IF(YAMLCPP_FOUND)
-# ADD_EXECUTABLE(test_yaml test_yaml.cpp)
-# TARGET_LINK_LIBRARIES(test_yaml ${PROJECT_NAME})
-
-# ADD_EXECUTABLE(test_yaml_conversions test_yaml_conversions.cpp)
-# TARGET_LINK_LIBRARIES(test_yaml_conversions ${PROJECT_NAME})
-
- # SensorFactory classes test
-# ADD_EXECUTABLE(test_wolf_factories test_wolf_factories.cpp)
-# TARGET_LINK_LIBRARIES(test_wolf_factories ${PROJECT_NAME})
-
- # Map load and save test
-# ADD_EXECUTABLE(test_map_yaml test_map_yaml.cpp)
-# TARGET_LINK_LIBRARIES(test_map_yaml ${PROJECT_NAME})
-ENDIF(YAMLCPP_FOUND)
-
-IF(Suitesparse_FOUND)
-IF (0) # These cannot compile on MacOSX -- we'll fix it some day.
- # Testing a ccolamd test
- ADD_EXECUTABLE(test_ccolamd solver/test_ccolamd.cpp)
- TARGET_LINK_LIBRARIES(test_ccolamd ${PROJECT_NAME})
-
- # Testing a blocks ccolamd test
- ADD_EXECUTABLE(test_ccolamd_blocks solver/test_ccolamd_blocks.cpp)
- TARGET_LINK_LIBRARIES(test_ccolamd_blocks ${PROJECT_NAME})
-
- # Testing an incremental blocks ccolamd test
- ADD_EXECUTABLE(test_incremental_ccolamd_blocks solver/test_incremental_ccolamd_blocks.cpp)
- TARGET_LINK_LIBRARIES(test_incremental_ccolamd_blocks ${PROJECT_NAME})
-
- # Testing an incremental QR with block ccolamd test
- ADD_EXECUTABLE(test_iQR solver/test_iQR.cpp)
- TARGET_LINK_LIBRARIES(test_iQR ${PROJECT_NAME})
-
- # Testing QR solver test tending to wolf
- ADD_EXECUTABLE(test_iQR_wolf solver/test_iQR_wolf.cpp)
- TARGET_LINK_LIBRARIES(test_iQR_wolf ${PROJECT_NAME})
-
- # Testing SPQR simple test
- #ADD_EXECUTABLE(test_SPQR solver/test_SPQR.cpp)
- #TARGET_LINK_LIBRARIES(test_SPQR ${PROJECT_NAME})
-ENDIF(0)
-
-ENDIF(Suitesparse_FOUND)
-
-# Building and populating the wolf tree
-# ADD_EXECUTABLE(test_wolf_tree test_wolf_tree.cpp)
-# TARGET_LINK_LIBRARIES(test_wolf_tree ${PROJECT_NAME})
-
-# Building and populating the wolf tree from .graph file (TORO)
-#ADD_EXECUTABLE(test_wolf_imported_graph test_wolf_imported_graph.cpp)
-#TARGET_LINK_LIBRARIES(test_wolf_imported_graph ${PROJECT_NAME})
-
-# Comparing performance of wolf auto_diff and ceres auto_diff
-#ADD_EXECUTABLE(test_wolf_autodiffwrapper test_wolf_autodiffwrapper.cpp)
-#TARGET_LINK_LIBRARIES(test_wolf_autodiffwrapper ${PROJECT_NAME})
-
-# Prunning wolf tree from .graph file (TORO)
-#ADD_EXECUTABLE(test_wolf_prunning test_wolf_prunning.cpp)
-#TARGET_LINK_LIBRARIES(test_wolf_prunning ${PROJECT_NAME})
-
-# Sparsification from wolf tree from .graph file (TORO)
-ADD_EXECUTABLE(test_sparsification test_sparsification.cpp)
-TARGET_LINK_LIBRARIES(test_sparsification ${PROJECT_NAME})
-
-# Comparing analytic and autodiff odometry factors
-#ADD_EXECUTABLE(test_analytic_odom_factor test_analytic_odom_factor.cpp)
-#TARGET_LINK_LIBRARIES(test_analytic_odom_factor ${PROJECT_NAME})
-
-# Vision
-IF(vision_utils_FOUND)
-
- IF(Ceres_FOUND)
- # Testing many things for the 3D image odometry
- ADD_EXECUTABLE(test_image test_image.cpp)
- TARGET_LINK_LIBRARIES(test_image ${PROJECT_NAME})
-
- # Processor Image Landmark test
- ADD_EXECUTABLE(test_processor_tracker_landmark_image test_processor_tracker_landmark_image.cpp)
- TARGET_LINK_LIBRARIES(test_processor_tracker_landmark_image ${PROJECT_NAME})
-
- # Simple AHP test
- ADD_EXECUTABLE(test_simple_AHP test_simple_AHP.cpp)
- TARGET_LINK_LIBRARIES(test_simple_AHP ${PROJECT_NAME})
-
- IF (APRILTAG_LIBRARY)
- ADD_EXECUTABLE(test_apriltag test_apriltag.cpp)
- TARGET_LINK_LIBRARIES(test_apriltag ${PROJECT_NAME})
- ENDIF(APRILTAG_LIBRARY)
-
- ENDIF(Ceres_FOUND)
-
- # Testing OpenCV functions for projection of points
- ADD_EXECUTABLE(test_projection_points test_projection_points.cpp)
- TARGET_LINK_LIBRARIES(test_projection_points ${PROJECT_NAME})
-
- # Factor test
- ADD_EXECUTABLE(test_factor_AHP test_factor_AHP.cpp)
- TARGET_LINK_LIBRARIES(test_factor_AHP ${PROJECT_NAME})
-
- # ORB tracker test
- ADD_EXECUTABLE(test_tracker_ORB test_tracker_ORB.cpp)
- TARGET_LINK_LIBRARIES(test_tracker_ORB ${PROJECT_NAME})
-
- # Trifocal optimization test
- ADD_EXECUTABLE(test_trifocal_optimization test_trifocal_optimization.cpp)
- TARGET_LINK_LIBRARIES(test_trifocal_optimization ${PROJECT_NAME})
-
-ENDIF(vision_utils_FOUND)
-
-# Processor Tracker Feature test
-ADD_EXECUTABLE(test_processor_tracker_feature test_processor_tracker_feature.cpp)
-TARGET_LINK_LIBRARIES(test_processor_tracker_feature ${PROJECT_NAME})
-
-# Processor Tracker Landmark test
-ADD_EXECUTABLE(test_processor_tracker_landmark test_processor_tracker_landmark.cpp)
-TARGET_LINK_LIBRARIES(test_processor_tracker_landmark ${PROJECT_NAME})
-
-# Processor IMU test
-ADD_EXECUTABLE(test_processor_imu test_processor_imu.cpp)
-TARGET_LINK_LIBRARIES(test_processor_imu ${PROJECT_NAME})
-
-# Processor Diff-Drive test
-ADD_EXECUTABLE(test_processor_diff_drive test_diff_drive.cpp)
-TARGET_LINK_LIBRARIES(test_processor_diff_drive ${PROJECT_NAME})
-
-# MPU6050 IMU test
-#ADD_EXECUTABLE(test_mpu test_mpu.cpp)
-#TARGET_LINK_LIBRARIES(test_mpu ${PROJECT_NAME})
-
-# Processor IMU - Jacobian checking test
-#ADD_EXECUTABLE(test_processor_imu_jacobians test_processor_imu_jacobians.cpp)
-#TARGET_LINK_LIBRARIES(test_processor_imu_jacobians ${PROJECT_NAME})
-
-# Test IMU using printed Dock
-#ADD_EXECUTABLE(test_imuDock test_imuDock.cpp)
-#TARGET_LINK_LIBRARIES(test_imuDock ${PROJECT_NAME})
-
-# Test IMU with auto KF generation (Humanoids 20017)
-#ADD_EXECUTABLE(test_imuDock_autoKFs test_imuDock_autoKFs.cpp)
-#TARGET_LINK_LIBRARIES(test_imuDock_autoKFs ${PROJECT_NAME})
-
-# FactorIMU - factors test
-#ADD_EXECUTABLE(test_imu_constrained0 test_imu_constrained0.cpp)
-#TARGET_LINK_LIBRARIES(test_imu_constrained0 ${PROJECT_NAME})
-
-# IMU - Offline plateform test
-#ADD_EXECUTABLE(test_imuPlateform_Offline test_imuPlateform_Offline.cpp)
-#TARGET_LINK_LIBRARIES(test_imuPlateform_Offline ${PROJECT_NAME})
-
-# IMU - factorIMU
-#ADD_EXECUTABLE(test_factor_imu test_factor_imu.cpp)
-#TARGET_LINK_LIBRARIES(test_factor_imu ${PROJECT_NAME})
-
-# IF (laser_scan_utils_FOUND)
-# ADD_EXECUTABLE(test_capture_laser_2D test_capture_laser_2D.cpp)
-# TARGET_LINK_LIBRARIES(test_capture_laser_2D ${PROJECT_NAME})
-# #ENDIF (laser_scan_utils_FOUND)
-
-# IF(faramotics_FOUND)
-# IF (laser_scan_utils_FOUND)
-# ADD_EXECUTABLE(test_ceres_2_lasers test_ceres_2_lasers.cpp)
-# TARGET_LINK_LIBRARIES(test_ceres_2_lasers
-# ${pose_state_time_LIBRARIES}
-# ${faramotics_LIBRARIES}
-# ${PROJECT_NAME})
-# ADD_EXECUTABLE(test_ceres_2_lasers_polylines test_ceres_2_lasers_polylines.cpp)
-# TARGET_LINK_LIBRARIES(test_ceres_2_lasers_polylines
-# ${pose_state_time_LIBRARIES}
-# ${faramotics_LIBRARIES}
-# ${PROJECT_NAME})
-# ADD_EXECUTABLE(test_2_lasers_offline test_2_lasers_offline.cpp)
-# TARGET_LINK_LIBRARIES(test_2_lasers_offline
-# ${pose_state_time_LIBRARIES}
-# ${faramotics_LIBRARIES}
-# ${PROJECT_NAME})
-# ADD_EXECUTABLE(test_faramotics_simulation test_faramotics_simulation.cpp)
-# TARGET_LINK_LIBRARIES(test_faramotics_simulation
-# ${pose_state_time_LIBRARIES}
-# ${faramotics_LIBRARIES}
-# ${PROJECT_NAME})
-# # ADD_EXECUTABLE(test_autodiff test_autodiff.cpp)
-# # TARGET_LINK_LIBRARIES(test_autodiff
-# # ${pose_state_time_LIBRARIES}
-# # ${faramotics_LIBRARIES}
-# # ${PROJECT_NAME})
-# # IF(Suitesparse_FOUND)
-# # ADD_EXECUTABLE(test_iQR_wolf2 solver/test_iQR_wolf2.cpp)
-# # TARGET_LINK_LIBRARIES(test_iQR_wolf2
-# # ${pose_state_time_LIBRARIES}
-# # ${faramotics_LIBRARIES}
-# # ${PROJECT_NAME})
-# # ENDIF(Suitesparse_FOUND)
-# # ENDIF (laser_scan_utils_FOUND)
-# # ENDIF(faramotics_FOUND)
diff --git a/demos/calib_logitech_webcam_params.yaml b/demos/calib_logitech_webcam_params.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..1285f904ca31fc446c5b2b07cbe0e22ba600f4e8
--- /dev/null
+++ b/demos/calib_logitech_webcam_params.yaml
@@ -0,0 +1,20 @@
+image_width: 640
+image_height: 480
+camera_name: narrow_stereo
+camera_matrix:
+ rows: 3
+ cols: 3
+ data: [942.816482, 0.000000, 334.175643, 0.000000, 897.287666, 309.904004, 0.000000, 0.000000, 1.000000]
+distortion_model: plumb_bob
+distortion_coefficients:
+ rows: 1
+ cols: 5
+ data: [0.159958, -0.578834, 0.000000, 0.000000, 0.000000]
+rectification_matrix:
+ rows: 3
+ cols: 3
+ data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000]
+projection_matrix:
+ rows: 3
+ cols: 4
+ data: [953.235413, 0.000000, 334.175675, 0.000000, 0.000000, 907.202332, 309.904238, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000]
diff --git a/demos/demo_processor_bundle_adjustment.cpp b/demos/demo_processor_bundle_adjustment.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d7a27c2d97889746bf6f06f8cae379ad5c134390
--- /dev/null
+++ b/demos/demo_processor_bundle_adjustment.cpp
@@ -0,0 +1,174 @@
+//std
+#include <iostream>
+
+#include "vision/processor/processor_bundle_adjustment.h"
+
+//Wolf
+#include <core/common/wolf.h>
+#include <core/problem/problem.h>
+#include <core/state_block/state_block.h>
+#include "vision/sensor/sensor_camera.h"
+#include "vision/capture/capture_image.h"
+#include <core/ceres_wrapper/ceres_manager.h>
+#include "vision/landmark/landmark_HP.h"
+#include "vision/internal/config.h"
+
+
+// Vision utils includes
+#include <vision_utils.h>
+#include <sensors.h>
+#include <common_class/buffer.h>
+#include <common_class/frame.h>
+
+////Mvbluefox includes
+//#include <iri/mvbluefox3/mvbluefox3.h>
+//#include <unistd.h>
+//#include <limits.h>
+//#include <sstream>
+//
+//typedef bool( *SUPPORTED_DEVICE_CHECK )( const mvIMPACT::acquire::Device* const );
+//
+//// mvIMPACT::acquire::DeviceManager devMgr_; // Device Manager.
+//
+//
+//std::string GetSerialFromUser(void)
+//{
+// mvIMPACT::acquire::DeviceManager devMgr; // Device Manager.
+// return getDeviceFromUserInput(devMgr)->serial.read();
+//}
+//
+//std::string AvailableDevices(SUPPORTED_DEVICE_CHECK pSupportedDeviceCheckFn)
+//{
+// mvIMPACT::acquire::DeviceManager devMgr; // Device Manager.
+// std::ostringstream devices;
+// devices << "Available device(s): ";
+// const unsigned int devCnt = devMgr.deviceCount();
+// if (devCnt == 0)
+// {
+// devices << "0.";
+// }
+// else
+// {
+// for( unsigned int i = 0; i < devCnt; i++ )
+// {
+// Device* pDev = devMgr[i];
+// if( pDev )
+// {
+// if( !pSupportedDeviceCheckFn || pSupportedDeviceCheckFn( pDev ) )
+// {
+// devices << " \n - " << pDev->product.read() << ": " << pDev->serial.read();
+// }
+// }
+// }
+// }
+// return devices.str();
+//}
+//
+//std::string GetPath()
+//{
+// char result[ PATH_MAX ];
+// ssize_t count = readlink( "/proc/self/exe", result, PATH_MAX );
+// return std::string( result, (count > 0) ? count : 0 );
+//}
+
+using Eigen::Vector3s;
+using Eigen::Vector4s;
+using Eigen::Vector6s;
+using Eigen::Vector7s;
+
+using namespace wolf;
+
+std::string wolf_vision_root = _WOLF_VISION_ROOT_DIR;
+
+int main(int argc, char** argv)
+{
+ std::cout << std::endl << "==================== processor bundle adjustment test ======================" << std::endl;
+
+ vision_utils::SensorCameraPtr sen_ptr = vision_utils::askUserSource(argc, argv);
+ if (sen_ptr==NULL)
+ return 0;
+
+ unsigned int buffer_size = 10;
+ vision_utils::Buffer<vision_utils::FramePtr> frame_buff(buffer_size);
+ frame_buff.add( vision_utils::setFrame(sen_ptr->getImage(), 0) );
+
+ unsigned int img_width = frame_buff.back()->getImage().cols;
+ unsigned int img_height = frame_buff.back()->getImage().rows;
+ std::cout << "Image size: " << img_width << "x" << img_height << std::endl;
+
+// ===========================================================================
+
+ // Wolf problem
+ ProblemPtr problem = Problem::create("PO", 3);
+ ceres::Solver::Options options;
+ options.max_num_iterations = 100;
+ options.function_tolerance = 1e-4;
+ CeresManagerPtr ceres_manager = std::make_shared<CeresManager>(problem, options);
+
+
+ // Install camera
+// IntrinsicsCameraPtr intr = std::make_shared<IntrinsicsCamera>(); // TODO init params or read from YAML
+// intr->pinhole_model_raw = Eigen::Vector4s(320,240,320,320);
+// intr->width = img_width;
+// intr->height = img_height;
+// auto sens_cam = problem->installSensor("CAMERA", "camera", (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), intr);
+ auto sens_cam = problem->installSensor("CAMERA", "camera", (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), wolf_vision_root + "/demos/calib_logitech_webcam_params.yaml");
+ SensorCameraPtr camera = std::static_pointer_cast<SensorCamera>(sens_cam);
+ camera->setImgWidth(img_width);
+ camera->setImgHeight(img_height);
+
+ // Install processor
+ ProcessorParamsBundleAdjustmentPtr params = std::make_shared<ProcessorParamsBundleAdjustment>();
+ params->delete_ambiguities = true;
+ params->yaml_file_params_vision_utils = wolf_vision_root + "/demos/processor_bundle_adjustment_vision_utils.yaml";
+ params->pixel_noise_std = 1.0;
+ params->min_track_length_for_factor = 1;
+ params->voting_active = true;
+ params->max_new_features = 200;
+ params->min_features_for_keyframe = 50;
+ params->time_tolerance = 0.01;
+ params->n_cells_h = 20;
+ params-> n_cells_v = 17;
+ params->min_response_new_feature = 10;
+ auto proc = problem->installProcessor("TRACKER BUNDLE ADJUSTMENT", "processor", sens_cam, params);
+ ProcessorBundleAdjustmentPtr proc_bundle_adj = std::static_pointer_cast<ProcessorBundleAdjustment>(proc);
+
+ std::cout << "sensor & processor created and added to wolf problem" << std::endl;
+
+
+// ==============================================================================
+
+ TimeStamp t = 0;
+ unsigned int number_of_KFs = 0;
+ // main loop
+ Scalar dt = 0.04;
+ for(int frame_count = 0; frame_count<10000; ++frame_count)
+ {
+ t += dt;
+
+ // Image ---------------------------------------------
+ frame_buff.add( vision_utils::setFrame(sen_ptr->getImage(), frame_count) );
+ CaptureImagePtr image = std::make_shared<CaptureImage>(t, camera, frame_buff.back()->getImage());
+ /* process */
+ camera->process(image);
+
+ // solve only when new KFs are added
+ if (problem->getTrajectory()->getFrameList().size() > number_of_KFs)
+ {
+ number_of_KFs = problem->getTrajectory()->getFrameList().size();
+ std::string report = ceres_manager->solve(wolf::SolverManager::ReportVerbosity::BRIEF);
+ std::cout << report << std::endl;
+ if (number_of_KFs > 5)
+ break;
+ }
+// problem->print(4,1,1,0);
+
+ cv::waitKey();//1e7);
+
+ }
+
+// problem->print(1,0,1,0);
+
+ return 0;
+}
+
diff --git a/demos/demo_trifocal_optimization.cpp b/demos/demo_trifocal_optimization.cpp
index 007bc39d95e92e6ab565b57100f341a594e316d8..dcb6d93bff16ef0a401fa96352b4765923fb166d 100644
--- a/demos/demo_trifocal_optimization.cpp
+++ b/demos/demo_trifocal_optimization.cpp
@@ -84,7 +84,7 @@ int main(int argc, char** argv)
// ===============================================
// Wolf problem
- ProblemPtr problem = Problem::create("PO 3D");
+ ProblemPtr problem = Problem::create("PO", 3);
// CERES WRAPPER
CeresManagerPtr ceres_manager;
diff --git a/demos/processor_bundle_adjustment.yaml b/demos/processor_bundle_adjustment.yaml
index dfe322d2140abec834c0b9cc2316fd276c125651..52242f47c6124749365d874dd2da494b2404705c 100644
--- a/demos/processor_bundle_adjustment.yaml
+++ b/demos/processor_bundle_adjustment.yaml
@@ -1,18 +1,19 @@
-processor type: "TRACKER FEATURE TRIFOCAL"
-processor name: "tracker feature trifocal example"
+processor type: "TRACKER BUNDLE ADJUSTMENT"
+processor name: "tracker bundle adjustment example"
vision_utils:
- YAML file params: processor_tracker_feature_trifocal_vision_utils.yaml
+ YAML file params: processor_bundle_adjustment_vision_utils.yaml
algorithm:
time tolerance: 0.005
+ delete ambiguities: true
voting active: false
minimum features for keyframe: 40
maximum new features: 100
grid horiz cells: 13
grid vert cells: 10
min response new features: 50
- min track length for constraint: 3
+ min track length for factor: 3
noise:
pixel noise std: 1 # pixels
diff --git a/demos/processor_bundle_adjustment_vision_utils.yaml b/demos/processor_bundle_adjustment_vision_utils.yaml
index 7621cd8ad1e516361241b88c9de390bd3ac2abb0..305f609b222c50c27f94f0f22ab44906fc378bfd 100644
--- a/demos/processor_bundle_adjustment_vision_utils.yaml
+++ b/demos/processor_bundle_adjustment_vision_utils.yaml
@@ -2,30 +2,32 @@ sensor:
type: "USB_CAM"
detector:
- type: "GFTT"
- maxCorners: 1000
- qualityLevel: 0.01
- minDistance: 1.0
- blockSize: 3
- k: 0.04
+ type: "ORB"
+ nfeatures: 400
+ scale factor: 1.0
+ nlevels: 1
+ edge threshold: 31 # 16
+ first level: 0
+ WTA_K: 2 # See: http://docs.opencv.org/trunk/db/d95/classcv_1_1ORB.html#a180ae17d3300cf2c619aa240d9b607e5
+ score type: 1 #enum { kBytes = 32, HARRIS_SCORE=0, FAST_SCORE=1 };
+ patch size: 31 # 31
descriptor:
type: "ORB"
- nfeatures: 1000
- scale factor: 1.2
+ nfeatures: 400
+ scale factor: 1.0
nlevels: 1
- edge threshold: 8 # 16
+ edge threshold: 31 # 16
first level: 0
WTA_K: 2 # See: http://docs.opencv.org/trunk/db/d95/classcv_1_1ORB.html#a180ae17d3300cf2c619aa240d9b607e5
score type: 1 #enum { kBytes = 32, HARRIS_SCORE=0, FAST_SCORE=1 };
- patch size: 15 # 31
-
+ patch size: 31 # 31
+
matcher:
- type: "BRUTEFORCE_HAMMING" # BRUTEFORCE, BRUTEFORCE_HAMMING, BRUTEFORCE_HAMMING_2, BRUTEFORCE_L1, FLANNBASED
- match type: 2 # Match type. MATCH = 1, KNNMATCH = 2, RADIUSMATCH = 3
- min normalized score: 0.9
+ type: "FLANNBASED" # BRUTEFORCE, BRUTEFORCE_HAMMING, BRUTEFORCE_HAMMING_2, BRUTEFORCE_L1, FLANNBASED
+ match type: 1 # Match type. MATCH = 1, KNNMATCH = 2, RADIUSMATCH = 3
filtering:
- ransac epipolar distance: 1 # Distance to epipolar [pixels]
- ransac confidence prob: 0.98 # Confidence probability
- max match euclidean dist: 50 # Max euclidean distance to consider a match as inlier
+ ransac epipolar distance: 3 # Distance to epipolar [pixels]
+ ransac confidence prob: 0.97 # Confidence probability
+ max match euclidean dist: 20 # Max euclidean distance to consider a match as inlier
\ No newline at end of file
diff --git a/include/vision/capture/capture_image.h b/include/vision/capture/capture_image.h
index 08a934052d307a6af00bc42f1e03ae2503390664..8b3ff0b0d1eff12116ed0591ef649669e57f4811 100644
--- a/include/vision/capture/capture_image.h
+++ b/include/vision/capture/capture_image.h
@@ -2,13 +2,13 @@
#define CAPTURE_IMAGE_H
//Wolf includes
-#include "core/capture/capture_base.h"
+#include <core/capture/capture_base.h>
#include "vision/feature/feature_point_image.h"
#include "vision/sensor/sensor_camera.h"
// Vision Utils includes
-#include "vision_utils/vision_utils.h"
-#include "vision_utils/common_class/frame.h"
+#include <vision_utils/vision_utils.h>
+#include <vision_utils/common_class/frame.h>
namespace wolf {
diff --git a/include/vision/factor/factor_AHP.h b/include/vision/factor/factor_AHP.h
index 3ef1b6c8bfec4a21a1fdec763ccd1bbaac8f8430..448ca1fcb8812df6735b4d818c1d204c34682011 100644
--- a/include/vision/factor/factor_AHP.h
+++ b/include/vision/factor/factor_AHP.h
@@ -116,7 +116,7 @@ inline void FactorAHP::expectation(const T* const _current_frame_p,
using namespace Eigen;
// All involved transforms typedef
- typedef Eigen::Transform<T, 3, Eigen::Affine> TransformType;
+ typedef Eigen::Transform<T, 3, Eigen::Isometry> TransformType;
// world to anchor robot transform
Map<const Matrix<T, 3, 1> > p_w_r0(_anchor_frame_p);
@@ -144,8 +144,8 @@ inline void FactorAHP::expectation(const T* const _current_frame_p,
// hmg point in current camera frame C1
Eigen::Map<const Eigen::Matrix<T, 4, 1> > landmark_hmg_c0(_lmk_hmg);
- Eigen::Matrix<T, 4, 1> landmark_hmg_c1 = T_R1_C1.inverse(Eigen::Affine)
- * T_W_R1. inverse(Eigen::Affine)
+ Eigen::Matrix<T, 4, 1> landmark_hmg_c1 = T_R1_C1.inverse(Eigen::Isometry)
+ * T_W_R1. inverse(Eigen::Isometry)
* T_W_R0
* T_R0_C0
* landmark_hmg_c0;
diff --git a/include/vision/factor/factor_autodiff_trifocal.h b/include/vision/factor/factor_autodiff_trifocal.h
index d95aa044d7f25fbdb72c60b98b98090d53321984..b469a6e8c0feb583c82cfcde343c0a7ee8759a69 100644
--- a/include/vision/factor/factor_autodiff_trifocal.h
+++ b/include/vision/factor/factor_autodiff_trifocal.h
@@ -6,8 +6,8 @@
#include "core/factor/factor_autodiff.h"
#include "vision/sensor/sensor_camera.h"
-#include <common_class/trifocaltensor.h>
-#include <vision_utils.h>
+#include <vision_utils/common_class/trifocaltensor.h>
+#include <vision_utils/vision_utils.h>
namespace wolf
{
@@ -270,7 +270,7 @@ inline void FactorAutodiffTrifocal::expectation(const MatrixBase<D1>& _wtr1,
{
typedef Translation<T, 3> TranslationType;
- typedef Eigen::Transform<T, 3, Eigen::Affine> TransformType;
+ typedef Eigen::Transform<T, 3, Eigen::Isometry> TransformType;
// All input Transforms
TransformType wHr1 = TranslationType(_wtr1) * _wqr1;
diff --git a/include/vision/factor/factor_pixelHP.h b/include/vision/factor/factor_pixelHP.h
new file mode 100644
index 0000000000000000000000000000000000000000..6ade3a3b15d3465f4ca63527592f21b7895664b6
--- /dev/null
+++ b/include/vision/factor/factor_pixelHP.h
@@ -0,0 +1,196 @@
+#ifndef FACTOR_AHP_H
+#define FACTOR_AHP_H
+
+//Wolf includes
+#include "vision/landmark/landmark_HP.h"
+#include "vision/sensor/sensor_camera.h"
+#include "vision/math/pinhole_tools.h"
+
+#include "core/factor/factor_autodiff.h"
+
+#include <iomanip> //setprecision
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(FactorPixelHP);
+
+//class
+class FactorPixelHP : public FactorAutodiff<FactorPixelHP, 2, 3, 4, 3, 4, 4>
+{
+ protected:
+
+ Eigen::Vector4s intrinsic_;
+ Eigen::VectorXs distortion_;
+
+ public:
+
+ FactorPixelHP(const FeatureBasePtr& _ftr_ptr,
+ const LandmarkHPPtr& _landmark_ptr,
+ const ProcessorBasePtr& _processor_ptr = nullptr,
+ bool _apply_loss_function = false,
+ FactorStatus _status = FAC_ACTIVE);
+
+ virtual ~FactorPixelHP() = default;
+
+ template<typename T>
+ void expectation(const T* const _frame_p,
+ const T* const _frame_o,
+ const T* const _sensor_p,
+ const T* const _sensor_o,
+ const T* const _lmk_hmg,
+ T* _expectation) const;
+
+ Eigen::VectorXs expectation() const;
+
+ template<typename T>
+ bool operator ()(const T* const _frame_p,
+ const T* const _frame_o,
+ const T* const _sensor_p,
+ const T* const _sensor_o,
+ const T* const _lmk_hmg,
+ T* _residuals) const;
+
+ // Static creator method
+ static FactorPixelHPPtr create(const FeatureBasePtr& _ftr_ptr,
+ const LandmarkHPPtr& _lmk_ahp_ptr,
+ const ProcessorBasePtr& _processor_ptr = nullptr,
+ bool _apply_loss_function = false,
+ FactorStatus _status = FAC_ACTIVE);
+
+};
+
+inline FactorPixelHP::FactorPixelHP(const FeatureBasePtr& _ftr_ptr,
+ const LandmarkHPPtr& _landmark_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status) :
+ FactorAutodiff<FactorPixelHP, 2, 3, 4, 3, 4, 4>("PIXELHP",
+ nullptr,
+ nullptr,
+ nullptr,
+ _landmark_ptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _ftr_ptr->getCapture()->getFrame()->getP(),
+ _ftr_ptr->getCapture()->getFrame()->getO(),
+ _ftr_ptr->getCapture()->getSensorP(),
+ _ftr_ptr->getCapture()->getSensorO(),
+ _landmark_ptr->getP()),
+ intrinsic_(_ftr_ptr->getCapture()->getSensor()->getIntrinsic()->getState()) //TODO: intrinsic
+{
+// std::cout << "FactorPixelHP::Constructor\n";
+ // obtain some intrinsics from provided sensor
+ distortion_ = (std::static_pointer_cast<SensorCamera>(_ftr_ptr->getCapture()->getSensor()))->getDistortionVector();
+}
+
+inline Eigen::VectorXs FactorPixelHP::expectation() const
+{
+ FrameBasePtr frm = getFeature()->getCapture()->getFrame();
+ SensorBasePtr sen = getFeature()->getCapture()->getSensor();
+ LandmarkBasePtr lmk = getLandmarkOther();
+
+ const Eigen::MatrixXs frame_pos = frm->getP()->getState();
+ const Eigen::MatrixXs frame_ori = frm->getO()->getState();
+ const Eigen::MatrixXs sensor_pos = sen ->getP()->getState();
+ const Eigen::MatrixXs sensor_ori = sen ->getO()->getState();
+ const Eigen::MatrixXs lmk_pos_hmg = lmk ->getP()->getState();
+
+ Eigen::Vector2s exp;
+ expectation(frame_pos.data(), frame_ori.data(), sensor_pos.data(), sensor_ori.data(),
+ lmk_pos_hmg.data(), exp.data());
+
+ return exp;
+}
+
+template<typename T>
+inline void FactorPixelHP::expectation(const T* const _frame_p,
+ const T* const _frame_o,
+ const T* const _sensor_p,
+ const T* const _sensor_o,
+ const T* const _lmk_hmg,
+ T* _expectation) const
+{
+ using namespace Eigen;
+
+ // All involved transforms typedef
+ typedef Eigen::Transform<T, 3, Eigen::Isometry> TransformType;
+
+ // world to current robot transform
+ Map<const Matrix<T, 3, 1> > p_w_r(_frame_p);
+ Translation<T, 3> t_w_r(p_w_r);
+ Map<const Quaternion<T> > q_w_r(_frame_o);
+ TransformType T_w_r = t_w_r * q_w_r;
+
+ // current robot to current camera transform
+ Map<const Matrix<T, 3, 1> > p_r_c(_sensor_p);
+ Translation<T, 3> t_r_c(p_r_c);
+ Map<const Quaternion<T> > q_r_c(_sensor_o);
+ TransformType T_r_c = t_r_c * q_r_c;
+
+ // hmg point in current camera frame C
+ Eigen::Map<const Eigen::Matrix<T, 4, 1> > landmark_hmg(_lmk_hmg);
+ Eigen::Matrix<T, 4, 1> landmark_hmg_c = T_r_c .inverse(Eigen::Isometry)
+ * T_w_r .inverse(Eigen::Isometry)
+ * landmark_hmg;
+
+ //std::cout << "p_w_r = \n\t" << _frame_p[0] << "\n\t" << _frame_p[1] << "\n\t" << _frame_p[2] << "\n";
+// std::cout << "q_w_r = \n\t" << _frame_o[0] << "\n\t" << _frame_o[1] << "\n\t" << _frame_o[2] << "\n\t" << _frame_o[3] << "\n";
+// std::cout << "p_r_c = \n\t" << _sensor_p[0] << "\n\t" << _sensor_p[1] << "\n\t" << _sensor_p[2] << "\n";
+// std::cout << "q_r_c = \n\t" << _sensor_o[0] << "\n\t" << _sensor_o[1] << "\n\t" << _sensor_o[2] << "\n\t" << _sensor_o[3] << "\n";
+// std::cout << "landmark_hmg_c = \n\t" << landmark_hmg_c(0) << "\n\t" << landmark_hmg_c(1) << "\n\t" << landmark_hmg_c(2) << "\n\t" << landmark_hmg_c(3) << "\n";
+
+ // lmk direction vector
+ Eigen::Matrix<T, 3, 1> v_dir = landmark_hmg_c.head(3);
+
+ // lmk inverse distance
+ T rho = landmark_hmg_c(3);
+
+ // camera parameters
+ Matrix<T, 4, 1> intrinsic = intrinsic_.cast<T>();
+ Eigen::Matrix<T, Eigen::Dynamic, 1> distortion = distortion_.cast<T>();
+
+ // project point and exit
+ Eigen::Map<Eigen::Matrix<T, 2, 1> > expectation(_expectation);
+ expectation = pinhole::projectPoint(intrinsic, distortion, v_dir/rho);
+
+// std::cout << "expectation = \n\t" << expectation(0) << "\n\t" << expectation(1) << "\n";
+
+}
+
+template<typename T>
+inline bool FactorPixelHP::operator ()(const T* const _frame_p,
+ const T* const _frame_o,
+ const T* const _sensor_p,
+ const T* const _sensor_o,
+ const T* const _lmk_hmg,
+ T* _residuals) const
+{
+ // expected
+ Eigen::Matrix<T, 2, 1> expected;
+ expectation(_frame_p, _frame_o, _sensor_p, _sensor_o, _lmk_hmg, expected.data());
+
+ // measured
+ Eigen::Matrix<T, 2, 1> measured = getMeasurement().cast<T>();
+
+ // residual
+ Eigen::Map<Eigen::Matrix<T, 2, 1> > residuals(_residuals);
+ residuals = getMeasurementSquareRootInformationUpper().cast<T>() * (expected - measured);
+ return true;
+}
+
+inline FactorPixelHPPtr FactorPixelHP::create(const FeatureBasePtr& _ftr_ptr,
+ const LandmarkHPPtr& _lmk_ahp_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status)
+{
+ // construct factor
+ FactorPixelHPPtr fac_ahp = std::make_shared<FactorPixelHP>(_ftr_ptr, _lmk_ahp_ptr, _processor_ptr, _apply_loss_function, _status);
+
+ return fac_ahp;
+}
+
+} // namespace wolf
+
+#endif // FACTOR_AHP_H
diff --git a/include/vision/feature/feature_point_image.h b/include/vision/feature/feature_point_image.h
index b4d7d089109da3dd204f3a5243efc75c86e56dd8..b95839d7f676e8de69af79e218e1d44938a82223 100644
--- a/include/vision/feature/feature_point_image.h
+++ b/include/vision/feature/feature_point_image.h
@@ -5,7 +5,7 @@
#include "core/feature/feature_base.h"
// Vision Utils includes
-#include <vision_utils.h>
+#include <vision_utils/vision_utils.h>
namespace wolf {
diff --git a/include/vision/landmark/landmark_HP.h b/include/vision/landmark/landmark_HP.h
new file mode 100644
index 0000000000000000000000000000000000000000..cea8c04bc1303bf1a78a96096d93ed6fba28e1fa
--- /dev/null
+++ b/include/vision/landmark/landmark_HP.h
@@ -0,0 +1,72 @@
+#ifndef LANDMARK_AHP_H
+#define LANDMARK_AHP_H
+
+//Wolf includes
+#include "core/landmark/landmark_base.h"
+
+// yaml
+#include <yaml-cpp/yaml.h>
+
+// Vision utils
+#include <vision_utils/vision_utils.h>
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(LandmarkHP);
+
+/* Landmark - Homogeneous Point*/
+class LandmarkHP : public LandmarkBase
+{
+ protected:
+ SensorBasePtr sensor_;
+ cv::Mat cv_descriptor_;
+
+
+ public:
+ LandmarkHP(Eigen::Vector4s _position_homogeneous, SensorBasePtr _sensor_, cv::Mat _2D_descriptor);
+
+ virtual ~LandmarkHP();
+
+ const cv::Mat& getCvDescriptor() const;
+ void setCvDescriptor(const cv::Mat& _descriptor);
+
+ void setSensor (SensorBasePtr _anchor_sensor);
+ const SensorBasePtr getSensor() const;
+
+ Eigen::Vector3s point() const;
+
+ YAML::Node saveToYaml() const;
+
+ /** \brief Creator for Factory<LandmarkBase, YAML::Node>
+ * Caution: This creator does not set the landmark's sensor.
+ * These need to be set afterwards.
+ */
+ static LandmarkBasePtr create(const YAML::Node& _node);
+};
+
+inline void LandmarkHP::setSensor(SensorBasePtr _sensor)
+{
+ sensor_ = _sensor;
+}
+
+
+inline const SensorBasePtr LandmarkHP::getSensor() const
+{
+ return sensor_;
+}
+
+
+inline const cv::Mat& LandmarkHP::getCvDescriptor() const
+{
+ return cv_descriptor_;
+}
+
+
+inline void LandmarkHP::setCvDescriptor(const cv::Mat& _descriptor)
+{
+ cv_descriptor_ = _descriptor;
+}
+
+} // namespace wolf
+
+#endif // LANDMARK_AHP_H
diff --git a/include/vision/math/pinhole_tools.h b/include/vision/math/pinhole_tools.h
index ef226651ea81e6bf2645b71069449e364a095a13..85ee2062aaff2e45cf15236943becf612bed9a04 100644
--- a/include/vision/math/pinhole_tools.h
+++ b/include/vision/math/pinhole_tools.h
@@ -761,6 +761,45 @@ void computeCorrectionModel(const Vk & k, const Vd & d, Vc & c)
c = iRd * (rc - rd);
}
+
+// //This method follows the one in: https://www.ncbi.nlm.nih.gov/pmc/articles/PMC4934233/
+// SizeEigen size = c.size();
+// if (size != 0 && size <=9) //Only implemented to compute a maximum of 9 distortion correction parameters
+// {
+// c(0) = -d(0);
+// if (d.size() >= 2)
+// {
+// c(1) = 3 * pow(d(0),2) - d(1);
+// }
+//
+// if (d.size() >= 3)
+// {
+// c(2) = -12 * pow(d(0), 3) + 8 * d(0) * d(1) - d(2);
+//
+//// c(3) = 55 * pow(d(0), 4) - 55 * pow(d(0), 2) * d(1) + 5 * pow(d(1), 2) + 10 * d(0) * d(2) - d(3);
+////
+//// c(4) = -273 * pow(d(0), 5) + 364 * pow(d(0), 3) * d(1) - 78 * d(0) * pow(d(1), 2) - 78 * pow(d(0), 2) * d(2) + 12 * d(1) * d(2) + 12 * d(0) * d(3);
+////
+//// c(5) = 1428 * pow(d(0), 6) - 2380 * pow(d(0), 4) * d(1) + 840 * pow(d(0), 2) * pow(d(1), 2) - 35 * pow(d(1), 3) + 560 * pow(d(0), 3) * d(2)
+//// - 210 * d(0) * d(1) * d(2) + 7 * pow(d(2), 2) - 105 * pow(d(0), 2) * d(3) + 14 * d(1) * d(3);
+////
+//// c(6) = -7752 * pow(d(0), 7) + 15504 * pow(d(0), 5) * d(1) - 7752 * pow(d(0), 3) * pow(d(1), 2) + 816 * d(0) * pow(d(1), 3) + 2448 * pow(d(0), 2) * d(1) * d(2)
+//// - 136 * pow(d(1), 2) * d(2) - 136 * d(0) * pow(d(2), 2) + 816 * pow(d(0), 3) * d(3) - 272 * d(0) * d(1) * d(3) + 16 * d(2) * d(3);
+////
+//// c(7) = 43263 * pow(d(0), 8) - 100947 * pow(d(0), 6) * d(1) + 65835 * pow(d(0), 4) * pow(d(1), 2) - 11970 * pow(d(0), 2) * pow(d(1), 3) + 285 * pow(d(1), 4)
+//// + 26334 * pow(d(0), 5) * d(2) - 23940 * pow(d(0), 3) * d(1) * d(2) + 3420 * d(0) * pow(d(1), 2) * d(2) + 1710 * pow(d(0), 2) * pow(d(2), 2)
+//// - 171 * d(1) * pow(d(2), 2) - 5985 * pow(d(0), 4) * d(3) + 3420 * pow(d(0), 2) * d(1) * d(3) - 171 * pow(d(1), 2) * d(3) - 342 * d(0) * d(2) * d(3)
+//// + 9 * pow(d(3), 2);
+////
+//// c(8) = -246675 * pow(d(0), 9) + 657800 * pow(d(0), 7) * d(1) - 531300 * pow(d(0), 5) * pow(d(1), 2) + 141680 * pow(d(0), 3) * pow(d(1), 3)
+//// - 8855 * d(0) * pow(d(1), 4) - 177100 * pow(d(0), 6) * d(2) + 212520 * pow(d(0), 4) * d(1) * d(2) - 53130 * pow(d(0), 2) * pow(d(1), 2) * d(2)
+//// + 1540 * pow(d(1), 3) * d(2) - 17710 * pow(d(0), 3) * pow(d(2), 2) + 4620 * d(0) * d(1) * pow(d(2), 2) - 70 * pow(d(2), 3)
+//// + 42504 * pow(d(0), 5) * d(3) - 35420 * pow(d(0), 3) * d(1) * d(3) + 4620 * d(0) * pow(d(1), 2) * d(3) + 4620 * pow(d(0), 2) * d(2) * d(3)
+//// - 420 * d(1) * d(2) * d(3) - 210 * d(0) * pow(d(3), 2);
+// }
+// c = c.head(size);
+// }
+
}
} // namespace pinhole
diff --git a/include/vision/processor/processor_bundle_adjustment.h b/include/vision/processor/processor_bundle_adjustment.h
new file mode 100644
index 0000000000000000000000000000000000000000..629ee924e25829a83b21ede451d82d5d751545fa
--- /dev/null
+++ b/include/vision/processor/processor_bundle_adjustment.h
@@ -0,0 +1,231 @@
+/*
+ * processor_bundle_adjustment.h
+ *
+ * Created on: May 3, 2019
+ * Author: ovendrell
+ */
+
+#ifndef INCLUDE_BASE_PROCESSOR_PROCESSOR_BUNDLE_ADJUSTMENT_H_
+#define INCLUDE_BASE_PROCESSOR_PROCESSOR_BUNDLE_ADJUSTMENT_H_
+
+//wolf includes
+#include "core/processor/processor_tracker_feature.h"
+#include "vision/capture/capture_image.h"
+#include "vision/landmark/landmark_HP.h"
+#include "vision/math/pinhole_tools.h"
+#include "vision/sensor/sensor_camera.h"
+#include "core/math/rotations.h"
+
+//vision utils includes
+#include "vision_utils/vision_utils.h"
+#include "vision_utils/detectors.h"
+#include "vision_utils/descriptors.h"
+#include "vision_utils/matchers.h"
+#include "../factor/factor_pixelHP.h"
+
+namespace wolf{
+
+WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsBundleAdjustment);
+
+struct ProcessorParamsBundleAdjustment : public ProcessorParamsTrackerFeature
+{
+ std::string yaml_file_params_vision_utils;
+
+ bool delete_ambiguities;
+
+ int n_cells_h;
+ int n_cells_v;
+ int min_response_new_feature;
+
+ Scalar pixel_noise_std; ///< std noise of the pixel
+ int min_track_length_for_factor; ///< Minimum track length of a matched feature to create a factor
+
+ ProcessorParamsBundleAdjustment() = default;
+
+};
+
+WOLF_PTR_TYPEDEFS(ProcessorBundleAdjustment);
+
+class ProcessorBundleAdjustment : public ProcessorTrackerFeature
+{
+ protected:
+ vision_utils::DetectorBasePtr det_ptr_;
+ vision_utils::DescriptorBasePtr des_ptr_;
+ vision_utils::MatcherBasePtr mat_ptr_;
+
+ ProcessorParamsBundleAdjustmentPtr params_bundle_adjustment_; // Configuration parameters
+
+ CaptureImagePtr capture_image_last_;
+ CaptureImagePtr capture_image_incoming_;
+
+ SensorCameraPtr camera;
+
+ Matrix2s pixel_cov_;
+
+ cv::Mat tvec_;
+ cv::Mat rvec_;
+
+
+ //TODO: correct to add this?
+ std::map<size_t, LandmarkBasePtr> lmk_track_map_; //LandmarkTrackMap;
+
+ private:
+ int frame_count_;
+
+
+ public:
+ /** \brief Class constructor
+ */
+ ProcessorBundleAdjustment(ProcessorParamsBundleAdjustmentPtr _params_bundle_adjustment);
+ /** \brief Class destructor
+ */
+ virtual ~ProcessorBundleAdjustment()
+ {
+ //
+ }
+
+ public:
+
+ virtual void configure(SensorBasePtr _sensor) override;
+
+ /** Pre-process incoming Capture
+ *
+ * This is called by process() just after assigning incoming_ptr_ to a valid Capture.
+ *
+ * Overload this function to prepare stuff on derived classes.
+ *
+ * Typical uses of prePrecess() are:
+ * - casting base types to derived types
+ * - initializing counters, flags, or any derived variables
+ * - initializing algorithms needed for processing the derived data
+ */
+ virtual void preProcess() override;
+
+ /** Post-process
+ *
+ * This is called by process() after finishing the processing algorithm.
+ *
+ * Overload this function to post-process stuff on derived classes.
+ *
+ * Typical uses of postPrecess() are:
+ * - resetting and/or clearing variables and/or algorithms at the end of processing
+ * - drawing / printing / logging the results of the processing
+ */
+ virtual void postProcess() override;
+
+ /** \brief Track provided features in \b _capture
+ * \param _features_in input list of features in \b last to track
+ * \param _capture the capture in which the _features_in should be searched
+ * \param _features_out returned list of features found in \b _capture
+ * \param _feature_correspondences returned map of correspondences: _feature_correspondences[feature_out_ptr] = feature_in_ptr
+ *
+ * \return the number of features tracked
+ */
+ virtual unsigned int trackFeatures(const FeatureBasePtrList& _features_in,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out,
+ FeatureMatchMap& _feature_correspondences) override;
+
+ /** \brief Correct the drift in incoming feature by re-comparing against the corresponding feature in origin.
+ * \param _origin_feature input feature in origin capture tracked
+ * \param _incoming_feature input/output feature in incoming capture to be corrected
+ * \return false if the the process discards the correspondence with origin's feature
+ */
+ virtual bool correctFeatureDrift(const FeatureBasePtr _origin_feature, const FeatureBasePtr _last_feature, FeatureBasePtr _incoming_feature) override;
+
+ /** \brief Vote for KeyFrame generation
+ *
+ * If a KeyFrame criterion is validated, this function returns true,
+ * meaning that it wants to create a KeyFrame at the \b last Capture.
+ *
+ * WARNING! This function only votes! It does not create KeyFrames!
+ */
+ virtual bool voteForKeyFrame() override;
+
+ bool isInlier(const cv::KeyPoint& _kp_incoming, const cv::KeyPoint& _kp_last);
+
+ bool is_tracked(int& kp_idx_);
+
+ /** \brief Detect new Features
+ * \param _max_features maximum number of features detected (-1: unlimited. 0: none)
+ * \param _capture The capture in which the new features should be detected.
+ * \param _features_out The list of detected Features in _capture.
+ * \return The number of detected Features.
+ *
+ * This function detects Features that do not correspond to known Features/Landmarks in the system.
+ *
+ * IMPORTANT: The features in _features_out should be emplaced. Don't use `make_shared`, use `FeatureBase::emplace` instead.
+ * Then, they will be already linked to the _capture.
+ * If you detect all the features at once in preprocess(), you should either emplace them (`FeatureBase::emplace()`) and remove the not returned features in _features_out (`FeatureBase::remove()`),
+ * or create them (`make_shared()`) and link all the returned features in _features_out (`FeatureBase::link(_capture)`).
+ *
+ * The function is called in ProcessorTrackerFeature::processNew() to set the member new_features_last_,
+ * the list of newly detected features of the capture last_ptr_.
+ */
+ virtual unsigned int detectNewFeatures(const int& _max_new_features,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out) override;
+
+ /** \brief Emplaces a new factor
+ * \param _feature_ptr pointer to the parent Feature
+ * \param _feature_other_ptr pointer to the other feature constrained.
+ *
+ * Implement this method in derived classes.
+ *
+ * This function emplaces a factor of the appropriate type for the derived processor.
+ */
+ virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr) override;
+
+ virtual LandmarkBasePtr emplaceLandmark(FeatureBasePtr _feature_ptr);
+
+ /** \brief Establish factors between features in Captures \b last and \b origin
+ */
+ virtual void establishFactors() override;
+
+ void setParams(const ProcessorParamsBundleAdjustmentPtr _params);
+
+ public:
+ /// @brief Factory method
+ static ProcessorBasePtr create(const std::string& _unique_name,
+ const ProcessorParamsBasePtr _params,
+ const SensorBasePtr _sensor_ptr);
+
+ private:
+
+ cv::Mat image_debug_;
+
+ public:
+
+ /**
+ * \brief Return Image for debug purposes
+ */
+ cv::Mat getImageDebug();
+
+ /**
+ * \brief Return list of Features tracked in a Capture
+ */
+ std::list<FeatureBasePtr> trackedFeatures(const CaptureBasePtr& _capture_ptr);
+ /**
+ * \brief Return list of Landmarks
+ */
+ std::list<LandmarkBasePtr> currentLandmarks();
+};
+
+inline cv::Mat ProcessorBundleAdjustment::getImageDebug()
+{
+ return image_debug_;
+}
+
+inline std::list<FeatureBasePtr> ProcessorBundleAdjustment::trackedFeatures(const CaptureBasePtr& _capture_ptr)
+{
+ return track_matrix_.snapshotAsList(_capture_ptr);
+}
+
+inline std::list<LandmarkBasePtr> ProcessorBundleAdjustment::currentLandmarks()
+{
+ return getProblem()->getMap()->getLandmarkList();
+}
+
+} //namespace wolf
+
+#endif /* INCLUDE_BASE_PROCESSOR_PROCESSOR_BUNDLE_ADJUSTMENT_H_ */
diff --git a/include/vision/processor/processor_tracker_feature_image.h b/include/vision/processor/processor_tracker_feature_image.h
index d0961d41b17f2ebcda48c68e22fe763d9e4140b5..ca835c422f0452d1414615e69f038c777b13f23a 100644
--- a/include/vision/processor/processor_tracker_feature_image.h
+++ b/include/vision/processor/processor_tracker_feature_image.h
@@ -5,17 +5,19 @@
#include "vision/sensor/sensor_camera.h"
#include "vision/capture/capture_image.h"
#include "vision/feature/feature_point_image.h"
+#include "vision/processor/processor_params_image.h"
#include <core/state_block/state_block.h>
#include <core/state_block/state_quaternion.h>
#include <core/processor/processor_tracker_feature.h>
#include <core/dummy/factor_feature_dummy.h>
#include "vision/processor/processor_params_image.h"
+
// vision_utils
-#include <detectors/detector_base.h>
-#include <descriptors/descriptor_base.h>
-#include <matchers/matcher_base.h>
-#include <algorithms/activesearch/alg_activesearch.h>
+#include <vision_utils/detectors/detector_base.h>
+#include <vision_utils/descriptors/descriptor_base.h>
+#include <vision_utils/matchers/matcher_base.h>
+#include <vision_utils/algorithms/activesearch/alg_activesearch.h>
// General includes
#include <cmath>
diff --git a/include/vision/processor/processor_tracker_feature_trifocal.h b/include/vision/processor/processor_tracker_feature_trifocal.h
index e307e9843e2dd7f36d2eec79d0ed1ed65171cfb2..752009c80d72674279ce43a604b9f1c29f669665 100644
--- a/include/vision/processor/processor_tracker_feature_trifocal.h
+++ b/include/vision/processor/processor_tracker_feature_trifocal.h
@@ -7,11 +7,11 @@
#include "core/utils/params_server.hpp"
// Vision utils
-#include <vision_utils.h>
-#include <detectors/detector_base.h>
-#include <descriptors/descriptor_base.h>
-#include <matchers/matcher_base.h>
-#include <algorithms/activesearch/alg_activesearch.h>
+#include <vision_utils/vision_utils.h>
+#include <vision_utils/detectors/detector_base.h>
+#include <vision_utils/descriptors/descriptor_base.h>
+#include <vision_utils/matchers/matcher_base.h>
+#include <vision_utils/algorithms/activesearch/alg_activesearch.h>
namespace wolf
{
diff --git a/include/vision/processor/processor_tracker_landmark_image.h b/include/vision/processor/processor_tracker_landmark_image.h
index 91c43ba6423e538ac6845597d6423f2b7053f023..8bc927a1d10265d6c6d6d5832cabcd76c70d7ab3 100644
--- a/include/vision/processor/processor_tracker_landmark_image.h
+++ b/include/vision/processor/processor_tracker_landmark_image.h
@@ -9,10 +9,10 @@
#include "core/processor/processor_tracker_landmark.h"
#include "core/common/wolf.h"
-#include <algorithms/activesearch/alg_activesearch.h>
-#include <descriptors/descriptor_base.h>
-#include <detectors/detector_base.h>
-#include <matchers/matcher_base.h>
+#include <vision_utils/algorithms/activesearch/alg_activesearch.h>
+#include <vision_utils/descriptors/descriptor_base.h>
+#include <vision_utils/detectors/detector_base.h>
+#include <vision_utils/matchers/matcher_base.h>
#include <opencv2/core/mat.hpp>
#include <opencv2/core/mat.inl.hpp>
diff --git a/serialization/CMakeLists.txt b/serialization/CMakeLists.txt
deleted file mode 100644
index a853ba8ec5158727fcd4bfd9752d6de398c5aefa..0000000000000000000000000000000000000000
--- a/serialization/CMakeLists.txt
+++ /dev/null
@@ -1 +0,0 @@
-add_subdirectory(cereal)
diff --git a/serialization/cereal/CMakeLists.txt b/serialization/cereal/CMakeLists.txt
deleted file mode 100644
index 1ae16bcd213401f6558a4ee80c2c5a8ef8c5f171..0000000000000000000000000000000000000000
--- a/serialization/cereal/CMakeLists.txt
+++ /dev/null
@@ -1,26 +0,0 @@
-INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR})
-
-SET(HDRS_SERIALIZATION ${HDRS_SERIALIZATION}
- ${CMAKE_CURRENT_SOURCE_DIR}/serialization_eigen_core.h
- ${CMAKE_CURRENT_SOURCE_DIR}/serialization_eigen_geometry.h
- ${CMAKE_CURRENT_SOURCE_DIR}/serialization_eigen_sparse.h
-
- ${CMAKE_CURRENT_SOURCE_DIR}/serialization_local_parametrization_base.h
- ${CMAKE_CURRENT_SOURCE_DIR}/serialization_local_parametrization_homogeneous.h
- ${CMAKE_CURRENT_SOURCE_DIR}/serialization_local_parametrization_quaternion.h
-
- ${CMAKE_CURRENT_SOURCE_DIR}/serialization_node_base.h
-
- ${CMAKE_CURRENT_SOURCE_DIR}/serialization_sensor_intrinsic_base.h
- ${CMAKE_CURRENT_SOURCE_DIR}/serialization_sensor_odom2d_intrinsic.h
-
- ${CMAKE_CURRENT_SOURCE_DIR}/serialization_processor_params_base.h
- ${CMAKE_CURRENT_SOURCE_DIR}/serialization_processor_odom2d_params.h
- ${CMAKE_CURRENT_SOURCE_DIR}/serialization_processor_odom3d_params.h
-
- ${CMAKE_CURRENT_SOURCE_DIR}/serialization_time_stamp.h
-
- ${CMAKE_CURRENT_SOURCE_DIR}/archives.h
- ${CMAKE_CURRENT_SOURCE_DIR}/io.h
-
- PARENT_SCOPE)
diff --git a/serialization/cereal/archives.h b/serialization/cereal/archives.h
deleted file mode 100644
index d8c16fd180ef04049ceedaf268fa33782790d27a..0000000000000000000000000000000000000000
--- a/serialization/cereal/archives.h
+++ /dev/null
@@ -1,9 +0,0 @@
-#ifndef _WOLF_IO_CEREAL_ARCHIVE_H_
-#define _WOLF_IO_CEREAL_ARCHIVE_H_
-
-#include <cereal/archives/binary.hpp>
-#include <cereal/archives/json.hpp>
-#include <cereal/archives/portable_binary.hpp>
-#include <cereal/archives/xml.hpp>
-
-#endif /* _WOLF_IO_CEREAL_ARCHIVE_H_ */
diff --git a/serialization/cereal/io.h b/serialization/cereal/io.h
deleted file mode 100644
index 8a8e58c2d8a873b4d327e1c8639fc007cb8604aa..0000000000000000000000000000000000000000
--- a/serialization/cereal/io.h
+++ /dev/null
@@ -1,229 +0,0 @@
-#ifndef _WOLF_SERIALIZATION_CEREAL_IO_H_
-#define _WOLF_SERIALIZATION_CEREAL_IO_H_
-
-#include <stdexcept>
-#include "archives.h"
-
-//#include <cereal/types/tuple.hpp>
-
-namespace wolf {
-namespace serialization {
-
-/// @todo demangle typeid.name ?
-template <typename T>
-inline const std::string& type_name(const T&)
-{
- static const std::string typeid_name = typeid(T).name();
- return typeid_name;
-}
-
-inline std::string extension(const std::string& file)
-{
- const std::size_t p = file.find_last_of(".");
- return (p != std::string::npos) ? file.substr(p) : "";
-}
-
-//struct Extensions
-//{
-// constexpr static const char* bin = ".bin";
-// constexpr static const char* json = ".json";
-// constexpr static const char* xml = ".xml";
-
-// constexpr static const char* fall_back = json;
-//};
-
-//enum class Extensions2 : std::size_t
-//{
-// BIN = 0,
-//// CBIN,
-// JSON,
-//// TEXT,
-// XML,
-//};
-
-//template <char... Chars>
-//struct constexp_str
-//{
-// using type = constexp_str<Chars...>;
-
-// virtual ~constexp_str() = default;
-
-// constexpr static const char value[sizeof...(Chars)+1] = {Chars..., '\0'};
-
-// constexpr static std::size_t size() { return sizeof...(Chars); }
-
-// constexpr static const char* c_str() { return &value[0]; }
-
-//// constexpr static bool comp(const std::string& s) { return s == value; }
-
-// /*constexpr*/ bool operator == (const std::string& s) { return s == value; }
-
-// constexpr /*static*/ operator const char* () { return c_str(); }
-// constexpr /*static*/ operator std::string& () { return c_str(); }
-//};
-
-struct Extensions
-{
-// template <char... Chars>
-// struct EXT : constexp_str<Chars...>
-// {
-// //
-// };
-
-// struct BIN : EXT<'.','b','i','n'> { };
-// struct XML : EXT<'.','x','m','l'> { };
-// struct JSON : EXT<'.','j','s','o','n'> { };
-
- struct EXT { virtual ~EXT() = default; };
-
- struct BIN : EXT
- {
- constexpr static const char* value = ".bin";
- bool operator == (const std::string& s) { return value == s; }
- };
-
- struct XML : EXT
- {
- constexpr static const char* value = ".xml";
- bool operator == (const std::string& s) { return value == s; }
- };
-
- struct JSON : EXT
- {
- constexpr static const char* value = ".json";
- bool operator == (const std::string& s) { return value == s; }
- };
-};
-
-template <typename Ar>
-void serialize_pack(Ar&&)
-{
- // end of expansion
-}
-
-template <typename Ar, typename T, typename... Args>
-void serialize_pack(Ar&& archive, T&& object, Args&&... args)
-{
- archive( cereal::make_nvp(type_name(object), std::forward<T>(object)) );
- serialize_pack(archive, std::forward<Args>(args)...);
-}
-
-template <typename Ar, typename S, typename T, typename... Args>
-void serialize(S& stream, T&& object, Args&&... args)
-{
- Ar archive(stream);
- archive( cereal::make_nvp(type_name(object), std::forward<T>(object)) );
-
- serialize_pack(archive, std::forward<Args>(args)...);
-}
-
-template <typename EXT, typename InAr, typename OutAr>
-struct Serializer
-{
- template <typename S, typename... T>
- static void serialize_in(S& stream, T&... object)
- {
- serialize<InAr>(stream, object...);
- }
-
- template <typename S, typename... T>
- static void serialize_out(S& stream, T&&... object)
- {
- serialize<OutAr>(stream, std::forward<T>(object)...);
- }
-
- template <typename... T>
- static void save(std::string filename, T&&... o)
- {
- const std::string ext = serialization::extension(filename);
-
- if (ext != EXT::value) filename += EXT::value;
-
- std::ofstream os(filename);
- serialize_out(os, std::forward<T>(o)...);
- }
-
- template <typename... T>
- static void load(std::string filename, T&... o)
- {
- const std::string ext = serialization::extension(filename);
-
- if (ext != EXT::value) filename += EXT::value;
-
- std::ifstream is(filename);
- serialize_in(is, o...);
- }
-};
-
-using SerializerBin = Serializer<Extensions::BIN,
- cereal::BinaryInputArchive,
- cereal::BinaryOutputArchive>;
-
-using SerializerXML = Serializer<Extensions::XML,
- cereal::XMLInputArchive,
- cereal::XMLOutputArchive>;
-
-using SerializerJSON = Serializer<Extensions::JSON,
- cereal::JSONInputArchive,
- cereal::JSONOutputArchive>;
-
-} /* namespace serialization */
-
-template <typename... T>
-void save(const std::string& filename, T&&... o)
-throw(std::runtime_error)
-{
- const std::string ext = serialization::extension(filename);
-
- if (ext == serialization::Extensions::BIN::value)
- {
- serialization::SerializerBin::save(filename, std::forward<T>(o)...);
- }
- else if (ext == serialization::Extensions::JSON::value)
- {
- serialization::SerializerJSON::save(filename, std::forward<T>(o)...);
- }
- else if (ext == serialization::Extensions::XML::value)
- {
- serialization::SerializerXML::save(filename, std::forward<T>(o)...);
- }
- else if (ext == "") // falback is json
- {
- serialization::SerializerJSON::save(filename, std::forward<T>(o)...);
- }
- else
- {
- throw std::runtime_error("Unknown file extension : " + filename);
- }
-}
-
-template <typename... T>
-void load(const std::string& filename, T&... o)
-{
- const std::string ext = serialization::extension(filename);
-
- if (ext == serialization::Extensions::BIN::value)
- {
- serialization::SerializerBin::load(filename, o...);
- }
- else if (ext == serialization::Extensions::XML::value)
- {
- serialization::SerializerXML::load(filename, o...);
- }
- else if (ext == serialization::Extensions::JSON::value)
- {
- serialization::SerializerJSON::load(filename, o...);
- }
- else if (ext == "") // falback is json
- {
- serialization::SerializerJSON::load(filename, o...);
- }
- else
- {
- throw std::runtime_error("Unknown file extension : " + filename);
- }
-}
-
-} /* namespace wolf */
-
-#endif /* _WOLF_SERIALIZATION_CEREAL_IO_H_ */
diff --git a/serialization/cereal/serialization_eigen_core.h b/serialization/cereal/serialization_eigen_core.h
deleted file mode 100644
index 8fb5b9b0c99637e7b4aed9efb3aaf68007aafb79..0000000000000000000000000000000000000000
--- a/serialization/cereal/serialization_eigen_core.h
+++ /dev/null
@@ -1,125 +0,0 @@
-#ifndef _WOLF_IO_CEREAL_EIGEN_H_
-#define _WOLF_IO_CEREAL_EIGEN_H_
-
-// Wolf includes
-#include <Eigen/Dense>
-#include <cereal/cereal.hpp>
-
-namespace cereal {
-
-/**
- * @brief Save Eigen::Matrix<...> to text based archives
- */
-template<class Archive, typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
-inline typename std::enable_if<!traits::is_output_serializable<BinaryData<_Scalar>, Archive>::value,
-void>::type save(Archive& ar,
- const Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& mat)
-{
- decltype(mat.rows()) rows = mat.rows();
- decltype(mat.cols()) cols = mat.cols();
-
- ar(cereal::make_nvp("rows", rows));
- ar(cereal::make_nvp("cols", cols));
-
- /// @todo find out something
- std::cerr << "Saving Eigen type to text-based archive is NOT supported !\n";
-}
-
-/**
- * @brief Save Eigen::Matrix<...> to binary based archives
- */
-template<class Archive, typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
-inline typename std::enable_if<traits::is_output_serializable<BinaryData<_Scalar>, Archive>::value,
-void>::type save(Archive& ar,
- const Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& mat)
-{
- decltype(mat.rows()) rows = mat.rows();
- decltype(mat.cols()) cols = mat.cols();
-
- ar(rows);
- ar(cols);
-
- ar(binary_data(mat.data(), static_cast<std::size_t>(rows * cols * sizeof(_Scalar))));
-}
-
-/**
- * @brief Load compile-time sized Eigen::Matrix from text based archives
- */
-template<class Archive, typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
-inline typename std::enable_if<!traits::is_input_serializable<BinaryData<_Scalar>, Archive>::value and
- _Rows != Eigen::Dynamic and _Cols != Eigen::Dynamic,
-void>::type load(Archive& ar,
- Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& mat)
-{
- decltype(mat.rows()) rows;
- decltype(mat.cols()) cols;
-
- ar(cereal::make_nvp("rows", rows));
- ar(cereal::make_nvp("cols", cols));
-
- /// @todo find out something
- std::cerr << "Saving Eigen type to text-based archive is NOT supported !\n";
-}
-
-/**
- * @brief Load dynamic sized Eigen::Matrix from text based archives
- */
-template<class Archive, typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
-inline typename std::enable_if<!traits::is_input_serializable<BinaryData<_Scalar>, Archive>::value and
- (_Rows == Eigen::Dynamic or _Cols == Eigen::Dynamic),
-void>::type load(Archive& ar,
- Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& mat)
-{
- decltype(mat.rows()) rows;
- decltype(mat.cols()) cols;
-
- ar(cereal::make_nvp("rows", rows));
- ar(cereal::make_nvp("cols", cols));
-
- /// @todo find out something
- std::cerr << "Saving Eigen type to text-based archive is NOT supported !\n";
-
- //mat.resize(rows, cols);
-}
-
-/**
- * @brief Load compile-time sized Eigen::Matrix from binary based archives
- */
-template<class Archive, typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
-inline typename std::enable_if<traits::is_input_serializable<BinaryData<_Scalar>, Archive>::value and
- _Rows != Eigen::Dynamic and _Cols != Eigen::Dynamic,
-void>::type load(Archive& ar,
- Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& mat)
-{
- decltype(mat.rows()) rows;
- decltype(mat.cols()) cols;
-
- ar(rows);
- ar(cols);
-
- ar(binary_data(mat.data(), static_cast<std::size_t>(rows * cols * sizeof(_Scalar))));
-}
-
-/**
- * @brief Load dynamic sized Eigen::Matrix from binary based archives
- */
-template<class Archive, typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
-inline typename std::enable_if<traits::is_input_serializable<BinaryData<_Scalar>, Archive>::value and
- (_Rows == Eigen::Dynamic or _Cols == Eigen::Dynamic),
-void>::type load(Archive& ar,
- Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& mat)
-{
- decltype(mat.rows()) rows;
- decltype(mat.cols()) cols;
-
- ar(rows);
- ar(cols);
-
- mat.resize(rows, cols);
-
- ar(binary_data(mat.data(), static_cast<std::size_t>(rows * cols * sizeof(_Scalar))));
-}
-
-} // namespace cereal
-
-#endif /* _WOLF_IO_CEREAL_EIGEN_H_ */
diff --git a/serialization/cereal/serialization_eigen_geometry.h b/serialization/cereal/serialization_eigen_geometry.h
deleted file mode 100644
index f5d3ac61fa92c85d5c2b98670e86e5c6d3d68c24..0000000000000000000000000000000000000000
--- a/serialization/cereal/serialization_eigen_geometry.h
+++ /dev/null
@@ -1,38 +0,0 @@
-#ifndef _WOLF_IO_CEREAL_EIGEN_GEOMETRY_H_
-#define _WOLF_IO_CEREAL_EIGEN_GEOMETRY_H_
-
-// Wolf includes
-#include <Eigen/Geometry>
-
-#include "serialization_eigen_core.h"
-
-namespace cereal {
-
-template<class Archive, typename _Scalar, int _Dim, int _Mode, int _Options>
-inline void save(Archive& ar,
- const Eigen::Transform<_Scalar, _Dim, _Mode, _Options>& t)
-{
- save(ar, t.matrix());
-}
-
-template<class Archive, typename _Scalar, int _Dim, int _Mode, int _Options>
-inline void load(Archive& ar,
- Eigen::Transform<_Scalar, _Dim, _Mode, _Options>& t)
-{
- load(ar, t.matrix());
-}
-
-template<class Archive, typename _Scalar>
-void serialize(Archive & ar,
- Eigen::Quaternion<_Scalar>& q,
- const std::uint32_t /*version*/)
-{
- ar(cereal::make_nvp("w", q.w()));
- ar(cereal::make_nvp("x", q.x()));
- ar(cereal::make_nvp("y", q.y()));
- ar(cereal::make_nvp("z", q.z()));
-}
-
-} // namespace cereal
-
-#endif /* _WOLF_IO_CEREAL_EIGEN_GEOMETRY_H_ */
diff --git a/serialization/cereal/serialization_eigen_sparse.h b/serialization/cereal/serialization_eigen_sparse.h
deleted file mode 100644
index 9039b6920bc74abb3b4ff9efa4e0b935674ca53f..0000000000000000000000000000000000000000
--- a/serialization/cereal/serialization_eigen_sparse.h
+++ /dev/null
@@ -1,79 +0,0 @@
-#ifndef _WOLF_IO_CEREAL_EIGEN_GEOMETRY_H_
-#define _WOLF_IO_CEREAL_EIGEN_GEOMETRY_H_
-
-// Wolf includes
-#include <Eigen/Sparse>
-
-#include "serialization_eigen_core.h"
-#include <cereal/types/vector.hpp>
-
-namespace cereal {
-
-template<class Archive, typename Scalar, typename Index>
-inline void save(Archive& ar,
- const Eigen::Triplet<Scalar, Index>& t)
-{
- ar(cereal::make_nvp("row", t.row()));
- ar(cereal::make_nvp("col", t.col()));
- ar(cereal::make_nvp("value", t.value()));
-}
-
-template<class Archive, typename Scalar, typename Index>
-inline void load(Archive& ar,
- Eigen::Triplet<Scalar, Index>& t)
-{
- Index row, col;
- Scalar value;
-
- ar(cereal::make_nvp("row", row));
- ar(cereal::make_nvp("col", col));
- ar(cereal::make_nvp("value", value));
-
- t = Eigen::Triplet<Scalar, Index>(row, col, value);
-}
-
-template <class Archive, typename _Scalar, int _Options, typename _Index>
-void save(Archive& ar,
- const Eigen::SparseMatrix<_Scalar, _Options, _Index>& m)
-{
- _Index inner_size = m.innerSize();
- _Index outer_size = m.outerSize();
-
- using Triplet = Eigen::Triplet<_Scalar>;
- std::vector<Triplet> triplets;
-
- for (_Index i=0; i < outer_size; ++i)
- for (typename Eigen::SparseMatrix<_Scalar, _Options, _Index>::InnerIterator it(m,i); it; ++it)
- triplets.emplace_back( it.row(), it.col(), it.value() );
-
- ar(cereal::make_nvp("inner_size", inner_size));
- ar(cereal::make_nvp("outer_size", outer_size));
- ar(cereal::make_nvp("triplets", triplets));
-}
-
-template <class Archive, typename _Scalar, int _Options, typename _Index>
-void load(Archive& ar,
- Eigen::SparseMatrix<_Scalar, _Options, _Index>& m)
-{
- _Index inner_size;
- _Index outer_size;
-
- ar(cereal::make_nvp("inner_size", inner_size));
- ar(cereal::make_nvp("outer_size", outer_size));
-
- _Index rows = (m.IsRowMajor)? outer_size : inner_size;
- _Index cols = (m.IsRowMajor)? inner_size : outer_size;
-
- m.resize(rows, cols);
-
- using Triplet = Eigen::Triplet<_Scalar>;
- std::vector<Triplet> triplets;
-
- ar(cereal::make_nvp("triplets", triplets));
-
- m.setFromTriplets(triplets.begin(), triplets.end());
-}
-
-} // namespace cereal
-
-#endif /* _WOLF_IO_CEREAL_EIGEN_GEOMETRY_H_ */
diff --git a/serialization/cereal/serialization_local_parametrization_base.h b/serialization/cereal/serialization_local_parametrization_base.h
deleted file mode 100644
index b43c35673ae05e9088c045c167fa9a02a3139c8a..0000000000000000000000000000000000000000
--- a/serialization/cereal/serialization_local_parametrization_base.h
+++ /dev/null
@@ -1,26 +0,0 @@
-#ifndef _WOLF_IO_CEREAL_LOCAL_PARAMETRIZATION_BASE_H_
-#define _WOLF_IO_CEREAL_LOCAL_PARAMETRIZATION_BASE_H_
-
-#include "core/state_block/local_parametrization_base.h"
-
-#include <cereal/cereal.hpp>
-#include <cereal/types/polymorphic.hpp>
-
-namespace cereal {
-
-// Since classes deriving from LocalParametrizationBase
-// have default constructor calling the non-default
-// LocalParametrizationBase constructor with pre-defined
-// arguments, there is nothing to save here.
-template<class Archive>
-inline void serialize(
- Archive& /*ar*/,
- wolf::LocalParametrizationBase& /*lpb*/,
- std::uint32_t const /*file_version*/)
-{
- //
-}
-
-} //namespace cereal
-
-#endif /* _WOLF_IO_CEREAL_LOCAL_PARAMETRIZATION_BASE_H_ */
diff --git a/serialization/cereal/serialization_local_parametrization_homogeneous.h b/serialization/cereal/serialization_local_parametrization_homogeneous.h
deleted file mode 100644
index 9fcc656d5b86c51eb3995d1a11fbdcc80b1e98c4..0000000000000000000000000000000000000000
--- a/serialization/cereal/serialization_local_parametrization_homogeneous.h
+++ /dev/null
@@ -1,27 +0,0 @@
-#ifndef _WOLF_IO_CEREAL_LOCAL_PARAMETRIZATION_BASE_HOMOGENEOUS_H_
-#define _WOLF_IO_CEREAL_LOCAL_PARAMETRIZATION_BASE_HOMOGENEOUS_H_
-
-#include "core/local_parametrization_homogeneous.h"
-
-#include "serialization_local_parametrization_base.h"
-
-#include <cereal/cereal.hpp>
-
-CEREAL_REGISTER_TYPE_WITH_NAME(wolf::LocalParametrizationHomogeneous,
- "LocalParametrizationHomogeneous");
-
-namespace cereal {
-
-template<class Archive>
-inline void serialize(
- Archive& ar,
- wolf::LocalParametrizationHomogeneous& lp,
- std::uint32_t const /*file_version*/)
-{
- ar( cereal::make_nvp("LocalParametrizationBase",
- cereal::base_class<wolf::LocalParametrizationBase>(&lp)) );
-}
-
-} //namespace cereal
-
-#endif /* _WOLF_IO_CEREAL_LOCAL_PARAMETRIZATION_BASE_HOMOGENEOUS_H_ */
diff --git a/serialization/cereal/serialization_local_parametrization_quaternion.h b/serialization/cereal/serialization_local_parametrization_quaternion.h
deleted file mode 100644
index 66fe30a361f0f1878a9444d9fb487ad2a068ad0d..0000000000000000000000000000000000000000
--- a/serialization/cereal/serialization_local_parametrization_quaternion.h
+++ /dev/null
@@ -1,30 +0,0 @@
-#ifndef WOLF_IO_SERIALIZATION_LOCAL_PARAMETRIZATION_QUATERNION_H_
-#define WOLF_IO_SERIALIZATION_LOCAL_PARAMETRIZATION_QUATERNION_H_
-
-#include "core/local_parametrization_quaternion.h"
-
-#include "serialization_local_parametrization_base.h"
-
-#include <cereal/cereal.hpp>
-
-CEREAL_REGISTER_TYPE_WITH_NAME(wolf::LocalParametrizationQuaternion<wolf::DQ_LOCAL>,
- "wolf_LocalParametrizationQuaternion_DQ_LOCAL")
-
-CEREAL_REGISTER_TYPE_WITH_NAME(wolf::LocalParametrizationQuaternion<wolf::DQ_GLOBAL>,
- "wolf_LocalParametrizationQuaternion_DQ_GLOBAL")
-
-namespace cereal {
-
-template<class Archive, unsigned int DeltaReference>
-inline void serialize(
- Archive &ar,
- wolf::LocalParametrizationQuaternion<DeltaReference> &lp,
- const unsigned int /*file_version*/)
-{
- ar( cereal::make_nvp("LocalParametrizationBase",
- cereal::base_class<wolf::LocalParametrizationBase>(&lp)) );
-}
-
-} //namespace boost
-
-#endif /* WOLF_IO_SERIALIZATION_LOCAL_PARAMETRIZATION_QUATERNION_H_ */
diff --git a/serialization/cereal/serialization_node_base.h b/serialization/cereal/serialization_node_base.h
deleted file mode 100644
index a2c592d6982485de2aebf189e09e764b68022783..0000000000000000000000000000000000000000
--- a/serialization/cereal/serialization_node_base.h
+++ /dev/null
@@ -1,77 +0,0 @@
-#ifndef _WOLF_IO_CEREAL_NODE_BASE_H_
-#define _WOLF_IO_CEREAL_NODE_BASE_H_
-
-// Wolf includes
-#include "core/node_base.h"
-
-#include <cereal/cereal.hpp>
-#include <cereal/types/polymorphic.hpp>
-
-namespace wolf {
-
-struct NodeBase::Serializer {
-
- template <class Archive>
- static void serialize(Archive& ar, NodeBase& o, std::uint32_t const /*version*/)
- {
- ar( cereal::make_nvp("node_class_", o.node_category_) );
- ar( cereal::make_nvp("node_type_", o.node_type_) );
- ar( cereal::make_nvp("node_name_", o.node_name_) );
- ar( cereal::make_nvp("node_id_", o.node_id_) );
-
-// ar( cereal::make_nvp("problem_ptr_", o.problem_ptr_) );
-
- // Not sure what to do with this guy ...
- //ar( cereal::make_nvp("node_id_count_", o.node_id_count_) );
- }
-
- template <class Archive>
- static void load_and_construct( Archive& ar, cereal::construct<wolf::NodeBase>& construct,
- std::uint32_t const /*version*/ )
- {
- decltype(std::declval<wolf::NodeBase>().getCategory()) nb_class;
- decltype(std::declval<wolf::NodeBase>().getType()) nb_type;
- decltype(std::declval<wolf::NodeBase>().getName()) nb_name;
-
- ar( cereal::make_nvp("node_class_", nb_class) );
- ar( cereal::make_nvp("node_type_", nb_type) );
- ar( cereal::make_nvp("node_name_", nb_name) );
-
- construct( nb_class, nb_type, nb_name );
-
- ar( cereal::make_nvp("node_id_", construct->node_id_) );
-
-// ar( cereal::make_nvp("problem_ptr_", construct->problem_ptr_) );
-
- // Not sure what to do with this guy ...
- //ar( cereal::make_nvp("node_id_count_", construct->node_id_count_) );
- }
-};
-
-} // namespace wolf
-
-namespace cereal {
-
-/// @note No default constructor thus the need
-/// for these specializations
-template <>
-struct LoadAndConstruct<wolf::NodeBase>
-{
- template <class Archive>
- static void load_and_construct( Archive& ar,
- cereal::construct<wolf::NodeBase>& construct,
- std::uint32_t const version )
- {
- wolf::NodeBase::Serializer::load_and_construct(ar, construct, version);
- }
-};
-
-template <class Archive>
-void serialize(Archive& ar, wolf::NodeBase& o, std::uint32_t const version)
-{
- wolf::NodeBase::Serializer::serialize(ar, o, version);
-}
-
-} // namespace cereal
-
-#endif /* _WOLF_IO_CEREAL_NODE_BASE_H_ */
diff --git a/serialization/cereal/serialization_processor_odom2d_params.h b/serialization/cereal/serialization_processor_odom2d_params.h
deleted file mode 100644
index dc0416b94634ef8415919c26fa972c86edb99a00..0000000000000000000000000000000000000000
--- a/serialization/cereal/serialization_processor_odom2d_params.h
+++ /dev/null
@@ -1,29 +0,0 @@
-#ifndef _WOLF_SERIALIZATION_CEREAL_PROCESSOR_ODOM2D_PARAMS_H_
-#define _WOLF_SERIALIZATION_CEREAL_PROCESSOR_ODOM2D_PARAMS_H_
-
-// Wolf includes
-#include "core/processor/processor_odom_2D.h"
-#include "serialization_processor_params_base.h"
-
-namespace cereal {
-
-template <class Archive>
-void serialize(Archive& ar, wolf::ProcessorParamsOdom2D& o,
- std::uint32_t const /*version*/)
-{
- ar( cereal::make_nvp("ProcessorParamsBase",
- cereal::base_class<wolf::ProcessorParamsBase>(&o)) );
-
- ar( cereal::make_nvp("cov_det_th_", o.cov_det) );
- ar( cereal::make_nvp("dist_traveled_th_", o.dist_traveled_th_) );
- ar( cereal::make_nvp("elapsed_time_th_", o.elapsed_time_th_) );
- ar( cereal::make_nvp("theta_traveled_th_", o.theta_traveled_th_) );
- ar( cereal::make_nvp("unmeasured_perturbation_std_",
- o.unmeasured_perturbation_std) );
-}
-
-} // namespace cereal
-
-CEREAL_REGISTER_TYPE_WITH_NAME(wolf::ProcessorParamsOdom2D, "ProcessorParamsOdom2D")
-
-#endif /* _WOLF_SERIALIZATION_CEREAL_PROCESSOR_ODOM3D_PARAMS_H_ */
diff --git a/serialization/cereal/serialization_processor_odom3d_params.h b/serialization/cereal/serialization_processor_odom3d_params.h
deleted file mode 100644
index d2fd7c077d868e4dafcfa209c10767415f092ab8..0000000000000000000000000000000000000000
--- a/serialization/cereal/serialization_processor_odom3d_params.h
+++ /dev/null
@@ -1,27 +0,0 @@
-#ifndef _WOLF_SERIALIZATION_CEREAL_PROCESSOR_ODOM3D_PARAMS_H_
-#define _WOLF_SERIALIZATION_CEREAL_PROCESSOR_ODOM3D_PARAMS_H_
-
-// Wolf includes
-#include "core/processor/processor_odom_3D.h"
-#include "serialization_processor_params_base.h"
-
-namespace cereal {
-
-template <class Archive>
-void serialize(Archive& ar, wolf::ProcessorParamsOdom3D& o,
- std::uint32_t const /*version*/)
-{
- ar( cereal::make_nvp("ProcessorParamsBase",
- cereal::base_class<wolf::ProcessorParamsBase>(&o)) );
-
- ar( cereal::make_nvp("angle_turned", o.angle_turned) );
- ar( cereal::make_nvp("dist_traveled", o.dist_traveled) );
- ar( cereal::make_nvp("max_buff_length", o.max_buff_length) );
- ar( cereal::make_nvp("max_time_span", o.max_time_span) );
-}
-
-} // namespace cereal
-
-CEREAL_REGISTER_TYPE_WITH_NAME(wolf::ProcessorParamsOdom3D, "ProcessorOdom3DParams")
-
-#endif /* _WOLF_SERIALIZATION_CEREAL_PROCESSOR_ODOM3D_PARAMS_H_ */
diff --git a/serialization/cereal/serialization_processor_params_base.h b/serialization/cereal/serialization_processor_params_base.h
deleted file mode 100644
index 03ea158c07ea0c18516400a6c51618998efb9473..0000000000000000000000000000000000000000
--- a/serialization/cereal/serialization_processor_params_base.h
+++ /dev/null
@@ -1,22 +0,0 @@
-#ifndef _WOLF_SERIALIZATION_CEREAL_PROCESSOR_PARAM_BASE_H_
-#define _WOLF_SERIALIZATION_CEREAL_PROCESSOR_PARAM_BASE_H_
-
-// Wolf includes
-#include "core/processor/processor_base.h"
-
-#include <cereal/cereal.hpp>
-#include <cereal/types/polymorphic.hpp>
-
-namespace cereal {
-
-template <class Archive>
-void serialize(Archive& ar, wolf::ProcessorParamsBase& o,
- std::uint32_t const /*version*/)
-{
- ar( cereal::make_nvp("type", o.type) );
- ar( cereal::make_nvp("name", o.name) );
-}
-
-} // namespace cereal
-
-#endif /* _WOLF_SERIALIZATION_CEREAL_SENSOR_INTRINSIC_BASE_H_ */
diff --git a/serialization/cereal/serialization_sensor_intrinsic_base.h b/serialization/cereal/serialization_sensor_intrinsic_base.h
deleted file mode 100644
index 86b2a9b482eb61dd290dd2d2723c41415ca11dae..0000000000000000000000000000000000000000
--- a/serialization/cereal/serialization_sensor_intrinsic_base.h
+++ /dev/null
@@ -1,25 +0,0 @@
-#ifndef _WOLF_SERIALIZATION_CEREAL_SENSOR_INTRINSIC_BASE_H_
-#define _WOLF_SERIALIZATION_CEREAL_SENSOR_INTRINSIC_BASE_H_
-
-// Wolf includes
-#include "core/sensor/sensor_base.h"
-
-#include <cereal/cereal.hpp>
-#include <cereal/types/polymorphic.hpp>
-
-namespace cereal {
-
-template <class Archive>
-void serialize(Archive& ar, wolf::IntrinsicsBase& o,
- std::uint32_t const /*version*/)
-{
- ar( cereal::make_nvp("type", o.type) );
- ar( cereal::make_nvp("name", o.name) );
-}
-
-} // namespace cereal
-
-// No need to register base
-//CEREAL_REGISTER_TYPE_WITH_NAME(wolf::IntrinsicsBase, "IntrinsicsBase");
-
-#endif /* _WOLF_SERIALIZATION_CEREAL_SENSOR_INTRINSIC_BASE_H_ */
diff --git a/serialization/cereal/serialization_sensor_odom2d_intrinsic.h b/serialization/cereal/serialization_sensor_odom2d_intrinsic.h
deleted file mode 100644
index 17d4160b3751d0a13f0f28836234b500e6674400..0000000000000000000000000000000000000000
--- a/serialization/cereal/serialization_sensor_odom2d_intrinsic.h
+++ /dev/null
@@ -1,26 +0,0 @@
-#ifndef _WOLF_SERIALIZATION_CEREAL_SENSOR_ODOM2D_INTRINSIC_H_
-#define _WOLF_SERIALIZATION_CEREAL_SENSOR_ODOM2D_INTRINSIC_H_
-
-// Wolf includes
-#include "core/sensor/sensor_odom_2D.h"
-
-#include "serialization_sensor_intrinsic_base.h"
-
-namespace cereal {
-
-template <class Archive>
-void serialize(Archive& ar, wolf::IntrinsicsOdom2D& o,
- std::uint32_t const /*version*/)
-{
- ar( cereal::make_nvp("IntrinsicsBase",
- cereal::base_class<wolf::IntrinsicsBase>(&o)) );
-
- ar( cereal::make_nvp("k_disp_to_disp", o.k_disp_to_disp) );
- ar( cereal::make_nvp("k_rot_to_rot", o.k_rot_to_rot) );
-}
-
-} // namespace cereal
-
-CEREAL_REGISTER_TYPE_WITH_NAME(wolf::IntrinsicsOdom2D, "IntrinsicsOdom2D")
-
-#endif /* _WOLF_SERIALIZATION_CEREAL_SENSOR_ODOM2D_INTRINSIC_H_ */
diff --git a/serialization/cereal/serialization_time_stamp.h b/serialization/cereal/serialization_time_stamp.h
deleted file mode 100644
index f0c978d36415be7598f2ba6eb5b83bd9a5cb0a61..0000000000000000000000000000000000000000
--- a/serialization/cereal/serialization_time_stamp.h
+++ /dev/null
@@ -1,31 +0,0 @@
-#ifndef _WOLF_IO_CEREAL_TIME_STAMP_H_
-#define _WOLF_IO_CEREAL_TIME_STAMP_H_
-
-// Wolf includes
-#include "core/time_stamp.h"
-
-#include <cereal/cereal.hpp>
-
-namespace cereal {
-
-/// @note serialization versionning raise
-/// a compile error here...
-template <class Archive>
-void save(Archive& ar, const wolf::TimeStamp& o/*, std::uint32_t const version*/)
-{
- ar( cereal::make_nvp("value", o.get()) );
-}
-
-template <class Archive>
-void load(Archive& ar, wolf::TimeStamp& o/*, std::uint32_t const version*/)
-{
- auto val = o.get();
-
- ar( cereal::make_nvp("value", val) );
-
- o.set(val);
-}
-
-} // namespace cereal
-
-#endif /* _WOLF_IO_CEREAL_TIME_STAMP_H_ */
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
deleted file mode 100644
index a7a0ac103ab9387d6b7b59646c073ed2a1c653fb..0000000000000000000000000000000000000000
--- a/src/CMakeLists.txt
+++ /dev/null
@@ -1,762 +0,0 @@
-#Start WOLF build
-MESSAGE("Starting WOLF CMakeLists ...")
-CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
-
-#CMAKE modules
-
-SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake_modules")
-MESSAGE(STATUS ${CMAKE_MODULE_PATH})
-
-# Some wolf compilation options
-
-IF((CMAKE_BUILD_TYPE MATCHES DEBUG) OR (CMAKE_BUILD_TYPE MATCHES debug) OR (CMAKE_BUILD_TYPE MATCHES Debug))
- set(_WOLF_DEBUG true)
-ENDIF()
-
-option(_WOLF_TRACE "Enable wolf tracing macro" ON)
-
-option(BUILD_EXAMPLES "Build examples" ON)
-set(BUILD_TESTS true)
-
-# Does this has any other interest
-# but for the examples ?
-# yes, for the tests !
-IF(BUILD_EXAMPLES OR BUILD_TESTS)
- set(_WOLF_ROOT_DIR ${CMAKE_SOURCE_DIR})
-ENDIF(BUILD_EXAMPLES OR BUILD_TESTS)
-
-#find dependencies.
-
-FIND_PACKAGE(Eigen3 3.2.92 REQUIRED)
-
-FIND_PACKAGE(Threads REQUIRED)
-
-FIND_PACKAGE(Ceres QUIET) #Ceres is not required
-IF(Ceres_FOUND)
- MESSAGE("Ceres Library FOUND: Ceres related sources will be built.")
-ENDIF(Ceres_FOUND)
-
-FIND_PACKAGE(faramotics QUIET) #faramotics is not required
-IF(faramotics_FOUND)
- FIND_PACKAGE(GLUT REQUIRED)
- FIND_PACKAGE(pose_state_time REQUIRED)
- MESSAGE("Faramotics Library FOUND: Faramotics related sources will be built.")
-ENDIF(faramotics_FOUND)
-
-FIND_PACKAGE(laser_scan_utils QUIET) #laser_scan_utils is not required
-IF(laser_scan_utils_FOUND)
- MESSAGE("laser_scan_utils Library FOUND: laser_scan_utils related sources will be built.")
-ENDIF(laser_scan_utils_FOUND)
-
-FIND_PACKAGE(raw_gps_utils QUIET) #raw_gps_utils is not required
-IF(raw_gps_utils_FOUND)
- MESSAGE("raw_gps_utils Library FOUND: raw_gps_utils related sources will be built.")
-ENDIF(raw_gps_utils_FOUND)
-
-# Vision Utils
-FIND_PACKAGE(vision_utils QUIET)
-IF (vision_utils_FOUND)
- MESSAGE("vision_utils Library FOUND: vision related sources will be built.")
- SET(PRINT_INFO_VU false)
- FIND_PACKAGE(OpenCV QUIET)
-ENDIF(vision_utils_FOUND)
-
-# OpenCV
-FIND_PACKAGE(OpenCV QUIET)
-IF (OPENCV_FOUND)
- MESSAGE("opencv Library FOUND: opencv related sources will be built.")
-ENDIF(OPENCV_FOUND)
-
-# Cereal
-FIND_PACKAGE(cereal QUIET)
-IF(cereal_FOUND)
- MESSAGE("cereal Library FOUND: cereal related sources will be built.")
-ENDIF(cereal_FOUND)
-
-# Apriltag
-# TODO: write proper files to be able to use find_package with apriltag library
-FIND_PATH(APRILTAG_INCLUDE_DIR NAMES apriltag.h PATH_SUFFIXES "apriltag" ${APRILTAG_INCLUDE_PATH})
-FIND_LIBRARY(APRILTAG_LIBRARY NAMES apriltag PATH_SUFFIXES "${CMAKE_LIBRARY_ARCHITECTURE}" "apriltag" ${APRILTAG_LIBRARY_PATH})
-
-IF(APRILTAG_LIBRARY)
- SET(Apriltag_FOUND TRUE)
- MESSAGE("apriltag Library FOUND in ${APRILTAG_LIBRARY}: apriltag related sources will be built.")
-ENDIF(APRILTAG_LIBRARY)
-
-# YAML with yaml-cpp
-INCLUDE (${PROJECT_SOURCE_DIR}/cmake_modules/FindYamlCpp.cmake)
-IF(YAMLCPP_FOUND)
- MESSAGE("yaml-cpp Library FOUND: yaml-cpp related sources will be built.")
-ELSEIF(YAMLCPP_FOUND)
- MESSAGE("yaml-cpp Library NOT FOUND!")
-ENDIF(YAMLCPP_FOUND)
-
-#GLOG
-INCLUDE (${PROJECT_SOURCE_DIR}/cmake_modules/FindGlog.cmake)
-IF(GLOG_FOUND)
- MESSAGE("glog Library FOUND: glog related sources will be built.")
- MESSAGE(STATUS ${GLOG_INCLUDE_DIR})
- MESSAGE(STATUS ${GLOG_LIBRARY})
-ELSEIF(GLOG_FOUND)
- MESSAGE("glog Library NOT FOUND!")
-ENDIF(GLOG_FOUND)
-
-# SuiteSparse doesn't have find*.cmake:
-FIND_PATH(
- Suitesparse_INCLUDE_DIRS
- NAMES SuiteSparse_config.h
- PATHS /usr/include/suitesparse /usr/local/include/suitesparse)
-MESSAGE("Found suitesparse_INCLUDE_DIRS:" ${Suitesparse_INCLUDE_DIRS})
-
-IF(Suitesparse_INCLUDE_DIRS)
- SET(Suitesparse_FOUND TRUE)
- MESSAGE("Suitesparse FOUND: wolf_solver will be built.")
-ELSE (Suitesparse_INCLUDE_DIRS)
- SET(Suitesparse_FOUND FALSE)
- MESSAGE(FATAL_ERROR "Suitesparse NOT FOUND")
-ENDIF (Suitesparse_INCLUDE_DIRS)
-
-# Define the directory where will be the configured config.h
-SET(WOLF_CONFIG_DIR ${PROJECT_BINARY_DIR}/conf/internal)
-
-# Create the specified output directory if it does not exist.
-IF(NOT EXISTS "${WOLF_CONFIG_DIR}")
- message(STATUS "Creating config output directory: ${WOLF_CONFIG_DIR}")
- file(MAKE_DIRECTORY "${WOLF_CONFIG_DIR}")
-ENDIF()
-IF(EXISTS "${WOLF_CONFIG_DIR}" AND NOT IS_DIRECTORY "${WOLF_CONFIG_DIR}")
- message(FATAL_ERROR "Bug: Specified CONFIG_DIR: "
- "${WOLF_CONFIG_DIR} exists, but is not a directory.")
-ENDIF()
-# Configure config.h
-configure_file(${CMAKE_CURRENT_SOURCE_DIR}/internal/config.h.in "${WOLF_CONFIG_DIR}/config.h")
-
-#TEMPORAL INCLUDE UNTIL WE FIGURE OUT WHAT TO DO WITH FILES in src/temp
-
-include_directories(${CMAKE_CURRENT_SOURCE_DIR})
-include_directories(../dummyInclude)
-
-# Include config.h directory at first.
-include_directories("${PROJECT_BINARY_DIR}/conf")
-
-INCLUDE_DIRECTORIES(.)
-
-# include spdlog (logging library)
-FIND_PATH(SPDLOG_INCLUDE_DIR spdlog.h /usr/local/include/spdlog /usr/include/spdlog)
-IF (SPDLOG_INCLUDE_DIR)
- INCLUDE_DIRECTORIES(${SPDLOG_INCLUDE_DIR})
- MESSAGE(STATUS "Found spdlog: ${SPDLOG_INCLUDE_DIR}")
-ELSE (SPDLOG_INCLUDE_DIR)
- MESSAGE(FATAL_ERROR "Could not find spdlog")
-ENDIF (SPDLOG_INCLUDE_DIR)
-
-INCLUDE_DIRECTORIES(${EIGEN_INCLUDE_DIRS})
-
-IF(Ceres_FOUND)
- INCLUDE_DIRECTORIES(${CERES_INCLUDE_DIRS})
-ENDIF(Ceres_FOUND)
-
-IF(faramotics_FOUND)
- INCLUDE_DIRECTORIES(${faramotics_INCLUDE_DIRS})
-ENDIF(faramotics_FOUND)
-
-IF(laser_scan_utils_FOUND)
- INCLUDE_DIRECTORIES(${laser_scan_utils_INCLUDE_DIRS})
-ENDIF(laser_scan_utils_FOUND)
-
-IF(raw_gps_utils_FOUND)
- INCLUDE_DIRECTORIES(${raw_gps_utils_INCLUDE_DIRS})
-ENDIF(raw_gps_utils_FOUND)
-
-IF(vision_utils_FOUND)
- INCLUDE_DIRECTORIES(${vision_utils_INCLUDE_DIR})
- INCLUDE_DIRECTORIES(${OpenCV_INCLUDE_DIRS})
-ENDIF(vision_utils_FOUND)
-
-IF(OPENCV_FOUND)
- INCLUDE_DIRECTORIES(${OpenCV_INCLUDE_DIRS})
-ENDIF(OPENCV_FOUND)
-
-# cereal
-IF(cereal_FOUND)
- INCLUDE_DIRECTORIES(${cereal_INCLUDE_DIRS})
-ENDIF(cereal_FOUND)
-
-IF(Suitesparse_FOUND)
- INCLUDE_DIRECTORIES(${Suitesparse_INCLUDE_DIRS})
-ENDIF(Suitesparse_FOUND)
-
-IF(YAMLCPP_FOUND)
- INCLUDE_DIRECTORIES(${YAMLCPP_INCLUDE_DIR})
-ENDIF(YAMLCPP_FOUND)
-
-IF(GLOG_FOUND)
- INCLUDE_DIRECTORIES(${GLOG_INCLUDE_DIR})
-ENDIF(GLOG_FOUND)
-
-IF(APRILTAG_INCLUDE_DIR)
- INCLUDE_DIRECTORIES(${APRILTAG_INCLUDE_DIR})
-ENDIF(APRILTAG_INCLUDE_DIR)
-
-#headers
-SET(HDRS_BASE
-capture/capture_motion.h
- eigen_assert.h
- eigen_predicates.h
-landmark/landmark_match.h
- make_unique.h
- pinhole_tools.h
-processor/processor_capture_holder.h
-processor/processor_tracker_landmark.h
- )
-SET(HDRS
-capture/capture_motion.h
-capture/capture_GPS_fix.h
-capture/capture_IMU.h
-capture/capture_odom_2D.h
-capture/capture_odom_3D.h
-factor/factor_block_absolute.h
-factor/factor_container.h
-factor/factor_corner_2D.h
-factor/factor_AHP.h
-factor/factor_epipolar.h
-factor/factor_IMU.h
-factor/factor_fix_bias.h
-factor/factor_GPS_2D.h
-factor/factor_GPS_pseudorange_3D.h
-factor/factor_GPS_pseudorange_2D.h
-factor/factor_odom_2D.h
-factor/factor_odom_2D_analytic.h
-factor/factor_odom_3D.h
-factor/factor_point_2D.h
-factor/factor_point_to_line_2D.h
-factor/factor_pose_2D.h
-factor/factor_pose_3D.h
-factor/factor_quaternion_absolute.h
-factor/factor_relative_2D_analytic.h
- temp/diff_drive_tools.h
- temp/diff_drive_tools.hpp
-feature/feature_corner_2D.h
-feature/feature_GPS_fix.h
-feature/feature_GPS_pseudorange.h
-feature/feature_IMU.h
-feature/feature_odom_2D.h
-feature/feature_polyline_2D.h
- IMU_tools.h
-landmark/landmark_corner_2D.h
-landmark/landmark_container.h
-landmark/landmark_line_2D.h
-landmark/landmark_polyline_2D.h
- local_parametrization_polyline_extreme.h
-processor/processor_frame_nearest_neighbor_filter.h
-processor/processor_IMU.h
- test/processor_IMU_UnitTester.h
-processor/processor_odom_2D.h
-processor/processor_odom_3D.h
-processor/processor_tracker_feature_dummy.h
-processor/processor_tracker_landmark_dummy.h
-sensor/sensor_camera.h
-sensor/sensor_GPS.h
-sensor/sensor_GPS_fix.h
-sensor/sensor_IMU.h
-sensor/sensor_odom_2D.h
-sensor/sensor_odom_3D.h
- )
- SET(HDRS_CAPTURE
-capture/capture_GPS_fix.h
-capture/capture_IMU.h
-capture/capture_odom_2D.h
-capture/capture_odom_3D.h
-capture/capture_velocity.h
-capture/capture_wheel_joint_position.h
- )
- SET(HDRS_CONSTRAINT
-factor/factor_autodiff_trifocal.h
-factor/factor_autodiff_distance_3D.h
-factor/factor_AHP.h
-factor/factor_block_absolute.h
-factor/factor_container.h
-factor/factor_corner_2D.h
-factor/factor_diff_drive.h
-factor/factor_epipolar.h
-factor/factor_IMU.h
-factor/factor_fix_bias.h
-factor/factor_GPS_2D.h
-factor/factor_GPS_pseudorange_3D.h
-factor/factor_GPS_pseudorange_2D.h
-factor/factor_odom_2D.h
-factor/factor_odom_2D_analytic.h
-factor/factor_odom_3D.h
-factor/factor_point_2D.h
-factor/factor_point_to_line_2D.h
-factor/factor_pose_2D.h
-factor/factor_pose_3D.h
-factor/factor_quaternion_absolute.h
-factor/factor_relative_2D_analytic.h
- )
- SET(HDRS_FEATURE
-feature/feature_corner_2D.h
-feature/feature_diff_drive.h
-feature/feature_GPS_fix.h
-feature/feature_GPS_pseudorange.h
-feature/feature_IMU.h
-feature/feature_odom_2D.h
-feature/feature_polyline_2D.h
- )
- SET(HDRS_LANDMARK
-landmark/landmark_match.h
-landmark/landmark_corner_2D.h
-landmark/landmark_container.h
-landmark/landmark_line_2D.h
-landmark/landmark_polyline_2D.h
- )
- SET(HDRS_PROCESSOR
-processor/processor_capture_holder.h
-processor/processor_diff_drive.h
-processor/processor_frame_nearest_neighbor_filter.h
-processor/processor_IMU.h
-processor/processor_odom_2D.h
-processor/processor_odom_3D.h
-processor/processor_tracker_feature_dummy.h
-processor/processor_tracker_landmark.h
-processor/processor_tracker_landmark_dummy.h
- )
- SET(HDRS_SENSOR
-sensor/sensor_camera.h
-sensor/sensor_diff_drive.h
-sensor/sensor_GPS.h
-sensor/sensor_GPS_fix.h
-sensor/sensor_IMU.h
-sensor/sensor_odom_2D.h
-sensor/sensor_odom_3D.h
- )
-# [Add generic derived header before this line]
-
-SET(HDRS_DTASSC
- data_association/matrix.h
- data_association/association_solver.h
- data_association/association_node.h
- data_association/association_tree.h
- data_association/association_nnls.h
- )
-
-SET(HDRS_CORE
-core/capture_base.h
-core/capture_buffer.h
-core/capture_pose.h
-core/capture_void.h
-core/factor_analytic.h
-core/factor_autodiff.h
-core/factor_base.h
-core/factory.h
-core/feature_base.h
-core/feature_match.h
-core/feature_pose.h
-core/frame_base.h
-core/hardware_base.h
-core/landmark_base.h
-core/local_parametrization_angle.h
-core/local_parametrization_base.h
-core/local_parametrization_homogeneous.h
-core/local_parametrization_quaternion.h
-core/logging.h
-core/map_base.h
-core/motion_buffer.h
-core/node_base.h
-core/problem.h
-core/processor_base.h
-core/factory.h
-core/processor_loopclosure_base.h
-core/processor_motion.h
-core/processor_tracker_feature.h
-core/processor_tracker.h
-core/rotations.h
-core/sensor_base.h
-core/factory.h
-core/singleton.h
-core/state_angle.h
-core/state_block.h
-core/state_homogeneous_3D.h
-core/state_quaternion.h
-core/three_D_tools.h
-core/time_stamp.h
-core/track_matrix.h
-core/trajectory_base.h
-core/wolf.h
- )
-SET(SRCS_CORE
-core/capture_base.cpp
-core/capture_pose.cpp
-core/capture_void.cpp
-core/factor_analytic.cpp
-core/factor_base.cpp
-core/feature_base.cpp
-core/feature_pose.cpp
-core/frame_base.cpp
-core/hardware_base.cpp
-core/landmark_base.cpp
-core/local_parametrization_base.cpp
-core/local_parametrization_homogeneous.cpp
-core/local_parametrization_quaternion.cpp
-core/map_base.cpp
-core/motion_buffer.cpp
-core/node_base.cpp
-core/problem.cpp
-core/processor_base.cpp
-core/processor_loopclosure_base.cpp
-core/processor_motion.cpp
-core/processor_tracker.cpp
-core/processor_tracker_feature.cpp
-core/sensor_base.cpp
-core/state_block.cpp
-core/time_stamp.cpp
-core/track_matrix.cpp
-core/trajectory_base.cpp
- )
-#sources
-SET(SRCS_BASE
-capture/capture_motion.cpp
-processor/processor_capture_holder.cpp
-processor/processor_tracker_landmark.cpp
- )
-
-SET(SRCS
- local_parametrization_polyline_extreme.cpp
- test/processor_IMU_UnitTester.cpp
- )
- SET(SRCS_CAPTURE
-capture/capture_GPS_fix.cpp
-capture/capture_IMU.cpp
-capture/capture_odom_2D.cpp
-capture/capture_odom_3D.cpp
-capture/capture_velocity.cpp
-capture/capture_wheel_joint_position.cpp
- )
- SET(SRCS_FEATURE
-feature/feature_corner_2D.cpp
-feature/feature_diff_drive.cpp
-feature/feature_GPS_fix.cpp
-feature/feature_GPS_pseudorange.cpp
-feature/feature_IMU.cpp
-feature/feature_odom_2D.cpp
-feature/feature_polyline_2D.cpp
- )
- SET(SRCS_LANDMARK
-landmark/landmark_corner_2D.cpp
-landmark/landmark_container.cpp
-landmark/landmark_line_2D.cpp
-landmark/landmark_polyline_2D.cpp
- )
- SET(SRCS_PROCESSOR
-processor/processor_frame_nearest_neighbor_filter.cpp
-processor/processor_diff_drive.cpp
-processor/processor_IMU.cpp
-processor/processor_odom_2D.cpp
-processor/processor_odom_3D.cpp
-processor/processor_tracker_feature_dummy.cpp
-processor/processor_tracker_landmark_dummy.cpp
- )
- SET(SRCS_SENSOR
-sensor/sensor_camera.cpp
-sensor/sensor_diff_drive.cpp
-sensor/sensor_GPS.cpp
-sensor/sensor_GPS_fix.cpp
-sensor/sensor_IMU.cpp
-sensor/sensor_odom_2D.cpp
-sensor/sensor_odom_3D.cpp
- )
-SET(SRCS_DTASSC
- data_association/association_solver.cpp
- data_association/association_node.cpp
- data_association/association_tree.cpp
- data_association/association_nnls.cpp
- )
-
-# Add the solver sub-directory
-add_subdirectory(solver)
-
-#optional HDRS and SRCS
-IF (Ceres_FOUND)
- SET(HDRS_WRAPPER
- ceres_wrapper/sparse_utils.h
- #ceres_wrapper/solver_manager.h
- ceres_wrapper/ceres_manager.h
- #ceres_wrapper/qr_manager.h
- ceres_wrapper/cost_function_wrapper.h
- ceres_wrapper/create_numeric_diff_cost_function.h
- ceres_wrapper/local_parametrization_wrapper.h
- )
- SET(SRCS_WRAPPER
- #ceres_wrapper/solver_manager.cpp
- ceres_wrapper/ceres_manager.cpp
- #ceres_wrapper/qr_manager.cpp
- ceres_wrapper/local_parametrization_wrapper.cpp
- )
-ELSE(Ceres_FOUND)
- SET(HDRS_WRAPPER)
- SET(SRCS_WRAPPER)
-ENDIF(Ceres_FOUND)
-
-IF (laser_scan_utils_FOUND)
- SET(HDRS ${HDRS}
-capture/capture_laser_2D.h
-sensor/sensor_laser_2D.h
-processor/processor_tracker_feature_corner.h
-processor/processor_tracker_landmark_corner.h
-processor/processor_tracker_landmark_polyline.h
- )
- SET(SRCS ${SRCS}
-capture/capture_laser_2D.cpp
-sensor/sensor_laser_2D.cpp
-processor/processor_tracker_feature_corner.cpp
-processor/processor_tracker_landmark_corner.cpp
-processor/processor_tracker_landmark_polyline.cpp
- )
-ENDIF(laser_scan_utils_FOUND)
-
-IF (raw_gps_utils_FOUND)
- SET(HDRS ${HDRS}
-capture/capture_GPS.h
-processor/processor_GPS.h
- )
- SET(SRCS ${SRCS}
-capture/capture_GPS.cpp
-processor/processor_GPS.cpp
- )
-ENDIF(raw_gps_utils_FOUND)
-
-# Vision
-IF (vision_utils_FOUND)
- SET(HDRS ${HDRS}
-capture/capture_image.h
-feature/feature_point_image.h
-landmark/landmark_AHP.h
-processor/processor_params_image.h
-processor/processor_tracker_feature_image.h
-processor/processor_tracker_landmark_image.h
- )
- SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
-processor/processor_tracker_feature_trifocal.h
- )
- SET(HDRS_LANDMARK ${HDRS_LANDMARK}
-landmark/landmark_point_3D.h
- )
- SET(SRCS ${SRCS}
-capture/capture_image.cpp
-feature/feature_point_image.cpp
-landmark/landmark_AHP.cpp
-processor/processor_tracker_feature_image.cpp
-processor/processor_tracker_landmark_image.cpp
- )
- SET(SRCS_LANDMARK ${SRCS_LANDMARK}
-landmark/landmark_point_3D.cpp
- )
- SET(SRCS_PROCESSOR ${SRCS_PROCESSOR}
-processor/processor_tracker_feature_trifocal.cpp
- )
-ENDIF(vision_utils_FOUND)
-
-IF (OPENCV_FOUND AND Apriltag_FOUND)
- SET(HDRS ${HDRS}
- landmark_apriltag.h
- )
- SET(SRCS ${SRCS}
- landmark_apriltag.cpp
- )
-ENDIF(OPENCV_FOUND AND Apriltag_FOUND)
-
-# Add the capture sub-directory
-# ADD_SUBDIRECTORY(captures)
-
-# Add the factors sub-directory
-# ADD_SUBDIRECTORY(factors)
-
-# Add the features sub-directory
-# ADD_SUBDIRECTORY(features)
-
-# Add the landmarks sub-directory
-# ADD_SUBDIRECTORY(landmarks)
-
-# Add the processors sub-directory
-# ADD_SUBDIRECTORY(processors)
-
-# Add the sensors sub-directory
-# ADD_SUBDIRECTORY(sensors)
-
-# Add the hello_wolf sub-directory
-ADD_SUBDIRECTORY(hello_wolf)
-
-IF (cereal_FOUND)
- ADD_SUBDIRECTORY(serialization/cereal)
-ENDIF(cereal_FOUND)
-
-IF (Suitesparse_FOUND)
- ADD_SUBDIRECTORY(solver_suitesparse)
-ENDIF(Suitesparse_FOUND)
-
-# LEAVE YAML FILES ALWAYS IN THE LAST POSITION !!
-IF(YAMLCPP_FOUND)
- # headers
- SET(HDRS ${HDRS}
- yaml/yaml_conversion.h
- )
-
- # sources
- SET(SRCS ${SRCS}
- yaml/processor_odom_3D_yaml.cpp
- yaml/processor_IMU_yaml.cpp
- yaml/sensor_camera_yaml.cpp
- yaml/sensor_odom_3D_yaml.cpp
- yaml/sensor_IMU_yaml.cpp
- )
- IF(laser_scan_utils_FOUND)
- SET(SRCS ${SRCS}
- yaml/sensor_laser_2D_yaml.cpp
- )
- ENDIF(laser_scan_utils_FOUND)
- IF(vision_utils_FOUND)
- SET(SRCS ${SRCS}
- yaml/processor_image_yaml.cpp
- yaml/processor_tracker_feature_trifocal_yaml.cpp
- )
- IF(Apriltag_FOUND)
- SET(SRCS ${SRCS}
- yaml/processor_tracker_landmark_apriltag_yaml.cpp
- )
- ENDIF(Apriltag_FOUND)
- ENDIF(vision_utils_FOUND)
-ENDIF(YAMLCPP_FOUND)
-
-# create the shared library
-add_library(${PROJECT_NAME}_core SHARED
- ${SRCS_CORE}
- ${SRCS_BASE}
- )
-ADD_LIBRARY(${PROJECT_NAME}
- SHARED
- ${SRCS_BASE}
- ${SRCS}
- ${SRCS_CAPTURE}
- ${SRCS_CONSTRAINT}
- ${SRCS_FEATURE}
- ${SRCS_LANDMARK}
- ${SRCS_PROCESSOR}
- ${SRCS_SENSOR}
- #${SRCS_DTASSC}
- ${SRCS_SOLVER}
- ${SRCS_WRAPPER}
- )
-TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${CMAKE_THREAD_LIBS_INIT})
-
-TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${PROJECT_NAME}_core)
-#Link the created libraries
-#=============================================================
-IF (Ceres_FOUND)
- TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${CERES_LIBRARIES})
-ENDIF(Ceres_FOUND)
-
-IF (laser_scan_utils_FOUND)
- TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${laser_scan_utils_LIBRARY})
-ENDIF (laser_scan_utils_FOUND)
-
-IF (raw_gps_utils_FOUND)
- TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${raw_gps_utils_LIBRARY})
-ENDIF (raw_gps_utils_FOUND)
-
-IF (OPENCV_FOUND)
- TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${OpenCV_LIBS})
- IF (vision_utils_FOUND)
- TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${vision_utils_LIBRARY})
- ENDIF (vision_utils_FOUND)
-ENDIF (OPENCV_FOUND)
-
-IF (OPENCV_FOUND)
- TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${OpenCV_LIBS})
-ENDIF (OPENCV_FOUND)
-
-IF (YAMLCPP_FOUND)
- TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${YAMLCPP_LIBRARY})
-ENDIF (YAMLCPP_FOUND)
-
-IF (GLOG_FOUND)
- TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${GLOG_LIBRARY})
-ENDIF (GLOG_FOUND)
-
-#check if this is done correctly
-IF (OPENCV_FOUND AND Apriltag_FOUND)
- LINK_LIBRARIES(apriltag m)
- TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${APRILTAG_LIBRARY} ${CMAKE_THREAD_LIBS_INIT} ${OPENCV_LDFLAGS} m)
-ENDIF(OPENCV_FOUND AND Apriltag_FOUND)
-
-#install library
-install(TARGETS ${PROJECT_NAME}_core DESTINATION lib/iri-algorithms EXPORT ${PROJECT_NAME}_core-targets)
-install(EXPORT ${PROJECT_NAME}_core-targets DESTINATION lib/iri-algorithms)
-#=============================================================
-INSTALL(TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME}Targets
- RUNTIME DESTINATION bin
- LIBRARY DESTINATION lib/iri-algorithms
- ARCHIVE DESTINATION lib/iri-algorithms)
-
-install(EXPORT ${PROJECT_NAME}Targets DESTINATION lib/cmake/${PROJECT_NAME})
-
-#install headers
-INSTALL(FILES ${HDRS_BASE}
- DESTINATION include/iri-algorithms/wolf)
-INSTALL(FILES ${HDRS}
- DESTINATION include/iri-algorithms/wolf)
-INSTALL(FILES ${HDRS_CORE}
- DESTINATION include/iri-algorithms/wolf/core)
-#INSTALL(FILES ${HDRS_DTASSC}
-# DESTINATION include/iri-algorithms/wolf/data_association)
-INSTALL(FILES ${HDRS_CAPTURE}
- DESTINATION include/iri-algorithms/wolf/capture)
-INSTALL(FILES ${HDRS_CONSTRAINT}
- DESTINATION include/iri-algorithms/wolf/factor)
-INSTALL(FILES ${HDRS_FEATURE}
- DESTINATION include/iri-algorithms/wolf/feature)
-INSTALL(FILES ${HDRS_SENSOR}
- DESTINATION include/iri-algorithms/wolf/sensor)
-INSTALL(FILES ${HDRS_PROCESSOR}
- DESTINATION include/iri-algorithms/wolf/processor)
-INSTALL(FILES ${HDRS_LANDMARK}
- DESTINATION include/iri-algorithms/wolf/landmark)
-INSTALL(FILES ${HDRS_WRAPPER}
- DESTINATION include/iri-algorithms/wolf/ceres_wrapper)
-#INSTALL(FILES ${HDRS_SOLVER_SUITESPARSE}
-# DESTINATION include/iri-algorithms/wolf/solver_suitesparse)
-INSTALL(FILES ${HDRS_SOLVER}
- DESTINATION include/iri-algorithms/wolf/solver)
-INSTALL(FILES ${HDRS_SERIALIZATION}
- DESTINATION include/iri-algorithms/wolf/serialization)
-INSTALL(FILES "${CMAKE_SOURCE_DIR}/cmake_modules/Findwolf.cmake"
- DESTINATION "lib/cmake/${PROJECT_NAME}")
-
-#install Find*.cmake
-configure_file("${CMAKE_SOURCE_DIR}/cmake_modules/wolfConfig.cmake"
- "${CMAKE_BINARY_DIR}/wolfConfig.cmake" @ONLY)
-
-INSTALL(FILES "${WOLF_CONFIG_DIR}/config.h"
-DESTINATION include/iri-algorithms/wolf/internal)
-
-INSTALL(FILES "${CMAKE_BINARY_DIR}/wolfConfig.cmake" DESTINATION "lib/cmake/${PROJECT_NAME}")
-
-INSTALL(DIRECTORY ${SPDLOG_INCLUDE_DIRS} DESTINATION "include/iri-algorithms/")
-
-export(PACKAGE ${PROJECT_NAME})
-
-#############
-## Testing ##
-#############
-IF (GLOG_FOUND)
- IF(BUILD_TESTS)
- MESSAGE("Building tests.")
- add_subdirectory(test)
- ENDIF(BUILD_TESTS)
-ENDIF (GLOG_FOUND)
-
-IF(BUILD_EXAMPLES)
- #Build examples
- MESSAGE("Building examples.")
- ADD_SUBDIRECTORY(examples)
-ENDIF(BUILD_EXAMPLES)
-
diff --git a/src/landmark/landmark_AHP.cpp b/src/landmark/landmark_AHP.cpp
index 28efec608025ab3d18452359697d202b008e17ff..d8d49a66de602d65a4a96c3b61edbcced1bad169 100644
--- a/src/landmark/landmark_AHP.cpp
+++ b/src/landmark/landmark_AHP.cpp
@@ -44,10 +44,10 @@ Eigen::Vector3s LandmarkAHP::getPointInAnchorSensor() const
Eigen::Vector3s LandmarkAHP::point() const
{
using namespace Eigen;
- Transform<Scalar,3,Affine> T_w_r
+ Transform<Scalar,3,Isometry> T_w_r
= Translation<Scalar,3>(getAnchorFrame()->getP()->getState())
* Quaternions(getAnchorFrame()->getO()->getState().data());
- Transform<Scalar,3,Affine> T_r_c
+ Transform<Scalar,3,Isometry> T_r_c
= Translation<Scalar,3>(getAnchorSensor()->getP()->getState())
* Quaternions(getAnchorSensor()->getO()->getState().data());
Vector4s point_hmg_c = getP()->getState();
diff --git a/src/landmark/landmark_HP.cpp b/src/landmark/landmark_HP.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..33e52b303a7b216b05068ba73aae4392b62e4fc2
--- /dev/null
+++ b/src/landmark/landmark_HP.cpp
@@ -0,0 +1,74 @@
+#include "vision/landmark/landmark_HP.h"
+
+#include "core/state_block/state_homogeneous_3D.h"
+#include "core/common/factory.h"
+#include "core/yaml/yaml_conversion.h"
+
+namespace wolf {
+
+/* Landmark - Homogeneous Point*/
+LandmarkHP::LandmarkHP(Eigen::Vector4s _position_homogeneous,
+ SensorBasePtr _sensor,
+ cv::Mat _2D_descriptor) :
+ LandmarkBase("HP", std::make_shared<StateHomogeneous3D>(_position_homogeneous)),
+ sensor_(_sensor),
+ cv_descriptor_(_2D_descriptor.clone())
+{
+}
+
+LandmarkHP::~LandmarkHP()
+{
+ //
+}
+
+YAML::Node LandmarkHP::saveToYaml() const
+{
+ // First base things
+ YAML::Node node = LandmarkBase::saveToYaml();
+
+ // Then add specific things
+ std::vector<int> v;
+ LandmarkHP::cv_descriptor_.copyTo(v);
+ node["descriptor"] = v;
+ return node;
+}
+
+Eigen::Vector3s LandmarkHP::point() const
+{
+ using namespace Eigen;
+
+ /* TODO: done when creating the landmark
+ Transform<Scalar,3,Isometry> T_w_r
+ = Translation<Scalar,3>(getAnchorFrame()->getP()->getState())
+ * Quaternions(getAnchorFrame()->getO()->getState().data());
+ Transform<Scalar,3,Isometry> T_r_c
+ = Translation<Scalar,3>(getAnchorSensor()->getP()->getState())
+ * Quaternions(getAnchorSensor()->getO()->getState().data());
+ */
+ //Vector4s point_hmg_c = getP()->getState();
+ //Vector4s point_hmg = T_w_r * T_r_c * point_hmg_c;
+
+ Vector4s point_hmg = getP()->getState();
+
+ return point_hmg.head<3>()/point_hmg(3);
+}
+
+LandmarkBasePtr LandmarkHP::create(const YAML::Node& _node)
+{
+ unsigned int id = _node["id"] .as< unsigned int >();
+ Eigen::VectorXs pos_homog = _node["position"] .as< Eigen::VectorXs >();
+ std::vector<int> v = _node["descriptor"] .as< std::vector<int> >();
+ cv::Mat desc(v);
+
+ LandmarkBasePtr lmk = std::make_shared<LandmarkHP>(pos_homog, nullptr, desc);
+ lmk->setId(id);
+ return lmk;
+}
+
+// Register landmark creator
+namespace
+{
+const bool WOLF_UNUSED registered_lmk_ahp = LandmarkFactory::get().registerCreator("HP", LandmarkHP::create);
+}
+
+} // namespace wolf
diff --git a/src/processor/processor_bundle_adjustment.cpp b/src/processor/processor_bundle_adjustment.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..38a0f1a41639147534e9cef3d1fe7c8e9b3388cc
--- /dev/null
+++ b/src/processor/processor_bundle_adjustment.cpp
@@ -0,0 +1,801 @@
+/*
+ * processor_bundle_adjustment.cpp
+ *
+ * Created on: May 3, 2019
+ * Author: ovendrell
+ */
+
+//wolf
+#include "vision_utils/vision_utils.h"
+#include "vision_utils/detectors.h"
+#include "vision_utils/descriptors.h"
+#include "vision_utils/matchers.h"
+
+//standard
+#include <memory>
+#include "vision/processor/processor_bundle_adjustment.h"
+
+namespace wolf{
+
+ProcessorBundleAdjustment::ProcessorBundleAdjustment(ProcessorParamsBundleAdjustmentPtr _params_bundle_adjustment) :
+ ProcessorTrackerFeature("TRACKER BUNDLE ADJUSTMENT", _params_bundle_adjustment),
+ params_bundle_adjustment_(_params_bundle_adjustment),
+ capture_image_last_(nullptr),
+ capture_image_incoming_(nullptr),
+ frame_count_(0)
+{
+ //Initialize detector-descriptor-matcher
+ pixel_cov_ = Eigen::Matrix2s::Identity() * params_bundle_adjustment_->pixel_noise_std * params_bundle_adjustment_->pixel_noise_std;
+
+ // Detector yaml file
+ std::string det_name = vision_utils::readYamlType(params_bundle_adjustment_->yaml_file_params_vision_utils, "detector");
+ // Create Detector
+ det_ptr_ = vision_utils::setupDetector(det_name, det_name + " detector", params_bundle_adjustment_->yaml_file_params_vision_utils);
+
+ // Descriptor yaml file
+ std::string des_name = vision_utils::readYamlType(params_bundle_adjustment_->yaml_file_params_vision_utils, "descriptor");
+ // Create Descriptor
+ des_ptr_ = vision_utils::setupDescriptor(des_name, des_name + " descriptor", params_bundle_adjustment_->yaml_file_params_vision_utils);
+
+ // Matcher yaml file
+ std::string mat_name = vision_utils::readYamlType(params_bundle_adjustment_->yaml_file_params_vision_utils, "matcher");
+ // Create Matcher
+ mat_ptr_ = vision_utils::setupMatcher(mat_name, mat_name + " matcher", params_bundle_adjustment_->yaml_file_params_vision_utils);
+
+ //Initialize rvec and tvec
+ tvec_ = cv::Mat::zeros(cv::Size(3,1), CV_32F);
+ rvec_ = cv::Mat::zeros(cv::Size(3,1), CV_32F);
+// Eigen::Vector3s last_pos = capture_image_last_->getFrame()->getP()->getState();
+// Eigen::Vector4s last_or = capture_image_last_->getFrame()->getO()->getState();
+// Eigen::Quaternion<Scalar> last_q(last_or(0), last_or(1), last_or(2), last_or(3));
+//
+// Eigen::VectorXs tvec_eigen = last_pos;
+// Eigen::VectorXs rvec_eigen = q2v(last_q);
+//
+// cv::eigen2cv(tvec_eigen, tvec);
+// cv::eigen2cv(rvec_eigen, rvec);
+
+
+}
+
+void ProcessorBundleAdjustment::configure(SensorBasePtr _sensor)
+{
+ //TODO: Implement if needed
+ //Initialize camera sensor pointer
+ camera = std::static_pointer_cast<SensorCamera>(_sensor);
+
+}
+
+void ProcessorBundleAdjustment::preProcess()
+{
+ // This method implements all Vision algorithms concerning OpenCV, so wolf algorithm only has to manage the data obtained
+ // Get Capture
+ capture_image_incoming_ = std::static_pointer_cast<CaptureImage>(incoming_ptr_);
+ // Detect KeyPoints
+ capture_image_incoming_->keypoints_ = det_ptr_->detect(capture_image_incoming_->getImage());
+
+ //Sort keypoints by response if needed
+ if (!capture_image_incoming_->keypoints_.empty())
+ vision_utils::sortByResponse(capture_image_incoming_->keypoints_, CV_SORT_DESCENDING);
+
+ // Compute Descriptors
+ capture_image_incoming_->descriptors_ = des_ptr_->getDescriptor(capture_image_incoming_->getImage(), capture_image_incoming_->keypoints_);
+
+ // Create and fill incoming grid
+ capture_image_incoming_->grid_features_ = std::make_shared<vision_utils::FeatureIdxGrid>(capture_image_incoming_->getImage().rows, capture_image_incoming_->getImage().cols, params_bundle_adjustment_->n_cells_v, params_bundle_adjustment_->n_cells_h);
+
+ capture_image_incoming_->grid_features_->insert(capture_image_incoming_->keypoints_);
+
+ if (last_ptr_ != nullptr)
+ {
+ // Get Capture
+ capture_image_last_ = std::static_pointer_cast<CaptureImage>(last_ptr_);
+ // Match image
+ capture_image_incoming_->matches_normalized_scores_ = mat_ptr_->robustMatch(capture_image_incoming_->keypoints_, capture_image_last_->keypoints_,
+ capture_image_incoming_->descriptors_, capture_image_last_->descriptors_,
+ des_ptr_->getSize(), capture_image_incoming_->matches_from_precedent_);
+
+
+ //TODO: Get only the best ones
+ if (params_bundle_adjustment_->delete_ambiguities) //filter ambiguities
+ {
+ std::map<int,bool> ambiguities_map;
+ for (auto match : capture_image_incoming_->matches_from_precedent_)
+ if (ambiguities_map.count(match.trainIdx)) //if ambiguity
+ ambiguities_map[match.trainIdx] = true; //ambiguity true
+ else //if not ambiguity
+ ambiguities_map[match.trainIdx] = false; //ambiguity false
+ // Set capture map of match indices
+ for (auto match : capture_image_incoming_->matches_from_precedent_)
+ {
+ if (!ambiguities_map.at(match.trainIdx))
+ capture_image_last_->map_index_to_next_[match.trainIdx] = match.queryIdx; // map[last] = incoming
+ }
+ }
+ else
+ {
+ // Set capture map of match indices
+ for (auto match : capture_image_incoming_->matches_from_precedent_)
+ capture_image_last_->map_index_to_next_[match.trainIdx] = match.queryIdx; // map[last] = incoming
+
+ }
+
+
+// //get points 2D from inlier matches
+// PointVector pts1, pts2;
+// for (auto match : capture_image_incoming_->matches_from_precedent_)
+// {
+// pts1.push_back(capture_image_incoming_->keypoints_[match.queryIdx].pt);
+// pts2.push_back(capture_image_last_->keypoints_[match.trainIdx].pt);
+// }
+//
+// //find Essential Matrix
+// std::vector<uchar> inliers(pts1.size(),0);
+//
+// auto camera_mat = std::static_pointer_cast<SensorCamera>(capture_image_last_->getSensor())->getIntrinsicMatrix();
+// cv::Mat camera_mat_cv;
+// cv::eigen2cv(camera_mat, camera_mat_cv);
+//
+// cv::Mat E = cv::findEssentialMat(pts1,
+// pts2,
+// camera_mat_cv,
+// CV_RANSAC,
+// 0.999,
+// 1.0,
+// inliers);
+//
+// //Compute rotation R and translation t from E
+// cv::Mat R_cv;
+// cv::Mat t_cv;
+//
+// cv::recoverPose(E,pts1,pts2, camera_mat_cv, R_cv, t_cv, inliers);
+//
+// //Convert R and t from OpenCV type to Eigen type and convert Rotation to a quaternion
+// Eigen::Matrix3s R;
+// cv::cv2eigen(R_cv, R);
+//// auto q = R2q(R); //Quaternion q
+//
+// Eigen::Vector3s t; //Translation t
+// cv::cv2eigen(t_cv,t);
+
+
+
+
+
+
+
+// cv::Mat img_mat = cv::Mat(capture_image_incoming_->getImage().rows, capture_image_incoming_->getImage().cols, capture_image_incoming_->getImage().type());
+//
+// cv::drawMatches(capture_image_incoming_->getImage(), capture_image_incoming_->getKeypoints(),
+// capture_image_last_->getImage(), capture_image_last_->getKeypoints(),
+// capture_image_incoming_->matches_from_precedent_, img_mat);
+//
+// cv::namedWindow("MATCHES VIEW", cv::WINDOW_NORMAL);
+// cv::resizeWindow("MATCHES VIEW", 800,600);
+// cv::imshow("MATCHES VIEW", img_mat);
+ }
+
+// std::cout << "preProcess() done " << std::endl;
+
+}
+
+void ProcessorBundleAdjustment::postProcess()
+{
+
+ WOLF_INFO("last ", last_ptr_, "camera ", camera);
+
+ //=====================================Compute prior for the frame===============================================
+// if (capture_image_last_)
+// {
+//
+// std::vector<cv::Point3f> landmark_points;
+// std::vector<cv::Point2f> image_points;
+//
+// //get camera information
+// auto camera_mat = camera->getIntrinsicMatrix();
+//
+// cv::Mat camera_mat_cv;
+// cv::eigen2cv(camera_mat, camera_mat_cv);
+//
+// auto dist_coeffs = camera->getDistortionVector();
+//
+// Eigen::Vector5s dist;
+// switch(dist_coeffs.size())
+// {
+// case 0:
+// {
+// dist << 0,0,0,0,0;
+// break;
+// }
+// case 1:
+// {
+// dist << dist_coeffs(0),0,0,0,0;
+// break;
+// }
+// case 2:
+// {
+// dist << dist_coeffs(0),dist_coeffs(1),0,0,0;
+// break;
+// }
+// case 3:
+// {
+// dist << dist_coeffs(0),dist_coeffs(1),0,0,dist_coeffs(2);
+// break;
+// }
+// default:
+// dist << 0,0,0,0,0;
+// }
+//
+// cv::Mat dist_coeffs_cv;
+// cv::eigen2cv(dist, dist_coeffs_cv);
+//
+//
+//
+// //fill points and landmarks list
+// int debug_counter = 0;
+//
+//// for (auto & feat : capture_image_last_->getFeatureList())
+//// {
+//// for (auto & fact : feat->getFactorList())
+//// {
+//// debug_counter++;
+//// //check the number of factors to the landmark
+//// if (fact->getLandmarkOther()->getConstrainedByList().size() >= 2)
+//// {
+//// //3d points
+//// auto point3D = std::static_pointer_cast<LandmarkHP>(fact->getLandmarkOther())->point();
+//// landmark_points.push_back(cv::Point3f(point3D(0),point3D(1),point3D(2)));
+//// //2d points
+//// auto point2D = feat->getMeasurement();
+//// image_points.push_back(cv::Point2f(point2D(0), point2D(1)));
+//// }
+//// }
+//// }
+//
+// for (auto & feat : track_matrix_.snapshotAsList(capture_image_last_))
+// {
+// debug_counter++;
+// auto trkId = feat->trackId();
+// if (lmk_track_map_.count(feat->trackId()))
+// {
+// auto lmk_base = lmk_track_map_[trkId];
+//// if (lmk_base->getConstrainedByList().size() >= 2)
+// {
+// //3d points
+// auto point3D = std::static_pointer_cast<LandmarkHP>(lmk_base)->point();
+// landmark_points.push_back(cv::Point3f(point3D(0),point3D(1),point3D(2)));
+// //2d points
+// auto point2D = feat->getMeasurement();
+// image_points.push_back(cv::Point2f(point2D(0), point2D(1)));
+// }
+// }
+// }
+//
+// WOLF_INFO("Num lmks in last:", debug_counter,"lmks constrained by >= 2 fctrs: ", landmark_points.size());
+//
+// //solvePnP
+// if (landmark_points.size() > 7)
+// {
+// WOLF_INFO("before PnP ", tvec, rvec);
+// WOLF_INFO("before PnP ", last_ptr_->getFrame()->getState().transpose());
+//
+//
+// cv::solvePnP(landmark_points, image_points, camera_mat_cv, dist_coeffs_cv, rvec, tvec, true);
+// WOLF_INFO("Solve PnP Done")
+//
+// //Compute and set Robot state
+// //q_w_s Camera quaternion
+// Eigen::Vector3s rvec_eigen;
+// cv::cv2eigen(rvec, rvec_eigen);
+// Eigen::Quaternion<Scalar> q_w_s = v2q(rvec_eigen);
+//
+// //p_w_s Camera position
+// Eigen::Vector3s p_w_s;
+// cv::cv2eigen(tvec, p_w_s);
+//
+// //Robot pose
+// // Eigen::Vector3s p_w_r = capture_image_last_->getFrame()->getP()->getState();
+// // Eigen::Quaternion<Scalar> q_w_r = Quaternions(capture_image_last_->getFrame()->getO()->getState().data());
+//
+// //Extrinsics (camera in robot reference frame)
+// Eigen::Vector3s p_r_s = camera->getP()->getState();
+// Eigen::Quaternion<Scalar> q_r_s = Quaternions(camera->getO()->getState().data());
+//
+// //Robot pose compositions
+// Eigen::Quaternion<Scalar> q_w_r = q_w_s * q_r_s.conjugate();
+// Eigen::Vector3s p_w_r = p_w_s - q_w_r * p_r_s;
+//
+//
+//
+// Eigen::Vector7s prior_state;
+// prior_state << p_w_r(0), p_w_r(1), p_w_r(2), q_w_r.x(), q_w_r.y(), q_w_r.z(), q_w_r.w();
+//
+// last_ptr_->getFrame()->setState(prior_state);
+//
+// WOLF_INFO("after PnP ", last_ptr_->getFrame()->getState().transpose());
+// WOLF_INFO("after PnP ", tvec, rvec);
+//
+// }
+//
+// //=================================================================================================================
+// }
+
+ //delete landmarks
+ getProblem()->print(4,1,0,0);
+ std::list<LandmarkBasePtr> lmks_to_remove;
+ for (auto & lmk : getProblem()->getMap()->getLandmarkList())
+ {
+ if (lmk->getConstrainedByList().size()==1)
+ {
+ if (lmk->getConstrainedByList().front()->getFeature()->getCapture() != origin_ptr_)
+ {
+ WOLF_INFO("Removing landmark: ", lmk->id());
+ lmk_track_map_.erase(lmk->getConstrainedByList().front()->getFeature()->trackId());
+ WOLF_INFO("Lmk deleted from track map: ", lmk->id());
+ lmks_to_remove.push_back(lmk);
+ }
+ }
+ }
+ for (auto & lmk : lmks_to_remove)
+ {
+ lmk->remove();
+ WOLF_INFO("Lmk deleted: ", lmk->id());
+ }
+ getProblem()->check(0);
+
+
+ // draw debug image ======================================================================================
+
+ // image to draw
+ CaptureBasePtr image_to_draw = last_ptr_;
+
+
+ std::map<int,std::list<vision_utils::KeyPointEnhanced> > kp_enh_tracks;
+ for (auto const & feat_base : image_to_draw->getFeatureList())
+ {
+ FeaturePointImagePtr feat = std::static_pointer_cast<FeaturePointImage>(feat_base);
+ unsigned int feat_id = feat->id();
+ unsigned int track_id = feat->trackId();
+ // tracks
+ std::list<vision_utils::KeyPointEnhanced> kp_enh_track;
+ for (auto feat_base_track : track_matrix_.trackAsVector(track_id))
+ {
+ FeaturePointImagePtr feat_img = std::static_pointer_cast<FeaturePointImage>(feat_base_track);
+ vision_utils::KeyPointEnhanced kp_enh(feat_img->getKeypoint(),
+ feat_id,
+ track_id,
+ track_matrix_.trackSize(track_id),
+ feat_img->getMeasurementCovariance());
+ kp_enh_track.push_back(kp_enh);
+ }
+
+ kp_enh_tracks[feat_id] = kp_enh_track;
+ }
+ // DEBUG
+ image_debug_ = vision_utils::buildImageProcessed((std::static_pointer_cast<CaptureImage>(image_to_draw))->getImage(), kp_enh_tracks);
+// Snapshot snapshot_last = track_matrix_.snapshot(last_ptr_);
+//// getProblem()->print(4,1,1,0);
+// for (auto pair : track_matrix_.snapshot(origin_ptr_))
+// {
+// if (snapshot_last.count(pair.first)==0)
+// {
+// if (!(pair.second->getFactorList()).empty())
+// {
+// auto factor = pair.second->getFactorList().front();
+// if (factor)
+// {
+// auto lmk = factor->getLandmarkOther();
+// if (lmk)
+// {
+//// lmk->remove();
+//// track_matrix_.remove(pair.second->trackId());
+// }
+// }
+// }
+// }
+// }
+// getProblem()->print(0,0,1,0);
+
+ list<FeatureBasePtr> snapshot = track_matrix_.snapshotAsList(image_to_draw);
+
+ for (auto const & feat_base : snapshot)
+ {
+ FeaturePointImagePtr feat = std::static_pointer_cast<FeaturePointImage>(feat_base);
+ unsigned int track_id = feat->trackId();
+ if (lmk_track_map_.count(track_id))
+ {
+ Vector3s point3D = std::static_pointer_cast<LandmarkHP>(lmk_track_map_[track_id])->point();
+
+ Transform<Scalar,3,Isometry> T_w_r
+ = Translation<Scalar,3>(feat_base->getFrame()->getP()->getState())
+ * Quaternions(feat_base->getFrame()->getO()->getState().data());
+ Transform<Scalar,3,Isometry> T_r_c
+ = Translation<Scalar,3>(feat_base->getCapture()->getSensorP()->getState())
+ * Quaternions(feat_base->getCapture()->getSensorO()->getState().data());
+
+ Eigen::Matrix<Scalar, 3, 1> point3D_c = T_r_c.inverse()
+ * T_w_r.inverse()
+ * point3D;
+
+// SensorCameraPtr camera = std::static_pointer_cast<SensorCamera>(getSensor());
+
+ Vector2s point2D = pinhole::projectPoint(getSensor()->getIntrinsic()->getState(), camera->getDistortionVector(), point3D_c);
+ cv::Point point = cv::Point(point2D(0), point2D(1));
+
+ cv::circle(image_debug_, point, 3, cv::Scalar(0,0,255) , 1 , 8);
+ std::string id = std::to_string(lmk_track_map_[track_id]->id());
+ cv::putText(image_debug_, id, point, cv::FONT_HERSHEY_DUPLEX, 0.5, CV_RGB(0,255,0),2);
+ }
+ }
+
+// cv::namedWindow("DEBUG VIEW", cv::WINDOW_NORMAL);
+// cv::resizeWindow("DEBUG VIEW", 800,600);
+// cv::imshow("DEBUG VIEW", image_debug_);
+// cv::waitKey(1);
+}
+
+
+unsigned int ProcessorBundleAdjustment::trackFeatures(const FeatureBasePtrList& _features_in,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out,
+ FeatureMatchMap& _feature_correspondences)
+{
+ for (auto feat_base: _features_in)
+ {
+ FeaturePointImagePtr feat_last = std::static_pointer_cast<FeaturePointImage>(feat_base);
+
+ if (capture_image_last_->map_index_to_next_.count(feat_last->getIndexKeyPoint())) //If the track to incoming exists
+ {
+ int index_inc = capture_image_last_->map_index_to_next_.at(feat_last->getIndexKeyPoint());
+
+ if (true)//capture_image_incoming_->matches_normalized_scores_.at(index_inc) > mat_ptr_->getParams()->min_norm_score )
+ {
+ // Get kp incoming and keypoint last
+ cv::KeyPoint kp_inc = capture_image_incoming_->keypoints_.at(index_inc);
+ cv::KeyPoint kp_last = capture_image_last_->keypoints_.at(feat_last->getIndexKeyPoint());
+
+ if (isInlier(kp_last, kp_inc))
+ {
+ FeatureBasePtr feat_inc = FeatureBase::emplace<FeaturePointImage>(_capture,
+ kp_inc,
+ index_inc,
+ capture_image_incoming_->descriptors_.row(index_inc),
+ pixel_cov_);
+
+ _features_out.push_back(feat_inc);
+
+ auto feature_match_ptr = std::make_shared<FeatureMatch>();
+ feature_match_ptr->feature_ptr_= feat_last;
+ feature_match_ptr->normalized_score_ = 1.0;//capture_image_incoming_->matches_normalized_scores_.at(index_inc);
+ _feature_correspondences[feat_inc] = feature_match_ptr;
+
+
+ // hit cell to acknowledge there's a tracked point in that cell
+ capture_image_incoming_->grid_features_->hitTrackingCell(kp_inc);
+ }
+ }
+ }
+ }
+// return _feature_correspondences.size();
+ return _features_out.size();
+}
+
+bool ProcessorBundleAdjustment::isInlier(const cv::KeyPoint& _kp_last, const cv::KeyPoint& _kp_incoming)
+{
+ // List of conditions
+ bool inlier = true;
+
+ // A. Check euclidean norm
+ inlier = inlier && (cv::norm(_kp_incoming.pt-_kp_last.pt) < mat_ptr_->getParams()->max_match_euclidean_dist);
+
+ // B. add your new condition test here
+ // inlier = inlier && ...
+
+ return inlier;
+}
+
+bool ProcessorBundleAdjustment::is_tracked(int& _kp_idx)
+{
+
+ for (auto ftr : known_features_incoming_)
+ {
+ FeaturePointImagePtr ftr_kp = std::static_pointer_cast<FeaturePointImage>(ftr);
+ if (ftr_kp->getIndexKeyPoint() == _kp_idx)
+ {
+ return true;
+ }
+ }
+ return false;
+}
+
+unsigned int ProcessorBundleAdjustment::detectNewFeatures(const int& _max_new_features,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out)
+{
+ //TODO: efficient implementation?
+
+////Simpler method to detect new features without using Grid Features form vision_utils
+// typedef std::map<int,int>::iterator iter;
+//
+// for (iter it = capture_image_last_->map_index_to_next_.begin(); it!=capture_image_last_->map_index_to_next_.end(); ++it)
+// {
+// if (_features_out.size() >= _max_new_features)
+// break;
+//
+// else if(!is_tracked(it->second))
+// {
+// //add to _features_out
+// int idx_last = it->first;
+// cv::KeyPoint kp_last = capture_image_last_->keypoints_.at(idx_last);
+// FeaturePointImagePtr feat_last_ = std::make_shared<FeaturePointImage>(kp_last, idx_last,
+// capture_image_last_->descriptors_.row(idx_last),
+// pixel_cov_);
+// _features_out.push_back(feat_last_);
+//
+// }
+// }
+//
+// return _features_out.size();
+
+ for (unsigned int n_iterations = 0; _max_new_features == -1 || n_iterations < _max_new_features; ++n_iterations)
+ {
+ Eigen::Vector2i cell_last;
+ assert(capture_image_last_->grid_features_!=nullptr);
+ if (capture_image_last_->grid_features_->pickEmptyTrackingCell(cell_last))
+ {
+ // Get best keypoint in cell
+ vision_utils::FeatureIdxMap cell_feat_map = capture_image_last_->grid_features_->getFeatureIdxMap(cell_last(0), cell_last(1), params_bundle_adjustment_->min_response_new_feature);
+ bool found_feature_in_cell = false;
+
+ for (auto target_last_pair_response_idx : cell_feat_map)
+ {
+ // Get KeyPoint in last
+ unsigned int index_last = target_last_pair_response_idx.second;
+ cv::KeyPoint kp_last = capture_image_last_->keypoints_.at(index_last);
+ assert(target_last_pair_response_idx.first == kp_last.response && "[ProcessorTrackerFeatureTrifocal::detectNewFeatures]: response mismatch.");
+
+ // Check if there is match with incoming, if not we do not want it
+ if (capture_image_last_->map_index_to_next_.count(index_last))
+ {
+ // matching keypoint in incoming
+ unsigned int index_incoming = capture_image_last_->map_index_to_next_[index_last];
+ cv::KeyPoint kp_incoming = capture_image_incoming_->keypoints_.at(index_incoming);
+
+ // validate match with extra tests
+ if (isInlier( kp_incoming, kp_last))
+ {
+ // Create WOLF feature
+ FeatureBasePtr ftr_point_last = FeatureBase::emplace<FeaturePointImage>(_capture,
+ kp_last,
+ index_last,
+ capture_image_last_->descriptors_.row(index_last),
+ pixel_cov_);
+
+ _features_out.push_back(ftr_point_last);
+
+ // hit cell to acknowledge there's a tracked point in that cell
+ capture_image_last_->grid_features_->hitTrackingCell(kp_last);
+
+ found_feature_in_cell = true;
+
+ break; // Good kp found for this grid. //Use to have only one keypoint per grid
+ }
+ }
+ }
+ if (!found_feature_in_cell)
+ capture_image_last_->grid_features_->blockTrackingCell(cell_last);
+ }
+ else
+ break; // There are no empty cells
+ }
+ return _features_out.size();
+}
+
+
+bool ProcessorBundleAdjustment::correctFeatureDrift(const FeatureBasePtr _origin_feature, const FeatureBasePtr _last_feature, FeatureBasePtr _incoming_feature)
+{
+ //TODO: Implement if needed
+ return true;
+}
+
+bool ProcessorBundleAdjustment::voteForKeyFrame()
+{
+
+ frame_count_ ++;
+
+
+ return ((frame_count_ % 5) == 0);
+
+
+
+
+
+
+ // // A. crossing voting threshold with ascending number of features
+ bool vote_up = false;
+ // // 1. vote if we did not have enough features before
+ // vote_up = vote_up && (previousNumberOfTracks() < params_bundle_adjustment_->min_features_for_keyframe);
+ // // 2. vote if we have enough features now
+ // vote_up = vote_up && (incoming_ptr_->getFeatureList().size() >= params_bundle_adjustment_->min_features_for_keyframe);
+
+ // B. crossing voting threshold with descending number of features
+ bool vote_down = true;
+ // 1. vote if we had enough features before
+ // vote_down = vote_down && (last_ptr_->getFeatureList().size() >= params_bundle_adjustment_->min_features_for_keyframe);
+ // 2. vote if we have not enough features now
+ vote_down = vote_down && (incoming_ptr_->getFeatureList().size() < params_bundle_adjustment_->min_features_for_keyframe);
+
+ // // C. Time-based policies
+ bool vote_time = false;
+ //// vote_time = vote_time || (incoming_ptr_->getTimeStamp()-origin_ptr_->getTimeStamp() > 1.0);
+ //
+ // if (vote_up)
+ // WOLF_TRACE("VOTE UP");
+ // if (vote_down)
+ // WOLF_TRACE("VOTE DOWN");
+ // if (vote_time)
+ // WOLF_TRACE("VOTE TIME");
+ if (vote_down)
+ WOLF_INFO("Creating KF. Number of features: ", incoming_ptr_->getFeatureList().size());
+
+ return vote_up || vote_down || vote_time;
+}
+
+
+FactorBasePtr ProcessorBundleAdjustment::emplaceFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr)
+{
+ /* This function is no creating any factor.
+ * We create factors with establishFactors()
+ */
+ return FactorBasePtr();
+}
+
+LandmarkBasePtr ProcessorBundleAdjustment::emplaceLandmark(FeatureBasePtr _feature_ptr)
+{
+ FeaturePointImagePtr feat_point_image_ptr = std::static_pointer_cast<FeaturePointImage>( _feature_ptr);
+ Eigen::Vector2s point2D = _feature_ptr->getMeasurement();
+
+ Eigen::Vector3s point3D;
+ point3D = pinhole::backprojectPoint(
+ getSensor()->getIntrinsic()->getState(),
+ (std::static_pointer_cast<SensorCamera>(getSensor()))->getCorrectionVector(),
+ point2D);
+
+
+
+ //Scalar distance = params_bundle_adjustment_->distance; // arbitrary value
+ Scalar distance = 1;
+ Eigen::Vector4s vec_homogeneous_c;
+ vec_homogeneous_c = {point3D(0),point3D(1),point3D(2),point3D.norm()/distance};
+ vec_homogeneous_c.normalize();
+
+ //TODO: lmk from camera to world coordinate frame.
+ Transform<Scalar,3,Isometry> T_w_r
+ = Translation<Scalar,3>(_feature_ptr->getFrame()->getP()->getState())
+ * Quaternions(_feature_ptr->getFrame()->getO()->getState().data());
+ Transform<Scalar,3,Isometry> T_r_c
+ = Translation<Scalar,3>(_feature_ptr->getCapture()->getSensorP()->getState())
+ * Quaternions(_feature_ptr->getCapture()->getSensorO()->getState().data());
+ Eigen::Matrix<Scalar, 4, 1> vec_homogeneous_w = T_w_r
+ * T_r_c
+ * vec_homogeneous_c;
+
+
+// std::cout << "Point2D: " << point2D.transpose() << std::endl;
+// std::cout << "Point3D: " << point3D.transpose() << std::endl;
+// std::cout << "Hmg_c: " << vec_homogeneous.transpose() << std::endl;
+// std::cout << "Hmg_w: " << vec_homogeneous_w.transpose() << std::endl;
+ //vec_homogeneous_w = vec_homogeneous;
+
+ LandmarkBasePtr lmk_hp_ptr = LandmarkBase::emplace<LandmarkHP>(getProblem()->getMap(), vec_homogeneous_w, getSensor(), feat_point_image_ptr->getDescriptor());
+
+// std::cout << "LANDMARKS CREATED AND ADDED to MAP: " << getProblem()->getMap()->getLandmarkList().size() << std::endl;
+
+ _feature_ptr->setLandmarkId(lmk_hp_ptr->id());
+ return lmk_hp_ptr;
+}
+
+void ProcessorBundleAdjustment::establishFactors()
+{
+
+// if (origin_ptr_ == last_ptr_)
+// return;
+//
+// TrackMatches matches_origin_inc = track_matrix_.matches(origin_ptr_, last_ptr_);
+//
+// for (auto const & pair_trkid_pair : matches_origin_inc)
+// {
+// size_t trkid = pair_trkid_pair.first;
+// //if track size is lower than a minimum, don't establish any factor.
+// if (track_matrix_.trackSize(trkid) < params_bundle_adjustment_->min_track_length_for_factor)
+// continue;
+//
+// FeatureBasePtr feature_origin = pair_trkid_pair.second.first;
+// FeatureBasePtr feature_last = pair_trkid_pair.second.second;
+//
+// if (lmk_track_map_.count(trkid)==0) //if the track doesn't have landmark associated -> we need a map: map[track_id] = landmarkptr
+// {
+// //emplaceLandmark
+// LandmarkBasePtr lmk = emplaceLandmark(feature_origin);
+// LandmarkHPPtr lmk_hp = std::static_pointer_cast<LandmarkHP>(lmk);
+//
+// //add only one Landmark to map: map[track_id] = landmarkptr
+// lmk_track_map_[trkid] = lmk;
+//
+// //emplace a factor for each feature in the track (only for keyframes)
+// Track full_track = track_matrix_.trackAtKeyframes(trkid);
+// for (auto it=full_track.begin(); it!=full_track.end(); ++it)
+// {
+// FactorBase::emplace<FactorPixelHP>(it->second, it->second, lmk_hp, shared_from_this());
+// }
+//
+// }
+// else
+// {
+// LandmarkBasePtr lmk = lmk_track_map_[trkid];
+// LandmarkHPPtr lmk_hp = std::static_pointer_cast<LandmarkHP>(lmk);
+//
+// //Create factor
+// FactorBase::emplace<FactorPixelHP>(feature_last, feature_last, lmk_hp, shared_from_this());
+// }
+//
+// }
+
+
+
+ for (auto & pair_id_ftr : track_matrix_.snapshot(last_ptr_))
+ {
+ auto & trkid = pair_id_ftr.first;
+ auto & ftr = pair_id_ftr.second;
+
+ if (lmk_track_map_.count(trkid)==0) //if the track doesn't have landmark associated -> we need a map: map[track_id] = landmarkptr
+ {
+ //emplaceLandmark
+ LandmarkBasePtr lmk = emplaceLandmark(ftr);
+ LandmarkHPPtr lmk_hp = std::static_pointer_cast<LandmarkHP>(lmk);
+
+ //add only one Landmark to map: map[track_id] = landmarkptr
+ lmk_track_map_[trkid] = lmk;
+
+ //emplace a factor
+ FactorBase::emplace<FactorPixelHP>(ftr, ftr, lmk_hp, shared_from_this());
+
+ }
+ else
+ {
+ // recover the landmark
+ LandmarkBasePtr lmk = lmk_track_map_[trkid];
+ LandmarkHPPtr lmk_hp = std::static_pointer_cast<LandmarkHP>(lmk);
+
+ //emplace a factor
+ FactorBase::emplace<FactorPixelHP>(ftr, ftr, lmk_hp, shared_from_this());
+ }
+
+ }
+}
+
+void ProcessorBundleAdjustment::setParams(const ProcessorParamsBundleAdjustmentPtr _params)
+{
+ params_bundle_adjustment_ = _params;
+}
+
+ProcessorBasePtr ProcessorBundleAdjustment::create(const std::string& _unique_name,
+ const ProcessorParamsBasePtr _params,
+ const SensorBasePtr _sensor_ptr)
+{
+ const auto params = std::static_pointer_cast<ProcessorParamsBundleAdjustment>(_params);
+
+ ProcessorBasePtr prc_ptr = std::make_shared<ProcessorBundleAdjustment>(params);
+ prc_ptr->setName(_unique_name);
+ prc_ptr->configure(_sensor_ptr);
+ return prc_ptr;
+}
+
+} //namespace wolf
+
+// Register in the ProcessorFactory
+#include "core/processor/processor_factory.h"
+namespace wolf {
+WOLF_REGISTER_PROCESSOR("TRACKER BUNDLE ADJUSTMENT", ProcessorBundleAdjustment)
+} // namespace wolf
+
diff --git a/src/processor/processor_tracker_feature_image.cpp b/src/processor/processor_tracker_feature_image.cpp
index 57f5c2eba7778411c487c6c2bc5f2f7f117bfe70..eb91908964cfa9ce3b0e7507e410065f2e1c90d7 100644
--- a/src/processor/processor_tracker_feature_image.cpp
+++ b/src/processor/processor_tracker_feature_image.cpp
@@ -2,10 +2,10 @@
#include "vision/processor/processor_tracker_feature_image.h"
// Vision utils
-#include <detectors.h>
-#include <descriptors.h>
-#include <matchers.h>
-#include <algorithms.h>
+#include <vision_utils/detectors.h>
+#include <vision_utils/descriptors.h>
+#include <vision_utils/matchers.h>
+#include <vision_utils/algorithms.h>
// standard libs
#include <bitset>
diff --git a/src/processor/processor_tracker_feature_trifocal.cpp b/src/processor/processor_tracker_feature_trifocal.cpp
index 1669a10116bd9895c83189c8fecde258ace8c97c..5d11a6bee264bfea4222573567b9819ca658a584 100644
--- a/src/processor/processor_tracker_feature_trifocal.cpp
+++ b/src/processor/processor_tracker_feature_trifocal.cpp
@@ -8,11 +8,11 @@
#include "vision/capture/capture_image.h"
// vision_utils
-#include <vision_utils.h>
-#include <detectors.h>
-#include <descriptors.h>
-#include <matchers.h>
-#include <algorithms.h>
+#include <vision_utils/vision_utils.h>
+#include <vision_utils/detectors.h>
+#include <vision_utils/descriptors.h>
+#include <vision_utils/matchers.h>
+#include <vision_utils/algorithms.h>
#include <memory>
diff --git a/src/processor/processor_tracker_landmark_image.cpp b/src/processor/processor_tracker_landmark_image.cpp
index bd7de009d94a89309690c6d224a741fa7c6e0c1f..f7b3b0fa02e0aa5caae3faad579a104286f1dbd1 100644
--- a/src/processor/processor_tracker_landmark_image.cpp
+++ b/src/processor/processor_tracker_landmark_image.cpp
@@ -311,7 +311,7 @@ void ProcessorTrackerLandmarkImage::landmarkInCurrentCamera(const Eigen::VectorX
assert((_current_state.size() == 7 || _current_state.size() == 16) && "Wrong state size! Should be 7 for 3D pose or 16 for IMU.");
// ALL TRANSFORMS
- Transform<Scalar,3,Eigen::Affine> T_W_R0, T_W_R1, T_R0_C0, T_R1_C1;
+ Transform<Scalar,3,Eigen::Isometry> T_W_R0, T_W_R1, T_R0_C0, T_R1_C1;
// world to anchor robot frame
Translation<Scalar,3> t_w_r0(_landmark->getAnchorFrame()->getP()->getState()); // sadly we cannot put a Map over a translation
@@ -335,7 +335,7 @@ void ProcessorTrackerLandmarkImage::landmarkInCurrentCamera(const Eigen::VectorX
// Transform lmk from c0 to c1 and exit
Vector4s landmark_hmg_c0 = _landmark->getP()->getState(); // lmk in anchor frame
- _point3D_hmg = T_R1_C1.inverse(Eigen::Affine) * T_W_R1.inverse(Eigen::Affine) * T_W_R0 * T_R0_C0 * landmark_hmg_c0;
+ _point3D_hmg = T_R1_C1.inverse(Eigen::Isometry) * T_W_R1.inverse(Eigen::Isometry) * T_W_R0 * T_R0_C0 * landmark_hmg_c0;
}
Scalar ProcessorTrackerLandmarkImage::match(const cv::Mat _target_descriptor, const cv::Mat _candidate_descriptors, DMatchVector& _cv_matches)
diff --git a/src/sensor/sensor_camera.cpp b/src/sensor/sensor_camera.cpp
index 76fb22d85fb59ea55d399b48eea82c5601635211..44b7fa6b0102391e18462c5d255d5dc8afb6aedb 100644
--- a/src/sensor/sensor_camera.cpp
+++ b/src/sensor/sensor_camera.cpp
@@ -13,7 +13,7 @@ SensorCamera::SensorCamera(const Eigen::VectorXs& _extrinsics, const IntrinsicsC
img_width_(_intrinsics.width), //
img_height_(_intrinsics.height), //
distortion_(_intrinsics.distortion), //
- correction_(distortion_.size() + 1), // make correction vector slightly larger in size than the distortion vector
+ correction_(distortion_.size()==0 ? 0 : distortion_.size() + 1), // make correction vector slightly larger in size than the distortion vector
pinhole_model_raw_(_intrinsics.pinhole_model_raw), //
pinhole_model_rectified_(_intrinsics.pinhole_model_rectified), //
using_raw_(true)
diff --git a/src/yaml/processor_bundle_adjustment_yaml.cpp b/src/yaml/processor_bundle_adjustment_yaml.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..11ec27e6c12d9aee99ab7a9471841407f0e90810
--- /dev/null
+++ b/src/yaml/processor_bundle_adjustment_yaml.cpp
@@ -0,0 +1,71 @@
+// wolf yaml
+#include "core/yaml/yaml_conversion.h"
+
+// wolf
+#include "core/common/factory.h"
+
+// yaml-cpp library
+#include <yaml-cpp/yaml.h>
+#include "vision/processor/processor_bundle_adjustment.h"
+
+namespace wolf
+{
+
+namespace
+{
+static ProcessorParamsBasePtr createProcessorParamsBundleAdjustment(const std::string & _filename_dot_yaml)
+{
+ YAML::Node config = YAML::LoadFile(_filename_dot_yaml);
+
+ if (config.IsNull())
+ {
+ WOLF_ERROR("Invalid YAML file!");
+ return nullptr;
+ }
+ else if (config["processor type"].as<std::string>() == "TRACKER BUNDLE ADJUSTMENT")
+ {
+ ProcessorParamsBundleAdjustmentPtr params = std::make_shared<ProcessorParamsBundleAdjustment>();
+
+ YAML::Node vision_utils = config ["vision_utils"];
+ params->yaml_file_params_vision_utils = vision_utils["YAML file params"].as<std::string>();
+
+ // relative to global path for Vision Utils YAML
+ assert(params->yaml_file_params_vision_utils.at(0) != ('/') && "The parameter YAML FILE PARAMS (in processor params YAML file) must be specified with a path relative to the processor YAML file.");
+ unsigned first = _filename_dot_yaml.find("/");
+ unsigned last = _filename_dot_yaml.find_last_of("/");
+ std::string strNew = _filename_dot_yaml.substr (first,last-first);
+ params->yaml_file_params_vision_utils = _filename_dot_yaml.substr (first,last-first) + "/" + params->yaml_file_params_vision_utils;
+
+ YAML::Node algorithm = config ["algorithm"];
+ params->time_tolerance = algorithm["time tolerance"] .as<Scalar>();
+ params->voting_active = algorithm["voting active"] .as<bool>();
+ params->delete_ambiguities = algorithm["delete ambiguities"] .as<bool>();
+ params->min_features_for_keyframe = algorithm["minimum features for keyframe"] .as<unsigned int>();
+ params->max_new_features = algorithm["maximum new features"] .as<unsigned int>();
+
+ params->n_cells_h = algorithm["grid horiz cells"] .as<int>();
+ params->n_cells_v = algorithm["grid vert cells"] .as<int>();
+ params->min_response_new_feature = algorithm["min response new features"] .as<int>();
+
+ params->min_track_length_for_factor = algorithm["min track length for factor"].as<int>();
+
+
+ YAML::Node noise = config["noise"];
+ params->pixel_noise_std = noise ["pixel noise std"].as<Scalar>();
+
+ return params;
+ }
+ else
+ {
+ WOLF_ERROR("Wrong processor type! Should be \"TRACKER BUNDLE ADJUSTMENT\"");
+ return nullptr;
+ }
+ return nullptr;
+}
+
+// Register in the SensorFactory
+const bool WOLF_UNUSED registered_prc_bundle_adjustment = ProcessorParamsFactory::get().registerCreator("TRACKER BUNDLE ADJUSTMENT", createProcessorParamsBundleAdjustment);
+
+} // namespace [unnamed]
+
+} // namespace wolf
diff --git a/src/yaml/processor_image_yaml.cpp b/src/yaml/processor_image_yaml.cpp
index 2ab14bfa4c8868a018f2f8c736c67bdf90ab50b0..f8cdeeac00184518ae9e2b9292c4f04fc0fe5b1f 100644
--- a/src/yaml/processor_image_yaml.cpp
+++ b/src/yaml/processor_image_yaml.cpp
@@ -5,6 +5,8 @@
* \author: jsola
*/
+#include "vision/processor/processor_params_image.h"
+
// wolf yaml
#include "core/yaml/yaml_conversion.h"
@@ -14,8 +16,6 @@
// yaml-cpp library
#include <yaml-cpp/yaml.h>
-#include "vision/processor/processor_params_image.h"
-
namespace wolf
{
namespace
diff --git a/src/yaml/processor_tracker_feature_trifocal_yaml.cpp b/src/yaml/processor_tracker_feature_trifocal_yaml.cpp
index 78bb7ee980021358080012a071d3bb1f1041c77a..9257e03735442b4f801746f21407f43834215412 100644
--- a/src/yaml/processor_tracker_feature_trifocal_yaml.cpp
+++ b/src/yaml/processor_tracker_feature_trifocal_yaml.cpp
@@ -5,11 +5,10 @@
* Author: asantamaria
*/
-// wolf yaml
+// wolf
#include "vision/processor/processor_tracker_feature_trifocal.h"
-#include "core/yaml/yaml_conversion.h"
-// wolf
+#include "core/yaml/yaml_conversion.h"
#include "core/common/factory.h"
// yaml-cpp library
@@ -47,7 +46,7 @@ static ProcessorParamsBasePtr createProcessorParamsTrackerFeatureTrifocal(const
params->time_tolerance = algorithm["time tolerance"] .as<Scalar>();
params->voting_active = algorithm["voting active"] .as<bool>();
params->min_features_for_keyframe = algorithm["minimum features for keyframe"] .as<unsigned int>();
- params->max_new_features = algorithm["maximum new features"] .as<unsigned int>();
+ params->max_new_features = algorithm["maximum new features"] .as<int>();
params->n_cells_h = algorithm["grid horiz cells"] .as<int>();
params->n_cells_v = algorithm["grid vert cells"] .as<int>();
params->min_response_new_feature = algorithm["min response new features"] .as<int>();
diff --git a/src/yaml/processor_tracker_landmark_apriltag_yaml.cpp b/src/yaml/processor_tracker_landmark_apriltag_yaml.cpp
deleted file mode 100644
index 70a44fc672b41eaacd09fd5dc36b7d642bb283cf..0000000000000000000000000000000000000000
--- a/src/yaml/processor_tracker_landmark_apriltag_yaml.cpp
+++ /dev/null
@@ -1,92 +0,0 @@
-/**
- * \file processor_tracker_landmark_apriltag_yaml.cpp
- *
- * Created on: Dec 6, 2018
- * \author: jsola
- */
-
-
-// wolf
-#include "base/processor/processor_tracker_landmark_apriltag.h"
-#include "base/yaml/yaml_conversion.h"
-#include "base/common/factory.h"
-
-// yaml-cpp library
-#include <yaml-cpp/yaml.h>
-
-namespace wolf
-{
-
-namespace
-{
-static ProcessorParamsBasePtr createProcessorParamsLandmarkApriltag(const std::string & _filename_dot_yaml)
-{
- YAML::Node config = YAML::LoadFile(_filename_dot_yaml);
-
- if (config.IsNull())
- {
- WOLF_ERROR("Invalid YAML file!");
- return nullptr;
- }
- else if (config["processor type"].as<std::string>() == "TRACKER LANDMARK APRILTAG")
- {
- ProcessorParamsTrackerLandmarkApriltagPtr params = std::make_shared<ProcessorParamsTrackerLandmarkApriltag>();
-
- YAML::Node detector_parameters = config["detector parameters"];
- params->quad_decimate_ = detector_parameters["quad_decimate"] .as<Scalar>();
- params->quad_sigma_ = detector_parameters["quad_sigma"] .as<Scalar>();
- params->nthreads_ = detector_parameters["nthreads"] .as<int>();
- params->debug_ = detector_parameters["debug"] .as<bool>();
- params->refine_edges_ = detector_parameters["refine_edges"] .as<bool>();
- params->ippe_min_ratio_ = detector_parameters["ippe_min_ratio"] .as<Scalar>();
- params->ippe_max_rep_error_ = detector_parameters["ippe_max_rep_error"] .as<Scalar>();
-
- YAML::Node tag_parameters = config["tag parameters"];
- params->tag_family_ = tag_parameters["tag_family"] .as<std::string>();
- // params->tag_black_border_ = tag_parameters["tag_black_border"] .as<int>();
- params->tag_width_default_ = tag_parameters["tag_width_default"] .as<Scalar>();
-
- // read list of tag widths
- YAML::Node tag_widths = config["tag widths"];
- for (auto pair_id_width : tag_widths)
- {
- int tag_id = pair_id_width.first .as<int>();
- Scalar tag_width = pair_id_width.second .as<Scalar>();
- params->tag_widths_.emplace(tag_id, tag_width);
- }
-
- YAML::Node noise = config["noise"];
- params->std_xy_ = noise["std_xy"] .as<Scalar>();
- params->std_z_ = noise["std_z"] .as<Scalar>();
- params->std_rpy_ = M_TORAD * noise["std_rpy_degrees"] .as<Scalar>();
- params->std_pix_ = noise["std_pix"] .as<Scalar>();
-
- YAML::Node vote = config["vote"];
- params->voting_active = vote["voting active"] .as<bool>();
- params->min_time_vote_ = vote["min_time_vote"] .as<Scalar>();
- params->max_time_vote_ = vote["max_time_vote"] .as<Scalar>();
- params->min_features_for_keyframe = vote["min_features_for_keyframe"] .as<unsigned int>();
- params->max_features_diff_ = vote["max_features_diff"] .as<int>();
- params->nb_vote_for_every_first_ = vote["nb_vote_for_every_first"] .as<int>();
- params->enough_info_necessary_ = vote["enough_info_necessary"] .as<bool>();
-
- params->reestimate_last_frame_ = config["reestimate_last_frame"] .as<bool>();
- params->add_3D_cstr_ = config["add_3D_cstr"] .as<bool>();
-
- return params;
- }
- else
- {
- WOLF_ERROR("Wrong processor type! Should be \"TRACKER LANDMARK APRILTAG\"");
- return nullptr;
- }
- return nullptr;
-}
-
-// Register in the SensorFactory
-const bool WOLF_UNUSED registered_prc_apriltag = ProcessorParamsFactory::get().registerCreator("TRACKER LANDMARK APRILTAG", createProcessorParamsLandmarkApriltag);
-const bool WOLF_UNUSED registered_prc_apriltag_wrapper = ProcessorParamsFactory::get().registerCreator("TRACKER LANDMARK APRILTAG WRAPPER", createProcessorParamsLandmarkApriltag);
-
-} // namespace [unnamed]
-
-} // namespace wolf
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index b51db6529108ac757397a0a6cef6f0abeaaf0ab3..84cc2085d0b60c5f301f8231aef5beaa2b2dbc32 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -27,3 +27,12 @@ target_link_libraries(gtest_factor_autodiff_trifocal ${PLUGIN_NAME} ${wolf_LIBRA
# ProcessorFeatureTrifocal test
wolf_add_gtest(gtest_processor_tracker_feature_trifocal gtest_processor_tracker_feature_trifocal.cpp)
target_link_libraries(gtest_processor_tracker_feature_trifocal ${PLUGIN_NAME} ${wolf_LIBRARY} ${OpenCV_LIBS} ${vision_utils_LIBRARY})
+
+# ProcessorBundleAdjustment test
+wolf_add_gtest(gtest_processor_bundle_adjustment gtest_processor_bundle_adjustment.cpp)
+target_link_libraries(gtest_processor_bundle_adjustment ${PLUGIN_NAME} ${wolf_LIBRARY} ${OpenCV_LIBS} ${vision_utils_LIBRARY})
+
+# FactorPixelHP test
+wolf_add_gtest(gtest_factor_pixelHP gtest_factor_pixelHP.cpp)
+target_link_libraries(gtest_factor_pixelHP ${PLUGIN_NAME} ${wolf_LIBRARY} ${OpenCV_LIBS} ${vision_utils_LIBRARY})
+
diff --git a/test/gtest_factor_pixelHP.cpp b/test/gtest_factor_pixelHP.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..807d5b0844f291a3e4c68aa8b167c00bdee4ddc8
--- /dev/null
+++ b/test/gtest_factor_pixelHP.cpp
@@ -0,0 +1,664 @@
+/*
+ * gtest_factor_pixelHP.cpp
+ *
+ * Created on: May 16, 2019
+ * Author: ovendrell
+ */
+#include "vision/factor/factor_pixelHP.h"
+#include "vision/landmark/landmark_HP.h"
+#include "vision/capture/capture_image.h"
+#include "vision/processor/processor_bundle_adjustment.h"
+#include "vision/internal/config.h"
+#include "vision/math/pinhole_tools.h"
+
+#include <core/utils/utils_gtest.h>
+#include <core/factor/factor_block_absolute.h>
+#include <core/factor/factor_quaternion_absolute.h>
+#include <core/ceres_wrapper/ceres_manager.h>
+#include <core/ceres_wrapper/ceres_manager.h>
+
+using namespace wolf;
+using namespace Eigen;
+
+std::string wolf_vision_root = _WOLF_VISION_ROOT_DIR;
+
+
+class FactorPixelHPTest : public testing::Test{
+ public:
+ Vector3s pos1, pos2, pos3, pos_cam, point;
+ Vector3s euler1, euler2, euler3, euler_cam;
+ Quaternions quat1, quat2, quat3, quat_cam;
+ Vector4s vquat1, vquat2, vquat3, vquat_cam; // quaternions as vectors
+ Vector7s pose1, pose2, pose3, pose_cam;
+ Vector4s lmkHP1, lmkHP2, lmkHP3, lmkHP4;
+
+ ProblemPtr problem;
+ CeresManagerPtr ceres_manager;
+
+
+ SensorCameraPtr camera;
+ ProcessorBundleAdjustmentPtr proc_bundle_adjustment;
+
+ SensorBasePtr S;
+ FrameBasePtr F1, F2, F3;
+ CaptureImagePtr I1, I2, I3;
+ FeaturePointImagePtr f11, f21, f31;
+ FeaturePointImagePtr f12, f13, f14;
+ FeaturePointImagePtr f22, f23, f24;
+ FeaturePointImagePtr f32, f33, f34;
+
+ LandmarkHPPtr L1; //only one landmark L1 is initialized
+ LandmarkHPPtr L2;
+ LandmarkHPPtr L3;
+ LandmarkHPPtr L4;
+
+ FactorPixelHPPtr c11, c12, c13, c14;
+ FactorPixelHPPtr c21, c22, c23, c24;
+ FactorPixelHPPtr c31, c32, c33, c34;
+
+ Scalar pixel_noise_std;
+
+ virtual ~FactorPixelHPTest()
+ {
+ std::cout << "destructor\n";
+ }
+
+ virtual void SetUp() override
+ {
+ // configuration
+ /*
+ * We have three robot poses, in three frames, with cameras C1, C2, C3
+ * looking towards the origin of coordinates:
+ *
+ * Z
+ * |
+ * ________ C3
+ * / | /
+ * --------- /| C2
+ * | / |
+ * |____________/_ | ___ Y
+ * / | /
+ * / | /
+ * --------- C1 |/
+ * | / |
+ * ---------
+ * /
+ * X
+ *
+ * Each robot pose is at one axis, facing the origin:
+ * F1: pos = (1,0,0), ori = (0,0,pi)
+ * F2: pos = (0,1,0), ori = (0,0,-pi/2)
+ * F3: pos = (0,0,1), ori = (0,pi/2,pi)
+ *
+ * The robot has a camera looking forward
+ * S: pos = (0,0,0), ori = (-pi/1, 0, -pi/1)
+ *
+ * There is a point at the origin
+ * P: pos = (0,0,0)
+ *
+ * The camera is canonical
+ * K = Id.
+ *
+ * Therefore, P projects exactly at the origin on each camera,
+ * creating three features:
+ * f1: p1 = (0,0)
+ * f2: p2 = (0,0)
+ * f3: p3 = (0,0)
+ *
+ * We form a Wolf tree with three frames, three captures,
+ * three features, one landmark and 3 factors:
+ *
+ * Frame F1, Capture C1, feature f1, factor c1
+ * Frame F2, Capture C2, feature f2, factor c2
+ * Frame F3, Capture C3, feature f3, factor c3
+ * Landmark L1
+ *
+ * The three frame poses F1, F2, F3, the camera pose S in the robot frame
+ * and the Landmark position are variables subject to optimization
+ *
+ * We perform a number of tests based on this configuration.
+ */
+
+ // all frames look to the origin
+ pos1 << 1, 0, 0;
+ pos2 << 0, 1, 0;
+ pos3 << 0, 0, 1;
+ euler1 << 0, 0 , M_PI ;
+ euler2 << 0, 0 , -M_PI_2 ;
+ euler3 << 0, M_PI_2, M_PI ;
+ quat1 = e2q(euler1);
+ quat2 = e2q(euler2);
+ quat3 = e2q(euler3);
+ vquat1 = quat1.coeffs();
+ vquat2 = quat2.coeffs();
+ vquat3 = quat3.coeffs();
+ pose1 << pos1, vquat1;
+ pose2 << pos2, vquat2;
+ pose3 << pos3, vquat3;
+
+ //landmarks
+ lmkHP1 << 0, 0, 0, 1;
+
+ // camera at the robot origin looking forward
+ pos_cam << 0, 0, 0;
+ euler_cam << -M_PI_2, 0, -M_PI_2;
+ quat_cam = e2q(euler_cam);
+ vquat_cam = quat_cam.coeffs();
+ pose_cam << pos_cam, vquat_cam;
+
+ // Build problem
+ problem = Problem::create("PO", 3);
+ ceres::Solver::Options options;
+ options.function_tolerance = 1e-6;
+ options.max_num_iterations = 200;
+ ceres_manager = std::make_shared<CeresManager>(problem, options);
+
+
+ // Install sensor and processor
+ IntrinsicsCameraPtr intr = std::make_shared<IntrinsicsCamera>();
+ intr->pinhole_model_raw = Eigen::Vector4s(320,240,320,320);
+ intr->width = 640;
+ intr->height = 480;
+ S = problem->installSensor("CAMERA", "camera", pose_cam, intr);
+ camera = std::static_pointer_cast<SensorCamera>(S);
+
+// ProcessorParamsBundleAdjustmentPtr params = std::make_shared<ProcessorParamsBundleAdjustment>();
+// params->delete_ambiguities = true;
+// params->yaml_file_params_vision_utils = wolf_root + "/demos/processor_bundle_adjustment_vision_utils.yaml";
+// params->pixel_noise_std = 1.0;
+// params->min_track_length_for_factor = 3;
+// params->voting_active = true;
+// params->max_new_features = 5;
+//
+// ProcessorBasePtr proc = problem->installProcessor("TRACKER BUNDLE ADJUSTMENT", "processor", camera, params);
+// proc_bundle_adjustment = std::static_pointer_cast<ProcessorBundleAdjustment>(proc);
+
+ // Add three viewpoints with frame, capture and feature
+ pixel_noise_std = 1.0; //params->pixel_noise_std;
+ Vector2s pix(0,0);
+ Matrix2s pix_cov(Matrix2s::Identity() * pow(pixel_noise_std, 2));
+
+ // Point
+ cv::Point2f p = cv::Point2f(intr->width /2, intr->height/2);
+ cv::KeyPoint kp = cv::KeyPoint(p, 32.0f);
+ cv::Mat des = cv::Mat::zeros(1,8, CV_8UC1);
+
+ F1 = problem->emplaceFrame(KEY, pose1, 1.0);
+ I1 = std::static_pointer_cast<CaptureImage>(CaptureBase::emplace<CaptureImage>(F1, 1.0, camera, cv::Mat(intr->width,intr->height,CV_8UC1)));
+ f11 = std::static_pointer_cast<FeaturePointImage>(FeatureBase::emplace<FeaturePointImage>(I1, kp, 0, des, pix_cov)); // pixel at origin
+
+ F2 = problem->emplaceFrame(KEY, pose2, 2.0);
+ I2 = std::static_pointer_cast<CaptureImage>((CaptureBase::emplace<CaptureImage>(F2, 2.0, camera, cv::Mat(intr->width,intr->height,CV_8UC1))));
+ f21 = std::static_pointer_cast<FeaturePointImage>(FeatureBase::emplace<FeaturePointImage>(I2, kp, 0, des, pix_cov)); // pixel at origin
+
+ F3 = problem->emplaceFrame(KEY, pose3, 3.0);
+ I3 = std::static_pointer_cast<CaptureImage>(CaptureBase::emplace<CaptureImage>(F3, 3.0, camera, cv::Mat(intr->width,intr->height,CV_8UC1)));
+ f31 = std::static_pointer_cast<FeaturePointImage>(FeatureBase::emplace<FeaturePointImage>(I3, kp, 0, des, pix_cov)); // pixel at origin
+
+ //create Landmark -- with f1, f2 or f3
+ //LandmarkBasePtr lmk = proc_bundle_adjustment->createLandmark(f1);
+ L1 = std::static_pointer_cast<LandmarkHP>(LandmarkBase::emplace<LandmarkHP>(problem->getMap(),lmkHP1, camera, des));
+// problem->addLandmark(L1);
+// lmk->link(problem->getMap());
+
+ // factors
+ c11 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f11, f11, L1, nullptr /*proc*/));
+ c21 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f21, f21, L1, nullptr /*proc*/));
+ c31 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f31, f31, L1, nullptr /*proc*/));
+ }
+};
+
+TEST(ProcessorFactorPixelHP, testZeroResidual)
+{
+ //Build problem
+ ProblemPtr problem_ptr = Problem::create("PO", 3);
+ CeresManagerPtr ceres_mgr = std::make_shared<CeresManager>(problem_ptr);
+
+ // Install sensor
+ IntrinsicsCameraPtr intr = std::make_shared<IntrinsicsCamera>();
+ intr->pinhole_model_raw = Eigen::Vector4s(0,0,1,1);
+ intr->width = 640;
+ intr->height = 480;
+ auto sens_cam = problem_ptr->installSensor("CAMERA", "camera", (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), intr);
+ SensorCameraPtr camera = std::static_pointer_cast<SensorCamera>(sens_cam);
+ // Install processor
+ ProcessorParamsBundleAdjustmentPtr params = std::make_shared<ProcessorParamsBundleAdjustment>();
+ params->delete_ambiguities = true;
+ params->yaml_file_params_vision_utils = wolf_vision_root + "/demos/processor_bundle_adjustment_vision_utils.yaml";
+ params->pixel_noise_std = 1.0;
+ params->min_track_length_for_factor = 3;
+ params->voting_active = true;
+ params->max_new_features = 5;
+ auto proc = problem_ptr->installProcessor("TRACKER BUNDLE ADJUSTMENT", "processor", sens_cam, params);
+ ProcessorBundleAdjustmentPtr proc_bundle_adj = std::static_pointer_cast<ProcessorBundleAdjustment>(proc);
+
+ // Frame
+ FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, problem_ptr->zeroState(), TimeStamp(0));
+
+ // Capture
+ auto cap0 = std::static_pointer_cast<CaptureImage>(CaptureImage::emplace<CaptureImage>(frm0, TimeStamp(0), camera, cv::Mat::zeros(480,640, 1)));
+
+ // Feature
+ cv::Point2f p = cv::Point2f(240, 320);
+ cv::KeyPoint kp = cv::KeyPoint(p, 32.0f);
+ cv::Mat des = cv::Mat::zeros(1,8, CV_8U);
+
+ FeatureBasePtr fea0 = FeatureBase::emplace<FeaturePointImage>(cap0, kp, 0, des, Eigen::Matrix2s::Identity()* pow(1, 2));
+
+ // Landmark
+ LandmarkBasePtr lmk = proc_bundle_adj->emplaceLandmark(fea0);
+ LandmarkHPPtr lmk_hp = std::static_pointer_cast<LandmarkHP>(lmk);
+
+ // Factor
+ auto fac0 = FactorBase::emplace<FactorPixelHP>(fea0, fea0, lmk_hp, proc);
+ auto fac_ptr = std::static_pointer_cast<FactorPixelHP>(fac0);
+
+ ASSERT_TRUE(problem_ptr->check(0));
+
+ //ASSERT the expectation of the landmark should be equal to the measurement
+ Eigen::VectorXs expect = fac_ptr->expectation();
+// std::cout << expect << std::endl;
+ ASSERT_FLOAT_EQ(expect(0,0),fea0->getMeasurement()(0,0));
+ ASSERT_FLOAT_EQ(expect(1,0),fea0->getMeasurement()(1,0));
+}
+
+TEST_F(FactorPixelHPTest, testSolveLandmark)
+{
+ ASSERT_TRUE(problem->check(0));
+
+ S->fix();
+ F1->fix();
+ F2->fix();
+ F3->fix();
+ L1->unfix();
+
+ auto orig = L1->point();
+ std::string report = ceres_manager->solve(wolf::SolverManager::ReportVerbosity::FULL);
+
+ std::cout << report << std::endl;
+
+ std::cout << orig.transpose() << std::endl;
+ std::cout << L1->point().transpose() << std::endl;
+
+ ASSERT_MATRIX_APPROX(L1->point(), orig, 1e-6);
+
+}
+
+TEST_F(FactorPixelHPTest, testSolveLandmarkAltered)
+{
+ ASSERT_TRUE(problem->check(0));
+
+ S->fix();
+ F1->fix();
+ F2->fix();
+ F3->fix();
+ L1->unfix();
+
+ auto orig = L1->point();
+ L1->getP()->setState(L1->getState() + Vector4s::Random());
+ std::string report = ceres_manager->solve(wolf::SolverManager::ReportVerbosity::FULL);
+
+ std::cout << report << std::endl;
+
+ std::cout << orig.transpose() << std::endl;
+ std::cout << L1->point().transpose() << std::endl;
+
+ ASSERT_MATRIX_APPROX(L1->point(), orig, 1e-6);
+
+}
+
+TEST_F(FactorPixelHPTest, testSolveFramePosition2ObservableDoF)
+{
+ ASSERT_TRUE(problem->check(0));
+
+ S->fix();
+ F2->fix();
+ F3->fix();
+ L1->fix();
+
+ auto orig = F1->getP()->getState();
+
+ //change state
+ Vector3s position;
+ position << 2.0, 2.0, 2.0;
+ auto ori = F1->getO()->getState();
+ Vector7s state;
+ state << position, ori;
+ F1->setState(state);
+
+ F1->getO()->fix();
+ F1->getP()->unfix();
+
+ std::string report = ceres_manager->solve(wolf::SolverManager::ReportVerbosity::FULL);
+
+ std::cout << report << std::endl;
+
+ std::cout << orig.transpose() << std::endl;
+ std::cout << F1->getP()->getState().transpose() << std::endl;
+
+ // This test is no good because it checks 3 DoF and only 2DoF are observable.
+ //ASSERT_MATRIX_APPROX(F1->getP()->getState(), orig, 1e-6);
+ // We use the following alternative:
+ // Frame must be in the X axis, so Y=0 and Z=0
+ ASSERT_MATRIX_APPROX(F1->getP()->getState().tail(2), orig.tail(2), 1e-6);
+
+ Eigen::VectorXs expect = c11->expectation();
+ ASSERT_FLOAT_EQ(expect(0,0),f11->getMeasurement()(0,0));
+ ASSERT_FLOAT_EQ(expect(1,0),f11->getMeasurement()(1,0));
+
+}
+
+TEST_F(FactorPixelHPTest, testSolveFramePosition)
+{
+ //landmark homogeneous coordinates in world reference
+ lmkHP2 << 0.5, 0.5, 0.5, 1;
+ lmkHP3 << -0.5, 0.75, 0.5, 1;
+ lmkHP4 << 0.5, 0, -0.5, 1;
+
+ //landmarks to camera coordinates
+ Transform<Scalar,3,Isometry> T_w_r
+ = Translation<Scalar,3>(F1->getP()->getState())
+ * Quaternions(F1->getO()->getState().data());
+ Transform<Scalar,3,Isometry> T_r_c
+ = Translation<Scalar,3>(I1->getSensorP()->getState())
+ * Quaternions(I1->getSensorO()->getState().data());
+ Eigen::Matrix<Scalar, 4, 1> lmkHP2_c = T_r_c.inverse() * T_w_r.inverse() * lmkHP2;
+ Eigen::Matrix<Scalar, 4, 1> lmkHP3_c = T_r_c.inverse() * T_w_r.inverse() * lmkHP3;
+ Eigen::Matrix<Scalar, 4, 1> lmkHP4_c = T_r_c.inverse() * T_w_r.inverse() * lmkHP4;
+
+ //landmark point in camera coord
+ Vector3s lmk_point_2_c = lmkHP2_c.head<3>()/lmkHP2_c(3);
+ Vector3s lmk_point_3_c = lmkHP3_c.head<3>()/lmkHP3_c(3);
+ Vector3s lmk_point_4_c = lmkHP4_c.head<3>()/lmkHP4_c(3);
+
+ //points projected to image plane
+ Vector2s point2 = pinhole::projectPoint(camera->getIntrinsic()->getState(), camera->getDistortionVector(), lmk_point_2_c);
+ Vector2s point3 = pinhole::projectPoint(camera->getIntrinsic()->getState(), camera->getDistortionVector(), lmk_point_3_c);
+ Vector2s point4 = pinhole::projectPoint(camera->getIntrinsic()->getState(), camera->getDistortionVector(), lmk_point_4_c);
+ cv::KeyPoint kp2 = cv::KeyPoint(cv::Point2f(point2(0),point2(1)), 32.0f);
+ cv::KeyPoint kp3 = cv::KeyPoint(cv::Point2f(point3(0),point3(1)), 32.0f);
+ cv::KeyPoint kp4 = cv::KeyPoint(cv::Point2f(point4(0),point4(1)), 32.0f);
+ cv::Mat des = cv::Mat::zeros(1,8, CV_8UC1);
+ pixel_noise_std = 1.0; //params->pixel_noise_std;
+ Vector2s pix(0,0);
+ Matrix2s pix_cov(Matrix2s::Identity() * pow(pixel_noise_std, 2));
+
+ //create features
+ f12 = std::static_pointer_cast<FeaturePointImage>(FeatureBase::emplace<FeaturePointImage>(I1, kp2, 0, des, pix_cov));
+ f13 = std::static_pointer_cast<FeaturePointImage>(FeatureBase::emplace<FeaturePointImage>(I1, kp3, 0, des, pix_cov));
+ f14 = std::static_pointer_cast<FeaturePointImage>(FeatureBase::emplace<FeaturePointImage>(I1, kp4, 0, des, pix_cov));
+
+ //create landmarks
+ L2 = std::static_pointer_cast<LandmarkHP>(LandmarkBase::emplace<LandmarkHP>(problem->getMap(),lmkHP2, camera, des));
+ L3 = std::static_pointer_cast<LandmarkHP>(LandmarkBase::emplace<LandmarkHP>(problem->getMap(),lmkHP3, camera, des));
+ L4 = std::static_pointer_cast<LandmarkHP>(LandmarkBase::emplace<LandmarkHP>(problem->getMap(),lmkHP4, camera, des));
+
+ //create factors
+ c12 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f12, f12, L2, nullptr /*proc*/));
+ c13 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f13, f13, L3, nullptr /*proc*/));
+ c14 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f14, f14, L4, nullptr /*proc*/));
+
+ ASSERT_TRUE(problem->check());
+
+ S->fix();
+ F2->fix();
+ F3->fix();
+ L1->fix();
+ L2->fix();
+ L3->fix();
+ L4->fix();
+
+ auto orig = F1->getP()->getState();
+
+ //change state
+ Vector3s position;
+ position << Vector3s::Random()*100;//2.0, 2.0, 2.0;
+ auto ori = F1->getO()->getState();
+ Vector7s state;
+ state << position, ori;
+ F1->setState(state);
+
+ F1->getO()->fix();
+ F1->getP()->unfix();
+
+ std::string report = ceres_manager->solve(wolf::SolverManager::ReportVerbosity::FULL);
+
+ std::cout << report << std::endl;
+
+ std::cout << orig.transpose() << std::endl;
+ std::cout << F1->getP()->getState().transpose() << std::endl;
+
+ // This test checks 3 DoF (3DoF are observable).
+ ASSERT_MATRIX_APPROX(F1->getP()->getState(), orig, 1e-6);
+
+ Eigen::VectorXs expect = c11->expectation();
+ ASSERT_FLOAT_EQ(expect(0,0),f11->getMeasurement()(0,0));
+ ASSERT_FLOAT_EQ(expect(1,0),f11->getMeasurement()(1,0));
+}
+
+
+TEST_F(FactorPixelHPTest, testSolveBundleAdjustment)
+{
+ //landmark homogeneous coordinates in world reference
+ lmkHP2 << 0.5, 0.5, 0.5, 1;
+ lmkHP3 << -0.5, 0.75, 0.5, 1;
+ lmkHP4 << 0.5, 0, -0.5, 1;
+
+ //landmarks to camera coordinates
+ Transform<Scalar,3,Isometry> T_w_r
+ = Translation<Scalar,3>(F1->getP()->getState())
+ * Quaternions(F1->getO()->getState().data());
+ Transform<Scalar,3,Isometry> T_r_c
+ = Translation<Scalar,3>(I1->getSensorP()->getState())
+ * Quaternions(I1->getSensorO()->getState().data());
+ Eigen::Matrix<Scalar, 4, 1> lmkHP2_c = T_r_c.inverse() * T_w_r.inverse() * lmkHP2;
+ Eigen::Matrix<Scalar, 4, 1> lmkHP3_c = T_r_c.inverse() * T_w_r.inverse() * lmkHP3;
+ Eigen::Matrix<Scalar, 4, 1> lmkHP4_c = T_r_c.inverse() * T_w_r.inverse() * lmkHP4;
+
+ //landmark point in camera coord
+ Vector3s lmk_point_2_c = lmkHP2_c.head<3>()/lmkHP2_c(3);
+ Vector3s lmk_point_3_c = lmkHP3_c.head<3>()/lmkHP3_c(3);
+ Vector3s lmk_point_4_c = lmkHP4_c.head<3>()/lmkHP4_c(3);
+
+ //landmarks to camera coordinates
+ Transform<Scalar,3,Isometry> T_w_r2
+ = Translation<Scalar,3>(F2->getP()->getState())
+ * Quaternions(F2->getO()->getState().data());
+ Transform<Scalar,3,Isometry> T_r_c2
+ = Translation<Scalar,3>(I2->getSensorP()->getState())
+ * Quaternions(I2->getSensorO()->getState().data());
+ Eigen::Matrix<Scalar, 4, 1> lmkHP2_c2 = T_r_c2.inverse() * T_w_r2.inverse() * lmkHP2;
+ Eigen::Matrix<Scalar, 4, 1> lmkHP3_c2 = T_r_c2.inverse() * T_w_r2.inverse() * lmkHP3;
+ Eigen::Matrix<Scalar, 4, 1> lmkHP4_c2 = T_r_c2.inverse() * T_w_r2.inverse() * lmkHP4;
+
+ //landmark point in camera coord
+ Vector3s lmk_point_2_c2 = lmkHP2_c2.head<3>()/lmkHP2_c2(3);
+ Vector3s lmk_point_3_c2 = lmkHP3_c2.head<3>()/lmkHP3_c2(3);
+ Vector3s lmk_point_4_c2 = lmkHP4_c2.head<3>()/lmkHP4_c2(3);
+
+ //landmarks to camera coordinates
+ Transform<Scalar,3,Isometry> T_w_r3
+ = Translation<Scalar,3>(F3->getP()->getState())
+ * Quaternions(F3->getO()->getState().data());
+ Transform<Scalar,3,Isometry> T_r_c3
+ = Translation<Scalar,3>(I3->getSensorP()->getState())
+ * Quaternions(I3->getSensorO()->getState().data());
+ Eigen::Matrix<Scalar, 4, 1> lmkHP2_c3 = T_r_c3.inverse() * T_w_r3.inverse() * lmkHP2;
+ Eigen::Matrix<Scalar, 4, 1> lmkHP3_c3 = T_r_c3.inverse() * T_w_r3.inverse() * lmkHP3;
+ Eigen::Matrix<Scalar, 4, 1> lmkHP4_c3 = T_r_c3.inverse() * T_w_r3.inverse() * lmkHP4;
+
+ //landmark point in camera coord
+ Vector3s lmk_point_2_c3 = lmkHP2_c3.head<3>()/lmkHP2_c3(3);
+ Vector3s lmk_point_3_c3 = lmkHP3_c3.head<3>()/lmkHP3_c3(3);
+ Vector3s lmk_point_4_c3 = lmkHP4_c3.head<3>()/lmkHP4_c3(3);
+
+ //points projected to image plane
+ Vector2s point2 = pinhole::projectPoint(camera->getIntrinsic()->getState(), camera->getDistortionVector(), lmk_point_2_c);
+ Vector2s point3 = pinhole::projectPoint(camera->getIntrinsic()->getState(), camera->getDistortionVector(), lmk_point_3_c);
+ Vector2s point4 = pinhole::projectPoint(camera->getIntrinsic()->getState(), camera->getDistortionVector(), lmk_point_4_c);
+ cv::KeyPoint kp12 = cv::KeyPoint(cv::Point2f(point2(0),point2(1)), 32.0f);
+ cv::KeyPoint kp13 = cv::KeyPoint(cv::Point2f(point3(0),point3(1)), 32.0f);
+ cv::KeyPoint kp14 = cv::KeyPoint(cv::Point2f(point4(0),point4(1)), 32.0f);
+
+ Vector2s point22 = pinhole::projectPoint(camera->getIntrinsic()->getState(), camera->getDistortionVector(), lmk_point_2_c2);
+ Vector2s point23 = pinhole::projectPoint(camera->getIntrinsic()->getState(), camera->getDistortionVector(), lmk_point_3_c2);
+ Vector2s point24 = pinhole::projectPoint(camera->getIntrinsic()->getState(), camera->getDistortionVector(), lmk_point_4_c2);
+ cv::KeyPoint kp22 = cv::KeyPoint(cv::Point2f(point22(0),point22(1)), 32.0f);
+ cv::KeyPoint kp23 = cv::KeyPoint(cv::Point2f(point23(0),point23(1)), 32.0f);
+ cv::KeyPoint kp24 = cv::KeyPoint(cv::Point2f(point24(0),point24(1)), 32.0f);
+
+ Vector2s point32 = pinhole::projectPoint(camera->getIntrinsic()->getState(), camera->getDistortionVector(), lmk_point_2_c3);
+ Vector2s point33 = pinhole::projectPoint(camera->getIntrinsic()->getState(), camera->getDistortionVector(), lmk_point_3_c3);
+ Vector2s point34 = pinhole::projectPoint(camera->getIntrinsic()->getState(), camera->getDistortionVector(), lmk_point_4_c3);
+ cv::KeyPoint kp32 = cv::KeyPoint(cv::Point2f(point32(0),point32(1)), 32.0f);
+ cv::KeyPoint kp33 = cv::KeyPoint(cv::Point2f(point33(0),point33(1)), 32.0f);
+ cv::KeyPoint kp34 = cv::KeyPoint(cv::Point2f(point34(0),point34(1)), 32.0f);
+
+ cv::Mat des = cv::Mat::zeros(1,8, CV_8UC1);
+ pixel_noise_std = 1.0; //params->pixel_noise_std;
+ Vector2s pix(0,0);
+ Matrix2s pix_cov(Matrix2s::Identity() * pow(pixel_noise_std, 2));
+
+ //create features
+ f12 = std::static_pointer_cast<FeaturePointImage>(FeatureBase::emplace<FeaturePointImage>(I1, kp12, 0, des, pix_cov));
+ f13 = std::static_pointer_cast<FeaturePointImage>(FeatureBase::emplace<FeaturePointImage>(I1, kp13, 0, des, pix_cov));
+ f14 = std::static_pointer_cast<FeaturePointImage>(FeatureBase::emplace<FeaturePointImage>(I1, kp14, 0, des, pix_cov));
+
+ f22 = std::static_pointer_cast<FeaturePointImage>(FeatureBase::emplace<FeaturePointImage>(I2, kp22, 0, des, pix_cov));
+ f23 = std::static_pointer_cast<FeaturePointImage>(FeatureBase::emplace<FeaturePointImage>(I2, kp23, 0, des, pix_cov));
+ f24 = std::static_pointer_cast<FeaturePointImage>(FeatureBase::emplace<FeaturePointImage>(I2, kp24, 0, des, pix_cov));
+
+ f32 = std::static_pointer_cast<FeaturePointImage>(FeatureBase::emplace<FeaturePointImage>(I3, kp32, 0, des, pix_cov));
+ f33 = std::static_pointer_cast<FeaturePointImage>(FeatureBase::emplace<FeaturePointImage>(I3, kp33, 0, des, pix_cov));
+ f34 = std::static_pointer_cast<FeaturePointImage>(FeatureBase::emplace<FeaturePointImage>(I3, kp34, 0, des, pix_cov));
+
+ //create landmarks
+ L2 = std::static_pointer_cast<LandmarkHP>(LandmarkBase::emplace<LandmarkHP>(problem->getMap(),lmkHP2, camera, des));
+ L3 = std::static_pointer_cast<LandmarkHP>(LandmarkBase::emplace<LandmarkHP>(problem->getMap(),lmkHP3, camera, des));
+ L4 = std::static_pointer_cast<LandmarkHP>(LandmarkBase::emplace<LandmarkHP>(problem->getMap(),lmkHP4, camera, des));
+
+ //create factors
+ c12 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f12, f12, L2, nullptr /*proc*/));
+ c13 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f13, f13, L3, nullptr /*proc*/));
+ c14 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f14, f14, L4, nullptr /*proc*/));
+
+ c22 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f22, f22, L2, nullptr /*proc*/));
+ c23 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f23, f23, L3, nullptr /*proc*/));
+ c24 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f24, f24, L4, nullptr /*proc*/));
+
+ c32 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f32, f32, L2, nullptr /*proc*/));
+ c33 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f33, f33, L3, nullptr /*proc*/));
+ c34 = std::static_pointer_cast<FactorPixelHP>(FactorBase::emplace<FactorPixelHP>(f34, f34, L4, nullptr /*proc*/));
+
+ ASSERT_TRUE(problem->check());
+
+// S->fix();
+ F1->fix();
+// F2->fix();
+// F3->fix();
+ L1->fix();
+// L2->fix();
+// L3->fix();
+// L4->fix();
+
+ auto p1 = F1->getP()->getState();
+ auto p2 = F2->getP()->getState();
+ auto p3 = F3->getP()->getState();
+ auto q1 = F1->getO()->getState();
+ auto q2 = F2->getO()->getState();
+ auto q3 = F3->getO()->getState();
+ auto l1 = L1->getP()->getState();
+ auto l2 = L2->getP()->getState();
+ auto l3 = L3->getP()->getState();
+ auto l4 = L4->getP()->getState();
+
+
+ std::string report = ceres_manager->solve(wolf::SolverManager::ReportVerbosity::FULL);
+ std::cout << report << std::endl;
+
+ //
+ ASSERT_MATRIX_APPROX(c11->expectation(), f11->getMeasurement(), 0.1); // 0.1 pixel error allowed
+ ASSERT_MATRIX_APPROX(c12->expectation(), f12->getMeasurement(), 0.1);
+ ASSERT_MATRIX_APPROX(c13->expectation(), f13->getMeasurement(), 0.1);
+ ASSERT_MATRIX_APPROX(c14->expectation(), f14->getMeasurement(), 0.1);
+
+ ASSERT_MATRIX_APPROX(F1->getP()->getState(), p1, 1e-6);
+ ASSERT_MATRIX_APPROX(F2->getP()->getState(), p2, 1e-6);
+ ASSERT_MATRIX_APPROX(F3->getP()->getState(), p3, 1e-6);
+ ASSERT_MATRIX_APPROX(F1->getO()->getState(), q1, 1e-6);
+ ASSERT_MATRIX_APPROX(F2->getO()->getState(), q2, 1e-6);
+ ASSERT_MATRIX_APPROX(F3->getO()->getState(), q3, 1e-6);
+
+ ASSERT_MATRIX_APPROX(L1->getP()->getState(), l1, 1e-6);
+ ASSERT_MATRIX_APPROX(L2->getP()->getState(), l2, 1e-6);
+ ASSERT_MATRIX_APPROX(L3->getP()->getState(), l3, 1e-6);
+ ASSERT_MATRIX_APPROX(L4->getP()->getState(), l4, 1e-6);
+
+ // perturb states
+
+ // sensor is not obvservable: comment all perturbations and unfixes
+ // S->getP()->fix();
+ // S->getP()->unfix();
+ // S->getP()->setState(S->getP()->getState() + 0.1*Vector3s::Random());
+ // S->getO()->fix();
+ // S->getO()->unfix();
+ // S->getO()->setState((Quaternions(S->getO()->getState().data()) * exp_q(0.1*Vector3s::Random())).coeffs());
+
+ // kfs
+ for (auto kf : problem->getTrajectory()->getFrameList())
+ {
+ if (kf == F1) continue;
+
+ kf->getP()->setState(kf->getP()->getState() + 0.5*Vector3s::Random());
+ kf->getO()->setState((Quaternions(kf->getO()->getState().data()) * exp_q(0.5*Vector3s::Random())).coeffs());
+ }
+
+ // lmks
+ for (auto lmk : problem->getMap()->getLandmarkList())
+ {
+ if (lmk == L1) continue;
+
+ lmk->getP()->setState(lmk->getP()->getState() + 0.5*Vector4s::Random());
+ }
+
+ // solve again
+ report = ceres_manager->solve(wolf::SolverManager::ReportVerbosity::FULL);
+ std::cout << report << std::endl;
+
+ problem->print(1,0,1,0);
+ for (auto lmk : problem->getMap()->getLandmarkList())
+ {
+ std::cout << "L" << lmk->id()<< ": " << std::static_pointer_cast<LandmarkHP>(lmk)->point().transpose() << std::endl;
+ }
+
+
+ // This test checks 3 DoF (3DoF are observable).
+ ASSERT_MATRIX_APPROX(F1->getP()->getState(), p1, 1e-6);
+ ASSERT_MATRIX_APPROX(F2->getP()->getState(), p2, 1e-6);
+ ASSERT_MATRIX_APPROX(F3->getP()->getState(), p3, 1e-6);
+ ASSERT_QUATERNION_APPROX(Quaternions(F1->getO()->getState().data()), Quaternions(q1.data()), 1e-6);
+ ASSERT_QUATERNION_APPROX(Quaternions(F2->getO()->getState().data()), Quaternions(q2.data()), 1e-6);
+ ASSERT_QUATERNION_APPROX(Quaternions(F3->getO()->getState().data()), Quaternions(q3.data()), 1e-6);
+
+ ASSERT_MATRIX_APPROX(L1->point(), lmkHP1.head<3>()/lmkHP1(3), 1e-6);
+ ASSERT_MATRIX_APPROX(L2->point(), lmkHP2.head<3>()/lmkHP2(3), 1e-6);
+ ASSERT_MATRIX_APPROX(L3->point(), lmkHP3.head<3>()/lmkHP3(3), 1e-6);
+ ASSERT_MATRIX_APPROX(L4->point(), lmkHP4.head<3>()/lmkHP4(3), 1e-6);
+
+ ASSERT_MATRIX_APPROX(c11->expectation(), f11->getMeasurement(), 0.1); // 0.1 pixel error allowed
+ ASSERT_MATRIX_APPROX(c12->expectation(), f12->getMeasurement(), 0.1);
+ ASSERT_MATRIX_APPROX(c13->expectation(), f13->getMeasurement(), 0.1);
+ ASSERT_MATRIX_APPROX(c14->expectation(), f14->getMeasurement(), 0.1);
+
+
+}
+
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ ::testing::GTEST_FLAG(filter) = "FactorPixelHPTest.testSolveBundleAdjustment*";
+
+ return RUN_ALL_TESTS();
+}
+
diff --git a/test/gtest_processor_bundle_adjustment.cpp b/test/gtest_processor_bundle_adjustment.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8fe8bfb723b170d48512c62a558d05f18e08de6d
--- /dev/null
+++ b/test/gtest_processor_bundle_adjustment.cpp
@@ -0,0 +1,358 @@
+
+// this plugin includes
+#include "vision/sensor/sensor_camera.h"
+#include "vision/processor/processor_bundle_adjustment.h"
+#include "vision/factor/factor_pixelHP.h"
+#include "vision/landmark/landmark_HP.h"
+#include "vision/capture/capture_image.h"
+#include "vision/internal/config.h"
+
+
+// wolf includes
+#include <core/utils/utils_gtest.h>
+#include <core/sensor/sensor_factory.h>
+
+// Vision utils includes
+#include <vision_utils/vision_utils.h>
+#include <vision_utils/sensors.h>
+#include <vision_utils/common_class/buffer.h>
+#include <vision_utils/common_class/frame.h>
+
+using namespace wolf;
+
+std::string wolf_vision_root = _WOLF_VISION_ROOT_DIR;
+
+class ProcessorBundleAdjustmentDummy : public ProcessorBundleAdjustment
+{
+ public:
+
+ ProcessorBundleAdjustmentDummy(ProcessorParamsBundleAdjustmentPtr& _params_bundle_adjustment):ProcessorBundleAdjustment(_params_bundle_adjustment){}
+
+ void setLast(CaptureImagePtr _last_ptr)
+ {
+ last_ptr_ = _last_ptr;
+ capture_image_last_ = _last_ptr;
+ }
+ void setInc(CaptureImagePtr _incoming_ptr)
+ {
+ incoming_ptr_ = _incoming_ptr;
+ capture_image_incoming_ = _incoming_ptr;
+ }
+
+ std::map<size_t, LandmarkBasePtr> getLandmarkMap()
+ {
+ return lmk_track_map_;
+ }
+
+ unsigned int getProcessKnown()
+ {
+ return this->processKnown();
+ }
+
+ unsigned int getProcessNew(const int& _max_new_features)
+ {
+ return this->processNew(_max_new_features);
+ }
+
+ TrackMatrix getTrackMatrix()
+ {
+ return track_matrix_;
+ }
+
+ FeatureBasePtrList getKnownFeaturesIncoming()
+ {
+ return known_features_incoming_;
+ }
+ FeatureMatchMap getMatchesLastFromIncoming()
+ {
+ return matches_last_from_incoming_;
+ }
+
+ CaptureImagePtr createCaptureImage(std::string _path, SensorCameraPtr _sensor, bool detectAndDescript = false)
+ {
+ const Scalar time = 0.0;
+ TimeStamp ts(time);
+ CaptureImagePtr im = std::make_shared<CaptureImage>(ts, _sensor, cv::imread(_path));
+ if (detectAndDescript){
+ // Detect KeyPoints
+ im->keypoints_ = det_ptr_->detect(im->getImage());
+ // Compute Descriptors
+ im->descriptors_ = des_ptr_->getDescriptor(im->getImage(), im->keypoints_);
+ // Create and fill incoming grid
+ im->grid_features_ = std::make_shared<vision_utils::FeatureIdxGrid>(im->getImage().rows, im->getImage().cols, params_bundle_adjustment_->n_cells_v, params_bundle_adjustment_->n_cells_h);
+ im->grid_features_->insert(im->keypoints_);
+ }
+ return im;
+ }
+};
+
+// Use the following in case you want to initialize tests with predefines variables or methods.
+//class ProcessorBundleAdjustment_class : public testing::demo{
+// public:
+// virtual void SetUp()
+// {
+// }
+//};
+
+TEST(ProcessorBundleAdjustment, installProcessor)
+{
+ // Wolf problem
+ ProblemPtr problem = Problem::create("PO", 3);
+
+ // Install camera
+ IntrinsicsCameraPtr intr = std::make_shared<IntrinsicsCamera>(); // TODO init params or read from YAML
+ intr->width = 640;
+ intr->height = 480;
+ auto sens_cam = problem->installSensor("CAMERA", "camera", (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), intr);
+
+ // Install processor
+ ProcessorParamsBundleAdjustmentPtr params = std::make_shared<ProcessorParamsBundleAdjustment>();
+ params->delete_ambiguities = true;
+ params->yaml_file_params_vision_utils = wolf_vision_root + "/demos/processor_bundle_adjustment_vision_utils.yaml";
+ params->pixel_noise_std = 1.0;
+ params->min_track_length_for_factor = 3;
+ params->voting_active = true;
+ params->max_new_features = 5;
+ auto proc = problem->installProcessor("TRACKER BUNDLE ADJUSTMENT", "processor", sens_cam, params);
+
+ std::cout << "sensor & processor created and added to wolf problem" << std::endl;
+
+ ASSERT_EQ(proc->getProblem(), problem);
+ ASSERT_TRUE(problem->check(0));
+}
+
+TEST(ProcessorBundleAdjustment, preProcess)
+{
+ // Create processor
+ ProcessorParamsBundleAdjustmentPtr params = std::make_shared<ProcessorParamsBundleAdjustment>();
+ params->delete_ambiguities = true;
+ params->yaml_file_params_vision_utils = wolf_vision_root + "/demos/processor_bundle_adjustment_vision_utils.yaml";
+ params->pixel_noise_std = 1.0;
+ params->min_track_length_for_factor = 3;
+ params->voting_active = true;
+ params->max_new_features = 5;
+ params->n_cells_h = 50;
+ params-> n_cells_v = 50;
+ params->min_response_new_feature = 50;
+
+ auto proc_dummy = std::make_shared<ProcessorBundleAdjustmentDummy>(params);
+
+ // Put an image on incoming_ptr_
+ CaptureImagePtr image_inc_ptr = proc_dummy->createCaptureImage(wolf_vision_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", nullptr);
+ proc_dummy->setInc(image_inc_ptr);
+ // Put an image on last_ptr_
+ CaptureImagePtr image_last_ptr = proc_dummy->createCaptureImage(wolf_vision_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", nullptr, true);
+ proc_dummy->setLast(image_last_ptr);
+ // demo dpreProcess
+ proc_dummy->preProcess();
+ CaptureImagePtr inc = std::static_pointer_cast<CaptureImage>(proc_dummy->getIncoming());
+ CaptureImagePtr last = std::static_pointer_cast<CaptureImage>(proc_dummy->getLast());
+ ASSERT_EQ(inc->keypoints_.size(), last->keypoints_.size());
+ ASSERT_NE(inc->keypoints_.size(), 0);
+}
+
+TEST(ProcessorBundleAdjustment, detectNewFeatures)
+{
+ // Create processor
+ ProcessorParamsBundleAdjustmentPtr params = std::make_shared<ProcessorParamsBundleAdjustment>();
+ params->delete_ambiguities = true;
+ params->yaml_file_params_vision_utils = wolf_vision_root + "/demos/processor_bundle_adjustment_vision_utils.yaml";
+ params->pixel_noise_std = 1.0;
+ params->min_track_length_for_factor = 3;
+ params->voting_active = true;
+ params->max_new_features = 5;
+ params->n_cells_h = 20;
+ params-> n_cells_v = 15;
+ params->min_response_new_feature = 0;
+ auto proc_dummy = std::make_shared<ProcessorBundleAdjustmentDummy>(params);
+
+ FeatureBasePtrList feat_list = std::list<FeatureBasePtr>();
+
+ // Put an image on last_ptr_
+ CaptureImagePtr image_last_ptr = proc_dummy->createCaptureImage(wolf_vision_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", nullptr, true);
+ ASSERT_NE(image_last_ptr->keypoints_.size(), 0);
+ proc_dummy->setLast(image_last_ptr);
+
+ // demo detectNewFeatures
+ unsigned int num = proc_dummy->detectNewFeatures(params->max_new_features, image_last_ptr, feat_list);
+ ASSERT_EQ(num, 0);
+
+ // Put an image on incoming_ptr_
+ CaptureImagePtr image_inc_ptr = proc_dummy->createCaptureImage(wolf_vision_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", nullptr);
+ proc_dummy->setInc(image_inc_ptr);
+
+ // demo detectNewFeatures
+ unsigned int num2 = proc_dummy->detectNewFeatures(params->max_new_features, image_last_ptr, feat_list);
+ ASSERT_EQ(num2, 0);
+
+ // preProcess Incoming to fill last_ptr_ map_index_to_next_
+ proc_dummy->preProcess();
+ CaptureImagePtr last = std::static_pointer_cast<CaptureImage>(proc_dummy->getLast());
+ ASSERT_NE(last->map_index_to_next_.size(),0);
+
+ // demo detectNewFeatures
+ unsigned int num3 = proc_dummy->detectNewFeatures(params->max_new_features, image_last_ptr, feat_list);
+ ASSERT_LE(num3, params->max_new_features);
+}
+
+TEST(ProcessorBundleAdjustment, trackFeatures)
+{
+ // Create processor
+ ProcessorParamsBundleAdjustmentPtr params = std::make_shared<ProcessorParamsBundleAdjustment>();
+ params->delete_ambiguities = true;
+ params->yaml_file_params_vision_utils = wolf_vision_root + "/demos/processor_bundle_adjustment_vision_utils.yaml";
+ params->pixel_noise_std = 1.0;
+ params->min_track_length_for_factor = 3;
+ params->voting_active = true;
+ params->max_new_features = 5;
+ params->n_cells_h = 50;
+ params-> n_cells_v = 50;
+ params->min_response_new_feature = 50;
+ auto proc_dummy = std::make_shared<ProcessorBundleAdjustmentDummy>(params);
+
+ //fill feat_last list
+ FeatureBasePtrList feat_list = std::list<FeatureBasePtr>();
+ CaptureImagePtr image_last_ptr = proc_dummy->createCaptureImage(wolf_vision_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", nullptr, true);
+ proc_dummy->setLast(image_last_ptr);
+ CaptureImagePtr image_inc_ptr = proc_dummy->createCaptureImage(wolf_vision_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", nullptr);
+ proc_dummy->setInc(image_inc_ptr);
+ proc_dummy->preProcess();
+ CaptureImagePtr last = std::static_pointer_cast<CaptureImage>(proc_dummy->getLast());
+ proc_dummy->detectNewFeatures(params->max_new_features, last, feat_list);
+ //demo trackFeatures
+ FeatureBasePtrList feat_list_out = std::list<FeatureBasePtr>();
+ FeatureMatchMap feat_correspondance = FeatureMatchMap();
+ proc_dummy->trackFeatures(feat_list, image_inc_ptr, feat_list_out, feat_correspondance);
+ ASSERT_EQ(feat_list.size(), feat_list_out.size());
+}
+
+
+TEST(ProcessorBundleAdjustment, establishFactors)
+{
+ std::cout << "EMPTY Test\n";
+}
+
+TEST(ProcessorBundleAdjustment, emplaceLandmark)
+{
+ //Build problem
+ ProblemPtr problem_ptr = Problem::create("PO", 3);
+
+ // Install sensor
+ IntrinsicsCameraPtr intr = std::make_shared<IntrinsicsCamera>();
+ intr->width = 640;
+ intr->height = 480;
+ auto sens_cam = problem_ptr->installSensor("CAMERA", "camera", (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), intr);
+ SensorCameraPtr camera = std::static_pointer_cast<SensorCamera>(sens_cam);
+ // Install processor
+ ProcessorParamsBundleAdjustmentPtr params = std::make_shared<ProcessorParamsBundleAdjustment>();
+ params->delete_ambiguities = true;
+ params->yaml_file_params_vision_utils = wolf_vision_root + "/demos/processor_bundle_adjustment_vision_utils.yaml";
+ params->pixel_noise_std = 1.0;
+ params->min_track_length_for_factor = 3;
+ params->voting_active = true;
+ params->max_new_features = 5;
+ auto proc = problem_ptr->installProcessor("TRACKER BUNDLE ADJUSTMENT", "processor", sens_cam, params);
+ ProcessorBundleAdjustmentPtr proc_bundle_adj = std::static_pointer_cast<ProcessorBundleAdjustment>(proc);
+
+ //Frame
+ FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY, problem_ptr->zeroState(), TimeStamp(0));
+
+ // Capture, feature and factor
+ auto cap0 = std::static_pointer_cast<CaptureImage>(CaptureImage::emplace<CaptureImage>(frm0, TimeStamp(0), camera, cv::Mat::zeros(480,640, 1)));
+
+ cv::Point2f p = cv::Point2f(240, 320);
+ cv::KeyPoint kp = cv::KeyPoint(p, 32.0f);
+ cv::Mat des = cv::Mat::zeros(1,8, CV_8U);
+
+ FeaturePointImagePtr fea0 = std::make_shared<FeaturePointImage>(kp, 0, des, Eigen::Matrix2s::Identity()* pow(1, 2));
+ fea0->link(cap0);
+
+ ASSERT_TRUE(problem_ptr->check(0));
+
+ LandmarkBasePtr lmk = proc_bundle_adj->emplaceLandmark(fea0);
+ LandmarkHPPtr lmk_hp = std::static_pointer_cast<LandmarkHP>(lmk);
+
+ ASSERT_EQ(problem_ptr->getMap()->getLandmarkList().size(), 1);
+}
+
+
+TEST(ProcessorBundleAdjustment, process)
+{
+ // Wolf problem
+ ProblemPtr problem = Problem::create("PO", 3);
+
+ // Install camera
+ IntrinsicsCameraPtr intr = std::make_shared<IntrinsicsCamera>(); // TODO init params or read from YAML
+ intr->pinhole_model_raw = Eigen::Vector4s(0,0,1,1); //TODO: initialize
+ intr->width = 640;
+ intr->height = 480;
+ SensorCameraPtr sens_cam = std::static_pointer_cast<SensorCamera>(problem->installSensor("CAMERA", "camera", (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), intr));
+
+ // Install processor
+ ProcessorParamsBundleAdjustmentPtr params = std::make_shared<ProcessorParamsBundleAdjustment>();
+ params->delete_ambiguities = true;
+ params->yaml_file_params_vision_utils = wolf_vision_root + "/demos/processor_bundle_adjustment_vision_utils.yaml";
+ params->pixel_noise_std = 1.0;
+ params->min_track_length_for_factor = 3;
+ params->voting_active = true;
+ params->max_new_features = 5;
+ params->n_cells_h = 50;
+ params-> n_cells_v = 50;
+ params->min_response_new_feature = 50;
+ auto proc = problem->installProcessor("TRACKER BUNDLE ADJUSTMENT", "processor", sens_cam, params);
+ auto proc_dummy = std::static_pointer_cast<ProcessorBundleAdjustmentDummy>(proc);
+
+ //1ST TIME
+ CaptureImagePtr image_inc_ptr = proc_dummy->createCaptureImage(wolf_vision_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", sens_cam);
+ image_inc_ptr->process();
+// proc_dummy->process(image_inc_ptr);
+ ASSERT_EQ(proc_dummy->getTrackMatrix().numTracks(), 0);
+ ASSERT_EQ(problem->getMap()->getLandmarkList().size(), 0);
+
+ //2ND TIME
+ CaptureImagePtr image_inc_ptr2 = proc_dummy->createCaptureImage(wolf_vision_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", sens_cam);
+// proc_dummy->setInc(image_inc_ptr2);
+// proc_dummy->preProcess();
+// proc_dummy->getProcessKnown();
+// proc_dummy->getProcessNew(params->max_new_features);
+// proc_dummy->establishFactors();
+ image_inc_ptr2->process();
+// proc_dummy->process(image_inc_ptr2);
+ ASSERT_LE(proc_dummy->getTrackMatrix().numTracks(), params->max_new_features);
+ ASSERT_NE(problem->getMap(), nullptr);
+ ASSERT_EQ(problem->getMap()->getLandmarkList().empty(), true);
+
+
+ //3RD TIME -- RUNNING
+ CaptureImagePtr image_inc_ptr3 = proc_dummy->createCaptureImage(wolf_vision_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", sens_cam);
+ assert(proc_dummy->getOrigin()!=nullptr);
+ assert(proc_dummy->getLast()!= nullptr && proc_dummy->getLast()!=proc_dummy->getOrigin());
+ image_inc_ptr3->process();
+// proc_dummy->process(image_inc_ptr3);
+ ASSERT_LE(proc_dummy->getTrackMatrix().numTracks(), params->max_new_features);
+
+ //4TH TIME -- RUNNING
+ CaptureImagePtr image_inc_ptr4 = proc_dummy->createCaptureImage(wolf_vision_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", sens_cam);
+ image_inc_ptr4->process();
+// proc_dummy->process(image_inc_ptr4);
+ ASSERT_LE(image_inc_ptr4->getFeatureList().size(), params->max_new_features);
+
+}
+
+
+TEST(ProcessorBundleAdjustment, processKnown)
+{
+ std::cout << "EMPTY Test\n";
+}
+
+TEST(ProcessorBundleAdjustment, processNew)
+{
+ std::cout << "EMPTY Test\n";
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
+
diff --git a/test/gtest_sensor_camera.cpp b/test/gtest_sensor_camera.cpp
index 9d66e50b7ec0eb32424cc996d293feef56d0f8f5..7cce55e0d6bb05a5455533de02cb5d5b7eaca74a 100644
--- a/test/gtest_sensor_camera.cpp
+++ b/test/gtest_sensor_camera.cpp
@@ -115,6 +115,30 @@ TEST(SensorCamera, create)
ASSERT_EQ(cam->getImgWidth(), 640);
}
+TEST(SensorCamera, install)
+{
+ Eigen::VectorXs extrinsics(7); extrinsics << 0,0,0, 0,0,0,1;
+ IntrinsicsCameraPtr params = std::make_shared<IntrinsicsCamera>();
+ params->width = 640;
+ params->height = 480;
+ params->pinhole_model_raw << 321, 241, 321, 321;
+ params->pinhole_model_rectified << 320, 240, 320, 320;
+ params->distortion = Eigen::Vector3s( 0, 0, 0 );
+
+ // Wolf problem
+ ProblemPtr problem = Problem::create("PO 3D",3);
+
+ // Install camera
+ auto sen = problem->installSensor("CAMERA", "camera", extrinsics, params);
+
+ ASSERT_NE(sen, nullptr);
+
+ SensorCameraPtr cam = std::static_pointer_cast<SensorCamera>(sen);
+
+ ASSERT_NE(cam, nullptr);
+ ASSERT_EQ(cam->getImgWidth(), 640);
+}
+
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
diff --git a/test/serialization/CMakeLists.txt b/test/serialization/CMakeLists.txt
deleted file mode 100644
index 6902132bd458245e2e45012662fdcf0b348b7d4a..0000000000000000000000000000000000000000
--- a/test/serialization/CMakeLists.txt
+++ /dev/null
@@ -1,4 +0,0 @@
-# cereal
-IF(cereal_FOUND)
- add_subdirectory(cereal)
-ENDIF(cereal_FOUND)
diff --git a/test/serialization/cereal/CMakeLists.txt b/test/serialization/cereal/CMakeLists.txt
deleted file mode 100644
index 733a36e25921c4f7c142d59884fb5c9b7dd9e4e4..0000000000000000000000000000000000000000
--- a/test/serialization/cereal/CMakeLists.txt
+++ /dev/null
@@ -1,43 +0,0 @@
-# NodeBase serialization class test
-wolf_add_gtest(gtest_cereal_serialization_node_base gtest_serialization_node_base.cpp)
-target_link_libraries(gtest_cereal_serialization_node_base ${PROJECT_NAME})
-
-wolf_add_gtest(gtest_cereal_serialization_local_parametrization
- gtest_serialization_local_parametrization.cpp)
-target_link_libraries(gtest_cereal_serialization_local_parametrization ${PROJECT_NAME})
-
-wolf_add_gtest(gtest_cereal_serialization_sensor_intrinsic_base
- gtest_serialization_sensor_intrinsic_base.cpp)
-target_link_libraries(gtest_cereal_serialization_sensor_intrinsic_base ${PROJECT_NAME})
-
-wolf_add_gtest(gtest_cereal_serialization_sensor_odom2d_intrinsic
- gtest_serialization_sensor_odom2d_intrinsic.cpp)
-target_link_libraries(gtest_cereal_serialization_sensor_odom2d_intrinsic ${PROJECT_NAME})
-
-wolf_add_gtest(gtest_cereal_serialization_save_load
- gtest_serialization_save_load.cpp)
-target_link_libraries(gtest_cereal_serialization_save_load ${PROJECT_NAME})
-
-wolf_add_gtest(gtest_cereal_serialization_processor_odom3d_params
- gtest_serialization_processor_odom3d_params.cpp)
-target_link_libraries(gtest_cereal_serialization_processor_odom3d_params ${PROJECT_NAME})
-
-wolf_add_gtest(gtest_cereal_serialization_processor_odom2d_params
- gtest_serialization_processor_odom2d_params.cpp)
-target_link_libraries(gtest_cereal_serialization_processor_odom2d_params ${PROJECT_NAME})
-
-wolf_add_gtest(gtest_cereal_serialization_time_stamp
- gtest_serialization_time_stamp.cpp)
-target_link_libraries(gtest_cereal_serialization_time_stamp ${PROJECT_NAME})
-
-wolf_add_gtest(gtest_cereal_serialization_eigen_core
- gtest_serialization_eigen_core.cpp)
-target_link_libraries(gtest_cereal_serialization_eigen_core ${PROJECT_NAME})
-
-wolf_add_gtest(gtest_cereal_serialization_eigen_geometry
- gtest_serialization_eigen_geometry.cpp)
-target_link_libraries(gtest_cereal_serialization_eigen_geometry ${PROJECT_NAME})
-
-wolf_add_gtest(gtest_cereal_serialization_eigen_sparse
- gtest_serialization_eigen_sparse.cpp)
-target_link_libraries(gtest_cereal_serialization_eigen_sparse ${PROJECT_NAME})
diff --git a/test/serialization/cereal/gtest_serialization_eigen_core.cpp b/test/serialization/cereal/gtest_serialization_eigen_core.cpp
deleted file mode 100644
index 28859dd7a5b7db706e041489341bf4c010eb126e..0000000000000000000000000000000000000000
--- a/test/serialization/cereal/gtest_serialization_eigen_core.cpp
+++ /dev/null
@@ -1,92 +0,0 @@
-/*
- * gtest_intrinsics_odom2d_serialization.cpp
- *
- * Created on: Jul 16, 2017
- * Author: Jeremie Deray
- */
-
-#include "../../utils_gtest.h"
-
-#include "../../../serialization/cereal/serialization_eigen_core.h"
-
-#include "../../../serialization/cereal/io.h"
-
-#include <cereal/types/memory.hpp>
-#include <fstream>
-
-class WolfTestCerealSerializationEigen : public testing::Test
-{
-public:
-
- WolfTestCerealSerializationEigen()
- {
- nb_ = f_mat_t::Random();
-
- dnb_ = d_mat_t::Random(10, 10);
- }
-
- const std::string path_to_io = "/tmp/";
- const std::string filename = "serialization_eigen";
-
- const std::vector<std::string> exts = {".bin"/*, ".xml", ".json"*/};
-
- using f_mat_t = Eigen::Matrix<double, 5, 5>;
-
- using d_mat_t = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>;
-
- f_mat_t nb_;
-
- d_mat_t dnb_;
-};
-
-TEST_F(WolfTestCerealSerializationEigen,
- CerealSerializationEigenFixedMat)
-{
- for (const auto ext : exts)
- {
- const std::string full_path = path_to_io + filename + ext;
-
- ASSERT_NO_THROW( wolf::save( full_path, nb_ ) )
- << "Failed on saving " << full_path;
-
- WolfTestCerealSerializationEigen::f_mat_t nb_load;
-
- ASSERT_NO_THROW( wolf::load( full_path, nb_load ) )
- << "Failed on loading " << full_path;
-
- EXPECT_EQ(nb_load, nb_) << full_path;
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationEigen::"
- "CerealSerializationEigenFixedMat !\n");
-}
-
-TEST_F(WolfTestCerealSerializationEigen,
- CerealSerializationEigenDynamicMat)
-{
- for (const auto ext : exts)
- {
- const std::string full_path = path_to_io + filename + ext;
-
- ASSERT_NO_THROW( wolf::save( full_path, dnb_ ) )
- << "Failed on saving " << full_path;
-
- WolfTestCerealSerializationEigen::d_mat_t dnb_load;
-
- ASSERT_NO_THROW( wolf::load( full_path, dnb_load ) )
- << "Failed on loading " << full_path;
-
- EXPECT_EQ(dnb_load, dnb_) << full_path;
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationEigen::"
- "CerealSerializationEigenDynamicMat !\n");
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
diff --git a/test/serialization/cereal/gtest_serialization_eigen_geometry.cpp b/test/serialization/cereal/gtest_serialization_eigen_geometry.cpp
deleted file mode 100644
index 87cafb4d5f171bafb74670348cd562ad49b1d306..0000000000000000000000000000000000000000
--- a/test/serialization/cereal/gtest_serialization_eigen_geometry.cpp
+++ /dev/null
@@ -1,72 +0,0 @@
-/*
- * gtest_intrinsics_odom2d_serialization.cpp
- *
- * Created on: Jul 16, 2017
- * Author: Jeremie Deray
- */
-
-#include "core/common/wolf.h"
-#include "../../utils_gtest.h"
-
-#include "../../../serialization/cereal/serialization_eigen_geometry.h"
-
-#include "../../../serialization/cereal/io.h"
-
-#include <cereal/types/memory.hpp>
-#include <fstream>
-
-class WolfTestCerealSerializationEigenGeo : public testing::Test
-{
-public:
-
- WolfTestCerealSerializationEigenGeo()
- {
- iso_2d_ = Eigen::Isometry2d(Eigen::Rotation2Dd(0.17));
- iso_2d_.translation() << 0.5, 1.8;
-
- q_d_ = Eigen::Vector4d().setRandom().normalized();
-
- iso_3d_ = Eigen::Isometry3d(q_d_);
- iso_3d_.translation() << -7.245, +3.88, 0.0001;
- }
-
- const std::string path_to_io = "/tmp/";
- const std::string filename = "serialization_eigen_geo";
-
- const std::vector<std::string> exts = {".bin"/*, ".xml", ".json"*/};
-
- Eigen::Isometry2d iso_2d_;
- Eigen::Isometry3d iso_3d_;
- Eigen::Quaterniond q_d_;
-};
-
-TEST_F(WolfTestCerealSerializationEigenGeo,
- CerealSerializationEigenIso2d)
-{
- for (const auto ext : exts)
- {
- const std::string full_path = path_to_io + filename + ext;
-
- ASSERT_NO_THROW( wolf::save( full_path, iso_2d_, iso_3d_, q_d_) )
- << "Failed on saving " << full_path;
-
- Eigen::Isometry2d iso_2d_loaded;
- Eigen::Isometry3d iso_3d_loaded;
- Eigen::Quaterniond q_d_loaded;
-
- ASSERT_NO_THROW( wolf::load( full_path, iso_2d_loaded, iso_3d_loaded, q_d_loaded) )
- << "Failed on loading " << full_path;
-
- ASSERT_MATRIX_APPROX(iso_2d_.matrix(), iso_2d_loaded.matrix(), wolf::Constants::EPS);
- ASSERT_MATRIX_APPROX(iso_3d_.matrix(), iso_3d_loaded.matrix(), wolf::Constants::EPS);
- ASSERT_MATRIX_APPROX(q_d_.coeffs(), q_d_loaded.coeffs(), wolf::Constants::EPS);
- }
-
- PRINT_TEST_FINISHED;
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
diff --git a/test/serialization/cereal/gtest_serialization_eigen_sparse.cpp b/test/serialization/cereal/gtest_serialization_eigen_sparse.cpp
deleted file mode 100644
index 0b803afdd30778a58b9536343c5ba5248ec8a148..0000000000000000000000000000000000000000
--- a/test/serialization/cereal/gtest_serialization_eigen_sparse.cpp
+++ /dev/null
@@ -1,114 +0,0 @@
-/*
- * gtest_intrinsics_odom2d_serialization.cpp
- *
- * Created on: Jul 16, 2017
- * Author: Jeremie Deray
- */
-
-#include "core/common/wolf.h"
-#include "../../utils_gtest.h"
-
-#include "../../../serialization/cereal/serialization_eigen_sparse.h"
-
-#include "../../../serialization/cereal/io.h"
-
-#include <fstream>
-
-class WolfTestCerealSerializationEigenSparse : public testing::Test
-{
-public:
-
- using triplet_t = Eigen::Triplet<double>;
- using sparse_mat_t = Eigen::SparseMatrix<double>;
-
- WolfTestCerealSerializationEigenSparse()
- {
- triplet_list_.reserve(10);
-
- for(int i=0; i<10; ++i)
- triplet_list_.emplace_back(i,i,i*5);
-
- m_.resize(10, 10);
- m_.setFromTriplets(triplet_list_.begin(), triplet_list_.end());
- }
-
- const std::string path_to_io = "/tmp/";
- const std::string filename_t = "serialization_eigen_triplet";
- const std::string filename_m = "serialization_eigen_sparse";
-
- const std::vector<std::string> exts = {".bin", ".xml", ".json"};
-
- triplet_t t_ = Eigen::Triplet<double>(1, 2, 5.5);
-
- std::vector<triplet_t> triplet_list_;
- Eigen::SparseMatrix<double> m_;
-};
-
-TEST_F(WolfTestCerealSerializationEigenSparse,
- CerealSerializationEigenTriplet)
-{
- for (const auto ext : exts)
- {
- const std::string full_path = path_to_io + filename_t + ext;
-
- ASSERT_NO_THROW( wolf::save( full_path, t_) )
- << "Failed on saving " << full_path;
-
- triplet_t t;
-
- ASSERT_NO_THROW( wolf::load( full_path, t) )
- << "Failed on loading " << full_path;
-
- ASSERT_EQ(t_.row(), t.row());
- ASSERT_EQ(t_.col(), t.col());
- ASSERT_EQ(t_.value(), t.value());
- }
-
- PRINT_TEST_FINISHED;
-}
-
-TEST_F(WolfTestCerealSerializationEigenSparse,
- CerealSerializationEigenSparseMatrix)
-{
- for (const auto ext : exts)
- {
- const std::string full_path = path_to_io + filename_m + ext;
-
- ASSERT_NO_THROW( wolf::save( full_path, m_) )
- << "Failed on saving " << full_path;
-
- sparse_mat_t m;
-
- ASSERT_NO_THROW( wolf::load( full_path, m) )
- << "Failed on loading " << full_path;
-
- ASSERT_EQ(m_.rows(), m.rows());
- ASSERT_EQ(m_.cols(), m.cols());
-
- std::vector<triplet_t> triplet_list;
- triplet_list.reserve(10);
-
- for (int k=0; k<m.outerSize(); ++k)
- for (sparse_mat_t::InnerIterator it(m, k); it; ++it)
- {
- triplet_list.emplace_back(it.row(), it.col(), it.value());
- }
-
- ASSERT_EQ(triplet_list_.size(), triplet_list.size());
-
- for (int i=0; i<triplet_list_.size(); ++i)
- {
- ASSERT_EQ(triplet_list_[i].row(), triplet_list[i].row());
- ASSERT_EQ(triplet_list_[i].col(), triplet_list[i].col());
- ASSERT_EQ(triplet_list_[i].value(), triplet_list[i].value());
- }
- }
-
- PRINT_TEST_FINISHED;
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
diff --git a/test/serialization/cereal/gtest_serialization_local_parametrization.cpp b/test/serialization/cereal/gtest_serialization_local_parametrization.cpp
deleted file mode 100644
index 3df34a27aa683fb5099cb8c108a25fe0f473f31b..0000000000000000000000000000000000000000
--- a/test/serialization/cereal/gtest_serialization_local_parametrization.cpp
+++ /dev/null
@@ -1,571 +0,0 @@
-/*
- * gtest_node_base_serialization.cpp
- *
- * Created on: Jul 16, 2017
- * Author: Jeremie Deray
- */
-
-#include "../../utils_gtest.h"
-
-#include "../../../serialization/cereal/serialization_local_parametrization_quaternion.h"
-#include "../../../serialization/cereal/serialization_local_parametrization_homogeneous.h"
-
-#include "../../../serialization/cereal/archives.h"
-
-#include <cereal/types/memory.hpp>
-#include <fstream>
-
-///////////////////////////////////////
-/// LocalParametrizationHomogeneous ///
-///////////////////////////////////////
-
-const std::string path_to_io = "/tmp/";
-
-TEST(TestSerialization, SerializationLocalParametrizationHomogeneousXML)
-{
- {
- std::ofstream os(path_to_io + "local_parametrization_serialization.xml");
- cereal::XMLOutputArchive archive(os);
-
- wolf::LocalParametrizationHomogeneous local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
- }
-
- {
- std::ifstream is(path_to_io + "local_parametrization_serialization.xml");
- cereal::XMLInputArchive archive(is);
-
- wolf::LocalParametrizationHomogeneous local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
-
- ASSERT_EQ(local_param_h.getGlobalSize(), 4);
- ASSERT_EQ(local_param_h.getLocalSize(), 3);
- }
-
- PRINTF("All good at TestSerialization::SerializationLocalParametrizationHomogeneousXML !\n");
-}
-
-TEST(TestSerialization, SerializationLocalParametrizationHomogeneousPtrXML)
-{
- using LocalParametrizationPtr = std::shared_ptr<wolf::LocalParametrizationBase> ;
-
- {
- std::ofstream os(path_to_io + "local_parametrization_ptr_serialization.xml");
- cereal::XMLOutputArchive archive(os);
-
- LocalParametrizationPtr local_param_h =
- std::make_shared<wolf::LocalParametrizationHomogeneous>();
-
- ASSERT_NO_THROW( archive( local_param_h ) );
- }
-
- {
- std::ifstream is(path_to_io + "local_parametrization_ptr_serialization.xml");
- cereal::XMLInputArchive archive(is);
-
- LocalParametrizationPtr local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
-
- ASSERT_EQ(local_param_h->getGlobalSize(), 4);
- ASSERT_EQ(local_param_h->getLocalSize(), 3);
-
- ASSERT_TRUE(
- std::dynamic_pointer_cast<
- wolf::LocalParametrizationHomogeneous>(local_param_h) != nullptr);
- }
-
- PRINTF("All good at TestSerialization::SerializationLocalParametrizationHomogeneousPtrXML !\n");
-}
-
-TEST(TestSerialization, SerializationLocalParametrizationHomogeneousJSON)
-{
- {
- std::ofstream os(path_to_io + "local_parametrization_serialization.json");
- cereal::JSONOutputArchive archive(os);
-
- wolf::LocalParametrizationHomogeneous local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
- }
-
- {
- std::ifstream is(path_to_io + "local_parametrization_serialization.json");
- cereal::JSONInputArchive archive(is);
-
- wolf::LocalParametrizationHomogeneous local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
-
- ASSERT_EQ(local_param_h.getGlobalSize(), 4);
- ASSERT_EQ(local_param_h.getLocalSize(), 3);
- }
-
- PRINTF("All good at TestSerialization::SerializationLocalParametrizationHomogeneousJSON !\n");
-}
-
-TEST(TestSerialization, SerializationLocalParametrizationHomogeneousPtrJSON)
-{
- using LocalParametrizationPtr = std::shared_ptr<wolf::LocalParametrizationBase> ;
-
- {
- std::ofstream os(path_to_io + "local_parametrization_ptr_serialization.json");
- cereal::JSONOutputArchive archive(os);
-
- LocalParametrizationPtr local_param_h =
- std::make_shared<wolf::LocalParametrizationHomogeneous>();
-
- ASSERT_NO_THROW( archive( local_param_h ) );
- }
-
- {
- std::ifstream is(path_to_io + "local_parametrization_ptr_serialization.json");
- cereal::JSONInputArchive archive(is);
-
- LocalParametrizationPtr local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
-
- ASSERT_EQ(local_param_h->getGlobalSize(), 4);
- ASSERT_EQ(local_param_h->getLocalSize(), 3);
-
- ASSERT_TRUE(
- std::dynamic_pointer_cast<
- wolf::LocalParametrizationHomogeneous>(local_param_h) != nullptr);
- }
-
- PRINTF("All good at TestSerialization::SerializationLocalParametrizationHomogeneousPtrJSON !\n");
-}
-
-TEST(TestSerialization, SerializationLocalParametrizationHomogeneousBIN)
-{
- {
- std::ofstream os(path_to_io + "local_parametrization_serialization.bin");
- cereal::BinaryOutputArchive archive(os);
-
- wolf::LocalParametrizationHomogeneous local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
- }
-
- {
- std::ifstream is(path_to_io + "local_parametrization_serialization.bin");
- cereal::BinaryInputArchive archive(is);
-
- wolf::LocalParametrizationHomogeneous local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
-
- ASSERT_EQ(local_param_h.getGlobalSize(), 4);
- ASSERT_EQ(local_param_h.getLocalSize(), 3);
- }
-
- PRINTF("All good at TestSerialization::SerializationLocalParametrizationHomogeneousBIN !\n");
-}
-
-TEST(TestSerialization, SerializationLocalParametrizationHomogeneousPtrBIN)
-{
- using LocalParametrizationPtr = std::shared_ptr<wolf::LocalParametrizationBase> ;
-
- {
- std::ofstream os(path_to_io + "local_parametrization_ptr_serialization.bin");
- cereal::BinaryOutputArchive archive(os);
-
- LocalParametrizationPtr local_param_h =
- std::make_shared<wolf::LocalParametrizationHomogeneous>();
-
- ASSERT_NO_THROW( archive( local_param_h ) );
- }
-
- {
- std::ifstream is(path_to_io + "local_parametrization_ptr_serialization.bin");
- cereal::BinaryInputArchive archive(is);
-
- LocalParametrizationPtr local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
-
- ASSERT_EQ(local_param_h->getGlobalSize(), 4);
- ASSERT_EQ(local_param_h->getLocalSize(), 3);
-
- ASSERT_TRUE(
- std::dynamic_pointer_cast<
- wolf::LocalParametrizationHomogeneous>(local_param_h) != nullptr);
- }
-
- PRINTF("All good at TestSerialization::SerializationLocalParametrizationHomogeneousPtrBIN !\n");
-}
-
-//////////////////////////////////////
-/// LocalParametrizationQuaternion ///
-//////////////////////////////////////
-
-//////////////////////////////////////
-/// LOCAL ///
-//////////////////////////////////////
-
-TEST(TestSerialization, SerializationLocalParametrizationQuaternionXML)
-{
- {
- std::ofstream os(path_to_io + "local_parametrization_quat_serialization.xml");
- cereal::XMLOutputArchive archive(os);
-
- wolf::LocalParametrizationQuaternionLocal local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
- }
-
- {
- std::ifstream is(path_to_io + "local_parametrization_quat_serialization.xml");
- cereal::XMLInputArchive archive(is);
-
- wolf::LocalParametrizationQuaternionLocal local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
-
- ASSERT_EQ(local_param_h.getGlobalSize(), 4);
- ASSERT_EQ(local_param_h.getLocalSize(), 3);
- }
-
- PRINTF("All good at TestSerialization::SerializationLocalParametrizationQuaternionXML !\n");
-}
-
-TEST(TestSerialization, SerializationLocalParametrizationQuaternionPtrXML)
-{
- using LocalParametrizationPtr = std::shared_ptr<wolf::LocalParametrizationBase> ;
-
- {
- std::ofstream os(path_to_io + "local_parametrization_quat_ptr_serialization.xml");
- cereal::XMLOutputArchive archive(os);
-
- LocalParametrizationPtr local_param_h =
- std::make_shared<wolf::LocalParametrizationQuaternionLocal>();
-
- ASSERT_NO_THROW( archive( local_param_h ) );
- }
-
- {
- std::ifstream is(path_to_io + "local_parametrization_quat_ptr_serialization.xml");
- cereal::XMLInputArchive archive(is);
-
- LocalParametrizationPtr local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
-
- ASSERT_EQ(local_param_h->getGlobalSize(), 4);
- ASSERT_EQ(local_param_h->getLocalSize(), 3);
-
- ASSERT_TRUE(
- std::dynamic_pointer_cast<
- wolf::LocalParametrizationQuaternionLocal>(local_param_h) != nullptr);
- }
-
- PRINTF("All good at TestSerialization::SerializationLocalParametrizationQuaternionPtrXML !\n");
-}
-
-TEST(TestSerialization, SerializationLocalParametrizationQuaternionJSON)
-{
- {
- std::ofstream os(path_to_io + "local_parametrization_quat_serialization.json");
- cereal::JSONOutputArchive archive(os);
-
- wolf::LocalParametrizationQuaternionLocal local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
- }
-
- {
- std::ifstream is(path_to_io + "local_parametrization_quat_serialization.json");
- cereal::JSONInputArchive archive(is);
-
- wolf::LocalParametrizationQuaternionLocal local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
-
- ASSERT_EQ(local_param_h.getGlobalSize(), 4);
- ASSERT_EQ(local_param_h.getLocalSize(), 3);
- }
-
- PRINTF("All good at TestSerialization::SerializationLocalParametrizationQuaternionJSON !\n");
-}
-
-TEST(TestSerialization, SerializationLocalParametrizationQuaternionPtrJSON)
-{
- using LocalParametrizationPtr = std::shared_ptr<wolf::LocalParametrizationBase> ;
-
- {
- std::ofstream os(path_to_io + "local_parametrization_quat_ptr_serialization.json");
- cereal::JSONOutputArchive archive(os);
-
- LocalParametrizationPtr local_param_h =
- std::make_shared<wolf::LocalParametrizationQuaternionLocal>();
-
- ASSERT_NO_THROW( archive( local_param_h ) );
- }
-
- {
- std::ifstream is(path_to_io + "local_parametrization_quat_ptr_serialization.json");
- cereal::JSONInputArchive archive(is);
-
- LocalParametrizationPtr local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
-
- ASSERT_EQ(local_param_h->getGlobalSize(), 4);
- ASSERT_EQ(local_param_h->getLocalSize(), 3);
-
- ASSERT_TRUE(
- std::dynamic_pointer_cast<
- wolf::LocalParametrizationQuaternionLocal>(local_param_h) != nullptr);
- }
-
- PRINTF("All good at TestSerialization::SerializationLocalParametrizationQuaternionPtrJSON !\n");
-}
-
-TEST(TestSerialization, SerializationLocalParametrizationQuaternionBIN)
-{
- {
- std::ofstream os(path_to_io + "local_parametrization_quat_serialization.bin");
- cereal::BinaryOutputArchive archive(os);
-
- wolf::LocalParametrizationQuaternionLocal local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
- }
-
- {
- std::ifstream is(path_to_io + "local_parametrization_quat_serialization.bin");
- cereal::BinaryInputArchive archive(is);
-
- wolf::LocalParametrizationQuaternionLocal local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
-
- ASSERT_EQ(local_param_h.getGlobalSize(), 4);
- ASSERT_EQ(local_param_h.getLocalSize(), 3);
- }
-
- PRINTF("All good at TestSerialization::SerializationLocalParametrizationQuaternionBIN !\n");
-}
-
-TEST(TestSerialization, SerializationLocalParametrizationQuaternionPtrBIN)
-{
- using LocalParametrizationPtr = std::shared_ptr<wolf::LocalParametrizationBase> ;
-
- {
- std::ofstream os(path_to_io + "local_parametrization_quat_ptr_serialization.bin");
- cereal::BinaryOutputArchive archive(os);
-
- LocalParametrizationPtr local_param_h =
- std::make_shared<wolf::LocalParametrizationQuaternionLocal>();
-
- ASSERT_NO_THROW( archive( local_param_h ) );
- }
-
- {
- std::ifstream is(path_to_io + "local_parametrization_quat_ptr_serialization.bin");
- cereal::BinaryInputArchive archive(is);
-
- LocalParametrizationPtr local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
-
- ASSERT_EQ(local_param_h->getGlobalSize(), 4);
- ASSERT_EQ(local_param_h->getLocalSize(), 3);
-
- ASSERT_TRUE(
- std::dynamic_pointer_cast<
- wolf::LocalParametrizationQuaternionLocal>(local_param_h) != nullptr);
- }
-
- PRINTF("All good at TestSerialization::SerializationLocalParametrizationQuaternionPtrBIN !\n");
-}
-
-//////////////////////////////////////
-/// GLOBAL ///
-//////////////////////////////////////
-
-TEST(TestSerialization, SerializationLocalParametrizationQuaternionGlobalXML)
-{
- {
- std::ofstream os(path_to_io + "local_parametrization_quatg_serialization.xml");
- cereal::XMLOutputArchive archive(os);
-
- wolf::LocalParametrizationQuaternionGlobal local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
- }
-
- {
- std::ifstream is(path_to_io + "local_parametrization_quatg_serialization.xml");
- cereal::XMLInputArchive archive(is);
-
- wolf::LocalParametrizationQuaternionGlobal local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
-
- ASSERT_EQ(local_param_h.getGlobalSize(), 4);
- ASSERT_EQ(local_param_h.getLocalSize(), 3);
- }
-
- PRINTF("All good at TestSerialization::SerializationLocalParametrizationQuaternionGlobalXML !\n");
-}
-
-TEST(TestSerialization, SerializationLocalParametrizationQuaternionGlobalPtrXML)
-{
- using LocalParametrizationPtr = std::shared_ptr<wolf::LocalParametrizationBase> ;
-
- {
- std::ofstream os(path_to_io + "local_parametrization_quatg_ptr_serialization.xml");
- cereal::XMLOutputArchive archive(os);
-
- LocalParametrizationPtr local_param_h =
- std::make_shared<wolf::LocalParametrizationQuaternionGlobal>();
-
- ASSERT_NO_THROW( archive( local_param_h ) );
- }
-
- {
- std::ifstream is(path_to_io + "local_parametrization_quatg_ptr_serialization.xml");
- cereal::XMLInputArchive archive(is);
-
- LocalParametrizationPtr local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
-
- ASSERT_EQ(local_param_h->getGlobalSize(), 4);
- ASSERT_EQ(local_param_h->getLocalSize(), 3);
-
- ASSERT_TRUE(
- std::dynamic_pointer_cast<
- wolf::LocalParametrizationQuaternionGlobal>(local_param_h) != nullptr);
- }
-
- PRINTF("All good at TestSerialization::SerializationLocalParametrizationQuaternionGlobalPtrXML !\n");
-}
-
-TEST(TestSerialization, SerializationLocalParametrizationQuaternionGlobalJSON)
-{
- {
- std::ofstream os(path_to_io + "local_parametrization_quatg_serialization.json");
- cereal::JSONOutputArchive archive(os);
-
- wolf::LocalParametrizationQuaternionGlobal local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
- }
-
- {
- std::ifstream is(path_to_io + "local_parametrization_quatg_serialization.json");
- cereal::JSONInputArchive archive(is);
-
- wolf::LocalParametrizationQuaternionGlobal local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
-
- ASSERT_EQ(local_param_h.getGlobalSize(), 4);
- ASSERT_EQ(local_param_h.getLocalSize(), 3);
- }
-
- PRINTF("All good at TestSerialization::SerializationLocalParametrizationQuaternionGlobalJSON !\n");
-}
-
-TEST(TestSerialization, SerializationLocalParametrizationQuaternionGlobalPtrJSON)
-{
- using LocalParametrizationPtr = std::shared_ptr<wolf::LocalParametrizationBase> ;
-
- {
- std::ofstream os(path_to_io + "local_parametrization_quatg_ptr_serialization.json");
- cereal::JSONOutputArchive archive(os);
-
- LocalParametrizationPtr local_param_h =
- std::make_shared<wolf::LocalParametrizationQuaternionGlobal>();
-
- ASSERT_NO_THROW( archive( local_param_h ) );
- }
-
- {
- std::ifstream is(path_to_io + "local_parametrization_quatg_ptr_serialization.json");
- cereal::JSONInputArchive archive(is);
-
- LocalParametrizationPtr local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
-
- ASSERT_EQ(local_param_h->getGlobalSize(), 4);
- ASSERT_EQ(local_param_h->getLocalSize(), 3);
-
- ASSERT_TRUE(
- std::dynamic_pointer_cast<
- wolf::LocalParametrizationQuaternionGlobal>(local_param_h) != nullptr);
- }
-
- PRINTF("All good at TestSerialization::SerializationLocalParametrizationQuaternionGlobalPtrJSON !\n");
-}
-
-TEST(TestSerialization, SerializationLocalParametrizationQuaternionGlobalBIN)
-{
- {
- std::ofstream os(path_to_io + "local_parametrization_quatg_serialization.bin");
- cereal::BinaryOutputArchive archive(os);
-
- wolf::LocalParametrizationQuaternionGlobal local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
- }
-
- {
- std::ifstream is(path_to_io + "local_parametrization_quatg_serialization.bin");
- cereal::BinaryInputArchive archive(is);
-
- wolf::LocalParametrizationQuaternionGlobal local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
-
- ASSERT_EQ(local_param_h.getGlobalSize(), 4);
- ASSERT_EQ(local_param_h.getLocalSize(), 3);
- }
-
- PRINTF("All good at TestSerialization::SerializationLocalParametrizationQuaternionGlobalBIN !\n");
-}
-
-TEST(TestSerialization, SerializationLocalParametrizationQuaternionGlobalPtrBIN)
-{
- using LocalParametrizationPtr = std::shared_ptr<wolf::LocalParametrizationBase> ;
-
- {
- std::ofstream os(path_to_io + "local_parametrization_quatg_ptr_serialization.bin");
- cereal::BinaryOutputArchive archive(os);
-
- LocalParametrizationPtr local_param_h =
- std::make_shared<wolf::LocalParametrizationQuaternionGlobal>();
-
- ASSERT_NO_THROW( archive( local_param_h ) );
- }
-
- {
- std::ifstream is(path_to_io + "local_parametrization_quatg_ptr_serialization.bin");
- cereal::BinaryInputArchive archive(is);
-
- LocalParametrizationPtr local_param_h;
-
- ASSERT_NO_THROW( archive( local_param_h ) );
-
- ASSERT_EQ(local_param_h->getGlobalSize(), 4);
- ASSERT_EQ(local_param_h->getLocalSize(), 3);
-
- ASSERT_TRUE(
- std::dynamic_pointer_cast<
- wolf::LocalParametrizationQuaternionGlobal>(local_param_h) != nullptr);
- }
-
- PRINTF("All good at TestSerialization::SerializationLocalParametrizationQuaternionGlobalPtrBIN !\n");
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
diff --git a/test/serialization/cereal/gtest_serialization_node_base.cpp b/test/serialization/cereal/gtest_serialization_node_base.cpp
deleted file mode 100644
index 8b6140fe70f6afe484249361a516b59d8b5dcf7b..0000000000000000000000000000000000000000
--- a/test/serialization/cereal/gtest_serialization_node_base.cpp
+++ /dev/null
@@ -1,247 +0,0 @@
-/*
- * gtest_node_base_serialization.cpp
- *
- * Created on: Jul 16, 2017
- * Author: Jeremie Deray
- */
-
-#include "../../utils_gtest.h"
-
-#include "../../../serialization/cereal/serialization_node_base.h"
-
-#include "../../../serialization/cereal/archives.h"
-
-#include <cereal/types/memory.hpp>
-#include <fstream>
-
-class WolfTestCerealSerializationNodeBase : public testing::Test
-{
-public:
-
- WolfTestCerealSerializationNodeBase() /*:
- nb(nb_class),
- nb_ptr(std::make_shared<wolf::NodeBase>(nb_class))*/
- {
- //
- }
-
- const std::string path_to_io = "/tmp/";
-
- decltype(std::declval<wolf::NodeBase>().getCategory()) nb_class = "Foo";
- decltype(std::declval<wolf::NodeBase>().getCategory()) nb_type = "Bar";
- decltype(std::declval<wolf::NodeBase>().getCategory()) nb_name = "Dummy";
-
- decltype(std::declval<wolf::NodeBase>().nodeId()) id;
-
-// wolf::NodeBase nb;
-// wolf::NodeBasePtr nb_ptr;
-};
-
-TEST_F(WolfTestCerealSerializationNodeBase, CerealSerializationNodeBaseXML)
-{
- {
- // This guy has node_id = 1
- wolf::NodeBase nb(nb_class, nb_type, nb_name);
-
- id = nb.nodeId();
-
- std::ofstream os(path_to_io + "node_base_serialization.xml");
- cereal::XMLOutputArchive xml_archive(os);
-
- ASSERT_NO_THROW( xml_archive( nb ) );
- }
-
- {
- std::ifstream is(path_to_io + "node_base_serialization.xml");
- cereal::XMLInputArchive xml_archive(is);
-
- // This guy has node_id = 2
- wolf::NodeBase nb("SuperDummy");
-
- ASSERT_NO_THROW( xml_archive( nb ) );
-
- ASSERT_EQ(nb.getCategory(), nb_class);
- ASSERT_EQ(nb.getType(), nb_type);
- ASSERT_EQ(nb.getName(), nb_name);
- ASSERT_EQ(nb.nodeId(), id);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationNodeBase::CerealSerializationNodeBaseXML !\n");
-}
-
-TEST_F(WolfTestCerealSerializationNodeBase, CerealSerializationNodeBasePtrXML)
-{
- {
- // This guy has node_id = 3
- wolf::NodeBasePtr nb = std::make_shared<wolf::NodeBase>(nb_class, nb_type, nb_name);
-
- id = nb->nodeId();
-
- std::ofstream os(path_to_io + "node_base_ptr_serialization.xml");
- cereal::XMLOutputArchive xml_archive(os);
-
- ASSERT_NO_THROW( xml_archive( nb ) );
- }
-
- {
- std::ifstream is(path_to_io + "node_base_ptr_serialization.xml");
- cereal::XMLInputArchive xml_archive(is);
-
- wolf::NodeBasePtr nb;
-
- ASSERT_NO_THROW( xml_archive( nb ) );
-
- ASSERT_EQ(nb->getCategory(), nb_class);
- ASSERT_EQ(nb->getType(), nb_type);
- ASSERT_EQ(nb->getName(), nb_name);
- ASSERT_EQ(nb->nodeId(), id);
-
- ASSERT_TRUE(
- std::dynamic_pointer_cast<
- wolf::NodeBase>(nb) != nullptr);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationNodeBase::CerealSerializationNodeBasePtrXML !\n");
-}
-
-TEST_F(WolfTestCerealSerializationNodeBase, CerealSerializationNodeBaseJSON)
-{
- {
- wolf::NodeBase nb(nb_class, nb_type, nb_name);
-
- id = nb.nodeId();
-
- std::ofstream os(path_to_io + "node_base_serialization.json");
- cereal::JSONOutputArchive json_archive(os);
-
- ASSERT_NO_THROW( json_archive( nb ) );
- }
-
- {
- std::ifstream is(path_to_io + "node_base_serialization.json");
- cereal::JSONInputArchive json_archive(is);
-
- wolf::NodeBase blank("This guy has node_id = 1");
- wolf::NodeBase nb("SuperDummy");
-
- ASSERT_NO_THROW( json_archive( nb ) );
-
- ASSERT_EQ(nb.getCategory(), nb_class);
- ASSERT_EQ(nb.getType(), nb_type);
- ASSERT_EQ(nb.getName(), nb_name);
- ASSERT_EQ(nb.nodeId(), id);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationNodeBase::CerealSerializationNodeBaseJSON !\n");
-}
-
-TEST_F(WolfTestCerealSerializationNodeBase, CerealSerializationNodeBasePtrJSON)
-{
- {
- wolf::NodeBasePtr nb = std::make_shared<wolf::NodeBase>(nb_class, nb_type, nb_name);
-
- id = nb->nodeId();
-
- std::ofstream os(path_to_io + "node_base_ptr_serialization.json");
- cereal::JSONOutputArchive json_archive(os);
-
- ASSERT_NO_THROW( json_archive( nb ) );
- }
-
- {
- std::ifstream is(path_to_io + "node_base_ptr_serialization.json");
- cereal::JSONInputArchive json_archive(is);
-
- wolf::NodeBasePtr nb;
-
- ASSERT_NO_THROW( json_archive( nb ) );
-
- ASSERT_EQ(nb->getCategory(), nb_class);
- ASSERT_EQ(nb->getType(), nb_type);
- ASSERT_EQ(nb->getName(), nb_name);
- ASSERT_EQ(nb->nodeId(), id);
-
- ASSERT_TRUE(
- std::dynamic_pointer_cast<
- wolf::NodeBase>(nb) != nullptr);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationNodeBase::CerealSerializationNodeBasePtrJSON !\n");
-}
-
-TEST_F(WolfTestCerealSerializationNodeBase, CerealSerializationNodeBaseBinary)
-{
- {
- wolf::NodeBase nb(nb_class, nb_type, nb_name);
-
- id = nb.nodeId();
-
- std::ofstream os(path_to_io + "node_base_serialization.bin");
- cereal::BinaryOutputArchive bin_archive(os);
-
- ASSERT_NO_THROW( bin_archive( nb ) );
- }
-
- {
- std::ifstream is(path_to_io + "node_base_serialization.bin");
- cereal::BinaryInputArchive bin_archive(is);
-
- wolf::NodeBase blank("This guy has node_id = 1");
- wolf::NodeBase nb("SuperDummy");
-
- ASSERT_NO_THROW( bin_archive( nb ) );
-
- ASSERT_EQ(nb.getCategory(), nb_class);
- ASSERT_EQ(nb.getType(), nb_type);
- ASSERT_EQ(nb.getName(), nb_name);
- ASSERT_EQ(nb.nodeId(), id);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationNodeBase::CerealSerializationNodeBaseBinary !\n");
-}
-
-TEST_F(WolfTestCerealSerializationNodeBase, CerealSerializationNodeBasePtrBinary)
-{
- {
- wolf::NodeBasePtr nb = std::make_shared<wolf::NodeBase>(nb_class, nb_type, nb_name);
-
- id = nb->nodeId();
-
- std::ofstream os(path_to_io + "node_base_ptr_serialization.bin");
- cereal::BinaryOutputArchive bin_archive(os);
-
- ASSERT_NO_THROW( bin_archive( nb ) );
- }
-
- {
- std::ifstream is(path_to_io + "node_base_ptr_serialization.bin");
- cereal::BinaryInputArchive bin_archive(is);
-
- wolf::NodeBasePtr nb;
-
- ASSERT_NO_THROW( bin_archive( nb ) );
-
- ASSERT_EQ(nb->getCategory(), nb_class);
- ASSERT_EQ(nb->getType(), nb_type);
- ASSERT_EQ(nb->getName(), nb_name);
- ASSERT_EQ(nb->nodeId(), id);
-
- ASSERT_TRUE(
- std::dynamic_pointer_cast<
- wolf::NodeBase>(nb) != nullptr);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationNodeBase::CerealSerializationNodeBasePtrBinary !\n");
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
diff --git a/test/serialization/cereal/gtest_serialization_processor_odom2d_params.cpp b/test/serialization/cereal/gtest_serialization_processor_odom2d_params.cpp
deleted file mode 100644
index 89fbef3d69dff80688dd234b0cec3742233b9220..0000000000000000000000000000000000000000
--- a/test/serialization/cereal/gtest_serialization_processor_odom2d_params.cpp
+++ /dev/null
@@ -1,108 +0,0 @@
-/*
- * gtest_intrinsics_odom2d_serialization.cpp
- *
- * Created on: Jul 16, 2017
- * Author: Jeremie Deray
- */
-
-#include "../../utils_gtest.h"
-
-#include "../../../serialization/cereal/serialization_processor_odom2d_params.h"
-
-#include "../../../serialization/cereal/io.h"
-
-#include <cereal/types/memory.hpp>
-#include <fstream>
-
-class WolfTestCerealSerializationProcessorParamsOdom2D : public testing::Test
-{
-public:
-
- WolfTestCerealSerializationProcessorParamsOdom2D()
- {
- nb_.name = "NAME";
- nb_.type = "ODOM 2D";
-
- nb_.dist_traveled_th_ = 0.17;
- nb_.theta_traveled_th_ = 0.3;
- nb_.cov_det = 0.4;
- nb_.elapsed_time_th_ = 1.5;
- nb_.unmeasured_perturbation_std = 1e-5;
- }
-
- const std::string path_to_io = "/tmp/";
- const std::string filename = "serialization_processor_odom2d_params";
- const std::string ptr_ext = "_ptr";
-
- const std::vector<std::string> exts = {".bin", ".xml", ".json"};
-
- wolf::ProcessorParamsOdom2D nb_;
-};
-
-TEST_F(WolfTestCerealSerializationProcessorParamsOdom2D,
- CerealSerializationProcessorParamsOdom2D)
-{
- for (const auto ext : exts)
- {
- const std::string full_path = path_to_io + filename + ext;
-
- ASSERT_NO_THROW( wolf::save( full_path, nb_ ) )
- << "Failed on saving " << full_path;
-
- wolf::ProcessorParamsOdom2D nb_load;
-
- ASSERT_NO_THROW( wolf::load( full_path, nb_load ) )
- << "Failed on loading " << full_path;
-
- ASSERT_EQ(nb_load.type, nb_.type) << full_path;
- ASSERT_EQ(nb_load.name, nb_.name) << full_path;
- ASSERT_EQ(nb_load.dist_traveled_th_, nb_.dist_traveled_th_) << full_path;
- ASSERT_EQ(nb_load.theta_traveled_th_, nb_.theta_traveled_th_) << full_path;
- ASSERT_EQ(nb_load.cov_det, nb_.cov_det) << full_path;
- ASSERT_EQ(nb_load.unmeasured_perturbation_std,
- nb_.unmeasured_perturbation_std) << full_path;
-
- /// Testing BasePtr
-
- const std::string ptr_full_path = path_to_io + filename + ptr_ext + ext;
-
- {
- wolf::ProcessorParamsBasePtr nb =
- std::make_shared<wolf::ProcessorParamsOdom2D>(nb_);
-
- ASSERT_NO_THROW( wolf::save( ptr_full_path, nb ) )
- << "Failed on saving " << ptr_full_path;
- }
-
- {
- wolf::ProcessorParamsBasePtr nb;
-
- ASSERT_NO_THROW( wolf::load( ptr_full_path, nb ) )
- << "Failed on loading " << ptr_full_path;
-
- wolf::ProcessorParamsOdom2DPtr nb_cast =
- std::dynamic_pointer_cast<wolf::ProcessorParamsOdom2D>(nb);
-
- ASSERT_TRUE(nb_cast != nullptr)
- << "Failed on casting " << ptr_full_path;
-
- ASSERT_EQ(nb_cast->type, nb_.type) << full_path;
- ASSERT_EQ(nb_cast->name, nb_.name) << full_path;
- ASSERT_EQ(nb_cast->dist_traveled_th_, nb_.dist_traveled_th_) << full_path;
- ASSERT_EQ(nb_cast->theta_traveled_th_, nb_.theta_traveled_th_) << full_path;
- ASSERT_EQ(nb_cast->cov_det, nb_.cov_det) << full_path;
- ASSERT_EQ(nb_cast->unmeasured_perturbation_std,
- nb_.unmeasured_perturbation_std) << full_path;
- }
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationProcessorParamsOdom2D::"
- "CerealSerializationProcessorParamsOdom2D !\n");
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
diff --git a/test/serialization/cereal/gtest_serialization_processor_odom3d_params.cpp b/test/serialization/cereal/gtest_serialization_processor_odom3d_params.cpp
deleted file mode 100644
index 41b2ca2f1b9c9eb6671ed81b4b04b5d97fb847d1..0000000000000000000000000000000000000000
--- a/test/serialization/cereal/gtest_serialization_processor_odom3d_params.cpp
+++ /dev/null
@@ -1,246 +0,0 @@
-/*
- * gtest_intrinsics_odom2d_serialization.cpp
- *
- * Created on: Jul 16, 2017
- * Author: Jeremie Deray
- */
-
-#include "../../utils_gtest.h"
-
-#include "../../../serialization/cereal/serialization_processor_odom3d_params.h"
-
-#include "../../../serialization/cereal/io.h"
-
-#include <cereal/types/memory.hpp>
-#include <fstream>
-
-class WolfTestCerealSerializationProcessorOdom3DParams : public testing::Test
-{
-public:
-
- WolfTestCerealSerializationProcessorOdom3DParams()
- {
- nb_.name = "NAME";
- //nb_.type = "ODOM 3D";
-
- nb_.max_time_span = 1.5;
- nb_.max_buff_length = 55.;
- nb_.dist_traveled = .25;
- nb_.angle_turned = .17;
- }
-
- const std::string path_to_io = "/tmp/";
-
- wolf::ProcessorParamsOdom3D nb_;
-};
-
-TEST_F(WolfTestCerealSerializationProcessorOdom3DParams,
- CerealSerializationProcessorOdom3DParamsXML)
-{
- const std::string filename(path_to_io + "params_odom3d_serialization.xml");
-
- wolf::ProcessorParamsOdom3D nb_save;
- nb_save.name = "NAME2";
- //nb_.type = "ODOM 3D";
-
- nb_save.max_time_span = 2.5;
- nb_save.max_buff_length = 52.;
- nb_save.dist_traveled = .24;
- nb_save.angle_turned = .18;
-
- ASSERT_NO_THROW( wolf::save( filename, nb_, nb_save, 10 ) );
-
- {
- wolf::ProcessorParamsOdom3D nb_load;
-
- ASSERT_NO_THROW( wolf::load( filename, nb_load ) );
-
- ASSERT_EQ(nb_load.type, nb_.type);
- ASSERT_EQ(nb_load.name, nb_.name);
- ASSERT_EQ(nb_load.max_time_span, nb_.max_time_span);
- ASSERT_EQ(nb_load.max_buff_length, nb_.max_buff_length);
- ASSERT_EQ(nb_load.dist_traveled, nb_.dist_traveled);
- ASSERT_EQ(nb_load.angle_turned, nb_.angle_turned);
-
- wolf::ProcessorParamsOdom3D nb_load0, nb_load1;
- int myint;
- ASSERT_NO_THROW( wolf::load( filename, nb_load0, nb_load1, myint ) );
-
- ASSERT_EQ(nb_load0.type, nb_.type);
- ASSERT_EQ(nb_load0.name, nb_.name);
- ASSERT_EQ(nb_load0.max_time_span, nb_.max_time_span);
- ASSERT_EQ(nb_load0.max_buff_length, nb_.max_buff_length);
- ASSERT_EQ(nb_load0.dist_traveled, nb_.dist_traveled);
- ASSERT_EQ(nb_load0.angle_turned, nb_.angle_turned);
-
- ASSERT_EQ(nb_load1.type, nb_save.type);
- ASSERT_EQ(nb_load1.name, nb_save.name);
- ASSERT_EQ(nb_load1.max_time_span, nb_save.max_time_span);
- ASSERT_EQ(nb_load1.max_buff_length, nb_save.max_buff_length);
- ASSERT_EQ(nb_load1.dist_traveled, nb_save.dist_traveled);
- ASSERT_EQ(nb_load1.angle_turned, nb_save.angle_turned);
-
- ASSERT_EQ(myint, 10);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationProcessorOdom3DParams::"
- "CerealSerializationProcessorOdom3DParamsXML !\n");
-}
-
-TEST_F(WolfTestCerealSerializationProcessorOdom3DParams,
- CerealSerializationProcessorParamsOdom3DPtrXML)
-{
- const std::string filename(path_to_io + "params_odom3d_ptr_serialization.xml");
-
- {
- wolf::ProcessorParamsBasePtr nb =
- std::make_shared<wolf::ProcessorParamsOdom3D>(nb_);
-
- ASSERT_NO_THROW( wolf::save( filename, nb ) );
- }
-
- {
- wolf::ProcessorParamsBasePtr nb;
-
- ASSERT_NO_THROW( wolf::load( filename, nb ) );
-
- wolf::ProcessorParamsOdom3DPtr nb_cast =
- std::dynamic_pointer_cast<wolf::ProcessorParamsOdom3D>(nb);
-
- ASSERT_TRUE(nb_cast != nullptr);
-
- ASSERT_EQ(nb_cast->type, nb_.type);
- ASSERT_EQ(nb_cast->name, nb_.name);
- ASSERT_EQ(nb_cast->max_time_span, nb_.max_time_span);
- ASSERT_EQ(nb_cast->max_buff_length, nb_.max_buff_length);
- ASSERT_EQ(nb_cast->dist_traveled, nb_.dist_traveled);
- ASSERT_EQ(nb_cast->angle_turned, nb_.angle_turned);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationProcessorOdom3DParams::"
- "CerealSerializationProcessorParamsOdom3DPtrXML !\n");
-}
-
-TEST_F(WolfTestCerealSerializationProcessorOdom3DParams,
- CerealSerializationProcessorOdom3DParamsJSON)
-{
- const std::string filename(path_to_io + "params_odom3d_serialization.json");
-
- ASSERT_NO_THROW( wolf::save( filename, nb_ ) );
-
- wolf::ProcessorParamsOdom3D nb_load;
-
- ASSERT_NO_THROW( wolf::load( filename, nb_load ) );
-
- ASSERT_EQ(nb_load.type, nb_.type);
- ASSERT_EQ(nb_load.name, nb_.name);
- ASSERT_EQ(nb_load.max_time_span, nb_.max_time_span);
- ASSERT_EQ(nb_load.max_buff_length, nb_.max_buff_length);
- ASSERT_EQ(nb_load.dist_traveled, nb_.dist_traveled);
- ASSERT_EQ(nb_load.angle_turned, nb_.angle_turned);
-
- PRINTF("All good at "
- "WolfTestCerealSerializationProcessorOdom3DParams::"
- "CerealSerializationProcessorOdom3DParamsJSON !\n");
-}
-
-TEST_F(WolfTestCerealSerializationProcessorOdom3DParams,
- CerealSerializationProcessorParamsOdom3DPtrJSON)
-{
- const std::string filename(path_to_io + "params_odom3d_ptr_serialization.json");
-
- {
- wolf::ProcessorParamsBasePtr nb =
- std::make_shared<wolf::ProcessorParamsOdom3D>(nb_);
-
- ASSERT_NO_THROW( wolf::save( filename, nb ) );
- }
-
- {
- wolf::ProcessorParamsBasePtr nb;
-
- ASSERT_NO_THROW( wolf::load( filename, nb ) );
-
- wolf::ProcessorParamsOdom3DPtr nb_cast =
- std::dynamic_pointer_cast<wolf::ProcessorParamsOdom3D>(nb);
-
- ASSERT_TRUE(nb_cast != nullptr);
-
- ASSERT_EQ(nb_cast->type, nb_.type);
- ASSERT_EQ(nb_cast->name, nb_.name);
- ASSERT_EQ(nb_cast->max_time_span, nb_.max_time_span);
- ASSERT_EQ(nb_cast->max_buff_length, nb_.max_buff_length);
- ASSERT_EQ(nb_cast->dist_traveled, nb_.dist_traveled);
- ASSERT_EQ(nb_cast->angle_turned, nb_.angle_turned);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationProcessorOdom3DParams::"
- "CerealSerializationProcessorParamsOdom3DPtrJSON !\n");
-}
-
-TEST_F(WolfTestCerealSerializationProcessorOdom3DParams,
- CerealSerializationProcessorOdom3DParamsBinary)
-{
- const std::string filename(path_to_io + "params_odom3d_serialization.bin");
-
- ASSERT_NO_THROW( wolf::save( filename, nb_ ) );
-
- wolf::ProcessorParamsOdom3D nb_load;
-
- ASSERT_NO_THROW( wolf::load( filename, nb_load ) );
-
- ASSERT_EQ(nb_load.type, nb_.type);
- ASSERT_EQ(nb_load.name, nb_.name);
- ASSERT_EQ(nb_load.max_time_span, nb_.max_time_span);
- ASSERT_EQ(nb_load.max_buff_length, nb_.max_buff_length);
- ASSERT_EQ(nb_load.dist_traveled, nb_.dist_traveled);
- ASSERT_EQ(nb_load.angle_turned, nb_.angle_turned);
-
- PRINTF("All good at "
- "WolfTestCerealSerializationProcessorOdom3DParams::"
- "CerealSerializationProcessorOdom3DParamsBinary !\n");
-}
-
-TEST_F(WolfTestCerealSerializationProcessorOdom3DParams,
- CerealSerializationProcessorParamsOdom3DPtrBinary)
-{
- const std::string filename(path_to_io + "params_odom3d_ptr_serialization.bin");
-
- {
- wolf::ProcessorParamsBasePtr nb =
- std::make_shared<wolf::ProcessorParamsOdom3D>(nb_);
-
- ASSERT_NO_THROW( wolf::save( filename, nb ) );
- }
-
- {
- wolf::ProcessorParamsBasePtr nb;
-
- ASSERT_NO_THROW( wolf::load( filename, nb ) );
-
- wolf::ProcessorParamsOdom3DPtr nb_cast =
- std::dynamic_pointer_cast<wolf::ProcessorParamsOdom3D>(nb);
-
- ASSERT_TRUE(nb_cast != nullptr);
-
- ASSERT_EQ(nb_cast->type, nb_.type);
- ASSERT_EQ(nb_cast->name, nb_.name);
- ASSERT_EQ(nb_cast->max_time_span, nb_.max_time_span);
- ASSERT_EQ(nb_cast->max_buff_length, nb_.max_buff_length);
- ASSERT_EQ(nb_cast->dist_traveled, nb_.dist_traveled);
- ASSERT_EQ(nb_cast->angle_turned, nb_.angle_turned);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationProcessorOdom3DParams::"
- "CerealSerializationProcessorParamsOdom3DPtrBinary !\n");
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
diff --git a/test/serialization/cereal/gtest_serialization_save_load.cpp b/test/serialization/cereal/gtest_serialization_save_load.cpp
deleted file mode 100644
index 23241af4887a43db6d48f027b30481e3408ade49..0000000000000000000000000000000000000000
--- a/test/serialization/cereal/gtest_serialization_save_load.cpp
+++ /dev/null
@@ -1,202 +0,0 @@
-/*
- * gtest_intrinsics_odom2d_serialization.cpp
- *
- * Created on: Jul 16, 2017
- * Author: Jeremie Deray
- */
-
-#include "../../utils_gtest.h"
-
-#include "../../../serialization/cereal/io.h"
-#include "../../../serialization/cereal/serialization_sensor_odom2d_intrinsic.h"
-
-#include "../../../serialization/cereal/archives.h"
-
-#include <cereal/types/memory.hpp>
-#include <fstream>
-
-namespace wolf {
-
-using IntrinsicsOdom2DPtr = std::shared_ptr<IntrinsicsOdom2D>;
-
-}
-
-class WolfTestCerealSerializationSaveLoad : public testing::Test
-{
-public:
-
- WolfTestCerealSerializationSaveLoad()
- {
- //
- }
-
- const std::string path_to_io = "/tmp/";
-
- decltype(std::declval<wolf::IntrinsicsOdom2D>().type) nb_type = "TYPE";
- decltype(std::declval<wolf::IntrinsicsOdom2D>().name) nb_name = "NAME";
- decltype(std::declval<wolf::IntrinsicsOdom2D>().k_disp_to_disp) nb_k_disp_to_disp = 0.54;
- decltype(std::declval<wolf::IntrinsicsOdom2D>().k_rot_to_rot) nb_k_rot_to_rot = 0.18;
-};
-
-TEST_F(WolfTestCerealSerializationSaveLoad, CerealSerializationSaveLoadExtension)
-{
- const std::string xml = "/test/filename.xml";
- const std::string bin = "/test/filename.bin";
- const std::string json = "/test/filename.json";
-
- ASSERT_EQ(wolf::serialization::extension(xml), ".xml");
- ASSERT_EQ(wolf::serialization::extension(bin), ".bin");
- ASSERT_EQ(wolf::serialization::extension(json), ".json");
-}
-
-TEST_F(WolfTestCerealSerializationSaveLoad,
- CerealSerializationSaveLoadXML)
-{
- const std::string filename = path_to_io + "save_load_serialization.xml";
-
- {
- wolf::IntrinsicsOdom2D nb;
- nb.type = nb_type;
- nb.name = nb_name;
- nb.k_disp_to_disp = nb_k_disp_to_disp;
- nb.k_rot_to_rot = nb_k_rot_to_rot;
-
- ASSERT_NO_THROW( wolf::save( filename, nb ) );
- }
-
- {
- wolf::IntrinsicsOdom2D nb;
-
- ASSERT_NO_THROW( wolf::load( filename, nb ) );
-
- ASSERT_EQ(nb.type, nb_type);
- ASSERT_EQ(nb.name, nb_name);
- ASSERT_EQ(nb.k_disp_to_disp, nb_k_disp_to_disp);
- ASSERT_EQ(nb.k_rot_to_rot, nb_k_rot_to_rot);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationSaveLoad::"
- "CerealSerializationSaveLoadXML !\n");
-}
-
-TEST_F(WolfTestCerealSerializationSaveLoad,
- CerealSerializationSaveLoadJSON)
-{
- const std::string filename = path_to_io + "save_load_serialization.json";
-
- {
- wolf::IntrinsicsOdom2D nb;
- nb.type = nb_type;
- nb.name = nb_name;
- nb.k_disp_to_disp = nb_k_disp_to_disp;
- nb.k_rot_to_rot = nb_k_rot_to_rot;
-
- ASSERT_NO_THROW( wolf::save( filename, nb ) );
- }
-
- {
- wolf::IntrinsicsOdom2D nb;
-
- ASSERT_NO_THROW( wolf::load( filename, nb ) );
-
- ASSERT_EQ(nb.type, nb_type);
- ASSERT_EQ(nb.name, nb_name);
- ASSERT_EQ(nb.k_disp_to_disp, nb_k_disp_to_disp);
- ASSERT_EQ(nb.k_rot_to_rot, nb_k_rot_to_rot);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationSaveLoad::"
- "CerealSerializationSaveLoadJSON !\n");
-}
-
-TEST_F(WolfTestCerealSerializationSaveLoad,
- CerealSerializationSaveLoadBinary)
-{
- const std::string filename = path_to_io + "save_load_serialization.bin";
-
- {
- wolf::IntrinsicsOdom2D nb;
- nb.type = nb_type;
- nb.name = nb_name;
- nb.k_disp_to_disp = nb_k_disp_to_disp;
- nb.k_rot_to_rot = nb_k_rot_to_rot;
-
- ASSERT_NO_THROW( wolf::save( filename, nb ) );
- }
-
- {
- wolf::IntrinsicsOdom2D nb;
-
- ASSERT_NO_THROW( wolf::load( filename, nb ) );
-
- ASSERT_EQ(nb.type, nb_type);
- ASSERT_EQ(nb.name, nb_name);
- ASSERT_EQ(nb.k_disp_to_disp, nb_k_disp_to_disp);
- ASSERT_EQ(nb.k_rot_to_rot, nb_k_rot_to_rot);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationSaveLoad::"
- "CerealSerializationSaveLoadBinary !\n");
-}
-
-TEST_F(WolfTestCerealSerializationSaveLoad,
- CerealSerializationSaveLoadNoExt)
-{
- const std::string filename = path_to_io + "save_load_serialization_no_ext";
-
- {
- wolf::IntrinsicsOdom2D nb;
- nb.type = nb_type;
- nb.name = nb_name;
- nb.k_disp_to_disp = nb_k_disp_to_disp;
- nb.k_rot_to_rot = nb_k_rot_to_rot;
-
- ASSERT_NO_THROW( wolf::save( filename, nb ) );
- }
-
- {
- wolf::IntrinsicsOdom2D nb;
-
- ASSERT_NO_THROW( wolf::load( filename, nb ) );
-
- ASSERT_EQ(nb.type, nb_type);
- ASSERT_EQ(nb.name, nb_name);
- ASSERT_EQ(nb.k_disp_to_disp, nb_k_disp_to_disp);
- ASSERT_EQ(nb.k_rot_to_rot, nb_k_rot_to_rot);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationSaveLoad::"
- "CerealSerializationSaveLoadNoExt !\n");
-}
-
-TEST_F(WolfTestCerealSerializationSaveLoad,
- CerealSerializationSaveLoadUnknownExt)
-{
- const std::string filename = path_to_io + "save_load_serialization.foo";
-
- {
- wolf::IntrinsicsOdom2D nb;
-
- ASSERT_THROW( wolf::save( filename, nb ), std::runtime_error );
- }
-
- {
- wolf::IntrinsicsOdom2D nb;
-
- ASSERT_THROW( wolf::load( filename, nb ), std::runtime_error );
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationSaveLoad::"
- "CerealSerializationSaveLoadUnknownExt !\n");
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
diff --git a/test/serialization/cereal/gtest_serialization_sensor_intrinsic_base.cpp b/test/serialization/cereal/gtest_serialization_sensor_intrinsic_base.cpp
deleted file mode 100644
index a8a67dc1977d184a91a26cae2420f1a04ef5a0ee..0000000000000000000000000000000000000000
--- a/test/serialization/cereal/gtest_serialization_sensor_intrinsic_base.cpp
+++ /dev/null
@@ -1,233 +0,0 @@
-/*
- * gtest_intrinsics_base_serialization.cpp
- *
- * Created on: Jul 16, 2017
- * Author: Jeremie Deray
- */
-
-#include "../../utils_gtest.h"
-
-#include "../../../serialization/cereal/serialization_sensor_intrinsic_base.h"
-
-#include "../../../serialization/cereal/archives.h"
-
-#include <cereal/types/memory.hpp>
-#include <fstream>
-
-class WolfTestCerealSerializationIntrinsicsBase : public testing::Test
-{
-public:
-
- WolfTestCerealSerializationIntrinsicsBase()
- {
- //
- }
-
- const std::string path_to_io = "/tmp/";
-
- decltype(std::declval<wolf::IntrinsicsBase>().type) nb_type = "TYPE";
- decltype(std::declval<wolf::IntrinsicsBase>().name) nb_name = "NAME";
-};
-
-TEST_F(WolfTestCerealSerializationIntrinsicsBase,
- CerealSerializationIntrinsicsBaseXML)
-{
- {
- wolf::IntrinsicsBase nb;
- nb.type = nb_type;
- nb.name = nb_name;
-
- std::ofstream os(path_to_io + "intrinsics_base_serialization.xml");
- cereal::XMLOutputArchive xml_archive(os);
-
- ASSERT_NO_THROW( xml_archive( nb ) );
- }
-
- {
- std::ifstream is(path_to_io + "intrinsics_base_serialization.xml");
- cereal::XMLInputArchive xml_archive(is);
-
- wolf::IntrinsicsBase nb;
-
- ASSERT_NO_THROW( xml_archive( nb ) );
-
- ASSERT_EQ(nb.type, nb_type);
- ASSERT_EQ(nb.name, nb_name);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationIntrinsicsBase::"
- "CerealSerializationIntrinsicsBaseXML !\n");
-}
-
-TEST_F(WolfTestCerealSerializationIntrinsicsBase,
- CerealSerializationIntrinsicsBasePtrXML)
-{
- {
- wolf::IntrinsicsBasePtr nb = std::make_shared<wolf::IntrinsicsBase>();
- nb->name = nb_name;
- nb->type = nb_type;
-
- std::ofstream os(path_to_io + "intrinsics_base_ptr_serialization.xml");
- cereal::XMLOutputArchive xml_archive(os);
-
- ASSERT_NO_THROW( xml_archive( nb ) );
- }
-
- {
- std::ifstream is(path_to_io + "intrinsics_base_ptr_serialization.xml");
- cereal::XMLInputArchive xml_archive(is);
-
- wolf::IntrinsicsBasePtr nb;
-
- ASSERT_NO_THROW( xml_archive( nb ) );
-
- ASSERT_EQ(nb->type, nb_type);
- ASSERT_EQ(nb->name, nb_name);
-
- ASSERT_TRUE(
- std::dynamic_pointer_cast<
- wolf::IntrinsicsBase>(nb) != nullptr);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationIntrinsicsBase::"
- "CerealSerializationIntrinsicsBasePtrXML !\n");
-}
-
-TEST_F(WolfTestCerealSerializationIntrinsicsBase,
- CerealSerializationIntrinsicsBaseJSON)
-{
- {
- wolf::IntrinsicsBase nb;
- nb.type = nb_type;
- nb.name = nb_name;
-
- std::ofstream os(path_to_io + "intrinsics_base_serialization.json");
- cereal::JSONOutputArchive json_archive(os);
-
- ASSERT_NO_THROW( json_archive( nb ) );
- }
-
- {
- std::ifstream is(path_to_io + "intrinsics_base_serialization.json");
- cereal::JSONInputArchive json_archive(is);
-
- wolf::IntrinsicsBase nb;
-
- ASSERT_NO_THROW( json_archive( nb ) );
-
- ASSERT_EQ(nb.type, nb_type);
- ASSERT_EQ(nb.name, nb_name);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationIntrinsicsBase::"
- "CerealSerializationIntrinsicsBaseJSON !\n");
-}
-
-TEST_F(WolfTestCerealSerializationIntrinsicsBase,
- CerealSerializationIntrinsicsBasePtrJSON)
-{
- {
- wolf::IntrinsicsBasePtr nb = std::make_shared<wolf::IntrinsicsBase>();
- nb->name = nb_name;
- nb->type = nb_type;
-
- std::ofstream os(path_to_io + "intrinsics_base_ptr_serialization.json");
- cereal::JSONOutputArchive json_archive(os);
-
- ASSERT_NO_THROW( json_archive( nb ) );
- }
-
- {
- std::ifstream is(path_to_io + "intrinsics_base_ptr_serialization.json");
- cereal::JSONInputArchive json_archive(is);
-
- wolf::IntrinsicsBasePtr nb;
-
- ASSERT_NO_THROW( json_archive( nb ) );
-
- ASSERT_EQ(nb->type, nb_type);
- ASSERT_EQ(nb->name, nb_name);
-
- ASSERT_TRUE(
- std::dynamic_pointer_cast<
- wolf::IntrinsicsBase>(nb) != nullptr);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationIntrinsicsBase::"
- "CerealSerializationIntrinsicsBasePtrJSON !\n");
-}
-
-TEST_F(WolfTestCerealSerializationIntrinsicsBase,
- CerealSerializationIntrinsicsBaseBinary)
-{
- {
- wolf::IntrinsicsBase nb;
- nb.type = nb_type;
- nb.name = nb_name;
-
- std::ofstream os(path_to_io + "intrinsics_base_serialization.bin");
- cereal::BinaryOutputArchive bin_archive(os);
-
- ASSERT_NO_THROW( bin_archive( nb ) );
- }
-
- {
- std::ifstream is(path_to_io + "intrinsics_base_serialization.bin");
- cereal::BinaryInputArchive bin_archive(is);
-
- wolf::IntrinsicsBase nb;
-
- ASSERT_NO_THROW( bin_archive( nb ) );
-
- ASSERT_EQ(nb.type, nb_type);
- ASSERT_EQ(nb.name, nb_name);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationIntrinsicsBase::"
- "CerealSerializationIntrinsicsBaseBinary !\n");
-}
-
-TEST_F(WolfTestCerealSerializationIntrinsicsBase, CerealSerializationIntrinsicsBasePtrBinary)
-{
- {
- wolf::IntrinsicsBasePtr nb = std::make_shared<wolf::IntrinsicsBase>();
- nb->name = nb_name;
- nb->type = nb_type;
-
- std::ofstream os(path_to_io + "intrinsics_base_ptr_serialization.bin");
- cereal::BinaryOutputArchive bin_archive(os);
-
- ASSERT_NO_THROW( bin_archive( nb ) );
- }
-
- {
- std::ifstream is(path_to_io + "intrinsics_base_ptr_serialization.bin");
- cereal::BinaryInputArchive bin_archive(is);
-
- wolf::IntrinsicsBasePtr nb;
-
- ASSERT_NO_THROW( bin_archive( nb ) );
-
- ASSERT_EQ(nb->type, nb_type);
- ASSERT_EQ(nb->name, nb_name);
-
- ASSERT_TRUE(
- std::dynamic_pointer_cast<
- wolf::IntrinsicsBase>(nb) != nullptr);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationIntrinsicsBase::"
- "CerealSerializationIntrinsicsBasePtrBinary !\n");
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
diff --git a/test/serialization/cereal/gtest_serialization_sensor_odom2d_intrinsic.cpp b/test/serialization/cereal/gtest_serialization_sensor_odom2d_intrinsic.cpp
deleted file mode 100644
index c5891c3d91d020293937236e582535ddd2e2beaf..0000000000000000000000000000000000000000
--- a/test/serialization/cereal/gtest_serialization_sensor_odom2d_intrinsic.cpp
+++ /dev/null
@@ -1,256 +0,0 @@
-/*
- * gtest_intrinsics_odom2d_serialization.cpp
- *
- * Created on: Jul 16, 2017
- * Author: Jeremie Deray
- */
-
-#include "../../utils_gtest.h"
-
-#include "../../../serialization/cereal/serialization_sensor_odom2d_intrinsic.h"
-
-#include "../../../serialization/cereal/archives.h"
-
-#include <cereal/types/memory.hpp>
-#include <fstream>
-
-namespace wolf {
-
-using IntrinsicsOdom2DPtr = std::shared_ptr<IntrinsicsOdom2D>;
-
-}
-
-class WolfTestCerealSerializationIntrinsicsOdom2D : public testing::Test
-{
-public:
-
- WolfTestCerealSerializationIntrinsicsOdom2D()
- {
- nb_.k_disp_to_disp = 0.54;
- nb_.k_rot_to_rot = 0.18;
- nb_.name = "NAME";
- nb_.type = "TYPE";
- }
-
- const std::string path_to_io = "/tmp/";
-
- wolf::IntrinsicsOdom2D nb_;
-};
-
-TEST_F(WolfTestCerealSerializationIntrinsicsOdom2D,
- CerealSerializationIntrinsicsOdom2DXML)
-{
- {
- wolf::IntrinsicsOdom2D nb;
- nb.type = nb_.type;
- nb.name = nb_.name;
- nb.k_disp_to_disp = nb_.k_disp_to_disp;
- nb.k_rot_to_rot = nb_.k_rot_to_rot;
-
- std::ofstream os(path_to_io + "intrinsics_odom2d_serialization.xml");
- cereal::XMLOutputArchive xml_archive(os);
-
- ASSERT_NO_THROW( xml_archive( nb ) );
- }
-
- {
- std::ifstream is(path_to_io + "intrinsics_odom2d_serialization.xml");
- cereal::XMLInputArchive xml_archive(is);
-
- wolf::IntrinsicsOdom2D nb;
-
- ASSERT_NO_THROW( xml_archive( nb ) );
-
- ASSERT_EQ(nb.type, nb_.type);
- ASSERT_EQ(nb.name, nb_.name);
- ASSERT_EQ(nb.k_disp_to_disp, nb_.k_disp_to_disp);
- ASSERT_EQ(nb.k_rot_to_rot, nb_.k_rot_to_rot);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationIntrinsicsOdom2D::"
- "CerealSerializationIntrinsicsOdom2DXML !\n");
-}
-
-TEST_F(WolfTestCerealSerializationIntrinsicsOdom2D,
- CerealSerializationIntrinsicsOdom2DPtrXML)
-{
- {
- wolf::IntrinsicsBasePtr nb = std::make_shared<wolf::IntrinsicsOdom2D>(nb_);
-
- std::ofstream os(path_to_io + "intrinsics_odom2d_ptr_serialization.xml");
- cereal::XMLOutputArchive xml_archive(os);
-
- ASSERT_NO_THROW( xml_archive( nb ) );
- }
-
- {
- std::ifstream is(path_to_io + "intrinsics_odom2d_ptr_serialization.xml");
- cereal::XMLInputArchive xml_archive(is);
-
- wolf::IntrinsicsBasePtr nb;
-
- ASSERT_NO_THROW( xml_archive( nb ) );
-
- wolf::IntrinsicsOdom2DPtr nb_cast =
- std::dynamic_pointer_cast<wolf::IntrinsicsOdom2D>(nb);
-
- ASSERT_TRUE(nb_cast != nullptr);
-
- ASSERT_EQ(nb_cast->type, nb_.type);
- ASSERT_EQ(nb_cast->name, nb_.name);
- ASSERT_EQ(nb_cast->k_disp_to_disp, nb_.k_disp_to_disp);
- ASSERT_EQ(nb_cast->k_rot_to_rot, nb_.k_rot_to_rot);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationIntrinsicsOdom2D::"
- "CerealSerializationIntrinsicsOdom2DPtrXML !\n");
-}
-
-TEST_F(WolfTestCerealSerializationIntrinsicsOdom2D,
- CerealSerializationIntrinsicsOdom2DJSON)
-{
- {
- wolf::IntrinsicsOdom2D nb;
- nb.type = nb_.type;
- nb.name = nb_.name;
- nb.k_disp_to_disp = nb_.k_disp_to_disp;
- nb.k_rot_to_rot = nb_.k_rot_to_rot;
-
- std::ofstream os(path_to_io + "intrinsics_odom2d_serialization.json");
- cereal::JSONOutputArchive json_archive(os);
-
- ASSERT_NO_THROW( json_archive( nb ) );
- }
-
- {
- std::ifstream is(path_to_io + "intrinsics_odom2d_serialization.json");
- cereal::JSONInputArchive json_archive(is);
-
- wolf::IntrinsicsOdom2D nb;
-
- ASSERT_NO_THROW( json_archive( nb ) );
-
- ASSERT_EQ(nb.type, nb_.type);
- ASSERT_EQ(nb.name, nb_.name);
- ASSERT_EQ(nb.k_disp_to_disp, nb_.k_disp_to_disp);
- ASSERT_EQ(nb.k_rot_to_rot, nb_.k_rot_to_rot);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationIntrinsicsOdom2D::"
- "CerealSerializationIntrinsicsOdom2DJSON !\n");
-}
-
-TEST_F(WolfTestCerealSerializationIntrinsicsOdom2D,
- CerealSerializationIntrinsicsOdom2DPtrJSON)
-{
- {
- wolf::IntrinsicsBasePtr nb = std::make_shared<wolf::IntrinsicsOdom2D>(nb_);
-
- std::ofstream os(path_to_io + "intrinsics_odom2d_ptr_serialization.json");
- cereal::JSONOutputArchive json_archive(os);
-
- ASSERT_NO_THROW( json_archive( nb ) );
- }
-
- {
- std::ifstream is(path_to_io + "intrinsics_odom2d_ptr_serialization.json");
- cereal::JSONInputArchive json_archive(is);
-
- wolf::IntrinsicsBasePtr nb;
-
- ASSERT_NO_THROW( json_archive( nb ) );
-
- wolf::IntrinsicsOdom2DPtr nb_cast =
- std::dynamic_pointer_cast<wolf::IntrinsicsOdom2D>(nb);
-
- ASSERT_TRUE(nb_cast != nullptr);
-
- ASSERT_EQ(nb_cast->type, nb_.type);
- ASSERT_EQ(nb_cast->name, nb_.name);
- ASSERT_EQ(nb_cast->k_disp_to_disp, nb_.k_disp_to_disp);
- ASSERT_EQ(nb_cast->k_rot_to_rot, nb_.k_rot_to_rot);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationIntrinsicsOdom2D::"
- "CerealSerializationIntrinsicsOdom2DPtrJSON !\n");
-}
-
-TEST_F(WolfTestCerealSerializationIntrinsicsOdom2D,
- CerealSerializationIntrinsicsOdom2DBinary)
-{
- {
- wolf::IntrinsicsOdom2D nb;
- nb.type = nb_.type;
- nb.name = nb_.name;
- nb.k_disp_to_disp = nb_.k_disp_to_disp;
- nb.k_rot_to_rot = nb_.k_rot_to_rot;
-
- std::ofstream os(path_to_io + "intrinsics_odom2d_serialization.bin");
- cereal::BinaryOutputArchive bin_archive(os);
-
- ASSERT_NO_THROW( bin_archive( nb ) );
- }
-
- {
- std::ifstream is(path_to_io + "intrinsics_odom2d_serialization.bin");
- cereal::BinaryInputArchive bin_archive(is);
-
- wolf::IntrinsicsOdom2D nb;
-
- ASSERT_NO_THROW( bin_archive( nb ) );
-
- ASSERT_EQ(nb.type, nb_.type);
- ASSERT_EQ(nb.name, nb_.name);
- ASSERT_EQ(nb.k_disp_to_disp, nb_.k_disp_to_disp);
- ASSERT_EQ(nb.k_rot_to_rot, nb_.k_rot_to_rot);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationIntrinsicsOdom2D::"
- "CerealSerializationIntrinsicsOdom2DBinary !\n");
-}
-
-TEST_F(WolfTestCerealSerializationIntrinsicsOdom2D, CerealSerializationIntrinsicsOdom2DPtrBinary)
-{
- {
- wolf::IntrinsicsBasePtr nb = std::make_shared<wolf::IntrinsicsOdom2D>(nb_);
-
- std::ofstream os(path_to_io + "intrinsics_odom2d_ptr_serialization.bin");
- cereal::BinaryOutputArchive bin_archive(os);
-
- ASSERT_NO_THROW( bin_archive( nb ) );
- }
-
- {
- std::ifstream is(path_to_io + "intrinsics_odom2d_ptr_serialization.bin");
- cereal::BinaryInputArchive bin_archive(is);
-
- wolf::IntrinsicsBasePtr nb;
-
- ASSERT_NO_THROW( bin_archive( nb ) );
-
- wolf::IntrinsicsOdom2DPtr nb_cast =
- std::dynamic_pointer_cast<wolf::IntrinsicsOdom2D>(nb);
-
- ASSERT_TRUE(nb_cast != nullptr);
-
- ASSERT_EQ(nb_cast->type, nb_.type);
- ASSERT_EQ(nb_cast->name, nb_.name);
- ASSERT_EQ(nb_cast->k_disp_to_disp, nb_.k_disp_to_disp);
- ASSERT_EQ(nb_cast->k_rot_to_rot, nb_.k_rot_to_rot);
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationIntrinsicsOdom2D::"
- "CerealSerializationIntrinsicsOdom2DPtrBinary !\n");
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
diff --git a/test/serialization/cereal/gtest_serialization_time_stamp.cpp b/test/serialization/cereal/gtest_serialization_time_stamp.cpp
deleted file mode 100644
index 8dc4cd479ad8c3986fbd4286899ab1081b1a9af0..0000000000000000000000000000000000000000
--- a/test/serialization/cereal/gtest_serialization_time_stamp.cpp
+++ /dev/null
@@ -1,62 +0,0 @@
-/*
- * gtest_intrinsics_odom2d_serialization.cpp
- *
- * Created on: Jul 16, 2017
- * Author: Jeremie Deray
- */
-
-#include "../../utils_gtest.h"
-
-#include "../../../serialization/cereal/serialization_time_stamp.h"
-
-#include "../../../serialization/cereal/io.h"
-
-#include <cereal/types/memory.hpp>
-#include <fstream>
-
-class WolfTestCerealSerializationTimeStamp : public testing::Test
-{
-public:
-
- WolfTestCerealSerializationTimeStamp()
- {
- nb_.setToNow();
- }
-
- const std::string path_to_io = "/tmp/";
- const std::string filename = "serialization_time_stamp";
- const std::string ptr_ext = "_ptr";
-
- const std::vector<std::string> exts = {".bin", ".xml", ".json"};
-
- wolf::TimeStamp nb_;
-};
-
-TEST_F(WolfTestCerealSerializationTimeStamp,
- CerealSerializationTimeStamp)
-{
- for (const auto ext : exts)
- {
- const std::string full_path = path_to_io + filename + ext;
-
- ASSERT_NO_THROW( wolf::save( full_path, nb_ ) )
- << "Failed on saving " << full_path;
-
- wolf::TimeStamp nb_load;
-
- ASSERT_NO_THROW( wolf::load( full_path, nb_load ) )
- << "Failed on loading " << full_path;
-
- ASSERT_EQ(nb_load, nb_) << full_path;
- }
-
- PRINTF("All good at "
- "WolfTestCerealSerializationTimeStamp::"
- "CerealSerializationTimeStamp !\n");
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}