├── .gitignore ├── Dockerfile ├── README.md ├── build_and_run.sh ├── docker_shared ├── dataset │ ├── KITTI │ │ ├── 10 │ │ │ ├── bag │ │ │ │ └── .bag │ │ │ ├── gt │ │ │ │ ├── 10_gt.tum │ │ │ │ └── 10_gt_lidar.txt │ │ │ ├── init_tum │ │ │ │ └── loam_opensource.tum │ │ │ └── process_bag.py │ │ ├── 00 │ │ │ ├── bag │ │ │ │ └── .bag │ │ │ ├── gt │ │ │ │ ├── 00_gt.tum │ │ │ │ └── 00_gt_lidar.tum │ │ │ ├── init_tum │ │ │ │ └── loam_opensource.tum │ │ │ └── process_bag.py │ │ ├── 01 │ │ │ ├── bag │ │ │ │ └── .bag │ │ │ ├── gt │ │ │ │ ├── 01_gt.tum │ │ │ │ └── 01_gt_lidar.txt │ │ │ ├── init_tum │ │ │ │ └── loam_opensource.tum │ │ │ └── process_bag.py │ │ ├── 02 │ │ │ ├── bag │ │ │ │ └── .bag │ │ │ ├── gt │ │ │ │ ├── 02_gt.tum │ │ │ │ └── 02_gt_lidar.txt │ │ │ ├── init_tum │ │ │ │ └── loam_opensource.tum │ │ │ └── process_bag.py │ │ ├── 03 │ │ │ ├── bag │ │ │ │ └── .bag │ │ │ ├── gt │ │ │ │ ├── 03_gt.tum │ │ │ │ └── 03_gt_lidar.txt │ │ │ ├── init_tum │ │ │ │ └── loam_opensource.tum │ │ │ └── process_bag.py │ │ ├── 04 │ │ │ ├── bag │ │ │ │ └── .bag │ │ │ ├── gt │ │ │ │ ├── 04_gt.tum │ │ │ │ └── 04_gt_lidar.txt │ │ │ ├── init_tum │ │ │ │ └── loam_opensource.tum │ │ │ └── process_bag.py │ │ ├── 05 │ │ │ ├── bag │ │ │ │ └── .bag │ │ │ ├── gt │ │ │ │ ├── 05_gt.tum │ │ │ │ └── 05_gt_lidar.txt │ │ │ ├── init_tum │ │ │ │ └── loam_opensource.tum │ │ │ └── process_bag.py │ │ ├── 06 │ │ │ ├── bag │ │ │ │ └── .bag │ │ │ ├── gt │ │ │ │ ├── 06_gt.tum │ │ │ │ └── 06_gt_lidar.txt │ │ │ ├── init_tum │ │ │ │ └── loam_opensource.tum │ │ │ └── process_bag.py │ │ ├── 07 │ │ │ ├── bag │ │ │ │ └── .bag │ │ │ ├── gt │ │ │ │ ├── 07_gt.tum │ │ │ │ └── 07_gt_lidar.txt │ │ │ ├── init_tum │ │ │ │ └── loam_opensource.tum │ │ │ └── process_bag.py │ │ ├── 08 │ │ │ ├── bag │ │ │ │ └── .bag │ │ │ ├── gt │ │ │ │ ├── 08_gt.tum │ │ │ │ └── 08_gt_lidar.txt │ │ │ ├── init_tum │ │ │ │ └── loam_opensource.tum │ │ │ └── process_bag.py │ │ └── 09 │ │ │ ├── bag │ │ │ └── .bag │ │ │ ├── gt │ │ │ ├── 09_gt.tum │ │ │ └── 09_gt_lidar.txt │ │ │ ├── init_tum │ │ │ └── loam_opensource.tum │ │ │ └── process_bag.py │ ├── NewerCollege │ │ ├── cloister │ │ │ ├── bag │ │ │ │ └── .bag │ │ │ ├── gt │ │ │ │ └── gt-nc-cloister.csv │ │ │ ├── init_tum │ │ │ │ └── loam_opensource.tum │ │ │ └── process_bag.py │ │ ├── maths_institute │ │ │ ├── bag │ │ │ │ └── .bag │ │ │ ├── gt │ │ │ │ ├── gt_math_easy.csv │ │ │ │ └── tum │ │ │ │ │ └── gt_math_easy.csv │ │ │ ├── init_tum │ │ │ │ └── loam_opensource.tum │ │ │ └── process_bag.py │ │ ├── park │ │ │ ├── bag │ │ │ │ └── .bag │ │ │ ├── gt │ │ │ │ └── gt-nc-park.tum │ │ │ ├── init_tum │ │ │ │ └── loam_opensource.tum │ │ │ └── process_bag.py │ │ ├── quad_easy │ │ │ ├── bag │ │ │ │ ├── .bag │ │ │ │ └── download_quad.sh │ │ │ ├── gt │ │ │ │ └── gt-nc-quad-easy.tum │ │ │ ├── init_tum │ │ │ │ └── loam_opensource.tum │ │ │ └── process_bag.py │ │ ├── stairs │ │ │ ├── bag │ │ │ │ └── .bag │ │ │ ├── gt │ │ │ │ └── gt-nc-stairs.csv │ │ │ ├── init_tum │ │ │ │ └── loam_opensource.tum │ │ │ └── process_bag.py │ │ └── underground_mine │ │ │ ├── bag │ │ │ └── .bag │ │ │ ├── gt │ │ │ └── easy_gt_state_tum.csv │ │ │ ├── init_tum │ │ │ └── loam_opensource.tum │ │ │ └── process_bag.py │ └── VBR │ │ ├── Campus │ │ ├── bag │ │ │ └── download_campus.sh │ │ ├── gt │ │ │ └── campus_train0_gt.txt │ │ ├── init_tum │ │ │ └── loam_opensource.tum │ │ └── process_bag.py │ │ ├── Ciampino │ │ ├── bag │ │ │ └── download_ciampino.sh │ │ ├── gt │ │ │ └── ciampino_train0_gt.txt │ │ ├── init_tum │ │ │ └── loam_opensource.tum │ │ └── process_bag.py │ │ ├── Colosseo │ │ ├── bag │ │ │ ├── download_colloseo.sh │ │ │ └── filter.sh │ │ ├── gt │ │ │ └── colosseo_train0_gt.txt │ │ ├── init_tum │ │ │ └── loam_opensource.tum │ │ └── process_bag.py │ │ ├── DIAG │ │ ├── bag │ │ │ └── download_diag.sh │ │ ├── gt │ │ │ └── diag_train0_gt.txt │ │ ├── init_tum │ │ │ └── loam_opensource.tum │ │ └── process_bag.py │ │ ├── Pincio │ │ ├── bag │ │ │ ├── .bag │ │ │ └── download_pincio.sh │ │ ├── gt │ │ │ └── pincio_train0_gt.txt │ │ ├── init_tum │ │ │ └── loam_opensource.tum │ │ └── process_bag.py │ │ └── Spagna │ │ ├── bag │ │ └── download_spagna.sh │ │ ├── gt │ │ └── spagna_train0_gt.txt │ │ ├── init_tum │ │ └── loam_opensource.tum │ │ └── process_bag.py └── output │ └── .out └── modules ├── mad_ba ├── .vscode │ └── settings.json ├── CMakeLists.txt ├── apps │ ├── CMakeLists.txt │ └── main_app.cpp ├── config │ ├── KITTI │ │ ├── kitti_00_odom.config │ │ ├── kitti_01_odom.config │ │ ├── kitti_02_odom.config │ │ ├── kitti_03_odom.config │ │ ├── kitti_04_odom.config │ │ ├── kitti_05_odom.config │ │ ├── kitti_06_odom.config │ │ ├── kitti_07_odom.config │ │ ├── kitti_08_odom.config │ │ ├── kitti_09_odom.config │ │ └── kitti_10_odom.config │ ├── NewerCollege │ │ ├── cloister.config │ │ ├── maths_institute.config │ │ ├── park.config │ │ ├── quad_easy.config │ │ ├── quad_easy_fast.config │ │ ├── stairs.config │ │ └── underground_mine_easy.config │ └── VBR │ │ ├── campus.config │ │ ├── ciampino.config │ │ ├── colosseo.config │ │ ├── diag.config │ │ ├── pincio.config │ │ └── spagna.config ├── dl.conf ├── package.xml └── src │ ├── CMakeLists.txt │ ├── mad_ba │ ├── CMakeLists.txt │ ├── data_association.cpp │ ├── data_association.h │ ├── instances.cpp │ ├── instances.h │ ├── kd_utils.h │ ├── kdtree.cuh │ ├── kdtree.hpp │ ├── point_cloud_proc.cpp │ ├── point_cloud_proc.h │ ├── point_cloud_proc_visual.cpp │ ├── surfel.cpp │ ├── surfel.h │ ├── surfel_matches.h │ ├── surfelv2.h │ ├── utils.cpp │ └── utils.h │ └── types │ ├── CMakeLists.txt │ ├── instances.cpp │ ├── instances.h │ ├── se3_pose_surfel_factor_ad.cpp │ ├── se3_pose_surfel_factor_ad.h │ ├── variable_se3.cpp │ ├── variable_se3.h │ ├── variable_se3_ad.cpp │ ├── variable_se3_ad.h │ ├── variable_surfel.cpp │ ├── variable_surfel.h │ ├── variable_surfel_ad.cpp │ └── variable_surfel_ad.h ├── rviz_visual_tools ├── .clang-format ├── .clang-tidy ├── .travis.yml ├── CHANGELOG.rst ├── CMakeLists.txt ├── Dockerfile ├── MIGRATION.md ├── README.md ├── icons │ └── classes │ │ └── KeyTool.svg ├── include │ └── rviz_visual_tools │ │ ├── deprecation.h │ │ ├── imarker_simple.h │ │ ├── remote_control.h │ │ ├── remote_reciever.h │ │ ├── rviz_visual_tools.h │ │ └── tf_visual_tools.h ├── launch │ ├── demo.launch │ ├── demo.rviz │ ├── demo_combined.launch │ ├── demo_rviz.launch │ └── rvt_test.launch ├── package.xml ├── plugin_description.xml ├── resources │ └── frame_locking.gif ├── src │ ├── imarker_simple.cpp │ ├── imarker_simple_demo.cpp │ ├── key_tool.cpp │ ├── key_tool.h │ ├── remote_control.cpp │ ├── rviz_visual_tools.cpp │ ├── rviz_visual_tools_demo.cpp │ ├── rviz_visual_tools_gui.cpp │ ├── rviz_visual_tools_gui.h │ └── tf_visual_tools.cpp └── tests │ └── rvt_test.cpp ├── srrg ├── srrg2_core │ ├── LICENSE │ ├── README.md │ ├── docker │ │ ├── Dockerfile │ │ └── build.sh │ ├── srrg2_core │ │ ├── CMakeLists.txt │ │ ├── README.md │ │ ├── apps │ │ │ ├── CMakeLists.txt │ │ │ ├── converter_clf_to_srrg.cpp │ │ │ ├── converter_euroc_to_srrg.cpp │ │ │ ├── converter_icl_to_srrg.cpp │ │ │ ├── converter_kitti_to_srrg.cpp │ │ │ ├── converter_malaga_to_srrg.cpp │ │ │ ├── converter_tum_to_srrg.cpp │ │ │ ├── extract_gt_from_srrg.cpp │ │ │ ├── geigerzaehler.cpp │ │ │ ├── image_matcher_features.cpp │ │ │ └── image_matcher_frobenius.cpp │ │ ├── deprecated │ │ │ ├── ad_multivariate.h │ │ │ ├── srrg_autodiff_bivariate_example.cpp │ │ │ ├── srrg_autodiff_multivariate_example.cpp │ │ │ └── srrg_autodiff_simple_example.cpp │ │ ├── package.xml │ │ ├── slides │ │ │ └── 2018_srrg_core.odp │ │ ├── src │ │ │ ├── CMakeLists.txt │ │ │ ├── TODO.txt │ │ │ ├── examples │ │ │ │ ├── CMakeLists.txt │ │ │ │ ├── boss_example.cpp │ │ │ │ ├── boss_indexed_blob_example.cpp │ │ │ │ ├── boss_property_example.cpp │ │ │ │ ├── boss_shared_example.cpp │ │ │ │ ├── command_line_example.cpp │ │ │ │ ├── container_pack_example.cpp │ │ │ │ ├── deprecated │ │ │ │ │ ├── abstract_container_example.cpp │ │ │ │ │ ├── geometry_example.cpp │ │ │ │ │ ├── image_data_example.cpp │ │ │ │ │ ├── matrix_bilinear_interpolation_example.cpp │ │ │ │ │ └── messages_example.cpp │ │ │ │ ├── field_pack_example.cpp │ │ │ │ ├── image_loading.cpp │ │ │ │ ├── kdtree_example.cpp │ │ │ │ ├── matrix_example.cpp │ │ │ │ ├── message_sorter_example.cpp │ │ │ │ ├── message_synchronizer_example.cpp │ │ │ │ ├── messages_read_example.cpp │ │ │ │ ├── messages_write_example.cpp │ │ │ │ ├── point_cloud_example.cpp │ │ │ │ ├── point_cloud_ops_example.cpp │ │ │ │ ├── shell_colors_example.cpp │ │ │ │ └── timestamp_filter_example.cpp │ │ │ ├── misc_tests │ │ │ │ ├── CMakeLists.txt │ │ │ │ ├── test_cloud_intensity.cpp │ │ │ │ ├── test_distance_map.cpp │ │ │ │ ├── test_key_events.cpp │ │ │ │ ├── test_normal_computation.cpp │ │ │ │ ├── test_platform_synthetic_joints.cpp │ │ │ │ ├── test_platform_synthetic_transforms.cpp │ │ │ │ ├── test_point_cloud_pinhole.cpp │ │ │ │ ├── test_point_cloud_polar.cpp │ │ │ │ ├── test_point_matrix_processor.cpp │ │ │ │ ├── test_point_ops.cpp │ │ │ │ ├── test_viewer_buffer_manager.cpp │ │ │ │ ├── test_viewer_packets_factory.cpp │ │ │ │ ├── test_viewer_packets_serialization.cpp │ │ │ │ └── test_viewer_shared_memory.cpp │ │ │ ├── srrg_benchmark │ │ │ │ ├── slam_benchmark_suite.hpp │ │ │ │ ├── slam_benchmark_suite_carmen.hpp │ │ │ │ ├── slam_benchmark_suite_euroc.hpp │ │ │ │ ├── slam_benchmark_suite_icl.hpp │ │ │ │ ├── slam_benchmark_suite_kitti.hpp │ │ │ │ ├── slam_benchmark_suite_malaga.hpp │ │ │ │ ├── slam_benchmark_suite_tum.hpp │ │ │ │ └── trajectory_writers.h │ │ │ ├── srrg_boss │ │ │ │ ├── CMakeLists.txt │ │ │ │ ├── blob.cpp │ │ │ │ ├── blob.h │ │ │ │ ├── deserializer.cpp │ │ │ │ ├── deserializer.h │ │ │ │ ├── id_context.cpp │ │ │ │ ├── id_context.h │ │ │ │ ├── id_placeholder.cpp │ │ │ │ ├── id_placeholder.h │ │ │ │ ├── identifiable.cpp │ │ │ │ ├── identifiable.h │ │ │ │ ├── json_object_writer.cpp │ │ │ │ ├── json_object_writer.h │ │ │ │ ├── json_recursive_object_parser.cpp │ │ │ │ ├── json_recursive_object_parser.h │ │ │ │ ├── object_data.cpp │ │ │ │ ├── object_data.h │ │ │ │ ├── object_parser.cpp │ │ │ │ ├── object_parser.h │ │ │ │ ├── object_writer.cpp │ │ │ │ ├── object_writer.h │ │ │ │ ├── serializable.cpp │ │ │ │ ├── serializable.h │ │ │ │ ├── serialization_context.cpp │ │ │ │ ├── serialization_context.h │ │ │ │ ├── serializer.cpp │ │ │ │ ├── serializer.h │ │ │ │ ├── stream.cpp │ │ │ │ ├── stream.h │ │ │ │ └── trusted_loaders.h │ │ │ ├── srrg_config │ │ │ │ ├── CMakeLists.txt │ │ │ │ ├── configurable.cpp │ │ │ │ ├── configurable.h │ │ │ │ ├── configurable_command.h │ │ │ │ ├── configurable_manager.h │ │ │ │ ├── configurable_shell.cpp │ │ │ │ ├── configurable_shell.h │ │ │ │ ├── linenoise.c │ │ │ │ ├── linenoise.h │ │ │ │ ├── param_macros.h │ │ │ │ ├── pipeline_runner.cpp │ │ │ │ ├── pipeline_runner.h │ │ │ │ ├── preemptible.cpp │ │ │ │ ├── preemptible.h │ │ │ │ ├── preemptible_example.cpp │ │ │ │ ├── property_configurable.h │ │ │ │ └── property_configurable_vector.h │ │ │ ├── srrg_data_structures │ │ │ │ ├── CMakeLists.txt │ │ │ │ ├── abstract_map.h │ │ │ │ ├── abstract_map_view.h │ │ │ │ ├── abstract_ptr_map.h │ │ │ │ ├── container_pack.h │ │ │ │ ├── correspondence.h │ │ │ │ ├── events.h │ │ │ │ ├── field_pack.h │ │ │ │ ├── file_handle.cpp │ │ │ │ ├── file_handle.h │ │ │ │ ├── file_vector_interface.h │ │ │ │ ├── file_vector_interface_test.cpp │ │ │ │ ├── generic_class_method_caller.h │ │ │ │ ├── graph.cpp │ │ │ │ ├── graph.h │ │ │ │ ├── grid_map_2d.cpp │ │ │ │ ├── grid_map_2d.h │ │ │ │ ├── indexed_container_interface.h │ │ │ │ ├── iterator_interface.h │ │ │ │ ├── kd_tree.h │ │ │ │ ├── kd_tree_impl.cpp │ │ │ │ ├── link.cpp │ │ │ │ ├── link.h │ │ │ │ ├── matrix.h │ │ │ │ ├── matrix.hpp │ │ │ │ ├── memory_mapped_file.cpp │ │ │ │ ├── memory_vector_interface.h │ │ │ │ ├── path_matrix.cpp │ │ │ │ ├── path_matrix.h │ │ │ │ ├── path_matrix_astar_search.cpp │ │ │ │ ├── path_matrix_astar_search.h │ │ │ │ ├── path_matrix_clusterer_search.cpp │ │ │ │ ├── path_matrix_clusterer_search.h │ │ │ │ ├── path_matrix_cost_search.cpp │ │ │ │ ├── path_matrix_cost_search.h │ │ │ │ ├── path_matrix_dijkstra_search.cpp │ │ │ │ ├── path_matrix_dijkstra_search.h │ │ │ │ ├── path_matrix_distance_search.cpp │ │ │ │ ├── path_matrix_distance_search.h │ │ │ │ ├── path_matrix_search_base.h │ │ │ │ ├── platform.cpp │ │ │ │ ├── platform.h │ │ │ │ ├── pointer_traits.h │ │ │ │ ├── traverse_line.h │ │ │ │ ├── traverse_line_main.cpp │ │ │ │ └── vector_interface.h │ │ │ ├── srrg_geometry │ │ │ │ ├── CMakeLists.txt │ │ │ │ ├── ad.h │ │ │ │ ├── ad_quaternion.h │ │ │ │ ├── deprecated │ │ │ │ │ ├── point_types.cpp │ │ │ │ │ └── point_types.h │ │ │ │ ├── epipolar.cpp │ │ │ │ ├── epipolar.h │ │ │ │ ├── geometry2d.h │ │ │ │ ├── geometry3d.h │ │ │ │ ├── geometry_defs.h │ │ │ │ ├── kahan_adder.h │ │ │ │ ├── permutation_sampler.cpp │ │ │ │ ├── permutation_sampler.h │ │ │ │ └── similiarity.hpp │ │ │ ├── srrg_image │ │ │ │ ├── CMakeLists.txt │ │ │ │ ├── image.cpp │ │ │ │ ├── image.h │ │ │ │ ├── image.hpp │ │ │ │ ├── image_data.cpp │ │ │ │ └── image_data.h │ │ │ ├── srrg_matchable │ │ │ │ ├── CMakeLists.txt │ │ │ │ ├── descriptor_owner.h │ │ │ │ ├── instances.cpp │ │ │ │ ├── instances.h │ │ │ │ ├── matchable.h │ │ │ │ ├── matchable.hpp │ │ │ │ └── visual_matchable.h │ │ │ ├── srrg_messages │ │ │ │ ├── CMakeLists.txt │ │ │ │ ├── deprecated │ │ │ │ │ ├── joint_state_message.cpp │ │ │ │ │ ├── joint_state_message.h │ │ │ │ │ ├── static_transform.cpp │ │ │ │ │ ├── static_transform.h │ │ │ │ │ ├── static_transform_tree.cpp │ │ │ │ │ ├── static_transform_tree.h │ │ │ │ │ ├── static_transform_tree_message.cpp │ │ │ │ │ ├── static_transform_tree_message.h │ │ │ │ │ ├── transform_stamped_message.cpp │ │ │ │ │ └── transform_stamped_message.h │ │ │ │ ├── instances.cpp │ │ │ │ ├── instances.h │ │ │ │ ├── message_handlers │ │ │ │ │ ├── image_message_viewer.cpp │ │ │ │ │ ├── image_message_viewer.h │ │ │ │ │ ├── message_container_odom_accumulator_sink.cpp │ │ │ │ │ ├── message_container_odom_accumulator_sink.h │ │ │ │ │ ├── message_file_sink.cpp │ │ │ │ │ ├── message_file_sink.h │ │ │ │ │ ├── message_file_sink_base.cpp │ │ │ │ │ ├── message_file_sink_base.h │ │ │ │ │ ├── message_file_source.cpp │ │ │ │ │ ├── message_file_source.h │ │ │ │ │ ├── message_file_source_base.cpp │ │ │ │ │ ├── message_file_source_base.h │ │ │ │ │ ├── message_filter_base.cpp │ │ │ │ │ ├── message_filter_base.h │ │ │ │ │ ├── message_odom_subsampler_sink.cpp │ │ │ │ │ ├── message_odom_subsampler_sink.h │ │ │ │ │ ├── message_odom_subsampler_source.cpp │ │ │ │ │ ├── message_odom_subsampler_source.h │ │ │ │ │ ├── message_pack.cpp │ │ │ │ │ ├── message_pack.h │ │ │ │ │ ├── message_platform_listener_sink.cpp │ │ │ │ │ ├── message_platform_listener_sink.h │ │ │ │ │ ├── message_selector_sink.cpp │ │ │ │ │ ├── message_selector_sink.h │ │ │ │ │ ├── message_sink_base.cpp │ │ │ │ │ ├── message_sink_base.h │ │ │ │ │ ├── message_sorted_sink.cpp │ │ │ │ │ ├── message_sorted_sink.h │ │ │ │ │ ├── message_sorted_source.cpp │ │ │ │ │ ├── message_sorted_source.h │ │ │ │ │ ├── message_source_base.cpp │ │ │ │ │ ├── message_source_base.h │ │ │ │ │ ├── message_source_platform.cpp │ │ │ │ │ ├── message_source_platform.h │ │ │ │ │ ├── message_synchronized_sink.cpp │ │ │ │ │ ├── message_synchronized_sink.h │ │ │ │ │ ├── message_synchronized_source.cpp │ │ │ │ │ └── message_synchronized_source.h │ │ │ │ └── messages │ │ │ │ │ ├── base_sensor_message.cpp │ │ │ │ │ ├── base_sensor_message.h │ │ │ │ │ ├── camera_info_message.cpp │ │ │ │ │ ├── camera_info_message.h │ │ │ │ │ ├── cmd_vel_message.cpp │ │ │ │ │ ├── cmd_vel_message.h │ │ │ │ │ ├── grid_map_message.cpp │ │ │ │ │ ├── grid_map_message.h │ │ │ │ │ ├── image_message.cpp │ │ │ │ │ ├── image_message.h │ │ │ │ │ ├── imu_message.cpp │ │ │ │ │ ├── imu_message.h │ │ │ │ │ ├── joints_message.cpp │ │ │ │ │ ├── joints_message.h │ │ │ │ │ ├── laser_message.cpp │ │ │ │ │ ├── laser_message.h │ │ │ │ │ ├── navsat_fix_message.cpp │ │ │ │ │ ├── navsat_fix_message.h │ │ │ │ │ ├── odometry_message.cpp │ │ │ │ │ ├── odometry_message.h │ │ │ │ │ ├── path_message.cpp │ │ │ │ │ ├── path_message.h │ │ │ │ │ ├── planner_status_message.cpp │ │ │ │ │ ├── planner_status_message.h │ │ │ │ │ ├── point_cloud2_message.cpp │ │ │ │ │ ├── point_cloud2_message.h │ │ │ │ │ ├── point_field.cpp │ │ │ │ │ ├── point_field.h │ │ │ │ │ ├── point_stamped_message.cpp │ │ │ │ │ ├── point_stamped_message.h │ │ │ │ │ ├── pose_array_message.cpp │ │ │ │ │ ├── pose_array_message.h │ │ │ │ │ ├── pose_message.cpp │ │ │ │ │ ├── pose_message.h │ │ │ │ │ ├── pose_stamped_message.cpp │ │ │ │ │ ├── pose_stamped_message.h │ │ │ │ │ ├── pose_with_covariance_stamped_message.cpp │ │ │ │ │ ├── pose_with_covariance_stamped_message.h │ │ │ │ │ ├── range_message.cpp │ │ │ │ │ ├── range_message.h │ │ │ │ │ ├── ticks_message.cpp │ │ │ │ │ ├── ticks_message.h │ │ │ │ │ ├── transform_events_message.cpp │ │ │ │ │ ├── transform_events_message.h │ │ │ │ │ ├── twist_stamped_message.cpp │ │ │ │ │ └── twist_stamped_message.h │ │ │ ├── srrg_pcl │ │ │ │ ├── CMakeLists.txt │ │ │ │ ├── camera_matrix_owner.h │ │ │ │ ├── instances.cpp │ │ │ │ ├── instances.h │ │ │ │ ├── lidar_3d_utils │ │ │ │ │ ├── lidar_3d_lookup_projector.h │ │ │ │ │ ├── lidar_3d_lookup_projector.hpp │ │ │ │ │ ├── lidar_3d_sensor.h │ │ │ │ │ ├── lidar_3d_utils.h │ │ │ │ │ ├── spacing.cpp │ │ │ │ │ └── spacing.h │ │ │ │ ├── normal_computator.h │ │ │ │ ├── normal_computator.hpp │ │ │ │ ├── point.h │ │ │ │ ├── point_base.h │ │ │ │ ├── point_cloud.h │ │ │ │ ├── point_color.h │ │ │ │ ├── point_default_field.h │ │ │ │ ├── point_defs.h │ │ │ │ ├── point_derived.h │ │ │ │ ├── point_descriptor_field.cpp │ │ │ │ ├── point_descriptor_field.h │ │ │ │ ├── point_example.h │ │ │ │ ├── point_fields.h │ │ │ │ ├── point_id.h │ │ │ │ ├── point_intensity.h │ │ │ │ ├── point_intensity_descriptor.h │ │ │ │ ├── point_matrix_processor.h │ │ │ │ ├── point_matrix_processor.hpp │ │ │ │ ├── point_normal.h │ │ │ │ ├── point_normal_color.h │ │ │ │ ├── point_normal_curvature.h │ │ │ │ ├── point_normal_curvature_color.h │ │ │ │ ├── point_normal_intensity.h │ │ │ │ ├── point_ops.h │ │ │ │ ├── point_projector.h │ │ │ │ ├── point_projector_lidar3d.h │ │ │ │ ├── point_projector_lidar3d.hpp │ │ │ │ ├── point_projector_lidar3d_types.h │ │ │ │ ├── point_projector_pinhole.h │ │ │ │ ├── point_projector_polar.h │ │ │ │ ├── point_projector_types.h │ │ │ │ ├── point_scalar_field.h │ │ │ │ ├── point_statistics_field.h │ │ │ │ ├── point_types.h │ │ │ │ ├── point_types_data.h │ │ │ │ ├── point_unprojector.h │ │ │ │ ├── point_unprojector_lidar3d.h │ │ │ │ ├── point_unprojector_lidar3d.hpp │ │ │ │ ├── point_unprojector_lidar3d_types.h │ │ │ │ ├── point_unprojector_types.h │ │ │ │ ├── point_utils.h │ │ │ │ └── point_vector_field.h │ │ │ ├── srrg_property │ │ │ │ ├── CMakeLists.txt │ │ │ │ ├── property.cpp │ │ │ │ ├── property.h │ │ │ │ ├── property_container.cpp │ │ │ │ ├── property_container.h │ │ │ │ ├── property_container_manager.cpp │ │ │ │ ├── property_container_manager.h │ │ │ │ ├── property_eigen.cpp │ │ │ │ ├── property_eigen.h │ │ │ │ ├── property_identifiable.cpp │ │ │ │ ├── property_identifiable.h │ │ │ │ ├── property_macros.h │ │ │ │ ├── property_pair.h │ │ │ │ ├── property_serializable.h │ │ │ │ ├── property_vector.h │ │ │ │ └── vector_data.h │ │ │ ├── srrg_system_utils │ │ │ │ ├── CMakeLists.txt │ │ │ │ ├── char_array.h │ │ │ │ ├── chrono.cpp │ │ │ │ ├── chrono.h │ │ │ │ ├── env.cpp │ │ │ │ ├── env.h │ │ │ │ ├── parse_command_line.cpp │ │ │ │ ├── parse_command_line.h │ │ │ │ ├── profiler.cpp │ │ │ │ ├── profiler.h │ │ │ │ ├── shell_colors.h │ │ │ │ ├── stream_helpers.h │ │ │ │ ├── stream_helpers_hex.h │ │ │ │ ├── system_utils.cpp │ │ │ │ ├── system_utils.h │ │ │ │ └── timestamp_filter.h │ │ │ ├── srrg_test │ │ │ │ ├── synthetic_world.hpp │ │ │ │ └── test_helper.hpp │ │ │ └── srrg_viewer │ │ │ │ ├── CMakeLists.txt │ │ │ │ ├── active_drawable.cpp │ │ │ │ ├── active_drawable.h │ │ │ │ ├── drawable_base.cpp │ │ │ │ ├── drawable_base.h │ │ │ │ ├── viewer_canvas.cpp │ │ │ │ ├── viewer_canvas.h │ │ │ │ ├── viewer_context_base.cpp │ │ │ │ ├── viewer_context_base.h │ │ │ │ ├── viewer_context_shared.cpp │ │ │ │ ├── viewer_context_shared.h │ │ │ │ ├── viewer_core │ │ │ │ ├── CMakeLists.txt │ │ │ │ ├── buffer_deque.h │ │ │ │ ├── buffer_manager.cpp │ │ │ │ ├── buffer_manager.h │ │ │ │ ├── buffer_memory.cpp │ │ │ │ ├── buffer_memory.h │ │ │ │ ├── buffer_sink.h │ │ │ │ ├── buffer_sink_shared.cpp │ │ │ │ ├── buffer_sink_shared.h │ │ │ │ ├── buffer_source.h │ │ │ │ ├── buffer_source_shared.cpp │ │ │ │ ├── buffer_source_shared.h │ │ │ │ ├── color_palette.h │ │ │ │ ├── packet_deserializer.cpp │ │ │ │ ├── packet_deserializer.h │ │ │ │ ├── packet_factory.h │ │ │ │ ├── packet_processor.h │ │ │ │ ├── packet_serializer.cpp │ │ │ │ ├── packet_serializer.h │ │ │ │ ├── packets.h │ │ │ │ ├── packets │ │ │ │ │ ├── packet_array.h │ │ │ │ │ ├── packet_base.h │ │ │ │ │ ├── packet_cv_mat.h │ │ │ │ │ ├── packet_eigen_object.h │ │ │ │ │ ├── packet_gl_list.h │ │ │ │ │ ├── packet_matchable_vector_cloud.h │ │ │ │ │ ├── packet_point_matrix_cloud.h │ │ │ │ │ ├── packet_point_vector_cloud.h │ │ │ │ │ ├── packet_scalar.h │ │ │ │ │ ├── packet_stl_vector.h │ │ │ │ │ ├── packet_string.h │ │ │ │ │ └── readme.md │ │ │ │ ├── semaphore.cpp │ │ │ │ ├── semaphore.h │ │ │ │ ├── viewport.cpp │ │ │ │ └── viewport.h │ │ │ │ ├── viewer_core_base.cpp │ │ │ │ ├── viewer_core_base.h │ │ │ │ ├── viewer_manager_base.cpp │ │ │ │ ├── viewer_manager_base.h │ │ │ │ ├── viewer_manager_shared.cpp │ │ │ │ └── viewer_manager_shared.h │ │ └── tests │ │ │ ├── CMakeLists.txt │ │ │ ├── test_abstract_map.cpp │ │ │ ├── test_epipolar.cpp │ │ │ ├── test_geometry.cpp │ │ │ ├── test_image_data.cpp │ │ │ ├── test_kd_tree.cpp │ │ │ ├── test_matchables.cpp │ │ │ ├── test_matrix_bilinear_interpolation.cpp │ │ │ ├── test_message_sources.cpp │ │ │ ├── test_messages.cpp │ │ │ ├── test_point_intensity_descriptor.cpp │ │ │ ├── test_point_statistics_field.cpp │ │ │ ├── test_points.cpp │ │ │ ├── test_profiler.cpp │ │ │ ├── test_projector.cpp │ │ │ ├── test_similiarity.cpp │ │ │ ├── test_streaming_configurable.cpp │ │ │ ├── test_unprojector.cpp │ │ │ └── test_viewer_packets.cpp │ └── srrg2_core_ros │ │ ├── CMakeLists.txt │ │ ├── README.md │ │ ├── msg │ │ ├── PlannerStatusMessage.msg │ │ └── ViewerBufferMessage.msg │ │ ├── package.xml │ │ └── src │ │ ├── CMakeLists.txt │ │ ├── app │ │ ├── CMakeLists.txt │ │ ├── extract_gt_from_ros.cpp │ │ ├── rosbag_to_srrg_converter_app.cpp │ │ └── srrg_to_rosbag_converter_app.cpp │ │ ├── node │ │ ├── CMakeLists.txt │ │ ├── ros_to_srrg_listener_node.cpp │ │ └── srrg_to_ros_publisher_node.cpp │ │ ├── srrg_benchmark_ros │ │ └── slam_benchmark_suite_simul.hpp │ │ ├── srrg_converters │ │ ├── CMakeLists.txt │ │ ├── converter.cpp │ │ ├── converter.h │ │ └── translator_utils.h │ │ ├── srrg_messages_ros │ │ ├── CMakeLists.txt │ │ ├── instances.cpp │ │ ├── instances.h │ │ └── message_handlers │ │ │ ├── message_ros_sink.cpp │ │ │ ├── message_ros_sink.h │ │ │ ├── message_ros_source.cpp │ │ │ ├── message_ros_source.h │ │ │ ├── message_rosbag_sink.cpp │ │ │ ├── message_rosbag_sink.h │ │ │ ├── message_rosbag_source.cpp │ │ │ └── message_rosbag_source.h │ │ ├── srrg_viewer_ros │ │ ├── CMakeLists.txt │ │ ├── viewer_context_ros.cpp │ │ ├── viewer_context_ros.h │ │ ├── viewer_core_ros │ │ │ ├── CMakeLists.txt │ │ │ ├── buffer_publisher.cpp │ │ │ ├── buffer_publisher.h │ │ │ ├── buffer_sink_ros.cpp │ │ │ ├── buffer_sink_ros.h │ │ │ ├── buffer_source_ros.cpp │ │ │ ├── buffer_source_ros.h │ │ │ ├── buffer_subscriber.cpp │ │ │ └── buffer_subscriber.h │ │ ├── viewer_manager_ros.cpp │ │ └── viewer_manager_ros.h │ │ └── tests │ │ ├── CMakeLists.txt │ │ ├── test_bag_creator.cpp │ │ ├── test_buffer_message.cpp │ │ ├── test_buffer_sink_ros.cpp │ │ ├── test_buffer_source_ros.cpp │ │ ├── test_conversion_in_place.cpp │ │ ├── test_generic_listener_node.cpp │ │ ├── test_get_all_topics.cpp │ │ ├── test_message_source_platform.cpp │ │ ├── test_message_source_ros.cpp │ │ ├── test_rviz.cpp │ │ ├── test_tf_from_source.cpp │ │ ├── test_tf_listener_node.cpp │ │ ├── test_tf_listener_playback.cpp │ │ └── test_tf_publisher_node.cpp ├── srrg2_executor │ ├── LICENSE │ ├── configs │ │ ├── dl.conf │ │ └── mc.conf │ ├── readme.md │ ├── results │ │ └── tum │ │ │ └── ate.txt │ ├── srrg2_executor │ │ ├── CMakeLists.txt │ │ ├── package.xml │ │ └── src │ │ │ └── apps │ │ │ ├── auto_dl_finder.cpp │ │ │ └── srrg2_shell.cpp │ └── srrg2_executor_nogui │ │ ├── CMakeLists.txt │ │ ├── package.xml │ │ └── src │ │ └── apps │ │ └── srrg2_shell.cpp ├── srrg2_qgl_viewport │ ├── .clang-format │ ├── .gitignore │ ├── .gitlab-ci-disabled.yml │ ├── LICENSE │ ├── srrg2_qgl_viewport │ │ ├── CMakeLists.txt │ │ ├── README.md │ │ ├── package.xml │ │ └── src │ │ │ ├── CMakeLists.txt │ │ │ ├── srrg_qgl_viewport │ │ │ ├── CMakeLists.txt │ │ │ ├── gl_helpers.cpp │ │ │ ├── gl_helpers.h │ │ │ ├── qgl_packet_processor.cpp │ │ │ ├── qgl_packet_processor.h │ │ │ ├── qgl_viewport.cpp │ │ │ ├── qgl_viewport.h │ │ │ ├── viewer_core_base_qgl.cpp │ │ │ ├── viewer_core_base_qgl.h │ │ │ ├── viewer_core_shared_qgl.cpp │ │ │ └── viewer_core_shared_qgl.h │ │ │ └── tests │ │ │ ├── CMakeLists.txt │ │ │ ├── qt_image_viewport.cpp │ │ │ ├── qt_image_viewport.h │ │ │ ├── test_cv_gl_interaction.cpp │ │ │ ├── test_image_rendering.cpp │ │ │ ├── test_multicanvas.cpp │ │ │ ├── test_opencv_shared.cpp │ │ │ ├── test_viewer_core_multicanvas.cpp │ │ │ ├── test_viewer_manager_shared.cpp │ │ │ └── test_viewport.cpp │ └── srrg2_qgl_viewport_ros │ │ ├── CMakeLists.txt │ │ ├── README.md │ │ ├── package.xml │ │ └── src │ │ ├── CMakeLists.txt │ │ ├── app │ │ ├── CMakeLists.txt │ │ └── srrg_viewer_ros_client.cpp │ │ ├── srrg_qgl_viewport_ros │ │ ├── CMakeLists.txt │ │ ├── viewer_core_ros_qgl.cpp │ │ └── viewer_core_ros_qgl.h │ │ └── tests │ │ ├── CMakeLists.txt │ │ ├── test_viewer_core_client.cpp │ │ ├── test_viewer_core_server.cpp │ │ ├── test_viewer_core_server_multicanvas.cpp │ │ ├── test_viewer_manager_client.cpp │ │ └── test_viewer_manager_server.cpp ├── srrg2_solver │ ├── .clang-format │ ├── .gitignore │ ├── .gitlab-ci-disabled.yml │ ├── LICENSE │ ├── docker │ │ ├── Dockerfile │ │ ├── build.sh │ │ └── dl.conf │ ├── readme.md │ └── srrg2_solver │ │ ├── CMakeLists.txt │ │ ├── app │ │ ├── CMakeLists.txt │ │ ├── README.md │ │ ├── converter │ │ │ ├── CMakeLists.txt │ │ │ ├── g2o_converter.cpp │ │ │ ├── g2o_converter.h │ │ │ ├── g2o_converter_action_base.h │ │ │ ├── g2o_converter_action_se2.cpp │ │ │ ├── g2o_converter_action_se2.h │ │ │ ├── g2o_converter_action_se3.cpp │ │ │ ├── g2o_converter_action_se3.h │ │ │ ├── g2o_converter_actions.h │ │ │ └── solver_app_graph_converter.cpp │ │ ├── graph_manipulators │ │ │ ├── CMakeLists.txt │ │ │ ├── factor_noise_adder.h │ │ │ ├── solver_app_graph_evaluator.cpp │ │ │ ├── solver_app_graph_gt_generator.cpp │ │ │ ├── solver_app_graph_incremental_reader.cpp │ │ │ ├── solver_app_graph_initializer.cpp │ │ │ ├── solver_app_graph_sorter.cpp │ │ │ ├── solver_app_info_overwrite.cpp │ │ │ └── solver_app_noise_adder.cpp │ │ ├── optimizer │ │ │ ├── CMakeLists.txt │ │ │ ├── solver_app_graph_optimizer.cpp │ │ │ └── solver_app_incremental_optimizer.cpp │ │ ├── plgo_simulator │ │ │ ├── CMakeLists.txt │ │ │ ├── graph_generator.cpp │ │ │ ├── graph_generator.h │ │ │ ├── plgo_simulator.cpp │ │ │ ├── plgo_simulator.h │ │ │ ├── sim_record.cpp │ │ │ ├── sim_record.h │ │ │ ├── solver_app_plgo_simulator.cpp │ │ │ ├── trajectory_generator.cpp │ │ │ └── trajectory_generator.h │ │ ├── plgo_simulator2 │ │ │ ├── CMakeLists.txt │ │ │ ├── instances.cpp │ │ │ ├── instances.h │ │ │ ├── sim.conf │ │ │ ├── sim_robot.cpp │ │ │ ├── sim_robot.h │ │ │ ├── sim_sensor_3d_points_base.cpp │ │ │ ├── sim_sensor_3d_points_base.h │ │ │ ├── sim_sensor_3d_points_fov_limited.cpp │ │ │ ├── sim_sensor_3d_points_fov_limited.h │ │ │ ├── sim_sensor_3d_poses.cpp │ │ │ ├── sim_sensor_3d_poses.h │ │ │ ├── sim_sensor_base.h │ │ │ ├── sim_sensor_mode_3d_landmarks.cpp │ │ │ ├── sim_sensor_mode_3d_landmarks.h │ │ │ ├── sim_sensor_mode_3d_poses.cpp │ │ │ ├── sim_sensor_mode_3d_poses.h │ │ │ ├── sim_sensor_mode_base.cpp │ │ │ ├── sim_sensor_mode_base.h │ │ │ ├── sim_sensor_omnicam.cpp │ │ │ ├── sim_sensor_omnicam.h │ │ │ ├── sim_sensor_pinhole_camera.cpp │ │ │ ├── sim_sensor_pinhole_camera.h │ │ │ ├── sim_sensor_rectified_stereo.cpp │ │ │ ├── sim_sensor_rectified_stereo.h │ │ │ ├── sim_sensors.h │ │ │ ├── solver_app_plgo_simulator.cpp │ │ │ ├── trajectory_generator.cpp │ │ │ └── trajectory_generator.h │ │ └── sfm │ │ │ ├── CMakeLists.txt │ │ │ ├── essential2orientations.cpp │ │ │ ├── essential2orientations_constrained.cpp │ │ │ ├── essential2translations.cpp │ │ │ ├── essential_translation_factor.cpp │ │ │ ├── essential_translation_factor.h │ │ │ ├── off_line_monocular_sfm_test.cpp │ │ │ ├── ovt.conf │ │ │ ├── plgo2sfm.cpp │ │ │ ├── script.sh │ │ │ ├── se3_epipolar_ba_factor_test.cpp │ │ │ ├── sfm2ba.cpp │ │ │ ├── sfm2eba.cpp │ │ │ ├── sfm2essential.cpp │ │ │ ├── sfm_common.cpp │ │ │ └── sfm_common.h │ │ ├── doc │ │ └── Doxygen │ │ ├── examples │ │ ├── .gitlab-ci.yml │ │ ├── CMakeLists.txt │ │ ├── data │ │ │ ├── KittiGps.txt │ │ │ ├── KittiImuBiased.txt │ │ │ ├── KittiImuBiasedMetadata.txt │ │ │ ├── matchable_graph.boss │ │ │ ├── odometry_imu_data1.txt │ │ │ ├── odometry_imu_data2.txt │ │ │ ├── pose_graph_100.boss │ │ │ ├── pose_graph_100.g2o │ │ │ └── pose_matchable_graph.g2o │ │ ├── example_constrained_estimation.cpp │ │ ├── example_factor_graph_visit.cpp │ │ ├── example_kitti_imu_gps.cpp │ │ ├── example_se2_bearing_only_slam.cpp │ │ ├── example_se2_icp.cpp │ │ ├── example_se3_pgo.cpp │ │ └── example_sparse_solver_block_inverse.cpp │ │ ├── package.xml │ │ ├── slides │ │ └── 2018_srrg_solver.odp │ │ ├── src │ │ ├── CMakeLists.txt │ │ └── srrg_solver │ │ │ ├── CMakeLists.txt │ │ │ ├── README.txt │ │ │ ├── solver_core │ │ │ ├── CMakeLists.txt │ │ │ ├── ad_constraint_factor.h │ │ │ ├── ad_constraint_factor_impl.cpp │ │ │ ├── ad_error_factor.h │ │ │ ├── ad_error_factor_impl.cpp │ │ │ ├── ad_variable.h │ │ │ ├── constraint_factor.h │ │ │ ├── constraint_factor_impl.cpp │ │ │ ├── error_factor.h │ │ │ ├── error_factor_impl.cpp │ │ │ ├── factor.h │ │ │ ├── factor_base.cpp │ │ │ ├── factor_base.h │ │ │ ├── factor_correspondence_driven.h │ │ │ ├── factor_correspondence_driven_dynamic.h │ │ │ ├── factor_graph.cpp │ │ │ ├── factor_graph.h │ │ │ ├── factor_graph_interface.cpp │ │ │ ├── factor_graph_interface.h │ │ │ ├── factor_graph_view.cpp │ │ │ ├── factor_graph_view.h │ │ │ ├── factor_impl.cpp │ │ │ ├── factor_updaters.h │ │ │ ├── instance_macros.h │ │ │ ├── instances.cpp │ │ │ ├── instances.h │ │ │ ├── internals │ │ │ │ ├── CMakeLists.txt │ │ │ │ ├── linear_solvers │ │ │ │ │ ├── CMakeLists.txt │ │ │ │ │ ├── instances.cpp │ │ │ │ │ ├── instances.h │ │ │ │ │ ├── quotient_graph.cpp │ │ │ │ │ ├── quotient_graph.h │ │ │ │ │ ├── sparse_block_linear_solver.cpp │ │ │ │ │ ├── sparse_block_linear_solver.h │ │ │ │ │ ├── sparse_block_linear_solver_cholesky.cpp │ │ │ │ │ ├── sparse_block_linear_solver_cholesky.h │ │ │ │ │ ├── sparse_block_linear_solver_cholesky_cholmod.cpp │ │ │ │ │ ├── sparse_block_linear_solver_cholesky_cholmod.h │ │ │ │ │ ├── sparse_block_linear_solver_cholesky_csparse.cpp │ │ │ │ │ ├── sparse_block_linear_solver_cholesky_csparse.h │ │ │ │ │ ├── sparse_block_linear_solver_cholesky_diagonal.cpp │ │ │ │ │ ├── sparse_block_linear_solver_cholesky_diagonal.h │ │ │ │ │ ├── sparse_block_linear_solver_cholesky_emd.cpp │ │ │ │ │ ├── sparse_block_linear_solver_cholesky_emd.h │ │ │ │ │ ├── sparse_block_linear_solver_cholmod_full.cpp │ │ │ │ │ ├── sparse_block_linear_solver_cholmod_full.h │ │ │ │ │ ├── sparse_block_linear_solver_ldl.cpp │ │ │ │ │ ├── sparse_block_linear_solver_ldl.h │ │ │ │ │ ├── sparse_block_linear_solver_nullspace.cpp │ │ │ │ │ └── sparse_block_linear_solver_nullspace.h │ │ │ │ └── sparse_block_matrix │ │ │ │ │ ├── CMakeLists.txt │ │ │ │ │ ├── c_matrix_ops.hpp │ │ │ │ │ ├── matrix_block.cpp │ │ │ │ │ ├── matrix_block.h │ │ │ │ │ ├── matrix_block_factory.cpp │ │ │ │ │ ├── matrix_block_factory.h │ │ │ │ │ ├── sparse_block_cholesky.cpp │ │ │ │ │ ├── sparse_block_cholesky.h │ │ │ │ │ ├── sparse_block_matrix.cpp │ │ │ │ │ └── sparse_block_matrix.h │ │ │ ├── iteration_algorithm_base.cpp │ │ │ ├── iteration_algorithm_base.h │ │ │ ├── iteration_algorithm_ddl.cpp │ │ │ ├── iteration_algorithm_ddl.h │ │ │ ├── iteration_algorithm_dl.cpp │ │ │ ├── iteration_algorithm_dl.h │ │ │ ├── iteration_algorithm_gn.cpp │ │ │ ├── iteration_algorithm_gn.h │ │ │ ├── iteration_algorithm_gn_ls.cpp │ │ │ ├── iteration_algorithm_gn_ls.h │ │ │ ├── iteration_algorithm_gn_pd.cpp │ │ │ ├── iteration_algorithm_gn_pd.h │ │ │ ├── iteration_algorithm_lm.cpp │ │ │ ├── iteration_algorithm_lm.h │ │ │ ├── measurement_owner.h │ │ │ ├── measurement_owner_impl.cpp │ │ │ ├── robustifier.cpp │ │ │ ├── robustifier.h │ │ │ ├── robustifier_policy.cpp │ │ │ ├── robustifier_policy.h │ │ │ ├── solver.cpp │ │ │ ├── solver.h │ │ │ ├── solver.hpp │ │ │ ├── solver_action_base.cpp │ │ │ ├── solver_action_base.h │ │ │ ├── solver_action_draw.cpp │ │ │ ├── solver_action_draw.h │ │ │ ├── solver_stats.cpp │ │ │ ├── solver_stats.h │ │ │ ├── solvers_all_impl.cpp │ │ │ ├── termination_criteria.cpp │ │ │ ├── termination_criteria.h │ │ │ ├── unscented.h │ │ │ ├── unscented_approx.h │ │ │ ├── variable.h │ │ │ ├── variable_impl.cpp │ │ │ └── variable_ptr_tuple.h │ │ │ ├── solver_incremental │ │ │ ├── CMakeLists.txt │ │ │ ├── factor_graph_incremental_sorter.cpp │ │ │ ├── factor_graph_incremental_sorter.h │ │ │ ├── instances.cpp │ │ │ ├── instances.h │ │ │ ├── solver_incremental.cpp │ │ │ ├── solver_incremental.h │ │ │ ├── solver_incremental_base.cpp │ │ │ ├── solver_incremental_base.h │ │ │ ├── solver_incremental_runner.cpp │ │ │ ├── solver_incremental_runner.h │ │ │ ├── solver_incremental_running_environment.cpp │ │ │ └── solver_incremental_running_environment.h │ │ │ ├── utils │ │ │ ├── CMakeLists.txt │ │ │ ├── factor_graph_closure_validator.cpp │ │ │ ├── factor_graph_closure_validator.h │ │ │ ├── factor_graph_initializer.cpp │ │ │ ├── factor_graph_initializer.h │ │ │ ├── factor_graph_initializer_rule.cpp │ │ │ ├── factor_graph_initializer_rule.h │ │ │ ├── factor_graph_visit.cpp │ │ │ ├── factor_graph_visit.h │ │ │ ├── factor_graph_visit_cost.cpp │ │ │ ├── factor_graph_visit_cost.h │ │ │ ├── factor_graph_visit_entry.cpp │ │ │ ├── factor_graph_visit_entry.h │ │ │ ├── factor_graph_visit_policy.cpp │ │ │ ├── factor_graph_visit_policy.h │ │ │ ├── instances.cpp │ │ │ ├── instances.h │ │ │ ├── solver_evaluator.cpp │ │ │ ├── solver_evaluator.h │ │ │ └── solver_evaluator_impl.cpp │ │ │ └── variables_and_factors │ │ │ ├── CMakeLists.txt │ │ │ ├── types_2d │ │ │ ├── CMakeLists.txt │ │ │ ├── all_types.h │ │ │ ├── instances.cpp │ │ │ ├── instances.h │ │ │ ├── se2_circumference_constraint_factor.cpp │ │ │ ├── se2_circumference_constraint_factor.h │ │ │ ├── se2_plane2plane_error_factor.cpp │ │ │ ├── se2_plane2plane_error_factor.h │ │ │ ├── se2_point2point_error_factor.cpp │ │ │ ├── se2_point2point_error_factor.h │ │ │ ├── se2_pose_point_bearing_error_factor.cpp │ │ │ ├── se2_pose_point_bearing_error_factor.h │ │ │ ├── se2_pose_point_error_factor.cpp │ │ │ ├── se2_pose_point_error_factor.h │ │ │ ├── se2_pose_point_range_constraint_factor.cpp │ │ │ ├── se2_pose_point_range_constraint_factor.h │ │ │ ├── se2_pose_pose_chordal_error_factor.cpp │ │ │ ├── se2_pose_pose_chordal_error_factor.h │ │ │ ├── se2_pose_pose_geodesic_error_factor.cpp │ │ │ ├── se2_pose_pose_geodesic_error_factor.h │ │ │ ├── se2_prior_error_factor.cpp │ │ │ ├── se2_prior_error_factor.h │ │ │ ├── variable_point2.h │ │ │ ├── variable_point2_ad.h │ │ │ ├── variable_se2.cpp │ │ │ ├── variable_se2.h │ │ │ ├── variable_se2_ad.cpp │ │ │ ├── variable_se2_ad.h │ │ │ ├── variable_so2.cpp │ │ │ ├── variable_so2.h │ │ │ ├── variable_so2_ad.cpp │ │ │ └── variable_so2_ad.h │ │ │ ├── types_3d │ │ │ ├── CMakeLists.txt │ │ │ ├── all_types.h │ │ │ ├── gps_factor_ad.cpp │ │ │ ├── gps_factor_ad.h │ │ │ ├── imu_bias_variable.cpp │ │ │ ├── imu_bias_variable.h │ │ │ ├── imu_factor_ad.cpp │ │ │ ├── imu_factor_ad.h │ │ │ ├── instances.cpp │ │ │ ├── instances.h │ │ │ ├── preintegrated_imu_measurements.cpp │ │ │ ├── preintegrated_imu_measurements.h │ │ │ ├── se3_matchable2matchable_error_factor.cpp │ │ │ ├── se3_matchable2matchable_error_factor.h │ │ │ ├── se3_plane2plane_error_factor.cpp │ │ │ ├── se3_plane2plane_error_factor.h │ │ │ ├── se3_point2point_error_factor.cpp │ │ │ ├── se3_point2point_error_factor.h │ │ │ ├── se3_pose_matchable_error_factor.cpp │ │ │ ├── se3_pose_matchable_error_factor.h │ │ │ ├── se3_pose_motion_error_factor_ad.cpp │ │ │ ├── se3_pose_motion_error_factor_ad.h │ │ │ ├── se3_pose_point_error_factor.cpp │ │ │ ├── se3_pose_point_error_factor.h │ │ │ ├── se3_pose_point_offset_error_factor.cpp │ │ │ ├── se3_pose_point_offset_error_factor.h │ │ │ ├── se3_pose_pose_chordal_error_factor.cpp │ │ │ ├── se3_pose_pose_chordal_error_factor.h │ │ │ ├── se3_pose_pose_chordal_hessian_factor.cpp │ │ │ ├── se3_pose_pose_chordal_hessian_factor.h │ │ │ ├── se3_pose_pose_error_factor_base.cpp │ │ │ ├── se3_pose_pose_error_factor_base.h │ │ │ ├── se3_pose_pose_geodesic_derivatives_helpers.h │ │ │ ├── se3_pose_pose_geodesic_derivatives_helpers.hpp │ │ │ ├── se3_pose_pose_geodesic_error_factor.cpp │ │ │ ├── se3_pose_pose_geodesic_error_factor.h │ │ │ ├── se3_pose_pose_right_chordal_error_factor.cpp │ │ │ ├── se3_pose_pose_right_chordal_error_factor.h │ │ │ ├── se3_prior_error_factor_ad.cpp │ │ │ ├── se3_prior_error_factor_ad.h │ │ │ ├── se3_prior_offset_error_factor_ad.cpp │ │ │ ├── se3_prior_offset_error_factor_ad.h │ │ │ ├── variable_matchable.cpp │ │ │ ├── variable_matchable.h │ │ │ ├── variable_point3.h │ │ │ ├── variable_point3_ad.h │ │ │ ├── variable_se3.cpp │ │ │ ├── variable_se3.h │ │ │ ├── variable_se3_ad.cpp │ │ │ └── variable_se3_ad.h │ │ │ ├── types_calib │ │ │ ├── CMakeLists.txt │ │ │ ├── all_types. │ │ │ ├── all_types.h │ │ │ ├── differential_drive_odom_error_factor_ad.cpp │ │ │ ├── differential_drive_odom_error_factor_ad.h │ │ │ ├── differential_drive_odom_predictor_ad.cpp │ │ │ ├── differential_drive_odom_predictor_ad.h │ │ │ ├── differential_drive_odom_sensor2d_error_factor_ad.cpp │ │ │ ├── differential_drive_odom_sensor2d_error_factor_ad.h │ │ │ ├── differential_drive_odom_sensor3d_error_factor_ad.cpp │ │ │ ├── differential_drive_odom_sensor3d_error_factor_ad.h │ │ │ ├── differential_drive_odom_time_delay_sensor2d_error_factor_ad.cpp │ │ │ ├── differential_drive_odom_time_delay_sensor2d_error_factor_ad.h │ │ │ ├── example_tricycle.cpp │ │ │ ├── instances.cpp │ │ │ ├── instances.h │ │ │ ├── sensor2d_extrinsic_pose_motion_calib_ad.cpp │ │ │ ├── sensor2d_extrinsic_pose_motion_calib_ad.h │ │ │ ├── sensor3d_extrinsic_pose_motion_calib_ad.cpp │ │ │ ├── sensor3d_extrinsic_pose_motion_calib_ad.h │ │ │ ├── sensor3d_pose_time_delay_error_factor_ad.cpp │ │ │ ├── sensor3d_pose_time_delay_error_factor_ad.h │ │ │ ├── variable_time.cpp │ │ │ ├── variable_time.h │ │ │ ├── variable_time_ad.cpp │ │ │ └── variable_time_ad.h │ │ │ ├── types_common │ │ │ ├── CMakeLists.txt │ │ │ ├── all_types.h │ │ │ ├── instances.cpp │ │ │ ├── instances.h │ │ │ ├── variable_matrix.h │ │ │ ├── variable_matrix_ad.h │ │ │ ├── variable_matrix_ad_impl.cpp │ │ │ ├── variable_matrix_impl.cpp │ │ │ ├── variable_vector.h │ │ │ ├── variable_vector_ad.h │ │ │ ├── variable_vector_ad_impl.cpp │ │ │ └── variable_vector_impl.cpp │ │ │ ├── types_projective │ │ │ ├── CMakeLists.txt │ │ │ ├── all_types.h │ │ │ ├── instances.cpp │ │ │ ├── instances.h │ │ │ ├── se3_pose_point_ba_error_factor.h │ │ │ ├── se3_pose_point_omni_ba_error_factor.cpp │ │ │ ├── se3_pose_point_omni_ba_error_factor.h │ │ │ ├── se3_pose_point_pinhole_ba_error_factor.cpp │ │ │ ├── se3_pose_point_pinhole_ba_error_factor.h │ │ │ ├── se3_pose_point_rectified_stereo_error_factor.cpp │ │ │ ├── se3_pose_point_rectified_stereo_error_factor.h │ │ │ ├── se3_projective_depth_error_factor.cpp │ │ │ ├── se3_projective_depth_error_factor.h │ │ │ ├── se3_projective_error_factor.cpp │ │ │ ├── se3_projective_error_factor.h │ │ │ ├── se3_rectified_stereo_projective_error_factor.cpp │ │ │ ├── se3_rectified_stereo_projective_error_factor.h │ │ │ ├── sim3_point2point_error_factor.cpp │ │ │ ├── sim3_point2point_error_factor.h │ │ │ ├── sim3_pose_pose_error_factor_ad.cpp │ │ │ ├── sim3_pose_pose_error_factor_ad.h │ │ │ ├── variable_sim3.cpp │ │ │ ├── variable_sim3.h │ │ │ ├── variable_sim3_ad.cpp │ │ │ └── variable_sim3_ad.h │ │ │ └── types_sfm │ │ │ ├── CMakeLists.txt │ │ │ ├── all_types.h │ │ │ ├── essential_translation_factor.cpp │ │ │ ├── essential_translation_factor.h │ │ │ ├── instances.cpp │ │ │ ├── instances.h │ │ │ ├── se3_epipolar_ba_factor_base.h │ │ │ ├── se3_epipolar_ba_factor_jacobian_helpers.h │ │ │ ├── se3_epipolar_ba_left_factor.cpp │ │ │ ├── se3_epipolar_ba_left_factor.h │ │ │ ├── se3_epipolar_ba_left_factor_impl.cpp │ │ │ ├── se3_epipolar_ba_offset_left_factor.cpp │ │ │ ├── se3_epipolar_ba_offset_left_factor.h │ │ │ ├── se3_epipolar_ba_offset_left_factor_impl.cpp │ │ │ ├── se3_epipolar_ba_right_factor.cpp │ │ │ ├── se3_epipolar_ba_right_factor.h │ │ │ ├── se3_epipolar_ba_right_factor_impl.cpp │ │ │ ├── se3_pairwise_scale_error_factor_ad.cpp │ │ │ ├── se3_pairwise_scale_error_factor_ad.h │ │ │ ├── so3_pose_constraint_factor.cpp │ │ │ ├── so3_pose_constraint_factor.h │ │ │ ├── so3_pose_pose_error_factor.cpp │ │ │ ├── so3_pose_pose_error_factor.h │ │ │ ├── variable_se3_relaxed.h │ │ │ ├── variable_so3.cpp │ │ │ └── variable_so3.h │ │ └── tests │ │ ├── CMakeLists.txt │ │ ├── factor_graph_loop_validator_test.cpp │ │ ├── test_g2o_graph_converter.cpp │ │ ├── test_imu.cpp │ │ ├── test_linear_solver_block_inverse.cpp │ │ ├── test_se2_extrinsic_motion_calib.cpp │ │ ├── test_se2_icp.cpp │ │ ├── test_se2_multi_point_registration.cpp │ │ ├── test_se2_nicp.cpp │ │ ├── test_se2_pgo.cpp │ │ ├── test_se2_prior_factor.cpp │ │ ├── test_se3_extrinsic_motion_calib.cpp │ │ ├── test_se3_icp.cpp │ │ ├── test_se3_matchables.cpp │ │ ├── test_se3_motion_based_calib.cpp │ │ ├── test_se3_multi_point_registration.cpp │ │ ├── test_se3_nicp.cpp │ │ ├── test_se3_pgo.cpp │ │ ├── test_se3_posit.cpp │ │ ├── test_se3_prior_factor.cpp │ │ ├── test_sim3.cpp │ │ ├── test_sim3_icp.cpp │ │ ├── test_sim3_pgo.cpp │ │ └── test_sparse_block_linear_solver_cholmod_full.cpp └── srrg_cmake_modules │ ├── .gitignore │ ├── CMakeLists.txt │ ├── LICENSE │ ├── README.md │ ├── ci_scripts │ ├── append_gtest_library.sh │ ├── benchmark │ │ ├── find_dl.sh │ │ ├── pass_bench.py │ │ └── run_benchmark_app.sh │ ├── build.sh │ ├── ci_run │ │ ├── srrg2_packages.txt │ │ ├── start_all_pipelines.sh │ │ ├── start_pipeline.sh │ │ └── stop_all_pipelines.sh │ ├── download_and_extract_file.sh │ ├── drive_download_and_extract_file.sh │ ├── evaluation │ │ └── tum │ │ │ ├── associate.py │ │ │ ├── evaluate_ate.py │ │ │ ├── evaluate_rpe.py │ │ │ └── plot_trajectory_into_image.py │ ├── install_catkin_workspace.sh │ ├── pack_artifacts.sh │ ├── push_benchmark_results.sh │ ├── run_benchmark.sh │ ├── run_benchmarks.sh │ ├── run_executable.sh │ ├── run_unittests.sh │ ├── unpack_all_external_artifacts.sh │ ├── unpack_external_artifacts.sh │ └── unpack_internal_artifacts.sh │ ├── cmake_modules │ ├── CMakeCompileOptions.txt │ ├── FindCholmod.cmake │ ├── FindDBoW2.cmake │ ├── FindEigen3.cmake │ ├── FindFlann.cmake │ ├── FindFreenect.cmake │ ├── FindG2O.cmake │ ├── FindLibWEBSOCKETS.cmake │ ├── FindQGLViewer.cmake │ ├── FindReadline.cmake │ ├── FindSuiteSparse.cmake │ └── FindUmfpack.cmake │ └── package.xml └── surfel_cloud_rviz_plugin ├── CMakeLists.txt ├── README.md ├── package.xml ├── plugin_description.xml ├── shaders ├── depth_circle.frag ├── flat_color_circle.frag ├── glsl120.program ├── include │ ├── circle_impl.frag │ ├── pack_depth.frag │ └── pass_depth.vert ├── pass_color_circle.frag ├── pickcolor_circle.frag ├── point.vert └── point_cloud_surfel.material └── src ├── point_cloud.cpp ├── point_cloud.h ├── point_cloud_common.cpp ├── point_cloud_common.h ├── surfel_cloud_display.cpp └── surfel_cloud_display.h /.gitignore: -------------------------------------------------------------------------------- 1 | # These are some examples of commonly ignored file patterns. 2 | # You should customize this list as applicable to your project. 3 | # Learn more about .gitignore: 4 | # https://www.atlassian.com/git/tutorials/saving-changes/gitignore 5 | 6 | # Node artifact files 7 | node_modules/ 8 | dist/ 9 | 10 | # Compiled Java class files 11 | *.class 12 | 13 | # Compiled Python bytecode 14 | *.py[cod] 15 | 16 | # Log files 17 | *.log 18 | 19 | # Package files 20 | *.jar 21 | 22 | # Maven 23 | target/ 24 | dist/ 25 | 26 | # JetBrains IDE 27 | .idea/ 28 | 29 | # Unit test reports 30 | TEST*.xml 31 | 32 | # Generated by MacOS 33 | .DS_Store 34 | 35 | # Generated by Windows 36 | Thumbs.db 37 | 38 | # Applications 39 | *.app 40 | *.exe 41 | *.war 42 | 43 | # Large media files 44 | *.mp4 45 | *.tiff 46 | *.avi 47 | *.flv 48 | *.mov 49 | *.wmv 50 | *.ply 51 | *.pcd 52 | *.tum 53 | *.png 54 | *.bag 55 | -------------------------------------------------------------------------------- /build_and_run.sh: -------------------------------------------------------------------------------- 1 | xhost +local:root 2 | 3 | # BUILD THE IMAGE 4 | ROS_IMAGE="mad_ba" 5 | if [ -z "$1" ] 6 | then 7 | echo "RUNNING WITH 1 WORKER" 8 | WORKERS=1 9 | else 10 | echo "RUNNING WITH $1 WORKERS" 11 | WORKERS=$1 12 | fi 13 | docker build --no-cache -f Dockerfile -t $ROS_IMAGE ./. 14 | 15 | # SETUP THE CONTAINER 16 | ROS_CONTAINER="mad_ba" 17 | 18 | docker run -dit \ 19 | --volume="$(pwd)/docker_shared:/root/share" \ 20 | --privileged \ 21 | --network=host \ 22 | --name="$ROS_CONTAINER" \ 23 | $ROS_IMAGE \ 24 | /bin/bash 25 | -------------------------------------------------------------------------------- /docker_shared/dataset/KITTI/00/bag/.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rvp-group/mad-ba/7fc025c9deb59c5a971ff313d91ae13053a21635/docker_shared/dataset/KITTI/00/bag/.bag -------------------------------------------------------------------------------- /docker_shared/dataset/KITTI/01/bag/.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rvp-group/mad-ba/7fc025c9deb59c5a971ff313d91ae13053a21635/docker_shared/dataset/KITTI/01/bag/.bag -------------------------------------------------------------------------------- /docker_shared/dataset/KITTI/02/bag/.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rvp-group/mad-ba/7fc025c9deb59c5a971ff313d91ae13053a21635/docker_shared/dataset/KITTI/02/bag/.bag -------------------------------------------------------------------------------- /docker_shared/dataset/KITTI/03/bag/.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rvp-group/mad-ba/7fc025c9deb59c5a971ff313d91ae13053a21635/docker_shared/dataset/KITTI/03/bag/.bag -------------------------------------------------------------------------------- /docker_shared/dataset/KITTI/04/bag/.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rvp-group/mad-ba/7fc025c9deb59c5a971ff313d91ae13053a21635/docker_shared/dataset/KITTI/04/bag/.bag -------------------------------------------------------------------------------- /docker_shared/dataset/KITTI/05/bag/.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rvp-group/mad-ba/7fc025c9deb59c5a971ff313d91ae13053a21635/docker_shared/dataset/KITTI/05/bag/.bag -------------------------------------------------------------------------------- /docker_shared/dataset/KITTI/06/bag/.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rvp-group/mad-ba/7fc025c9deb59c5a971ff313d91ae13053a21635/docker_shared/dataset/KITTI/06/bag/.bag -------------------------------------------------------------------------------- /docker_shared/dataset/KITTI/07/bag/.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rvp-group/mad-ba/7fc025c9deb59c5a971ff313d91ae13053a21635/docker_shared/dataset/KITTI/07/bag/.bag -------------------------------------------------------------------------------- /docker_shared/dataset/KITTI/08/bag/.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rvp-group/mad-ba/7fc025c9deb59c5a971ff313d91ae13053a21635/docker_shared/dataset/KITTI/08/bag/.bag -------------------------------------------------------------------------------- /docker_shared/dataset/KITTI/09/bag/.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rvp-group/mad-ba/7fc025c9deb59c5a971ff313d91ae13053a21635/docker_shared/dataset/KITTI/09/bag/.bag -------------------------------------------------------------------------------- /docker_shared/dataset/KITTI/10/bag/.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rvp-group/mad-ba/7fc025c9deb59c5a971ff313d91ae13053a21635/docker_shared/dataset/KITTI/10/bag/.bag -------------------------------------------------------------------------------- /docker_shared/dataset/NewerCollege/cloister/bag/.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rvp-group/mad-ba/7fc025c9deb59c5a971ff313d91ae13053a21635/docker_shared/dataset/NewerCollege/cloister/bag/.bag -------------------------------------------------------------------------------- /docker_shared/dataset/NewerCollege/maths_institute/bag/.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rvp-group/mad-ba/7fc025c9deb59c5a971ff313d91ae13053a21635/docker_shared/dataset/NewerCollege/maths_institute/bag/.bag -------------------------------------------------------------------------------- /docker_shared/dataset/NewerCollege/park/bag/.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rvp-group/mad-ba/7fc025c9deb59c5a971ff313d91ae13053a21635/docker_shared/dataset/NewerCollege/park/bag/.bag -------------------------------------------------------------------------------- /docker_shared/dataset/NewerCollege/quad_easy/bag/.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rvp-group/mad-ba/7fc025c9deb59c5a971ff313d91ae13053a21635/docker_shared/dataset/NewerCollege/quad_easy/bag/.bag -------------------------------------------------------------------------------- /docker_shared/dataset/NewerCollege/quad_easy/bag/download_quad.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | gdown https://drive.google.com/uc?id=1hF2h83E1THbFAvs7wpR6ORmrscIHxKMo 3 | -------------------------------------------------------------------------------- /docker_shared/dataset/NewerCollege/stairs/bag/.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rvp-group/mad-ba/7fc025c9deb59c5a971ff313d91ae13053a21635/docker_shared/dataset/NewerCollege/stairs/bag/.bag -------------------------------------------------------------------------------- /docker_shared/dataset/NewerCollege/underground_mine/bag/.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rvp-group/mad-ba/7fc025c9deb59c5a971ff313d91ae13053a21635/docker_shared/dataset/NewerCollege/underground_mine/bag/.bag -------------------------------------------------------------------------------- /docker_shared/dataset/VBR/Campus/bag/download_campus.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | vbr download campus_train0 . 3 | mv ./vbr_slam/campus/campus_train0/* . 4 | -------------------------------------------------------------------------------- /docker_shared/dataset/VBR/Ciampino/bag/download_ciampino.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | vbr download ciampino_train0 . 3 | mv ./vbr_slam/ciampino/ciampino_train0/* . 4 | -------------------------------------------------------------------------------- /docker_shared/dataset/VBR/Colosseo/bag/download_colloseo.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | vbr download colosseo_train0 . 3 | mv ./vbr_slam/colosseo/colosseo_train0/* . 4 | -------------------------------------------------------------------------------- /docker_shared/dataset/VBR/Colosseo/bag/filter.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | rosbag filter input.bag output.bag "t.to_sec() < 2884.772651110 or t.to_sec() > 2884.775651110" 3 | -------------------------------------------------------------------------------- /docker_shared/dataset/VBR/DIAG/bag/download_diag.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | vbr download diag_train0 . 3 | mv ./vbr_slam/diag/diag_train0/* . 4 | -------------------------------------------------------------------------------- /docker_shared/dataset/VBR/Pincio/bag/.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rvp-group/mad-ba/7fc025c9deb59c5a971ff313d91ae13053a21635/docker_shared/dataset/VBR/Pincio/bag/.bag -------------------------------------------------------------------------------- /docker_shared/dataset/VBR/Pincio/bag/download_pincio.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | vbr download pincio_train0 . 3 | mv ./vbr_slam/pincio/pincio_train0/* . 4 | -------------------------------------------------------------------------------- /docker_shared/dataset/VBR/Spagna/bag/download_spagna.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | vbr download spagna_train0 . 3 | mv ./vbr_slam/spagna/spagna_train0/* . 4 | -------------------------------------------------------------------------------- /docker_shared/output/.out: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rvp-group/mad-ba/7fc025c9deb59c5a971ff313d91ae13053a21635/docker_shared/output/.out -------------------------------------------------------------------------------- /modules/mad_ba/apps/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_executable(main_app main_app.cpp) 2 | 3 | target_link_libraries(main_app 4 | srrg2_solver_core_library 5 | srrg2_point_cloud_library 6 | srrg2_messages_library 7 | srrg2_data_structures_library 8 | srrg2_solver_types3d_library 9 | srrg2_image_library 10 | srrg2_config_library 11 | srrg2_property_library 12 | srrg2_boss_library 13 | srrg2_system_utils_library 14 | srrg2_viewer_library 15 | srrg2_configurable_shell_library 16 | mad_ba_library 17 | librviz_visual_tools.so 18 | ${OpenCV_LIBS} 19 | ${QGLViewer_LIBRARIES} 20 | ${GLUT_LIBRARIES} 21 | ${SRRG_QT_LIBRARIES} 22 | ${catkin_LIBRARIES} 23 | -lGL 24 | ) -------------------------------------------------------------------------------- /modules/mad_ba/src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_subdirectory(mad_ba) 2 | add_subdirectory(types) -------------------------------------------------------------------------------- /modules/mad_ba/src/mad_ba/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(mad_ba_library SHARED 2 | point_cloud_proc.cpp 3 | point_cloud_proc_visual.cpp 4 | surfel.cpp 5 | instances.cpp 6 | data_association.cpp 7 | utils.cpp 8 | ) 9 | 10 | target_link_libraries(mad_ba_library 11 | srrg2_solver_core_library 12 | srrg2_point_cloud_library 13 | srrg2_messages_library 14 | srrg2_data_structures_library 15 | srrg2_solver_types3d_library 16 | srrg2_solver_types_library 17 | srrg2_image_library 18 | srrg2_converters_library 19 | librviz_visual_tools.so 20 | ${OpenCV_LIBS} 21 | ${PCL_LIBRARIES} 22 | ) 23 | 24 | set_target_properties(mad_ba_library 25 | PROPERTIES CUDA_SEPARABLE_COMPILATION OFF 26 | ) 27 | -------------------------------------------------------------------------------- /modules/mad_ba/src/mad_ba/instances.cpp: -------------------------------------------------------------------------------- 1 | #include "instances.h" 2 | #include "point_cloud_proc.h" 3 | #include "surfel.h" 4 | 5 | namespace mad_ba { 6 | 7 | void registerTypes() { 8 | BOSS_REGISTER_CLASS(PointCloudProc); 9 | BOSS_REGISTER_CLASS(Surfel); 10 | } 11 | } // namespace mad_ba 12 | -------------------------------------------------------------------------------- /modules/mad_ba/src/mad_ba/instances.h: -------------------------------------------------------------------------------- 1 | namespace mad_ba { 2 | void registerTypes() __attribute__((constructor)); 3 | } 4 | -------------------------------------------------------------------------------- /modules/mad_ba/src/mad_ba/surfel_matches.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | namespace mad_ba { 11 | using ContainerType = std::vector; 12 | using TreeNodeType = TreeNode3D; 13 | using TreeNodeTypePtr = TreeNodeType*; 14 | 15 | struct SurfelMatches { 16 | TreeNodeTypePtr surfelA; 17 | TreeNodeTypePtr surfelB; 18 | bool matched; 19 | }; 20 | 21 | } // namespace mad_ba 22 | -------------------------------------------------------------------------------- /modules/mad_ba/src/mad_ba/utils.cpp: -------------------------------------------------------------------------------- 1 | #include "utils.h" 2 | #include 3 | #include 4 | #include 5 | 6 | namespace mad_ba { 7 | 8 | Eigen::Matrix3f matrixBetween2Vectors(Eigen::Vector3f a, Eigen::Vector3f b) { 9 | a = a / a.norm(); 10 | b = b / b.norm(); 11 | Eigen::Vector3f v = a.cross(b); 12 | float s = v.norm(); 13 | float c = a.dot(b); 14 | Eigen::Matrix3f vx; 15 | vx << 0, -v[2], v[1], v[2], 0, -v[0], -v[1], v[0], 0; 16 | Eigen::Matrix3f r = Eigen::Matrix3f::Identity(3, 3); 17 | if (s != 0) { 18 | r = r + vx + vx * vx * ((1 - c) / std::pow(s, 2)); 19 | } else { 20 | // std::cout << "doesn't work if a == -b" << std::endl; 21 | } 22 | return r; 23 | } 24 | 25 | float angleBetween2Vectors(const Eigen::Vector3f& a, const Eigen::Vector3f& b) { 26 | return atan2(a.cross(b).norm(), a.dot(b)); 27 | } 28 | 29 | } -------------------------------------------------------------------------------- /modules/mad_ba/src/mad_ba/utils.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace mad_ba { 6 | 7 | Eigen::Matrix3f matrixBetween2Vectors(Eigen::Vector3f a, Eigen::Vector3f b); 8 | float angleBetween2Vectors(const Eigen::Vector3f& a, const Eigen::Vector3f& b); 9 | 10 | } -------------------------------------------------------------------------------- /modules/mad_ba/src/types/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(srrg2_solver_types_library SHARED 2 | variable_surfel.cpp 3 | variable_surfel.h 4 | variable_surfel_ad.cpp 5 | variable_surfel_ad.h 6 | se3_pose_surfel_factor_ad.cpp 7 | se3_pose_surfel_factor_ad.h 8 | instances.h 9 | instances.cpp 10 | ) 11 | 12 | target_link_libraries(srrg2_solver_types_library 13 | srrg2_solver_types3d_library 14 | srrg2_solver_types_common_library 15 | srrg2_solver_core_library 16 | ) 17 | -------------------------------------------------------------------------------- /modules/mad_ba/src/types/instances.cpp: -------------------------------------------------------------------------------- 1 | //! include all the types you declared here 2 | #include "instances.h" 3 | #include "variable_surfel.h" 4 | #include "variable_surfel_ad.h" 5 | #include "se3_pose_surfel_factor_ad.h" 6 | //! we try to instantiate all solvers 7 | namespace srrg2_solver { 8 | using namespace srrg2_core; 9 | 10 | // this is the function you have to call to initialize 11 | // the serialization subsystem 12 | void variables_and_factors_types_registerTypes() { 13 | BOSS_REGISTER_CLASS(VariableSurfel); 14 | BOSS_REGISTER_CLASS(VariableSurfel1D); 15 | BOSS_REGISTER_CLASS(VariableSurfelAD); 16 | BOSS_REGISTER_CLASS(VariableSurfelAD1D); 17 | BOSS_REGISTER_CLASS(SE3PoseSurfelQuaternionErrorFactorAD); 18 | BOSS_REGISTER_CLASS(SE3PoseSurfelQuaternionErrorFactorAD1D); 19 | } 20 | } // namespace srrg2_solver 21 | -------------------------------------------------------------------------------- /modules/mad_ba/src/types/instances.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | namespace srrg2_solver { 4 | void variables_and_factors_types_registerTypes() __attribute__((constructor)); 5 | } // namespace srrg2_solver 6 | -------------------------------------------------------------------------------- /modules/mad_ba/src/types/variable_se3_ad.cpp: -------------------------------------------------------------------------------- 1 | #include "variable_se3_ad.h" 2 | #include "srrg_solver/solver_core/instance_macros.h" 3 | #include "srrg_solver/solver_core/variable_impl.cpp" 4 | 5 | namespace srrg2_solver { 6 | INSTANTIATE(VariableSE3EulerRightAD) 7 | INSTANTIATE(VariableSE3EulerLeftAD) 8 | INSTANTIATE(VariableSE3QuaternionRightAD) 9 | INSTANTIATE(VariableSE3QuaternionLeftAD) 10 | 11 | } // namespace srrg2_solver 12 | -------------------------------------------------------------------------------- /modules/mad_ba/src/types/variable_surfel.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include "srrg_solver/solver_core/variable.h" 4 | 5 | namespace srrg2_solver { 6 | 7 | using namespace srrg2_core; 8 | class VariableSurfel : public Variable_<5, Isometry3_> { 9 | public: 10 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 11 | 12 | virtual void setZero() override; 13 | 14 | virtual void applyPerturbation(const Vector5f& pert) override; 15 | 16 | void _drawImpl(ViewerCanvasPtr canvas_) const override; 17 | 18 | }; 19 | 20 | class VariableSurfel1D : public Variable_<1, Isometry3_> { 21 | public: 22 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 23 | 24 | virtual void setZero() override; 25 | 26 | virtual void applyPerturbation(const Vector1f& pert) override; 27 | 28 | void _drawImpl(ViewerCanvasPtr canvas_) const override; 29 | 30 | }; 31 | 32 | } 33 | -------------------------------------------------------------------------------- /modules/mad_ba/src/types/variable_surfel_ad.cpp: -------------------------------------------------------------------------------- 1 | #include "variable_surfel_ad.h" 2 | #include "srrg_solver/solver_core/instance_macros.h" 3 | #include "srrg_solver/solver_core/variable_impl.cpp" 4 | 5 | namespace srrg2_solver { 6 | INSTANTIATE(VariableSurfelAD) 7 | INSTANTIATE(VariableSurfelAD1D) 8 | } // namespace srrg2_solver 9 | -------------------------------------------------------------------------------- /modules/rviz_visual_tools/.travis.yml: -------------------------------------------------------------------------------- 1 | # This config file for Travis CI utilizes https://github.com/ros-planning/moveit_ci package. 2 | sudo: required 3 | dist: xenial # distro used by Travis, moveit_ci uses the docker image's distro 4 | services: 5 | - docker 6 | language: cpp 7 | compiler: gcc 8 | cache: ccache 9 | 10 | env: 11 | global: 12 | - DOCKER_IMAGE=moveit/moveit:master-source 13 | - CXXFLAGS="-Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wno-unused-parameter -Wno-unused-function" 14 | - WARNINGS_OK=false 15 | matrix: 16 | - TEST="clang-format" # TODO(davetcoleman): enable catkin_lint in the future 17 | - TEST=clang-tidy-fix 18 | - DOCKER_IMAGE=moveit/moveit:melodic-source 19 | - DOCKER_IMAGE=moveit/moveit:master-source 20 | - DOCKER_IMAGE=moveit/moveit:kinetic-ci TEST_BLACKLIST=moveit_ros_perception 21 | 22 | before_script: 23 | - git clone -q --depth=1 https://github.com/ros-planning/moveit_ci.git .moveit_ci 24 | 25 | script: 26 | - .moveit_ci/travis.sh 27 | -------------------------------------------------------------------------------- /modules/rviz_visual_tools/MIGRATION.md: -------------------------------------------------------------------------------- 1 | # Migration Notes 2 | 3 | API changes in Rviz Visual Tools releases 4 | 5 | ## ROS Melodic 6 | 7 | - Affine3d no longer used, replaced by more computationally efficient Isometry3d. Simple find-replace should suffice 8 | - RvizVisualTools::publishMarkers called ros::spinOnce() in previous versions. This was usually a bug as it might lead to running ROS callback functions in unrelated threads (e.g. your UI thread). In the unlikely case that your program relied on publishMarkers calling ros::spinOnce() you will now have to add either a ros::spinOnce after your call to publishMarkers or simply add a ros::AsyncSpinner. See http://wiki.ros.org/roscpp/Overview/Callbacks%20and%20Spinning for more details. 9 | -------------------------------------------------------------------------------- /modules/rviz_visual_tools/launch/demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /modules/rviz_visual_tools/launch/demo_combined.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /modules/rviz_visual_tools/launch/demo_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /modules/rviz_visual_tools/launch/rvt_test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 9 | 10 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /modules/rviz_visual_tools/plugin_description.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | A panel widget allowing control of a ROS pipeline 7 | 8 | 9 | 10 | 11 | 12 | Tool to capture keyboard input 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /modules/rviz_visual_tools/resources/frame_locking.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rvp-group/mad-ba/7fc025c9deb59c5a971ff313d91ae13053a21635/modules/rviz_visual_tools/resources/frame_locking.gif -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/docker/build.sh: -------------------------------------------------------------------------------- 1 | docker build -t giorgiogrisetti/ubuntu_20.04_cuda_ros_noetic_srrg2_core . 2 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/deprecated/srrg_autodiff_bivariate_example.cpp: -------------------------------------------------------------------------------- 1 | #include "srrg_autodiff/ad.h" 2 | #include 3 | 4 | using namespace std; 5 | using namespace srrg2_core; 6 | 7 | DualValuef fancyFn(const DualValuef& x, const DualValuef& y){ 8 | return sin(y)*exp(-x*x)/(DualValuef(2.0)+cos(DualValuef(2.0)+log(y))); 9 | } 10 | 11 | typedef DualValue_ DualValuef; 12 | int main(int argc, char** argv) { 13 | DualValuef x(0.5); 14 | DualValuef y(0.1); 15 | 16 | DualValuef result=fancyFn(x,y); 17 | cerr << "value of the function in the point: " << result.value << endl; 18 | 19 | x.derivative=1; 20 | result = fancyFn(x,y); 21 | cerr << "value of the derivtive in the point w.r.t. x: " << result.derivative << endl; 22 | x.derivative=0; 23 | 24 | y.derivative=1; 25 | result = fancyFn(x,y); 26 | cerr << "value of the derivtive in the point w.r.t. y: " << result.derivative << endl; 27 | y.derivative=0; 28 | 29 | } 30 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/deprecated/srrg_autodiff_simple_example.cpp: -------------------------------------------------------------------------------- 1 | #include "srrg_autodiff/ad.h" 2 | #include 3 | 4 | using namespace std; 5 | using namespace srrg2_core; 6 | 7 | DualValuef fancyFn(const DualValuef& x){ 8 | return sin(x)*exp(-x*x)/(DualValuef(2.0)+cos(DualValuef(2.0)+log(x))); 9 | } 10 | 11 | int main(int argc, char** argv) { 12 | DualValuef result=fancyFn(DualValuef (0.5,1)); 13 | cerr << "value of the function in the point: " << result.value << endl; 14 | cerr << "value of the derivtive in the point: " << result.derivative << endl; 15 | } 16 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | srrg2_core 4 | 1.0.0 5 | The srrg2_core package 6 | Giorgio Grisetti 7 | Dominik Schlegel 8 | Bartolomeo Della Corte 9 | Irvin Aloise 10 | 11 | BSD-4 12 | catkin 13 | 14 | srrg_cmake_modules 15 | 16 | 17 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/slides/2018_srrg_core.odp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rvp-group/mad-ba/7fc025c9deb59c5a971ff313d91ae13053a21635/modules/srrg/srrg2_core/srrg2_core/slides/2018_srrg_core.odp -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_subdirectory(srrg_system_utils) 2 | add_subdirectory(srrg_geometry) 3 | add_subdirectory(srrg_boss) 4 | add_subdirectory(srrg_property) 5 | add_subdirectory(srrg_config) 6 | add_subdirectory(srrg_image) 7 | add_subdirectory(srrg_pcl) 8 | add_subdirectory(srrg_matchable) 9 | add_subdirectory(srrg_data_structures) 10 | add_subdirectory(srrg_messages) 11 | add_subdirectory(srrg_viewer) 12 | add_subdirectory(examples) 13 | 14 | #ia miscellaneous tests (not gtests) 15 | add_subdirectory(misc_tests) 16 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/TODO.txt: -------------------------------------------------------------------------------- 1 | 1. path_matrix: 2 | finish polishing 3 | 4 | 2. point_cloud: 5 | make it compatible with KD tree 6 | make it easy to use 7 | implement explicit instantiation (saves compiling time) 8 | 9 | 3 viewers: 10 | leave only outer interface. in core 11 | No dependancies allowed. 12 | Buffers: pimp them? 13 | Linear buffers of maximum size with encoded commands? 14 | User interaction? 15 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/examples/container_pack_example.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include "srrg_data_structures/container_pack.h" 5 | 6 | using namespace srrg2_core; 7 | using namespace std; 8 | 9 | using MyContainerPack=ContainerPack_, std::vector, std::list >; 10 | 11 | int main(int argc, char** argv) { 12 | MyContainerPack pack; 13 | cerr << "constructed" << endl; 14 | pack.field<0>().push_back(10); 15 | pack.field<0>().push_back(20); 16 | pack.field<0>().push_back(30); 17 | pack.field<1>().push_back(40); 18 | pack.field<1>().push_back(50); 19 | pack.field<2>().push_back(60); 20 | pack.field<2>().push_back(70); 21 | 22 | cerr << "num_fields: " << MyContainerPack::NumFields << endl; 23 | 24 | cerr << pack.size() << endl; 25 | 26 | pack.updateBeginEnd(); 27 | 28 | for (auto v : pack) { 29 | cerr << v << endl; 30 | } 31 | pack.field<2>().push_front(-20); 32 | pack.updateBeginEnd(); 33 | 34 | for (auto v : pack) { 35 | cerr << v << endl; 36 | } 37 | 38 | } 39 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/misc_tests/test_cloud_intensity.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | using namespace srrg2_core; 8 | using namespace std; 9 | 10 | using namespace srrg2_core; 11 | using MatrixCloudPointNormalIntensity3f=srrg2_core::PointCloud_< 12 | Matrix_ > >; 13 | using Projector = PointUnprojectorPinhole_; 14 | 15 | int main(void) { 16 | 17 | ImageFloat depth; 18 | ImageFloat intensity; 19 | 20 | MatrixCloudPointNormalIntensity3f out; 21 | 22 | Projector projector; 23 | projector.computeMatrix(out, depth, intensity); 24 | 25 | return 0; 26 | 27 | } 28 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_boss/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(srrg2_boss_library SHARED 2 | blob.cpp blob.h 3 | deserializer.cpp deserializer.h 4 | id_context.cpp id_context.h 5 | identifiable.cpp identifiable.h 6 | id_placeholder.cpp id_placeholder.h 7 | json_recursive_object_parser.cpp json_recursive_object_parser.h 8 | json_object_writer.cpp json_object_writer.h 9 | object_data.cpp object_data.h 10 | object_parser.cpp object_parser.h 11 | object_writer.cpp object_writer.h 12 | serializable.cpp serializable.h 13 | serialization_context.cpp serialization_context.h 14 | serializer.cpp serializer.h 15 | stream.cpp stream.h 16 | ) 17 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_boss/id_placeholder.cpp: -------------------------------------------------------------------------------- 1 | #include "id_placeholder.h" 2 | #include "id_context.h" 3 | 4 | using namespace srrg2_core; 5 | using namespace std; 6 | 7 | IdentifiablePtr AbstractPlaceHolderAssigner::getSharedPtr(Identifiable* ident) { 8 | // std::cerr << "query ptr " << _context << " id: " << ident << " " << /*getSharedPtr(ident) <<*/ std::endl; 9 | // std::cerr << "shared ptr: " << _context->getSharedPtr(ident) << std::endl; 10 | return _context->getSharedPtr(ident); 11 | } 12 | 13 | void IdPlaceholder::resolve(Identifiable* instance) { 14 | for (vector::iterator it=_assigners.begin();it!=_assigners.end();it++) { 15 | (*it)->assign(instance); 16 | } 17 | } 18 | 19 | IdPlaceholder::~IdPlaceholder() { 20 | for (vector::iterator it=_assigners.begin();it!=_assigners.end();it++) { 21 | delete *it; 22 | } 23 | } 24 | 25 | 26 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_boss/json_object_writer.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "object_writer.h" 4 | 5 | namespace srrg2_core { 6 | 7 | class JSONObjectWriter: public ObjectWriter { 8 | public: 9 | virtual void writeObject(std::ostream& os, 10 | const std::string& type, 11 | ObjectData& object, 12 | bool comments_enabled=false); 13 | protected: 14 | int indentation_level=0; 15 | }; 16 | 17 | } 18 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_boss/object_parser.cpp: -------------------------------------------------------------------------------- 1 | #include "object_parser.h" 2 | 3 | using namespace srrg2_core; 4 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_boss/object_writer.cpp: -------------------------------------------------------------------------------- 1 | #include "object_writer.h" 2 | 3 | using namespace srrg2_core; 4 | 5 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_boss/object_writer.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace srrg2_core { 7 | 8 | class ObjectData; 9 | 10 | class ObjectWriter { 11 | public: 12 | virtual void writeObject(std::ostream& os, 13 | const std::string& type, 14 | ObjectData& object, 15 | bool enable_comments=false)=0; 16 | virtual ~ObjectWriter() {} 17 | protected: 18 | }; 19 | 20 | } 21 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_config/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(srrg2_config_library SHARED 2 | preemptible.cpp preemptible.h 3 | configurable.cpp configurable.h 4 | ) 5 | target_link_libraries(srrg2_config_library 6 | srrg2_property_library 7 | srrg2_boss_library 8 | srrg2_system_utils_library 9 | ${OpenCV_LIBS} 10 | ) 11 | 12 | 13 | add_library(srrg2_configurable_shell_library SHARED 14 | pipeline_runner.cpp 15 | pipeline_runner.h 16 | 17 | configurable_shell.cpp 18 | configurable_shell.h 19 | 20 | linenoise.c 21 | linenoise.h 22 | ) 23 | 24 | target_link_libraries( 25 | srrg2_configurable_shell_library 26 | srrg2_property_library 27 | srrg2_messages_library 28 | srrg2_viewer_library 29 | srrg2_config_library 30 | srrg2_boss_library 31 | ) 32 | 33 | add_executable(preemptible_example preemptible_example.cpp) 34 | 35 | target_link_libraries(preemptible_example 36 | srrg2_configurable_shell_library 37 | ${catkin_LIBRARIES} 38 | ) 39 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_config/configurable_manager.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "srrg_property/property_container_manager.h" 3 | 4 | namespace srrg2_core { 5 | 6 | using ConfigurableManager = PropertyContainerManager; 7 | using ConfigurableManagerPtr = std::shared_ptr; 8 | 9 | } // namespace srrg2_core 10 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_config/param_macros.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #define PARAM(__type__, __name__, __description__, __value__, __flag__) \ 4 | __type__ param_##__name__ = __type__(#__name__, __description__, this, __value__, __flag__) 5 | 6 | #define PARAM_VECTOR(__type__, __name__, __description__, __flag__) \ 7 | __type__ param_##__name__ = __type__(#__name__, __description__, this, __flag__) 8 | 9 | #define PARAM_VECTOR_INIT(__type__, __name__, __description__, __value__, __flag__) \ 10 | __type__ param_##__name__ = __type__(#__name__, __description__, this, __value__, __flag__) 11 | 12 | #define PARAM_MH(__type__, __name__, __this__, __description__, __value__, __flag__) \ 13 | __type__ param_##__name__ = __type__(#__name__, __description__, __this__, __value__, __flag__) 14 | 15 | #define PARAM_VECTOR_MH(__type__, __name__, __description__, __this__, __flag__) \ 16 | __type__ param_##__name__ = __type__(#__name__, __description__, __this__, __flag__) 17 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_config/pipeline_runner.cpp: -------------------------------------------------------------------------------- 1 | #include "pipeline_runner.h" 2 | 3 | namespace srrg2_core { 4 | 5 | void srrg2_core_registerRunner() { 6 | BOSS_REGISTER_CLASS(PipelineRunner); 7 | } 8 | 9 | void PipelineRunner::compute() { 10 | BaseSensorMessagePtr msg; 11 | while ((msg = param_source->getMessage()) || (!isFlushed())) { 12 | bool do_preempt = false; 13 | if (msg) { 14 | _compute_mutex.lock(); 15 | do_preempt = putMessage(msg); 16 | _compute_mutex.unlock(); 17 | } else { 18 | do_preempt = flush(); 19 | } 20 | if (do_preempt) 21 | preemptLocal(); 22 | } 23 | } 24 | 25 | void PipelineRunner::reset() { 26 | MessageSinkBase::reset(); 27 | if (param_source.value()) 28 | param_source->reset(); 29 | } 30 | 31 | } // namespace srrg2_core 32 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_config/pipeline_runner.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "configurable.h" 3 | #include "preemptible.h" 4 | #include "srrg_messages/message_handlers/message_source_base.h" 5 | #include "srrg_messages/message_handlers/message_sink_base.h" 6 | #include 7 | namespace srrg2_core { 8 | struct PipelineRunner: public MessageSinkBase, public Preemptible { 9 | PARAM(PropertyConfigurable_, source, "the source of the pipeline", nullptr, nullptr); 10 | void compute() override ; 11 | void reset() override; 12 | std::mutex& computeMutex() {return _compute_mutex;} 13 | protected: 14 | std::mutex _compute_mutex; 15 | }; 16 | 17 | void srrg2_core_registerRunner() __attribute__((constructor)); 18 | } 19 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_config/property_configurable.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "configurable.h" 3 | #include "srrg_property/property_identifiable.h" 4 | 5 | namespace srrg2_core { 6 | 7 | template 8 | using PropertyConfigurable_ = PropertyIdentifiablePtr_>; 9 | 10 | template 11 | using PropertyConfigurableNoOwnership_ = PropertyIdentifiablePtr_>; 12 | } // namespace srrg2_core 13 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_config/property_configurable_vector.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "property_configurable.h" 3 | 4 | namespace srrg2_core { 5 | 6 | template 7 | using PropertyConfigurableVector_ = 8 | PropertyIdentifiablePtrVector_>; 9 | } 10 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_data_structures/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(srrg2_data_structures_library SHARED 2 | graph.cpp graph.h 3 | path_matrix.cpp path_matrix.h 4 | path_matrix_distance_search.cpp 5 | path_matrix_dijkstra_search.cpp 6 | path_matrix_clusterer_search.cpp 7 | path_matrix_astar_search.cpp 8 | path_matrix_cost_search.cpp 9 | link.cpp link.h 10 | platform.cpp platform.h 11 | events.h 12 | grid_map_2d.cpp grid_map_2d.h 13 | file_handle.cpp 14 | ) 15 | 16 | target_link_libraries(srrg2_data_structures_library 17 | srrg2_image_library 18 | srrg2_property_library 19 | srrg2_system_utils_library 20 | srrg2_config_library 21 | ) 22 | 23 | add_executable(file_vector_interface_test file_vector_interface_test) 24 | 25 | target_link_libraries( file_vector_interface_test 26 | srrg2_data_structures_library 27 | srrg2_image_library 28 | srrg2_property_library 29 | srrg2_system_utils_library 30 | srrg2_config_library 31 | ) 32 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_data_structures/correspondence.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | 4 | namespace srrg2_core { 5 | 6 | //! bookkeeping of the corespondence 7 | struct Correspondence { 8 | int fixed_idx = -1; 9 | int moving_idx = -1; 10 | float response; 11 | 12 | Correspondence(int fixed_idx_ = -1, int moving_idx_ = -1, float response_ = 0.f) : 13 | fixed_idx(fixed_idx_), 14 | moving_idx(moving_idx_), 15 | response(response_) { 16 | } 17 | 18 | inline bool operator<(const Correspondence& other) const { 19 | return (fixed_idx < other.fixed_idx) || 20 | (fixed_idx == other.fixed_idx && moving_idx < other.moving_idx); 21 | } 22 | }; 23 | 24 | using CorrespondenceVector = std::vector; 25 | } // namespace srrg2_core 26 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_data_structures/file_handle.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | 5 | namespace srrg2_core { 6 | 7 | struct FileHandle { 8 | FileHandle(){} 9 | FileHandle(std::string filename, 10 | bool read_only=false, 11 | bool create=false) { 12 | open(filename, read_only, create); 13 | } 14 | 15 | FileHandle(const FileHandle& other) = delete; 16 | FileHandle& operator=(const FileHandle&) =delete; 17 | 18 | void open(std::string filename, 19 | bool read_only=false, 20 | bool create=false); 21 | 22 | void close(); 23 | 24 | inline bool readOnly() const {return _read_only;} 25 | size_t size() const; 26 | void lseek(size_t pos); 27 | void write(const void* data, size_t size); 28 | void read(void* data, int size); 29 | ~FileHandle(); 30 | 31 | int _fd=-1; 32 | bool _read_only=false; 33 | }; 34 | 35 | using FileHandlePtr = std::shared_ptr; 36 | } 37 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_data_structures/path_matrix_cost_search.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "path_matrix_search_base.h" 3 | #include 4 | #include 5 | 6 | namespace srrg2_core { 7 | class PathMatrixCostSearch : public PathMatrixSearchBase { 8 | public: 9 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 10 | PARAM(PropertyFloat, 11 | min_distance, 12 | "min distance from an obstacle", 13 | 3.f, 14 | &this->_search_param_changed_flag); 15 | 16 | PARAM(PropertyVector_, 17 | cost_polynomial, 18 | "cost=p0*distance_to_next + p1*d + p2*d^2 + ... pn*d^n", 19 | std::vector(), 20 | &this->_search_param_changed_flag); 21 | enum SearchStatus { GoalFound = 1, GoalNotFound = -1, HeuristicMismatch = -2 }; 22 | float traversalCost(float distance_to_next, float distance_to_obstacle) const; 23 | 24 | protected: 25 | bool _search_param_changed_flag = true; 26 | }; 27 | } // namespace srrg2_core 28 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_data_structures/traverse_line_main.cpp: -------------------------------------------------------------------------------- 1 | #include "traverse_line_bresenham.h" 2 | #include 3 | #include 4 | 5 | using namespace srrg2_core; 6 | 7 | int main(int argc, char** argv) { 8 | int x1=atoi(argv[1]); 9 | int y1=atoi(argv[2]); 10 | int x2=atoi(argv[3]); 11 | int y2=atoi(argv[4]); 12 | LineActionPrint action; 13 | traverseLine(x1, y1, x2, y2, action); 14 | 15 | } 16 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_geometry/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(srrg2_geometry_library 2 | epipolar.cpp 3 | permutation_sampler.cpp 4 | ) 5 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_geometry/deprecated/point_types.cpp: -------------------------------------------------------------------------------- 1 | #include "point_types.h" 2 | /* 3 | namespace srrg2_core { 4 | 5 | void registerPointTypes() { 6 | Point2f::registerType(); 7 | PointWithIntensity2f::registerType(); 8 | PointWithNormal2f::registerType(); 9 | PointWithNormalIntensity2f::registerType(); 10 | PointWithNormalRGB2f::registerType(); 11 | 12 | Point2d::registerType(); 13 | PointWithIntensity2d::registerType(); 14 | PointWithNormal2d::registerType(); 15 | PointWithNormalIntensity2d::registerType(); 16 | PointWithNormalRGB2d::registerType(); 17 | 18 | Point3f::registerType(); 19 | PointWithIntensity3f::registerType(); 20 | PointWithNormal3f::registerType(); 21 | PointWithNormalIntensity3f::registerType(); 22 | PointWithNormalRGB3f::registerType(); 23 | 24 | Point3d::registerType(); 25 | PointWithIntensity3d::registerType(); 26 | PointWithNormal3d::registerType(); 27 | PointWithNormalIntensity3d::registerType(); 28 | PointWithNormalRGB3d::registerType(); 29 | } 30 | } 31 | */ 32 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_image/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(srrg2_image_library SHARED 2 | image.cpp image.h 3 | image_data.cpp image_data.h 4 | ) 5 | 6 | target_link_libraries(srrg2_image_library 7 | srrg2_boss_library 8 | ${OpenCV_LIBS} 9 | ) 10 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_matchable/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | #ia matchable library 2 | add_library(srrg2_matchable_library SHARED 3 | matchable.h 4 | matchable.hpp 5 | descriptor_owner.h 6 | visual_matchable.h 7 | instances.h 8 | instances.cpp) 9 | target_link_libraries(srrg2_matchable_library 10 | srrg2_boss_library 11 | srrg2_property_library 12 | srrg2_point_cloud_library) -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_matchable/instances.cpp: -------------------------------------------------------------------------------- 1 | #include "instances.h" 2 | 3 | namespace srrg2_core { 4 | 5 | void matchable_registerTypes() { 6 | // ia TODO check this, should work 7 | BOSS_REGISTER_BLOB(MatchablefVectorData); 8 | BOSS_REGISTER_BLOB(MatchabledVectorData); 9 | BOSS_REGISTER_BLOB(VisualMatchablefVectorData); 10 | BOSS_REGISTER_BLOB(VisualMatchabledVectorData); 11 | } 12 | 13 | } // namespace srrg2_core 14 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_matchable/instances.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | 4 | #include "matchable.h" 5 | #include "srrg_property/vector_data.h" 6 | #include "visual_matchable.h" 7 | 8 | namespace srrg2_core { 9 | 10 | using MatchablefVectorData = VectorData_; 11 | using MatchabledVectorData = VectorData_; 12 | using VisualMatchablefVectorData = VectorData_; 13 | using VisualMatchabledVectorData = VectorData_; 14 | 15 | //! @brief serialization and instanciation of the types 16 | void matchable_registerTypes() __attribute__((constructor)); 17 | 18 | } // namespace srrg2_core 19 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_messages/deprecated/joint_state_message.cpp: -------------------------------------------------------------------------------- 1 | #include "joint_state_message.h" 2 | 3 | namespace srrg2_core { 4 | 5 | JointStateMessage::JointStateMessage(const std::string& topic_, 6 | const std::string& frame_id_, 7 | const int& seq_, 8 | const double& timestamp_): BaseSensorMessage(topic_, frame_id_, seq_, timestamp_) {} 9 | 10 | JointStateMessage::~JointStateMessage() {} 11 | 12 | BOSS_REGISTER_CLASS(JointStateMessage); 13 | 14 | } //namespace srrg2_core 15 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_messages/deprecated/joint_state_message.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "../srrg_data_structures/events.h" 3 | #include "srrg_property/property_vector.h" 4 | #include "base_sensor_message.h" 5 | 6 | namespace srrg2_core { 7 | 8 | class JointStateMessage: public BaseSensorMessage { 9 | public: 10 | 11 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 12 | 13 | JointStateMessage(const std::string& topic_ = "", 14 | const std::string& frame_id_ = "", 15 | const int& seq_ = -1, 16 | const double& timestamp_ = -1.0); 17 | ~JointStateMessage(); 18 | 19 | BaseEventPtrVector joint_events; //ds TODO serialization 20 | }; 21 | 22 | } //namespace srrg2_core 23 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_messages/deprecated/static_transform_tree_message.cpp: -------------------------------------------------------------------------------- 1 | #include "static_transform_tree_message.h" 2 | 3 | namespace srrg2_core { 4 | 5 | StaticTransformTreeMessage::StaticTransformTreeMessage(const std::string& topic_, 6 | const std::string& frame_id_, 7 | const int& seq_, 8 | const double& timestamp_): 9 | BaseSensorMessage(topic_, frame_id_, seq_, timestamp_), 10 | SETUP_PROPERTY_NV(tree) { 11 | } 12 | 13 | BOSS_REGISTER_CLASS(StaticTransformTreeMessage); 14 | 15 | } /* namespace srrg2_core */ 16 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_messages/deprecated/static_transform_tree_message.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "srrg_property/property_serializable.h" 3 | #include "base_sensor_message.h" 4 | #include "static_transform_tree.h" 5 | 6 | namespace srrg2_core { 7 | 8 | class StaticTransformTreeMessage: public BaseSensorMessage { 9 | public: 10 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 11 | 12 | StaticTransformTreeMessage(const std::string& topic_ = "", 13 | const std::string& frame_id_ = "", 14 | const int& seq_ = -1, 15 | const double& timestamp_ = -1); 16 | PropertySerializable_ tree; 17 | }; 18 | 19 | } /* namespace srrg2_core */ 20 | 21 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_messages/deprecated/transform_stamped_message.cpp: -------------------------------------------------------------------------------- 1 | #include "transform_stamped_message.h" 2 | 3 | namespace srrg2_core { 4 | 5 | TransformStampedMessage::TransformStampedMessage(const std::string& topic_, 6 | const std::string& frame_id_, 7 | const int& seq_, 8 | const double& timestamp_): 9 | BaseSensorMessage(topic_, frame_id_, seq_, timestamp_), 10 | SETUP_PROPERTY(to_frame_id, ""), 11 | SETUP_PROPERTY(pose, Isometry3f::Identity()) { 12 | } 13 | 14 | BOSS_REGISTER_CLASS(TransformStampedMessage); 15 | 16 | } // namespace srrg2_core 17 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_messages/deprecated/transform_stamped_message.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "srrg_property/property_eigen.h" 3 | #include "base_sensor_message.h" 4 | 5 | namespace srrg2_core { 6 | 7 | using namespace geometry3d; 8 | 9 | class TransformStampedMessage: public BaseSensorMessage { 10 | public: 11 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 12 | TransformStampedMessage(const std::string& topic_ = "", 13 | const std::string& frame_id_ = "", 14 | const int& seq_ = -1, 15 | const double& timestamp_ = -1); 16 | 17 | PropertyString to_frame_id; 18 | PropertyEigen_ pose; 19 | }; 20 | 21 | } // namespace srrg2_core 22 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_messages/instances.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | //#include "messages/base_sensor_message.h" 3 | 4 | namespace srrg2_core { 5 | void messages_registerTypes() __attribute__((constructor)); 6 | } 7 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_messages/message_handlers/image_message_viewer.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "message_sink_base.h" 3 | #include "srrg_viewer/active_drawable.h" 4 | 5 | namespace srrg2_core { 6 | class ImageMessageViewer : public MessageSinkBase, public ActiveDrawable { 7 | public: 8 | PARAM_VECTOR(PropertyVector_, 9 | image_topics, 10 | "image topics to show", 11 | nullptr); 12 | 13 | ImageMessageViewer() = default; 14 | 15 | virtual ~ImageMessageViewer() = default; 16 | 17 | bool putMessage(BaseSensorMessagePtr msg_) override; 18 | 19 | void reset() override; 20 | 21 | protected: 22 | void _drawImpl(ViewerCanvasPtr canvas) const override; 23 | std::vector _images; 24 | }; 25 | 26 | using MessageSinkBasePtr = std::shared_ptr; 27 | } // namespace srrg2_core 28 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_messages/message_handlers/message_container_odom_accumulator_sink.cpp: -------------------------------------------------------------------------------- 1 | #include "message_container_odom_accumulator_sink.h" 2 | #include "srrg_messages/messages/odometry_message.h" 3 | 4 | namespace srrg2_core { 5 | 6 | bool MessageContainerOdomAccumulatorSink::putMessage(BaseSensorMessagePtr msg) { 7 | if (!_container) { 8 | return false; 9 | } 10 | if (_topic != msg->topic.value()) { 11 | return false; 12 | } 13 | if (auto odom_ptr = std::dynamic_pointer_cast(msg)) { 14 | double timestamp_seconds = odom_ptr->timestamp.value(); 15 | const Isometry3f& pose = odom_ptr->pose.value(); 16 | _container->insert(std::make_pair(timestamp_seconds, pose)); 17 | } 18 | return true; 19 | } 20 | 21 | } // namespace srrg2_core 22 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_messages/message_handlers/message_file_sink.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "message_file_sink_base.h" 3 | 4 | namespace srrg2_core { 5 | 6 | class MessageFileSink : public MessageFileSinkBase { 7 | public: 8 | ~MessageFileSink(); 9 | virtual void open(const std::string& filename) override; 10 | virtual void open() override; 11 | virtual void close() override; 12 | bool putMessage(BaseSensorMessagePtr msg_) override; 13 | 14 | protected: 15 | std::unique_ptr _serializer = nullptr; 16 | }; 17 | using MessageFileSinkPtr = std::shared_ptr; 18 | 19 | } // namespace srrg2_core 20 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_messages/message_handlers/message_file_sink_base.cpp: -------------------------------------------------------------------------------- 1 | #include "message_file_sink_base.h" 2 | 3 | namespace srrg2_core { 4 | 5 | MessageFileSinkBase::~MessageFileSinkBase() { 6 | if (_is_open) { 7 | close(); 8 | } 9 | } 10 | 11 | void MessageFileSinkBase::open(const std::string& filename_) { 12 | if (_is_open) { 13 | this->close(); 14 | } 15 | param_filename.setValue(filename_); 16 | open(); 17 | } 18 | 19 | void MessageFileSinkBase::close() { 20 | _is_open = false; 21 | } 22 | 23 | } // namespace srrg2_core 24 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_messages/message_handlers/message_file_sink_base.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "message_sink_base.h" 3 | 4 | namespace srrg2_core { 5 | 6 | class MessageFileSinkBase : public MessageSinkBase { 7 | public: 8 | PARAM(PropertyBool, verbose, "verbose", false, 0); 9 | PARAM(PropertyString, filename, "file to write", "", &_file_changed_flag); 10 | 11 | public: 12 | MessageFileSinkBase() { 13 | this->_is_open = false; 14 | } 15 | 16 | virtual ~MessageFileSinkBase(); 17 | virtual void open(const std::string& filename); 18 | virtual void open() = 0; 19 | virtual void close(); 20 | 21 | protected: 22 | bool _file_changed_flag = true; 23 | }; 24 | 25 | using MessageFileSinkBasePtr = std::shared_ptr; 26 | 27 | } // namespace srrg2_core 28 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_messages/message_handlers/message_file_source.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "message_file_source_base.h" 3 | #include "srrg_messages/messages/base_sensor_message.h" 4 | 5 | namespace srrg2_core { 6 | 7 | class MessageFileSource : public MessageFileSourceBase { 8 | public: 9 | virtual void open(const std::string& filename) override; 10 | virtual void open() override; 11 | virtual void close() override; 12 | 13 | MessageFileSource() = default; 14 | virtual ~MessageFileSource(); 15 | BaseSensorMessagePtr getMessage() override; 16 | void reset() override; 17 | 18 | protected: 19 | std::unique_ptr _deserializer = nullptr; 20 | }; 21 | 22 | using MessageFileSourcePtr = std::shared_ptr; 23 | 24 | } // namespace srrg2_core 25 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_messages/message_handlers/message_file_source_base.cpp: -------------------------------------------------------------------------------- 1 | #include "message_file_source_base.h" 2 | #include "srrg_config/configurable.h" 3 | #include "srrg_system_utils/system_utils.h" 4 | 5 | namespace srrg2_core { 6 | 7 | void MessageFileSourceBase::open(const std::string& filename_) { 8 | if (_is_open) { 9 | this->close(); 10 | } 11 | if (!isAccessible(filename_)) { 12 | std::string msg = filename_ + " not found"; 13 | throw std::runtime_error(msg.c_str()); 14 | } 15 | param_filename.setValue(filename_); 16 | open(); 17 | _is_open = true; 18 | _file_changed_flag = false; 19 | } 20 | 21 | MessageSourceBase* MessageFileSourceBase::getRootSource() { 22 | return this; 23 | } 24 | 25 | void MessageFileSourceBase::close() { 26 | if (!_is_open) { 27 | return; 28 | } 29 | _is_open = false; 30 | } 31 | 32 | } // namespace srrg2_core 33 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_messages/message_handlers/message_file_source_base.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "message_source_base.h" 3 | #include "srrg_config/configurable.h" 4 | #include "srrg_property/property.h" 5 | #include 6 | 7 | namespace srrg2_core { 8 | 9 | class MessageFileSourceBase : public MessageSourceBase { 10 | // configuration 11 | public: 12 | PARAM(PropertyString, filename, "file to read", "", &_file_changed_flag); 13 | 14 | public: 15 | MessageFileSourceBase() : MessageSourceBase(){}; 16 | virtual ~MessageFileSourceBase() = default; 17 | 18 | MessageSourceBase* getRootSource() override; 19 | 20 | virtual void open(const std::string& filename); 21 | virtual void open() override = 0; 22 | virtual void close(); 23 | 24 | protected: 25 | bool _file_changed_flag = true; 26 | 27 | public: 28 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 29 | }; 30 | 31 | using MessageFileSourceBasePtr = std::shared_ptr; 32 | 33 | } // namespace srrg2_core 34 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_messages/message_handlers/message_filter_base.cpp: -------------------------------------------------------------------------------- 1 | #include "message_filter_base.h" 2 | namespace srrg2_core { 3 | 4 | void MessageFilterBase::reset() { 5 | if (param_source.value()) { 6 | return param_source->reset(); 7 | } 8 | } 9 | 10 | MessageSourceBase* MessageFilterBase::getRootSource() { 11 | if (param_source.value()) { 12 | return param_source->getRootSource(); 13 | } 14 | return nullptr; 15 | } 16 | } // namespace srrg2_core 17 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_messages/message_handlers/message_filter_base.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "message_source_base.h" 3 | #include "srrg_config/property_configurable.h" 4 | namespace srrg2_core { 5 | 6 | class MessageFilterBase : public MessageSourceBase { 7 | public: 8 | PARAM(PropertyConfigurable_, 9 | source, 10 | "the source from where this filter reads", 11 | nullptr, 12 | nullptr); 13 | 14 | virtual ~MessageFilterBase() = default; 15 | MessageSourceBase* getRootSource() override; 16 | BaseSensorMessagePtr getMessage() = 0; 17 | void reset() override; 18 | }; 19 | 20 | using MessageFilterBasePtr = std::shared_ptr; 21 | 22 | } // namespace srrg2_core 23 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_messages/message_handlers/message_odom_subsampler_sink.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "message_sink_base.h" 3 | #include "srrg_config/configurable.h" 4 | #include "srrg_config/param_macros.h" 5 | 6 | namespace srrg2_core { 7 | 8 | class MessageOdomSubsamplerSink : public MessageSinkBase { 9 | public: 10 | PARAM(PropertyString, odom_topic, "odometry topic to subsample", "/odom", 0); 11 | PARAM(PropertyFloat, translation_min, "minimum translation between odometries [m]", 0.25, 0); 12 | PARAM(PropertyFloat, rotation_min, "minimum rotation between odometries [rad]", 0.25, 0); 13 | bool putMessage(BaseSensorMessagePtr msg_) override; 14 | void reset() override; 15 | 16 | protected: 17 | float _cum_rotation = 0; // cumulative rotation since last output message 18 | float _cum_translation = 0; // cumulative translation since last output message 19 | Isometry3f _prev_pose = Isometry3f::Identity(); 20 | }; 21 | 22 | } // namespace srrg2_core 23 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_messages/message_handlers/message_odom_subsampler_source.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "message_filter_base.h" 3 | #include "srrg_config/configurable.h" 4 | #include "srrg_config/param_macros.h" 5 | 6 | namespace srrg2_core { 7 | 8 | class MessageOdomSubsamplerSource : public MessageFilterBase { 9 | public: 10 | PARAM(PropertyString, odom_topic, "odometry topic to subsample", "/odom", 0); 11 | PARAM(PropertyFloat, translation_min, "minimum translation between odometries [m]", 0.25, 0); 12 | PARAM(PropertyFloat, rotation_min, "minimum rotation between odometries [rad]", 0.25, 0); 13 | 14 | public: 15 | BaseSensorMessagePtr getMessage() override; 16 | 17 | protected: 18 | float _cum_rotation = 0; // cumulative rotation since last output message 19 | float _cum_translation = 0; // cumulative translation since last output message 20 | Isometry3f _prev_pose = Isometry3f::Identity(); 21 | }; 22 | 23 | } // namespace srrg2_core 24 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_messages/message_handlers/message_pack.cpp: -------------------------------------------------------------------------------- 1 | #include "message_pack.h" 2 | 3 | namespace srrg2_core { 4 | 5 | MessagePack::MessagePack(const std::string& topic_, 6 | const std::string& frame_id_, 7 | const int& seq_, 8 | const double& timestamp_) : 9 | BaseSensorMessage(topic_, frame_id_, seq_, timestamp_) { 10 | } 11 | 12 | } // namespace srrg2_core 13 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_messages/message_handlers/message_pack.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "../messages/base_sensor_message.h" 3 | 4 | namespace srrg2_core { 5 | 6 | // a message pack "owns" its messages 7 | class MessagePack : public BaseSensorMessage { 8 | public: 9 | MessagePack(const std::string& topic_ = "", 10 | const std::string& frame_id_ = "", 11 | const int& seq_ = -1, 12 | const double& timestamp_ = -1.0); 13 | 14 | std::vector messages; 15 | 16 | }; 17 | 18 | typedef std::shared_ptr MessagePackPtr; 19 | } // namespace srrg2_core 20 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_messages/message_handlers/message_selector_sink.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "message_sink_base.h" 3 | #include 4 | 5 | namespace srrg2_core { 6 | 7 | class MessageSelectorSink : public MessageSinkBase { 8 | public: 9 | PARAM_VECTOR(PropertyVector_, 10 | topics, 11 | "topics to be propagated", 12 | &_topics_changed_flag); 13 | 14 | MessageSelectorSink() = default; 15 | virtual ~MessageSelectorSink() = default; 16 | 17 | bool putMessage(BaseSensorMessagePtr msg_) override; 18 | 19 | protected: 20 | bool _topics_changed_flag = true; 21 | 22 | void refreshMap(); 23 | void putMessageRecursive(BaseSensorMessagePtr msg_); 24 | std::set _topics; 25 | }; 26 | 27 | } // namespace srrg2_core 28 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_messages/message_handlers/message_sorted_source.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "message_filter_base.h" 3 | 4 | namespace srrg2_core { 5 | 6 | class MessageSortedSource : public MessageFilterBase { 7 | public: 8 | PARAM(PropertyDouble, time_interval, "lag time to sort messages", 1., 0); 9 | 10 | MessageSortedSource(); 11 | BaseSensorMessagePtr getMessage() override; 12 | 13 | int numDroppedMessages() const { 14 | return _num_dropped_messages; 15 | } 16 | 17 | void resetCounters(); 18 | void reset() override; 19 | 20 | protected: 21 | double earliestStamp(); 22 | inline double latestStamp(); 23 | std::multimap _msg_queue; 24 | int _num_dropped_messages = 0; 25 | }; 26 | 27 | using MessageSortedSourcePtr = std::shared_ptr; 28 | 29 | } // namespace srrg2_core 30 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_messages/message_handlers/message_source_base.cpp: -------------------------------------------------------------------------------- 1 | #include "message_source_base.h" 2 | 3 | namespace srrg2_core { 4 | 5 | MessageSourceBase::MessageSourceBase() : _is_open(false) { 6 | } 7 | 8 | } // namespace srrg2_core 9 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_messages/message_handlers/message_source_base.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "srrg_boss/deserializer.h" 3 | #include "srrg_config/configurable.h" 4 | #include "srrg_config/property_configurable.h" 5 | #include "srrg_messages/messages/base_sensor_message.h" 6 | #include "srrg_property/property_vector.h" 7 | 8 | namespace srrg2_core { 9 | 10 | class MessageSourceBase : public Configurable { 11 | public: 12 | MessageSourceBase(); 13 | virtual ~MessageSourceBase() = default; 14 | 15 | virtual MessageSourceBase* getRootSource() = 0; 16 | virtual BaseSensorMessagePtr getMessage() = 0; 17 | 18 | inline virtual void open(){}; 19 | inline bool isOpen() { 20 | return _is_open; 21 | } 22 | inline virtual void reset() { 23 | } 24 | volatile bool _running = true; 25 | 26 | protected: 27 | bool _is_open = false; 28 | }; 29 | 30 | using MessageSourceBasePtr = std::shared_ptr; 31 | 32 | } // namespace srrg2_core 33 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_messages/messages/base_sensor_message.cpp: -------------------------------------------------------------------------------- 1 | #include "base_sensor_message.h" 2 | 3 | namespace srrg2_core { 4 | using namespace geometry3d; 5 | 6 | BaseSensorMessage::BaseSensorMessage(const std::string& topic_, const std::string& frame_id_, int seq_, const double& timestamp_): 7 | SETUP_PROPERTY(topic, topic_), 8 | SETUP_PROPERTY(frame_id, frame_id_), 9 | SETUP_PROPERTY(seq, seq_), 10 | SETUP_PROPERTY(timestamp, timestamp_) { 11 | std::setlocale(LC_ALL, "en_US.UTF-8"); 12 | } 13 | 14 | } /* namespace srrg2_core */ 15 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_messages/messages/base_sensor_message.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include "srrg_property/property_container.h" 7 | #include "srrg_geometry/geometry_defs.h" 8 | #include "srrg_geometry/geometry3d.h" 9 | 10 | namespace srrg2_core { 11 | 12 | class BaseSensorMessage: public PropertyContainerSerializable { 13 | public: 14 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 15 | BaseSensorMessage(const std::string& topic_ = "", 16 | const std::string& frame_id_ = "", 17 | int seq_ = -1, 18 | const double& timestamp_ = -1); 19 | 20 | PropertyString topic; 21 | PropertyString frame_id; 22 | PropertyInt seq; 23 | PropertyDouble timestamp; 24 | }; 25 | 26 | typedef std::shared_ptr BaseSensorMessagePtr; 27 | typedef std::vector BaseSensorMessagePtrVector; 28 | 29 | } /* namespace srrg2_core */ 30 | 31 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_messages/messages/camera_info_message.cpp: -------------------------------------------------------------------------------- 1 | #include "camera_info_message.h" 2 | 3 | namespace srrg2_core { 4 | CameraInfoMessage::CameraInfoMessage(const std::string& topic_, 5 | const std::string& frame_id_, 6 | const int& seq_, 7 | const double& timestamp_) : 8 | BaseSensorMessage(topic_, frame_id_, seq_, timestamp_), 9 | SETUP_PROPERTY(rows, 0), 10 | SETUP_PROPERTY(cols, 0), 11 | SETUP_PROPERTY(depth_scale, 1e-3), 12 | SETUP_PROPERTY(projection_model, "pinhole"), 13 | SETUP_PROPERTY(distortion_model, ""), 14 | SETUP_PROPERTY(camera_matrix, Matrix3f::Identity()), 15 | SETUP_PROPERTY(distortion_coefficients, Eigen::VectorXd()) { 16 | } 17 | 18 | } // namespace srrg2_core 19 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_messages/messages/cmd_vel_message.cpp: -------------------------------------------------------------------------------- 1 | #include "cmd_vel_message.h" 2 | 3 | namespace srrg2_core { 4 | CmdVelMessage::CmdVelMessage(const std::string& topic_, 5 | const std::string& frame_id_, 6 | const int& seq_, 7 | const double& timestamp_) : 8 | BaseSensorMessage(topic_, frame_id_, seq_, timestamp_), 9 | SETUP_PROPERTY(linear, 0), 10 | SETUP_PROPERTY(angular, 0) { 11 | } 12 | 13 | } // namespace srrg2_core 14 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_messages/messages/cmd_vel_message.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "base_sensor_message.h" 3 | #include "srrg_property/property.h" 4 | 5 | namespace srrg2_core { 6 | 7 | class CmdVelMessage : public BaseSensorMessage { 8 | public: 9 | CmdVelMessage(const std::string& topic_ = "", 10 | const std::string& frame_id_ = "", 11 | const int& seq_ = -1, 12 | const double& timestamp_ = -1); 13 | PropertyFloat linear; 14 | PropertyFloat angular; 15 | }; 16 | 17 | using CmdVelMessagePtr = std::shared_ptr; 18 | 19 | } // namespace srrg2_core -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_messages/messages/grid_map_message.cpp: -------------------------------------------------------------------------------- 1 | #include "grid_map_message.h" 2 | namespace srrg2_core { 3 | GridMapMessage::GridMapMessage(const std::string& topic_, 4 | const std::string& frame_id_, 5 | const int& seq_, 6 | const double& timestamp_) : 7 | BaseSensorMessage(topic_, frame_id_, seq_, timestamp_), 8 | SETUP_PROPERTY_NV(grid_map_struct) { 9 | } 10 | } // namespace srrg2_core 11 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_messages/messages/grid_map_message.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "base_sensor_message.h" 3 | #include "srrg_data_structures/grid_map_2d.h" 4 | #include "srrg_property/property_serializable.h" 5 | 6 | namespace srrg2_core { 7 | class GridMapMessage : public BaseSensorMessage { 8 | public: 9 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 10 | GridMapMessage(const std::string& topic_ = "", 11 | const std::string& frame_id_ = "", 12 | const int& seq_ = -1, 13 | const double& timestamp_ = -1.0); 14 | PropertySerializable_ grid_map_struct; 15 | }; 16 | using GridMapMessagePtr = std::shared_ptr; 17 | } // namespace srrg2_core 18 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_messages/messages/joints_message.cpp: -------------------------------------------------------------------------------- 1 | #include "joints_message.h" 2 | 3 | namespace srrg2_core { 4 | 5 | JointsMessage::JointsMessage(const std::string& topic_, 6 | const std::string& frame_id_, 7 | const int& seq_, 8 | const double& timestamp_): BaseSensorMessage(topic_, frame_id_, seq_, timestamp_), 9 | SETUP_PROPERTY_NV(joint_events) { 10 | } 11 | } 12 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_messages/messages/joints_message.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "base_sensor_message.h" 3 | #include "srrg_data_structures/events.h" 4 | 5 | namespace srrg2_core { 6 | 7 | class JointsMessage: public BaseSensorMessage { 8 | public: 9 | JointsMessage(const std::string& topic_ = "", 10 | const std::string& frame_id_ = "", 11 | const int& seq_ = -1, 12 | const double& timestamp_ = -1); 13 | 14 | PropertyJointEventVector joint_events; 15 | 16 | public: 17 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 18 | }; 19 | 20 | using JointsMessagePtr = std::shared_ptr; 21 | 22 | } 23 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_messages/messages/laser_message.cpp: -------------------------------------------------------------------------------- 1 | #include "laser_message.h" 2 | 3 | namespace srrg2_core { 4 | 5 | LaserMessage::LaserMessage(const std::string& topic_, 6 | const std::string& frame_id_, 7 | const int& seq_, 8 | const double& timestamp_): 9 | BaseSensorMessage(topic_, frame_id_, seq_, timestamp_), 10 | SETUP_PROPERTY(angle_min, 0), 11 | SETUP_PROPERTY(angle_max, 0), 12 | SETUP_PROPERTY(angle_increment, 0), 13 | SETUP_PROPERTY(time_increment, 0), 14 | SETUP_PROPERTY(scan_time, 0), 15 | SETUP_PROPERTY(range_min, 0), 16 | SETUP_PROPERTY(range_max, 0), 17 | SETUP_PROPERTY(ranges, std::vector()), 18 | SETUP_PROPERTY(intensities, std::vector()) { 19 | } 20 | 21 | } // namespace srrg2_core 22 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_messages/messages/laser_message.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "srrg_property/property_vector.h" 3 | #include "base_sensor_message.h" 4 | 5 | namespace srrg2_core { 6 | 7 | class LaserMessage: public BaseSensorMessage { 8 | public: 9 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 10 | LaserMessage(const std::string& topic_ = "", 11 | const std::string& frame_id_ = "", 12 | const int& seq_ = -1, 13 | const double& timestamp_ = -1.0); 14 | PropertyFloat angle_min; 15 | PropertyFloat angle_max; 16 | PropertyFloat angle_increment; 17 | PropertyFloat time_increment; 18 | PropertyFloat scan_time; 19 | PropertyFloat range_min; 20 | PropertyFloat range_max; 21 | PropertyVector_ ranges; 22 | PropertyVector_ intensities; 23 | }; 24 | 25 | using LaserMessagePtr = std::shared_ptr; 26 | 27 | } // namespace srrg2_core 28 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_messages/messages/odometry_message.cpp: -------------------------------------------------------------------------------- 1 | #include "odometry_message.h" 2 | 3 | namespace srrg2_core { 4 | OdometryMessage::OdometryMessage(const std::string& topic_, 5 | const std::string& frame_id_, 6 | const int& seq_, 7 | const double& timestamp_) : 8 | BaseSensorMessage(topic_, frame_id_, seq_, timestamp_), 9 | SETUP_PROPERTY(child_frame, ""), 10 | SETUP_PROPERTY(pose, Isometry3f::Identity()), 11 | SETUP_PROPERTY(linear_velocity, Vector3f(0, 0, 0)), 12 | SETUP_PROPERTY(angular_velocity, Vector3f(0, 0, 0)), 13 | SETUP_PROPERTY(pose_covariance, Matrix6f::Identity()), 14 | SETUP_PROPERTY(twist_covariance, Matrix6f::Identity()) { 15 | } 16 | 17 | } // namespace srrg2_core 18 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_messages/messages/odometry_message.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "base_sensor_message.h" 4 | #include 5 | 6 | namespace srrg2_core { 7 | class OdometryMessage : public BaseSensorMessage { 8 | public: 9 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 10 | OdometryMessage(const std::string& topic_ = "", 11 | const std::string& frame_id_ = "", 12 | const int& seq_ = -1, 13 | const double& timestamp_ = -1); 14 | 15 | PropertyString child_frame; 16 | PropertyEigen_ pose; 17 | PropertyEigen_ linear_velocity; 18 | PropertyEigen_ angular_velocity; 19 | PropertyEigen_ pose_covariance; 20 | PropertyEigen_ twist_covariance; 21 | }; 22 | 23 | using OdometryMessagePtr = std::shared_ptr; 24 | 25 | } // namespace srrg2_core 26 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_messages/messages/path_message.cpp: -------------------------------------------------------------------------------- 1 | #include "path_message.h" 2 | 3 | namespace srrg2_core { 4 | PathMessage::PathMessage(const std::string& topic_, 5 | const std::string& frame_id_, 6 | const int& seq_, 7 | const double& timestamp_): 8 | BaseSensorMessage(topic_, frame_id_, seq_, timestamp_), 9 | SETUP_PROPERTY_NV(poses) 10 | {} 11 | 12 | PathMessage::~PathMessage(){} 13 | 14 | } 15 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_messages/messages/path_message.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "base_sensor_message.h" 3 | #include "pose_stamped_message.h" 4 | #include "srrg_geometry/geometry_defs.h" 5 | #include "srrg_property/property_serializable.h" 6 | 7 | namespace srrg2_core { 8 | class PathMessage: public BaseSensorMessage { 9 | public: 10 | PathMessage(const std::string& topic_ = "", 11 | const std::string& frame_id_ = "", 12 | const int& seq_ = -1, 13 | const double& timestamp_ = -1); 14 | 15 | PropertySerializableVector_ poses; 16 | virtual ~PathMessage(); 17 | }; 18 | 19 | using PathMessagePtr = std::shared_ptr; 20 | } 21 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_messages/messages/planner_status_message.cpp: -------------------------------------------------------------------------------- 1 | #include "planner_status_message.h" 2 | 3 | namespace srrg2_core { 4 | 5 | PlannerStatusMessage::PlannerStatusMessage(const std::string& topic_, 6 | const std::string& frame_id_, 7 | int seq_, 8 | const double& timestamp_): 9 | BaseSensorMessage(topic_, frame_id_, seq_, timestamp_), 10 | SETUP_PROPERTY(status, "idle"), 11 | SETUP_PROPERTY(a_star_status, 0), 12 | SETUP_PROPERTY(path_type, 0), 13 | SETUP_PROPERTY(cost_to_global_goal, 0.f), 14 | SETUP_PROPERTY(distance_to_global_goal, 0.f), 15 | SETUP_PROPERTY(distance_to_local_goal, 0.f) 16 | { 17 | 18 | } 19 | 20 | } 21 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_messages/messages/planner_status_message.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "base_sensor_message.h" 3 | 4 | namespace srrg2_core { 5 | 6 | class PlannerStatusMessage: public BaseSensorMessage { 7 | public: 8 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 9 | PlannerStatusMessage(const std::string& topic_ = "", 10 | const std::string& frame_id_ = "", 11 | int seq_ = -1, 12 | const double& timestamp_ = -1); 13 | 14 | PropertyString status; 15 | PropertyInt a_star_status; 16 | PropertyInt path_type; 17 | PropertyFloat cost_to_global_goal; 18 | PropertyFloat distance_to_global_goal; 19 | PropertyFloat distance_to_local_goal; 20 | }; 21 | 22 | typedef std::shared_ptr PlannerStatusMessagePtr; 23 | 24 | } /* namespace srrg2_core */ 25 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_messages/messages/point_field.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "srrg_property/property_container.h" 3 | 4 | namespace srrg2_core { 5 | 6 | class PointField : public PropertyContainerIdentifiable { 7 | public: 8 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 9 | 10 | PointField(); 11 | PointField(const PointField& cp_); 12 | static const uint8_t INT8; 13 | static const uint8_t UINT8; 14 | static const uint8_t INT16; 15 | static const uint8_t UINT16; 16 | static const uint8_t INT32; 17 | static const uint8_t UINT32; 18 | static const uint8_t FLOAT32; 19 | static const uint8_t FLOAT64; 20 | PropertyString name; 21 | PropertyUnsignedInt offset; 22 | PropertyUInt8 datatype; 23 | PropertyUnsignedInt count; 24 | }; 25 | 26 | std::ostream& operator<<(std::ostream& stream_, const PointField& point_field_); 27 | 28 | } // namespace srrg2_core 29 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_messages/messages/point_stamped_message.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace srrg2_core { 4 | 5 | PointStampedMessage::PointStampedMessage(const std::string& topic_, 6 | const std::string& frame_id_, 7 | const int& seq_, 8 | const double& timestamp_) : 9 | BaseSensorMessage(topic_, frame_id_, seq_, timestamp_), 10 | SETUP_PROPERTY(point, Vector3f(0, 0, 0)) { 11 | } 12 | 13 | } /* namespace srrg2_slam_architecture */ // srrg2_slam_architecture 14 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_messages/messages/point_stamped_message.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "srrg_property/property_eigen.h" 3 | #include "base_sensor_message.h" 4 | 5 | namespace srrg2_core { 6 | 7 | class PointStampedMessage: public BaseSensorMessage { 8 | public: 9 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 10 | PointStampedMessage(const std::string& topic_ = "", 11 | const std::string& frame_id_ = "", 12 | const int& seq_ = -1, 13 | const double& timestamp_ = -1); 14 | PropertyEigen_ point; 15 | }; 16 | 17 | using PointStampedMessagePtr = std::shared_ptr; 18 | 19 | } /* namespace srrg2_slam_architecture */ 20 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_messages/messages/pose_array_message.cpp: -------------------------------------------------------------------------------- 1 | #include "pose_array_message.h" 2 | namespace srrg2_core { 3 | PoseArrayMessage::PoseArrayMessage(const std::string& topic_, 4 | const std::string& frame_id_, 5 | const int& seq_, 6 | const double& timestamp_) : 7 | BaseSensorMessage(topic_, frame_id_, seq_, timestamp_), 8 | SETUP_PROPERTY_NV(poses){} 9 | 10 | PoseArrayMessage::~PoseArrayMessage() {} 11 | } 12 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_messages/messages/pose_array_message.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "base_sensor_message.h" 3 | #include "pose_message.h" 4 | #include "srrg_property/property_serializable.h" 5 | #include "srrg_geometry/geometry_defs.h" 6 | 7 | namespace srrg2_core { 8 | class PoseArrayMessage: public BaseSensorMessage { 9 | public: 10 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 11 | PoseArrayMessage(const std::string& topic_ = "", 12 | const std::string& frame_id_ = "", 13 | const int& seq_ = -1, 14 | const double& timestamp_ = -1); 15 | 16 | PropertySerializableVector_ poses; 17 | 18 | virtual ~PoseArrayMessage(); 19 | 20 | protected: 21 | 22 | 23 | }; 24 | 25 | using PoseArrayMessagePtr = std::shared_ptr; 26 | } 27 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_messages/messages/pose_message.cpp: -------------------------------------------------------------------------------- 1 | #include "pose_message.h" 2 | namespace srrg2_core { 3 | PoseMessage::PoseMessage(): 4 | SETUP_PROPERTY(pose_vector, Vector6f::Zero()){ 5 | } 6 | 7 | PoseMessage::~PoseMessage(){} 8 | 9 | } 10 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_messages/messages/pose_stamped_message.cpp: -------------------------------------------------------------------------------- 1 | #include "pose_stamped_message.h" 2 | namespace srrg2_core { 3 | PoseStampedMessage::PoseStampedMessage(const std::string& topic_, 4 | const std::string& frame_id_, 5 | const int& seq_, 6 | const double& timestamp_): 7 | BaseSensorMessage(topic_, frame_id_, seq_, timestamp_), 8 | SETUP_PROPERTY_NV(pose){} 9 | } 10 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_messages/messages/pose_stamped_message.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "base_sensor_message.h" 3 | #include "pose_message.h" 4 | #include "srrg_property/property_serializable.h" 5 | #include "srrg_geometry/geometry_defs.h" 6 | #include "srrg_property/property_eigen.h" 7 | 8 | namespace srrg2_core { 9 | class PoseStampedMessage: public BaseSensorMessage { 10 | public: 11 | PoseStampedMessage(const std::string& topic_ = "", 12 | const std::string& frame_id_ = "", 13 | const int& seq_ = -1, 14 | const double& timestamp_ = -1); 15 | 16 | PropertySerializable_ pose; 17 | }; 18 | 19 | using PoseStampedMessagePtr = std::shared_ptr; 20 | } 21 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_messages/messages/pose_with_covariance_stamped_message.cpp: -------------------------------------------------------------------------------- 1 | #include "pose_with_covariance_stamped_message.h" 2 | namespace srrg2_core { 3 | PoseWithCovarianceStampedMessage::PoseWithCovarianceStampedMessage(const std::string& topic_, 4 | const std::string& frame_id_, 5 | const int& seq_, 6 | const double& timestamp_): 7 | PoseStampedMessage(topic_, frame_id_, seq_, timestamp_), 8 | SETUP_PROPERTY(covariance, Matrix6f::Identity()){} 9 | 10 | PoseWithCovarianceStampedMessage::~PoseWithCovarianceStampedMessage(){} 11 | 12 | } 13 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_messages/messages/pose_with_covariance_stamped_message.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "pose_stamped_message.h" 3 | #include "srrg_geometry/geometry_defs.h" 4 | 5 | namespace srrg2_core { 6 | class PoseWithCovarianceStampedMessage: public PoseStampedMessage { 7 | public: 8 | PoseWithCovarianceStampedMessage(const std::string& topic_ = "", 9 | const std::string& frame_id_ = "", 10 | const int& seq_ = -1, 11 | const double& timestamp_ = -1); 12 | 13 | PropertyEigen_ covariance; 14 | virtual ~PoseWithCovarianceStampedMessage(); 15 | }; 16 | 17 | using PoseWithCovarianceStampedMessagePtr = std::shared_ptr; 18 | } 19 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_messages/messages/range_message.cpp: -------------------------------------------------------------------------------- 1 | #include "range_message.h" 2 | 3 | namespace srrg2_core { 4 | 5 | RangeMessage::RangeMessage(const std::string& topic_, 6 | const std::string& frame_id_, 7 | const int& seq_, 8 | const double& timestamp_): 9 | BaseSensorMessage(topic_, frame_id_, seq_, timestamp_), 10 | SETUP_PROPERTY(range_min, 0), 11 | SETUP_PROPERTY(range_max, 10), 12 | SETUP_PROPERTY(field_of_view, M_PI), 13 | SETUP_PROPERTY(radiation_type, RadiationType::ULTRASOUND), 14 | SETUP_PROPERTY(range, 0) { 15 | } 16 | 17 | } // namespace srrg2_core 18 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_messages/messages/range_message.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "base_sensor_message.h" 3 | #include "srrg_property/property_vector.h" 4 | 5 | namespace srrg2_core { 6 | 7 | enum RadiationType { 8 | ULTRASOUND = 0, INFRARED = 1 9 | }; 10 | 11 | class RangeMessage: public BaseSensorMessage { 12 | public: 13 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 14 | RangeMessage(const std::string& topic_ = "", 15 | const std::string& frame_id_ = "", 16 | const int& seq_ = -1, 17 | const double& timestamp_ = -1.0); 18 | PropertyFloat range_min; 19 | PropertyFloat range_max; 20 | PropertyFloat field_of_view; 21 | PropertyInt radiation_type; 22 | PropertyFloat range; 23 | }; 24 | 25 | using RangeMessagePtr = std::shared_ptr; 26 | } //namespace srrg2_core 27 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_messages/messages/ticks_message.cpp: -------------------------------------------------------------------------------- 1 | #include "ticks_message.h" 2 | 3 | namespace srrg2_core { 4 | void Tick::serialize(ObjectData& odata, IdContext& context) { 5 | odata.setString("joint_name", joint_name); 6 | odata.setInt("max_count", max_count); 7 | odata.setInt("count", count); 8 | } 9 | 10 | void Tick::deserialize(ObjectData& odata, IdContext& context) { 11 | joint_name = odata.getString("joint_name"); 12 | max_count = odata.getInt("max_count"); 13 | count = odata.getInt("count"); 14 | } 15 | 16 | TicksMessage::TicksMessage(const std::string& topic_, 17 | const std::string& frame_id_, 18 | const int& seq_, 19 | const double& timestamp_): 20 | BaseSensorMessage(topic_, frame_id_, seq_, timestamp_), 21 | SETUP_PROPERTY_NV(ticks) { 22 | } 23 | 24 | } // namespace srrg2_core 25 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_messages/messages/ticks_message.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "base_sensor_message.h" 6 | #include "srrg_property/property_serializable.h" 7 | 8 | namespace srrg2_core { 9 | 10 | struct Tick: public Serializable { 11 | void serialize(ObjectData& odata, IdContext& context) override; 12 | void deserialize(ObjectData& odata, IdContext& context) override; 13 | 14 | std::string joint_name; 15 | int max_count; 16 | int count; 17 | }; 18 | 19 | class TicksMessage: public BaseSensorMessage { 20 | public: 21 | 22 | TicksMessage(const std::string& topic_ = "", 23 | const std::string& frame_id_ = "", 24 | const int& seq_ = -1, 25 | const double& timestamp_ = -1.0); 26 | 27 | PropertySerializableVector_ ticks; 28 | 29 | }; 30 | 31 | using TickMessagePtr = std::shared_ptr; 32 | 33 | 34 | } // namespace srrg2_core 35 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_messages/messages/transform_events_message.cpp: -------------------------------------------------------------------------------- 1 | #include "transform_events_message.h" 2 | 3 | namespace srrg2_core { 4 | 5 | TransformEventsMessage::TransformEventsMessage(const std::string& topic_, 6 | const std::string& frame_id_, 7 | const int& seq_, 8 | const double& timestamp_) : 9 | BaseSensorMessage(topic_, frame_id_, seq_, timestamp_), 10 | SETUP_PROPERTY_NV(events) { 11 | } 12 | 13 | using TransformEventsMessagePtr = std::shared_ptr; 14 | } // namespace srrg2_core 15 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_messages/messages/transform_events_message.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "srrg_property/property_eigen.h" 3 | #include "base_sensor_message.h" 4 | #include "srrg_data_structures/events.h" 5 | 6 | namespace srrg2_core{ 7 | 8 | class TransformEventsMessage: public BaseSensorMessage { 9 | public: 10 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 11 | 12 | TransformEventsMessage(const std::string& topic_ = "", 13 | const std::string& frame_id_ = "", 14 | const int& seq_ = -1, 15 | const double& timestamp_ = -1); 16 | 17 | PropertyTransformEventVector events; 18 | }; 19 | 20 | using TransformEventsMessagePtr = std::shared_ptr; 21 | 22 | } 23 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_messages/messages/twist_stamped_message.cpp: -------------------------------------------------------------------------------- 1 | #include "twist_stamped_message.h" 2 | 3 | namespace srrg2_core { 4 | 5 | TwistStampedMessage::TwistStampedMessage(const std::string& topic_, 6 | const std::string& frame_id_, 7 | const int& seq_, 8 | const double& timestamp_) : 9 | BaseSensorMessage(topic_, frame_id_, seq_, timestamp_), 10 | SETUP_PROPERTY(linear, Vector3f(0, 0, 0)), 11 | SETUP_PROPERTY(angular, Vector3f(0, 0, 0)) { 12 | } 13 | 14 | 15 | } /* namespace srrg2_slam_architecture */ // srrg2_slam_architecture 16 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_messages/messages/twist_stamped_message.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "srrg_property/property_eigen.h" 3 | #include "base_sensor_message.h" 4 | 5 | namespace srrg2_core { 6 | 7 | class TwistStampedMessage: public BaseSensorMessage { 8 | public: 9 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 10 | TwistStampedMessage(const std::string& topic_ = "", 11 | const std::string& frame_id_ = "", 12 | const int& seq_ = -1, 13 | const double& timestamp_ = -1); 14 | PropertyEigen_ linear; 15 | PropertyEigen_ angular; 16 | }; 17 | using TwistStampedMessagePtr = std::shared_ptr; 18 | 19 | } /* namespace srrg2_slam_architecture */ 20 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_pcl/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(srrg2_point_cloud_library 2 | lidar_3d_utils/spacing.cpp 3 | point_descriptor_field.cpp 4 | instances.cpp instances.h 5 | ) 6 | 7 | target_link_libraries(srrg2_point_cloud_library 8 | srrg2_boss_library 9 | srrg2_property_library 10 | srrg2_config_library 11 | srrg2_system_utils_library 12 | opencv_core #ds can be removed when cv::Mat descriptors are removed 13 | ) 14 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_pcl/instances.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "normal_computator.h" 3 | #include "point_projector_lidar3d_types.h" 4 | #include "point_projector_types.h" 5 | #include "point_types_data.h" 6 | #include "point_unprojector_lidar3d_types.h" 7 | #include "point_unprojector_types.h" 8 | 9 | namespace srrg2_core { 10 | 11 | void point_cloud_registerTypes() __attribute__((constructor)); 12 | } 13 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_pcl/point_defs.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | namespace srrg2_core { 3 | 4 | enum TRANSFORM_CLASS { 5 | Isometry = 0x0, 6 | PinholeProjection = 0x1, 7 | PinholeUnprojection = 0x2, 8 | PolarProjection = 0x3, 9 | PolarUnprojection = 0x4 10 | }; 11 | 12 | enum POINT_STATUS { Valid, OutOfRange, BehindObserver, Invalid }; 13 | 14 | } // namespace srrg2_core 15 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_pcl/point_fields.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "point_default_field.h" 3 | #include "point_descriptor_field.h" 4 | #include "point_scalar_field.h" 5 | #include "point_statistics_field.h" 6 | #include "point_vector_field.h" 7 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_pcl/point_types.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "point.h" 4 | #include "point_id.h" 5 | #include "point_color.h" 6 | #include "point_intensity.h" 7 | #include "point_intensity_descriptor.h" 8 | #include "point_normal.h" 9 | #include "point_normal_color.h" 10 | #include "point_normal_curvature.h" 11 | #include "point_normal_curvature_color.h" 12 | #include "point_normal_intensity.h" 13 | #include "point_id.h" 14 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_property/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(srrg2_property_library SHARED 2 | 3 | property.h 4 | property.cpp 5 | 6 | property_eigen.h 7 | property_eigen.cpp 8 | 9 | property_identifiable.cpp 10 | property_identifiable.h 11 | 12 | property_container.h 13 | property_container.cpp 14 | 15 | property_container_manager.cpp 16 | property_container_manager.h 17 | 18 | ) 19 | target_link_libraries(srrg2_property_library 20 | srrg2_boss_library 21 | ${CMAKE_DL_LIBS}) 22 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_property/property_identifiable.cpp: -------------------------------------------------------------------------------- 1 | #include "property_identifiable.h" 2 | 3 | namespace srrg2_core { 4 | 5 | bool PropertyIdentifiablePtrVectorInterface::pushBack(Identifiable* id) { 6 | if (!canAssign(id)) 7 | return false; 8 | resize(size() + 1); 9 | return assign(size() - 1, id); 10 | } 11 | 12 | bool PropertyIdentifiablePtrVectorInterface::pushBack(std::shared_ptr id) { 13 | if (!canAssign(id)) { 14 | std::cerr << "PropertyIdentifiablePtrVectorInterface::pushBack|warning, cannot assign object " 15 | "with ID [" 16 | << id << "]\n"; 17 | return false; 18 | } 19 | resize(size() + 1); 20 | return assign(size() - 1, id); 21 | } 22 | 23 | } // namespace srrg2_core 24 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_system_utils/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(srrg2_system_utils_library SHARED 2 | system_utils.cpp 3 | parse_command_line.cpp 4 | profiler.cpp 5 | chrono.cpp 6 | env.cpp 7 | ) 8 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_system_utils/char_array.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | namespace srrg2_core { 5 | template 6 | void writeToCharArray(char*& dest, size_t& size, const T& v) { 7 | if (size 17 | void readFromCharArray(T& v, const char*& src, size_t& size) { 18 | if (size 3 | #include 4 | #include 5 | namespace srrg2_core { 6 | struct Chrono{ 7 | using ChronoMap=std::map > >; 8 | Chrono(const std::string& tag_, 9 | ChronoMap* time_map_, 10 | bool verbose_); 11 | 12 | Chrono(const std::string& tag_, 13 | double& accumulator_, 14 | bool verbose_); 15 | 16 | ~Chrono(); 17 | double duration() const; 18 | static void printReport(const ChronoMap& chrono_map); 19 | protected: 20 | bool _verbose; 21 | std::string _tag; 22 | double _time; 23 | double* _accumulator=nullptr; 24 | ChronoMap* _time_map=nullptr; 25 | }; 26 | 27 | } 28 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_system_utils/env.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | namespace srrg2_core { 6 | 7 | void initEnvMap(); 8 | 9 | // returns the environment variables 10 | const std::map& envMap(); 11 | 12 | void envMapAddCmdArgs(const std::vector& cmd_args); 13 | 14 | // replaces in a string the tags "" with "VAR_VALUE" 15 | // according to the environment map; 16 | void replaceEnvTags(std::string& str); 17 | 18 | } 19 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_system_utils/stream_helpers.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace srrg2_core { 4 | /*! 5 | Class that contains static methods for binary serialization of 6 | POD objects 7 | */ 8 | class StreamHelpers { 9 | public: 10 | //! writes on a binary stream the POD v 11 | template 12 | inline static void writeBinary(std::ostream& os, const T& v) { 13 | const char* c = (const char*) &v; 14 | os.write(c, sizeof(T)); 15 | } 16 | 17 | //! reads from a binary stream the POD v 18 | template 19 | inline static void readBinary(std::istream& is, T& v) { 20 | char* c = (char*) &v; 21 | is.read(c, sizeof(T)); 22 | } 23 | }; 24 | } // namespace srrg2_core 25 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_viewer/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_subdirectory(viewer_core) 2 | 3 | add_library(srrg2_viewer_library SHARED 4 | viewer_core_base.cpp viewer_core_base.h 5 | drawable_base.cpp drawable_base.h 6 | active_drawable.cpp active_drawable.h 7 | viewer_canvas.cpp viewer_canvas.h 8 | viewer_context_base.cpp viewer_context_base.h 9 | viewer_context_shared.cpp viewer_context_shared.h 10 | viewer_manager_base.cpp viewer_manager_base.h 11 | viewer_manager_shared.cpp viewer_manager_shared.h 12 | ) 13 | 14 | target_link_libraries(srrg2_viewer_library 15 | srrg2_viewer_core_library 16 | srrg2_system_utils_library 17 | ) 18 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_viewer/active_drawable.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "drawable_base.h" 3 | 4 | namespace srrg2_core { 5 | 6 | //! @brief base derivable class that represent an object that could be "drawn" 7 | //! using OpenGL and OpenCV 8 | class ActiveDrawable: public DrawableBase { 9 | public: 10 | void addCanvas(ViewerCanvasPtr canvas); 11 | void removeCanvas(ViewerCanvasPtr canvas); 12 | void draw(); 13 | protected: 14 | using ViewerCanvasWeakPtr = std::weak_ptr; 15 | std::list _canvases; 16 | std::list::iterator findCanvas(ViewerCanvasPtr canvas); 17 | }; 18 | 19 | using ActiveDrawablePtr = std::shared_ptr; 20 | 21 | } // namespace srrg2_core 22 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_viewer/drawable_base.cpp: -------------------------------------------------------------------------------- 1 | #include "drawable_base.h" 2 | 3 | namespace srrg2_core { 4 | DrawableBase::~DrawableBase() {} 5 | } 6 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_viewer/viewer_core/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | #ia base protocol 2 | #add_subdirectory(packets) 3 | add_library(srrg2_viewer_core_library SHARED 4 | packets.h 5 | color_palette.h 6 | semaphore.h 7 | semaphore.cpp 8 | buffer_memory.h 9 | buffer_memory.cpp 10 | buffer_deque.h 11 | buffer_manager.h 12 | buffer_manager.cpp 13 | packets.h 14 | packet_factory.h 15 | packet_serializer.h 16 | packet_serializer.cpp 17 | packet_deserializer.h 18 | packet_deserializer.cpp 19 | packet_processor.h 20 | buffer_sink.h 21 | buffer_source.h 22 | viewport.h 23 | viewport.cpp 24 | # shared things 25 | buffer_sink_shared.h 26 | buffer_sink_shared.cpp 27 | buffer_source_shared.h 28 | buffer_source_shared.cpp 29 | ) 30 | 31 | target_link_libraries(srrg2_viewer_core_library 32 | srrg2_property_library 33 | srrg2_point_cloud_library 34 | srrg2_matchable_library 35 | pthread) 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_viewer/viewer_core/buffer_sink_shared.cpp: -------------------------------------------------------------------------------- 1 | #include "buffer_sink_shared.h" 2 | #include "packet_deserializer.h" 3 | #include "packet_serializer.h" 4 | 5 | namespace srrg2_core { 6 | 7 | BufferSinkShared::~BufferSinkShared() {} 8 | 9 | void BufferSinkShared::putBuffer(BufferMemory* buffer_) { 10 | //ia in this case the buffer is shared between the two entities 11 | // if (not source_ptr) 12 | // sgancell buffer 13 | // else 14 | // mand buffer 15 | 16 | if (!param_manager_ptr.value()) 17 | throw std::runtime_error("[BufferSinkShared::putBuffer] please set a valid BufferManager"); 18 | 19 | if (!param_source_ptr.value()) { 20 | param_manager_ptr->freeBuffer(buffer_); 21 | return; 22 | } 23 | param_source_ptr->pushBuffer(buffer_); 24 | } 25 | } //ia end namespace 26 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_viewer/viewer_core/packet_deserializer.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "buffer_memory.h" 3 | #include "packet_factory.h" 4 | 5 | namespace srrg2_core { 6 | 7 | //! @brief serializes packets on an external buffer. it owns nothing. 8 | //! you specify a packet and a type and it goes. 9 | class PacketDeserializer { 10 | public: 11 | PacketDeserializer(); 12 | ~PacketDeserializer(); 13 | 14 | inline void setBuffer(BufferMemory* buffer_) { 15 | if (!buffer_->size) 16 | throw std::runtime_error("PacketDeserializer::setBuffer|setting an empty buffer"); 17 | _buffer = buffer_; 18 | } 19 | 20 | PacketBase* getPacket(); 21 | 22 | protected: 23 | BufferMemory* _buffer = 0; 24 | PacketFactory* _factory = 0; 25 | 26 | public: 27 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 28 | }; 29 | } //ia end namespace 30 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_viewer/viewer_core/packet_serializer.cpp: -------------------------------------------------------------------------------- 1 | #include "packet_serializer.h" 2 | #include "packets.h" 3 | 4 | namespace srrg2_core { 5 | PacketSerializer::PacketSerializer() { 6 | } 7 | 8 | PacketSerializer::~PacketSerializer() { 9 | } 10 | 11 | void PacketSerializer::putPacket(const PacketBase& packet_) { 12 | if (!_buffer || !_buffer->size) 13 | throw std::runtime_error("[PacketSerializer] invalid buffer, please set the buffer"); 14 | 15 | if (packet_.type == PACKET_TYPE_INVALID) 16 | throw std::runtime_error("[PacketSerializer] invalid packet type"); 17 | 18 | // ia we put first the packet type 19 | _buffer->data = putInBuffer(_buffer->data, packet_.type); 20 | 21 | // ia finally the payload 22 | _buffer->data = packet_.serialize(_buffer->data); 23 | 24 | // ia increment the packet counter 25 | ++_buffer->num_packets; 26 | } 27 | 28 | } // namespace srrg2_core 29 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_viewer/viewer_core/packet_serializer.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "buffer_memory.h" 3 | #include "packets/packet_base.h" 4 | 5 | namespace srrg2_core { 6 | 7 | //! @brief serializes packets on an external buffer. it owns nothing. 8 | //! you specify a packet and a type and it goes. 9 | class PacketSerializer { 10 | public: 11 | PacketSerializer(); 12 | ~PacketSerializer(); 13 | 14 | inline void setBuffer(BufferMemory* buffer_) { 15 | if (!buffer_->size) 16 | throw std::runtime_error("[PacketSerializer] setting an empty buffer"); 17 | _buffer = buffer_; 18 | } 19 | 20 | void putPacket(const PacketBase& packet_); 21 | 22 | protected: 23 | BufferMemory* _buffer = 0; 24 | 25 | public: 26 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 27 | 28 | }; 29 | } //ia end namespace 30 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_viewer/viewer_core/packets/packet_gl_list.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "packet_base.h" 3 | #include "packet_string.h" 4 | 5 | namespace srrg2_core { 6 | 7 | 8 | static constexpr uint8_t PACKET_TYPE_CREATE_LIST = 0xB0; 9 | static constexpr uint8_t PACKET_TYPE_BEGIN_LIST = 0xB1; 10 | static constexpr uint8_t PACKET_TYPE_END_LIST = 0xB2; 11 | static constexpr uint8_t PACKET_TYPE_DESTROY_LIST = 0xB3; 12 | static constexpr uint8_t PACKET_TYPE_CALL_LIST = 0xB4; 13 | 14 | using PacketCreateList = PacketString_; 15 | using PacketBeginList = PacketString_; 16 | using PacketEndList = PacketCommand_; 17 | using PacketCallList = PacketString_; 18 | using PacketDestroyList = PacketString_; 19 | 20 | } 21 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_viewer/viewer_core/semaphore.cpp: -------------------------------------------------------------------------------- 1 | #include "semaphore.h" 2 | 3 | namespace srrg2_core { 4 | 5 | Semaphore::Semaphore(const uint64_t& value_) { 6 | _count = value_; 7 | } 8 | 9 | Semaphore::~Semaphore() {} 10 | 11 | } //ia end namespace 12 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_viewer/viewer_core/viewport.cpp: -------------------------------------------------------------------------------- 1 | #include "viewport.h" 2 | 3 | namespace srrg2_core { 4 | 5 | ViewportBase::~ViewportBase() {} 6 | 7 | } //ia end namespace 8 | 9 | 10 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/src/srrg_viewer/viewer_manager_base.cpp: -------------------------------------------------------------------------------- 1 | #include "viewer_manager_base.h" 2 | 3 | namespace srrg2_core { 4 | 5 | ViewerManagerBase::~ViewerManagerBase() { 6 | //ia delete all the contexts 7 | ViewerContextContainer::iterator c_it = _contexts.begin(); 8 | ViewerContextContainer::iterator c_end = _contexts.end(); 9 | 10 | while(c_it != c_end) { 11 | delete c_it->second; 12 | ++c_it; 13 | } 14 | 15 | _contexts.clear(); 16 | } 17 | 18 | } //ia end namespace srrg2_core 19 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core/tests/test_profiler.cpp: -------------------------------------------------------------------------------- 1 | #include "srrg_system_utils/profiler.h" 2 | #include "srrg_test/test_helper.hpp" 3 | 4 | using namespace srrg2_core; 5 | 6 | // ds the object we want to profile 7 | class Foo : public Profiler { 8 | public: 9 | Foo() { 10 | PROFILE_TIME("Foo::Foo"); 11 | usleep(10000); 12 | } 13 | ~Foo() { 14 | PROFILE_TIME("Foo::~Foo"); 15 | usleep(20000); 16 | } 17 | void bar() { 18 | PROFILE_TIME("Foo::bar"); 19 | usleep(30000); 20 | } 21 | }; 22 | 23 | int main(int argc_, char** argv_) { 24 | Profiler::enable_logging = true; 25 | return srrg2_test::runTests(argc_, argv_); 26 | } 27 | 28 | // ds TODO enable actual unittests 29 | TEST(Profiler, ObjectLife) { 30 | Foo foo; 31 | } 32 | 33 | TEST(Profiler, ObjectFunctionSingle) { 34 | Foo foo; 35 | foo.bar(); 36 | } 37 | 38 | TEST(Profiler, ObjectFunctionMultiple) { 39 | Foo foo; 40 | for (size_t i = 0; i < 10; ++i) { 41 | foo.bar(); 42 | } 43 | } 44 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core_ros/msg/PlannerStatusMessage.msg: -------------------------------------------------------------------------------- 1 | #gg mesage for the planner status, shot continuosly by the planner 2 | 3 | Header header 4 | string status #status of the planner ("idle", "moving", "invalid_goal", "unreachable") 5 | int32 a_star_status #result of the enum for the local path search 6 | int32 path_type #type of path 0: gradient, 1: grid 7 | float32 cost_to_global_goal #value of the cost function at the current location 8 | float32 distance_to_global_goal #distance to the current location [meters] 9 | float32 distance_to_local_goal #distance to the current location [meters] 10 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core_ros/msg/ViewerBufferMessage.msg: -------------------------------------------------------------------------------- 1 | #ia generic viewer buffer message. it contains all the fields of a MemoryBuffer 2 | #ia in this way we use ros to send serialized packets containing objects and commands 3 | #ia those can be either: 4 | #ia - received, deserialized and passed through a qglVieweport 5 | #ia - received, deserialized and converted into RViz compliant stuff 6 | 7 | Header header 8 | uint64 size #ia size in bytes 9 | uint64 num_packets #ia number of packets serialized in this buffer 10 | uint8 status #ia status of the buffer (Free=0, Read=1, Write=2, Ready=3) 11 | uint8[] data #ia actual buffer of stuff 12 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core_ros/src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_subdirectory(app) 2 | add_subdirectory(node) 3 | add_subdirectory(srrg_converters) 4 | add_subdirectory(srrg_messages_ros) 5 | add_subdirectory(srrg_viewer_ros) 6 | add_subdirectory(tests) 7 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core_ros/src/app/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_executable(rosbag_to_srrg_converter_app rosbag_to_srrg_converter_app.cpp) 2 | target_link_libraries(rosbag_to_srrg_converter_app 3 | srrg2_messages_ros_library 4 | ${catkin_LIBRARIES}) 5 | 6 | add_executable(srrg_to_rosbag_converter_app srrg_to_rosbag_converter_app.cpp) 7 | target_link_libraries(srrg_to_rosbag_converter_app 8 | srrg2_system_utils_library 9 | srrg2_messages_ros_library 10 | ${catkin_LIBRARIES} 11 | ) 12 | 13 | add_executable(extract_gt_from_ros extract_gt_from_ros.cpp) 14 | target_link_libraries(extract_gt_from_ros 15 | srrg2_messages_ros_library 16 | ${catkin_LIBRARIES}) 17 | 18 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core_ros/src/node/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_executable(srrg_to_ros_publisher_node srrg_to_ros_publisher_node.cpp) 2 | target_link_libraries(srrg_to_ros_publisher_node 3 | srrg2_system_utils_library 4 | srrg2_messages_ros_library 5 | ${catkin_LIBRARIES} 6 | ) 7 | 8 | add_executable(ros_to_srrg_listener_node ros_to_srrg_listener_node.cpp) 9 | target_link_libraries(ros_to_srrg_listener_node 10 | srrg2_system_utils_library 11 | srrg2_messages_ros_library 12 | ${catkin_LIBRARIES} 13 | ) 14 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core_ros/src/srrg_converters/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(srrg2_converters_library SHARED 2 | converter.cpp 3 | ) 4 | 5 | target_link_libraries(srrg2_converters_library 6 | srrg2_messages_library 7 | srrg2_data_structures_library 8 | ${catkin_LIBRARIES} 9 | ) 10 | 11 | add_dependencies(srrg2_converters_library ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 12 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core_ros/src/srrg_messages_ros/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(srrg2_messages_ros_library SHARED 2 | message_handlers/message_ros_source.cpp message_handlers/message_ros_source.cpp 3 | message_handlers/message_rosbag_source.cpp message_handlers/message_rosbag_source.h 4 | message_handlers/message_rosbag_sink.cpp message_handlers/message_rosbag_sink.h 5 | message_handlers/message_ros_sink.cpp message_handlers/message_ros_sink.h 6 | 7 | instances.cpp instances.h 8 | 9 | # message_handlers/message_ros_sink_base.cpp message_handlers/message_ros_sink_base.h 10 | ) 11 | 12 | target_link_libraries(srrg2_messages_ros_library 13 | srrg2_converters_library 14 | ) 15 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core_ros/src/srrg_messages_ros/instances.cpp: -------------------------------------------------------------------------------- 1 | #include "instances.h" 2 | namespace srrg2_core_ros { 3 | 4 | void messages_ros_registerTypes() { 5 | srrg2_core::messages_registerTypes(); 6 | BOSS_REGISTER_CLASS(MessageROSSink); 7 | BOSS_REGISTER_CLASS(MessageROSSource); 8 | BOSS_REGISTER_CLASS(MessageROSBagSink); 9 | BOSS_REGISTER_CLASS(MessageROSBagSource); 10 | } 11 | } 12 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core_ros/src/srrg_messages_ros/instances.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "message_handlers/message_ros_sink.h" 3 | #include "message_handlers/message_ros_source.h" 4 | #include "message_handlers/message_rosbag_sink.h" 5 | #include "message_handlers/message_rosbag_source.h" 6 | #include "srrg_messages/instances.h" 7 | 8 | namespace srrg2_core_ros { 9 | void messages_ros_registerTypes() __attribute__((constructor)); 10 | } 11 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core_ros/src/srrg_messages_ros/message_handlers/message_rosbag_sink.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | 6 | namespace srrg2_core_ros { 7 | 8 | struct MessageROSBagSink: public srrg2_core::MessageFileSinkBase { 9 | 10 | MessageROSBagSink(); 11 | 12 | void open() override; 13 | void open(const std::string& filename_) override; 14 | void close() override; 15 | bool putMessage(srrg2_core::BaseSensorMessagePtr msg_) override; 16 | protected: 17 | rosbag::Bag _bag; 18 | }; 19 | 20 | } 21 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core_ros/src/srrg_viewer_ros/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | #ia internal stuff 2 | add_subdirectory(viewer_core_ros) 3 | 4 | #ia exposed viewer manager library 5 | add_library(srrg2_viewer_ros_library SHARED 6 | viewer_context_ros.h 7 | viewer_context_ros.cpp 8 | viewer_manager_ros.h 9 | viewer_manager_ros.cpp 10 | ) 11 | target_link_libraries(srrg2_viewer_ros_library 12 | srrg2_viewer_core_ros_library) 13 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core_ros/src/srrg_viewer_ros/viewer_core_ros/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(srrg2_viewer_core_ros_library SHARED 2 | buffer_publisher.h 3 | buffer_publisher.cpp 4 | buffer_subscriber.h 5 | buffer_subscriber.cpp 6 | buffer_sink_ros.h 7 | buffer_sink_ros.cpp 8 | buffer_source_ros.h 9 | buffer_source_ros.cpp) 10 | target_link_libraries(srrg2_viewer_core_ros_library 11 | ${catkin_LIBRARIES}) ##ia TODO polish this 12 | 13 | #ia this tells catkin to build first the messege, then this shit 14 | #add_dependencies(srrg2_viewer_core_ros_library ${PROJECT_NAME}_generate_messages_cpp) 15 | add_dependencies(srrg2_viewer_core_ros_library ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp) 16 | 17 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core_ros/src/srrg_viewer_ros/viewer_core_ros/buffer_sink_ros.cpp: -------------------------------------------------------------------------------- 1 | #include "buffer_sink_ros.h" 2 | 3 | using namespace srrg2_core; 4 | 5 | namespace srrg2_core_ros { 6 | 7 | BufferSinkRos::~BufferSinkRos() {} 8 | 9 | void BufferSinkRos::putBuffer(BufferMemory* buffer_) { 10 | if (!param_manager_ptr.value()) 11 | throw std::runtime_error("BufferSinkRos::putBuffer|please set a valid BufferManager"); 12 | 13 | if (param_publisher_ptr.value()) { 14 | //ia if there is a publisher connected, do something 15 | _is_active = param_publisher_ptr->publishBuffer(buffer_); 16 | } else { 17 | //ia else free buffer 18 | param_manager_ptr->freeBuffer(buffer_); 19 | } 20 | } 21 | 22 | } //ia end namespace srrg2_core_ros 23 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_core/srrg2_core_ros/src/srrg_viewer_ros/viewer_core_ros/buffer_source_ros.cpp: -------------------------------------------------------------------------------- 1 | #include "buffer_source_ros.h" 2 | 3 | namespace srrg2_core_ros { 4 | 5 | using namespace srrg2_core; 6 | 7 | 8 | BufferSourceRos::~BufferSourceRos() {} 9 | 10 | 11 | BufferMemory* BufferSourceRos::getBuffer() { 12 | if (!param_manager_ptr.value()) 13 | throw std::runtime_error("BufferSourceRos::getBuffer|please set a valid BufferManager"); 14 | 15 | BufferMemory* b = param_manager_ptr->getBuffer(true); //ia get a buffer for reading purposes 16 | return b; 17 | } 18 | 19 | 20 | void BufferSourceRos::releaseBuffer(srrg2_core::BufferMemory* buffer_) { 21 | if (!param_manager_ptr.value()) 22 | throw std::runtime_error("BufferSourceRos::getBuffer|please set a valid BufferManager"); 23 | 24 | param_manager_ptr->releaseBuffer(buffer_); //ia release a buffer 25 | } 26 | 27 | } //ia end namespace srrg2_core_ros 28 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_executor/results/tum/ate.txt: -------------------------------------------------------------------------------- 1 | 0.001 2 | 0.001 3 | 0.001 4 | 0.001 5 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_executor/srrg2_executor/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | srrg2_executor 4 | 0.9.0 5 | The srrg2_executor package 6 | 7 | Giorgio Grisetti 8 | BSD-4 9 | 10 | 11 | catkin 12 | srrg_cmake_modules 13 | srrg2_core 14 | srrg2_core_ros 15 | srrg2_qgl_viewport 16 | srrg2_qgl_viewport_ros 17 | 18 | 19 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_executor/srrg2_executor_nogui/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | srrg2_executor_nogui 4 | 0.9.0 5 | The srrg2_executor package (gui disabled version) 6 | 7 | Giorgio Grisetti 8 | BSD-4 9 | 10 | 11 | catkin 12 | srrg_cmake_modules 13 | srrg2_core 14 | srrg2_core_ros 15 | 16 | 17 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_qgl_viewport/.gitignore: -------------------------------------------------------------------------------- 1 | .cproject 2 | .project 3 | .settings/* 4 | 5 | */.cproject 6 | */.project 7 | */.settings/* 8 | 9 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_qgl_viewport/srrg2_qgl_viewport/README.md: -------------------------------------------------------------------------------- 1 | # SRRG_QGL_VIEWPORT 2 | QGL implementation of the core viewport -------------------------------------------------------------------------------- /modules/srrg/srrg2_qgl_viewport/srrg2_qgl_viewport/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | srrg2_qgl_viewport 4 | 1.0.0 5 | Viewport for our viewer based on opengl 6 | 7 | Giorgio Grisetti 8 | Irvin Aloise 9 | BSD-4 10 | 11 | catkin 12 | 13 | srrg_cmake_modules 14 | srrg2_core 15 | 16 | 17 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_qgl_viewport/srrg2_qgl_viewport/src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_subdirectory(srrg_qgl_viewport) 2 | add_subdirectory(tests) 3 | 4 | 5 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_qgl_viewport/srrg2_qgl_viewport/src/srrg_qgl_viewport/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(srrg2_qgl_viewport_library SHARED 2 | gl_helpers.cpp 3 | qgl_packet_processor.cpp 4 | qgl_viewport.cpp 5 | viewer_core_base_qgl.cpp 6 | viewer_core_shared_qgl.cpp 7 | ) 8 | 9 | target_link_libraries(srrg2_qgl_viewport_library 10 | srrg2_viewer_library 11 | ${SRRG_QT_LIBRARIES} 12 | ${GLUT_LIBRARIES} 13 | ${QGLViewer_LIBRARIES} 14 | ${OpenCV_LIBRARIES} 15 | ) 16 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_qgl_viewport/srrg2_qgl_viewport_ros/README.md: -------------------------------------------------------------------------------- 1 | # SRRG_QGL_VIEWPORT 2 | QGL implementation of the core viewport -------------------------------------------------------------------------------- /modules/srrg/srrg2_qgl_viewport/srrg2_qgl_viewport_ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | srrg2_qgl_viewport_ros 4 | 1.0.0 5 | Viewport for our viewer based on opengl 6 | 7 | Giorgio Grisetti 8 | Irvin Aloise 9 | BSD-4 10 | 11 | catkin 12 | 13 | srrg_cmake_modules 14 | srrg2_core 15 | srrg2_core_ros 16 | srrg2_qgl_viewport 17 | 18 | 19 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_qgl_viewport/srrg2_qgl_viewport_ros/src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_subdirectory(srrg_qgl_viewport_ros) 2 | add_subdirectory(tests) 3 | add_subdirectory(app) 4 | 5 | 6 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_qgl_viewport/srrg2_qgl_viewport_ros/src/app/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_executable(srrg_viewer_ros_client srrg_viewer_ros_client.cpp) 2 | target_link_libraries(srrg_viewer_ros_client 3 | srrg2_viewer_ros_library 4 | srrg2_qgl_viewport_library 5 | srrg2_qgl_viewport_ros_library 6 | ${catkin_LIBRARIES} 7 | pthread 8 | ) 9 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_qgl_viewport/srrg2_qgl_viewport_ros/src/srrg_qgl_viewport_ros/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(srrg2_qgl_viewport_ros_library SHARED 2 | viewer_core_ros_qgl.cpp viewer_core_ros_qgl.h 3 | ) 4 | target_link_libraries(srrg2_qgl_viewport_ros_library 5 | srrg2_viewer_ros_library 6 | srrg2_qgl_viewport_library 7 | ${SRRG_QT_LIBRARIES} 8 | ${GLUT_LIBRARIES} 9 | ${QGLViewer_LIBRARIES}) 10 | 11 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/.gitignore: -------------------------------------------------------------------------------- 1 | *~ 2 | .o 3 | \#* 4 | *.cproject 5 | *.project 6 | */.settings/* 7 | ./settings/* 8 | tags 9 | html/ 10 | latex/ 11 | .vscode/* 12 | */.vscode/* 13 | sphere5000.boss 14 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/docker/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM giorgiogrisetti/ubuntu_20.04_cuda_ros_noetic_srrg2_core:latest 2 | ENV DEBIAN_FRONTEND=noninteractive 3 | ENV APT_KEY_DONT_WARN_ON_DANGEROUS_USAGE=1 4 | 5 | MAINTAINER Giorgio Grisetti 6 | 7 | RUN cd /workspace/src && git clone https://gitlab.com/srrg-software/srrg2_solver && cd srrg2_solver && git checkout devel 8 | 9 | # build everything 10 | RUN . /opt/ros/noetic/setup.sh && cd /workspace/ && catkin init && catkin build 11 | 12 | 13 | #WORKDIR / 14 | CMD ["bash"] 15 | 16 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/docker/build.sh: -------------------------------------------------------------------------------- 1 | docker build -t giorgiogrisetti/ubuntu_20.04_cuda_ros_noetic_srrg2_solver . 2 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/docker/dl.conf: -------------------------------------------------------------------------------- 1 | "DynamicLoaderConfig" { 2 | "so_names" : [ 3 | "libsrrg2_point_cloud_library.so", 4 | "libsrrg2_viewer_core_ros_library.so", 5 | "libsrrg2_solver_core_library.so", 6 | "libsrrg2_solver_types_common_library.so", 7 | "libsrrg2_solver_types2d_library.so", 8 | "libsrrg2_solver_linear_solvers_library.so", 9 | "libsrrg2_solver_sparse_block_matrix_library.so", 10 | "libsrrg2_solver_extras_types2d_library.so", 11 | "libsrrg2_solver_factor_graph_utils_library.so", 12 | "libsrrg2_solver_calib_utils_library.so", 13 | "libsrrg2_solver_calib_library.so", 14 | "libsrrg2_solver_types3d_library.so", 15 | "libsrrg2_solver_linear_solvers_library.so", 16 | "libsrrg2_solver_projective_library.so", 17 | "libsrrg2_solver_extras_types3d_library.so", 18 | "libsrrg2_solver_star_library.so" 19 | ], 20 | "so_paths" : [ 21 | "/workspace/devel/lib" 22 | ] 23 | } 24 | 25 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/readme.md: -------------------------------------------------------------------------------- 1 | ## SRRG2_SOLVER 2 | [![pipeline status](https://gitlab.com/srrg-software/srrg2_solver/badges/master/pipeline.svg)](https://gitlab.com/srrg-software/srrg2_solver/commits/master) 3 | 4 | Basic solver specifically designed for SLAM pipelines. Highly efficient, no fancy stuff, but man if it runs! 5 | 6 | ##### What's inside the directories? 7 | - `srrg2_solver`: core package for the solver. use this for your SLAM pipeline, minimal dependecies. 8 | - `srrg2_solver_gui`: requires openGL, to see what's going on under the hood [work in progress]. 9 | 10 | ##### License 11 | BSD-4 12 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/app/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_subdirectory(converter) 2 | add_subdirectory(graph_manipulators) 3 | add_subdirectory(optimizer) 4 | add_subdirectory(plgo_simulator) 5 | add_subdirectory(plgo_simulator2) 6 | add_subdirectory(sfm) 7 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/app/converter/g2o_converter_actions.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "g2o_converter_action_base.h" 3 | #include "g2o_converter_action_se2.h" 4 | #include "g2o_converter_action_se3.h" 5 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/app/optimizer/solver_app_incremental_optimizer.cpp: -------------------------------------------------------------------------------- 1 | #include "srrg_solver/solver_incremental/solver_incremental_running_environment.h" 2 | 3 | using namespace srrg2_core; 4 | using namespace srrg2_solver; 5 | using namespace std; 6 | 7 | std::string exe_name = environ[0]; 8 | 9 | #define LOG std::cerr << exe_name << "|" 10 | 11 | static const char* banner[] = { 12 | "optimizes a graph incrementally", 13 | 0 14 | }; 15 | 16 | 17 | // ia THE PROGRAM 18 | int main(int argc, char** argv) { 19 | exe_name=argv[0]; 20 | ParseCommandLine cmd_line(argv, banner); 21 | SolverIncrementalRunningEnvironment environment(cmd_line); 22 | cmd_line.parse(); 23 | std::set relaxed; 24 | relaxed.insert(0); // unlock the first node, and let th star solver do its job 25 | environment.setup(relaxed); 26 | environment.run(); 27 | return 0; 28 | } 29 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/app/plgo_simulator/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_executable(solver_app_plgo_simulator 2 | solver_app_plgo_simulator.cpp 3 | graph_generator.cpp 4 | plgo_simulator.cpp 5 | trajectory_generator.cpp 6 | sim_record.cpp) 7 | 8 | target_link_libraries(solver_app_plgo_simulator 9 | srrg2_solver_types2d_library 10 | srrg2_solver_types3d_library 11 | srrg2_solver_calib_library 12 | srrg2_solver_projective_library 13 | ${SOLVER_EXAMPLES_CORE_LIBRARIES} 14 | ${catkin_LIBRARIES} 15 | ) 16 | 17 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/app/plgo_simulator/sim_record.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include "srrg_solver/solver_core/instances.h" 5 | #include "srrg_solver/solver_core/factor_graph.h" 6 | 7 | namespace srrg2_solver { 8 | using namespace srrg2_core; 9 | 10 | struct SimRecord: public Serializable { 11 | int epoch; 12 | std::vector vars; 13 | std::vector factors; 14 | void serialize(ObjectData& odata, IdContext& context) override; 15 | void deserialize(ObjectData& odata, IdContext& context) override; 16 | }; 17 | 18 | using SimRecordPtr = std::shared_ptr; 19 | using SimRecordList = std::list; 20 | 21 | } 22 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/app/plgo_simulator2/instances.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | namespace srrg2_solver { 3 | void srrg2_solver_plgo_simulator_register_types() __attribute__((constructor)); 4 | } 5 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/app/plgo_simulator2/sim_sensor_3d_points_fov_limited.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "sim_sensor_3d_points_base.h" 3 | #include "srrg_solver/variables_and_factors/types_3d/se3_pose_point_offset_error_factor.h" 4 | 5 | namespace srrg2_solver { 6 | using namespace srrg2_core; 7 | 8 | 9 | struct SimSensor3DPointsFOVLimited: public SimSensor3DPointsFOVLimitedBase { 10 | using FactorType=SE3PosePointOffsetErrorFactor; 11 | bool computeMeasurementInSensor(Eigen::Vector3f& measurement, 12 | const Eigen::Vector3f& world_point); 13 | 14 | void computeEpoch(FactorGraph& graph, VariableBase::Id current_pose_id) override; 15 | }; 16 | 17 | } 18 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/app/plgo_simulator2/sim_sensor_base.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "sim_sensor_mode_base.h" 3 | #include 4 | #include 5 | #include "srrg_solver/solver_core/factor_graph.h" 6 | 7 | namespace srrg2_solver { 8 | using namespace srrg2_core; 9 | struct SimRobot; 10 | 11 | struct SimSensorBase: public Configurable { 12 | 13 | virtual void setRobotPose(const Eigen::Isometry3f& robot_pose) { 14 | _robot_pose=robot_pose; 15 | _inv_robot_pose=robot_pose.inverse(); 16 | } 17 | virtual VariableBase::Id setupConstants(FactorGraph& graph, VariableBase::Id start_idx) {return start_idx;} 18 | virtual int populateWorld() {return 0;} 19 | virtual void computeEpoch(FactorGraph& graph, VariableBase::Id current_pose_id){} 20 | protected: 21 | Eigen::Isometry3f _robot_pose=Eigen::Isometry3f::Identity(); 22 | Eigen::Isometry3f _inv_robot_pose=Eigen::Isometry3f::Identity(); 23 | }; 24 | 25 | using SimSensorBasePtr=std::shared_ptr; 26 | } 27 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/app/plgo_simulator2/sim_sensor_mode_3d_poses.cpp: -------------------------------------------------------------------------------- 1 | #include "sim_sensor_mode_3d_poses.h" 2 | 3 | namespace srrg2_solver { 4 | using namespace srrg2_core; 5 | void SimSensorMode3DPoses::addPose(const Isometry3f& iso) { 6 | poses.push_back(iso); 7 | origins.push_back(iso.translation()); 8 | } 9 | 10 | void SimSensorMode3DPoses::setup(){ 11 | _search_tree.reset(new KDTree3D(origins, 1)); 12 | } 13 | 14 | void SimSensorMode3DPoses::findMatches(IntVector& answer_indices, 15 | const Isometry3f& query, 16 | const float range) { 17 | Vector3fVector answer_points; 18 | answer_indices.clear(); 19 | _search_tree->findNeighbors(answer_points, 20 | answer_indices, 21 | query.translation(), 22 | range, 23 | KDTreeSearchType::Complete); 24 | 25 | } 26 | 27 | } 28 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/app/plgo_simulator2/sim_sensor_mode_3d_poses.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "sim_sensor_mode_base.h" 3 | 4 | namespace srrg2_solver { 5 | using namespace srrg2_core; 6 | 7 | 8 | 9 | struct SimSensorMode3DPoses: public SimSensorModeBase { 10 | PARAM(PropertyFloat, 11 | min_landmark_distance, 12 | "minimum distance at which two landmarks can be [m]", 13 | 1.f, 14 | 0); 15 | void addPose(const Isometry3f& iso); 16 | void setup() override; 17 | void findMatches(IntVector& matches, 18 | const Isometry3f& query, 19 | const float range); 20 | int size() const override{return poses.size();} 21 | Isometry3fVector poses; 22 | Vector3fVector origins; 23 | std::unique_ptr _search_tree; 24 | }; 25 | 26 | using SimSensorMode3DPosesPtr=std::shared_ptr; 27 | 28 | } 29 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/app/plgo_simulator2/sim_sensor_mode_base.cpp: -------------------------------------------------------------------------------- 1 | #include "sim_sensor_mode_base.h" 2 | 3 | namespace srrg2_solver { 4 | using namespace srrg2_core; 5 | SimSensorModeBase::~SimSensorModeBase(){} 6 | 7 | } 8 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/app/plgo_simulator2/sim_sensor_omnicam.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "sim_sensor_3d_points_fov_limited.h" 3 | #include "srrg_solver/variables_and_factors/types_projective/se3_pose_point_omni_ba_error_factor.h" 4 | 5 | namespace srrg2_solver { 6 | using namespace srrg2_core; 7 | 8 | struct SimSensorOmnicam: public SimSensor3DPointsFOVLimitedBase { 9 | using FactorType=SE3PosePointOmniBAErrorFactor; 10 | bool computeMeasurementInSensor(Eigen::Vector3f& measurement, 11 | const Eigen::Vector3f& world_point); 12 | void computeEpoch(FactorGraph& graph, VariableBase::Id current_pose_id) override; 13 | }; 14 | } 15 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/app/plgo_simulator2/sim_sensors.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "sim_sensor_3d_points_fov_limited.h" 3 | #include "sim_sensor_3d_poses.h" 4 | #include "sim_sensor_omnicam.h" 5 | #include "sim_sensor_rectified_stereo.h" 6 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/app/sfm/ovt.conf: -------------------------------------------------------------------------------- 1 | SE2PosePointErrorFactor 1 0 1 2 | SE2PosePoseGeodesicErrorFactor 1 0 0 1 0 1 3 | SE3PosePointOffsetErrorFactor 1 0 0 1 0 1 4 | SE3PosePointOmniBAErrorFactor 1e4 0 0 1e4 0 1e4 5 | SE3PosePointPinholeBAErrorFactor 1 0 1 6 | SE3PosePointRectifiedStereoErrorFactor 1 0 0 1 0 1 7 | SE3PosePoseGeodesicErrorFactor 1e3 0 0 0 0 0 1e3 0 0 0 0 1e3 0 0 0 1e3 0 0 1e3 0 1e3 8 | Sim3PosePoseErrorFactorAD 1 0 0 0 0 0 0 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1 9 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/examples/data/KittiImuBiasedMetadata.txt: -------------------------------------------------------------------------------- 1 | BodyPtx BodyPty BodyPtz BodyPrx BodyPry BodyPrz AccelerometerSigma GyroscopeSigma IntegrationSigma AccelerometerBiasSigma GyroscopeBiasSigma AverageDeltaT 2 | 0 0 0 0 0 0 0.01 0.000175 0 0.000167 2.91e-006 0.0100395199348279 -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | srrg2_solver 4 | 0.8.0 5 | The srrg2_solver package 6 | 7 | Giorgio Grisetti 8 | Tiziano Guadagnino 9 | BSD-4 10 | 11 | catkin 12 | 13 | srrg2_core 14 | srrg_cmake_modules 15 | 16 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/slides/2018_srrg_solver.odp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rvp-group/mad-ba/7fc025c9deb59c5a971ff313d91ae13053a21635/modules/srrg/srrg2_solver/srrg2_solver/slides/2018_srrg_solver.odp -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_subdirectory(srrg_solver) 2 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_subdirectory(solver_core) 2 | add_subdirectory(variables_and_factors) 3 | add_subdirectory(utils) 4 | add_subdirectory(solver_incremental) 5 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/solver_core/instance_macros.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #define BOSS_REGISTER_AND_INSTANTIATE(class_name) \ 3 | namespace explicit_instances { \ 4 | class_name class_name##_instance; \ 5 | } \ 6 | void class_name##_registerClass() { \ 7 | BOSS_REGISTER_CLASS(class_name); \ 8 | } \ 9 | 10 | #define INSTANTIATE(class_name) \ 11 | namespace explicit_instances { \ 12 | class_name class_name##_instance; \ 13 | } \ 14 | 15 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/solver_core/instances.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | namespace srrg2_solver { 4 | void solver_registerTypes() __attribute__((constructor)) ; 5 | } 6 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/solver_core/internals/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_subdirectory(sparse_block_matrix) 2 | add_subdirectory(linear_solvers) 3 | 4 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/solver_core/internals/linear_solvers/instances.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | namespace srrg2_solver { 4 | void linear_solver_registerTypes() __attribute__((constructor)); 5 | } 6 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/solver_core/internals/linear_solvers/sparse_block_linear_solver_cholesky_cholmod.h: -------------------------------------------------------------------------------- 1 | #include "sparse_block_linear_solver_cholesky.h" 2 | 3 | namespace srrg2_solver { 4 | 5 | class SparseBlockLinearSolverCholeskyCholmod : public SparseBlockLinearSolverCholesky { 6 | public: 7 | virtual void computeOrderingHint(std::vector& ordering, 8 | const std::vector& block_layout) const; 9 | }; 10 | } // namespace srrg2_solver 11 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/solver_core/internals/linear_solvers/sparse_block_linear_solver_cholesky_csparse.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "sparse_block_linear_solver_cholesky.h" 3 | 4 | namespace srrg2_solver { 5 | 6 | class SparseBlockLinearSolverCholeskyCSparse : public SparseBlockLinearSolverCholesky { 7 | public: 8 | virtual void computeOrderingHint(std::vector& ordering, 9 | const std::vector& block_layout) const; 10 | }; 11 | using SparseBlockLinearSolverCholeskyCSparsePtr = 12 | std::shared_ptr; 13 | 14 | } // namespace srrg2_solver 15 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/solver_core/internals/linear_solvers/sparse_block_linear_solver_cholesky_diagonal.cpp: -------------------------------------------------------------------------------- 1 | #include "sparse_block_linear_solver_cholesky_diagonal.h" 2 | #include "quotient_graph.h" 3 | 4 | namespace srrg2_solver { 5 | void SparseBlockLinearSolverCholeskyDiagonal::computeOrderingHint( 6 | std::vector& ordering, 7 | const std::vector& layout) const { 8 | const int dim = ordering.size(); 9 | for (int i = 0; i < dim; ++i) { 10 | ordering[i] = i; 11 | } 12 | } 13 | 14 | } // namespace srrg2_solver 15 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/solver_core/internals/linear_solvers/sparse_block_linear_solver_cholesky_diagonal.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "sparse_block_linear_solver_cholesky.h" 3 | 4 | namespace srrg2_solver { 5 | 6 | class SparseBlockLinearSolverCholeskyDiagonal : public SparseBlockLinearSolverCholesky { 7 | public: 8 | virtual void computeOrderingHint(std::vector& ordering, 9 | const std::vector& block_layout) const; 10 | }; 11 | 12 | } // namespace srrg2_solver 13 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/solver_core/internals/linear_solvers/sparse_block_linear_solver_cholesky_emd.cpp: -------------------------------------------------------------------------------- 1 | #include "sparse_block_linear_solver_cholesky_emd.h" 2 | #include "quotient_graph.h" 3 | 4 | namespace srrg2_solver { 5 | void SparseBlockLinearSolverCholeskyEMD ::computeOrderingHint( 6 | std::vector& ordering, 7 | const std::vector& layout) const { 8 | const int dim = ordering.size(); 9 | QuotientGraph emd_graph(layout, dim); 10 | emd_graph.setPolicy(QuotientGraph::Policy::Approximate); 11 | emd_graph.mdo(ordering); 12 | } 13 | 14 | } // namespace srrg2_solver 15 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/solver_core/internals/linear_solvers/sparse_block_linear_solver_cholesky_emd.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "sparse_block_linear_solver_cholesky.h" 3 | 4 | namespace srrg2_solver { 5 | 6 | class SparseBlockLinearSolverCholeskyEMD : public SparseBlockLinearSolverCholesky { 7 | public: 8 | virtual void computeOrderingHint(std::vector& ordering, 9 | const std::vector& block_layout) const; 10 | }; 11 | 12 | } // namespace srrg2_solver 13 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/solver_core/internals/sparse_block_matrix/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(srrg2_solver_sparse_block_matrix_library SHARED 2 | c_matrix_ops.hpp 3 | 4 | matrix_block.h 5 | matrix_block.cpp 6 | 7 | matrix_block_factory.h 8 | matrix_block_factory.cpp 9 | 10 | sparse_block_matrix.h 11 | sparse_block_matrix.cpp 12 | 13 | sparse_block_cholesky.h 14 | sparse_block_cholesky.cpp 15 | ) 16 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/solver_core/iteration_algorithm_ddl.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "iteration_algorithm_dl.h" 3 | 4 | namespace srrg2_solver { 5 | using namespace srrg2_core; 6 | /* @brief Double DogLeg optimization algorithm with ellipsoidal trust region 7 | * see IterationAlgorithmDL for the implementation of the inner loop and the computation of the 8 | * steps */ 9 | class IterationAlgorithmDDL : public IterationAlgorithmDL { 10 | protected: 11 | /*! Double dogleg step computation */ 12 | void _computeStep(float& linear_decrease, const float& current_chi) final; 13 | }; 14 | } // namespace srrg2_solver 15 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/solver_core/iteration_algorithm_gn.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | 4 | #include "iteration_algorithm_base.h" 5 | 6 | namespace srrg2_solver { 7 | using namespace srrg2_core; 8 | /*! @brief Gauss-Netwon optimization algorithm. In our implementation you can set 9 | a fixed damping factor to enforce full rank H matrix */ 10 | class IterationAlgorithmGN : public IterationAlgorithmBase { 11 | public: 12 | PARAM(PropertyFloat, 13 | damping, 14 | "damping factor, the higher the closer to gradient descend. Default:0", 15 | 0, 16 | 0); 17 | 18 | virtual void setSolver(Solver* _solver) override; 19 | virtual bool oneRound() override; 20 | 21 | protected: 22 | std::vector _diagonal; /*!< Store the diagonal of the H matrix */ 23 | std::vector _diagonal_backup; /*!< Store the diagonal of the H matrix */ 24 | }; 25 | } // namespace srrg2_solver 26 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/solver_core/iteration_algorithm_gn_pd.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "iteration_algorithm_base.h" 3 | 4 | namespace srrg2_solver { 5 | using namespace srrg2_core; 6 | 7 | class IterationAlgorithmGNPD : public IterationAlgorithmBase { 8 | public: 9 | PARAM(PropertyInt, num_runs, "Number of internal iterations", 100, nullptr); 10 | PARAM(PropertyFloat, damping, "Damping of the GN step ( H + damping * I )", 10, 0); 11 | void setSolver(Solver* solver_) final; 12 | bool oneRound() final; 13 | 14 | protected: 15 | bool _computeGaussNewtonStep(IterationStats& istat); 16 | bool _primalDual(IterationStats& istat); 17 | std::vector _direction; 18 | }; 19 | 20 | } // namespace srrg2_solver 21 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/solver_core/robustifier_policy.cpp: -------------------------------------------------------------------------------- 1 | #include "robustifier_policy.h" 2 | #include "factor_base.h" 3 | 4 | namespace srrg2_solver { 5 | 6 | RobustifierBasePtr RobustifierPolicyByType::getRobustifier(FactorBase* factor) { 7 | if (param_factor_class_name.value() == factor->className()) { 8 | return param_robustifier.value(); 9 | } 10 | return 0; 11 | } 12 | 13 | } // namespace srrg2_solver 14 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/solver_core/solver_action_base.cpp: -------------------------------------------------------------------------------- 1 | #include "solver_action_base.h" 2 | #include "solver.h" 3 | 4 | namespace srrg2_solver { 5 | 6 | /*! @brief Base solver action interface, might be pre or post a solver iteration. In the derived 7 | class you need to override the doAction() method */ 8 | SolverActionBase::~SolverActionBase(){} 9 | 10 | 11 | 12 | SolverVerboseAction::SolverVerboseAction() { 13 | param_event.setValue(Solver::SolverEvent::IterationEnd); 14 | } 15 | 16 | void SolverVerboseAction::doAction() { 17 | std::cerr << _solver_ptr->lastIterationStats() << std::endl; 18 | } 19 | 20 | SolverPreemptAction::SolverPreemptAction() { 21 | param_event.setValue(Solver::SolverEvent::IterationEnd); 22 | } 23 | 24 | void SolverPreemptAction::doAction() { 25 | _solver_ptr->preemptGlobal(); 26 | } 27 | 28 | } // namespace srrg2_solver 29 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/solver_core/solver_action_draw.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "solver_action_base.h" 3 | #include 4 | 5 | namespace srrg2_solver { 6 | 7 | //! @brief action that draws a factor graph on the canvas 8 | class SolverActionDraw : public srrg2_solver::SolverActionBase, public srrg2_core::ActiveDrawable { 9 | public: 10 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 11 | 12 | using BaseType = srrg2_solver::SolverActionBase; 13 | SolverActionDraw(); 14 | virtual ~SolverActionDraw(); 15 | 16 | void doAction(); 17 | protected: 18 | void _drawImpl(srrg2_core::ViewerCanvasPtr gl_canvas_) const override; 19 | }; 20 | 21 | using SolverActionDrawPtr = std::shared_ptr; 22 | } // namespace srrg2_solver_gui 23 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/solver_core/solvers_all_impl.cpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | //! includes all the implementation layers 3 | //! put this at the beginning of a cpp file 4 | //! where you instantiate *all* the types you declared 5 | 6 | #include "srrg_solver/solver_core/variable_impl.cpp" 7 | #include "srrg_solver/solver_core/factor_impl.cpp" 8 | #include "srrg_solver/solver_core/ad_error_factor_impl.cpp" 9 | #include "srrg_solver/solver_core/error_factor_impl.cpp" 10 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/solver_incremental/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(srrg2_solver_incremental_library SHARED 2 | factor_graph_incremental_sorter.cpp 3 | factor_graph_incremental_sorter.h 4 | solver_incremental_base.cpp 5 | solver_incremental_base.h 6 | solver_incremental.cpp 7 | solver_incremental.h 8 | solver_incremental_running_environment.h 9 | solver_incremental_running_environment.cpp 10 | solver_incremental_runner.h 11 | solver_incremental_runner.cpp 12 | 13 | # instances 14 | instances.cpp instances.h 15 | ) 16 | 17 | target_link_libraries(srrg2_solver_incremental_library 18 | srrg2_solver_types2d_library 19 | srrg2_solver_types3d_library 20 | srrg2_solver_projective_library 21 | srrg2_solver_calib_library 22 | srrg2_solver_core_library 23 | srrg2_solver_factor_graph_utils_library 24 | ) 25 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/solver_incremental/instances.cpp: -------------------------------------------------------------------------------- 1 | #include "instances.h" 2 | #include "solver_incremental.h" 3 | #include "factor_graph_incremental_sorter.h" 4 | #include "solver_incremental_runner.h" 5 | 6 | namespace srrg2_solver { 7 | void solver_incremental_registerTypes() { 8 | BOSS_REGISTER_CLASS(SolverIncremental); 9 | BOSS_REGISTER_CLASS(SolverIncrementalRunner); 10 | BOSS_REGISTER_CLASS(EndEpoch); 11 | } 12 | } // namespace srrg2_solver 13 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/solver_incremental/instances.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | namespace srrg2_solver { 4 | void solver_incremental_registerTypes() __attribute__((constructor)) ; 5 | } 6 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/solver_incremental/solver_incremental_base.cpp: -------------------------------------------------------------------------------- 1 | #include "solver_incremental_base.h" 2 | namespace srrg2_solver { 3 | SolverIncrementalBase::~SolverIncrementalBase(){} 4 | } 5 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/utils/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(srrg2_solver_factor_graph_utils_library SHARED 2 | #visit 3 | factor_graph_visit_entry.cpp factor_graph_visit_entry.h 4 | factor_graph_visit_cost.cpp factor_graph_visit_cost.h 5 | factor_graph_visit_policy.cpp factor_graph_visit_policy.h 6 | factor_graph_visit.cpp factor_graph_visit.h 7 | 8 | # initializer 9 | factor_graph_initializer_rule.cpp factor_graph_initializer_rule.h 10 | factor_graph_initializer.cpp factor_graph_initializer.h 11 | 12 | # closure validator 13 | factor_graph_closure_validator.cpp factor_graph_closure_validator.h 14 | 15 | solver_evaluator.cpp 16 | solver_evaluator.h 17 | # instances 18 | instances.cpp instances.h 19 | ) 20 | 21 | target_link_libraries(srrg2_solver_factor_graph_utils_library 22 | srrg2_solver_types2d_library 23 | srrg2_solver_types3d_library 24 | srrg2_solver_projective_library 25 | srrg2_solver_calib_library 26 | srrg2_solver_core_library 27 | ) 28 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/utils/factor_graph_initializer_rule.cpp: -------------------------------------------------------------------------------- 1 | #include "factor_graph_initializer_rule.h" 2 | #include "factor_graph_initializer.h" 3 | 4 | namespace srrg2_solver { 5 | FactorGraphInitializerRuleBase::FactorGraphInitializerRuleBase(FactorGraphInitializer* initializer_) { 6 | _initializer=initializer_; 7 | } 8 | 9 | } 10 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/utils/instances.cpp: -------------------------------------------------------------------------------- 1 | #include "instances.h" 2 | #include "factor_graph_closure_validator.h" 3 | #include "factor_graph_initializer.h" 4 | #include "factor_graph_visit.h" 5 | 6 | namespace srrg2_solver { 7 | void solver_utils_registerTypes() { 8 | BOSS_REGISTER_CLASS(FactorGraphVisit); 9 | BOSS_REGISTER_CLASS(FactorGraphVisitCostUniform); 10 | BOSS_REGISTER_CLASS(FactorGraphVisitTouchMap); 11 | BOSS_REGISTER_CLASS(FactorGraphVisitCostCount); 12 | BOSS_REGISTER_CLASS(FactorGraphVisitPolicyBase); 13 | BOSS_REGISTER_CLASS(FactorGraphVisitPolicyByType); 14 | BOSS_REGISTER_CLASS(FactorGraphClosureValidator); 15 | BOSS_REGISTER_CLASS(FactorGraphInitializer); 16 | } 17 | } // namespace srrg2_solver 18 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/utils/instances.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | namespace srrg2_solver { 4 | void solver_utils_registerTypes() __attribute__((constructor)) ; 5 | } 6 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/utils/solver_evaluator.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "srrg_solver/solver_core/factor_graph_view.h" 3 | 4 | namespace srrg2_solver { 5 | 6 | struct SolverEvaluator { 7 | void setGroundTruth(FactorGraphInterface& gt_); 8 | void compute(FactorGraphInterface& view_); 9 | 10 | template 11 | void align(FactorGraphInterface& view, bool adjust=false); 12 | 13 | void alignSE2(FactorGraphInterface& view, bool adjust=false); 14 | void alignSE3(FactorGraphInterface& view, bool adjust=false); 15 | void alignSim3(FactorGraphInterface& view, bool adjust=false); 16 | 17 | inline const float chiCutoff() const {return _chi_cutoff;} 18 | inline void setChiCutoff(const float& chi_cutoff_) {_chi_cutoff=chi_cutoff_;} 19 | protected: 20 | float _chi_cutoff=std::numeric_limits::max(); 21 | FactorGraphView _eval_view; 22 | FactorGraphInterface* _gt=nullptr; 23 | }; 24 | } 25 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/variables_and_factors/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_subdirectory(types_common) 2 | add_subdirectory(types_2d) 3 | add_subdirectory(types_3d) 4 | add_subdirectory(types_projective) 5 | add_subdirectory(types_calib) 6 | add_subdirectory(types_sfm) 7 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/variables_and_factors/types_2d/all_types.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "se2_plane2plane_error_factor.h" 4 | #include "se2_point2point_error_factor.h" 5 | #include "se2_prior_error_factor.h" 6 | 7 | #include "se2_pose_pose_chordal_error_factor.h" 8 | #include "se2_pose_pose_geodesic_error_factor.h" 9 | 10 | #include "se2_circumference_constraint_factor.h" 11 | #include "se2_pose_point_error_factor.h" 12 | 13 | #include "se2_pose_point_bearing_error_factor.h" 14 | 15 | #include "se2_pose_point_range_constraint_factor.h" 16 | 17 | #include "variable_point2.h" 18 | #include "variable_point2_ad.h" 19 | #include "variable_se2.h" 20 | #include "variable_se2_ad.h" 21 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/variables_and_factors/types_2d/instances.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | namespace srrg2_solver { 4 | void variables_and_factors_2d_registerTypes() __attribute__((constructor)); 5 | } // namespace srrg2_solver 6 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/variables_and_factors/types_2d/se2_circumference_constraint_factor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "srrg_solver/solver_core/constraint_factor.h" 3 | #include "variable_se2.h" 4 | #include 5 | #include 6 | 7 | namespace srrg2_solver { 8 | using namespace srrg2_core; 9 | class SE2CircumferenceConstraintFactor : public ConstraintFactor_<1, VariableSE2Right> { 10 | public: 11 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 12 | using BaseFactorType = ConstraintFactor_<1, VariableSE2Right>; 13 | using EstimateType = VariableSE2Right::EstimateType; 14 | 15 | void setRadius(const float& radius_) { 16 | _radius = radius_; 17 | } 18 | 19 | void constraintAndJacobian(bool constraint_only_ = false) final; 20 | 21 | protected: 22 | float _radius = 0.f; 23 | }; 24 | } // namespace srrg2_solver 25 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/variables_and_factors/types_2d/se2_pose_point_bearing_error_factor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | 4 | #include "srrg_solver/solver_core/error_factor.h" 5 | #include "variable_point2.h" 6 | #include "variable_se2.h" 7 | 8 | namespace srrg2_solver { 9 | 10 | /** @brief 2D Bearing only factor. Estimate both the pose of the robot and the position of the 2D 11 | * landmark. 12 | * Error is estimated using the difference between the predicted landmark angle wrt the 13 | * robot pose and the measurement. 14 | */ 15 | class SE2PosePointBearingErrorFactor : public ErrorFactor_<1, VariableSE2Right, VariablePoint2>, 16 | public MeasurementOwnerEigen_ { 17 | public: 18 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 19 | void errorAndJacobian(bool error_only_ = false) final; 20 | void _drawImpl(srrg2_core::ViewerCanvasPtr canvas) const override; 21 | }; 22 | 23 | } // namespace srrg2_solver 24 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/variables_and_factors/types_2d/se2_pose_point_error_factor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "srrg_solver/solver_core/error_factor.h" 3 | #include "variable_point2.h" 4 | #include "variable_se2.h" 5 | #include 6 | 7 | namespace srrg2_solver { 8 | using namespace srrg2_core; 9 | 10 | /** @brief 2D Pose-point factor. Estimates the robot pose and the 2D landmark position. 11 | * Error is computed by the difference of landmark's prediction in robot's coordinates and the 12 | * measurement gathered 13 | */ 14 | class SE2PosePointErrorFactor : public ErrorFactor_<2, VariableSE2Right, VariablePoint2>, 15 | public MeasurementOwnerEigen_ { 16 | public: 17 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 18 | void errorAndJacobian(bool error_only_ = false) final; 19 | }; 20 | } // namespace srrg2_solver 21 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/variables_and_factors/types_2d/variable_point2.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "srrg_solver/variables_and_factors/types_common/all_types.h" 3 | 4 | namespace srrg2_solver { 5 | 6 | using namespace srrg2_core; 7 | 8 | } // namespace srrg2_solver 9 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/variables_and_factors/types_2d/variable_point2_ad.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "srrg_solver/variables_and_factors/types_common/all_types.h" 3 | 4 | namespace srrg2_solver { 5 | 6 | using namespace srrg2_core; 7 | 8 | } // namespace srrg2_solver 9 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/variables_and_factors/types_2d/variable_so2.cpp: -------------------------------------------------------------------------------- 1 | #include "variable_so2.h" 2 | 3 | //! include this: this contains all the implementations of the factors 4 | //! that are hidden to the modules that do not need them to avoid excessive compilation times (EVIL) 5 | #include "srrg_solver/solver_core/instance_macros.h" 6 | #include "srrg_solver/solver_core/variable_impl.cpp" 7 | 8 | namespace srrg2_solver { 9 | using namespace srrg2_core; 10 | 11 | INSTANTIATE(VariableSO2Right) 12 | INSTANTIATE(VariableSO2Left) 13 | 14 | } // namespace srrg2_solver 15 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/variables_and_factors/types_3d/gps_factor_ad.cpp: -------------------------------------------------------------------------------- 1 | #include "gps_factor_ad.h" 2 | #include 3 | #include 4 | #include 5 | 6 | namespace srrg2_solver { 7 | 8 | void GpsErrorFactorAD::_drawImpl(ViewerCanvasPtr canvas_) const { 9 | } 10 | 11 | INSTANTIATE(GpsErrorFactorAD) 12 | } // namespace srrg2_solver 13 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/variables_and_factors/types_3d/imu_bias_variable.cpp: -------------------------------------------------------------------------------- 1 | #include "imu_bias_variable.h" 2 | #include "srrg_solver/solver_core/instance_macros.h" 3 | #include "srrg_solver/solver_core/variable_impl.cpp" 4 | #include 5 | 6 | namespace srrg2_solver { 7 | using namespace srrg2_core; 8 | 9 | void VariableImuBias::setZero() { 10 | this->_updated = true; 11 | _estimate.setZero(); 12 | } 13 | 14 | void VariableImuBias::applyPerturbation(const Vector6f& pert) { 15 | this->_updated = true; 16 | _estimate += pert; 17 | } 18 | 19 | void VariableImuBias::_drawImpl(ViewerCanvasPtr canvas_) const { 20 | throw std::runtime_error("VariableImuBias::draw| draw not implemented"); 21 | } 22 | 23 | void VariableImuBiasAD::applyPerturbationAD(const ADPerturbationVectorType& ad_pert) { 24 | _ad_estimate += ad_pert; 25 | } 26 | 27 | INSTANTIATE(VariableImuBias) 28 | INSTANTIATE(VariableImuBiasAD) 29 | } // namespace srrg2_solver 30 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/variables_and_factors/types_3d/instances.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | namespace srrg2_solver { 4 | void variables_and_factors_3d_registerTypes() __attribute__((constructor)); 5 | } // namespace srrg2_solver 6 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/variables_and_factors/types_3d/se3_pose_motion_error_factor_ad.cpp: -------------------------------------------------------------------------------- 1 | #include "se3_pose_motion_error_factor_ad.h" 2 | #include "srrg_solver/solver_core/ad_error_factor_impl.cpp" 3 | #include "srrg_solver/solver_core/error_factor_impl.cpp" 4 | #include "srrg_solver/solver_core/instance_macros.h" 5 | 6 | namespace srrg2_solver { 7 | INSTANTIATE(SE3PoseMotionErrorFactorAD) 8 | INSTANTIATE(SE3PoseMotionErrorFactorDataDriven) 9 | } 10 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/variables_and_factors/types_3d/se3_pose_point_error_factor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "srrg_solver/solver_core/error_factor.h" 3 | #include "srrg_solver/solver_core/measurement_owner.h" 4 | #include "variable_point3.h" 5 | #include "variable_se3.h" 6 | 7 | namespace srrg2_solver { 8 | using namespace srrg2_core; 9 | 10 | /** @brief 3D Pose-point factor. Estimates the robot pose and the landmark position. 11 | * Error is computed by the difference of landmark's prediction in robot's coordinates and the 12 | * measurement gathered 13 | */ 14 | class SE3PosePointErrorFactor 15 | : public ErrorFactor_<3, VariableSE3QuaternionRight, VariablePoint3>, 16 | public MeasurementOwnerEigen_ { 17 | public: 18 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 19 | void errorAndJacobian(bool error_only_ = false); 20 | void _drawImpl(ViewerCanvasPtr canvas_) const override; 21 | }; 22 | } // namespace srrg2_solver 23 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/variables_and_factors/types_3d/se3_pose_pose_chordal_error_factor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "srrg_solver/solver_core/error_factor.h" 3 | #include "variable_se3.h" 4 | 5 | namespace srrg2_solver { 6 | /** 7 | * @brief 3D PGO error factor exploiting chordal distance. 8 | * Error vector is 12 component (from h(x) - Z) 9 | * vertices use EULER angles with increment PRE-multiplied 10 | */ 11 | class SE3PosePoseChordalEulerLeftErrorFactor 12 | : public ErrorFactor_<12, VariableSE3EulerLeft, VariableSE3EulerLeft>, 13 | public MeasurementOwnerEigen_ { 14 | public: 15 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 16 | using BaseType = ErrorFactor_<12, VariableSE3EulerLeft, VariableSE3EulerLeft>; 17 | using Scalar = MeasurementType::Scalar; 18 | void errorAndJacobian(bool error_only_ = false); 19 | 20 | void _drawImpl(ViewerCanvasPtr canvas_) const override; 21 | }; 22 | } // namespace srrg2_solver 23 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/variables_and_factors/types_3d/se3_pose_pose_error_factor_base.cpp: -------------------------------------------------------------------------------- 1 | #include "se3_pose_pose_error_factor_base.h" 2 | #include "variable_se3.h" 3 | 4 | namespace srrg2_solver { 5 | 6 | void SE3PosePoseErrorFactorBase::_drawImpl(ViewerCanvasPtr canvas_) const { 7 | Vector3f coords[2]; 8 | coords[0] = 9 | static_cast(variable(0))->estimate().translation(); 10 | coords[1] = 11 | static_cast(variable(1))->estimate().translation(); 12 | canvas_->pushColor(); 13 | float fading=1.-0.5*level(); 14 | Vector3f color=srrg2_core::ColorPalette::color3fBlue()*fading; 15 | canvas_->setColor(color); 16 | canvas_->putLine(2, coords); 17 | canvas_->popAttribute(); 18 | } 19 | 20 | } // namespace srrg2_solver 21 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/variables_and_factors/types_3d/se3_pose_pose_right_chordal_error_factor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "srrg_solver/solver_core/error_factor.h" 3 | #include "variable_se3.h" 4 | 5 | namespace srrg2_solver { 6 | /** 7 | * @brief 3D PGO error factor exploiting chordal distance. 8 | * Error vector is 12 component (from h(x) - Z) 9 | * vertices use EULER angles with increment POST-multiplied 10 | */ 11 | class SE3PosePoseChordalEulerRightErrorFactor 12 | : public ErrorFactor_<12, VariableSE3EulerRight, VariableSE3EulerRight>, 13 | public MeasurementOwnerEigen_ { 14 | public: 15 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 16 | using BaseType = ErrorFactor_<12, VariableSE3EulerRight, VariableSE3EulerRight>; 17 | using Scalar = MeasurementType::Scalar; 18 | void errorAndJacobian(bool error_only_ = false); 19 | 20 | void _drawImpl(ViewerCanvasPtr canvas_) const override; 21 | }; 22 | } // namespace srrg2_solver 23 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/variables_and_factors/types_3d/se3_prior_error_factor_ad.cpp: -------------------------------------------------------------------------------- 1 | #include "se3_prior_error_factor_ad.h" 2 | #include "srrg_solver/solver_core/ad_error_factor_impl.cpp" 3 | #include "srrg_solver/solver_core/error_factor_impl.cpp" 4 | #include "srrg_solver/solver_core/instance_macros.h" 5 | 6 | namespace srrg2_solver { 7 | INSTANTIATE(SE3PriorErrorFactorAD) 8 | } 9 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/variables_and_factors/types_3d/se3_prior_offset_error_factor_ad.cpp: -------------------------------------------------------------------------------- 1 | #include "se3_prior_offset_error_factor_ad.h" 2 | #include "srrg_solver/solver_core/ad_error_factor_impl.cpp" 3 | #include "srrg_solver/solver_core/error_factor_impl.cpp" 4 | #include "srrg_solver/solver_core/instance_macros.h" 5 | 6 | namespace srrg2_solver { 7 | 8 | SE3PriorOffsetErrorFactorAD::ADErrorVectorType SE3PriorOffsetErrorFactorAD:: 9 | operator()(VariableTupleType& vars_) { 10 | Isometry3_ prediction_ad = 11 | _robot_in_sensor_ad * vars_.at<0>()->adEstimate() * _sensor_in_robot_ad; 12 | return geometry3d::t2v(_inv_measure_ad * prediction_ad); 13 | } 14 | 15 | INSTANTIATE(SE3PriorOffsetErrorFactorAD) 16 | } // namespace srrg2_solver 17 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/variables_and_factors/types_3d/variable_matchable.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | 4 | #include "srrg_solver/solver_core/variable.h" 5 | 6 | namespace srrg2_solver { 7 | class VariableMatchable : public VariableGeneric_<5, srrg2_core::Matchable_> { 8 | public: 9 | using BaseVariableType = Variable_<5, srrg2_core::Matchable_>; 10 | 11 | //! @brief object life (ds style) 12 | ~VariableMatchable(); 13 | 14 | void setZero(); 15 | 16 | void applyPerturbation(const srrg2_core::Vector5f& pert_) override; 17 | 18 | //! @brief serialization of the variable through BOSS. 19 | void serialize(ObjectData& odata, IdContext& context) override; 20 | 21 | //! @brief deserialization of the variable through BOSS. 22 | void deserialize(ObjectData& odata, IdContext& context) override; 23 | 24 | void _drawImpl(ViewerCanvasPtr canvas_) const override; 25 | }; 26 | } // namespace srrg2_solver 27 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/variables_and_factors/types_3d/variable_point3.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "srrg_solver/variables_and_factors/types_common/all_types.h" 3 | 4 | namespace srrg2_solver { 5 | 6 | } // namespace srrg2_solver 7 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/variables_and_factors/types_3d/variable_point3_ad.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "srrg_solver/variables_and_factors/types_common/all_types.h" 3 | 4 | namespace srrg2_solver { 5 | 6 | } // namespace srrg2_solver 7 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/variables_and_factors/types_3d/variable_se3_ad.cpp: -------------------------------------------------------------------------------- 1 | #include "variable_se3_ad.h" 2 | #include "srrg_solver/solver_core/instance_macros.h" 3 | #include "srrg_solver/solver_core/variable_impl.cpp" 4 | 5 | namespace srrg2_solver { 6 | INSTANTIATE(VariableSE3EulerRightAD) 7 | INSTANTIATE(VariableSE3EulerLeftAD) 8 | INSTANTIATE(VariableSE3QuaternionRightAD) 9 | INSTANTIATE(VariableSE3QuaternionLeftAD) 10 | 11 | } // namespace srrg2_solver 12 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/variables_and_factors/types_calib/all_types.: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "srrg_solver/variables_and_factors/types_2d/instances.h" 3 | #include "srrg_solver/solver_core/factor_impl.cpp" 4 | #include "srrg_solver/variables_and_factors/types_3d/instances.h" 5 | #include "differential_drive_odom_error_factor_ad.h" 6 | #include "differential_drive_odom_predictor_ad.h" 7 | #include "differential_drive_odom_sensor2d_error_factor_ad.h" 8 | #include "differential_drive_odom_sensor3d_error_factor_ad.h" 9 | #include "differential_drive_odom_time_delay_sensor2d_error_factor_ad.h" 10 | #include "sensor2d_extrinsic_pose_motion_calib_ad.h" 11 | #include "sensor3d_extrinsic_pose_motion_calib_ad.h" 12 | #include "sensor3d_pose_time_delay_error_factor_ad.h" 13 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/variables_and_factors/types_calib/all_types.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "differential_drive_odom_error_factor_ad.h" 3 | #include "differential_drive_odom_predictor_ad.h" 4 | #include "differential_drive_odom_sensor2d_error_factor_ad.h" 5 | #include "differential_drive_odom_sensor3d_error_factor_ad.h" 6 | #include "differential_drive_odom_time_delay_sensor2d_error_factor_ad.h" 7 | #include "sensor2d_extrinsic_pose_motion_calib_ad.h" 8 | #include "sensor3d_extrinsic_pose_motion_calib_ad.h" 9 | #include "sensor3d_pose_time_delay_error_factor_ad.h" 10 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/variables_and_factors/types_calib/differential_drive_odom_error_factor_ad.cpp: -------------------------------------------------------------------------------- 1 | #include "differential_drive_odom_error_factor_ad.h" 2 | #include "srrg_solver/solver_core/ad_error_factor_impl.cpp" 3 | #include "srrg_solver/solver_core/error_factor_impl.cpp" 4 | #include "srrg_solver/solver_core/instance_macros.h" 5 | 6 | namespace srrg2_solver { 7 | INSTANTIATE(DifferentialDriveOdomErrorFactorAD) 8 | } 9 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/variables_and_factors/types_calib/differential_drive_odom_sensor2d_error_factor_ad.cpp: -------------------------------------------------------------------------------- 1 | #include "differential_drive_odom_sensor2d_error_factor_ad.h" 2 | 3 | //! include this: this contains all the implementations of the factors 4 | //! that are hidden to the modules that do not need them to avoid excessive compilation times (EVIL) 5 | #include "srrg_solver/solver_core/ad_error_factor_impl.cpp" 6 | #include "srrg_solver/solver_core/error_factor_impl.cpp" 7 | #include "srrg_solver/solver_core/instance_macros.h" 8 | 9 | namespace srrg2_solver { 10 | INSTANTIATE(DifferentialDriveOdomSensor2DErrorFactorAD) 11 | } // namespace srrg2_solver 12 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/variables_and_factors/types_calib/differential_drive_odom_sensor3d_error_factor_ad.cpp: -------------------------------------------------------------------------------- 1 | #include "differential_drive_odom_sensor3d_error_factor_ad.h" 2 | 3 | //! include this: this contains all the implementations of the factors 4 | //! that are hidden to the modules that do not need them to avoid excessive compilation times (EVIL) 5 | #include "srrg_solver/solver_core/error_factor_impl.cpp" 6 | #include "srrg_solver/solver_core/ad_error_factor_impl.cpp" 7 | #include "srrg_solver/solver_core/instance_macros.h" 8 | 9 | namespace srrg2_solver { 10 | using namespace srrg2_core; 11 | INSTANTIATE(DifferentialDriveOdomSensor3DErrorFactorAD) 12 | 13 | } // namespace srrg2_solver 14 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/variables_and_factors/types_calib/differential_drive_odom_time_delay_sensor2d_error_factor_ad.cpp: -------------------------------------------------------------------------------- 1 | #include "differential_drive_odom_time_delay_sensor2d_error_factor_ad.h" 2 | 3 | //! include this: this contains all the implementations of the factors 4 | //! that are hidden to the modules that do not need them to avoid excessive compilation times (EVIL) 5 | #include "srrg_solver/solver_core/ad_error_factor_impl.cpp" 6 | #include "srrg_solver/solver_core/error_factor_impl.cpp" 7 | #include "srrg_solver/solver_core/instance_macros.h" 8 | 9 | namespace srrg2_solver { 10 | INSTANTIATE(DifferentialDriveOdomTimeDelaySensor2DErrorFactorAD) 11 | } // namespace srrg2_solver 12 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/variables_and_factors/types_calib/instances.cpp: -------------------------------------------------------------------------------- 1 | #include "instances.h" 2 | #include "all_types.h" 3 | 4 | namespace srrg2_solver { 5 | using namespace srrg2_core; 6 | 7 | // this is the function you have to call to initialize 8 | // the serialization subsystem 9 | void variables_and_factors_calib_registerTypes() { 10 | BOSS_REGISTER_CLASS(DifferentialDriveOdomErrorFactorAD); 11 | BOSS_REGISTER_CLASS(DifferentialDriveOdomSensor2DErrorFactorAD); 12 | BOSS_REGISTER_CLASS(DifferentialDriveOdomSensor3DErrorFactorAD); 13 | BOSS_REGISTER_CLASS(VariableTime); 14 | BOSS_REGISTER_CLASS(VariableTimeAD); 15 | BOSS_REGISTER_CLASS(SE3SensorPoseTimeDelayErrorFactorAD); 16 | BOSS_REGISTER_CLASS(DifferentialDriveOdomTimeDelaySensor2DErrorFactorAD); 17 | BOSS_REGISTER_CLASS(SE2SensorExtrinsicPoseMotionCalibAD); 18 | } 19 | } // namespace srrg2_solver 20 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/variables_and_factors/types_calib/instances.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | namespace srrg2_solver { 4 | void variables_and_factors_calib_registerTypes() __attribute__((constructor)); 5 | } 6 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/variables_and_factors/types_calib/sensor2d_extrinsic_pose_motion_calib_ad.cpp: -------------------------------------------------------------------------------- 1 | #include "sensor2d_extrinsic_pose_motion_calib_ad.h" 2 | #include "srrg_solver/solver_core/ad_error_factor_impl.cpp" 3 | #include "srrg_solver/solver_core/error_factor_impl.cpp" 4 | #include "srrg_solver/solver_core/instance_macros.h" 5 | 6 | namespace srrg2_solver { 7 | using namespace srrg2_core; 8 | 9 | void SE2SensorExtrinsicPoseMotionCalibAD::serialize(ObjectData& odata, IdContext& context) { 10 | BaseType::serialize(odata, context); 11 | odata.setEigen("from", _from); 12 | odata.setEigen("to", _to); 13 | } 14 | 15 | void SE2SensorExtrinsicPoseMotionCalibAD::deserialize(ObjectData& odata, IdContext& context) { 16 | BaseType::deserialize(odata, context); 17 | _from = odata.getEigen("from"); 18 | _to = odata.getEigen("to"); 19 | setFrom(_from); 20 | setTo(_to); 21 | } 22 | 23 | INSTANTIATE(SE2SensorExtrinsicPoseMotionCalibAD) 24 | } // namespace srrg2_solver 25 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/variables_and_factors/types_calib/sensor3d_extrinsic_pose_motion_calib_ad.cpp: -------------------------------------------------------------------------------- 1 | #include "sensor3d_extrinsic_pose_motion_calib_ad.h" 2 | #include "srrg_solver/solver_core/ad_error_factor_impl.cpp" 3 | #include "srrg_solver/solver_core/error_factor_impl.cpp" 4 | #include "srrg_solver/solver_core/instance_macros.h" 5 | 6 | namespace srrg2_solver { 7 | using namespace srrg2_core; 8 | 9 | void SE3SensorExtrinsicPoseMotionCalibAD::serialize(ObjectData& odata, IdContext& context) { 10 | BaseType::serialize(odata, context); 11 | odata.setEigen("from", _from); 12 | odata.setEigen("to", _to); 13 | } 14 | 15 | void SE3SensorExtrinsicPoseMotionCalibAD::deserialize(ObjectData& odata, IdContext& context) { 16 | BaseType::deserialize(odata, context); 17 | _from = odata.getEigen("from"); 18 | _to = odata.getEigen("to"); 19 | setFrom(_from); 20 | setTo(_to); 21 | } 22 | 23 | INSTANTIATE(SE3SensorExtrinsicPoseMotionCalibAD) 24 | } // namespace srrg2_solver 25 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/variables_and_factors/types_calib/sensor3d_pose_time_delay_error_factor_ad.cpp: -------------------------------------------------------------------------------- 1 | #include "sensor3d_pose_time_delay_error_factor_ad.h" 2 | #include "srrg_solver/solver_core/ad_error_factor_impl.cpp" 3 | #include "srrg_solver/solver_core/error_factor_impl.cpp" 4 | #include "srrg_solver/solver_core/instance_macros.h" 5 | 6 | namespace srrg2_solver { 7 | INSTANTIATE(SE3SensorPoseTimeDelayErrorFactorAD) 8 | } 9 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/variables_and_factors/types_calib/variable_time.cpp: -------------------------------------------------------------------------------- 1 | #include "variable_time.h" 2 | #include "srrg_solver/solver_core/variable_impl.cpp" 3 | #include "srrg_solver/solver_core/instance_macros.h" 4 | 5 | namespace srrg2_solver { 6 | VariableTime::~VariableTime(){} 7 | INSTANTIATE(VariableTime) 8 | } 9 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/variables_and_factors/types_calib/variable_time.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | 5 | namespace srrg2_solver { 6 | using namespace srrg2_core; 7 | 8 | /** 9 | * @brief 1D Variable Time 10 | */ 11 | class VariableTime : public Variable_<1, Vector1_> { 12 | public: 13 | using BaseVariableType = VariableTime; 14 | 15 | virtual void setZero() override { 16 | this->_updated = true; 17 | _estimate.setZero(); 18 | } 19 | virtual void applyPerturbation(const Vector1f& pert) override { 20 | this->_updated = true; 21 | _estimate += pert; 22 | } 23 | 24 | virtual ~VariableTime(); 25 | }; 26 | 27 | } // namespace srrg2_solver 28 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/variables_and_factors/types_calib/variable_time_ad.cpp: -------------------------------------------------------------------------------- 1 | #include "variable_time_ad.h" 2 | #include "srrg_solver/solver_core/variable_impl.cpp" 3 | #include "srrg_solver/solver_core/instance_macros.h" 4 | 5 | namespace srrg2_solver { 6 | VariableTimeAD::~VariableTimeAD(){} 7 | INSTANTIATE(VariableTimeAD) 8 | } 9 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/variables_and_factors/types_common/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(srrg2_solver_types_common_library SHARED 2 | instances.h 3 | instances.cpp 4 | ) 5 | 6 | target_link_libraries(srrg2_solver_types_common_library 7 | srrg2_solver_core_library 8 | ) 9 | 10 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/variables_and_factors/types_common/instances.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | namespace srrg2_solver { 4 | void variables_and_factors_common_registerTypes() __attribute__((constructor)); 5 | } // namespace srrg2_solver 6 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/variables_and_factors/types_common/variable_matrix_ad.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "srrg_solver/solver_core/ad_variable.h" 3 | #include "variable_vector.h" 4 | 5 | namespace srrg2_solver { 6 | 7 | using namespace srrg2_core; 8 | 9 | template 10 | class VariableMatrixAD_ : public ADVariable_ { 11 | public: 12 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 13 | using VariableType = VariableMatrixBase_; 14 | using ADVariableType = ADVariable_; 15 | using ADPerturbationVectorType = typename ADVariableType::ADPerturbationVectorType; 16 | using ADEstimateType = typename ADVariableType::ADEstimateType; 17 | 18 | virtual ~VariableMatrixAD_() = default; 19 | 20 | virtual void setZero() override; 21 | 22 | virtual void applyPerturbationAD(const ADPerturbationVectorType& ad_pert); 23 | 24 | }; 25 | 26 | } // namespace srrg2_solver 27 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/variables_and_factors/types_common/variable_matrix_ad_impl.cpp: -------------------------------------------------------------------------------- 1 | #include "variable_vector_ad.h" 2 | namespace srrg2_solver { 3 | 4 | using namespace srrg2_core; 5 | 6 | template 7 | void VariableMatrixAD_::setZero() { 8 | ADVariableType::setEstimate(VariableType::EstimateType::Zero()); 9 | } 10 | 11 | 12 | template 13 | void VariableMatrixAD_ 14 | ::applyPerturbationAD(const typename VariableMatrixAD_::ADPerturbationVectorType& ad_pert) { 15 | int k=0; 16 | for(int r=0; r_ad_estimate(r,c) += ad_pert(k); 19 | } 20 | 21 | } // namespace srrg2_solver 22 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/variables_and_factors/types_common/variable_matrix_impl.cpp: -------------------------------------------------------------------------------- 1 | #include "variable_matrix.h" 2 | #include 3 | 4 | namespace srrg2_solver { 5 | template 6 | void VariableMatrix_::setZero() { 7 | this->_updated=true; 8 | this->_estimate.setZero(); 9 | } 10 | 11 | template 12 | void VariableMatrix_::applyPerturbation(const typename VariableMatrix_::PerturbationVectorType& pert) { 13 | this->_updated=true; 14 | int k=0; 15 | for (int r=0; r_estimate(r,c) += pert(k); 18 | } 19 | 20 | } // namespace srrg2_solver 21 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/variables_and_factors/types_common/variable_vector_ad.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "srrg_solver/solver_core/ad_variable.h" 3 | #include "variable_vector.h" 4 | 5 | namespace srrg2_solver { 6 | 7 | using namespace srrg2_core; 8 | 9 | template 10 | class VariableVectorAD_ : public ADVariable_ { 11 | public: 12 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 13 | using VariableType = VariableVectorBase_; 14 | using ADVariableType = ADVariable_; 15 | using ADPerturbationVectorType = typename ADVariableType::ADPerturbationVectorType; 16 | using ADEstimateType = typename ADVariableType::ADEstimateType; 17 | 18 | virtual ~VariableVectorAD_() = default; 19 | 20 | virtual void setZero() override; 21 | 22 | virtual void applyPerturbationAD(const ADPerturbationVectorType& ad_pert); 23 | 24 | }; 25 | 26 | } // namespace srrg2_solver 27 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/variables_and_factors/types_common/variable_vector_ad_impl.cpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "variable_vector_ad.h" 3 | namespace srrg2_solver { 4 | 5 | using namespace srrg2_core; 6 | 7 | template 8 | void VariableVectorAD_::setZero() { 9 | ADVariableType::setEstimate(VariableType::EstimateType::Zero()); 10 | } 11 | 12 | 13 | template 14 | void VariableVectorAD_ 15 | ::applyPerturbationAD(const typename VariableVectorAD_::ADPerturbationVectorType& ad_pert) { 16 | this->_ad_estimate += ad_pert; 17 | } 18 | 19 | } // namespace srrg2_solver 20 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/variables_and_factors/types_common/variable_vector_impl.cpp: -------------------------------------------------------------------------------- 1 | #include "variable_vector.h" 2 | 3 | namespace srrg2_solver { 4 | template 5 | void VariableVector_::setZero() { 6 | this->_updated = true; 7 | this->_estimate.setZero(); 8 | } 9 | 10 | template 11 | void VariableVector_::applyPerturbation( 12 | const typename VariableVector_::EstimateType& pert) { 13 | this->_updated = true; 14 | this->_estimate += pert; 15 | } 16 | 17 | } // namespace srrg2_solver 18 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/variables_and_factors/types_projective/all_types.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "se3_projective_depth_error_factor.h" 3 | #include "se3_projective_error_factor.h" 4 | #include "se3_rectified_stereo_projective_error_factor.h" 5 | 6 | // ldg similiarity variables 7 | #include "variable_sim3.h" 8 | #include "variable_sim3_ad.h" 9 | // // ldg similiarity factors 10 | #include "sim3_point2point_error_factor.h" 11 | #include "sim3_pose_pose_error_factor_ad.h" 12 | #include "se3_pose_point_omni_ba_error_factor.h" 13 | #include "se3_pose_point_pinhole_ba_error_factor.h" 14 | #include "se3_pose_point_rectified_stereo_error_factor.h" 15 | 16 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/variables_and_factors/types_projective/instances.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | namespace srrg2_solver { 4 | void variables_and_factors_projective_registerTypes() __attribute__((constructor)); 5 | } 6 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/variables_and_factors/types_projective/variable_sim3_ad.cpp: -------------------------------------------------------------------------------- 1 | #include "variable_sim3_ad.h" 2 | #include "srrg_solver/solver_core/instance_macros.h" 3 | #include "srrg_solver/solver_core/variable_impl.cpp" 4 | 5 | namespace srrg2_solver { 6 | INSTANTIATE(VariableSim3EulerRightAD) 7 | INSTANTIATE(VariableSim3EulerLeftAD) 8 | INSTANTIATE(VariableSim3QuaternionRightAD) 9 | INSTANTIATE(VariableSim3QuaternionLeftAD) 10 | 11 | } // namespace srrg2_solver 12 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/variables_and_factors/types_sfm/all_types.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "se3_epipolar_ba_left_factor.h" 4 | #include "se3_epipolar_ba_right_factor.h" 5 | #include "se3_pairwise_scale_error_factor_ad.h" 6 | #include "so3_pose_pose_error_factor.h" 7 | #include "variable_so3.h" 8 | 9 | #include "se3_epipolar_ba_offset_left_factor.h" 10 | //#include "se3_epipolar_translation_factor.h" 11 | #include "essential_translation_factor.h" 12 | #include "so3_pose_constraint_factor.h" 13 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/variables_and_factors/types_sfm/instances.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | namespace srrg2_solver { 4 | void variables_and_factors_sfm_registerTypes() __attribute__((constructor)); 5 | } 6 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/variables_and_factors/types_sfm/se3_epipolar_ba_factor_base.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | 5 | 6 | namespace srrg2_solver { 7 | using namespace srrg2_core; 8 | 9 | class SE3EpipolarBAFactorBase { 10 | public: 11 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 12 | using InformationMatrixType=Eigen::Matrix; 13 | inline void setFixed(const Point3f& fixed_) {_fixed=fixed_;} 14 | inline void setMoving(const Point3f& moving_) {_moving=moving_;} 15 | inline const InformationMatrixType& informationMatrix() const {return _information_matrix;} 16 | inline void setInformationMatrix(const InformationMatrixType& info) {_information_matrix = info;} 17 | 18 | protected: 19 | Point3f _fixed, _moving; 20 | InformationMatrixType _information_matrix=InformationMatrixType::Identity(); 21 | }; 22 | } 23 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/variables_and_factors/types_sfm/variable_se3_relaxed.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "srrg_solver/solver_core/variable.h" 3 | #include 4 | 5 | namespace srrg2_solver { 6 | using namespace srrg2_core; 7 | 8 | /** @brief SE3 Pose Variable base class. 9 | */ 10 | class VariableSE3Relaxed : public Variable_<12, Isometry3_> { 11 | public: 12 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 13 | virtual void applyPerturbation(const Vector6f& pert) override; 14 | void normalize() override; 15 | }; 16 | 17 | } // namespace srrg2_solver 18 | -------------------------------------------------------------------------------- /modules/srrg/srrg2_solver/srrg2_solver/src/srrg_solver/variables_and_factors/types_sfm/variable_so3.cpp: -------------------------------------------------------------------------------- 1 | #include "variable_so3.h" 2 | #include 3 | 4 | namespace srrg2_solver { 5 | using namespace srrg2_core; 6 | void 7 | VariableSO3Relaxed::applyPerturbation(const VariableSO3Relaxed::PerturbationVectorType& pert) { 8 | this->_updated = true; 9 | auto m = unflatten(pert); 10 | this->_estimate += m; 11 | } 12 | void VariableSO3Relaxed::normalize() { 13 | // Eigen::JacobiSVD svd(this->_estimate, 14 | // Eigen::ComputeFullU|Eigen::ComputeFullV); 15 | // _estimate=svd.matrixU()*svd.matrixV().transpose(); 16 | // Eigen::Matrix3f W=Matrix3f::Identity(); 17 | // if (svd.matrixU().determinant() * svd.matrixV().determinant() < 0.0) { 18 | // W(2, 2) = -1.0; 19 | // } 20 | // this->_estimate=svd.matrixU()*W*svd.matrixV().transpose(); 21 | } 22 | 23 | } // namespace srrg2_solver 24 | -------------------------------------------------------------------------------- /modules/srrg/srrg_cmake_modules/.gitignore: -------------------------------------------------------------------------------- 1 | *~ 2 | *\# 3 | 4 | .cproject 5 | .project 6 | .settings/* 7 | */.settings/* 8 | 9 | -------------------------------------------------------------------------------- /modules/srrg/srrg_cmake_modules/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(srrg_cmake_modules) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | catkin_package( 7 | INCLUDE_DIRS 8 | cmake_modules 9 | ) -------------------------------------------------------------------------------- /modules/srrg/srrg_cmake_modules/ci_scripts/benchmark/find_dl.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | cd ${WS}/src/ 4 | git clone git@gitlab.com:srrg-software/srrg2_executor.git 5 | cd srrg2_executor 6 | git checkout $CI_BUILD_REF_NAME 7 | cd ${WS} 8 | catkin build srrg_cmake_modules --no-deps 9 | catkin build srrg2_executor --no-deps 10 | ${WS}/devel/srrg2_executor/lib/srrg2_executor/auto_dl_finder 11 | export DLC=$(realpath ${WS}/dl.conf) 12 | -------------------------------------------------------------------------------- /modules/srrg/srrg_cmake_modules/ci_scripts/ci_run/srrg2_packages.txt: -------------------------------------------------------------------------------- 1 | srrg2_core 2 | srrg2_state_machine 3 | srrg2_qgl_viewport 4 | srrg2_solver 5 | srrg2_slam_interfaces 6 | srrg2_nw_calibration 7 | srrg2_laser_slam_2d 8 | srrg2_proslam 9 | srrg2_shaslam 10 | srrg2_mpr 11 | srrg2_nicp 12 | srrg2_executor 13 | srrg2_config_visualizer 14 | srrg2_finslam 15 | -------------------------------------------------------------------------------- /modules/srrg/srrg_cmake_modules/ci_scripts/ci_run/start_all_pipelines.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | INPUT_FILE=${1} 4 | if [[ ! -f ${INPUT_FILE} ]]; then 5 | echo "no repos file as first" 6 | exit -1 7 | fi 8 | declare -a PROJECT_NAMES 9 | PROJECT_NAMES=() 10 | 11 | while IFS= read -r line; do 12 | PROJECT_NAMES+=(${line}) 13 | done < ${INPUT_FILE} 14 | 15 | TOKEN=${2} 16 | if [[ -z {PROJECT_NAME} ]]; then 17 | echo "no token given as second argument" 18 | exit -1 19 | fi 20 | 21 | # echo "${PROJECT_NAMES[@]}" 22 | for PROJECT_NAME in "${PROJECT_NAMES[@]}"; do 23 | echo -e "starting ${PROJECT_NAME} CI pipeline" 24 | ./start_pipeline.sh ${PROJECT_NAME} ${TOKEN} 25 | done 26 | -------------------------------------------------------------------------------- /modules/srrg/srrg_cmake_modules/ci_scripts/ci_run/start_pipeline.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | PROJECT_NAME=${1} 4 | 5 | if [[ -z {PROJECT_NAME} ]]; then 6 | echo "set a repo name" 7 | exit -1 8 | fi 9 | 10 | TOKEN=${2} 11 | if [[ -z {PROJECT_NAME} ]]; then 12 | echo "no token given as second argument" 13 | exit -1 14 | fi 15 | 16 | REF_BRANCH='devel' 17 | 18 | echo "create pipeline for ${PROJECT_NAME} on branch ${REF_BRANCH}" 19 | 20 | RES=$(curl --request POST --header "PRIVATE-TOKEN: ${TOKEN}" "https://gitlab.com/api/v4/projects/srrg-software%2F${PROJECT_NAME}/pipeline?ref=${REF_BRANCH}") 21 | 22 | echo $RES 23 | -------------------------------------------------------------------------------- /modules/srrg/srrg_cmake_modules/ci_scripts/install_catkin_workspace.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | #ds check input parameters 4 | if [ "$#" -ne 2 ]; then 5 | echo "ERROR: call as $0 PROJECT_DIRECTORY PROJECT_NAME" 6 | exit -1 7 | fi 8 | 9 | #ds set parameters 10 | PROJECT_DIRECTORY="$1" 11 | PROJECT_NAME="$2" 12 | echo -e "\e[1;96m--------------------------------------------------------------------------------\e[0m" 13 | echo -e "\e[1;96mbash version: ${BASH_VERSION}\e[0m" 14 | 15 | #ds create catkin workspace and link this repository for build with catkin 16 | mkdir -p /root/workspace/src 17 | ln -s "$PROJECT_DIRECTORY" "/root/workspace/src/${PROJECT_NAME}" 18 | 19 | #ds setup test data path (routed through source directory for local compatibility) 20 | mkdir -p /root/source/srrg && mkdir -p /root/source/srrg2 21 | ln -s "$PROJECT_DIRECTORY" "/root/source/srrg2/${PROJECT_NAME}" 22 | 23 | catkin config -w /root/workspace --init --isolate-devel 24 | echo -e "\e[1;96m--------------------------------------------------------------------------------\e[0m" 25 | -------------------------------------------------------------------------------- /modules/srrg/srrg_cmake_modules/ci_scripts/run_executable.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | #ds check input parameters 4 | if [ "$#" -lt 2 ]; then 5 | echo "ERROR: call as $0 PROJECT_NAME EXE_NAME [WORKSPACE_PATH]" 6 | exit -1 7 | fi 8 | 9 | #ds set parameters 10 | PROJECT_NAME="$1" 11 | EXE_NAME="$2" 12 | WORKSPACE_PATH="$3" 13 | if [[ -z ${WORKSPACE_PATH} ]]; then 14 | WORKSPACE_PATH=${WS} 15 | fi 16 | 17 | if [[ -z ${EXE_NAME} ]]; then 18 | echo -e "\e[1;91mEXE_NAME is empty\e[0m" 19 | exit -1 20 | fi 21 | 22 | echo "" 23 | EXE="${WORKSPACE_PATH}/devel/${PROJECT_NAME}/lib/${PROJECT_NAME}/${EXE_NAME}" 24 | echo -e "\e[1;96m${EXE}\e[0m" 25 | ${EXE} 26 | -------------------------------------------------------------------------------- /modules/srrg/srrg_cmake_modules/cmake_modules/FindEigen3.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find Eigen3 lib 2 | # 3 | # This module supports requiring a minimum version, e.g. you can do 4 | # find_package(Eigen3 3.1.2) 5 | # to require version 3.1.2 or newer of Eigen3. 6 | # 7 | # Once done this will define 8 | # 9 | # EIGEN3_FOUND - system has eigen lib with correct version 10 | # EIGEN3_INCLUDE_DIR - the eigen include directory 11 | # EIGEN3_VERSION - eigen version 12 | 13 | # Copyright (c) 2006, 2007 Montel Laurent, 14 | # Copyright (c) 2008, 2009 Gael Guennebaud, 15 | # Copyright (c) 2009 Benoit Jacob 16 | # Redistribution and use is allowed according to the terms of the 2-clause BSD license. 17 | 18 | find_path(EIGEN3_INCLUDE_DIR signature_of_eigen3_matrix_library 19 | PATHS 20 | /usr/include/eigen3 21 | /usr/local/include/eigen3 22 | ) 23 | -------------------------------------------------------------------------------- /modules/srrg/srrg_cmake_modules/cmake_modules/FindFlann.cmake: -------------------------------------------------------------------------------- 1 | ############################################################################### 2 | # Find Flann 3 | # 4 | # This sets the following variables: 5 | # FLANN_FOUND - True if FLANN was found. 6 | # FLANN_INCLUDE_DIRS - Directories containing the FLANN include files. 7 | # FLANN_LIBRARIES - Libraries needed to use FLANN. 8 | # FLANN_DEFINITIONS - Compiler flags for FLANN. 9 | 10 | find_package(PkgConfig) 11 | pkg_check_modules(PC_FLANN flann) 12 | set(FLANN_DEFINITIONS ${PC_FLANN_CFLAGS_OTHER}) 13 | 14 | find_path(FLANN_INCLUDE_DIR flann/flann.hpp 15 | HINTS ${PC_FLANN_INCLUDEDIR} ${PC_FLANN_INCLUDE_DIRS}) 16 | 17 | find_library(FLANN_LIBRARY flann 18 | HINTS ${PC_FLANN_LIBDIR} ${PC_FLANN_LIBRARY_DIRS}) 19 | 20 | set(FLANN_INCLUDE_DIRS ${FLANN_INCLUDE_DIR}) 21 | set(FLANN_LIBRARIES ${FLANN_LIBRARY}) 22 | 23 | include(FindPackageHandleStandardArgs) 24 | find_package_handle_standard_args(Flann DEFAULT_MSG 25 | FLANN_LIBRARY FLANN_INCLUDE_DIR) 26 | 27 | mark_as_advanced(FLANN_LIBRARY FLANN_INCLUDE_DIR) 28 | -------------------------------------------------------------------------------- /modules/srrg/srrg_cmake_modules/cmake_modules/FindLibWEBSOCKETS.cmake: -------------------------------------------------------------------------------- 1 | FIND_PATH ( LIBWEBSOCKETS_INCLUDE_DIR libwebsockets.h 2 | /usr/local/include 3 | /usr/include 4 | ) 5 | 6 | FIND_LIBRARY ( LIBWEBSOCKETS_LIBRARIES websockets 7 | /usr/local/lib 8 | /usr/lib 9 | ) 10 | 11 | GET_FILENAME_COMPONENT( LIBWEBSOCKETS_LIBRARY_DIR ${LIBWEBSOCKETS_LIBRARIES} PATH ) 12 | 13 | SET ( LIBWEBSOCKETS_FOUND "NO" ) 14 | IF ( LIBWEBSOCKETS_INCLUDE_DIR ) 15 | IF ( LIBWEBSOCKETS_LIBRARIES ) 16 | SET ( LIBWEBSOCKETS_FOUND "YES" ) 17 | ENDIF ( LIBWEBSOCKETS_LIBRARIES ) 18 | ENDIF ( LIBWEBSOCKETS_INCLUDE_DIR ) 19 | 20 | MARK_AS_ADVANCED( 21 | LIBWEBSOCKETS_LIBRARY_DIR 22 | LIBWEBSOCKETS_INCLUDE_DIR 23 | LIBWEBSOCKETS_LIBRARIES 24 | ) 25 | 26 | 27 | set(LibWEBSOCKETS_INCLUDE_DIRS ${LIBWEBSOCKETS_INCLUDE_DIR}) 28 | set(LibWEBSOCKETS_LIBRARIES ${LIBWEBSOCKETS_LIBRARIES}) 29 | set(LibWEBSOCKETS_LIBRARY_DIR ${LIBWEBSOCKETS_LIBRARY_DIR}) 30 | set(LibWEBSOCKETS_FOUND ${LIBWEBSOCKETS_FOUND}) 31 | 32 | -------------------------------------------------------------------------------- /modules/surfel_cloud_rviz_plugin/README.md: -------------------------------------------------------------------------------- 1 | Surfel Cloud RViz Plugin 2 | ------------------------ 3 | 4 | Plugin for the ROS (Robot Operating System) visualizer RViz, to display surfel clouds. 5 | Surfel clouds are point clouds where each point has a normal and a radius, i.e., it represents a small oriented disk. 6 | 7 | This plugin adds a new display type, named `SurfelCloud`. 8 | 9 | This plugin is an extension of the built-in PointCloud2 plugin, and likewise it receives messages of type `sensor_msgs/PointCloud2`. 10 | However, a new display style is present, named `Surfels`, selectable using the `Style` drop-down menu. 11 | 12 | The fields `normal_x`, `normal_y`, `normal_z` and `radius` must be present in the point cloud. This requirement is compatible with the `pcl::PointSurfel` point type from PCL (Point Cloud Library). 13 | 14 | The `surfel_cloud_rviz_plugin` was tested on ROS Kinetic and Melodic. 15 | 16 | 2019-08-08 -------------------------------------------------------------------------------- /modules/surfel_cloud_rviz_plugin/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | surfel_cloud_rviz_plugin 4 | 0.0.0 5 | The surfel_cloud_rviz_plugin package 6 | 7 | rimlab 8 | 9 | TODO 10 | 11 | catkin 12 | roscpp 13 | rviz 14 | sensor_msgs 15 | roslib 16 | roscpp 17 | rviz 18 | sensor_msgs 19 | roslib 20 | roscpp 21 | rviz 22 | sensor_msgs 23 | roslib 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /modules/surfel_cloud_rviz_plugin/plugin_description.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | Displays a surfel cloud from sensor_msgs/PointCloud2 messages. 7 | 8 | sensor_msgs/PointCloud2 9 | 10 | 11 | -------------------------------------------------------------------------------- /modules/surfel_cloud_rviz_plugin/shaders/depth_circle.frag: -------------------------------------------------------------------------------- 1 | #version 120 2 | 3 | // Draws a circle with the packed depth value 4 | 5 | // includes 6 | vec4 packDepth( ); 7 | void circleImpl( ); 8 | 9 | void main() 10 | { 11 | circleImpl(); 12 | 13 | gl_FragColor = packDepth(); 14 | } 15 | -------------------------------------------------------------------------------- /modules/surfel_cloud_rviz_plugin/shaders/flat_color_circle.frag: -------------------------------------------------------------------------------- 1 | #version 120 2 | 3 | // Draws a circle in the fragment color, multiplying a with the alpha param 4 | 5 | uniform vec4 highlight; 6 | uniform float alpha; 7 | 8 | void circleImpl(); 9 | 10 | void main() 11 | { 12 | circleImpl(); 13 | vec3 col = gl_Color.xyz + gl_Color.xyz * highlight.xyz; 14 | gl_FragColor = vec4(col, alpha * gl_Color.a); 15 | } 16 | -------------------------------------------------------------------------------- /modules/surfel_cloud_rviz_plugin/shaders/include/circle_impl.frag: -------------------------------------------------------------------------------- 1 | #version 120 2 | 3 | varying vec3 position; 4 | varying vec3 normal; 5 | varying float sqr_radius; 6 | varying float billboard_half_side; 7 | 8 | void circleImpl() 9 | { 10 | vec2 billboard_d = (gl_PointCoord.st - vec2(0.5f)) * vec2(2.0f,-2.0f) * billboard_half_side; 11 | vec3 billboard_pos = position; 12 | billboard_pos.xy += billboard_d; 13 | vec3 l = normalize(billboard_pos); 14 | if (abs(dot(l,normal)) < 0.005f) 15 | discard; 16 | 17 | vec3 cpos = (dot(position, normal) / dot(l, normal)) * l; 18 | 19 | vec3 diff = cpos - position; 20 | if (dot(diff, diff) > sqr_radius) 21 | discard; 22 | } 23 | -------------------------------------------------------------------------------- /modules/surfel_cloud_rviz_plugin/shaders/include/pack_depth.frag: -------------------------------------------------------------------------------- 1 | #version 120 2 | 3 | // Splits up a normalized depth value in the range (0..1) 4 | // into the vertex RGB values. 5 | // Alpha values below 1/255 are rendered transparent. 6 | 7 | uniform float alpha; 8 | uniform float far_clip_distance; 9 | 10 | const float minimum_alpha = 1.0 / 255.0; 11 | 12 | varying float depth; 13 | 14 | vec4 packDepth() 15 | { 16 | float normalized_depth = depth / far_clip_distance; 17 | 18 | // split up float into rgb components 19 | const vec3 shift = vec3(256.0 * 256.0, 256.0, 1.0); 20 | const vec3 mask = vec3(0.0, 1.0 / 256.0, 1.0 / 256.0); 21 | vec3 depth_packed = fract(normalized_depth * shift); 22 | depth_packed -= depth_packed.xxy * mask; 23 | 24 | return vec4( depth_packed.zyx, step( minimum_alpha, alpha )); 25 | } 26 | 27 | -------------------------------------------------------------------------------- /modules/surfel_cloud_rviz_plugin/shaders/include/pass_depth.vert: -------------------------------------------------------------------------------- 1 | #version 120 2 | 3 | uniform mat4 worldview_matrix; 4 | 5 | varying float depth; 6 | 7 | void passDepth( vec4 pos ) 8 | { 9 | vec4 pos_rel_view = worldview_matrix * pos; 10 | depth = -pos_rel_view.z; 11 | } 12 | -------------------------------------------------------------------------------- /modules/surfel_cloud_rviz_plugin/shaders/pass_color_circle.frag: -------------------------------------------------------------------------------- 1 | #version 120 2 | 3 | // Draws a circle in the fragment color 4 | 5 | varying vec3 normal; 6 | 7 | //includes: 8 | void circleImpl(); 9 | 10 | void main() 11 | { 12 | circleImpl(); 13 | gl_FragColor = gl_Color; 14 | } 15 | -------------------------------------------------------------------------------- /modules/surfel_cloud_rviz_plugin/shaders/pickcolor_circle.frag: -------------------------------------------------------------------------------- 1 | #version 120 2 | 3 | // Draws a circle in the pick color 4 | 5 | //includes: 6 | void circleImpl(); 7 | 8 | uniform vec4 pick_color; 9 | 10 | void main() 11 | { 12 | circleImpl(); 13 | gl_FragColor = pick_color; 14 | } 15 | --------------------------------------------------------------------------------