From d860c2b0b185a38a81e3d1d1464cc01d0a712ca5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joaquim=20Casals=20Bu=C3=B1uel?= <jcasals@iri.upc.edu> Date: Mon, 28 Jan 2019 14:49:26 +0100 Subject: [PATCH] First step refactor, examples & hello_wolf working --- CMakeLists.txt | 33 ++++--- hello_wolf/capture_range_bearing.h | 2 +- hello_wolf/constraint_bearing.h | 3 +- hello_wolf/constraint_range_bearing.h | 3 +- hello_wolf/feature_range_bearing.cpp | 1 - hello_wolf/feature_range_bearing.h | 2 +- hello_wolf/hello_wolf.cpp | 19 +--- hello_wolf/landmark_point_2D.h | 2 +- hello_wolf/processor_range_bearing.cpp | 3 +- hello_wolf/processor_range_bearing.h | 3 +- hello_wolf/sensor_range_bearing.cpp | 7 +- hello_wolf/sensor_range_bearing.h | 4 +- include/base/IMU_tools.h | 13 +-- include/base/association/association_nnls.h | 3 +- include/base/association/association_node.h | 5 +- include/base/association/association_solver.h | 2 +- include/base/association/association_tree.h | 7 +- {src => include/base}/association/matrix.h | 1 - include/base/capture/capture_GPS.h | 3 +- include/base/capture/capture_GPS_fix.h | 4 +- include/base/capture/capture_IMU.h | 4 +- include/base/capture/capture_base.h | 17 ++-- include/base/capture/capture_buffer.h | 5 +- include/base/capture/capture_image.h | 6 +- include/base/capture/capture_laser_2D.h | 5 +- include/base/capture/capture_motion.h | 4 +- include/base/capture/capture_odom_2D.h | 6 +- include/base/capture/capture_odom_3D.h | 5 +- include/base/capture/capture_pose.h | 8 +- include/base/capture/capture_velocity.h | 2 +- include/base/capture/capture_void.h | 3 +- .../capture/capture_wheel_joint_position.h | 4 +- include/base/ceres_wrapper/ceres_manager.h | 6 +- .../ceres_wrapper/cost_function_wrapper.h | 6 +- .../create_numeric_diff_cost_function.h | 6 +- .../local_parametrization_wrapper.h | 4 +- include/base/ceres_wrapper/qr_manager.h | 4 +- include/base/ceres_wrapper/solver_manager.h | 6 +- include/base/constraint/constraint_AHP.h | 13 +-- include/base/constraint/constraint_GPS_2D.h | 6 +- .../constraint_GPS_pseudorange_2D.h | 14 +-- .../constraint_GPS_pseudorange_3D.h | 9 +- include/base/constraint/constraint_IMU.h | 29 +----- include/base/constraint/constraint_analytic.h | 4 +- include/base/constraint/constraint_autodiff.h | 13 +-- .../constraint_autodiff_distance_3D.h | 2 +- .../constraint/constraint_autodiff_trifocal.h | 16 +--- include/base/constraint/constraint_base.h | 15 ++- .../constraint/constraint_block_absolute.h | 5 +- .../base/constraint/constraint_container.h | 7 +- .../base/constraint/constraint_corner_2D.h | 6 +- .../base/constraint/constraint_diff_drive.h | 8 +- include/base/constraint/constraint_epipolar.h | 3 +- include/base/constraint/constraint_fix_bias.h | 11 +-- include/base/constraint/constraint_odom_2D.h | 6 +- .../constraint/constraint_odom_2D_analytic.h | 3 +- include/base/constraint/constraint_odom_3D.h | 6 +- include/base/constraint/constraint_point_2D.h | 6 +- .../constraint/constraint_point_to_line_2D.h | 6 +- include/base/constraint/constraint_pose_2D.h | 7 +- include/base/constraint/constraint_pose_3D.h | 7 +- .../constraint_quaternion_absolute.h | 9 +- .../constraint_relative_2D_analytic.h | 5 +- .../base}/diff_drive_tools.h | 5 +- .../base}/diff_drive_tools.hpp | 0 include/base/eigen_predicates.h | 3 +- include/base/factory.h | 7 +- include/base/feature/feature_GPS_fix.h | 4 +- .../base/feature/feature_GPS_pseudorange.h | 3 +- include/base/feature/feature_IMU.h | 9 +- include/base/feature/feature_base.h | 7 +- include/base/feature/feature_corner_2D.h | 3 +- .../base}/feature/feature_diff_drive.h | 2 +- include/base/feature/feature_line_2D.h | 3 +- include/base/feature/feature_match.h | 3 +- include/base/feature/feature_motion.h | 4 +- include/base/feature/feature_odom_2D.h | 7 +- include/base/feature/feature_point_image.h | 3 +- include/base/feature/feature_polyline_2D.h | 2 +- include/base/feature/feature_pose.h | 3 +- include/base/frame_base.h | 19 ++-- include/base/hardware_base.h | 4 +- include/base/landmark/landmark_AHP.h | 3 +- include/base/landmark/landmark_base.h | 15 ++- include/base/landmark/landmark_container.h | 6 +- include/base/landmark/landmark_corner_2D.h | 3 +- include/base/landmark/landmark_line_2D.h | 6 +- include/base/landmark/landmark_match.h | 3 +- include/base/landmark/landmark_polyline_2D.h | 2 +- include/base/local_parametrization_angle.h | 4 +- include/base/local_parametrization_base.h | 3 +- .../base/local_parametrization_homogeneous.h | 3 +- .../local_parametrization_polyline_extreme.h | 3 +- .../base/local_parametrization_quaternion.h | 3 +- include/base/logging.h | 2 +- include/base/map_base.h | 4 +- include/base/motion_buffer.h | 6 +- include/base/node_base.h | 3 +- include/base/pinhole_tools.h | 20 +--- include/base/problem.h | 16 +--- include/base/processor/processor_GPS.h | 5 +- include/base/processor/processor_IMU.h | 7 +- include/base/processor/processor_base.h | 14 ++- .../base/processor/processor_capture_holder.h | 6 +- .../base}/processor/processor_diff_drive.h | 4 +- include/base/processor/processor_factory.h | 13 ++- .../processor_frame_nearest_neighbor_filter.h | 4 +- include/base/processor/processor_logging.h | 2 +- .../processor/processor_loopclosure_base.h | 4 +- include/base/processor/processor_motion.h | 18 +--- include/base/processor/processor_odom_2D.h | 9 +- include/base/processor/processor_odom_3D.h | 12 +-- .../base/processor/processor_params_image.h | 4 +- include/base/processor/processor_tracker.h | 4 +- .../processor/processor_tracker_feature.h | 10 +- .../processor_tracker_feature_corner.h | 17 ++-- .../processor_tracker_feature_dummy.h | 6 +- .../processor_tracker_feature_image.h | 16 ++-- .../processor/processor_tracker_landmark.h | 9 +- .../processor_tracker_landmark_corner.h | 18 ++-- .../processor_tracker_landmark_dummy.h | 2 +- .../processor_tracker_landmark_image.h | 15 +-- .../processor_tracker_landmark_polyline.h | 21 ++--- include/base/rotations.h | 14 +-- include/base/sensor/sensor_GPS.h | 5 +- include/base/sensor/sensor_GPS_fix.h | 5 +- include/base/sensor/sensor_IMU.h | 2 +- include/base/sensor/sensor_base.h | 17 ++-- include/base/sensor/sensor_camera.h | 3 +- .../base}/sensor/sensor_diff_drive.h | 4 +- include/base/sensor/sensor_factory.h | 4 +- include/base/sensor/sensor_laser_2D.h | 2 +- include/base/sensor/sensor_odom_2D.h | 3 +- include/base/sensor/sensor_odom_3D.h | 3 +- include/base/solver/solver_manager.h | 6 +- .../solver_suitesparse/ccolamd_ordering.h | 3 - .../solver_suitesparse/cost_function_base.h | 3 +- .../solver_suitesparse/cost_function_sparse.h | 86 +----------------- .../cost_function_sparse_base.h | 2 +- include/base/solver_suitesparse/qr_solver.h | 12 +-- include/base/solver_suitesparse/solver_QR.h | 1 - .../base/solver_suitesparse/solver_manager.h | 7 +- include/base/state_angle.h | 4 +- include/base/state_block.h | 8 +- include/base/state_homogeneous_3D.h | 4 +- include/base/state_quaternion.h | 4 +- include/base/time_stamp.h | 2 +- include/base/track_matrix.h | 4 +- include/base/trajectory_base.h | 6 +- include/base/wolf.h | 8 +- serialization/cereal/io.h | 1 - ...serialization_local_parametrization_base.h | 3 +- ...zation_local_parametrization_homogeneous.h | 2 +- ...ization_local_parametrization_quaternion.h | 2 +- .../cereal/serialization_node_base.h | 2 +- .../serialization_processor_odom2d_params.h | 2 +- .../serialization_processor_odom3d_params.h | 2 +- .../serialization_processor_params_base.h | 2 +- .../serialization_sensor_intrinsic_base.h | 2 +- .../serialization_sensor_odom2d_intrinsic.h | 2 +- .../cereal/serialization_time_stamp.h | 2 +- src/SE3.h | 10 +- src/association/association_nnls.cpp | 2 +- src/association/association_node.cpp | 2 +- src/association/association_solver.cpp | 2 +- src/association/association_tree.cpp | 2 +- src/capture/capture_GPS.cpp | 4 +- src/capture/capture_GPS_fix.cpp | 4 +- src/capture/capture_IMU.cpp | 9 +- src/capture/capture_base.cpp | 8 +- src/capture/capture_image.cpp | 3 +- src/capture/capture_laser_2D.cpp | 2 +- src/capture/capture_motion.cpp | 6 +- src/capture/capture_odom_2D.cpp | 2 +- src/capture/capture_odom_3D.cpp | 4 +- src/capture/capture_pose.cpp | 3 +- src/capture/capture_velocity.cpp | 2 +- src/capture/capture_void.cpp | 3 +- src/capture/capture_wheel_joint_position.cpp | 6 +- src/ceres_wrapper/ceres_manager.cpp | 12 +-- .../local_parametrization_wrapper.cpp | 2 +- src/ceres_wrapper/qr_manager.cpp | 1 - src/ceres_wrapper/solver_manager.cpp | 8 +- src/constraint/constraint_analytic.cpp | 4 +- src/constraint/constraint_base.cpp | 6 +- {examples => src/examples}/.gitignore | 0 {examples => src/examples}/ACTIVESEARCH.yaml | 0 {examples => src/examples}/CMakeLists.txt | 0 {examples => src/examples}/Test_ORB.png | Bin {examples => src/examples}/camera_params.yaml | 0 .../examples}/camera_params_canonical.yaml | 0 .../examples}/camera_params_ueye.yaml | 0 .../camera_params_ueye_radial_dist.yaml | 0 .../examples}/camera_params_ueye_sim.yaml | 0 .../examples}/input_M3500b_toro.graph | 0 .../examples}/map_polyline_example.yaml | 0 .../examples}/processor_image_feature.yaml | 0 .../processor_image_vision_utils.yaml | 0 {examples => src/examples}/processor_imu.yaml | 0 .../examples}/processor_imu_no_vote.yaml | 0 .../examples}/processor_imu_t1.yaml | 0 .../examples}/processor_imu_t6.yaml | 0 .../examples}/processor_odom_3D.yaml | 0 .../processor_tracker_feature_trifocal.yaml | 0 {examples => src/examples}/sensor_imu.yaml | 0 .../examples}/sensor_odom_3D.yaml | 0 .../examples}/sensor_odom_3D_HQ.yaml | 0 .../examples}/solver/test_SPQR.cpp | 1 - .../examples}/solver/test_ccolamd.cpp | 2 +- .../examples}/solver/test_ccolamd_blocks.cpp | 5 - .../examples}/solver/test_iQR.cpp | 7 -- .../examples}/solver/test_iQR_wolf.cpp | 8 -- .../examples}/solver/test_iQR_wolf2.cpp | 17 +--- .../test_incremental_ccolamd_blocks.cpp | 9 -- .../examples}/solver/test_permutations.cpp | 3 - .../examples}/test_2_lasers_offline.cpp | 18 ++-- .../test_analytic_odom_constraint.cpp | 8 +- {examples => src/examples}/test_autodiff.cpp | 5 +- .../examples}/test_capture_laser_2D.cpp | 2 +- .../examples}/test_ceres_2_lasers.cpp | 17 ++-- .../test_ceres_2_lasers_polylines.cpp | 17 ++-- .../examples}/test_constraint_AHP.cpp | 21 ++--- .../examples}/test_constraint_imu.cpp | 22 ++--- .../examples}/test_constraint_odom_3D.cpp | 2 +- .../examples}/test_diff_drive.cpp | 10 +- .../examples}/test_eigen_quaternion.cpp | 2 +- .../examples}/test_eigen_template.cpp | 3 - .../examples}/test_faramotics_simulation.cpp | 10 +- {examples => src/examples}/test_fcn_ptr.cpp | 2 - {examples => src/examples}/test_image.cpp | 8 +- {examples => src/examples}/test_imuDock.cpp | 18 ++-- .../examples}/test_imuDock_autoKFs.cpp | 18 ++-- .../examples}/test_imuPlateform_Offline.cpp | 25 +++-- .../examples}/test_imu_constrained0.cpp | 27 +++--- .../examples}/test_kf_callback.cpp | 10 +- .../examples}/test_list_remove.cpp | 2 - {examples => src/examples}/test_map_yaml.cpp | 15 ++- .../examples}/test_matrix_prod.cpp | 2 - {examples => src/examples}/test_mpu.cpp | 15 ++- .../examples}/test_processor_imu.cpp | 16 ++-- .../test_processor_imu_jacobians.cpp | 13 ++- .../examples}/test_processor_odom_3D.cpp | 17 ++-- .../test_processor_tracker_feature.cpp | 12 +-- .../test_processor_tracker_landmark.cpp | 15 ++- .../test_processor_tracker_landmark_image.cpp | 23 ++--- .../examples}/test_projection_points.cpp | 43 +-------- {examples => src/examples}/test_sh_ptr.cpp | 8 -- .../examples}/test_simple_AHP.cpp | 38 ++------ .../examples}/test_sort_keyframes.cpp | 10 +- .../examples}/test_sparsification.cpp | 9 +- .../examples}/test_state_quaternion.cpp | 8 +- .../examples}/test_tracker_ORB.cpp | 3 +- .../examples}/test_virtual_hierarchy.cpp | 0 .../examples}/test_wolf_autodiffwrapper.cpp | 6 +- .../examples}/test_wolf_factories.cpp | 22 ++--- .../examples}/test_wolf_imported_graph.cpp | 8 +- .../examples}/test_wolf_logging.cpp | 4 +- .../examples}/test_wolf_prunning.cpp | 9 +- {examples => src/examples}/test_wolf_root.cpp | 2 +- {examples => src/examples}/test_wolf_tree.cpp | 0 {examples => src/examples}/test_yaml.cpp | 8 +- .../examples}/test_yaml_conversions.cpp | 4 +- .../examples}/vision_utils_active_search.yaml | 0 src/feature/feature_GPS_fix.cpp | 2 +- src/feature/feature_GPS_pseudorange.cpp | 2 +- src/feature/feature_IMU.cpp | 2 +- src/feature/feature_base.cpp | 6 +- src/feature/feature_corner_2D.cpp | 2 +- src/feature/feature_diff_drive.cpp | 2 +- src/feature/feature_line_2D.cpp | 2 +- src/feature/feature_odom_2D.cpp | 2 +- src/feature/feature_point_image.cpp | 2 +- src/feature/feature_polyline_2D.cpp | 2 +- src/feature/feature_pose.cpp | 3 +- src/frame_base.cpp | 18 ++-- src/hardware_base.cpp | 5 +- src/landmark/landmark_AHP.cpp | 6 +- src/landmark/landmark_base.cpp | 12 +-- src/landmark/landmark_container.cpp | 4 +- src/landmark/landmark_corner_2D.cpp | 3 +- src/landmark/landmark_line_2D.cpp | 4 +- src/landmark/landmark_point_3D.h | 3 +- src/landmark/landmark_polyline_2D.cpp | 18 ++-- src/local_parametrization_base.cpp | 3 +- src/local_parametrization_homogeneous.cpp | 4 +- ...local_parametrization_polyline_extreme.cpp | 6 +- src/local_parametrization_quaternion.cpp | 7 +- src/map_base.cpp | 9 +- src/motion_buffer.cpp | 4 +- src/node_base.cpp | 2 +- src/problem.cpp | 37 +++----- src/processor/processor_GPS.cpp | 9 +- src/processor/processor_IMU.cpp | 9 +- src/processor/processor_base.cpp | 12 +-- src/processor/processor_capture_holder.cpp | 5 +- src/processor/processor_diff_drive.cpp | 16 ++-- ...rocessor_frame_nearest_neighbor_filter.cpp | 2 +- src/processor/processor_loopclosure_base.cpp | 2 +- src/processor/processor_motion.cpp | 8 +- src/processor/processor_odom_2D.cpp | 7 +- src/processor/processor_odom_3D.cpp | 7 +- src/processor/processor_tracker.cpp | 6 +- src/processor/processor_tracker_feature.cpp | 2 +- .../processor_tracker_feature_corner.cpp | 4 +- .../processor_tracker_feature_dummy.cpp | 4 +- .../processor_tracker_feature_image.cpp | 5 +- .../processor_tracker_feature_trifocal.cpp | 10 +- .../processor_tracker_feature_trifocal.h | 5 +- src/processor/processor_tracker_landmark.cpp | 5 +- .../processor_tracker_landmark_corner.cpp | 6 +- .../processor_tracker_landmark_dummy.cpp | 6 +- .../processor_tracker_landmark_image.cpp | 34 +++---- .../processor_tracker_landmark_polyline.cpp | 7 +- src/sensor/sensor_GPS.cpp | 10 +- src/sensor/sensor_GPS_fix.cpp | 10 +- src/sensor/sensor_IMU.cpp | 10 +- src/sensor/sensor_base.cpp | 11 +-- src/sensor/sensor_camera.cpp | 14 ++- src/sensor/sensor_diff_drive.cpp | 12 +-- src/sensor/sensor_laser_2D.cpp | 10 +- src/sensor/sensor_odom_2D.cpp | 10 +- src/sensor/sensor_odom_3D.cpp | 10 +- src/solver/solver_manager.cpp | 8 +- src/solver_suitesparse/solver_manager.cpp | 2 +- src/state_block.cpp | 2 +- src/time_stamp.cpp | 4 +- src/track_matrix.cpp | 2 +- src/trajectory_base.cpp | 6 +- src/yaml/processor_IMU_yaml.cpp | 6 +- src/yaml/processor_image_yaml.cpp | 8 +- src/yaml/processor_odom_3D_yaml.cpp | 6 +- ...rocessor_tracker_feature_trifocal_yaml.cpp | 8 +- src/yaml/sensor_IMU_yaml.cpp | 6 +- src/yaml/sensor_camera_yaml.cpp | 10 +- src/yaml/sensor_laser_2D_yaml.cpp | 10 +- src/yaml/sensor_odom_3D_yaml.cpp | 6 +- test/gtest_IMU.cpp | 66 ++------------ test/gtest_IMU_tools.cpp | 5 +- test/gtest_capture_base.cpp | 5 +- test/gtest_ceres_manager.cpp | 25 +++-- test/gtest_constraint_IMU.cpp | 20 ++-- test/gtest_constraint_absolute.cpp | 10 +- test/gtest_constraint_autodiff.cpp | 13 ++- .../gtest_constraint_autodiff_distance_3D.cpp | 10 +- test/gtest_constraint_autodiff_trifocal.cpp | 10 +- test/gtest_constraint_odom_3D.cpp | 8 +- test/gtest_constraint_pose_2D.cpp | 9 +- test/gtest_constraint_pose_3D.cpp | 11 +-- test/gtest_constraint_sparse.cpp | 1 - test/gtest_eigen_predicates.cpp | 2 +- test/gtest_feature_IMU.cpp | 16 ++-- test/gtest_feature_base.cpp | 3 +- test/gtest_frame_base.cpp | 24 ++--- test/gtest_local_param.cpp | 13 +-- test/gtest_make_posdef.cpp | 2 +- test/gtest_motion_buffer.cpp | 8 +- test/gtest_odom_2D.cpp | 20 ++-- test/gtest_odom_3D.cpp | 19 +--- test/gtest_pack_KF_buffer.cpp | 10 +- test/gtest_pinhole.cpp | 4 +- test/gtest_problem.cpp | 20 ++-- test/gtest_processor_IMU.cpp | 19 ++-- test/gtest_processor_IMU_jacobians.cpp | 16 ++-- test/gtest_processor_base.cpp | 11 +-- ...essor_frame_nearest_neighbor_filter_2D.cpp | 7 +- test/gtest_processor_motion.cpp | 12 +-- ...est_processor_tracker_feature_trifocal.cpp | 12 +-- test/gtest_rotation.cpp | 10 +- test/gtest_sensor_base.cpp | 3 +- test/gtest_shared_from_this.cpp | 5 +- test/gtest_solver_manager.cpp | 20 ++-- test/gtest_time_stamp.cpp | 2 +- test/gtest_track_matrix.cpp | 2 +- test/gtest_trajectory.cpp | 10 +- test/processor_IMU_UnitTester.h | 10 +- .../gtest_serialization_eigen_geometry.cpp | 2 +- .../gtest_serialization_eigen_sparse.cpp | 2 +- ...st_serialization_local_parametrization.cpp | 1 - test/utils_gtest.h | 3 - wolf_scripts/include_refactor.sh | 39 ++++---- 380 files changed, 1019 insertions(+), 1838 deletions(-) rename {src => include/base}/association/matrix.h (99%) rename {src/sensor => include/base}/diff_drive_tools.h (99%) rename {src/sensor => include/base}/diff_drive_tools.hpp (100%) rename {src => include/base}/feature/feature_diff_drive.h (95%) rename {src => include/base}/processor/processor_diff_drive.h (98%) rename {src => include/base}/sensor/sensor_diff_drive.h (97%) rename {examples => src/examples}/.gitignore (100%) rename {examples => src/examples}/ACTIVESEARCH.yaml (100%) rename {examples => src/examples}/CMakeLists.txt (100%) rename {examples => src/examples}/Test_ORB.png (100%) rename {examples => src/examples}/camera_params.yaml (100%) rename {examples => src/examples}/camera_params_canonical.yaml (100%) rename {examples => src/examples}/camera_params_ueye.yaml (100%) rename {examples => src/examples}/camera_params_ueye_radial_dist.yaml (100%) rename {examples => src/examples}/camera_params_ueye_sim.yaml (100%) rename {examples => src/examples}/input_M3500b_toro.graph (100%) rename {examples => src/examples}/map_polyline_example.yaml (100%) rename {examples => src/examples}/processor_image_feature.yaml (100%) rename {examples => src/examples}/processor_image_vision_utils.yaml (100%) rename {examples => src/examples}/processor_imu.yaml (100%) rename {examples => src/examples}/processor_imu_no_vote.yaml (100%) rename {examples => src/examples}/processor_imu_t1.yaml (100%) rename {examples => src/examples}/processor_imu_t6.yaml (100%) rename {examples => src/examples}/processor_odom_3D.yaml (100%) rename {examples => src/examples}/processor_tracker_feature_trifocal.yaml (100%) rename {examples => src/examples}/sensor_imu.yaml (100%) rename {examples => src/examples}/sensor_odom_3D.yaml (100%) rename {examples => src/examples}/sensor_odom_3D_HQ.yaml (100%) rename {examples => src/examples}/solver/test_SPQR.cpp (99%) rename {examples => src/examples}/solver/test_ccolamd.cpp (99%) rename {examples => src/examples}/solver/test_ccolamd_blocks.cpp (99%) rename {examples => src/examples}/solver/test_iQR.cpp (99%) rename {examples => src/examples}/solver/test_iQR_wolf.cpp (99%) rename {examples => src/examples}/solver/test_iQR_wolf2.cpp (99%) rename {examples => src/examples}/solver/test_incremental_ccolamd_blocks.cpp (99%) rename {examples => src/examples}/solver/test_permutations.cpp (99%) rename {examples => src/examples}/test_2_lasers_offline.cpp (97%) rename {examples => src/examples}/test_analytic_odom_constraint.cpp (99%) rename {examples => src/examples}/test_autodiff.cpp (99%) rename {examples => src/examples}/test_capture_laser_2D.cpp (99%) rename {examples => src/examples}/test_ceres_2_lasers.cpp (98%) rename {examples => src/examples}/test_ceres_2_lasers_polylines.cpp (98%) rename {examples => src/examples}/test_constraint_AHP.cpp (96%) rename {examples => src/examples}/test_constraint_imu.cpp (97%) rename {examples => src/examples}/test_constraint_odom_3D.cpp (73%) rename {examples => src/examples}/test_diff_drive.cpp (97%) rename {examples => src/examples}/test_eigen_quaternion.cpp (97%) rename {examples => src/examples}/test_eigen_template.cpp (99%) rename {examples => src/examples}/test_faramotics_simulation.cpp (98%) rename {examples => src/examples}/test_fcn_ptr.cpp (99%) rename {examples => src/examples}/test_image.cpp (97%) rename {examples => src/examples}/test_imuDock.cpp (97%) rename {examples => src/examples}/test_imuDock_autoKFs.cpp (97%) rename {examples => src/examples}/test_imuPlateform_Offline.cpp (96%) rename {examples => src/examples}/test_imu_constrained0.cpp (97%) rename {examples => src/examples}/test_kf_callback.cpp (93%) rename {examples => src/examples}/test_list_remove.cpp (99%) rename {examples => src/examples}/test_map_yaml.cpp (94%) rename {examples => src/examples}/test_matrix_prod.cpp (99%) rename {examples => src/examples}/test_mpu.cpp (97%) rename {examples => src/examples}/test_processor_imu.cpp (97%) rename {examples => src/examples}/test_processor_imu_jacobians.cpp (99%) rename {examples => src/examples}/test_processor_odom_3D.cpp (91%) rename {examples => src/examples}/test_processor_tracker_feature.cpp (87%) rename {examples => src/examples}/test_processor_tracker_landmark.cpp (93%) rename {examples => src/examples}/test_processor_tracker_landmark_image.cpp (96%) rename {examples => src/examples}/test_projection_points.cpp (99%) rename {examples => src/examples}/test_sh_ptr.cpp (99%) rename {examples => src/examples}/test_simple_AHP.cpp (96%) rename {examples => src/examples}/test_sort_keyframes.cpp (95%) rename {examples => src/examples}/test_sparsification.cpp (98%) rename {examples => src/examples}/test_state_quaternion.cpp (90%) rename {examples => src/examples}/test_tracker_ORB.cpp (99%) rename {examples => src/examples}/test_virtual_hierarchy.cpp (100%) rename {examples => src/examples}/test_wolf_autodiffwrapper.cpp (99%) rename {examples => src/examples}/test_wolf_factories.cpp (92%) rename {examples => src/examples}/test_wolf_imported_graph.cpp (99%) rename {examples => src/examples}/test_wolf_logging.cpp (87%) rename {examples => src/examples}/test_wolf_prunning.cpp (99%) rename {examples => src/examples}/test_wolf_root.cpp (92%) rename {examples => src/examples}/test_wolf_tree.cpp (100%) rename {examples => src/examples}/test_yaml.cpp (98%) rename {examples => src/examples}/test_yaml_conversions.cpp (97%) rename {examples => src/examples}/vision_utils_active_search.yaml (100%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 2fb76c6fa..b904e0722 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -197,6 +197,8 @@ IF(EXISTS "${WOLF_CONFIG_DIR}" AND NOT IS_DIRECTORY "${WOLF_CONFIG_DIR}") ENDIF() # Configure config.h configure_file(${CMAKE_CURRENT_SOURCE_DIR}/internal/config.h.in "${WOLF_CONFIG_DIR}/config.h") +message("CONFIG DIRECTORY ${PROJECT_BINARY_DIR}") +include_directories("${PROJECT_BINARY_DIR}/conf") # include spdlog (logging library) FIND_PATH(SPDLOG_INCLUDE_DIR spdlog.h /usr/local/include/spdlog /usr/include/spdlog) IF (SPDLOG_INCLUDE_DIR) @@ -207,7 +209,8 @@ ELSE (SPDLOG_INCLUDE_DIR) ENDIF (SPDLOG_INCLUDE_DIR) INCLUDE_DIRECTORIES(${EIGEN_INCLUDE_DIRS}) - +include_directories("include") +include_directories("templinks") IF(Ceres_FOUND) INCLUDE_DIRECTORIES(${CERES_INCLUDE_DIRS}) ENDIF(Ceres_FOUND) @@ -283,8 +286,6 @@ include/base/constraint/constraint_pose_2D.h include/base/constraint/constraint_pose_3D.h include/base/constraint/constraint_quaternion_absolute.h include/base/constraint/constraint_relative_2D_analytic.h -src/sensor/diff_drive_tools.hpp - temp/diff_drive_tools.hpp include/base/feature/feature_corner_2D.h include/base/feature/feature_GPS_fix.h include/base/feature/feature_GPS_pseudorange.h @@ -460,7 +461,6 @@ src/processor/processor_base.cpp src/processor/processor_loopclosure_base.cpp src/processor/processor_motion.cpp src/processor/processor_tracker.cpp -examples/test_processor_tracker_feature.cpp src/sensor/sensor_base.cpp src/state_block.cpp src/time_stamp.cpp @@ -471,7 +471,7 @@ src/trajectory_base.cpp SET(SRCS_BASE src/capture/capture_motion.cpp src/processor/processor_capture_holder.cpp -examples/test_processor_tracker_landmark.cpp +# examples/test_processor_tracker_landmark.cpp ) SET(SRCS @@ -506,7 +506,7 @@ src/processor/processor_frame_nearest_neighbor_filter.cpp src/processor/processor_diff_drive.cpp src/processor/processor_IMU.cpp src/processor/processor_odom_2D.cpp -examples/test_processor_odom_3D.cpp +src/processor/processor_odom_3D.cpp src/processor/processor_tracker_feature_dummy.cpp src/processor/processor_tracker_landmark_dummy.cpp ) @@ -560,11 +560,14 @@ include/base/processor/processor_tracker_landmark_corner.h include/base/processor/processor_tracker_landmark_polyline.h ) SET(SRCS ${SRCS} -examples/test_capture_laser_2D.cpp +# examples/test_capture_laser_2D.cpp src/sensor/sensor_laser_2D.cpp +src/processor/processor_tracker_feature.cpp src/processor/processor_tracker_feature_corner.cpp src/processor/processor_tracker_landmark_corner.cpp src/processor/processor_tracker_landmark_polyline.cpp +src/processor/processor_tracker_landmark.cpp +src/processor/processor_tracker_landmark_dummy.cpp ) ENDIF(laser_scan_utils_FOUND) @@ -600,7 +603,7 @@ src/capture/capture_image.cpp src/feature/feature_point_image.cpp src/landmark/landmark_AHP.cpp src/processor/processor_tracker_feature_image.cpp -examples/test_processor_tracker_landmark_image.cpp +# examples/test_processor_tracker_landmark_image.cpp ) SET(SRCS_LANDMARK ${SRCS_LANDMARK} src/landmark/landmark_point_3D.cpp @@ -611,9 +614,7 @@ src/processor/processor_tracker_feature_trifocal.cpp ENDIF(vision_utils_FOUND) #SUBDIRECTORIES - -ADD_SUBDIRECTORY(hello_wolf) - +add_subdirectory(hello_wolf) IF (cereal_FOUND) ADD_SUBDIRECTORY(serialization/cereal) ENDIF(cereal_FOUND) @@ -669,9 +670,8 @@ ADD_LIBRARY(${PROJECT_NAME} ${SRCS_SOLVER} ${SRCS_WRAPPER} ) -TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${CMAKE_THREAD_LIBS_INIT}) +TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${PROJECT_NAME}_core ${CMAKE_THREAD_LIBS_INIT}) -TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${PROJECT_NAME}_core) #Link the created libraries #============================================================= IF (Ceres_FOUND) @@ -700,6 +700,13 @@ ENDIF (YAMLCPP_FOUND) IF (GLOG_FOUND) TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${GLOG_LIBRARY}) ENDIF (GLOG_FOUND) +IF (GLOG_FOUND) + IF(BUILD_TESTS) + MESSAGE("Building tests.") + add_subdirectory(test) + ENDIF(BUILD_TESTS) +ENDIF (GLOG_FOUND) + #-END_SRC -------------------------------------------------------------------------------------------------------------------------------- FIND_PACKAGE(Doxygen) diff --git a/hello_wolf/capture_range_bearing.h b/hello_wolf/capture_range_bearing.h index 6e18876cb..5a7e5a771 100644 --- a/hello_wolf/capture_range_bearing.h +++ b/hello_wolf/capture_range_bearing.h @@ -8,7 +8,7 @@ #ifndef HELLO_WOLF_CAPTURE_RANGE_BEARING_H_ #define HELLO_WOLF_CAPTURE_RANGE_BEARING_H_ -#include "capture_base.h" +#include "base/capture/capture_base.h" namespace wolf { diff --git a/hello_wolf/constraint_bearing.h b/hello_wolf/constraint_bearing.h index 1ce1c7d78..7849f17d4 100644 --- a/hello_wolf/constraint_bearing.h +++ b/hello_wolf/constraint_bearing.h @@ -8,7 +8,7 @@ #ifndef HELLO_WOLF_CONSTRAINT_BEARING_H_ #define HELLO_WOLF_CONSTRAINT_BEARING_H_ -#include "constraint_autodiff.h" +#include "base/constraint/constraint_autodiff.h" namespace wolf { @@ -46,7 +46,6 @@ class ConstraintBearing : public ConstraintAutodiff<ConstraintBearing, 1, 2, 1, } /* namespace wolf */ - //////////////// IMPLEMENTATION ////////////////////////////////////////// namespace wolf diff --git a/hello_wolf/constraint_range_bearing.h b/hello_wolf/constraint_range_bearing.h index 89b513786..cf7af72a4 100644 --- a/hello_wolf/constraint_range_bearing.h +++ b/hello_wolf/constraint_range_bearing.h @@ -8,7 +8,7 @@ #ifndef HELLO_WOLF_CONSTRAINT_RANGE_BEARING_H_ #define HELLO_WOLF_CONSTRAINT_RANGE_BEARING_H_ -#include "constraint_autodiff.h" +#include "base/constraint/constraint_autodiff.h" namespace wolf { @@ -60,7 +60,6 @@ class ConstraintRangeBearing : public ConstraintAutodiff<ConstraintRangeBearing, } /* namespace wolf */ - ////////////// IMPLEMENTATION ////////////////////////////////// namespace wolf diff --git a/hello_wolf/feature_range_bearing.cpp b/hello_wolf/feature_range_bearing.cpp index ce35c61f3..3be936567 100644 --- a/hello_wolf/feature_range_bearing.cpp +++ b/hello_wolf/feature_range_bearing.cpp @@ -16,7 +16,6 @@ FeatureRangeBearing::FeatureRangeBearing(const Eigen::VectorXs& _measurement, co // } - FeatureRangeBearing::~FeatureRangeBearing() { // diff --git a/hello_wolf/feature_range_bearing.h b/hello_wolf/feature_range_bearing.h index e3d30bba2..9da7215a5 100644 --- a/hello_wolf/feature_range_bearing.h +++ b/hello_wolf/feature_range_bearing.h @@ -8,7 +8,7 @@ #ifndef HELLO_WOLF_FEATURE_RANGE_BEARING_H_ #define HELLO_WOLF_FEATURE_RANGE_BEARING_H_ -#include "feature_base.h" +#include "base/feature/feature_base.h" namespace wolf { diff --git a/hello_wolf/hello_wolf.cpp b/hello_wolf/hello_wolf.cpp index 51ef873b3..646f1c2d6 100644 --- a/hello_wolf/hello_wolf.cpp +++ b/hello_wolf/hello_wolf.cpp @@ -12,11 +12,10 @@ * \author: jsola */ +#include "base/wolf.h" -#include "wolf.h" - -#include "sensor_odom_2D.h" -#include "processor_odom_2D.h" +#include "base/sensor/sensor_odom_2D.h" +#include "base/processor/processor_odom_2D.h" #include "sensor_range_bearing.h" #include "processor_range_bearing.h" #include "capture_range_bearing.h" @@ -24,24 +23,21 @@ #include "constraint_range_bearing.h" #include "landmark_point_2D.h" -#include "ceres_wrapper/ceres_manager.h" +#include "base/ceres_wrapper/ceres_manager.h" int main() { /* * ============= PROBLEM DEFINITION ================== * - * * We have a planar robot with a range-and-bearing sensor 'S' mounted at its front-left corner, looking forward: * * ^ Y * | * ------------------S-> sensor at location (1,1) and orientation 0 degrees, that is, at pose (1,1,0). * | | | - * | | | * | +--------|--> X robot axes X, Y * | | - * | | * ------------------- * * The robot performs a straight trajectory with 3 keyframes 'KF', and observes 3 landmarks 'L'. @@ -62,7 +58,6 @@ int main() * KF1->---KF2->---KF3-> KEYFRAMES -- robot poses * (0,0,0) (1,0,0) (2,0,0) keyframe poses in world frame * | - * | * * prior Initial robot pose in world frame * (0,0,0) * @@ -82,7 +77,6 @@ int main() * - Observations have ranges 1 or sqrt(2) * - Observations have bearings pi/2 or 3pi/4 * - * * The robot starts at (0,0,0) with a map with no previously known landmarks. * At each keyframe, it does: * - Create a motion factor to the previous keyframe @@ -143,7 +137,6 @@ int main() params_rb->time_tolerance = 0.01; ProcessorBasePtr processor_rb = problem->installProcessor("RANGE BEARING", "processor RB", sensor_rb, params_rb); - // SELF CALIBRATION =================================================== // NOTE: SELF-CALIBRATION OF SENSOR ORIENTATION @@ -154,7 +147,6 @@ int main() // The position is however not observable, and thus self-calibration would not work. You can try uncommenting it too. // sensor_rb->getPPtr()->unfix(); - // CONFIGURE ========================================================== // Motion data @@ -165,7 +157,6 @@ int main() VectorXi ids; VectorXs ranges, bearings; - // SET OF EVENTS ======================================================= std::cout << std::endl; WOLF_TRACE("======== BUILD PROBLEM =======") @@ -219,7 +210,6 @@ int main() sensor_rb ->process(cap_rb); // L1 : (1,2), L2 : (2,2), L3 : (3,2) problem->print(1,0,1,0); - // SOLVE ================================================================ // SOLVE with exact initial guess @@ -283,7 +273,6 @@ int main() * */ - /* * ============= DETAILED DESCRIPTION OF THE PRINTED RESULT ================== * diff --git a/hello_wolf/landmark_point_2D.h b/hello_wolf/landmark_point_2D.h index cb588f8eb..1bd845763 100644 --- a/hello_wolf/landmark_point_2D.h +++ b/hello_wolf/landmark_point_2D.h @@ -8,7 +8,7 @@ #ifndef HELLO_WOLF_LANDMARK_POINT_2D_H_ #define HELLO_WOLF_LANDMARK_POINT_2D_H_ -#include "landmark_base.h" +#include "base/landmark/landmark_base.h" namespace wolf { diff --git a/hello_wolf/processor_range_bearing.cpp b/hello_wolf/processor_range_bearing.cpp index 4f8e0dabf..670a61ea6 100644 --- a/hello_wolf/processor_range_bearing.cpp +++ b/hello_wolf/processor_range_bearing.cpp @@ -161,9 +161,8 @@ Eigen::Vector2s ProcessorRangeBearing::rect(Scalar range, Scalar bearing) const } /* namespace wolf */ - // Register in the SensorFactory -#include "processor_factory.h" +#include "base/processor/processor_factory.h" namespace wolf { WOLF_REGISTER_PROCESSOR("RANGE BEARING", ProcessorRangeBearing) diff --git a/hello_wolf/processor_range_bearing.h b/hello_wolf/processor_range_bearing.h index 5d3d2a8dd..6e8c77cd6 100644 --- a/hello_wolf/processor_range_bearing.h +++ b/hello_wolf/processor_range_bearing.h @@ -8,7 +8,7 @@ #ifndef HELLO_WOLF_PROCESSOR_RANGE_BEARING_H_ #define HELLO_WOLF_PROCESSOR_RANGE_BEARING_H_ -#include "processor_base.h" +#include "base/processor/processor_base.h" #include "sensor_range_bearing.h" #include "Eigen/Geometry" @@ -24,7 +24,6 @@ struct ProcessorParamsRangeBearing : public ProcessorParamsBase // We do not need special parameters, but in case you need they should be defined here. }; - using namespace Eigen; WOLF_PTR_TYPEDEFS(ProcessorRangeBearing); diff --git a/hello_wolf/sensor_range_bearing.cpp b/hello_wolf/sensor_range_bearing.cpp index d7de5aa0b..04d704664 100644 --- a/hello_wolf/sensor_range_bearing.cpp +++ b/hello_wolf/sensor_range_bearing.cpp @@ -6,7 +6,7 @@ */ #include "sensor_range_bearing.h" -#include "state_angle.h" +#include "base/state_angle.h" namespace wolf { @@ -28,7 +28,6 @@ SensorRangeBearing::~SensorRangeBearing() // } - SensorBasePtr SensorRangeBearing::create(const std::string& _unique_name, // const Eigen::VectorXs& _extrinsics, // const IntrinsicsBasePtr _intrinsics) @@ -45,12 +44,10 @@ SensorBasePtr SensorRangeBearing::create(const std::string& _unique_name, // } /* namespace wolf */ - // Register in the SensorFactory -#include "sensor_factory.h" +#include "base/sensor/sensor_factory.h" namespace wolf { WOLF_REGISTER_SENSOR("RANGE BEARING", SensorRangeBearing) } // namespace wolf - diff --git a/hello_wolf/sensor_range_bearing.h b/hello_wolf/sensor_range_bearing.h index 3e4e7c744..01ac8d93e 100644 --- a/hello_wolf/sensor_range_bearing.h +++ b/hello_wolf/sensor_range_bearing.h @@ -8,7 +8,7 @@ #ifndef HELLO_WOLF_SENSOR_RANGE_BEARING_H_ #define HELLO_WOLF_SENSOR_RANGE_BEARING_H_ -#include "sensor_base.h" +#include "base/sensor/sensor_base.h" namespace wolf { @@ -20,8 +20,6 @@ struct IntrinsicsRangeBearing : public IntrinsicsBase Scalar noise_bearing_degrees_std = 0.5; }; - - WOLF_PTR_TYPEDEFS(SensorRangeBearing) class SensorRangeBearing : public SensorBase diff --git a/include/base/IMU_tools.h b/include/base/IMU_tools.h index 6d2312235..eecad244c 100644 --- a/include/base/IMU_tools.h +++ b/include/base/IMU_tools.h @@ -8,9 +8,8 @@ #ifndef IMU_TOOLS_H_ #define IMU_TOOLS_H_ - -#include "wolf.h" -#include "rotations.h" +#include "base/wolf.h" +#include "base/rotations.h" /* * Most functions in this file are explained in the document: @@ -41,8 +40,6 @@ * - body2delta: construct a delta from body magnitudes of linAcc and angVel */ - - namespace wolf { namespace imu { @@ -110,7 +107,6 @@ inline void inverse(const MatrixBase<D1>& d, inverse(dp, dq, dv, dt, idp, idq, idv); } - template<typename D, class T> inline Matrix<typename D::Scalar, 10, 1> inverse(const MatrixBase<D>& d, T dt) @@ -247,7 +243,6 @@ inline void between(const MatrixBase<D1>& d1, between(dp1, dq1, dv1, dp2, dq2, dv2, dt, diff_p, diff_q, diff_v); } - template<typename D1, typename D2, class T> inline Matrix<typename D1::Scalar, 10, 1> between(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2, @@ -445,7 +440,6 @@ inline void diff(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1, const J_do_dq2 = jac_SO3_right_inv(diff_o); } - template<typename D1, typename D2, typename D3> inline void diff(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2, @@ -511,7 +505,6 @@ inline Matrix<typename D1::Scalar, 9, 1> diff(const MatrixBase<D1>& d1, return ret; } - template<typename D1, typename D2, typename D3, typename D4, typename D5> inline void body2delta(const MatrixBase<D1>& a, const MatrixBase<D2>& w, @@ -611,8 +604,6 @@ Matrix<typename D1::Scalar, 6, 1> motion2data(const MatrixBase<D1>& motion, cons return data; } - - } // namespace imu } // namespace wolf diff --git a/include/base/association/association_nnls.h b/include/base/association/association_nnls.h index 0883a7292..6563a1f66 100644 --- a/include/base/association/association_nnls.h +++ b/include/base/association/association_nnls.h @@ -7,8 +7,7 @@ #include <vector> //pipol tracker -#include "association_solver.h" - +#include "base/association/association_solver.h" namespace wolf { diff --git a/include/base/association/association_node.h b/include/base/association/association_node.h index cc4422e81..21a10d480 100644 --- a/include/base/association/association_node.h +++ b/include/base/association/association_node.h @@ -1,5 +1,4 @@ - - + #ifndef association_node_H #define association_node_H @@ -10,7 +9,7 @@ #include <algorithm> //find() //pipol tracker -#include "matrix.h" +#include "base/association/matrix.h" //constants const double PROB_ZERO_ = 1e-3; diff --git a/include/base/association/association_solver.h b/include/base/association/association_solver.h index 24efc159b..6ac986e07 100644 --- a/include/base/association/association_solver.h +++ b/include/base/association/association_solver.h @@ -7,7 +7,7 @@ #include <vector> //matrix class -#include "matrix.h" +#include "base/association/matrix.h" namespace wolf { diff --git a/include/base/association/association_tree.h b/include/base/association/association_tree.h index 29ae34a2c..b6566a604 100644 --- a/include/base/association/association_tree.h +++ b/include/base/association/association_tree.h @@ -9,12 +9,11 @@ //#include <memory> //pipol tracker -#include "matrix.h" -#include "association_solver.h" -#include "association_node.h" +#include "base/association/matrix.h" +#include "base/association/association_solver.h" +#include "base/association/association_node.h" #include <map> - namespace wolf { diff --git a/src/association/matrix.h b/include/base/association/matrix.h similarity index 99% rename from src/association/matrix.h rename to include/base/association/matrix.h index 10031b131..c76bba51e 100644 --- a/src/association/matrix.h +++ b/include/base/association/matrix.h @@ -7,7 +7,6 @@ #include <vector> #include <assert.h> //assert - template <typename T> class Matrixx { diff --git a/include/base/capture/capture_GPS.h b/include/base/capture/capture_GPS.h index 946619527..5f5088bd6 100644 --- a/include/base/capture/capture_GPS.h +++ b/include/base/capture/capture_GPS.h @@ -1,10 +1,9 @@ #ifndef CAPTURE_GPS_H_ #define CAPTURE_GPS_H_ - // Wolf includes #include "raw_gps_utils/satellites_obs.h" -#include "capture_base.h" +#include "base/capture/capture_base.h" namespace wolf { diff --git a/include/base/capture/capture_GPS_fix.h b/include/base/capture/capture_GPS_fix.h index 6f65b733b..8cb4afbf4 100644 --- a/include/base/capture/capture_GPS_fix.h +++ b/include/base/capture/capture_GPS_fix.h @@ -2,8 +2,8 @@ #define CAPTURE_GPS_FIX_H_ //Wolf includes -#include "feature_GPS_fix.h" -#include "capture_base.h" +#include "base/feature/feature_GPS_fix.h" +#include "base/capture/capture_base.h" //std includes // diff --git a/include/base/capture/capture_IMU.h b/include/base/capture/capture_IMU.h index 01c1f5aab..6e2de9d9d 100644 --- a/include/base/capture/capture_IMU.h +++ b/include/base/capture/capture_IMU.h @@ -2,8 +2,8 @@ #define CAPTURE_IMU_H //Wolf includes -#include "IMU_tools.h" -#include "capture_motion.h" +#include "base/IMU_tools.h" +#include "base/capture/capture_motion.h" namespace wolf { diff --git a/include/base/capture/capture_base.h b/include/base/capture/capture_base.h index 9da734e64..293ab9d03 100644 --- a/include/base/capture/capture_base.h +++ b/include/base/capture/capture_base.h @@ -8,9 +8,9 @@ class FeatureBase; } //Wolf includes -#include "wolf.h" -#include "node_base.h" -#include "time_stamp.h" +#include "base/wolf.h" +#include "base/node_base.h" +#include "base/time_stamp.h" //std includes @@ -109,10 +109,10 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture } -#include "sensor_base.h" -#include "frame_base.h" -#include "feature_base.h" -#include "state_block.h" +#include "base/sensor/sensor_base.h" +#include "base/frame_base.h" +#include "base/feature/feature_base.h" +#include "base/state_block.h" namespace wolf{ @@ -148,7 +148,6 @@ inline void CaptureBase::setStateBlockPtr(unsigned int _i, const StateBlockPtr _ state_block_vec_[_i] = _sb_ptr; } - inline StateBlockPtr CaptureBase::getSensorPPtr() const { return getStateBlockPtr(0); @@ -164,7 +163,6 @@ inline StateBlockPtr CaptureBase::getSensorIntrinsicPtr() const return getStateBlockPtr(2); } - inline unsigned int CaptureBase::id() { return capture_id_; @@ -195,7 +193,6 @@ inline ConstraintBaseList& CaptureBase::getConstrainedByList() return constrained_by_list_; } - inline TimeStamp CaptureBase::getTimeStamp() const { return time_stamp_; diff --git a/include/base/capture/capture_buffer.h b/include/base/capture/capture_buffer.h index 13cb1b9d0..434716dcf 100644 --- a/include/base/capture/capture_buffer.h +++ b/include/base/capture/capture_buffer.h @@ -8,8 +8,8 @@ #ifndef _WOLF_CAPTURE_BUFFER_H_ #define _WOLF_CAPTURE_BUFFER_H_ -#include "wolf.h" -#include "time_stamp.h" +#include "base/wolf.h" +#include "base/time_stamp.h" #include <list> #include <algorithm> @@ -79,7 +79,6 @@ public: std::list<CaptureBasePtr> container_; }; - CaptureBuffer::CaptureBuffer(const Scalar _buffer_dt, const int _max_capture) : max_capture_(_max_capture), buffer_dt_(_buffer_dt) { diff --git a/include/base/capture/capture_image.h b/include/base/capture/capture_image.h index ca10a873f..4e9771cf0 100644 --- a/include/base/capture/capture_image.h +++ b/include/base/capture/capture_image.h @@ -2,9 +2,9 @@ #define CAPTURE_IMAGE_H //Wolf includes -#include "capture_base.h" -#include "feature_point_image.h" -#include "sensor_camera.h" +#include "base/capture/capture_base.h" +#include "base/feature/feature_point_image.h" +#include "base/sensor/sensor_camera.h" // Vision Utils includes #include "vision_utils/vision_utils.h" diff --git a/include/base/capture/capture_laser_2D.h b/include/base/capture/capture_laser_2D.h index f1cbf9eae..9abd6bf4b 100644 --- a/include/base/capture/capture_laser_2D.h +++ b/include/base/capture/capture_laser_2D.h @@ -8,8 +8,8 @@ class SensorLaser2D; } //wolf includes -#include "capture_base.h" -#include "sensor_laser_2D.h" +#include "base/capture/capture_base.h" +#include "base/sensor/sensor_laser_2D.h" //laserscanutils includes #include "laser_scan_utils/laser_scan.h" @@ -18,7 +18,6 @@ namespace wolf { WOLF_PTR_TYPEDEFS(CaptureLaser2D); - class CaptureLaser2D : public CaptureBase { public: diff --git a/include/base/capture/capture_motion.h b/include/base/capture/capture_motion.h index a07ddc875..b867d2ec1 100644 --- a/include/base/capture/capture_motion.h +++ b/include/base/capture/capture_motion.h @@ -9,8 +9,8 @@ #define SRC_CAPTURE_MOTION_H_ // Wolf includes -#include "capture_base.h" -#include "motion_buffer.h" +#include "base/capture/capture_base.h" +#include "base/motion_buffer.h" // STL includes #include <list> diff --git a/include/base/capture/capture_odom_2D.h b/include/base/capture/capture_odom_2D.h index 208f99fa4..eafb6ead7 100644 --- a/include/base/capture/capture_odom_2D.h +++ b/include/base/capture/capture_odom_2D.h @@ -8,9 +8,9 @@ #ifndef CAPTURE_ODOM_2D_H_ #define CAPTURE_ODOM_2D_H_ -#include "capture_motion.h" +#include "base/capture/capture_motion.h" -#include "rotations.h" +#include "base/rotations.h" namespace wolf { @@ -44,8 +44,6 @@ inline Eigen::VectorXs CaptureOdom2D::correctDelta(const VectorXs& _delta, const return delta; } - - } /* namespace wolf */ #endif /* CAPTURE_ODOM_2D_H_ */ diff --git a/include/base/capture/capture_odom_3D.h b/include/base/capture/capture_odom_3D.h index 21fee1943..d1f29508b 100644 --- a/include/base/capture/capture_odom_3D.h +++ b/include/base/capture/capture_odom_3D.h @@ -8,9 +8,9 @@ #ifndef CAPTURE_ODOM_3D_H_ #define CAPTURE_ODOM_3D_H_ -#include "capture_motion.h" +#include "base/capture/capture_motion.h" -#include "rotations.h" +#include "base/rotations.h" namespace wolf { @@ -39,5 +39,4 @@ class CaptureOdom3D : public CaptureMotion } /* namespace wolf */ - #endif /* CAPTURE_ODOM_3D_H_ */ diff --git a/include/base/capture/capture_pose.h b/include/base/capture/capture_pose.h index 9d47d39c9..51fb833e4 100644 --- a/include/base/capture/capture_pose.h +++ b/include/base/capture/capture_pose.h @@ -2,10 +2,10 @@ #define CAPTURE_POSE_H_ //Wolf includes -#include "capture_base.h" -#include "constraint_pose_2D.h" -#include "constraint_pose_3D.h" -#include "feature_pose.h" +#include "base/capture/capture_base.h" +#include "base/constraint/constraint_pose_2D.h" +#include "base/constraint/constraint_pose_3D.h" +#include "base/feature/feature_pose.h" //std includes // diff --git a/include/base/capture/capture_velocity.h b/include/base/capture/capture_velocity.h index 55b1bcf1a..4853fce2a 100644 --- a/include/base/capture/capture_velocity.h +++ b/include/base/capture/capture_velocity.h @@ -9,7 +9,7 @@ #define _WOLF_CAPTURE_VELOCITY_H_ //wolf includes -#include "capture_motion.h" +#include "base/capture/capture_motion.h" namespace wolf { diff --git a/include/base/capture/capture_void.h b/include/base/capture/capture_void.h index 50b3b3c7d..b6b7a755d 100644 --- a/include/base/capture/capture_void.h +++ b/include/base/capture/capture_void.h @@ -2,8 +2,7 @@ #define CAPTURE_VOID_H_ //Wolf includes -#include "capture_base.h" - +#include "base/capture/capture_base.h" namespace wolf { diff --git a/include/base/capture/capture_wheel_joint_position.h b/include/base/capture/capture_wheel_joint_position.h index 02829cd1a..0bade4029 100644 --- a/include/base/capture/capture_wheel_joint_position.h +++ b/include/base/capture/capture_wheel_joint_position.h @@ -9,7 +9,7 @@ #define CAPTURE_WHEEL_JOINT_POSITION_H_ //wolf includes -#include "../capture_motion.h" +#include "base/capture/capture_motion.h" namespace wolf { @@ -60,7 +60,6 @@ protected: Eigen::MatrixXs positions_cov_; }; - /// @todo Enforce some logic on the wheel joint pos data //template <typename E> @@ -178,7 +177,6 @@ protected: //using CaptureDiffDriveWheelJointPosition = CaptureWheelJointPosition<DiffDriveController>; - } // namespace wolf #endif /* CAPTURE_WHEEL_JOINT_POSITION_H_ */ diff --git a/include/base/ceres_wrapper/ceres_manager.h b/include/base/ceres_wrapper/ceres_manager.h index 8852bc06d..5eb3436f5 100644 --- a/include/base/ceres_wrapper/ceres_manager.h +++ b/include/base/ceres_wrapper/ceres_manager.h @@ -7,10 +7,10 @@ #include "glog/logging.h" //wolf includes -#include "../solver/solver_manager.h" -#include "cost_function_wrapper.h" +#include "base/solver/solver_manager.h" +#include "base/ceres_wrapper/cost_function_wrapper.h" #include "local_parametrization_wrapper.h" -#include "create_numeric_diff_cost_function.h" +#include "base/ceres_wrapper/create_numeric_diff_cost_function.h" namespace ceres { typedef std::shared_ptr<CostFunction> CostFunctionPtr; diff --git a/include/base/ceres_wrapper/cost_function_wrapper.h b/include/base/ceres_wrapper/cost_function_wrapper.h index ffd881914..119839dee 100644 --- a/include/base/ceres_wrapper/cost_function_wrapper.h +++ b/include/base/ceres_wrapper/cost_function_wrapper.h @@ -2,8 +2,8 @@ #define TRUNK_SRC_COST_FUNCTION_WRAPPER_H_ // WOLF -#include "../wolf.h" -#include "../constraint_analytic.h" +#include "base/wolf.h" +#include "base/constraint/constraint_analytic.h" // CERES #include "ceres/cost_function.h" @@ -56,6 +56,4 @@ inline ConstraintBasePtr CostFunctionWrapper::getConstraintPtr() const } // namespace wolf - - #endif /* TRUNK_SRC_COST_FUNCTION_WRAPPER_H_ */ diff --git a/include/base/ceres_wrapper/create_numeric_diff_cost_function.h b/include/base/ceres_wrapper/create_numeric_diff_cost_function.h index d6650b203..8f8e9686b 100644 --- a/include/base/ceres_wrapper/create_numeric_diff_cost_function.h +++ b/include/base/ceres_wrapper/create_numeric_diff_cost_function.h @@ -12,9 +12,8 @@ #include "ceres/numeric_diff_cost_function.h" // Constraints -#include "../constraint_odom_2D.h" -#include "../constraint_base.h" - +#include "base/constraint/constraint_odom_2D.h" +#include "base/constraint/constraint_base.h" namespace wolf { @@ -29,7 +28,6 @@ std::shared_ptr<ceres::NumericDiffCostFunction<T, ceres::CENTRAL, T::residualSiz T::block5Size,T::block6Size,T::block7Size,T::block8Size,T::block9Size> >(std::static_pointer_cast<T>(_constraint_ptr).get()); }; - inline std::shared_ptr<ceres::CostFunction> createNumericDiffCostFunction(ConstraintBasePtr _ctr_ptr) { // switch (_ctr_ptr->getTypeId()) diff --git a/include/base/ceres_wrapper/local_parametrization_wrapper.h b/include/base/ceres_wrapper/local_parametrization_wrapper.h index d9e0d9e82..4ae4182cc 100644 --- a/include/base/ceres_wrapper/local_parametrization_wrapper.h +++ b/include/base/ceres_wrapper/local_parametrization_wrapper.h @@ -7,7 +7,7 @@ class LocalParametrizationBase; } //Ceres includes -#include "../wolf.h" +#include "base/wolf.h" #include "ceres/ceres.h" namespace wolf { @@ -38,7 +38,7 @@ using LocalParametrizationWrapperPtr = std::shared_ptr<LocalParametrizationWrapp } // namespace wolf -#include "../local_parametrization_base.h" +#include "base/local_parametrization_base.h" namespace wolf { diff --git a/include/base/ceres_wrapper/qr_manager.h b/include/base/ceres_wrapper/qr_manager.h index ae7fae7a1..d09a5147b 100644 --- a/include/base/ceres_wrapper/qr_manager.h +++ b/include/base/ceres_wrapper/qr_manager.h @@ -8,8 +8,8 @@ #ifndef SRC_CERES_WRAPPER_QR_MANAGER_H_ #define SRC_CERES_WRAPPER_QR_MANAGER_H_ -#include "solver_manager.h" -#include "sparse_utils.h" +#include "base/solver/solver_manager.h" +#include "base/solver_suitesparse/sparse_utils.h" namespace wolf { diff --git a/include/base/ceres_wrapper/solver_manager.h b/include/base/ceres_wrapper/solver_manager.h index f7725df4a..6ffda3f7c 100644 --- a/include/base/ceres_wrapper/solver_manager.h +++ b/include/base/ceres_wrapper/solver_manager.h @@ -2,9 +2,9 @@ #define SOLVER_MANAGER_H_ //wolf includes -#include "../wolf.h" -#include "../state_block.h" -#include "../constraint_base.h" +#include "base/wolf.h" +#include "base/state_block.h" +#include "base/constraint/constraint_base.h" namespace wolf { diff --git a/include/base/constraint/constraint_AHP.h b/include/base/constraint/constraint_AHP.h index a97ac9840..340acc9c3 100644 --- a/include/base/constraint/constraint_AHP.h +++ b/include/base/constraint/constraint_AHP.h @@ -2,11 +2,11 @@ #define CONSTRAINT_AHP_H //Wolf includes -#include "constraint_autodiff.h" -#include "landmark_AHP.h" -#include "sensor_camera.h" -//#include "feature_point_image.h" -#include "pinhole_tools.h" +#include "base/constraint/constraint_autodiff.h" +#include "base/landmark/landmark_AHP.h" +#include "base/sensor/sensor_camera.h" +//#include "base/feature/feature_point_image.h" +#include "base/pinhole_tools.h" #include <iomanip> //setprecision @@ -68,7 +68,6 @@ inline ConstraintAHP::ConstraintAHP(const FeatureBasePtr& _ftr_ptr, ConstraintAutodiff<ConstraintAHP, 2, 3, 4, 3, 4, 4>("AHP", _landmark_ptr->getAnchorFrame(), nullptr, - nullptr, _landmark_ptr, _processor_ptr, _apply_loss_function, @@ -115,7 +114,6 @@ inline void ConstraintAHP::expectation(const T* const _current_frame_p, { using namespace Eigen; - // All involved transforms typedef typedef Eigen::Transform<T, 3, Eigen::Affine> TransformType; @@ -198,5 +196,4 @@ inline ConstraintAHPPtr ConstraintAHP::create(const FeatureBasePtr& _ftr_ptr, } // namespace wolf - #endif // CONSTRAINT_AHP_H diff --git a/include/base/constraint/constraint_GPS_2D.h b/include/base/constraint/constraint_GPS_2D.h index 5feb77fb9..2f85d0663 100644 --- a/include/base/constraint/constraint_GPS_2D.h +++ b/include/base/constraint/constraint_GPS_2D.h @@ -3,9 +3,9 @@ #define CONSTRAINT_GPS_2D_H_ //Wolf includes -#include "wolf.h" -#include "constraint_autodiff.h" -#include "frame_base.h" +#include "base/wolf.h" +#include "base/constraint/constraint_autodiff.h" +#include "base/frame_base.h" namespace wolf { diff --git a/include/base/constraint/constraint_GPS_pseudorange_2D.h b/include/base/constraint/constraint_GPS_pseudorange_2D.h index 7a04463b9..3c15a0246 100644 --- a/include/base/constraint/constraint_GPS_pseudorange_2D.h +++ b/include/base/constraint/constraint_GPS_pseudorange_2D.h @@ -4,9 +4,9 @@ #define LIGHT_SPEED_ 299792458 //Wolf includes -#include "sensor_GPS.h" -#include "feature_GPS_pseudorange.h" -#include "constraint_autodiff.h" +#include "base/sensor/sensor_GPS.h" +#include "base/feature/feature_GPS_pseudorange.h" +#include "base/constraint/constraint_autodiff.h" //std #include <string> @@ -31,10 +31,6 @@ class ConstraintGPSPseudorange2D : public ConstraintAutodiff<ConstraintGPSPseudo bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) : ConstraintAutodiff<ConstraintGPSPseudorange2D, 1, 2, 1, 3, 1, 3, 1>("GPS PR 2D", - nullptr, - nullptr, - nullptr, - nullptr, nullptr, _apply_loss_function, _status, @@ -98,7 +94,6 @@ inline bool ConstraintGPSPseudorange2D::operator ()(const T* const _vehicle_p, c //Filling Eigen vectors Eigen::Matrix<T, 4, 1> sensor_p_base(_sensor_p[0], _sensor_p[1], _sensor_p[2], T(1)); //sensor position with respect base frame - /* * Base-to-map transform matrix */ @@ -175,7 +170,6 @@ inline bool ConstraintGPSPseudorange2D::operator ()(const T* const _vehicle_p, c T_lon_lat(2, 0) = T(sin(lat)); T_lon_lat(2, 2) = T(cos(lat)); - Eigen::Matrix<T, 4, 4> T_lat_enu = Eigen::Matrix<T, 4, 4>::Zero(); T_lat_enu(0, 2) = T_lat_enu(1, 0) = T_lat_enu(2, 1) = T_lat_enu(3, 3) = T(1); @@ -190,7 +184,6 @@ inline bool ConstraintGPSPseudorange2D::operator ()(const T* const _vehicle_p, c //sensor position with respect to ecef coordinate system Eigen::Matrix<T, 4, 1> sensor_p_ecef = T_ecef_map * sensor_p_map; - /* * calculate the residual */ @@ -209,7 +202,6 @@ inline bool ConstraintGPSPseudorange2D::operator ()(const T* const _vehicle_p, c // normalizing by the covariance _residual[0] = _residual[0] / T(getMeasurementCovariance()(0, 0));//T(sqrt(getMeasurementCovariance()(0, 0))); - if (verbose_level_ >= 1) { aux.str(std::string()); diff --git a/include/base/constraint/constraint_GPS_pseudorange_3D.h b/include/base/constraint/constraint_GPS_pseudorange_3D.h index 4c44d262e..02323d2c2 100644 --- a/include/base/constraint/constraint_GPS_pseudorange_3D.h +++ b/include/base/constraint/constraint_GPS_pseudorange_3D.h @@ -4,9 +4,9 @@ #define LIGHT_SPEED 299792458 //Wolf includes -#include "sensor_GPS.h" -#include "feature_GPS_pseudorange.h" -#include "constraint_autodiff.h" +#include "base/sensor/sensor_GPS.h" +#include "base/feature/feature_GPS_pseudorange.h" +#include "base/constraint/constraint_autodiff.h" namespace wolf { @@ -45,7 +45,6 @@ class ConstraintGPSPseudorange3D: public ConstraintAutodiff<ConstraintGPSPseudor //std::cout << "ConstraintGPSPseudorange3D() pr=" << pseudorange_ << "\tsat_pos=(" << sat_position_[0] << ", " << sat_position_[1] << ", " << sat_position_[2] << ")" << std::endl; } - virtual ~ConstraintGPSPseudorange3D() = default; template<typename T> @@ -66,7 +65,6 @@ class ConstraintGPSPseudorange3D: public ConstraintAutodiff<ConstraintGPSPseudor return std::make_shared<ConstraintGPSPseudorange3D>(_feature_ptr); } - }; /* @@ -115,7 +113,6 @@ inline bool ConstraintGPSPseudorange3D::operator ()(const T* const _vehicle_p, c for (int i = 0; i < 3; ++i) T_map_base(i, 3) = _vehicle_p[i]; - /* * Compute sensor_p wrt ECEF */ diff --git a/include/base/constraint/constraint_IMU.h b/include/base/constraint/constraint_IMU.h index 9ccf54eeb..87dd49f0f 100644 --- a/include/base/constraint/constraint_IMU.h +++ b/include/base/constraint/constraint_IMU.h @@ -2,14 +2,13 @@ #define CONSTRAINT_IMU_THETA_H_ //Wolf includes -#include "feature_IMU.h" -#include "sensor_IMU.h" -#include "constraint_autodiff.h" -#include "rotations.h" +#include "base/feature/feature_IMU.h" +#include "base/sensor/sensor_IMU.h" +#include "base/constraint/constraint_autodiff.h" +#include "base/rotations.h" //Eigen include - namespace wolf { WOLF_PTR_TYPEDEFS(ConstraintIMU); @@ -101,7 +100,6 @@ class ConstraintIMU : public ConstraintAutodiff<ConstraintIMU, 15, 3, 4, 3, 6, 3 */ Eigen::VectorXs expectation() const; - private: /// Preintegrated delta Eigen::Vector3s dp_preint_; @@ -183,8 +181,6 @@ inline ConstraintIMU::ConstraintIMU(const FeatureIMUPtr& _ftr_ptr, // } - - template<typename T> inline bool ConstraintIMU::operator ()(const T* const _p1, const T* const _q1, @@ -267,12 +263,10 @@ inline bool ConstraintIMU::residual(const Eigen::MatrixBase<D1> & _p1, * results in : * res = W.sqrt * ( ( diff ( D_preint , D_exp ) ) - J_preint * (b - b_preint) ) * - * * NOTE: See optimization report at the end of this file for comparisons of both methods. */ #define METHOD_1 // if commented, then METHOD_2 will be applied - //needed typedefs typedef typename D1::Scalar T; @@ -285,7 +279,6 @@ inline bool ConstraintIMU::residual(const Eigen::MatrixBase<D1> & _p1, imu::betweenStates(_p1, _q1, _v1, _p2, _q2, _v2, (T)dt_, dp_exp, dq_exp, dv_exp); - // 2. Corrected integrated delta: delta_corr = delta_preint (+) J_bias * (bias_current - bias_preint) // 2.a. Compute the delta step in tangent space: step = J_bias * (bias - bias_preint) @@ -309,7 +302,6 @@ inline bool ConstraintIMU::residual(const Eigen::MatrixBase<D1> & _p1, dp_step, do_step, dv_step, dp_correct, dq_correct, dv_correct); - // 3. Delta error in minimal form: D_err = diff(D_exp , D_corr) // Note the Dt here is zero because it's the delta-time between the same time stamps! Eigen::Matrix<T, 9, 1> d_error; @@ -361,7 +353,6 @@ inline bool ConstraintIMU::residual(const Eigen::MatrixBase<D1> & _p1, _res.segment(9,3) = sqrt_A_r_dt_inv.cast<T>() * ab_error; _res.tail(3) = sqrt_W_r_dt_inv.cast<T>() * wb_error; - ////////////////////////////////////////////////////////////////////////////////////////////// ///////////////////////////////// PRINT VALUES /////////////////////////////////// #if 0 @@ -396,14 +387,10 @@ inline bool ConstraintIMU::residual(const Eigen::MatrixBase<D1> & _p1, WOLF_TRACE("-----------------------------------------") #endif ///////////////////////////////////////////////////////////////////////////////////////////// - ///////////////////////////////////////////////////////////////////////////////////////////// - - return true; } - inline Eigen::VectorXs ConstraintIMU::expectation() const { FrameBasePtr frm_current = getFeaturePtr()->getFramePtr(); @@ -448,25 +435,20 @@ inline void ConstraintIMU::expectation(const Eigen::MatrixBase<D1> & _p1, imu::betweenStates(_p1, _q1, _v1, _p2, _q2, _v2, _dt, _pe, _qe, _ve); } - } // namespace wolf #endif - /* * Optimization results * ================================================ * - * * Using gtest_IMU.cpp * * Conclusion: Residuals with method 1 and 2 are essentially identical, after exactly the same number of iterations. * * You can verify this by looking at the 'Iterations' and 'Final costs' in the Ceres reports below. * - * - * * With Method 1: * [ RUN ] Process_Constraint_IMU.Var_B1_B2_Invar_P1_Q1_V1_P2_Q2_V2 @@ -490,8 +472,6 @@ inline void ConstraintIMU::expectation(const Eigen::MatrixBase<D1> & _p1, [ OK ] Process_Constraint_IMU_ODO.Var_P0_Q0_B0_P1_Q1_V1_B1__Invar_V0 (52 ms) [----------] 2 tests from Process_Constraint_IMU_ODO (120 ms total) * -* -* * With Method 2: * [ RUN ] Process_Constraint_IMU.Var_B1_B2_Invar_P1_Q1_V1_P2_Q2_V2 @@ -515,5 +495,4 @@ inline void ConstraintIMU::expectation(const Eigen::MatrixBase<D1> & _p1, [ OK ] Process_Constraint_IMU_ODO.Var_P0_Q0_B0_P1_Q1_V1_B1__Invar_V0 (59 ms) [----------] 2 tests from Process_Constraint_IMU_ODO (127 ms total) * -* */ diff --git a/include/base/constraint/constraint_analytic.h b/include/base/constraint/constraint_analytic.h index 17ff4a079..3b5015be0 100644 --- a/include/base/constraint/constraint_analytic.h +++ b/include/base/constraint/constraint_analytic.h @@ -3,7 +3,7 @@ #define CONSTRAINT_ANALYTIC_H_ //Wolf includes -#include "constraint_base.h" +#include "base/constraint/constraint_base.h" #include <Eigen/StdVector> namespace wolf { @@ -37,7 +37,6 @@ class ConstraintAnalytic: public ConstraintBase StateBlockPtr _state8Ptr = nullptr, StateBlockPtr _state9Ptr = nullptr ) ; - ConstraintAnalytic(const std::string& _tp, const FrameBasePtr& _frame_other_ptr, const CaptureBasePtr& _capture_other_ptr, @@ -57,7 +56,6 @@ class ConstraintAnalytic: public ConstraintBase StateBlockPtr _state8Ptr = nullptr, StateBlockPtr _state9Ptr = nullptr ); - virtual ~ConstraintAnalytic() = default; /** \brief Returns a vector of pointers to the states diff --git a/include/base/constraint/constraint_autodiff.h b/include/base/constraint/constraint_autodiff.h index ed61b0756..e6a687036 100644 --- a/include/base/constraint/constraint_autodiff.h +++ b/include/base/constraint/constraint_autodiff.h @@ -3,8 +3,8 @@ #define CONSTRAINT_AUTODIFF_H_ //Wolf includes -#include "constraint_base.h" -#include "state_block.h" +#include "base/constraint/constraint_base.h" +#include "base/state_block.h" // CERES #include "ceres/jet.h" @@ -299,7 +299,6 @@ class ConstraintAutodiff : public ConstraintBase } }; - ////////////////// SPECIALIZATION 9 BLOCKS //////////////////////////////////////////////////////////////////////// template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8> @@ -2159,17 +2158,9 @@ class ConstraintAutodiff<CtrT,RES,B0,0,0,0,0,0,0,0,0,0> : public ConstraintBase } }; -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // STATIC CONST VECTORS INITIALIZATION //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // state_block_sizes_ // 10 BLOCKS template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9> diff --git a/include/base/constraint/constraint_autodiff_distance_3D.h b/include/base/constraint/constraint_autodiff_distance_3D.h index a77e3880f..6e9452ff6 100644 --- a/include/base/constraint/constraint_autodiff_distance_3D.h +++ b/include/base/constraint/constraint_autodiff_distance_3D.h @@ -8,7 +8,7 @@ #ifndef CONSTRAINT_AUTODIFF_DISTANCE_3D_H_ #define CONSTRAINT_AUTODIFF_DISTANCE_3D_H_ -#include "constraint_autodiff.h" +#include "base/constraint/constraint_autodiff.h" namespace wolf { diff --git a/include/base/constraint/constraint_autodiff_trifocal.h b/include/base/constraint/constraint_autodiff_trifocal.h index ac54629a6..d3844c350 100644 --- a/include/base/constraint/constraint_autodiff_trifocal.h +++ b/include/base/constraint/constraint_autodiff_trifocal.h @@ -2,9 +2,9 @@ #define _CONSTRAINT_AUTODIFF_TRIFOCAL_H_ //Wolf includes -//#include "wolf.h" -#include "constraint_autodiff.h" -#include "sensor_camera.h" +//#include "base/wolf.h" +#include "base/constraint/constraint_autodiff.h" +#include "base/sensor/sensor_camera.h" #include <common_class/trifocaltensor.h> #include <vision_utils.h> @@ -108,7 +108,6 @@ class ConstraintAutodiffTrifocal : public ConstraintAutodiff<ConstraintAutodiffT MatrixBase<D3>& _J_e_m2, MatrixBase<D4>& _J_e_m3); - private: FeatureBaseWPtr feature_prev_ptr_; // To look for measurements SensorCameraPtr camera_ptr_; // To look for intrinsics @@ -118,10 +117,8 @@ class ConstraintAutodiffTrifocal : public ConstraintAutodiff<ConstraintAutodiffT } // namespace wolf - - // Includes for implentation -#include "rotations.h" +#include "base/rotations.h" namespace wolf { @@ -136,7 +133,6 @@ ConstraintAutodiffTrifocal::ConstraintAutodiffTrifocal( const ProcessorBasePtr& _processor_ptr, bool _apply_loss_function, ConstraintStatus _status) : ConstraintAutodiff( "TRIFOCAL PLP", - nullptr, nullptr, _feature_origin_ptr, nullptr, @@ -217,7 +213,6 @@ inline FeatureBasePtr ConstraintAutodiffTrifocal::getFeaturePrevPtr() return feature_prev_ptr_.lock(); } - template<typename T> bool ConstraintAutodiffTrifocal::operator ()( const T* const _prev_pos, const T* const _prev_quat, @@ -379,7 +374,6 @@ inline Matrix<T, 3, 1> ConstraintAutodiffTrifocal::error_jacobians(const vision_ J_e2_m3.setZero(); // Not involved in epipolar c1->c2 - // Compact Jacobians _J_e_m1.topRows(2) = J_e1_m1; _J_e_m1.row(2) = J_e2_m1; @@ -397,8 +391,6 @@ inline Matrix<T, 3, 1> ConstraintAutodiffTrifocal::error_jacobians(const vision_ } - } // namespace wolf - #endif /* _CONSTRAINT_AUTODIFF_TRIFOCAL_H_ */ diff --git a/include/base/constraint/constraint_base.h b/include/base/constraint/constraint_base.h index 3341a55c8..1a0b4f14f 100644 --- a/include/base/constraint/constraint_base.h +++ b/include/base/constraint/constraint_base.h @@ -7,8 +7,8 @@ class FeatureBase; } //Wolf includes -#include "wolf.h" -#include "node_base.h" +#include "base/wolf.h" +#include "base/node_base.h" //std includes @@ -178,16 +178,15 @@ class ConstraintBase : public NodeBase, public std::enable_shared_from_this<Cons // void print(const std::string& name, const Eigen::Matrix<Scalar, R, C>& mat) const; // Normal print if Scalar type is wolf::Scalar }; - } // IMPLEMENTATION // -#include "problem.h" -#include "frame_base.h" -#include "feature_base.h" -#include "sensor_base.h" -#include "landmark_base.h" +#include "base/problem.h" +#include "base/frame_base.h" +#include "base/feature/feature_base.h" +#include "base/sensor/sensor_base.h" +#include "base/landmark/landmark_base.h" namespace wolf{ diff --git a/include/base/constraint/constraint_block_absolute.h b/include/base/constraint/constraint_block_absolute.h index fe8eec7a9..22b4ee1b1 100644 --- a/include/base/constraint/constraint_block_absolute.h +++ b/include/base/constraint/constraint_block_absolute.h @@ -9,9 +9,8 @@ #define CONSTRAINT_BLOCK_ABSOLUTE_H_ //Wolf includes -#include "constraint_autodiff.h" -#include "frame_base.h" - +#include "base/constraint/constraint_autodiff.h" +#include "base/frame_base.h" namespace wolf { diff --git a/include/base/constraint/constraint_container.h b/include/base/constraint/constraint_container.h index c796d1e32..1e578f10d 100644 --- a/include/base/constraint/constraint_container.h +++ b/include/base/constraint/constraint_container.h @@ -2,9 +2,9 @@ #define CONSTRAINT_CONTAINER_H_ //Wolf includes -#include "wolf.h" -#include "constraint_autodiff.h" -#include "landmark_container.h" +#include "base/wolf.h" +#include "base/constraint/constraint_autodiff.h" +#include "base/landmark/landmark_container.h" namespace wolf { @@ -116,7 +116,6 @@ class ConstraintContainer: public ConstraintAutodiff<ConstraintContainer,3,2,1,2 return true; } - public: static ConstraintBasePtr create(const FeatureBasePtr& _feature_ptr, const NodeBasePtr& _correspondant_ptr, diff --git a/include/base/constraint/constraint_corner_2D.h b/include/base/constraint/constraint_corner_2D.h index 96afcfd50..4692ad29a 100644 --- a/include/base/constraint/constraint_corner_2D.h +++ b/include/base/constraint/constraint_corner_2D.h @@ -2,8 +2,8 @@ #define CONSTRAINT_CORNER_2D_THETA_H_ //Wolf includes -#include "constraint_autodiff.h" -#include "landmark_corner_2D.h" +#include "base/constraint/constraint_autodiff.h" +#include "base/landmark/landmark_corner_2D.h" namespace wolf { @@ -111,6 +111,4 @@ inline bool ConstraintCorner2D::operator ()(const T* const _robotP, const T* con } // namespace wolf - - #endif diff --git a/include/base/constraint/constraint_diff_drive.h b/include/base/constraint/constraint_diff_drive.h index 8d6a898a0..017223784 100644 --- a/include/base/constraint/constraint_diff_drive.h +++ b/include/base/constraint/constraint_diff_drive.h @@ -9,10 +9,10 @@ #define WOLF_CONSTRAINT_DIFF_DRIVE_H_ //Wolf includes -#include "../constraint_autodiff.h" -#include "../frame_base.h" -#include "../features/feature_diff_drive.h" -#include "../captures/capture_wheel_joint_position.h" +#include "base/constraint/constraint_autodiff.h" +#include "base/frame_base.h" +#include "base/feature/feature_diff_drive.h" +#include "base/capture/capture_wheel_joint_position.h" namespace { diff --git a/include/base/constraint/constraint_epipolar.h b/include/base/constraint/constraint_epipolar.h index 3172173cf..2887af376 100644 --- a/include/base/constraint/constraint_epipolar.h +++ b/include/base/constraint/constraint_epipolar.h @@ -1,7 +1,7 @@ #ifndef CONSTRAINT_EPIPOLAR_H #define CONSTRAINT_EPIPOLAR_H -#include "constraint_base.h" +#include "base/constraint/constraint_base.h" namespace wolf { @@ -19,7 +19,6 @@ class ConstraintEpipolar : public ConstraintBase virtual ~ConstraintEpipolar() = default; - /** \brief Evaluate the constraint given the input parameters and returning the residuals and jacobians **/ virtual bool evaluate(Scalar const* const* parameters, Scalar* residuals, Scalar** jacobians) const override {return true;}; diff --git a/include/base/constraint/constraint_fix_bias.h b/include/base/constraint/constraint_fix_bias.h index ed69c9049..55dbbf8db 100644 --- a/include/base/constraint/constraint_fix_bias.h +++ b/include/base/constraint/constraint_fix_bias.h @@ -3,15 +3,14 @@ #define CONSTRAINT_FIX_BIAS_H_ //Wolf includes -#include "capture_IMU.h" -#include "feature_IMU.h" -#include "constraint_autodiff.h" -#include "frame_base.h" -#include "rotations.h" +#include "base/capture/capture_IMU.h" +#include "base/feature/feature_IMU.h" +#include "base/constraint/constraint_autodiff.h" +#include "base/frame_base.h" +#include "base/rotations.h" //#include "ceres/jet.h" - namespace wolf { WOLF_PTR_TYPEDEFS(ConstraintFixBias); diff --git a/include/base/constraint/constraint_odom_2D.h b/include/base/constraint/constraint_odom_2D.h index 11333302f..5df37fbb9 100644 --- a/include/base/constraint/constraint_odom_2D.h +++ b/include/base/constraint/constraint_odom_2D.h @@ -2,8 +2,8 @@ #define CONSTRAINT_ODOM_2D_THETA_H_ //Wolf includes -#include "constraint_autodiff.h" -#include "frame_base.h" +#include "base/constraint/constraint_autodiff.h" +#include "base/frame_base.h" //#include "ceres/jet.h" @@ -73,7 +73,6 @@ inline bool ConstraintOdom2D::operator ()(const T* const _p1, const T* const _o1 // Residuals res = getMeasurementSquareRootInformationUpper().cast<T>() * er; - //////////////////////////////////////////////////////// // print Jacobian. Uncomment this as you wish (remember to uncomment #include "ceres/jet.h" above): // using ceres::Jet; @@ -92,7 +91,6 @@ inline bool ConstraintOdom2D::operator ()(const T* const _p1, const T* const _o1 // } //////////////////////////////////////////////////////// - return true; } diff --git a/include/base/constraint/constraint_odom_2D_analytic.h b/include/base/constraint/constraint_odom_2D_analytic.h index d69655e27..43048d3ff 100644 --- a/include/base/constraint/constraint_odom_2D_analytic.h +++ b/include/base/constraint/constraint_odom_2D_analytic.h @@ -2,7 +2,7 @@ #define CONSTRAINT_ODOM_2D_ANALYTIC_H_ //Wolf includes -#include "constraint_relative_2D_analytic.h" +#include "base/constraint/constraint_relative_2D_analytic.h" #include <Eigen/StdVector> namespace wolf { @@ -93,7 +93,6 @@ class ConstraintOdom2DAnalytic : public ConstraintRelative2DAnalytic // jacobians[3] = getMeasurementSquareRootInformationUpper() * jacobians[0]; // } - public: static ConstraintBasePtr create(const FeatureBasePtr& _feature_ptr, const NodeBasePtr& _correspondant_ptr, diff --git a/include/base/constraint/constraint_odom_3D.h b/include/base/constraint/constraint_odom_3D.h index 26811e470..9047b7891 100644 --- a/include/base/constraint/constraint_odom_3D.h +++ b/include/base/constraint/constraint_odom_3D.h @@ -8,8 +8,8 @@ #ifndef CONSTRAINT_ODOM_3D_H_ #define CONSTRAINT_ODOM_3D_H_ -#include "constraint_autodiff.h" -#include "rotations.h" +#include "base/constraint/constraint_autodiff.h" +#include "base/rotations.h" namespace wolf { @@ -49,7 +49,6 @@ class ConstraintOdom3D : public ConstraintAutodiff<ConstraintOdom3D,6,3,4,3,4> template<typename T> void printRes(const Eigen::Matrix<T, 6, 1>& r) const; - }; template<typename T> @@ -71,7 +70,6 @@ inline void ConstraintOdom3D::printRes (const Eigen::Matrix<Scalar,6,1> & r) co std::cout << r.transpose() << std::endl; } - inline ConstraintOdom3D::ConstraintOdom3D(const FeatureBasePtr& _ftr_current_ptr, const FrameBasePtr& _frame_past_ptr, const ProcessorBasePtr& _processor_ptr, diff --git a/include/base/constraint/constraint_point_2D.h b/include/base/constraint/constraint_point_2D.h index 50d190409..1900991f7 100644 --- a/include/base/constraint/constraint_point_2D.h +++ b/include/base/constraint/constraint_point_2D.h @@ -2,9 +2,9 @@ #define CONSTRAINT_POINT_2D_THETA_H_ //Wolf includes -#include "constraint_autodiff.h" -#include "feature_polyline_2D.h" -#include "landmark_polyline_2D.h" +#include "base/constraint/constraint_autodiff.h" +#include "base/feature/feature_polyline_2D.h" +#include "base/landmark/landmark_polyline_2D.h" namespace wolf { diff --git a/include/base/constraint/constraint_point_to_line_2D.h b/include/base/constraint/constraint_point_to_line_2D.h index 324ae24d6..842b84a80 100644 --- a/include/base/constraint/constraint_point_to_line_2D.h +++ b/include/base/constraint/constraint_point_to_line_2D.h @@ -2,9 +2,9 @@ #define CONSTRAINT_POINT_TO_LINE_2D_H_ //Wolf includes -#include "constraint_autodiff.h" -#include "feature_polyline_2D.h" -#include "landmark_polyline_2D.h" +#include "base/constraint/constraint_autodiff.h" +#include "base/feature/feature_polyline_2D.h" +#include "base/landmark/landmark_polyline_2D.h" namespace wolf { diff --git a/include/base/constraint/constraint_pose_2D.h b/include/base/constraint/constraint_pose_2D.h index 652c1c377..b6fe79ef3 100644 --- a/include/base/constraint/constraint_pose_2D.h +++ b/include/base/constraint/constraint_pose_2D.h @@ -3,13 +3,12 @@ #define CONSTRAINT_POSE_2D_H_ //Wolf includes -#include "constraint_autodiff.h" -#include "frame_base.h" -#include "rotations.h" +#include "base/constraint/constraint_autodiff.h" +#include "base/frame_base.h" +#include "base/rotations.h" //#include "ceres/jet.h" - namespace wolf { WOLF_PTR_TYPEDEFS(ConstraintPose2D); diff --git a/include/base/constraint/constraint_pose_3D.h b/include/base/constraint/constraint_pose_3D.h index 8823eefb7..5fa1ec6c2 100644 --- a/include/base/constraint/constraint_pose_3D.h +++ b/include/base/constraint/constraint_pose_3D.h @@ -3,10 +3,9 @@ #define CONSTRAINT_POSE_3D_H_ //Wolf includes -#include "constraint_autodiff.h" -#include "frame_base.h" -#include "rotations.h" - +#include "base/constraint/constraint_autodiff.h" +#include "base/frame_base.h" +#include "base/rotations.h" namespace wolf { diff --git a/include/base/constraint/constraint_quaternion_absolute.h b/include/base/constraint/constraint_quaternion_absolute.h index 1b147a03d..eb2e2431a 100644 --- a/include/base/constraint/constraint_quaternion_absolute.h +++ b/include/base/constraint/constraint_quaternion_absolute.h @@ -9,11 +9,10 @@ #define CONSTRAINT_QUATERNION_ABSOLUTE_H_ //Wolf includes -#include "constraint_autodiff.h" -#include "local_parametrization_quaternion.h" -#include "frame_base.h" -#include "rotations.h" - +#include "base/constraint/constraint_autodiff.h" +#include "base/local_parametrization_quaternion.h" +#include "base/frame_base.h" +#include "base/rotations.h" namespace wolf { diff --git a/include/base/constraint/constraint_relative_2D_analytic.h b/include/base/constraint/constraint_relative_2D_analytic.h index 32b8b3059..a9655a914 100644 --- a/include/base/constraint/constraint_relative_2D_analytic.h +++ b/include/base/constraint/constraint_relative_2D_analytic.h @@ -2,8 +2,8 @@ #define CONSTRAINT_RELATIVE_2D_ANALYTIC_H_ //Wolf includes -#include "constraint_analytic.h" -#include "landmark_base.h" +#include "base/constraint/constraint_analytic.h" +#include "base/landmark/landmark_base.h" #include <Eigen/StdVector> namespace wolf { @@ -93,7 +93,6 @@ class ConstraintRelative2DAnalytic : public ConstraintAnalytic }; - /// IMPLEMENTATION /// inline Eigen::VectorXs ConstraintRelative2DAnalytic::evaluateResiduals( diff --git a/src/sensor/diff_drive_tools.h b/include/base/diff_drive_tools.h similarity index 99% rename from src/sensor/diff_drive_tools.h rename to include/base/diff_drive_tools.h index ccf243cc1..142b8c502 100644 --- a/src/sensor/diff_drive_tools.h +++ b/include/base/diff_drive_tools.h @@ -8,7 +8,7 @@ #ifndef _WOLF_PROCESSOR_DIFF_DRIVE_TOOLS_H_ #define _WOLF_PROCESSOR_DIFF_DRIVE_TOOLS_H_ -#include "../eigen_assert.h" +#include "base/eigen_assert.h" namespace wolf { @@ -359,7 +359,6 @@ void integrateExact(const Eigen::MatrixBase<T0> &data, delta_cov = jacobian * data_cov * jacobian.transpose(); } - /** * @brief integrate. Helper function to call either * `integrateRung` or `integrateExact` depending on the @@ -420,6 +419,6 @@ Eigen::Matrix<typename T::Scalar, 2, 2> computeWheelJointPositionCov( } // namespace wolf -#include "diff_drive_tools.hpp" +#include "base/diff_drive_tools.hpp" #endif /* _WOLF_PROCESSOR_DIFF_DRIVE_TOOLS_H_ */ diff --git a/src/sensor/diff_drive_tools.hpp b/include/base/diff_drive_tools.hpp similarity index 100% rename from src/sensor/diff_drive_tools.hpp rename to include/base/diff_drive_tools.hpp diff --git a/include/base/eigen_predicates.h b/include/base/eigen_predicates.h index 65e737b1b..55f2eeeb7 100644 --- a/include/base/eigen_predicates.h +++ b/include/base/eigen_predicates.h @@ -8,7 +8,7 @@ #ifndef _WOLF_EIGEN_PREDICATES_H_ #define _WOLF_EIGEN_PREDICATES_H_ -#include "rotations.h" +#include "base/rotations.h" namespace wolf { @@ -97,5 +97,4 @@ auto pred_angle_zero = [](const Scalar lhs, const Scalar precision) } // namespace wolf - #endif /* _WOLF_EIGEN_PREDICATES_H_ */ diff --git a/include/base/factory.h b/include/base/factory.h index c63d6df8d..b161126e4 100644 --- a/include/base/factory.h +++ b/include/base/factory.h @@ -9,7 +9,7 @@ #define FACTORY_H_ // wolf -#include "wolf.h" +#include "base/wolf.h" // std #include <string> @@ -308,8 +308,6 @@ inline std::string Factory<TypeBase, TypeInput...>::getClass() } // namespace wolf - - namespace wolf { @@ -349,7 +347,7 @@ inline std::string LandmarkFactory::getClass() // Frames class TimeStamp; } // namespace wolf -#include "frame_base.h" +#include "base/frame_base.h" namespace wolf{ typedef Factory<FrameBase, const FrameType&, const TimeStamp&, const Eigen::VectorXs&> FrameFactory; template<> @@ -378,7 +376,6 @@ inline std::string FrameFactory::getClass() namespace{ const bool WOLF_UNUSED FrameName##Registered = \ wolf::FrameFactory::get().registerCreator(FrameType, FrameName::create); }\ - } /* namespace wolf */ #endif /* FACTORY_H_ */ diff --git a/include/base/feature/feature_GPS_fix.h b/include/base/feature/feature_GPS_fix.h index 436d7f593..688176fdf 100644 --- a/include/base/feature/feature_GPS_fix.h +++ b/include/base/feature/feature_GPS_fix.h @@ -2,8 +2,8 @@ #define FEATURE_GPS_FIX_H_ //Wolf includes -#include "constraint_GPS_2D.h" -#include "feature_base.h" +#include "base/constraint/constraint_GPS_2D.h" +#include "base/feature/feature_base.h" //std includes diff --git a/include/base/feature/feature_GPS_pseudorange.h b/include/base/feature/feature_GPS_pseudorange.h index 5adaa5b6c..57f327e25 100644 --- a/include/base/feature/feature_GPS_pseudorange.h +++ b/include/base/feature/feature_GPS_pseudorange.h @@ -3,12 +3,11 @@ #define FEATURE_GPS_PSEUDORANGE_H_ //Wolf includes -#include "feature_base.h" +#include "base/feature/feature_base.h" //std includes #include <iomanip> - namespace wolf { WOLF_PTR_TYPEDEFS(FeatureGPSPseudorange); diff --git a/include/base/feature/feature_IMU.h b/include/base/feature/feature_IMU.h index 37e6884db..dfce11ff7 100644 --- a/include/base/feature/feature_IMU.h +++ b/include/base/feature/feature_IMU.h @@ -2,13 +2,12 @@ #define FEATURE_IMU_H_ //Wolf includes -#include "capture_IMU.h" -#include "feature_base.h" -#include "wolf.h" +#include "base/capture/capture_IMU.h" +#include "base/feature/feature_base.h" +#include "base/wolf.h" //std includes - namespace wolf { //WOLF_PTR_TYPEDEFS(CaptureIMU); @@ -18,7 +17,6 @@ class FeatureIMU : public FeatureBase { public: - /** \brief Constructor from and measures * * \param _measurement the measurement @@ -58,7 +56,6 @@ class FeatureIMU : public FeatureBase EIGEN_MAKE_ALIGNED_OPERATOR_NEW; }; - inline const Eigen::Vector3s& FeatureIMU::getAccBiasPreint() const { return acc_bias_preint_; diff --git a/include/base/feature/feature_base.h b/include/base/feature/feature_base.h index 1eb4fbf85..1ab46cccc 100644 --- a/include/base/feature/feature_base.h +++ b/include/base/feature/feature_base.h @@ -8,14 +8,13 @@ class ConstraintBase; } //Wolf includes -#include "wolf.h" -#include "node_base.h" +#include "base/wolf.h" +#include "base/node_base.h" //std includes namespace wolf { - //class FeatureBase class FeatureBase : public NodeBase, public std::enable_shared_from_this<FeatureBase> { @@ -97,7 +96,7 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature // IMPLEMENTATION -#include "constraint_base.h" +#include "base/constraint/constraint_base.h" namespace wolf{ diff --git a/include/base/feature/feature_corner_2D.h b/include/base/feature/feature_corner_2D.h index d43b82025..8e206749d 100644 --- a/include/base/feature/feature_corner_2D.h +++ b/include/base/feature/feature_corner_2D.h @@ -2,11 +2,10 @@ #define FEATURE_CORNER_2D_H_ //Wolf includes -#include "feature_base.h" +#include "base/feature/feature_base.h" //std includes - namespace wolf { WOLF_PTR_TYPEDEFS(FeatureCorner2D); diff --git a/src/feature/feature_diff_drive.h b/include/base/feature/feature_diff_drive.h similarity index 95% rename from src/feature/feature_diff_drive.h rename to include/base/feature/feature_diff_drive.h index 5c837469b..52877f4f5 100644 --- a/src/feature/feature_diff_drive.h +++ b/include/base/feature/feature_diff_drive.h @@ -9,7 +9,7 @@ #define _WOLF_FEATURE_DIFF_DRIVE_H_ //Wolf includes -#include "../feature_base.h" +#include "base/feature/feature_base.h" namespace wolf { diff --git a/include/base/feature/feature_line_2D.h b/include/base/feature/feature_line_2D.h index cd99621a0..5d5e6162d 100644 --- a/include/base/feature/feature_line_2D.h +++ b/include/base/feature/feature_line_2D.h @@ -2,13 +2,12 @@ #define FEATURE_LINE_2D_H_ //Wolf includes -#include "feature_base.h" +#include "base/feature/feature_base.h" namespace wolf { WOLF_PTR_TYPEDEFS(FeatureLine2D); - /** \brief class FeatureLine2D * * Line segment feature. diff --git a/include/base/feature/feature_match.h b/include/base/feature/feature_match.h index e776b1fcd..b96ce0023 100644 --- a/include/base/feature/feature_match.h +++ b/include/base/feature/feature_match.h @@ -2,7 +2,7 @@ #define FEATURE_MATCH_H_ // Wolf includes -#include "wolf.h" +#include "base/wolf.h" //wolf nampseace namespace wolf { @@ -34,4 +34,3 @@ struct FeatureMatch #endif - diff --git a/include/base/feature/feature_motion.h b/include/base/feature/feature_motion.h index 61b9bb59a..92331b6b1 100644 --- a/include/base/feature/feature_motion.h +++ b/include/base/feature/feature_motion.h @@ -8,8 +8,8 @@ #ifndef FEATURE_MOTION_H_ #define FEATURE_MOTION_H_ -#include "feature_base.h" -#include "capture_motion.h" +#include "base/feature/feature_base.h" +#include "base/capture/capture_motion.h" namespace wolf { diff --git a/include/base/feature/feature_odom_2D.h b/include/base/feature/feature_odom_2D.h index 3a43ee64f..92fd84fa7 100644 --- a/include/base/feature/feature_odom_2D.h +++ b/include/base/feature/feature_odom_2D.h @@ -2,13 +2,12 @@ #define FEATURE_ODOM_2D_H_ //Wolf includes -#include "feature_base.h" -#include "constraint_odom_2D.h" -#include "constraint_odom_2D_analytic.h" +#include "base/feature/feature_base.h" +#include "base/constraint/constraint_odom_2D.h" +#include "base/constraint/constraint_odom_2D_analytic.h" //std includes - namespace wolf { WOLF_PTR_TYPEDEFS(FeatureOdom2D); diff --git a/include/base/feature/feature_point_image.h b/include/base/feature/feature_point_image.h index 6e82b4305..aa07d80df 100644 --- a/include/base/feature/feature_point_image.h +++ b/include/base/feature/feature_point_image.h @@ -1,9 +1,8 @@ #ifndef FEATURE_IMAGE_H #define FEATURE_IMAGE_H - //Wolf includes -#include "feature_base.h" +#include "base/feature/feature_base.h" // Vision Utils includes #include <vision_utils.h> diff --git a/include/base/feature/feature_polyline_2D.h b/include/base/feature/feature_polyline_2D.h index 5c621a0bf..d289a58f3 100644 --- a/include/base/feature/feature_polyline_2D.h +++ b/include/base/feature/feature_polyline_2D.h @@ -8,7 +8,7 @@ #ifndef FEATURE_POLYLINE_2D_H_ #define FEATURE_POLYLINE_2D_H_ -#include "feature_base.h" +#include "base/feature/feature_base.h" namespace wolf { diff --git a/include/base/feature/feature_pose.h b/include/base/feature/feature_pose.h index f4d7ba061..1431f9207 100644 --- a/include/base/feature/feature_pose.h +++ b/include/base/feature/feature_pose.h @@ -1,9 +1,8 @@ #ifndef FEATURE_POSE_H_ #define FEATURE_POSE_H_ - //Wolf includes -#include "feature_base.h" +#include "base/feature/feature_base.h" //std includes // diff --git a/include/base/frame_base.h b/include/base/frame_base.h index f7e5f8379..16d13dbee 100644 --- a/include/base/frame_base.h +++ b/include/base/frame_base.h @@ -10,9 +10,9 @@ class StateBlock; } //Wolf includes -#include "wolf.h" -#include "time_stamp.h" -#include "node_base.h" +#include "base/wolf.h" +#include "base/time_stamp.h" +#include "base/node_base.h" //std includes @@ -26,7 +26,6 @@ typedef enum KEY_FRAME = 1 ///< key frame. It plays at optimizations. } FrameType; - //class FrameBase class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase> { @@ -68,9 +67,6 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase virtual ~FrameBase(); virtual void remove(); - - - // Frame properties ----------------------------------------------- public: unsigned int id(); @@ -160,14 +156,13 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase // IMPLEMENTATION // -#include "trajectory_base.h" -#include "capture_base.h" -#include "constraint_base.h" -#include "state_block.h" +#include "base/trajectory_base.h" +#include "base/capture/capture_base.h" +#include "base/constraint/constraint_base.h" +#include "base/state_block.h" namespace wolf { - inline unsigned int FrameBase::id() { return frame_id_; diff --git a/include/base/hardware_base.h b/include/base/hardware_base.h index 9d60cdd10..f3679dc4a 100644 --- a/include/base/hardware_base.h +++ b/include/base/hardware_base.h @@ -8,8 +8,8 @@ class SensorBase; } //Wolf includes -#include "wolf.h" -#include "node_base.h" +#include "base/wolf.h" +#include "base/node_base.h" namespace wolf { diff --git a/include/base/landmark/landmark_AHP.h b/include/base/landmark/landmark_AHP.h index e8d695b19..a706f87da 100644 --- a/include/base/landmark/landmark_AHP.h +++ b/include/base/landmark/landmark_AHP.h @@ -2,7 +2,7 @@ #define LANDMARK_AHP_H //Wolf includes -#include "landmark_base.h" +#include "base/landmark/landmark_base.h" // yaml #include <yaml-cpp/yaml.h> @@ -10,7 +10,6 @@ // Vision utils #include <vision_utils/vision_utils.h> - namespace wolf { WOLF_PTR_TYPEDEFS(LandmarkAHP); diff --git a/include/base/landmark/landmark_base.h b/include/base/landmark/landmark_base.h index 66485387a..8b365e9be 100644 --- a/include/base/landmark/landmark_base.h +++ b/include/base/landmark/landmark_base.h @@ -8,9 +8,9 @@ class StateBlock; } //Wolf includes -#include "wolf.h" -#include "node_base.h" -#include "time_stamp.h" +#include "base/wolf.h" +#include "base/node_base.h" +#include "base/time_stamp.h" //std includes @@ -19,7 +19,6 @@ class StateBlock; namespace wolf { - //class LandmarkBase class LandmarkBase : public NodeBase, public std::enable_shared_from_this<LandmarkBase> { @@ -75,7 +74,6 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma bool getCovariance(Eigen::MatrixXs& _cov) const; Eigen::MatrixXs getCovariance() const; - protected: virtual void removeStateBlocks(); @@ -85,7 +83,6 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma Scalar getDescriptor(unsigned int _ii) const; void setDescriptor(const Eigen::VectorXs& _descriptor); - // Navigate wolf tree virtual void setProblem(ProblemPtr _problem) final; @@ -100,9 +97,9 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma } -#include "map_base.h" -#include "constraint_base.h" -#include "state_block.h" +#include "base/map_base.h" +#include "base/constraint/constraint_base.h" +#include "base/state_block.h" namespace wolf{ diff --git a/include/base/landmark/landmark_container.h b/include/base/landmark/landmark_container.h index 0f51f3c5a..65a2c5b71 100644 --- a/include/base/landmark/landmark_container.h +++ b/include/base/landmark/landmark_container.h @@ -3,8 +3,8 @@ #define LANDMARK_CONTAINER_H_ //Wolf includes -#include "landmark_base.h" -#include "wolf.h" +#include "base/landmark/landmark_base.h" +#include "base/wolf.h" // Std includes @@ -36,7 +36,6 @@ class LandmarkContainer : public LandmarkBase * * B ---------------------------- A * | | - * | | * | <---+ | * | | | * | v | @@ -103,7 +102,6 @@ class LandmarkContainer : public LandmarkBase }; - } // namespace wolf #endif diff --git a/include/base/landmark/landmark_corner_2D.h b/include/base/landmark/landmark_corner_2D.h index 1eb99a975..c1ce86fbd 100644 --- a/include/base/landmark/landmark_corner_2D.h +++ b/include/base/landmark/landmark_corner_2D.h @@ -3,11 +3,10 @@ #define LANDMARK_CORNER_H_ //Wolf includes -#include "landmark_base.h" +#include "base/landmark/landmark_base.h" // Std includes - namespace wolf { WOLF_PTR_TYPEDEFS(LandmarkCorner2D); diff --git a/include/base/landmark/landmark_line_2D.h b/include/base/landmark/landmark_line_2D.h index 514cd550b..3a0f84c34 100644 --- a/include/base/landmark/landmark_line_2D.h +++ b/include/base/landmark/landmark_line_2D.h @@ -3,17 +3,15 @@ #define LANDMARK_LINE_2D_H_ //Wolf includes -#include "landmark_base.h" -#include "wolf.h" +#include "base/landmark/landmark_base.h" +#include "base/wolf.h" //std includes - namespace wolf { WOLF_PTR_TYPEDEFS(LandmarkLine2D); - //class LandmarkLine2D class LandmarkLine2D : public LandmarkBase { diff --git a/include/base/landmark/landmark_match.h b/include/base/landmark/landmark_match.h index ae47aa619..0c5d3a491 100644 --- a/include/base/landmark/landmark_match.h +++ b/include/base/landmark/landmark_match.h @@ -2,7 +2,7 @@ #define LANDMARK_MATCH_H_ // Wolf includes -#include "wolf.h" +#include "base/wolf.h" //wolf nampseace namespace wolf { @@ -33,7 +33,6 @@ struct LandmarkMatch } }; - }//end namespace #endif diff --git a/include/base/landmark/landmark_polyline_2D.h b/include/base/landmark/landmark_polyline_2D.h index a44a00b4c..4414a69f6 100644 --- a/include/base/landmark/landmark_polyline_2D.h +++ b/include/base/landmark/landmark_polyline_2D.h @@ -9,7 +9,7 @@ #define LANDMARK_POLYLINE_2D_H_ // Wolf -#include "landmark_base.h" +#include "base/landmark/landmark_base.h" // STL #include <deque> diff --git a/include/base/local_parametrization_angle.h b/include/base/local_parametrization_angle.h index 2445de112..912237c07 100644 --- a/include/base/local_parametrization_angle.h +++ b/include/base/local_parametrization_angle.h @@ -8,8 +8,8 @@ #ifndef LOCAL_PARAMETRIZATION_ANGLE_H_ #define LOCAL_PARAMETRIZATION_ANGLE_H_ -#include "local_parametrization_base.h" -#include "rotations.h" +#include "base/local_parametrization_base.h" +#include "base/rotations.h" namespace wolf { diff --git a/include/base/local_parametrization_base.h b/include/base/local_parametrization_base.h index acb9ec683..3b7dcc38d 100644 --- a/include/base/local_parametrization_base.h +++ b/include/base/local_parametrization_base.h @@ -8,8 +8,7 @@ #ifndef LOCAL_PARAMETRIZATION_BASE_H_ #define LOCAL_PARAMETRIZATION_BASE_H_ -#include "wolf.h" - +#include "base/wolf.h" namespace wolf { diff --git a/include/base/local_parametrization_homogeneous.h b/include/base/local_parametrization_homogeneous.h index 852c9aba7..f06dd2fb1 100644 --- a/include/base/local_parametrization_homogeneous.h +++ b/include/base/local_parametrization_homogeneous.h @@ -8,8 +8,7 @@ #ifndef LOCALPARAMETRIZATIONHOMOGENEOUS_H_ #define LOCALPARAMETRIZATIONHOMOGENEOUS_H_ -#include "local_parametrization_base.h" - +#include "base/local_parametrization_base.h" namespace wolf { diff --git a/include/base/local_parametrization_polyline_extreme.h b/include/base/local_parametrization_polyline_extreme.h index 62f7760d7..3a75a8a0f 100644 --- a/include/base/local_parametrization_polyline_extreme.h +++ b/include/base/local_parametrization_polyline_extreme.h @@ -8,8 +8,7 @@ #ifndef LOCAL_PARAMETRIZATION_POLYLINE_EXTREME_H_ #define LOCAL_PARAMETRIZATION_POLYLINE_EXTREME_H_ -#include "local_parametrization_base.h" - +#include "base/local_parametrization_base.h" namespace wolf { diff --git a/include/base/local_parametrization_quaternion.h b/include/base/local_parametrization_quaternion.h index 6cbcae7bb..f92676558 100644 --- a/include/base/local_parametrization_quaternion.h +++ b/include/base/local_parametrization_quaternion.h @@ -8,8 +8,7 @@ #ifndef LOCAL_PARAMETRIZATION_QUATERNION_H_ #define LOCAL_PARAMETRIZATION_QUATERNION_H_ -#include "local_parametrization_base.h" - +#include "base/local_parametrization_base.h" namespace wolf { diff --git a/include/base/logging.h b/include/base/logging.h index 03d21c002..3593fc718 100644 --- a/include/base/logging.h +++ b/include/base/logging.h @@ -17,7 +17,7 @@ #include "spdlog/fmt/bundled/ostream.h" // Wolf includes -#include "singleton.h" +#include "base/singleton.h" namespace wolf { namespace internal { diff --git a/include/base/map_base.h b/include/base/map_base.h index ef08f9469..f9e5d32b6 100644 --- a/include/base/map_base.h +++ b/include/base/map_base.h @@ -9,8 +9,8 @@ class LandmarkBase; } //Wolf includes -#include "wolf.h" -#include "node_base.h" +#include "base/wolf.h" +#include "base/node_base.h" //std includes diff --git a/include/base/motion_buffer.h b/include/base/motion_buffer.h index 33dfa226e..f8d0ad5be 100644 --- a/include/base/motion_buffer.h +++ b/include/base/motion_buffer.h @@ -8,8 +8,8 @@ #ifndef SRC_MOTIONBUFFER_H_ #define SRC_MOTIONBUFFER_H_ -#include "wolf.h" -#include "time_stamp.h" +#include "base/wolf.h" +#include "base/time_stamp.h" #include <list> #include <algorithm> @@ -51,7 +51,6 @@ struct Motion }; ///< One instance of the buffered data, corresponding to a particular time stamp. - /** \brief class for motion buffers. * * It consists of a buffer of pre-integrated motions (aka. delta-integrals) that is being filled @@ -117,7 +116,6 @@ inline void MotionBuffer::setCalibrationPreint(const VectorXs& _calib_preint) calib_preint_ = _calib_preint; } - } // namespace wolf #endif /* SRC_MOTIONBUFFER_H_ */ diff --git a/include/base/node_base.h b/include/base/node_base.h index bedc86612..2426f53a5 100644 --- a/include/base/node_base.h +++ b/include/base/node_base.h @@ -2,8 +2,7 @@ #define NODE_BASE_H_ // Wolf includes -#include "wolf.h" - +#include "base/wolf.h" namespace wolf { diff --git a/include/base/pinhole_tools.h b/include/base/pinhole_tools.h index ac721bd97..46cb1fb9d 100644 --- a/include/base/pinhole_tools.h +++ b/include/base/pinhole_tools.h @@ -7,12 +7,11 @@ * \date 06/04/2010 * \author jsola * - * * ## Add a description here ## * */ -#include "wolf.h" +#include "base/wolf.h" #include <iostream> @@ -131,7 +130,6 @@ projectPointToNormalizedPlane(const MatrixBase<Derived1>& _v, _dist = _v.norm(); } - /** * Canonical back-projection. * \param _u the 2D point in the image plane @@ -262,7 +260,6 @@ Matrix<typename Derived2::Scalar, 2, 1> distortPoint(const MatrixBase<Derived1> } } - /** * Radial distortion: ud = (1 + d_0 * r^2 + d_1 * r^4 + d_2 * r^6 + etc) * u, with jacobians * \param d the radial distortion parameters vector @@ -398,7 +395,6 @@ void undistortPoint(const MatrixBase<Derived1>& c, } } - /** * Pixellization from k = [u_0, v_0, a_u, a_v] * \param k the vector of intrinsic parameters, k = [u0, v0, au, av] @@ -426,8 +422,6 @@ Matrix<typename Derived2::Scalar, 2, 1> pixellizePoint(const MatrixBase<Derived1 return u; } - - /** * Pixellization from k = [u_0, v_0, a_u, a_v] with jacobians * \param k the vector of intrinsic parameters, k = [u0, v0, au, av] @@ -459,7 +453,6 @@ void pixellizePoint(const MatrixBase<Derived1>& k, U_ud(1, 1) = a_v; } - /** * Depixellization from k = [u_0, v_0, a_u, a_v] * \param k the vector of intrinsic parameters, k = [u0, v0, au, av] @@ -487,7 +480,6 @@ Matrix<typename Derived2::Scalar, 2, 1> depixellizePoint(const MatrixBase<Derive return ud; } - /** * Depixellization from k = [u_0, v_0, a_u, a_v], with Jacobians * \param k the vector of intrinsic parameters, k = [u0, v0, au, av] @@ -518,7 +510,6 @@ void depixellizePoint(const MatrixBase<Derived1>& k, UD_u(1, 1) = 1.0 / a_v; } - /** * Project a point into a pin-hole camera with radial distortion * \param k the vector of intrinsic parameters, k = [u0, v0, au, av] @@ -594,8 +585,6 @@ void projectPoint(const MatrixBase<Derived1>& k, u = pixellizePoint( k, distortPoint( d, up )); } - - /** * Project a point into a pin-hole camera with radial distortion * \param k the vector of intrinsic parameters, k = [u0, v0, au, av] @@ -630,7 +619,6 @@ void projectPoint(const MatrixBase<Derived1>& k, U_v.noalias() = U_ud * UD_up * UP_v; } - /** * Back-Project a point from a pin-hole camera with radial distortion * \param k the vector of intrinsic parameters, k = [u0, v0, au, av] @@ -688,7 +676,6 @@ void backprojectPoint(const MatrixBase<Derived1>& k, P_u.noalias() = P_up * UP_ud * UD_u; } - /** * Determine if a pixel is inside the region of interest * \param pix the pixel to test @@ -713,7 +700,6 @@ bool isInImage(const MatrixBase<VPix> & pix, const int & width, const int & heig return isInRoi(pix, 0, 0, width, height); } - /** * Compute distortion correction parameters. * @@ -764,7 +750,6 @@ void computeCorrectionModel(const Vk & k, const Vd & d, Vc & c) // this does not work: // jmath::LinearSolvers::solve_Cholesky(Rd, (rc - rd), c); - // therefore we solve manually the pseudo-inverse: Eigen::MatrixXs RdtRd(size, size); RdtRd = Rd.transpose() * Rd; @@ -780,9 +765,6 @@ void computeCorrectionModel(const Vk & k, const Vd & d, Vc & c) } // namespace pinhole - - } // namespace wolf - #endif // PINHOLETOOLS_H diff --git a/include/base/problem.h b/include/base/problem.h index 70cb8b7c4..f0ade3c32 100644 --- a/include/base/problem.h +++ b/include/base/problem.h @@ -14,16 +14,14 @@ struct ProcessorParamsBase; } //wolf includes -#include "wolf.h" -#include "frame_base.h" -#include "state_block.h" +#include "base/wolf.h" +#include "base/frame_base.h" +#include "base/state_block.h" // std includes - namespace wolf { - enum Notification { ADD, @@ -91,7 +89,6 @@ class Problem : public std::enable_shared_from_this<Problem> */ SensorBasePtr getSensorPtr(const std::string& _sensor_name); - /** \brief Factory method to install (create, and add to sensor) processors only from its properties * * This method creates a Processor, and adds it to the specified sensor's list of processors @@ -129,7 +126,6 @@ class Problem : public std::enable_shared_from_this<Problem> void clearProcessorMotion(); ProcessorMotionPtr& getProcessorMotionPtr(); - // Trajectory branch ---------------------------------- TrajectoryBasePtr getTrajectoryPtr(); virtual FrameBasePtr setPrior(const Eigen::VectorXs& _prior_state, // @@ -198,7 +194,6 @@ class Problem : public std::enable_shared_from_this<Problem> FrameBasePtr getLastKeyFramePtr ( ); FrameBasePtr closestKeyFrameToTimeStamp(const TimeStamp& _ts); - /** \brief Give the permission to a processor to create a new keyFrame * * This should implement a black list of processors that have forbidden keyframe creation. @@ -225,8 +220,6 @@ class Problem : public std::enable_shared_from_this<Problem> Eigen::VectorXs zeroState ( ); bool priorIsSet() const; - - // Map branch ----------------------------------------- MapBasePtr getMapPtr(); LandmarkBasePtr addLandmark(LandmarkBasePtr _lmk_ptr); @@ -235,8 +228,6 @@ class Problem : public std::enable_shared_from_this<Problem> void saveMap(const std::string& _filename_dot_yaml, // const std::string& _map_name = "Map automatically saved by Wolf"); - - // Covariances ------------------------------------------- void clearCovariance(); void addCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, const Eigen::MatrixXs& _cov); @@ -329,5 +320,4 @@ inline std::map<ConstraintBasePtr,Notification>& Problem::getConstraintNotificat } // namespace wolf - #endif // PROBLEM_H diff --git a/include/base/processor/processor_GPS.h b/include/base/processor/processor_GPS.h index a490bd2c1..35658410c 100644 --- a/include/base/processor/processor_GPS.h +++ b/include/base/processor/processor_GPS.h @@ -6,12 +6,11 @@ #define WOLF_PROCESSOR_GPS_H // Wolf includes -#include "processor_base.h" -#include "capture_GPS.h" +#include "base/processor/processor_base.h" +#include "base/capture/capture_GPS.h" // Std includes - namespace wolf { WOLF_PTR_TYPEDEFS(ProcessorGPS); diff --git a/include/base/processor/processor_IMU.h b/include/base/processor/processor_IMU.h index 92eb67efb..98c7cffee 100644 --- a/include/base/processor/processor_IMU.h +++ b/include/base/processor/processor_IMU.h @@ -2,10 +2,9 @@ #define PROCESSOR_IMU_H // Wolf -#include "capture_IMU.h" -#include "feature_IMU.h" -#include "processor_motion.h" - +#include "base/capture/capture_IMU.h" +#include "base/feature/feature_IMU.h" +#include "base/processor/processor_motion.h" namespace wolf { WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsIMU); diff --git a/include/base/processor/processor_base.h b/include/base/processor/processor_base.h index bcf5c46b6..6a1d23cfb 100644 --- a/include/base/processor/processor_base.h +++ b/include/base/processor/processor_base.h @@ -7,10 +7,10 @@ class SensorBase; } // Wolf includes -#include "wolf.h" -#include "node_base.h" -#include "time_stamp.h" -#include "frame_base.h" +#include "base/wolf.h" +#include "base/node_base.h" +#include "base/time_stamp.h" +#include "base/frame_base.h" // std #include <memory> @@ -35,8 +35,6 @@ class PackKeyFrame WOLF_PTR_TYPEDEFS(PackKeyFrame); - - /** \brief Buffer of Key frame class objects * * Object and functions to manage a buffer of KFPack objects. @@ -216,8 +214,8 @@ inline void ProcessorBase::setVotingActive(bool _voting_active) } -#include "sensor_base.h" -#include "constraint_base.h" +#include "base/sensor/sensor_base.h" +#include "base/constraint/constraint_base.h" namespace wolf { diff --git a/include/base/processor/processor_capture_holder.h b/include/base/processor/processor_capture_holder.h index b79743f45..9067b4cae 100644 --- a/include/base/processor/processor_capture_holder.h +++ b/include/base/processor/processor_capture_holder.h @@ -9,9 +9,9 @@ #define _WOLF_PROCESSOR_CAPTURE_HOLDER_H_ //Wolf includes -#include "processor_base.h" -#include "capture_base.h" -#include "capture_buffer.h" +#include "base/processor/processor_base.h" +#include "base/capture/capture_base.h" +#include "base/capture/capture_buffer.h" namespace wolf { diff --git a/src/processor/processor_diff_drive.h b/include/base/processor/processor_diff_drive.h similarity index 98% rename from src/processor/processor_diff_drive.h rename to include/base/processor/processor_diff_drive.h index f06b5940f..e219b552f 100644 --- a/src/processor/processor_diff_drive.h +++ b/include/base/processor/processor_diff_drive.h @@ -8,8 +8,8 @@ #ifndef _WOLF_PROCESSOR_DIFF_DRIVE_H_ #define _WOLF_PROCESSOR_DIFF_DRIVE_H_ -#include "../processor_motion.h" -#include "../sensors/diff_drive_tools.h" +#include "base/processor/processor_motion.h" +#include "base/diff_drive_tools.h" namespace wolf { diff --git a/include/base/processor/processor_factory.h b/include/base/processor/processor_factory.h index 00d2d50f6..8077aae49 100644 --- a/include/base/processor/processor_factory.h +++ b/include/base/processor/processor_factory.h @@ -15,7 +15,7 @@ struct ProcessorParamsBase; } // wolf -#include "factory.h" +#include "base/factory.h" // std @@ -125,8 +125,8 @@ namespace wolf * and bind it to a SensorOdom2D: * * \code - * #include "sensor_odom_2D.h" // provides SensorOdom2D and SensorFactory - * #include "processor_odom_2D.h" // provides ProcessorOdom2D and ProcessorFactory + * #include "base/sensor/sensor_odom_2D.h" // provides SensorOdom2D and SensorFactory + * #include "base/processor/processor_odom_2D.h" // provides ProcessorOdom2D and ProcessorFactory * * // Note: SensorOdom2D::create() is already registered, automatically. * // Note: ProcessorOdom2D::create() is already registered, automatically. @@ -153,9 +153,9 @@ namespace wolf * The example 1 above can be accomplished as follows (we obviated for simplicity all the parameter creation), * * \code - * #include "sensor_odom_2D.h" - * #include "processor_odom_2D.h" - * #include "problem.h" + * #include "base/sensor/sensor_odom_2D.h" + * #include "base/processor/processor_odom_2D.h" + * #include "base/problem.h" * * Problem problem(FRM_PO_2D); * problem.installSensor ( "ODOM 2D" , "Main odometer" , extrinsics , &intrinsics ); @@ -175,7 +175,6 @@ inline std::string ProcessorFactory::getClass() return "ProcessorFactory"; } - #define WOLF_REGISTER_PROCESSOR(ProcessorType, ProcessorName) \ namespace{ const bool WOLF_UNUSED ProcessorName##Registered = \ wolf::ProcessorFactory::get().registerCreator(ProcessorType, ProcessorName::create); }\ diff --git a/include/base/processor/processor_frame_nearest_neighbor_filter.h b/include/base/processor/processor_frame_nearest_neighbor_filter.h index 1d9fb56b6..be1205e27 100644 --- a/include/base/processor/processor_frame_nearest_neighbor_filter.h +++ b/include/base/processor/processor_frame_nearest_neighbor_filter.h @@ -2,8 +2,8 @@ #define _WOLF_SRC_PROCESSOR_FRAME_NEAREST_NEIGHBOR_FILTER_H // Wolf related headers -#include "processor_loopclosure_base.h" -#include "state_block.h" +#include "base/processor/processor_loopclosure_base.h" +#include "base/state_block.h" namespace wolf{ diff --git a/include/base/processor/processor_logging.h b/include/base/processor/processor_logging.h index 167beb892..4add7a8e9 100644 --- a/include/base/processor/processor_logging.h +++ b/include/base/processor/processor_logging.h @@ -9,7 +9,7 @@ #define _WOLF_PROCESSOR_LOGGING_H_ /// @brief un-comment for IDE highlights. -//#include "logging.h" +//#include "base/logging.h" #define __INTERNAL_WOLF_ASSERT_PROCESSOR \ static_assert(std::is_base_of<ProcessorBase, \ diff --git a/include/base/processor/processor_loopclosure_base.h b/include/base/processor/processor_loopclosure_base.h index 78dd9afce..1ded95bf0 100644 --- a/include/base/processor/processor_loopclosure_base.h +++ b/include/base/processor/processor_loopclosure_base.h @@ -2,7 +2,7 @@ #define _WOLF_PROCESSOR_LOOPCLOSURE_BASE_H // Wolf related headers -#include "processor_base.h" +#include "base/processor/processor_base.h" namespace wolf{ @@ -35,7 +35,6 @@ class ProcessorLoopClosureBase : public ProcessorBase { protected: - ProcessorParamsLoopClosurePtr params_loop_closure_; // Frames that are possible loop closure candidates according to @@ -96,7 +95,6 @@ protected: */ virtual void postProcess() { } - /** Find a loop closure between incoming data and all keyframe data * * This is called by process() . diff --git a/include/base/processor/processor_motion.h b/include/base/processor/processor_motion.h index e399bddcc..0ce05f202 100644 --- a/include/base/processor/processor_motion.h +++ b/include/base/processor/processor_motion.h @@ -9,9 +9,9 @@ #define PROCESSOR_MOTION_H_ // Wolf -#include "capture_motion.h" -#include "processor_base.h" -#include "time_stamp.h" +#include "base/capture/capture_motion.h" +#include "base/processor/processor_base.h" +#include "base/time_stamp.h" // std #include <iomanip> @@ -91,7 +91,6 @@ struct ProcessorParamsMotion : public ProcessorParamsBase * * \code statePlusDelta(R_old, delta_R, R_new) \endcode * - * * ### Defining (or not) the fromSensorFrame(): * * In most cases, one will be interested in avoiding the \b fromSensorFrame() issue. @@ -201,7 +200,6 @@ class ProcessorMotion : public ProcessorBase MotionBuffer& getBuffer(); const MotionBuffer& getBuffer() const; - // Helper functions: protected: @@ -237,11 +235,9 @@ class ProcessorMotion : public ProcessorBase PackKeyFramePtr computeProcessingStep(); - // These are the pure virtual functions doing the mathematics protected: - /** \brief convert raw CaptureMotion data to the delta-state format * * This function accepts raw data and time step dt, @@ -355,7 +351,6 @@ class ProcessorMotion : public ProcessorBase */ virtual Eigen::VectorXs deltaZero() const = 0; - /** \brief Interpolate motion to an intermediate time-stamp * * @param _ref The motion reference @@ -449,8 +444,6 @@ class ProcessorMotion : public ProcessorBase void setDistTraveled(const Scalar& _dist_traveled); void setAngleTurned(const Scalar& _angle_turned); - - protected: // Attributes SizeEigen x_size_; ///< The size of the state vector @@ -481,7 +474,7 @@ class ProcessorMotion : public ProcessorBase } -#include "frame_base.h" +#include "base/frame_base.h" namespace wolf{ @@ -558,7 +551,6 @@ inline const MotionBuffer& ProcessorMotion::getBuffer() const return last_ptr_->getBuffer(); } - inline MotionBuffer& ProcessorMotion::getBuffer() { return last_ptr_->getBuffer(); @@ -631,8 +623,6 @@ inline void ProcessorMotion::setAngleTurned(const Scalar& _angle_turned) params_motion_->angle_turned = _angle_turned; } - - } // namespace wolf #endif /* PROCESSOR_MOTION2_H_ */ diff --git a/include/base/processor/processor_odom_2D.h b/include/base/processor/processor_odom_2D.h index bf797ca8b..b7ea895df 100644 --- a/include/base/processor/processor_odom_2D.h +++ b/include/base/processor/processor_odom_2D.h @@ -8,11 +8,10 @@ #ifndef SRC_PROCESSOR_ODOM_2D_H_ #define SRC_PROCESSOR_ODOM_2D_H_ -#include "processor_motion.h" -#include "capture_odom_2D.h" -#include "constraint_odom_2D.h" -#include "rotations.h" - +#include "base/processor/processor_motion.h" +#include "base/capture/capture_odom_2D.h" +#include "base/constraint/constraint_odom_2D.h" +#include "base/rotations.h" namespace wolf { diff --git a/include/base/processor/processor_odom_3D.h b/include/base/processor/processor_odom_3D.h index 9a9828d4b..77766efc3 100644 --- a/include/base/processor/processor_odom_3D.h +++ b/include/base/processor/processor_odom_3D.h @@ -8,14 +8,13 @@ #ifndef SRC_PROCESSOR_ODOM_3D_H_ #define SRC_PROCESSOR_ODOM_3D_H_ -#include "processor_motion.h" -#include "sensor_odom_3D.h" -#include "capture_odom_3D.h" -#include "constraint_odom_3D.h" -#include "rotations.h" +#include "base/processor/processor_motion.h" +#include "base/sensor/sensor_odom_3D.h" +#include "base/capture/capture_odom_3D.h" +#include "base/constraint/constraint_odom_3D.h" +#include "base/rotations.h" #include <cmath> - namespace wolf { WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsOdom3D); @@ -25,7 +24,6 @@ struct ProcessorParamsOdom3D : public ProcessorParamsMotion // }; - WOLF_PTR_TYPEDEFS(ProcessorOdom3D); /** \brief Processor for 3d odometry integration. diff --git a/include/base/processor/processor_params_image.h b/include/base/processor/processor_params_image.h index fde468176..6ce3eaace 100644 --- a/include/base/processor/processor_params_image.h +++ b/include/base/processor/processor_params_image.h @@ -2,8 +2,8 @@ #define PROCESSOR_IMAGE_PARAMS_H // wolf -#include "processor_tracker_feature.h" -#include "processor_tracker_landmark.h" +#include "base/processor/processor_tracker_feature.h" +#include "base/processor/processor_tracker_landmark.h" namespace wolf { diff --git a/include/base/processor/processor_tracker.h b/include/base/processor/processor_tracker.h index 5d18402b6..f132626d7 100644 --- a/include/base/processor/processor_tracker.h +++ b/include/base/processor/processor_tracker.h @@ -8,8 +8,8 @@ #ifndef SRC_PROCESSOR_TRACKER_H_ #define SRC_PROCESSOR_TRACKER_H_ -#include "processor_base.h" -#include "capture_motion.h" +#include "base/processor/processor_base.h" +#include "base/capture/capture_motion.h" namespace wolf { diff --git a/include/base/processor/processor_tracker_feature.h b/include/base/processor/processor_tracker_feature.h index fc35c5465..52bd493f6 100644 --- a/include/base/processor/processor_tracker_feature.h +++ b/include/base/processor/processor_tracker_feature.h @@ -9,11 +9,11 @@ #define PROCESSOR_TRACKER_FEATURE_H_ //wolf includes -#include "processor_tracker.h" -#include "capture_base.h" -#include "feature_match.h" -#include "track_matrix.h" -#include "wolf.h" +#include "base/processor/processor_tracker.h" +#include "base/capture/capture_base.h" +#include "base/feature/feature_match.h" +#include "base/track_matrix.h" +#include "base/wolf.h" namespace wolf { diff --git a/include/base/processor/processor_tracker_feature_corner.h b/include/base/processor/processor_tracker_feature_corner.h index efc40a145..5bc33bf47 100644 --- a/include/base/processor/processor_tracker_feature_corner.h +++ b/include/base/processor/processor_tracker_feature_corner.h @@ -9,14 +9,14 @@ #define PROCESSOR_TRACKER_FEATURE_CORNER_H_ // Wolf includes -#include "sensor_laser_2D.h" -#include "capture_laser_2D.h" -#include "feature_corner_2D.h" -#include "landmark_corner_2D.h" -#include "constraint_corner_2D.h" -#include "state_block.h" -#include "data_association/association_tree.h" -#include "processor_tracker_feature.h" +#include "base/sensor/sensor_laser_2D.h" +#include "base/capture/capture_laser_2D.h" +#include "base/feature/feature_corner_2D.h" +#include "base/landmark/landmark_corner_2D.h" +#include "base/constraint/constraint_corner_2D.h" +#include "base/state_block.h" +#include "base/association/association_tree.h" +#include "base/processor/processor_tracker_feature.h" //laser_scan_utils //#include "laser_scan_utils/scan_basics.h" @@ -25,7 +25,6 @@ #include "laser_scan_utils/line_finder_iterative.h" #include "laser_scan_utils/corner_finder.h" - namespace wolf { WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTrackerFeatureCorner); diff --git a/include/base/processor/processor_tracker_feature_dummy.h b/include/base/processor/processor_tracker_feature_dummy.h index 3c163d276..dbc024b7c 100644 --- a/include/base/processor/processor_tracker_feature_dummy.h +++ b/include/base/processor/processor_tracker_feature_dummy.h @@ -8,9 +8,9 @@ #ifndef PROCESSOR_TRACKER_FEATURE_DUMMY_H_ #define PROCESSOR_TRACKER_FEATURE_DUMMY_H_ -#include "wolf.h" -#include "processor_tracker_feature.h" -#include "constraint_epipolar.h" +#include "base/wolf.h" +#include "base/processor/processor_tracker_feature.h" +#include "base/constraint/constraint_epipolar.h" namespace wolf { diff --git a/include/base/processor/processor_tracker_feature_image.h b/include/base/processor/processor_tracker_feature_image.h index 792c5d645..48f7e7a70 100644 --- a/include/base/processor/processor_tracker_feature_image.h +++ b/include/base/processor/processor_tracker_feature_image.h @@ -2,14 +2,14 @@ #define PROCESSOR_TRACKER_FEATURE_IMAGE_H // Wolf includes -#include "sensor_camera.h" -#include "capture_image.h" -#include "feature_point_image.h" -#include "state_block.h" -#include "state_quaternion.h" -#include "processor_tracker_feature.h" -#include "constraint_epipolar.h" -#include "processor_params_image.h" +#include "base/sensor/sensor_camera.h" +#include "base/capture/capture_image.h" +#include "base/feature/feature_point_image.h" +#include "base/state_block.h" +#include "base/state_quaternion.h" +#include "base/processor/processor_tracker_feature.h" +#include "base/constraint/constraint_epipolar.h" +#include "base/processor/processor_params_image.h" // vision_utils #include <detectors/detector_base.h> diff --git a/include/base/processor/processor_tracker_landmark.h b/include/base/processor/processor_tracker_landmark.h index bae94066b..293570afa 100644 --- a/include/base/processor/processor_tracker_landmark.h +++ b/include/base/processor/processor_tracker_landmark.h @@ -9,9 +9,9 @@ #define PROCESSOR_TRACKER_LANDMARK_H_ //wolf includes -#include "processor_tracker.h" -#include "capture_base.h" -#include "landmark_match.h" +#include "base/processor/processor_tracker.h" +#include "base/capture/capture_base.h" +#include "base/landmark/landmark_match.h" namespace wolf { @@ -23,7 +23,6 @@ struct ProcessorParamsTrackerLandmark : public ProcessorParamsTracker // }; - WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmark); /** \brief Landmark tracker processor @@ -168,6 +167,6 @@ class ProcessorTrackerLandmark : public ProcessorTracker }// namespace wolf // IMPLEMENTATION -#include "landmark_base.h" +#include "base/landmark/landmark_base.h" #endif /* PROCESSOR_TRACKER_LANDMARK_H_ */ diff --git a/include/base/processor/processor_tracker_landmark_corner.h b/include/base/processor/processor_tracker_landmark_corner.h index bb9eff8db..bd81b1643 100644 --- a/include/base/processor/processor_tracker_landmark_corner.h +++ b/include/base/processor/processor_tracker_landmark_corner.h @@ -9,14 +9,14 @@ #define SRC_PROCESSOR_TRACKER_LANDMARK_CORNER_H_ // Wolf includes -#include "sensor_laser_2D.h" -#include "capture_laser_2D.h" -#include "feature_corner_2D.h" -#include "landmark_corner_2D.h" -#include "constraint_corner_2D.h" -#include "state_block.h" -#include "data_association/association_tree.h" -#include "processor_tracker_landmark.h" +#include "base/sensor/sensor_laser_2D.h" +#include "base/capture/capture_laser_2D.h" +#include "base/feature/feature_corner_2D.h" +#include "base/landmark/landmark_corner_2D.h" +#include "base/constraint/constraint_corner_2D.h" +#include "base/state_block.h" +#include "base/association/association_tree.h" +#include "base/processor/processor_tracker_landmark.h" //laser_scan_utils //#include "laser_scan_utils/scan_basics.h" @@ -34,7 +34,6 @@ const Scalar angular_error_th_ = 10.0 * M_PI / 180.; //10 degrees; const Scalar position_error_th_ = 1; const Scalar min_features_ratio_th_ = 0.5; - //forward declaration to typedef class pointers struct ProcessorParamsLaser; typedef std::shared_ptr<ProcessorParamsLaser> ProcessorParamsLaserPtr; @@ -62,7 +61,6 @@ class ProcessorTrackerLandmarkCorner : public ProcessorTrackerLandmark laserscanutils::CornerFinder corner_finder_; //TODO: add corner_finder_params - FeatureBaseList corners_incoming_; FeatureBaseList corners_last_; unsigned int new_corners_th_; diff --git a/include/base/processor/processor_tracker_landmark_dummy.h b/include/base/processor/processor_tracker_landmark_dummy.h index 410b9c72c..1d9d66bba 100644 --- a/include/base/processor/processor_tracker_landmark_dummy.h +++ b/include/base/processor/processor_tracker_landmark_dummy.h @@ -8,7 +8,7 @@ #ifndef PROCESSOR_TRACKER_LANDMARK_DUMMY_H_ #define PROCESSOR_TRACKER_LANDMARK_DUMMY_H_ -#include "processor_tracker_landmark.h" +#include "base/processor/processor_tracker_landmark.h" namespace wolf { diff --git a/include/base/processor/processor_tracker_landmark_image.h b/include/base/processor/processor_tracker_landmark_image.h index 6d86249e7..1b7a0dbd6 100644 --- a/include/base/processor/processor_tracker_landmark_image.h +++ b/include/base/processor/processor_tracker_landmark_image.h @@ -3,11 +3,11 @@ // Wolf includes -#include "landmark_AHP.h" -#include "landmark_match.h" -#include "processor_params_image.h" -#include "processor_tracker_landmark.h" -#include "wolf.h" +#include "base/landmark/landmark_AHP.h" +#include "base/landmark/landmark_match.h" +#include "base/processor/processor_params_image.h" +#include "base/processor/processor_tracker_landmark.h" +#include "base/wolf.h" #include <algorithms/activesearch/alg_activesearch.h> #include <descriptors/descriptor_base.h> @@ -102,7 +102,6 @@ class ProcessorTrackerLandmarkImage : public ProcessorTrackerLandmark image_last_ = image_incoming_; } - /** * \brief Does the drawing of the features. * @@ -147,8 +146,6 @@ class ProcessorTrackerLandmarkImage : public ProcessorTrackerLandmark public: static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr = nullptr); - - /** \brief Create a new constraint * \param _feature_ptr pointer to the Feature to constrain * \param _landmark_ptr LandmarkBase pointer to the Landmark constrained. @@ -190,8 +187,6 @@ class ProcessorTrackerLandmarkImage : public ProcessorTrackerLandmark }; - } // namespace wolf - #endif // PROCESSOR_TRACKER_LANDMARK_IMAGE_H diff --git a/include/base/processor/processor_tracker_landmark_polyline.h b/include/base/processor/processor_tracker_landmark_polyline.h index 3c4c69031..e8d70824a 100644 --- a/include/base/processor/processor_tracker_landmark_polyline.h +++ b/include/base/processor/processor_tracker_landmark_polyline.h @@ -9,15 +9,15 @@ #define SRC_PROCESSOR_TRACKER_LANDMARK_POLYLINE_H_ // Wolf includes -#include "sensor_laser_2D.h" -#include "capture_laser_2D.h" -#include "feature_polyline_2D.h" -#include "landmark_polyline_2D.h" -#include "constraint_point_2D.h" -#include "constraint_point_to_line_2D.h" -#include "state_block.h" -#include "data_association/association_tree.h" -#include "processor_tracker_landmark.h" +#include "base/sensor/sensor_laser_2D.h" +#include "base/capture/capture_laser_2D.h" +#include "base/feature/feature_polyline_2D.h" +#include "base/landmark/landmark_polyline_2D.h" +#include "base/constraint/constraint_point_2D.h" +#include "base/constraint/constraint_point_to_line_2D.h" +#include "base/state_block.h" +#include "base/association/association_tree.h" +#include "base/processor/processor_tracker_landmark.h" //laser_scan_utils #include "laser_scan_utils/laser_scan.h" @@ -31,7 +31,6 @@ namespace wolf const Scalar position_error_th_ = 1; const Scalar min_features_ratio_th_ = 0.5; - //forward declaration to typedef class pointers struct LandmarkPolylineMatch; typedef std::shared_ptr<LandmarkPolylineMatch> LandmarkPolylineMatchPtr; @@ -40,10 +39,8 @@ typedef std::shared_ptr<LandmarkPolylineMatch> LandmarkPolylineMatchPtr; struct ProcessorParamsPolyline; typedef std::shared_ptr<ProcessorParamsPolyline> ProcessorParamsPolylinePtr; - WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmarkPolyline); - // Match Feature - Landmark struct LandmarkPolylineMatch : public LandmarkMatch { diff --git a/include/base/rotations.h b/include/base/rotations.h index 2ce225537..d4d3e711f 100644 --- a/include/base/rotations.h +++ b/include/base/rotations.h @@ -8,7 +8,7 @@ #ifndef ROTATIONS_H_ #define ROTATIONS_H_ -#include "wolf.h" +#include "base/wolf.h" namespace wolf { @@ -96,7 +96,6 @@ vee(const Eigen::MatrixBase<Derived>& _m) return (Eigen::Matrix<T, 3, 1>() << _m(2, 1), _m(0, 2), _m(1, 0)).finished(); } - //////////////////////////////////////////////////// // Rotation conversions q, R, Euler, back and forth // @@ -161,7 +160,6 @@ e2q(const Eigen::MatrixBase<D>& _euler) return Eigen::Quaternion<T>(az * ay * ax); } - /** \brief Euler angles to rotation matrix * \param _e = (roll pitch yaw) in ZYX convention * \return equivalent rotation matrix @@ -175,7 +173,6 @@ e2R(const Eigen::MatrixBase<Derived>& _e) return e2q(_e).matrix(); } - template <typename Derived> inline typename Eigen::MatrixBase<Derived>::Scalar getYaw(const Eigen::MatrixBase<Derived>& R) @@ -199,7 +196,6 @@ R2e(const Eigen::MatrixBase<Derived>& _R) return e; } - template<typename Derived> inline typename Eigen::Matrix<typename Derived::Scalar, 3, 1> q2e(const Eigen::MatrixBase<Derived>& _q) @@ -235,7 +231,6 @@ q2e(const Eigen::MatrixBase<Derived>& _q) return e; } - template<typename Derived> inline typename Eigen::Matrix<typename Derived::Scalar, 3, 1> q2e(const Eigen::QuaternionBase<Derived>& _q) @@ -243,9 +238,6 @@ q2e(const Eigen::QuaternionBase<Derived>& _q) return q2e(_q.coeffs()); } - - - /////////////////////////////////////////////////////////////// // Rotation conversions - exp and log maps @@ -295,7 +287,6 @@ log_q(const Eigen::QuaternionBase<Derived>& _q) // Eigen::AngleAxis<T> aa(_q); // return aa.angle() * aa.axis(); - // In the meanwhile, we have a custom implementation as follows typedef typename Derived::Scalar T; @@ -411,8 +402,6 @@ R2v(const Eigen::MatrixBase<Derived>& _R) return log_R(_R); } - - ///////////////////////////////////////////////////////////////// // Jacobians of SO(3) @@ -651,7 +640,6 @@ diff(const Eigen::QuaternionBase<D1>& q1, const Eigen::QuaternionBase<D2>& q2) return minus(q1, q2); } - } // namespace wolf #endif /* ROTATIONS_H_ */ diff --git a/include/base/sensor/sensor_GPS.h b/include/base/sensor/sensor_GPS.h index c43cd13cb..17b6f2033 100644 --- a/include/base/sensor/sensor_GPS.h +++ b/include/base/sensor/sensor_GPS.h @@ -9,9 +9,9 @@ */ //wolf -#include "sensor_base.h" +#include "base/sensor/sensor_base.h" //#include "sensor_factory.h" -//#include "factory.h" +//#include "base/factory.h" // std @@ -40,7 +40,6 @@ public: public: static SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_p, const IntrinsicsBasePtr _intrinsics); - }; } // namespace wolf diff --git a/include/base/sensor/sensor_GPS_fix.h b/include/base/sensor/sensor_GPS_fix.h index c319d159f..e74ff5e89 100644 --- a/include/base/sensor/sensor_GPS_fix.h +++ b/include/base/sensor/sensor_GPS_fix.h @@ -3,13 +3,10 @@ #define SENSOR_GPS_FIX_H_ //wolf includes -#include "sensor_base.h" +#include "base/sensor/sensor_base.h" // std includes - - - namespace wolf { WOLF_STRUCT_PTR_TYPEDEFS(IntrinsicsGPSFix); diff --git a/include/base/sensor/sensor_IMU.h b/include/base/sensor/sensor_IMU.h index 2f5d9baa5..17c1c873d 100644 --- a/include/base/sensor/sensor_IMU.h +++ b/include/base/sensor/sensor_IMU.h @@ -2,7 +2,7 @@ #define SENSOR_IMU_H //wolf includes -#include "sensor_base.h" +#include "base/sensor/sensor_base.h" #include <iostream> namespace wolf { diff --git a/include/base/sensor/sensor_base.h b/include/base/sensor/sensor_base.h index b52e8816e..8707d13ac 100644 --- a/include/base/sensor/sensor_base.h +++ b/include/base/sensor/sensor_base.h @@ -9,15 +9,14 @@ class StateBlock; } //Wolf includes -#include "wolf.h" -#include "node_base.h" -#include "time_stamp.h" +#include "base/wolf.h" +#include "base/node_base.h" +#include "base/time_stamp.h" //std includes namespace wolf { - /** \brief base struct for intrinsic sensor parameters * * Derive from this struct to create structs of sensor intrinsic parameters. @@ -162,10 +161,10 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa } -#include "problem.h" -#include "hardware_base.h" -#include "capture_base.h" -#include "processor_base.h" +#include "base/problem.h" +#include "base/hardware_base.h" +#include "base/capture/capture_base.h" +#include "base/processor/processor_base.h" namespace wolf{ @@ -273,8 +272,6 @@ inline void SensorBase::setIntrinsicDynamic(bool _intrinsic_dynamic) intrinsic_dynamic_ = _intrinsic_dynamic; } - - } // namespace wolf #endif diff --git a/include/base/sensor/sensor_camera.h b/include/base/sensor/sensor_camera.h index d8641510d..2214fc14a 100644 --- a/include/base/sensor/sensor_camera.h +++ b/include/base/sensor/sensor_camera.h @@ -2,7 +2,7 @@ #define SENSOR_CAMERA_H //wolf includes -#include "sensor_base.h" +#include "base/sensor/sensor_base.h" namespace wolf { @@ -20,7 +20,6 @@ struct IntrinsicsCamera : public IntrinsicsBase virtual ~IntrinsicsCamera() = default; }; - WOLF_PTR_TYPEDEFS(SensorCamera); /**Pin-hole camera sensor */ diff --git a/src/sensor/sensor_diff_drive.h b/include/base/sensor/sensor_diff_drive.h similarity index 97% rename from src/sensor/sensor_diff_drive.h rename to include/base/sensor/sensor_diff_drive.h index 925c840e5..0f5cf4259 100644 --- a/src/sensor/sensor_diff_drive.h +++ b/include/base/sensor/sensor_diff_drive.h @@ -9,8 +9,8 @@ #define WOLF_SENSOR_DIFF_DRIVE_H_ //wolf includes -#include "../sensor_base.h" -#include "diff_drive_tools.h" +#include "base/sensor/sensor_base.h" +#include "base/diff_drive_tools.h" namespace wolf { diff --git a/include/base/sensor/sensor_factory.h b/include/base/sensor/sensor_factory.h index 3e7e1aabc..32884c18f 100644 --- a/include/base/sensor/sensor_factory.h +++ b/include/base/sensor/sensor_factory.h @@ -15,7 +15,7 @@ struct IntrinsicsBase; } // wolf -#include "factory.h" +#include "base/factory.h" namespace wolf { @@ -174,7 +174,7 @@ namespace wolf * * \code * #include "sensor_factory.h" - * #include "sensor_camera.h" // provides SensorCamera + * #include "base/sensor/sensor_camera.h" // provides SensorCamera * * // Note: SensorCamera::create() is already registered, automatically. * diff --git a/include/base/sensor/sensor_laser_2D.h b/include/base/sensor/sensor_laser_2D.h index 9cdcf5567..6b927c5b4 100644 --- a/include/base/sensor/sensor_laser_2D.h +++ b/include/base/sensor/sensor_laser_2D.h @@ -4,7 +4,7 @@ #define SENSOR_LASER_2D_H_ //wolf -#include "sensor_base.h" +#include "base/sensor/sensor_base.h" //laser_scan_utils #include "laser_scan_utils/laser_scan.h" diff --git a/include/base/sensor/sensor_odom_2D.h b/include/base/sensor/sensor_odom_2D.h index 4ba0fc37d..75110b2cd 100644 --- a/include/base/sensor/sensor_odom_2D.h +++ b/include/base/sensor/sensor_odom_2D.h @@ -3,7 +3,7 @@ #define SENSOR_ODOM_2D_H_ //wolf includes -#include "sensor_base.h" +#include "base/sensor/sensor_base.h" namespace wolf { @@ -59,7 +59,6 @@ class SensorOdom2D : public SensorBase public: static SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_pq, const IntrinsicsBasePtr _intrinsics); - }; } // namespace wolf diff --git a/include/base/sensor/sensor_odom_3D.h b/include/base/sensor/sensor_odom_3D.h index 19f037162..76c9a3d5b 100644 --- a/include/base/sensor/sensor_odom_3D.h +++ b/include/base/sensor/sensor_odom_3D.h @@ -9,7 +9,7 @@ #define SRC_SENSOR_ODOM_3D_H_ //wolf includes -#include "sensor_base.h" +#include "base/sensor/sensor_base.h" namespace wolf { @@ -60,7 +60,6 @@ class SensorOdom3D : public SensorBase public: static SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_pq, const IntrinsicsBasePtr _intrinsics); - }; inline Scalar SensorOdom3D::getDispVarToDispNoiseFactor() const diff --git a/include/base/solver/solver_manager.h b/include/base/solver/solver_manager.h index 9f1189495..4801fd57f 100644 --- a/include/base/solver/solver_manager.h +++ b/include/base/solver/solver_manager.h @@ -2,9 +2,9 @@ #define _WOLF_SOLVER_MANAGER_H_ //wolf includes -#include "../wolf.h" -#include "../state_block.h" -#include "../constraint_base.h" +#include "base/wolf.h" +#include "base/state_block.h" +#include "base/constraint/constraint_base.h" namespace wolf { diff --git a/include/base/solver_suitesparse/ccolamd_ordering.h b/include/base/solver_suitesparse/ccolamd_ordering.h index d88c0d41b..bae2eff4d 100644 --- a/include/base/solver_suitesparse/ccolamd_ordering.h +++ b/include/base/solver_suitesparse/ccolamd_ordering.h @@ -8,8 +8,6 @@ #ifndef TRUNK_SRC_WOLF_SOLVER_CCOLAMD_ORDERING_H_ #define TRUNK_SRC_WOLF_SOLVER_CCOLAMD_ORDERING_H_ - - //std includes #include <iostream> @@ -79,5 +77,4 @@ class CCOLAMDOrdering }; } - #endif /* TRUNK_SRC_WOLF_SOLVER_CCOLAMD_ORDERING_H_ */ diff --git a/include/base/solver_suitesparse/cost_function_base.h b/include/base/solver_suitesparse/cost_function_base.h index 76ba74da3..8aae707ae 100644 --- a/include/base/solver_suitesparse/cost_function_base.h +++ b/include/base/solver_suitesparse/cost_function_base.h @@ -8,7 +8,7 @@ #ifndef TRUNK_SRC_SOLVER_COST_FUNCTION_BASE_H_ #define TRUNK_SRC_SOLVER_COST_FUNCTION_BASE_H_ -#include "wolf.h" +#include "base/wolf.h" #include <Eigen/StdVector> class CostFunctionBase @@ -75,7 +75,6 @@ class CostFunctionBase virtual void evaluateResidualJacobians() = 0; - void getResidual(Eigen::VectorXs &residual) { residual.resize(residual_.size()); diff --git a/include/base/solver_suitesparse/cost_function_sparse.h b/include/base/solver_suitesparse/cost_function_sparse.h index 37033bcb9..713e29bd3 100644 --- a/include/base/solver_suitesparse/cost_function_sparse.h +++ b/include/base/solver_suitesparse/cost_function_sparse.h @@ -2,7 +2,7 @@ #define TRUNK_SRC_SOLVER_COST_FUNCTION_SPARSE_H_ //wolf includes -#include "wolf.h" +#include "base/wolf.h" #include "cost_function_sparse_base.h" // CERES JET @@ -229,7 +229,6 @@ class CostFunctionSparse<ConstraintT, BLOCK_5_SIZE, BLOCK_6_SIZE, 0, - 0, 0> : CostFunctionSparseBase<ConstraintT, MEASUREMENT_SIZE, BLOCK_0_SIZE, @@ -240,7 +239,6 @@ class CostFunctionSparse<ConstraintT, BLOCK_5_SIZE, BLOCK_6_SIZE, 0, - 0, 0> { public: @@ -255,7 +253,6 @@ class CostFunctionSparse<ConstraintT, BLOCK_5_SIZE, BLOCK_6_SIZE, 0, - 0, 0>(_constraint_ptr) { @@ -284,8 +281,6 @@ class CostFunctionSparse<ConstraintT, BLOCK_4_SIZE, BLOCK_5_SIZE, 0, - 0, - 0, 0> : CostFunctionSparseBase<ConstraintT, MEASUREMENT_SIZE, BLOCK_0_SIZE, @@ -295,8 +290,6 @@ class CostFunctionSparse<ConstraintT, BLOCK_4_SIZE, BLOCK_5_SIZE, 0, - 0, - 0, 0> { public: @@ -310,8 +303,6 @@ class CostFunctionSparse<ConstraintT, BLOCK_4_SIZE, BLOCK_5_SIZE, 0, - 0, - 0, 0>(_constraint_ptr) { @@ -338,9 +329,6 @@ class CostFunctionSparse<ConstraintT, BLOCK_3_SIZE, BLOCK_4_SIZE, 0, - 0, - 0, - 0, 0> : CostFunctionSparseBase<ConstraintT, MEASUREMENT_SIZE, BLOCK_0_SIZE, @@ -349,9 +337,6 @@ class CostFunctionSparse<ConstraintT, BLOCK_3_SIZE, BLOCK_4_SIZE, 0, - 0, - 0, - 0, 0> { public: @@ -364,9 +349,6 @@ class CostFunctionSparse<ConstraintT, BLOCK_3_SIZE, BLOCK_4_SIZE, 0, - 0, - 0, - 0, 0>(_constraint_ptr) { @@ -391,10 +373,6 @@ class CostFunctionSparse<ConstraintT, BLOCK_2_SIZE, BLOCK_3_SIZE, 0, - 0, - 0, - 0, - 0, 0> : CostFunctionSparseBase<ConstraintT, MEASUREMENT_SIZE, BLOCK_0_SIZE, @@ -402,10 +380,6 @@ class CostFunctionSparse<ConstraintT, BLOCK_2_SIZE, BLOCK_3_SIZE, 0, - 0, - 0, - 0, - 0, 0> { public: @@ -417,10 +391,6 @@ class CostFunctionSparse<ConstraintT, BLOCK_2_SIZE, BLOCK_3_SIZE, 0, - 0, - 0, - 0, - 0, 0>(_constraint_ptr) { @@ -443,22 +413,12 @@ class CostFunctionSparse<ConstraintT, BLOCK_1_SIZE, BLOCK_2_SIZE, 0, - 0, - 0, - 0, - 0, - 0, 0> : CostFunctionSparseBase<ConstraintT, MEASUREMENT_SIZE, BLOCK_0_SIZE, BLOCK_1_SIZE, BLOCK_2_SIZE, 0, - 0, - 0, - 0, - 0, - 0, 0> { public: @@ -469,11 +429,6 @@ class CostFunctionSparse<ConstraintT, BLOCK_1_SIZE, BLOCK_2_SIZE, 0, - 0, - 0, - 0, - 0, - 0, 0>(_constraint_ptr) { @@ -494,23 +449,11 @@ class CostFunctionSparse<ConstraintT, BLOCK_0_SIZE, BLOCK_1_SIZE, 0, - 0, - 0, - 0, - 0, - 0, - 0, 0> : CostFunctionSparseBase<ConstraintT, MEASUREMENT_SIZE, BLOCK_0_SIZE, BLOCK_1_SIZE, 0, - 0, - 0, - 0, - 0, - 0, - 0, 0> { public: @@ -520,12 +463,6 @@ class CostFunctionSparse<ConstraintT, BLOCK_0_SIZE, BLOCK_1_SIZE, 0, - 0, - 0, - 0, - 0, - 0, - 0, 0>(_constraint_ptr) { @@ -544,24 +481,10 @@ class CostFunctionSparse<ConstraintT, MEASUREMENT_SIZE, BLOCK_0_SIZE, 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, 0> : CostFunctionSparseBase<ConstraintT, MEASUREMENT_SIZE, BLOCK_0_SIZE, 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, 0> { public: @@ -570,13 +493,6 @@ class CostFunctionSparse<ConstraintT, MEASUREMENT_SIZE, BLOCK_0_SIZE, 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, 0>(_constraint_ptr) { diff --git a/include/base/solver_suitesparse/cost_function_sparse_base.h b/include/base/solver_suitesparse/cost_function_sparse_base.h index 4b54001f6..d10fbab58 100644 --- a/include/base/solver_suitesparse/cost_function_sparse_base.h +++ b/include/base/solver_suitesparse/cost_function_sparse_base.h @@ -9,7 +9,7 @@ #define TRUNK_SRC_SOLVER_COST_FUNCTION_SPARSE_BASE_H_ //wolf includes -#include "wolf.h" +#include "base/wolf.h" #include "cost_function_base.h" // CERES JET diff --git a/include/base/solver_suitesparse/qr_solver.h b/include/base/solver_suitesparse/qr_solver.h index 493ee2dc8..6d032e8b2 100644 --- a/include/base/solver_suitesparse/qr_solver.h +++ b/include/base/solver_suitesparse/qr_solver.h @@ -13,12 +13,12 @@ #include <ctime> //Wolf includes -#include "state_block.h" +#include "base/state_block.h" #include "../constraint_sparse.h" -#include "../constraint_odom_2D.h" -#include "../constraint_corner_2D.h" -#include "../constraint_container.h" -#include "sparse_utils.h" +#include "base/constraint/constraint_odom_2D.h" +#include "base/constraint/constraint_corner_2D.h" +#include "base/constraint/constraint_container.h" +#include "base/solver_suitesparse/sparse_utils.h" // wolf solver #include "solver/ccolamd_ordering.h" @@ -28,7 +28,7 @@ #include <eigen3/Eigen/OrderingMethods> #include <eigen3/Eigen/SparseQR> #include <Eigen/StdVector> -#include "../constraint_pose_2D.h" +#include "base/constraint/constraint_pose_2D.h" namespace wolf { diff --git a/include/base/solver_suitesparse/solver_QR.h b/include/base/solver_suitesparse/solver_QR.h index 2fa0d11d1..9451ceec9 100644 --- a/include/base/solver_suitesparse/solver_QR.h +++ b/include/base/solver_suitesparse/solver_QR.h @@ -29,5 +29,4 @@ class SolverQR private: }; - #endif /* TRUNK_SRC_SOLVER_SOLVER_QR_H_ */ diff --git a/include/base/solver_suitesparse/solver_manager.h b/include/base/solver_suitesparse/solver_manager.h index b9084d4fb..479cc4509 100644 --- a/include/base/solver_suitesparse/solver_manager.h +++ b/include/base/solver_suitesparse/solver_manager.h @@ -2,9 +2,9 @@ #define CERES_MANAGER_H_ //wolf includes -#include "../constraint_GPS_2D.h" -#include "../wolf.h" -#include "../state_block.h" +#include "base/constraint/constraint_GPS_2D.h" +#include "base/wolf.h" +#include "base/state_block.h" #include "../state_point.h" #include "../state_complex_angle.h" #include "../state_theta.h" @@ -21,7 +21,6 @@ class SolverManager { protected: - public: SolverManager(ceres::Problem::Options _options); diff --git a/include/base/state_angle.h b/include/base/state_angle.h index 81da7a882..c61286e13 100644 --- a/include/base/state_angle.h +++ b/include/base/state_angle.h @@ -8,8 +8,8 @@ #ifndef STATE_ANGLE_H_ #define STATE_ANGLE_H_ -#include "state_block.h" -#include "local_parametrization_angle.h" +#include "base/state_block.h" +#include "base/local_parametrization_angle.h" namespace wolf { diff --git a/include/base/state_block.h b/include/base/state_block.h index cdfc94c24..06962ced0 100644 --- a/include/base/state_block.h +++ b/include/base/state_block.h @@ -9,7 +9,7 @@ class LocalParametrizationBase; } //Wolf includes -#include "wolf.h" +#include "base/wolf.h" //std includes #include <iostream> @@ -154,9 +154,9 @@ public: } // namespace wolf // IMPLEMENTATION -#include "local_parametrization_base.h" -#include "node_base.h" -#include "problem.h" +#include "base/local_parametrization_base.h" +#include "base/node_base.h" +#include "base/problem.h" namespace wolf { diff --git a/include/base/state_homogeneous_3D.h b/include/base/state_homogeneous_3D.h index f8d4951a8..adfda9018 100644 --- a/include/base/state_homogeneous_3D.h +++ b/include/base/state_homogeneous_3D.h @@ -8,8 +8,8 @@ #ifndef SRC_STATE_HOMOGENEOUS_3D_H_ #define SRC_STATE_HOMOGENEOUS_3D_H_ -#include "state_block.h" -#include "local_parametrization_homogeneous.h" +#include "base/state_block.h" +#include "base/local_parametrization_homogeneous.h" namespace wolf { diff --git a/include/base/state_quaternion.h b/include/base/state_quaternion.h index b68fc574e..d990ce1f8 100644 --- a/include/base/state_quaternion.h +++ b/include/base/state_quaternion.h @@ -8,8 +8,8 @@ #ifndef SRC_STATE_QUATERNION_H_ #define SRC_STATE_QUATERNION_H_ -#include "state_block.h" -#include "local_parametrization_quaternion.h" +#include "base/state_block.h" +#include "base/local_parametrization_quaternion.h" namespace wolf { diff --git a/include/base/time_stamp.h b/include/base/time_stamp.h index d38d25b5f..02a5599af 100644 --- a/include/base/time_stamp.h +++ b/include/base/time_stamp.h @@ -3,7 +3,7 @@ #define TIME_STAMP_H_ //wolf includes -#include "wolf.h" +#include "base/wolf.h" //C, std #include <sys/time.h> diff --git a/include/base/track_matrix.h b/include/base/track_matrix.h index 6f32d4cdf..5aa6159c4 100644 --- a/include/base/track_matrix.h +++ b/include/base/track_matrix.h @@ -8,8 +8,8 @@ #ifndef TRACK_MATRIX_H_ #define TRACK_MATRIX_H_ -#include "feature_base.h" -#include "capture_base.h" +#include "base/feature/feature_base.h" +#include "base/capture/capture_base.h" #include <map> #include <vector> diff --git a/include/base/trajectory_base.h b/include/base/trajectory_base.h index cb394c5bb..4e8d261d3 100644 --- a/include/base/trajectory_base.h +++ b/include/base/trajectory_base.h @@ -10,8 +10,8 @@ class TimeStamp; } //Wolf includes -#include "wolf.h" -#include "node_base.h" +#include "base/wolf.h" +#include "base/node_base.h" //std includes @@ -71,13 +71,11 @@ inline void TrajectoryBase::setLastKeyFramePtr(FrameBasePtr _key_frame_ptr) last_key_frame_ptr_ = _key_frame_ptr; } - inline std::string TrajectoryBase::getFrameStructure() const { return frame_structure_; } - } // namespace wolf #endif diff --git a/include/base/wolf.h b/include/base/wolf.h index bb5439030..d642f730a 100644 --- a/include/base/wolf.h +++ b/include/base/wolf.h @@ -10,7 +10,7 @@ // Enable project-specific definitions and macros #include "internal/config.h" -#include "logging.h" +#include "base/logging.h" //includes from Eigen lib #include <Eigen/Dense> @@ -45,7 +45,6 @@ namespace wolf { typedef double Scalar; // Use this for double, 64 bit precision //typedef long double Scalar; // Use this for long double, 128 bit precision - /** * \brief Vector and Matrices size type for the Wolf project * @@ -205,7 +204,6 @@ struct MatrixSizeCheck // // End of check matrix sizes ///////////////////////////////////////////////// - ///////////////////////////////////////////////////////////////////////// // TYPEDEFS FOR POINTERS, LISTS AND ITERATORS IN THE WOLF TREE ///////////////////////////////////////////////////////////////////////// @@ -227,7 +225,6 @@ struct MatrixSizeCheck typedef std::shared_ptr<StructName> StructName##Ptr; \ typedef std::shared_ptr<const StructName> StructName##ConstPtr; \ - // NodeBase WOLF_PTR_TYPEDEFS(NodeBase); @@ -289,7 +286,6 @@ WOLF_PTR_TYPEDEFS(StateQuaternion); // - - Local Parametrization WOLF_PTR_TYPEDEFS(LocalParametrizationBase); - // ================================================== // Some dangling functions @@ -373,6 +369,4 @@ bool makePosDef(Eigen::Matrix<T,N,N,RC>& M, const T& eps = Constants::EPS) } // namespace wolf - - #endif /* WOLF_H_ */ diff --git a/serialization/cereal/io.h b/serialization/cereal/io.h index 524af2e41..8a8e58c2d 100644 --- a/serialization/cereal/io.h +++ b/serialization/cereal/io.h @@ -167,7 +167,6 @@ using SerializerJSON = Serializer<Extensions::JSON, cereal::JSONInputArchive, cereal::JSONOutputArchive>; - } /* namespace serialization */ template <typename... T> diff --git a/serialization/cereal/serialization_local_parametrization_base.h b/serialization/cereal/serialization_local_parametrization_base.h index b57b9f564..0c38a5a61 100644 --- a/serialization/cereal/serialization_local_parametrization_base.h +++ b/serialization/cereal/serialization_local_parametrization_base.h @@ -1,7 +1,7 @@ #ifndef _WOLF_IO_CEREAL_LOCAL_PARAMETRIZATION_BASE_H_ #define _WOLF_IO_CEREAL_LOCAL_PARAMETRIZATION_BASE_H_ -#include "../../local_parametrization_base.h" +#include "base/local_parametrization_base.h" #include <cereal/cereal.hpp> #include <cereal/types/polymorphic.hpp> @@ -21,7 +21,6 @@ inline void serialize( // } - } //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 index aa04a9904..2b36232d5 100644 --- a/serialization/cereal/serialization_local_parametrization_homogeneous.h +++ b/serialization/cereal/serialization_local_parametrization_homogeneous.h @@ -1,7 +1,7 @@ #ifndef _WOLF_IO_CEREAL_LOCAL_PARAMETRIZATION_BASE_HOMOGENEOUS_H_ #define _WOLF_IO_CEREAL_LOCAL_PARAMETRIZATION_BASE_HOMOGENEOUS_H_ -#include "../../local_parametrization_homogeneous.h" +#include "base/local_parametrization_homogeneous.h" #include "serialization_local_parametrization_base.h" diff --git a/serialization/cereal/serialization_local_parametrization_quaternion.h b/serialization/cereal/serialization_local_parametrization_quaternion.h index 10b04f67c..95c71f4c2 100644 --- a/serialization/cereal/serialization_local_parametrization_quaternion.h +++ b/serialization/cereal/serialization_local_parametrization_quaternion.h @@ -1,7 +1,7 @@ #ifndef WOLF_IO_SERIALIZATION_LOCAL_PARAMETRIZATION_QUATERNION_H_ #define WOLF_IO_SERIALIZATION_LOCAL_PARAMETRIZATION_QUATERNION_H_ -#include "../../local_parametrization_quaternion.h" +#include "base/local_parametrization_quaternion.h" #include "serialization_local_parametrization_base.h" diff --git a/serialization/cereal/serialization_node_base.h b/serialization/cereal/serialization_node_base.h index dbf3292d5..5a0a039a3 100644 --- a/serialization/cereal/serialization_node_base.h +++ b/serialization/cereal/serialization_node_base.h @@ -2,7 +2,7 @@ #define _WOLF_IO_CEREAL_NODE_BASE_H_ // Wolf includes -#include "../../node_base.h" +#include "base/node_base.h" #include <cereal/cereal.hpp> #include <cereal/types/polymorphic.hpp> diff --git a/serialization/cereal/serialization_processor_odom2d_params.h b/serialization/cereal/serialization_processor_odom2d_params.h index 4e34becad..2056f6de9 100644 --- a/serialization/cereal/serialization_processor_odom2d_params.h +++ b/serialization/cereal/serialization_processor_odom2d_params.h @@ -2,7 +2,7 @@ #define _WOLF_SERIALIZATION_CEREAL_PROCESSOR_ODOM2D_PARAMS_H_ // Wolf includes -#include "../../processor_odom_2D.h" +#include "base/processor/processor_odom_2D.h" #include "serialization_processor_params_base.h" namespace cereal { diff --git a/serialization/cereal/serialization_processor_odom3d_params.h b/serialization/cereal/serialization_processor_odom3d_params.h index 9b658a233..1556a39e4 100644 --- a/serialization/cereal/serialization_processor_odom3d_params.h +++ b/serialization/cereal/serialization_processor_odom3d_params.h @@ -2,7 +2,7 @@ #define _WOLF_SERIALIZATION_CEREAL_PROCESSOR_ODOM3D_PARAMS_H_ // Wolf includes -#include "../../processor_odom_3D.h" +#include "base/processor/processor_odom_3D.h" #include "serialization_processor_params_base.h" namespace cereal { diff --git a/serialization/cereal/serialization_processor_params_base.h b/serialization/cereal/serialization_processor_params_base.h index 03cbce782..2c75b85c9 100644 --- a/serialization/cereal/serialization_processor_params_base.h +++ b/serialization/cereal/serialization_processor_params_base.h @@ -2,7 +2,7 @@ #define _WOLF_SERIALIZATION_CEREAL_PROCESSOR_PARAM_BASE_H_ // Wolf includes -#include "../../processor_base.h" +#include "base/processor/processor_base.h" #include <cereal/cereal.hpp> #include <cereal/types/polymorphic.hpp> diff --git a/serialization/cereal/serialization_sensor_intrinsic_base.h b/serialization/cereal/serialization_sensor_intrinsic_base.h index 2398fbad0..3e3b1cdb4 100644 --- a/serialization/cereal/serialization_sensor_intrinsic_base.h +++ b/serialization/cereal/serialization_sensor_intrinsic_base.h @@ -2,7 +2,7 @@ #define _WOLF_SERIALIZATION_CEREAL_SENSOR_INTRINSIC_BASE_H_ // Wolf includes -#include "../../sensor_base.h" +#include "base/sensor/sensor_base.h" #include <cereal/cereal.hpp> #include <cereal/types/polymorphic.hpp> diff --git a/serialization/cereal/serialization_sensor_odom2d_intrinsic.h b/serialization/cereal/serialization_sensor_odom2d_intrinsic.h index d4bf37044..ccfe569a7 100644 --- a/serialization/cereal/serialization_sensor_odom2d_intrinsic.h +++ b/serialization/cereal/serialization_sensor_odom2d_intrinsic.h @@ -2,7 +2,7 @@ #define _WOLF_SERIALIZATION_CEREAL_SENSOR_ODOM2D_INTRINSIC_H_ // Wolf includes -#include "../../sensor_odom_2D.h" +#include "base/sensor/sensor_odom_2D.h" #include "serialization_sensor_intrinsic_base.h" diff --git a/serialization/cereal/serialization_time_stamp.h b/serialization/cereal/serialization_time_stamp.h index 770e622f0..6cc9e29c7 100644 --- a/serialization/cereal/serialization_time_stamp.h +++ b/serialization/cereal/serialization_time_stamp.h @@ -2,7 +2,7 @@ #define _WOLF_IO_CEREAL_TIME_STAMP_H_ // Wolf includes -#include "../../time_stamp.h" +#include "base/time_stamp.h" #include <cereal/cereal.hpp> diff --git a/src/SE3.h b/src/SE3.h index cecd71757..cea354607 100644 --- a/src/SE3.h +++ b/src/SE3.h @@ -8,9 +8,8 @@ #ifndef SE3_H_ #define SE3_H_ - -#include "wolf.h" -#include "rotations.h" +#include "base/wolf.h" +#include "base/rotations.h" /* * The functions in this file are related to manipulations of Delta motion magnitudes used in 3D motion. @@ -34,8 +33,6 @@ * - interpolate: dd = D1 * exp ( log( D1.inv() * D2 ) * t ) = D1 (+) ( (D2 (-) D1) * t) */ - - namespace wolf { namespace three_D { @@ -95,7 +92,6 @@ inline void inverse(const MatrixBase<D1>& d, inverse(dp, dq, idp, idq); } - template<typename D> inline Matrix<typename D::Scalar, 7, 1> inverse(const MatrixBase<D>& d) { @@ -208,7 +204,6 @@ inline void between(const MatrixBase<D1>& d1, between(dp1, dq1, dp2, dq2, dp12, dq12); } - template<typename D1, typename D2> inline Matrix<typename D1::Scalar, 7, 1> between(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2 ) @@ -320,7 +315,6 @@ inline void minus(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1, J_do_dq2 = jac_SO3_right_inv(diff_o); } - template<typename D1, typename D2, typename D3> inline void minus(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2, diff --git a/src/association/association_nnls.cpp b/src/association/association_nnls.cpp index 0c42108ea..5d8286a78 100644 --- a/src/association/association_nnls.cpp +++ b/src/association/association_nnls.cpp @@ -1,4 +1,4 @@ -#include "association_nnls.h" +#include "base/association/association_nnls.h" namespace wolf { diff --git a/src/association/association_node.cpp b/src/association/association_node.cpp index 360c9c5e8..f1705db6e 100644 --- a/src/association/association_node.cpp +++ b/src/association/association_node.cpp @@ -1,5 +1,5 @@ -#include "association_node.h" +#include "base/association/association_node.h" AssociationNode::AssociationNode(const unsigned int _det_idx, const unsigned int _tar_idx, const double _prob, AssociationNode * _un_ptr, bool _is_root) : is_root_(_is_root), diff --git a/src/association/association_solver.cpp b/src/association/association_solver.cpp index 0e31e4cfc..f640efa3e 100644 --- a/src/association/association_solver.cpp +++ b/src/association/association_solver.cpp @@ -1,5 +1,5 @@ -#include "association_solver.h" +#include "base/association/association_solver.h" namespace wolf { diff --git a/src/association/association_tree.cpp b/src/association/association_tree.cpp index e42f51b5d..1337e4bab 100644 --- a/src/association/association_tree.cpp +++ b/src/association/association_tree.cpp @@ -1,5 +1,5 @@ -#include "association_tree.h" +#include "base/association/association_tree.h" namespace wolf { diff --git a/src/capture/capture_GPS.cpp b/src/capture/capture_GPS.cpp index 8c27eebd1..5c6520810 100644 --- a/src/capture/capture_GPS.cpp +++ b/src/capture/capture_GPS.cpp @@ -1,4 +1,4 @@ -#include "capture_GPS.h" +#include "base/capture/capture_GPS.h" namespace wolf { @@ -11,7 +11,6 @@ CaptureGPS::CaptureGPS(const TimeStamp &_ts, SensorBasePtr _sensor_ptr, rawgpsut // std::cout << obs_.toString() << std::endl; } - CaptureGPS::~CaptureGPS() { //std::cout << "deleting CaptureGPS " << id() << std::endl; @@ -22,5 +21,4 @@ rawgpsutils::SatellitesObs &CaptureGPS::getData() return obs_; } - } //namespace wolf diff --git a/src/capture/capture_GPS_fix.cpp b/src/capture/capture_GPS_fix.cpp index f31273687..052bf8f63 100644 --- a/src/capture/capture_GPS_fix.cpp +++ b/src/capture/capture_GPS_fix.cpp @@ -1,5 +1,4 @@ -#include "capture_GPS_fix.h" - +#include "base/capture/capture_GPS_fix.h" namespace wolf { @@ -32,5 +31,4 @@ void CaptureGPSFix::process() getFeatureList().front()->addConstraint(std::make_shared <ConstraintGPS2D>(getFeatureList().front())); } - } //namespace wolf diff --git a/src/capture/capture_IMU.cpp b/src/capture/capture_IMU.cpp index 3f5195dad..6dbc07d59 100644 --- a/src/capture/capture_IMU.cpp +++ b/src/capture/capture_IMU.cpp @@ -1,11 +1,9 @@ -#include "capture_IMU.h" -#include "sensor_IMU.h" -#include "state_quaternion.h" +#include "base/capture/capture_IMU.h" +#include "base/sensor/sensor_IMU.h" +#include "base/state_quaternion.h" namespace wolf { - - CaptureIMU::CaptureIMU(const TimeStamp& _init_ts, SensorBasePtr _sensor_ptr, const Eigen::Vector6s& _acc_gyro_data, @@ -27,7 +25,6 @@ CaptureIMU::CaptureIMU(const TimeStamp& _init_ts, // } - CaptureIMU::~CaptureIMU() { // diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp index 880303f49..051b93c9f 100644 --- a/src/capture/capture_base.cpp +++ b/src/capture/capture_base.cpp @@ -1,5 +1,5 @@ -#include "capture_base.h" -#include "sensor_base.h" +#include "base/capture/capture_base.h" +#include "base/sensor/sensor_base.h" namespace wolf{ @@ -59,7 +59,6 @@ CaptureBase::CaptureBase(const std::string& _type, // WOLF_TRACE("New Capture ", id(), " -- type ", getType(), " -- t = ", getTimeStamp(), " s"); } - CaptureBase::~CaptureBase() { removeStateBlocks(); @@ -294,8 +293,5 @@ void CaptureBase::setCalibration(const VectorXs& _calib) } } - - - } // namespace wolf diff --git a/src/capture/capture_image.cpp b/src/capture/capture_image.cpp index 6a7950d62..1d8ac8b8d 100644 --- a/src/capture/capture_image.cpp +++ b/src/capture/capture_image.cpp @@ -1,4 +1,4 @@ -#include "capture_image.h" +#include "base/capture/capture_image.h" namespace wolf { @@ -45,5 +45,4 @@ std::vector<cv::KeyPoint>& CaptureImage::getKeypoints() return frame_.getKeyPoints(); } - } // namespace wolf diff --git a/src/capture/capture_laser_2D.cpp b/src/capture/capture_laser_2D.cpp index 8d1a4ffea..0c3c6fe58 100644 --- a/src/capture/capture_laser_2D.cpp +++ b/src/capture/capture_laser_2D.cpp @@ -1,4 +1,4 @@ -#include "capture_laser_2D.h" +#include "base/capture/capture_laser_2D.h" namespace wolf { diff --git a/src/capture/capture_motion.cpp b/src/capture/capture_motion.cpp index 07fcd6c68..439228aa9 100644 --- a/src/capture/capture_motion.cpp +++ b/src/capture/capture_motion.cpp @@ -5,7 +5,7 @@ * \author: jsola */ -#include "capture_motion.h" +#include "base/capture/capture_motion.h" namespace wolf { @@ -46,15 +46,11 @@ CaptureMotion::CaptureMotion(const std::string & _type, // } - - - CaptureMotion::~CaptureMotion() { // } - Eigen::VectorXs CaptureMotion::getDeltaCorrected(const VectorXs& _calib_current) { VectorXs calib_preint = getCalibrationPreint(); diff --git a/src/capture/capture_odom_2D.cpp b/src/capture/capture_odom_2D.cpp index 5d8eabccb..99b6e4281 100644 --- a/src/capture/capture_odom_2D.cpp +++ b/src/capture/capture_odom_2D.cpp @@ -5,7 +5,7 @@ * Author: jsola */ -#include "capture_odom_2D.h" +#include "base/capture/capture_odom_2D.h" namespace wolf { diff --git a/src/capture/capture_odom_3D.cpp b/src/capture/capture_odom_3D.cpp index f6ba1dea9..26f4e6d6f 100644 --- a/src/capture/capture_odom_3D.cpp +++ b/src/capture/capture_odom_3D.cpp @@ -5,7 +5,7 @@ * Author: jsola */ -#include "capture_odom_3D.h" +#include "base/capture/capture_odom_3D.h" namespace wolf { @@ -44,5 +44,3 @@ Eigen::VectorXs CaptureOdom3D::correctDelta(const VectorXs& _delta, const Vector } /* namespace wolf */ - - diff --git a/src/capture/capture_pose.cpp b/src/capture/capture_pose.cpp index c8422aa32..96aab020b 100644 --- a/src/capture/capture_pose.cpp +++ b/src/capture/capture_pose.cpp @@ -1,4 +1,4 @@ -#include "capture_pose.h" +#include "base/capture/capture_pose.h" namespace wolf{ @@ -30,5 +30,4 @@ void CapturePose::emplaceFeatureAndConstraint() throw std::runtime_error("Wrong data size in CapturePose. Use 3 for 2D. Use 7 for 3D."); } - } // namespace wolf diff --git a/src/capture/capture_velocity.cpp b/src/capture/capture_velocity.cpp index 07062242a..9c4e7675f 100644 --- a/src/capture/capture_velocity.cpp +++ b/src/capture/capture_velocity.cpp @@ -1,4 +1,4 @@ -#include "capture_velocity.h" +#include "base/capture/capture_velocity.h" namespace wolf { diff --git a/src/capture/capture_void.cpp b/src/capture/capture_void.cpp index 721353d2f..44bf87788 100644 --- a/src/capture/capture_void.cpp +++ b/src/capture/capture_void.cpp @@ -1,4 +1,4 @@ -#include "capture_void.h" +#include "base/capture/capture_void.h" namespace wolf { @@ -13,5 +13,4 @@ CaptureVoid::~CaptureVoid() //std::cout << "deleting CaptureVoid " << id() << std::endl; } - } // namespace wolf diff --git a/src/capture/capture_wheel_joint_position.cpp b/src/capture/capture_wheel_joint_position.cpp index a43786df6..9290da89a 100644 --- a/src/capture/capture_wheel_joint_position.cpp +++ b/src/capture/capture_wheel_joint_position.cpp @@ -1,6 +1,6 @@ -#include "capture_wheel_joint_position.h" -#include "../sensors/sensor_diff_drive.h" -#include "../rotations.h" +#include "base/capture/capture_wheel_joint_position.h" +#include "base/sensor/sensor_diff_drive.h" +#include "base/rotations.h" namespace wolf { diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp index 26babf9af..e7e145567 100644 --- a/src/ceres_wrapper/ceres_manager.cpp +++ b/src/ceres_wrapper/ceres_manager.cpp @@ -1,9 +1,9 @@ -#include "ceres_manager.h" -#include "create_numeric_diff_cost_function.h" -#include "../trajectory_base.h" -#include "../map_base.h" -#include "../landmark_base.h" -#include "../make_unique.h" +#include "base/ceres_wrapper/ceres_manager.h" +#include "base/ceres_wrapper/create_numeric_diff_cost_function.h" +#include "base/trajectory_base.h" +#include "base/map_base.h" +#include "base/landmark/landmark_base.h" +#include "base/make_unique.h" namespace wolf { diff --git a/src/ceres_wrapper/local_parametrization_wrapper.cpp b/src/ceres_wrapper/local_parametrization_wrapper.cpp index 7b7ae9414..5cade1553 100644 --- a/src/ceres_wrapper/local_parametrization_wrapper.cpp +++ b/src/ceres_wrapper/local_parametrization_wrapper.cpp @@ -1,4 +1,4 @@ -#include "local_parametrization_wrapper.h" +#include "base/ceres_wrapper/local_parametrization_wrapper.h" namespace wolf { diff --git a/src/ceres_wrapper/qr_manager.cpp b/src/ceres_wrapper/qr_manager.cpp index 36caa290b..9c63afd03 100644 --- a/src/ceres_wrapper/qr_manager.cpp +++ b/src/ceres_wrapper/qr_manager.cpp @@ -154,7 +154,6 @@ void QRManager::addConstraint(ConstraintBasePtr _ctr_ptr) pending_changes_ = true; } - void QRManager::removeConstraint(ConstraintBasePtr _ctr_ptr) { //std::cout << "remove constraint " << _ctr_ptr->id() << std::endl; diff --git a/src/ceres_wrapper/solver_manager.cpp b/src/ceres_wrapper/solver_manager.cpp index 2a5d8efec..cdaae51c3 100644 --- a/src/ceres_wrapper/solver_manager.cpp +++ b/src/ceres_wrapper/solver_manager.cpp @@ -1,7 +1,7 @@ -#include "solver_manager.h" -#include "../trajectory_base.h" -#include "../map_base.h" -#include "../landmark_base.h" +#include "base/solver/solver_manager.h" +#include "base/trajectory_base.h" +#include "base/map_base.h" +#include "base/landmark/landmark_base.h" namespace wolf { diff --git a/src/constraint/constraint_analytic.cpp b/src/constraint/constraint_analytic.cpp index 8b0422380..23ab82362 100644 --- a/src/constraint/constraint_analytic.cpp +++ b/src/constraint/constraint_analytic.cpp @@ -1,5 +1,5 @@ -#include "constraint_analytic.h" -#include "state_block.h" +#include "base/constraint/constraint_analytic.h" +#include "base/state_block.h" namespace wolf { diff --git a/src/constraint/constraint_base.cpp b/src/constraint/constraint_base.cpp index 3e33cc6cc..f521cf214 100644 --- a/src/constraint/constraint_base.cpp +++ b/src/constraint/constraint_base.cpp @@ -1,6 +1,6 @@ -#include "constraint_base.h" -#include "frame_base.h" -#include "landmark_base.h" +#include "base/constraint/constraint_base.h" +#include "base/frame_base.h" +#include "base/landmark/landmark_base.h" namespace wolf { diff --git a/examples/.gitignore b/src/examples/.gitignore similarity index 100% rename from examples/.gitignore rename to src/examples/.gitignore diff --git a/examples/ACTIVESEARCH.yaml b/src/examples/ACTIVESEARCH.yaml similarity index 100% rename from examples/ACTIVESEARCH.yaml rename to src/examples/ACTIVESEARCH.yaml diff --git a/examples/CMakeLists.txt b/src/examples/CMakeLists.txt similarity index 100% rename from examples/CMakeLists.txt rename to src/examples/CMakeLists.txt diff --git a/examples/Test_ORB.png b/src/examples/Test_ORB.png similarity index 100% rename from examples/Test_ORB.png rename to src/examples/Test_ORB.png diff --git a/examples/camera_params.yaml b/src/examples/camera_params.yaml similarity index 100% rename from examples/camera_params.yaml rename to src/examples/camera_params.yaml diff --git a/examples/camera_params_canonical.yaml b/src/examples/camera_params_canonical.yaml similarity index 100% rename from examples/camera_params_canonical.yaml rename to src/examples/camera_params_canonical.yaml diff --git a/examples/camera_params_ueye.yaml b/src/examples/camera_params_ueye.yaml similarity index 100% rename from examples/camera_params_ueye.yaml rename to src/examples/camera_params_ueye.yaml diff --git a/examples/camera_params_ueye_radial_dist.yaml b/src/examples/camera_params_ueye_radial_dist.yaml similarity index 100% rename from examples/camera_params_ueye_radial_dist.yaml rename to src/examples/camera_params_ueye_radial_dist.yaml diff --git a/examples/camera_params_ueye_sim.yaml b/src/examples/camera_params_ueye_sim.yaml similarity index 100% rename from examples/camera_params_ueye_sim.yaml rename to src/examples/camera_params_ueye_sim.yaml diff --git a/examples/input_M3500b_toro.graph b/src/examples/input_M3500b_toro.graph similarity index 100% rename from examples/input_M3500b_toro.graph rename to src/examples/input_M3500b_toro.graph diff --git a/examples/map_polyline_example.yaml b/src/examples/map_polyline_example.yaml similarity index 100% rename from examples/map_polyline_example.yaml rename to src/examples/map_polyline_example.yaml diff --git a/examples/processor_image_feature.yaml b/src/examples/processor_image_feature.yaml similarity index 100% rename from examples/processor_image_feature.yaml rename to src/examples/processor_image_feature.yaml diff --git a/examples/processor_image_vision_utils.yaml b/src/examples/processor_image_vision_utils.yaml similarity index 100% rename from examples/processor_image_vision_utils.yaml rename to src/examples/processor_image_vision_utils.yaml diff --git a/examples/processor_imu.yaml b/src/examples/processor_imu.yaml similarity index 100% rename from examples/processor_imu.yaml rename to src/examples/processor_imu.yaml diff --git a/examples/processor_imu_no_vote.yaml b/src/examples/processor_imu_no_vote.yaml similarity index 100% rename from examples/processor_imu_no_vote.yaml rename to src/examples/processor_imu_no_vote.yaml diff --git a/examples/processor_imu_t1.yaml b/src/examples/processor_imu_t1.yaml similarity index 100% rename from examples/processor_imu_t1.yaml rename to src/examples/processor_imu_t1.yaml diff --git a/examples/processor_imu_t6.yaml b/src/examples/processor_imu_t6.yaml similarity index 100% rename from examples/processor_imu_t6.yaml rename to src/examples/processor_imu_t6.yaml diff --git a/examples/processor_odom_3D.yaml b/src/examples/processor_odom_3D.yaml similarity index 100% rename from examples/processor_odom_3D.yaml rename to src/examples/processor_odom_3D.yaml diff --git a/examples/processor_tracker_feature_trifocal.yaml b/src/examples/processor_tracker_feature_trifocal.yaml similarity index 100% rename from examples/processor_tracker_feature_trifocal.yaml rename to src/examples/processor_tracker_feature_trifocal.yaml diff --git a/examples/sensor_imu.yaml b/src/examples/sensor_imu.yaml similarity index 100% rename from examples/sensor_imu.yaml rename to src/examples/sensor_imu.yaml diff --git a/examples/sensor_odom_3D.yaml b/src/examples/sensor_odom_3D.yaml similarity index 100% rename from examples/sensor_odom_3D.yaml rename to src/examples/sensor_odom_3D.yaml diff --git a/examples/sensor_odom_3D_HQ.yaml b/src/examples/sensor_odom_3D_HQ.yaml similarity index 100% rename from examples/sensor_odom_3D_HQ.yaml rename to src/examples/sensor_odom_3D_HQ.yaml diff --git a/examples/solver/test_SPQR.cpp b/src/examples/solver/test_SPQR.cpp similarity index 99% rename from examples/solver/test_SPQR.cpp rename to src/examples/solver/test_SPQR.cpp index bbd535de6..04592dbd0 100644 --- a/examples/solver/test_SPQR.cpp +++ b/src/examples/solver/test_SPQR.cpp @@ -45,7 +45,6 @@ int main (int argc, char **argv) std::cout << "solved x_" << std::endl << x_ << std::endl; std::cout << "ordering: " << solver.colsPermutation().indices().transpose() << std::endl; - /////////////////////////////////////////////////////////////////////// // Directly in suitesparse cholmod_common Common, *cc ; diff --git a/examples/solver/test_ccolamd.cpp b/src/examples/solver/test_ccolamd.cpp similarity index 99% rename from examples/solver/test_ccolamd.cpp rename to src/examples/solver/test_ccolamd.cpp index 41c742ad6..ebba25db7 100644 --- a/examples/solver/test_ccolamd.cpp +++ b/src/examples/solver/test_ccolamd.cpp @@ -6,7 +6,7 @@ */ // Wolf includes -#include "wolf.h" +#include "base/wolf.h" //std includes #include <cstdlib> diff --git a/examples/solver/test_ccolamd_blocks.cpp b/src/examples/solver/test_ccolamd_blocks.cpp similarity index 99% rename from examples/solver/test_ccolamd_blocks.cpp rename to src/examples/solver/test_ccolamd_blocks.cpp index 8c5c91219..b3d4d646b 100644 --- a/examples/solver/test_ccolamd_blocks.cpp +++ b/src/examples/solver/test_ccolamd_blocks.cpp @@ -5,7 +5,6 @@ * Author: jvallve */ - //std includes #include <cstdlib> #include <iostream> @@ -52,7 +51,6 @@ void permutation_2_block_permutation(const PermutationMatrix<Dynamic, Dynamic, i perm_blocks.indices() = idx_blocks; } - //main int main(int argc, char *argv[]) { @@ -180,6 +178,3 @@ int main(int argc, char *argv[]) //std::cout << "x = " << x_block_ordered.transpose() << std::endl; } - - - diff --git a/examples/solver/test_iQR.cpp b/src/examples/solver/test_iQR.cpp similarity index 99% rename from examples/solver/test_iQR.cpp rename to src/examples/solver/test_iQR.cpp index 79bfb03b3..868e84440 100644 --- a/examples/solver/test_iQR.cpp +++ b/src/examples/solver/test_iQR.cpp @@ -12,7 +12,6 @@ * Author: jvallve */ - //std includes #include <cstdlib> #include <iostream> @@ -97,7 +96,6 @@ void augment_permutation(PermutationMatrix<Dynamic, Dynamic, int> &perm, const i perm.indices() = new_indices; } - //main int main(int argc, char *argv[]) { @@ -350,8 +348,3 @@ int main(int argc, char *argv[]) } } - - - - - diff --git a/examples/solver/test_iQR_wolf.cpp b/src/examples/solver/test_iQR_wolf.cpp similarity index 99% rename from examples/solver/test_iQR_wolf.cpp rename to src/examples/solver/test_iQR_wolf.cpp index dd8707b8b..70624a0d6 100644 --- a/examples/solver/test_iQR_wolf.cpp +++ b/src/examples/solver/test_iQR_wolf.cpp @@ -5,7 +5,6 @@ * Author: jvallve */ - //std includes #include <cstdlib> #include <string> @@ -222,7 +221,6 @@ class SolverQR A_nodes_.coeffRef(A_nodes_.rows()-1, ordered_node) = 1; - assert(_meas.jacobians.at(j).cols() == nodes_.at(_meas.nodes_idx.at(j)).dim); assert(_meas.jacobians.at(j).rows() == _meas.dim); @@ -396,7 +394,6 @@ class SolverQR return 1; } - void nodePermutation2VariablesPermutation(const PermutationMatrix<Dynamic, Dynamic, int> &_perm_nodes, PermutationMatrix<Dynamic, Dynamic, int> &perm_variables) { ArrayXi locations = perm_nodes_2_locations(_perm_nodes); @@ -561,8 +558,3 @@ int main(int argc, char *argv[]) } } - - - - - diff --git a/examples/solver/test_iQR_wolf2.cpp b/src/examples/solver/test_iQR_wolf2.cpp similarity index 99% rename from examples/solver/test_iQR_wolf2.cpp rename to src/examples/solver/test_iQR_wolf2.cpp index 07aba80d4..d3fb71cad 100644 --- a/examples/solver/test_iQR_wolf2.cpp +++ b/src/examples/solver/test_iQR_wolf2.cpp @@ -17,9 +17,9 @@ #include <queue> //Wolf includes -#include "state_block.h" -#include "constraint_base.h" -#include "sensor_laser_2D.h" +#include "base/state_block.h" +#include "base/constraint/constraint_base.h" +#include "base/sensor/sensor_laser_2D.h" #include "wolf_manager.h" // wolf solver @@ -35,14 +35,12 @@ //Ceres includes #include "glog/logging.h" -#include "ceres_wrapper/ceres_manager.h" +#include "base/ceres_wrapper/ceres_manager.h" //laser_scan_utils #include "iri-algorithms/laser_scan_utils/corner_detector.h" #include "iri-algorithms/laser_scan_utils/entities.h" - - //function travel around void motionCampus(unsigned int ii, Cpose3d & pose, double& displacement_, double& rotation_) { @@ -96,7 +94,6 @@ void motionCampus(unsigned int ii, Cpose3d & pose, double& displacement_, double pose.rt.setEuler(pose.rt.head() + rotation_, pose.rt.pitch(), pose.rt.roll()); } - //main int main(int argc, char *argv[]) { @@ -118,7 +115,6 @@ int main(int argc, char *argv[]) // INITIALIZATION ============================================================================================ - //init random generators Scalar odom_std_factor = 0.1; Scalar gps_std = 10; @@ -432,8 +428,3 @@ int main(int argc, char *argv[]) return 0; } - - - - - diff --git a/examples/solver/test_incremental_ccolamd_blocks.cpp b/src/examples/solver/test_incremental_ccolamd_blocks.cpp similarity index 99% rename from examples/solver/test_incremental_ccolamd_blocks.cpp rename to src/examples/solver/test_incremental_ccolamd_blocks.cpp index de4f82f00..792250a81 100644 --- a/examples/solver/test_incremental_ccolamd_blocks.cpp +++ b/src/examples/solver/test_incremental_ccolamd_blocks.cpp @@ -5,7 +5,6 @@ * Author: jvallve */ - //std includes #include <cstdlib> #include <iostream> @@ -54,7 +53,6 @@ void permutation_2_block_permutation(const PermutationMatrix<Dynamic, Dynamic, i perm_blocks.indices() = idx_blocks; } - //main int main(int argc, char *argv[]) { @@ -143,7 +141,6 @@ int main(int argc, char *argv[]) // r.h.v b.segment(i*dim, dim) = VectorXd::LinSpaced(Sequential, dim, dim*i, dim *(i+1)-1); - std::cout << "Solving factor graph:" << std::endl; std::cout << "Factors: " << std::endl << factors * MatrixXi::Identity((i+1), (i+1)) << std::endl << std::endl; // std::cout << "H: " << std::endl << H * MatrixXd::Identity(dim*(i+1), dim*(i+1)) << std::endl << std::endl; @@ -160,7 +157,6 @@ int main(int argc, char *argv[]) x = solver.solve(b); time1 += ((double) clock() - t1) / CLOCKS_PER_SEC; - // SOLVING WITH REORDERING ------------------------------------ // Order with previous orderings acc_permutation.conservativeResize(dim*(i+1)); @@ -198,7 +194,6 @@ int main(int argc, char *argv[]) x_ordered = acc_permutation_matrix.inverse() * x_ordered; time2 += ((double) clock() - t2) / CLOCKS_PER_SEC; - // SOLVING WITH BLOCK REORDERING ------------------------------------ // Order with previous orderings acc_permutation_b.conservativeResize(dim*(i+1)); @@ -245,7 +240,6 @@ int main(int argc, char *argv[]) x_b_ordered = acc_permutation_b_matrix.inverse() * x_b_ordered; time3 += ((double) clock() - t3) / CLOCKS_PER_SEC; - // RESULTS ------------------------------------ std::cout << "========================= RESULTS " << i << ":" << std::endl; std::cout << "NO REORDERING: solved in " << time1*1e3 << " ms" << std::endl; @@ -266,6 +260,3 @@ int main(int argc, char *argv[]) //std::cout << "x = " << x_b_ordered.transpose() << std::endl; } - - - diff --git a/examples/solver/test_permutations.cpp b/src/examples/solver/test_permutations.cpp similarity index 99% rename from examples/solver/test_permutations.cpp rename to src/examples/solver/test_permutations.cpp index 5d28ec06c..c33c744c6 100644 --- a/examples/solver/test_permutations.cpp +++ b/src/examples/solver/test_permutations.cpp @@ -5,7 +5,6 @@ * Author: jvallve */ - //std includes #include <cstdlib> #include <iostream> @@ -19,7 +18,6 @@ // eigen includes #include <eigen3/Eigen/OrderingMethods> - using namespace Eigen; //main @@ -111,6 +109,5 @@ int main(int argc, char *argv[]) std::cout << "mapped_a = " << mapped_a.transpose() << std::endl << std::endl; std::cout << "maps are affected of the reorderings in mapped vectors" << std::endl; - // Map<> } diff --git a/examples/test_2_lasers_offline.cpp b/src/examples/test_2_lasers_offline.cpp similarity index 97% rename from examples/test_2_lasers_offline.cpp rename to src/examples/test_2_lasers_offline.cpp index 5c5672c79..d8d91b638 100644 --- a/examples/test_2_lasers_offline.cpp +++ b/src/examples/test_2_lasers_offline.cpp @@ -1,5 +1,5 @@ //std includes -#include <sensor_GPS_fix.h> +#include "base/sensor/sensor_GPS_fix.h" #include <cstdlib> #include <iostream> #include <fstream> @@ -17,12 +17,12 @@ #include "glog/logging.h" //Wolf includes -#include "../problem.h" -#include "../processor_tracker_landmark_corner.h" -#include "../processor_odom_2D.h" -#include "../sensor_laser_2D.h" -#include "../sensor_odom_2D.h" -#include "../ceres_wrapper/ceres_manager.h" +#include "base/problem.h" +#include "base/processor/processor_tracker_landmark_corner.h" +#include "base/processor/processor_odom_2D.h" +#include "base/sensor/sensor_laser_2D.h" +#include "base/sensor/sensor_odom_2D.h" +#include "base/ceres_wrapper/ceres_manager.h" // laserscanutils #include "laser_scan_utils/line_finder_iterative.h" @@ -31,7 +31,7 @@ //C includes for sleep, time and main args #include "unistd.h" -#include "../capture_pose.h" +#include "base/capture/capture_pose.h" //faramotics includes #include "faramotics/dynamicSceneRender.h" #include "faramotics/rangeScan2D.h" @@ -88,7 +88,6 @@ int main(int argc, char** argv) else std::cout << "Simulated data files opened correctly..." << std::endl; - unsigned int n_execution = (unsigned int) atoi(argv[1]); //number of iterations of the whole execution // INITIALIZATION ============================================================================================ @@ -226,7 +225,6 @@ int main(int argc, char** argv) } mean_times(0) += ((double) clock() - t1) / CLOCKS_PER_SEC; - // SOLVE OPTIMIZATION --------------------------- std::cout << "SOLVING..." << std::endl; t1 = clock(); diff --git a/examples/test_analytic_odom_constraint.cpp b/src/examples/test_analytic_odom_constraint.cpp similarity index 99% rename from examples/test_analytic_odom_constraint.cpp rename to src/examples/test_analytic_odom_constraint.cpp index 029b0280b..854d4543f 100644 --- a/examples/test_analytic_odom_constraint.cpp +++ b/src/examples/test_analytic_odom_constraint.cpp @@ -12,8 +12,8 @@ //Wolf includes #include "wolf_manager.h" -#include "capture_void.h" -#include "ceres_wrapper/ceres_manager.h" +#include "base/capture/capture_void.h" +#include "base/ceres_wrapper/ceres_manager.h" // EIGEN //#include <Eigen/CholmodSupport> @@ -28,8 +28,6 @@ void insertSparseBlock(const Eigen::SparseMatrix<Scalar>& ins, Eigen::SparseMatr original.makeCompressed(); } -} - int main(int argc, char** argv) { @@ -71,8 +69,6 @@ int main(int argc, char** argv) CeresManager* ceres_manager_autodiff = new CeresManager(wolf_problem_autodiff, ceres_options); CeresManager* ceres_manager_analytic = new CeresManager(wolf_problem_analytic, ceres_options); - - // load graph from .txt offLineFile_.open(file_path_.c_str(), std::ifstream::in); if (offLineFile_.is_open()) diff --git a/examples/test_autodiff.cpp b/src/examples/test_autodiff.cpp similarity index 99% rename from examples/test_autodiff.cpp rename to src/examples/test_autodiff.cpp index c027f9fcc..a60563546 100644 --- a/examples/test_autodiff.cpp +++ b/src/examples/test_autodiff.cpp @@ -17,8 +17,8 @@ //Wolf includes #include "wolf_manager.h" -#include "sensor_laser_2D.h" -#include "ceres_wrapper/ceres_manager.h" +#include "base/sensor/sensor_laser_2D.h" +#include "base/ceres_wrapper/ceres_manager.h" //C includes for sleep, time and main args #include "unistd.h" @@ -49,7 +49,6 @@ void motionCampus(unsigned int ii, Cpose3d & pose, double& displacement_, double pose.moveForward(displacement_); pose.rt.setEuler(pose.rt.head() + rotation_, pose.rt.pitch(), pose.rt.roll()); } -} int main(int argc, char** argv) { diff --git a/examples/test_capture_laser_2D.cpp b/src/examples/test_capture_laser_2D.cpp similarity index 99% rename from examples/test_capture_laser_2D.cpp rename to src/examples/test_capture_laser_2D.cpp index f5c43ac41..8ca0e376b 100644 --- a/examples/test_capture_laser_2D.cpp +++ b/src/examples/test_capture_laser_2D.cpp @@ -3,7 +3,7 @@ #include <random> //wolf -#include "capture_laser_2D.h" +#include "base/capture/capture_laser_2D.h" // Eigen in std vector #include <Eigen/StdVector> diff --git a/examples/test_ceres_2_lasers.cpp b/src/examples/test_ceres_2_lasers.cpp similarity index 98% rename from examples/test_ceres_2_lasers.cpp rename to src/examples/test_ceres_2_lasers.cpp index 679777dd4..897838353 100644 --- a/examples/test_ceres_2_lasers.cpp +++ b/src/examples/test_ceres_2_lasers.cpp @@ -1,5 +1,5 @@ //std includes -#include <sensor_GPS_fix.h> +#include "base/sensor/sensor_GPS_fix.h" #include <cstdlib> #include <iostream> #include <fstream> @@ -17,12 +17,12 @@ #include "glog/logging.h" //Wolf includes -#include "problem.h" -#include "processor_tracker_landmark_corner.h" -#include "processor_odom_2D.h" -#include "sensor_laser_2D.h" -#include "sensor_odom_2D.h" -#include "ceres_wrapper/ceres_manager.h" +#include "base/problem.h" +#include "base/processor/processor_tracker_landmark_corner.h" +#include "base/processor/processor_odom_2D.h" +#include "base/sensor/sensor_laser_2D.h" +#include "base/sensor/sensor_odom_2D.h" +#include "base/ceres_wrapper/ceres_manager.h" // laserscanutils #include "laser_scan_utils/line_finder_iterative.h" @@ -31,7 +31,7 @@ //C includes for sleep, time and main args #include "unistd.h" -#include "../capture_pose.h" +#include "base/capture/capture_pose.h" //faramotics includes #include "faramotics/dynamicSceneRender.h" #include "faramotics/rangeScan2D.h" @@ -71,7 +71,6 @@ class FaramoticsRobot myScanner->loadAssimpModel(modelFileName); } - //function travel around Eigen::Vector3s motionCampus(unsigned int ii, double& displacement_, double& rotation_) { diff --git a/examples/test_ceres_2_lasers_polylines.cpp b/src/examples/test_ceres_2_lasers_polylines.cpp similarity index 98% rename from examples/test_ceres_2_lasers_polylines.cpp rename to src/examples/test_ceres_2_lasers_polylines.cpp index 9668e1c4a..70928589a 100644 --- a/examples/test_ceres_2_lasers_polylines.cpp +++ b/src/examples/test_ceres_2_lasers_polylines.cpp @@ -1,5 +1,5 @@ //std includes -#include <sensor_GPS_fix.h> +#include "base/sensor/sensor_GPS_fix.h" #include <cstdlib> #include <iostream> #include <fstream> @@ -17,12 +17,12 @@ #include "glog/logging.h" //Wolf includes -#include "problem.h" -#include "processor_tracker_landmark_polyline.h" -#include "processor_odom_2D.h" -#include "sensor_laser_2D.h" -#include "sensor_odom_2D.h" -#include "ceres_wrapper/ceres_manager.h" +#include "base/problem.h" +#include "base/processor/processor_tracker_landmark_polyline.h" +#include "base/processor/processor_odom_2D.h" +#include "base/sensor/sensor_laser_2D.h" +#include "base/sensor/sensor_odom_2D.h" +#include "base/ceres_wrapper/ceres_manager.h" // laserscanutils #include "laser_scan_utils/line_finder_iterative.h" @@ -31,7 +31,7 @@ //C includes for sleep, time and main args #include "unistd.h" -#include "../capture_pose.h" +#include "base/capture/capture_pose.h" //faramotics includes #include "faramotics/dynamicSceneRender.h" #include "faramotics/rangeScan2D.h" @@ -71,7 +71,6 @@ class FaramoticsRobot myScanner->loadAssimpModel(modelFileName); } - //function travel around Eigen::Vector3s motionCampus(unsigned int ii, double& displacement_, double& rotation_) { diff --git a/examples/test_constraint_AHP.cpp b/src/examples/test_constraint_AHP.cpp similarity index 96% rename from examples/test_constraint_AHP.cpp rename to src/examples/test_constraint_AHP.cpp index 8e555cac0..7dc1eabe7 100644 --- a/examples/test_constraint_AHP.cpp +++ b/src/examples/test_constraint_AHP.cpp @@ -1,10 +1,10 @@ -#include "pinhole_tools.h" -#include "landmark_AHP.h" -#include "constraint_AHP.h" -#include "state_block.h" -#include "state_quaternion.h" -#include "sensor_camera.h" -#include "capture_image.h" +#include "base/pinhole_tools.h" +#include "base/landmark/landmark_AHP.h" +#include "base/constraint/constraint_AHP.h" +#include "base/state_block.h" +#include "base/state_quaternion.h" +#include "base/sensor/sensor_camera.h" +#include "base/capture/capture_image.h" int main() { @@ -45,7 +45,6 @@ int main() // one-liner API ProcessorBasePtr processor_ptr = wolf_problem_ptr_->installProcessor("IMAGE LANDMARK", "ORB", "PinHole", wolf_root + "/src/examples/processor_image_feature.yaml"); - // create the current frame Eigen::Vector7s frame_pos_ori; frame_pos_ori.setRandom(); @@ -71,7 +70,6 @@ int main() image_ptr = std::make_shared< CaptureImage>(t, camera_ptr_, frame); last_frame->addCapture(image_ptr); - // create the feature cv::KeyPoint kp; kp.pt = {10,20}; cv::Mat desc; @@ -82,14 +80,12 @@ int main() FrameBasePtr anchor_frame = std::make_shared< FrameBase>(t,std::make_shared<StateBlock>(frame_val.head(3)), std::make_shared<StateQuaternion>(frame_val.tail(4))); //FrameBasePtr anchor_frame = wolf_problem_ptr_->getTrajectoryPtr()->getLastFramePtr(); - // create the landmark Eigen::Vector2s point2D; point2D[0] = feat_point_image_ptr->getKeypoint().pt.x; point2D[1] = feat_point_image_ptr->getKeypoint().pt.y; std::cout << "point2D: " << point2D.transpose() << std::endl; - Scalar distance = 2; // arbitrary value Eigen::Vector4s vec_homogeneous; @@ -115,8 +111,6 @@ int main() std::cout << "Landmark AHP created" << std::endl; - - // Create the constraint ConstraintAHPPtr constraint_ptr = std::make_shared<ConstraintAHP>(feat_point_image_ptr, std::static_pointer_cast<LandmarkAHP>(landmark), processor_ptr); @@ -146,7 +140,6 @@ int main() landmark_.data(), expectation.data()); // current_frame p; current_frame o; anchor_frame p; anchor_frame o; homogeneous_vector landmark, residual - std::cout << "expectation computed" << std::endl; std::cout << "expectation = " << expectation[0] << " " << expectation[1] << std::endl; diff --git a/examples/test_constraint_imu.cpp b/src/examples/test_constraint_imu.cpp similarity index 97% rename from examples/test_constraint_imu.cpp rename to src/examples/test_constraint_imu.cpp index e4a3b8f2b..77464a9e4 100644 --- a/examples/test_constraint_imu.cpp +++ b/src/examples/test_constraint_imu.cpp @@ -1,14 +1,14 @@ //Wolf -#include <capture_IMU.h> -#include <processor_IMU.h> -#include <sensor_IMU.h> -#include "../capture_pose.h" -#include "wolf.h" -#include "problem.h" -#include "constraint_odom_3D.h" -#include "state_block.h" -#include "state_quaternion.h" -#include "ceres_wrapper/ceres_manager.h" +#include "base/capture/capture_IMU.h" +#include "base/processor/processor_IMU.h" +#include "base/sensor/sensor_IMU.h" +#include "base/capture/capture_pose.h" +#include "base/wolf.h" +#include "base/problem.h" +#include "base/constraint/constraint_odom_3D.h" +#include "base/state_block.h" +#include "base/state_quaternion.h" +#include "base/ceres_wrapper/ceres_manager.h" //#define DEBUG_RESULTS @@ -56,7 +56,6 @@ int main(int argc, char** argv) CaptureIMUPtr imu_ptr( std::make_shared<CaptureIMU>(t, sensor_ptr, data_, Eigen::Matrix6s::Identity()) ); imu_ptr->setFramePtr(wolf_problem_ptr_->getTrajectoryPtr()->getFrameList().back()); - // set variables using namespace std; Eigen::VectorXs state_vec; @@ -237,7 +236,6 @@ int main(int argc, char** argv) wolf_problem_ptr_->print(4,1,1,1); } - ///having a look at covariances Eigen::MatrixXs predelta_cov; predelta_cov.resize(9,9); diff --git a/examples/test_constraint_odom_3D.cpp b/src/examples/test_constraint_odom_3D.cpp similarity index 73% rename from examples/test_constraint_odom_3D.cpp rename to src/examples/test_constraint_odom_3D.cpp index 41cf14f2d..5cdbfccf5 100644 --- a/examples/test_constraint_odom_3D.cpp +++ b/src/examples/test_constraint_odom_3D.cpp @@ -5,7 +5,7 @@ * Author: jsola */ -#include "../src/constraint_odom_3D.h" +#include "base/constraint/constraint_odom_3D.h" namespace wolf { diff --git a/examples/test_diff_drive.cpp b/src/examples/test_diff_drive.cpp similarity index 97% rename from examples/test_diff_drive.cpp rename to src/examples/test_diff_drive.cpp index 821f5bdd8..a36a92692 100644 --- a/examples/test_diff_drive.cpp +++ b/src/examples/test_diff_drive.cpp @@ -6,11 +6,11 @@ */ //Wolf -#include "wolf.h" -#include "problem.h" -#include "../sensors/sensor_diff_drive.h" -#include "../captures/capture_wheel_joint_position.h" -#include "../processors/processor_diff_drive.h" +#include "base/wolf.h" +#include "base/problem.h" +#include "base/sensor/sensor_diff_drive.h" +#include "base/capture/capture_wheel_joint_position.h" +#include "processor/processor_diff_drive.h" //std #include <iostream> diff --git a/examples/test_eigen_quaternion.cpp b/src/examples/test_eigen_quaternion.cpp similarity index 97% rename from examples/test_eigen_quaternion.cpp rename to src/examples/test_eigen_quaternion.cpp index cea232487..1c01f5e13 100644 --- a/examples/test_eigen_quaternion.cpp +++ b/src/examples/test_eigen_quaternion.cpp @@ -6,7 +6,7 @@ #include <eigen3/Eigen/Geometry> //Wolf -#include "wolf.h" +#include "base/wolf.h" int main() { diff --git a/examples/test_eigen_template.cpp b/src/examples/test_eigen_template.cpp similarity index 99% rename from examples/test_eigen_template.cpp rename to src/examples/test_eigen_template.cpp index 3ea96954d..47e5aa419 100644 --- a/examples/test_eigen_template.cpp +++ b/src/examples/test_eigen_template.cpp @@ -5,7 +5,6 @@ * \author: jsola */ - #include <eigen3/Eigen/Dense> #include <eigen3/Eigen/Geometry> #include <iostream> @@ -57,12 +56,10 @@ inline Eigen::Quaternion<typename Derived::Scalar> v2q(const Eigen::MatrixBase<D } } - int main(void) { using namespace Eigen; - VectorXd x(10); x << 1,2,3,4,5,6,7,8,9,10; diff --git a/examples/test_faramotics_simulation.cpp b/src/examples/test_faramotics_simulation.cpp similarity index 98% rename from examples/test_faramotics_simulation.cpp rename to src/examples/test_faramotics_simulation.cpp index 63f0cd8e7..1a3feedaf 100644 --- a/examples/test_faramotics_simulation.cpp +++ b/src/examples/test_faramotics_simulation.cpp @@ -16,10 +16,10 @@ #include "unistd.h" // wolf -#include "wolf.h" -#include "feature_base.h" -#include "landmark_base.h" -#include "state_block.h" +#include "base/wolf.h" +#include "base/feature/feature_base.h" +#include "base/landmark/landmark_base.h" +#include "base/state_block.h" //faramotics includes #include "faramotics/dynamicSceneRender.h" @@ -60,7 +60,6 @@ class FaramoticsRobot myScanner->loadAssimpModel(modelFileName); } - //function travel around Eigen::Vector3s motionCampus(unsigned int ii, double& displacement_, double& rotation_) { @@ -216,7 +215,6 @@ int main(int argc, char** argv) // ROBOT MOVEMENT --------------------------- ground_truth = robot.motionCampus(step, odom_data(0), odom_data(1)); - // LIDAR DATA --------------------------- scan1 = robot.computeScan(1); scan2 = robot.computeScan(2); diff --git a/examples/test_fcn_ptr.cpp b/src/examples/test_fcn_ptr.cpp similarity index 99% rename from examples/test_fcn_ptr.cpp rename to src/examples/test_fcn_ptr.cpp index 70b38393f..8ca82790e 100644 --- a/examples/test_fcn_ptr.cpp +++ b/src/examples/test_fcn_ptr.cpp @@ -15,7 +15,6 @@ double divide (double _n, double _d) { return _n/_d; } double mult (double _x, double _y) { return _x*_y; } double mult_div (double _x, double _y, double _z) { return _x*_y/_z; } - //======== test_simple usage of function pointers ============ typedef double (*FcnType)(double); @@ -119,7 +118,6 @@ void test_var() std::cout << "4*3/6 = " << run_v((FcnTypeV)mult_div, 3, 4.0, 3.0, 6.0) << std::endl; } - //#################################################################################### int main() diff --git a/examples/test_image.cpp b/src/examples/test_image.cpp similarity index 97% rename from examples/test_image.cpp rename to src/examples/test_image.cpp index c5afcc61b..feb794e1f 100644 --- a/examples/test_image.cpp +++ b/src/examples/test_image.cpp @@ -1,10 +1,10 @@ // Testing things for the 3D image odometry //Wolf includes -#include "sensor_camera.h" -#include "capture_image.h" -#include "processor_tracker_feature_image.h" -#include "ceres_wrapper/ceres_manager.h" +#include "base/sensor/sensor_camera.h" +#include "base/capture/capture_image.h" +#include "base/processor/processor_tracker_feature_image.h" +#include "base/ceres_wrapper/ceres_manager.h" // Vision utils includes #include <vision_utils.h> diff --git a/examples/test_imuDock.cpp b/src/examples/test_imuDock.cpp similarity index 97% rename from examples/test_imuDock.cpp rename to src/examples/test_imuDock.cpp index d68e8dd43..bcaa85d9c 100644 --- a/examples/test_imuDock.cpp +++ b/src/examples/test_imuDock.cpp @@ -5,21 +5,21 @@ * \author: Dinesh Atchuthan */ -#include <processor_IMU.h> -#include <sensor_IMU.h> -#include "wolf.h" -#include "problem.h" -#include "ceres_wrapper/ceres_manager.h" -#include "sensor_odom_3D.h" -#include "processor_odom_3D.h" +#include "base/processor/processor_IMU.h" +#include "base/sensor/sensor_IMU.h" +#include "base/wolf.h" +#include "base/problem.h" +#include "base/ceres_wrapper/ceres_manager.h" +#include "base/sensor/sensor_odom_3D.h" +#include "base/processor/processor_odom_3D.h" //Constraints headers -#include "constraint_fix_bias.h" +#include "base/constraint/constraint_fix_bias.h" //std #include <iostream> #include <fstream> -#include "../constraint_pose_3D.h" +#include "base/constraint/constraint_pose_3D.h" #define OUTPUT_RESULTS //#define ADD_KF3 diff --git a/examples/test_imuDock_autoKFs.cpp b/src/examples/test_imuDock_autoKFs.cpp similarity index 97% rename from examples/test_imuDock_autoKFs.cpp rename to src/examples/test_imuDock_autoKFs.cpp index 03677027f..d0665c746 100644 --- a/examples/test_imuDock_autoKFs.cpp +++ b/src/examples/test_imuDock_autoKFs.cpp @@ -5,21 +5,21 @@ * \author: Dinesh Atchuthan */ -#include <processor_IMU.h> -#include <sensor_IMU.h> -#include "wolf.h" -#include "problem.h" -#include "ceres_wrapper/ceres_manager.h" -#include "sensor_odom_3D.h" -#include "processor_odom_3D.h" +#include "base/processor/processor_IMU.h" +#include "base/sensor/sensor_IMU.h" +#include "base/wolf.h" +#include "base/problem.h" +#include "base/ceres_wrapper/ceres_manager.h" +#include "base/sensor/sensor_odom_3D.h" +#include "base/processor/processor_odom_3D.h" //Constraints headers -#include "constraint_fix_bias.h" +#include "base/constraint/constraint_fix_bias.h" //std #include <iostream> #include <fstream> -#include "../constraint_pose_3D.h" +#include "base/constraint/constraint_pose_3D.h" #define OUTPUT_RESULTS //#define AUTO_KFS diff --git a/examples/test_imuPlateform_Offline.cpp b/src/examples/test_imuPlateform_Offline.cpp similarity index 96% rename from examples/test_imuPlateform_Offline.cpp rename to src/examples/test_imuPlateform_Offline.cpp index 9d834d73e..e1f9d23fb 100644 --- a/examples/test_imuPlateform_Offline.cpp +++ b/src/examples/test_imuPlateform_Offline.cpp @@ -1,15 +1,15 @@ //Wolf -#include <capture_IMU.h> -#include <processor_IMU.h> -#include <sensor_IMU.h> -#include "wolf.h" -#include "problem.h" -#include "sensor_odom_3D.h" -#include "constraint_odom_3D.h" -#include "state_block.h" -#include "state_quaternion.h" -#include "processor_odom_3D.h" -#include "ceres_wrapper/ceres_manager.h" +#include "base/capture/capture_IMU.h" +#include "base/processor/processor_IMU.h" +#include "base/sensor/sensor_IMU.h" +#include "base/wolf.h" +#include "base/problem.h" +#include "base/sensor/sensor_odom_3D.h" +#include "base/constraint/constraint_odom_3D.h" +#include "base/state_block.h" +#include "base/state_quaternion.h" +#include "base/processor/processor_odom_3D.h" +#include "base/ceres_wrapper/ceres_manager.h" //std #include <iostream> @@ -93,7 +93,6 @@ int main(int argc, char** argv) SensorIMUPtr sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr); ProcessorIMUPtr processor_ptr_imu = std::static_pointer_cast<ProcessorIMU>(processor_ptr_); - // SENSOR + PROCESSOR ODOM 3D SensorBasePtr sen1_ptr = wolf_problem_ptr_->installSensor("ODOM 3D", "odom", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_odom_3D.yaml"); ProcessorParamsOdom3DPtr prc_odom3D_params = std::make_shared<ProcessorParamsOdom3D>(); @@ -177,7 +176,6 @@ int main(int argc, char** argv) std::cout << "Integrated std : " << std::fixed << std::setprecision(3) << std::setw(8) << (wolf_problem_ptr_->getProcessorMotionPtr()->getMotion().delta_integr_cov_.diagonal().transpose()).array().sqrt() << std::endl; - // Print statistics std::cout << "\nStatistics -----------------------------------------------------------------------------------" << std::endl; std::cout << "If you want meaningful CPU metrics, remove all couts in the loop / remove DEBUG_RESULTS definition variable, and compile in RELEASE mode!" << std::endl; @@ -235,7 +233,6 @@ int main(int argc, char** argv) for(int i = 0; i<16; i++) cov_stdev(i) = ( covX(i,i)? 2*sqrt(covX(i,i)):0); //if diagonal value is 0 then store 0 else store 2*sqrt(diag_value) - debug_results << std::setprecision(16) << ts.get() << "\t" << frm_state(0) << "\t" << frm_state(1) << "\t" << frm_state(2) << "\t" << frm_state(3) << "\t" << frm_state(4) << "\t" << frm_state(5) << "\t" << frm_state(6) << "\t" << frm_state(7) << "\t" << frm_state(8) << "\t" << frm_state(9) diff --git a/examples/test_imu_constrained0.cpp b/src/examples/test_imu_constrained0.cpp similarity index 97% rename from examples/test_imu_constrained0.cpp rename to src/examples/test_imu_constrained0.cpp index 87330497c..d20f108d8 100644 --- a/examples/test_imu_constrained0.cpp +++ b/src/examples/test_imu_constrained0.cpp @@ -1,15 +1,15 @@ //Wolf -#include <capture_IMU.h> -#include <processor_IMU.h> -#include <sensor_IMU.h> -#include "wolf.h" -#include "problem.h" -#include "sensor_odom_3D.h" -#include "constraint_odom_3D.h" -#include "state_block.h" -#include "state_quaternion.h" -#include "processor_odom_3D.h" -#include "ceres_wrapper/ceres_manager.h" +#include "base/capture/capture_IMU.h" +#include "base/processor/processor_IMU.h" +#include "base/sensor/sensor_IMU.h" +#include "base/wolf.h" +#include "base/problem.h" +#include "base/sensor/sensor_odom_3D.h" +#include "base/constraint/constraint_odom_3D.h" +#include "base/state_block.h" +#include "base/state_quaternion.h" +#include "base/processor/processor_odom_3D.h" +#include "base/ceres_wrapper/ceres_manager.h" //std #include <iostream> @@ -34,7 +34,6 @@ int main(int argc, char** argv) // FOR IMU, file content is : Timestampt\t Ax\t Ay\t Az\t Wx\t Wy\t Wz // FOR ODOM, file content is : Timestampt\t Δpx\t Δpy\t Δpz\t Δox\t Δoy\t Δoz - std::ifstream imu_data_input; std::ifstream odom_data_input; const char * filename_imu; @@ -115,7 +114,6 @@ int main(int argc, char** argv) SensorIMUPtr sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr); ProcessorIMUPtr processor_ptr_imu = std::static_pointer_cast<ProcessorIMU>(processor_ptr_); - // SENSOR + PROCESSOR ODOM 3D SensorBasePtr sen1_ptr = wolf_problem_ptr_->installSensor("ODOM 3D", "odom", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_odom_3D_HQ.yaml"); ProcessorParamsOdom3DPtr prc_odom3D_params = std::make_shared<ProcessorParamsOdom3D>(); @@ -195,7 +193,6 @@ int main(int argc, char** argv) t = ts; //std::string report = ceres_manager_wolf_diff->solve(1); //0: nothing, 1: BriefReport, 2: FullReport - Eigen::VectorXs frm_state(16); frm_state = origin_KF->getState(); @@ -234,7 +231,6 @@ int main(int argc, char** argv) std::cout << "Integrated std : " << std::fixed << std::setprecision(3) << std::setw(8) << (wolf_problem_ptr_->getProcessorMotionPtr()->getMotion().delta_integr_cov_.diagonal()).array().sqrt() << std::endl; - // Print statistics std::cout << "\nStatistics -----------------------------------------------------------------------------------" << std::endl; std::cout << "If you want meaningful CPU metrics, remove all couts in the loop / remove DEBUG_RESULTS definition variable, and compile in RELEASE mode!" << std::endl; @@ -300,7 +296,6 @@ int main(int argc, char** argv) for(int i = 0; i<16; i++) cov_stdev(i) = ( covX(i,i)? 2*sqrt(covX(i,i)):0); //if diagonal value is 0 then store 0 else store 2*sqrt(diag_value) - debug_results << std::setprecision(16) << ts.get() << "\t" << frm_state(0) << "\t" << frm_state(1) << "\t" << frm_state(2) << "\t" << frm_state(3) << "\t" << frm_state(4) << "\t" << frm_state(5) << "\t" << frm_state(6) << "\t" << frm_state(7) << "\t" << frm_state(8) << "\t" << frm_state(9) diff --git a/examples/test_kf_callback.cpp b/src/examples/test_kf_callback.cpp similarity index 93% rename from examples/test_kf_callback.cpp rename to src/examples/test_kf_callback.cpp index 2822294e7..8f049c8d6 100644 --- a/examples/test_kf_callback.cpp +++ b/src/examples/test_kf_callback.cpp @@ -5,12 +5,10 @@ * Author: jsola */ - - -#include "../sensor_odom_2D.h" -#include "../processor_odom_2D.h" -#include "../processor_tracker_feature_dummy.h" -#include "../capture_void.h" +#include "base/sensor/sensor_odom_2D.h" +#include "base/processor/processor_odom_2D.h" +#include "base/processor/processor_tracker_feature_dummy.h" +#include "base/capture/capture_void.h" int main() { diff --git a/examples/test_list_remove.cpp b/src/examples/test_list_remove.cpp similarity index 99% rename from examples/test_list_remove.cpp rename to src/examples/test_list_remove.cpp index 458ae19db..b0795d0a6 100644 --- a/examples/test_list_remove.cpp +++ b/src/examples/test_list_remove.cpp @@ -5,8 +5,6 @@ * Author: jsola */ - - #include <memory> #include <list> #include <algorithm> diff --git a/examples/test_map_yaml.cpp b/src/examples/test_map_yaml.cpp similarity index 94% rename from examples/test_map_yaml.cpp rename to src/examples/test_map_yaml.cpp index 80273a384..7990919ca 100644 --- a/examples/test_map_yaml.cpp +++ b/src/examples/test_map_yaml.cpp @@ -5,13 +5,13 @@ * \author: jsola */ -#include "wolf.h" -#include "problem.h" -#include "map_base.h" -#include "landmark_polyline_2D.h" -#include "landmark_AHP.h" -#include "state_block.h" -#include "../yaml/yaml_conversion.h" +#include "base/wolf.h" +#include "base/problem.h" +#include "base/map_base.h" +#include "base/landmark/landmark_polyline_2D.h" +#include "base/landmark/landmark_AHP.h" +#include "base/state_block.h" +#include "base/yaml/yaml_conversion.h" #include <iostream> using namespace wolf; @@ -88,7 +88,6 @@ int main() std::string thisfile = __FILE__; problem->getMapPtr()->save(filename, "Example generated by test file " + thisfile); - std::cout << "Clearing map... " << std::endl; problem->getMapPtr()->getLandmarkList().clear(); diff --git a/examples/test_matrix_prod.cpp b/src/examples/test_matrix_prod.cpp similarity index 99% rename from examples/test_matrix_prod.cpp rename to src/examples/test_matrix_prod.cpp index a99ff1424..b03068283 100644 --- a/examples/test_matrix_prod.cpp +++ b/src/examples/test_matrix_prod.cpp @@ -88,8 +88,6 @@ EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix<double,9,1>) std::cout << "Time Co = C * C: " << (long double)(t1 - t0) * 1e9 / CLOCKS_PER_SEC / N \ << "ns <-- this is the Eigen default!" << std::endl; - - /** * We multiply matrices and see how long it takes. * We compare different combinations of row-major and column-major to see which one is the fastest. diff --git a/examples/test_mpu.cpp b/src/examples/test_mpu.cpp similarity index 97% rename from examples/test_mpu.cpp rename to src/examples/test_mpu.cpp index 4acbf55b7..08db29a81 100644 --- a/examples/test_mpu.cpp +++ b/src/examples/test_mpu.cpp @@ -6,11 +6,11 @@ */ //Wolf -#include <capture_IMU.h> -#include "wolf.h" -#include "problem.h" -#include "state_block.h" -#include "state_quaternion.h" +#include "base/capture/capture_IMU.h" +#include "base/wolf.h" +#include "base/problem.h" +#include "base/state_block.h" +#include "base/state_quaternion.h" #include <iostream> #include <fstream> #include <iomanip> @@ -18,8 +18,8 @@ #include <cmath> #include <termios.h> #include <fcntl.h> -#include <processor_IMU.h> -#include <sensor_IMU.h> +#include "base/processor/processor_IMU.h" +#include "base/sensor/sensor_IMU.h" #define DEBUG_RESULTS #define FROM_FILE @@ -194,7 +194,6 @@ int main(int argc, char** argv) std::cout << "Integrated std : " << std::fixed << std::setprecision(3) << std::setw(8) << (wolf_problem_ptr_->getProcessorMotionPtr()->getMotion().delta_integr_cov_.diagonal().transpose()).array().sqrt() << std::endl; - // Print statistics std::cout << "\nStatistics -----------------------------------------------------------------------------------" << std::endl; std::cout << "If you want meaningful CPU metrics, remove all couts in the loop / remove DEBUG_RESULTS definition variable, and compile in RELEASE mode!" << std::endl; diff --git a/examples/test_processor_imu.cpp b/src/examples/test_processor_imu.cpp similarity index 97% rename from examples/test_processor_imu.cpp rename to src/examples/test_processor_imu.cpp index 119e2fe62..f9b57a323 100644 --- a/examples/test_processor_imu.cpp +++ b/src/examples/test_processor_imu.cpp @@ -6,13 +6,13 @@ */ //Wolf -#include <capture_IMU.h> -#include <processor_IMU.h> -#include <sensor_IMU.h> -#include "wolf.h" -#include "problem.h" -#include "state_block.h" -#include "state_quaternion.h" +#include "base/capture/capture_IMU.h" +#include "base/processor/processor_IMU.h" +#include "base/sensor/sensor_IMU.h" +#include "base/wolf.h" +#include "base/problem.h" +#include "base/state_block.h" +#include "base/state_quaternion.h" #include <iostream> #include <fstream> #include <iomanip> @@ -21,7 +21,6 @@ //#define DEBUG_RESULTS - int main(int argc, char** argv) { using namespace wolf; @@ -185,7 +184,6 @@ int main(int argc, char** argv) // std::cout << "Integrated std : " << std::fixed << std::setprecision(3) << std::setw(8) // << (problem_ptr_->getProcessorMotionPtr()->getMotion().delta_integr_cov_.diagonal().transpose()).array().sqrt() << std::endl; - // Print statistics std::cout << "\nStatistics -----------------------------------------------------------------------------------" << std::endl; std::cout << "If you want meaningful CPU metrics, remove all couts in the loop / remove DEBUG_RESULTS definition variable, and compile in RELEASE mode!" << std::endl; diff --git a/examples/test_processor_imu_jacobians.cpp b/src/examples/test_processor_imu_jacobians.cpp similarity index 99% rename from examples/test_processor_imu_jacobians.cpp rename to src/examples/test_processor_imu_jacobians.cpp index 6cdb01c97..e876a21d9 100644 --- a/examples/test_processor_imu_jacobians.cpp +++ b/src/examples/test_processor_imu_jacobians.cpp @@ -6,13 +6,13 @@ */ //Wolf -#include <capture_IMU.h> -#include <sensor_IMU.h> +#include "base/capture/capture_IMU.h" +#include "base/sensor/sensor_IMU.h" #include <test/processor_IMU_UnitTester.h> -#include "wolf.h" -#include "problem.h" -#include "state_block.h" -#include "state_quaternion.h" +#include "base/wolf.h" +#include "base/problem.h" +#include "base/state_block.h" +#include "base/state_quaternion.h" #include <iostream> #include <fstream> #include <iomanip> @@ -192,7 +192,6 @@ int main(int argc, char** argv) else std::cout<< "\t\tdDq_dab_ jacobian is not Zero :" << dDq_dab << std::endl; - /* Numerical method to check jacobians wrt noise dDp_dP = [dDp_dPx, dDp_dPy, dDp_dPz] diff --git a/examples/test_processor_odom_3D.cpp b/src/examples/test_processor_odom_3D.cpp similarity index 91% rename from examples/test_processor_odom_3D.cpp rename to src/examples/test_processor_odom_3D.cpp index b7c2c9a9d..c9321b51e 100644 --- a/examples/test_processor_odom_3D.cpp +++ b/src/examples/test_processor_odom_3D.cpp @@ -5,14 +5,13 @@ * \author: jsola */ - -#include <capture_IMU.h> -#include "problem.h" -#include "sensor_odom_2D.h" -#include "processor_odom_3D.h" -#include "map_base.h" -#include "landmark_base.h" -#include "ceres_wrapper/ceres_manager.h" +#include "base/capture/capture_IMU.h" +#include "base/problem.h" +#include "base/sensor/sensor_odom_2D.h" +#include "base/processor/processor_odom_3D.h" +#include "base/map_base.h" +#include "base/landmark/landmark_base.h" +#include "base/ceres_wrapper/ceres_manager.h" #include <cstdlib> @@ -25,8 +24,6 @@ using Eigen::Vector7s; using Eigen::Quaternions; using Eigen::VectorXs; - - int main (int argc, char** argv) { cout << "\n========= Test ProcessorOdom3D ===========" << endl; diff --git a/examples/test_processor_tracker_feature.cpp b/src/examples/test_processor_tracker_feature.cpp similarity index 87% rename from examples/test_processor_tracker_feature.cpp rename to src/examples/test_processor_tracker_feature.cpp index 943e18010..ab22be418 100644 --- a/examples/test_processor_tracker_feature.cpp +++ b/src/examples/test_processor_tracker_feature.cpp @@ -9,12 +9,12 @@ #include <iostream> //Wolf -#include "wolf.h" -#include "problem.h" -#include "sensor_base.h" -#include "state_block.h" -#include "processor_tracker_feature_dummy.h" -#include "capture_void.h" +#include "base/wolf.h" +#include "base/problem.h" +#include "base/sensor/sensor_base.h" +#include "base/state_block.h" +#include "base/processor/processor_tracker_feature_dummy.h" +#include "base/capture/capture_void.h" int main() { diff --git a/examples/test_processor_tracker_landmark.cpp b/src/examples/test_processor_tracker_landmark.cpp similarity index 93% rename from examples/test_processor_tracker_landmark.cpp rename to src/examples/test_processor_tracker_landmark.cpp index bbf3e4132..15ed24d72 100644 --- a/examples/test_processor_tracker_landmark.cpp +++ b/src/examples/test_processor_tracker_landmark.cpp @@ -9,12 +9,12 @@ #include <iostream> //Wolf -#include "wolf.h" -#include "problem.h" -#include "sensor_base.h" -#include "state_block.h" -#include "processor_tracker_landmark_dummy.h" -#include "capture_void.h" +#include "base/wolf.h" +#include "base/problem.h" +#include "base/sensor/sensor_base.h" +#include "base/state_block.h" +#include "base/processor/processor_tracker_landmark_dummy.h" +#include "base/capture/capture_void.h" void print_problem_pointers(wolf::ProblemPtr wolf_problem_ptr_) { @@ -87,9 +87,6 @@ int main() wolf_problem_ptr_->print(2); - return 0; } - - diff --git a/examples/test_processor_tracker_landmark_image.cpp b/src/examples/test_processor_tracker_landmark_image.cpp similarity index 96% rename from examples/test_processor_tracker_landmark_image.cpp rename to src/examples/test_processor_tracker_landmark_image.cpp index 5a1915901..f5b084e13 100644 --- a/examples/test_processor_tracker_landmark_image.cpp +++ b/src/examples/test_processor_tracker_landmark_image.cpp @@ -1,18 +1,18 @@ //std #include <iostream> -#include "processor_tracker_landmark_image.h" +#include "base/processor/processor_tracker_landmark_image.h" //Wolf -#include "wolf.h" -#include "problem.h" -#include "state_block.h" -#include "processor_odom_3D.h" -#include "sensor_odom_3D.h" -#include "sensor_camera.h" -#include "capture_image.h" -#include "capture_pose.h" -#include "ceres_wrapper/ceres_manager.h" +#include "base/wolf.h" +#include "base/problem.h" +#include "base/state_block.h" +#include "base/processor/processor_odom_3D.h" +#include "base/sensor/sensor_odom_3D.h" +#include "base/sensor/sensor_camera.h" +#include "base/capture/capture_image.h" +#include "base/capture/capture_pose.h" +#include "base/ceres_wrapper/ceres_manager.h" // Vision utils includes #include <vision_utils.h> @@ -221,12 +221,10 @@ int main(int argc, char** argv) // std::cout << "current ts: " << t.get() << std::endl; // std::cout << " dt: " << t_prev - t_prev_prev << "; dx: " << dx.transpose() << std::endl; - // Cleanup map --------------------------------------- cleanupMap(problem, t, 2, 5); // dt, min_ctr - // Solve ----------------------------------------------- // solve only when new KFs are added if (problem->getTrajectoryPtr()->getFrameList().size() > number_of_KFs) @@ -236,7 +234,6 @@ int main(int argc, char** argv) std::cout << summary << std::endl; } - // Finish loop ----------------------------------------- cv::Mat image_graphics = frame_buff.back()->getImage().clone(); prc_img_ptr->drawTrackerRoi(image_graphics, cv::Scalar(255.0, 0.0, 255.0)); //tracker roi diff --git a/examples/test_projection_points.cpp b/src/examples/test_projection_points.cpp similarity index 99% rename from examples/test_projection_points.cpp rename to src/examples/test_projection_points.cpp index 7945c939f..c24e8904e 100644 --- a/examples/test_projection_points.cpp +++ b/src/examples/test_projection_points.cpp @@ -6,7 +6,7 @@ #include <iostream> //wolf includes -#include "pinhole_tools.h" +#include "base/pinhole_tools.h" int main(int argc, char** argv) { @@ -41,7 +41,6 @@ int main(int argc, char** argv) for (int ii = 0; ii < points3D.rows(); ++ii) std::cout << "points2D- X: " << points2D(ii,0) << "; Y: " << points2D(ii,1) << std::endl; - std::cout << std::endl << " ========= PinholeTools DUALITY TEST ===========" << std::endl << std::endl; //================================= projectPoint and backprojectPoint to/from NormalizedPlane @@ -67,14 +66,12 @@ int main(int argc, char** argv) pinhole::backprojectPointFromNormalizedPlane(project_point_normalized_output, backproject_point_normalized_depth); - std::cout << "TEST project and backproject PointToNormalizedPlane" << std::endl; std::cout << std:: endl << "Original " << project_point_normalized_test.transpose() << std::endl; std::cout << std:: endl << "Project " << project_point_normalized_output.transpose() << std::endl; std::cout << std:: endl << "Alternate project" << project_point_normalized_output.transpose() << std::endl; std::cout << std:: endl << "Backproject " << backproject_point_normalized_output.transpose() << std::endl; - //================================= projectPoint and backprojectPoint to/from NormalizedPlane WITH JACOBIANS Eigen::Vector3s pp_normalized_test; @@ -92,7 +89,6 @@ int main(int argc, char** argv) Eigen::Matrix<Scalar, 3, 2> bpp_normalized_jacobian; Eigen::Vector3s bpp_normalized_jacobian_depth; - pinhole::projectPointToNormalizedPlane(pp_normalized_test, pp_normalized_output, pp_normalized_jacobian); @@ -123,13 +119,11 @@ int main(int argc, char** argv) std::cout << "\n--> Jacobian" << std::endl << bpp_normalized_jacobian << std::endl; std::cout << "\n--> Jacobian - depth" << bpp_normalized_jacobian_depth.transpose() << std::endl; - Eigen::Matrix2s test_jacobian; // (2x3) * (3x2) = (2x2) test_jacobian = pp_normalized_jacobian * bpp_normalized_jacobian; std::cout << "\n\n\n==> Jacobian Testing" << std::endl << test_jacobian << std::endl; - //================================= IsInRoi / IsInImage Eigen::Vector2s pix; @@ -166,8 +160,6 @@ int main(int argc, char** argv) std::cout << "width: " << image_width << "; height: " << image_height << std::endl; std::cout << "is_in_image: " << is_in_image << std::endl; - - //================================= computeCorrectionModel Eigen::Vector2s distortion2; @@ -194,11 +186,8 @@ int main(int argc, char** argv) std::cout << std::endl << "correction" << std::endl; std::cout << "c1: " << correction_test2[0] << "; c2: " << correction_test2[1] << std::endl; - - //================================= distortPoint - Eigen::Vector2s distorting_point; distorting_point[0] = 0.35355; distorting_point[1] = 0.35355; @@ -237,8 +226,6 @@ int main(int argc, char** argv) std::cout << "\n--> Jacobian" << std::endl << distortion_jacobian2 << std::endl; - - Eigen::Vector2s corrected_point5; Eigen::Matrix2s corrected_jacobian2; pinhole::undistortPoint(correction_test2, @@ -280,7 +267,6 @@ int main(int argc, char** argv) std::cout << std::endl << "Depixelized" << std::endl; std::cout << "x: " << depixelize_output3[0] << "; y: " << depixelize_output3[1] << std::endl; - //// Eigen::Vector2s pixelize_output4; @@ -298,7 +284,6 @@ int main(int argc, char** argv) std::cout << "\n--> Jacobian" << std::endl << pixelize_jacobian2 << std::endl; - Eigen::Vector2s depixelize_output4; Eigen::Matrix2s depixelize_jacobian2; pinhole::depixellizePoint(k_test2, @@ -317,12 +302,8 @@ int main(int argc, char** argv) std::cout << "\n\n\n==> Jacobian Testing" << std::endl << test_jacobian_pix << std::endl; - - - //================================= projectPoint Complete - // //distortion // distortion2; @@ -332,7 +313,6 @@ int main(int argc, char** argv) // //3Dpoint // project_point_normalized_test; - Eigen::Vector2s point2D_test5; point2D_test5 = pinhole::projectPoint(k_test2, distortion2, @@ -393,7 +373,6 @@ int main(int argc, char** argv) std::cout << "\n-->Jacobian" << std::endl << jacobian_test4 << std::endl; - ///////////////////////////// // //correction @@ -414,8 +393,6 @@ int main(int argc, char** argv) std::cout << std::endl << "First function output" << std::endl; std::cout << "x: " << point3D_backproj5[0] << "; y: " << point3D_backproj5[1] << "; z: " << point3D_backproj5[2] << std::endl; - - //jacobian Eigen::Vector3s point3D_backproj4; Eigen::MatrixXs jacobian_backproj2(3,2); @@ -435,20 +412,12 @@ int main(int argc, char** argv) std::cout << "\n--> Jacobian - depth" << std::endl << depth_jacobian2[0] << " " << depth_jacobian2[1] << " " << depth_jacobian2[2] << " " << std::endl; - - - Eigen::Matrix2s test_jacobian_complete; test_jacobian_complete = jacobian_test4 * jacobian_backproj2; std::cout << "\n\n\n==> Jacobian Testing" << std::endl << test_jacobian_complete << std::endl; - - - - - /* Test */ std::cout << "\n\n\n\nTEST\n" << std::endl; @@ -476,8 +445,6 @@ int main(int argc, char** argv) Eigen::Vector2s test_point2D = {123,321}; std::cout << "\ntest_point2D ORIGINAL:\n" << test_point2D << std::endl; - - test_point2D = pinhole::depixellizePoint(K_params, test_point2D); std::cout << "\ntest_point2D DEPIXELIZED:\n" << test_point2D << std::endl; @@ -488,8 +455,6 @@ int main(int argc, char** argv) 2); std::cout << "\ntest_point3D BACKPROJECTED:\n" << test_point3D << std::endl; - - test_point2D = pinhole::projectPointToNormalizedPlane(test_point3D); std::cout << "\ntest_point2D NORMALIZED:\n" << test_point2D << std::endl; test_point2D = pinhole::distortPoint(distortion_vector, @@ -501,9 +466,3 @@ int main(int argc, char** argv) } - - - - - - diff --git a/examples/test_sh_ptr.cpp b/src/examples/test_sh_ptr.cpp similarity index 99% rename from examples/test_sh_ptr.cpp rename to src/examples/test_sh_ptr.cpp index 74bdd8bd7..294555ed1 100644 --- a/examples/test_sh_ptr.cpp +++ b/src/examples/test_sh_ptr.cpp @@ -39,7 +39,6 @@ class c; // constraint class M; // map class L; // landmark - ////////////////////////////////////////////////////////////////////////////////// // DECLARE WOLF TREE @@ -318,8 +317,6 @@ class M : public enable_shared_from_this<M> shared_ptr<L> add_L(shared_ptr<L> _L); }; - - class L : public enable_shared_from_this<L> { private: @@ -522,7 +519,6 @@ void c::remove() } } - shared_ptr<L> M::add_L(shared_ptr<L> _L) { L_list_.push_back(_L); @@ -544,8 +540,6 @@ void L::remove() } } - - } ////////////////////////////////////////////////////////////////////////////////// @@ -770,7 +764,6 @@ int main() cout<<endl; print_c(Pp); - //------------------------------------------------------------------ // Several tests. Uncomment the desired test. // Run only one test at a time, otherwise you'll get segfaults! @@ -800,4 +793,3 @@ int main() return 1; } - diff --git a/examples/test_simple_AHP.cpp b/src/examples/test_simple_AHP.cpp similarity index 96% rename from examples/test_simple_AHP.cpp rename to src/examples/test_simple_AHP.cpp index 6a3279326..818250e17 100644 --- a/examples/test_simple_AHP.cpp +++ b/src/examples/test_simple_AHP.cpp @@ -5,15 +5,15 @@ * \author: jtarraso */ -#include "wolf.h" -#include "frame_base.h" -#include "pinhole_tools.h" -#include "sensor_camera.h" -#include "rotations.h" -#include "capture_image.h" -#include "landmark_AHP.h" -#include "constraint_AHP.h" -#include "ceres_wrapper/ceres_manager.h" +#include "base/wolf.h" +#include "base/frame_base.h" +#include "base/pinhole_tools.h" +#include "base/sensor/sensor_camera.h" +#include "base/rotations.h" +#include "base/capture/capture_image.h" +#include "base/landmark/landmark_AHP.h" +#include "base/constraint/constraint_AHP.h" +#include "base/ceres_wrapper/ceres_manager.h" // Vision utils #include <vision_utils/vision_utils.h> @@ -61,12 +61,10 @@ * 0 160 320 480 640 * +----+----+----+----+ * | - * | * | 320 * | * * * - * * projected pixels: * p0 = (320,240) // at optical axis or relation 1:0 * p1 = ( 0 ,240) // at 45 deg or relation 1:1 @@ -129,7 +127,6 @@ int main(int argc, char** argv) // std::cout << "lmk hmg in C frame: " << lmk_hmg_c.transpose() << std::endl; // ============================================================================================================ - // Captures------------------ cv::Mat cv_image; cv_image.zeros(2,2,0); @@ -199,7 +196,6 @@ int main(int argc, char** argv) FeaturePointImagePtr feat_4 = std::make_shared<FeaturePointImage>(kp_2, 0, desc, Eigen::Matrix2s::Identity()); image_2->addFeature(feat_4); - // New landmark with measured pixels from kf2 (anchor) kf3 and kf4 (measurements) Scalar unknown_distance = 2; // the real distance is 1 Matrix3s K = camera->getIntrinsicMatrix(); @@ -241,7 +237,6 @@ int main(int argc, char** argv) CeresManager ceres_manager(problem, ceres_options); - std::string summary = ceres_manager.solve(SolverManager::ReportVerbosity::FULL);// 0: nothing, 1: BriefReport, 2: FullReport std::cout << summary << std::endl; @@ -260,18 +255,3 @@ int main(int argc, char** argv) } - - - - - - - - - - - - - - - diff --git a/examples/test_sort_keyframes.cpp b/src/examples/test_sort_keyframes.cpp similarity index 95% rename from examples/test_sort_keyframes.cpp rename to src/examples/test_sort_keyframes.cpp index a1e908a61..967d37006 100644 --- a/examples/test_sort_keyframes.cpp +++ b/src/examples/test_sort_keyframes.cpp @@ -6,10 +6,10 @@ */ // Wolf includes -#include "../wolf.h" -#include "../problem.h" -#include "../frame_base.h" -#include "../trajectory_base.h" +#include "base/wolf.h" +#include "base/problem.h" +#include "base/frame_base.h" +#include "base/trajectory_base.h" // STL includes #include <list> @@ -75,7 +75,5 @@ int main() printFrames(problem_ptr); - - return 0; } diff --git a/examples/test_sparsification.cpp b/src/examples/test_sparsification.cpp similarity index 98% rename from examples/test_sparsification.cpp rename to src/examples/test_sparsification.cpp index ea8113d1b..41b3b8eae 100644 --- a/examples/test_sparsification.cpp +++ b/src/examples/test_sparsification.cpp @@ -14,10 +14,10 @@ #include <queue> //Wolf includes -#include "capture_void.h" -#include "feature_odom_2D.h" -#include "constraint_base.h" -#include "ceres_wrapper/ceres_manager.h" +#include "base/capture/capture_void.h" +#include "base/feature/feature_odom_2D.h" +#include "base/constraint/constraint_base.h" +#include "base/ceres_wrapper/ceres_manager.h" // EIGEN //#include <Eigen/CholmodSupport> @@ -297,7 +297,6 @@ int main(int argc, char** argv) // covariance bl_ceres_manager->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL);//ALL_MARGINALS - // t1 = clock(); // double t_sigma_manual = 0; // t_sigma_manual += ((double) clock() - t1) / CLOCKS_PER_SEC; diff --git a/examples/test_state_quaternion.cpp b/src/examples/test_state_quaternion.cpp similarity index 90% rename from examples/test_state_quaternion.cpp rename to src/examples/test_state_quaternion.cpp index ec23b9e0f..4d2aa60cc 100644 --- a/examples/test_state_quaternion.cpp +++ b/src/examples/test_state_quaternion.cpp @@ -5,10 +5,9 @@ * \author: jsola */ - -#include "frame_base.h" -#include "state_quaternion.h" -#include "time_stamp.h" +#include "base/frame_base.h" +#include "base/state_quaternion.h" +#include "base/time_stamp.h" #include <iostream> @@ -36,6 +35,5 @@ int main (void) std::cout << "Done" << std::endl; - return 1; } diff --git a/examples/test_tracker_ORB.cpp b/src/examples/test_tracker_ORB.cpp similarity index 99% rename from examples/test_tracker_ORB.cpp rename to src/examples/test_tracker_ORB.cpp index f695a3cb4..536e06d62 100644 --- a/examples/test_tracker_ORB.cpp +++ b/src/examples/test_tracker_ORB.cpp @@ -11,7 +11,7 @@ #include <matchers/bruteforce_hamming_2/matcher_bruteforce_hamming_2.h> //Wolf -#include "../processor_tracker_landmark_image.h" +#include "base/processor/processor_tracker_landmark_image.h" int main(int argc, char** argv) { @@ -244,7 +244,6 @@ int main(int argc, char** argv) current_keypoints.clear(); current_descriptors.release(); - std::cout << "\nAFTER THE ADVANCE" << std::endl; // for(unsigned int i = 0; i < target_keypoints.size(); i++) // { diff --git a/examples/test_virtual_hierarchy.cpp b/src/examples/test_virtual_hierarchy.cpp similarity index 100% rename from examples/test_virtual_hierarchy.cpp rename to src/examples/test_virtual_hierarchy.cpp diff --git a/examples/test_wolf_autodiffwrapper.cpp b/src/examples/test_wolf_autodiffwrapper.cpp similarity index 99% rename from examples/test_wolf_autodiffwrapper.cpp rename to src/examples/test_wolf_autodiffwrapper.cpp index 5728739a8..a8de6a82b 100644 --- a/examples/test_wolf_autodiffwrapper.cpp +++ b/src/examples/test_wolf_autodiffwrapper.cpp @@ -12,8 +12,8 @@ //Wolf includes #include "wolf_manager.h" -#include "capture_void.h" -#include "ceres_wrapper/ceres_manager.h" +#include "base/capture/capture_void.h" +#include "base/ceres_wrapper/ceres_manager.h" int main(int argc, char** argv) { @@ -57,8 +57,6 @@ int main(int argc, char** argv) CeresManager* ceres_manager_ceres_diff = new CeresManager(wolf_problem_ceres_diff, ceres_options, false); CeresManager* ceres_manager_wolf_diff = new CeresManager(wolf_problem_wolf_diff, ceres_options, true); - - // load graph from .txt offLineFile_.open(file_path_.c_str(), std::ifstream::in); if (offLineFile_.is_open()) diff --git a/examples/test_wolf_factories.cpp b/src/examples/test_wolf_factories.cpp similarity index 92% rename from examples/test_wolf_factories.cpp rename to src/examples/test_wolf_factories.cpp index 3b2b68c18..4108864a7 100644 --- a/examples/test_wolf_factories.cpp +++ b/src/examples/test_wolf_factories.cpp @@ -5,27 +5,26 @@ * \author: jsola */ -#include <processor_IMU.h> -#include <sensor_GPS_fix.h> -#include "../hardware_base.h" -#include "../sensor_camera.h" -#include "../sensor_odom_2D.h" +#include "base/processor/processor_IMU.h" +#include "base/sensor/sensor_GPS_fix.h" +#include "base/hardware_base.h" +#include "base/sensor/sensor_camera.h" +#include "base/sensor/sensor_odom_2D.h" #include "../sensor_imu.h" //#include "../sensor_gps.h" -#include "../processor_odom_2D.h" -#include "../processor_odom_3D.h" +#include "base/processor/processor_odom_2D.h" +#include "base/processor/processor_odom_3D.h" #include "../processor_image_feature.h" -#include "../problem.h" +#include "base/problem.h" -#include "../factory.h" +#include "base/factory.h" #include <iostream> #include <iomanip> #include <cstdlib> - int main(void) { using namespace wolf; @@ -34,8 +33,6 @@ int main(void) using std::make_shared; using std::static_pointer_cast; - - //============================================================================================== std::string wolf_root = _WOLF_ROOT_DIR; std::string wolf_config = wolf_root + "/src/examples"; @@ -105,7 +102,6 @@ int main(void) << " | name: " << setw(17) << prc->getName() << " | bound to sensor " << setw(2) << prc->getSensorPtr()->id() << ": " << prc->getSensorPtr()->getName() << endl; - return 0; } diff --git a/examples/test_wolf_imported_graph.cpp b/src/examples/test_wolf_imported_graph.cpp similarity index 99% rename from examples/test_wolf_imported_graph.cpp rename to src/examples/test_wolf_imported_graph.cpp index fc8129dc5..cbfe23b78 100644 --- a/examples/test_wolf_imported_graph.cpp +++ b/src/examples/test_wolf_imported_graph.cpp @@ -12,8 +12,8 @@ //Wolf includes #include "wolf_manager.h" -#include "capture_void.h" -#include "ceres_wrapper/ceres_manager.h" +#include "base/capture/capture_void.h" +#include "base/ceres_wrapper/ceres_manager.h" // EIGEN //#include <Eigen/CholmodSupport> @@ -28,7 +28,6 @@ void insertSparseBlock(const Eigen::SparseMatrix<Scalar>& ins, Eigen::SparseMatr original.makeCompressed(); } -} int main(int argc, char** argv) { @@ -76,8 +75,6 @@ int main(int argc, char** argv) CeresManager* ceres_manager_full = new CeresManager(wolf_problem_full, ceres_options); CeresManager* ceres_manager_prun = new CeresManager(wolf_problem_prun, ceres_options); - - // load graph from .txt offLineFile_.open(file_path_.c_str(), std::ifstream::in); if (offLineFile_.is_open()) @@ -362,7 +359,6 @@ int main(int argc, char** argv) // std::cout << "Sigma_ij" << std::endl << Sigma_ij << std::endl; // std::cout << "Sigma(i,j)" << std::endl << Sigma.block<3,3>(frame_ptr_2_index_prun[(*c_it)->getFrameToPtr()]*3, frame_ptr_2_index_prun[(*c_it)->getCapturePtr()->getFramePtr()]*3) << std::endl; - //jacobian xi = *(*c_it)->getFrameOtherPtr()->getPPtr()->getPtr(); yi = *((*c_it)->getFrameOtherPtr()->getPPtr()->getPtr()+1); diff --git a/examples/test_wolf_logging.cpp b/src/examples/test_wolf_logging.cpp similarity index 87% rename from examples/test_wolf_logging.cpp rename to src/examples/test_wolf_logging.cpp index 0f92079cd..b516acd09 100644 --- a/examples/test_wolf_logging.cpp +++ b/src/examples/test_wolf_logging.cpp @@ -5,8 +5,8 @@ * \author: Jeremie Deray */ -#include "wolf.h" -#include "logging.h" +#include "base/wolf.h" +#include "base/logging.h" int main(int, char*[]) { diff --git a/examples/test_wolf_prunning.cpp b/src/examples/test_wolf_prunning.cpp similarity index 99% rename from examples/test_wolf_prunning.cpp rename to src/examples/test_wolf_prunning.cpp index 3191326c0..13b3dd86d 100644 --- a/examples/test_wolf_prunning.cpp +++ b/src/examples/test_wolf_prunning.cpp @@ -12,9 +12,9 @@ //Wolf includes #include "wolf_manager.h" -#include "capture_void.h" -#include "constraint_base.h" -#include "ceres_wrapper/ceres_manager.h" +#include "base/capture/capture_void.h" +#include "base/constraint/constraint_base.h" +#include "base/ceres_wrapper/ceres_manager.h" // EIGEN //#include <Eigen/CholmodSupport> @@ -31,7 +31,6 @@ void insertSparseBlock(const Eigen::SparseMatrix<Scalar>& ins, Eigen::SparseMatr original.makeCompressed(); } -} int main(int argc, char** argv) { @@ -403,7 +402,6 @@ int main(int argc, char** argv) // std::cout << "Sigma_34" << std::endl << Sigma_34 << std::endl; // std::cout << "Sigma_44" << std::endl << Sigma_44 << std::endl; - // jacobians ((ConstraintAnalytic*)(*c_it))->evaluatePureJacobians(jacobians); Eigen::MatrixXs& J1 = jacobians[0]; @@ -434,7 +432,6 @@ int main(int argc, char** argv) J4 * Sigma_34.transpose() * J3.transpose() + J4 * Sigma_44 * J4.transpose()) ).determinant() ); - // std::cout << "part = " << std::endl << (J1 * Sigma_11 * J1.transpose() + // J1 * Sigma_12 * J2.transpose() + // J1 * Sigma_13 * J3.transpose() + diff --git a/examples/test_wolf_root.cpp b/src/examples/test_wolf_root.cpp similarity index 92% rename from examples/test_wolf_root.cpp rename to src/examples/test_wolf_root.cpp index f6dc51ce9..a2e3ccef5 100644 --- a/examples/test_wolf_root.cpp +++ b/src/examples/test_wolf_root.cpp @@ -6,7 +6,7 @@ */ //Wolf -#include "wolf.h" +#include "base/wolf.h" //std #include <iostream> diff --git a/examples/test_wolf_tree.cpp b/src/examples/test_wolf_tree.cpp similarity index 100% rename from examples/test_wolf_tree.cpp rename to src/examples/test_wolf_tree.cpp diff --git a/examples/test_yaml.cpp b/src/examples/test_yaml.cpp similarity index 98% rename from examples/test_yaml.cpp rename to src/examples/test_yaml.cpp index 56a33fe1d..cfd03d18d 100644 --- a/examples/test_yaml.cpp +++ b/src/examples/test_yaml.cpp @@ -5,10 +5,10 @@ * \author: jsola */ -#include "pinhole_tools.h" +#include "base/pinhole_tools.h" #include "yaml/yaml_conversion.h" #include "processor_image_feature.h" -#include "factory.h" +#include "base/factory.h" #include <yaml-cpp/yaml.h> @@ -17,7 +17,6 @@ #include <iostream> #include <fstream> - int main() { @@ -69,8 +68,6 @@ int main() else std::cout << "Bad configuration file. No sensor type found." << std::endl; - - // // Processor Image parameters // // ProcessorParamsImage p; @@ -93,6 +90,5 @@ int main() // p.min_features_for_keyframe = alg["minimum features for new keyframe"].as<unsigned int>(); // } - return 0; } diff --git a/examples/test_yaml_conversions.cpp b/src/examples/test_yaml_conversions.cpp similarity index 97% rename from examples/test_yaml_conversions.cpp rename to src/examples/test_yaml_conversions.cpp index d6393d8b8..78cf30da7 100644 --- a/examples/test_yaml_conversions.cpp +++ b/src/examples/test_yaml_conversions.cpp @@ -5,7 +5,7 @@ * \author: jsola */ -#include "../yaml/yaml_conversion.h" +#include "base/yaml/yaml_conversion.h" #include <yaml-cpp/yaml.h> @@ -14,13 +14,11 @@ #include <iostream> //#include <fstream> - int main() { using namespace Eigen; - std::cout << "\nTrying different yaml specs for matrix..." << std::endl; YAML::Node mat_sized_23, mat_sized_33, mat_sized_bad, mat_23, mat_33, mat_bad; diff --git a/examples/vision_utils_active_search.yaml b/src/examples/vision_utils_active_search.yaml similarity index 100% rename from examples/vision_utils_active_search.yaml rename to src/examples/vision_utils_active_search.yaml diff --git a/src/feature/feature_GPS_fix.cpp b/src/feature/feature_GPS_fix.cpp index ca370b59b..d4b5a12f2 100644 --- a/src/feature/feature_GPS_fix.cpp +++ b/src/feature/feature_GPS_fix.cpp @@ -1,4 +1,4 @@ -#include "feature_GPS_fix.h" +#include "base/feature/feature_GPS_fix.h" namespace wolf { diff --git a/src/feature/feature_GPS_pseudorange.cpp b/src/feature/feature_GPS_pseudorange.cpp index 37bbfd8fa..7979f30e4 100644 --- a/src/feature/feature_GPS_pseudorange.cpp +++ b/src/feature/feature_GPS_pseudorange.cpp @@ -1,4 +1,4 @@ -#include "feature_GPS_pseudorange.h" +#include "base/feature/feature_GPS_pseudorange.h" namespace wolf { diff --git a/src/feature/feature_IMU.cpp b/src/feature/feature_IMU.cpp index 796f4834a..0f811cff5 100644 --- a/src/feature/feature_IMU.cpp +++ b/src/feature/feature_IMU.cpp @@ -1,4 +1,4 @@ -#include "feature_IMU.h" +#include "base/feature/feature_IMU.h" namespace wolf { diff --git a/src/feature/feature_base.cpp b/src/feature/feature_base.cpp index 6459ffc24..314457be2 100644 --- a/src/feature/feature_base.cpp +++ b/src/feature/feature_base.cpp @@ -1,6 +1,6 @@ -#include "feature_base.h" -#include "constraint_base.h" -#include "capture_base.h" +#include "base/feature/feature_base.h" +#include "base/constraint/constraint_base.h" +#include "base/capture/capture_base.h" namespace wolf { diff --git a/src/feature/feature_corner_2D.cpp b/src/feature/feature_corner_2D.cpp index 59db17e3b..27803ef4a 100644 --- a/src/feature/feature_corner_2D.cpp +++ b/src/feature/feature_corner_2D.cpp @@ -1,5 +1,5 @@ -#include "feature_corner_2D.h" +#include "base/feature/feature_corner_2D.h" namespace wolf { diff --git a/src/feature/feature_diff_drive.cpp b/src/feature/feature_diff_drive.cpp index c33d5fdec..c2b8d9ed1 100644 --- a/src/feature/feature_diff_drive.cpp +++ b/src/feature/feature_diff_drive.cpp @@ -1,4 +1,4 @@ -#include "feature_diff_drive.h" +#include "base/feature/feature_diff_drive.h" namespace wolf { diff --git a/src/feature/feature_line_2D.cpp b/src/feature/feature_line_2D.cpp index d6c54f4e9..c69ba2c1d 100644 --- a/src/feature/feature_line_2D.cpp +++ b/src/feature/feature_line_2D.cpp @@ -1,5 +1,5 @@ -#include "feature_corner_2D.h" +#include "base/feature/feature_corner_2D.h" namespace wolf { diff --git a/src/feature/feature_odom_2D.cpp b/src/feature/feature_odom_2D.cpp index 23c28cc7b..4f37086f9 100644 --- a/src/feature/feature_odom_2D.cpp +++ b/src/feature/feature_odom_2D.cpp @@ -1,4 +1,4 @@ -#include "feature_odom_2D.h" +#include "base/feature/feature_odom_2D.h" namespace wolf { diff --git a/src/feature/feature_point_image.cpp b/src/feature/feature_point_image.cpp index 5e240fcbe..e7d3781b6 100644 --- a/src/feature/feature_point_image.cpp +++ b/src/feature/feature_point_image.cpp @@ -1,5 +1,5 @@ -#include "feature_point_image.h" +#include "base/feature/feature_point_image.h" namespace wolf { diff --git a/src/feature/feature_polyline_2D.cpp b/src/feature/feature_polyline_2D.cpp index c2a19ff5a..0f14a701c 100644 --- a/src/feature/feature_polyline_2D.cpp +++ b/src/feature/feature_polyline_2D.cpp @@ -5,7 +5,7 @@ * \author: jvallve */ -#include "feature_polyline_2D.h" +#include "base/feature/feature_polyline_2D.h" namespace wolf { diff --git a/src/feature/feature_pose.cpp b/src/feature/feature_pose.cpp index 35193d2b0..15d40e60a 100644 --- a/src/feature/feature_pose.cpp +++ b/src/feature/feature_pose.cpp @@ -1,5 +1,4 @@ -#include "feature_pose.h" - +#include "base/feature/feature_pose.h" namespace wolf { diff --git a/src/frame_base.cpp b/src/frame_base.cpp index 4f0f619fe..5c1b7fee2 100644 --- a/src/frame_base.cpp +++ b/src/frame_base.cpp @@ -1,11 +1,11 @@ -#include "frame_base.h" -#include "constraint_base.h" -#include "trajectory_base.h" -#include "capture_base.h" -#include "state_block.h" -#include "state_angle.h" -#include "state_quaternion.h" +#include "base/frame_base.h" +#include "base/constraint/constraint_base.h" +#include "base/trajectory_base.h" +#include "base/capture/capture_base.h" +#include "base/state_block.h" +#include "base/state_angle.h" +#include "base/state_quaternion.h" namespace wolf { @@ -226,8 +226,6 @@ Eigen::MatrixXs FrameBase::getCovariance() const return getProblem()->getFrameCovariance(shared_from_this()); } - - FrameBasePtr FrameBase::getPreviousFrame() const { //std::cout << "finding previous frame of " << this->frame_id_ << std::endl; @@ -386,7 +384,7 @@ FrameBasePtr FrameBase::create_POV_3D(const FrameType & _tp, } // namespace wolf -#include "factory.h" +#include "base/factory.h" namespace wolf { namespace{ const bool WOLF_UNUSED Frame_PO_2D_Registered = FrameFactory::get().registerCreator("PO 2D", FrameBase::create_PO_2D ); } diff --git a/src/hardware_base.cpp b/src/hardware_base.cpp index b2171c3cd..334900512 100644 --- a/src/hardware_base.cpp +++ b/src/hardware_base.cpp @@ -1,6 +1,5 @@ -#include "hardware_base.h" -#include "sensor_base.h" - +#include "base/hardware_base.h" +#include "base/sensor/sensor_base.h" namespace wolf { diff --git a/src/landmark/landmark_AHP.cpp b/src/landmark/landmark_AHP.cpp index 78b2bdb32..f1a683978 100644 --- a/src/landmark/landmark_AHP.cpp +++ b/src/landmark/landmark_AHP.cpp @@ -1,7 +1,7 @@ -#include "landmark_AHP.h" +#include "base/landmark/landmark_AHP.h" -#include "state_homogeneous_3D.h" -#include "factory.h" +#include "base/state_homogeneous_3D.h" +#include "base/factory.h" #include "yaml/yaml_conversion.h" namespace wolf { diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp index 830ae2940..1ae594cb3 100644 --- a/src/landmark/landmark_base.cpp +++ b/src/landmark/landmark_base.cpp @@ -1,9 +1,9 @@ -#include "landmark_base.h" -#include "constraint_base.h" -#include "map_base.h" -#include "state_block.h" -#include "yaml/yaml_conversion.h" +#include "base/landmark/landmark_base.h" +#include "base/constraint/constraint_base.h" +#include "base/map_base.h" +#include "base/state_block.h" +#include "base/yaml/yaml_conversion.h" namespace wolf { @@ -120,7 +120,6 @@ void LandmarkBase::removeStateBlocks() } } - Eigen::VectorXs LandmarkBase::getState() const { SizeEigen size = 0; @@ -153,7 +152,6 @@ void LandmarkBase::getState(Eigen::VectorXs& _state) const } } - YAML::Node LandmarkBase::saveToYaml() const { YAML::Node node; diff --git a/src/landmark/landmark_container.cpp b/src/landmark/landmark_container.cpp index a1ef7ed10..6cef90a38 100644 --- a/src/landmark/landmark_container.cpp +++ b/src/landmark/landmark_container.cpp @@ -1,6 +1,6 @@ -#include "landmark_container.h" -#include "state_block.h" +#include "base/landmark/landmark_container.h" +#include "base/state_block.h" namespace wolf { diff --git a/src/landmark/landmark_corner_2D.cpp b/src/landmark/landmark_corner_2D.cpp index 200b5a7cc..558292f90 100644 --- a/src/landmark/landmark_corner_2D.cpp +++ b/src/landmark/landmark_corner_2D.cpp @@ -1,5 +1,5 @@ -#include "landmark_corner_2D.h" +#include "base/landmark/landmark_corner_2D.h" namespace wolf { @@ -14,7 +14,6 @@ LandmarkCorner2D::~LandmarkCorner2D() // } - Scalar LandmarkCorner2D::getAperture() const { return descriptor_(0); diff --git a/src/landmark/landmark_line_2D.cpp b/src/landmark/landmark_line_2D.cpp index 80c916932..aa2a14d9f 100644 --- a/src/landmark/landmark_line_2D.cpp +++ b/src/landmark/landmark_line_2D.cpp @@ -1,5 +1,5 @@ -#include "landmark_line_2D.h" +#include "base/landmark/landmark_line_2D.h" namespace wolf { @@ -43,8 +43,6 @@ void LandmarkLine2D::updateExtremePoints(Eigen::Vector3s & _q1, Eigen::Vector3s //compute all necessary scalar products. //s_p1q1 = p1p2.dot() - - } const Eigen::Vector3s & LandmarkLine2D::point1() const diff --git a/src/landmark/landmark_point_3D.h b/src/landmark/landmark_point_3D.h index cb29852b8..7ee1c0507 100644 --- a/src/landmark/landmark_point_3D.h +++ b/src/landmark/landmark_point_3D.h @@ -1,9 +1,8 @@ #ifndef LANDMARK_POINT_3D_H #define LANDMARK_POINT_3D_H - //Wolf includes -#include "landmark_base.h" +#include "base/landmark/landmark_base.h" // Vision Utils includes #include <vision_utils/vision_utils.h> diff --git a/src/landmark/landmark_polyline_2D.cpp b/src/landmark/landmark_polyline_2D.cpp index f38680318..3e1fcf45c 100644 --- a/src/landmark/landmark_polyline_2D.cpp +++ b/src/landmark/landmark_polyline_2D.cpp @@ -5,19 +5,18 @@ * \author: jvallve */ -#include "feature_polyline_2D.h" -#include "landmark_polyline_2D.h" -#include "local_parametrization_polyline_extreme.h" -#include "constraint_point_2D.h" -#include "constraint_point_to_line_2D.h" -#include "state_block.h" -#include "factory.h" -#include "yaml/yaml_conversion.h" +#include "base/feature/feature_polyline_2D.h" +#include "base/landmark/landmark_polyline_2D.h" +#include "base/local_parametrization_polyline_extreme.h" +#include "base/constraint/constraint_point_2D.h" +#include "base/constraint/constraint_point_to_line_2D.h" +#include "base/state_block.h" +#include "base/factory.h" +#include "base/yaml/yaml_conversion.h" namespace wolf { - LandmarkPolyline2D::LandmarkPolyline2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const Eigen::MatrixXs& _points, const bool _first_extreme, const bool _last_extreme, unsigned int _first_id, LandmarkClassification _class) : LandmarkBase("POLYLINE 2D", _p_ptr, _o_ptr), first_id_(_first_id), first_defined_(_first_extreme), last_defined_(_last_extreme), closed_(false), classification_(_class) { @@ -337,7 +336,6 @@ void LandmarkPolyline2D::removeStateBlocks() LandmarkBase::removeStateBlocks(); } - // static LandmarkBasePtr LandmarkPolyline2D::create(const YAML::Node& _lmk_node) { diff --git a/src/local_parametrization_base.cpp b/src/local_parametrization_base.cpp index 7b8552f39..e2bc1cf4e 100644 --- a/src/local_parametrization_base.cpp +++ b/src/local_parametrization_base.cpp @@ -1,5 +1,4 @@ -#include "local_parametrization_base.h" - +#include "base/local_parametrization_base.h" namespace wolf { diff --git a/src/local_parametrization_homogeneous.cpp b/src/local_parametrization_homogeneous.cpp index 482178155..14abaecd1 100644 --- a/src/local_parametrization_homogeneous.cpp +++ b/src/local_parametrization_homogeneous.cpp @@ -5,9 +5,9 @@ * Author: jsola */ -#include "local_parametrization_homogeneous.h" +#include "base/local_parametrization_homogeneous.h" #include "iostream" -#include "rotations.h" // we use quaternion algebra here +#include "base/rotations.h" // we use quaternion algebra here namespace wolf { diff --git a/src/local_parametrization_polyline_extreme.cpp b/src/local_parametrization_polyline_extreme.cpp index 43f64d069..da318cafb 100644 --- a/src/local_parametrization_polyline_extreme.cpp +++ b/src/local_parametrization_polyline_extreme.cpp @@ -1,6 +1,6 @@ -#include "local_parametrization_polyline_extreme.h" -#include "state_block.h" -#include "rotations.h" +#include "base/local_parametrization_polyline_extreme.h" +#include "base/state_block.h" +#include "base/rotations.h" namespace wolf { diff --git a/src/local_parametrization_quaternion.cpp b/src/local_parametrization_quaternion.cpp index 46e9ca9f8..d2af8f544 100644 --- a/src/local_parametrization_quaternion.cpp +++ b/src/local_parametrization_quaternion.cpp @@ -1,6 +1,6 @@ -#include "local_parametrization_quaternion.h" -#include "rotations.h" +#include "base/local_parametrization_quaternion.h" +#include "base/rotations.h" #include <iostream> namespace wolf { @@ -57,7 +57,6 @@ bool LocalParametrizationQuaternion<DQ_LOCAL>::minus(Eigen::Map<const Eigen::Vec return true; } - ////////// GLOBAL PERTURBATION ////////////// template <> @@ -110,6 +109,4 @@ bool LocalParametrizationQuaternion<DQ_GLOBAL>::minus(Eigen::Map<const Eigen::Ve return true; } - - } // namespace wolf diff --git a/src/map_base.cpp b/src/map_base.cpp index ebcf2d0d8..5e8008a6a 100644 --- a/src/map_base.cpp +++ b/src/map_base.cpp @@ -1,8 +1,8 @@ // wolf -#include "map_base.h" -#include "landmark_base.h" -#include "factory.h" +#include "base/map_base.h" +#include "base/landmark/landmark_base.h" +#include "base/factory.h" // YAML #include <yaml-cpp/yaml.h> @@ -14,8 +14,6 @@ #include <iostream> #include <sstream> - - namespace wolf { MapBase::MapBase() : @@ -85,7 +83,6 @@ void MapBase::save(const std::string& _map_file_yaml, const std::string& _map_na } emitter << YAML::EndSeq << YAML::EndMap; - std::ofstream fout(_map_file_yaml); fout << emitter.c_str(); fout.close(); diff --git a/src/motion_buffer.cpp b/src/motion_buffer.cpp index dbdb539f6..20d4fc260 100644 --- a/src/motion_buffer.cpp +++ b/src/motion_buffer.cpp @@ -1,4 +1,4 @@ -#include "motion_buffer.h" +#include "base/motion_buffer.h" namespace wolf { @@ -60,7 +60,6 @@ void Motion::resize(SizeEigen _data_s, SizeEigen _delta_s, SizeEigen _delta_cov_ jacobian_calib_.resize(_delta_cov_s, _calib_s); } - //////////////////////////////////////////////////////////////////////////////////////// MotionBuffer::MotionBuffer(SizeEigen _data_size, @@ -221,6 +220,5 @@ void MotionBuffer::print(bool show_data, bool show_delta, bool show_delta_int, b } } - } diff --git a/src/node_base.cpp b/src/node_base.cpp index f6f4ceb53..24cb56c20 100644 --- a/src/node_base.cpp +++ b/src/node_base.cpp @@ -1,4 +1,4 @@ -#include "node_base.h" +#include "base/node_base.h" namespace wolf { diff --git a/src/problem.cpp b/src/problem.cpp index fec23b174..40ef35f2f 100644 --- a/src/problem.cpp +++ b/src/problem.cpp @@ -1,20 +1,20 @@ // wolf includes -#include "problem.h" -#include "hardware_base.h" -#include "trajectory_base.h" -#include "map_base.h" -#include "sensor_base.h" -#include "factory.h" -#include "sensor_factory.h" -#include "processor_factory.h" -#include "constraint_base.h" -#include "state_block.h" -#include "processor_motion.h" -#include "sensor_GPS.h" - -#include "processor_tracker.h" +#include "base/problem.h" +#include "base/hardware_base.h" +#include "base/trajectory_base.h" +#include "base/map_base.h" +#include "base/sensor/sensor_base.h" +#include "base/factory.h" +#include "base/sensor/sensor_factory.h" +#include "base/processor/processor_factory.h" +#include "base/constraint/constraint_base.h" +#include "base/state_block.h" +#include "base/processor/processor_motion.h" +#include "base/sensor/sensor_GPS.h" + +#include "base/processor/processor_tracker.h" //#include "processors/processor_tracker_feature_trifocal.h" -#include "capture_pose.h" +#include "base/capture/capture_pose.h" // IRI libs includes @@ -30,7 +30,6 @@ namespace std::string uppercase(std::string s) {for (auto & c: s) c = std::toupper(c); return s;} } - Problem::Problem(const std::string& _frame_structure) : hardware_ptr_(std::make_shared<HardwareBase>()), trajectory_ptr_(std::make_shared<TrajectoryBase>(_frame_structure)), @@ -214,7 +213,6 @@ void Problem::clearProcessorMotion() processor_motion_ptr_.reset(); } - FrameBasePtr Problem::emplaceFrame(const std::string& _frame_structure, // FrameType _frame_key_type, // const Eigen::VectorXs& _frame_state, // @@ -264,7 +262,6 @@ void Problem::getCurrentState(Eigen::VectorXs& state) state = zeroState(); } - void Problem::getCurrentStateAndStamp(Eigen::VectorXs& state, TimeStamp& ts) { assert(state.size() == getFrameStructureSize() && "Problem::getCurrentState: bad state size"); @@ -677,8 +674,6 @@ StateBlockList& Problem::getStateBlockList() return state_block_list_; } - - FrameBasePtr Problem::setPrior(const Eigen::VectorXs& _prior_state, const Eigen::MatrixXs& _prior_cov, const TimeStamp& _ts, const Scalar _time_tolerance) { if ( ! prior_is_set_ ) @@ -1401,6 +1396,4 @@ void Problem::print(const std::string& depth, bool constr_by, bool metric, bool print(0, constr_by, metric, state_blocks); } - - } // namespace wolf diff --git a/src/processor/processor_GPS.cpp b/src/processor/processor_GPS.cpp index ad30cff7e..ffb3f7d0e 100644 --- a/src/processor/processor_GPS.cpp +++ b/src/processor/processor_GPS.cpp @@ -2,11 +2,11 @@ // Created by ptirindelli on 16/12/15. // -#include "constraint_GPS_pseudorange_2D.h" -#include "feature_GPS_pseudorange.h" -#include "processor_GPS.h" +#include "base/constraint/constraint_GPS_pseudorange_2D.h" +#include "base/feature/feature_GPS_pseudorange.h" +#include "base/processor/processor_GPS.h" -#include "capture_GPS.h" +#include "base/capture/capture_GPS.h" namespace wolf { @@ -68,7 +68,6 @@ ProcessorBasePtr ProcessorGPS::create(const std::string& _unique_name, const Pro } // namespace wolf - // Register in the SensorFactory #include "processor_factory.h" namespace wolf { diff --git a/src/processor/processor_IMU.cpp b/src/processor/processor_IMU.cpp index e69126258..568b61a78 100644 --- a/src/processor/processor_IMU.cpp +++ b/src/processor/processor_IMU.cpp @@ -1,7 +1,7 @@ // wolf -#include "processor_IMU.h" -#include "constraint_IMU.h" -#include "IMU_tools.h" +#include "base/processor/processor_IMU.h" +#include "base/constraint/constraint_IMU.h" +#include "base/IMU_tools.h" namespace wolf { @@ -329,9 +329,8 @@ void ProcessorIMU::deltaPlusDelta(const Eigen::VectorXs& _delta_preint, } // namespace wolf - // Register in the SensorFactory -#include "processor_factory.h" +#include "base/processor/processor_factory.h" namespace wolf { diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp index ea4bdc4e8..aa259d706 100644 --- a/src/processor/processor_base.cpp +++ b/src/processor/processor_base.cpp @@ -1,7 +1,7 @@ -#include "processor_base.h" -#include "processor_motion.h" -#include "capture_base.h" -#include "frame_base.h" +#include "base/processor/processor_base.h" +#include "base/processor/processor_motion.h" +#include "base/capture/capture_base.h" +#include "base/frame_base.h" namespace wolf { @@ -73,8 +73,6 @@ void ProcessorBase::remove() } } - - ///////////////////////////////////////////////////////////////////////////////////////// void PackKeyFrameBuffer::removeUpTo(const TimeStamp& _time_stamp) @@ -186,6 +184,4 @@ bool PackKeyFrameBuffer::checkTimeTolerance(const TimeStamp& _time_stamp1, const return pass; } - - } // namespace wolf diff --git a/src/processor/processor_capture_holder.cpp b/src/processor/processor_capture_holder.cpp index 10ded0c0b..58e3550ab 100644 --- a/src/processor/processor_capture_holder.cpp +++ b/src/processor/processor_capture_holder.cpp @@ -6,7 +6,7 @@ */ //Wolf includes -#include "processor_capture_holder.h" +#include "base/processor/processor_capture_holder.h" namespace wolf { @@ -121,7 +121,6 @@ CaptureBasePtr ProcessorCaptureHolder::findCaptureContainingTimeStamp(const Time // return capt; - return buffer_.getCapture(_ts); } @@ -146,7 +145,7 @@ ProcessorBasePtr ProcessorCaptureHolder::create(const std::string& _unique_name, } // namespace wolf // Register in the ProcessorFactory -#include "processor_factory.h" +#include "base/processor/processor_factory.h" namespace wolf { WOLF_REGISTER_PROCESSOR("CAPTURE HOLDER", ProcessorCaptureHolder) } // namespace wolf diff --git a/src/processor/processor_diff_drive.cpp b/src/processor/processor_diff_drive.cpp index dba867cc3..423e429db 100644 --- a/src/processor/processor_diff_drive.cpp +++ b/src/processor/processor_diff_drive.cpp @@ -1,13 +1,13 @@ -#include "processor_diff_drive.h" +#include "base/processor/processor_diff_drive.h" -#include "../sensors/sensor_diff_drive.h" +#include "base/sensor/sensor_diff_drive.h" -#include "../captures/capture_wheel_joint_position.h" -#include "../captures/capture_velocity.h" +#include "base/capture/capture_wheel_joint_position.h" +#include "base/capture/capture_velocity.h" -#include "../rotations.h" -#include "../constraint_odom_2D.h" -#include "../features/feature_diff_drive.h" +#include "base/rotations.h" +#include "base/constraint/constraint_odom_2D.h" +#include "base/feature/feature_diff_drive.h" namespace wolf { @@ -259,7 +259,7 @@ ProcessorBasePtr ProcessorDiffDrive::create(const std::string& _unique_name, } // namespace wolf // Register in the ProcessorFactory -#include "processor_factory.h" +#include "base/processor/processor_factory.h" namespace wolf { WOLF_REGISTER_PROCESSOR("DIFF DRIVE", ProcessorDiffDrive) } // namespace wolf diff --git a/src/processor/processor_frame_nearest_neighbor_filter.cpp b/src/processor/processor_frame_nearest_neighbor_filter.cpp index f705e368f..4901170c2 100644 --- a/src/processor/processor_frame_nearest_neighbor_filter.cpp +++ b/src/processor/processor_frame_nearest_neighbor_filter.cpp @@ -1,4 +1,4 @@ -#include "processor_frame_nearest_neighbor_filter.h" +#include "base/processor/processor_frame_nearest_neighbor_filter.h" namespace wolf { diff --git a/src/processor/processor_loopclosure_base.cpp b/src/processor/processor_loopclosure_base.cpp index 160185238..6953cd103 100644 --- a/src/processor/processor_loopclosure_base.cpp +++ b/src/processor/processor_loopclosure_base.cpp @@ -5,7 +5,7 @@ * \author: Tessa Johanna */ -#include "processor_loopclosure_base.h" +#include "base/processor/processor_loopclosure_base.h" namespace wolf { diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp index 0abab2143..0fe59c59a 100644 --- a/src/processor/processor_motion.cpp +++ b/src/processor/processor_motion.cpp @@ -1,4 +1,4 @@ -#include "processor_motion.h" +#include "base/processor/processor_motion.h" namespace wolf { @@ -88,7 +88,6 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr) // and give the part of the buffer before the new keyframe to the capture for the KF callback existing_capture->getBuffer().split(ts_from_callback, capture_for_keyframe_callback->getBuffer()); - // interpolate individual delta if (!existing_capture->getBuffer().get().empty() && capture_for_keyframe_callback->getBuffer().get().back().ts_ != ts_from_callback) { @@ -100,7 +99,6 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr) capture_for_keyframe_callback->getBuffer().get().push_back(motion_interpolated); } - // create motion feature and add it to the capture auto new_feature = emplaceFeature(capture_for_keyframe_callback); @@ -131,7 +129,6 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr) break; } - case RUNNING_WITH_PACK_AFTER_ORIGIN : { // extract pack elements @@ -185,8 +182,6 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr) break; } - - default : break; } @@ -256,7 +251,6 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr) postProcess(); } - bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXs& _x) { CaptureMotionPtr capture_motion; diff --git a/src/processor/processor_odom_2D.cpp b/src/processor/processor_odom_2D.cpp index 19818dffc..c375d52fa 100644 --- a/src/processor/processor_odom_2D.cpp +++ b/src/processor/processor_odom_2D.cpp @@ -1,4 +1,4 @@ -#include "processor_odom_2D.h" +#include "base/processor/processor_odom_2D.h" namespace wolf { @@ -184,13 +184,10 @@ ProcessorBasePtr ProcessorOdom2D::create(const std::string& _unique_name, const return prc_ptr; } - - } - // Register in the ProcessorFactory -#include "processor_factory.h" +#include "base/processor/processor_factory.h" namespace wolf { WOLF_REGISTER_PROCESSOR("ODOM 2D", ProcessorOdom2D) } // namespace wolf diff --git a/src/processor/processor_odom_3D.cpp b/src/processor/processor_odom_3D.cpp index 49da6be81..4f6d75911 100644 --- a/src/processor/processor_odom_3D.cpp +++ b/src/processor/processor_odom_3D.cpp @@ -1,4 +1,4 @@ -#include "processor_odom_3D.h" +#include "base/processor/processor_odom_3D.h" namespace wolf { @@ -15,7 +15,6 @@ ProcessorOdom3D::ProcessorOdom3D(ProcessorParamsOdom3DPtr _params, SensorOdom3DP jacobian_delta_.setZero(delta_cov_size_, delta_cov_size_); } - ProcessorOdom3D::~ProcessorOdom3D() { } @@ -200,7 +199,6 @@ Motion ProcessorOdom3D::interpolate(const Motion& _motion_ref, Motion& _motion_s // assert(_motion_second.delta_integr_cov_.cols() == delta_cov_size_ && "Wrong delta cov size"); // assert(_motion_second.delta_integr_cov_.rows() == delta_cov_size_ && "Wrong delta cov size"); - using namespace Eigen; // Interpolate between motion_ref and motion, as in: // @@ -347,9 +345,8 @@ void ProcessorOdom3D::remap(const Eigen::VectorXs& _x1, const Eigen::VectorXs& _ } - // Register in the SensorFactory -#include "processor_factory.h" +#include "base/processor/processor_factory.h" namespace wolf { WOLF_REGISTER_PROCESSOR("ODOM 3D", ProcessorOdom3D) } // namespace wolf diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp index 630a856e7..f8138e1fc 100644 --- a/src/processor/processor_tracker.cpp +++ b/src/processor/processor_tracker.cpp @@ -6,7 +6,7 @@ */ // wolf -#include "processor_tracker.h" +#include "base/processor/processor_tracker.h" // std #include <cmath> @@ -233,7 +233,6 @@ void ProcessorTracker::computeProcessingStep() else step = RUNNING; - // Then combine with the existence (or not) of a keyframe callback pack switch (step) { @@ -277,8 +276,5 @@ void ProcessorTracker::computeProcessingStep() } } - - - } // namespace wolf diff --git a/src/processor/processor_tracker_feature.cpp b/src/processor/processor_tracker_feature.cpp index 0ce945f30..cfb6aa984 100644 --- a/src/processor/processor_tracker_feature.cpp +++ b/src/processor/processor_tracker_feature.cpp @@ -5,7 +5,7 @@ * \author: jsola */ -#include "processor_tracker_feature.h" +#include "base/processor/processor_tracker_feature.h" namespace wolf { diff --git a/src/processor/processor_tracker_feature_corner.cpp b/src/processor/processor_tracker_feature_corner.cpp index 387333fd7..0a8e7c7c2 100644 --- a/src/processor/processor_tracker_feature_corner.cpp +++ b/src/processor/processor_tracker_feature_corner.cpp @@ -5,8 +5,8 @@ * \author: jvallve */ -#include "processor_tracker_feature_corner.h" -#include "feature_corner_2D.h" +#include "base/processor/processor_tracker_feature_corner.h" +#include "base/feature/feature_corner_2D.h" namespace wolf { diff --git a/src/processor/processor_tracker_feature_dummy.cpp b/src/processor/processor_tracker_feature_dummy.cpp index dc81126f9..6ce3d4fe1 100644 --- a/src/processor/processor_tracker_feature_dummy.cpp +++ b/src/processor/processor_tracker_feature_dummy.cpp @@ -5,8 +5,8 @@ * \author: jvallve */ -#include "processor_tracker_feature_dummy.h" -#include "feature_base.h" +#include "base/processor/processor_tracker_feature_dummy.h" +#include "base/feature/feature_base.h" namespace wolf { diff --git a/src/processor/processor_tracker_feature_image.cpp b/src/processor/processor_tracker_feature_image.cpp index 1a860c217..6ec2b8aab 100644 --- a/src/processor/processor_tracker_feature_image.cpp +++ b/src/processor/processor_tracker_feature_image.cpp @@ -1,5 +1,5 @@ // Wolf includes -#include "processor_tracker_feature_image.h" +#include "base/processor/processor_tracker_feature_image.h" // Vision utils #include <detectors.h> @@ -382,7 +382,6 @@ void ProcessorTrackerFeatureImage::drawFeatures(cv::Mat _image) } } - ProcessorBasePtr ProcessorTrackerFeatureImage::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr _sensor_ptr) { ProcessorTrackerFeatureImagePtr prc_ptr = std::make_shared<ProcessorTrackerFeatureImage>(std::static_pointer_cast<ProcessorParamsTrackerFeatureImage>(_params)); @@ -390,10 +389,8 @@ ProcessorBasePtr ProcessorTrackerFeatureImage::create(const std::string& _unique return prc_ptr; } - } // namespace wolf - // Register in the SensorFactory #include "processor_factory.h" namespace wolf { diff --git a/src/processor/processor_tracker_feature_trifocal.cpp b/src/processor/processor_tracker_feature_trifocal.cpp index 7ffb99375..5557e10d6 100644 --- a/src/processor/processor_tracker_feature_trifocal.cpp +++ b/src/processor/processor_tracker_feature_trifocal.cpp @@ -1,11 +1,11 @@ // wolf -#include "processor_tracker_feature_trifocal.h" +#include "processor/processor_tracker_feature_trifocal.h" -#include "sensor_camera.h" -#include "feature_point_image.h" +#include "base/sensor/sensor_camera.h" +#include "base/feature/feature_point_image.h" #include "constraints/constraint_autodiff_trifocal.h" -#include "capture_image.h" +#include "base/capture/capture_image.h" // vision_utils #include <vision_utils.h> @@ -18,7 +18,6 @@ namespace wolf { - //// DEBUG ===================================== //debug_tTmp = clock(); //WOLF_TRACE("======== DetectNewFeatures ========="); @@ -29,7 +28,6 @@ namespace wolf { //WOLF_TRACE("--> TIME: Detect New Features: detect ",debug_comp_time_); //// =========================================== - // Constructor ProcessorTrackerFeatureTrifocal::ProcessorTrackerFeatureTrifocal(ProcessorParamsTrackerFeatureTrifocalPtr _params_tracker_feature_trifocal) : ProcessorTrackerFeature("TRACKER FEATURE TRIFOCAL", _params_tracker_feature_trifocal ), diff --git a/src/processor/processor_tracker_feature_trifocal.h b/src/processor/processor_tracker_feature_trifocal.h index 9065c718c..dca3b791a 100644 --- a/src/processor/processor_tracker_feature_trifocal.h +++ b/src/processor/processor_tracker_feature_trifocal.h @@ -2,8 +2,8 @@ #define _PROCESSOR_TRACKER_FEATURE_TRIFOCAL_H_ //Wolf includes -#include "../processor_tracker_feature.h" -#include "../capture_image.h" +#include "base/processor/processor_tracker_feature.h" +#include "base/capture/capture_image.h" // Vision utils #include <vision_utils.h> @@ -104,7 +104,6 @@ class ProcessorTrackerFeatureTrifocal : public ProcessorTrackerFeature */ virtual ConstraintBasePtr createConstraint(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr) override; - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** \brief advance pointers diff --git a/src/processor/processor_tracker_landmark.cpp b/src/processor/processor_tracker_landmark.cpp index 8e3f5d0d9..08bbf8c52 100644 --- a/src/processor/processor_tracker_landmark.cpp +++ b/src/processor/processor_tracker_landmark.cpp @@ -5,12 +5,11 @@ * \author: jvallve */ -#include "processor_tracker_landmark.h" -#include "map_base.h" +#include "base/processor/processor_tracker_landmark.h" +#include "base/map_base.h" #include <utility> - namespace wolf { diff --git a/src/processor/processor_tracker_landmark_corner.cpp b/src/processor/processor_tracker_landmark_corner.cpp index e687e5fcd..522034204 100644 --- a/src/processor/processor_tracker_landmark_corner.cpp +++ b/src/processor/processor_tracker_landmark_corner.cpp @@ -1,5 +1,5 @@ -#include "processor_tracker_landmark_corner.h" -#include "rotations.h" +#include "base/processor/processor_tracker_landmark_corner.h" +#include "base/rotations.h" namespace wolf { @@ -422,7 +422,7 @@ ProcessorTrackerLandmarkCorner::ProcessorTrackerLandmarkCorner(ProcessorParamsLa } //namespace wolf // Register in the SensorFactory -#include "processor_factory.h" +#include "base/processor/processor_factory.h" namespace wolf { WOLF_REGISTER_PROCESSOR("TRACKER LANDMARK CORNER", ProcessorTrackerLandmarkCorner) } // namespace wolf diff --git a/src/processor/processor_tracker_landmark_dummy.cpp b/src/processor/processor_tracker_landmark_dummy.cpp index e806c23e5..06c10b37f 100644 --- a/src/processor/processor_tracker_landmark_dummy.cpp +++ b/src/processor/processor_tracker_landmark_dummy.cpp @@ -5,9 +5,9 @@ * \author: jvallve */ -#include "processor_tracker_landmark_dummy.h" -#include "landmark_corner_2D.h" -#include "constraint_corner_2D.h" +#include "base/processor/processor_tracker_landmark_dummy.h" +#include "base/landmark/landmark_corner_2D.h" +#include "base/constraint/constraint_corner_2D.h" namespace wolf { diff --git a/src/processor/processor_tracker_landmark_image.cpp b/src/processor/processor_tracker_landmark_image.cpp index da3f1c7b4..04bba415f 100644 --- a/src/processor/processor_tracker_landmark_image.cpp +++ b/src/processor/processor_tracker_landmark_image.cpp @@ -1,17 +1,17 @@ -#include "processor_tracker_landmark_image.h" - -#include "capture_image.h" -#include "constraint_AHP.h" -#include "feature_base.h" -#include "feature_point_image.h" -#include "frame_base.h" -#include "logging.h" -#include "map_base.h" -#include "pinhole_tools.h" -#include "problem.h" -#include "sensor_camera.h" -#include "state_block.h" -#include "time_stamp.h" +#include "base/processor/processor_tracker_landmark_image.h" + +#include "base/capture/capture_image.h" +#include "base/constraint/constraint_AHP.h" +#include "base/feature/feature_base.h" +#include "base/feature/feature_point_image.h" +#include "base/frame_base.h" +#include "base/logging.h" +#include "base/map_base.h" +#include "base/pinhole_tools.h" +#include "base/problem.h" +#include "base/sensor/sensor_camera.h" +#include "base/state_block.h" +#include "base/time_stamp.h" // vision_utils #include <detectors.h> @@ -196,7 +196,6 @@ unsigned int ProcessorTrackerLandmarkImage::findLandmarks(const LandmarkBaseList _features_incoming_out.push_back(incoming_point_ptr); - _feature_landmark_correspondences[incoming_point_ptr] = std::make_shared<LandmarkMatch>(landmark_in_ptr, normalized_score); feat_lmk_found_.push_back(incoming_point_ptr); @@ -302,7 +301,6 @@ LandmarkBasePtr ProcessorTrackerLandmarkImage::createLandmark(FeatureBasePtr _fe point3D.normalize(); - vec_homogeneous = {point3D(0),point3D(1),point3D(2),1/distance}; LandmarkAHPPtr lmk_ahp_ptr = std::make_shared<LandmarkAHP>(vec_homogeneous, anchor_frame, getSensorPtr(), feat_point_image_ptr->getDescriptor()); @@ -330,7 +328,6 @@ ConstraintBasePtr ProcessorTrackerLandmarkImage::createConstraint(FeatureBasePtr } } - // ==================================================================== My own functions void ProcessorTrackerLandmarkImage::landmarkInCurrentCamera(const Eigen::VectorXs& _current_state, @@ -361,7 +358,6 @@ void ProcessorTrackerLandmarkImage::landmarkInCurrentCamera(const Eigen::VectorX * We use Eigen::Transform which is like using homogeneous transform matrices with a simpler API */ - // Assert frame is 3D with at least PQ assert((_current_state.size() == 7 || _current_state.size() == 16) && "Wrong state size! Should be 7 for 3D pose or 16 for IMU."); @@ -493,7 +489,6 @@ void ProcessorTrackerLandmarkImage::drawLandmarks(cv::Mat _image) } } - //namespace wolf{ ProcessorBasePtr ProcessorTrackerLandmarkImage::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr _sen_ptr) @@ -505,7 +500,6 @@ ProcessorBasePtr ProcessorTrackerLandmarkImage::create(const std::string& _uniqu } // namespace wolf - // Register in the SensorFactory #include "processor_factory.h" namespace wolf { diff --git a/src/processor/processor_tracker_landmark_polyline.cpp b/src/processor/processor_tracker_landmark_polyline.cpp index 0475ab31a..70efbcde6 100644 --- a/src/processor/processor_tracker_landmark_polyline.cpp +++ b/src/processor/processor_tracker_landmark_polyline.cpp @@ -1,4 +1,4 @@ -#include "processor_tracker_landmark_polyline.h" +#include "base/processor/processor_tracker_landmark_polyline.h" namespace wolf { @@ -268,7 +268,6 @@ unsigned int ProcessorTrackerLandmarkPolyline::findLandmarks(const LandmarkBaseL //if (!polyline_feature->isFirstDefined() && !polyline_landmark->isLastDefined()) // std::cout << "\tLIMITED min offset " << min_offset << std::endl; - } // Match found for this feature if (best_match != nullptr) @@ -444,7 +443,6 @@ Scalar ProcessorTrackerLandmarkPolyline::sqDistPointToLine(const Eigen::Vector3s * Returns the distance B-Line if the projection of B to the line is in [A, inf). Otherwise, return squared distance A-B. * Checks if the angle BAAaux is >= 90º: (BAaux)² >= (BA)² + (AAaux)² * - * * ( Case B and A are defined is not point-to-line, is point to point -> assertion ) * */ @@ -510,7 +508,6 @@ LandmarkBasePtr ProcessorTrackerLandmarkPolyline::createLandmark(FeatureBasePtr //std::cout << "Robot global pose: " << t_world_robot_.transpose() << std::endl; //std::cout << "Sensor global pose: " << t_world_sensor_.transpose() << std::endl; - FeaturePolyline2DPtr polyline_ptr = std::static_pointer_cast<FeaturePolyline2D>(_feature_ptr); // compute feature global pose Eigen::MatrixXs points_global = R_world_sensor_ * polyline_ptr->getPoints().topRows<2>() + @@ -1017,7 +1014,7 @@ ProcessorBasePtr ProcessorTrackerLandmarkPolyline::create(const std::string& _un } //namespace wolf // Register in the SensorFactory -#include "processor_factory.h" +#include "base/processor/processor_factory.h" namespace wolf { WOLF_REGISTER_PROCESSOR("POLYLINE", ProcessorTrackerLandmarkPolyline) } // namespace wolf diff --git a/src/sensor/sensor_GPS.cpp b/src/sensor/sensor_GPS.cpp index faeb7d112..7745e65b1 100644 --- a/src/sensor/sensor_GPS.cpp +++ b/src/sensor/sensor_GPS.cpp @@ -1,7 +1,7 @@ -#include "sensor_GPS.h" -#include "state_block.h" -#include "state_quaternion.h" +#include "base/sensor/sensor_GPS.h" +#include "base/state_block.h" +#include "base/state_quaternion.h" namespace wolf { @@ -34,7 +34,6 @@ StateBlockPtr SensorGPS::getMapOPtr() const return getStateBlockPtrStatic(4); } - // Define the factory method SensorBasePtr SensorGPS::create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_p, const IntrinsicsBasePtr _intrinsics) @@ -50,9 +49,8 @@ SensorBasePtr SensorGPS::create(const std::string& _unique_name, const Eigen::Ve } // namespace wolf - // Register in the SensorFactory -#include "sensor_factory.h" +#include "base/sensor/sensor_factory.h" namespace wolf { WOLF_REGISTER_SENSOR("GPS",SensorGPS) } // namespace wolf diff --git a/src/sensor/sensor_GPS_fix.cpp b/src/sensor/sensor_GPS_fix.cpp index fa1f46eb1..788dc06dc 100644 --- a/src/sensor/sensor_GPS_fix.cpp +++ b/src/sensor/sensor_GPS_fix.cpp @@ -1,6 +1,6 @@ -#include "sensor_GPS_fix.h" -#include "state_block.h" -#include "state_quaternion.h" +#include "base/sensor/sensor_GPS_fix.h" +#include "base/state_block.h" +#include "base/state_quaternion.h" namespace wolf { @@ -22,7 +22,6 @@ SensorGPSFix::SensorGPSFix(const Eigen::VectorXs & _extrinsics, IntrinsicsGPSFix // } - SensorGPSFix::~SensorGPSFix() { // @@ -42,9 +41,8 @@ SensorBasePtr SensorGPSFix::create(const std::string& _unique_name, const Eigen: } // namespace wolf - // Register in the SensorFactory -#include "sensor_factory.h" +#include "base/sensor/sensor_factory.h" namespace wolf { WOLF_REGISTER_SENSOR("GPS FIX", SensorGPSFix) } // namespace wolf diff --git a/src/sensor/sensor_IMU.cpp b/src/sensor/sensor_IMU.cpp index 9e5da5f0d..94a30bb43 100644 --- a/src/sensor/sensor_IMU.cpp +++ b/src/sensor/sensor_IMU.cpp @@ -1,6 +1,6 @@ -#include "sensor_IMU.h" -#include "state_block.h" -#include "state_quaternion.h" +#include "base/sensor/sensor_IMU.h" +#include "base/state_block.h" +#include "base/state_quaternion.h" namespace wolf { @@ -40,7 +40,6 @@ SensorIMU::SensorIMU(const Eigen::VectorXs& _extrinsics, const IntrinsicsIMU& _p assert(_extrinsics.size() == 7 && "Wrong extrinsics vector size! Should be 7 for 2D."); } - SensorIMU::~SensorIMU() { // @@ -61,9 +60,8 @@ SensorBasePtr SensorIMU::create(const std::string& _unique_name, const Eigen::Ve } // namespace wolf - // Register in the SensorFactory -#include "sensor_factory.h" +#include "base/sensor/sensor_factory.h" namespace wolf { WOLF_REGISTER_SENSOR("IMU", SensorIMU) } // namespace wolf diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp index 429aaf9f1..a05fec1ce 100644 --- a/src/sensor/sensor_base.cpp +++ b/src/sensor/sensor_base.cpp @@ -1,6 +1,5 @@ -#include "sensor_base.h" -#include "state_block.h" - +#include "base/sensor/sensor_base.h" +#include "base/state_block.h" namespace wolf { @@ -103,8 +102,6 @@ void SensorBase::removeStateBlocks() } } - - void SensorBase::fix() { for( auto sbp : state_block_vec_) @@ -165,8 +162,6 @@ void SensorBase::unfixIntrinsics() updateCalibSize(); } - - void SensorBase::registerNewStateBlocks() { if (getProblem() != nullptr) @@ -283,7 +278,6 @@ SizeEigen SensorBase::computeCalibSize() const return sz; } - Eigen::VectorXs SensorBase::getCalibration() const { SizeEigen index = 0; @@ -301,7 +295,6 @@ Eigen::VectorXs SensorBase::getCalibration() const return calib; } - bool SensorBase::process(const CaptureBasePtr capture_ptr) { capture_ptr->setSensorPtr(shared_from_this()); diff --git a/src/sensor/sensor_camera.cpp b/src/sensor/sensor_camera.cpp index 7a9a9a2c7..3a3dae9bd 100644 --- a/src/sensor/sensor_camera.cpp +++ b/src/sensor/sensor_camera.cpp @@ -1,8 +1,8 @@ -#include "sensor_camera.h" +#include "base/sensor/sensor_camera.h" -#include "pinhole_tools.h" -#include "state_block.h" -#include "state_quaternion.h" +#include "base/pinhole_tools.h" +#include "base/state_block.h" +#include "base/state_quaternion.h" namespace wolf { @@ -51,7 +51,6 @@ Eigen::Matrix3s SensorCamera::setIntrinsicMatrix(Eigen::Vector4s _pinhole_model) return K; } - // Define the factory method SensorBasePtr SensorCamera::create(const std::string& _unique_name, // const Eigen::VectorXs& _extrinsics_pq, // @@ -66,12 +65,11 @@ SensorBasePtr SensorCamera::create(const std::string& _unique_name, // return sen_ptr; } - } // namespace wolf // Register in the SensorFactory -#include "sensor_factory.h" -//#include "factory.h" +#include "base/sensor/sensor_factory.h" +//#include "base/factory.h" namespace wolf { WOLF_REGISTER_SENSOR("CAMERA", SensorCamera) diff --git a/src/sensor/sensor_diff_drive.cpp b/src/sensor/sensor_diff_drive.cpp index 4c57144c9..d710d9e77 100644 --- a/src/sensor/sensor_diff_drive.cpp +++ b/src/sensor/sensor_diff_drive.cpp @@ -1,7 +1,7 @@ -#include "sensor_diff_drive.h" -#include "../state_block.h" -#include "../capture_motion.h" -#include "../eigen_assert.h" +#include "base/sensor/sensor_diff_drive.h" +#include "base/state_block.h" +#include "base/capture/capture_motion.h" +#include "base/eigen_assert.h" namespace wolf { @@ -46,7 +46,6 @@ SensorBasePtr SensorDiffDrive::create(const std::string& _unique_name, return odo; } - /// @todo Further work to enforce wheel model //const IntrinsicsDiffDrive& SensorDiffDrive::getIntrinsics() @@ -127,9 +126,8 @@ SensorBasePtr SensorDiffDrive::create(const std::string& _unique_name, } // namespace wolf - // Register in the SensorFactory -#include "sensor_factory.h" +#include "base/sensor/sensor_factory.h" namespace wolf { WOLF_REGISTER_SENSOR("DIFF DRIVE", SensorDiffDrive) } // namespace wolf diff --git a/src/sensor/sensor_laser_2D.cpp b/src/sensor/sensor_laser_2D.cpp index 227734568..a7a5677af 100644 --- a/src/sensor/sensor_laser_2D.cpp +++ b/src/sensor/sensor_laser_2D.cpp @@ -1,5 +1,5 @@ -#include "sensor_laser_2D.h" -#include "state_block.h" +#include "base/sensor/sensor_laser_2D.h" +#include "base/state_block.h" namespace wolf { @@ -79,14 +79,10 @@ SensorBasePtr SensorLaser2D::create(const std::string& _unique_name, const Eigen return sen; } - } // namespace wolf - - - // Register in the SensorFactory and the ParameterFactory -#include "sensor_factory.h" +#include "base/sensor/sensor_factory.h" //#include "intrinsics_factory.h" namespace wolf { WOLF_REGISTER_SENSOR("LASER 2D", SensorLaser2D) diff --git a/src/sensor/sensor_odom_2D.cpp b/src/sensor/sensor_odom_2D.cpp index fdb7ab480..7f0abaed4 100644 --- a/src/sensor/sensor_odom_2D.cpp +++ b/src/sensor/sensor_odom_2D.cpp @@ -1,6 +1,6 @@ -#include "sensor_odom_2D.h" -#include "state_block.h" -#include "state_angle.h" +#include "base/sensor/sensor_odom_2D.h" +#include "base/state_block.h" +#include "base/state_angle.h" namespace wolf { @@ -27,7 +27,6 @@ SensorOdom2D::SensorOdom2D(Eigen::VectorXs _extrinsics, IntrinsicsOdom2DPtr _int // } - SensorOdom2D::~SensorOdom2D() { // @@ -69,9 +68,8 @@ SensorBasePtr SensorOdom2D::create(const std::string& _unique_name, const Eigen: } // namespace wolf - // Register in the SensorFactory -#include "sensor_factory.h" +#include "base/sensor/sensor_factory.h" namespace wolf { WOLF_REGISTER_SENSOR("ODOM 2D", SensorOdom2D) } // namespace wolf diff --git a/src/sensor/sensor_odom_3D.cpp b/src/sensor/sensor_odom_3D.cpp index c5f3fb388..aa6efc42b 100644 --- a/src/sensor/sensor_odom_3D.cpp +++ b/src/sensor/sensor_odom_3D.cpp @@ -5,10 +5,10 @@ * \author: jsola */ -#include "sensor_odom_3D.h" +#include "base/sensor/sensor_odom_3D.h" -#include "state_block.h" -#include "state_quaternion.h" +#include "base/state_block.h" +#include "base/state_quaternion.h" namespace wolf { @@ -44,7 +44,6 @@ SensorOdom3D::SensorOdom3D(const Eigen::VectorXs& _extrinsics_pq, IntrinsicsOdom // } - SensorOdom3D::~SensorOdom3D() { // @@ -69,9 +68,8 @@ SensorBasePtr SensorOdom3D::create(const std::string& _unique_name, const Eigen: } // namespace wolf - // Register in the SensorFactory -#include "sensor_factory.h" +#include "base/sensor/sensor_factory.h" namespace wolf { WOLF_REGISTER_SENSOR("ODOM 3D", SensorOdom3D) } diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp index b6a2e6a40..f85416468 100644 --- a/src/solver/solver_manager.cpp +++ b/src/solver/solver_manager.cpp @@ -1,7 +1,7 @@ -#include "solver_manager.h" -#include "../trajectory_base.h" -#include "../map_base.h" -#include "../landmark_base.h" +#include "base/solver/solver_manager.h" +#include "base/trajectory_base.h" +#include "base/map_base.h" +#include "base/landmark/landmark_base.h" namespace wolf { diff --git a/src/solver_suitesparse/solver_manager.cpp b/src/solver_suitesparse/solver_manager.cpp index cb39335b0..a25c72bef 100644 --- a/src/solver_suitesparse/solver_manager.cpp +++ b/src/solver_suitesparse/solver_manager.cpp @@ -1,4 +1,4 @@ -#include "ceres_manager.h" +#include "base/ceres_wrapper/ceres_manager.h" SolverManager::SolverManager() { diff --git a/src/state_block.cpp b/src/state_block.cpp index 2599ea21c..46411e7c3 100644 --- a/src/state_block.cpp +++ b/src/state_block.cpp @@ -1,4 +1,4 @@ -#include "state_block.h" +#include "base/state_block.h" namespace wolf { diff --git a/src/time_stamp.cpp b/src/time_stamp.cpp index 543ceb2ca..55d25fc6e 100644 --- a/src/time_stamp.cpp +++ b/src/time_stamp.cpp @@ -1,5 +1,5 @@ -#include "time_stamp.h" +#include "base/time_stamp.h" namespace wolf { @@ -10,8 +10,6 @@ std::ostream& operator<<(std::ostream& os, const TimeStamp& _ts) return os; } - - TimeStamp::TimeStamp() : //time_stamp_(0) time_stamp_nano_(0) diff --git a/src/track_matrix.cpp b/src/track_matrix.cpp index e51b38614..8bb0e0abe 100644 --- a/src/track_matrix.cpp +++ b/src/track_matrix.cpp @@ -5,7 +5,7 @@ * \author: jsola */ -#include "track_matrix.h" +#include "base/track_matrix.h" namespace wolf { diff --git a/src/trajectory_base.cpp b/src/trajectory_base.cpp index 3dcb41267..557d072fd 100644 --- a/src/trajectory_base.cpp +++ b/src/trajectory_base.cpp @@ -1,6 +1,5 @@ -#include "trajectory_base.h" -#include "frame_base.h" - +#include "base/trajectory_base.h" +#include "base/frame_base.h" namespace wolf { @@ -39,7 +38,6 @@ FrameBasePtr TrajectoryBase::addFrame(FrameBasePtr _frame_ptr) return _frame_ptr; } - void TrajectoryBase::getConstraintList(ConstraintBaseList & _ctr_list) { for(auto fr_ptr : getFrameList()) diff --git a/src/yaml/processor_IMU_yaml.cpp b/src/yaml/processor_IMU_yaml.cpp index 0e6587e29..38b83b82f 100644 --- a/src/yaml/processor_IMU_yaml.cpp +++ b/src/yaml/processor_IMU_yaml.cpp @@ -6,11 +6,11 @@ */ // wolf yaml -#include "processor_IMU.h" -#include "yaml_conversion.h" +#include "base/processor/processor_IMU.h" +#include "base/yaml/yaml_conversion.h" // wolf -#include "../factory.h" +#include "base/factory.h" // yaml-cpp library #include <yaml-cpp/yaml.h> diff --git a/src/yaml/processor_image_yaml.cpp b/src/yaml/processor_image_yaml.cpp index 7de4b1587..051174ef5 100644 --- a/src/yaml/processor_image_yaml.cpp +++ b/src/yaml/processor_image_yaml.cpp @@ -6,15 +6,15 @@ */ // wolf yaml -#include "yaml_conversion.h" +#include "base/yaml/yaml_conversion.h" // wolf -#include "../factory.h" +#include "base/factory.h" // yaml-cpp library #include <yaml-cpp/yaml.h> -#include "../processor_params_image.h" +#include "base/processor/processor_params_image.h" namespace wolf { @@ -64,7 +64,5 @@ static ProcessorParamsBasePtr createProcessorParamsImage(const std::string & _fi const bool WOLF_UNUSED registered_prc_image_feature_par = ProcessorParamsFactory::get().registerCreator("IMAGE FEATURE", createProcessorParamsImage); const bool WOLF_UNUSED registered_prc_image_landmark_par = ProcessorParamsFactory::get().registerCreator("IMAGE LANDMARK", createProcessorParamsImage); - -} } diff --git a/src/yaml/processor_odom_3D_yaml.cpp b/src/yaml/processor_odom_3D_yaml.cpp index 1b5a3f40d..af8934b07 100644 --- a/src/yaml/processor_odom_3D_yaml.cpp +++ b/src/yaml/processor_odom_3D_yaml.cpp @@ -6,11 +6,11 @@ */ // wolf yaml -#include "yaml_conversion.h" +#include "base/yaml/yaml_conversion.h" // wolf -#include "../processor_odom_3D.h" -#include "../factory.h" +#include "base/processor/processor_odom_3D.h" +#include "base/factory.h" // yaml-cpp library #include <yaml-cpp/yaml.h> diff --git a/src/yaml/processor_tracker_feature_trifocal_yaml.cpp b/src/yaml/processor_tracker_feature_trifocal_yaml.cpp index 9e339d41a..2a6ae97f7 100644 --- a/src/yaml/processor_tracker_feature_trifocal_yaml.cpp +++ b/src/yaml/processor_tracker_feature_trifocal_yaml.cpp @@ -5,13 +5,12 @@ * Author: asantamaria */ - // wolf yaml -#include "../processors/processor_tracker_feature_trifocal.h" -#include "yaml_conversion.h" +#include "processor/processor_tracker_feature_trifocal.h" +#include "base/yaml/yaml_conversion.h" // wolf -#include "../factory.h" +#include "base/factory.h" // yaml-cpp library #include <yaml-cpp/yaml.h> @@ -58,7 +57,6 @@ static ProcessorParamsBasePtr createProcessorParamsTrackerFeatureTrifocal(const params->max_euclidean_distance = algorithm["max euclidean distance"] .as<Scalar>(); params->min_track_length_for_constraint = algorithm["min track length for constraint"].as<int>(); - YAML::Node noise = config["noise"]; params->pixel_noise_std = noise ["pixel noise std"].as<Scalar>(); diff --git a/src/yaml/sensor_IMU_yaml.cpp b/src/yaml/sensor_IMU_yaml.cpp index 658becb6b..c64914c60 100644 --- a/src/yaml/sensor_IMU_yaml.cpp +++ b/src/yaml/sensor_IMU_yaml.cpp @@ -6,11 +6,11 @@ */ // wolf yaml -#include "sensor_IMU.h" -#include "yaml_conversion.h" +#include "base/sensor/sensor_IMU.h" +#include "base/yaml/yaml_conversion.h" // wolf -#include "../factory.h" +#include "base/factory.h" // yaml-cpp library #include <yaml-cpp/yaml.h> diff --git a/src/yaml/sensor_camera_yaml.cpp b/src/yaml/sensor_camera_yaml.cpp index 852adc977..d1a76b78b 100644 --- a/src/yaml/sensor_camera_yaml.cpp +++ b/src/yaml/sensor_camera_yaml.cpp @@ -6,11 +6,11 @@ */ // wolf yaml -#include "yaml_conversion.h" +#include "base/yaml/yaml_conversion.h" // wolf -#include "../sensor_camera.h" -#include "../factory.h" +#include "base/sensor/sensor_camera.h" +#include "base/factory.h" // yaml-cpp library #include <yaml-cpp/yaml.h> @@ -59,7 +59,6 @@ static IntrinsicsBasePtr createIntrinsicsCamera(const std::string & _filename_do intrinsics_cam->width = width; intrinsics_cam->height = height; - //========================================= // ===== this part for debugging only ===== //========================================= @@ -80,9 +79,6 @@ static IntrinsicsBasePtr createIntrinsicsCamera(const std::string & _filename_do // std::cout << "\tintrinsic : " << intrinsic.transpose() << std::endl; // std::cout << "\tdistoriton : " << distortion.transpose() << std::endl; //========================================= - //========================================= - - return intrinsics_cam; } diff --git a/src/yaml/sensor_laser_2D_yaml.cpp b/src/yaml/sensor_laser_2D_yaml.cpp index 160386e6e..6a6b3f4ae 100644 --- a/src/yaml/sensor_laser_2D_yaml.cpp +++ b/src/yaml/sensor_laser_2D_yaml.cpp @@ -5,14 +5,13 @@ * \author: jsola */ - // wolf yaml -#include "yaml_conversion.h" +#include "base/yaml/yaml_conversion.h" // wolf -//#include "../intrinsics_factory.h" -#include "../factory.h" -#include "../sensor_laser_2D.h" +//#include "base/intrinsics_factory.h" +#include "base/factory.h" +#include "base/sensor/sensor_laser_2D.h" // yaml library #include <yaml-cpp/yaml.h> @@ -29,7 +28,6 @@ IntrinsicsBasePtr createIntrinsicsLaser2D(const std::string& _filename_dot_yaml) return params; } - // register into factory const bool WOLF_UNUSED registered_laser_params = IntrinsicsFactory::get().registerCreator("LASER 2D", createIntrinsicsLaser2D); diff --git a/src/yaml/sensor_odom_3D_yaml.cpp b/src/yaml/sensor_odom_3D_yaml.cpp index a6012aeef..f423169a9 100644 --- a/src/yaml/sensor_odom_3D_yaml.cpp +++ b/src/yaml/sensor_odom_3D_yaml.cpp @@ -6,11 +6,11 @@ */ // wolf yaml -#include "yaml_conversion.h" +#include "base/yaml/yaml_conversion.h" // wolf -#include "../sensor_odom_3D.h" -#include "../factory.h" +#include "base/sensor/sensor_odom_3D.h" +#include "base/factory.h" // yaml-cpp library #include <yaml-cpp/yaml.h> diff --git a/test/gtest_IMU.cpp b/test/gtest_IMU.cpp index c423f80a7..e9261f75b 100644 --- a/test/gtest_IMU.cpp +++ b/test/gtest_IMU.cpp @@ -6,15 +6,15 @@ */ //Wolf -#include "processor_IMU.h" -#include "sensor_IMU.h" -#include "wolf.h" -#include "sensor_odom_3D.h" -#include "processor_odom_3D.h" -#include "ceres_wrapper/ceres_manager.h" +#include "base/processor/processor_IMU.h" +#include "base/sensor/sensor_IMU.h" +#include "base/wolf.h" +#include "base/sensor/sensor_odom_3D.h" +#include "base/processor/processor_odom_3D.h" +#include "base/ceres_wrapper/ceres_manager.h" #include "utils_gtest.h" -#include "logging.h" +#include "base/logging.h" // make my life easier using namespace Eigen; @@ -24,7 +24,6 @@ using std::make_shared; using std::static_pointer_cast; using std::string; - class Process_Constraint_IMU : public testing::Test { public: @@ -83,7 +82,6 @@ class Process_Constraint_IMU : public testing::Test bool p0_fixed, q0_fixed, v0_fixed; bool p1_fixed, q1_fixed, v1_fixed; - virtual void SetUp( ) { string wolf_root = _WOLF_ROOT_DIR; @@ -114,7 +112,6 @@ class Process_Constraint_IMU : public testing::Test } - /* Integrate one step of acc and angVel motion, obtain Delta_preintegrated * Input: @@ -153,7 +150,6 @@ class Process_Constraint_IMU : public testing::Test Delta = Delta_plus; } - /* Integrate acc and angVel motion, obtain Delta_preintegrated * Input: * N: number of steps @@ -258,8 +254,6 @@ class Process_Constraint_IMU : public testing::Test return trajectory; } - - MotionBuffer integrateWithProcessor(int N, const TimeStamp& t0, const Quaternions q0, const MatrixXs& motion, const VectorXs& bias_real, const VectorXs& bias_preint, Scalar dt, VectorXs& D_preint, VectorXs& D_corrected) { Vector6s motion_now; @@ -288,8 +282,6 @@ class Process_Constraint_IMU : public testing::Test return processor_imu->getBuffer(); } - - // Initial configuration of variables bool configureAll() { @@ -326,8 +318,6 @@ class Process_Constraint_IMU : public testing::Test return true; } - - // Integrate using all methods virtual void integrateAll() { @@ -338,7 +328,6 @@ class Process_Constraint_IMU : public testing::Test D_exact = integrateDelta(q0, motion, bias_null, bias_null, dt, J_D_bias); x1_exact = imu::composeOverState(x0, D_exact, DT ); - // ===================================== INTEGRATE USING IMU_TOOLS // pre-integrate if (motion.cols() == 1) @@ -363,7 +352,6 @@ class Process_Constraint_IMU : public testing::Test x1_corrected = imu::composeOverState(x0, D_corrected , DT ); } - // Integrate Trajectories all methods virtual void integrateAllTrajectories() { @@ -371,7 +359,6 @@ class Process_Constraint_IMU : public testing::Test Trj_D_exact.resize(10,cols); Trj_D_preint_imu.resize(10,cols); Trj_D_preint_prc.resize(10,cols); Trj_D_corrected_imu.resize(10,cols); Trj_D_corrected_prc.resize(10,cols); Trj_x_exact.resize(10,cols); Trj_x_preint_imu.resize(10,cols); Trj_x_preint_prc.resize(10,cols); Trj_x_corrected_imu.resize(10,cols); Trj_x_corrected_prc.resize(10,cols); - // ===================================== INTEGRATE EXACTLY WITH IMU_TOOLS with no bias at all MotionBuffer Buf_exact = integrateDeltaTrajectory(q0, motion, bias_null, bias_null, dt, J_D_bias); @@ -425,7 +412,6 @@ class Process_Constraint_IMU : public testing::Test MotionBuffer Buf_D_preint_prc = integrateWithProcessor(num_integrations, t0, q0, motion, bias_real, bias_preint, dt, D_preint, D_corrected); - // Build trajectories preintegrated and corrected with processorIMU Dt = 0; col = 0; @@ -451,8 +437,6 @@ class Process_Constraint_IMU : public testing::Test x1_corrected = imu::composeOverState(x0, D_corrected , DT ); } - - // Set state_blocks status void setFixedBlocks() { @@ -465,8 +449,6 @@ class Process_Constraint_IMU : public testing::Test KF_1->getVPtr()->setFixed(v1_fixed); } - - void setKfStates() { // This perturbs states to estimate around the exact value, then assigns to the keyframe @@ -495,8 +477,6 @@ class Process_Constraint_IMU : public testing::Test KF_1->setState(x_pert); } - - virtual void buildProblem() { // ===================================== SET KF in Wolf tree @@ -519,8 +499,6 @@ class Process_Constraint_IMU : public testing::Test setKfStates(); } - - string solveProblem(SolverManager::ReportVerbosity verbose = SolverManager::ReportVerbosity::BRIEF) { string report = ceres_manager->solve(verbose); @@ -542,8 +520,6 @@ class Process_Constraint_IMU : public testing::Test return report; } - - string runAll(SolverManager::ReportVerbosity verbose) { configureAll(); @@ -553,8 +529,6 @@ class Process_Constraint_IMU : public testing::Test return report; } - - void printAll(std::string report = "") { WOLF_TRACE(report); @@ -585,8 +559,6 @@ class Process_Constraint_IMU : public testing::Test WOLF_TRACE("X0_optim : ", x0_optim .transpose() ); // optimal state after solving using processor } - - virtual void assertAll() { // check delta and state integrals @@ -729,7 +701,6 @@ TEST_F(Process_Constraint_IMU, MotionConstant_PQV_b__PQV_b) // F_ixed___e_stimat // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== // // ================================================================================================================ // - // ===================================== RUN ALL string report = runAll(SolverManager::ReportVerbosity::BRIEF); @@ -774,7 +745,6 @@ TEST_F(Process_Constraint_IMU, test_capture) // F_ixed___e_stimated // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== // // ================================================================================================================ // - // ===================================== RUN ALL Eigen::Vector6s initial_bias((Eigen::Vector6s()<< .002, .0007, -.001, .003, -.002, .001).finished()); sensor_imu->getIntrinsicPtr()->setState(initial_bias); @@ -786,7 +756,6 @@ TEST_F(Process_Constraint_IMU, test_capture) // F_ixed___e_stimated ASSERT_MATRIX_APPROX(KF_0->getCaptureOf(sensor_imu)->getCalibration(), KF_1->getCaptureOf(sensor_imu)->getCalibration(), 1e-8 ); } - TEST_F(Process_Constraint_IMU, MotionConstant_pqv_b__PQV_b) // F_ixed___e_stimated { @@ -822,7 +791,6 @@ TEST_F(Process_Constraint_IMU, MotionConstant_pqv_b__PQV_b) // F_ixed___e_stimat // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== // // ================================================================================================================ // - // ===================================== RUN ALL string report = runAll(SolverManager::ReportVerbosity::BRIEF); @@ -832,7 +800,6 @@ TEST_F(Process_Constraint_IMU, MotionConstant_pqv_b__PQV_b) // F_ixed___e_stimat } - TEST_F(Process_Constraint_IMU, MotionConstant_pqV_b__PQv_b) // F_ixed___e_stimated { @@ -868,7 +835,6 @@ TEST_F(Process_Constraint_IMU, MotionConstant_pqV_b__PQv_b) // F_ixed___e_stimat // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== // // ================================================================================================================ // - // ===================================== RUN ALL string report = runAll(SolverManager::ReportVerbosity::BRIEF); @@ -878,7 +844,6 @@ TEST_F(Process_Constraint_IMU, MotionConstant_pqV_b__PQv_b) // F_ixed___e_stimat } - TEST_F(Process_Constraint_IMU, MotionRandom_PQV_b__PQV_b) // F_ixed___e_stimated { @@ -914,7 +879,6 @@ TEST_F(Process_Constraint_IMU, MotionRandom_PQV_b__PQV_b) // F_ixed___e_stimated // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== // // ================================================================================================================ // - // ===================================== RUN ALL string report = runAll(SolverManager::ReportVerbosity::BRIEF); @@ -924,7 +888,6 @@ TEST_F(Process_Constraint_IMU, MotionRandom_PQV_b__PQV_b) // F_ixed___e_stimated } - TEST_F(Process_Constraint_IMU, MotionRandom_pqV_b__PQv_b) // F_ixed___e_stimated { @@ -960,7 +923,6 @@ TEST_F(Process_Constraint_IMU, MotionRandom_pqV_b__PQv_b) // F_ixed___e_stimated // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== // // ================================================================================================================ // - // ===================================== RUN ALL string report = runAll(SolverManager::ReportVerbosity::BRIEF); @@ -1005,7 +967,6 @@ TEST_F(Process_Constraint_IMU, MotionRandom_pqV_b__pQV_b) // F_ixed___e_stimated // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== // // ================================================================================================================ // - // ===================================== RUN ALL string report = runAll(SolverManager::ReportVerbosity::BRIEF); @@ -1050,7 +1011,6 @@ TEST_F(Process_Constraint_IMU, MotionConstant_NonNullState_PQV_b__PQV_b) // F_ix // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== // // ================================================================================================================ // - // ===================================== RUN ALL string report = runAll(SolverManager::ReportVerbosity::BRIEF); @@ -1095,7 +1055,6 @@ TEST_F(Process_Constraint_IMU_ODO, MotionConstantRotation_PQV_b__PQV_b) // F_ixe // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== // // ================================================================================================================ // - // ===================================== RUN ALL string report = runAll(SolverManager::ReportVerbosity::BRIEF); @@ -1140,7 +1099,6 @@ TEST_F(Process_Constraint_IMU_ODO, MotionConstantRotation_PQV_b__PQv_b) // F_ixe // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== // // ================================================================================================================ // - // ===================================== RUN ALL string report = runAll(SolverManager::ReportVerbosity::BRIEF); @@ -1185,7 +1143,6 @@ TEST_F(Process_Constraint_IMU_ODO, MotionConstantRotation_PQV_b__Pqv_b) // F_ixe // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== // // ================================================================================================================ // - // ===================================== RUN ALL string report = runAll(SolverManager::ReportVerbosity::BRIEF); @@ -1230,7 +1187,6 @@ TEST_F(Process_Constraint_IMU_ODO, MotionConstantRotation_PQV_b__pQv_b) // F_ixe // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== // // ================================================================================================================ // - // ===================================== RUN ALL string report = runAll(SolverManager::ReportVerbosity::BRIEF); @@ -1275,7 +1231,6 @@ TEST_F(Process_Constraint_IMU_ODO, MotionConstantRotation_PQV_b__pqv_b) // F_ixe // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== // // ================================================================================================================ // - // ===================================== RUN ALL string report = runAll(SolverManager::ReportVerbosity::BRIEF); @@ -1320,7 +1275,6 @@ TEST_F(Process_Constraint_IMU_ODO, MotionConstant_pqv_b__pqV_b) // F_ixed___e_st // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== // // ================================================================================================================ // - // ===================================== RUN ALL string report = runAll(SolverManager::ReportVerbosity::BRIEF); @@ -1365,7 +1319,6 @@ TEST_F(Process_Constraint_IMU_ODO, MotionConstant_pqV_b__pqv_b) // F_ixed___e_st // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== // // ================================================================================================================ // - // ===================================== RUN ALL string report = runAll(SolverManager::ReportVerbosity::BRIEF); @@ -1410,7 +1363,6 @@ TEST_F(Process_Constraint_IMU_ODO, MotionRandom_PQV_b__pqv_b) // F_ixed___e_stim // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== // // ================================================================================================================ // - // ===================================== RUN ALL string report = runAll(SolverManager::ReportVerbosity::BRIEF); @@ -1455,7 +1407,6 @@ TEST_F(Process_Constraint_IMU_ODO, MotionRandom_PqV_b__pqV_b) // F_ixed___e_stim // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== // // ================================================================================================================ // - // ===================================== RUN ALL string report = runAll(SolverManager::ReportVerbosity::BRIEF); @@ -1500,7 +1451,6 @@ TEST_F(Process_Constraint_IMU_ODO, RecoverTrajectory_MotionRandom_PqV_b__pqV_b) // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== // // ================================================================================================================ // - // ===================================== RUN ALL configureAll(); integrateAllTrajectories(); @@ -1561,8 +1511,6 @@ int main(int argc, char **argv) return RUN_ALL_TESTS(); } - - /* Some notes : * * - Process_Constraint_IMU_ODO.MotionConstant_PQv_b__PQv_b : diff --git a/test/gtest_IMU_tools.cpp b/test/gtest_IMU_tools.cpp index 9f0f8c398..e41007154 100644 --- a/test/gtest_IMU_tools.cpp +++ b/test/gtest_IMU_tools.cpp @@ -5,10 +5,9 @@ * Author: jsola */ -#include "IMU_tools.h" +#include "base/IMU_tools.h" #include "utils_gtest.h" - using namespace Eigen; using namespace wolf; using namespace imu; @@ -376,7 +375,6 @@ TEST(motion2data, AllRandom) Vector6s bias; Quaternions q; - motion .setRandom(); bias .setRandom(); q.coeffs() .setRandom().normalize(); @@ -390,7 +388,6 @@ TEST(motion2data, AllRandom) ASSERT_MATRIX_APPROX(data, data_true, 1e-12); } - /* Integrate acc and angVel motion, obtain Delta_preintegrated * Input: * N: number of steps diff --git a/test/gtest_capture_base.cpp b/test/gtest_capture_base.cpp index 00fdf8883..d1ec1884c 100644 --- a/test/gtest_capture_base.cpp +++ b/test/gtest_capture_base.cpp @@ -5,11 +5,10 @@ * Author: jsola */ - #include "utils_gtest.h" -#include "capture_base.h" -#include "state_angle.h" +#include "base/capture/capture_base.h" +#include "base/state_angle.h" using namespace wolf; using namespace Eigen; diff --git a/test/gtest_ceres_manager.cpp b/test/gtest_ceres_manager.cpp index 0df86c75e..097969799 100644 --- a/test/gtest_ceres_manager.cpp +++ b/test/gtest_ceres_manager.cpp @@ -6,18 +6,18 @@ */ #include "utils_gtest.h" -#include "../src/logging.h" - -#include "../problem.h" -#include "../sensor_base.h" -#include "../state_block.h" -#include "../capture_void.h" -#include "../constraint_pose_2D.h" -#include "../constraint_quaternion_absolute.h" -#include "../solver/solver_manager.h" -#include "../ceres_wrapper/ceres_manager.h" -#include "../local_parametrization_angle.h" -#include "../local_parametrization_quaternion.h" +#include "base/logging.h" + +#include "base/problem.h" +#include "base/sensor/sensor_base.h" +#include "base/state_block.h" +#include "base/capture/capture_void.h" +#include "base/constraint/constraint_pose_2D.h" +#include "base/constraint/constraint_quaternion_absolute.h" +#include "base/solver/solver_manager.h" +#include "base/ceres_wrapper/ceres_manager.h" +#include "base/local_parametrization_angle.h" +#include "base/local_parametrization_quaternion.h" #include "ceres/ceres.h" @@ -629,7 +629,6 @@ TEST(CeresManager, ConstraintsUpdateLocalParam) ceres_manager_ptr->check(); } - int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); diff --git a/test/gtest_constraint_IMU.cpp b/test/gtest_constraint_IMU.cpp index bc212aaaa..7ef93e9a3 100644 --- a/test/gtest_constraint_IMU.cpp +++ b/test/gtest_constraint_IMU.cpp @@ -6,15 +6,15 @@ */ //Wolf -#include "capture_IMU.h" -#include "constraint_pose_3D.h" -#include "processor_IMU.h" -#include "sensor_IMU.h" -#include "processor_odom_3D.h" +#include "base/capture/capture_IMU.h" +#include "base/constraint/constraint_pose_3D.h" +#include "base/processor/processor_IMU.h" +#include "base/sensor/sensor_IMU.h" +#include "base/processor/processor_odom_3D.h" #include "ceres_wrapper/ceres_manager.h" #include "utils_gtest.h" -#include "logging.h" +#include "base/logging.h" #include <iostream> #include <fstream> @@ -90,7 +90,6 @@ class ConstraintIMU_biasTest_Static_NullBias : public testing::Test //===================================================== END{INITIALIZATION} - //===================================================== PROCESS DATA // PROCESS DATA @@ -436,7 +435,6 @@ class ConstraintIMU_biasTest_Move_NullBias : public testing::Test //===================================================== END{INITIALIZATION} - //===================================================== PROCESS DATA // PROCESS DATA @@ -1011,7 +1009,6 @@ class ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test // WOLF_TRACE("KF2 delta cor: ", last_CM ->getDeltaCorrected(origin_CB->getCalibration()).transpose()); // WOLF_TRACE("KF2 jacob : ", last_CM ->getJacobianCalib().row(0)); - // ==================================================== show problem status problem->print(4,1,1,1); @@ -1335,8 +1332,6 @@ class ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test virtual void TearDown(){} }; - - // tests with following conditions : // var(b1,b2), invar(p1,q1,v1,p2,q2,v2), factor : imu(p,q,v) @@ -1642,7 +1637,6 @@ TEST_F(ConstraintIMU_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_E ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) - //============================================================== //WOLF_INFO("Starting error bias 1e-2") epsilon_bias = 0.01; @@ -1723,7 +1717,6 @@ TEST_F(ConstraintIMU_biasTest_Static_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBi ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) - //============================================================== //WOLF_INFO("Starting error bias 1e-2") epsilon_bias = 0.01; @@ -2843,6 +2836,5 @@ int main(int argc, char **argv) // ::testing::GTEST_FLAG(filter) = "ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot.VarB1B2_InvarP1Q1V1P2Q2V2_initOK"; // ::testing::GTEST_FLAG(filter) = "ConstraintIMU_biasTest_Move_NonNullBiasRot.VarB1B2V1P2V2_InvarP1Q1Q2_initOK"; - return RUN_ALL_TESTS(); } diff --git a/test/gtest_constraint_absolute.cpp b/test/gtest_constraint_absolute.cpp index a0587e29c..53252fc5f 100644 --- a/test/gtest_constraint_absolute.cpp +++ b/test/gtest_constraint_absolute.cpp @@ -5,14 +5,12 @@ * \author: datchuth */ - #include "utils_gtest.h" -#include "constraint_block_absolute.h" -#include "constraint_quaternion_absolute.h" -#include "capture_motion.h" - -#include "ceres_wrapper/ceres_manager.h" +#include "base/constraint/constraint_block_absolute.h" +#include "base/constraint/constraint_quaternion_absolute.h" +#include "base/capture/capture_motion.h" +#include "base/ceres_wrapper/ceres_manager.h" using namespace Eigen; using namespace wolf; diff --git a/test/gtest_constraint_autodiff.cpp b/test/gtest_constraint_autodiff.cpp index c5820537f..5138bb899 100644 --- a/test/gtest_constraint_autodiff.cpp +++ b/test/gtest_constraint_autodiff.cpp @@ -5,15 +5,14 @@ * Author: jvallve */ - #include "utils_gtest.h" -#include "sensor_odom_2D.h" -#include "capture_void.h" -#include "feature_odom_2D.h" -#include "constraint_odom_2D.h" -#include "constraint_odom_2D_analytic.h" -#include "constraint_autodiff.h" +#include "base/sensor/sensor_odom_2D.h" +#include "base/capture/capture_void.h" +#include "base/feature/feature_odom_2D.h" +#include "base/constraint/constraint_odom_2D.h" +#include "base/constraint/constraint_odom_2D_analytic.h" +#include "base/constraint/constraint_autodiff.h" using namespace wolf; using namespace Eigen; diff --git a/test/gtest_constraint_autodiff_distance_3D.cpp b/test/gtest_constraint_autodiff_distance_3D.cpp index bc6e402e4..ae3313afe 100644 --- a/test/gtest_constraint_autodiff_distance_3D.cpp +++ b/test/gtest_constraint_autodiff_distance_3D.cpp @@ -5,11 +5,11 @@ * \author: jsola */ -#include "constraints/constraint_autodiff_distance_3D.h" -#include "problem.h" -#include "logging.h" -#include "ceres_wrapper/ceres_manager.h" -#include "rotations.h" +#include "base/constraint/constraint_autodiff_distance_3D.h" +#include "base/problem.h" +#include "base/logging.h" +#include "base/ceres_wrapper/ceres_manager.h" +#include "base/rotations.h" #include "utils_gtest.h" diff --git a/test/gtest_constraint_autodiff_trifocal.cpp b/test/gtest_constraint_autodiff_trifocal.cpp index aa34b7ea7..1615cc4ab 100644 --- a/test/gtest_constraint_autodiff_trifocal.cpp +++ b/test/gtest_constraint_autodiff_trifocal.cpp @@ -1,10 +1,10 @@ #include "utils_gtest.h" -#include "logging.h" +#include "base/logging.h" #include "ceres_wrapper/ceres_manager.h" #include "processors/processor_tracker_feature_trifocal.h" -#include "capture_image.h" +#include "base/capture/capture_image.h" #include "constraints/constraint_autodiff_trifocal.h" using namespace Eigen; @@ -278,7 +278,6 @@ TEST_F(ConstraintAutodiffTrifocalTest, error_jacobians) ASSERT_MATRIX_APPROX(J_r_m1, Jn_r_m1, 1e-6); - // jacs wrt m2 pix0 = c123->getPixelCanonicalOrigin(); for (int i=0; i<3; i++) @@ -295,7 +294,6 @@ TEST_F(ConstraintAutodiffTrifocalTest, error_jacobians) ASSERT_MATRIX_APPROX(J_r_m2, Jn_r_m2, 1e-6); - // jacs wrt m3 pix0 = c123->getPixelCanonicalLast(); for (int i=0; i<3; i++) @@ -671,7 +669,6 @@ class ConstraintAutodiffTrifocalMultiPointTest : public ConstraintAutodiffTrifoc * */ - public: std::vector<FeatureBasePtr> fv1, fv2, fv3; std::vector<ConstraintAutodiffTrifocalPtr> cv123; @@ -735,7 +732,6 @@ TEST_F(ConstraintAutodiffTrifocalMultiPointTest, solve_multi_point) * */ - S ->getPPtr()->fix(); // do not calibrate sensor pos S ->getOPtr()->fix(); // do not calibrate sensor ori F1->getPPtr()->fix(); // Cam 1 acts as reference @@ -801,7 +797,6 @@ TEST_F(ConstraintAutodiffTrifocalMultiPointTest, solve_multi_point_scale) * */ - S ->getPPtr()->fix(); // do not calibrate sensor pos S ->getOPtr()->fix(); // do not calibrate sensor ori F1->getPPtr()->fix(); // Cam 1 acts as reference @@ -868,7 +863,6 @@ TEST_F(ConstraintAutodiffTrifocalMultiPointTest, solve_multi_point_distance) * */ - S ->getPPtr()->fix(); // do not calibrate sensor pos S ->getOPtr()->fix(); // do not calibrate sensor ori F1->getPPtr()->fix(); // Cam 1 acts as reference diff --git a/test/gtest_constraint_odom_3D.cpp b/test/gtest_constraint_odom_3D.cpp index 684082775..d71050ec6 100644 --- a/test/gtest_constraint_odom_3D.cpp +++ b/test/gtest_constraint_odom_3D.cpp @@ -5,14 +5,12 @@ * \author: jsola */ - #include "utils_gtest.h" -#include "constraint_odom_3D.h" -#include "capture_motion.h" - -#include "ceres_wrapper/ceres_manager.h" +#include "base/constraint/constraint_odom_3D.h" +#include "base/capture/capture_motion.h" +#include "base/ceres_wrapper/ceres_manager.h" using namespace Eigen; using namespace wolf; diff --git a/test/gtest_constraint_pose_2D.cpp b/test/gtest_constraint_pose_2D.cpp index dd95c82d8..125893e67 100644 --- a/test/gtest_constraint_pose_2D.cpp +++ b/test/gtest_constraint_pose_2D.cpp @@ -5,14 +5,12 @@ * \author: jsola */ - -#include "../constraint_pose_2D.h" +#include "base/constraint/constraint_pose_2D.h" #include "utils_gtest.h" -#include "capture_motion.h" - -#include "ceres_wrapper/ceres_manager.h" +#include "base/capture/capture_motion.h" +#include "base/ceres_wrapper/ceres_manager.h" using namespace Eigen; using namespace wolf; @@ -65,7 +63,6 @@ TEST(ConstraintPose2D, solve) } - int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); diff --git a/test/gtest_constraint_pose_3D.cpp b/test/gtest_constraint_pose_3D.cpp index 48fbedebd..05de4bb61 100644 --- a/test/gtest_constraint_pose_3D.cpp +++ b/test/gtest_constraint_pose_3D.cpp @@ -5,14 +5,12 @@ * \author: jsola */ - -#include "../constraint_pose_3D.h" +#include "base/constraint/constraint_pose_3D.h" #include "utils_gtest.h" -#include "capture_motion.h" - -#include "ceres_wrapper/ceres_manager.h" +#include "base/capture/capture_motion.h" +#include "base/ceres_wrapper/ceres_manager.h" using namespace Eigen; using namespace wolf; @@ -24,7 +22,6 @@ Vector7s pose6toPose7(Vector6s _pose6) return (Vector7s() << _pose6.head<3>() , v2q(_pose6.tail<3>()).coeffs()).finished(); } - // Input pose6 and covariance Vector6s pose(Vector6s::Random()); Vector7s pose7 = pose6toPose7(pose); @@ -46,7 +43,6 @@ CaptureBasePtr cap0 = frm0->addCapture(std::make_shared<CaptureMotion>("ODOM 3D" FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("ODOM 3D", pose7, data_cov)); ConstraintPose3DPtr ctr0 = std::static_pointer_cast<ConstraintPose3D>(fea0->addConstraint(std::make_shared<ConstraintPose3D>(fea0))); - //////////////////////////////////////////////////////// TEST(ConstraintPose3D, check_tree) @@ -73,7 +69,6 @@ TEST(ConstraintPose3D, solve) } - int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); diff --git a/test/gtest_constraint_sparse.cpp b/test/gtest_constraint_sparse.cpp index 875c68c8e..900f39d5b 100644 --- a/test/gtest_constraint_sparse.cpp +++ b/test/gtest_constraint_sparse.cpp @@ -5,7 +5,6 @@ * \author: jsola */ - #include "utils_gtest.h" #include "constraint_sparse.h" diff --git a/test/gtest_eigen_predicates.cpp b/test/gtest_eigen_predicates.cpp index 23fa8e234..649ba85e9 100644 --- a/test/gtest_eigen_predicates.cpp +++ b/test/gtest_eigen_predicates.cpp @@ -1,6 +1,6 @@ #include "utils_gtest.h" -#include "../eigen_predicates.h" +#include "base/eigen_predicates.h" TEST(TestEigenPredicates, TestEigenDynPredZero) { diff --git a/test/gtest_feature_IMU.cpp b/test/gtest_feature_IMU.cpp index 970556a3e..5ce136d66 100644 --- a/test/gtest_feature_IMU.cpp +++ b/test/gtest_feature_IMU.cpp @@ -1,13 +1,13 @@ //Wolf -#include "capture_IMU.h" -#include "processor_IMU.h" -#include "sensor_IMU.h" -#include "wolf.h" -#include "problem.h" -#include "state_block.h" -#include "state_quaternion.h" +#include "base/capture/capture_IMU.h" +#include "base/processor/processor_IMU.h" +#include "base/sensor/sensor_IMU.h" +#include "base/wolf.h" +#include "base/problem.h" +#include "base/state_block.h" +#include "base/state_quaternion.h" #include "utils_gtest.h" -#include "../src/logging.h" +#include "base/logging.h" class FeatureIMU_test : public testing::Test { diff --git a/test/gtest_feature_base.cpp b/test/gtest_feature_base.cpp index bc867f48f..f9da19cff 100644 --- a/test/gtest_feature_base.cpp +++ b/test/gtest_feature_base.cpp @@ -5,11 +5,10 @@ * Author: jsola */ -#include "feature_base.h" +#include "base/feature/feature_base.h" #include "utils_gtest.h" - using namespace wolf; using namespace Eigen; diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp index ae6b78068..e5a4d4f3c 100644 --- a/test/gtest_frame_base.cpp +++ b/test/gtest_frame_base.cpp @@ -5,20 +5,17 @@ * Author: jsola */ - - #include "utils_gtest.h" -#include "../logging.h" +#include "base/logging.h" -#include "../frame_base.h" -#include "../sensor_odom_2D.h" -#include "../processor_odom_2D.h" -#include "../constraint_odom_2D.h" -#include "../capture_motion.h" +#include "base/frame_base.h" +#include "base/sensor/sensor_odom_2D.h" +#include "base/processor/processor_odom_2D.h" +#include "base/constraint/constraint_odom_2D.h" +#include "base/capture/capture_motion.h" #include <iostream> - using namespace Eigen; using namespace std; using namespace wolf; @@ -61,7 +58,6 @@ TEST(FrameBase, LinksBasic) ASSERT_EQ(F->getHits() , (unsigned int) 0); } - TEST(FrameBase, LinksToTree) { // Problem with 2 frames and one motion constraint between them @@ -120,8 +116,6 @@ TEST(FrameBase, LinksToTree) F1->getOPtr()->fix(); ASSERT_TRUE(F1->isFixed()); - - // set key F1->setKey(); ASSERT_TRUE(F1->isKey()); @@ -131,7 +125,7 @@ TEST(FrameBase, LinksToTree) ASSERT_TRUE(F1->getCaptureList().empty()); } -#include "state_quaternion.h" +#include "base/state_quaternion.h" TEST(FrameBase, GetSetState) { // Create PQV_3D state blocks @@ -166,13 +160,9 @@ TEST(FrameBase, GetSetState) ASSERT_TRUE((x2 - x).isMuchSmallerThan(1, Constants::EPS_SMALL)); } - int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } - - - diff --git a/test/gtest_local_param.cpp b/test/gtest_local_param.cpp index bdd413a82..42e01489e 100644 --- a/test/gtest_local_param.cpp +++ b/test/gtest_local_param.cpp @@ -6,14 +6,13 @@ */ #include "utils_gtest.h" -#include "../src/logging.h" +#include "base/logging.h" +#include "base/local_parametrization_quaternion.h" +#include "base/local_parametrization_homogeneous.h" +#include "base/rotations.h" -#include "../src/local_parametrization_quaternion.h" -#include "../src/local_parametrization_homogeneous.h" -#include "../src/rotations.h" - -#include "../src/wolf.h" +#include "base/wolf.h" #include <iostream> @@ -32,7 +31,6 @@ } \ } - using namespace Eigen; using namespace std; using namespace wolf; @@ -126,7 +124,6 @@ TEST(TestLocalParametrization, QuaternionGlobal) ASSERT_MATRIX_APPROX(da_m, da2_m, 1e-10); - } TEST(TestLocalParametrization, Homogeneous) diff --git a/test/gtest_make_posdef.cpp b/test/gtest_make_posdef.cpp index d2b82d1ea..1dfcce935 100644 --- a/test/gtest_make_posdef.cpp +++ b/test/gtest_make_posdef.cpp @@ -1,5 +1,5 @@ #include "utils_gtest.h" -#include "wolf.h" +#include "base/wolf.h" using namespace Eigen; using namespace wolf; diff --git a/test/gtest_motion_buffer.cpp b/test/gtest_motion_buffer.cpp index 3bed94433..6d0301692 100644 --- a/test/gtest_motion_buffer.cpp +++ b/test/gtest_motion_buffer.cpp @@ -5,13 +5,12 @@ * Author: jsola */ - #include "utils_gtest.h" -#include "../logging.h" +#include "base/logging.h" -#include "../motion_buffer.h" +#include "base/motion_buffer.h" -#include "wolf.h" +#include "base/wolf.h" #include <iostream> @@ -184,4 +183,3 @@ int main(int argc, char **argv) return RUN_ALL_TESTS(); } - diff --git a/test/gtest_odom_2D.cpp b/test/gtest_odom_2D.cpp index d8e10b949..fcf60798e 100644 --- a/test/gtest_odom_2D.cpp +++ b/test/gtest_odom_2D.cpp @@ -8,15 +8,14 @@ #include "utils_gtest.h" // Classes under test -#include "../processor_odom_2D.h" -#include "../constraint_odom_2D.h" +#include "base/processor/processor_odom_2D.h" +#include "base/constraint/constraint_odom_2D.h" // Wolf includes -#include "../sensor_odom_2D.h" -#include "../state_block.h" -#include "../wolf.h" -#include "../ceres_wrapper/ceres_manager.h" -#include "../capture_pose.h" +#include "base/sensor/sensor_odom_2D.h" +#include "base/state_block.h" +#include "base/wolf.h" +#include "base/ceres_wrapper/ceres_manager.h" // STL includes #include <map> @@ -27,11 +26,11 @@ // General includes #include <iostream> #include <iomanip> // std::setprecision +#include "base/capture/capture_pose.h" using namespace wolf; using namespace Eigen; - VectorXs plus(const VectorXs& _pose, const Vector2s& _data) { VectorXs _pose_plus_data(3); @@ -98,7 +97,6 @@ void show(const ProblemPtr& problem) } } - TEST(Odom2D, ConstraintFix_and_ConstraintOdom2D) { using std::cout; @@ -131,7 +129,6 @@ TEST(Odom2D, ConstraintFix_and_ConstraintOdom2D) // KF1 and motion from KF0 t += dt; - t += dt; FrameBasePtr F1 = Pr->emplaceFrame(KEY_FRAME, Vector3s::Zero(), t); CaptureBasePtr C1 = F1->addCapture(std::make_shared<CaptureBase>("ODOM 2D", t)); FeatureBasePtr f1 = C1->addFeature(std::make_shared<FeatureBase>("ODOM 2D", delta, delta_cov)); @@ -140,7 +137,6 @@ TEST(Odom2D, ConstraintFix_and_ConstraintOdom2D) // KF2 and motion from KF1 t += dt; - t += dt; FrameBasePtr F2 = Pr->emplaceFrame(KEY_FRAME, Vector3s::Zero(), t); CaptureBasePtr C2 = F2->addCapture(std::make_shared<CaptureBase>("ODOM 2D", t)); FeatureBasePtr f2 = C2->addFeature(std::make_shared<FeatureBase>("ODOM 2D", delta, delta_cov)); @@ -471,8 +467,6 @@ TEST(Odom2D, KF_callback) } } - - int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); diff --git a/test/gtest_odom_3D.cpp b/test/gtest_odom_3D.cpp index bc36ac845..864f19c14 100644 --- a/test/gtest_odom_3D.cpp +++ b/test/gtest_odom_3D.cpp @@ -5,19 +5,15 @@ * \author: jsola */ - - - #include "utils_gtest.h" -#include "wolf.h" -#include "logging.h" +#include "base/wolf.h" +#include "base/logging.h" -#include "processor_odom_3D.h" +#include "base/processor/processor_odom_3D.h" #include <iostream> - #define JAC_NUMERIC(prc_ptr, D, d, dt, J_D, J_d, dx) \ { VectorXs Do(7); \ prc_ptr->deltaPlusDelta(D, d, dt, Do); \ @@ -155,7 +151,6 @@ TEST(ProcessorOdom3D, deltaPlusDelta_Jac) } - TEST(ProcessorOdom3D, Interpolate0) // basic test { /* Conditions: @@ -177,7 +172,6 @@ TEST(ProcessorOdom3D, Interpolate0) // basic test WOLF_DEBUG("ref delta= ", ref.delta_.transpose()); WOLF_DEBUG("ref Delta= ", ref.delta_integr_.transpose()); - // set final final.ts_ = 5; final.delta_ << 1,0,0, 0,0,0,1; @@ -271,7 +265,6 @@ TEST(ProcessorOdom3D, Interpolate1) // delta algebra test // Motion structures Motion R(0.0,6,7,6,0), I(0.0,6,7,6,0), S(0.0,6,7,6,0), F(0.0,6,7,6,0); - /////////// start experiment /////////////// // set origin and ref states @@ -315,7 +308,6 @@ TEST(ProcessorOdom3D, Interpolate1) // delta algebra test Dp_of = q_o.conjugate() * (p_f - p_o); Dq_of = q_o.conjugate() * q_f; - // set ref R.ts_ = t_r; R.delta_ = dx_or; // origin to ref @@ -405,7 +397,6 @@ TEST(ProcessorOdom3D, Interpolate2) // timestamp out of bounds test // Motion structures Motion R(0.0,6,7,6,0), I(0.0,6,7,6,0), S(0.0,6,7,6,0), F(0.0,6,7,6,0); - /////////// start experiment /////////////// // set final and ref deltas @@ -495,13 +486,9 @@ TEST(ProcessorOdom3D, Interpolate2) // timestamp out of bounds test } - - - int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } - diff --git a/test/gtest_pack_KF_buffer.cpp b/test/gtest_pack_KF_buffer.cpp index 1204254c1..8f68a5bd5 100644 --- a/test/gtest_pack_KF_buffer.cpp +++ b/test/gtest_pack_KF_buffer.cpp @@ -7,13 +7,13 @@ //Wolf #include "utils_gtest.h" -#include "processor_odom_2D.h" -#include "sensor_odom_2D.h" +#include "base/processor/processor_odom_2D.h" +#include "base/sensor/sensor_odom_2D.h" -#include "processor_tracker_feature_dummy.h" -#include "capture_void.h" +#include "base/processor/processor_tracker_feature_dummy.h" +#include "base/capture/capture_void.h" -#include "problem.h" +#include "base/problem.h" // STL #include <iterator> diff --git a/test/gtest_pinhole.cpp b/test/gtest_pinhole.cpp index b46e682ab..fb36c6d12 100644 --- a/test/gtest_pinhole.cpp +++ b/test/gtest_pinhole.cpp @@ -5,10 +5,9 @@ * Author: jsola */ -#include "pinhole_tools.h" +#include "base/pinhole_tools.h" #include "utils_gtest.h" - using namespace Eigen; using namespace wolf; using namespace pinhole; @@ -200,4 +199,3 @@ int main(int argc, char **argv) return RUN_ALL_TESTS(); } - diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp index 7ae62578b..1cf49acaa 100644 --- a/test/gtest_problem.cpp +++ b/test/gtest_problem.cpp @@ -6,13 +6,13 @@ */ #include "utils_gtest.h" -#include "../src/logging.h" +#include "base/logging.h" -#include "../problem.h" -#include "../sensor_base.h" -#include "../sensor_odom_3D.h" -#include "../processor_odom_3D.h" -#include "../processor_tracker_feature_dummy.h" +#include "base/problem.h" +#include "base/sensor/sensor_base.h" +#include "base/sensor/sensor_odom_3D.h" +#include "base/processor/processor_odom_3D.h" +#include "base/processor/processor_tracker_feature_dummy.h" #include <iostream> @@ -127,7 +127,6 @@ TEST(Problem, SetOrigin_PO_2D) // check that the constraint is absolute (no pointers to other F, f, or L) ConstraintBasePtr c = f->getConstraintList().front(); ASSERT_FALSE(c->getFrameOtherPtr()); - ASSERT_FALSE(c->getFrameOtherPtr()); ASSERT_FALSE(c->getLandmarkOtherPtr()); // check that the Feature measurement and covariance are the ones provided @@ -137,7 +136,6 @@ TEST(Problem, SetOrigin_PO_2D) // P->print(4,1,1,1); } - TEST(Problem, SetOrigin_PO_3D) { ProblemPtr P = Problem::create("PO 3D"); @@ -166,7 +164,6 @@ TEST(Problem, SetOrigin_PO_3D) // check that the constraint is absolute (no pointers to other F, f, or L) ConstraintBasePtr c = f->getConstraintList().front(); ASSERT_FALSE(c->getFrameOtherPtr()); - ASSERT_FALSE(c->getFrameOtherPtr()); ASSERT_FALSE(c->getLandmarkOtherPtr()); // check that the Feature measurement and covariance are the ones provided @@ -176,8 +173,6 @@ TEST(Problem, SetOrigin_PO_3D) // P->print(4,1,1,1); } - - TEST(Problem, emplaceFrame_factory) { ProblemPtr P = Problem::create("PO 2D"); @@ -203,7 +198,6 @@ TEST(Problem, emplaceFrame_factory) ASSERT_EQ(f2->getProblem(), P); } - TEST(Problem, StateBlocks) { std::string wolf_root = _WOLF_ROOT_DIR; @@ -235,7 +229,6 @@ TEST(Problem, StateBlocks) ASSERT_EQ(P->getStateBlockList().size(), (unsigned int)(2 + 3 + 2)); ASSERT_EQ(P->getStateBlockNotificationMap().size(), (unsigned int)(2 + 3 + 2)); - // P->print(4,1,1,1); // change some SB properties @@ -277,7 +270,6 @@ TEST(Problem, Covariances) } - int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); diff --git a/test/gtest_processor_IMU.cpp b/test/gtest_processor_IMU.cpp index 8dfb9bd57..a6ee8c8ac 100644 --- a/test/gtest_processor_IMU.cpp +++ b/test/gtest_processor_IMU.cpp @@ -5,16 +5,16 @@ * \author: jsola */ -#include "capture_IMU.h" -#include "processor_IMU.h" -#include "sensor_IMU.h" -#include "wolf.h" +#include "base/capture/capture_IMU.h" +#include "base/processor/processor_IMU.h" +#include "base/sensor/sensor_IMU.h" +#include "base/wolf.h" #include "utils_gtest.h" -#include "logging.h" +#include "base/logging.h" -#include "rotations.h" -#include "ceres_wrapper/ceres_manager.h" +#include "base/rotations.h" +#include "base/ceres_wrapper/ceres_manager.h" #include <cmath> #include <iostream> @@ -78,12 +78,10 @@ class ProcessorIMUt : public testing::Test } }; - TEST(ProcessorIMU_constructors, ALL) { using namespace wolf; - //constructor with ProcessorIMUParamsPtr argument only ProcessorParamsIMUPtr param_ptr = std::make_shared<ProcessorParamsIMU>(); param_ptr->max_time_span = 2.0; @@ -248,7 +246,6 @@ ASSERT_MATRIX_APPROX(mot_int.data_, mot_int_gt.data_, 1e-6); //ASSERT_MATRIX_APPROX(mot_int.delta_, mot_int_gt.delta_, 1e-6); // FIXME: delta_p not correctly interpolated ASSERT_MATRIX_APPROX(mot_final.delta_integr_, mot_sec.delta_integr_, 1e-6); - } TEST_F(ProcessorIMUt, acc_x) @@ -351,7 +348,6 @@ TEST_F(ProcessorIMUt, acc_z) ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibrationPreint() , b, wolf::Constants::EPS); } - TEST_F(ProcessorIMUt, check_Covariance) { t.set(0); // clock in 0,1 ms ticks @@ -394,7 +390,6 @@ TEST_F(ProcessorIMUt, gyro_x) VectorXs x(10); x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0; // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180 - ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL); //do so for 5s diff --git a/test/gtest_processor_IMU_jacobians.cpp b/test/gtest_processor_IMU_jacobians.cpp index dc9ab0397..1e3b0940c 100644 --- a/test/gtest_processor_IMU_jacobians.cpp +++ b/test/gtest_processor_IMU_jacobians.cpp @@ -6,13 +6,13 @@ */ //Wolf -#include "capture_IMU.h" -#include "sensor_IMU.h" +#include "base/capture/capture_IMU.h" +#include "base/sensor/sensor_IMU.h" #include "test/processor_IMU_UnitTester.h" -#include "wolf.h" -#include "problem.h" -#include "state_block.h" -#include "state_quaternion.h" +#include "base/wolf.h" +#include "base/problem.h" +#include "base/state_block.h" +#include "base/state_quaternion.h" #include <iostream> #include <fstream> #include <iomanip> @@ -110,7 +110,6 @@ class ProcessorIMU_jacobians : public testing::Test } }; - class ProcessorIMU_jacobians_Dq : public testing::Test { public: @@ -470,7 +469,6 @@ TEST_F(ProcessorIMU_jacobians, dDv_dO) //dDo_dP = dDo_dV = [0, 0, 0] - TEST_F(ProcessorIMU_jacobians, dDo_dO) { using namespace wolf; @@ -506,7 +504,6 @@ TEST_F(ProcessorIMU_jacobians, dDp_dp) //dDv_dp = [0, 0, 0] - TEST_F(ProcessorIMU_jacobians, dDv_dv) { using namespace wolf; @@ -544,7 +541,6 @@ TEST_F(ProcessorIMU_jacobians, dDo_do) "\ndDo_do_a - dDo_do_ : \n" << dDo_do_a - dDo_do << std::endl; } - int main(int argc, char **argv) { using namespace wolf; diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp index 23baef7b2..aba232636 100644 --- a/test/gtest_processor_base.cpp +++ b/test/gtest_processor_base.cpp @@ -8,13 +8,13 @@ //Wolf #include "utils_gtest.h" -#include "processor_odom_2D.h" -#include "sensor_odom_2D.h" +#include "base/processor/processor_odom_2D.h" +#include "base/sensor/sensor_odom_2D.h" -#include "processor_tracker_feature_dummy.h" -#include "capture_void.h" +#include "base/processor/processor_tracker_feature_dummy.h" +#include "base/capture/capture_void.h" -#include "problem.h" +#include "base/problem.h" // STL #include <iterator> @@ -23,7 +23,6 @@ using namespace wolf; using namespace Eigen; - TEST(ProcessorBase, KeyFrameCallback) { diff --git a/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp b/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp index dcb32770e..4e2b427fb 100644 --- a/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp +++ b/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp @@ -6,12 +6,11 @@ * \author: tessajohanna */ - #include "utils_gtest.h" -#include "../logging.h" +#include "base/logging.h" -#include "../sensor_odom_2D.h" -#include "../processor_frame_nearest_neighbor_filter.h" +#include "base/sensor/sensor_odom_2D.h" +#include "base/processor/processor_frame_nearest_neighbor_filter.h" #include <iostream> diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp index 94a2f0c15..3e379c9a5 100644 --- a/test/gtest_processor_motion.cpp +++ b/test/gtest_processor_motion.cpp @@ -5,15 +5,14 @@ * Author: jsola */ - #include "utils_gtest.h" -#include "wolf.h" -#include "logging.h" +#include "base/wolf.h" +#include "base/logging.h" -#include "sensor_odom_2D.h" -#include "processor_odom_2D.h" -#include "ceres_wrapper/ceres_manager.h" +#include "base/sensor/sensor_odom_2D.h" +#include "base/processor/processor_odom_2D.h" +#include "base/ceres_wrapper/ceres_manager.h" using namespace Eigen; using namespace wolf; @@ -58,7 +57,6 @@ class ProcessorMotion_test : public ProcessorOdom2D, public testing::Test{ }; - TEST_F(ProcessorMotion_test, IntegrateStraight) { data << 1, 0; // advance straight diff --git a/test/gtest_processor_tracker_feature_trifocal.cpp b/test/gtest_processor_tracker_feature_trifocal.cpp index 71a8e7f46..47e42c577 100644 --- a/test/gtest_processor_tracker_feature_trifocal.cpp +++ b/test/gtest_processor_tracker_feature_trifocal.cpp @@ -1,14 +1,14 @@ #include "utils_gtest.h" -#include "wolf.h" -#include "logging.h" +#include "base/wolf.h" +#include "base/logging.h" #include "vision_utils.h" #include "processors/processor_tracker_feature_trifocal.h" -#include "processor_odom_3D.h" -#include "capture_image.h" -#include "sensor_camera.h" +#include "base/processor/processor_odom_3D.h" +#include "base/capture/capture_image.h" +#include "base/sensor/sensor_camera.h" using namespace Eigen; using namespace wolf; @@ -21,7 +21,6 @@ using namespace wolf; // } //}; - //TEST(ProcessorTrackerFeatureTrifocal, Constructor) //{ // std::cout << "\033[1;33m [WARN]:\033[0m gtest for ProcessorTrackerFeatureTrifocal Constructor is empty." << std::endl; @@ -146,7 +145,6 @@ TEST(ProcessorTrackerFeatureTrifocal, KeyFrameCallback) } } - int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); diff --git a/test/gtest_rotation.cpp b/test/gtest_rotation.cpp index 125d8a3a3..5d656eda2 100644 --- a/test/gtest_rotation.cpp +++ b/test/gtest_rotation.cpp @@ -9,8 +9,8 @@ #include <Eigen/Geometry> //Wolf -#include "wolf.h" -#include "../rotations.h" +#include "base/wolf.h" +#include "base/rotations.h" //std #include <iostream> @@ -27,7 +27,6 @@ using namespace wolf; using namespace Eigen; - TEST(exp_q, unit_norm) { Vector3s v0 = Vector3s::Random(); @@ -41,7 +40,6 @@ TEST(exp_q, unit_norm) } } - TEST(rotations, pi2pi) { ASSERT_NEAR(M_PI_2, pi2pi((Scalar)M_PI_2), 1e-10); @@ -219,7 +217,6 @@ TEST(rotations, AngleAxis_limits) } } - TEST(compose, Quat_compos_const_rateOfTurn) { using namespace wolf; @@ -480,7 +477,6 @@ TEST(Minus_and_diff, Random) q1 .coeffs().setRandom().normalize(); q2 .coeffs().setRandom().normalize(); - Vector3s vr = log_q(q1.conjugate() * q2); Vector3s vl = log_q(q2 * q1.conjugate()); @@ -751,12 +747,10 @@ TEST(Conversions, e2R_R2q_q2e) ASSERT_MATRIX_APPROX(eo, e, 1e-10); } - int main(int argc, char **argv) { using namespace wolf; - /* LIST OF FUNCTIONS : - pi2pi diff --git a/test/gtest_sensor_base.cpp b/test/gtest_sensor_base.cpp index aacedf976..cd809e75c 100644 --- a/test/gtest_sensor_base.cpp +++ b/test/gtest_sensor_base.cpp @@ -5,11 +5,10 @@ * \author: jsola */ -#include "sensor_base.h" +#include "base/sensor/sensor_base.h" #include "utils_gtest.h" - using namespace wolf; TEST(SensorBase, setNoiseStd) diff --git a/test/gtest_shared_from_this.cpp b/test/gtest_shared_from_this.cpp index 99a9983db..cebdce1f8 100644 --- a/test/gtest_shared_from_this.cpp +++ b/test/gtest_shared_from_this.cpp @@ -1,5 +1,5 @@ #include "utils_gtest.h" -#include "node_base.h" +#include "base/node_base.h" class CChildBase; @@ -21,7 +21,6 @@ class CParentBase : public wolf::NodeBase } }; - class CParentDerived : public CParentBase { public: @@ -51,8 +50,6 @@ class CChildDerived : public CChildBase CChildDerived(std::shared_ptr<CParentBase> _parent_ptr) : CChildBase(_parent_ptr){}; }; - - TEST(TestTest, SingleTest) { std::shared_ptr<CParentDerived> parent_derived_ptr = std::make_shared<CParentDerived>(); diff --git a/test/gtest_solver_manager.cpp b/test/gtest_solver_manager.cpp index 62bc03454..9d5fe610b 100644 --- a/test/gtest_solver_manager.cpp +++ b/test/gtest_solver_manager.cpp @@ -6,16 +6,16 @@ */ #include "utils_gtest.h" -#include "../src/logging.h" - -#include "../problem.h" -#include "../sensor_base.h" -#include "../state_block.h" -#include "../capture_void.h" -#include "../constraint_pose_2D.h" -#include "../solver/solver_manager.h" -#include "../local_parametrization_base.h" -#include "../local_parametrization_angle.h" +#include "base/logging.h" + +#include "base/problem.h" +#include "base/sensor/sensor_base.h" +#include "base/state_block.h" +#include "base/capture/capture_void.h" +#include "base/constraint/constraint_pose_2D.h" +#include "base/solver/solver_manager.h" +#include "base/local_parametrization_base.h" +#include "base/local_parametrization_angle.h" #include <iostream> diff --git a/test/gtest_time_stamp.cpp b/test/gtest_time_stamp.cpp index a5feaf39b..aad321781 100644 --- a/test/gtest_time_stamp.cpp +++ b/test/gtest_time_stamp.cpp @@ -1,5 +1,5 @@ #include "utils_gtest.h" -#include "../time_stamp.h" +#include "base/time_stamp.h" #include <thread> diff --git a/test/gtest_track_matrix.cpp b/test/gtest_track_matrix.cpp index 98eedac4d..9d65d8c0f 100644 --- a/test/gtest_track_matrix.cpp +++ b/test/gtest_track_matrix.cpp @@ -7,7 +7,7 @@ #include "utils_gtest.h" -#include "track_matrix.h" +#include "base/track_matrix.h" using namespace wolf; diff --git a/test/gtest_trajectory.cpp b/test/gtest_trajectory.cpp index 92c848039..acbd4f242 100644 --- a/test/gtest_trajectory.cpp +++ b/test/gtest_trajectory.cpp @@ -5,13 +5,12 @@ * Author: jsola */ - #include "utils_gtest.h" -#include "../src/logging.h" +#include "base/logging.h" -#include "../problem.h" -#include "../trajectory_base.h" -#include "../frame_base.h" +#include "base/problem.h" +#include "base/trajectory_base.h" +#include "base/frame_base.h" #include <iostream> @@ -56,7 +55,6 @@ struct DummyNotificationProcessor ProblemPtr problem_; }; - /// Set to true if you want debug info bool debug = false; diff --git a/test/processor_IMU_UnitTester.h b/test/processor_IMU_UnitTester.h index 4c2bece6c..c114086a1 100644 --- a/test/processor_IMU_UnitTester.h +++ b/test/processor_IMU_UnitTester.h @@ -3,8 +3,8 @@ #define PROCESSOR_IMU_UNITTESTER_H // Wolf -#include "processor_IMU.h" -#include "processor_motion.h" +#include "base/processor/processor_IMU.h" +#include "base/processor/processor_motion.h" namespace wolf { struct IMU_jac_bias{ //struct used for checking jacobians by finite difference @@ -65,7 +65,6 @@ namespace wolf { Eigen::Matrix3s dDv_dwb_; Eigen::Matrix3s dDq_dwb_; - public: void copyfrom(IMU_jac_bias const& right){ @@ -197,9 +196,8 @@ namespace wolf { ///////////////////////////////////////////////////////// // Wolf -#include "state_block.h" -#include "rotations.h" - +#include "base/state_block.h" +#include "base/rotations.h" namespace wolf{ diff --git a/test/serialization/cereal/gtest_serialization_eigen_geometry.cpp b/test/serialization/cereal/gtest_serialization_eigen_geometry.cpp index b47964a5d..8532cc9f2 100644 --- a/test/serialization/cereal/gtest_serialization_eigen_geometry.cpp +++ b/test/serialization/cereal/gtest_serialization_eigen_geometry.cpp @@ -5,7 +5,7 @@ * Author: Jeremie Deray */ -#include "../../../wolf.h" +#include "base/wolf.h" #include "../../utils_gtest.h" #include "../../../serialization/cereal/serialization_eigen_geometry.h" diff --git a/test/serialization/cereal/gtest_serialization_eigen_sparse.cpp b/test/serialization/cereal/gtest_serialization_eigen_sparse.cpp index a124da37a..5ca60e7f1 100644 --- a/test/serialization/cereal/gtest_serialization_eigen_sparse.cpp +++ b/test/serialization/cereal/gtest_serialization_eigen_sparse.cpp @@ -5,7 +5,7 @@ * Author: Jeremie Deray */ -#include "../../../wolf.h" +#include "base/wolf.h" #include "../../utils_gtest.h" #include "../../../serialization/cereal/serialization_eigen_sparse.h" diff --git a/test/serialization/cereal/gtest_serialization_local_parametrization.cpp b/test/serialization/cereal/gtest_serialization_local_parametrization.cpp index 667c5d844..3df34a27a 100644 --- a/test/serialization/cereal/gtest_serialization_local_parametrization.cpp +++ b/test/serialization/cereal/gtest_serialization_local_parametrization.cpp @@ -198,7 +198,6 @@ TEST(TestSerialization, SerializationLocalParametrizationHomogeneousPtrBIN) PRINTF("All good at TestSerialization::SerializationLocalParametrizationHomogeneousPtrBIN !\n"); } - ////////////////////////////////////// /// LocalParametrizationQuaternion /// ////////////////////////////////////// diff --git a/test/utils_gtest.h b/test/utils_gtest.h index 0883d86ff..9cb2166e8 100644 --- a/test/utils_gtest.h +++ b/test/utils_gtest.h @@ -11,7 +11,6 @@ #include <gtest/gtest.h> - // Macros for testing equalities and inequalities. // // * {ASSERT|EXPECT}_EQ(expected, actual): Tests that expected == actual @@ -48,7 +47,6 @@ // // * {ASSERT|EXPECT}_NO_FATAL_FAILURE(statement); - // http://stackoverflow.com/a/29155677 namespace testing @@ -145,5 +143,4 @@ TEST(Test, Foo) } // namespace testing - #endif /* WOLF_UTILS_GTEST_H */ diff --git a/wolf_scripts/include_refactor.sh b/wolf_scripts/include_refactor.sh index bae2b8ef1..6d2c2fd8d 100755 --- a/wolf_scripts/include_refactor.sh +++ b/wolf_scripts/include_refactor.sh @@ -1,22 +1,21 @@ #!/bin/bash -# First parameter $1 is the path to the wolf root -# Second parameter $2 is the name of the new folder - -hdrs_folder=$1/include/base/$2 -srcs_folder=$1/src/$2 -#Fix the includes in general -core_headers=$(ag -g .*\\.h -l $1/include/base/$2/ | rev | cut -d '/' -f 1 | rev) -for f in $core_headers; do - ch_files=$(ag \#include[[:space:]]+\"\(.*\/\)*"$2/$f"\" $1/include/base/ -l) - for fp in $ch_files; do - # echo ".h -> "$fp - sed -i -E "s/(#include[[:space:]]+\")("$2"\/)?"$f"\"/\1base\/"$2"\/"$f"\"/gp" $fp - sed -i -E "s/(#include[[:space:]]+<)("$2"\/)?"$f">/\1base\/"$2"\/"$f">/gp" $fp +for ff in $(find ~/workspace/wip/wolf/templinks/ -follow | cut -d '/' -f 8- | grep ".h$\|.cpp$"); do + for f in $(cat ~/workspace/wip/wolf/files.txt); do + path=$(ag -g /$f$ -l ~/workspace/wip/wolf/ | cut -d '/' -f 8-) + matches=$(echo $path | wc -w) + if [ $matches -gt 1 ]; then + # echo $f " -> " $path + path=$(echo $path | cut -d ' ' -f 1) + fi + # echo $f " now in -> " $path " modifying file "$ff + # sed -i -E "s:(#include[[:space:]]+)."$f".:\1\""$path"\":gp" ~/workspace/wip/wolf/$ff + sed -i -E "s:(#include[[:space:]]+).(\.\.\/)+(.+\/)+"$f".:\1\""$path"\":g" ~/workspace/wip/wolf/$ff done - cs_files=$(ag \#include[[:space:]]+\"\(.*\/\)*"$2/$f"\" $1/src/ -l) - for fp in $cs_files; do - # echo ".cpp -> "$fp - sed -i -E "s/(#include[[:space:]]+\")("$2"\/)?"$f"\"/\1base\/"$2"\/"$f"\"/gp" $fp - sed -i -E "s/(#include[[:space:]]+<)("$2"\/)?"$f">/\1base\/"$2"\/"$f">/gp" $fp - done -done \ No newline at end of file +done +# for f in $(cat ~/workspace/wip/wolf/files.txt); do +# path=$(ag -g /$f$ -l ~/workspace/wip/wolf/ | cut -d '/' -f 7-) +# matches=$(echo $path | wc -w) +# if [ $matches -gt 1 ]; then +# echo $f " -> " $(echo $path | cut -d ' ' -f 1) +# fi +# done \ No newline at end of file -- GitLab