From 6a809284e9045185bd993191fe5db3bfff416d24 Mon Sep 17 00:00:00 2001 From: Nathan Hughes Date: Fri, 23 Jun 2023 15:48:48 -0400 Subject: [PATCH] Feature/reorg (#170) * (wip) start moving core hydra files * (wip) rename package in code * finish moving core package files around * rename frontend class * rename backend class * rename lcd class * misc other renaming for configs * (wip) start moving hydra topology into hydra * fully drop hydra_topology package * rework reconstruction vs places namespace a little * move non-ros hydra utils to hydra package * get hydra ros code building again * move last of hydra_utils file into hydra_ros * move topology configs * drop config specializations for each enum * make hydra unit tests compile again * drop last ros package call * get hydra ros tests to compile * fix ros unit tests * (wip) fix config test resource filepaths * (wip) update lcd paths * (wip) fix loop closure tests with new convention * rework packages in launch files * update clang format file * reformat * update licenses and add dev scripts * fix cmake format * (wip) drop hydra msg defs from non-ros code * (wip) update cmake a little * adapt to frontend changes * (wip) fix ros compilation issues * address comments from Yun * minor cleanup to ros interface Co-authored-by: Nathan Hughes --- .clang-format | 98 +----- README.md | 7 +- dev/check-code.sh | 8 + dev/check-licenses.sh | 10 + dev/check_license.py | 73 ++++ dev/cpp-header.txt | 34 ++ dev/python-header.txt | 32 ++ dev/reformat.sh | 11 + {hydra_utils/doc => doc}/config_parsing.md | 0 {hydra_dsg_builder => hydra}/.gitignore | 0 hydra/CMakeLists.txt | 100 ++++++ {hydra_dsg_builder => hydra}/README.md | 0 hydra/cmake/nanoflann.CMakeLists.txt.in | 15 + .../config/ade20k_mp3d_typology.yaml | 0 .../config/ade20k_typology.yaml | 0 .../config/d455/d455_typology.yaml | 0 .../config/d455/dsg_backend_config.yaml | 0 .../config/d455/dsg_frontend_config.yaml | 0 .../config/d455/dsg_lcd_config.yaml | 0 .../config/default_reconstruction_config.yaml | 0 .../default_robot/dsg_backend_config.yaml | 0 .../default_robot/dsg_frontend_config.yaml | 0 .../default_robot/dsg_gnn_lcd_config.yaml | 0 .../config/default_robot/dsg_lcd_config.yaml | 0 .../config/extrinsics/mp3d.yaml | 0 .../config/extrinsics/sidpac.yaml | 0 .../config/extrinsics/sparkal1.yaml | 0 .../config/extrinsics/sparkal2.yaml | 0 .../config/extrinsics/uhumans2.yaml | 0 .../config/habitat/dsg_backend_config.yaml | 0 .../config/habitat/dsg_frontend_config.yaml | 0 .../config/habitat/dsg_lcd_config.yaml | 0 .../config/habitat/mp3d_typology.yaml | 0 .../config/habitat/reconstruction_config.yaml | 0 .../config/sidpac/dsg_backend_config.yaml | 0 .../config/sidpac/dsg_frontend_config.yaml | 0 .../config/sidpac/dsg_gnn_lcd_config.yaml | 0 .../config/sidpac/dsg_lcd_config.yaml | 0 .../config/sidpac/reconstruction_config.yaml | 0 .../config/sidpac/sidpac_typology.yaml | 0 .../config/simmons_a1/dsg_backend_config.yaml | 0 .../simmons_a1/dsg_frontend_config.yaml | 0 .../config/simmons_a1/dsg_gnn_lcd_config.yaml | 0 .../config/simmons_a1/dsg_lcd_config.yaml | 0 .../simmons_a1/reconstruction_config.yaml | 0 .../simmons_jackal/dsg_backend_config.yaml | 0 .../simmons_jackal/dsg_frontend_config.yaml | 0 .../simmons_jackal/dsg_gnn_lcd_config.yaml | 0 .../config/simmons_jackal/dsg_lcd_config.yaml | 0 .../simmons_jackal/reconstruction_config.yaml | 0 .../uhumans2/apartment_lcd_embeddings.yaml | 0 .../config/uhumans2/dsg_backend_config.yaml | 0 .../config/uhumans2/dsg_frontend_config.yaml | 0 .../config/uhumans2/dsg_gnn_lcd_config.yaml | 0 .../config/uhumans2/dsg_lcd_config.yaml | 0 .../uhumans2/office_lcd_embeddings.yaml | 0 .../uhumans2/reconstruction_config.yaml | 0 .../uhumans2/subway_lcd_embeddings.yaml | 0 .../uhumans2/uhumans2_apartment_typology.yaml | 0 .../uhumans2/uhumans2_office_typology.yaml | 0 .../uhumans2/uhumans2_subway_typology.yaml | 0 .../hydra_build_config.h.in | 0 .../include/hydra/backend}/backend_config.h | 19 +- .../include/hydra/backend/backend_module.h | 44 ++- .../include/hydra/backend}/merge_handler.h | 4 +- .../include/hydra/backend/update_functions.h | 29 +- .../include/hydra/common/common.h | 12 +- hydra/include/hydra/common/dsg_types.h | 45 +++ .../include/hydra/common}/hydra_config.h | 0 .../include/hydra/common}/input_queue.h | 0 .../hydra/common}/robot_prefix_config.h | 0 .../hydra/common}/shared_module_state.h | 12 +- .../include/hydra/config}/config.h | 81 +---- .../include/hydra/config}/config_formatter.h | 4 +- .../include/hydra/config}/config_parser.h | 6 +- .../include/hydra/config}/config_traits.h | 3 + .../include/hydra/config}/config_visitor.h | 4 +- .../hydra/config}/eigen_config_types.h | 26 +- .../include/hydra/config}/ostream_formatter.h | 10 +- .../include/hydra/config}/yaml_parser.h | 17 +- .../include/hydra/frontend}/frontend_config.h | 18 +- .../include/hydra/frontend/frontend_module.h | 41 +-- .../include/hydra/frontend/mesh_segmenter.h | 5 +- .../include/hydra}/gnn/gnn_interface.h | 4 +- .../include/hydra}/gnn/ort_utilities.h | 4 +- .../include/hydra}/gnn/tensor.h | 1 - .../hydra/loop_closure/descriptor_matching.h | 2 +- .../include/hydra/loop_closure/detector.h | 14 +- .../hydra/loop_closure/gnn_descriptors.h | 4 +- .../hydra/loop_closure/loop_closure_config.h | 19 +- .../hydra/loop_closure/loop_closure_module.h | 33 +- .../include/hydra/loop_closure/registration.h | 6 +- .../loop_closure/scene_graph_descriptors.h | 5 +- .../hydra/loop_closure}/subgraph_extraction.h | 2 +- .../places}/compression_graph_extractor.h | 8 +- .../hydra/places}/floodfill_graph_extractor.h | 12 +- .../hydra/places}/graph_extractor_config.h | 4 +- .../hydra/places}/graph_extractor_interface.h | 15 +- .../hydra/places}/graph_extractor_types.h | 9 +- .../hydra/places}/graph_extractor_utilities.h | 15 +- .../include/hydra/places}/gvd_graph.h | 8 +- .../include/hydra/places}/gvd_integrator.h | 19 +- .../hydra/places}/gvd_integrator_config.h | 10 +- .../hydra/places}/gvd_parent_tracker.h | 8 +- .../include/hydra/places}/gvd_thinning.h | 4 +- .../include/hydra/places}/gvd_utilities.h | 10 +- .../include/hydra/places}/gvd_voxel.h | 21 +- .../hydra/places/nearest_voxel_utilities.h | 32 +- .../include/hydra/places}/update_statistics.h | 4 +- hydra/include/hydra/places/vertex_voxel.h | 49 +++ .../include/hydra/places}/voxblox_types.h | 4 +- .../hydra/reconstruction}/combo_integrator.h | 8 +- .../include/hydra/reconstruction}/configs.h | 26 +- .../reconstruction}/reconstruction_config.h | 10 +- .../reconstruction}/reconstruction_module.h | 27 +- .../reconstruction}/reconstruction_output.h | 19 +- .../hydra/reconstruction/voxblox_utilities.h | 28 +- .../voxel_aware_marching_cubes.h | 19 +- .../voxel_aware_mesh_integrator.h | 49 ++- .../include/hydra/rooms}/graph_clustering.h | 2 +- .../include/hydra/rooms}/graph_filtration.h | 5 +- .../include/hydra/rooms}/room_finder.h | 11 +- .../include/hydra/rooms}/room_finder_config.h | 2 +- .../include/hydra/rooms/room_utilities.h | 4 +- .../include/hydra/utils}/disjoint_set.h | 4 +- .../include/hydra/utils/display_utilities.h | 4 +- .../include/hydra/utils}/log_utilities.h | 0 .../hydra/utils}/minimum_spanning_tree.h | 2 +- .../hydra/utils/nearest_neighbor_utilities.h | 41 ++- .../include/hydra/utils}/timing_utilities.h | 0 .../models/.gitignore | 0 .../models/lcd/.gitignore | 0 .../models/lcd/object_gnn.onnx | Bin .../models/lcd/place_gnn.onnx | Bin .../output/.gitignore | 0 .../output/default/.gitignore | 0 .../output/default/backend/.gitignore | 0 .../output/default/frontend/.gitignore | 0 .../output/default/lcd/.gitignore | 0 .../output/default/pgmo/.gitignore | 0 .../output/default/topology/.gitignore | 0 .../output/sidpac/.gitignore | 0 .../output/sidpac/backend/.gitignore | 0 .../output/sidpac/frontend/.gitignore | 0 .../output/sidpac/lcd/.gitignore | 0 .../output/sidpac/pgmo/.gitignore | 0 .../output/sidpac/topology/.gitignore | 0 .../output/uhumans2/.gitignore | 0 .../output/uhumans2/backend/.gitignore | 0 .../output/uhumans2/frontend/.gitignore | 0 .../output/uhumans2/lcd/.gitignore | 0 .../output/uhumans2/pgmo/.gitignore | 0 .../output/uhumans2/topology/.gitignore | 0 {hydra_dsg_builder => hydra}/package.xml | 4 +- .../scripts/export_label_embeddings.py | 33 ++ hydra/scripts/make_colormap.py | 60 ++++ .../scripts/modify_dgraph.py | 0 hydra/src/CMakeLists.txt | 53 +++ .../src/backend/backend_module.cpp | 97 +++--- .../src/backend}/merge_handler.cpp | 6 +- .../src/backend/update_functions.cpp | 11 +- .../src => hydra/src/common}/hydra_config.cpp | 2 +- .../src/common}/robot_prefix_config.cpp | 2 +- .../src/common}/shared_module_state.cpp | 4 +- .../src => hydra/src/config}/yaml_parser.cpp | 7 +- .../src/frontend/frontend_module.cpp | 161 ++++----- .../src/frontend/mesh_segmenter.cpp | 7 +- .../src/gnn/CMakeLists.txt | 6 +- .../src/gnn/cmake/ort.CMakeLists.txt.in | 3 +- .../src/gnn/gnn_interface.cpp | 5 +- .../src/gnn/gnn_model_reader.cpp | 5 +- .../src/gnn/ort_utilities.cpp | 2 +- .../src/gnn/tensor.cpp | 2 +- .../gnn/tests/resources/make_simple_model.py | 84 +++++ .../src/gnn/tests/resources/simple_model.onnx | Bin .../src/gnn/tests/utest_gnn_interface.cpp | 6 +- .../src/gnn/tests/utest_main.cpp | 0 .../src/gnn/tests/utest_tensor.cpp | 4 +- .../src/loop_closure/descriptor_matching.cpp | 2 +- .../src/loop_closure/detector.cpp | 56 ++-- .../src/loop_closure/gnn_descriptors.cpp | 2 +- .../src/loop_closure/loop_closure_module.cpp | 42 ++- .../src/loop_closure/registration.cpp | 7 +- .../loop_closure/scene_graph_descriptors.cpp | 2 +- .../src/loop_closure}/subgraph_extraction.cpp | 2 +- .../places}/compression_graph_extractor.cpp | 24 +- .../src/places}/floodfill_graph_extractor.cpp | 11 +- .../src/places}/graph_extractor_interface.cpp | 9 +- .../src/places}/graph_extractor_types.cpp | 6 +- .../src/places}/graph_extractor_utilities.cpp | 10 +- .../src => hydra/src/places}/gvd_graph.cpp | 6 +- .../src/places}/gvd_integrator.cpp | 24 +- .../src/places}/gvd_parent_tracker.cpp | 9 +- .../src => hydra/src/places}/gvd_thinning.cpp | 8 +- .../src/places}/gvd_utilities.cpp | 6 +- .../src => hydra/src/places}/gvd_voxel.cpp | 6 +- .../src/places/nearest_voxel_utilities.cpp | 159 +-------- .../src/places}/update_statistics.cpp | 6 +- .../reconstruction}/reconstruction_config.cpp | 2 +- .../reconstruction}/reconstruction_module.cpp | 105 +++--- .../voxel_aware_marching_cubes.cpp | 11 +- .../voxel_aware_mesh_integrator.cpp | 20 +- .../src/rooms}/graph_clustering.cpp | 2 +- .../src/rooms}/graph_filtration.cpp | 2 +- .../src => hydra/src/rooms}/room_finder.cpp | 36 +- .../src/rooms/room_utilities.cpp | 2 +- .../src => hydra/src/utils}/disjoint_set.cpp | 2 +- .../src/utils/display_utilities.cpp | 6 +- .../src => hydra/src/utils}/log_utilities.cpp | 6 +- .../src/utils}/minimum_spanning_tree.cpp | 5 +- .../src/utils/nearest_neighbor_utilities.cpp | 112 +++++++ .../src/utils}/timing_utilities.cpp | 2 +- hydra/tests/CMakeLists.txt | 43 +++ .../tests/backend/test_merge_handler.cpp | 5 +- .../tests/backend/test_update_functions.cpp | 3 +- .../tests/config/test_config.cpp | 119 +------ .../tests/frontend/test_frontend.cpp | 23 +- .../include/hydra_test/gvd_layer_utils.h | 16 +- .../tests/include/hydra_test/place_fixtures.h | 12 +- hydra/tests/include/hydra_test/resources.h | 48 +++ .../loop_closure/test_descriptor_matching.cpp | 35 +- .../tests/loop_closure/test_detector.cpp | 41 ++- .../loop_closure/test_gnn_descriptors.cpp | 44 ++- .../tests/loop_closure/test_registration.cpp | 17 +- .../test_scene_graph_descriptors.cpp | 11 +- .../loop_closure/test_subgraph_extraction.cpp | 3 +- .../utest_main.cpp => hydra/tests/main.cpp | 0 .../test_compression_graph_extractor.cpp | 13 +- .../tests/places/test_esdf_comparison.cpp | 26 +- .../places/test_floodfill_graph_extractor.cpp | 14 +- .../places/test_graph_extractor_utilities.cpp | 13 +- .../tests/places/test_gvd_incremental.cpp | 23 +- .../tests/places/test_gvd_integrator.cpp | 16 +- .../tests/places/test_gvd_thinning.cpp | 6 +- .../tests/places/test_gvd_utilities.cpp | 6 +- .../reconstruction/test_marching_cubes.cpp | 7 +- .../test_reconstruction_module.cpp | 5 +- .../tests/resources/config}/map_config.yaml | 0 .../resources/config}/missing_config.yaml | 1 - .../resources/config}/nested_test_config.yaml | 0 .../tests/resources/config}/test_config.yaml | 0 .../loop_closure}/make_lcd_models.py | 32 ++ .../resources/loop_closure}/objects.onnx | Bin .../resources/loop_closure}/objects_pos.onnx | Bin .../tests/resources/loop_closure}/places.onnx | Bin .../resources/loop_closure}/places_pos.onnx | Bin .../loop_closure}/test_embeddings.yaml | 0 hydra/tests/resources/test_semantic_map.csv | 22 ++ .../tests/rooms/test_graph_clustering.cpp | 3 +- .../tests/rooms/test_graph_filtration.cpp | 2 +- .../tests/rooms/test_room_finder.cpp | 3 +- .../tests/rooms/test_room_finder_config.cpp | 5 +- .../tests/rooms/test_room_utilities.cpp | 3 +- .../tests/src/place_fixtures.cpp | 19 +- hydra/tests/src/resources.cpp | 55 +++ .../utils/test_minimum_spanning_tree.cpp | 3 +- .../utils/test_nearest_neighbor_utilities.cpp | 5 +- .../tests/utils/test_timing_utilities.cpp | 3 +- hydra_dsg_builder/CMakeLists.txt | 134 -------- .../uh2_office_label_conversion.yaml | 56 ---- hydra_dsg_builder/scripts/make_colormap.py | 28 -- hydra_dsg_builder/scripts/setup_logs.sh | 7 - .../gnn/tests/resources/make_simple_model.py | 52 --- hydra_dsg_builder_ros/CMakeLists.txt | 61 ---- .../hydra_dsg_builder_ros/lcd_visualizer.h | 96 ------ hydra_dsg_builder_ros/package.xml | 26 -- hydra_dsg_builder_ros/src/lcd_visualizer.cpp | 315 ------------------ .../.gitignore | 0 hydra_ros/CMakeLists.txt | 127 +++++++ hydra_ros/README.md | 10 + .../nodes => hydra_ros/bin}/csv_to_tf | 0 .../bin/gnn_node | 0 .../nodes => hydra_ros/bin}/odom_to_tf | 0 {hydra_utils => hydra_ros}/cfg/Colormap.cfg | 2 +- .../cfg/DynamicLayerVisualizer.cfg | 2 +- .../cfg/GvdVisualizer.cfg | 2 +- .../cfg/LayerVisualizer.cfg | 2 +- {hydra_utils => hydra_ros}/cfg/Visualizer.cfg | 2 +- .../config/graph_visualization_config.yaml | 0 .../config/gvd_validator}/uh2.yaml | 0 .../config/hydra_visualizer/agent_layer.yaml | 0 .../hydra_visualizer/buildings_layer.yaml | 0 .../hydra_visualizer/objects_layer.yaml | 0 .../config/hydra_visualizer/places_layer.yaml | 0 .../config/hydra_visualizer/rooms_layer.yaml | 0 .../config/hydra_visualizer/visualizer.yaml | 0 .../config/object_visualizer/agent_layer.yaml | 0 .../object_visualizer/buildings_layer.yaml | 0 .../object_visualizer/objects_layer.yaml | 0 .../object_visualizer/places_layer.yaml | 0 .../config/object_visualizer/rooms_layer.yaml | 0 .../config/object_visualizer/visualizer.yaml | 0 .../config/overrides.yaml | 0 .../config/ros_params.yaml | 0 .../hydra_utils_cmake | 42 --- .../include/hydra_ros/config}/ros_parser.h | 16 +- .../include/hydra_ros/config}/ros_utilities.h | 67 +++- .../hydra_ros/pipeline}/hydra_ros_pipeline.h | 20 +- .../include/hydra_ros/pipeline}/ros_backend.h | 16 +- .../hydra_ros/pipeline}/ros_frontend.h | 16 +- .../pipeline}/ros_lcd_registration.h | 2 +- .../hydra_ros/pipeline}/ros_reconstruction.h | 10 +- .../utils}/dsg_streaming_interface.h | 3 +- .../include/hydra_ros/utils}/node_utilities.h | 2 +- .../utils}/semantic_ros_publishers.h | 0 .../hydra_ros/visualizer/colormap_utilities.h | 4 +- .../hydra_ros/visualizer}/dsg_mesh_plugins.h | 4 +- .../visualizer}/dsg_visualizer_plugin.h | 2 +- .../dynamic_scene_graph_visualizer.h | 7 +- .../visualizer}/gvd_visualization_utilities.h | 57 ++-- .../visualizer}/topology_server_visualizer.h | 45 ++- .../visualizer}/visualizer_plugins.h | 9 +- .../hydra_ros/visualizer}/visualizer_types.h | 19 +- .../visualizer/visualizer_utilities.h | 4 +- .../launch/datasets}/mp3d.launch | 10 +- .../launch/datasets}/sidpac.launch | 14 +- .../launch/datasets}/simmons_a1.launch | 12 +- .../launch/datasets}/simmons_jackal.launch | 12 +- .../launch/datasets}/uhumans2.launch | 14 +- .../launch/hydra.launch | 26 +- .../launch/hydra_robot.launch | 12 +- .../launch/hydra_robot_t265.launch | 16 +- .../launch/misc}/dsg_optimizer.launch | 10 +- .../launch/places}/gvd_validator.launch | 0 .../launch/places}/hydra_topology.launch | 0 .../places}/hydra_topology_sidpac.launch | 8 +- .../hydra_topology_simmons_jackal.launch | 8 +- .../places}/hydra_topology_uhumans2.launch | 8 +- .../utils}/data_sources/a1_pointcloud.launch | 4 +- .../data_sources/d455_pointcloud.launch | 8 +- .../data_sources/jackal_pointcloud.launch | 4 +- .../data_sources/sidpac_pointcloud.launch | 4 +- .../data_sources/uhumans2_pointcloud.launch | 4 +- .../utils}/gt_pose/robot_gt_pose.launch | 4 +- .../utils}/gt_pose/sidpac_gt_pose.launch | 2 +- .../utils}/hydra_streaming_visualizer.launch | 18 +- .../launch/utils}/hydra_visualizer.launch | 8 +- .../launch/utils}/includes/a1_static_tfs.xml | 0 .../includes/a1_t265_camera_static_tfs.xml | 0 .../utils}/includes/a1_t265_static_tfs.xml | 2 +- .../utils}/includes/hydra_visualizer_base.xml | 6 +- .../includes/hydra_visualizer_params.xml | 0 .../utils}/includes/hydra_visualizer_rviz.xml | 2 +- .../utils}/includes/rgbd_to_pointcloud.xml | 0 .../utils}/includes/sparkal1_static_tfs.xml | 0 .../utils}/includes/sparkal2_static_tfs.xml | 0 .../utils}/includes/uhumans2_static_tfs.xml | 0 .../launch/utils}/odom_to_tf.launch | 2 +- .../utils}/publish_rotation_transform.launch | 2 +- .../launch/utils}/scene_graph_logger.launch | 6 +- .../nodelet_plugins.xml | 0 {hydra_utils => hydra_ros}/package.xml | 28 +- .../resources/a1_center_mask_roi_640.png | Bin .../resources/a1_left_mask_roi.png | Bin .../rviz/a1.rviz | 0 .../rviz/a1_topdown.rviz | 0 .../rviz/dsg_optimizer.rviz | 0 .../rviz/gvd_validator.rviz | 0 .../rviz/hydra_streaming_visualizer.rviz | 0 .../hydra_streaming_visualizer_topdown.rviz | 0 .../rviz/hydra_visualizer.rviz | 0 .../rviz/mp3d.rviz | 0 .../rviz/sidpac.rviz | 0 .../rviz/sidpac_topdown.rviz | 0 .../rviz/sparkal1.rviz | 0 .../rviz/sparkal2.rviz | 0 .../rviz/uhumans2.rviz | 52 +-- .../rviz/uhumans2_topdown.rviz | 0 .../scripts/copy_static_tfs.py | 0 .../scripts/decompress_uhumans2.py | 0 .../scripts/download_uhumans2.py | 0 .../scripts/run_robot.sh | 0 .../scripts/run_robot_t265.sh | 0 hydra_ros/scripts/scrape_kimera_extrinsics.py | 81 +++++ .../src/config}/ros_parser.cpp | 2 +- .../src/nodes}/dsg_optimizer_node.cpp | 23 +- .../src/nodes/gvd_validator_node.cpp | 20 +- .../src/nodes/hydra_node.cpp | 10 +- .../src/nodes}/hydra_topology_node.cpp | 11 +- .../src/nodes}/hydra_visualizer_node.cpp | 13 +- .../src/nodes}/rotate_tf_node.cpp | 0 .../src/nodes}/scene_graph_logger_node.cpp | 6 +- .../src/pipeline}/hydra_ros_pipeline.cpp | 35 +- .../src/pipeline}/ros_backend.cpp | 20 +- .../src/pipeline}/ros_frontend.cpp | 12 +- .../src/pipeline}/ros_lcd_registration.cpp | 4 +- .../src/pipeline}/ros_reconstruction.cpp | 33 +- .../src/utils}/dsg_streaming_interface.cpp | 13 +- .../src/utils}/mask_nodelet.cpp | 0 .../src/utils}/node_utilities.cpp | 4 +- .../src/visualizer/colormap_utilities.cpp | 2 +- .../src/visualizer}/dsg_mesh_plugins.cpp | 5 +- .../dynamic_scene_graph_visualizer.cpp | 9 +- .../gvd_visualization_utilities.cpp | 18 +- .../topology_server_visualizer.cpp | 28 +- .../src/visualizer}/visualizer_plugins.cpp | 9 +- .../src/visualizer/visualizer_utilities.cpp | 24 +- hydra_ros/tests/CMakeLists.txt | 3 + hydra_ros/tests/hydra_ros.test | 15 + .../tests/main.cpp | 4 +- hydra_ros/tests/resources/map_config.yaml | 10 + .../tests/resources/nested_test_config.yaml | 9 + hydra_ros/tests/resources/test_config.yaml | 17 + hydra_ros/tests/test_ros_config.cpp | 282 ++++++++++++++++ hydra_topology/CMakeLists.txt | 148 -------- hydra_topology/README.md | 3 - .../cmake/nanoflann.CMakeLists.txt.in | 14 - hydra_topology/output/.gitignore | 3 - hydra_topology/package.xml | 28 -- hydra_topology/rviz/d455.rviz | 210 ------------ hydra_topology/rviz/sidpac.rviz | 247 -------------- hydra_topology/rviz/uhumans2.rviz | 255 -------------- hydra_utils/README.md | 13 - hydra_utils/config/robots/a1_t265.yaml | 2 - hydra_utils/include/hydra_utils/dsg_types.h | 11 - hydra_utils/launch/visual_lcd.launch | 24 -- hydra_utils/rviz/a1_debug.rviz | 278 ---------------- .../scripts/scrape_kimera_extrinsics.py | 49 --- hydra_utils/tests/hydra_utils.test | 15 - 419 files changed, 2934 insertions(+), 3995 deletions(-) create mode 100755 dev/check-code.sh create mode 100755 dev/check-licenses.sh create mode 100644 dev/check_license.py create mode 100644 dev/cpp-header.txt create mode 100644 dev/python-header.txt create mode 100755 dev/reformat.sh rename {hydra_utils/doc => doc}/config_parsing.md (100%) rename {hydra_dsg_builder => hydra}/.gitignore (100%) create mode 100644 hydra/CMakeLists.txt rename {hydra_dsg_builder => hydra}/README.md (100%) create mode 100644 hydra/cmake/nanoflann.CMakeLists.txt.in rename {hydra_dsg_builder => hydra}/config/ade20k_mp3d_typology.yaml (100%) rename {hydra_dsg_builder => hydra}/config/ade20k_typology.yaml (100%) rename {hydra_dsg_builder => hydra}/config/d455/d455_typology.yaml (100%) rename {hydra_dsg_builder => hydra}/config/d455/dsg_backend_config.yaml (100%) rename {hydra_dsg_builder => hydra}/config/d455/dsg_frontend_config.yaml (100%) rename {hydra_dsg_builder => hydra}/config/d455/dsg_lcd_config.yaml (100%) rename hydra_topology/config/default_topology_config.yaml => hydra/config/default_reconstruction_config.yaml (100%) rename {hydra_dsg_builder => hydra}/config/default_robot/dsg_backend_config.yaml (100%) rename {hydra_dsg_builder => hydra}/config/default_robot/dsg_frontend_config.yaml (100%) rename {hydra_dsg_builder => hydra}/config/default_robot/dsg_gnn_lcd_config.yaml (100%) rename {hydra_dsg_builder => hydra}/config/default_robot/dsg_lcd_config.yaml (100%) rename {hydra_topology => hydra}/config/extrinsics/mp3d.yaml (100%) rename {hydra_topology => hydra}/config/extrinsics/sidpac.yaml (100%) rename {hydra_topology => hydra}/config/extrinsics/sparkal1.yaml (100%) rename {hydra_topology => hydra}/config/extrinsics/sparkal2.yaml (100%) rename {hydra_topology => hydra}/config/extrinsics/uhumans2.yaml (100%) rename {hydra_dsg_builder => hydra}/config/habitat/dsg_backend_config.yaml (100%) rename {hydra_dsg_builder => hydra}/config/habitat/dsg_frontend_config.yaml (100%) rename {hydra_dsg_builder => hydra}/config/habitat/dsg_lcd_config.yaml (100%) rename {hydra_dsg_builder => hydra}/config/habitat/mp3d_typology.yaml (100%) rename {hydra_dsg_builder => hydra}/config/habitat/reconstruction_config.yaml (100%) rename {hydra_dsg_builder => hydra}/config/sidpac/dsg_backend_config.yaml (100%) rename {hydra_dsg_builder => hydra}/config/sidpac/dsg_frontend_config.yaml (100%) rename {hydra_dsg_builder => hydra}/config/sidpac/dsg_gnn_lcd_config.yaml (100%) rename {hydra_dsg_builder => hydra}/config/sidpac/dsg_lcd_config.yaml (100%) rename hydra_topology/config/sidpac_topology_config.yaml => hydra/config/sidpac/reconstruction_config.yaml (100%) rename {hydra_dsg_builder => hydra}/config/sidpac/sidpac_typology.yaml (100%) rename {hydra_dsg_builder => hydra}/config/simmons_a1/dsg_backend_config.yaml (100%) rename {hydra_dsg_builder => hydra}/config/simmons_a1/dsg_frontend_config.yaml (100%) rename {hydra_dsg_builder => hydra}/config/simmons_a1/dsg_gnn_lcd_config.yaml (100%) rename {hydra_dsg_builder => hydra}/config/simmons_a1/dsg_lcd_config.yaml (100%) rename hydra_topology/config/mp3d_topology_config.yaml => hydra/config/simmons_a1/reconstruction_config.yaml (100%) rename {hydra_dsg_builder => hydra}/config/simmons_jackal/dsg_backend_config.yaml (100%) rename {hydra_dsg_builder => hydra}/config/simmons_jackal/dsg_frontend_config.yaml (100%) rename {hydra_dsg_builder => hydra}/config/simmons_jackal/dsg_gnn_lcd_config.yaml (100%) rename {hydra_dsg_builder => hydra}/config/simmons_jackal/dsg_lcd_config.yaml (100%) rename hydra_topology/config/simmons_topology_config.yaml => hydra/config/simmons_jackal/reconstruction_config.yaml (100%) rename {hydra_dsg_builder => hydra}/config/uhumans2/apartment_lcd_embeddings.yaml (100%) rename {hydra_dsg_builder => hydra}/config/uhumans2/dsg_backend_config.yaml (100%) rename {hydra_dsg_builder => hydra}/config/uhumans2/dsg_frontend_config.yaml (100%) rename {hydra_dsg_builder => hydra}/config/uhumans2/dsg_gnn_lcd_config.yaml (100%) rename {hydra_dsg_builder => hydra}/config/uhumans2/dsg_lcd_config.yaml (100%) rename {hydra_dsg_builder => hydra}/config/uhumans2/office_lcd_embeddings.yaml (100%) rename hydra_topology/config/uhumans2_topology_config.yaml => hydra/config/uhumans2/reconstruction_config.yaml (100%) rename {hydra_dsg_builder => hydra}/config/uhumans2/subway_lcd_embeddings.yaml (100%) rename {hydra_dsg_builder => hydra}/config/uhumans2/uhumans2_apartment_typology.yaml (100%) rename {hydra_dsg_builder => hydra}/config/uhumans2/uhumans2_office_typology.yaml (100%) rename {hydra_dsg_builder => hydra}/config/uhumans2/uhumans2_subway_typology.yaml (100%) rename {hydra_dsg_builder => hydra}/hydra_build_config.h.in (100%) rename {hydra_dsg_builder/include/hydra_dsg_builder => hydra/include/hydra/backend}/backend_config.h (95%) rename hydra_dsg_builder/include/hydra_dsg_builder/incremental_dsg_backend.h => hydra/include/hydra/backend/backend_module.h (86%) rename {hydra_dsg_builder/include/hydra_dsg_builder => hydra/include/hydra/backend}/merge_handler.h (97%) rename hydra_dsg_builder/include/hydra_dsg_builder/dsg_update_functions.h => hydra/include/hydra/backend/update_functions.h (82%) rename hydra_dsg_builder/include/hydra_dsg_builder/incremental_types.h => hydra/include/hydra/common/common.h (96%) create mode 100644 hydra/include/hydra/common/dsg_types.h rename {hydra_dsg_builder/include/hydra_dsg_builder => hydra/include/hydra/common}/hydra_config.h (100%) rename {hydra_utils/include/hydra_utils => hydra/include/hydra/common}/input_queue.h (100%) rename {hydra_utils/include/hydra_utils => hydra/include/hydra/common}/robot_prefix_config.h (100%) rename {hydra_dsg_builder/include/hydra_dsg_builder => hydra/include/hydra/common}/shared_module_state.h (93%) rename {hydra_utils/include/hydra_utils => hydra/include/hydra/config}/config.h (62%) rename {hydra_utils/include/hydra_utils => hydra/include/hydra/config}/config_formatter.h (98%) rename {hydra_utils/include/hydra_utils => hydra/include/hydra/config}/config_parser.h (97%) rename {hydra_utils/include/hydra_utils => hydra/include/hydra/config}/config_traits.h (97%) rename {hydra_utils/include/hydra_utils => hydra/include/hydra/config}/config_visitor.h (99%) rename {hydra_utils/include/hydra_utils => hydra/include/hydra/config}/eigen_config_types.h (86%) rename {hydra_utils/include/hydra_utils => hydra/include/hydra/config}/ostream_formatter.h (93%) rename {hydra_utils/include/hydra_utils => hydra/include/hydra/config}/yaml_parser.h (89%) rename {hydra_dsg_builder/include/hydra_dsg_builder => hydra/include/hydra/frontend}/frontend_config.h (93%) rename hydra_dsg_builder/include/hydra_dsg_builder/incremental_dsg_frontend.h => hydra/include/hydra/frontend/frontend_module.h (84%) rename hydra_dsg_builder/include/hydra_dsg_builder/incremental_mesh_segmenter.h => hydra/include/hydra/frontend/mesh_segmenter.h (98%) rename {hydra_dsg_builder/include/hydra_dsg_builder => hydra/include/hydra}/gnn/gnn_interface.h (98%) rename {hydra_dsg_builder/include/hydra_dsg_builder => hydra/include/hydra}/gnn/ort_utilities.h (98%) rename {hydra_dsg_builder/include/hydra_dsg_builder => hydra/include/hydra}/gnn/tensor.h (99%) rename hydra_dsg_builder/include/hydra_dsg_builder/dsg_lcd_matching.h => hydra/include/hydra/loop_closure/descriptor_matching.h (98%) rename hydra_dsg_builder/include/hydra_dsg_builder/dsg_lcd_detector.h => hydra/include/hydra/loop_closure/detector.h (94%) rename hydra_dsg_builder/include/hydra_dsg_builder/lcd_gnn_descriptors.h => hydra/include/hydra/loop_closure/gnn_descriptors.h (97%) rename hydra_dsg_builder/include/hydra_dsg_builder/lcd_module_config.h => hydra/include/hydra/loop_closure/loop_closure_config.h (94%) rename hydra_dsg_builder/include/hydra_dsg_builder/incremental_dsg_lcd.h => hydra/include/hydra/loop_closure/loop_closure_module.h (82%) rename hydra_dsg_builder/include/hydra_dsg_builder/dsg_lcd_registration.h => hydra/include/hydra/loop_closure/registration.h (98%) rename hydra_dsg_builder/include/hydra_dsg_builder/dsg_lcd_descriptors.h => hydra/include/hydra/loop_closure/scene_graph_descriptors.h (97%) rename {hydra_dsg_builder/include/hydra_dsg_builder => hydra/include/hydra/loop_closure}/subgraph_extraction.h (98%) rename {hydra_topology/include/hydra_topology => hydra/include/hydra/places}/compression_graph_extractor.h (97%) rename {hydra_topology/include/hydra_topology => hydra/include/hydra/places}/floodfill_graph_extractor.h (96%) rename {hydra_topology/include/hydra_topology => hydra/include/hydra/places}/graph_extractor_config.h (99%) rename {hydra_topology/include/hydra_topology => hydra/include/hydra/places}/graph_extractor_interface.h (94%) rename {hydra_topology/include/hydra_topology => hydra/include/hydra/places}/graph_extractor_types.h (96%) rename {hydra_topology/include/hydra_topology => hydra/include/hydra/places}/graph_extractor_utilities.h (96%) rename {hydra_topology/include/hydra_topology => hydra/include/hydra/places}/gvd_graph.h (97%) rename {hydra_topology/include/hydra_topology => hydra/include/hydra/places}/gvd_integrator.h (93%) rename {hydra_topology/include/hydra_topology => hydra/include/hydra/places}/gvd_integrator_config.h (96%) rename {hydra_topology/include/hydra_topology => hydra/include/hydra/places}/gvd_parent_tracker.h (97%) rename {hydra_topology/include/hydra_topology => hydra/include/hydra/places}/gvd_thinning.h (98%) rename {hydra_topology/include/hydra_topology => hydra/include/hydra/places}/gvd_utilities.h (96%) rename {hydra_topology/include/hydra_topology => hydra/include/hydra/places}/gvd_voxel.h (91%) rename hydra_topology/include/hydra_topology/nearest_neighbor_utilities.h => hydra/include/hydra/places/nearest_voxel_utilities.h (81%) rename {hydra_topology/include/hydra_topology => hydra/include/hydra/places}/update_statistics.h (98%) create mode 100644 hydra/include/hydra/places/vertex_voxel.h rename {hydra_topology/include/hydra_topology => hydra/include/hydra/places}/voxblox_types.h (98%) rename {hydra_topology/include/hydra_topology => hydra/include/hydra/reconstruction}/combo_integrator.h (96%) rename {hydra_topology/include/hydra_topology => hydra/include/hydra/reconstruction}/configs.h (77%) rename {hydra_topology/include/hydra_topology => hydra/include/hydra/reconstruction}/reconstruction_config.h (97%) rename {hydra_topology/include/hydra_topology => hydra/include/hydra/reconstruction}/reconstruction_module.h (90%) rename {hydra_topology/include/hydra_topology => hydra/include/hydra/reconstruction}/reconstruction_output.h (85%) rename hydra_topology/tests/utest_main.cpp => hydra/include/hydra/reconstruction/voxblox_utilities.h (84%) rename {hydra_topology/include/hydra_topology => hydra/include/hydra/reconstruction}/voxel_aware_marching_cubes.h (85%) rename {hydra_topology/include/hydra_topology => hydra/include/hydra/reconstruction}/voxel_aware_mesh_integrator.h (62%) rename {hydra_dsg_builder/include/hydra_dsg_builder => hydra/include/hydra/rooms}/graph_clustering.h (98%) rename {hydra_dsg_builder/include/hydra_dsg_builder => hydra/include/hydra/rooms}/graph_filtration.h (98%) rename {hydra_dsg_builder/include/hydra_dsg_builder => hydra/include/hydra/rooms}/room_finder.h (95%) rename {hydra_dsg_builder/include/hydra_dsg_builder => hydra/include/hydra/rooms}/room_finder_config.h (99%) rename hydra_dsg_builder/include/hydra_dsg_builder/room_helpers.h => hydra/include/hydra/rooms/room_utilities.h (98%) rename {hydra_dsg_builder/include/hydra_dsg_builder => hydra/include/hydra/utils}/disjoint_set.h (98%) rename hydra_utils/include/hydra_utils/display_utils.h => hydra/include/hydra/utils/display_utilities.h (97%) rename {hydra_utils/include/hydra_utils => hydra/include/hydra/utils}/log_utilities.h (100%) rename {hydra_dsg_builder/include/hydra_dsg_builder => hydra/include/hydra/utils}/minimum_spanning_tree.h (98%) rename hydra_topology/include/hydra_topology/ros_interface.h => hydra/include/hydra/utils/nearest_neighbor_utilities.h (77%) rename {hydra_utils/include/hydra_utils => hydra/include/hydra/utils}/timing_utilities.h (100%) rename {hydra_dsg_builder => hydra}/models/.gitignore (100%) rename {hydra_dsg_builder => hydra}/models/lcd/.gitignore (100%) rename {hydra_dsg_builder => hydra}/models/lcd/object_gnn.onnx (100%) rename {hydra_dsg_builder => hydra}/models/lcd/place_gnn.onnx (100%) rename {hydra_dsg_builder => hydra}/output/.gitignore (100%) rename {hydra_dsg_builder => hydra}/output/default/.gitignore (100%) rename {hydra_dsg_builder => hydra}/output/default/backend/.gitignore (100%) rename {hydra_dsg_builder => hydra}/output/default/frontend/.gitignore (100%) rename {hydra_dsg_builder => hydra}/output/default/lcd/.gitignore (100%) rename {hydra_dsg_builder => hydra}/output/default/pgmo/.gitignore (100%) rename {hydra_dsg_builder => hydra}/output/default/topology/.gitignore (100%) rename {hydra_dsg_builder => hydra}/output/sidpac/.gitignore (100%) rename {hydra_dsg_builder => hydra}/output/sidpac/backend/.gitignore (100%) rename {hydra_dsg_builder => hydra}/output/sidpac/frontend/.gitignore (100%) rename {hydra_dsg_builder => hydra}/output/sidpac/lcd/.gitignore (100%) rename {hydra_dsg_builder => hydra}/output/sidpac/pgmo/.gitignore (100%) rename {hydra_dsg_builder => hydra}/output/sidpac/topology/.gitignore (100%) rename {hydra_dsg_builder => hydra}/output/uhumans2/.gitignore (100%) rename {hydra_dsg_builder => hydra}/output/uhumans2/backend/.gitignore (100%) rename {hydra_dsg_builder => hydra}/output/uhumans2/frontend/.gitignore (100%) rename {hydra_dsg_builder => hydra}/output/uhumans2/lcd/.gitignore (100%) rename {hydra_dsg_builder => hydra}/output/uhumans2/pgmo/.gitignore (100%) rename {hydra_dsg_builder => hydra}/output/uhumans2/topology/.gitignore (100%) rename {hydra_dsg_builder => hydra}/package.xml (87%) rename {hydra_dsg_builder => hydra}/scripts/export_label_embeddings.py (50%) create mode 100644 hydra/scripts/make_colormap.py rename {hydra_dsg_builder => hydra}/scripts/modify_dgraph.py (100%) create mode 100644 hydra/src/CMakeLists.txt rename hydra_dsg_builder/src/incremental_dsg_backend.cpp => hydra/src/backend/backend_module.cpp (91%) rename {hydra_dsg_builder/src => hydra/src/backend}/merge_handler.cpp (99%) rename hydra_dsg_builder/src/dsg_update_functions.cpp => hydra/src/backend/update_functions.cpp (98%) rename {hydra_dsg_builder/src => hydra/src/common}/hydra_config.cpp (98%) rename {hydra_utils/src => hydra/src/common}/robot_prefix_config.cpp (98%) rename {hydra_dsg_builder/src => hydra/src/common}/shared_module_state.cpp (96%) rename {hydra_utils/src => hydra/src/config}/yaml_parser.cpp (95%) rename hydra_dsg_builder/src/incremental_dsg_frontend.cpp => hydra/src/frontend/frontend_module.cpp (82%) rename hydra_dsg_builder/src/incremental_mesh_segmenter.cpp => hydra/src/frontend/mesh_segmenter.cpp (98%) rename {hydra_dsg_builder => hydra}/src/gnn/CMakeLists.txt (89%) rename {hydra_dsg_builder => hydra}/src/gnn/cmake/ort.CMakeLists.txt.in (94%) rename {hydra_dsg_builder => hydra}/src/gnn/gnn_interface.cpp (98%) rename {hydra_dsg_builder => hydra}/src/gnn/gnn_model_reader.cpp (98%) rename {hydra_dsg_builder => hydra}/src/gnn/ort_utilities.cpp (99%) rename {hydra_dsg_builder => hydra}/src/gnn/tensor.cpp (99%) create mode 100644 hydra/src/gnn/tests/resources/make_simple_model.py rename {hydra_dsg_builder => hydra}/src/gnn/tests/resources/simple_model.onnx (100%) rename {hydra_dsg_builder => hydra}/src/gnn/tests/utest_gnn_interface.cpp (96%) rename {hydra_dsg_builder => hydra}/src/gnn/tests/utest_main.cpp (100%) rename {hydra_dsg_builder => hydra}/src/gnn/tests/utest_tensor.cpp (99%) rename hydra_dsg_builder/src/dsg_lcd_matching.cpp => hydra/src/loop_closure/descriptor_matching.cpp (99%) rename hydra_dsg_builder/src/dsg_lcd_detector.cpp => hydra/src/loop_closure/detector.cpp (89%) rename hydra_dsg_builder/src/lcd_gnn_descriptors.cpp => hydra/src/loop_closure/gnn_descriptors.cpp (99%) rename hydra_dsg_builder/src/incremental_dsg_lcd.cpp => hydra/src/loop_closure/loop_closure_module.cpp (87%) rename hydra_dsg_builder/src/dsg_lcd_registration.cpp => hydra/src/loop_closure/registration.cpp (98%) rename hydra_dsg_builder/src/dsg_lcd_descriptors.cpp => hydra/src/loop_closure/scene_graph_descriptors.cpp (99%) rename {hydra_dsg_builder/src => hydra/src/loop_closure}/subgraph_extraction.cpp (99%) rename {hydra_topology/src => hydra/src/places}/compression_graph_extractor.cpp (98%) rename {hydra_topology/src => hydra/src/places}/floodfill_graph_extractor.cpp (98%) rename {hydra_topology/src => hydra/src/places}/graph_extractor_interface.cpp (98%) rename {hydra_topology/src => hydra/src/places}/graph_extractor_types.cpp (97%) rename {hydra_topology/src => hydra/src/places}/graph_extractor_utilities.cpp (98%) rename {hydra_topology/src => hydra/src/places}/gvd_graph.cpp (97%) rename {hydra_topology/src => hydra/src/places}/gvd_integrator.cpp (97%) rename {hydra_topology/src => hydra/src/places}/gvd_parent_tracker.cpp (97%) rename {hydra_topology/src => hydra/src/places}/gvd_thinning.cpp (98%) rename {hydra_topology/src => hydra/src/places}/gvd_utilities.cpp (98%) rename {hydra_topology/src => hydra/src/places}/gvd_voxel.cpp (97%) rename hydra_topology/src/nearest_neighbor_utilities.cpp => hydra/src/places/nearest_voxel_utilities.cpp (52%) rename {hydra_topology/src => hydra/src/places}/update_statistics.cpp (97%) rename {hydra_topology/src => hydra/src/reconstruction}/reconstruction_config.cpp (98%) rename {hydra_topology/src => hydra/src/reconstruction}/reconstruction_module.cpp (86%) rename {hydra_topology/src => hydra/src/reconstruction}/voxel_aware_marching_cubes.cpp (97%) rename {hydra_topology/src => hydra/src/reconstruction}/voxel_aware_mesh_integrator.cpp (96%) rename {hydra_dsg_builder/src => hydra/src/rooms}/graph_clustering.cpp (99%) rename {hydra_dsg_builder/src => hydra/src/rooms}/graph_filtration.cpp (99%) rename {hydra_dsg_builder/src => hydra/src/rooms}/room_finder.cpp (93%) rename hydra_dsg_builder/src/room_helpers.cpp => hydra/src/rooms/room_utilities.cpp (99%) rename {hydra_dsg_builder/src => hydra/src/utils}/disjoint_set.cpp (98%) rename hydra_utils/src/display_utils.cpp => hydra/src/utils/display_utilities.cpp (96%) rename {hydra_utils/src => hydra/src/utils}/log_utilities.cpp (98%) rename {hydra_dsg_builder/src => hydra/src/utils}/minimum_spanning_tree.cpp (97%) create mode 100644 hydra/src/utils/nearest_neighbor_utilities.cpp rename {hydra_utils/src => hydra/src/utils}/timing_utilities.cpp (99%) create mode 100644 hydra/tests/CMakeLists.txt rename hydra_dsg_builder/tests/utest_merge_handler.cpp => hydra/tests/backend/test_merge_handler.cpp (99%) rename hydra_dsg_builder/tests/utest_dsg_update_functions.cpp => hydra/tests/backend/test_update_functions.cpp (99%) rename hydra_utils/tests/utest_config.cpp => hydra/tests/config/test_config.cpp (73%) rename hydra_dsg_builder/tests/utest_frontend.cpp => hydra/tests/frontend/test_frontend.cpp (90%) rename hydra_topology/tests/include/hydra_topology_test/layer_utils.h => hydra/tests/include/hydra_test/gvd_layer_utils.h (95%) rename hydra_topology/tests/include/hydra_topology_test/test_fixtures.h => hydra/tests/include/hydra_test/place_fixtures.h (96%) create mode 100644 hydra/tests/include/hydra_test/resources.h rename hydra_dsg_builder/tests/utest_dsg_lcd_matching.cpp => hydra/tests/loop_closure/test_descriptor_matching.cpp (92%) rename hydra_dsg_builder/tests/utest_dsg_lcd_module.cpp => hydra/tests/loop_closure/test_detector.cpp (89%) rename hydra_dsg_builder/tests/utest_lcd_gnn_descriptors.cpp => hydra/tests/loop_closure/test_gnn_descriptors.cpp (91%) rename hydra_dsg_builder/tests/utest_dsg_lcd_registration.cpp => hydra/tests/loop_closure/test_registration.cpp (97%) rename hydra_dsg_builder/tests/utest_dsg_lcd_descriptors.cpp => hydra/tests/loop_closure/test_scene_graph_descriptors.cpp (97%) rename hydra_dsg_builder/tests/utest_subgraph_extraction.cpp => hydra/tests/loop_closure/test_subgraph_extraction.cpp (99%) rename hydra_dsg_builder/tests/utest_main.cpp => hydra/tests/main.cpp (100%) rename hydra_topology/tests/utest_compression_graph_extractor.cpp => hydra/tests/places/test_compression_graph_extractor.cpp (98%) rename hydra_topology/tests/utest_esdf.cpp => hydra/tests/places/test_esdf_comparison.cpp (88%) rename hydra_topology/tests/utest_floodfill_graph_extractor.cpp => hydra/tests/places/test_floodfill_graph_extractor.cpp (97%) rename hydra_topology/tests/utest_graph_extractor_utilities.cpp => hydra/tests/places/test_graph_extractor_utilities.cpp (98%) rename hydra_topology/tests/utest_incremental_integration.cpp => hydra/tests/places/test_gvd_incremental.cpp (91%) rename hydra_topology/tests/utest_incremental_gvd.cpp => hydra/tests/places/test_gvd_integrator.cpp (98%) rename hydra_topology/tests/utest_gvd_thinning.cpp => hydra/tests/places/test_gvd_thinning.cpp (97%) rename hydra_topology/tests/utest_gvd_utilities.cpp => hydra/tests/places/test_gvd_utilities.cpp (99%) rename hydra_topology/tests/utest_marching_cubes.cpp => hydra/tests/reconstruction/test_marching_cubes.cpp (97%) rename hydra_topology/tests/utest_pose_graph.cpp => hydra/tests/reconstruction/test_reconstruction_module.cpp (97%) rename {hydra_utils/tests/resources => hydra/tests/resources/config}/map_config.yaml (100%) rename {hydra_utils/tests/resources => hydra/tests/resources/config}/missing_config.yaml (76%) rename {hydra_utils/tests/resources => hydra/tests/resources/config}/nested_test_config.yaml (100%) rename {hydra_utils/tests/resources => hydra/tests/resources/config}/test_config.yaml (100%) rename {hydra_dsg_builder/tests/resources => hydra/tests/resources/loop_closure}/make_lcd_models.py (60%) rename {hydra_dsg_builder/tests/resources => hydra/tests/resources/loop_closure}/objects.onnx (100%) rename {hydra_dsg_builder/tests/resources => hydra/tests/resources/loop_closure}/objects_pos.onnx (100%) rename {hydra_dsg_builder/tests/resources => hydra/tests/resources/loop_closure}/places.onnx (100%) rename {hydra_dsg_builder/tests/resources => hydra/tests/resources/loop_closure}/places_pos.onnx (100%) rename {hydra_dsg_builder/tests/resources => hydra/tests/resources/loop_closure}/test_embeddings.yaml (100%) create mode 100644 hydra/tests/resources/test_semantic_map.csv rename hydra_dsg_builder/tests/utest_graph_clustering.cpp => hydra/tests/rooms/test_graph_clustering.cpp (99%) rename hydra_dsg_builder/tests/utest_graph_filtration.cpp => hydra/tests/rooms/test_graph_filtration.cpp (99%) rename hydra_dsg_builder/tests/utest_room_finder.cpp => hydra/tests/rooms/test_room_finder.cpp (99%) rename hydra_dsg_builder/tests/utest_configs.cpp => hydra/tests/rooms/test_room_finder_config.cpp (98%) rename hydra_dsg_builder/tests/utest_room_helpers.cpp => hydra/tests/rooms/test_room_utilities.cpp (99%) rename hydra_topology/tests/src/test_fixtures.cpp => hydra/tests/src/place_fixtures.cpp (95%) create mode 100644 hydra/tests/src/resources.cpp rename hydra_dsg_builder/tests/utest_minimum_spanning_tree.cpp => hydra/tests/utils/test_minimum_spanning_tree.cpp (98%) rename hydra_topology/tests/utest_nearest_neighbor_utilities.cpp => hydra/tests/utils/test_nearest_neighbor_utilities.cpp (97%) rename hydra_utils/tests/utest_timing_utilities.cpp => hydra/tests/utils/test_timing_utilities.cpp (99%) delete mode 100644 hydra_dsg_builder/CMakeLists.txt delete mode 100644 hydra_dsg_builder/config/category_labels/uh2_office_label_conversion.yaml delete mode 100644 hydra_dsg_builder/scripts/make_colormap.py delete mode 100755 hydra_dsg_builder/scripts/setup_logs.sh delete mode 100644 hydra_dsg_builder/src/gnn/tests/resources/make_simple_model.py delete mode 100644 hydra_dsg_builder_ros/CMakeLists.txt delete mode 100644 hydra_dsg_builder_ros/include/hydra_dsg_builder_ros/lcd_visualizer.h delete mode 100644 hydra_dsg_builder_ros/package.xml delete mode 100644 hydra_dsg_builder_ros/src/lcd_visualizer.cpp rename {hydra_dsg_builder_ros => hydra_ros}/.gitignore (100%) create mode 100644 hydra_ros/CMakeLists.txt create mode 100644 hydra_ros/README.md rename {hydra_utils/nodes => hydra_ros/bin}/csv_to_tf (100%) rename {hydra_dsg_builder_ros => hydra_ros}/bin/gnn_node (100%) rename {hydra_utils/nodes => hydra_ros/bin}/odom_to_tf (100%) rename {hydra_utils => hydra_ros}/cfg/Colormap.cfg (96%) rename {hydra_utils => hydra_ros}/cfg/DynamicLayerVisualizer.cfg (98%) rename {hydra_topology => hydra_ros}/cfg/GvdVisualizer.cfg (98%) rename {hydra_utils => hydra_ros}/cfg/LayerVisualizer.cfg (99%) rename {hydra_utils => hydra_ros}/cfg/Visualizer.cfg (98%) rename {hydra_topology => hydra_ros}/config/graph_visualization_config.yaml (100%) rename {hydra_topology/config/validator => hydra_ros/config/gvd_validator}/uh2.yaml (100%) rename {hydra_utils => hydra_ros}/config/hydra_visualizer/agent_layer.yaml (100%) rename {hydra_utils => hydra_ros}/config/hydra_visualizer/buildings_layer.yaml (100%) rename {hydra_utils => hydra_ros}/config/hydra_visualizer/objects_layer.yaml (100%) rename {hydra_utils => hydra_ros}/config/hydra_visualizer/places_layer.yaml (100%) rename {hydra_utils => hydra_ros}/config/hydra_visualizer/rooms_layer.yaml (100%) rename {hydra_utils => hydra_ros}/config/hydra_visualizer/visualizer.yaml (100%) rename {hydra_utils => hydra_ros}/config/object_visualizer/agent_layer.yaml (100%) rename {hydra_utils => hydra_ros}/config/object_visualizer/buildings_layer.yaml (100%) rename {hydra_utils => hydra_ros}/config/object_visualizer/objects_layer.yaml (100%) rename {hydra_utils => hydra_ros}/config/object_visualizer/places_layer.yaml (100%) rename {hydra_utils => hydra_ros}/config/object_visualizer/rooms_layer.yaml (100%) rename {hydra_utils => hydra_ros}/config/object_visualizer/visualizer.yaml (100%) rename {hydra_dsg_builder_ros => hydra_ros}/config/overrides.yaml (100%) rename {hydra_dsg_builder_ros => hydra_ros}/config/ros_params.yaml (100%) rename hydra_utils/CMakeLists.txt => hydra_ros/hydra_utils_cmake (68%) rename {hydra_utils/include/hydra_utils => hydra_ros/include/hydra_ros/config}/ros_parser.h (94%) rename {hydra_utils/include/hydra_utils => hydra_ros/include/hydra_ros/config}/ros_utilities.h (67%) rename {hydra_dsg_builder_ros/include/hydra_dsg_builder_ros => hydra_ros/include/hydra_ros/pipeline}/hydra_ros_pipeline.h (88%) rename {hydra_dsg_builder_ros/include/hydra_dsg_builder_ros => hydra_ros/include/hydra_ros/pipeline}/ros_backend.h (92%) rename {hydra_dsg_builder_ros/include/hydra_dsg_builder_ros => hydra_ros/include/hydra_ros/pipeline}/ros_frontend.h (94%) rename {hydra_dsg_builder_ros/include/hydra_dsg_builder_ros => hydra_ros/include/hydra_ros/pipeline}/ros_lcd_registration.h (98%) rename {hydra_topology/include/hydra_topology => hydra_ros/include/hydra_ros/pipeline}/ros_reconstruction.h (95%) rename {hydra_utils/include/hydra_utils => hydra_ros/include/hydra_ros/utils}/dsg_streaming_interface.h (99%) rename {hydra_dsg_builder_ros/include/hydra_dsg_builder_ros => hydra_ros/include/hydra_ros/utils}/node_utilities.h (98%) rename {hydra_utils/include/hydra_utils => hydra_ros/include/hydra_ros/utils}/semantic_ros_publishers.h (100%) rename hydra_utils/include/hydra_utils/colormap_utils.h => hydra_ros/include/hydra_ros/visualizer/colormap_utilities.h (98%) rename {hydra_utils/include/hydra_utils => hydra_ros/include/hydra_ros/visualizer}/dsg_mesh_plugins.h (98%) rename {hydra_utils/include/hydra_utils => hydra_ros/include/hydra_ros/visualizer}/dsg_visualizer_plugin.h (98%) rename {hydra_utils/include/hydra_utils => hydra_ros/include/hydra_ros/visualizer}/dynamic_scene_graph_visualizer.h (98%) rename {hydra_topology/include/hydra_topology => hydra_ros/include/hydra_ros/visualizer}/gvd_visualization_utilities.h (71%) rename {hydra_topology/include/hydra_topology => hydra_ros/include/hydra_ros/visualizer}/topology_server_visualizer.h (79%) rename {hydra_dsg_builder_ros/include/hydra_dsg_builder_ros => hydra_ros/include/hydra_ros/visualizer}/visualizer_plugins.h (96%) rename {hydra_utils/include/hydra_utils => hydra_ros/include/hydra_ros/visualizer}/visualizer_types.h (85%) rename hydra_utils/include/hydra_utils/visualizer_utils.h => hydra_ros/include/hydra_ros/visualizer/visualizer_utilities.h (99%) rename {hydra_dsg_builder_ros/launch => hydra_ros/launch/datasets}/mp3d.launch (76%) rename {hydra_dsg_builder_ros/launch => hydra_ros/launch/datasets}/sidpac.launch (60%) rename {hydra_dsg_builder_ros/launch => hydra_ros/launch/datasets}/simmons_a1.launch (73%) rename {hydra_dsg_builder_ros/launch => hydra_ros/launch/datasets}/simmons_jackal.launch (72%) rename {hydra_dsg_builder_ros/launch => hydra_ros/launch/datasets}/uhumans2.launch (66%) rename {hydra_dsg_builder_ros => hydra_ros}/launch/hydra.launch (88%) rename {hydra_dsg_builder_ros => hydra_ros}/launch/hydra_robot.launch (71%) rename {hydra_dsg_builder_ros => hydra_ros}/launch/hydra_robot_t265.launch (63%) rename {hydra_dsg_builder_ros/launch => hydra_ros/launch/misc}/dsg_optimizer.launch (80%) rename {hydra_topology/launch => hydra_ros/launch/places}/gvd_validator.launch (100%) rename {hydra_topology/launch => hydra_ros/launch/places}/hydra_topology.launch (100%) rename {hydra_topology/launch => hydra_ros/launch/places}/hydra_topology_sidpac.launch (73%) rename {hydra_topology/launch => hydra_ros/launch/places}/hydra_topology_simmons_jackal.launch (67%) rename {hydra_topology/launch => hydra_ros/launch/places}/hydra_topology_uhumans2.launch (75%) rename {hydra_utils/launch => hydra_ros/launch/utils}/data_sources/a1_pointcloud.launch (87%) rename {hydra_utils/launch => hydra_ros/launch/utils}/data_sources/d455_pointcloud.launch (77%) rename {hydra_utils/launch => hydra_ros/launch/utils}/data_sources/jackal_pointcloud.launch (88%) rename {hydra_utils/launch => hydra_ros/launch/utils}/data_sources/sidpac_pointcloud.launch (80%) rename {hydra_utils/launch => hydra_ros/launch/utils}/data_sources/uhumans2_pointcloud.launch (83%) rename {hydra_utils/launch => hydra_ros/launch/utils}/gt_pose/robot_gt_pose.launch (76%) rename {hydra_utils/launch => hydra_ros/launch/utils}/gt_pose/sidpac_gt_pose.launch (86%) rename {hydra_utils/launch => hydra_ros/launch/utils}/hydra_streaming_visualizer.launch (70%) rename {hydra_utils/launch => hydra_ros/launch/utils}/hydra_visualizer.launch (74%) rename {hydra_utils/launch => hydra_ros/launch/utils}/includes/a1_static_tfs.xml (100%) rename {hydra_utils/launch => hydra_ros/launch/utils}/includes/a1_t265_camera_static_tfs.xml (100%) rename {hydra_utils/launch => hydra_ros/launch/utils}/includes/a1_t265_static_tfs.xml (90%) rename {hydra_utils/launch => hydra_ros/launch/utils}/includes/hydra_visualizer_base.xml (80%) rename {hydra_utils/launch => hydra_ros/launch/utils}/includes/hydra_visualizer_params.xml (100%) rename {hydra_utils/launch => hydra_ros/launch/utils}/includes/hydra_visualizer_rviz.xml (79%) rename {hydra_utils/launch => hydra_ros/launch/utils}/includes/rgbd_to_pointcloud.xml (100%) rename {hydra_utils/launch => hydra_ros/launch/utils}/includes/sparkal1_static_tfs.xml (100%) rename {hydra_utils/launch => hydra_ros/launch/utils}/includes/sparkal2_static_tfs.xml (100%) rename {hydra_utils/launch => hydra_ros/launch/utils}/includes/uhumans2_static_tfs.xml (100%) rename {hydra_utils/launch => hydra_ros/launch/utils}/odom_to_tf.launch (76%) rename {hydra_utils/launch => hydra_ros/launch/utils}/publish_rotation_transform.launch (88%) rename {hydra_utils/launch => hydra_ros/launch/utils}/scene_graph_logger.launch (62%) rename {hydra_utils => hydra_ros}/nodelet_plugins.xml (100%) rename {hydra_utils => hydra_ros}/package.xml (77%) rename {hydra_utils => hydra_ros}/resources/a1_center_mask_roi_640.png (100%) rename {hydra_utils => hydra_ros}/resources/a1_left_mask_roi.png (100%) rename {hydra_dsg_builder_ros => hydra_ros}/rviz/a1.rviz (100%) rename {hydra_dsg_builder_ros => hydra_ros}/rviz/a1_topdown.rviz (100%) rename {hydra_dsg_builder_ros => hydra_ros}/rviz/dsg_optimizer.rviz (100%) rename hydra_topology/rviz/validator.rviz => hydra_ros/rviz/gvd_validator.rviz (100%) rename {hydra_utils => hydra_ros}/rviz/hydra_streaming_visualizer.rviz (100%) rename {hydra_utils => hydra_ros}/rviz/hydra_streaming_visualizer_topdown.rviz (100%) rename {hydra_utils => hydra_ros}/rviz/hydra_visualizer.rviz (100%) rename {hydra_dsg_builder_ros => hydra_ros}/rviz/mp3d.rviz (100%) rename {hydra_dsg_builder_ros => hydra_ros}/rviz/sidpac.rviz (100%) rename {hydra_dsg_builder_ros => hydra_ros}/rviz/sidpac_topdown.rviz (100%) rename {hydra_dsg_builder_ros => hydra_ros}/rviz/sparkal1.rviz (100%) rename {hydra_dsg_builder_ros => hydra_ros}/rviz/sparkal2.rviz (100%) rename {hydra_dsg_builder_ros => hydra_ros}/rviz/uhumans2.rviz (85%) rename {hydra_dsg_builder_ros => hydra_ros}/rviz/uhumans2_topdown.rviz (100%) rename {hydra_dsg_builder_ros => hydra_ros}/scripts/copy_static_tfs.py (100%) rename {hydra_dsg_builder_ros => hydra_ros}/scripts/decompress_uhumans2.py (100%) rename {hydra_dsg_builder_ros => hydra_ros}/scripts/download_uhumans2.py (100%) rename {hydra_dsg_builder_ros => hydra_ros}/scripts/run_robot.sh (100%) rename {hydra_dsg_builder_ros => hydra_ros}/scripts/run_robot_t265.sh (100%) create mode 100755 hydra_ros/scripts/scrape_kimera_extrinsics.py rename {hydra_utils/src => hydra_ros/src/config}/ros_parser.cpp (98%) rename {hydra_dsg_builder_ros/src => hydra_ros/src/nodes}/dsg_optimizer_node.cpp (91%) rename hydra_topology/src/gvd_validator.cpp => hydra_ros/src/nodes/gvd_validator_node.cpp (97%) rename hydra_dsg_builder_ros/src/incremental_dsg_builder_node.cpp => hydra_ros/src/nodes/hydra_node.cpp (92%) rename {hydra_topology/src => hydra_ros/src/nodes}/hydra_topology_node.cpp (94%) rename {hydra_utils/src => hydra_ros/src/nodes}/hydra_visualizer_node.cpp (96%) rename {hydra_utils/src => hydra_ros/src/nodes}/rotate_tf_node.cpp (100%) rename {hydra_utils/src => hydra_ros/src/nodes}/scene_graph_logger_node.cpp (97%) rename {hydra_dsg_builder_ros/src => hydra_ros/src/pipeline}/hydra_ros_pipeline.cpp (89%) rename {hydra_dsg_builder_ros/src => hydra_ros/src/pipeline}/ros_backend.cpp (93%) rename {hydra_dsg_builder_ros/src => hydra_ros/src/pipeline}/ros_frontend.cpp (96%) rename {hydra_dsg_builder_ros/src => hydra_ros/src/pipeline}/ros_lcd_registration.cpp (98%) rename {hydra_topology/src => hydra_ros/src/pipeline}/ros_reconstruction.cpp (92%) rename {hydra_utils/src => hydra_ros/src/utils}/dsg_streaming_interface.cpp (94%) rename {hydra_utils/src => hydra_ros/src/utils}/mask_nodelet.cpp (100%) rename {hydra_dsg_builder_ros/src => hydra_ros/src/utils}/node_utilities.cpp (98%) rename hydra_utils/src/colormap_utils.cpp => hydra_ros/src/visualizer/colormap_utilities.cpp (98%) rename {hydra_utils/src => hydra_ros/src/visualizer}/dsg_mesh_plugins.cpp (99%) rename {hydra_utils/src => hydra_ros/src/visualizer}/dynamic_scene_graph_visualizer.cpp (98%) rename {hydra_topology/src => hydra_ros/src/visualizer}/gvd_visualization_utilities.cpp (98%) rename {hydra_topology/src => hydra_ros/src/visualizer}/topology_server_visualizer.cpp (95%) rename {hydra_dsg_builder_ros/src => hydra_ros/src/visualizer}/visualizer_plugins.cpp (98%) rename hydra_utils/src/visualizer_utils.cpp => hydra_ros/src/visualizer/visualizer_utilities.cpp (98%) create mode 100644 hydra_ros/tests/CMakeLists.txt create mode 100644 hydra_ros/tests/hydra_ros.test rename hydra_utils/tests/utest_main.cpp => hydra_ros/tests/main.cpp (96%) create mode 100644 hydra_ros/tests/resources/map_config.yaml create mode 100644 hydra_ros/tests/resources/nested_test_config.yaml create mode 100644 hydra_ros/tests/resources/test_config.yaml create mode 100644 hydra_ros/tests/test_ros_config.cpp delete mode 100644 hydra_topology/CMakeLists.txt delete mode 100644 hydra_topology/README.md delete mode 100644 hydra_topology/cmake/nanoflann.CMakeLists.txt.in delete mode 100644 hydra_topology/output/.gitignore delete mode 100644 hydra_topology/package.xml delete mode 100644 hydra_topology/rviz/d455.rviz delete mode 100644 hydra_topology/rviz/sidpac.rviz delete mode 100644 hydra_topology/rviz/uhumans2.rviz delete mode 100644 hydra_utils/README.md delete mode 100644 hydra_utils/config/robots/a1_t265.yaml delete mode 100644 hydra_utils/include/hydra_utils/dsg_types.h delete mode 100644 hydra_utils/launch/visual_lcd.launch delete mode 100644 hydra_utils/rviz/a1_debug.rviz delete mode 100755 hydra_utils/scripts/scrape_kimera_extrinsics.py delete mode 100644 hydra_utils/tests/hydra_utils.test diff --git a/.clang-format b/.clang-format index 2b7327ca..81e83496 100644 --- a/.clang-format +++ b/.clang-format @@ -1,94 +1,10 @@ --- -Language: Cpp -BasedOnStyle: Google -AccessModifierOffset: -1 -AlignAfterOpenBracket: Align -AlignConsecutiveAssignments: false -AlignConsecutiveDeclarations: false -AlignOperands: true -AlignTrailingComments: true -AllowAllParametersOfDeclarationOnNextLine: false -AllowShortBlocksOnASingleLine: false -AllowShortCaseLabelsOnASingleLine: false -AllowShortFunctionsOnASingleLine: All -AllowShortIfStatementsOnASingleLine: true -AllowShortLoopsOnASingleLine: true -AlwaysBreakAfterDefinitionReturnType: None -AlwaysBreakAfterReturnType: None -AlwaysBreakBeforeMultilineStrings: true -AlwaysBreakTemplateDeclarations: true +BasedOnStyle: Google +--- +Language: Cpp +DerivePointerAlignment: false +PointerAlignment: Left +ColumnLimit: 88 BinPackArguments: false BinPackParameters: false -BraceWrapping: - AfterClass: false - AfterControlStatement: false - AfterEnum: false - AfterFunction: false - AfterNamespace: false - AfterObjCDeclaration: false - AfterStruct: false - AfterUnion: false - BeforeCatch: false - BeforeElse: false - IndentBraces: false -BreakBeforeBinaryOperators: None -BreakBeforeBraces: Attach -BreakBeforeTernaryOperators: true -BreakConstructorInitializersBeforeComma: false -ColumnLimit: 88 -CommentPragmas: '^ IWYU pragma:' -ConstructorInitializerAllOnOneLineOrOnePerLine: true -ConstructorInitializerIndentWidth: 4 -ContinuationIndentWidth: 4 -Cpp11BracedListStyle: true -DerivePointerAlignment: true -DisableFormat: false -ExperimentalAutoDetectBinPacking: false -ForEachMacros: - - foreach - - Q_FOREACH - - BOOST_FOREACH -IncludeCategories: - - Regex: '^' - Priority: 2 - - Regex: '^<.*\.h>' - Priority: 1 - - Regex: '^<.*' - Priority: 2 - - Regex: '.*' - Priority: 3 -IndentCaseLabels: true -IndentWidth: 2 -IndentWrappedFunctionNames: false -KeepEmptyLinesAtTheStartOfBlocks: false -MacroBlockBegin: '' -MacroBlockEnd: '' -MaxEmptyLinesToKeep: 1 -NamespaceIndentation: None -ObjCBlockIndentWidth: 2 -ObjCSpaceAfterProperty: false -ObjCSpaceBeforeProtocolList: false -PenaltyBreakBeforeFirstCallParameter: 1 -PenaltyBreakComment: 300 -PenaltyBreakFirstLessLess: 120 -PenaltyBreakString: 1000 -PenaltyExcessCharacter: 1000000 -PenaltyReturnTypeOnItsOwnLine: 200 -PointerAlignment: Left -ReflowComments: true -SortIncludes: true -SpaceAfterCStyleCast: false -SpaceBeforeAssignmentOperators: true -SpaceBeforeParens: ControlStatements -SpaceInEmptyParentheses: false -SpacesBeforeTrailingComments: 2 -SpacesInAngles: false -SpacesInContainerLiterals: true -SpacesInCStyleCastParentheses: false -SpacesInParentheses: false -SpacesInSquareBrackets: false -Standard: Auto -TabWidth: 8 -UseTab: Never -... - +--- diff --git a/README.md b/README.md index a1fdf4f8..9a0ebf1b 100644 --- a/README.md +++ b/README.md @@ -202,12 +202,9 @@ Finally, build your workspace: catkin build ``` -### Components +### Config Library -See the following for more information: - - [hydra_dsg_builder](hydra_dsg_builder/README.md) - - [hydra_topology](hydra_topology/README.md) - - [hydra_utils](hydra_utils/README.md) +Hydra has a wrapper around config parsing that is documented [here](doc/config_parsing.md). ### Filing Issues diff --git a/dev/check-code.sh b/dev/check-code.sh new file mode 100755 index 00000000..5e637fd3 --- /dev/null +++ b/dev/check-code.sh @@ -0,0 +1,8 @@ +#!/bin/bash + +SCRIPT_DIR=$( cd -- "$( dirname -- "${BASH_SOURCE[0]}" )" &> /dev/null && pwd ) +pushd $(dirname $SCRIPT_DIR) + +find ./python -iname '*.py' | xargs python3 -m flake8 + +popd diff --git a/dev/check-licenses.sh b/dev/check-licenses.sh new file mode 100755 index 00000000..b162539d --- /dev/null +++ b/dev/check-licenses.sh @@ -0,0 +1,10 @@ +#!/bin/bash + +SCRIPT_DIR=$( cd -- "$( dirname -- "${BASH_SOURCE[0]}" )" &> /dev/null && pwd ) + +pushd $(dirname $SCRIPT_DIR) + +find . -iname *.h -o -iname *.cpp | xargs python3 $SCRIPT_DIR/check_license.py +find . -iname '*.py' | xargs python3 $SCRIPT_DIR/check_license.py + +popd diff --git a/dev/check_license.py b/dev/check_license.py new file mode 100644 index 00000000..91f2492d --- /dev/null +++ b/dev/check_license.py @@ -0,0 +1,73 @@ +#!/usr/bin/env python3 +"""Script to check source file for license info.""" +import difflib +import pathlib +import click + + +def get_filetype(extension): + """Get filetype based on extension.""" + if extension in [".cpp", ".h"]: + return "cpp" + elif extension == ".py": + return "python" + else: + return None + + +@click.command() +@click.argument("input_files", type=click.File("r"), nargs=-1) +@click.option("--verbose", "-v", is_flag=True, help="show files checked") +def main(input_files, verbose): + """Check if header of file matches license.""" + parent_dir = pathlib.Path(__file__).absolute().parent + headers = {} + with (parent_dir / "cpp-header.txt").open("r") as fin: + headers["cpp"] = fin.read() + + with (parent_dir / "python-header.txt").open("r") as fin: + headers["python"] = fin.read() + + for input_file in input_files: + ext = pathlib.Path(input_file.name).suffix + filetype = get_filetype(ext) + if not filetype: + click.secho(f"unknown extension for {input_file.name}: {ext}", fg="yellow") + continue + + target = headers[filetype] + num_lines = len(target.split("\n")) - 1 + + try: + lines = [next(input_file) for _ in range(num_lines)] + except StopIteration: + click.secho(f"{input_file.name}: missing", fg="red") + continue + + if filetype == "python": + if lines and "#!" in lines[0]: + lines = lines[1:] + try: + lines.append(next(input_file)) + except StopIteration: + click.secho(f"{input_file.name}: missing", fg="red") + continue + + header = "".join(lines) + if header != target: + click.secho(f"{input_file.name}: bad", fg="red") + if verbose: + diff = difflib.context_diff( + header.splitlines(keepends=True), + target.splitlines(keepends=True), + fromfile=input_file.name, + tofile="license", + ) + diff_str = "".join(diff) + click.secho(f"{diff_str}", fg="red") + elif verbose: + click.secho(f"{input_file.name}: good", fg="green") + + +if __name__ == "__main__": + main() diff --git a/dev/cpp-header.txt b/dev/cpp-header.txt new file mode 100644 index 00000000..254cf9a6 --- /dev/null +++ b/dev/cpp-header.txt @@ -0,0 +1,34 @@ +/* ----------------------------------------------------------------------------- + * Copyright 2022 Massachusetts Institute of Technology. + * All Rights Reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Research was sponsored by the United States Air Force Research Laboratory and + * the United States Air Force Artificial Intelligence Accelerator and was + * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views + * and conclusions contained in this document are those of the authors and should + * not be interpreted as representing the official policies, either expressed or + * implied, of the United States Air Force or the U.S. Government. The U.S. + * Government is authorized to reproduce and distribute reprints for Government + * purposes notwithstanding any copyright notation herein. + * -------------------------------------------------------------------------- */ diff --git a/dev/python-header.txt b/dev/python-header.txt new file mode 100644 index 00000000..38e6d5cb --- /dev/null +++ b/dev/python-header.txt @@ -0,0 +1,32 @@ +# Copyright 2022, Massachusetts Institute of Technology. +# All Rights Reserved +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +# WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +# +# Research was sponsored by the United States Air Force Research Laboratory and +# the United States Air Force Artificial Intelligence Accelerator and was +# accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views +# and conclusions contained in this document are those of the authors and should +# not be interpreted as representing the official policies, either expressed or +# implied, of the United States Air Force or the U.S. Government. The U.S. +# Government is authorized to reproduce and distribute reprints for Government +# purposes notwithstanding any copyright notation herein. diff --git a/dev/reformat.sh b/dev/reformat.sh new file mode 100755 index 00000000..abdae9f8 --- /dev/null +++ b/dev/reformat.sh @@ -0,0 +1,11 @@ +#!/bin/bash + +SCRIPT_DIR=$( cd -- "$( dirname -- "${BASH_SOURCE[0]}" )" &> /dev/null && pwd ) + +pushd $(dirname $SCRIPT_DIR) + +find . -iname *.h -o -iname *.cpp | xargs clang-format -i +python3 -m black . +find . -iname *CMakeLists.txt* | xargs python3 -m cmakelang.format -i + +popd diff --git a/hydra_utils/doc/config_parsing.md b/doc/config_parsing.md similarity index 100% rename from hydra_utils/doc/config_parsing.md rename to doc/config_parsing.md diff --git a/hydra_dsg_builder/.gitignore b/hydra/.gitignore similarity index 100% rename from hydra_dsg_builder/.gitignore rename to hydra/.gitignore diff --git a/hydra/CMakeLists.txt b/hydra/CMakeLists.txt new file mode 100644 index 00000000..5c265a5b --- /dev/null +++ b/hydra/CMakeLists.txt @@ -0,0 +1,100 @@ +cmake_minimum_required(VERSION 3.1) +project(hydra) + +add_compile_options(-Wall -Wextra) +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) + +option(HYDRA_USE_COVERAGE "Build core library with GCC --coverage option" OFF) +option(HYDRA_GNN "build GNN interface" ON) +if(HYDRA_GNN) + set(HYDRA_USE_GNN_CXX_VALUE 1) +else() + set(HYDRA_USE_GNN_CXX_VALUE 0) +endif() +configure_file(hydra_build_config.h.in ${CMAKE_CURRENT_BINARY_DIR}/hydra_build_config.h) + +find_package(Boost REQUIRED COMPONENTS timer) +find_package(spark_dsg REQUIRED) +find_package(teaserpp REQUIRED) +find_package(GTSAM REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(yaml-cpp REQUIRED) +find_package( + catkin REQUIRED + COMPONENTS kimera_pgmo + kimera_semantics + tf2_eigen + voxblox +) + +set(PCL_FIND_QUIETLY TRUE) +find_package(PCL REQUIRED COMPONENTS common) + +configure_file(cmake/nanoflann.CMakeLists.txt.in nanoflann-download/CMakeLists.txt) +execute_process( + COMMAND "${CMAKE_COMMAND}" -G "${CMAKE_GENERATOR}" . + WORKING_DIRECTORY "${CMAKE_BINARY_DIR}/nanoflann-download" + OUTPUT_QUIET +) +execute_process( + COMMAND "${CMAKE_COMMAND}" --build . + WORKING_DIRECTORY "${CMAKE_BINARY_DIR}/nanoflann-download" + OUTPUT_QUIET +) + +set(NANOFLANN_BUILD_EXAMPLES + OFF + CACHE INTERNAL "" +) +set(NANOFLANN_BUILD_TESTS + OFF + CACHE INTERNAL "" +) +add_subdirectory(${CMAKE_BINARY_DIR}/nanoflann-src ${CMAKE_BINARY_DIR}/nanoflann-build) + +catkin_package( + CATKIN_DEPENDS + kimera_pgmo + kimera_semantics + tf2_eigen + voxblox + DEPENDS spark_dsg YAML_CPP + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} gtsam +) + +add_library(${PROJECT_NAME}) +target_include_directories( + ${PROJECT_NAME} + PUBLIC include ${catkin_INCLUDE_DIRS} + PRIVATE ${CMAKE_CURRENT_BINARY_DIR} ${PCL_INCLUDE_DIRS} +) +target_link_libraries( + ${PROJECT_NAME} + PUBLIC ${catkin_LIBRARIES} + teaserpp::teaser_registration + spark_dsg::spark_dsg + yaml-cpp + gtsam + PRIVATE nanoflann::nanoflann ${PCL_LIBRARIES} +) +add_subdirectory(src) + +if(HYDRA_GNN) + add_subdirectory(src/gnn) + target_link_libraries(${PROJECT_NAME} PUBLIC ${PROJECT_NAME}_gnn) +endif(HYDRA_GNN) + +if(HYDRA_USE_COVERAGE) + target_compile_options(${PROJECT_NAME} PRIVATE --coverage) + # TODO(nathan) once we have newer cmake, target link options + target_link_libraries(${PROJECT_NAME} PRIVATE --coverage) +endif(HYDRA_USE_COVERAGE) + +if(CATKIN_ENABLE_TESTING) + add_subdirectory(tests) +endif(CATKIN_ENABLE_TESTING) + +# TODO(nathan) handle install diff --git a/hydra_dsg_builder/README.md b/hydra/README.md similarity index 100% rename from hydra_dsg_builder/README.md rename to hydra/README.md diff --git a/hydra/cmake/nanoflann.CMakeLists.txt.in b/hydra/cmake/nanoflann.CMakeLists.txt.in new file mode 100644 index 00000000..175c24e9 --- /dev/null +++ b/hydra/cmake/nanoflann.CMakeLists.txt.in @@ -0,0 +1,15 @@ +cmake_minimum_required(VERSION 3.1) + +project(nanoflann_download NONE) + +include(ExternalProject) +ExternalProject_Add( + nanoflann + URL https://github.com/jlblancoc/nanoflann/archive/refs/tags/v1.4.2.zip + SOURCE_DIR "${CMAKE_CURRENT_BINARY_DIR}/nanoflann-src" + BINARY_DIR "${CMAKE_CURRENT_BINARY_DIR}/nanoflann-build" + CONFIGURE_COMMAND "" + BUILD_COMMAND "" + INSTALL_COMMAND "" + TEST_COMMAND "" +) diff --git a/hydra_dsg_builder/config/ade20k_mp3d_typology.yaml b/hydra/config/ade20k_mp3d_typology.yaml similarity index 100% rename from hydra_dsg_builder/config/ade20k_mp3d_typology.yaml rename to hydra/config/ade20k_mp3d_typology.yaml diff --git a/hydra_dsg_builder/config/ade20k_typology.yaml b/hydra/config/ade20k_typology.yaml similarity index 100% rename from hydra_dsg_builder/config/ade20k_typology.yaml rename to hydra/config/ade20k_typology.yaml diff --git a/hydra_dsg_builder/config/d455/d455_typology.yaml b/hydra/config/d455/d455_typology.yaml similarity index 100% rename from hydra_dsg_builder/config/d455/d455_typology.yaml rename to hydra/config/d455/d455_typology.yaml diff --git a/hydra_dsg_builder/config/d455/dsg_backend_config.yaml b/hydra/config/d455/dsg_backend_config.yaml similarity index 100% rename from hydra_dsg_builder/config/d455/dsg_backend_config.yaml rename to hydra/config/d455/dsg_backend_config.yaml diff --git a/hydra_dsg_builder/config/d455/dsg_frontend_config.yaml b/hydra/config/d455/dsg_frontend_config.yaml similarity index 100% rename from hydra_dsg_builder/config/d455/dsg_frontend_config.yaml rename to hydra/config/d455/dsg_frontend_config.yaml diff --git a/hydra_dsg_builder/config/d455/dsg_lcd_config.yaml b/hydra/config/d455/dsg_lcd_config.yaml similarity index 100% rename from hydra_dsg_builder/config/d455/dsg_lcd_config.yaml rename to hydra/config/d455/dsg_lcd_config.yaml diff --git a/hydra_topology/config/default_topology_config.yaml b/hydra/config/default_reconstruction_config.yaml similarity index 100% rename from hydra_topology/config/default_topology_config.yaml rename to hydra/config/default_reconstruction_config.yaml diff --git a/hydra_dsg_builder/config/default_robot/dsg_backend_config.yaml b/hydra/config/default_robot/dsg_backend_config.yaml similarity index 100% rename from hydra_dsg_builder/config/default_robot/dsg_backend_config.yaml rename to hydra/config/default_robot/dsg_backend_config.yaml diff --git a/hydra_dsg_builder/config/default_robot/dsg_frontend_config.yaml b/hydra/config/default_robot/dsg_frontend_config.yaml similarity index 100% rename from hydra_dsg_builder/config/default_robot/dsg_frontend_config.yaml rename to hydra/config/default_robot/dsg_frontend_config.yaml diff --git a/hydra_dsg_builder/config/default_robot/dsg_gnn_lcd_config.yaml b/hydra/config/default_robot/dsg_gnn_lcd_config.yaml similarity index 100% rename from hydra_dsg_builder/config/default_robot/dsg_gnn_lcd_config.yaml rename to hydra/config/default_robot/dsg_gnn_lcd_config.yaml diff --git a/hydra_dsg_builder/config/default_robot/dsg_lcd_config.yaml b/hydra/config/default_robot/dsg_lcd_config.yaml similarity index 100% rename from hydra_dsg_builder/config/default_robot/dsg_lcd_config.yaml rename to hydra/config/default_robot/dsg_lcd_config.yaml diff --git a/hydra_topology/config/extrinsics/mp3d.yaml b/hydra/config/extrinsics/mp3d.yaml similarity index 100% rename from hydra_topology/config/extrinsics/mp3d.yaml rename to hydra/config/extrinsics/mp3d.yaml diff --git a/hydra_topology/config/extrinsics/sidpac.yaml b/hydra/config/extrinsics/sidpac.yaml similarity index 100% rename from hydra_topology/config/extrinsics/sidpac.yaml rename to hydra/config/extrinsics/sidpac.yaml diff --git a/hydra_topology/config/extrinsics/sparkal1.yaml b/hydra/config/extrinsics/sparkal1.yaml similarity index 100% rename from hydra_topology/config/extrinsics/sparkal1.yaml rename to hydra/config/extrinsics/sparkal1.yaml diff --git a/hydra_topology/config/extrinsics/sparkal2.yaml b/hydra/config/extrinsics/sparkal2.yaml similarity index 100% rename from hydra_topology/config/extrinsics/sparkal2.yaml rename to hydra/config/extrinsics/sparkal2.yaml diff --git a/hydra_topology/config/extrinsics/uhumans2.yaml b/hydra/config/extrinsics/uhumans2.yaml similarity index 100% rename from hydra_topology/config/extrinsics/uhumans2.yaml rename to hydra/config/extrinsics/uhumans2.yaml diff --git a/hydra_dsg_builder/config/habitat/dsg_backend_config.yaml b/hydra/config/habitat/dsg_backend_config.yaml similarity index 100% rename from hydra_dsg_builder/config/habitat/dsg_backend_config.yaml rename to hydra/config/habitat/dsg_backend_config.yaml diff --git a/hydra_dsg_builder/config/habitat/dsg_frontend_config.yaml b/hydra/config/habitat/dsg_frontend_config.yaml similarity index 100% rename from hydra_dsg_builder/config/habitat/dsg_frontend_config.yaml rename to hydra/config/habitat/dsg_frontend_config.yaml diff --git a/hydra_dsg_builder/config/habitat/dsg_lcd_config.yaml b/hydra/config/habitat/dsg_lcd_config.yaml similarity index 100% rename from hydra_dsg_builder/config/habitat/dsg_lcd_config.yaml rename to hydra/config/habitat/dsg_lcd_config.yaml diff --git a/hydra_dsg_builder/config/habitat/mp3d_typology.yaml b/hydra/config/habitat/mp3d_typology.yaml similarity index 100% rename from hydra_dsg_builder/config/habitat/mp3d_typology.yaml rename to hydra/config/habitat/mp3d_typology.yaml diff --git a/hydra_dsg_builder/config/habitat/reconstruction_config.yaml b/hydra/config/habitat/reconstruction_config.yaml similarity index 100% rename from hydra_dsg_builder/config/habitat/reconstruction_config.yaml rename to hydra/config/habitat/reconstruction_config.yaml diff --git a/hydra_dsg_builder/config/sidpac/dsg_backend_config.yaml b/hydra/config/sidpac/dsg_backend_config.yaml similarity index 100% rename from hydra_dsg_builder/config/sidpac/dsg_backend_config.yaml rename to hydra/config/sidpac/dsg_backend_config.yaml diff --git a/hydra_dsg_builder/config/sidpac/dsg_frontend_config.yaml b/hydra/config/sidpac/dsg_frontend_config.yaml similarity index 100% rename from hydra_dsg_builder/config/sidpac/dsg_frontend_config.yaml rename to hydra/config/sidpac/dsg_frontend_config.yaml diff --git a/hydra_dsg_builder/config/sidpac/dsg_gnn_lcd_config.yaml b/hydra/config/sidpac/dsg_gnn_lcd_config.yaml similarity index 100% rename from hydra_dsg_builder/config/sidpac/dsg_gnn_lcd_config.yaml rename to hydra/config/sidpac/dsg_gnn_lcd_config.yaml diff --git a/hydra_dsg_builder/config/sidpac/dsg_lcd_config.yaml b/hydra/config/sidpac/dsg_lcd_config.yaml similarity index 100% rename from hydra_dsg_builder/config/sidpac/dsg_lcd_config.yaml rename to hydra/config/sidpac/dsg_lcd_config.yaml diff --git a/hydra_topology/config/sidpac_topology_config.yaml b/hydra/config/sidpac/reconstruction_config.yaml similarity index 100% rename from hydra_topology/config/sidpac_topology_config.yaml rename to hydra/config/sidpac/reconstruction_config.yaml diff --git a/hydra_dsg_builder/config/sidpac/sidpac_typology.yaml b/hydra/config/sidpac/sidpac_typology.yaml similarity index 100% rename from hydra_dsg_builder/config/sidpac/sidpac_typology.yaml rename to hydra/config/sidpac/sidpac_typology.yaml diff --git a/hydra_dsg_builder/config/simmons_a1/dsg_backend_config.yaml b/hydra/config/simmons_a1/dsg_backend_config.yaml similarity index 100% rename from hydra_dsg_builder/config/simmons_a1/dsg_backend_config.yaml rename to hydra/config/simmons_a1/dsg_backend_config.yaml diff --git a/hydra_dsg_builder/config/simmons_a1/dsg_frontend_config.yaml b/hydra/config/simmons_a1/dsg_frontend_config.yaml similarity index 100% rename from hydra_dsg_builder/config/simmons_a1/dsg_frontend_config.yaml rename to hydra/config/simmons_a1/dsg_frontend_config.yaml diff --git a/hydra_dsg_builder/config/simmons_a1/dsg_gnn_lcd_config.yaml b/hydra/config/simmons_a1/dsg_gnn_lcd_config.yaml similarity index 100% rename from hydra_dsg_builder/config/simmons_a1/dsg_gnn_lcd_config.yaml rename to hydra/config/simmons_a1/dsg_gnn_lcd_config.yaml diff --git a/hydra_dsg_builder/config/simmons_a1/dsg_lcd_config.yaml b/hydra/config/simmons_a1/dsg_lcd_config.yaml similarity index 100% rename from hydra_dsg_builder/config/simmons_a1/dsg_lcd_config.yaml rename to hydra/config/simmons_a1/dsg_lcd_config.yaml diff --git a/hydra_topology/config/mp3d_topology_config.yaml b/hydra/config/simmons_a1/reconstruction_config.yaml similarity index 100% rename from hydra_topology/config/mp3d_topology_config.yaml rename to hydra/config/simmons_a1/reconstruction_config.yaml diff --git a/hydra_dsg_builder/config/simmons_jackal/dsg_backend_config.yaml b/hydra/config/simmons_jackal/dsg_backend_config.yaml similarity index 100% rename from hydra_dsg_builder/config/simmons_jackal/dsg_backend_config.yaml rename to hydra/config/simmons_jackal/dsg_backend_config.yaml diff --git a/hydra_dsg_builder/config/simmons_jackal/dsg_frontend_config.yaml b/hydra/config/simmons_jackal/dsg_frontend_config.yaml similarity index 100% rename from hydra_dsg_builder/config/simmons_jackal/dsg_frontend_config.yaml rename to hydra/config/simmons_jackal/dsg_frontend_config.yaml diff --git a/hydra_dsg_builder/config/simmons_jackal/dsg_gnn_lcd_config.yaml b/hydra/config/simmons_jackal/dsg_gnn_lcd_config.yaml similarity index 100% rename from hydra_dsg_builder/config/simmons_jackal/dsg_gnn_lcd_config.yaml rename to hydra/config/simmons_jackal/dsg_gnn_lcd_config.yaml diff --git a/hydra_dsg_builder/config/simmons_jackal/dsg_lcd_config.yaml b/hydra/config/simmons_jackal/dsg_lcd_config.yaml similarity index 100% rename from hydra_dsg_builder/config/simmons_jackal/dsg_lcd_config.yaml rename to hydra/config/simmons_jackal/dsg_lcd_config.yaml diff --git a/hydra_topology/config/simmons_topology_config.yaml b/hydra/config/simmons_jackal/reconstruction_config.yaml similarity index 100% rename from hydra_topology/config/simmons_topology_config.yaml rename to hydra/config/simmons_jackal/reconstruction_config.yaml diff --git a/hydra_dsg_builder/config/uhumans2/apartment_lcd_embeddings.yaml b/hydra/config/uhumans2/apartment_lcd_embeddings.yaml similarity index 100% rename from hydra_dsg_builder/config/uhumans2/apartment_lcd_embeddings.yaml rename to hydra/config/uhumans2/apartment_lcd_embeddings.yaml diff --git a/hydra_dsg_builder/config/uhumans2/dsg_backend_config.yaml b/hydra/config/uhumans2/dsg_backend_config.yaml similarity index 100% rename from hydra_dsg_builder/config/uhumans2/dsg_backend_config.yaml rename to hydra/config/uhumans2/dsg_backend_config.yaml diff --git a/hydra_dsg_builder/config/uhumans2/dsg_frontend_config.yaml b/hydra/config/uhumans2/dsg_frontend_config.yaml similarity index 100% rename from hydra_dsg_builder/config/uhumans2/dsg_frontend_config.yaml rename to hydra/config/uhumans2/dsg_frontend_config.yaml diff --git a/hydra_dsg_builder/config/uhumans2/dsg_gnn_lcd_config.yaml b/hydra/config/uhumans2/dsg_gnn_lcd_config.yaml similarity index 100% rename from hydra_dsg_builder/config/uhumans2/dsg_gnn_lcd_config.yaml rename to hydra/config/uhumans2/dsg_gnn_lcd_config.yaml diff --git a/hydra_dsg_builder/config/uhumans2/dsg_lcd_config.yaml b/hydra/config/uhumans2/dsg_lcd_config.yaml similarity index 100% rename from hydra_dsg_builder/config/uhumans2/dsg_lcd_config.yaml rename to hydra/config/uhumans2/dsg_lcd_config.yaml diff --git a/hydra_dsg_builder/config/uhumans2/office_lcd_embeddings.yaml b/hydra/config/uhumans2/office_lcd_embeddings.yaml similarity index 100% rename from hydra_dsg_builder/config/uhumans2/office_lcd_embeddings.yaml rename to hydra/config/uhumans2/office_lcd_embeddings.yaml diff --git a/hydra_topology/config/uhumans2_topology_config.yaml b/hydra/config/uhumans2/reconstruction_config.yaml similarity index 100% rename from hydra_topology/config/uhumans2_topology_config.yaml rename to hydra/config/uhumans2/reconstruction_config.yaml diff --git a/hydra_dsg_builder/config/uhumans2/subway_lcd_embeddings.yaml b/hydra/config/uhumans2/subway_lcd_embeddings.yaml similarity index 100% rename from hydra_dsg_builder/config/uhumans2/subway_lcd_embeddings.yaml rename to hydra/config/uhumans2/subway_lcd_embeddings.yaml diff --git a/hydra_dsg_builder/config/uhumans2/uhumans2_apartment_typology.yaml b/hydra/config/uhumans2/uhumans2_apartment_typology.yaml similarity index 100% rename from hydra_dsg_builder/config/uhumans2/uhumans2_apartment_typology.yaml rename to hydra/config/uhumans2/uhumans2_apartment_typology.yaml diff --git a/hydra_dsg_builder/config/uhumans2/uhumans2_office_typology.yaml b/hydra/config/uhumans2/uhumans2_office_typology.yaml similarity index 100% rename from hydra_dsg_builder/config/uhumans2/uhumans2_office_typology.yaml rename to hydra/config/uhumans2/uhumans2_office_typology.yaml diff --git a/hydra_dsg_builder/config/uhumans2/uhumans2_subway_typology.yaml b/hydra/config/uhumans2/uhumans2_subway_typology.yaml similarity index 100% rename from hydra_dsg_builder/config/uhumans2/uhumans2_subway_typology.yaml rename to hydra/config/uhumans2/uhumans2_subway_typology.yaml diff --git a/hydra_dsg_builder/hydra_build_config.h.in b/hydra/hydra_build_config.h.in similarity index 100% rename from hydra_dsg_builder/hydra_build_config.h.in rename to hydra/hydra_build_config.h.in diff --git a/hydra_dsg_builder/include/hydra_dsg_builder/backend_config.h b/hydra/include/hydra/backend/backend_config.h similarity index 95% rename from hydra_dsg_builder/include/hydra_dsg_builder/backend_config.h rename to hydra/include/hydra/backend/backend_config.h index 646d87d0..da9e28fa 100644 --- a/hydra_dsg_builder/include/hydra_dsg_builder/backend_config.h +++ b/hydra/include/hydra/backend/backend_config.h @@ -34,13 +34,12 @@ * -------------------------------------------------------------------------- */ #pragma once #include -#include -#include -#include #include -#include -#include "hydra_dsg_builder/room_finder_config.h" +#include "hydra/common/dsg_types.h" +#include "hydra/config/config.h" +#include "hydra/config/eigen_config_types.h" +#include "hydra/rooms/room_finder_config.h" DECLARE_CONFIG_ENUM(KimeraRPGO, Verbosity, @@ -108,9 +107,8 @@ void visit_config(const Visitor& v, KimeraPgmoConfig& config) { } // namespace kimera_pgmo namespace hydra { -namespace incremental { -struct DsgBackendConfig { +struct BackendConfig { bool should_log = true; std::string log_path; @@ -185,7 +183,7 @@ struct EnableMapConverter { }; template -void visit_config(const Visitor& v, DsgBackendConfig& config) { +void visit_config(const Visitor& v, BackendConfig& config) { // TODO(nathan) replace with single param (derive should_log from log_path) v.visit("should_log", config.should_log); if (config.should_log) { @@ -223,7 +221,7 @@ void visit_config(const Visitor& v, DsgBackendConfig& config) { } template -void visit_config(const Visitor& v, DsgBackendConfig::PgmoConfig& config) { +void visit_config(const Visitor& v, BackendConfig::PgmoConfig& config) { // TODO(nathan) replace with single param (derive should_log from log_path) v.visit("should_log", config.should_log); if (config.should_log) { @@ -241,8 +239,7 @@ void visit_config(const Visitor& v, DsgBackendConfig::PgmoConfig& config) { rpgo_handle.visit("solver", config.rpgo_solver); } -} // namespace incremental } // namespace hydra -DECLARE_CONFIG_OSTREAM_OPERATOR(hydra::incremental, DsgBackendConfig) +DECLARE_CONFIG_OSTREAM_OPERATOR(hydra, BackendConfig) DECLARE_CONFIG_OSTREAM_OPERATOR(kimera_pgmo, KimeraPgmoConfig) diff --git a/hydra_dsg_builder/include/hydra_dsg_builder/incremental_dsg_backend.h b/hydra/include/hydra/backend/backend_module.h similarity index 86% rename from hydra_dsg_builder/include/hydra_dsg_builder/incremental_dsg_backend.h rename to hydra/include/hydra/backend/backend_module.h index e1d7760b..2ecfacda 100644 --- a/hydra_dsg_builder/include/hydra_dsg_builder/incremental_dsg_backend.h +++ b/hydra/include/hydra/backend/backend_module.h @@ -33,13 +33,6 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include "hydra_dsg_builder/backend_config.h" -#include "hydra_dsg_builder/dsg_update_functions.h" -#include "hydra_dsg_builder/incremental_types.h" -#include "hydra_dsg_builder/merge_handler.h" -#include "hydra_dsg_builder/shared_module_state.h" - -#include #include #include #include @@ -48,8 +41,14 @@ #include #include +#include "hydra/backend/backend_config.h" +#include "hydra/backend/merge_handler.h" +#include "hydra/backend/update_functions.h" +#include "hydra/common/common.h" +#include "hydra/common/robot_prefix_config.h" +#include "hydra/common/shared_module_state.h" + namespace hydra { -namespace incremental { struct LoopClosureLog { gtsam::Symbol src; // factor edge "source" @@ -59,25 +58,25 @@ struct LoopClosureLog { int64_t level; }; -class DsgBackend : public kimera_pgmo::KimeraPgmoInterface { +class BackendModule : public kimera_pgmo::KimeraPgmoInterface { public: - using Ptr = std::shared_ptr; + using Ptr = std::shared_ptr; using OutputCallback = std::function; - DsgBackend(const RobotPrefixConfig& prefix, - const DsgBackendConfig& config, - const kimera_pgmo::KimeraPgmoConfig& pgmo_config, - const SharedDsgInfo::Ptr& dsg, - const SharedDsgInfo::Ptr& backend_dsg, - const SharedModuleState::Ptr& state); + BackendModule(const RobotPrefixConfig& prefix, + const BackendConfig& config, + const kimera_pgmo::KimeraPgmoConfig& pgmo_config, + const SharedDsgInfo::Ptr& dsg, + const SharedDsgInfo::Ptr& backend_dsg, + const SharedModuleState::Ptr& state); - virtual ~DsgBackend(); + virtual ~BackendModule(); - DsgBackend(const DsgBackend& other) = delete; + BackendModule(const BackendModule& other) = delete; - DsgBackend& operator=(const DsgBackend& other) = delete; + BackendModule& operator=(const BackendModule& other) = delete; void start(); @@ -166,14 +165,14 @@ class DsgBackend : public kimera_pgmo::KimeraPgmoInterface { bool reset_backend_dsg_{false}; RobotPrefixConfig prefix_; - DsgBackendConfig config_; + BackendConfig config_; std::unordered_map place_pos_cache_; SharedDsgInfo::Ptr shared_dsg_; SharedDsgInfo::Ptr private_dsg_; IsolatedSceneGraphLayer shared_places_copy_; - std::map proposed_node_merges_; + std::map proposed_node_merges_; std::unique_ptr merge_handler_; SharedModuleState::Ptr state_; std::vector mesh_timestamps_; @@ -185,7 +184,7 @@ class DsgBackend : public kimera_pgmo::KimeraPgmoInterface { std::unique_ptr update_rooms_functor_; std::unique_ptr update_buildings_functor_; - DsgBackendStatus status_; + BackendModuleStatus status_; SceneGraphLogger backend_graph_logger_; std::list loop_closures_; @@ -200,5 +199,4 @@ class DsgBackend : public kimera_pgmo::KimeraPgmoInterface { std::unique_ptr zmq_receiver_; }; -} // namespace incremental } // namespace hydra diff --git a/hydra_dsg_builder/include/hydra_dsg_builder/merge_handler.h b/hydra/include/hydra/backend/merge_handler.h similarity index 97% rename from hydra_dsg_builder/include/hydra_dsg_builder/merge_handler.h rename to hydra/include/hydra/backend/merge_handler.h index 26552ef7..52784e9c 100644 --- a/hydra_dsg_builder/include/hydra_dsg_builder/merge_handler.h +++ b/hydra/include/hydra/backend/merge_handler.h @@ -33,10 +33,9 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include "hydra_dsg_builder/dsg_update_functions.h" +#include "hydra/backend/update_functions.h" namespace hydra { -namespace incremental { struct NodeInfo { using Ptr = std::unique_ptr; @@ -106,5 +105,4 @@ class MergeHandler { bool undo_allowed_; }; -} // namespace incremental } // namespace hydra diff --git a/hydra_dsg_builder/include/hydra_dsg_builder/dsg_update_functions.h b/hydra/include/hydra/backend/update_functions.h similarity index 82% rename from hydra_dsg_builder/include/hydra_dsg_builder/dsg_update_functions.h rename to hydra/include/hydra/backend/update_functions.h index 681d0f8f..57a63b6d 100644 --- a/hydra_dsg_builder/include/hydra_dsg_builder/dsg_update_functions.h +++ b/hydra/include/hydra/backend/update_functions.h @@ -33,10 +33,10 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include "hydra_dsg_builder/incremental_types.h" - #include -#include + +#include "hydra/common/common.h" +#include "hydra/utils/nearest_neighbor_utilities.h" namespace hydra { @@ -53,16 +53,14 @@ struct UpdateInfo { }; using LayerUpdateFunc = - std::function(incremental::SharedDsgInfo&, - const UpdateInfo&)>; + std::function(SharedDsgInfo&, const UpdateInfo&)>; namespace dsg_updates { struct UpdateObjectsFunctor { using MeshVertices = DynamicSceneGraph::MeshVertices; - std::map call(incremental::SharedDsgInfo& dsg, - const UpdateInfo& info) const; + std::map call(SharedDsgInfo& dsg, const UpdateInfo& info) const; size_t makeNodeFinders(const SceneGraphLayer& layer) const; @@ -81,16 +79,14 @@ struct UpdateObjectsFunctor { size_t num_merges_to_consider = 1; bool use_active_flag = true; std::shared_ptr> invalid_indices; - mutable std::map> - node_finders; + mutable std::map> node_finders; }; struct UpdatePlacesFunctor { UpdatePlacesFunctor(double pos_threshold, double distance_tolerance) : pos_threshold_m(pos_threshold), distance_tolerance_m(distance_tolerance) {} - std::map call(incremental::SharedDsgInfo& dsg, - const UpdateInfo& info) const; + std::map call(SharedDsgInfo& dsg, const UpdateInfo& info) const; size_t makeNodeFinder(const SceneGraphLayer& layer) const; @@ -113,7 +109,7 @@ struct UpdatePlacesFunctor { bool use_active_flag = true; double pos_threshold_m; double distance_tolerance_m; - mutable std::unique_ptr node_finder; + mutable std::unique_ptr node_finder; }; struct UpdateRoomsFunctor { @@ -121,8 +117,7 @@ struct UpdateRoomsFunctor { ~UpdateRoomsFunctor(); - std::map call(incremental::SharedDsgInfo& dsg, - const UpdateInfo& info) const; + std::map call(SharedDsgInfo& dsg, const UpdateInfo& info) const; void rewriteRooms(const SceneGraphLayer* new_rooms, DynamicSceneGraph& graph) const; @@ -133,15 +128,13 @@ struct UpdateBuildingsFunctor { UpdateBuildingsFunctor(const SemanticNodeAttributes::ColorVector& color, SemanticNodeAttributes::Label label); - std::map call(incremental::SharedDsgInfo& dsg, - const UpdateInfo& info) const; + std::map call(SharedDsgInfo& dsg, const UpdateInfo& info) const; SemanticNodeAttributes::ColorVector building_color; SemanticNodeAttributes::Label building_semantic_label; }; -std::map updateAgents(incremental::SharedDsgInfo& graph, - const UpdateInfo& info); +std::map updateAgents(SharedDsgInfo& graph, const UpdateInfo& info); } // namespace dsg_updates diff --git a/hydra_dsg_builder/include/hydra_dsg_builder/incremental_types.h b/hydra/include/hydra/common/common.h similarity index 96% rename from hydra_dsg_builder/include/hydra_dsg_builder/incremental_types.h rename to hydra/include/hydra/common/common.h index 17d93d6a..0e11bc44 100644 --- a/hydra_dsg_builder/include/hydra_dsg_builder/incremental_types.h +++ b/hydra/include/hydra/common/common.h @@ -34,13 +34,14 @@ * -------------------------------------------------------------------------- */ #pragma once #include -#include #include #include #include #include +#include "hydra/common/dsg_types.h" + namespace hydra { namespace lcd { @@ -56,18 +57,12 @@ struct DsgRegistrationSolution { } // namespace lcd -namespace dsg_updates { struct NodeMergeInfo { NodeId to_node; gtsam::Pose3 from_T_to; }; typedef std::unordered_map NodeMergeLog; - -} // namespace dsg_updates - -namespace incremental { - typedef std::unordered_set NodeIdSet; struct SharedDsgInfo { @@ -102,7 +97,7 @@ struct SharedDsgInfo { }; // TODO(nathan) switch code style -struct DsgBackendStatus { +struct BackendModuleStatus { size_t total_loop_closures_; size_t new_loop_closures_; size_t total_factors_; @@ -124,5 +119,4 @@ struct DsgBackendStatus { } }; -} // namespace incremental } // namespace hydra diff --git a/hydra/include/hydra/common/dsg_types.h b/hydra/include/hydra/common/dsg_types.h new file mode 100644 index 00000000..ac367d41 --- /dev/null +++ b/hydra/include/hydra/common/dsg_types.h @@ -0,0 +1,45 @@ +/* ----------------------------------------------------------------------------- + * Copyright 2022 Massachusetts Institute of Technology. + * All Rights Reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Research was sponsored by the United States Air Force Research Laboratory and + * the United States Air Force Artificial Intelligence Accelerator and was + * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views + * and conclusions contained in this document are those of the authors and should + * not be interpreted as representing the official policies, either expressed or + * implied, of the United States Air Force or the U.S. Government. The U.S. + * Government is authorized to reproduce and distribute reprints for Government + * purposes notwithstanding any copyright notation herein. + * -------------------------------------------------------------------------- */ +#pragma once +#include +#include +#include +#include + +namespace hydra { + +using namespace spark_dsg; + +} // namespace hydra diff --git a/hydra_dsg_builder/include/hydra_dsg_builder/hydra_config.h b/hydra/include/hydra/common/hydra_config.h similarity index 100% rename from hydra_dsg_builder/include/hydra_dsg_builder/hydra_config.h rename to hydra/include/hydra/common/hydra_config.h diff --git a/hydra_utils/include/hydra_utils/input_queue.h b/hydra/include/hydra/common/input_queue.h similarity index 100% rename from hydra_utils/include/hydra_utils/input_queue.h rename to hydra/include/hydra/common/input_queue.h diff --git a/hydra_utils/include/hydra_utils/robot_prefix_config.h b/hydra/include/hydra/common/robot_prefix_config.h similarity index 100% rename from hydra_utils/include/hydra_utils/robot_prefix_config.h rename to hydra/include/hydra/common/robot_prefix_config.h diff --git a/hydra_dsg_builder/include/hydra_dsg_builder/shared_module_state.h b/hydra/include/hydra/common/shared_module_state.h similarity index 93% rename from hydra_dsg_builder/include/hydra_dsg_builder/shared_module_state.h rename to hydra/include/hydra/common/shared_module_state.h index 2a2e455c..c2961a9f 100644 --- a/hydra_dsg_builder/include/hydra_dsg_builder/shared_module_state.h +++ b/hydra/include/hydra/common/shared_module_state.h @@ -33,11 +33,6 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include "hydra_dsg_builder/incremental_types.h" - -#include -#include -#include #include #include #include @@ -50,8 +45,12 @@ #include #include +#include "hydra/common/common.h" +#include "hydra/common/dsg_types.h" +#include "hydra/common/input_queue.h" +#include "hydra/common/robot_prefix_config.h" + namespace hydra { -namespace incremental { struct LcdInput { using Ptr = std::shared_ptr; @@ -86,5 +85,4 @@ struct SharedModuleState { InputQueue backend_lcd_queue; }; -} // namespace incremental } // namespace hydra diff --git a/hydra_utils/include/hydra_utils/config.h b/hydra/include/hydra/config/config.h similarity index 62% rename from hydra_utils/include/hydra_utils/config.h rename to hydra/include/hydra/config/config.h index 8cbc4c09..28aaf057 100644 --- a/hydra_utils/include/hydra_utils/config.h +++ b/hydra/include/hydra/config/config.h @@ -33,9 +33,10 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include "hydra_utils/ostream_formatter.h" -#include "hydra_utils/ros_parser.h" -#include "hydra_utils/yaml_parser.h" +#include + +#include "hydra/config/ostream_formatter.h" +#include "hydra/config/yaml_parser.h" namespace config_parser { @@ -66,32 +67,6 @@ Config load_from_yaml_ns(const std::string& filepath, return config; } -template -Config load_from_ros(const std::string& ns, Logger::Ptr logger = nullptr) { - RosParser parser(std::make_unique(ros::NodeHandle(ns))); - if (logger) { - parser.setLogger(logger); - } - - Config config; - ConfigVisitor::visit_config(parser, config); - return config; -} - -template -Config load_from_ros_nh(const ros::NodeHandle& nh, - const std::string& ns = "", - Logger::Ptr logger = nullptr) { - RosParser parser(std::make_unique(nh, ns)); - if (logger) { - parser.setLogger(logger); - } - - Config config; - ConfigVisitor::visit_config(parser, config); - return config; -} - inline std::string to_uppercase(const std::string& original) { std::string result = original; std::transform(result.begin(), result.end(), result.begin(), [](unsigned char c) { @@ -118,52 +93,28 @@ inline std::string to_uppercase(const std::string& original) { // must be called in the global namespace #define DECLARE_CONFIG_ENUM(ns, enum_name, values...) \ + template <> \ + struct config_parser::is_config_enum : std::true_type {}; \ + \ namespace ns { \ - inline enum_name read##enum_name##FromString(const std::string& enum_string) { \ + \ + inline void readConfigEnumFromString(const std::string& enum_string, \ + enum_name& value) { \ std::map value_map{values}; \ for (const auto& kv_pair : value_map) { \ if (kv_pair.second == enum_string) { \ - return kv_pair.first; \ + value = kv_pair.first; \ + return; \ } \ } \ throw std::domain_error("invalid value for " #enum_name ": " + enum_string); \ } \ \ - inline std::ostream& operator<<(std::ostream& out, enum_name v) { \ + inline std::string configEnumToString(const enum_name& v) { \ std::map value_map{values}; \ if (value_map.count(v)) { \ - out << value_map.at(v); \ - } else { \ - out << "INVALID"; \ - } \ - return out; \ - } \ - \ - inline bool readRosParam(const ros::NodeHandle& nh, \ - const std::string& name, \ - enum_name& value) { \ - std::string value_str = ""; \ - if (!nh.getParam(name, value_str)) { \ - return false; \ + return value_map.at(v); \ } \ - value = read##enum_name##FromString(value_str); \ - return true; \ + return "INVALID"; \ } \ - } \ - \ - template <> \ - struct YAML::convert { \ - static ::YAML::Node encode(const ns::enum_name& rhs) { \ - std::stringstream ss; \ - ss << rhs; \ - return YAML::Node(ss.str()); \ - } \ - \ - static bool decode(const Node& node, ns::enum_name& rhs) { \ - if (node.IsNull()) { \ - return false; \ - } \ - rhs = ns::read##enum_name##FromString(node.as()); \ - return true; \ - } \ - }; + } diff --git a/hydra_utils/include/hydra_utils/config_formatter.h b/hydra/include/hydra/config/config_formatter.h similarity index 98% rename from hydra_utils/include/hydra_utils/config_formatter.h rename to hydra/include/hydra/config/config_formatter.h index 7e1797c7..3495f81c 100644 --- a/hydra_utils/include/hydra_utils/config_formatter.h +++ b/hydra/include/hydra/config/config_formatter.h @@ -33,10 +33,10 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include "hydra_utils/config_visitor.h" - #include +#include "hydra/config/config_visitor.h" + namespace config_parser { template diff --git a/hydra_utils/include/hydra_utils/config_parser.h b/hydra/include/hydra/config/config_parser.h similarity index 97% rename from hydra_utils/include/hydra_utils/config_parser.h rename to hydra/include/hydra/config/config_parser.h index 38797751..ed53ca08 100644 --- a/hydra_utils/include/hydra_utils/config_parser.h +++ b/hydra/include/hydra/config/config_parser.h @@ -33,14 +33,14 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include "hydra_utils/config_visitor.h" -#include "hydra_utils/ostream_formatter.h" - #include #include #include #include +#include "hydra/config/config_visitor.h" +#include "hydra/config/ostream_formatter.h" + namespace config_parser { struct Logger { diff --git a/hydra_utils/include/hydra_utils/config_traits.h b/hydra/include/hydra/config/config_traits.h similarity index 97% rename from hydra_utils/include/hydra_utils/config_traits.h rename to hydra/include/hydra/config/config_traits.h index 51439838..dea5f461 100644 --- a/hydra_utils/include/hydra_utils/config_traits.h +++ b/hydra/include/hydra/config/config_traits.h @@ -62,6 +62,9 @@ struct is_config : std::false_type {}; template struct is_parser : std::false_type {}; +template +struct is_config_enum : std::false_type {}; + namespace detail { // ODR workaround diff --git a/hydra_utils/include/hydra_utils/config_visitor.h b/hydra/include/hydra/config/config_visitor.h similarity index 99% rename from hydra_utils/include/hydra_utils/config_visitor.h rename to hydra/include/hydra/config/config_visitor.h index c4f1dadd..990535b5 100644 --- a/hydra_utils/include/hydra_utils/config_visitor.h +++ b/hydra/include/hydra/config/config_visitor.h @@ -33,11 +33,11 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include "hydra_utils/config_traits.h" - #include #include +#include "hydra/config/config_traits.h" + // argument-dependent-lookup for arbitrary config structures. See the following: // - http://www.open-std.org/jtc1/sc22/wg21/docs/papers/2015/n4381.html // - https:://github.com/nlohmann/json/blob/develop/include/nlohmann/adl_serializer.hpp diff --git a/hydra_utils/include/hydra_utils/eigen_config_types.h b/hydra/include/hydra/config/eigen_config_types.h similarity index 86% rename from hydra_utils/include/hydra_utils/eigen_config_types.h rename to hydra/include/hydra/config/eigen_config_types.h index 4a194b52..e1bba39d 100644 --- a/hydra_utils/include/hydra_utils/eigen_config_types.h +++ b/hydra/include/hydra/config/eigen_config_types.h @@ -35,7 +35,8 @@ #pragma once #include #include -#include "hydra_utils/config.h" + +#include "hydra/config/config.h" namespace YAML { @@ -92,29 +93,6 @@ struct convert> { namespace Eigen { -template -bool readRosParam(const ros::NodeHandle& nh, - const std::string& name, - Matrix& value) { - std::vector raw_values; - config_parser::readRosParam(nh, name, raw_values); - if (raw_values.empty()) { - return false; - } - - if (raw_values.size() != static_cast(N)) { - std::stringstream ss; - ss << "invalid param length: " << raw_values.size() << " != " << N; - throw std::domain_error(ss.str()); - } - - for (int i = 0; i < N; ++i) { - value(i) = raw_values[i]; - } - - return true; -} - template void displayParam(std::ostream& out, const Eigen::Matrix& value) { Eigen::IOFormat format( diff --git a/hydra_utils/include/hydra_utils/ostream_formatter.h b/hydra/include/hydra/config/ostream_formatter.h similarity index 93% rename from hydra_utils/include/hydra_utils/ostream_formatter.h rename to hydra/include/hydra/config/ostream_formatter.h index 8a2d0648..51596656 100644 --- a/hydra_utils/include/hydra_utils/ostream_formatter.h +++ b/hydra/include/hydra/config/ostream_formatter.h @@ -38,17 +38,23 @@ #include #include -#include "hydra_utils/config_formatter.h" +#include "hydra/config/config_formatter.h" namespace config_parser { namespace detail { -template +template >, bool> = true> void displayParam(std::ostream& out, const T& value) { out << value; } +template ::value, bool> = true> +void displayParam(std::ostream& out, const T& value) { + out << configEnumToString(value); +} + template <> inline void displayParam(std::ostream& out, const uint8_t& value) { out << static_cast(value); diff --git a/hydra_utils/include/hydra_utils/yaml_parser.h b/hydra/include/hydra/config/yaml_parser.h similarity index 89% rename from hydra_utils/include/hydra_utils/yaml_parser.h rename to hydra/include/hydra/config/yaml_parser.h index 2375a006..0ebe82a2 100644 --- a/hydra_utils/include/hydra_utils/yaml_parser.h +++ b/hydra/include/hydra/config/yaml_parser.h @@ -33,11 +33,12 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include "hydra_utils/config_parser.h" - #include + #include +#include "hydra/config/config_parser.h" + namespace config_parser { class YamlParserImpl { @@ -81,12 +82,22 @@ class YamlParserImpl { private: YamlParserImpl(const YAML::Node& node, const std::string& name); - template + template >::value, + bool>::type = true> bool parseImpl(T& value) const { value = node_.as(); return true; } + template ::value, bool>::type = true> + bool parseImpl(T& value) const { + const auto placeholder = node_.as(); + readConfigEnumFromString(placeholder, value); + return true; + } + template bool parseImpl(std::vector& value) const { if (!node_.IsSequence()) { diff --git a/hydra_dsg_builder/include/hydra_dsg_builder/frontend_config.h b/hydra/include/hydra/frontend/frontend_config.h similarity index 93% rename from hydra_dsg_builder/include/hydra_dsg_builder/frontend_config.h rename to hydra/include/hydra/frontend/frontend_config.h index ffe7e4b6..7cebc75c 100644 --- a/hydra_dsg_builder/include/hydra_dsg_builder/frontend_config.h +++ b/hydra/include/hydra/frontend/frontend_config.h @@ -33,12 +33,12 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include "hydra_dsg_builder/incremental_mesh_segmenter.h" - -#include -#include #include +#include "hydra/config/config.h" +#include "hydra/config/eigen_config_types.h" +#include "hydra/frontend/mesh_segmenter.h" + namespace spark_dsg { using BoundingBoxType = BoundingBox::Type; @@ -71,9 +71,8 @@ void visit_config(const Visitor& v, kimera_pgmo::MeshFrontendConfig& config) { } // namespace kimera_pgmo namespace hydra { -namespace incremental { -struct DsgFrontendConfig { +struct FrontendConfig { // TODO(nathan) consider unifying log path with backend bool should_log = true; std::string log_path; @@ -128,7 +127,7 @@ void visit_config(const Visitor& v, MeshSegmenterConfig& config) { } template -void visit_config(const Visitor& v, DsgFrontendConfig& config) { +void visit_config(const Visitor& v, FrontendConfig& config) { // TODO(nathan) replace with single param (derive should_log from log_path) v.visit("should_log", config.should_log); if (config.should_log) { @@ -146,9 +145,8 @@ void visit_config(const Visitor& v, DsgFrontendConfig& config) { v.visit("min_places_component_size", config.min_places_component_size); } -} // namespace incremental } // namespace hydra DECLARE_CONFIG_OSTREAM_OPERATOR(kimera_pgmo, MeshFrontendConfig) -DECLARE_CONFIG_OSTREAM_OPERATOR(hydra::incremental, MeshSegmenterConfig) -DECLARE_CONFIG_OSTREAM_OPERATOR(hydra::incremental, DsgFrontendConfig) +DECLARE_CONFIG_OSTREAM_OPERATOR(hydra, MeshSegmenterConfig) +DECLARE_CONFIG_OSTREAM_OPERATOR(hydra, FrontendConfig) diff --git a/hydra_dsg_builder/include/hydra_dsg_builder/incremental_dsg_frontend.h b/hydra/include/hydra/frontend/frontend_module.h similarity index 84% rename from hydra_dsg_builder/include/hydra_dsg_builder/incremental_dsg_frontend.h rename to hydra/include/hydra/frontend/frontend_module.h index 352b16dd..b39adb7c 100644 --- a/hydra_dsg_builder/include/hydra_dsg_builder/incremental_dsg_frontend.h +++ b/hydra/include/hydra/frontend/frontend_module.h @@ -33,15 +33,6 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include "hydra_dsg_builder/frontend_config.h" -#include "hydra_dsg_builder/incremental_mesh_segmenter.h" -#include "hydra_dsg_builder/incremental_types.h" -#include "hydra_dsg_builder/shared_module_state.h" - -#include -#include -#include -#include #include #include #include @@ -50,26 +41,31 @@ #include #include -namespace hydra { -namespace incremental { +#include "hydra/common/common.h" +#include "hydra/common/input_queue.h" +#include "hydra/common/robot_prefix_config.h" +#include "hydra/common/shared_module_state.h" +#include "hydra/frontend/frontend_config.h" +#include "hydra/frontend/mesh_segmenter.h" +#include "hydra/reconstruction/reconstruction_output.h" +#include "hydra/utils/nearest_neighbor_utilities.h" -using topology::NearestNodeFinder; +namespace hydra { -class DsgFrontend { +class FrontendModule { public: using FrontendInputQueue = InputQueue; using InputCallback = std::function; - using OutputCallback = std::function; + using OutputCallback = std::function; using DynamicLayer = DynamicSceneGraphLayer; - DsgFrontend(const RobotPrefixConfig& prefix, - const DsgFrontendConfig& config, - const SharedDsgInfo::Ptr& dsg, - const SharedModuleState::Ptr& state); + FrontendModule(const RobotPrefixConfig& prefix, + const FrontendConfig& config, + const SharedDsgInfo::Ptr& dsg, + const SharedModuleState::Ptr& state); - virtual ~DsgFrontend(); + virtual ~FrontendModule(); void start(); @@ -128,7 +124,7 @@ class DsgFrontend { BackendInput::Ptr backend_input_; RobotPrefixConfig prefix_; - DsgFrontendConfig config_; + FrontendConfig config_; std::unique_ptr label_map_; SharedDsgInfo::Ptr dsg_; SharedModuleState::Ptr state_; @@ -153,5 +149,4 @@ class DsgFrontend { std::vector output_callbacks_; }; -} // namespace incremental } // namespace hydra diff --git a/hydra_dsg_builder/include/hydra_dsg_builder/incremental_mesh_segmenter.h b/hydra/include/hydra/frontend/mesh_segmenter.h similarity index 98% rename from hydra_dsg_builder/include/hydra_dsg_builder/incremental_mesh_segmenter.h rename to hydra/include/hydra/frontend/mesh_segmenter.h index 78b2af72..312e6d33 100644 --- a/hydra_dsg_builder/include/hydra_dsg_builder/incremental_mesh_segmenter.h +++ b/hydra/include/hydra/frontend/mesh_segmenter.h @@ -33,7 +33,6 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include #include #include #include @@ -41,8 +40,9 @@ #include +#include "hydra/common/dsg_types.h" + namespace hydra { -namespace incremental { struct Cluster { using PointT = pcl::PointXYZRGBA; @@ -131,5 +131,4 @@ class MeshSegmenter { std::vector callback_funcs_; }; -} // namespace incremental } // namespace hydra diff --git a/hydra_dsg_builder/include/hydra_dsg_builder/gnn/gnn_interface.h b/hydra/include/hydra/gnn/gnn_interface.h similarity index 98% rename from hydra_dsg_builder/include/hydra_dsg_builder/gnn/gnn_interface.h rename to hydra/include/hydra/gnn/gnn_interface.h index 75ef8f22..52d36524 100644 --- a/hydra_dsg_builder/include/hydra_dsg_builder/gnn/gnn_interface.h +++ b/hydra/include/hydra/gnn/gnn_interface.h @@ -33,13 +33,13 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include "hydra_dsg_builder/gnn/tensor.h" - #include #include #include #include +#include "hydra/gnn/tensor.h" + namespace hydra { namespace gnn { diff --git a/hydra_dsg_builder/include/hydra_dsg_builder/gnn/ort_utilities.h b/hydra/include/hydra/gnn/ort_utilities.h similarity index 98% rename from hydra_dsg_builder/include/hydra_dsg_builder/gnn/ort_utilities.h rename to hydra/include/hydra/gnn/ort_utilities.h index 560f9ee2..9bca8a9a 100644 --- a/hydra_dsg_builder/include/hydra_dsg_builder/gnn/ort_utilities.h +++ b/hydra/include/hydra/gnn/ort_utilities.h @@ -33,8 +33,6 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include "hydra_dsg_builder/gnn/tensor.h" - #include #include @@ -42,6 +40,8 @@ #include #include +#include "hydra/gnn/tensor.h" + namespace hydra { namespace gnn { diff --git a/hydra_dsg_builder/include/hydra_dsg_builder/gnn/tensor.h b/hydra/include/hydra/gnn/tensor.h similarity index 99% rename from hydra_dsg_builder/include/hydra_dsg_builder/gnn/tensor.h rename to hydra/include/hydra/gnn/tensor.h index 91764ebd..5c5125ea 100644 --- a/hydra_dsg_builder/include/hydra_dsg_builder/gnn/tensor.h +++ b/hydra/include/hydra/gnn/tensor.h @@ -34,7 +34,6 @@ * -------------------------------------------------------------------------- */ #pragma once #include - #include #include #include diff --git a/hydra_dsg_builder/include/hydra_dsg_builder/dsg_lcd_matching.h b/hydra/include/hydra/loop_closure/descriptor_matching.h similarity index 98% rename from hydra_dsg_builder/include/hydra_dsg_builder/dsg_lcd_matching.h rename to hydra/include/hydra/loop_closure/descriptor_matching.h index 80dff9fb..543a543a 100644 --- a/hydra_dsg_builder/include/hydra_dsg_builder/dsg_lcd_matching.h +++ b/hydra/include/hydra/loop_closure/descriptor_matching.h @@ -33,7 +33,7 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include "hydra_dsg_builder/dsg_lcd_descriptors.h" +#include "hydra/loop_closure/scene_graph_descriptors.h" namespace hydra { namespace lcd { diff --git a/hydra_dsg_builder/include/hydra_dsg_builder/dsg_lcd_detector.h b/hydra/include/hydra/loop_closure/detector.h similarity index 94% rename from hydra_dsg_builder/include/hydra_dsg_builder/dsg_lcd_detector.h rename to hydra/include/hydra/loop_closure/detector.h index f098165a..15a52e3e 100644 --- a/hydra_dsg_builder/include/hydra_dsg_builder/dsg_lcd_detector.h +++ b/hydra/include/hydra/loop_closure/detector.h @@ -33,9 +33,9 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include "hydra_dsg_builder/dsg_lcd_descriptors.h" -#include "hydra_dsg_builder/dsg_lcd_matching.h" -#include "hydra_dsg_builder/dsg_lcd_registration.h" +#include "hydra/loop_closure/descriptor_matching.h" +#include "hydra/loop_closure/registration.h" +#include "hydra/loop_closure/scene_graph_descriptors.h" namespace hydra { namespace lcd { @@ -51,7 +51,7 @@ struct GnnLcdConfig { bool objects_pos_in_feature = false; }; -struct DsgLcdDetectorConfig { +struct LcdDetectorConfig { std::map search_configs; DescriptorMatchConfig agent_search_config; @@ -68,11 +68,11 @@ struct DsgLcdDetectorConfig { GnnLcdConfig gnn_lcd; }; -class DsgLcdDetector { +class LcdDetector { public: using FactoryMap = std::map; - explicit DsgLcdDetector(const DsgLcdDetectorConfig& config); + explicit LcdDetector(const LcdDetectorConfig& config); void setDescriptorFactories(FactoryMap&& factories); @@ -114,7 +114,7 @@ class DsgLcdDetector { NodeId agent_node, uint64_t timestamp = 0) const; - DsgLcdDetectorConfig config_; + LcdDetectorConfig config_; DescriptorFactory::Ptr agent_factory_; FactoryMap layer_factories_; diff --git a/hydra_dsg_builder/include/hydra_dsg_builder/lcd_gnn_descriptors.h b/hydra/include/hydra/loop_closure/gnn_descriptors.h similarity index 97% rename from hydra_dsg_builder/include/hydra_dsg_builder/lcd_gnn_descriptors.h rename to hydra/include/hydra/loop_closure/gnn_descriptors.h index 02ae5807..99c974c9 100644 --- a/hydra_dsg_builder/include/hydra_dsg_builder/lcd_gnn_descriptors.h +++ b/hydra/include/hydra/loop_closure/gnn_descriptors.h @@ -33,8 +33,8 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include "hydra_dsg_builder/dsg_lcd_descriptors.h" -#include "hydra_dsg_builder/gnn/gnn_interface.h" +#include "hydra/gnn/gnn_interface.h" +#include "hydra/loop_closure/scene_graph_descriptors.h" namespace hydra { namespace lcd { diff --git a/hydra_dsg_builder/include/hydra_dsg_builder/lcd_module_config.h b/hydra/include/hydra/loop_closure/loop_closure_config.h similarity index 94% rename from hydra_dsg_builder/include/hydra_dsg_builder/lcd_module_config.h rename to hydra/include/hydra/loop_closure/loop_closure_config.h index fca8f164..ff6855b1 100644 --- a/hydra_dsg_builder/include/hydra_dsg_builder/lcd_module_config.h +++ b/hydra/include/hydra/loop_closure/loop_closure_config.h @@ -33,10 +33,9 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include -#include - -#include "hydra_dsg_builder/dsg_lcd_detector.h" +#include "hydra/config/config.h" +#include "hydra/config/eigen_config_types.h" +#include "hydra/loop_closure/detector.h" namespace teaser { @@ -86,8 +85,8 @@ struct ConfigVisitor> { namespace hydra { -struct DsgLcdModuleConfig { - lcd::DsgLcdDetectorConfig detector; +struct LoopClosureConfig { + lcd::LcdDetectorConfig detector; bool visualize_dsg_lcd = false; std::string lcd_visualizer_ns = "/dsg/lcd_visualizer"; double lcd_agent_horizon_s = 1.5; @@ -152,7 +151,7 @@ void visit_config(const Visitor& v, GnnLcdConfig& config) { } template -void visit_config(const Visitor& v, DsgLcdDetectorConfig& config) { +void visit_config(const Visitor& v, LcdDetectorConfig& config) { v.visit("search_configs", config.search_configs); auto v_search = v["search_configs"]; v_search.visit("agent", config.agent_search_config); @@ -178,7 +177,7 @@ void visit_config(const Visitor& v, DsgLcdDetectorConfig& config) { } // namespace lcd template -void visit_config(const Visitor& v, DsgLcdModuleConfig& config) { +void visit_config(const Visitor& v, LoopClosureConfig& config) { v.visit("lcd", config.detector); v.visit("visualize_dsg_lcd", config.visualize_dsg_lcd); v.visit("lcd_visualizer_ns", config.lcd_visualizer_ns); @@ -208,5 +207,5 @@ DECLARE_CONFIG_OSTREAM_OPERATOR(hydra, SubgraphConfig) DECLARE_CONFIG_OSTREAM_OPERATOR(hydra::lcd, HistogramConfig) DECLARE_CONFIG_OSTREAM_OPERATOR(hydra::lcd, LayerRegistrationConfig) DECLARE_CONFIG_OSTREAM_OPERATOR(hydra::lcd, DescriptorMatchConfig) -DECLARE_CONFIG_OSTREAM_OPERATOR(hydra::lcd, DsgLcdDetectorConfig) -DECLARE_CONFIG_OSTREAM_OPERATOR(hydra, DsgLcdModuleConfig) +DECLARE_CONFIG_OSTREAM_OPERATOR(hydra::lcd, LcdDetectorConfig) +DECLARE_CONFIG_OSTREAM_OPERATOR(hydra, LoopClosureConfig) diff --git a/hydra_dsg_builder/include/hydra_dsg_builder/incremental_dsg_lcd.h b/hydra/include/hydra/loop_closure/loop_closure_module.h similarity index 82% rename from hydra_dsg_builder/include/hydra_dsg_builder/incremental_dsg_lcd.h rename to hydra/include/hydra/loop_closure/loop_closure_module.h index e814faf7..afd1713e 100644 --- a/hydra_dsg_builder/include/hydra_dsg_builder/incremental_dsg_lcd.h +++ b/hydra/include/hydra/loop_closure/loop_closure_module.h @@ -33,27 +33,25 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include "hydra_dsg_builder/dsg_lcd_detector.h" -#include "hydra_dsg_builder/incremental_types.h" -#include "hydra_dsg_builder/lcd_module_config.h" -#include "hydra_dsg_builder/shared_module_state.h" - -#include - #include #include +#include "hydra/common/common.h" +#include "hydra/common/robot_prefix_config.h" +#include "hydra/common/shared_module_state.h" +#include "hydra/loop_closure/detector.h" +#include "hydra/loop_closure/loop_closure_config.h" + namespace hydra { -namespace incremental { -class DsgLcd { +class LoopClosureModule { public: - DsgLcd(const RobotPrefixConfig& prefix, - const DsgLcdModuleConfig& config, - const SharedDsgInfo::Ptr& dsg, - const SharedModuleState::Ptr& state); + LoopClosureModule(const RobotPrefixConfig& prefix, + const LoopClosureConfig& config, + const SharedDsgInfo::Ptr& dsg, + const SharedModuleState::Ptr& state); - virtual ~DsgLcd(); + virtual ~LoopClosureModule(); void start(); @@ -65,7 +63,7 @@ class DsgLcd { bool spinOnce(bool force_update); - lcd::DsgLcdDetector& getDetector() const; + lcd::LcdDetector& getDetector() const; protected: void spinOnceImpl(bool force_update); @@ -81,16 +79,15 @@ class DsgLcd { std::unique_ptr spin_thread_; RobotPrefixConfig prefix_; - DsgLcdModuleConfig config_; + LoopClosureConfig config_; SharedDsgInfo::Ptr dsg_; SharedModuleState::Ptr state_; std::priority_queue, std::greater> agent_queue_; std::list potential_lcd_root_nodes_; - std::unique_ptr lcd_detector_; + std::unique_ptr lcd_detector_; DynamicSceneGraph::Ptr lcd_graph_; }; -} // namespace incremental } // namespace hydra diff --git a/hydra_dsg_builder/include/hydra_dsg_builder/dsg_lcd_registration.h b/hydra/include/hydra/loop_closure/registration.h similarity index 98% rename from hydra_dsg_builder/include/hydra_dsg_builder/dsg_lcd_registration.h rename to hydra/include/hydra/loop_closure/registration.h index 2e2228bb..c0ad0eb7 100644 --- a/hydra_dsg_builder/include/hydra_dsg_builder/dsg_lcd_registration.h +++ b/hydra/include/hydra/loop_closure/registration.h @@ -39,9 +39,9 @@ #include -#include "hydra_dsg_builder/dsg_lcd_matching.h" -#include "hydra_dsg_builder/subgraph_extraction.h" -#include "hydra_dsg_builder/incremental_types.h" +#include "hydra/common/common.h" +#include "hydra/loop_closure/descriptor_matching.h" +#include "hydra/loop_closure/subgraph_extraction.h" namespace hydra { namespace lcd { diff --git a/hydra_dsg_builder/include/hydra_dsg_builder/dsg_lcd_descriptors.h b/hydra/include/hydra/loop_closure/scene_graph_descriptors.h similarity index 97% rename from hydra_dsg_builder/include/hydra_dsg_builder/dsg_lcd_descriptors.h rename to hydra/include/hydra/loop_closure/scene_graph_descriptors.h index f8aac030..6cf05cbf 100644 --- a/hydra_dsg_builder/include/hydra_dsg_builder/dsg_lcd_descriptors.h +++ b/hydra/include/hydra/loop_closure/scene_graph_descriptors.h @@ -33,9 +33,8 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include "hydra_dsg_builder/subgraph_extraction.h" - -#include +#include "hydra/common/dsg_types.h" +#include "hydra/loop_closure/subgraph_extraction.h" namespace hydra { namespace lcd { diff --git a/hydra_dsg_builder/include/hydra_dsg_builder/subgraph_extraction.h b/hydra/include/hydra/loop_closure/subgraph_extraction.h similarity index 98% rename from hydra_dsg_builder/include/hydra_dsg_builder/subgraph_extraction.h rename to hydra/include/hydra/loop_closure/subgraph_extraction.h index 4c610a8e..12c38b35 100644 --- a/hydra_dsg_builder/include/hydra_dsg_builder/subgraph_extraction.h +++ b/hydra/include/hydra/loop_closure/subgraph_extraction.h @@ -33,7 +33,7 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include +#include "hydra/common/dsg_types.h" namespace hydra { diff --git a/hydra_topology/include/hydra_topology/compression_graph_extractor.h b/hydra/include/hydra/places/compression_graph_extractor.h similarity index 97% rename from hydra_topology/include/hydra_topology/compression_graph_extractor.h rename to hydra/include/hydra/places/compression_graph_extractor.h index 00df06c6..4b44013f 100644 --- a/hydra_topology/include/hydra_topology/compression_graph_extractor.h +++ b/hydra/include/hydra/places/compression_graph_extractor.h @@ -33,14 +33,12 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include - #include -#include "hydra_topology/graph_extractor_interface.h" +#include "hydra/places/graph_extractor_interface.h" namespace hydra { -namespace topology { +namespace places { struct IndexVoxelPair { GlobalIndex index; @@ -165,5 +163,5 @@ class CompressionGraphExtractor : public GraphExtractorInterface { std::unordered_set archived_node_ids_; }; -} // namespace topology +} // namespace places } // namespace hydra diff --git a/hydra_topology/include/hydra_topology/floodfill_graph_extractor.h b/hydra/include/hydra/places/floodfill_graph_extractor.h similarity index 96% rename from hydra_topology/include/hydra_topology/floodfill_graph_extractor.h rename to hydra/include/hydra/places/floodfill_graph_extractor.h index 35fe7f18..b8e9a270 100644 --- a/hydra_topology/include/hydra_topology/floodfill_graph_extractor.h +++ b/hydra/include/hydra/places/floodfill_graph_extractor.h @@ -33,14 +33,14 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include "hydra_topology/graph_extractor_interface.h" -#include "hydra_topology/graph_extractor_types.h" -#include "hydra_topology/graph_extractor_utilities.h" - #include +#include "hydra/places/graph_extractor_interface.h" +#include "hydra/places/graph_extractor_types.h" +#include "hydra/places/graph_extractor_utilities.h" + namespace hydra { -namespace topology { +namespace places { class FloodfillGraphExtractor : public GraphExtractorInterface { public: @@ -128,5 +128,5 @@ class FloodfillGraphExtractor : public GraphExtractorInterface { std::set connected_edges_; }; -} // namespace topology +} // namespace places } // namespace hydra diff --git a/hydra_topology/include/hydra_topology/graph_extractor_config.h b/hydra/include/hydra/places/graph_extractor_config.h similarity index 99% rename from hydra_topology/include/hydra_topology/graph_extractor_config.h rename to hydra/include/hydra/places/graph_extractor_config.h index 4b7e1e4d..e9d84099 100644 --- a/hydra_topology/include/hydra_topology/graph_extractor_config.h +++ b/hydra/include/hydra/places/graph_extractor_config.h @@ -37,7 +37,7 @@ #include namespace hydra { -namespace topology { +namespace places { struct OverlapEdgeConfig { size_t num_neighbors_to_check; @@ -158,5 +158,5 @@ void visit_config(const Visitor& v, GraphExtractorConfig& config) { } } -} // namespace topology +} // namespace places } // namespace hydra diff --git a/hydra_topology/include/hydra_topology/graph_extractor_interface.h b/hydra/include/hydra/places/graph_extractor_interface.h similarity index 94% rename from hydra_topology/include/hydra_topology/graph_extractor_interface.h rename to hydra/include/hydra/places/graph_extractor_interface.h index e5b73536..c68b56bd 100644 --- a/hydra_topology/include/hydra_topology/graph_extractor_interface.h +++ b/hydra/include/hydra/places/graph_extractor_interface.h @@ -33,15 +33,14 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include - -#include "hydra_topology/graph_extractor_config.h" -#include "hydra_topology/gvd_graph.h" -#include "hydra_topology/gvd_voxel.h" -#include "hydra_topology/voxblox_types.h" +#include "hydra/common/dsg_types.h" +#include "hydra/places/graph_extractor_config.h" +#include "hydra/places/gvd_graph.h" +#include "hydra/places/gvd_voxel.h" +#include "hydra/places/voxblox_types.h" namespace hydra { -namespace topology { +namespace places { class GraphExtractorInterface { public: @@ -120,5 +119,5 @@ class GraphExtractorInterface { IsolatedSceneGraphLayer::Ptr graph_; }; -} // namespace topology +} // namespace places } // namespace hydra diff --git a/hydra_topology/include/hydra_topology/graph_extractor_types.h b/hydra/include/hydra/places/graph_extractor_types.h similarity index 96% rename from hydra_topology/include/hydra_topology/graph_extractor_types.h rename to hydra/include/hydra/places/graph_extractor_types.h index df755d9a..33563fdc 100644 --- a/hydra_topology/include/hydra_topology/graph_extractor_types.h +++ b/hydra/include/hydra/places/graph_extractor_types.h @@ -33,12 +33,11 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include "hydra_topology/voxblox_types.h" - -#include +#include "hydra/common/dsg_types.h" +#include "hydra/places/voxblox_types.h" namespace hydra { -namespace topology { +namespace places { struct VoxelGraphInfo { // TODO(nathan) consider copy constructor-eqsue cleanup of extract edges @@ -84,5 +83,5 @@ std::ostream& operator<<(std::ostream& out, const VoxelGraphInfo& info); std::ostream& operator<<(std::ostream& out, const EdgeInfo& info); -} // namespace topology +} // namespace places } // namespace hydra diff --git a/hydra_topology/include/hydra_topology/graph_extractor_utilities.h b/hydra/include/hydra/places/graph_extractor_utilities.h similarity index 96% rename from hydra_topology/include/hydra_topology/graph_extractor_utilities.h rename to hydra/include/hydra/places/graph_extractor_utilities.h index 4ffb8464..d103aaf0 100644 --- a/hydra_topology/include/hydra_topology/graph_extractor_utilities.h +++ b/hydra/include/hydra/places/graph_extractor_utilities.h @@ -33,17 +33,16 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include "hydra_topology/graph_extractor_config.h" -#include "hydra_topology/gvd_voxel.h" -#include "hydra_topology/voxblox_types.h" - -#include - #include #include +#include "hydra/common/dsg_types.h" +#include "hydra/places/graph_extractor_config.h" +#include "hydra/places/gvd_voxel.h" +#include "hydra/places/voxblox_types.h" + namespace hydra { -namespace topology { +namespace places { using EdgeInfoMap = std::map; using NodeIndexMap = std::unordered_map; @@ -131,5 +130,5 @@ void findFreespaceEdges(const FreespaceEdgeConfig& config, const NodeIndexMap& node_index_map, EdgeInfoMap& proposed_edges); -} // namespace topology +} // namespace places } // namespace hydra diff --git a/hydra_topology/include/hydra_topology/gvd_graph.h b/hydra/include/hydra/places/gvd_graph.h similarity index 97% rename from hydra_topology/include/hydra_topology/gvd_graph.h rename to hydra/include/hydra/places/gvd_graph.h index 7a22736e..df8a028b 100644 --- a/hydra_topology/include/hydra_topology/gvd_graph.h +++ b/hydra/include/hydra/places/gvd_graph.h @@ -33,8 +33,6 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include "hydra_topology/voxblox_types.h" - #include #include #include @@ -42,8 +40,10 @@ #include #include +#include "hydra/places/voxblox_types.h" + namespace hydra { -namespace topology { +namespace places { struct GvdMemberInfo { double distance; @@ -84,5 +84,5 @@ class GvdGraph { Nodes nodes_; }; -} // namespace topology +} // namespace places } // namespace hydra diff --git a/hydra_topology/include/hydra_topology/gvd_integrator.h b/hydra/include/hydra/places/gvd_integrator.h similarity index 93% rename from hydra_topology/include/hydra_topology/gvd_integrator.h rename to hydra/include/hydra/places/gvd_integrator.h index d07eadfb..67eaa4ab 100644 --- a/hydra_topology/include/hydra_topology/gvd_integrator.h +++ b/hydra/include/hydra/places/gvd_integrator.h @@ -35,16 +35,17 @@ #pragma once #include -#include "hydra_topology/graph_extractor_interface.h" -#include "hydra_topology/gvd_integrator_config.h" -#include "hydra_topology/gvd_parent_tracker.h" -#include "hydra_topology/gvd_utilities.h" -#include "hydra_topology/gvd_voxel.h" -#include "hydra_topology/update_statistics.h" -#include "hydra_topology/voxblox_types.h" +#include "hydra/places/graph_extractor_interface.h" +#include "hydra/places/gvd_integrator_config.h" +#include "hydra/places/gvd_parent_tracker.h" +#include "hydra/places/gvd_utilities.h" +#include "hydra/places/gvd_voxel.h" +#include "hydra/places/update_statistics.h" +#include "hydra/places/voxblox_types.h" +#include "hydra/places/vertex_voxel.h" namespace hydra { -namespace topology { +namespace places { struct OpenQueueEntry { GlobalIndex index; @@ -145,5 +146,5 @@ class GvdIntegrator { FloatingPoint min_integration_distance_m_; }; -} // namespace topology +} // namespace places } // namespace hydra diff --git a/hydra_topology/include/hydra_topology/gvd_integrator_config.h b/hydra/include/hydra/places/gvd_integrator_config.h similarity index 96% rename from hydra_topology/include/hydra_topology/gvd_integrator_config.h rename to hydra/include/hydra/places/gvd_integrator_config.h index 922389b5..d6e1683f 100644 --- a/hydra_topology/include/hydra_topology/gvd_integrator_config.h +++ b/hydra/include/hydra/places/gvd_integrator_config.h @@ -33,10 +33,10 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include "hydra_topology/graph_extractor_config.h" +#include "hydra/places/graph_extractor_config.h" namespace hydra { -namespace topology { +namespace places { enum class ParentUniquenessMode { ANGLE, @@ -44,11 +44,11 @@ enum class ParentUniquenessMode { L1_THEN_ANGLE, }; -} // namespace topology +} // namespace places } // namespace hydra namespace hydra { -namespace topology { +namespace places { struct VoronoiCheckConfig { ParentUniquenessMode mode = ParentUniquenessMode::L1_THEN_ANGLE; @@ -94,5 +94,5 @@ void visit_config(const Visitor& v, GvdIntegratorConfig& config) { v.visit("graph_extractor", config.graph_extractor); } -} // namespace topology +} // namespace places } // namespace hydra diff --git a/hydra_topology/include/hydra_topology/gvd_parent_tracker.h b/hydra/include/hydra/places/gvd_parent_tracker.h similarity index 97% rename from hydra_topology/include/hydra_topology/gvd_parent_tracker.h rename to hydra/include/hydra/places/gvd_parent_tracker.h index 4e48f0b7..9fd36db2 100644 --- a/hydra_topology/include/hydra_topology/gvd_parent_tracker.h +++ b/hydra/include/hydra/places/gvd_parent_tracker.h @@ -33,12 +33,12 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include "hydra_topology/gvd_voxel.h" - #include +#include "hydra/places/gvd_voxel.h" + namespace hydra { -namespace topology { +namespace places { // forward declare to avoid header struct VoronoiCheckConfig; @@ -59,5 +59,5 @@ struct GvdParentTracker { GvdVertexMap parent_vertices; }; -} // namespace topology +} // namespace places } // namespace hydra diff --git a/hydra_topology/include/hydra_topology/gvd_thinning.h b/hydra/include/hydra/places/gvd_thinning.h similarity index 98% rename from hydra_topology/include/hydra_topology/gvd_thinning.h rename to hydra/include/hydra/places/gvd_thinning.h index da2ce496..7ae8e1a9 100644 --- a/hydra_topology/include/hydra_topology/gvd_thinning.h +++ b/hydra/include/hydra/places/gvd_thinning.h @@ -37,11 +37,11 @@ #include namespace hydra { -namespace topology { +namespace places { bool noEulerChange(const std::bitset<26>& neighborhood); bool isSimplePoint(const std::bitset<26>& neighborhood); -} // namespace topology +} // namespace places } // namespace hydra diff --git a/hydra_topology/include/hydra_topology/gvd_utilities.h b/hydra/include/hydra/places/gvd_utilities.h similarity index 96% rename from hydra_topology/include/hydra_topology/gvd_utilities.h rename to hydra/include/hydra/places/gvd_utilities.h index d46a2618..e034cc23 100644 --- a/hydra_topology/include/hydra_topology/gvd_utilities.h +++ b/hydra/include/hydra/places/gvd_utilities.h @@ -33,12 +33,12 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include "hydra_topology/gvd_integrator_config.h" -#include "hydra_topology/gvd_voxel.h" -#include "hydra_topology/voxblox_types.h" +#include "hydra/places/gvd_integrator_config.h" +#include "hydra/places/gvd_voxel.h" +#include "hydra/places/voxblox_types.h" namespace hydra { -namespace topology { +namespace places { struct DistancePotential { bool is_lower; @@ -99,5 +99,5 @@ inline bool isParentUnique(const VoronoiCheckConfig& config, } } -} // namespace topology +} // namespace places } // namespace hydra diff --git a/hydra_topology/include/hydra_topology/gvd_voxel.h b/hydra/include/hydra/places/gvd_voxel.h similarity index 91% rename from hydra_topology/include/hydra_topology/gvd_voxel.h rename to hydra/include/hydra/places/gvd_voxel.h index 115721ee..f3abcff3 100644 --- a/hydra_topology/include/hydra_topology/gvd_voxel.h +++ b/hydra/include/hydra/places/gvd_voxel.h @@ -35,16 +35,10 @@ #pragma once #include -#include "hydra_topology/voxblox_types.h" +#include "hydra/places/voxblox_types.h" namespace hydra { -namespace topology { - -struct VertexVoxel { - bool on_surface = false; - size_t block_vertex_index; - int32_t mesh_block[3]; -}; +namespace places { // TODO(nathan) packed? struct GvdVoxel { @@ -141,14 +135,5 @@ inline Eigen::Matrix getVoxelPosition(const Layer& layer .cast(); } -template -std::string showIndex(const Eigen::Matrix& vector) { - std::stringstream ss; - const Eigen::IOFormat format( - Eigen::FullPrecision, Eigen::DontAlignCols, ", ", ", ", "", "", "[", "]"); - ss << vector.format(format); - return ss.str(); -} - -} // namespace topology +} // namespace places } // namespace hydra diff --git a/hydra_topology/include/hydra_topology/nearest_neighbor_utilities.h b/hydra/include/hydra/places/nearest_voxel_utilities.h similarity index 81% rename from hydra_topology/include/hydra_topology/nearest_neighbor_utilities.h rename to hydra/include/hydra/places/nearest_voxel_utilities.h index d558f5d2..0f708d55 100644 --- a/hydra_topology/include/hydra_topology/nearest_neighbor_utilities.h +++ b/hydra/include/hydra/places/nearest_voxel_utilities.h @@ -33,38 +33,14 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include -#include "hydra_topology/voxblox_types.h" - #include #include #include -namespace hydra { -namespace topology { - -// TODO(nathan) this probably belongs in spark_dsg -class NearestNodeFinder { - public: - using Callback = std::function; - - NearestNodeFinder(const SceneGraphLayer& layer, const std::vector& nodes); - - NearestNodeFinder(const SceneGraphLayer& layer, - const std::unordered_set& nodes); - - virtual ~NearestNodeFinder(); +#include "hydra/places/voxblox_types.h" - void find(const Eigen::Vector3d& position, - size_t num_to_find, - bool skip_first, - const Callback& callback); - - private: - struct Detail; - - std::unique_ptr internals_; -}; +namespace hydra { +namespace places { class NearestVoxelFinder { public: @@ -103,5 +79,5 @@ inline FurthestIndexResult findFurthestIndexFromLine( return findFurthestIndexFromLine(indices, start, end, indices.size()); } -} // namespace topology +} // namespace places } // namespace hydra diff --git a/hydra_topology/include/hydra_topology/update_statistics.h b/hydra/include/hydra/places/update_statistics.h similarity index 98% rename from hydra_topology/include/hydra_topology/update_statistics.h rename to hydra/include/hydra/places/update_statistics.h index e237e129..37627718 100644 --- a/hydra_topology/include/hydra_topology/update_statistics.h +++ b/hydra/include/hydra/places/update_statistics.h @@ -36,7 +36,7 @@ #include namespace hydra { -namespace topology { +namespace places { /** * @brief Tracking statistics for what the integrator did @@ -59,5 +59,5 @@ struct UpdateStatistics { std::ostream& operator<<(std::ostream& out, const UpdateStatistics& stats); -} // namespace topology +} // namespace places } // namespace hydra diff --git a/hydra/include/hydra/places/vertex_voxel.h b/hydra/include/hydra/places/vertex_voxel.h new file mode 100644 index 00000000..285719e6 --- /dev/null +++ b/hydra/include/hydra/places/vertex_voxel.h @@ -0,0 +1,49 @@ +/* ----------------------------------------------------------------------------- + * Copyright 2022 Massachusetts Institute of Technology. + * All Rights Reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Research was sponsored by the United States Air Force Research Laboratory and + * the United States Air Force Artificial Intelligence Accelerator and was + * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views + * and conclusions contained in this document are those of the authors and should + * not be interpreted as representing the official policies, either expressed or + * implied, of the United States Air Force or the U.S. Government. The U.S. + * Government is authorized to reproduce and distribute reprints for Government + * purposes notwithstanding any copyright notation herein. + * -------------------------------------------------------------------------- */ +#pragma once +#include +#include + +namespace hydra { +namespace places { + +struct VertexVoxel { + bool on_surface = false; + size_t block_vertex_index; + int32_t mesh_block[3]; +}; + +} // namespace places +} // namespace hydra diff --git a/hydra_topology/include/hydra_topology/voxblox_types.h b/hydra/include/hydra/places/voxblox_types.h similarity index 98% rename from hydra_topology/include/hydra_topology/voxblox_types.h rename to hydra/include/hydra/places/voxblox_types.h index 5087a3de..9f8a7e6b 100644 --- a/hydra_topology/include/hydra_topology/voxblox_types.h +++ b/hydra/include/hydra/places/voxblox_types.h @@ -42,7 +42,7 @@ #include namespace hydra { -namespace topology { +namespace places { using voxblox::AlignedQueue; using voxblox::Block; @@ -63,5 +63,5 @@ using voxblox::TsdfVoxel; using voxblox::VertexIndex; using voxblox::VoxelIndex; -} // namespace topology +} // namespace places } // namespace hydra diff --git a/hydra_topology/include/hydra_topology/combo_integrator.h b/hydra/include/hydra/reconstruction/combo_integrator.h similarity index 96% rename from hydra_topology/include/hydra_topology/combo_integrator.h rename to hydra/include/hydra/reconstruction/combo_integrator.h index 9dca9ca1..19bbbc1b 100644 --- a/hydra_topology/include/hydra_topology/combo_integrator.h +++ b/hydra/include/hydra/reconstruction/combo_integrator.h @@ -35,11 +35,11 @@ #pragma once #include -#include "hydra_topology/gvd_integrator.h" -#include "hydra_topology/voxel_aware_mesh_integrator.h" +#include "hydra/places/gvd_integrator.h" +#include "hydra/reconstruction/voxel_aware_mesh_integrator.h" namespace hydra { -namespace topology { +namespace places { class ComboIntegrator { public: @@ -89,5 +89,5 @@ class ComboIntegrator { Layer::Ptr vertices_; }; -} // namespace topology +} // namespace places } // namespace hydra diff --git a/hydra_topology/include/hydra_topology/configs.h b/hydra/include/hydra/reconstruction/configs.h similarity index 77% rename from hydra_topology/include/hydra_topology/configs.h rename to hydra/include/hydra/reconstruction/configs.h index 87054254..7a66f929 100644 --- a/hydra_topology/include/hydra_topology/configs.h +++ b/hydra/include/hydra/reconstruction/configs.h @@ -33,24 +33,14 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include #include -#include #include -#include "hydra_topology/gvd_integrator_config.h" +#include "hydra/config/config.h" +#include "hydra/places/gvd_integrator_config.h" -DECLARE_CONFIG_ENUM(voxblox, - ColorMode, - {ColorMode::kColor, "color"}, - {ColorMode::kHeight, "height"}, - {ColorMode::kNormals, "normals"}, - {ColorMode::kGray, "gray"}, - {ColorMode::kLambert, "lambert"}, - {ColorMode::kLambertColor, "lambert_color"}); - -DECLARE_CONFIG_ENUM(hydra::topology, +DECLARE_CONFIG_ENUM(hydra::places, ParentUniquenessMode, {ParentUniquenessMode::ANGLE, "ANGLE"}, {ParentUniquenessMode::L1_DISTANCE, "L1_DISTANCE"}, @@ -76,8 +66,8 @@ void visit_config(const Visitor& v, MeshIntegratorConfig& config) { } // namespace voxblox DECLARE_CONFIG_OSTREAM_OPERATOR(voxblox, MeshIntegratorConfig) -DECLARE_CONFIG_OSTREAM_OPERATOR(hydra::topology, VoronoiCheckConfig) -DECLARE_CONFIG_OSTREAM_OPERATOR(hydra::topology, FloodfillExtractorConfig) -DECLARE_CONFIG_OSTREAM_OPERATOR(hydra::topology, CompressionExtractorConfig) -DECLARE_CONFIG_OSTREAM_OPERATOR(hydra::topology, GraphExtractorConfig) -DECLARE_CONFIG_OSTREAM_OPERATOR(hydra::topology, GvdIntegratorConfig) +DECLARE_CONFIG_OSTREAM_OPERATOR(hydra::places, VoronoiCheckConfig) +DECLARE_CONFIG_OSTREAM_OPERATOR(hydra::places, FloodfillExtractorConfig) +DECLARE_CONFIG_OSTREAM_OPERATOR(hydra::places, CompressionExtractorConfig) +DECLARE_CONFIG_OSTREAM_OPERATOR(hydra::places, GraphExtractorConfig) +DECLARE_CONFIG_OSTREAM_OPERATOR(hydra::places, GvdIntegratorConfig) diff --git a/hydra_topology/include/hydra_topology/reconstruction_config.h b/hydra/include/hydra/reconstruction/reconstruction_config.h similarity index 97% rename from hydra_topology/include/hydra_topology/reconstruction_config.h rename to hydra/include/hydra/reconstruction/reconstruction_config.h index e6c71644..015e0dd9 100644 --- a/hydra_topology/include/hydra_topology/reconstruction_config.h +++ b/hydra/include/hydra/reconstruction/reconstruction_config.h @@ -33,13 +33,13 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include "hydra_topology/configs.h" - -#include -#include #include #include +#include "hydra/config/config.h" +#include "hydra/config/eigen_config_types.h" +#include "hydra/reconstruction/configs.h" + DECLARE_CONFIG_ENUM(kimera, ColorMode, {ColorMode::kColor, "color"}, @@ -111,7 +111,7 @@ struct ReconstructionConfig { size_t num_poses_per_update = 1; size_t max_input_queue_size = 0; bool make_pose_graph = false; - topology::GvdIntegratorConfig gvd; + places::GvdIntegratorConfig gvd; voxblox::TsdfIntegratorBase::Config tsdf; kimera::SemanticIntegratorBase::SemanticConfig semantics; voxblox::MeshIntegratorConfig mesh; diff --git a/hydra_topology/include/hydra_topology/reconstruction_module.h b/hydra/include/hydra/reconstruction/reconstruction_module.h similarity index 90% rename from hydra_topology/include/hydra_topology/reconstruction_module.h rename to hydra/include/hydra/reconstruction/reconstruction_module.h index cf2f362c..96664354 100644 --- a/hydra_topology/include/hydra_topology/reconstruction_module.h +++ b/hydra/include/hydra/reconstruction/reconstruction_module.h @@ -33,26 +33,27 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include -#include -#include -#include -#include #include #include #include -#include "hydra_topology/reconstruction_config.h" -#include "hydra_topology/reconstruction_output.h" +#include "hydra/common/input_queue.h" +#include "hydra/common/robot_prefix_config.h" +#include "hydra/places/gvd_voxel.h" +#include "hydra/places/vertex_voxel.h" +#include "hydra/reconstruction/configs.h" +#include "hydra/reconstruction/reconstruction_config.h" +#include "hydra/reconstruction/reconstruction_output.h" namespace hydra { -namespace topology { +namespace places { // forward declare to avoid include class GvdIntegrator; +} // namespace places + class VoxelAwareMeshIntegrator; -} // namespace topology struct ReconstructionInput { using Ptr = std::shared_ptr; @@ -146,13 +147,13 @@ class ReconstructionModule { voxblox::Layer::Ptr tsdf_; voxblox::Layer::Ptr semantics_; - voxblox::Layer::Ptr gvd_; - voxblox::Layer::Ptr vertices_; + voxblox::Layer::Ptr gvd_; + voxblox::Layer::Ptr vertices_; voxblox::MeshLayer::Ptr mesh_; std::unique_ptr tsdf_integrator_; - std::unique_ptr mesh_integrator_; - std::unique_ptr gvd_integrator_; + std::unique_ptr mesh_integrator_; + std::unique_ptr gvd_integrator_; uint64_t prev_time_; size_t num_poses_received_; diff --git a/hydra_topology/include/hydra_topology/reconstruction_output.h b/hydra/include/hydra/reconstruction/reconstruction_output.h similarity index 85% rename from hydra_topology/include/hydra_topology/reconstruction_output.h rename to hydra/include/hydra/reconstruction/reconstruction_output.h index 53f19206..26f772a9 100644 --- a/hydra_topology/include/hydra_topology/reconstruction_output.h +++ b/hydra/include/hydra/reconstruction/reconstruction_output.h @@ -33,23 +33,32 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include -#include #include +#include #include - #include #include +#include "hydra/common/dsg_types.h" + namespace hydra { +struct ActiveLayerInfo { + using Ptr = std::shared_ptr; + std::vector deleted_nodes; + std::vector deleted_edges; + std::map active_attributes; + std::list edges; +}; + struct ReconstructionOutput { using Ptr = std::shared_ptr; uint64_t timestamp_ns; - hydra_msgs::ActiveLayer::ConstPtr places; - hydra_msgs::ActiveMesh::ConstPtr mesh; + ActiveLayerInfo::Ptr places; + voxblox::MeshLayer::Ptr mesh; + voxblox::IndexSet archived_blocks; std::list pose_graphs; Eigen::Vector3d current_position; }; diff --git a/hydra_topology/tests/utest_main.cpp b/hydra/include/hydra/reconstruction/voxblox_utilities.h similarity index 84% rename from hydra_topology/tests/utest_main.cpp rename to hydra/include/hydra/reconstruction/voxblox_utilities.h index 4f7bc4df..a20d4adb 100644 --- a/hydra_topology/tests/utest_main.cpp +++ b/hydra/include/hydra/reconstruction/voxblox_utilities.h @@ -32,21 +32,19 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include -#include -#include +#pragma once +#include +#include -auto main(int argc, char **argv) -> int { - ::testing::InitGoogleTest(&argc, argv); +namespace hydra { - ros::init(argc, argv, "utest_hydra_topology"); - - FLAGS_minloglevel = 3; - FLAGS_logtostderr = 1; - FLAGS_colorlogtostderr = 1; - - google::ParseCommandLineFlags(&argc, &argv, true); - google::InitGoogleLogging(argv[0]); - - return RUN_ALL_TESTS(); +template +std::string showIndex(const Eigen::Matrix& vector) { + std::stringstream ss; + const Eigen::IOFormat format( + Eigen::FullPrecision, Eigen::DontAlignCols, ", ", ", ", "", "", "[", "]"); + ss << vector.format(format); + return ss.str(); } + +} // namespace hydra diff --git a/hydra_topology/include/hydra_topology/voxel_aware_marching_cubes.h b/hydra/include/hydra/reconstruction/voxel_aware_marching_cubes.h similarity index 85% rename from hydra_topology/include/hydra_topology/voxel_aware_marching_cubes.h rename to hydra/include/hydra/reconstruction/voxel_aware_marching_cubes.h index 81c1f18b..32a6f90c 100644 --- a/hydra_topology/include/hydra_topology/voxel_aware_marching_cubes.h +++ b/hydra/include/hydra/reconstruction/voxel_aware_marching_cubes.h @@ -35,15 +35,13 @@ #pragma once #include -#include "hydra_topology/gvd_voxel.h" -#include "hydra_topology/voxblox_types.h" +#include "hydra/places/vertex_voxel.h" namespace hydra { -namespace topology { -using PointMatrix = Eigen::Matrix; -using SdfMatrix = Eigen::Matrix; -using EdgeIndexMatrix = Eigen::Matrix; +using PointMatrix = Eigen::Matrix; +using SdfMatrix = Eigen::Matrix; +using EdgeIndexMatrix = Eigen::Matrix; void interpolateEdges(const PointMatrix& vertex_coords, const SdfMatrix& vertex_sdf, @@ -61,13 +59,12 @@ class VoxelAwareMarchingCubes : voxblox::MarchingCubes { virtual ~VoxelAwareMarchingCubes() = default; - static void meshCube(const BlockIndex& block, + static void meshCube(const voxblox::BlockIndex& block, const PointMatrix& vertex_coords, const SdfMatrix& vertex_sdf, - VertexIndex* next_index, - Mesh* mesh, - const std::vector& vertex_voxels); + voxblox::VertexIndex* next_index, + voxblox::Mesh* mesh, + const std::vector& vertex_voxels); }; -} // namespace topology } // namespace hydra diff --git a/hydra_topology/include/hydra_topology/voxel_aware_mesh_integrator.h b/hydra/include/hydra/reconstruction/voxel_aware_mesh_integrator.h similarity index 62% rename from hydra_topology/include/hydra_topology/voxel_aware_mesh_integrator.h rename to hydra/include/hydra/reconstruction/voxel_aware_mesh_integrator.h index 63c9d803..c7355ec5 100644 --- a/hydra_topology/include/hydra_topology/voxel_aware_mesh_integrator.h +++ b/hydra/include/hydra/reconstruction/voxel_aware_mesh_integrator.h @@ -35,56 +35,55 @@ #pragma once #include -#include "hydra_topology/gvd_voxel.h" -#include "hydra_topology/voxblox_types.h" +#include "hydra/places/vertex_voxel.h" namespace hydra { -namespace topology { -class VoxelAwareMeshIntegrator : public voxblox::MeshIntegrator { +class VoxelAwareMeshIntegrator : public voxblox::MeshIntegrator { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW VoxelAwareMeshIntegrator(const voxblox::MeshIntegratorConfig& config, - Layer* sdf_layer, - const Layer::Ptr& vertex_layer, - MeshLayer* mesh_layer); + voxblox::Layer* sdf_layer, + const voxblox::Layer::Ptr& vertex_layer, + voxblox::MeshLayer* mesh_layer); virtual ~VoxelAwareMeshIntegrator() = default; virtual void generateMesh(bool only_mesh_updated_blocks, bool clear_updated_flag) override; - virtual void extractMeshInsideBlock(const Block& block, - const VoxelIndex& index, + virtual void extractMeshInsideBlock(const voxblox::Block& block, + const voxblox::VoxelIndex& index, const voxblox::Point& coords, - VertexIndex* next_mesh_index, - Mesh* mesh) override; + voxblox::VertexIndex* next_mesh_index, + voxblox::Mesh* mesh) override; - virtual void extractMeshOnBorder(const Block& block, - const VoxelIndex& index, + virtual void extractMeshOnBorder(const voxblox::Block& block, + const voxblox::VoxelIndex& index, const voxblox::Point& coords, - VertexIndex* next_mesh_index, - Mesh* mesh) override; + voxblox::VertexIndex* next_mesh_index, + voxblox::Mesh* mesh) override; - void processInterior(const BlockIndexList& blocks, ThreadSafeIndex* index_getter); + void processInterior(const voxblox::BlockIndexList& blocks, + voxblox::ThreadSafeIndex* index_getter); - void processExterior(const BlockIndexList& blocks, ThreadSafeIndex* index_getter); + void processExterior(const voxblox::BlockIndexList& blocks, + voxblox::ThreadSafeIndex* index_getter); - void updateBlockInterior(const BlockIndex& block_index); + void updateBlockInterior(const voxblox::BlockIndex& block_index); - void updateBlockExterior(const BlockIndex& block_index); + void updateBlockExterior(const voxblox::BlockIndex& block_index); - void launchThreads(const BlockIndexList& blocks, bool interior_pass); + void launchThreads(const voxblox::BlockIndexList& blocks, bool interior_pass); - BlockIndex getNeighborBlockIndex(const BlockIndex& block_idx, - VoxelIndex& corner_index); + voxblox::BlockIndex getNeighborBlockIndex(const voxblox::BlockIndex& block_idx, + voxblox::VoxelIndex& corner_index); protected: - Layer::Ptr vertex_layer_; + voxblox::Layer::Ptr vertex_layer_; - Eigen::Matrix cube_coord_offsets_; + Eigen::Matrix cube_coord_offsets_; }; -} // namespace topology } // namespace hydra diff --git a/hydra_dsg_builder/include/hydra_dsg_builder/graph_clustering.h b/hydra/include/hydra/rooms/graph_clustering.h similarity index 98% rename from hydra_dsg_builder/include/hydra_dsg_builder/graph_clustering.h rename to hydra/include/hydra/rooms/graph_clustering.h index df3a4add..9bad3a63 100644 --- a/hydra_dsg_builder/include/hydra_dsg_builder/graph_clustering.h +++ b/hydra/include/hydra/rooms/graph_clustering.h @@ -33,7 +33,7 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include +#include "hydra/common/dsg_types.h" namespace hydra { diff --git a/hydra_dsg_builder/include/hydra_dsg_builder/graph_filtration.h b/hydra/include/hydra/rooms/graph_filtration.h similarity index 98% rename from hydra_dsg_builder/include/hydra_dsg_builder/graph_filtration.h rename to hydra/include/hydra/rooms/graph_filtration.h index 25b1e2bc..5816aa2a 100644 --- a/hydra_dsg_builder/include/hydra_dsg_builder/graph_filtration.h +++ b/hydra/include/hydra/rooms/graph_filtration.h @@ -33,9 +33,8 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include - -#include "hydra_dsg_builder/disjoint_set.h" +#include "hydra/common/dsg_types.h" +#include "hydra/utils/disjoint_set.h" namespace hydra { diff --git a/hydra_dsg_builder/include/hydra_dsg_builder/room_finder.h b/hydra/include/hydra/rooms/room_finder.h similarity index 95% rename from hydra_dsg_builder/include/hydra_dsg_builder/room_finder.h rename to hydra/include/hydra/rooms/room_finder.h index 9171f0ab..0a0fa7da 100644 --- a/hydra_dsg_builder/include/hydra_dsg_builder/room_finder.h +++ b/hydra/include/hydra/rooms/room_finder.h @@ -33,13 +33,12 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include "hydra_dsg_builder/graph_clustering.h" -#include "hydra_dsg_builder/room_finder_config.h" - -#include - -#include #include +#include + +#include "hydra/common/dsg_types.h" +#include "hydra/rooms/graph_clustering.h" +#include "hydra/rooms/room_finder_config.h" namespace hydra { diff --git a/hydra_dsg_builder/include/hydra_dsg_builder/room_finder_config.h b/hydra/include/hydra/rooms/room_finder_config.h similarity index 99% rename from hydra_dsg_builder/include/hydra_dsg_builder/room_finder_config.h rename to hydra/include/hydra/rooms/room_finder_config.h index f474dd09..0ac4e1e9 100644 --- a/hydra_dsg_builder/include/hydra_dsg_builder/room_finder_config.h +++ b/hydra/include/hydra/rooms/room_finder_config.h @@ -33,7 +33,7 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include +#include "hydra/config/config.h" namespace hydra { diff --git a/hydra_dsg_builder/include/hydra_dsg_builder/room_helpers.h b/hydra/include/hydra/rooms/room_utilities.h similarity index 98% rename from hydra_dsg_builder/include/hydra_dsg_builder/room_helpers.h rename to hydra/include/hydra/rooms/room_utilities.h index 38943810..4e0446c0 100644 --- a/hydra_dsg_builder/include/hydra_dsg_builder/room_helpers.h +++ b/hydra/include/hydra/rooms/room_utilities.h @@ -33,10 +33,10 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include - #include +#include "hydra/common/dsg_types.h" + namespace hydra { Eigen::Vector3d getRoomPosition(const SceneGraphLayer& places, diff --git a/hydra_dsg_builder/include/hydra_dsg_builder/disjoint_set.h b/hydra/include/hydra/utils/disjoint_set.h similarity index 98% rename from hydra_dsg_builder/include/hydra_dsg_builder/disjoint_set.h rename to hydra/include/hydra/utils/disjoint_set.h index e9b8b0e8..feda56b6 100644 --- a/hydra_dsg_builder/include/hydra_dsg_builder/disjoint_set.h +++ b/hydra/include/hydra/utils/disjoint_set.h @@ -33,10 +33,10 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include - #include +#include "hydra/common/dsg_types.h" + namespace hydra { struct DisjointSet { diff --git a/hydra_utils/include/hydra_utils/display_utils.h b/hydra/include/hydra/utils/display_utilities.h similarity index 97% rename from hydra_utils/include/hydra_utils/display_utils.h rename to hydra/include/hydra/utils/display_utilities.h index a91e3965..a1f79063 100644 --- a/hydra_utils/include/hydra_utils/display_utils.h +++ b/hydra/include/hydra/utils/display_utilities.h @@ -38,8 +38,8 @@ #include #include -namespace hydra_utils { +namespace hydra { std::string getHumanReadableMemoryString(size_t bytes); -} // namespace hydra_utils +} // namespace hydra diff --git a/hydra_utils/include/hydra_utils/log_utilities.h b/hydra/include/hydra/utils/log_utilities.h similarity index 100% rename from hydra_utils/include/hydra_utils/log_utilities.h rename to hydra/include/hydra/utils/log_utilities.h diff --git a/hydra_dsg_builder/include/hydra_dsg_builder/minimum_spanning_tree.h b/hydra/include/hydra/utils/minimum_spanning_tree.h similarity index 98% rename from hydra_dsg_builder/include/hydra_dsg_builder/minimum_spanning_tree.h rename to hydra/include/hydra/utils/minimum_spanning_tree.h index 5451a54d..ed642c76 100644 --- a/hydra_dsg_builder/include/hydra_dsg_builder/minimum_spanning_tree.h +++ b/hydra/include/hydra/utils/minimum_spanning_tree.h @@ -33,7 +33,7 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include +#include "hydra/common/dsg_types.h" namespace hydra { diff --git a/hydra_topology/include/hydra_topology/ros_interface.h b/hydra/include/hydra/utils/nearest_neighbor_utilities.h similarity index 77% rename from hydra_topology/include/hydra_topology/ros_interface.h rename to hydra/include/hydra/utils/nearest_neighbor_utilities.h index f95f787a..c2a80db8 100644 --- a/hydra_topology/include/hydra_topology/ros_interface.h +++ b/hydra/include/hydra/utils/nearest_neighbor_utilities.h @@ -33,27 +33,34 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include "hydra_topology/configs.h" -#include "hydra_topology/topology_server_visualizer.h" +#include +#include +#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include +#include "hydra/common/dsg_types.h" namespace hydra { -namespace topology { -class TopologyRosInterface : public TopologyModule { - public: - TopologyRosInterface(); +class NearestNodeFinder { + public: + using Callback = std::function; + + NearestNodeFinder(const SceneGraphLayer& layer, const std::vector& nodes); + + NearestNodeFinder(const SceneGraphLayer& layer, + const std::unordered_set& nodes); + + virtual ~NearestNodeFinder(); + + void find(const Eigen::Vector3d& position, + size_t num_to_find, + bool skip_first, + const Callback& callback); + + private: + struct Detail; + + std::unique_ptr internals_; }; -} // namespace topology } // namespace hydra diff --git a/hydra_utils/include/hydra_utils/timing_utilities.h b/hydra/include/hydra/utils/timing_utilities.h similarity index 100% rename from hydra_utils/include/hydra_utils/timing_utilities.h rename to hydra/include/hydra/utils/timing_utilities.h diff --git a/hydra_dsg_builder/models/.gitignore b/hydra/models/.gitignore similarity index 100% rename from hydra_dsg_builder/models/.gitignore rename to hydra/models/.gitignore diff --git a/hydra_dsg_builder/models/lcd/.gitignore b/hydra/models/lcd/.gitignore similarity index 100% rename from hydra_dsg_builder/models/lcd/.gitignore rename to hydra/models/lcd/.gitignore diff --git a/hydra_dsg_builder/models/lcd/object_gnn.onnx b/hydra/models/lcd/object_gnn.onnx similarity index 100% rename from hydra_dsg_builder/models/lcd/object_gnn.onnx rename to hydra/models/lcd/object_gnn.onnx diff --git a/hydra_dsg_builder/models/lcd/place_gnn.onnx b/hydra/models/lcd/place_gnn.onnx similarity index 100% rename from hydra_dsg_builder/models/lcd/place_gnn.onnx rename to hydra/models/lcd/place_gnn.onnx diff --git a/hydra_dsg_builder/output/.gitignore b/hydra/output/.gitignore similarity index 100% rename from hydra_dsg_builder/output/.gitignore rename to hydra/output/.gitignore diff --git a/hydra_dsg_builder/output/default/.gitignore b/hydra/output/default/.gitignore similarity index 100% rename from hydra_dsg_builder/output/default/.gitignore rename to hydra/output/default/.gitignore diff --git a/hydra_dsg_builder/output/default/backend/.gitignore b/hydra/output/default/backend/.gitignore similarity index 100% rename from hydra_dsg_builder/output/default/backend/.gitignore rename to hydra/output/default/backend/.gitignore diff --git a/hydra_dsg_builder/output/default/frontend/.gitignore b/hydra/output/default/frontend/.gitignore similarity index 100% rename from hydra_dsg_builder/output/default/frontend/.gitignore rename to hydra/output/default/frontend/.gitignore diff --git a/hydra_dsg_builder/output/default/lcd/.gitignore b/hydra/output/default/lcd/.gitignore similarity index 100% rename from hydra_dsg_builder/output/default/lcd/.gitignore rename to hydra/output/default/lcd/.gitignore diff --git a/hydra_dsg_builder/output/default/pgmo/.gitignore b/hydra/output/default/pgmo/.gitignore similarity index 100% rename from hydra_dsg_builder/output/default/pgmo/.gitignore rename to hydra/output/default/pgmo/.gitignore diff --git a/hydra_dsg_builder/output/default/topology/.gitignore b/hydra/output/default/topology/.gitignore similarity index 100% rename from hydra_dsg_builder/output/default/topology/.gitignore rename to hydra/output/default/topology/.gitignore diff --git a/hydra_dsg_builder/output/sidpac/.gitignore b/hydra/output/sidpac/.gitignore similarity index 100% rename from hydra_dsg_builder/output/sidpac/.gitignore rename to hydra/output/sidpac/.gitignore diff --git a/hydra_dsg_builder/output/sidpac/backend/.gitignore b/hydra/output/sidpac/backend/.gitignore similarity index 100% rename from hydra_dsg_builder/output/sidpac/backend/.gitignore rename to hydra/output/sidpac/backend/.gitignore diff --git a/hydra_dsg_builder/output/sidpac/frontend/.gitignore b/hydra/output/sidpac/frontend/.gitignore similarity index 100% rename from hydra_dsg_builder/output/sidpac/frontend/.gitignore rename to hydra/output/sidpac/frontend/.gitignore diff --git a/hydra_dsg_builder/output/sidpac/lcd/.gitignore b/hydra/output/sidpac/lcd/.gitignore similarity index 100% rename from hydra_dsg_builder/output/sidpac/lcd/.gitignore rename to hydra/output/sidpac/lcd/.gitignore diff --git a/hydra_dsg_builder/output/sidpac/pgmo/.gitignore b/hydra/output/sidpac/pgmo/.gitignore similarity index 100% rename from hydra_dsg_builder/output/sidpac/pgmo/.gitignore rename to hydra/output/sidpac/pgmo/.gitignore diff --git a/hydra_dsg_builder/output/sidpac/topology/.gitignore b/hydra/output/sidpac/topology/.gitignore similarity index 100% rename from hydra_dsg_builder/output/sidpac/topology/.gitignore rename to hydra/output/sidpac/topology/.gitignore diff --git a/hydra_dsg_builder/output/uhumans2/.gitignore b/hydra/output/uhumans2/.gitignore similarity index 100% rename from hydra_dsg_builder/output/uhumans2/.gitignore rename to hydra/output/uhumans2/.gitignore diff --git a/hydra_dsg_builder/output/uhumans2/backend/.gitignore b/hydra/output/uhumans2/backend/.gitignore similarity index 100% rename from hydra_dsg_builder/output/uhumans2/backend/.gitignore rename to hydra/output/uhumans2/backend/.gitignore diff --git a/hydra_dsg_builder/output/uhumans2/frontend/.gitignore b/hydra/output/uhumans2/frontend/.gitignore similarity index 100% rename from hydra_dsg_builder/output/uhumans2/frontend/.gitignore rename to hydra/output/uhumans2/frontend/.gitignore diff --git a/hydra_dsg_builder/output/uhumans2/lcd/.gitignore b/hydra/output/uhumans2/lcd/.gitignore similarity index 100% rename from hydra_dsg_builder/output/uhumans2/lcd/.gitignore rename to hydra/output/uhumans2/lcd/.gitignore diff --git a/hydra_dsg_builder/output/uhumans2/pgmo/.gitignore b/hydra/output/uhumans2/pgmo/.gitignore similarity index 100% rename from hydra_dsg_builder/output/uhumans2/pgmo/.gitignore rename to hydra/output/uhumans2/pgmo/.gitignore diff --git a/hydra_dsg_builder/output/uhumans2/topology/.gitignore b/hydra/output/uhumans2/topology/.gitignore similarity index 100% rename from hydra_dsg_builder/output/uhumans2/topology/.gitignore rename to hydra/output/uhumans2/topology/.gitignore diff --git a/hydra_dsg_builder/package.xml b/hydra/package.xml similarity index 87% rename from hydra_dsg_builder/package.xml rename to hydra/package.xml index cf6be58e..0ea7718e 100644 --- a/hydra_dsg_builder/package.xml +++ b/hydra/package.xml @@ -1,6 +1,6 @@ - hydra_dsg_builder + hydra 1.0.0 Utilities for building a scene graph from sensor data @@ -12,11 +12,9 @@ catkin gtsam - hydra_topology kimera_pgmo kimera_semantics spark_dsg - std_msgs teaserpp tf2_eigen voxblox diff --git a/hydra_dsg_builder/scripts/export_label_embeddings.py b/hydra/scripts/export_label_embeddings.py similarity index 50% rename from hydra_dsg_builder/scripts/export_label_embeddings.py rename to hydra/scripts/export_label_embeddings.py index 7857fd61..3ea9b340 100644 --- a/hydra_dsg_builder/scripts/export_label_embeddings.py +++ b/hydra/scripts/export_label_embeddings.py @@ -1,4 +1,37 @@ #!/usr/bin/env python3 +# Copyright 2022, Massachusetts Institute of Technology. +# All Rights Reserved +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +# WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +# +# Research was sponsored by the United States Air Force Research Laboratory and +# the United States Air Force Artificial Intelligence Accelerator and was +# accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views +# and conclusions contained in this document are those of the authors and should +# not be interpreted as representing the official policies, either expressed or +# implied, of the United States Air Force or the U.S. Government. The U.S. +# Government is authorized to reproduce and distribute reprints for Government +# purposes notwithstanding any copyright notation herein. + """Quick script to export label embeddings.""" import gensim.models as gm import numpy as np diff --git a/hydra/scripts/make_colormap.py b/hydra/scripts/make_colormap.py new file mode 100644 index 00000000..578c0ae3 --- /dev/null +++ b/hydra/scripts/make_colormap.py @@ -0,0 +1,60 @@ +# Copyright 2022, Massachusetts Institute of Technology. +# All Rights Reserved +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +# WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +# +# Research was sponsored by the United States Air Force Research Laboratory and +# the United States Air Force Artificial Intelligence Accelerator and was +# accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views +# and conclusions contained in this document are those of the authors and should +# not be interpreted as representing the official policies, either expressed or +# implied, of the United States Air Force or the U.S. Government. The U.S. +# Government is authorized to reproduce and distribute reprints for Government +# purposes notwithstanding any copyright notation herein. +"""Make a colormap.""" +import click +import seaborn as sns +import matplotlib.pyplot as plt + + +@click.command() +@click.option("-n", "--name", default=None) +@click.option("-s", "--show", is_flag=True) +def main(name, show): + """Construct a qualitative colormap.""" + sns.set() + palette = sns.color_palette() if name is None else sns.color_palette(name) + print(name) + + if show: + sns.palplot(palette) + plt.show() + + for color in palette: + r = int(255 * color[0]) + g = int(255 * color[1]) + b = int(255 * color[2]) + print(f"{{{r}, {g}, {b}}},") + + +if __name__ == "__main__": + main() diff --git a/hydra_dsg_builder/scripts/modify_dgraph.py b/hydra/scripts/modify_dgraph.py similarity index 100% rename from hydra_dsg_builder/scripts/modify_dgraph.py rename to hydra/scripts/modify_dgraph.py diff --git a/hydra/src/CMakeLists.txt b/hydra/src/CMakeLists.txt new file mode 100644 index 00000000..aa8dc2ec --- /dev/null +++ b/hydra/src/CMakeLists.txt @@ -0,0 +1,53 @@ + target_sources( + ${PROJECT_NAME} + PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/backend/backend_module.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/backend/merge_handler.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/backend/update_functions.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/common/hydra_config.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/common/robot_prefix_config.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/common/shared_module_state.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/config/yaml_parser.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/frontend/frontend_module.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/frontend/mesh_segmenter.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/loop_closure/descriptor_matching.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/loop_closure/detector.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/loop_closure/loop_closure_module.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/loop_closure/registration.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/loop_closure/scene_graph_descriptors.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/loop_closure/subgraph_extraction.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/places/compression_graph_extractor.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/places/floodfill_graph_extractor.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/places/graph_extractor_interface.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/places/graph_extractor_types.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/places/graph_extractor_utilities.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/places/gvd_graph.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/places/gvd_integrator.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/places/gvd_parent_tracker.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/places/gvd_thinning.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/places/gvd_utilities.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/places/gvd_voxel.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/places/nearest_voxel_utilities.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/places/update_statistics.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/reconstruction/reconstruction_config.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/reconstruction/reconstruction_module.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/reconstruction/voxel_aware_marching_cubes.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/reconstruction/voxel_aware_mesh_integrator.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/rooms/graph_clustering.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/rooms/graph_filtration.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/rooms/room_finder.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/rooms/room_utilities.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/utils/disjoint_set.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/utils/display_utilities.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/utils/log_utilities.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/utils/minimum_spanning_tree.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/utils/nearest_neighbor_utilities.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/utils/timing_utilities.cpp + ) + +if(HYDRA_GNN) + target_sources( + ${PROJECT_NAME} + PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/loop_closure/gnn_descriptors.cpp + ) +endif(HYDRA_GNN) diff --git a/hydra_dsg_builder/src/incremental_dsg_backend.cpp b/hydra/src/backend/backend_module.cpp similarity index 91% rename from hydra_dsg_builder/src/incremental_dsg_backend.cpp rename to hydra/src/backend/backend_module.cpp index cd478359..3840e5d6 100644 --- a/hydra_dsg_builder/src/incremental_dsg_backend.cpp +++ b/hydra/src/backend/backend_module.cpp @@ -32,21 +32,19 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_dsg_builder/incremental_dsg_backend.h" +#include "hydra/backend/backend_module.h" #include -#include #include #include -#include "hydra_dsg_builder/hydra_config.h" -#include "hydra_dsg_builder/minimum_spanning_tree.h" -#include "hydra_dsg_builder/room_finder.h" +#include "hydra/common/hydra_config.h" +#include "hydra/rooms/room_finder.h" +#include "hydra/utils/minimum_spanning_tree.h" +#include "hydra/utils/timing_utilities.h" namespace hydra { -namespace incremental { -using hydra::dsg_updates::NodeMergeLog; using hydra::timing::ScopedTimer; using kimera_pgmo::DeformationGraph; using kimera_pgmo::DeformationGraphPtr; @@ -66,12 +64,12 @@ std::optional getTimeNs(const DynamicSceneGraph& graph, gtsam::Symbol return graph.getDynamicNode(node).value().get().timestamp.count(); } -DsgBackend::DsgBackend(const RobotPrefixConfig& prefix, - const DsgBackendConfig& config, - const kimera_pgmo::KimeraPgmoConfig& pgmo_config, - const SharedDsgInfo::Ptr& dsg, - const SharedDsgInfo::Ptr& backend_dsg, - const SharedModuleState::Ptr& state) +BackendModule::BackendModule(const RobotPrefixConfig& prefix, + const BackendConfig& config, + const kimera_pgmo::KimeraPgmoConfig& pgmo_config, + const SharedDsgInfo::Ptr& dsg, + const SharedDsgInfo::Ptr& backend_dsg, + const SharedModuleState::Ptr& state) : KimeraPgmoInterface(), prefix_(prefix), config_(config), @@ -108,21 +106,21 @@ DsgBackend::DsgBackend(const RobotPrefixConfig& prefix, } } -DsgBackend::~DsgBackend() { +BackendModule::~BackendModule() { VLOG(1) << "[Hydra Backend] destructor called!"; stop(); } -void DsgBackend::start() { - spin_thread_.reset(new std::thread(&DsgBackend::spin, this)); +void BackendModule::start() { + spin_thread_.reset(new std::thread(&BackendModule::spin, this)); if (config_.use_zmq_interface) { - zmq_thread_.reset(new std::thread(&DsgBackend::runZmqUpdates, this)); + zmq_thread_.reset(new std::thread(&BackendModule::runZmqUpdates, this)); } LOG(INFO) << "[Hydra Backend] started!"; } -void DsgBackend::stop() { +void BackendModule::stop() { should_shutdown_ = true; if (spin_thread_) { @@ -142,7 +140,7 @@ void DsgBackend::stop() { VLOG(2) << "[Hydra Backend]: " << state_->backend_queue.size() << " messages left"; } -void DsgBackend::save(const std::string& output_path) { +void BackendModule::save(const std::string& output_path) { private_dsg_->graph->save(output_path + "/dsg.json", false); private_dsg_->graph->save(output_path + "/dsg_with_mesh.json"); // TODO(Yun) Technically not strictly a g2o @@ -185,7 +183,7 @@ void DsgBackend::save(const std::string& output_path) { } } -void DsgBackend::spin() { +void BackendModule::spin() { bool should_shutdown = false; while (!should_shutdown) { bool has_data = state_->backend_queue.poll(); @@ -203,7 +201,7 @@ void DsgBackend::spin() { } } -bool DsgBackend::spinOnce(bool force_update) { +bool BackendModule::spinOnce(bool force_update) { bool has_data = state_->backend_queue.poll(); if (!has_data) { return false; @@ -214,7 +212,7 @@ bool DsgBackend::spinOnce(bool force_update) { return true; } -void DsgBackend::spinOnce(const BackendInput& input, bool force_update) { +void BackendModule::spinOnce(const BackendInput& input, bool force_update) { ScopedTimer spin_timer("backend/spin", input.timestamp_ns); status_.reset(); @@ -276,8 +274,8 @@ void DsgBackend::spinOnce(const BackendInput& input, bool force_update) { } } -void DsgBackend::loadState(const std::string& state_path, - const std::string& dgrf_path) { +void BackendModule::loadState(const std::string& state_path, + const std::string& dgrf_path) { const std::string mesh_path = state_path + "/mesh.ply"; pcl::PolygonMesh mesh; @@ -292,11 +290,11 @@ void DsgBackend::loadState(const std::string& state_path, << " vertices for deformation graph"; } -void DsgBackend::setUpdateFuncs(const std::list& update_funcs) { +void BackendModule::setUpdateFuncs(const std::list& update_funcs) { dsg_update_funcs_ = update_funcs; } -void DsgBackend::setSolverParams() { +void BackendModule::setSolverParams() { KimeraRPGO::RobustSolverParams params = deformation_graph_->getParams(); params.verbosity = config_.pgmo.rpgo_verbosity; params.solver = config_.pgmo.rpgo_solver; @@ -308,7 +306,7 @@ void DsgBackend::setSolverParams() { setVerboseFlag(false); } -void DsgBackend::setDefaultUpdateFunctions() { +void BackendModule::setDefaultUpdateFunctions() { update_objects_functor_.reset(new dsg_updates::UpdateObjectsFunctor()); update_objects_functor_->angle_step = config_.angle_step; update_places_functor_.reset(new dsg_updates::UpdatePlacesFunctor( @@ -373,7 +371,7 @@ std::string logPoseGraphConnections(const pose_graph_tools::PoseGraph& msg) { return ss.str(); } -void DsgBackend::updateFactorGraph(const BackendInput& input) { +void BackendModule::updateFactorGraph(const BackendInput& input) { ScopedTimer timer("backend/process_factors", input.timestamp_ns); const size_t prev_loop_closures = num_loop_closures_; @@ -411,7 +409,7 @@ void DsgBackend::updateFactorGraph(const BackendInput& input) { status_.total_values_ = deformation_graph_->getGtsamValues().size(); } -bool DsgBackend::updateFromLcdQueue() { +bool BackendModule::updateFromLcdQueue() { bool added_new_loop_closure = false; while (!state_->backend_lcd_queue.empty()) { const auto result = state_->backend_lcd_queue.pop(); @@ -438,7 +436,7 @@ bool DsgBackend::updateFromLcdQueue() { return added_new_loop_closure; } -void DsgBackend::copyMeshDelta(const BackendInput& input) { +void BackendModule::copyMeshDelta(const BackendInput& input) { ScopedTimer timer("backend/copy_mesh_delta", input.timestamp_ns); input.mesh_update->updateMesh(*private_dsg_->graph->getMeshVertices(), mesh_timestamps_, @@ -450,7 +448,7 @@ void DsgBackend::copyMeshDelta(const BackendInput& input) { have_new_mesh_ = true; } -bool DsgBackend::updatePrivateDsg(size_t timestamp_ns, bool force_update) { +bool BackendModule::updatePrivateDsg(size_t timestamp_ns, bool force_update) { std::unique_lock graph_lock(private_dsg_->mutex); { // start joint critical section cachePlacePos(); // save place positions before grabbing new attributes from @@ -510,7 +508,7 @@ bool DsgBackend::updatePrivateDsg(size_t timestamp_ns, bool force_update) { return true; } -void DsgBackend::cachePlacePos() { +void BackendModule::cachePlacePos() { place_pos_cache_.clear(); const auto& places = private_dsg_->graph->getLayer(DsgLayers::PLACES); for (const auto& id_node_pair : places.nodes()) { @@ -523,7 +521,7 @@ void DsgBackend::cachePlacePos() { } } -void DsgBackend::updatePlacePosFromCache() { +void BackendModule::updatePlacePosFromCache() { const auto& places = private_dsg_->graph->getLayer(DsgLayers::PLACES); for (const auto& id_node_pair : places.nodes()) { auto iter = place_pos_cache_.find(id_node_pair.first); @@ -535,7 +533,7 @@ void DsgBackend::updatePlacePosFromCache() { } } -void DsgBackend::addPlacesToDeformationGraph(size_t timestamp_ns) { +void BackendModule::addPlacesToDeformationGraph(size_t timestamp_ns) { if (shared_places_copy_.nodes().empty()) { LOG(WARNING) << "Attempting to add places to deformation graph without places"; return; @@ -600,10 +598,10 @@ void DsgBackend::addPlacesToDeformationGraph(size_t timestamp_ns) { } // end timing scope } -void DsgBackend::addLoopClosure(const gtsam::Key& src, - const gtsam::Key& dest, - const gtsam::Pose3& src_T_dest, - double variance) { +void BackendModule::addLoopClosure(const gtsam::Key& src, + const gtsam::Key& dest, + const gtsam::Pose3& src_T_dest, + double variance) { if (full_sparse_frame_map_.size() == 0 || !KimeraPgmoInterface::config_.b_enable_sparsify) { deformation_graph_->addNewBetween(src, dest, src_T_dest, gtsam::Pose3(), variance); @@ -625,7 +623,7 @@ void DsgBackend::addLoopClosure(const gtsam::Key& src, } } -void DsgBackend::runZmqUpdates() { +void BackendModule::runZmqUpdates() { while (!should_shutdown_) { if (!zmq_receiver_->recv(config_.zmq_poll_time_ms)) { continue; @@ -659,7 +657,7 @@ void DsgBackend::runZmqUpdates() { } } -void DsgBackend::updateDsgMesh(size_t timestamp_ns, bool force_mesh_update) { +void BackendModule::updateDsgMesh(size_t timestamp_ns, bool force_mesh_update) { if (!force_mesh_update && !have_new_mesh_) { return; } @@ -690,7 +688,7 @@ void DsgBackend::updateDsgMesh(size_t timestamp_ns, bool force_mesh_update) { prev_num_archived_vertices_ = num_archived_vertices_; } -void DsgBackend::optimize(size_t timestamp_ns) { +void BackendModule::optimize(size_t timestamp_ns) { if (config_.add_places_to_deformation_graph) { addPlacesToDeformationGraph(timestamp_ns); } @@ -709,7 +707,7 @@ void DsgBackend::optimize(size_t timestamp_ns) { have_new_loopclosures_ = false; } -void DsgBackend::resetBackendDsg(size_t timestamp_ns) { +void BackendModule::resetBackendDsg(size_t timestamp_ns) { ScopedTimer timer("backend/reset_dsg", timestamp_ns, true, 0, false); merge_handler_->reset(); proposed_node_merges_.clear(); @@ -723,11 +721,11 @@ void DsgBackend::resetBackendDsg(size_t timestamp_ns) { reset_backend_dsg_ = false; } -void DsgBackend::callUpdateFunctions(size_t timestamp_ns, - const gtsam::Values& places_values, - const gtsam::Values& pgmo_values, - bool new_loop_closure, - const LayerMerges& given_merges) { +void BackendModule::callUpdateFunctions(size_t timestamp_ns, + const gtsam::Values& places_values, + const gtsam::Values& pgmo_values, + bool new_loop_closure, + const LayerMerges& given_merges) { bool enable_node_merging = config_.enable_node_merging; if (given_merges.size() > 0) { enable_node_merging = false; @@ -786,7 +784,7 @@ void DsgBackend::callUpdateFunctions(size_t timestamp_ns, } } -void DsgBackend::logStatus(bool init) const { +void BackendModule::logStatus(bool init) const { std::ofstream file; std::string filename = config_.pgmo.log_path + std::string("/dsg_pgmo_status.csv"); if (init) { @@ -815,7 +813,7 @@ void DsgBackend::logStatus(bool init) const { return; } -void DsgBackend::logIncrementalLoopClosures(const PoseGraph& msg) { +void BackendModule::logIncrementalLoopClosures(const PoseGraph& msg) { for (const auto& edge : msg.edges) { if (edge.type != pose_graph_tools::PoseGraphEdge::LOOPCLOSE) { continue; @@ -830,5 +828,4 @@ void DsgBackend::logIncrementalLoopClosures(const PoseGraph& msg) { } } -} // namespace incremental } // namespace hydra diff --git a/hydra_dsg_builder/src/merge_handler.cpp b/hydra/src/backend/merge_handler.cpp similarity index 99% rename from hydra_dsg_builder/src/merge_handler.cpp rename to hydra/src/backend/merge_handler.cpp index f4799e71..9bdf29d1 100644 --- a/hydra_dsg_builder/src/merge_handler.cpp +++ b/hydra/src/backend/merge_handler.cpp @@ -32,10 +32,11 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_dsg_builder/merge_handler.h" +#include "hydra/backend/merge_handler.h" + +#include namespace hydra { -namespace incremental { using PlaceAttrs = PlaceNodeAttributes; using ObjectAttrs = ObjectNodeAttributes; @@ -403,5 +404,4 @@ bool MergeHandler::shouldUndo(const NodeInfo& from_info, } } -} // namespace incremental } // namespace hydra diff --git a/hydra_dsg_builder/src/dsg_update_functions.cpp b/hydra/src/backend/update_functions.cpp similarity index 98% rename from hydra_dsg_builder/src/dsg_update_functions.cpp rename to hydra/src/backend/update_functions.cpp index b6a4f802..6cbcf651 100644 --- a/hydra_dsg_builder/src/dsg_update_functions.cpp +++ b/hydra/src/backend/update_functions.cpp @@ -32,23 +32,21 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_dsg_builder/dsg_update_functions.h" +#include "hydra/backend/update_functions.h" #include #include -#include #include #include #include -#include "hydra_dsg_builder/room_finder.h" +#include "hydra/rooms/room_finder.h" +#include "hydra/utils/timing_utilities.h" namespace hydra { namespace dsg_updates { -using incremental::SharedDsgInfo; using timing::ScopedTimer; -using topology::NearestNodeFinder; using Node = SceneGraphNode; using Centroid = pcl::CentroidPoint; using NodeColor = SemanticNodeAttributes::ColorVector; @@ -117,7 +115,8 @@ void UpdateObjectsFunctor::updateObject(const MeshVertices::Ptr& mesh, indices.reset(new std::vector(connections.begin(), connections.end())); } - attrs.bounding_box = bounding_box::extract(mesh, attrs.bounding_box.type, indices, angle_step); + attrs.bounding_box = + bounding_box::extract(mesh, attrs.bounding_box.type, indices, angle_step); Centroid centroid; for (const auto& idx : *indices) { diff --git a/hydra_dsg_builder/src/hydra_config.cpp b/hydra/src/common/hydra_config.cpp similarity index 98% rename from hydra_dsg_builder/src/hydra_config.cpp rename to hydra/src/common/hydra_config.cpp index 124845cb..ae1eca7e 100644 --- a/hydra_dsg_builder/src/hydra_config.cpp +++ b/hydra/src/common/hydra_config.cpp @@ -32,7 +32,7 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_dsg_builder/hydra_config.h" +#include "hydra/common/hydra_config.h" namespace hydra { diff --git a/hydra_utils/src/robot_prefix_config.cpp b/hydra/src/common/robot_prefix_config.cpp similarity index 98% rename from hydra_utils/src/robot_prefix_config.cpp rename to hydra/src/common/robot_prefix_config.cpp index fd22901f..00b6f8a8 100644 --- a/hydra_utils/src/robot_prefix_config.cpp +++ b/hydra/src/common/robot_prefix_config.cpp @@ -32,7 +32,7 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_utils/robot_prefix_config.h" +#include "hydra/common/robot_prefix_config.h" #include diff --git a/hydra_dsg_builder/src/shared_module_state.cpp b/hydra/src/common/shared_module_state.cpp similarity index 96% rename from hydra_dsg_builder/src/shared_module_state.cpp rename to hydra/src/common/shared_module_state.cpp index 683e8b7c..e7af6d7f 100644 --- a/hydra_dsg_builder/src/shared_module_state.cpp +++ b/hydra/src/common/shared_module_state.cpp @@ -32,12 +32,11 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_dsg_builder/shared_module_state.h" +#include "hydra/common/shared_module_state.h" #include namespace hydra { -namespace incremental { SharedModuleState::SharedModuleState() {} @@ -52,5 +51,4 @@ SharedModuleState::~SharedModuleState() { VLOG(2) << "backend_lcd_queue: " << backend_lcd_queue.size(); } -} // namespace incremental } // namespace hydra diff --git a/hydra_utils/src/yaml_parser.cpp b/hydra/src/config/yaml_parser.cpp similarity index 95% rename from hydra_utils/src/yaml_parser.cpp rename to hydra/src/config/yaml_parser.cpp index 8f9d6a1b..242ab4db 100644 --- a/hydra_utils/src/yaml_parser.cpp +++ b/hydra/src/config/yaml_parser.cpp @@ -32,7 +32,7 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include +#include "hydra/config/yaml_parser.h" namespace config_parser { @@ -57,7 +57,10 @@ YamlParserImpl YamlParserImpl::child(const std::string& child_name) const { return YamlParserImpl(node_[child_name], new_name); } -bool YamlParserImpl::parseImpl(uint8_t& value) const { value = node_.as(); return true; } +bool YamlParserImpl::parseImpl(uint8_t& value) const { + value = node_.as(); + return true; +} std::vector YamlParserImpl::children() const { if (!node_.IsMap()) { diff --git a/hydra_dsg_builder/src/incremental_dsg_frontend.cpp b/hydra/src/frontend/frontend_module.cpp similarity index 82% rename from hydra_dsg_builder/src/incremental_dsg_frontend.cpp rename to hydra/src/frontend/frontend_module.cpp index 384f058b..5d108fda 100644 --- a/hydra_dsg_builder/src/incremental_dsg_frontend.cpp +++ b/hydra/src/frontend/frontend_module.cpp @@ -32,29 +32,29 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_dsg_builder/incremental_dsg_frontend.h" +#include "hydra/frontend/frontend_module.h" #include -#include #include +#include #include #include -#include "hydra_dsg_builder/hydra_config.h" +#include "hydra/common/hydra_config.h" +#include "hydra/utils/timing_utilities.h" namespace hydra { -namespace incremental { using hydra::timing::ScopedTimer; using pose_graph_tools::PoseGraph; using LabelClusters = MeshSegmenter::LabelClusters; -DsgFrontend::DsgFrontend(const RobotPrefixConfig& prefix, - const DsgFrontendConfig& config, - const SharedDsgInfo::Ptr& dsg, - const SharedModuleState::Ptr& state) +FrontendModule::FrontendModule(const RobotPrefixConfig& prefix, + const FrontendConfig& config, + const SharedDsgInfo::Ptr& dsg, + const SharedModuleState::Ptr& state) : queue_(std::make_shared()), prefix_(prefix), config_(config), @@ -82,23 +82,23 @@ DsgFrontend::DsgFrontend(const RobotPrefixConfig& prefix, } input_callbacks_.push_back( - std::bind(&DsgFrontend::updateMeshAndObjects, this, std::placeholders::_1)); + std::bind(&FrontendModule::updateMeshAndObjects, this, std::placeholders::_1)); input_callbacks_.push_back( - std::bind(&DsgFrontend::updateDeformationGraph, this, std::placeholders::_1)); + std::bind(&FrontendModule::updateDeformationGraph, this, std::placeholders::_1)); input_callbacks_.push_back( - std::bind(&DsgFrontend::updatePoseGraph, this, std::placeholders::_1)); + std::bind(&FrontendModule::updatePoseGraph, this, std::placeholders::_1)); input_callbacks_.push_back( - std::bind(&DsgFrontend::updatePlaces, this, std::placeholders::_1)); + std::bind(&FrontendModule::updatePlaces, this, std::placeholders::_1)); } -DsgFrontend::~DsgFrontend() { stop(); } +FrontendModule::~FrontendModule() { stop(); } -void DsgFrontend::start() { - spin_thread_.reset(new std::thread(&DsgFrontend::spin, this)); +void FrontendModule::start() { + spin_thread_.reset(new std::thread(&FrontendModule::spin, this)); LOG(INFO) << "[Hydra Frontend] started!"; } -void DsgFrontend::stop() { +void FrontendModule::stop() { should_shutdown_ = true; if (spin_thread_) { @@ -111,7 +111,7 @@ void DsgFrontend::stop() { VLOG(2) << "[Hydra Frontend]: " << queue_->size() << " messages left"; } -void DsgFrontend::save(const std::string& output_path) { +void FrontendModule::save(const std::string& output_path) { dsg_->graph->save(output_path + "/dsg.json", false); dsg_->graph->save(output_path + "/dsg_with_mesh.json"); @@ -122,7 +122,7 @@ void DsgFrontend::save(const std::string& output_path) { } } -void DsgFrontend::spin() { +void FrontendModule::spin() { bool should_shutdown = false; while (!should_shutdown) { bool has_data = queue_->poll(); @@ -140,7 +140,7 @@ void DsgFrontend::spin() { } } -bool DsgFrontend::spinOnce() { +bool FrontendModule::spinOnce() { bool has_data = queue_->poll(); if (!has_data) { return false; @@ -151,13 +151,13 @@ bool DsgFrontend::spinOnce() { return true; } -void DsgFrontend::addOutputCallback(const OutputCallback& callback) { +void FrontendModule::addOutputCallback(const OutputCallback& callback) { output_callbacks_.push_back(callback); } -size_t DsgFrontend::maxSemanticLabel() const { return label_map_->getNumLabels(); } +size_t FrontendModule::maxSemanticLabel() const { return label_map_->getNumLabels(); } -void DsgFrontend::spinOnce(const ReconstructionOutput& msg) { +void FrontendModule::spinOnce(const ReconstructionOutput& msg) { VLOG(5) << "[Hydra Frontend] Popped input packet @ " << msg.timestamp_ns << " [ns]"; ScopedTimer timer("frontend/spin", msg.timestamp_ns); @@ -209,16 +209,12 @@ void DsgFrontend::spinOnce(const ReconstructionOutput& msg) { } } -void DsgFrontend::updateMeshAndObjects(const ReconstructionOutput& input) { +void FrontendModule::updateMeshAndObjects(const ReconstructionOutput& input) { { // start timing scope ScopedTimer timer("frontend/mesh_archive", input.timestamp_ns, true, 1, false); voxblox::BlockIndexList to_archive; - for (const auto& block : input.mesh->archived_blocks.mesh_blocks) { - to_archive.push_back({static_cast(block.index[0]), - static_cast(block.index[1]), - static_cast(block.index[2])}); - } - + to_archive.insert( + to_archive.begin(), input.archived_blocks.begin(), input.archived_blocks.end()); VLOG(5) << "[Hydra Frontend] Clearing " << to_archive.size() << " blocks from mesh"; mesh_compression_->clearArchivedBlocks(to_archive); } // end timing scope @@ -228,9 +224,10 @@ void DsgFrontend::updateMeshAndObjects(const ReconstructionOutput& input) { ScopedTimer timer("frontend/mesh_compression", input.timestamp_ns, true, 1, false); mesh_remapping_.reset(new kimera_pgmo::VoxbloxIndexMapping()); VLOG(5) << "[Hydra Frontend] Updating mesh with" - << input.mesh->mesh.mesh_blocks.size() << " blocks"; - mesh_update = mesh_compression_->update( - input.mesh->mesh, input.timestamp_ns, mesh_remapping_.get()); + << input.mesh->getNumberOfAllocatedMeshes() << " blocks"; + kimera_pgmo::VoxbloxMeshInterface interface(input.mesh); + mesh_update = + mesh_compression_->update(interface, input.timestamp_ns, mesh_remapping_.get()); } // end timing scope { // start timing scope @@ -261,11 +258,13 @@ void DsgFrontend::updateMeshAndObjects(const ReconstructionOutput& input) { backend_input_->mesh_update = mesh_update; } -void DsgFrontend::updateDeformationGraph(const ReconstructionOutput& input) { +void FrontendModule::updateDeformationGraph(const ReconstructionOutput& input) { { // start timing scope ScopedTimer timer( "frontend/dgraph_compresssion", input.timestamp_ns, true, 1, false); - mesh_frontend_.processVoxbloxMeshGraph(input.mesh->mesh); + const auto time_ns = std::chrono::nanoseconds(input.timestamp_ns); + double time_s = std::chrono::duration_cast(time_ns).count(); + mesh_frontend_.processVoxbloxMeshLayerGraph(input.mesh, time_s); } // end timing scope if (backend_input_) { @@ -274,10 +273,10 @@ void DsgFrontend::updateDeformationGraph(const ReconstructionOutput& input) { } } -void DsgFrontend::filterPlaces(const SceneGraphLayer& places, - NodeIdSet& objects_to_check, - NodeIdSet& active_places, - const NodeIdSet& active_neighborhood) { +void FrontendModule::filterPlaces(const SceneGraphLayer& places, + NodeIdSet& objects_to_check, + NodeIdSet& active_places, + const NodeIdSet& active_neighborhood) { // we grab connected components using the subgraph of all active places and all // archived places that used to be a neighbor with an active place so that we don't // miss disconnected components that comprised of archived nodes and formed when an @@ -300,7 +299,7 @@ void DsgFrontend::filterPlaces(const SceneGraphLayer& places, } } -void DsgFrontend::deletePlaceNode(NodeId node_id, NodeIdSet& objects_to_check) { +void FrontendModule::deletePlaceNode(NodeId node_id, NodeIdSet& objects_to_check) { const auto to_check = dsg_->graph->getNode(node_id); if (!to_check) { return; @@ -317,21 +316,19 @@ void DsgFrontend::deletePlaceNode(NodeId node_id, NodeIdSet& objects_to_check) { dsg_->graph->removeNode(node_id); } -void DsgFrontend::updatePlaces(const ReconstructionOutput& input) { +void FrontendModule::updatePlaces(const ReconstructionOutput& input) { ScopedTimer timer("frontend/update_places", input.timestamp_ns, true, 2, false); - SceneGraphLayer temp_layer(DsgLayers::PLACES); - auto edges = temp_layer.deserializeLayer(input.places->layer_contents); - VLOG(3) << "[Hydra Frontend] Received " << temp_layer.numNodes() - << " place nodes and " << edges->size() << " edges from hydra_topology"; + VLOG(3) << "[Hydra Frontend] Received " << input.places->active_attributes.size() + << " place nodes and " << input.places->edges.size() + << " edges from hydra_places"; NodeIdSet active_nodes; NodeIdSet active_neighborhood; - for (const auto& id_node_pair : temp_layer.nodes()) { - active_nodes.insert(id_node_pair.first); - active_neighborhood.insert(id_node_pair.first); - auto& attrs = id_node_pair.second->attributes(); - attrs.is_active = true; - attrs.last_update_time_ns = input.timestamp_ns; + for (const auto& id_attr_pair : input.places->active_attributes) { + active_nodes.insert(id_attr_pair.first); + active_neighborhood.insert(id_attr_pair.first); + id_attr_pair.second->is_active = true; + id_attr_pair.second->last_update_time_ns = input.timestamp_ns; } const auto& places = dsg_->graph->getLayer(DsgLayers::PLACES); @@ -358,7 +355,13 @@ void DsgFrontend::updatePlaces(const ReconstructionOutput& input) { dsg_->graph->removeEdge(n1, n2); } - dsg_->graph->updateFromLayer(temp_layer, std::move(edges)); + for (auto&& [node, attrs] : input.places->active_attributes) { + dsg_->graph->addOrUpdateNode(DsgLayers::PLACES, node, attrs->clone()); + } + + for (auto& edge : input.places->edges) { + dsg_->graph->addOrUpdateEdge(edge.source, edge.target, edge.info->clone()); + } if (config_.filter_places) { auto iter = active_neighborhood.begin(); @@ -384,7 +387,7 @@ void DsgFrontend::updatePlaces(const ReconstructionOutput& input) { previous_active_places_ = active_nodes; } -void DsgFrontend::updatePoseGraph(const ReconstructionOutput& input) { +void FrontendModule::updatePoseGraph(const ReconstructionOutput& input) { std::unique_lock lock(dsg_->mutex); const auto& agents = dsg_->graph->getLayer(DsgLayers::AGENTS, prefix_.key); @@ -433,7 +436,7 @@ void DsgFrontend::updatePoseGraph(const ReconstructionOutput& input) { } } -void DsgFrontend::assignBowVectors(const DynamicLayer& agents) { +void FrontendModule::assignBowVectors(const DynamicLayer& agents) { // TODO(nathan) take care of synchronization better // lcd_input_->new_agent_nodes.clear(); @@ -479,7 +482,7 @@ void DsgFrontend::assignBowVectors(const DynamicLayer& agents) { << prior_size << " original"; } -void DsgFrontend::invalidateMeshEdges(const kimera_pgmo::MeshDelta& delta) { +void FrontendModule::invalidateMeshEdges(const kimera_pgmo::MeshDelta& delta) { std::unique_lock lock(dsg_->mutex); std::vector objects_to_delete; @@ -512,7 +515,7 @@ void DsgFrontend::invalidateMeshEdges(const kimera_pgmo::MeshDelta& delta) { } } -void DsgFrontend::archivePlaces(const NodeIdSet active_places) { +void FrontendModule::archivePlaces(const NodeIdSet active_places) { { // start graph update critical section std::unique_lock graph_lock(dsg_->mutex); @@ -535,8 +538,8 @@ void DsgFrontend::archivePlaces(const NodeIdSet active_places) { } // end graph update critical section } -void DsgFrontend::addPlaceObjectEdges(uint64_t timestamp_ns, - NodeIdSet* extra_objects_to_check) { +void FrontendModule::addPlaceObjectEdges(uint64_t timestamp_ns, + NodeIdSet* extra_objects_to_check) { ScopedTimer timer("frontend/place_object_edges", timestamp_ns); if (!places_nn_finder_) { return; // haven't received places yet @@ -570,7 +573,7 @@ void DsgFrontend::addPlaceObjectEdges(uint64_t timestamp_ns, segmenter_->pruneObjectsToCheckForPlaces(*dsg_->graph); } -void DsgFrontend::addPlaceAgentEdges(uint64_t timestamp_ns) { +void FrontendModule::addPlaceAgentEdges(uint64_t timestamp_ns) { ScopedTimer timer("frontend/place_agent_edges", timestamp_ns); if (!places_nn_finder_) { return; // haven't received places yet @@ -649,9 +652,9 @@ size_t remapConnections(const kimera_pgmo::VoxbloxIndexMapping& remapping, using MeshIndexMap = voxblox::AnyIndexHashMapType::type; -size_t getPlaceSemanticLabels(const voxblox_msgs::Mesh& mesh, +size_t getPlaceSemanticLabels(const voxblox::MeshLayer& mesh, const voxblox::IndexSet& archived_blocks, - const MeshIndexMap& block_index_map, + const voxblox::IndexSet& allocated_blocks, const kimera::SemanticLabel2Color& label_map, PlaceNodeAttributes& attrs) { size_t num_invalid = 0; @@ -661,48 +664,33 @@ size_t getPlaceSemanticLabels(const voxblox_msgs::Mesh& mesh, continue; } - auto iter = block_index_map.find(idx); - if (iter == block_index_map.end()) { + if (!allocated_blocks.count(idx)) { num_invalid++; continue; } - const auto& block = mesh.mesh_blocks[iter->second]; - if (connection.vertex > block.r.size()) { + const auto& block = mesh.getMeshPtrByIndex(idx); + if (connection.vertex > block->size()) { num_invalid++; continue; } - const kimera::HashableColor color(block.r[connection.vertex], - block.g[connection.vertex], - block.b[connection.vertex], - 255); + const kimera::HashableColor color(block->colors.at(connection.vertex)); attrs.mesh_vertex_labels.push_back(label_map.getSemanticLabelFromColor(color)); } return num_invalid; } -void DsgFrontend::updatePlaceMeshMapping(const ReconstructionOutput& input) { +void FrontendModule::updatePlaceMeshMapping(const ReconstructionOutput& input) { std::unique_lock lock(dsg_->mutex); const auto& places = dsg_->graph->getLayer(DsgLayers::PLACES); const auto& graph_mapping = mesh_frontend_.getVoxbloxMsgToGraphMapping(); - MeshIndexMap block_index_map; - size_t count = 0; - for (const auto& block : input.mesh->mesh.mesh_blocks) { - voxblox::BlockIndex idx; - idx << block.index[0], block.index[1], block.index[2]; - block_index_map[idx] = count; - ++count; - } + voxblox::BlockIndexList allocated_list; + input.mesh->getAllAllocatedMeshes(&allocated_list); - voxblox::IndexSet archived_blocks; - for (const auto& block : input.mesh->archived_blocks.mesh_blocks) { - voxblox::BlockIndex idx; - idx << block.index[0], block.index[1], block.index[2]; - archived_blocks.insert(idx); - } + voxblox::IndexSet allocated(allocated_list.begin(), allocated_list.end()); size_t num_missing = 0; size_t num_deform_invalid = 0; @@ -724,18 +712,18 @@ void DsgFrontend::updatePlaceMeshMapping(const ReconstructionOutput& input) { attrs.pcl_mesh_connections.clear(); attrs.mesh_vertex_labels.clear(); num_deform_invalid += remapConnections(graph_mapping, - archived_blocks, + input.archived_blocks, attrs.voxblox_mesh_connections, attrs.deformation_connections); if (mesh_remapping_) { num_mesh_invalid += remapConnections(*mesh_remapping_, - archived_blocks, + input.archived_blocks, attrs.voxblox_mesh_connections, attrs.pcl_mesh_connections); } num_semantic_invalid += getPlaceSemanticLabels( - input.mesh->mesh, archived_blocks, block_index_map, *label_map_, attrs); + *input.mesh, input.archived_blocks, allocated, *label_map_, attrs); } if (config_.validate_vertices) { @@ -746,5 +734,4 @@ void DsgFrontend::updatePlaceMeshMapping(const ReconstructionOutput& input) { } } -} // namespace incremental } // namespace hydra diff --git a/hydra_dsg_builder/src/incremental_mesh_segmenter.cpp b/hydra/src/frontend/mesh_segmenter.cpp similarity index 98% rename from hydra_dsg_builder/src/incremental_mesh_segmenter.cpp rename to hydra/src/frontend/mesh_segmenter.cpp index 54952bb1..7f833942 100644 --- a/hydra_dsg_builder/src/incremental_mesh_segmenter.cpp +++ b/hydra/src/frontend/mesh_segmenter.cpp @@ -32,14 +32,13 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_dsg_builder/incremental_mesh_segmenter.h" +#include "hydra/frontend/mesh_segmenter.h" #include #include #include namespace hydra { -namespace incremental { using kimera::HashableColor; using kimera::SemanticLabel2Color; @@ -257,8 +256,7 @@ std::set MeshSegmenter::archiveOldObjects(const DynamicSceneGraph& graph } std::optional MeshSegmenter::getVertexLabel( - const SemanticLabel2Color& label_map, - size_t index) const { + const SemanticLabel2Color& label_map, size_t index) const { if (index >= full_mesh_vertices_->size()) { return std::nullopt; } @@ -411,5 +409,4 @@ void MeshSegmenter::addObjectToGraph(DynamicSceneGraph& graph, ++next_node_id_; } -} // namespace incremental } // namespace hydra diff --git a/hydra_dsg_builder/src/gnn/CMakeLists.txt b/hydra/src/gnn/CMakeLists.txt similarity index 89% rename from hydra_dsg_builder/src/gnn/CMakeLists.txt rename to hydra/src/gnn/CMakeLists.txt index 9d1702e6..01145717 100644 --- a/hydra_dsg_builder/src/gnn/CMakeLists.txt +++ b/hydra/src/gnn/CMakeLists.txt @@ -1,13 +1,11 @@ configure_file(cmake/ort.CMakeLists.txt.in ort-download/CMakeLists.txt) execute_process( COMMAND "${CMAKE_COMMAND}" -G "${CMAKE_GENERATOR}" . - WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/ort-download" - #OUTPUT_QUIET + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/ort-download" # OUTPUT_QUIET ) execute_process( COMMAND "${CMAKE_COMMAND}" --build . - WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/ort-download" - #OUTPUT_QUIET + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/ort-download" # OUTPUT_QUIET ) add_library(ort::ort IMPORTED INTERFACE) diff --git a/hydra_dsg_builder/src/gnn/cmake/ort.CMakeLists.txt.in b/hydra/src/gnn/cmake/ort.CMakeLists.txt.in similarity index 94% rename from hydra_dsg_builder/src/gnn/cmake/ort.CMakeLists.txt.in rename to hydra/src/gnn/cmake/ort.CMakeLists.txt.in index b71d9205..e5017c3f 100644 --- a/hydra_dsg_builder/src/gnn/cmake/ort.CMakeLists.txt.in +++ b/hydra/src/gnn/cmake/ort.CMakeLists.txt.in @@ -3,7 +3,8 @@ project(ort-download NONE) # TODO(nathan) architecture dependent URL with cross-compiled version include(ExternalProject) -ExternalProject_Add(ort +ExternalProject_Add( + ort URL https://github.com/microsoft/onnxruntime/releases/download/v1.13.1/onnxruntime-linux-x64-gpu-1.13.1.tgz SOURCE_DIR "${CMAKE_CURRENT_BINARY_DIR}/ort-src" BINARY_DIR "${CMAKE_CURRENT_BINARY_DIR}/ort-build" diff --git a/hydra_dsg_builder/src/gnn/gnn_interface.cpp b/hydra/src/gnn/gnn_interface.cpp similarity index 98% rename from hydra_dsg_builder/src/gnn/gnn_interface.cpp rename to hydra/src/gnn/gnn_interface.cpp index 77416ebf..d91e92bc 100644 --- a/hydra_dsg_builder/src/gnn/gnn_interface.cpp +++ b/hydra/src/gnn/gnn_interface.cpp @@ -32,11 +32,12 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_dsg_builder/gnn/gnn_interface.h" -#include "hydra_dsg_builder/gnn/ort_utilities.h" +#include "hydra/gnn/gnn_interface.h" #include +#include "hydra/gnn/ort_utilities.h" + namespace hydra { namespace gnn { diff --git a/hydra_dsg_builder/src/gnn/gnn_model_reader.cpp b/hydra/src/gnn/gnn_model_reader.cpp similarity index 98% rename from hydra_dsg_builder/src/gnn/gnn_model_reader.cpp rename to hydra/src/gnn/gnn_model_reader.cpp index 78e8c2d1..8c8d635f 100644 --- a/hydra_dsg_builder/src/gnn/gnn_model_reader.cpp +++ b/hydra/src/gnn/gnn_model_reader.cpp @@ -32,11 +32,12 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_dsg_builder/gnn/gnn_interface.h" - #include + #include +#include "hydra/gnn/gnn_interface.h" + using EdgeIndex = Eigen::Matrix; template diff --git a/hydra_dsg_builder/src/gnn/ort_utilities.cpp b/hydra/src/gnn/ort_utilities.cpp similarity index 99% rename from hydra_dsg_builder/src/gnn/ort_utilities.cpp rename to hydra/src/gnn/ort_utilities.cpp index 75981555..50f3eadd 100644 --- a/hydra_dsg_builder/src/gnn/ort_utilities.cpp +++ b/hydra/src/gnn/ort_utilities.cpp @@ -32,7 +32,7 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_dsg_builder/gnn/ort_utilities.h" +#include "hydra/gnn/ort_utilities.h" #include #include diff --git a/hydra_dsg_builder/src/gnn/tensor.cpp b/hydra/src/gnn/tensor.cpp similarity index 99% rename from hydra_dsg_builder/src/gnn/tensor.cpp rename to hydra/src/gnn/tensor.cpp index 16a89332..e1cd1ff6 100644 --- a/hydra_dsg_builder/src/gnn/tensor.cpp +++ b/hydra/src/gnn/tensor.cpp @@ -32,7 +32,7 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_dsg_builder/gnn/tensor.h" +#include "hydra/gnn/tensor.h" #include diff --git a/hydra/src/gnn/tests/resources/make_simple_model.py b/hydra/src/gnn/tests/resources/make_simple_model.py new file mode 100644 index 00000000..f37ac1b3 --- /dev/null +++ b/hydra/src/gnn/tests/resources/make_simple_model.py @@ -0,0 +1,84 @@ +#!/usr/bin/env python3 +# Copyright 2022, Massachusetts Institute of Technology. +# All Rights Reserved +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +# WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +# +# Research was sponsored by the United States Air Force Research Laboratory and +# the United States Air Force Artificial Intelligence Accelerator and was +# accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views +# and conclusions contained in this document are those of the authors and should +# not be interpreted as representing the official policies, either expressed or +# implied, of the United States Air Force or the U.S. Government. The U.S. +# Government is authorized to reproduce and distribute reprints for Government +# purposes notwithstanding any copyright notation herein. +"""Make a simple model.""" +import torch +import torch.onnx +import torch.nn as nn +import torch_geometric.nn as pyg + + +class SimpleNetwork(nn.Module): + """Simple network to make testable onnx export.""" + + def __init__(self): + """Make the network.""" + super(SimpleNetwork, self).__init__() + self.mp = pyg.GraphConv(2, 2) + state_dict = { + "lin_rel.weight": torch.eye(2, requires_grad=True, dtype=torch.float32), + "lin_rel.bias": torch.zeros(2, requires_grad=True, dtype=torch.float32), + "lin_root.weight": torch.eye(2, requires_grad=True, dtype=torch.float32), + } + self.mp.load_state_dict(state_dict) + + def forward(self, x, edge_index): + """Do a forward pass.""" + return self.mp(x, edge_index) + + +def main(): + """Export model to onnx.""" + + model = SimpleNetwork() + + x = torch.ones((5, 2)) + edge_index = torch.tensor([[0, 1, 2, 3], [1, 2, 3, 4]], dtype=torch.long) + + y = model(x, edge_index) + print(y) + + torch.onnx.export( + model, + (x, edge_index), + "model.onnx", + export_params=True, + do_constant_folding=True, + input_names=["x", "edge_index"], + output_names=["output"], + dynamic_axes={"x": {0: "num_nodes"}, "edge_index": {1: "num_edges"}}, + ) + + +if __name__ == "__main__": + main() diff --git a/hydra_dsg_builder/src/gnn/tests/resources/simple_model.onnx b/hydra/src/gnn/tests/resources/simple_model.onnx similarity index 100% rename from hydra_dsg_builder/src/gnn/tests/resources/simple_model.onnx rename to hydra/src/gnn/tests/resources/simple_model.onnx diff --git a/hydra_dsg_builder/src/gnn/tests/utest_gnn_interface.cpp b/hydra/src/gnn/tests/utest_gnn_interface.cpp similarity index 96% rename from hydra_dsg_builder/src/gnn/tests/utest_gnn_interface.cpp rename to hydra/src/gnn/tests/utest_gnn_interface.cpp index 7bb2e900..93e3d465 100644 --- a/hydra_dsg_builder/src/gnn/tests/utest_gnn_interface.cpp +++ b/hydra/src/gnn/tests/utest_gnn_interface.cpp @@ -32,17 +32,17 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_dsg_builder/gnn/gnn_interface.h" - #include #include #include +#include "hydra/gnn/gnn_interface.h" + namespace hydra { namespace gnn { TEST(GnnInterfaceTests, TestSimpleNetwork) { - std::string package_path = ros::package::getPath("hydra_dsg_builder"); + std::string package_path = ros::package::getPath("hydra"); std::string model_path = package_path + "/src/gnn/tests/resources/simple_model.onnx"; GnnInterface model(model_path, {{"output", {0}}}); diff --git a/hydra_dsg_builder/src/gnn/tests/utest_main.cpp b/hydra/src/gnn/tests/utest_main.cpp similarity index 100% rename from hydra_dsg_builder/src/gnn/tests/utest_main.cpp rename to hydra/src/gnn/tests/utest_main.cpp diff --git a/hydra_dsg_builder/src/gnn/tests/utest_tensor.cpp b/hydra/src/gnn/tests/utest_tensor.cpp similarity index 99% rename from hydra_dsg_builder/src/gnn/tests/utest_tensor.cpp rename to hydra/src/gnn/tests/utest_tensor.cpp index c4e80e6c..35df2b57 100644 --- a/hydra_dsg_builder/src/gnn/tests/utest_tensor.cpp +++ b/hydra/src/gnn/tests/utest_tensor.cpp @@ -32,10 +32,10 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_dsg_builder/gnn/tensor.h" - #include +#include "hydra/gnn/tensor.h" + namespace hydra { namespace gnn { diff --git a/hydra_dsg_builder/src/dsg_lcd_matching.cpp b/hydra/src/loop_closure/descriptor_matching.cpp similarity index 99% rename from hydra_dsg_builder/src/dsg_lcd_matching.cpp rename to hydra/src/loop_closure/descriptor_matching.cpp index 2289a5f9..6854fdcb 100644 --- a/hydra_dsg_builder/src/dsg_lcd_matching.cpp +++ b/hydra/src/loop_closure/descriptor_matching.cpp @@ -32,7 +32,7 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_dsg_builder/dsg_lcd_matching.h" +#include "hydra/loop_closure/descriptor_matching.h" #include diff --git a/hydra_dsg_builder/src/dsg_lcd_detector.cpp b/hydra/src/loop_closure/detector.cpp similarity index 89% rename from hydra_dsg_builder/src/dsg_lcd_detector.cpp rename to hydra/src/loop_closure/detector.cpp index 1ea74e55..88f86d0f 100644 --- a/hydra_dsg_builder/src/dsg_lcd_detector.cpp +++ b/hydra/src/loop_closure/detector.cpp @@ -32,18 +32,19 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_dsg_builder/dsg_lcd_detector.h" +#include "hydra/loop_closure/detector.h" #include "hydra_build_config.h" #if defined(HYDRA_USE_GNN) && HYDRA_USE_GNN -#include "hydra_dsg_builder/lcd_gnn_descriptors.h" +#include "hydra/loop_closure/gnn_descriptors.h" #endif #include -#include #include +#include "hydra/utils/timing_utilities.h" + namespace hydra { namespace lcd { @@ -69,8 +70,8 @@ std::string showVector(const Eigen::Matrix& vector, } #if defined(HYDRA_USE_GNN) && HYDRA_USE_GNN -void configureDescriptorFactories(lcd::DsgLcdDetector& detector, - const DsgLcdDetectorConfig& config) { +void configureDescriptorFactories(lcd::LcdDetector& detector, + const LcdDetectorConfig& config) { ObjectGnnDescriptor::LabelEmbeddings embeddings; if (config.gnn_lcd.use_onehot_encoding) { for (size_t i = 0; i < config.gnn_lcd.onehot_encoding_dim; ++i) { @@ -82,7 +83,7 @@ void configureDescriptorFactories(lcd::DsgLcdDetector& detector, embeddings = loadLabelEmbeddings(config.gnn_lcd.label_embeddings_file); } - DsgLcdDetector::FactoryMap factories; + LcdDetector::FactoryMap factories; factories.emplace( DsgLayers::OBJECTS, std::make_unique(config.gnn_lcd.object_model_path, @@ -98,7 +99,7 @@ void configureDescriptorFactories(lcd::DsgLcdDetector& detector, detector.setDescriptorFactories(std::move(factories)); } #else -void configureDescriptorFactories(lcd::DsgLcdDetector&, const DsgLcdDetectorConfig&) { +void configureDescriptorFactories(lcd::LcdDetector&, const LcdDetectorConfig&) { LOG(ERROR) << "Unable to initialize GNN descriptors: not built with -DHYDRA_GNN=ON"; } #endif @@ -106,7 +107,7 @@ void configureDescriptorFactories(lcd::DsgLcdDetector&, const DsgLcdDetectorConf using DsgNode = DynamicSceneGraphNode; using hydra::timing::ScopedTimer; -DsgLcdDetector::DsgLcdDetector(const DsgLcdDetectorConfig& config) : config_(config) { +LcdDetector::LcdDetector(const LcdDetectorConfig& config) : config_(config) { for (const auto& id_func_pair : layer_factories_) { cache_map_[id_func_pair.first] = DescriptorCache(); } @@ -119,13 +120,13 @@ DsgLcdDetector::DsgLcdDetector(const DsgLcdDetectorConfig& config) : config_(con resetLayerAssignments(); } -void DsgLcdDetector::setDescriptorFactories(FactoryMap&& factories) { +void LcdDetector::setDescriptorFactories(FactoryMap&& factories) { layer_factories_ = std::move(factories); resetLayerAssignments(); } -void DsgLcdDetector::setRegistrationSolver(size_t level, - DsgRegistrationSolver::Ptr&& solver) { +void LcdDetector::setRegistrationSolver(size_t level, + DsgRegistrationSolver::Ptr&& solver) { if (level == 0 && !config_.enable_agent_registration) { return; } @@ -133,7 +134,7 @@ void DsgLcdDetector::setRegistrationSolver(size_t level, registration_solvers_[level] = std::move(solver); } -size_t DsgLcdDetector::numDescriptors() const { +size_t LcdDetector::numDescriptors() const { size_t num_descriptors = 0; for (const auto& id_cache_pair : cache_map_) { num_descriptors += id_cache_pair.second.size(); @@ -141,7 +142,7 @@ size_t DsgLcdDetector::numDescriptors() const { return num_descriptors + numAgentDescriptors(); } -size_t DsgLcdDetector::numGraphDescriptors(LayerId layer) const { +size_t LcdDetector::numGraphDescriptors(LayerId layer) const { if (!cache_map_.count(layer)) { return 0; } @@ -149,7 +150,7 @@ size_t DsgLcdDetector::numGraphDescriptors(LayerId layer) const { return cache_map_.at(layer).size(); } -size_t DsgLcdDetector::numAgentDescriptors() const { +size_t LcdDetector::numAgentDescriptors() const { size_t count = 0; for (const auto& id_cache_pair : leaf_cache_) { count += id_cache_pair.second.size(); @@ -157,19 +158,19 @@ size_t DsgLcdDetector::numAgentDescriptors() const { return count; } -const std::map& DsgLcdDetector::getLatestMatches() const { +const std::map& LcdDetector::getLatestMatches() const { return matches_; } -const std::map& DsgLcdDetector::getLayerRemapping() const { +const std::map& LcdDetector::getLayerRemapping() const { return layer_to_internal_index_; } -const DescriptorCache& DsgLcdDetector::getDescriptorCache(LayerId layer) { +const DescriptorCache& LcdDetector::getDescriptorCache(LayerId layer) { return cache_map_.at(layer); } -void DsgLcdDetector::dumpDescriptors(const std::string& log_path) const { +void LcdDetector::dumpDescriptors(const std::string& log_path) const { const Eigen::IOFormat format( Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", ", ", "", "", "[", "]"); std::ofstream fout(log_path + "/descriptors.yaml"); @@ -205,7 +206,7 @@ void DsgLcdDetector::dumpDescriptors(const std::string& log_path) const { } } -void DsgLcdDetector::makeDefaultDescriptorFactories() { +void LcdDetector::makeDefaultDescriptorFactories() { layer_factories_.emplace( DsgLayers::OBJECTS, std::make_unique(config_.object_extraction, @@ -217,7 +218,7 @@ void DsgLcdDetector::makeDefaultDescriptorFactories() { agent_factory_ = std::make_unique(); } -void DsgLcdDetector::resetLayerAssignments() { +void LcdDetector::resetLayerAssignments() { // TODO(nathan) this is messy registration_solvers_.clear(); @@ -251,8 +252,8 @@ void DsgLcdDetector::resetLayerAssignments() { match_config_map_[0] = config_.agent_search_config; } -bool DsgLcdDetector::addNewDescriptors(const DynamicSceneGraph& graph, - const DynamicSceneGraphNode& agent_node) { +bool LcdDetector::addNewDescriptors(const DynamicSceneGraph& graph, + const DynamicSceneGraphNode& agent_node) { auto parent = agent_node.getParent(); if (!parent) { return false; @@ -284,7 +285,7 @@ bool DsgLcdDetector::addNewDescriptors(const DynamicSceneGraph& graph, return true; } -void DsgLcdDetector::updateDescriptorCache( +void LcdDetector::updateDescriptorCache( const DynamicSceneGraph& dsg, const std::unordered_set& archived_places, uint64_t timestamp) { @@ -312,7 +313,7 @@ void DsgLcdDetector::updateDescriptorCache( } } -std::vector DsgLcdDetector::registerAndVerify( +std::vector LcdDetector::registerAndVerify( const DynamicSceneGraph& dsg, const std::map& matches, NodeId agent_id, @@ -389,10 +390,9 @@ std::vector DsgLcdDetector::registerAndVerify( return results; } -std::vector DsgLcdDetector::detect( - const DynamicSceneGraph& dsg, - NodeId agent_id, - uint64_t timestamp) { +std::vector LcdDetector::detect(const DynamicSceneGraph& dsg, + NodeId agent_id, + uint64_t timestamp) { ScopedTimer timer("lcd/detect", timestamp, true, 2, false); std::set prev_valid_roots; for (const auto& id_desc_pair : cache_map_[root_layer_]) { diff --git a/hydra_dsg_builder/src/lcd_gnn_descriptors.cpp b/hydra/src/loop_closure/gnn_descriptors.cpp similarity index 99% rename from hydra_dsg_builder/src/lcd_gnn_descriptors.cpp rename to hydra/src/loop_closure/gnn_descriptors.cpp index f0badb7e..b5122ea9 100644 --- a/hydra_dsg_builder/src/lcd_gnn_descriptors.cpp +++ b/hydra/src/loop_closure/gnn_descriptors.cpp @@ -32,7 +32,7 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_dsg_builder/lcd_gnn_descriptors.h" +#include "hydra/loop_closure/gnn_descriptors.h" #include #include diff --git a/hydra_dsg_builder/src/incremental_dsg_lcd.cpp b/hydra/src/loop_closure/loop_closure_module.cpp similarity index 87% rename from hydra_dsg_builder/src/incremental_dsg_lcd.cpp rename to hydra/src/loop_closure/loop_closure_module.cpp index b678d17c..40f37c77 100644 --- a/hydra_dsg_builder/src/incremental_dsg_lcd.cpp +++ b/hydra/src/loop_closure/loop_closure_module.cpp @@ -32,40 +32,39 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_dsg_builder/incremental_dsg_lcd.h" +#include "hydra/loop_closure/loop_closure_module.h" #include -#include #include -#include "hydra_dsg_builder/hydra_config.h" +#include "hydra/common/hydra_config.h" +#include "hydra/utils/timing_utilities.h" namespace hydra { -namespace incremental { using hydra::timing::ScopedTimer; using lcd::LayerRegistrationConfig; -DsgLcd::DsgLcd(const RobotPrefixConfig& prefix, - const DsgLcdModuleConfig& config, - const SharedDsgInfo::Ptr& dsg, - const SharedModuleState::Ptr& state) +LoopClosureModule::LoopClosureModule(const RobotPrefixConfig& prefix, + const LoopClosureConfig& config, + const SharedDsgInfo::Ptr& dsg, + const SharedModuleState::Ptr& state) : prefix_(prefix), config_(config), dsg_(dsg), state_(state), lcd_graph_(new DynamicSceneGraph()) { - lcd_detector_.reset(new lcd::DsgLcdDetector(config_.detector)); + lcd_detector_.reset(new lcd::LcdDetector(config_.detector)); } -DsgLcd::~DsgLcd() { stop(); } +LoopClosureModule::~LoopClosureModule() { stop(); } -void DsgLcd::start() { - spin_thread_.reset(new std::thread(&DsgLcd::spin, this)); +void LoopClosureModule::start() { + spin_thread_.reset(new std::thread(&LoopClosureModule::spin, this)); LOG(INFO) << "[DSG LCD] LCD started!"; } -void DsgLcd::stop() { +void LoopClosureModule::stop() { VLOG(2) << "[DSG LCD] stopping lcd!"; should_shutdown_ = true; @@ -77,12 +76,12 @@ void DsgLcd::stop() { } } -void DsgLcd::save(const std::string& log_path) { +void LoopClosureModule::save(const std::string& log_path) { lcd_detector_->dumpDescriptors(log_path); lcd_graph_->save(log_path + "/dsg.json", false); } -void DsgLcd::spin() { +void LoopClosureModule::spin() { if (!state_->lcd_queue) { LOG(ERROR) << "LCD queue required to run LCD"; return; @@ -112,7 +111,7 @@ void DsgLcd::spin() { } } -bool DsgLcd::spinOnce(bool force_update) { +bool LoopClosureModule::spinOnce(bool force_update) { if (!state_->lcd_queue) { LOG(ERROR) << "LCD queue required to run LCD"; return false; @@ -127,9 +126,9 @@ bool DsgLcd::spinOnce(bool force_update) { return true; } -lcd::DsgLcdDetector& DsgLcd::getDetector() const { return *lcd_detector_; } +lcd::LcdDetector& LoopClosureModule::getDetector() const { return *lcd_detector_; } -void DsgLcd::spinOnceImpl(bool force_update) { +void LoopClosureModule::spinOnceImpl(bool force_update) { const size_t timestamp_ns = processFrontendOutput(); { // start critical section @@ -171,7 +170,7 @@ void DsgLcd::spinOnceImpl(bool force_update) { } } -size_t DsgLcd::processFrontendOutput() { +size_t LoopClosureModule::processFrontendOutput() { const auto& msg = state_->lcd_queue->front(); VLOG(5) << "[Hydra LCD] Received archived places: " << displayNodeSymbolContainer(msg->archived_places); @@ -191,7 +190,7 @@ size_t DsgLcd::processFrontendOutput() { return timestamp_ns; } -NodeIdSet DsgLcd::getPlacesToCache(const Eigen::Vector3d& agent_pos) { +NodeIdSet LoopClosureModule::getPlacesToCache(const Eigen::Vector3d& agent_pos) { NodeIdSet to_cache; auto iter = potential_lcd_root_nodes_.begin(); while (iter != potential_lcd_root_nodes_.end()) { @@ -218,7 +217,7 @@ NodeIdSet DsgLcd::getPlacesToCache(const Eigen::Vector3d& agent_pos) { return to_cache; } -std::optional DsgLcd::getQueryAgentId(size_t stamp_ns) { +std::optional LoopClosureModule::getQueryAgentId(size_t stamp_ns) { if (agent_queue_.empty()) { return std::nullopt; } @@ -251,5 +250,4 @@ std::optional DsgLcd::getQueryAgentId(size_t stamp_ns) { return valid_node; } -} // namespace incremental } // namespace hydra diff --git a/hydra_dsg_builder/src/dsg_lcd_registration.cpp b/hydra/src/loop_closure/registration.cpp similarity index 98% rename from hydra_dsg_builder/src/dsg_lcd_registration.cpp rename to hydra/src/loop_closure/registration.cpp index af7184b7..5e5de909 100644 --- a/hydra_dsg_builder/src/dsg_lcd_registration.cpp +++ b/hydra/src/loop_closure/registration.cpp @@ -32,13 +32,13 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_dsg_builder/dsg_lcd_registration.h" - -#include +#include "hydra/loop_closure/registration.h" #include #include +#include "hydra/utils/timing_utilities.h" + namespace hydra { namespace lcd { @@ -48,7 +48,6 @@ inline std::ostream& operator<<(std::ostream& out, const gtsam::Quaternion& q) { return out; } -using incremental::SharedDsgInfo; using DsgNode = DynamicSceneGraphNode; using hydra::timing::ScopedTimer; diff --git a/hydra_dsg_builder/src/dsg_lcd_descriptors.cpp b/hydra/src/loop_closure/scene_graph_descriptors.cpp similarity index 99% rename from hydra_dsg_builder/src/dsg_lcd_descriptors.cpp rename to hydra/src/loop_closure/scene_graph_descriptors.cpp index c27423d5..2a9af8d8 100644 --- a/hydra_dsg_builder/src/dsg_lcd_descriptors.cpp +++ b/hydra/src/loop_closure/scene_graph_descriptors.cpp @@ -32,7 +32,7 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_dsg_builder/dsg_lcd_descriptors.h" +#include "hydra/loop_closure/scene_graph_descriptors.h" #include diff --git a/hydra_dsg_builder/src/subgraph_extraction.cpp b/hydra/src/loop_closure/subgraph_extraction.cpp similarity index 99% rename from hydra_dsg_builder/src/subgraph_extraction.cpp rename to hydra/src/loop_closure/subgraph_extraction.cpp index 4bef85eb..0a69d18e 100644 --- a/hydra_dsg_builder/src/subgraph_extraction.cpp +++ b/hydra/src/loop_closure/subgraph_extraction.cpp @@ -32,7 +32,7 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_dsg_builder/subgraph_extraction.h" +#include "hydra/loop_closure/subgraph_extraction.h" #include diff --git a/hydra_topology/src/compression_graph_extractor.cpp b/hydra/src/places/compression_graph_extractor.cpp similarity index 98% rename from hydra_topology/src/compression_graph_extractor.cpp rename to hydra/src/places/compression_graph_extractor.cpp index 4182d735..bce54306 100644 --- a/hydra_topology/src/compression_graph_extractor.cpp +++ b/hydra/src/places/compression_graph_extractor.cpp @@ -32,17 +32,17 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_topology/compression_graph_extractor.h" +#include "hydra/places/compression_graph_extractor.h" #include -#include -#include "hydra_topology/configs.h" -#include "hydra_topology/graph_extractor_utilities.h" -#include "hydra_topology/nearest_neighbor_utilities.h" +#include "hydra/places/graph_extractor_utilities.h" +#include "hydra/places/nearest_voxel_utilities.h" +#include "hydra/reconstruction/configs.h" +#include "hydra/utils/timing_utilities.h" namespace hydra { -namespace topology { +namespace places { using GvdLayer = CompressionGraphExtractor::GvdLayer; using PlaceAttrs = PlaceNodeAttributes; @@ -314,7 +314,7 @@ void CompressionGraphExtractor::removeDistantIndex(const GlobalIndex& index) { void CompressionGraphExtractor::fillSeenVoxels(const GvdLayer& layer, uint64_t timestamp_ns, IndexVoxelQueue& seen_voxels) { - ScopedTimer timer("topology/prune_gvd_queue", timestamp_ns); + ScopedTimer timer("places/prune_gvd_queue", timestamp_ns); voxblox::LongIndexSet seen_indices; while (!modified_voxel_queue_.empty()) { @@ -427,7 +427,7 @@ void CompressionGraphExtractor::updateNode(const GlobalIndex& index, void CompressionGraphExtractor::updateNodeInfoMap(const GvdLayer& layer, const IndexVoxelQueue& update_info, uint64_t timestamp_ns) { - ScopedTimer timer("topology/handle_gvd_queue", timestamp_ns); + ScopedTimer timer("places/handle_gvd_queue", timestamp_ns); for (const auto& index_voxel_pair : update_info) { updateNode(index_voxel_pair.index, @@ -439,7 +439,7 @@ void CompressionGraphExtractor::updateNodeInfoMap(const GvdLayer& layer, void CompressionGraphExtractor::updateGvdGraph(const IndexVoxelQueue& update_info, uint64_t timestamp_ns) { - ScopedTimer timer("topology/update_gvd_graph", timestamp_ns); + ScopedTimer timer("places/update_gvd_graph", timestamp_ns); std::list to_delete; GvdNeighborhood::IndexMatrix neighbor_indices; @@ -506,9 +506,7 @@ uint64_t CompressionGraphExtractor::getNextId() { } std::optional CompressionGraphExtractor::findCluster( - uint64_t node_id, - const GvdMemberInfo& node, - const std::set& clusters) { + uint64_t node_id, const GvdMemberInfo& node, const std::set& clusters) { for (const auto cluster : clusters) { CHECK(compressed_info_map_.count(cluster)) << "missing cluster: " << cluster << " from clusters: " << clusters; @@ -858,5 +856,5 @@ void CompressionGraphExtractor::validate(const GvdLayer& layer) const { } } -} // namespace topology +} // namespace places } // namespace hydra diff --git a/hydra_topology/src/floodfill_graph_extractor.cpp b/hydra/src/places/floodfill_graph_extractor.cpp similarity index 98% rename from hydra_topology/src/floodfill_graph_extractor.cpp rename to hydra/src/places/floodfill_graph_extractor.cpp index b8de85fe..77c45a61 100644 --- a/hydra_topology/src/floodfill_graph_extractor.cpp +++ b/hydra/src/places/floodfill_graph_extractor.cpp @@ -32,12 +32,13 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_topology/floodfill_graph_extractor.h" -#include "hydra_topology/graph_extractor_utilities.h" -#include "hydra_topology/nearest_neighbor_utilities.h" +#include "hydra/places/floodfill_graph_extractor.h" + +#include "hydra/places/graph_extractor_utilities.h" +#include "hydra/places/nearest_voxel_utilities.h" namespace hydra { -namespace topology { +namespace places { using GvdLayer = FloodfillGraphExtractor::GvdLayer; using GlobalIndexVector = voxblox::AlignedVector; @@ -569,5 +570,5 @@ bool FloodfillGraphExtractor::isVertex(const GvdLayer& layer, return false; } -} // namespace topology +} // namespace places } // namespace hydra diff --git a/hydra_topology/src/graph_extractor_interface.cpp b/hydra/src/places/graph_extractor_interface.cpp similarity index 98% rename from hydra_topology/src/graph_extractor_interface.cpp rename to hydra/src/places/graph_extractor_interface.cpp index 5374ad6d..9af3144d 100644 --- a/hydra_topology/src/graph_extractor_interface.cpp +++ b/hydra/src/places/graph_extractor_interface.cpp @@ -32,13 +32,12 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_topology/graph_extractor_interface.h" +#include "hydra/places/graph_extractor_interface.h" -#include "hydra_topology/graph_extractor_utilities.h" -#include "hydra_topology/nearest_neighbor_utilities.h" +#include "hydra/places/graph_extractor_utilities.h" namespace hydra { -namespace topology { +namespace places { GraphExtractorInterface::GraphExtractorInterface(const GraphExtractorConfig& config) : config_(config), @@ -288,5 +287,5 @@ void GraphExtractorInterface::updateHeuristicEdges(const GvdLayer& gvd) { } } -} // namespace topology +} // namespace places } // namespace hydra diff --git a/hydra_topology/src/graph_extractor_types.cpp b/hydra/src/places/graph_extractor_types.cpp similarity index 97% rename from hydra_topology/src/graph_extractor_types.cpp rename to hydra/src/places/graph_extractor_types.cpp index 9acb5d7b..ffb5132f 100644 --- a/hydra_topology/src/graph_extractor_types.cpp +++ b/hydra/src/places/graph_extractor_types.cpp @@ -32,10 +32,10 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_topology/graph_extractor_types.h" +#include "hydra/places/graph_extractor_types.h" namespace hydra { -namespace topology { +namespace places { VoxelGraphInfo::VoxelGraphInfo() : is_node(false), is_split_node(false) {} @@ -71,5 +71,5 @@ std::ostream& operator<<(std::ostream& out, const EdgeInfo& info) { return out; } -} // namespace topology +} // namespace places } // namespace hydra diff --git a/hydra_topology/src/graph_extractor_utilities.cpp b/hydra/src/places/graph_extractor_utilities.cpp similarity index 98% rename from hydra_topology/src/graph_extractor_utilities.cpp rename to hydra/src/places/graph_extractor_utilities.cpp index fed4abf7..d28a763b 100644 --- a/hydra_topology/src/graph_extractor_utilities.cpp +++ b/hydra/src/places/graph_extractor_utilities.cpp @@ -32,11 +32,13 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_topology/graph_extractor_utilities.h" -#include "hydra_topology/nearest_neighbor_utilities.h" +#include "hydra/places/graph_extractor_utilities.h" + +#include "hydra/places/nearest_voxel_utilities.h" +#include "hydra/utils/nearest_neighbor_utilities.h" namespace hydra { -namespace topology { +namespace places { using voxblox::Connectivity; using Neighborhood26Connected = Neighborhood; @@ -392,5 +394,5 @@ void findFreespaceEdges(const FreespaceEdgeConfig& config, } } -} // namespace topology +} // namespace places } // namespace hydra diff --git a/hydra_topology/src/gvd_graph.cpp b/hydra/src/places/gvd_graph.cpp similarity index 97% rename from hydra_topology/src/gvd_graph.cpp rename to hydra/src/places/gvd_graph.cpp index bfc2b535..7fa9b9f7 100644 --- a/hydra_topology/src/gvd_graph.cpp +++ b/hydra/src/places/gvd_graph.cpp @@ -32,10 +32,10 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_topology/gvd_graph.h" +#include "hydra/places/gvd_graph.h" namespace hydra { -namespace topology { +namespace places { GvdGraph::GvdGraph() : next_id_(0) {} @@ -92,5 +92,5 @@ uint64_t GvdGraph::getNextId() { bool GvdGraph::hasNode(uint64_t node) const { return nodes_.count(node) > 0; } -} // namespace topology +} // namespace places } // namespace hydra diff --git a/hydra_topology/src/gvd_integrator.cpp b/hydra/src/places/gvd_integrator.cpp similarity index 97% rename from hydra_topology/src/gvd_integrator.cpp rename to hydra/src/places/gvd_integrator.cpp index 12093c3b..1329cff6 100644 --- a/hydra_topology/src/gvd_integrator.cpp +++ b/hydra/src/places/gvd_integrator.cpp @@ -41,16 +41,16 @@ // implied, of the United States Air Force or the U.S. Government. The U.S. // Government is authorized to reproduce and distribute reprints for Government // purposes notwithstanding any copyright notation herein. -#include "hydra_topology/gvd_integrator.h" +#include "hydra/places/gvd_integrator.h" -#include - -#include "hydra_topology/compression_graph_extractor.h" -#include "hydra_topology/floodfill_graph_extractor.h" -#include "hydra_topology/gvd_utilities.h" +#include "hydra/places/compression_graph_extractor.h" +#include "hydra/places/floodfill_graph_extractor.h" +#include "hydra/places/gvd_utilities.h" +#include "hydra/reconstruction/voxblox_utilities.h" +#include "hydra/utils/timing_utilities.h" namespace hydra { -namespace topology { +namespace places { using timing::ScopedTimer; using EdgeNeighborhood = Neighborhood; @@ -106,7 +106,7 @@ void GvdIntegrator::updateFromTsdf(uint64_t timestamp_ns, } VLOG(1) << "[GVD update] Propagating TSDF using " << blocks.size() << " TSDF blocks"; - ScopedTimer timer("topology/propagate_tsdf", timestamp_ns); + ScopedTimer timer("places/propagate_tsdf", timestamp_ns); update_stats_.clear(); for (const BlockIndex& idx : blocks) { @@ -125,11 +125,11 @@ void GvdIntegrator::updateFromTsdf(uint64_t timestamp_ns, void GvdIntegrator::updateGvd(uint64_t timestamp_ns) { // TODO(nathan) this depends on marching cubes being called beforehand... - ScopedTimer timer("topology/overall_update", timestamp_ns); + ScopedTimer timer("places/overall_update", timestamp_ns); VLOG(3) << "[GVD update] Processing open queue"; { // timing scope - ScopedTimer timer("topology/open_queue", timestamp_ns); + ScopedTimer timer("places/open_queue", timestamp_ns); processOpenQueue(); } // timing scope @@ -137,7 +137,7 @@ void GvdIntegrator::updateGvd(uint64_t timestamp_ns) { if (config_.extract_graph) { VLOG(3) << "[GVD update] Starting graph extraction"; - ScopedTimer timer("topology/graph_extractor", timestamp_ns); + ScopedTimer timer("places/graph_extractor", timestamp_ns); graph_extractor_->extract(*gvd_layer_, timestamp_ns); graph_extractor_->assignMeshVertices( *gvd_layer_, parent_tracker_.parents, parent_tracker_.parent_vertices); @@ -679,5 +679,5 @@ voxblox::Point GvdIntegrator::getPosition(const GlobalIndex& index) const { return getVoxelPosition(*gvd_layer_, index); } -} // namespace topology +} // namespace places } // namespace hydra diff --git a/hydra_topology/src/gvd_parent_tracker.cpp b/hydra/src/places/gvd_parent_tracker.cpp similarity index 97% rename from hydra_topology/src/gvd_parent_tracker.cpp rename to hydra/src/places/gvd_parent_tracker.cpp index 0db271df..44660ce6 100644 --- a/hydra_topology/src/gvd_parent_tracker.cpp +++ b/hydra/src/places/gvd_parent_tracker.cpp @@ -32,11 +32,12 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_topology/gvd_parent_tracker.h" -#include "hydra_topology/gvd_utilities.h" +#include "hydra/places/gvd_parent_tracker.h" + +#include "hydra/places/gvd_utilities.h" namespace hydra { -namespace topology { +namespace places { uint8_t GvdParentTracker::updateGvdParentMap(const Layer& layer, const VoronoiCheckConfig& config, @@ -127,5 +128,5 @@ void GvdParentTracker::updateVertexMapping(const Layer& layer) { } } -} // namespace topology +} // namespace places } // namespace hydra diff --git a/hydra_topology/src/gvd_thinning.cpp b/hydra/src/places/gvd_thinning.cpp similarity index 98% rename from hydra_topology/src/gvd_thinning.cpp rename to hydra/src/places/gvd_thinning.cpp index e9350e3d..c4883bd4 100644 --- a/hydra_topology/src/gvd_thinning.cpp +++ b/hydra/src/places/gvd_thinning.cpp @@ -32,15 +32,15 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_topology/gvd_thinning.h" +#include "hydra/places/gvd_thinning.h" #include +#include #include #include -#include namespace hydra { -namespace topology { +namespace places { // based on: // https://github.com/scikit-image/scikit-image/blob/v0.19.2/skimage/morphology/_skeletonize_3d_cy.pyx.in @@ -198,5 +198,5 @@ bool isSimplePoint(const std::bitset<26>& neighborhood) { return true; } -} // namespace topology +} // namespace places } // namespace hydra diff --git a/hydra_topology/src/gvd_utilities.cpp b/hydra/src/places/gvd_utilities.cpp similarity index 98% rename from hydra_topology/src/gvd_utilities.cpp rename to hydra/src/places/gvd_utilities.cpp index e1ffd27d..01390ed6 100644 --- a/hydra_topology/src/gvd_utilities.cpp +++ b/hydra/src/places/gvd_utilities.cpp @@ -32,10 +32,10 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_topology/gvd_utilities.h" +#include "hydra/places/gvd_utilities.h" namespace hydra { -namespace topology { +namespace places { DistancePotential getLowerDistance(FloatingPoint v_dist, FloatingPoint n_dist, @@ -98,5 +98,5 @@ VoronoiCondition checkVoronoi(const VoronoiCheckConfig& cfg, return result; } -} // namespace topology +} // namespace places } // namespace hydra diff --git a/hydra_topology/src/gvd_voxel.cpp b/hydra/src/places/gvd_voxel.cpp similarity index 97% rename from hydra_topology/src/gvd_voxel.cpp rename to hydra/src/places/gvd_voxel.cpp index 6aa76eb9..3b0a49b8 100644 --- a/hydra_topology/src/gvd_voxel.cpp +++ b/hydra/src/places/gvd_voxel.cpp @@ -32,10 +32,10 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_topology/gvd_voxel.h" +#include "hydra/places/gvd_voxel.h" namespace hydra { -namespace topology { +namespace places { std::ostream& operator<<(std::ostream& out, const GvdVoxel& voxel) { out << "GvdVoxel +#include "hydra/places/graph_extractor_utilities.h" + namespace hydra { -namespace topology { +namespace places { using nanoflann::KDTreeSingleIndexAdaptor; using nanoflann::KDTreeSingleIndexDynamicAdaptor; using nanoflann::L2_Simple_Adaptor; using GlobalIndexVector = voxblox::AlignedVector; -struct GraphKdTreeAdaptor { - GraphKdTreeAdaptor(const SceneGraphLayer& layer, const std::vector& nodes) - : layer(layer), nodes(nodes) {} - - inline size_t kdtree_get_point_count() const { return nodes.size(); } - - inline double kdtree_get_pt(const size_t idx, const size_t dim) const { - return layer.getPosition(nodes[idx])(dim); - } - - template - bool kdtree_get_bbox(T&) const { - return false; - } - - const SceneGraphLayer& layer; - std::vector nodes; -}; - -struct NearestNodeFinder::Detail { - using Dist = L2_Simple_Adaptor; - using KDTree = KDTreeSingleIndexAdaptor; - - Detail(const SceneGraphLayer& layer, const std::vector& nodes) - : adaptor(layer, nodes) { - kdtree.reset(new KDTree(3, adaptor)); - kdtree->buildIndex(); - } - - ~Detail() = default; - - GraphKdTreeAdaptor adaptor; - std::unique_ptr kdtree; -}; - -NearestNodeFinder::NearestNodeFinder(const SceneGraphLayer& layer, - const std::vector& nodes) - : internals_(new Detail(layer, nodes)) {} - -NearestNodeFinder::NearestNodeFinder(const SceneGraphLayer& layer, - const std::unordered_set& nodes) { - std::vector node_vector(nodes.begin(), nodes.end()); - VLOG(10) << "Making node finder with " << nodes.size() << " nodes"; - internals_.reset(new Detail(layer, node_vector)); -} - -NearestNodeFinder::~NearestNodeFinder() {} - -void NearestNodeFinder::find(const Eigen::Vector3d& position, - size_t num_to_find, - bool skip_first, - const NearestNodeFinder::Callback& callback) { - const size_t limit = skip_first ? num_to_find + 1 : num_to_find; - std::vector nn_indices(limit); - std::vector distances(limit); - - size_t num_found = internals_->kdtree->knnSearch( - position.data(), limit, nn_indices.data(), distances.data()); - - size_t i = skip_first ? 1 : 0; - for (; i < num_found; ++i) { - callback(internals_->adaptor.nodes[nn_indices[i]], nn_indices[i], distances[i]); - } -} - struct VoxelKdTreeAdaptor { explicit VoxelKdTreeAdaptor(const GlobalIndexVector& indices) : indices(indices) {} @@ -161,91 +97,6 @@ void NearestVoxelFinder::find(const GlobalIndex& index, } } -// TODO(nathan) not working: bug in nanoflann w.r.t accessing removed indices -// during index building -struct GraphDynamicKdTreeAdaptor { - GraphDynamicKdTreeAdaptor(const SceneGraphLayer& layer) : layer(layer), count(0) {} - - struct AddInfo { - size_t start; - size_t num_added = 0; - }; - - struct RemoveInfo { - size_t index = 0; - bool valid = false; - }; - - AddInfo addNodes(const std::unordered_set& new_nodes) { - AddInfo info; - info.start = count; - - for (const auto& node : new_nodes) { - if (curr_nodes.count(node)) { - continue; - } - - idx_node_map[count] = node; - node_idx_map[node] = count; - curr_nodes.insert(node); - - info.num_added++; - count++; - } - LOG(WARNING) << "Added " << info.start << " through " << count - 1; - - return info; - } - - RemoveInfo removeNode(NodeId node) { - RemoveInfo info; - if (!curr_nodes.count(node)) { - return info; - } - - info.index = node_idx_map.at(node); - LOG(WARNING) << "Removing " << info.index; - node_idx_map.erase(node); - idx_node_map.erase(info.index); - curr_nodes.erase(node); - info.valid = true; - return info; - } - - inline size_t kdtree_get_point_count() const { return curr_nodes.size(); } - - inline double kdtree_get_pt(const size_t idx, const size_t dim) const { - if (!idx_node_map.count(idx)) { - std::stringstream ss; - auto iter = idx_node_map.begin(); - ss << "["; - while (iter != idx_node_map.end()) { - ss << iter->first; - ++iter; - if (iter != idx_node_map.end()) { - ss << ", "; - } - } - ss << "]"; - - LOG(FATAL) << "idx: " << idx << " doesn't exist. current count: " << count - << " indices: " << ss.str(); - } - return layer.getPosition(idx_node_map.at(idx))(dim); - } - - template - bool kdtree_get_bbox(T&) const { - return false; - } - - const SceneGraphLayer& layer; - size_t count; - std::map idx_node_map; - std::map node_idx_map; - std::unordered_set curr_nodes; -}; - FurthestIndexResult findFurthestIndexFromLine(const GlobalIndexVector& indices, const GlobalIndex& start, const GlobalIndex& end, @@ -273,5 +124,5 @@ FurthestIndexResult findFurthestIndexFromLine(const GlobalIndexVector& indices, return result; } -} // namespace topology +} // namespace places } // namespace hydra diff --git a/hydra_topology/src/update_statistics.cpp b/hydra/src/places/update_statistics.cpp similarity index 97% rename from hydra_topology/src/update_statistics.cpp rename to hydra/src/places/update_statistics.cpp index dc3d4ec9..ba6563cf 100644 --- a/hydra_topology/src/update_statistics.cpp +++ b/hydra/src/places/update_statistics.cpp @@ -32,10 +32,10 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_topology/update_statistics.h" +#include "hydra/places/update_statistics.h" namespace hydra { -namespace topology { +namespace places { void UpdateStatistics::clear() { number_surface_flipped = 0; @@ -65,5 +65,5 @@ std::ostream& operator<<(std::ostream& out, const UpdateStatistics& stats) { return out; } -} // namespace topology +} // namespace places } // namespace hydra diff --git a/hydra_topology/src/reconstruction_config.cpp b/hydra/src/reconstruction/reconstruction_config.cpp similarity index 98% rename from hydra_topology/src/reconstruction_config.cpp rename to hydra/src/reconstruction/reconstruction_config.cpp index 61e96589..9b623aef 100644 --- a/hydra_topology/src/reconstruction_config.cpp +++ b/hydra/src/reconstruction/reconstruction_config.cpp @@ -32,7 +32,7 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_topology/reconstruction_config.h" +#include "hydra/reconstruction/reconstruction_config.h" namespace voxblox { diff --git a/hydra_topology/src/reconstruction_module.cpp b/hydra/src/reconstruction/reconstruction_module.cpp similarity index 86% rename from hydra_topology/src/reconstruction_module.cpp rename to hydra/src/reconstruction/reconstruction_module.cpp index 2a507e5a..2c992b5b 100644 --- a/hydra_topology/src/reconstruction_module.cpp +++ b/hydra/src/reconstruction/reconstruction_module.cpp @@ -32,32 +32,34 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_topology/reconstruction_module.h" +#include "hydra/reconstruction/reconstruction_module.h" -#include #include #include -#include "hydra_topology/gvd_integrator.h" -#include "hydra_topology/voxel_aware_mesh_integrator.h" +#include "hydra/places/gvd_integrator.h" +#include "hydra/reconstruction/voxel_aware_mesh_integrator.h" +#include "hydra/utils/display_utilities.h" +#include "hydra/utils/timing_utilities.h" namespace hydra { using kimera::SemanticTsdfIntegratorFactory; using kimera::SemanticVoxel; +using places::GvdIntegrator; +using places::GvdVoxel; +using places::VertexVoxel; using pose_graph_tools::PoseGraph; using pose_graph_tools::PoseGraphEdge; using pose_graph_tools::PoseGraphNode; using timing::ScopedTimer; -using topology::GvdIntegrator; -using topology::GvdVoxel; -using topology::VertexVoxel; -using MeshIntegrator = topology::VoxelAwareMeshIntegrator; using voxblox::BlockIndexList; using voxblox::Layer; using voxblox::MeshLayer; using voxblox::TsdfVoxel; +using MeshIntegrator = VoxelAwareMeshIntegrator; + inline Eigen::Affine3d getPose(const geometry_msgs::Pose& pose) { Eigen::Affine3d world_T_body; tf2::convert(pose, world_T_body); @@ -209,7 +211,7 @@ bool ReconstructionModule::spinOnce(const ReconstructionInput& msg) { return false; } - ScopedTimer timer("topology/spin", msg.timestamp_ns); + ScopedTimer timer("places/spin", msg.timestamp_ns); VLOG(2) << "[Hydra Reconstruction]: Processing msg @ " << msg.timestamp_ns; VLOG(2) << "[Hydra Reconstruction]: " << queue_->size() << " message(s) left"; @@ -237,7 +239,7 @@ void ReconstructionModule::update(const ReconstructionInput& msg, bool full_upda const auto world_T_camera = getCameraPose(msg); { // timing scope - ScopedTimer timer("topology/tsdf", msg.timestamp_ns); + ScopedTimer timer("places/tsdf", msg.timestamp_ns); tsdf_integrator_->integratePointCloud( world_T_camera, *msg.pointcloud, *msg.pointcloud_colors, false); } // timing scope @@ -247,7 +249,7 @@ void ReconstructionModule::update(const ReconstructionInput& msg, bool full_upda } { // timing scope - ScopedTimer timer("topology/mesh", msg.timestamp_ns); + ScopedTimer timer("places/mesh", msg.timestamp_ns); mesh_integrator_->generateMesh(true, true); } // timing scope @@ -296,7 +298,7 @@ void ReconstructionModule::updateGvd() { msg = gvd_queue_.front(); msg->pose_graphs.insert( msg->pose_graphs.begin(), pose_graphs.begin(), pose_graphs.end()); - timer.reset(new ScopedTimer("topology/spin_gvd", msg->timestamp_ns)); + timer.reset(new ScopedTimer("places/spin_gvd", msg->timestamp_ns)); gvd_integrator_->updateFromTsdf( msg->timestamp_ns, *tsdf_, *vertices_, *mesh_, true); @@ -385,10 +387,8 @@ PoseGraph ReconstructionModule::makePoseGraph(uint64_t timestamp_ns, } void ReconstructionModule::addPlacesToOutput(ReconstructionOutput& output) { - output.places.reset(new hydra_msgs::ActiveLayer()); - auto& places = const_cast(*output.places); - places.header.stamp.fromNSec(output.timestamp_ns); - places.header.frame_id = config_.world_frame; + output.places.reset(new ActiveLayerInfo()); + auto& places = *output.places; // non-const, as clearDeletedNodes modifies internal state auto& extractor = gvd_integrator_->getGraphExtractor(); std::unordered_set active_nodes = extractor.getActiveNodes(); @@ -434,7 +434,21 @@ void ReconstructionModule::addPlacesToOutput(ReconstructionOutput& output) { } } - places.layer_contents = extractor.getGraph().serializeLayer(active_nodes); + std::set edges; + for (const auto& node_id : active_nodes) { + const auto& node = graph.getNode(node_id)->get(); + places.active_attributes.emplace(node_id, node.attributes().clone()); + for (const auto& sibling : node.siblings()) { + spark_dsg::EdgeKey key(node_id, sibling); + if (edges.count(key)) { + continue; + } + + edges.insert(key); + places.edges.emplace_back( + node_id, sibling, graph.getEdge(node_id, sibling)->get().info->clone()); + } + } extractor.clearDeleted(); VLOG(5) << "[Hydra Reconstruction] exporting " << active_nodes.size() @@ -443,57 +457,46 @@ void ReconstructionModule::addPlacesToOutput(ReconstructionOutput& output) { void ReconstructionModule::addMeshToOutput(ReconstructionOutput& output, const BlockIndexList& archived_blocks) { - output.mesh.reset(new hydra_msgs::ActiveMesh()); - auto& active_mesh = const_cast(*output.mesh); - active_mesh.header.stamp.fromNSec(output.timestamp_ns); - // TODO(nathan) selectable color mode? - generateVoxbloxMeshMsg(mesh_.get(), - voxblox::ColorMode::kColor, - &active_mesh.mesh, - active_mesh.header.stamp); - active_mesh.mesh.header.frame_id = config_.world_frame; - active_mesh.mesh.header.stamp = active_mesh.header.stamp; - - const voxblox::IndexSet archived_set(archived_blocks.begin(), archived_blocks.end()); - - auto iter = active_mesh.mesh.mesh_blocks.begin(); - while (iter != active_mesh.mesh.mesh_blocks.end()) { - voxblox::BlockIndex idx(iter->index[0], iter->index[1], iter->index[2]); - - // we can't just check if the message is empty (it's valid for an observed and - // active block to be empty), so we have to check if the GVD layer has pruned the - // corresponding block yet) - if (archived_set.count(idx)) { - iter = active_mesh.mesh.mesh_blocks.erase(iter); + output.archived_blocks.insert(archived_blocks.begin(), archived_blocks.end()); + + output.mesh.reset(new voxblox::MeshLayer(mesh_->block_size())); + auto& active_mesh = *output.mesh; + + voxblox::BlockIndexList mesh_indices; + mesh_->getAllUpdatedMeshes(&mesh_indices); + for (const auto& block_index : mesh_indices) { + if (output.archived_blocks.count(block_index)) { continue; } - ++iter; + + auto block = active_mesh.allocateNewBlock(block_index); + *block = *(mesh_->getMeshPtrByIndex(block_index)); } - for (const auto& block_idx : archived_blocks) { - voxblox_msgs::MeshBlock block; - block.index[0] = block_idx.x(); - block.index[1] = block_idx.y(); - block.index[2] = block_idx.z(); - active_mesh.archived_blocks.mesh_blocks.push_back(block); + // TODO(nathan) maybe only do this for archived blocks? + for (const auto& block_index : mesh_indices) { + auto block = mesh_->getMeshPtrByIndex(block_index); + if (!block->hasVertices()) { + mesh_->removeMesh(block_index); + } } } void ReconstructionModule::showStats() const { const std::string tsdf_memory_str = - hydra_utils::getHumanReadableMemoryString(tsdf_->getMemorySize()); + getHumanReadableMemoryString(tsdf_->getMemorySize()); const std::string semantic_memory_str = - hydra_utils::getHumanReadableMemoryString(semantics_->getMemorySize()); + getHumanReadableMemoryString(semantics_->getMemorySize()); const std::string gvd_memory_str = - hydra_utils::getHumanReadableMemoryString(gvd_->getMemorySize()); + getHumanReadableMemoryString(gvd_->getMemorySize()); const std::string mesh_memory_str = - hydra_utils::getHumanReadableMemoryString(mesh_->getMemorySize()); + getHumanReadableMemoryString(mesh_->getMemorySize()); const size_t total = tsdf_->getMemorySize() + semantics_->getMemorySize() + gvd_->getMemorySize(); LOG(INFO) << "Memory used: [TSDF=" << tsdf_memory_str << ", Semantics=" << semantic_memory_str << ", GVD=" << gvd_memory_str << ", Mesh= " << mesh_memory_str - << ", Total=" << hydra_utils::getHumanReadableMemoryString(total) << "]"; + << ", Total=" << getHumanReadableMemoryString(total) << "]"; } BlockIndexList ReconstructionModule::findBlocksToArchive( diff --git a/hydra_topology/src/voxel_aware_marching_cubes.cpp b/hydra/src/reconstruction/voxel_aware_marching_cubes.cpp similarity index 97% rename from hydra_topology/src/voxel_aware_marching_cubes.cpp rename to hydra/src/reconstruction/voxel_aware_marching_cubes.cpp index ce4f3f89..2cb9d5c8 100644 --- a/hydra_topology/src/voxel_aware_marching_cubes.cpp +++ b/hydra/src/reconstruction/voxel_aware_marching_cubes.cpp @@ -35,14 +35,18 @@ // implied, of the United States Air Force or the U.S. Government. The U.S. // Government is authorized to reproduce and distribute reprints for Government // purposes notwithstanding any copyright notation herein. -#include "hydra_topology/voxel_aware_marching_cubes.h" +#include "hydra/reconstruction/voxel_aware_marching_cubes.h" -#include "hydra_topology/gvd_utilities.h" +#include "hydra/reconstruction/voxblox_utilities.h" namespace hydra { -namespace topology { +using places::VertexVoxel; +using voxblox::BlockIndex; +using voxblox::FloatingPoint; +using voxblox::Mesh; using voxblox::Point; +using voxblox::VertexIndex; static constexpr FloatingPoint kMinSdfDifference = 1e-6; @@ -198,5 +202,4 @@ void VoxelAwareMarchingCubes::meshCube(const BlockIndex& block, } } -} // namespace topology } // namespace hydra diff --git a/hydra_topology/src/voxel_aware_mesh_integrator.cpp b/hydra/src/reconstruction/voxel_aware_mesh_integrator.cpp similarity index 96% rename from hydra_topology/src/voxel_aware_mesh_integrator.cpp rename to hydra/src/reconstruction/voxel_aware_mesh_integrator.cpp index 385999be..dbb1d49d 100644 --- a/hydra_topology/src/voxel_aware_mesh_integrator.cpp +++ b/hydra/src/reconstruction/voxel_aware_mesh_integrator.cpp @@ -36,28 +36,39 @@ // implied, of the United States Air Force or the U.S. Government. The U.S. // Government is authorized to reproduce and distribute reprints for Government // purposes notwithstanding any copyright notation herein. -#include "hydra_topology/voxel_aware_mesh_integrator.h" +#include "hydra/reconstruction/voxel_aware_mesh_integrator.h" #include #include #include -#include "hydra_topology/voxel_aware_marching_cubes.h" +#include "hydra/reconstruction/voxblox_utilities.h" +#include "hydra/reconstruction/voxel_aware_marching_cubes.h" namespace hydra { -namespace topology { +using places::VertexVoxel; +using voxblox::Block; +using voxblox::BlockIndex; +using voxblox::BlockIndexList; +using voxblox::FloatingPoint; using voxblox::IndexElement; +using voxblox::Layer; +using voxblox::Mesh; using voxblox::MeshIntegratorConfig; +using voxblox::MeshLayer; using voxblox::MixedThreadSafeIndex; using voxblox::Point; +using voxblox::ThreadSafeIndex; +using voxblox::TsdfVoxel; +using voxblox::VertexIndex; +using voxblox::VoxelIndex; namespace vutils = voxblox::utils; using TsdfLayer = Layer; using TsdfBlock = Block; using VertexLayer = Layer; -using GvdBlock = Block; VoxelAwareMeshIntegrator::VoxelAwareMeshIntegrator(const MeshIntegratorConfig& config, TsdfLayer* sdf_layer, @@ -318,5 +329,4 @@ void VoxelAwareMeshIntegrator::extractMeshOnBorder(const TsdfBlock& block, VoxelAwareMarchingCubes::meshCube(block_idx, coords, sdf, new_mesh_idx, mesh, voxels); } -} // namespace topology } // namespace hydra diff --git a/hydra_dsg_builder/src/graph_clustering.cpp b/hydra/src/rooms/graph_clustering.cpp similarity index 99% rename from hydra_dsg_builder/src/graph_clustering.cpp rename to hydra/src/rooms/graph_clustering.cpp index 6f3ed56c..1047e08c 100644 --- a/hydra_dsg_builder/src/graph_clustering.cpp +++ b/hydra/src/rooms/graph_clustering.cpp @@ -32,7 +32,7 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_dsg_builder/graph_clustering.h" +#include "hydra/rooms/graph_clustering.h" #include diff --git a/hydra_dsg_builder/src/graph_filtration.cpp b/hydra/src/rooms/graph_filtration.cpp similarity index 99% rename from hydra_dsg_builder/src/graph_filtration.cpp rename to hydra/src/rooms/graph_filtration.cpp index a45c3a1a..b0a4e559 100644 --- a/hydra_dsg_builder/src/graph_filtration.cpp +++ b/hydra/src/rooms/graph_filtration.cpp @@ -32,7 +32,7 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_dsg_builder/graph_filtration.h" +#include "hydra/rooms/graph_filtration.h" #include diff --git a/hydra_dsg_builder/src/room_finder.cpp b/hydra/src/rooms/room_finder.cpp similarity index 93% rename from hydra_dsg_builder/src/room_finder.cpp rename to hydra/src/rooms/room_finder.cpp index e503c417..f09e4125 100644 --- a/hydra_dsg_builder/src/room_finder.cpp +++ b/hydra/src/rooms/room_finder.cpp @@ -32,7 +32,7 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_dsg_builder/room_finder.h" +#include "hydra/rooms/room_finder.h" #include @@ -40,9 +40,9 @@ #include #include -#include "hydra_dsg_builder/graph_filtration.h" -#include "hydra_dsg_builder/hydra_config.h" -#include "hydra_dsg_builder/room_helpers.h" +#include "hydra/common/hydra_config.h" +#include "hydra/rooms/graph_filtration.h" +#include "hydra/rooms/room_utilities.h" namespace hydra { @@ -155,20 +155,20 @@ void RoomFinder::enableLogging(const std::string& log_path) { InitialClusters RoomFinder::getBestComponents(const SceneGraphLayer& places) const { BarcodeTracker tracker(config_.min_component_size); - const auto filtration = - getGraphFiltration(places, - tracker, - config_.dilation_diff_threshold_m, - [this](const DisjointSet& components) -> size_t { - size_t num_components = 0; - for (const auto id_size_pair : components.sizes) { - if (id_size_pair.second >= config_.min_component_size) { - ++num_components; - } - } - return num_components; - }, - false); + const auto filtration = getGraphFiltration( + places, + tracker, + config_.dilation_diff_threshold_m, + [this](const DisjointSet& components) -> size_t { + size_t num_components = 0; + for (const auto id_size_pair : components.sizes) { + if (id_size_pair.second >= config_.min_component_size) { + ++num_components; + } + } + return num_components; + }, + false); VLOG(10) << "[RoomFinder] Filtration: " << filtration; diff --git a/hydra_dsg_builder/src/room_helpers.cpp b/hydra/src/rooms/room_utilities.cpp similarity index 99% rename from hydra_dsg_builder/src/room_helpers.cpp rename to hydra/src/rooms/room_utilities.cpp index fa5bfd19..93cf1e9c 100644 --- a/hydra_dsg_builder/src/room_helpers.cpp +++ b/hydra/src/rooms/room_utilities.cpp @@ -32,7 +32,7 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_dsg_builder/room_helpers.h" +#include "hydra/rooms/room_utilities.h" namespace hydra { diff --git a/hydra_dsg_builder/src/disjoint_set.cpp b/hydra/src/utils/disjoint_set.cpp similarity index 98% rename from hydra_dsg_builder/src/disjoint_set.cpp rename to hydra/src/utils/disjoint_set.cpp index 83fa20d9..6df5a5c1 100644 --- a/hydra_dsg_builder/src/disjoint_set.cpp +++ b/hydra/src/utils/disjoint_set.cpp @@ -32,7 +32,7 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_dsg_builder/disjoint_set.h" +#include "hydra/utils/disjoint_set.h" #include diff --git a/hydra_utils/src/display_utils.cpp b/hydra/src/utils/display_utilities.cpp similarity index 96% rename from hydra_utils/src/display_utils.cpp rename to hydra/src/utils/display_utilities.cpp index 81a6ea7e..8845fa31 100644 --- a/hydra_utils/src/display_utils.cpp +++ b/hydra/src/utils/display_utilities.cpp @@ -32,9 +32,9 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_utils/display_utils.h" +#include "hydra/utils/display_utilities.h" -namespace hydra_utils { +namespace hydra { std::string getHumanReadableMemoryString(size_t bytes) { const size_t log_bytes = static_cast(std::floor(std::log2(bytes))); @@ -55,4 +55,4 @@ std::string getHumanReadableMemoryString(size_t bytes) { return ss.str(); } -} // namespace hydra_utils +} // namespace hydra diff --git a/hydra_utils/src/log_utilities.cpp b/hydra/src/utils/log_utilities.cpp similarity index 98% rename from hydra_utils/src/log_utilities.cpp rename to hydra/src/utils/log_utilities.cpp index 0956aa79..14b7f3b0 100644 --- a/hydra_utils/src/log_utilities.cpp +++ b/hydra/src/utils/log_utilities.cpp @@ -32,12 +32,14 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_utils/log_utilities.h" -#include "hydra_utils/timing_utilities.h" +#include "hydra/utils/log_utilities.h" #include + #include +#include "hydra/utils/timing_utilities.h" + namespace fs = boost::filesystem; namespace hydra { diff --git a/hydra_dsg_builder/src/minimum_spanning_tree.cpp b/hydra/src/utils/minimum_spanning_tree.cpp similarity index 97% rename from hydra_dsg_builder/src/minimum_spanning_tree.cpp rename to hydra/src/utils/minimum_spanning_tree.cpp index 93fe88ec..fae48461 100644 --- a/hydra_dsg_builder/src/minimum_spanning_tree.cpp +++ b/hydra/src/utils/minimum_spanning_tree.cpp @@ -32,11 +32,12 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_dsg_builder/minimum_spanning_tree.h" -#include "hydra_dsg_builder/disjoint_set.h" +#include "hydra/utils/minimum_spanning_tree.h" #include +#include "hydra/utils/disjoint_set.h" + namespace hydra { // implementation mainly from: https://en.wikipedia.org/wiki/Kruskal%27s_algorithm diff --git a/hydra/src/utils/nearest_neighbor_utilities.cpp b/hydra/src/utils/nearest_neighbor_utilities.cpp new file mode 100644 index 00000000..889ecfbb --- /dev/null +++ b/hydra/src/utils/nearest_neighbor_utilities.cpp @@ -0,0 +1,112 @@ +/* ----------------------------------------------------------------------------- + * Copyright 2022 Massachusetts Institute of Technology. + * All Rights Reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Research was sponsored by the United States Air Force Research Laboratory and + * the United States Air Force Artificial Intelligence Accelerator and was + * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views + * and conclusions contained in this document are those of the authors and should + * not be interpreted as representing the official policies, either expressed or + * implied, of the United States Air Force or the U.S. Government. The U.S. + * Government is authorized to reproduce and distribute reprints for Government + * purposes notwithstanding any copyright notation herein. + * -------------------------------------------------------------------------- */ +#include "hydra/utils/nearest_neighbor_utilities.h" + +#include + +#include + +namespace hydra { + +using nanoflann::KDTreeSingleIndexAdaptor; +using nanoflann::KDTreeSingleIndexDynamicAdaptor; +using nanoflann::L2_Simple_Adaptor; + +struct GraphKdTreeAdaptor { + GraphKdTreeAdaptor(const SceneGraphLayer& layer, const std::vector& nodes) + : layer(layer), nodes(nodes) {} + + inline size_t kdtree_get_point_count() const { return nodes.size(); } + + inline double kdtree_get_pt(const size_t idx, const size_t dim) const { + return layer.getPosition(nodes[idx])(dim); + } + + template + bool kdtree_get_bbox(T&) const { + return false; + } + + const SceneGraphLayer& layer; + std::vector nodes; +}; + +struct NearestNodeFinder::Detail { + using Dist = L2_Simple_Adaptor; + using KDTree = KDTreeSingleIndexAdaptor; + + Detail(const SceneGraphLayer& layer, const std::vector& nodes) + : adaptor(layer, nodes) { + kdtree.reset(new KDTree(3, adaptor)); + kdtree->buildIndex(); + } + + ~Detail() = default; + + GraphKdTreeAdaptor adaptor; + std::unique_ptr kdtree; +}; + +NearestNodeFinder::NearestNodeFinder(const SceneGraphLayer& layer, + const std::vector& nodes) + : internals_(new Detail(layer, nodes)) {} + +NearestNodeFinder::NearestNodeFinder(const SceneGraphLayer& layer, + const std::unordered_set& nodes) { + std::vector node_vector(nodes.begin(), nodes.end()); + VLOG(10) << "Making node finder with " << nodes.size() << " nodes"; + internals_.reset(new Detail(layer, node_vector)); +} + +NearestNodeFinder::~NearestNodeFinder() {} + +void NearestNodeFinder::find(const Eigen::Vector3d& position, + size_t num_to_find, + bool skip_first, + const NearestNodeFinder::Callback& callback) { + const size_t limit = skip_first ? num_to_find + 1 : num_to_find; + std::vector nn_indices(limit); + std::vector distances(limit); + + size_t num_found = internals_->kdtree->knnSearch( + position.data(), limit, nn_indices.data(), distances.data()); + + size_t i = skip_first ? 1 : 0; + for (; i < num_found; ++i) { + callback(internals_->adaptor.nodes[nn_indices[i]], nn_indices[i], distances[i]); + } +} + +} // namespace hydra diff --git a/hydra_utils/src/timing_utilities.cpp b/hydra/src/utils/timing_utilities.cpp similarity index 99% rename from hydra_utils/src/timing_utilities.cpp rename to hydra/src/utils/timing_utilities.cpp index 9b36b2c9..a84aca0e 100644 --- a/hydra_utils/src/timing_utilities.cpp +++ b/hydra/src/utils/timing_utilities.cpp @@ -32,7 +32,7 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_utils/timing_utilities.h" +#include "hydra/utils/timing_utilities.h" #include diff --git a/hydra/tests/CMakeLists.txt b/hydra/tests/CMakeLists.txt new file mode 100644 index 00000000..8b55120a --- /dev/null +++ b/hydra/tests/CMakeLists.txt @@ -0,0 +1,43 @@ +catkin_add_gtest( + test_${PROJECT_NAME} + main.cpp + src/resources.cpp + src/place_fixtures.cpp + backend/test_merge_handler.cpp + backend/test_update_functions.cpp + config/test_config.cpp + frontend/test_frontend.cpp + loop_closure/test_descriptor_matching.cpp + loop_closure/test_detector.cpp + loop_closure/test_registration.cpp + loop_closure/test_scene_graph_descriptors.cpp + loop_closure/test_subgraph_extraction.cpp + places/test_compression_graph_extractor.cpp + places/test_esdf_comparison.cpp + places/test_floodfill_graph_extractor.cpp + places/test_graph_extractor_utilities.cpp + places/test_gvd_incremental.cpp + places/test_gvd_integrator.cpp + places/test_gvd_thinning.cpp + places/test_gvd_utilities.cpp + reconstruction/test_marching_cubes.cpp + reconstruction/test_reconstruction_module.cpp + rooms/test_graph_clustering.cpp + rooms/test_graph_filtration.cpp + rooms/test_room_finder.cpp + rooms/test_room_finder_config.cpp + rooms/test_room_utilities.cpp + utils/test_minimum_spanning_tree.cpp + utils/test_nearest_neighbor_utilities.cpp + utils/test_timing_utilities.cpp +) +target_include_directories(test_${PROJECT_NAME} PUBLIC include) +target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME}) + +if(HYDRA_GNN) + target_sources( + test_${PROJECT_NAME} + PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/loop_closure/test_gnn_descriptors.cpp + ) + target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME}_gnn) +endif() diff --git a/hydra_dsg_builder/tests/utest_merge_handler.cpp b/hydra/tests/backend/test_merge_handler.cpp similarity index 99% rename from hydra_dsg_builder/tests/utest_merge_handler.cpp rename to hydra/tests/backend/test_merge_handler.cpp index 9f0de526..da788b82 100644 --- a/hydra_dsg_builder/tests/utest_merge_handler.cpp +++ b/hydra/tests/backend/test_merge_handler.cpp @@ -32,12 +32,10 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include - #include +#include namespace hydra { -namespace incremental { using ObjectAttrs = ObjectNodeAttributes; using PlaceAttrs = PlaceNodeAttributes; @@ -487,5 +485,4 @@ TEST(MergeHandlerTests, TestInvalidUpdaters) { } } -} // namespace incremental } // namespace hydra diff --git a/hydra_dsg_builder/tests/utest_dsg_update_functions.cpp b/hydra/tests/backend/test_update_functions.cpp similarity index 99% rename from hydra_dsg_builder/tests/utest_dsg_update_functions.cpp rename to hydra/tests/backend/test_update_functions.cpp index 7b043142..82093757 100644 --- a/hydra_dsg_builder/tests/utest_dsg_update_functions.cpp +++ b/hydra/tests/backend/test_update_functions.cpp @@ -34,13 +34,12 @@ * -------------------------------------------------------------------------- */ #include #include -#include +#include namespace hydra { using MeshVertices = DynamicSceneGraph::MeshVertices; using MeshFaces = DynamicSceneGraph::MeshFaces; -using incremental::SharedDsgInfo; #define MAKE_POINT(cloud, x_val, y_val, z_val) \ { \ diff --git a/hydra_utils/tests/utest_config.cpp b/hydra/tests/config/test_config.cpp similarity index 73% rename from hydra_utils/tests/utest_config.cpp rename to hydra/tests/config/test_config.cpp index e51e89fb..52b8266a 100644 --- a/hydra_utils/tests/utest_config.cpp +++ b/hydra/tests/config/test_config.cpp @@ -32,16 +32,11 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include -#include - -#include - #include +#include +#include -std::string get_test_path() { - return ros::package::getPath("hydra_utils") + "/tests/resources/"; -} +#include "hydra_test/resources.h" namespace hydra { @@ -178,7 +173,8 @@ std::map MapConverter::to( std::map to_return; for (const auto& kv_pair : other) { // defined by enum macro - TestEnum new_enum = readTestEnumFromString(kv_pair.first); + TestEnum new_enum; + readConfigEnumFromString(kv_pair.first, new_enum); to_return[new_enum] = kv_pair.second; } return to_return; @@ -189,37 +185,15 @@ std::map MapConverter::from( std::map to_return; for (const auto& kv_pair : other) { std::stringstream ss; - ss << kv_pair.first; + ss << configEnumToString(kv_pair.first); to_return[ss.str()] = kv_pair.second; } return to_return; } -TEST(ConfigParser, RosName) { - config_parser::RosParserImpl parser(ros::NodeHandle("/plain_test_config")); - EXPECT_EQ(parser.name(), "/plain_test_config"); - - auto new_parser = parser.child("foo"); - EXPECT_EQ(new_parser.name(), "/plain_test_config/foo"); -} - -TEST(ConfigParsing, RosChildren) { - config_parser::RosParserImpl parser(ros::NodeHandle("/plain_test_config")); - auto children = parser.children(); - EXPECT_EQ(children.size(), 12u); - - auto value_parser = parser.child("value_map"); - auto value_children = value_parser.children(); - EXPECT_EQ(value_children.size(), 2u); - - auto enable_parser = parser.child("enable_map"); - auto enable_children = enable_parser.children(); - EXPECT_EQ(enable_children.size(), 2u); -} - TEST(ConfigParsing, YamlChildren) { - const std::string filepath = get_test_path() + "test_config.yaml"; + const std::string filepath = test::get_resource_path("config/test_config.yaml"); config_parser::YamlParserImpl parser(filepath); auto children = parser.children(); EXPECT_EQ(children.size(), 12u); @@ -234,7 +208,7 @@ TEST(ConfigParsing, YamlChildren) { } TEST(ConfigParsing, ParseSingleStructYaml) { - const std::string filepath = get_test_path() + "test_config.yaml"; + const std::string filepath = test::get_resource_path("config/test_config.yaml"); auto config = config_parser::load_from_yaml(filepath); EXPECT_EQ(config.foo, 10.0f); @@ -264,38 +238,9 @@ TEST(ConfigParsing, ParseSingleStructYaml) { EXPECT_EQ(config.unique_values, expected_unique_values); } -TEST(ConfigParsing, ParseSingleStructRos) { - auto config = config_parser::load_from_ros("/plain_test_config"); - - EXPECT_EQ(config.foo, 10.0f); - EXPECT_EQ(config.bar, 5.0); - EXPECT_EQ(config.a, -3); - EXPECT_EQ(static_cast(config.b), 1); - EXPECT_EQ(config.c, 2); - EXPECT_EQ(config.msg, "world"); - - std::vector expected_values{4, 5, 6}; - EXPECT_EQ(config.values, expected_values); - - std::map expected_value_map{{"3", 4}, {"5", 6}}; - EXPECT_EQ(config.value_map, expected_value_map); - - EXPECT_EQ(config.type, TestEnum::GREEN); - - Eigen::Matrix expected_vec; - expected_vec << 7, 8, 9; - EXPECT_EQ(config.vec, expected_vec); - - std::map expected_enable_map{{TestEnum::BLUE, 0}, - {TestEnum::GREEN, 1}}; - EXPECT_EQ(config.enable_map, expected_enable_map); - - std::set expected_unique_values{4, 5}; - EXPECT_EQ(config.unique_values, expected_unique_values); -} - TEST(ConfigParsing, ParseNestedStructYaml) { - const std::string filepath = get_test_path() + "nested_test_config.yaml"; + const std::string filepath = + test::get_resource_path("config/nested_test_config.yaml"); auto config = config_parser::load_from_yaml(filepath); EXPECT_EQ(config.fake_config.foo, 10.0f); @@ -315,26 +260,6 @@ TEST(ConfigParsing, ParseNestedStructYaml) { EXPECT_EQ(config.fake_config.enable_map, expected_enable_map); } -TEST(ConfigParsing, ParseNestedStructRos) { - auto config = config_parser::load_from_ros("/nested_test_config"); - - EXPECT_EQ(config.fake_config.foo, 10.0f); - EXPECT_EQ(config.fake_config.bar, 5.0); - EXPECT_EQ(config.fake_config.a, -3); - EXPECT_EQ(static_cast(config.fake_config.b), 1); - EXPECT_EQ(config.fake_config.c, 2); - EXPECT_EQ(config.fake_config.msg, "hello"); - - std::vector expected_values{1, 2, 3}; - EXPECT_EQ(config.fake_config.values, expected_values); - EXPECT_EQ(config.msg, "again"); - - // make sure conversion respects the default - std::map expected_enable_map{{TestEnum::RED, 1}, - {TestEnum::GREEN, 0}}; - EXPECT_EQ(config.fake_config.enable_map, expected_enable_map); -} - TEST(ConfigParsing, OutputSingleConfig) { FakeConfig config; std::stringstream ss; @@ -386,7 +311,7 @@ TEST(ConfigParsing, OutputNestedConfig) { } TEST(ConfigParsing, ParseMapStructYaml) { - const std::string filepath = get_test_path() + "map_config.yaml"; + const std::string filepath = test::get_resource_path("config/map_config.yaml"); auto config = config_parser::load_from_yaml(filepath); EXPECT_EQ(config.configs.size(), 2u); @@ -404,26 +329,8 @@ TEST(ConfigParsing, ParseMapStructYaml) { EXPECT_EQ(config_b.c, "other"); } -TEST(ConfigParsing, ParseMapStructRos) { - auto config = config_parser::load_from_ros("/map_test_config"); - - EXPECT_EQ(config.configs.size(), 2u); - ASSERT_TRUE(config.configs.count("config_a")); - ASSERT_TRUE(config.configs.count("config_b")); - - BarConfig config_a = config.configs["config_a"]; - EXPECT_EQ(config_a.a, -3); - EXPECT_NEAR(config_a.b, 5.2f, 1.0e-6f); - EXPECT_EQ(config_a.c, "hello"); - - BarConfig config_b = config.configs["config_b"]; - EXPECT_EQ(config_b.a, 5); - EXPECT_NEAR(config_b.b, -1.2f, 1.0e-6f); - EXPECT_EQ(config_b.c, "other"); -} - TEST(ConfigParsing, OutputMapConfig) { - const std::string filepath = get_test_path() + "map_config.yaml"; + const std::string filepath = test::get_resource_path("config/map_config.yaml"); auto config = config_parser::load_from_yaml(filepath); std::stringstream ss; @@ -447,7 +354,7 @@ TEST(ConfigParsing, OutputMapConfig) { TEST(ConfigParsing, TestMissing) { auto logger = std::make_shared(); - const std::string filepath = get_test_path() + "missing_config.yaml"; + const std::string filepath = test::get_resource_path("config/missing_config.yaml"); auto config = config_parser::load_from_yaml(filepath, logger); std::string expected = R"out( diff --git a/hydra_dsg_builder/tests/utest_frontend.cpp b/hydra/tests/frontend/test_frontend.cpp similarity index 90% rename from hydra_dsg_builder/tests/utest_frontend.cpp rename to hydra/tests/frontend/test_frontend.cpp index ad23d13e..e94a747d 100644 --- a/hydra_dsg_builder/tests/utest_frontend.cpp +++ b/hydra/tests/frontend/test_frontend.cpp @@ -32,18 +32,13 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include - -#include - #include +#include + +#include "hydra_test/resources.h" namespace hydra { -using incremental::DsgFrontend; -using incremental::DsgFrontendConfig; -using incremental::SharedDsgInfo; -using incremental::SharedModuleState; using pose_graph_tools::PoseGraph; using pose_graph_tools::PoseGraphNode; @@ -58,15 +53,13 @@ inline SharedDsgInfo::Ptr makeSharedDsg() { return SharedDsgInfo::Ptr(new SharedDsgInfo(layer_id_map, mesh_layer_id)); } -inline DsgFrontendConfig getDefaultConfig() { - DsgFrontendConfig to_return; +inline FrontendConfig getDefaultConfig() { + FrontendConfig to_return; to_return.should_log = false; to_return.filter_places = false; to_return.validate_vertices = false; - std::string semantics_path = ros::package::getPath("kimera_semantics_ros"); - to_return.semantic_label_file = - semantics_path + "/cfg/tesse_multiscene_archviz1_segmentation_mapping.csv"; + to_return.semantic_label_file = hydra::test::get_default_semantic_map(); return to_return; } @@ -129,12 +122,12 @@ void addPlaceNode(IsolatedSceneGraphLayer& graph, } // namespace -TEST(DsgFrontendTests, TestAgentEdges) { +TEST(FrontendModuleTests, TestAgentEdges) { auto prefix = RobotPrefixConfig(0); auto config = getDefaultConfig(); auto dsg = makeSharedDsg(); auto state = std::make_shared(); - auto frontend = std::make_shared(prefix, config, dsg, state); + auto frontend = std::make_shared(prefix, config, dsg, state); auto queue = frontend->getQueue(); IsolatedSceneGraphLayer places(2); diff --git a/hydra_topology/tests/include/hydra_topology_test/layer_utils.h b/hydra/tests/include/hydra_test/gvd_layer_utils.h similarity index 95% rename from hydra_topology/tests/include/hydra_topology_test/layer_utils.h rename to hydra/tests/include/hydra_test/gvd_layer_utils.h index c2d2d8bd..305e6512 100644 --- a/hydra_topology/tests/include/hydra_topology_test/layer_utils.h +++ b/hydra/tests/include/hydra_test/gvd_layer_utils.h @@ -33,8 +33,8 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include -#include +#include +#include #include #include @@ -42,10 +42,10 @@ namespace voxblox { namespace utils { template <> -bool isObservedVoxel(const ::hydra::topology::GvdVoxel& voxel); +bool isObservedVoxel(const ::hydra::places::GvdVoxel& voxel); template <> -inline bool isObservedVoxel(const ::hydra::topology::GvdVoxel& voxel) { +inline bool isObservedVoxel(const ::hydra::places::GvdVoxel& voxel) { return voxel.observed; } @@ -53,8 +53,8 @@ inline bool isObservedVoxel(const ::hydra::topology::GvdVoxel& voxel) { } // namespace voxblox namespace hydra { -namespace topology { -namespace test_helpers { +namespace places { +namespace test { struct LayerComparisonResult { bool valid = false; @@ -192,6 +192,6 @@ inline std::ostream& operator<<(std::ostream& out, return out; } -} // namespace test_helpers -} // namespace topology +} // namespace test +} // namespace places } // namespace hydra diff --git a/hydra_topology/tests/include/hydra_topology_test/test_fixtures.h b/hydra/tests/include/hydra_test/place_fixtures.h similarity index 96% rename from hydra_topology/tests/include/hydra_topology_test/test_fixtures.h rename to hydra/tests/include/hydra_test/place_fixtures.h index fe2e07e8..c0033556 100644 --- a/hydra_topology/tests/include/hydra_topology_test/test_fixtures.h +++ b/hydra/tests/include/hydra_test/place_fixtures.h @@ -34,14 +34,14 @@ * -------------------------------------------------------------------------- */ #pragma once #include -#include -#include +#include +#include #include #include namespace hydra { -namespace topology { -namespace test_helpers { +namespace places { +namespace test { // loosely based on the test fixture in test_sdf_integrators.cc in voxblox class EsdfTestFixture : public ::testing::Test { @@ -157,6 +157,6 @@ class TestFixture2d : public ::testing::Test { voxblox::Block::Ptr vertex_block; }; -} // namespace test_helpers -} // namespace topology +} // namespace test +} // namespace places } // namespace hydra diff --git a/hydra/tests/include/hydra_test/resources.h b/hydra/tests/include/hydra_test/resources.h new file mode 100644 index 00000000..60dc425d --- /dev/null +++ b/hydra/tests/include/hydra_test/resources.h @@ -0,0 +1,48 @@ +/* ----------------------------------------------------------------------------- + * Copyright 2022 Massachusetts Institute of Technology. + * All Rights Reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Research was sponsored by the United States Air Force Research Laboratory and + * the United States Air Force Artificial Intelligence Accelerator and was + * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views + * and conclusions contained in this document are those of the authors and should + * not be interpreted as representing the official policies, either expressed or + * implied, of the United States Air Force or the U.S. Government. The U.S. + * Government is authorized to reproduce and distribute reprints for Government + * purposes notwithstanding any copyright notation herein. + * -------------------------------------------------------------------------- */ +#pragma once +#include + +namespace hydra { +namespace test { + +std::string get_resource_path(); + +std::string get_resource_path(const std::string& name); + +std::string get_default_semantic_map(); + +} // namespace test +} // namespace hydra diff --git a/hydra_dsg_builder/tests/utest_dsg_lcd_matching.cpp b/hydra/tests/loop_closure/test_descriptor_matching.cpp similarity index 92% rename from hydra_dsg_builder/tests/utest_dsg_lcd_matching.cpp rename to hydra/tests/loop_closure/test_descriptor_matching.cpp index 3b647fc2..5c387289 100644 --- a/hydra_dsg_builder/tests/utest_dsg_lcd_matching.cpp +++ b/hydra/tests/loop_closure/test_descriptor_matching.cpp @@ -32,14 +32,13 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_dsg_builder/dsg_lcd_detector.h" - #include +#include namespace hydra { namespace lcd { -TEST(DsgLcdMatchingTests, TestCosineDistanceFixedSize) { +TEST(LoopClosureModuleMatchingTests, TestCosineDistanceFixedSize) { Descriptor d1; d1.values.resize(5, 1); d1.values << 1.0f, 2.0f, 3.0f, 4.0f, 5.0f; @@ -92,7 +91,7 @@ TEST(DsgLcdMatchingTests, TestCosineDistanceFixedSize) { } } -TEST(DsgLcdMatchingTests, TestCosineDistanceVariableSize) { +TEST(LoopClosureModuleMatchingTests, TestCosineDistanceVariableSize) { Descriptor d1; d1.values.resize(5, 1); d1.words.resize(5, 1); @@ -123,7 +122,7 @@ TEST(DsgLcdMatchingTests, TestCosineDistanceVariableSize) { } } -TEST(DsgLcdMatchingTests, TestComputeL1DistanceBow) { +TEST(LoopClosureModuleMatchingTests, TestComputeL1DistanceBow) { Descriptor d1; d1.values.resize(5, 1); d1.words.resize(5, 1); @@ -164,7 +163,7 @@ TEST(DsgLcdMatchingTests, TestComputeL1DistanceBow) { } } -TEST(DsgLcdMatchingTests, TestComputeL1DistanceFixed) { +TEST(LoopClosureModuleMatchingTests, TestComputeL1DistanceFixed) { Descriptor d1; d1.values.resize(5, 1); d1.values << 1.0f, 2.0f, 3.0f, 4.0f, 5.0f; @@ -189,7 +188,7 @@ TEST(DsgLcdMatchingTests, TestComputeL1DistanceFixed) { } } -TEST(DsgLcdMatchingTests, TestComputeL1DistanceEmpty) { +TEST(LoopClosureModuleMatchingTests, TestComputeL1DistanceEmpty) { Descriptor d1; d1.values.resize(5, 1); d1.values.setZero(); @@ -208,7 +207,7 @@ TEST(DsgLcdMatchingTests, TestComputeL1DistanceEmpty) { } } -TEST(DsgLcdMatchingTests, TestComputeCosDistanceEmpty) { +TEST(LoopClosureModuleMatchingTests, TestComputeCosDistanceEmpty) { Descriptor d1; d1.values.resize(5, 1); d1.values.setZero(); @@ -244,7 +243,7 @@ void fillDescriptor(Descriptor& descriptor, NodeId root, std::set nodes) descriptor.nodes = nodes; } -TEST(DsgLcdMatchingTests, SearchDescriptorsNoValid) { +TEST(LoopClosureModuleMatchingTests, SearchDescriptorsNoValid) { Descriptor::Ptr query = makeDescriptor(1.0f); DescriptorMatchConfig config; @@ -264,13 +263,14 @@ TEST(DsgLcdMatchingTests, SearchDescriptorsNoValid) { root_leaf_map[3] = {}; LayerSearchResults results = searchDescriptors(*query, config, valid_matches, descriptors, root_leaf_map, 5); - EXPECT_TRUE(results.score.empty()); + ASSERT_EQ(results.score.size(), 1u); + EXPECT_EQ(results.score.front(), 0.0); EXPECT_TRUE(results.valid_matches.empty()); EXPECT_TRUE(results.query_nodes.empty()); EXPECT_TRUE(results.match_nodes.empty()); } -TEST(DsgLcdMatchingTests, SearchDescriptorsValidNoMatches) { +TEST(LoopClosureModuleMatchingTests, SearchDescriptorsValidNoMatches) { Descriptor::Ptr query = makeDescriptor(1.0f); DescriptorMatchConfig config; @@ -291,13 +291,14 @@ TEST(DsgLcdMatchingTests, SearchDescriptorsValidNoMatches) { LayerSearchResults results = searchDescriptors(*query, config, valid_matches, descriptors, root_leaf_map, 5); - EXPECT_TRUE(results.score.empty()); + ASSERT_EQ(results.score.size(), 1u); + EXPECT_NEAR(results.score.front(), 1.0, 1.0e-9); EXPECT_TRUE(results.valid_matches.empty()); EXPECT_TRUE(results.query_nodes.empty()); EXPECT_TRUE(results.match_nodes.empty()); } -TEST(DsgLcdMatchingTests, SearchDescriptorsValidSomeMatches) { +TEST(LoopClosureModuleMatchingTests, SearchDescriptorsValidSomeMatches) { Descriptor::Ptr query = makeDescriptor(1.0f, 0.0f); fillDescriptor(*query, 0, {13, 14, 15}); @@ -341,7 +342,7 @@ TEST(DsgLcdMatchingTests, SearchDescriptorsValidSomeMatches) { EXPECT_EQ(0u, results.query_root); } -TEST(DsgLcdMatchingTests, searchLeafDescriptorsNoValid) { +TEST(LoopClosureModuleMatchingTests, searchLeafDescriptorsNoValid) { Descriptor::Ptr query = makeDescriptor(1.0f); DescriptorMatchConfig config; @@ -365,7 +366,7 @@ TEST(DsgLcdMatchingTests, searchLeafDescriptorsNoValid) { EXPECT_TRUE(results.match_nodes.empty()); } -TEST(DsgLcdMatchingTests, SearchLeafDescriptorsAllValid) { +TEST(LoopClosureModuleMatchingTests, SearchLeafDescriptorsAllValid) { Descriptor::Ptr query = makeDescriptor(1.0f); DescriptorMatchConfig config; @@ -396,7 +397,7 @@ TEST(DsgLcdMatchingTests, SearchLeafDescriptorsAllValid) { EXPECT_EQ(expected_match_nodes, results.match_nodes[0]); } -TEST(DsgLcdMatchingTests, SearchLeafDescriptorsTimeSeparation) { +TEST(LoopClosureModuleMatchingTests, SearchLeafDescriptorsTimeSeparation) { Descriptor::Ptr query = makeDescriptor(1.0f); DescriptorMatchConfig config; @@ -421,7 +422,7 @@ TEST(DsgLcdMatchingTests, SearchLeafDescriptorsTimeSeparation) { EXPECT_TRUE(results.match_nodes.empty()); } -TEST(DsgLcdMatchingTests, SearchLeafDescriptorsMaxRegistrationMatches) { +TEST(LoopClosureModuleMatchingTests, SearchLeafDescriptorsMaxRegistrationMatches) { Descriptor::Ptr query = makeDescriptor(1.0f, 0.0f); fillDescriptor(*query, 0, {13, 14, 15}); diff --git a/hydra_dsg_builder/tests/utest_dsg_lcd_module.cpp b/hydra/tests/loop_closure/test_detector.cpp similarity index 89% rename from hydra_dsg_builder/tests/utest_dsg_lcd_module.cpp rename to hydra/tests/loop_closure/test_detector.cpp index 8b2ddc92..2ef60cc9 100644 --- a/hydra_dsg_builder/tests/utest_dsg_lcd_module.cpp +++ b/hydra/tests/loop_closure/test_detector.cpp @@ -32,19 +32,16 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_dsg_builder/dsg_lcd_detector.h" - #include +#include namespace hydra { namespace lcd { -using incremental::SharedDsgInfo; - -struct DsgLcdDetectorTests : public ::testing::Test { - DsgLcdDetectorTests() = default; +struct LcdDetectorTests : public ::testing::Test { + LcdDetectorTests() = default; - virtual ~DsgLcdDetectorTests() = default; + virtual ~LcdDetectorTests() = default; virtual void SetUp() override { dsg.reset(new DynamicSceneGraph()); @@ -58,12 +55,12 @@ struct DsgLcdDetectorTests : public ::testing::Test { config.search_configs[DsgLayers::PLACES] = {0.8, 0.8, 0.0, 1, 0.0, 0.0}; } - DsgLcdDetectorConfig config; + LcdDetectorConfig config; DynamicSceneGraph::Ptr dsg; }; -TEST_F(DsgLcdDetectorTests, TestEmptyUpdate) { - DsgLcdDetector module(config); +TEST_F(LcdDetectorTests, TestEmptyUpdate) { + LcdDetector module(config); std::unordered_set active_places; module.updateDescriptorCache(*dsg, active_places); EXPECT_EQ(0u, module.numGraphDescriptors(DsgLayers::PLACES)); @@ -71,8 +68,8 @@ TEST_F(DsgLcdDetectorTests, TestEmptyUpdate) { EXPECT_EQ(0u, module.numAgentDescriptors()); } -TEST_F(DsgLcdDetectorTests, TestInvalidNodeUpdate) { - DsgLcdDetector module(config); +TEST_F(LcdDetectorTests, TestInvalidNodeUpdate) { + LcdDetector module(config); std::unordered_set active_places{1, 2, 3, 4, 5}; module.updateDescriptorCache(*dsg, active_places); EXPECT_EQ(0u, module.numGraphDescriptors(DsgLayers::PLACES)); @@ -80,14 +77,14 @@ TEST_F(DsgLcdDetectorTests, TestInvalidNodeUpdate) { EXPECT_EQ(0u, module.numAgentDescriptors()); } -TEST_F(DsgLcdDetectorTests, TestNoChildren) { +TEST_F(LcdDetectorTests, TestNoChildren) { dsg->emplaceNode(DsgLayers::PLACES, 1, std::make_unique()); dsg->emplaceNode(DsgLayers::PLACES, 2, std::make_unique()); dsg->emplaceNode(DsgLayers::PLACES, 3, std::make_unique()); dsg->emplaceNode(DsgLayers::PLACES, 4, std::make_unique()); dsg->emplaceNode(DsgLayers::PLACES, 5, std::make_unique()); - DsgLcdDetector module(config); + LcdDetector module(config); std::unordered_set active_places{1, 2, 3, 4, 5}; module.updateDescriptorCache(*dsg, active_places); EXPECT_EQ(0u, module.numGraphDescriptors(DsgLayers::PLACES)); @@ -95,14 +92,14 @@ TEST_F(DsgLcdDetectorTests, TestNoChildren) { EXPECT_EQ(0u, module.numAgentDescriptors()); } -TEST_F(DsgLcdDetectorTests, TestNoDynamicChildren) { +TEST_F(LcdDetectorTests, TestNoDynamicChildren) { dsg->emplaceNode(DsgLayers::PLACES, 1, std::make_unique()); dsg->emplaceNode(DsgLayers::OBJECTS, 2, std::make_unique()); dsg->emplaceNode(DsgLayers::OBJECTS, 3, std::make_unique()); dsg->insertEdge(1, 2); dsg->insertEdge(1, 3); - DsgLcdDetector module(config); + LcdDetector module(config); std::unordered_set active_places{1, 2, 3, 4, 5}; module.updateDescriptorCache(*dsg, active_places); EXPECT_EQ(0u, module.numGraphDescriptors(DsgLayers::PLACES)); @@ -110,7 +107,7 @@ TEST_F(DsgLcdDetectorTests, TestNoDynamicChildren) { EXPECT_EQ(0u, module.numAgentDescriptors()); } -TEST_F(DsgLcdDetectorTests, TestActualChildren) { +TEST_F(LcdDetectorTests, TestActualChildren) { using namespace std::chrono_literals; dsg->emplaceNode(DsgLayers::PLACES, 1, std::make_unique()); dsg->emplaceNode(DsgLayers::OBJECTS, 2, std::make_unique()); @@ -128,7 +125,7 @@ TEST_F(DsgLcdDetectorTests, TestActualChildren) { dsg->insertEdge(1, NodeSymbol('a', 0)); dsg->insertEdge(1, NodeSymbol('a', 1)); - DsgLcdDetector module(config); + LcdDetector module(config); std::unordered_set active_places{1}; module.updateDescriptorCache(*dsg, active_places); EXPECT_EQ(1u, module.numGraphDescriptors(DsgLayers::PLACES)); @@ -136,7 +133,7 @@ TEST_F(DsgLcdDetectorTests, TestActualChildren) { EXPECT_EQ(2u, module.numAgentDescriptors()); } -TEST_F(DsgLcdDetectorTests, TestEmptySearch) { +TEST_F(LcdDetectorTests, TestEmptySearch) { using namespace std::chrono_literals; dsg->emplaceNode(DsgLayers::AGENTS, 'a', @@ -144,7 +141,7 @@ TEST_F(DsgLcdDetectorTests, TestEmptySearch) { std::make_unique( Eigen::Quaterniond::Identity(), Eigen::Vector3d::Zero(), 0)); - DsgLcdDetector module(config); + LcdDetector module(config); std::unordered_set active_places{1}; std::vector results = @@ -152,7 +149,7 @@ TEST_F(DsgLcdDetectorTests, TestEmptySearch) { EXPECT_EQ(0u, results.size()); } -TEST_F(DsgLcdDetectorTests, TestNonEmptySearch) { +TEST_F(LcdDetectorTests, TestNonEmptySearch) { using namespace std::chrono_literals; dsg->emplaceNode(DsgLayers::PLACES, 1, std::make_unique()); dsg->emplaceNode(DsgLayers::OBJECTS, 2, std::make_unique()); @@ -170,7 +167,7 @@ TEST_F(DsgLcdDetectorTests, TestNonEmptySearch) { dsg->insertEdge(1, NodeSymbol('a', 0)); dsg->insertEdge(1, NodeSymbol('a', 1)); - DsgLcdDetector module(config); + LcdDetector module(config); std::unordered_set active_places{1}; module.updateDescriptorCache(*dsg, active_places); EXPECT_EQ(1u, module.numGraphDescriptors(DsgLayers::PLACES)); diff --git a/hydra_dsg_builder/tests/utest_lcd_gnn_descriptors.cpp b/hydra/tests/loop_closure/test_gnn_descriptors.cpp similarity index 91% rename from hydra_dsg_builder/tests/utest_lcd_gnn_descriptors.cpp rename to hydra/tests/loop_closure/test_gnn_descriptors.cpp index adb4f1c2..24301797 100644 --- a/hydra_dsg_builder/tests/utest_lcd_gnn_descriptors.cpp +++ b/hydra/tests/loop_closure/test_gnn_descriptors.cpp @@ -34,8 +34,9 @@ * -------------------------------------------------------------------------- */ #include #include -#include -#include +#include + +#include "hydra_test/resources.h" namespace hydra { namespace lcd { @@ -73,16 +74,12 @@ inline void emplaceObjectNode(DynamicSceneGraph& graph, ++next_index; } -std::string getResourcePath(const std::string& name) { - std::string package_path = ros::package::getPath("hydra_dsg_builder"); - return package_path + "/tests/resources/" + name; -} - } // namespace TEST(GnnLcdTests, testEmptyDescriptor) { SubgraphConfig config; - PlaceGnnDescriptor factory(getResourcePath("places.onnx"), config); + PlaceGnnDescriptor factory(test::get_resource_path("loop_closure/places.onnx"), + config); DynamicSceneGraph graph; const auto& root_node = makeDefaultAgentNode(graph); @@ -95,7 +92,8 @@ TEST(GnnLcdTests, testPlacesTensors) { config.max_radius_m = 5.0; config.min_radius_m = 2.0; config.min_nodes = 10; - PlaceGnnDescriptor factory(getResourcePath("places.onnx"), config); + PlaceGnnDescriptor factory(test::get_resource_path("loop_closure/places.onnx"), + config); size_t node_idx = 0; DynamicSceneGraph graph; @@ -142,8 +140,10 @@ TEST(GnnLcdTests, testObjectTensors) { Eigen::VectorXf fake_embedding(2); fake_embedding << 1.0, 2.0; - ObjectGnnDescriptor factory( - getResourcePath("objects.onnx"), config, 0.1, {{0, fake_embedding}}); + ObjectGnnDescriptor factory(test::get_resource_path("loop_closure/objects.onnx"), + config, + 0.1, + {{0, fake_embedding}}); size_t node_idx = 0; DynamicSceneGraph graph; @@ -181,7 +181,8 @@ TEST(GnnLcdTests, testObjectTensors) { } TEST(GnnLcdTests, testLoadEmbeddings) { - const auto embeddings = loadLabelEmbeddings(getResourcePath("test_embeddings.yaml")); + const auto embeddings = + loadLabelEmbeddings(test::get_resource_path("loop_closure/test_embeddings.yaml")); ObjectGnnDescriptor::LabelEmbeddings expected_embeddings; expected_embeddings[0] = Eigen::VectorXf(4); @@ -214,7 +215,8 @@ TEST(GnnLcdTests, testPlacesDescriptor) { config.max_radius_m = 5.0; config.min_radius_m = 2.0; config.min_nodes = 10; - PlaceGnnDescriptor factory(getResourcePath("places.onnx"), config); + PlaceGnnDescriptor factory(test::get_resource_path("loop_closure/places.onnx"), + config); DynamicSceneGraph graph; const auto& root_node = makeDefaultAgentNode(graph); @@ -253,8 +255,10 @@ TEST(GnnLcdTests, testObjectDescriptor) { Eigen::VectorXf fake_embedding(2); fake_embedding << 1.0, 1.0; - ObjectGnnDescriptor factory( - getResourcePath("objects.onnx"), config, 0.1, {{0, fake_embedding}}); + ObjectGnnDescriptor factory(test::get_resource_path("loop_closure/objects.onnx"), + config, + 0.1, + {{0, fake_embedding}}); size_t node_idx = 0; DynamicSceneGraph graph; @@ -301,7 +305,8 @@ TEST(GnnLcdTests, testPlacesPosDescriptor) { config.max_radius_m = 5.0; config.min_radius_m = 2.0; config.min_nodes = 10; - PlaceGnnDescriptor factory(getResourcePath("places_pos.onnx"), config, false); + PlaceGnnDescriptor factory( + test::get_resource_path("loop_closure/places_pos.onnx"), config, false); DynamicSceneGraph graph; const auto& root_node = makeDefaultAgentNode(graph); @@ -343,8 +348,11 @@ TEST(GnnLcdTests, testObjectPosDescriptor) { Eigen::VectorXf fake_embedding(2); fake_embedding << 4.0, 5.0; - ObjectGnnDescriptor factory( - getResourcePath("objects_pos.onnx"), config, 1.6, {{0, fake_embedding}}, false); + ObjectGnnDescriptor factory(test::get_resource_path("loop_closure/objects_pos.onnx"), + config, + 1.6, + {{0, fake_embedding}}, + false); DynamicSceneGraph graph; const auto& root_node = makeDefaultAgentNode(graph); diff --git a/hydra_dsg_builder/tests/utest_dsg_lcd_registration.cpp b/hydra/tests/loop_closure/test_registration.cpp similarity index 97% rename from hydra_dsg_builder/tests/utest_dsg_lcd_registration.cpp rename to hydra/tests/loop_closure/test_registration.cpp index 697deae5..3709dfa7 100644 --- a/hydra_dsg_builder/tests/utest_dsg_lcd_registration.cpp +++ b/hydra/tests/loop_closure/test_registration.cpp @@ -32,15 +32,12 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include - #include +#include namespace hydra { namespace lcd { -using incremental::SharedDsgInfo; - struct LayerRegistrationTests : public ::testing::Test { EIGEN_MAKE_ALIGNED_OPERATOR_NEW @@ -85,12 +82,12 @@ struct LayerRegistrationTests : public ::testing::Test { LayerRegistrationConfig reg_config; }; -struct DsgRegistrationTests : public ::testing::Test { +struct GraphRegistrationTests : public ::testing::Test { EIGEN_MAKE_ALIGNED_OPERATOR_NEW - DsgRegistrationTests() : testing::Test() {} + GraphRegistrationTests() : testing::Test() {} - ~DsgRegistrationTests() = default; + ~GraphRegistrationTests() = default; virtual void SetUp() override { std::map layer_map = { @@ -272,7 +269,7 @@ TEST_F(LayerRegistrationTests, TestSemanticRegistration) { } } -TEST_F(DsgRegistrationTests, TestFullObjectRegistration) { +TEST_F(GraphRegistrationTests, TestFullObjectRegistration) { DsgRegistrationInput match; for (int i = 0; i < src_points.cols(); ++i) { match.query_nodes.insert(NodeSymbol('O', i + src_points.cols())); @@ -295,7 +292,7 @@ TEST_F(DsgRegistrationTests, TestFullObjectRegistration) { EXPECT_NEAR(0.0, err, 1.0e-3); } -TEST_F(DsgRegistrationTests, TestFullObjectRegistrationWithSubgraphExtraction) { +TEST_F(GraphRegistrationTests, TestFullObjectRegistrationWithSubgraphExtraction) { DsgRegistrationInput match; for (int i = 0; i < src_points.cols(); ++i) { match.query_nodes.insert(NodeSymbol('O', i + src_points.cols())); @@ -318,7 +315,7 @@ TEST_F(DsgRegistrationTests, TestFullObjectRegistrationWithSubgraphExtraction) { EXPECT_NEAR(0.0, err, 1.0e-3); } -TEST_F(DsgRegistrationTests, DISABLED_TestFullPlaceRegistration) { +TEST_F(GraphRegistrationTests, DISABLED_TestFullPlaceRegistration) { DsgRegistrationInput match; for (int i = 0; i < src_points.cols(); ++i) { match.query_nodes.insert(NodeSymbol('p', i + src_points.cols())); diff --git a/hydra_dsg_builder/tests/utest_dsg_lcd_descriptors.cpp b/hydra/tests/loop_closure/test_scene_graph_descriptors.cpp similarity index 97% rename from hydra_dsg_builder/tests/utest_dsg_lcd_descriptors.cpp rename to hydra/tests/loop_closure/test_scene_graph_descriptors.cpp index 5ec5aa0a..a56604c8 100644 --- a/hydra_dsg_builder/tests/utest_dsg_lcd_descriptors.cpp +++ b/hydra/tests/loop_closure/test_scene_graph_descriptors.cpp @@ -32,9 +32,8 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_dsg_builder/dsg_lcd_detector.h" - #include +#include namespace hydra { namespace lcd { @@ -61,7 +60,7 @@ inline void emplacePlaceNode(DynamicSceneGraph& graph, } // namespace -TEST(DsgLcdDescriptorTests, TestAgentDescriptor) { +TEST(LoopClosureModuleDescriptorTests, TestAgentDescriptor) { DynamicSceneGraph graph; const DynamicSceneGraphNode& node = makeDefaultAgentNode(graph); @@ -96,7 +95,7 @@ TEST(DsgLcdDescriptorTests, TestAgentDescriptor) { } } -TEST(DsgLcdDescriptorTests, TestGetBinCorrect) { +TEST(LoopClosureModuleDescriptorTests, TestGetBinCorrect) { HistogramConfig config(0.0, 1.0, 5); EXPECT_EQ(0u, config.getBin(-1.0)); EXPECT_EQ(4u, config.getBin(2.0)); @@ -109,7 +108,7 @@ TEST(DsgLcdDescriptorTests, TestGetBinCorrect) { EXPECT_EQ(4u, config.getBin(1.0)); } -TEST(DsgLcdDescriptorTests, TestPlaceDescriptor) { +TEST(LoopClosureModuleDescriptorTests, TestPlaceDescriptor) { DynamicSceneGraph graph; const DynamicSceneGraphNode& node = makeDefaultAgentNode(graph); @@ -198,7 +197,7 @@ void emplaceObjectNode(DynamicSceneGraph& graph, ++next_index; } -TEST(DsgLcdDescriptorTests, TestObjectDescriptor) { +TEST(LoopClosureModuleDescriptorTests, TestObjectDescriptor) { DynamicSceneGraph graph; const DynamicSceneGraphNode& node = makeDefaultAgentNode(graph); diff --git a/hydra_dsg_builder/tests/utest_subgraph_extraction.cpp b/hydra/tests/loop_closure/test_subgraph_extraction.cpp similarity index 99% rename from hydra_dsg_builder/tests/utest_subgraph_extraction.cpp rename to hydra/tests/loop_closure/test_subgraph_extraction.cpp index 6a64efd2..18eebc2a 100644 --- a/hydra_dsg_builder/tests/utest_subgraph_extraction.cpp +++ b/hydra/tests/loop_closure/test_subgraph_extraction.cpp @@ -32,9 +32,8 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include - #include +#include namespace hydra { diff --git a/hydra_dsg_builder/tests/utest_main.cpp b/hydra/tests/main.cpp similarity index 100% rename from hydra_dsg_builder/tests/utest_main.cpp rename to hydra/tests/main.cpp diff --git a/hydra_topology/tests/utest_compression_graph_extractor.cpp b/hydra/tests/places/test_compression_graph_extractor.cpp similarity index 98% rename from hydra_topology/tests/utest_compression_graph_extractor.cpp rename to hydra/tests/places/test_compression_graph_extractor.cpp index ca59ab16..3ed88d80 100644 --- a/hydra_topology/tests/utest_compression_graph_extractor.cpp +++ b/hydra/tests/places/test_compression_graph_extractor.cpp @@ -33,10 +33,10 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #include -#include +#include namespace hydra { -namespace topology { +namespace places { using GvdLayer = GraphExtractorInterface::GvdLayer; @@ -67,11 +67,8 @@ class TestGraphExtractor : public CompressionGraphExtractor { using CompressionGraphExtractor::to_archive_; using CompressionGraphExtractor::updated_nodes_; - void setGvdNode(uint64_t x, - uint64_t y, - uint64_t z, - double distance, - uint8_t num_basis) { + void setGvdNode( + uint64_t x, uint64_t y, uint64_t z, double distance, uint8_t num_basis) { const GlobalIndex index(x, y, z); const Eigen::Vector3d position(x, y, z); updateNode(index, position, distance, num_basis); @@ -691,5 +688,5 @@ TEST(CompressionNode, testRemoveEdges) { EXPECT_EQ(removed_siblings, expected_removed); } -} // namespace topology +} // namespace places } // namespace hydra diff --git a/hydra_topology/tests/utest_esdf.cpp b/hydra/tests/places/test_esdf_comparison.cpp similarity index 88% rename from hydra_topology/tests/utest_esdf.cpp rename to hydra/tests/places/test_esdf_comparison.cpp index 36a33161..a276588a 100644 --- a/hydra_topology/tests/utest_esdf.cpp +++ b/hydra/tests/places/test_esdf_comparison.cpp @@ -33,23 +33,23 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #include -#include -#include +#include +#include #include #include -#include "hydra_topology_test/layer_utils.h" -#include "hydra_topology_test/test_fixtures.h" +#include "hydra_test/gvd_layer_utils.h" +#include "hydra_test/place_fixtures.h" namespace hydra { -namespace topology { +namespace places { using namespace voxblox; -using test_helpers::compareLayers; -using test_helpers::EsdfTestFixture; -using test_helpers::esdfVoxelsSame; -using test_helpers::LayerComparisonResult; +using test::compareLayers; +using test::EsdfTestFixture; +using test::esdfVoxelsSame; +using test::LayerComparisonResult; #define COPY_FIELD(dest, src, field) dest.field = src.field; @@ -99,7 +99,7 @@ TEST_F(EsdfTestFixture, TestEsdfSame) { original_integrator.updateFromTsdfLayer(true); LayerComparisonResult result = - compareLayers(*gvd_layer, original_layer, &test_helpers::gvdEsdfVoxelsSame); + compareLayers(*gvd_layer, original_layer, &test::gvdEsdfVoxelsSame); EXPECT_EQ(0u, result.num_missing_lhs); EXPECT_EQ(0u, result.num_missing_rhs); EXPECT_EQ(0u, result.num_lhs_seen_rhs_unseen); @@ -113,15 +113,15 @@ TEST_F(EsdfTestFixture, TestEsdfSame) { world.generateSdfFromWorld(esdf_config.max_distance_m, &simulation_esdf); LayerComparisonResult voxblox_results = - compareLayers(original_layer, simulation_esdf, &test_helpers::esdfVoxelsSame); + compareLayers(original_layer, simulation_esdf, &test::esdfVoxelsSame); VLOG(3) << "Voxblox " << voxblox_results; LayerComparisonResult gvd_results = - compareLayers(*gvd_layer, simulation_esdf, &test_helpers::gvdEsdfVoxelsSame); + compareLayers(*gvd_layer, simulation_esdf, &test::gvdEsdfVoxelsSame); VLOG(3) << "GVD " << gvd_results; EXPECT_LT(gvd_results.rmse, voxblox_results.rmse); } -} // namespace topology +} // namespace places } // namespace hydra diff --git a/hydra_topology/tests/utest_floodfill_graph_extractor.cpp b/hydra/tests/places/test_floodfill_graph_extractor.cpp similarity index 97% rename from hydra_topology/tests/utest_floodfill_graph_extractor.cpp rename to hydra/tests/places/test_floodfill_graph_extractor.cpp index b4dd9278..8ace3896 100644 --- a/hydra_topology/tests/utest_floodfill_graph_extractor.cpp +++ b/hydra/tests/places/test_floodfill_graph_extractor.cpp @@ -33,11 +33,10 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #include - -#include +#include namespace hydra { -namespace topology { +namespace places { using NodeSet = std::unordered_set; @@ -107,11 +106,8 @@ class FloodfillGraphExtractorTestFixture : public ::testing::Test { return to_return; } - void makeLine(FloodfillGraphExtractor& extractor, - size_t r1, - size_t c1, - size_t r2, - size_t c2) { + void makeLine( + FloodfillGraphExtractor& extractor, size_t r1, size_t c1, size_t r2, size_t c2) { GlobalIndex start(r1, c1, 0); GlobalIndex end(r2, c2, 0); voxblox::AlignedVector points = makeBresenhamLine(start, end); @@ -291,5 +287,5 @@ TEST_F(FloodfillGraphExtractorTestFixture, SimpleExtractionWithNodeMerging) { EXPECT_EQ(3u, graph.edges().size()); } -} // namespace topology +} // namespace places } // namespace hydra diff --git a/hydra_topology/tests/utest_graph_extractor_utilities.cpp b/hydra/tests/places/test_graph_extractor_utilities.cpp similarity index 98% rename from hydra_topology/tests/utest_graph_extractor_utilities.cpp rename to hydra/tests/places/test_graph_extractor_utilities.cpp index 4416d158..67ab0bbe 100644 --- a/hydra_topology/tests/utest_graph_extractor_utilities.cpp +++ b/hydra/tests/places/test_graph_extractor_utilities.cpp @@ -32,17 +32,16 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_topology_test/test_fixtures.h" - #include +#include +#include -#include -#include +#include "hydra_test/place_fixtures.h" namespace hydra { -namespace topology { +namespace places { -using test_helpers::SingleBlockExtractionTestFixture; +using test::SingleBlockExtractionTestFixture; using voxblox::Connectivity; using voxblox::IndexElement; @@ -387,5 +386,5 @@ TEST(GraphExtractionUtilities, TestBresenhamLineEmpty) { } } -} // namespace topology +} // namespace places } // namespace hydra diff --git a/hydra_topology/tests/utest_incremental_integration.cpp b/hydra/tests/places/test_gvd_incremental.cpp similarity index 91% rename from hydra_topology/tests/utest_incremental_integration.cpp rename to hydra/tests/places/test_gvd_incremental.cpp index d078309e..1a88fe82 100644 --- a/hydra_topology/tests/utest_incremental_integration.cpp +++ b/hydra/tests/places/test_gvd_incremental.cpp @@ -33,22 +33,22 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #include -#include -#include -#include +#include +#include #include #include -#include "hydra_topology_test/layer_utils.h" -#include "hydra_topology_test/test_fixtures.h" +#include "hydra_test/gvd_layer_utils.h" +#include "hydra_test/place_fixtures.h" +#include "hydra_test/resources.h" namespace hydra { -namespace topology { +namespace places { using namespace voxblox; -using test_helpers::compareLayers; -using test_helpers::LayerComparisonResult; +using test::compareLayers; +using test::LayerComparisonResult; class IncrementalIntegrationTestFixture : public ::testing::Test { public: @@ -75,8 +75,7 @@ class IncrementalIntegrationTestFixture : public ::testing::Test { } void integrateTsdf(size_t index) { - std::string package_path = ros::package::getPath("hydra_topology"); - std::string resource_path = package_path + "/tests/resources"; + std::string resource_path = hydra::test::get_resource_path(); std::string num = std::to_string(index); if (num.size() < 3) { num.insert(num.begin(), 3 - num.size(), '0'); @@ -150,7 +149,7 @@ TEST_F(IncrementalIntegrationTestFixture, DISABLED_TestBatchSame) { auto batch_layer = getBatchGvd(); LayerComparisonResult result = - compareLayers(*gvd_layer, *batch_layer, &test_helpers::gvdVoxelsSame); + compareLayers(*gvd_layer, *batch_layer, &test::gvdVoxelsSame); EXPECT_EQ(0u, result.num_missing_lhs); EXPECT_EQ(0u, result.num_missing_rhs); EXPECT_EQ(0u, result.num_lhs_seen_rhs_unseen); @@ -162,5 +161,5 @@ TEST_F(IncrementalIntegrationTestFixture, DISABLED_TestBatchSame) { } } -} // namespace topology +} // namespace places } // namespace hydra diff --git a/hydra_topology/tests/utest_incremental_gvd.cpp b/hydra/tests/places/test_gvd_integrator.cpp similarity index 98% rename from hydra_topology/tests/utest_incremental_gvd.cpp rename to hydra/tests/places/test_gvd_integrator.cpp index 23288924..bee1378e 100644 --- a/hydra_topology/tests/utest_incremental_gvd.cpp +++ b/hydra/tests/places/test_gvd_integrator.cpp @@ -33,17 +33,17 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #include -#include -#include +#include +#include -#include "hydra_topology_test/test_fixtures.h" +#include "hydra_test/place_fixtures.h" namespace hydra { -namespace topology { +namespace places { -using test_helpers::LargeSingleBlockTestFixture; -using test_helpers::SingleBlockTestFixture; -using test_helpers::TestFixture2d; +using test::LargeSingleBlockTestFixture; +using test::SingleBlockTestFixture; +using test::TestFixture2d; using voxblox::VoxelIndex; class ParentTestFixture : public SingleBlockTestFixture { @@ -517,5 +517,5 @@ TEST_F(SingleBlockTestFixture, RaiseCorrectForSurface) { } } -} // namespace topology +} // namespace places } // namespace hydra diff --git a/hydra_topology/tests/utest_gvd_thinning.cpp b/hydra/tests/places/test_gvd_thinning.cpp similarity index 97% rename from hydra_topology/tests/utest_gvd_thinning.cpp rename to hydra/tests/places/test_gvd_thinning.cpp index 0c041f65..c0cfd2b5 100644 --- a/hydra_topology/tests/utest_gvd_thinning.cpp +++ b/hydra/tests/places/test_gvd_thinning.cpp @@ -33,10 +33,10 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #include -#include +#include namespace hydra { -namespace topology { +namespace places { struct InputOutputPair { std::bitset<26> neighborhood; @@ -106,5 +106,5 @@ INSTANTIATE_TEST_CASE_P(GvdThinning, GvdThinningFixture, testing::ValuesIn(THINNING_TEST_CASES)); -} // namespace topology +} // namespace places } // namespace hydra diff --git a/hydra_topology/tests/utest_gvd_utilities.cpp b/hydra/tests/places/test_gvd_utilities.cpp similarity index 99% rename from hydra_topology/tests/utest_gvd_utilities.cpp rename to hydra/tests/places/test_gvd_utilities.cpp index 7ec417a5..58ff2fdc 100644 --- a/hydra_topology/tests/utest_gvd_utilities.cpp +++ b/hydra/tests/places/test_gvd_utilities.cpp @@ -33,10 +33,10 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #include -#include +#include namespace hydra { -namespace topology { +namespace places { using ParentMap = Eigen::Map; using ParentPosMap = Eigen::Map; @@ -381,5 +381,5 @@ TEST(GvdUtilities, checkVoronoiParentSeparation) { } } -} // namespace topology +} // namespace places } // namespace hydra diff --git a/hydra_topology/tests/utest_marching_cubes.cpp b/hydra/tests/reconstruction/test_marching_cubes.cpp similarity index 97% rename from hydra_topology/tests/utest_marching_cubes.cpp rename to hydra/tests/reconstruction/test_marching_cubes.cpp index 8b9326d9..dd6c76a1 100644 --- a/hydra_topology/tests/utest_marching_cubes.cpp +++ b/hydra/tests/reconstruction/test_marching_cubes.cpp @@ -33,12 +33,14 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #include -#include +#include +#include #include namespace hydra { -namespace topology { + +using voxblox::BlockIndex; static constexpr float TEST_TOLERANCE = 1.0e-6f; @@ -124,5 +126,4 @@ TEST(VoxelAwareMarchingCubes, CubeMeshingNearestVertexIndexCorrect) { EXPECT_EQ(2u, actual_voxels[1].block_vertex_index); } -} // namespace topology } // namespace hydra diff --git a/hydra_topology/tests/utest_pose_graph.cpp b/hydra/tests/reconstruction/test_reconstruction_module.cpp similarity index 97% rename from hydra_topology/tests/utest_pose_graph.cpp rename to hydra/tests/reconstruction/test_reconstruction_module.cpp index 1f89b771..73d03414 100644 --- a/hydra_topology/tests/utest_pose_graph.cpp +++ b/hydra/tests/reconstruction/test_reconstruction_module.cpp @@ -32,10 +32,8 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include #include - -#include +#include TEST(PoseGraph, TestAffineConstruction) { Eigen::Quaterniond q(0.0, 1.0, 0.0, 0.0); @@ -48,4 +46,3 @@ TEST(PoseGraph, TestAffineConstruction) { Eigen::Affine3d T(Eigen::Translation3d(t) * q); EXPECT_TRUE(expected.isApprox(T.matrix(), 1.0e-6)); } - diff --git a/hydra_utils/tests/resources/map_config.yaml b/hydra/tests/resources/config/map_config.yaml similarity index 100% rename from hydra_utils/tests/resources/map_config.yaml rename to hydra/tests/resources/config/map_config.yaml diff --git a/hydra_utils/tests/resources/missing_config.yaml b/hydra/tests/resources/config/missing_config.yaml similarity index 76% rename from hydra_utils/tests/resources/missing_config.yaml rename to hydra/tests/resources/config/missing_config.yaml index d261200a..7c6a3f39 100644 --- a/hydra_utils/tests/resources/missing_config.yaml +++ b/hydra/tests/resources/config/missing_config.yaml @@ -1,3 +1,2 @@ ---- a: -3 b: 5.2 diff --git a/hydra_utils/tests/resources/nested_test_config.yaml b/hydra/tests/resources/config/nested_test_config.yaml similarity index 100% rename from hydra_utils/tests/resources/nested_test_config.yaml rename to hydra/tests/resources/config/nested_test_config.yaml diff --git a/hydra_utils/tests/resources/test_config.yaml b/hydra/tests/resources/config/test_config.yaml similarity index 100% rename from hydra_utils/tests/resources/test_config.yaml rename to hydra/tests/resources/config/test_config.yaml diff --git a/hydra_dsg_builder/tests/resources/make_lcd_models.py b/hydra/tests/resources/loop_closure/make_lcd_models.py similarity index 60% rename from hydra_dsg_builder/tests/resources/make_lcd_models.py rename to hydra/tests/resources/loop_closure/make_lcd_models.py index 1a5ba28c..8914447d 100644 --- a/hydra_dsg_builder/tests/resources/make_lcd_models.py +++ b/hydra/tests/resources/loop_closure/make_lcd_models.py @@ -1,4 +1,36 @@ #!/usr/bin/env python3 +# Copyright 2022, Massachusetts Institute of Technology. +# All Rights Reserved +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +# WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +# +# Research was sponsored by the United States Air Force Research Laboratory and +# the United States Air Force Artificial Intelligence Accelerator and was +# accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views +# and conclusions contained in this document are those of the authors and should +# not be interpreted as representing the official policies, either expressed or +# implied, of the United States Air Force or the U.S. Government. The U.S. +# Government is authorized to reproduce and distribute reprints for Government +# purposes notwithstanding any copyright notation herein. """Make a simple model.""" import torch import torch.onnx diff --git a/hydra_dsg_builder/tests/resources/objects.onnx b/hydra/tests/resources/loop_closure/objects.onnx similarity index 100% rename from hydra_dsg_builder/tests/resources/objects.onnx rename to hydra/tests/resources/loop_closure/objects.onnx diff --git a/hydra_dsg_builder/tests/resources/objects_pos.onnx b/hydra/tests/resources/loop_closure/objects_pos.onnx similarity index 100% rename from hydra_dsg_builder/tests/resources/objects_pos.onnx rename to hydra/tests/resources/loop_closure/objects_pos.onnx diff --git a/hydra_dsg_builder/tests/resources/places.onnx b/hydra/tests/resources/loop_closure/places.onnx similarity index 100% rename from hydra_dsg_builder/tests/resources/places.onnx rename to hydra/tests/resources/loop_closure/places.onnx diff --git a/hydra_dsg_builder/tests/resources/places_pos.onnx b/hydra/tests/resources/loop_closure/places_pos.onnx similarity index 100% rename from hydra_dsg_builder/tests/resources/places_pos.onnx rename to hydra/tests/resources/loop_closure/places_pos.onnx diff --git a/hydra_dsg_builder/tests/resources/test_embeddings.yaml b/hydra/tests/resources/loop_closure/test_embeddings.yaml similarity index 100% rename from hydra_dsg_builder/tests/resources/test_embeddings.yaml rename to hydra/tests/resources/loop_closure/test_embeddings.yaml diff --git a/hydra/tests/resources/test_semantic_map.csv b/hydra/tests/resources/test_semantic_map.csv new file mode 100644 index 00000000..a226dfbc --- /dev/null +++ b/hydra/tests/resources/test_semantic_map.csv @@ -0,0 +1,22 @@ +name,red,green,blue,alpha,id +Unknown,53,173,156,255,0 +Wall,0,102,51,255,1 +Floor,191,191,181,255,2 +Ceiling,59,163,236,255,3 +Door,246,112,136,255,4 +Stairs,244,93,235,255,5 +Structure,54,171,171,255,6 +Shelf,222,135,49,255,7 +Plant,197,147,49,255,8 +Bed,52,175,137,255,9 +Storage,156,162,49,255,10 +Table,204,0,102,255,11 +Chair,177,155,49,255,12 +Wall_Decoration,246,107,171,255,13 +Couch,204,0,0,255,14 +Light,128,150,244,255,15 +Appliance,173,136,244,255,16 +Thing,50,177,107,255,17 +Deformable,247,117,83,255,18 +Dynamic_NonHuman,56,167,205,255,19 +Human,55,170,186,255,20 diff --git a/hydra_dsg_builder/tests/utest_graph_clustering.cpp b/hydra/tests/rooms/test_graph_clustering.cpp similarity index 99% rename from hydra_dsg_builder/tests/utest_graph_clustering.cpp rename to hydra/tests/rooms/test_graph_clustering.cpp index 665579e3..e46edb06 100644 --- a/hydra_dsg_builder/tests/utest_graph_clustering.cpp +++ b/hydra/tests/rooms/test_graph_clustering.cpp @@ -32,9 +32,8 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include - #include +#include namespace hydra { diff --git a/hydra_dsg_builder/tests/utest_graph_filtration.cpp b/hydra/tests/rooms/test_graph_filtration.cpp similarity index 99% rename from hydra_dsg_builder/tests/utest_graph_filtration.cpp rename to hydra/tests/rooms/test_graph_filtration.cpp index 22be9d33..cfb7e81b 100644 --- a/hydra_dsg_builder/tests/utest_graph_filtration.cpp +++ b/hydra/tests/rooms/test_graph_filtration.cpp @@ -33,7 +33,7 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #include -#include +#include namespace hydra { diff --git a/hydra_dsg_builder/tests/utest_room_finder.cpp b/hydra/tests/rooms/test_room_finder.cpp similarity index 99% rename from hydra_dsg_builder/tests/utest_room_finder.cpp rename to hydra/tests/rooms/test_room_finder.cpp index bf7a503b..a0f62c9c 100644 --- a/hydra_dsg_builder/tests/utest_room_finder.cpp +++ b/hydra/tests/rooms/test_room_finder.cpp @@ -32,9 +32,8 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include - #include +#include namespace hydra { diff --git a/hydra_dsg_builder/tests/utest_configs.cpp b/hydra/tests/rooms/test_room_finder_config.cpp similarity index 98% rename from hydra_dsg_builder/tests/utest_configs.cpp rename to hydra/tests/rooms/test_room_finder_config.cpp index 88b2fd1b..298a8794 100644 --- a/hydra_dsg_builder/tests/utest_configs.cpp +++ b/hydra/tests/rooms/test_room_finder_config.cpp @@ -32,11 +32,10 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include - +#include #include +#include #include -#include namespace hydra { diff --git a/hydra_dsg_builder/tests/utest_room_helpers.cpp b/hydra/tests/rooms/test_room_utilities.cpp similarity index 99% rename from hydra_dsg_builder/tests/utest_room_helpers.cpp rename to hydra/tests/rooms/test_room_utilities.cpp index 4410353c..a57f846a 100644 --- a/hydra_dsg_builder/tests/utest_room_helpers.cpp +++ b/hydra/tests/rooms/test_room_utilities.cpp @@ -32,9 +32,8 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include - #include +#include namespace hydra { diff --git a/hydra_topology/tests/src/test_fixtures.cpp b/hydra/tests/src/place_fixtures.cpp similarity index 95% rename from hydra_topology/tests/src/test_fixtures.cpp rename to hydra/tests/src/place_fixtures.cpp index e64cb76b..003b1979 100644 --- a/hydra_topology/tests/src/test_fixtures.cpp +++ b/hydra/tests/src/place_fixtures.cpp @@ -32,9 +32,9 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_topology_test/test_fixtures.h" +#include "hydra_test/place_fixtures.h" -#include +#include #include #include #include @@ -43,8 +43,8 @@ using namespace voxblox; namespace hydra { -namespace topology { -namespace test_helpers { +namespace places { +namespace test { void EsdfTestFixture::SetUp() { setupWorld(); } @@ -121,11 +121,8 @@ void GvdTestFixture::setupWorld() { world.addGroundLevel(0.0); } -void SingleBlockTestFixture::setTsdfVoxel(int x, - int y, - int z, - float distance, - float weight) { +void SingleBlockTestFixture::setTsdfVoxel( + int x, int y, int z, float distance, float weight) { CHECK_LT(x, voxels_per_side); CHECK_LT(y, voxels_per_side); CHECK_LT(z, voxels_per_side); @@ -272,6 +269,6 @@ void TestFixture2d::SetUp() { } } -} // namespace test_helpers -} // namespace topology +} // namespace test +} // namespace places } // namespace hydra diff --git a/hydra/tests/src/resources.cpp b/hydra/tests/src/resources.cpp new file mode 100644 index 00000000..0eeb4a24 --- /dev/null +++ b/hydra/tests/src/resources.cpp @@ -0,0 +1,55 @@ +/* ----------------------------------------------------------------------------- + * Copyright 2022 Massachusetts Institute of Technology. + * All Rights Reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Research was sponsored by the United States Air Force Research Laboratory and + * the United States Air Force Artificial Intelligence Accelerator and was + * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views + * and conclusions contained in this document are those of the authors and should + * not be interpreted as representing the official policies, either expressed or + * implied, of the United States Air Force or the U.S. Government. The U.S. + * Government is authorized to reproduce and distribute reprints for Government + * purposes notwithstanding any copyright notation herein. + * -------------------------------------------------------------------------- */ +#include "hydra_test/resources.h" + +#include + +namespace hydra { +namespace test { + +std::string get_resource_path() { + return ros::package::getPath("hydra") + "/tests/resources/"; +} + +std::string get_resource_path(const std::string& name) { + return get_resource_path() + name; +} + +std::string get_default_semantic_map() { + return get_resource_path("test_semantic_map.csv"); +} + +} // namespace test +} // namespace hydra diff --git a/hydra_dsg_builder/tests/utest_minimum_spanning_tree.cpp b/hydra/tests/utils/test_minimum_spanning_tree.cpp similarity index 98% rename from hydra_dsg_builder/tests/utest_minimum_spanning_tree.cpp rename to hydra/tests/utils/test_minimum_spanning_tree.cpp index 8a1f3db0..c0a40001 100644 --- a/hydra_dsg_builder/tests/utest_minimum_spanning_tree.cpp +++ b/hydra/tests/utils/test_minimum_spanning_tree.cpp @@ -32,9 +32,8 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include - #include +#include namespace hydra { diff --git a/hydra_topology/tests/utest_nearest_neighbor_utilities.cpp b/hydra/tests/utils/test_nearest_neighbor_utilities.cpp similarity index 97% rename from hydra_topology/tests/utest_nearest_neighbor_utilities.cpp rename to hydra/tests/utils/test_nearest_neighbor_utilities.cpp index a4c997aa..fb9f0196 100644 --- a/hydra_topology/tests/utest_nearest_neighbor_utilities.cpp +++ b/hydra/tests/utils/test_nearest_neighbor_utilities.cpp @@ -32,12 +32,10 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include - #include +#include namespace hydra { -namespace topology { TEST(NearestNeighborUtilities, TestSkipFirst) { IsolatedSceneGraphLayer layer(1); @@ -74,5 +72,4 @@ TEST(NearestNeighborUtilities, TestSkipFirst) { } } -} // namespace topology } // namespace hydra diff --git a/hydra_utils/tests/utest_timing_utilities.cpp b/hydra/tests/utils/test_timing_utilities.cpp similarity index 99% rename from hydra_utils/tests/utest_timing_utilities.cpp rename to hydra/tests/utils/test_timing_utilities.cpp index 73ea2e19..75a8324d 100644 --- a/hydra_utils/tests/utest_timing_utilities.cpp +++ b/hydra/tests/utils/test_timing_utilities.cpp @@ -32,9 +32,8 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_utils/timing_utilities.h" - #include +#include #include diff --git a/hydra_dsg_builder/CMakeLists.txt b/hydra_dsg_builder/CMakeLists.txt deleted file mode 100644 index a8844457..00000000 --- a/hydra_dsg_builder/CMakeLists.txt +++ /dev/null @@ -1,134 +0,0 @@ -cmake_minimum_required(VERSION 3.1) -project(hydra_dsg_builder) - -add_compile_options(-Wall -Wextra) -set(CMAKE_CXX_STANDARD 17) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - -option(HYDRA_USE_COVERAGE "Build core library with GCC --coverage option" OFF) -option(HYDRA_GNN "build GNN interface" ON) -if(HYDRA_GNN) - set(HYDRA_USE_GNN_CXX_VALUE 1) -else() - set(HYDRA_USE_GNN_CXX_VALUE 0) -endif() -configure_file(hydra_build_config.h.in ${CMAKE_CURRENT_BINARY_DIR}/hydra_build_config.h) - -find_package(Boost REQUIRED COMPONENTS timer) -find_package(spark_dsg REQUIRED) -find_package( - catkin REQUIRED - COMPONENTS hydra_topology - kimera_pgmo - kimera_semantics - std_msgs - tf2_eigen - voxblox -) - -find_package(teaserpp REQUIRED) -find_package(GTSAM REQUIRED) -find_package(Eigen3 REQUIRED) -find_package(KimeraRPGO REQUIRED) -find_package(yaml-cpp REQUIRED) - -catkin_package( - CATKIN_DEPENDS - hydra_topology - kimera_pgmo - kimera_semantics - std_msgs - tf2_eigen - voxblox - DEPENDS spark_dsg - INCLUDE_DIRS include - LIBRARIES ${PROJECT_NAME} gtsam -) - -if(HYDRA_GNN) - add_subdirectory(src/gnn) -endif() - -add_library( - ${PROJECT_NAME} - src/disjoint_set.cpp - src/dsg_lcd_descriptors.cpp - src/dsg_lcd_matching.cpp - src/dsg_lcd_detector.cpp - src/dsg_lcd_registration.cpp - src/dsg_update_functions.cpp - src/hydra_config.cpp - src/graph_clustering.cpp - src/graph_filtration.cpp - src/incremental_dsg_backend.cpp - src/incremental_dsg_frontend.cpp - src/incremental_dsg_lcd.cpp - src/incremental_mesh_segmenter.cpp - src/merge_handler.cpp - src/minimum_spanning_tree.cpp - src/room_finder.cpp - src/room_helpers.cpp - src/shared_module_state.cpp - src/subgraph_extraction.cpp -) -target_include_directories( - ${PROJECT_NAME} - PUBLIC include ${catkin_INCLUDE_DIRS} - PRIVATE ${CMAKE_CURRENT_BINARY_DIR} -) -target_link_libraries( - ${PROJECT_NAME} - PUBLIC ${catkin_LIBRARIES} - teaserpp::teaser_registration - spark_dsg::spark_dsg - yaml-cpp - gtsam -) -if(HYDRA_USE_COVERAGE) - target_compile_options(${PROJECT_NAME} PRIVATE --coverage) - # TODO(nathan) once we have newer cmake, target link options - target_link_libraries(${PROJECT_NAME} PRIVATE --coverage) -endif() - -if(HYDRA_GNN) - target_sources( - ${PROJECT_NAME} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/src/lcd_gnn_descriptors.cpp - ) - target_link_libraries(${PROJECT_NAME} PUBLIC ${PROJECT_NAME}_gnn) -endif() - -if(CATKIN_ENABLE_TESTING) - find_package(yaml-cpp) - catkin_add_gtest( - utest_${PROJECT_NAME} - tests/utest_main.cpp - tests/utest_configs.cpp - tests/utest_dsg_lcd_registration.cpp - tests/utest_dsg_lcd_descriptors.cpp - tests/utest_dsg_lcd_matching.cpp - tests/utest_dsg_lcd_module.cpp - tests/utest_dsg_update_functions.cpp - tests/utest_frontend.cpp - tests/utest_graph_clustering.cpp - tests/utest_graph_filtration.cpp - tests/utest_merge_handler.cpp - tests/utest_minimum_spanning_tree.cpp - tests/utest_room_helpers.cpp - tests/utest_room_finder.cpp - tests/utest_subgraph_extraction.cpp - ) - target_link_libraries(utest_${PROJECT_NAME} ${PROJECT_NAME} yaml-cpp) - - if(HYDRA_GNN) - target_sources( - utest_${PROJECT_NAME} - PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/tests/utest_lcd_gnn_descriptors.cpp - ) - target_link_libraries(utest_${PROJECT_NAME} ${PROJECT_NAME}_gnn) - endif() - - target_link_libraries(utest_${PROJECT_NAME} ${PROJECT_NAME}) -endif() - -# TODO(nathan) handle install diff --git a/hydra_dsg_builder/config/category_labels/uh2_office_label_conversion.yaml b/hydra_dsg_builder/config/category_labels/uh2_office_label_conversion.yaml deleted file mode 100644 index bd88a127..00000000 --- a/hydra_dsg_builder/config/category_labels/uh2_office_label_conversion.yaml +++ /dev/null @@ -1,56 +0,0 @@ ---- -object_labels: - 0: Unknown - 1: Thing - 2: Furniture - 3: Floor - 4: Ceiling - 5: Seating - 6: Seating - 7: Seating - 8: Unknown - 9: Storage - 10: Thing - 11: Light - 12: Wall_Decoration - 13: Plant - 14: Wall_Decoration - 16: Furniture - 17: Thing - 18: Thing - 19: Door - 20: Unknown -room_labels: - - bathroom - - bedroom - - closet - - dining_room - - lobby - - family_room - - garage - - hallway - - library - - laundry_room - - kitchen - - living_room - - conference_auditorium - - lounge - - office - - porch - - game_room - - staircase - - bathroom - - utility_room - - television_room - - gym - - yard - - balcony - - bar - - classroom - - dining_room - - spa - - None -building_labels: - - house - - office_building - - resort diff --git a/hydra_dsg_builder/scripts/make_colormap.py b/hydra_dsg_builder/scripts/make_colormap.py deleted file mode 100644 index b141c4c5..00000000 --- a/hydra_dsg_builder/scripts/make_colormap.py +++ /dev/null @@ -1,28 +0,0 @@ -"""Make a colormap.""" -import click -import seaborn as sns -import matplotlib.pyplot as plt - - -@click.command() -@click.option("-n", "--name", default=None) -@click.option("-s", "--show", is_flag=True) -def main(name, show): - """Construct a qualitative colormap.""" - sns.set() - palette = sns.color_palette() if name is None else sns.color_palette(name) - print(name) - - if show: - sns.palplot(palette) - plt.show() - - for color in palette: - r = int(255 * color[0]) - g = int(255 * color[1]) - b = int(255 * color[2]) - print(f"{{{r}, {g}, {b}}},") - - -if __name__ == "__main__": - main() diff --git a/hydra_dsg_builder/scripts/setup_logs.sh b/hydra_dsg_builder/scripts/setup_logs.sh deleted file mode 100755 index 2ef57998..00000000 --- a/hydra_dsg_builder/scripts/setup_logs.sh +++ /dev/null @@ -1,7 +0,0 @@ -#!/bin/bash -mkdir -p $1 -mkdir -p "$1/frontend" -mkdir -p "$1/backend" -mkdir -p "$1/lcd" -mkdir -p "$1/pgmo" -mkdir -p "$1/logs" diff --git a/hydra_dsg_builder/src/gnn/tests/resources/make_simple_model.py b/hydra_dsg_builder/src/gnn/tests/resources/make_simple_model.py deleted file mode 100644 index a4c63434..00000000 --- a/hydra_dsg_builder/src/gnn/tests/resources/make_simple_model.py +++ /dev/null @@ -1,52 +0,0 @@ -#!/usr/bin/env python3 -"""Make a simple model.""" -import torch -import torch.onnx -import torch.nn as nn -import torch_geometric.nn as pyg - - -class SimpleNetwork(nn.Module): - """Simple network to make testable onnx export.""" - - def __init__(self): - """Make the network.""" - super(SimpleNetwork, self).__init__() - self.mp = pyg.GraphConv(2, 2) - state_dict = { - "lin_rel.weight": torch.eye(2, requires_grad=True, dtype=torch.float32), - "lin_rel.bias": torch.zeros(2, requires_grad=True, dtype=torch.float32), - "lin_root.weight": torch.eye(2, requires_grad=True, dtype=torch.float32), - } - self.mp.load_state_dict(state_dict) - - def forward(self, x, edge_index): - """Do a forward pass.""" - return self.mp(x, edge_index) - - -def main(): - """Export model to onnx.""" - - model = SimpleNetwork() - - x = torch.ones((5, 2)) - edge_index = torch.tensor([[0, 1, 2, 3], [1, 2, 3, 4]], dtype=torch.long) - - y = model(x, edge_index) - print(y) - - torch.onnx.export( - model, - (x, edge_index), - "model.onnx", - export_params=True, - do_constant_folding=True, - input_names=["x", "edge_index"], - output_names=["output"], - dynamic_axes={"x": {0: "num_nodes"}, "edge_index": {1: "num_edges"}}, - ) - - -if __name__ == "__main__": - main() diff --git a/hydra_dsg_builder_ros/CMakeLists.txt b/hydra_dsg_builder_ros/CMakeLists.txt deleted file mode 100644 index 859d12dd..00000000 --- a/hydra_dsg_builder_ros/CMakeLists.txt +++ /dev/null @@ -1,61 +0,0 @@ -cmake_minimum_required(VERSION 3.1) -project(hydra_dsg_builder_ros) - -add_compile_options(-Wall -Wextra) -set(CMAKE_CXX_STANDARD 17) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - -find_package(Boost REQUIRED COMPONENTS timer) -find_package(spark_dsg REQUIRED) -find_package(GTSAM REQUIRED) -find_package( - catkin REQUIRED - COMPONENTS hydra_topology - hydra_dsg_builder - kimera_semantics_ros - roscpp - std_msgs - tf2_eigen - voxblox_ros -) - -catkin_package( - CATKIN_DEPENDS - hydra_topology - hydra_dsg_builder - kimera_semantics_ros - roscpp - std_msgs - tf2_eigen - voxblox_ros - DEPENDS spark_dsg - INCLUDE_DIRS include - LIBRARIES ${PROJECT_NAME} -) - -add_library( - ${PROJECT_NAME} - src/hydra_ros_pipeline.cpp - src/lcd_visualizer.cpp - src/node_utilities.cpp - src/ros_backend.cpp - src/ros_frontend.cpp - src/ros_lcd_registration.cpp - src/visualizer_plugins.cpp -) -target_include_directories(${PROJECT_NAME} PUBLIC include ${catkin_INCLUDE_DIRS}) -target_link_libraries( - ${PROJECT_NAME} - ${catkin_LIBRARIES} - spark_dsg::spark_dsg - gtsam -) - -add_executable(${PROJECT_NAME}_node src/incremental_dsg_builder_node.cpp) -target_link_libraries(${PROJECT_NAME}_node PUBLIC ${PROJECT_NAME}) - -add_executable(dsg_optimizer_node src/dsg_optimizer_node.cpp) -target_link_libraries(dsg_optimizer_node ${PROJECT_NAME}) - -# TODO(nathan) handle install diff --git a/hydra_dsg_builder_ros/include/hydra_dsg_builder_ros/lcd_visualizer.h b/hydra_dsg_builder_ros/include/hydra_dsg_builder_ros/lcd_visualizer.h deleted file mode 100644 index a6eefe23..00000000 --- a/hydra_dsg_builder_ros/include/hydra_dsg_builder_ros/lcd_visualizer.h +++ /dev/null @@ -1,96 +0,0 @@ -/* ----------------------------------------------------------------------------- - * Copyright 2022 Massachusetts Institute of Technology. - * All Rights Reserved - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Research was sponsored by the United States Air Force Research Laboratory and - * the United States Air Force Artificial Intelligence Accelerator and was - * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views - * and conclusions contained in this document are those of the authors and should - * not be interpreted as representing the official policies, either expressed or - * implied, of the United States Air Force or the U.S. Government. The U.S. - * Government is authorized to reproduce and distribute reprints for Government - * purposes notwithstanding any copyright notation herein. - * -------------------------------------------------------------------------- */ -#pragma once -#include "hydra_dsg_builder/dsg_lcd_detector.h" - -#include -#include -#include - -namespace hydra { -namespace lcd { - -using visualization_msgs::Marker; -using visualization_msgs::MarkerArray; - -class LcdVisualizer : public DynamicSceneGraphVisualizer { - public: - using DynamicLayerConfigManager = ConfigManager; - - static DynamicSceneGraph::LayerIds getDefaultLayerIds() { - return {DsgLayers::OBJECTS, DsgLayers::PLACES}; - } - - LcdVisualizer(const ros::NodeHandle& nh, double radius); - - void setLcdDetector(DsgLcdDetector* detector); - - protected: - virtual void redrawImpl(const std_msgs::Header& header, MarkerArray& msg) override; - - virtual void drawLayer(const std_msgs::Header& header, - const SceneGraphLayer& layer, - const LayerConfig& config, - MarkerArray& msg) override; - - virtual void drawLayerMeshEdges(const std_msgs::Header& header, - LayerId layer_id, - const std::string& ns, - MarkerArray& msg) override; - - void drawAgent(const std_msgs::Header& header, MarkerArray& msg); - - std::optional getQueryNode() const; - - std::vector getMatchRoots(LayerId layer) const; - - std::set getMatchedNodes(LayerId layer) const; - - std::set getValidNodes(LayerId layer) const; - - private: - DsgLcdDetector* lcd_detector_; - - double radius_; - - DynamicLayerConfigManager::Ptr agent_config_; - NodeColor invalid_match_color_; - NodeColor valid_match_color_; - NodeColor default_graph_color_; - NodeColor query_color_; -}; - -} // namespace lcd -} // namespace hydra diff --git a/hydra_dsg_builder_ros/package.xml b/hydra_dsg_builder_ros/package.xml deleted file mode 100644 index 66b6d981..00000000 --- a/hydra_dsg_builder_ros/package.xml +++ /dev/null @@ -1,26 +0,0 @@ - - - hydra_dsg_builder_ros - 1.0.0 - hydra ros nodes - - Nathan Hughes - Yun Chang - Nathan Hughes - - BSD - - catkin - hydra_topology - hydra_dsg_builder - kimera_semantics_ros - roscpp - spark_dsg - std_msgs - tf2_eigen - voxblox_ros - - - - - diff --git a/hydra_dsg_builder_ros/src/lcd_visualizer.cpp b/hydra_dsg_builder_ros/src/lcd_visualizer.cpp deleted file mode 100644 index 30c90631..00000000 --- a/hydra_dsg_builder_ros/src/lcd_visualizer.cpp +++ /dev/null @@ -1,315 +0,0 @@ -/* ----------------------------------------------------------------------------- - * Copyright 2022 Massachusetts Institute of Technology. - * All Rights Reserved - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Research was sponsored by the United States Air Force Research Laboratory and - * the United States Air Force Artificial Intelligence Accelerator and was - * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views - * and conclusions contained in this document are those of the authors and should - * not be interpreted as representing the official policies, either expressed or - * implied, of the United States Air Force or the U.S. Government. The U.S. - * Government is authorized to reproduce and distribute reprints for Government - * purposes notwithstanding any copyright notation herein. - * -------------------------------------------------------------------------- */ -#include "hydra_dsg_builder_ros/lcd_visualizer.h" - -#include -#include - -#include - -namespace hydra { -namespace lcd { - -using visualization_msgs::Marker; -using visualization_msgs::MarkerArray; -using Node = SceneGraphLayer::Node; - -void makeCircle(Marker& marker, - const Eigen::Vector3d& position, - double z_offset, - double radius, - size_t num_points = 100) { - geometry_msgs::Point prev; - prev.x = radius + position.x(); - prev.y = position.y(); - prev.z = position.z() + z_offset; - - for (size_t i = 1; i <= num_points; ++i) { - double t = i / static_cast(num_points); - - geometry_msgs::Point curr; - curr.x = radius * std::cos(2 * M_PI * t) + position.x(); - curr.y = radius * std::sin(2 * M_PI * t) + position.y(); - curr.z = position.z() + z_offset; - marker.points.push_back(prev); - marker.points.push_back(curr); - - prev = curr; - } -} - -void readColor(const ros::NodeHandle& nh, - const std::string& name, - std::vector default_color, - NodeColor& color) { - nh.getParam(name, default_color); - CHECK_EQ(default_color.size(), 3u) << name << " size != 3"; - color << static_cast(default_color[0] * 255.0), - static_cast(default_color[1] * 255.0), - static_cast(default_color[2] * 255.0); -} - -LcdVisualizer::LcdVisualizer(const ros::NodeHandle& nh, double radius) - : DynamicSceneGraphVisualizer(nh, LcdVisualizer::getDefaultLayerIds()), - radius_(radius) { - // red-ish - std::vector invalid_color{0.78, 0.192, 0.173}; - readColor(nh, "invalid_color", invalid_color, invalid_match_color_); - - // green - std::vector valid_color{0.231, 0.820, 0.278}; - readColor(nh, "valid_color", valid_color, valid_match_color_); - - std::vector default_color{0.0, 0.0, 0.0}; - readColor(nh, "default_color", default_color, default_graph_color_); - - std::vector query_color{0.259, 0.529, 0.961}; - readColor(nh, "query_color", query_color, query_color_); - - const std::string ns = visualizer_ns_ + "/agent_layer"; - agent_config_ = std::make_shared(ros::NodeHandle(""), ns); -} - -void LcdVisualizer::setLcdDetector(DsgLcdDetector* detector) { - lcd_detector_ = detector; -} - -std::vector LcdVisualizer::getMatchRoots(LayerId layer) const { - const auto& layer_match_map = lcd_detector_->getLayerRemapping(); - if (!layer_match_map.count(layer)) { - return {}; - } - - const auto& matches = lcd_detector_->getLatestMatches(); - const size_t match_idx = layer_match_map.at(layer); - if (!matches.count(match_idx)) { - return {}; - } - - return matches.at(match_idx).match_root; -} - -std::set LcdVisualizer::getMatchedNodes(LayerId layer) const { - const auto& layer_match_map = lcd_detector_->getLayerRemapping(); - if (!layer_match_map.count(layer)) { - return {}; - } - - const auto& matches = lcd_detector_->getLatestMatches(); - const size_t match_idx = layer_match_map.at(layer); - if (!matches.count(match_idx)) { - return {}; - } - - const auto& result = matches.at(match_idx); - - std::set to_return; - for (const auto& match : result.match_nodes) { - to_return.insert(match.begin(), match.end()); - } - - return to_return; -} - -std::optional LcdVisualizer::getQueryNode() const { - const auto& matches = lcd_detector_->getLatestMatches(); - if (!matches.count(0)) { - return std::nullopt; - } - - return *matches.at(0).query_nodes.begin(); -} - -std::set LcdVisualizer::getValidNodes(LayerId layer) const { - const auto& layer_match_map = lcd_detector_->getLayerRemapping(); - if (!layer_match_map.count(layer)) { - return {}; - } - - const auto& matches = lcd_detector_->getLatestMatches(); - const size_t match_idx = layer_match_map.at(layer); - if (!matches.count(match_idx)) { - return {}; - } - - const auto& descriptor_cache = lcd_detector_->getDescriptorCache(layer); - const auto& result = matches.at(match_idx); - - std::set to_return; - for (const auto& valid_root : result.valid_matches) { - const auto& nodes = descriptor_cache.at(valid_root)->nodes; - to_return.insert(nodes.begin(), nodes.end()); - } - - return to_return; -} - -Marker makeCircleMarker(const std_msgs::Header& header, - const NodeColor& color, - double alpha, - const std::string& ns) { - Marker marker; - marker.header = header; - marker.type = Marker::LINE_LIST; - marker.action = Marker::ADD; - marker.id = 0; - marker.ns = ns; - marker.color = dsg_utils::makeColorMsg(color, alpha); - - Eigen::Vector3d identity_pos = Eigen::Vector3d::Zero(); - tf2::convert(identity_pos, marker.pose.position); - tf2::convert(Eigen::Quaterniond::Identity(), marker.pose.orientation); - - return marker; -} - -void LcdVisualizer::drawAgent(const std_msgs::Header& header, MarkerArray& msg) { - const DynamicLayerConfig& config = agent_config_->get(); - const std::string node_ns = "lcd_agent_nodes"; - const std::string circle_ns = "query_radius"; - - if (!config.visualize) { - deleteMultiMarker(header, node_ns, msg); - return; - } - - const auto& agent_layer = scene_graph_->getLayer(DsgLayers::AGENTS, 'a'); - - auto query_node = getQueryNode(); - if (!query_node) { - deleteMultiMarker(header, circle_ns, msg); - } else { - Marker query_radius = makeCircleMarker(header, query_color_, 1.0, circle_ns); - query_radius.scale.x = config.edge_scale; - - const double z_offset = - getZOffset(config.z_offset_scale, visualizer_config_->get()); - makeCircle(query_radius, agent_layer.getPosition(*query_node), z_offset, radius_); - addMultiMarkerIfValid(query_radius, msg); - } - - Marker nodes = - makeDynamicCentroidMarkers(header, - config, - agent_layer, - config.z_offset_scale, - visualizer_config_->get(), - node_ns, - [&](const auto& node) -> NodeColor { - if (query_node && *query_node == node.id) { - return query_color_; - } else { - return default_graph_color_; - } - }); - addMultiMarkerIfValid(nodes, msg); -} - -void LcdVisualizer::redrawImpl(const std_msgs::Header& header, MarkerArray& msg) { - if (!lcd_detector_) { - return; - } - - DynamicSceneGraphVisualizer::redrawImpl(header, msg); - drawAgent(header, msg); -} - -void LcdVisualizer::drawLayer(const std_msgs::Header& header, - const SceneGraphLayer& layer, - const LayerConfig& config, - MarkerArray& msg) { - const auto& viz_config = visualizer_config_->get(); - const std::string node_ns = getLayerNodeNamespace(layer.id); - - std::set matched_ids = getMatchedNodes(layer.id); - std::set valid_ids = getValidNodes(layer.id); - - Marker nodes = makeCentroidMarkers(header, - config, - layer, - viz_config, - node_ns, - [&](const SceneGraphNode& node) -> NodeColor { - if (matched_ids.count(node.id)) { - return valid_match_color_; - } else if (valid_ids.count(node.id)) { - return invalid_match_color_; - } else { - return default_graph_color_; - } - }); - addMultiMarkerIfValid(nodes, msg); - - auto match_roots = getMatchRoots(layer.id); - const std::string circle_ns = "match_radius" + std::to_string(layer.id); - if (match_roots.empty()) { - deleteMultiMarker(header, circle_ns, msg); - } else { - Marker match_radius = makeCircleMarker(header, valid_match_color_, 1.0, circle_ns); - match_radius.scale.x = agent_config_->get().edge_scale; - - const double z_offset = getZOffset(config, visualizer_config_->get()); - for (const auto& node_id : match_roots) { - makeCircle(match_radius, scene_graph_->getPosition(node_id), z_offset, radius_); - } - addMultiMarkerIfValid(match_radius, msg); - } - - const std::string edge_ns = getLayerEdgeNamespace(layer.id); - Marker edges = makeLayerEdgeMarkers( - header, - config, - layer, - viz_config, - edge_ns, - [&](const SceneGraphNode& src, const SceneGraphNode& tgt, bool) -> NodeColor { - if (matched_ids.count(src.id) && matched_ids.count(tgt.id)) { - return valid_match_color_; - } else if (valid_ids.count(src.id) && valid_ids.count(tgt.id)) { - return invalid_match_color_; - } else { - return default_graph_color_; - } - }); - addMultiMarkerIfValid(edges, msg); -} - -void LcdVisualizer::drawLayerMeshEdges(const std_msgs::Header&, - LayerId, - const std::string&, - MarkerArray&) {} - -} // namespace lcd -} // namespace hydra diff --git a/hydra_dsg_builder_ros/.gitignore b/hydra_ros/.gitignore similarity index 100% rename from hydra_dsg_builder_ros/.gitignore rename to hydra_ros/.gitignore diff --git a/hydra_ros/CMakeLists.txt b/hydra_ros/CMakeLists.txt new file mode 100644 index 00000000..39502f8f --- /dev/null +++ b/hydra_ros/CMakeLists.txt @@ -0,0 +1,127 @@ +cmake_minimum_required(VERSION 3.1) +project(hydra_ros) + +add_compile_options(-Wall -Wextra) +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) + +find_package(Boost REQUIRED COMPONENTS timer) +# find_package(Boost REQUIRED COMPONENTS filesystem) +find_package(spark_dsg REQUIRED) +find_package(GTSAM REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(yaml-cpp REQUIRED) +# TODO(nathan) find hydra independently +find_package( + catkin REQUIRED + COMPONENTS roscpp + dynamic_reconfigure + std_msgs + geometry_msgs + visualization_msgs + hydra_msgs + pcl_ros + tf2_eigen + tf2_ros + kimera_pgmo + voxblox_ros + pose_graph_tools + kimera_semantics_ros + hydra +) + +generate_dynamic_reconfigure_options( + cfg/Colormap.cfg + cfg/DynamicLayerVisualizer.cfg + cfg/GvdVisualizer.cfg + cfg/LayerVisualizer.cfg + cfg/Visualizer.cfg +) + +catkin_package( + CATKIN_DEPENDS + roscpp + dynamic_reconfigure + std_msgs + geometry_msgs + visualization_msgs + hydra_msgs + pcl_ros + tf2_eigen + tf2_ros + kimera_pgmo + voxblox_ros + pose_graph_tools + kimera_semantics_ros + DEPENDS spark_dsg hydra + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} +) + +add_library( + ${PROJECT_NAME} + src/config/ros_parser.cpp + src/pipeline/hydra_ros_pipeline.cpp + src/pipeline/ros_backend.cpp + src/pipeline/ros_frontend.cpp + src/pipeline/ros_lcd_registration.cpp + src/pipeline/ros_reconstruction.cpp + src/utils/dsg_streaming_interface.cpp + src/utils/mask_nodelet.cpp + src/utils/node_utilities.cpp + src/visualizer/colormap_utilities.cpp + src/visualizer/dsg_mesh_plugins.cpp + src/visualizer/dynamic_scene_graph_visualizer.cpp + src/visualizer/gvd_visualization_utilities.cpp + src/visualizer/topology_server_visualizer.cpp + src/visualizer/visualizer_plugins.cpp + src/visualizer/visualizer_utilities.cpp +) +target_include_directories(${PROJECT_NAME} PUBLIC include ${catkin_INCLUDE_DIRS}) +target_link_libraries( + ${PROJECT_NAME} + ${catkin_LIBRARIES} + spark_dsg::spark_dsg + gtsam +) +add_dependencies( + ${PROJECT_NAME} ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS} +) + +add_executable(dsg_optimizer_node src/nodes/dsg_optimizer_node.cpp) +target_link_libraries(dsg_optimizer_node ${PROJECT_NAME}) + +add_executable(gvd_validator_node src/nodes/gvd_validator_node.cpp) +target_link_libraries(gvd_validator_node PUBLIC ${PROJECT_NAME} yaml-cpp) + +add_executable(hydra_ros_node src/nodes/hydra_node.cpp) +target_link_libraries(hydra_ros_node PUBLIC ${PROJECT_NAME}) + +add_executable(hydra_topology_node src/nodes/hydra_topology_node.cpp) +target_link_libraries(hydra_topology_node PUBLIC ${PROJECT_NAME} Boost::boost) + +add_executable(hydra_visualizer_node src/nodes/hydra_visualizer_node.cpp) +target_link_libraries(hydra_visualizer_node PUBLIC ${PROJECT_NAME}) + +add_executable(rotate_tf_node src/nodes/rotate_tf_node.cpp) +target_link_libraries(rotate_tf_node PUBLIC ${PROJECT_NAME}) + +add_executable(scene_graph_logger_node src/nodes/scene_graph_logger_node.cpp) +target_link_libraries(scene_graph_logger_node PUBLIC ${PROJECT_NAME}) + +if(CATKIN_ENABLE_TESTING) + add_subdirectory(tests) +endif() + +# TODO(nathan) handle install +install( + TARGETS ${PROJECT_NAME} hydra_ros_node + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) diff --git a/hydra_ros/README.md b/hydra_ros/README.md new file mode 100644 index 00000000..83f81c93 --- /dev/null +++ b/hydra_ros/README.md @@ -0,0 +1,10 @@ +## Hydra-ROS + +Contains ROS code specific to Hydra. + +To build and run tests (assuming you've built the package normally): + +``` +catkin build hydra_utils --catkin-make-args tests +rostest hydra_utils hydra_utils.test +``` diff --git a/hydra_utils/nodes/csv_to_tf b/hydra_ros/bin/csv_to_tf similarity index 100% rename from hydra_utils/nodes/csv_to_tf rename to hydra_ros/bin/csv_to_tf diff --git a/hydra_dsg_builder_ros/bin/gnn_node b/hydra_ros/bin/gnn_node similarity index 100% rename from hydra_dsg_builder_ros/bin/gnn_node rename to hydra_ros/bin/gnn_node diff --git a/hydra_utils/nodes/odom_to_tf b/hydra_ros/bin/odom_to_tf similarity index 100% rename from hydra_utils/nodes/odom_to_tf rename to hydra_ros/bin/odom_to_tf diff --git a/hydra_utils/cfg/Colormap.cfg b/hydra_ros/cfg/Colormap.cfg similarity index 96% rename from hydra_utils/cfg/Colormap.cfg rename to hydra_ros/cfg/Colormap.cfg index 3d99d6d2..b089b60b 100755 --- a/hydra_utils/cfg/Colormap.cfg +++ b/hydra_ros/cfg/Colormap.cfg @@ -1,6 +1,6 @@ #!/usr/bin/env python """Generic HLS colormap dynamic config.""" -PACKAGE = "hydra_utils" +PACKAGE = "hydra_ros" import dynamic_reconfigure.parameter_generator_catkin as dr_gen # NOQA diff --git a/hydra_utils/cfg/DynamicLayerVisualizer.cfg b/hydra_ros/cfg/DynamicLayerVisualizer.cfg similarity index 98% rename from hydra_utils/cfg/DynamicLayerVisualizer.cfg rename to hydra_ros/cfg/DynamicLayerVisualizer.cfg index f1a23dd9..ab1f7cd2 100755 --- a/hydra_utils/cfg/DynamicLayerVisualizer.cfg +++ b/hydra_ros/cfg/DynamicLayerVisualizer.cfg @@ -1,6 +1,6 @@ #!/usr/bin/env python """Config for the visualization of dynamic layers in the scene graph.""" -PACKAGE = "hydra_utils" +PACKAGE = "hydra_ros" import dynamic_reconfigure.parameter_generator_catkin as dr_gen # NOQA diff --git a/hydra_topology/cfg/GvdVisualizer.cfg b/hydra_ros/cfg/GvdVisualizer.cfg similarity index 98% rename from hydra_topology/cfg/GvdVisualizer.cfg rename to hydra_ros/cfg/GvdVisualizer.cfg index d9e5f434..703b5dfc 100755 --- a/hydra_topology/cfg/GvdVisualizer.cfg +++ b/hydra_ros/cfg/GvdVisualizer.cfg @@ -1,6 +1,6 @@ #!/usr/bin/env python """Config for GVD visualizations.""" -PACKAGE = "hydra_topology" +PACKAGE = "hydra_ros" import dynamic_reconfigure.parameter_generator_catkin as dr_gen # NOQA diff --git a/hydra_utils/cfg/LayerVisualizer.cfg b/hydra_ros/cfg/LayerVisualizer.cfg similarity index 99% rename from hydra_utils/cfg/LayerVisualizer.cfg rename to hydra_ros/cfg/LayerVisualizer.cfg index 142680c9..edf19032 100755 --- a/hydra_utils/cfg/LayerVisualizer.cfg +++ b/hydra_ros/cfg/LayerVisualizer.cfg @@ -1,6 +1,6 @@ #!/usr/bin/env python """Visualization config for static layers in the scene graph.""" -PACKAGE = "hydra_utils" +PACKAGE = "hydra_ros" import dynamic_reconfigure.parameter_generator_catkin as dr_gen # NOQA diff --git a/hydra_utils/cfg/Visualizer.cfg b/hydra_ros/cfg/Visualizer.cfg similarity index 98% rename from hydra_utils/cfg/Visualizer.cfg rename to hydra_ros/cfg/Visualizer.cfg index 4912ca86..8713fe3d 100755 --- a/hydra_utils/cfg/Visualizer.cfg +++ b/hydra_ros/cfg/Visualizer.cfg @@ -1,6 +1,6 @@ #!/usr/bin/env python """Configuration options for the scene graph visualization.""" -PACKAGE = "hydra_utils" +PACKAGE = "hydra_ros" import dynamic_reconfigure.parameter_generator_catkin as dr_gen # NOQA diff --git a/hydra_topology/config/graph_visualization_config.yaml b/hydra_ros/config/graph_visualization_config.yaml similarity index 100% rename from hydra_topology/config/graph_visualization_config.yaml rename to hydra_ros/config/graph_visualization_config.yaml diff --git a/hydra_topology/config/validator/uh2.yaml b/hydra_ros/config/gvd_validator/uh2.yaml similarity index 100% rename from hydra_topology/config/validator/uh2.yaml rename to hydra_ros/config/gvd_validator/uh2.yaml diff --git a/hydra_utils/config/hydra_visualizer/agent_layer.yaml b/hydra_ros/config/hydra_visualizer/agent_layer.yaml similarity index 100% rename from hydra_utils/config/hydra_visualizer/agent_layer.yaml rename to hydra_ros/config/hydra_visualizer/agent_layer.yaml diff --git a/hydra_utils/config/hydra_visualizer/buildings_layer.yaml b/hydra_ros/config/hydra_visualizer/buildings_layer.yaml similarity index 100% rename from hydra_utils/config/hydra_visualizer/buildings_layer.yaml rename to hydra_ros/config/hydra_visualizer/buildings_layer.yaml diff --git a/hydra_utils/config/hydra_visualizer/objects_layer.yaml b/hydra_ros/config/hydra_visualizer/objects_layer.yaml similarity index 100% rename from hydra_utils/config/hydra_visualizer/objects_layer.yaml rename to hydra_ros/config/hydra_visualizer/objects_layer.yaml diff --git a/hydra_utils/config/hydra_visualizer/places_layer.yaml b/hydra_ros/config/hydra_visualizer/places_layer.yaml similarity index 100% rename from hydra_utils/config/hydra_visualizer/places_layer.yaml rename to hydra_ros/config/hydra_visualizer/places_layer.yaml diff --git a/hydra_utils/config/hydra_visualizer/rooms_layer.yaml b/hydra_ros/config/hydra_visualizer/rooms_layer.yaml similarity index 100% rename from hydra_utils/config/hydra_visualizer/rooms_layer.yaml rename to hydra_ros/config/hydra_visualizer/rooms_layer.yaml diff --git a/hydra_utils/config/hydra_visualizer/visualizer.yaml b/hydra_ros/config/hydra_visualizer/visualizer.yaml similarity index 100% rename from hydra_utils/config/hydra_visualizer/visualizer.yaml rename to hydra_ros/config/hydra_visualizer/visualizer.yaml diff --git a/hydra_utils/config/object_visualizer/agent_layer.yaml b/hydra_ros/config/object_visualizer/agent_layer.yaml similarity index 100% rename from hydra_utils/config/object_visualizer/agent_layer.yaml rename to hydra_ros/config/object_visualizer/agent_layer.yaml diff --git a/hydra_utils/config/object_visualizer/buildings_layer.yaml b/hydra_ros/config/object_visualizer/buildings_layer.yaml similarity index 100% rename from hydra_utils/config/object_visualizer/buildings_layer.yaml rename to hydra_ros/config/object_visualizer/buildings_layer.yaml diff --git a/hydra_utils/config/object_visualizer/objects_layer.yaml b/hydra_ros/config/object_visualizer/objects_layer.yaml similarity index 100% rename from hydra_utils/config/object_visualizer/objects_layer.yaml rename to hydra_ros/config/object_visualizer/objects_layer.yaml diff --git a/hydra_utils/config/object_visualizer/places_layer.yaml b/hydra_ros/config/object_visualizer/places_layer.yaml similarity index 100% rename from hydra_utils/config/object_visualizer/places_layer.yaml rename to hydra_ros/config/object_visualizer/places_layer.yaml diff --git a/hydra_utils/config/object_visualizer/rooms_layer.yaml b/hydra_ros/config/object_visualizer/rooms_layer.yaml similarity index 100% rename from hydra_utils/config/object_visualizer/rooms_layer.yaml rename to hydra_ros/config/object_visualizer/rooms_layer.yaml diff --git a/hydra_utils/config/object_visualizer/visualizer.yaml b/hydra_ros/config/object_visualizer/visualizer.yaml similarity index 100% rename from hydra_utils/config/object_visualizer/visualizer.yaml rename to hydra_ros/config/object_visualizer/visualizer.yaml diff --git a/hydra_dsg_builder_ros/config/overrides.yaml b/hydra_ros/config/overrides.yaml similarity index 100% rename from hydra_dsg_builder_ros/config/overrides.yaml rename to hydra_ros/config/overrides.yaml diff --git a/hydra_dsg_builder_ros/config/ros_params.yaml b/hydra_ros/config/ros_params.yaml similarity index 100% rename from hydra_dsg_builder_ros/config/ros_params.yaml rename to hydra_ros/config/ros_params.yaml diff --git a/hydra_utils/CMakeLists.txt b/hydra_ros/hydra_utils_cmake similarity index 68% rename from hydra_utils/CMakeLists.txt rename to hydra_ros/hydra_utils_cmake index a9fdffc0..df32fb65 100644 --- a/hydra_utils/CMakeLists.txt +++ b/hydra_ros/hydra_utils_cmake @@ -1,11 +1,3 @@ -cmake_minimum_required(VERSION 3.1) -project(hydra_utils) - -add_compile_options(-Wall -Wextra) -set(CMAKE_CXX_STANDARD 17) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - find_package( catkin REQUIRED COMPONENTS dynamic_reconfigure @@ -20,20 +12,9 @@ find_package( visualization_msgs glog_catkin ) -find_package(GTSAM REQUIRED) -find_package(spark_dsg REQUIRED) -find_package(Eigen3 REQUIRED) -find_package(yaml-cpp REQUIRED) find_package(OpenCV REQUIRED) find_package(cv_bridge REQUIRED) -generate_dynamic_reconfigure_options( - cfg/Colormap.cfg - cfg/DynamicLayerVisualizer.cfg - cfg/LayerVisualizer.cfg - cfg/Visualizer.cfg -) - catkin_package( CATKIN_DEPENDS dynamic_reconfigure @@ -51,20 +32,6 @@ catkin_package( LIBRARIES ${PROJECT_NAME} ) -add_library( - ${PROJECT_NAME} - src/colormap_utils.cpp - src/display_utils.cpp - src/dsg_streaming_interface.cpp - src/dsg_mesh_plugins.cpp - src/dynamic_scene_graph_visualizer.cpp - src/log_utilities.cpp - src/ros_parser.cpp - src/robot_prefix_config.cpp - src/timing_utilities.cpp - src/visualizer_utils.cpp - src/yaml_parser.cpp -) target_include_directories( ${PROJECT_NAME} PUBLIC include ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} @@ -99,15 +66,6 @@ target_link_libraries( ${cv_bridge_LIBRARIES} ) -if(CATKIN_ENABLE_TESTING) - find_package(rostest REQUIRED) - add_rostest_gtest( - utest_${PROJECT_NAME} tests/hydra_utils.test - tests/utest_main.cpp tests/utest_config.cpp tests/utest_timing_utilities.cpp - ) - target_link_libraries(utest_${PROJECT_NAME} ${PROJECT_NAME} ${catkin_LIBRARIES}) -endif() - install( TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_nodelet diff --git a/hydra_utils/include/hydra_utils/ros_parser.h b/hydra_ros/include/hydra_ros/config/ros_parser.h similarity index 94% rename from hydra_utils/include/hydra_utils/ros_parser.h rename to hydra_ros/include/hydra_ros/config/ros_parser.h index 2e6cb319..d656788e 100644 --- a/hydra_utils/include/hydra_utils/ros_parser.h +++ b/hydra_ros/include/hydra_ros/config/ros_parser.h @@ -33,9 +33,9 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include "hydra_utils/config_parser.h" - +#include #include + #include namespace config_parser { @@ -135,6 +135,18 @@ bool readRosParam(const ros::NodeHandle& nh, return true; } +template ::value, bool>::type = true> +bool readRosParam(const ros::NodeHandle& nh, const std::string& name, T& value) { + std::string placeholder; + if (!readRosParam(nh, name, placeholder)) { + return false; + } + + readConfigEnumFromString(placeholder, value); + return true; +} + // adl indirection struct read_ros_param_fn { template diff --git a/hydra_utils/include/hydra_utils/ros_utilities.h b/hydra_ros/include/hydra_ros/config/ros_utilities.h similarity index 67% rename from hydra_utils/include/hydra_utils/ros_utilities.h rename to hydra_ros/include/hydra_ros/config/ros_utilities.h index 57765e93..6f4cd619 100644 --- a/hydra_utils/include/hydra_utils/ros_utilities.h +++ b/hydra_ros/include/hydra_ros/config/ros_utilities.h @@ -34,9 +34,43 @@ * -------------------------------------------------------------------------- */ #pragma once #include -#include +#include #include +#include + +#include "hydra_ros/config/ros_parser.h" + +namespace config_parser { + +template +Config load_from_ros(const std::string& ns, Logger::Ptr logger = nullptr) { + RosParser parser(std::make_unique(ros::NodeHandle(ns))); + if (logger) { + parser.setLogger(logger); + } + + Config config; + ConfigVisitor::visit_config(parser, config); + return config; +} + +template +Config load_from_ros_nh(const ros::NodeHandle& nh, + const std::string& ns = "", + Logger::Ptr logger = nullptr) { + RosParser parser(std::make_unique(nh, ns)); + if (logger) { + parser.setLogger(logger); + } + + Config config; + ConfigVisitor::visit_config(parser, config); + return config; +} + +} // namespace config_parser + namespace hydra { struct HydraParamLogger : config_parser::Logger { @@ -62,9 +96,7 @@ Config load_config(const ros::NodeHandle& nh, if (verbose) { const auto name = ConfigStructName::get(); std::string filler(name.size() + 50, '='); - VLOG(dump_verbosity) << name << std::endl - << filler << std::endl - << config; + VLOG(dump_verbosity) << name << std::endl << filler << std::endl << config; } return config; @@ -72,6 +104,33 @@ Config load_config(const ros::NodeHandle& nh, } // namespace hydra +namespace Eigen { + +template +bool readRosParam(const ros::NodeHandle& nh, + const std::string& name, + Matrix& value) { + std::vector raw_values; + config_parser::readRosParam(nh, name, raw_values); + if (raw_values.empty()) { + return false; + } + + if (raw_values.size() != static_cast(N)) { + std::stringstream ss; + ss << "invalid param length: " << raw_values.size() << " != " << N; + throw std::domain_error(ss.str()); + } + + for (int i = 0; i < N; ++i) { + value(i) = raw_values[i]; + } + + return true; +} + +} // namespace Eigen + #define DECLARE_STRUCT_NAME(name) \ template <> \ struct ConfigStructName { \ diff --git a/hydra_dsg_builder_ros/include/hydra_dsg_builder_ros/hydra_ros_pipeline.h b/hydra_ros/include/hydra_ros/pipeline/hydra_ros_pipeline.h similarity index 88% rename from hydra_dsg_builder_ros/include/hydra_dsg_builder_ros/hydra_ros_pipeline.h rename to hydra_ros/include/hydra_ros/pipeline/hydra_ros_pipeline.h index f41fc169..d5b58981 100644 --- a/hydra_dsg_builder_ros/include/hydra_dsg_builder_ros/hydra_ros_pipeline.h +++ b/hydra_ros/include/hydra_ros/pipeline/hydra_ros_pipeline.h @@ -33,14 +33,14 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include "hydra_dsg_builder_ros/ros_backend.h" -#include "hydra_dsg_builder_ros/ros_frontend.h" - -#include -#include -#include +#include +#include #include +#include "hydra_ros/pipeline/ros_backend.h" +#include "hydra_ros/pipeline/ros_frontend.h" +#include "hydra_ros/pipeline/ros_reconstruction.h" + namespace hydra { struct HydraRosConfig { @@ -63,7 +63,7 @@ struct HydraRosPipeline { void bowCallback(const pose_graph_tools::BowQueries::ConstPtr& msg); void sendFrontendOutput(const DynamicSceneGraph& graph, - const incremental::BackendInput& backend_input, + const BackendInput& backend_input, uint64_t timestamp_ns); void sendFrontendGraph(const DynamicSceneGraph& graph, uint64_t timestamp_ns); @@ -77,10 +77,10 @@ struct HydraRosPipeline { SharedModuleState::Ptr shared_state; std::shared_ptr reconstruction; - std::shared_ptr frontend; - std::shared_ptr backend; + std::shared_ptr frontend; + std::shared_ptr backend; std::shared_ptr backend_visualizer; - std::shared_ptr lcd; + std::shared_ptr lcd; std::unique_ptr dsg_sender; ros::Publisher mesh_graph_pub; diff --git a/hydra_dsg_builder_ros/include/hydra_dsg_builder_ros/ros_backend.h b/hydra_ros/include/hydra_ros/pipeline/ros_backend.h similarity index 92% rename from hydra_dsg_builder_ros/include/hydra_dsg_builder_ros/ros_backend.h rename to hydra_ros/include/hydra_ros/pipeline/ros_backend.h index 1c9748c2..79e55d0e 100644 --- a/hydra_dsg_builder_ros/include/hydra_dsg_builder_ros/ros_backend.h +++ b/hydra_ros/include/hydra_ros/pipeline/ros_backend.h @@ -32,22 +32,20 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include -#include +#include #include #include #include +#include "hydra_ros/config/ros_utilities.h" +#include "hydra_ros/utils/dsg_streaming_interface.h" + namespace hydra { -using incremental::DsgBackend; -using incremental::DsgBackendConfig; -using incremental::SharedDsgInfo; -using incremental::SharedModuleState; using kimera_pgmo::KimeraPgmoMesh; using pose_graph_tools::PoseGraph; -class RosBackend : public DsgBackend { +class RosBackend : public BackendModule { public: using Policy = message_filters::sync_policies::ApproximateTime; @@ -89,7 +87,7 @@ class RosBackend : public DsgBackend { class RosBackendVisualizer { public: RosBackendVisualizer(const ros::NodeHandle& nh, - const DsgBackendConfig& config, + const BackendConfig& config, const RobotPrefixConfig& prefix); virtual ~RosBackendVisualizer() = default; @@ -107,7 +105,7 @@ class RosBackendVisualizer { protected: ros::NodeHandle nh_; - DsgBackendConfig config_; + BackendConfig config_; RobotPrefixConfig prefix_; ros::Publisher mesh_mesh_edges_pub_; diff --git a/hydra_dsg_builder_ros/include/hydra_dsg_builder_ros/ros_frontend.h b/hydra_ros/include/hydra_ros/pipeline/ros_frontend.h similarity index 94% rename from hydra_dsg_builder_ros/include/hydra_dsg_builder_ros/ros_frontend.h rename to hydra_ros/include/hydra_ros/pipeline/ros_frontend.h index 807d4fa6..69ba0585 100644 --- a/hydra_dsg_builder_ros/include/hydra_dsg_builder_ros/ros_frontend.h +++ b/hydra_ros/include/hydra_ros/pipeline/ros_frontend.h @@ -33,26 +33,26 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include -#include -#include +#include #include #include #include #include +#include +#include #include #include #include #include +#include "hydra_ros/config/ros_utilities.h" +#include "hydra_ros/utils/dsg_streaming_interface.h" +#include "hydra_ros/utils/semantic_ros_publishers.h" + namespace hydra { using hydra_msgs::ActiveLayer; using hydra_msgs::ActiveMesh; -using incremental::DsgFrontend; -using incremental::MeshSegmenter; -using incremental::SharedDsgInfo; -using incremental::SharedModuleState; using pose_graph_tools::PoseGraph; using ObjectCloudPub = SemanticRosPublishers; @@ -78,7 +78,7 @@ void visit_config(const Visitor& v, ROSFrontendConfig& config) { v.visit("use_posegraph_pos", config.use_posegraph_pos); } -struct RosFrontend : public DsgFrontend { +struct RosFrontend : public FrontendModule { using Policy = message_filters::sync_policies::ApproximateTime; using Sync = message_filters::Synchronizer; diff --git a/hydra_dsg_builder_ros/include/hydra_dsg_builder_ros/ros_lcd_registration.h b/hydra_ros/include/hydra_ros/pipeline/ros_lcd_registration.h similarity index 98% rename from hydra_dsg_builder_ros/include/hydra_dsg_builder_ros/ros_lcd_registration.h rename to hydra_ros/include/hydra_ros/pipeline/ros_lcd_registration.h index efab0b06..091bfce3 100644 --- a/hydra_dsg_builder_ros/include/hydra_dsg_builder_ros/ros_lcd_registration.h +++ b/hydra_ros/include/hydra_ros/pipeline/ros_lcd_registration.h @@ -33,7 +33,7 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include +#include namespace hydra { diff --git a/hydra_topology/include/hydra_topology/ros_reconstruction.h b/hydra_ros/include/hydra_ros/pipeline/ros_reconstruction.h similarity index 95% rename from hydra_topology/include/hydra_topology/ros_reconstruction.h rename to hydra_ros/include/hydra_ros/pipeline/ros_reconstruction.h index 4ca857c0..eb932299 100644 --- a/hydra_topology/include/hydra_topology/ros_reconstruction.h +++ b/hydra_ros/include/hydra_ros/pipeline/ros_reconstruction.h @@ -34,6 +34,7 @@ * -------------------------------------------------------------------------- */ #pragma once #include +#include #include #include #include @@ -41,8 +42,8 @@ #include #include -#include "hydra_topology/reconstruction_module.h" -#include "hydra_topology/topology_server_visualizer.h" +#include "hydra_ros/config/ros_utilities.h" +#include "hydra_ros/visualizer/topology_server_visualizer.h" namespace hydra { @@ -103,12 +104,11 @@ class RosReconstruction : public ReconstructionModule { ros::NodeHandle nh_; RosReconstructionConfig ros_config_; - // unsynchronzied receive via tf ros::Subscriber pcl_sub_; ros::Subscriber pose_graph_sub_; - // tf2_ros::Buffer buffer_; std::unique_ptr buffer_; std::unique_ptr tf_listener_; + PointcloudQueue pointcloud_queue_; std::unique_ptr pointcloud_thread_; std::unique_ptr last_time_received_; @@ -116,7 +116,7 @@ class RosReconstruction : public ReconstructionModule { std::list pose_graphs_; // visualizer - std::unique_ptr visualizer_; + std::unique_ptr visualizer_; ros::Publisher mesh_pub_; // freespace query diff --git a/hydra_utils/include/hydra_utils/dsg_streaming_interface.h b/hydra_ros/include/hydra_ros/utils/dsg_streaming_interface.h similarity index 99% rename from hydra_utils/include/hydra_utils/dsg_streaming_interface.h rename to hydra_ros/include/hydra_ros/utils/dsg_streaming_interface.h index 3e6ae1f3..b79436b6 100644 --- a/hydra_utils/include/hydra_utils/dsg_streaming_interface.h +++ b/hydra_ros/include/hydra_ros/utils/dsg_streaming_interface.h @@ -33,8 +33,7 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include "hydra_utils/dsg_types.h" - +#include #include #include #include diff --git a/hydra_dsg_builder_ros/include/hydra_dsg_builder_ros/node_utilities.h b/hydra_ros/include/hydra_ros/utils/node_utilities.h similarity index 98% rename from hydra_dsg_builder_ros/include/hydra_dsg_builder_ros/node_utilities.h rename to hydra_ros/include/hydra_ros/utils/node_utilities.h index d73651bf..d56a0f12 100644 --- a/hydra_dsg_builder_ros/include/hydra_dsg_builder_ros/node_utilities.h +++ b/hydra_ros/include/hydra_ros/utils/node_utilities.h @@ -33,7 +33,7 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include +#include #include #include #include diff --git a/hydra_utils/include/hydra_utils/semantic_ros_publishers.h b/hydra_ros/include/hydra_ros/utils/semantic_ros_publishers.h similarity index 100% rename from hydra_utils/include/hydra_utils/semantic_ros_publishers.h rename to hydra_ros/include/hydra_ros/utils/semantic_ros_publishers.h diff --git a/hydra_utils/include/hydra_utils/colormap_utils.h b/hydra_ros/include/hydra_ros/visualizer/colormap_utilities.h similarity index 98% rename from hydra_utils/include/hydra_utils/colormap_utils.h rename to hydra_ros/include/hydra_ros/visualizer/colormap_utilities.h index 15dc1f8e..ef15f08c 100644 --- a/hydra_utils/include/hydra_utils/colormap_utils.h +++ b/hydra_ros/include/hydra_ros/visualizer/colormap_utilities.h @@ -33,10 +33,10 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include "hydra_utils/visualizer_types.h" - #include +#include "hydra_ros/visualizer/visualizer_types.h" + namespace hydra { namespace dsg_utils { diff --git a/hydra_utils/include/hydra_utils/dsg_mesh_plugins.h b/hydra_ros/include/hydra_ros/visualizer/dsg_mesh_plugins.h similarity index 98% rename from hydra_utils/include/hydra_utils/dsg_mesh_plugins.h rename to hydra_ros/include/hydra_ros/visualizer/dsg_mesh_plugins.h index 7df90208..f7902e99 100644 --- a/hydra_utils/include/hydra_utils/dsg_mesh_plugins.h +++ b/hydra_ros/include/hydra_ros/visualizer/dsg_mesh_plugins.h @@ -33,11 +33,11 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include "hydra_utils/dsg_visualizer_plugin.h" - #include #include +#include "hydra_ros/visualizer/dsg_visualizer_plugin.h" + namespace hydra { class RvizMeshPlugin : public DsgVisualizerPlugin { diff --git a/hydra_utils/include/hydra_utils/dsg_visualizer_plugin.h b/hydra_ros/include/hydra_ros/visualizer/dsg_visualizer_plugin.h similarity index 98% rename from hydra_utils/include/hydra_utils/dsg_visualizer_plugin.h rename to hydra_ros/include/hydra_ros/visualizer/dsg_visualizer_plugin.h index a65b54ee..6dd573d0 100644 --- a/hydra_utils/include/hydra_utils/dsg_visualizer_plugin.h +++ b/hydra_ros/include/hydra_ros/visualizer/dsg_visualizer_plugin.h @@ -33,8 +33,8 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once +#include #include -#include "hydra_utils/dsg_types.h" namespace hydra { diff --git a/hydra_utils/include/hydra_utils/dynamic_scene_graph_visualizer.h b/hydra_ros/include/hydra_ros/visualizer/dynamic_scene_graph_visualizer.h similarity index 98% rename from hydra_utils/include/hydra_utils/dynamic_scene_graph_visualizer.h rename to hydra_ros/include/hydra_ros/visualizer/dynamic_scene_graph_visualizer.h index f005d67e..7745f2a9 100644 --- a/hydra_utils/include/hydra_utils/dynamic_scene_graph_visualizer.h +++ b/hydra_ros/include/hydra_ros/visualizer/dynamic_scene_graph_visualizer.h @@ -33,17 +33,16 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include "hydra_utils/dsg_visualizer_plugin.h" -#include "hydra_utils/visualizer_types.h" - #include +#include #include #include #include #include -#include +#include "hydra_ros/visualizer/dsg_visualizer_plugin.h" +#include "hydra_ros/visualizer/visualizer_types.h" namespace hydra { diff --git a/hydra_topology/include/hydra_topology/gvd_visualization_utilities.h b/hydra_ros/include/hydra_ros/visualizer/gvd_visualization_utilities.h similarity index 71% rename from hydra_topology/include/hydra_topology/gvd_visualization_utilities.h rename to hydra_ros/include/hydra_ros/visualizer/gvd_visualization_utilities.h index 51f8740d..8bcf61d2 100644 --- a/hydra_topology/include/hydra_topology/gvd_visualization_utilities.h +++ b/hydra_ros/include/hydra_ros/visualizer/gvd_visualization_utilities.h @@ -33,21 +33,19 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include -#include +#include +#include +#include +#include #include #include -#include "hydra_topology/compression_graph_extractor.h" -#include "hydra_topology/gvd_graph.h" -#include "hydra_topology/gvd_voxel.h" -#include "hydra_topology/voxblox_types.h" +#include "hydra_ros/visualizer/visualizer_types.h" namespace hydra { -namespace topology { -using hydra_topology::GvdVisualizerConfig; -using CompressedNodeMap = std::unordered_map; +using hydra_ros::GvdVisualizerConfig; +using CompressedNodeMap = std::unordered_map; class MarkerGroupPub { public: @@ -66,46 +64,50 @@ class MarkerGroupPub { }; enum class GvdVisualizationMode : int { - DEFAULT = hydra_topology::GvdVisualizer_DEFAULT, - DISTANCE = hydra_topology::GvdVisualizer_DISTANCE, - BASIS_POINTS = hydra_topology::GvdVisualizer_BASIS_POINTS, + DEFAULT = hydra_ros::GvdVisualizer_DEFAULT, + DISTANCE = hydra_ros::GvdVisualizer_DISTANCE, + BASIS_POINTS = hydra_ros::GvdVisualizer_BASIS_POINTS, }; GvdVisualizationMode getModeFromString(const std::string& mode); visualization_msgs::Marker makeGvdMarker(const GvdVisualizerConfig& config, const ColormapConfig& colors, - const Layer& layer); + const voxblox::Layer& layer); -visualization_msgs::Marker makeSurfaceVoxelMarker(const GvdVisualizerConfig& config, - const ColormapConfig& colors, - const Layer& layer); +visualization_msgs::Marker makeSurfaceVoxelMarker( + const GvdVisualizerConfig& config, + const ColormapConfig& colors, + const voxblox::Layer& layer); visualization_msgs::Marker makeErrorMarker(const GvdVisualizerConfig& config, const ColormapConfig& colors, - const Layer& lhs, - const Layer& rhs, + const voxblox::Layer& lhs, + const voxblox::Layer& rhs, double threshold); -visualization_msgs::Marker makeEsdfMarker(const GvdVisualizerConfig& config, - const ColormapConfig& colors, - const Layer& layer); +visualization_msgs::Marker makeEsdfMarker( + const GvdVisualizerConfig& config, + const ColormapConfig& colors, + const voxblox::Layer& layer); -visualization_msgs::Marker makeBlocksMarker(const Layer& layer, - double scale); +visualization_msgs::Marker makeBlocksMarker( + const voxblox::Layer& layer, double scale); -visualization_msgs::Marker makeBlocksMarker(const Layer& layer, double scale); +visualization_msgs::Marker makeBlocksMarker( + const voxblox::Layer& layer, double scale); -visualization_msgs::Marker makeMeshBlocksMarker(const MeshLayer& layer, double scale); +visualization_msgs::Marker makeMeshBlocksMarker(const voxblox::MeshLayer& layer, + double scale); -visualization_msgs::MarkerArray makeGvdGraphMarkers(const GvdGraph& graph, +visualization_msgs::MarkerArray makeGvdGraphMarkers(const places::GvdGraph& graph, const GvdVisualizerConfig& config, const ColormapConfig& colors, const std::string& ns, size_t marker_id = 0); visualization_msgs::MarkerArray showGvdClusters( - const GvdGraph& graph, + const places::GvdGraph& graph, const CompressedNodeMap& clusters, const std::unordered_map& remapping, const GvdVisualizerConfig& config, @@ -118,5 +120,4 @@ visualization_msgs::MarkerArray makePlaceSpheres(const std_msgs::Header& header, const std::string& ns, double alpha = 0.1); -} // namespace topology } // namespace hydra diff --git a/hydra_topology/include/hydra_topology/topology_server_visualizer.h b/hydra_ros/include/hydra_ros/visualizer/topology_server_visualizer.h similarity index 79% rename from hydra_topology/include/hydra_topology/topology_server_visualizer.h rename to hydra_ros/include/hydra_ros/visualizer/topology_server_visualizer.h index bdcc3533..827a435f 100644 --- a/hydra_topology/include/hydra_topology/topology_server_visualizer.h +++ b/hydra_ros/include/hydra_ros/visualizer/topology_server_visualizer.h @@ -34,19 +34,18 @@ * -------------------------------------------------------------------------- */ #pragma once #include -#include -#include -#include -#include +#include +#include +#include +#include -#include "hydra_topology/compression_graph_extractor.h" -#include "hydra_topology/configs.h" -#include "hydra_topology/gvd_visualization_utilities.h" +#include "hydra_ros/visualizer/gvd_visualization_utilities.h" +#include "hydra_ros/visualizer/visualizer_types.h" +#include "hydra_ros/visualizer/visualizer_utilities.h" namespace hydra { -namespace topology { -using hydra_topology::GvdVisualizerConfig; +using hydra_ros::GvdVisualizerConfig; using RqtMutexPtr = std::unique_ptr; struct TopologyVisualizerConfig { @@ -78,32 +77,33 @@ class TopologyServerVisualizer { virtual ~TopologyServerVisualizer() = default; void visualize(const SceneGraphLayer& graph, - const GvdGraph& gvd_graph, - const Layer& gvd, - const Layer& tsdf, + const places::GvdGraph& gvd_graph, + const voxblox::Layer& gvd, + const voxblox::Layer& tsdf, uint64_t timestamp_ns, - const MeshLayer* mesh = nullptr); + const voxblox::MeshLayer* mesh = nullptr); - void visualizeError(const Layer& lhs, - const Layer& rhs, + void visualizeError(const voxblox::Layer& lhs, + const voxblox::Layer& rhs, double threshold, uint64_t timestamp_ns); void visualizeExtractor(uint64_t timestamp_ns, - const CompressionGraphExtractor& extractor); + const places::CompressionGraphExtractor& extractor); private: void visualizeGraph(const std_msgs::Header& header, const SceneGraphLayer& graph); - void visualizeGvd(const std_msgs::Header& header, const Layer& gvd) const; + void visualizeGvd(const std_msgs::Header& header, + const voxblox::Layer& gvd) const; void visualizeGvdGraph(const std_msgs::Header& header, - const GvdGraph& gvd_graph) const; + const places::GvdGraph& gvd_graph) const; void visualizeBlocks(const std_msgs::Header& header, - const Layer& gvd, - const Layer& tsdf, - const MeshLayer* mesh) const; + const voxblox::Layer& gvd, + const voxblox::Layer& tsdf, + const voxblox::MeshLayer* mesh) const; void publishGraphLabels(const std_msgs::Header& header, const SceneGraphLayer& graph); @@ -142,7 +142,6 @@ class TopologyServerVisualizer { std::unique_ptr> colormap_server_; }; -} // namespace topology } // namespace hydra -DECLARE_CONFIG_OSTREAM_OPERATOR(hydra::topology, TopologyVisualizerConfig) +DECLARE_CONFIG_OSTREAM_OPERATOR(hydra, TopologyVisualizerConfig) diff --git a/hydra_dsg_builder_ros/include/hydra_dsg_builder_ros/visualizer_plugins.h b/hydra_ros/include/hydra_ros/visualizer/visualizer_plugins.h similarity index 96% rename from hydra_dsg_builder_ros/include/hydra_dsg_builder_ros/visualizer_plugins.h rename to hydra_ros/include/hydra_ros/visualizer/visualizer_plugins.h index cca4bf48..fc9f0248 100644 --- a/hydra_dsg_builder_ros/include/hydra_dsg_builder_ros/visualizer_plugins.h +++ b/hydra_ros/include/hydra_ros/visualizer/visualizer_plugins.h @@ -33,14 +33,13 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include "hydra_dsg_builder/minimum_spanning_tree.h" - -#include -#include +#include #include - #include +#include "hydra_ros/visualizer/dsg_visualizer_plugin.h" +#include "hydra_ros/visualizer/visualizer_types.h" + namespace hydra { struct PMGraphPluginConfig { diff --git a/hydra_utils/include/hydra_utils/visualizer_types.h b/hydra_ros/include/hydra_ros/visualizer/visualizer_types.h similarity index 85% rename from hydra_utils/include/hydra_utils/visualizer_types.h rename to hydra_ros/include/hydra_ros/visualizer/visualizer_types.h index c7743bc8..00a42914 100644 --- a/hydra_utils/include/hydra_utils/visualizer_types.h +++ b/hydra_ros/include/hydra_ros/visualizer/visualizer_types.h @@ -33,19 +33,18 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include "hydra_utils/dsg_types.h" - -#include -#include -#include -#include +#include +#include +#include +#include +#include namespace hydra { -using LayerConfig = hydra_utils::LayerVisualizerConfig; -using DynamicLayerConfig = hydra_utils::DynamicLayerVisualizerConfig; -using VisualizerConfig = hydra_utils::VisualizerConfig; -using ColormapConfig = hydra_utils::ColormapConfig; +using LayerConfig = hydra_ros::LayerVisualizerConfig; +using DynamicLayerConfig = hydra_ros::DynamicLayerVisualizerConfig; +using VisualizerConfig = hydra_ros::VisualizerConfig; +using ColormapConfig = hydra_ros::ColormapConfig; using NodeColor = SemanticNodeAttributes::ColorVector; inline double getZOffset(double z_offset_scale, diff --git a/hydra_utils/include/hydra_utils/visualizer_utils.h b/hydra_ros/include/hydra_ros/visualizer/visualizer_utilities.h similarity index 99% rename from hydra_utils/include/hydra_utils/visualizer_utils.h rename to hydra_ros/include/hydra_ros/visualizer/visualizer_utilities.h index 698cdcbb..5a889c7d 100644 --- a/hydra_utils/include/hydra_utils/visualizer_utils.h +++ b/hydra_ros/include/hydra_ros/visualizer/visualizer_utilities.h @@ -33,11 +33,11 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include "hydra_utils/visualizer_types.h" - #include #include +#include "hydra_ros/visualizer/visualizer_types.h" + namespace hydra { using ColorFunction = std::function; diff --git a/hydra_dsg_builder_ros/launch/mp3d.launch b/hydra_ros/launch/datasets/mp3d.launch similarity index 76% rename from hydra_dsg_builder_ros/launch/mp3d.launch rename to hydra_ros/launch/datasets/mp3d.launch index 0ee709d5..6a442a68 100644 --- a/hydra_dsg_builder_ros/launch/mp3d.launch +++ b/hydra_ros/launch/datasets/mp3d.launch @@ -18,17 +18,17 @@ - + - - + + - + - + diff --git a/hydra_dsg_builder_ros/launch/sidpac.launch b/hydra_ros/launch/datasets/sidpac.launch similarity index 60% rename from hydra_dsg_builder_ros/launch/sidpac.launch rename to hydra_ros/launch/datasets/sidpac.launch index c82cccd1..5dfe51ec 100644 --- a/hydra_dsg_builder_ros/launch/sidpac.launch +++ b/hydra_ros/launch/datasets/sidpac.launch @@ -11,19 +11,19 @@ - + - + - + - - - + + + - + diff --git a/hydra_dsg_builder_ros/launch/simmons_a1.launch b/hydra_ros/launch/datasets/simmons_a1.launch similarity index 73% rename from hydra_dsg_builder_ros/launch/simmons_a1.launch rename to hydra_ros/launch/datasets/simmons_a1.launch index 031eb072..69f70c8d 100644 --- a/hydra_dsg_builder_ros/launch/simmons_a1.launch +++ b/hydra_ros/launch/datasets/simmons_a1.launch @@ -15,20 +15,20 @@ - - + - - + + - + - + diff --git a/hydra_dsg_builder_ros/launch/simmons_jackal.launch b/hydra_ros/launch/datasets/simmons_jackal.launch similarity index 72% rename from hydra_dsg_builder_ros/launch/simmons_jackal.launch rename to hydra_ros/launch/datasets/simmons_jackal.launch index a137c56c..f72c6678 100644 --- a/hydra_dsg_builder_ros/launch/simmons_jackal.launch +++ b/hydra_ros/launch/datasets/simmons_jackal.launch @@ -16,19 +16,19 @@ - - + - - + + - + - + diff --git a/hydra_dsg_builder_ros/launch/uhumans2.launch b/hydra_ros/launch/datasets/uhumans2.launch similarity index 66% rename from hydra_dsg_builder_ros/launch/uhumans2.launch rename to hydra_ros/launch/datasets/uhumans2.launch index aa96e750..a35285df 100644 --- a/hydra_dsg_builder_ros/launch/uhumans2.launch +++ b/hydra_ros/launch/datasets/uhumans2.launch @@ -18,19 +18,19 @@ - + - + - + - - - + + + - + diff --git a/hydra_dsg_builder_ros/launch/hydra.launch b/hydra_ros/launch/hydra.launch similarity index 88% rename from hydra_dsg_builder_ros/launch/hydra.launch rename to hydra_ros/launch/hydra.launch index 59c75d3d..c96a3288 100644 --- a/hydra_dsg_builder_ros/launch/hydra.launch +++ b/hydra_ros/launch/hydra.launch @@ -8,7 +8,7 @@ - + @@ -25,10 +25,8 @@ - - + + @@ -38,7 +36,7 @@ - + @@ -48,8 +46,8 @@ - - + + @@ -59,7 +57,7 @@ - + @@ -80,8 +78,8 @@ - - + - + - + diff --git a/hydra_dsg_builder_ros/launch/hydra_robot.launch b/hydra_ros/launch/hydra_robot.launch similarity index 71% rename from hydra_dsg_builder_ros/launch/hydra_robot.launch rename to hydra_ros/launch/hydra_robot.launch index a07f175c..680bbc59 100644 --- a/hydra_dsg_builder_ros/launch/hydra_robot.launch +++ b/hydra_ros/launch/hydra_robot.launch @@ -19,25 +19,25 @@ - - + - - + + - + + args="-O $(arg dsg_path)/hydra_output.bag /hydra_ros_node/dsg /hydra_ros_node/dsg_mesh $(arg extra_bag_topics)"/> diff --git a/hydra_dsg_builder_ros/launch/hydra_robot_t265.launch b/hydra_ros/launch/hydra_robot_t265.launch similarity index 63% rename from hydra_dsg_builder_ros/launch/hydra_robot_t265.launch rename to hydra_ros/launch/hydra_robot_t265.launch index 1bb58e77..68f89d40 100644 --- a/hydra_dsg_builder_ros/launch/hydra_robot_t265.launch +++ b/hydra_ros/launch/hydra_robot_t265.launch @@ -19,27 +19,27 @@ - - + - + - - + + - + - + + args="-O $(arg dsg_path)/hydra_output.bag /hydra_ros_node/dsg /hydra_ros_node/dsg_mesh $(arg extra_bag_topics)"/> diff --git a/hydra_dsg_builder_ros/launch/dsg_optimizer.launch b/hydra_ros/launch/misc/dsg_optimizer.launch similarity index 80% rename from hydra_dsg_builder_ros/launch/dsg_optimizer.launch rename to hydra_ros/launch/misc/dsg_optimizer.launch index c179c98b..42df1e71 100644 --- a/hydra_dsg_builder_ros/launch/dsg_optimizer.launch +++ b/hydra_ros/launch/misc/dsg_optimizer.launch @@ -1,10 +1,10 @@ - + - + @@ -21,18 +21,18 @@ - + - - + diff --git a/hydra_topology/launch/gvd_validator.launch b/hydra_ros/launch/places/gvd_validator.launch similarity index 100% rename from hydra_topology/launch/gvd_validator.launch rename to hydra_ros/launch/places/gvd_validator.launch diff --git a/hydra_topology/launch/hydra_topology.launch b/hydra_ros/launch/places/hydra_topology.launch similarity index 100% rename from hydra_topology/launch/hydra_topology.launch rename to hydra_ros/launch/places/hydra_topology.launch diff --git a/hydra_topology/launch/hydra_topology_sidpac.launch b/hydra_ros/launch/places/hydra_topology_sidpac.launch similarity index 73% rename from hydra_topology/launch/hydra_topology_sidpac.launch rename to hydra_ros/launch/places/hydra_topology_sidpac.launch index 36c9881c..3a5f9ec9 100644 --- a/hydra_topology/launch/hydra_topology_sidpac.launch +++ b/hydra_ros/launch/places/hydra_topology_sidpac.launch @@ -18,11 +18,11 @@ - + - - - + + + diff --git a/hydra_topology/launch/hydra_topology_simmons_jackal.launch b/hydra_ros/launch/places/hydra_topology_simmons_jackal.launch similarity index 67% rename from hydra_topology/launch/hydra_topology_simmons_jackal.launch rename to hydra_ros/launch/places/hydra_topology_simmons_jackal.launch index 0dd53b3f..cd54d8f4 100644 --- a/hydra_topology/launch/hydra_topology_simmons_jackal.launch +++ b/hydra_ros/launch/places/hydra_topology_simmons_jackal.launch @@ -14,11 +14,11 @@ - + - - - + + + diff --git a/hydra_topology/launch/hydra_topology_uhumans2.launch b/hydra_ros/launch/places/hydra_topology_uhumans2.launch similarity index 75% rename from hydra_topology/launch/hydra_topology_uhumans2.launch rename to hydra_ros/launch/places/hydra_topology_uhumans2.launch index 7aca9aae..d41b47ee 100644 --- a/hydra_topology/launch/hydra_topology_uhumans2.launch +++ b/hydra_ros/launch/places/hydra_topology_uhumans2.launch @@ -20,11 +20,11 @@ - + - - - + + + diff --git a/hydra_utils/launch/data_sources/a1_pointcloud.launch b/hydra_ros/launch/utils/data_sources/a1_pointcloud.launch similarity index 87% rename from hydra_utils/launch/data_sources/a1_pointcloud.launch rename to hydra_ros/launch/utils/data_sources/a1_pointcloud.launch index 4a6cc744..fe45bd70 100644 --- a/hydra_utils/launch/data_sources/a1_pointcloud.launch +++ b/hydra_ros/launch/utils/data_sources/a1_pointcloud.launch @@ -20,10 +20,10 @@ - - - + @@ -16,7 +16,7 @@ - - + diff --git a/hydra_utils/launch/data_sources/jackal_pointcloud.launch b/hydra_ros/launch/utils/data_sources/jackal_pointcloud.launch similarity index 88% rename from hydra_utils/launch/data_sources/jackal_pointcloud.launch rename to hydra_ros/launch/utils/data_sources/jackal_pointcloud.launch index 3357cef5..d017f626 100644 --- a/hydra_utils/launch/data_sources/jackal_pointcloud.launch +++ b/hydra_ros/launch/utils/data_sources/jackal_pointcloud.launch @@ -21,10 +21,10 @@ - - - - - + - - + - + diff --git a/hydra_utils/launch/gt_pose/sidpac_gt_pose.launch b/hydra_ros/launch/utils/gt_pose/sidpac_gt_pose.launch similarity index 86% rename from hydra_utils/launch/gt_pose/sidpac_gt_pose.launch rename to hydra_ros/launch/utils/gt_pose/sidpac_gt_pose.launch index 22d2f5fc..5b3541bc 100644 --- a/hydra_utils/launch/gt_pose/sidpac_gt_pose.launch +++ b/hydra_ros/launch/utils/gt_pose/sidpac_gt_pose.launch @@ -5,7 +5,7 @@ - + diff --git a/hydra_utils/launch/hydra_streaming_visualizer.launch b/hydra_ros/launch/utils/hydra_streaming_visualizer.launch similarity index 70% rename from hydra_utils/launch/hydra_streaming_visualizer.launch rename to hydra_ros/launch/utils/hydra_streaming_visualizer.launch index 2e422591..57a82171 100644 --- a/hydra_utils/launch/hydra_streaming_visualizer.launch +++ b/hydra_ros/launch/utils/hydra_streaming_visualizer.launch @@ -1,37 +1,37 @@ - - - - + + + + - + - + - + - + - diff --git a/hydra_utils/launch/hydra_visualizer.launch b/hydra_ros/launch/utils/hydra_visualizer.launch similarity index 74% rename from hydra_utils/launch/hydra_visualizer.launch rename to hydra_ros/launch/utils/hydra_visualizer.launch index 15476e74..a340f9f6 100644 --- a/hydra_utils/launch/hydra_visualizer.launch +++ b/hydra_ros/launch/utils/hydra_visualizer.launch @@ -3,10 +3,10 @@ - + - + @@ -15,10 +15,10 @@ - - diff --git a/hydra_utils/launch/includes/a1_static_tfs.xml b/hydra_ros/launch/utils/includes/a1_static_tfs.xml similarity index 100% rename from hydra_utils/launch/includes/a1_static_tfs.xml rename to hydra_ros/launch/utils/includes/a1_static_tfs.xml diff --git a/hydra_utils/launch/includes/a1_t265_camera_static_tfs.xml b/hydra_ros/launch/utils/includes/a1_t265_camera_static_tfs.xml similarity index 100% rename from hydra_utils/launch/includes/a1_t265_camera_static_tfs.xml rename to hydra_ros/launch/utils/includes/a1_t265_camera_static_tfs.xml diff --git a/hydra_utils/launch/includes/a1_t265_static_tfs.xml b/hydra_ros/launch/utils/includes/a1_t265_static_tfs.xml similarity index 90% rename from hydra_utils/launch/includes/a1_t265_static_tfs.xml rename to hydra_ros/launch/utils/includes/a1_t265_static_tfs.xml index 28a0ff7e..6d217fcf 100644 --- a/hydra_utils/launch/includes/a1_t265_static_tfs.xml +++ b/hydra_ros/launch/utils/includes/a1_t265_static_tfs.xml @@ -15,6 +15,6 @@ - + diff --git a/hydra_utils/launch/includes/hydra_visualizer_base.xml b/hydra_ros/launch/utils/includes/hydra_visualizer_base.xml similarity index 80% rename from hydra_utils/launch/includes/hydra_visualizer_base.xml rename to hydra_ros/launch/utils/includes/hydra_visualizer_base.xml index b3956de0..37b93428 100644 --- a/hydra_utils/launch/includes/hydra_visualizer_base.xml +++ b/hydra_ros/launch/utils/includes/hydra_visualizer_base.xml @@ -1,18 +1,18 @@ - + - + - diff --git a/hydra_utils/launch/includes/hydra_visualizer_params.xml b/hydra_ros/launch/utils/includes/hydra_visualizer_params.xml similarity index 100% rename from hydra_utils/launch/includes/hydra_visualizer_params.xml rename to hydra_ros/launch/utils/includes/hydra_visualizer_params.xml diff --git a/hydra_utils/launch/includes/hydra_visualizer_rviz.xml b/hydra_ros/launch/utils/includes/hydra_visualizer_rviz.xml similarity index 79% rename from hydra_utils/launch/includes/hydra_visualizer_rviz.xml rename to hydra_ros/launch/utils/includes/hydra_visualizer_rviz.xml index da75e5a3..c26894cc 100644 --- a/hydra_utils/launch/includes/hydra_visualizer_rviz.xml +++ b/hydra_ros/launch/utils/includes/hydra_visualizer_rviz.xml @@ -5,6 +5,6 @@ + args="-d $(find hydra_ros)/rviz/hydra_visualizer.rviz"/> diff --git a/hydra_utils/launch/includes/rgbd_to_pointcloud.xml b/hydra_ros/launch/utils/includes/rgbd_to_pointcloud.xml similarity index 100% rename from hydra_utils/launch/includes/rgbd_to_pointcloud.xml rename to hydra_ros/launch/utils/includes/rgbd_to_pointcloud.xml diff --git a/hydra_utils/launch/includes/sparkal1_static_tfs.xml b/hydra_ros/launch/utils/includes/sparkal1_static_tfs.xml similarity index 100% rename from hydra_utils/launch/includes/sparkal1_static_tfs.xml rename to hydra_ros/launch/utils/includes/sparkal1_static_tfs.xml diff --git a/hydra_utils/launch/includes/sparkal2_static_tfs.xml b/hydra_ros/launch/utils/includes/sparkal2_static_tfs.xml similarity index 100% rename from hydra_utils/launch/includes/sparkal2_static_tfs.xml rename to hydra_ros/launch/utils/includes/sparkal2_static_tfs.xml diff --git a/hydra_utils/launch/includes/uhumans2_static_tfs.xml b/hydra_ros/launch/utils/includes/uhumans2_static_tfs.xml similarity index 100% rename from hydra_utils/launch/includes/uhumans2_static_tfs.xml rename to hydra_ros/launch/utils/includes/uhumans2_static_tfs.xml diff --git a/hydra_utils/launch/odom_to_tf.launch b/hydra_ros/launch/utils/odom_to_tf.launch similarity index 76% rename from hydra_utils/launch/odom_to_tf.launch rename to hydra_ros/launch/utils/odom_to_tf.launch index a00b0a71..a99eaaf6 100644 --- a/hydra_utils/launch/odom_to_tf.launch +++ b/hydra_ros/launch/utils/odom_to_tf.launch @@ -3,7 +3,7 @@ - + diff --git a/hydra_utils/launch/publish_rotation_transform.launch b/hydra_ros/launch/utils/publish_rotation_transform.launch similarity index 88% rename from hydra_utils/launch/publish_rotation_transform.launch rename to hydra_ros/launch/utils/publish_rotation_transform.launch index 655823f2..55670b14 100644 --- a/hydra_utils/launch/publish_rotation_transform.launch +++ b/hydra_ros/launch/utils/publish_rotation_transform.launch @@ -7,7 +7,7 @@ - + diff --git a/hydra_utils/launch/scene_graph_logger.launch b/hydra_ros/launch/utils/scene_graph_logger.launch similarity index 62% rename from hydra_utils/launch/scene_graph_logger.launch rename to hydra_ros/launch/utils/scene_graph_logger.launch index 517f0a9a..0efdd896 100644 --- a/hydra_utils/launch/scene_graph_logger.launch +++ b/hydra_ros/launch/utils/scene_graph_logger.launch @@ -2,10 +2,10 @@ - - + + - diff --git a/hydra_utils/nodelet_plugins.xml b/hydra_ros/nodelet_plugins.xml similarity index 100% rename from hydra_utils/nodelet_plugins.xml rename to hydra_ros/nodelet_plugins.xml diff --git a/hydra_utils/package.xml b/hydra_ros/package.xml similarity index 77% rename from hydra_utils/package.xml rename to hydra_ros/package.xml index 98f3bd63..8afd85bf 100644 --- a/hydra_utils/package.xml +++ b/hydra_ros/package.xml @@ -1,8 +1,8 @@ - hydra_utils - 0.0.1 - Utility library for hydra + hydra_ros + 1.0.0 + hydra ros nodes Nathan Hughes Yun Chang @@ -13,18 +13,26 @@ catkin roscpp dynamic_reconfigure + std_msgs geometry_msgs - gtsam + visualization_msgs + hydra_msgs + pcl_ros + tf2_eigen + tf2_ros cv_bridge nodelet image_transport - kimera_pgmo - pose_graph_tools - spark_dsg - tf2_eigen - visualization_msgs - tf2_ros glog_catkin + + gtsam + spark_dsg + pose_graph_tools + kimera_pgmo + kimera_semantics_ros + voxblox_ros + hydra + image_proc depth_image_proc rviz diff --git a/hydra_utils/resources/a1_center_mask_roi_640.png b/hydra_ros/resources/a1_center_mask_roi_640.png similarity index 100% rename from hydra_utils/resources/a1_center_mask_roi_640.png rename to hydra_ros/resources/a1_center_mask_roi_640.png diff --git a/hydra_utils/resources/a1_left_mask_roi.png b/hydra_ros/resources/a1_left_mask_roi.png similarity index 100% rename from hydra_utils/resources/a1_left_mask_roi.png rename to hydra_ros/resources/a1_left_mask_roi.png diff --git a/hydra_dsg_builder_ros/rviz/a1.rviz b/hydra_ros/rviz/a1.rviz similarity index 100% rename from hydra_dsg_builder_ros/rviz/a1.rviz rename to hydra_ros/rviz/a1.rviz diff --git a/hydra_dsg_builder_ros/rviz/a1_topdown.rviz b/hydra_ros/rviz/a1_topdown.rviz similarity index 100% rename from hydra_dsg_builder_ros/rviz/a1_topdown.rviz rename to hydra_ros/rviz/a1_topdown.rviz diff --git a/hydra_dsg_builder_ros/rviz/dsg_optimizer.rviz b/hydra_ros/rviz/dsg_optimizer.rviz similarity index 100% rename from hydra_dsg_builder_ros/rviz/dsg_optimizer.rviz rename to hydra_ros/rviz/dsg_optimizer.rviz diff --git a/hydra_topology/rviz/validator.rviz b/hydra_ros/rviz/gvd_validator.rviz similarity index 100% rename from hydra_topology/rviz/validator.rviz rename to hydra_ros/rviz/gvd_validator.rviz diff --git a/hydra_utils/rviz/hydra_streaming_visualizer.rviz b/hydra_ros/rviz/hydra_streaming_visualizer.rviz similarity index 100% rename from hydra_utils/rviz/hydra_streaming_visualizer.rviz rename to hydra_ros/rviz/hydra_streaming_visualizer.rviz diff --git a/hydra_utils/rviz/hydra_streaming_visualizer_topdown.rviz b/hydra_ros/rviz/hydra_streaming_visualizer_topdown.rviz similarity index 100% rename from hydra_utils/rviz/hydra_streaming_visualizer_topdown.rviz rename to hydra_ros/rviz/hydra_streaming_visualizer_topdown.rviz diff --git a/hydra_utils/rviz/hydra_visualizer.rviz b/hydra_ros/rviz/hydra_visualizer.rviz similarity index 100% rename from hydra_utils/rviz/hydra_visualizer.rviz rename to hydra_ros/rviz/hydra_visualizer.rviz diff --git a/hydra_dsg_builder_ros/rviz/mp3d.rviz b/hydra_ros/rviz/mp3d.rviz similarity index 100% rename from hydra_dsg_builder_ros/rviz/mp3d.rviz rename to hydra_ros/rviz/mp3d.rviz diff --git a/hydra_dsg_builder_ros/rviz/sidpac.rviz b/hydra_ros/rviz/sidpac.rviz similarity index 100% rename from hydra_dsg_builder_ros/rviz/sidpac.rviz rename to hydra_ros/rviz/sidpac.rviz diff --git a/hydra_dsg_builder_ros/rviz/sidpac_topdown.rviz b/hydra_ros/rviz/sidpac_topdown.rviz similarity index 100% rename from hydra_dsg_builder_ros/rviz/sidpac_topdown.rviz rename to hydra_ros/rviz/sidpac_topdown.rviz diff --git a/hydra_dsg_builder_ros/rviz/sparkal1.rviz b/hydra_ros/rviz/sparkal1.rviz similarity index 100% rename from hydra_dsg_builder_ros/rviz/sparkal1.rviz rename to hydra_ros/rviz/sparkal1.rviz diff --git a/hydra_dsg_builder_ros/rviz/sparkal2.rviz b/hydra_ros/rviz/sparkal2.rviz similarity index 100% rename from hydra_dsg_builder_ros/rviz/sparkal2.rviz rename to hydra_ros/rviz/sparkal2.rviz diff --git a/hydra_dsg_builder_ros/rviz/uhumans2.rviz b/hydra_ros/rviz/uhumans2.rviz similarity index 85% rename from hydra_dsg_builder_ros/rviz/uhumans2.rviz rename to hydra_ros/rviz/uhumans2.rviz index 3beeecbc..07faad17 100644 --- a/hydra_dsg_builder_ros/rviz/uhumans2.rviz +++ b/hydra_ros/rviz/uhumans2.rviz @@ -4,9 +4,12 @@ Panels: Name: Displays Property Tree Widget: Expanded: - - /Global Options1 + - /Scene Graph1/Scene Graph (Static)1 + - /Scene Graph1/Mesh (Voxblox)1 + - /Scene Graph1/Mesh (TriangleMesh)1 + - /Input1 Splitter Ratio: 0.5 - Tree Height: 441 + Tree Height: 140 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -22,10 +25,9 @@ Panels: Name: Views Splitter Ratio: 0.5 - Class: rviz/Time - Experimental: false Name: Time SyncMode: 0 - SyncSource: "" + SyncSource: Semantics Preferences: PromptSaveOnExit: true Toolbars: @@ -60,15 +62,18 @@ Visualization Manager: Namespaces: interlayer_edges_3_2: true interlayer_edges_4_3: true + interlayer_edges_5_4: true layer_bounding_boxes_2: true layer_bounding_boxes_edges_2: true layer_edges_3: true layer_edges_4: true layer_labels_2: true layer_labels_4: true + layer_labels_5: true layer_nodes_2: true layer_nodes_3: true layer_nodes_4: true + layer_nodes_5: true Queue Size: 100 Value: true - Class: rviz/MarkerArray @@ -84,6 +89,7 @@ Visualization Manager: - Class: voxblox_rviz_plugin/VoxbloxMesh Enabled: true Name: Mesh (Voxblox) + Queue Size: 10 Topic: /hydra_dsg_visualizer/dsg_mesh/voxblox Unreliable: false Value: true @@ -126,9 +132,7 @@ Visualization Manager: Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 4096 Min Color: 0; 0; 0 - Min Intensity: 0 Name: Input Pointcloud Position Transformer: XYZ Queue Size: 10 @@ -142,7 +146,7 @@ Visualization Manager: Use rainbow: true Value: false - Class: rviz/Image - Enabled: false + Enabled: true Image Topic: /tesse/depth_cam/mono/image_raw Max Value: 1 Median window: 300 @@ -152,9 +156,9 @@ Visualization Manager: Queue Size: 2 Transport Hint: raw Unreliable: false - Value: false + Value: true - Class: rviz/Image - Enabled: false + Enabled: true Image Topic: /tesse/seg_cam/rgb/image_raw Max Value: 1 Median window: 5 @@ -164,9 +168,9 @@ Visualization Manager: Queue Size: 2 Transport Hint: raw Unreliable: false - Value: false + Value: true - Class: rviz/Image - Enabled: false + Enabled: true Image Topic: /tesse/left_cam/rgb/image_raw Max Value: 1 Median window: 5 @@ -176,7 +180,7 @@ Visualization Manager: Queue Size: 2 Transport Hint: raw Unreliable: false - Value: false + Value: true Enabled: true Name: Input - Class: rviz/Group @@ -271,35 +275,35 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 36.48575973510742 + Distance: 87.75440979003906 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false + Field of View: 0.7853981852531433 Focal Point: - X: 5.443012237548828 - Y: -3.0196070671081543 - Z: 7.406179904937744 + X: 8.067428588867188 + Y: 6.879875183105469 + Z: -1.1174240112304688 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.47479772567749023 + Pitch: 0.6197975873947144 Target Frame: - Value: Orbit (rviz) - Yaw: 2.4403927326202393 + Yaw: 2.470393180847168 Saved: ~ Window Geometry: Depth: collapsed: false Displays: collapsed: false - Height: 1543 + Height: 1016 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000001560000056bfc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d000001c9000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a004400650070007400680000000134000000730000001600fffffffb0000001200530065006d0061006e0074006900630073000000003d000002ac0000001600fffffffb00000006005200470042000000003d0000056b0000001600ffffff000000010000010f0000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000035a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000075f0000003efc0100000002fb0000000800540069006d006501000000000000075f000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000075f0000056900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001560000035efc020000000bfb0000001200530065006c0065006300740069006f006e000000003b0000018f0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000000c7000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a004400650070007400680100000108000000bd0000001600fffffffb0000001200530065006d0061006e007400690063007301000001cb000000dc0000001600fffffffb0000000600520047004201000002ad000000ec0000001600ffffff000000010000010f0000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000035a000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000030700fffffffb0000000800540069006d00650100000000000004500000000000000000000006240000035e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 RGB: collapsed: false Selection: @@ -312,6 +316,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1887 - X: 67 - Y: 27 \ No newline at end of file + Width: 1920 + X: 0 + Y: 27 diff --git a/hydra_dsg_builder_ros/rviz/uhumans2_topdown.rviz b/hydra_ros/rviz/uhumans2_topdown.rviz similarity index 100% rename from hydra_dsg_builder_ros/rviz/uhumans2_topdown.rviz rename to hydra_ros/rviz/uhumans2_topdown.rviz diff --git a/hydra_dsg_builder_ros/scripts/copy_static_tfs.py b/hydra_ros/scripts/copy_static_tfs.py similarity index 100% rename from hydra_dsg_builder_ros/scripts/copy_static_tfs.py rename to hydra_ros/scripts/copy_static_tfs.py diff --git a/hydra_dsg_builder_ros/scripts/decompress_uhumans2.py b/hydra_ros/scripts/decompress_uhumans2.py similarity index 100% rename from hydra_dsg_builder_ros/scripts/decompress_uhumans2.py rename to hydra_ros/scripts/decompress_uhumans2.py diff --git a/hydra_dsg_builder_ros/scripts/download_uhumans2.py b/hydra_ros/scripts/download_uhumans2.py similarity index 100% rename from hydra_dsg_builder_ros/scripts/download_uhumans2.py rename to hydra_ros/scripts/download_uhumans2.py diff --git a/hydra_dsg_builder_ros/scripts/run_robot.sh b/hydra_ros/scripts/run_robot.sh similarity index 100% rename from hydra_dsg_builder_ros/scripts/run_robot.sh rename to hydra_ros/scripts/run_robot.sh diff --git a/hydra_dsg_builder_ros/scripts/run_robot_t265.sh b/hydra_ros/scripts/run_robot_t265.sh similarity index 100% rename from hydra_dsg_builder_ros/scripts/run_robot_t265.sh rename to hydra_ros/scripts/run_robot_t265.sh diff --git a/hydra_ros/scripts/scrape_kimera_extrinsics.py b/hydra_ros/scripts/scrape_kimera_extrinsics.py new file mode 100755 index 00000000..21504ffe --- /dev/null +++ b/hydra_ros/scripts/scrape_kimera_extrinsics.py @@ -0,0 +1,81 @@ +#!/usr/bin/env python3 +# Copyright 2022, Massachusetts Institute of Technology. +# All Rights Reserved +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +# WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +# +# Research was sponsored by the United States Air Force Research Laboratory and +# the United States Air Force Artificial Intelligence Accelerator and was +# accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views +# and conclusions contained in this document are those of the authors and should +# not be interpreted as representing the official policies, either expressed or +# implied, of the United States Air Force or the U.S. Government. The U.S. +# Government is authorized to reproduce and distribute reprints for Government +# purposes notwithstanding any copyright notation herein. +from scipy.spatial.transform.rotation import Rotation as R +import numpy as np +import pathlib +import yaml +import sys + + +def main(): + """Scrape TF from kimera config and write to hydra format.""" + if len(sys.argv) < 3: + print("missing required arguments!", file=sys.stderr) + print( + "usage: scrape_kimera_extrinsics.py PATH_TO_KIMERA_CONIFG PATH_TO_OUTPUT", + file=sys.stderr, + ) + sys.exit(1) + + input_path = pathlib.Path(sys.argv[1]).expanduser().absolute() + output_path = pathlib.Path(sys.argv[2]).expanduser().absolute() + + with input_path.open("r") as fin: + contents = [x for x in fin] + # skip first line + if "%YAML:1.0" in contents[0]: + contents_str = "\n".join(contents[1:]) + else: + contents_str = "\n".join(contents) + config = yaml.load(contents_str, Loader=yaml.Loader) + + body_T_sensor = np.array([float(x) for x in config["T_BS"]["data"]]).reshape((4, 4)) + body_t_sensor = body_T_sensor[:3, 3] + body_R_sensor = R.from_matrix(body_T_sensor[:3, :3]).as_quat().tolist() + + to_write = { + "body_t_camera": body_t_sensor.tolist(), + "body_R_camera": { + "w": body_R_sensor[3], + "x": body_R_sensor[0], + "y": body_R_sensor[1], + "z": body_R_sensor[2], + }, + } + with output_path.open("w") as fout: + fout.write(yaml.dump(to_write)) + + +if __name__ == "__main__": + main() diff --git a/hydra_utils/src/ros_parser.cpp b/hydra_ros/src/config/ros_parser.cpp similarity index 98% rename from hydra_utils/src/ros_parser.cpp rename to hydra_ros/src/config/ros_parser.cpp index b42f0349..7c297846 100644 --- a/hydra_utils/src/ros_parser.cpp +++ b/hydra_ros/src/config/ros_parser.cpp @@ -32,7 +32,7 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_utils/ros_parser.h" +#include "hydra_ros/config/ros_parser.h" #include diff --git a/hydra_dsg_builder_ros/src/dsg_optimizer_node.cpp b/hydra_ros/src/nodes/dsg_optimizer_node.cpp similarity index 91% rename from hydra_dsg_builder_ros/src/dsg_optimizer_node.cpp rename to hydra_ros/src/nodes/dsg_optimizer_node.cpp index 91aab7b2..412851fa 100644 --- a/hydra_dsg_builder_ros/src/dsg_optimizer_node.cpp +++ b/hydra_ros/src/nodes/dsg_optimizer_node.cpp @@ -32,15 +32,15 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include -#include -#include -#include -#include #include +#include "hydra_ros/config/ros_utilities.h" +#include "hydra_ros/pipeline/ros_backend.h" +#include "hydra_ros/pipeline/ros_frontend.h" +#include "hydra_ros/visualizer/dsg_mesh_plugins.h" +#include "hydra_ros/visualizer/dynamic_scene_graph_visualizer.h" + namespace hydra { -namespace incremental { struct DsgOptimizer { DsgOptimizer(const ros::NodeHandle& node_handle) @@ -77,11 +77,11 @@ struct DsgOptimizer { void do_optimize() { // TODO(nathan) maybe pull robot id from somewhere RobotPrefixConfig prefix(0); - auto config = load_config(nh); + auto config = load_config(nh); auto pgmo_config = load_config(nh, "pgmo"); SharedModuleState::Ptr state(new SharedModuleState()); - backend.reset( - new DsgBackend(prefix, config, pgmo_config, frontend_dsg, backend_dsg, state)); + backend.reset(new BackendModule( + prefix, config, pgmo_config, frontend_dsg, backend_dsg, state)); LOG(ERROR) << "Loading backend state!"; backend->loadState(frontend_filepath, dgrf_filepath); LOG(ERROR) << "Loaded backend state!"; @@ -128,13 +128,12 @@ struct DsgOptimizer { SharedDsgInfo::Ptr frontend_dsg; SharedDsgInfo::Ptr backend_dsg; - DsgBackend::Ptr backend; + BackendModule::Ptr backend; std::unique_ptr visualizer; ros::ServiceServer optimize_service; }; -} // namespace incremental } // namespace hydra int main(int argc, char* argv[]) { @@ -149,7 +148,7 @@ int main(int argc, char* argv[]) { ros::NodeHandle nh("~"); nh.setParam("dsg/call_update_periodically", false); - hydra::incremental::DsgOptimizer optimizer(nh); + hydra::DsgOptimizer optimizer(nh); optimizer.run(); return 0; diff --git a/hydra_topology/src/gvd_validator.cpp b/hydra_ros/src/nodes/gvd_validator_node.cpp similarity index 97% rename from hydra_topology/src/gvd_validator.cpp rename to hydra_ros/src/nodes/gvd_validator_node.cpp index 81c1d085..9062c5e5 100644 --- a/hydra_topology/src/gvd_validator.cpp +++ b/hydra_ros/src/nodes/gvd_validator_node.cpp @@ -1,4 +1,4 @@ -/* +/* ----------------------------------------------------------------------------- * Copyright 2022 Massachusetts Institute of Technology. * All Rights Reserved * @@ -35,7 +35,9 @@ #include #include #include -#include +#include +#include +#include #include #include #include @@ -45,12 +47,12 @@ #include #include #include +#include +#include #include -#include "hydra_topology/combo_integrator.h" -#include "hydra_topology/configs.h" -#include "hydra_topology/topology_server_visualizer.h" +#include "hydra_ros/visualizer/topology_server_visualizer.h" DEFINE_string(config, "", "gvd integrator yaml config"); DEFINE_string(output_path, "", "output directory"); @@ -61,9 +63,9 @@ DEFINE_double(voxel_size, 0.1, "voxel size"); using config_parser::ConfigVisitor; using config_parser::YamlParser; using config_parser::YamlParserImpl; -using hydra::topology::ComboIntegrator; -using hydra::topology::GvdVoxel; -using hydra::topology::TopologyServerVisualizer; +using hydra::TopologyServerVisualizer; +using hydra::places::ComboIntegrator; +using hydra::places::GvdVoxel; using sensor_msgs::Image; using voxblox::BlockIndexList; using voxblox::Layer; @@ -76,7 +78,7 @@ using Policy = message_filters::sync_policies::ApproximateTime; using TimeSync = message_filters::Synchronizer; using TsdfConfig = voxblox::TsdfIntegratorBase::Config; using MeshConfig = voxblox::MeshIntegratorConfig; -using GvdConfig = hydra::topology::GvdIntegratorConfig; +using GvdConfig = hydra::places::GvdIntegratorConfig; struct ComparisonResult { size_t num_missing_lhs = 0; diff --git a/hydra_dsg_builder_ros/src/incremental_dsg_builder_node.cpp b/hydra_ros/src/nodes/hydra_node.cpp similarity index 92% rename from hydra_dsg_builder_ros/src/incremental_dsg_builder_node.cpp rename to hydra_ros/src/nodes/hydra_node.cpp index 8ad6d1a1..4f7ed383 100644 --- a/hydra_dsg_builder_ros/src/incremental_dsg_builder_node.cpp +++ b/hydra_ros/src/nodes/hydra_node.cpp @@ -32,16 +32,16 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_dsg_builder_ros/hydra_ros_pipeline.h" -#include "hydra_dsg_builder_ros/node_utilities.h" +#include -#include -#include +#include "hydra_ros/config/ros_utilities.h" +#include "hydra_ros/pipeline/hydra_ros_pipeline.h" +#include "hydra_ros/utils/node_utilities.h" using hydra::timing::ElapsedTimeRecorder; int main(int argc, char* argv[]) { - ros::init(argc, argv, "incremental_dsg_builder_node"); + ros::init(argc, argv, "hydra_node"); FLAGS_minloglevel = 3; FLAGS_logtostderr = 1; diff --git a/hydra_topology/src/hydra_topology_node.cpp b/hydra_ros/src/nodes/hydra_topology_node.cpp similarity index 94% rename from hydra_topology/src/hydra_topology_node.cpp rename to hydra_ros/src/nodes/hydra_topology_node.cpp index ab3e31d5..090d22ad 100644 --- a/hydra_topology/src/hydra_topology_node.cpp +++ b/hydra_ros/src/nodes/hydra_topology_node.cpp @@ -32,17 +32,16 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_topology/ros_reconstruction.h" - -#include -#include - #include +#include + +#include "hydra_ros/config/ros_utilities.h" +#include "hydra_ros/pipeline/ros_reconstruction.h" DECLARE_CONFIG_OSTREAM_OPERATOR(hydra, LogConfig); namespace hydra { DECLARE_STRUCT_NAME(LogConfig); -} // namespace hydra +} // namespace hydra int main(int argc, char* argv[]) { ros::init(argc, argv, "hydra_topology_node"); diff --git a/hydra_utils/src/hydra_visualizer_node.cpp b/hydra_ros/src/nodes/hydra_visualizer_node.cpp similarity index 96% rename from hydra_utils/src/hydra_visualizer_node.cpp rename to hydra_ros/src/nodes/hydra_visualizer_node.cpp index 13ceed0f..de4eb443 100644 --- a/hydra_utils/src/hydra_visualizer_node.cpp +++ b/hydra_ros/src/nodes/hydra_visualizer_node.cpp @@ -33,21 +33,18 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #include +#include #include #include #include -#include "hydra_utils/config.h" -#include "hydra_utils/dsg_mesh_plugins.h" -#include "hydra_utils/dsg_streaming_interface.h" -#include "hydra_utils/dynamic_scene_graph_visualizer.h" -#include "hydra_utils/timing_utilities.h" +#include "hydra_ros/config/ros_utilities.h" +#include "hydra_ros/utils/dsg_streaming_interface.h" +#include "hydra_ros/visualizer/dsg_mesh_plugins.h" +#include "hydra_ros/visualizer/dynamic_scene_graph_visualizer.h" using DsgVisualizer = hydra::DynamicSceneGraphVisualizer; -using hydra::PgmoMeshPlugin; -using hydra::RvizMeshPlugin; -using hydra::VoxbloxMeshPlugin; using spark_dsg::getDefaultLayerIds; namespace hydra { diff --git a/hydra_utils/src/rotate_tf_node.cpp b/hydra_ros/src/nodes/rotate_tf_node.cpp similarity index 100% rename from hydra_utils/src/rotate_tf_node.cpp rename to hydra_ros/src/nodes/rotate_tf_node.cpp diff --git a/hydra_utils/src/scene_graph_logger_node.cpp b/hydra_ros/src/nodes/scene_graph_logger_node.cpp similarity index 97% rename from hydra_utils/src/scene_graph_logger_node.cpp rename to hydra_ros/src/nodes/scene_graph_logger_node.cpp index 2720a68f..459de875 100644 --- a/hydra_utils/src/scene_graph_logger_node.cpp +++ b/hydra_ros/src/nodes/scene_graph_logger_node.cpp @@ -32,12 +32,12 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_utils/dsg_streaming_interface.h" - #include #include +#include "hydra_ros/utils/dsg_streaming_interface.h" + namespace hydra { struct SceneGraphLoggerNode { @@ -88,7 +88,7 @@ struct SceneGraphLoggerNode { } // namespace hydra int main(int argc, char** argv) { - ros::init(argc, argv, "dsg_visualizer_node"); + ros::init(argc, argv, "scene_graph_logger_node"); ros::NodeHandle nh("~"); hydra::SceneGraphLoggerNode node(nh); diff --git a/hydra_dsg_builder_ros/src/hydra_ros_pipeline.cpp b/hydra_ros/src/pipeline/hydra_ros_pipeline.cpp similarity index 89% rename from hydra_dsg_builder_ros/src/hydra_ros_pipeline.cpp rename to hydra_ros/src/pipeline/hydra_ros_pipeline.cpp index 1332aa0e..b722ea9d 100644 --- a/hydra_dsg_builder_ros/src/hydra_ros_pipeline.cpp +++ b/hydra_ros/src/pipeline/hydra_ros_pipeline.cpp @@ -32,10 +32,10 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_dsg_builder_ros/hydra_ros_pipeline.h" +#include "hydra_ros/pipeline/hydra_ros_pipeline.h" -#include "hydra_dsg_builder_ros/ros_lcd_registration.h" -#include "hydra_utils/ros_utilities.h" +#include "hydra_ros/config/ros_utilities.h" +#include "hydra_ros/pipeline/ros_lcd_registration.h" namespace hydra { @@ -49,10 +49,10 @@ void visit_config(const Visitor& v, HydraRosConfig& config) { } DECLARE_STRUCT_NAME(HydraRosConfig); -DECLARE_STRUCT_NAME(incremental::DsgFrontendConfig); -DECLARE_STRUCT_NAME(incremental::DsgBackendConfig); +DECLARE_STRUCT_NAME(FrontendConfig); +DECLARE_STRUCT_NAME(BackendConfig); +DECLARE_STRUCT_NAME(LoopClosureConfig); DECLARE_STRUCT_NAME(kimera_pgmo::KimeraPgmoConfig); -DECLARE_STRUCT_NAME(DsgLcdModuleConfig); } // namespace hydra @@ -60,10 +60,6 @@ DECLARE_CONFIG_OSTREAM_OPERATOR(hydra, HydraRosConfig) namespace hydra { -using incremental::DsgFrontendConfig; -using incremental::DsgLcd; -using incremental::LcdInput; - HydraRosPipeline::HydraRosPipeline(const ros::NodeHandle& node_handle, int robot_id) : nh(node_handle), prefix(robot_id) { config = load_config(nh); @@ -80,8 +76,8 @@ HydraRosPipeline::HydraRosPipeline(const ros::NodeHandle& node_handle, int robot shared_state.reset(new SharedModuleState()); if (config.do_reconstruction) { - const auto frontend_config = load_config(nh); - frontend = std::make_shared( + const auto frontend_config = load_config(nh); + frontend = std::make_shared( prefix, frontend_config, frontend_dsg, shared_state); reconstruction = std::make_shared(nh, prefix, frontend->getQueue()); @@ -104,13 +100,13 @@ HydraRosPipeline::HydraRosPipeline(const ros::NodeHandle& node_handle, int robot std::placeholders::_3)); } - const auto backend_config = load_config(nh); + const auto backend_config = load_config(nh); if (config.use_ros_backend) { backend = std::make_shared( nh, prefix, frontend_dsg, backend_dsg, shared_state); } else { const auto pgmo_config = load_config(nh, "pgmo"); - backend = std::make_shared( + backend = std::make_shared( prefix, backend_config, pgmo_config, frontend_dsg, backend_dsg, shared_state); } @@ -123,12 +119,12 @@ HydraRosPipeline::HydraRosPipeline(const ros::NodeHandle& node_handle, int robot std::placeholders::_3)); if (config.enable_lcd) { - auto lcd_config = load_config(nh, ""); + auto lcd_config = load_config(nh, ""); lcd_config.detector.num_semantic_classes = frontend->maxSemanticLabel(); VLOG(1) << "Number of classes for LCD: " << lcd_config.detector.num_semantic_classes; shared_state->lcd_queue.reset(new InputQueue()); - lcd.reset(new DsgLcd(prefix, lcd_config, frontend_dsg, shared_state)); + lcd.reset(new LoopClosureModule(prefix, lcd_config, frontend_dsg, shared_state)); bow_sub = nh.subscribe("bow_vectors", 100, &HydraRosPipeline::bowCallback, this); if (lcd_config.detector.enable_agent_registration) { @@ -200,10 +196,9 @@ void HydraRosPipeline::save(const std::string& output_path) { } } -void HydraRosPipeline::sendFrontendOutput( - const DynamicSceneGraph& graph, - const incremental::BackendInput& backend_input, - uint64_t timestamp_ns) { +void HydraRosPipeline::sendFrontendOutput(const DynamicSceneGraph& graph, + const BackendInput& backend_input, + uint64_t timestamp_ns) { if (backend_input.deformation_graph) { mesh_graph_pub.publish(*backend_input.deformation_graph); } diff --git a/hydra_dsg_builder_ros/src/ros_backend.cpp b/hydra_ros/src/pipeline/ros_backend.cpp similarity index 93% rename from hydra_dsg_builder_ros/src/ros_backend.cpp rename to hydra_ros/src/pipeline/ros_backend.cpp index 4b37cf71..e2badbd1 100644 --- a/hydra_dsg_builder_ros/src/ros_backend.cpp +++ b/hydra_ros/src/pipeline/ros_backend.cpp @@ -32,9 +32,9 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_dsg_builder_ros/ros_backend.h" +#include "hydra_ros/pipeline/ros_backend.h" -#include +#include "hydra_ros/config/ros_utilities.h" namespace hydra { @@ -48,12 +48,12 @@ RosBackend::RosBackend(const ros::NodeHandle& nh, const SharedDsgInfo::Ptr& dsg, const SharedDsgInfo::Ptr& backend_dsg, const SharedModuleState::Ptr& state) - : DsgBackend(prefix, - load_config(nh), - load_config(nh, "pgmo"), - dsg, - backend_dsg, - state), + : BackendModule(prefix, + load_config(nh), + load_config(nh, "pgmo"), + dsg, + backend_dsg, + state), nh_(nh) { pose_graph_sub_ = nh_.subscribe( "pose_graph_incremental", 10000, &RosBackend::poseGraphCallback, this); @@ -72,7 +72,7 @@ void RosBackend::inputCallback(const KimeraPgmoMesh::ConstPtr& mesh, latest_mesh_msg_ = mesh; have_new_mesh_ = true; - incremental::BackendInput::Ptr input(new incremental::BackendInput()); + auto input = std::make_shared(); input->deformation_graph = deformation_graph; input->timestamp_ns = mesh->header.stamp.toNSec(); input->pose_graphs = pose_graph_queue_; @@ -88,7 +88,7 @@ void RosBackend::poseGraphCallback(const PoseGraph::ConstPtr& msg) { // TODO(nathan) copy mesh message into MeshStruct RosBackendVisualizer::RosBackendVisualizer(const ros::NodeHandle& nh, - const DsgBackendConfig& config, + const BackendConfig& config, const RobotPrefixConfig& prefix) : nh_(nh), config_(config), prefix_(prefix), last_zmq_pub_time_(0) { mesh_mesh_edges_pub_ = diff --git a/hydra_dsg_builder_ros/src/ros_frontend.cpp b/hydra_ros/src/pipeline/ros_frontend.cpp similarity index 96% rename from hydra_dsg_builder_ros/src/ros_frontend.cpp rename to hydra_ros/src/pipeline/ros_frontend.cpp index 2a8b9fbc..14c16b2c 100644 --- a/hydra_dsg_builder_ros/src/ros_frontend.cpp +++ b/hydra_ros/src/pipeline/ros_frontend.cpp @@ -32,9 +32,9 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_dsg_builder_ros/ros_frontend.h" +#include "hydra_ros/pipeline/ros_frontend.h" -#include +#include "hydra_ros/config/ros_utilities.h" namespace hydra { @@ -44,8 +44,7 @@ RosFrontend::RosFrontend(const ros::NodeHandle& nh, const RobotPrefixConfig& prefix, const SharedDsgInfo::Ptr& dsg, const SharedModuleState::Ptr& state) - : DsgFrontend(prefix, load_config(nh), dsg, state), - nh_(nh) { + : FrontendModule(prefix, load_config(nh), dsg, state), nh_(nh) { ros_config_ = load_config(nh); pose_graph_sub_ = nh_.subscribe( @@ -79,6 +78,7 @@ RosFrontend::~RosFrontend() { segmented_vertices_pub_.reset(); } void RosFrontend::inputCallback(const ActiveLayer::ConstPtr& places, const ActiveMesh::ConstPtr& mesh) { VLOG(5) << "Received input @ " << places->header.stamp.toNSec() << " [ns]"; + LOG(FATAL) << "ROS input for frontend not implemented currently"; ReconstructionOutput::Ptr input(new ReconstructionOutput()); @@ -104,8 +104,8 @@ void RosFrontend::inputCallback(const ActiveLayer::ConstPtr& places, input->current_position = *latest_position; } - input->places = places; - input->mesh = mesh; + //input->places = places; + //input->mesh = mesh; input->timestamp_ns = places->header.stamp.toNSec(); // send all cached messages to frontend diff --git a/hydra_dsg_builder_ros/src/ros_lcd_registration.cpp b/hydra_ros/src/pipeline/ros_lcd_registration.cpp similarity index 98% rename from hydra_dsg_builder_ros/src/ros_lcd_registration.cpp rename to hydra_ros/src/pipeline/ros_lcd_registration.cpp index 0384b2bb..de5bb44b 100644 --- a/hydra_dsg_builder_ros/src/ros_lcd_registration.cpp +++ b/hydra_ros/src/pipeline/ros_lcd_registration.cpp @@ -32,9 +32,9 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_dsg_builder_ros/ros_lcd_registration.h" +#include "hydra_ros/pipeline/ros_lcd_registration.h" -#include +#include #include #include #include diff --git a/hydra_topology/src/ros_reconstruction.cpp b/hydra_ros/src/pipeline/ros_reconstruction.cpp similarity index 92% rename from hydra_topology/src/ros_reconstruction.cpp rename to hydra_ros/src/pipeline/ros_reconstruction.cpp index 06364505..d9a73331 100644 --- a/hydra_topology/src/ros_reconstruction.cpp +++ b/hydra_ros/src/pipeline/ros_reconstruction.cpp @@ -32,15 +32,19 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_topology/ros_reconstruction.h" +#include "hydra_ros/pipeline/ros_reconstruction.h" #include +#include +#include +#include #include -#include #include +#include #include +#include -#include "hydra_topology/gvd_integrator.h" +#include "hydra_ros/config/ros_utilities.h" namespace hydra { @@ -51,7 +55,7 @@ using pose_graph_tools::PoseGraph; using pose_graph_tools::PoseGraphEdge; using pose_graph_tools::PoseGraphNode; using RosPointcloud = RosReconstruction::Pointcloud; -using topology::CompressionGraphExtractor; +using places::CompressionGraphExtractor; inline geometry_msgs::Pose tfToPose(const geometry_msgs::Transform& transform) { geometry_msgs::Pose pose; @@ -136,8 +140,7 @@ RosReconstruction::RosReconstruction(const ros::NodeHandle& nh, } if (ros_config_.visualize_reconstruction) { - visualizer_.reset( - new topology::TopologyServerVisualizer(ros_config_.topology_visualizer_ns)); + visualizer_.reset(new TopologyServerVisualizer(ros_config_.topology_visualizer_ns)); } if (ros_config_.publish_mesh) { @@ -171,7 +174,8 @@ void RosReconstruction::handlePointcloud(const RosPointcloud::ConstPtr& cloud) { ros::Time curr_time; curr_time.fromNSec(cloud->header.stamp * 1000); - VLOG(1) << "[Hydra Reconstruction] Got raw pointcloud input @ " << curr_time.toNSec() << " [ns]"; + VLOG(1) << "[Hydra Reconstruction] Got raw pointcloud input @ " << curr_time.toNSec() + << " [ns]"; if (num_poses_received_ > 0) { if (last_time_received_ && ((curr_time - *last_time_received_).toSec() < ros_config_.pointcloud_separation_s)) { @@ -209,7 +213,8 @@ void RosReconstruction::pointcloudSpin() { ros::Time curr_time; curr_time.fromNSec(cloud->header.stamp * 1000); - VLOG(1) << "[Hydra Reconstruction] popped pointcloud input @ " << curr_time.toNSec() << " [ns]"; + VLOG(1) << "[Hydra Reconstruction] popped pointcloud input @ " << curr_time.toNSec() + << " [ns]"; ros::WallRate tf_wait_rate(1.0 / ros_config_.tf_wait_duration_s); @@ -219,10 +224,10 @@ void RosReconstruction::pointcloudSpin() { std::string err_str; for (size_t i = 0; i < 5; ++i) { if (buffer_->canTransform(config_.world_frame, - config_.robot_frame, - curr_time, - ros::Duration(0), - &err_str)) { + config_.robot_frame, + curr_time, + ros::Duration(0), + &err_str)) { have_transform = true; break; } @@ -276,7 +281,9 @@ void RosReconstruction::pointcloudSpin() { void RosReconstruction::visualize(const ReconstructionOutput& output) { if (ros_config_.publish_mesh && output.mesh) { - mesh_pub_.publish(output.mesh->mesh); + hydra_msgs::ActiveMesh::ConstPtr msg(new hydra_msgs::ActiveMesh()); + auto mesh = const_cast(*msg); + mesh_pub_.publish(msg); } if (visualizer_) { diff --git a/hydra_utils/src/dsg_streaming_interface.cpp b/hydra_ros/src/utils/dsg_streaming_interface.cpp similarity index 94% rename from hydra_utils/src/dsg_streaming_interface.cpp rename to hydra_ros/src/utils/dsg_streaming_interface.cpp index e549d8a0..ecbee564 100644 --- a/hydra_utils/src/dsg_streaming_interface.cpp +++ b/hydra_ros/src/utils/dsg_streaming_interface.cpp @@ -32,11 +32,11 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_utils/dsg_streaming_interface.h" -#include "hydra_utils/display_utils.h" -#include "hydra_utils/dsg_types.h" -#include "hydra_utils/timing_utilities.h" +#include "hydra_ros/utils/dsg_streaming_interface.h" +#include +#include +#include #include #include @@ -89,7 +89,7 @@ void DsgSender::sendGraph(const DynamicSceneGraph& graph, mesh_msgs::TriangleMeshStamped msg; msg.header.stamp.fromNSec(timestamp_ns); - msg.header.frame_id="world"; + msg.header.frame_id = "world"; msg.mesh = kimera_pgmo::PolygonMeshToTriangleMeshMsg(*graph.getMeshVertices(), *graph.getMeshFaces()); mesh_pub_.publish(msg); @@ -116,8 +116,7 @@ void DsgReceiver::handleUpdate(const hydra_msgs::DsgUpdate::ConstPtr& msg) { (*log_callback_)(msg->header.stamp, msg->layer_contents.size()); } - const auto size_bytes = - hydra_utils::getHumanReadableMemoryString(msg->layer_contents.size()); + const auto size_bytes = getHumanReadableMemoryString(msg->layer_contents.size()); ROS_INFO_STREAM("Received dsg update message of " << size_bytes); try { if (!graph_) { diff --git a/hydra_utils/src/mask_nodelet.cpp b/hydra_ros/src/utils/mask_nodelet.cpp similarity index 100% rename from hydra_utils/src/mask_nodelet.cpp rename to hydra_ros/src/utils/mask_nodelet.cpp diff --git a/hydra_dsg_builder_ros/src/node_utilities.cpp b/hydra_ros/src/utils/node_utilities.cpp similarity index 98% rename from hydra_dsg_builder_ros/src/node_utilities.cpp rename to hydra_ros/src/utils/node_utilities.cpp index df8ebba7..90596b04 100644 --- a/hydra_dsg_builder_ros/src/node_utilities.cpp +++ b/hydra_ros/src/utils/node_utilities.cpp @@ -32,10 +32,10 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_dsg_builder_ros/node_utilities.h" +#include "hydra_ros/utils/node_utilities.h" -#include #include +#include namespace hydra { diff --git a/hydra_utils/src/colormap_utils.cpp b/hydra_ros/src/visualizer/colormap_utilities.cpp similarity index 98% rename from hydra_utils/src/colormap_utils.cpp rename to hydra_ros/src/visualizer/colormap_utilities.cpp index 6e9dc8d8..96344fb4 100644 --- a/hydra_utils/src/colormap_utils.cpp +++ b/hydra_ros/src/visualizer/colormap_utilities.cpp @@ -32,7 +32,7 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_utils/colormap_utils.h" +#include "hydra_ros/visualizer/colormap_utilities.h" #include #include diff --git a/hydra_utils/src/dsg_mesh_plugins.cpp b/hydra_ros/src/visualizer/dsg_mesh_plugins.cpp similarity index 99% rename from hydra_utils/src/dsg_mesh_plugins.cpp rename to hydra_ros/src/visualizer/dsg_mesh_plugins.cpp index fd4af500..b3e5ec64 100644 --- a/hydra_utils/src/dsg_mesh_plugins.cpp +++ b/hydra_ros/src/visualizer/dsg_mesh_plugins.cpp @@ -32,13 +32,12 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_utils/dsg_mesh_plugins.h" +#include "hydra_ros/visualizer/dsg_mesh_plugins.h" #include -#include - #include #include +#include #include #include diff --git a/hydra_utils/src/dynamic_scene_graph_visualizer.cpp b/hydra_ros/src/visualizer/dynamic_scene_graph_visualizer.cpp similarity index 98% rename from hydra_utils/src/dynamic_scene_graph_visualizer.cpp rename to hydra_ros/src/visualizer/dynamic_scene_graph_visualizer.cpp index a80f48ed..b88a8cd9 100644 --- a/hydra_utils/src/dynamic_scene_graph_visualizer.cpp +++ b/hydra_ros/src/visualizer/dynamic_scene_graph_visualizer.cpp @@ -32,12 +32,12 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_utils/dynamic_scene_graph_visualizer.h" +#include "hydra_ros/visualizer/dynamic_scene_graph_visualizer.h" #include -#include "hydra_utils/colormap_utils.h" -#include "hydra_utils/visualizer_utils.h" +#include "hydra_ros/visualizer/colormap_utilities.h" +#include "hydra_ros/visualizer/visualizer_utilities.h" namespace hydra { @@ -63,8 +63,7 @@ void clearPrevMarkers(const std_msgs::Header& header, } DynamicSceneGraphVisualizer::DynamicSceneGraphVisualizer( - const ros::NodeHandle& nh, - const DynamicSceneGraph::LayerIds& layer_ids) + const ros::NodeHandle& nh, const DynamicSceneGraph::LayerIds& layer_ids) : nh_(nh), need_redraw_(false), periodic_redraw_(false), diff --git a/hydra_topology/src/gvd_visualization_utilities.cpp b/hydra_ros/src/visualizer/gvd_visualization_utilities.cpp similarity index 98% rename from hydra_topology/src/gvd_visualization_utilities.cpp rename to hydra_ros/src/visualizer/gvd_visualization_utilities.cpp index 05c6d1e7..b1265497 100644 --- a/hydra_topology/src/gvd_visualization_utilities.cpp +++ b/hydra_ros/src/visualizer/gvd_visualization_utilities.cpp @@ -32,18 +32,25 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_topology/gvd_visualization_utilities.h" +#include "hydra_ros/visualizer/gvd_visualization_utilities.h" -#include #include #include -using visualization_msgs::Marker; -using visualization_msgs::MarkerArray; +#include "hydra_ros/visualizer/colormap_utilities.h" namespace hydra { -namespace topology { + +using places::GvdGraph; +using places::GvdVoxel; +using visualization_msgs::Marker; +using visualization_msgs::MarkerArray; +using voxblox::BlockIndexList; +using voxblox::FloatingPoint; +using voxblox::Layer; +using voxblox::MeshLayer; +using voxblox::TsdfVoxel; MarkerGroupPub::MarkerGroupPub(const ros::NodeHandle& nh) : nh_(nh) {} @@ -725,5 +732,4 @@ MarkerArray makePlaceSpheres(const std_msgs::Header& header, return spheres; } -} // namespace topology } // namespace hydra diff --git a/hydra_topology/src/topology_server_visualizer.cpp b/hydra_ros/src/visualizer/topology_server_visualizer.cpp similarity index 95% rename from hydra_topology/src/topology_server_visualizer.cpp rename to hydra_ros/src/visualizer/topology_server_visualizer.cpp index c7924b60..68e932c5 100644 --- a/hydra_topology/src/topology_server_visualizer.cpp +++ b/hydra_ros/src/visualizer/topology_server_visualizer.cpp @@ -32,16 +32,23 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_topology/topology_server_visualizer.h" +#include "hydra_ros/visualizer/topology_server_visualizer.h" -#include +#include + +#include "hydra_ros/config/ros_utilities.h" namespace hydra { -namespace topology { +using places::CompressionGraphExtractor; +using places::GvdGraph; +using places::GvdVoxel; using timing::ScopedTimer; using visualization_msgs::Marker; using visualization_msgs::MarkerArray; +using voxblox::Layer; +using voxblox::MeshLayer; +using voxblox::TsdfVoxel; TopologyServerVisualizer::TopologyServerVisualizer(const std::string& ns) : nh_(ns), previous_spheres_(0), published_gvd_graph_(false) { @@ -75,8 +82,7 @@ void TopologyServerVisualizer::visualize(const SceneGraphLayer& graph, } void TopologyServerVisualizer::visualizeExtractor( - uint64_t timestamp_ns, - const CompressionGraphExtractor& extractor) { + uint64_t timestamp_ns, const CompressionGraphExtractor& extractor) { std_msgs::Header header; header.frame_id = config_.world_frame; header.stamp.fromNSec(timestamp_ns); @@ -281,13 +287,10 @@ void TopologyServerVisualizer::publishFreespace(const std_msgs::Header& header, freespace_conf.use_sphere_marker = false; freespace_conf.marker_scale = 0.08; freespace_conf.marker_alpha = 0.5; - Marker node_marker = - makeCentroidMarkers(header, - freespace_conf, - graph, - config_.graph, - node_ns, - [](const auto&) { return NodeColor::Zero(); }); + Marker node_marker = makeCentroidMarkers( + header, freespace_conf, graph, config_.graph, node_ns, [](const auto&) { + return NodeColor::Zero(); + }); markers.markers.push_back(node_marker); if (!graph.edges().empty()) { @@ -381,5 +384,4 @@ void TopologyServerVisualizer::setupConfigServers() { "visualizer_colormap", colormap_server_, &TopologyServerVisualizer::colormapCb); } -} // namespace topology } // namespace hydra diff --git a/hydra_dsg_builder_ros/src/visualizer_plugins.cpp b/hydra_ros/src/visualizer/visualizer_plugins.cpp similarity index 98% rename from hydra_dsg_builder_ros/src/visualizer_plugins.cpp rename to hydra_ros/src/visualizer/visualizer_plugins.cpp index 68d033fa..754a2047 100644 --- a/hydra_dsg_builder_ros/src/visualizer_plugins.cpp +++ b/hydra_ros/src/visualizer/visualizer_plugins.cpp @@ -32,18 +32,17 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_dsg_builder_ros/visualizer_plugins.h" - -#include -#include +#include "hydra_ros/visualizer/visualizer_plugins.h" #include #include - #include #include #include +#include "hydra_ros/visualizer/colormap_utilities.h" +#include "hydra_ros/visualizer/visualizer_utilities.h" + namespace hydra { using dsg_utils::makeColorMsg; diff --git a/hydra_utils/src/visualizer_utils.cpp b/hydra_ros/src/visualizer/visualizer_utilities.cpp similarity index 98% rename from hydra_utils/src/visualizer_utils.cpp rename to hydra_ros/src/visualizer/visualizer_utilities.cpp index e4db5f4d..96b99bb9 100644 --- a/hydra_utils/src/visualizer_utils.cpp +++ b/hydra_ros/src/visualizer/visualizer_utilities.cpp @@ -32,11 +32,12 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "hydra_utils/visualizer_utils.h" -#include "hydra_utils/colormap_utils.h" +#include "hydra_ros/visualizer/visualizer_utilities.h" #include +#include "hydra_ros/visualizer/colormap_utilities.h" + namespace hydra { using visualization_msgs::Marker; @@ -313,7 +314,7 @@ Marker makeTextMarker(const std_msgs::Header& header, Marker makeTextMarkerNoHeight(const std_msgs::Header& header, const LayerConfig& config, const Node& node, - const VisualizerConfig& visualizer_config, + const VisualizerConfig&, const std::string& ns) { Marker marker; marker.header = header; @@ -826,14 +827,15 @@ Marker makeDynamicCentroidMarkers(const std_msgs::Header& header, const NodeColor& color, const std::string& ns, size_t marker_id) { - return makeDynamicCentroidMarkers(header, - config, - layer, - config.z_offset_scale, - visualizer_config, - ns, - [&](const auto&) -> NodeColor { return color; }, - marker_id); + return makeDynamicCentroidMarkers( + header, + config, + layer, + config.z_offset_scale, + visualizer_config, + ns, + [&](const auto&) -> NodeColor { return color; }, + marker_id); } Marker makeDynamicCentroidMarkers(const std_msgs::Header& header, diff --git a/hydra_ros/tests/CMakeLists.txt b/hydra_ros/tests/CMakeLists.txt new file mode 100644 index 00000000..9d9ad41e --- /dev/null +++ b/hydra_ros/tests/CMakeLists.txt @@ -0,0 +1,3 @@ +find_package(rostest REQUIRED) +add_rostest_gtest(test_${PROJECT_NAME} hydra_ros.test main.cpp test_ros_config.cpp) +target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME} ${catkin_LIBRARIES}) diff --git a/hydra_ros/tests/hydra_ros.test b/hydra_ros/tests/hydra_ros.test new file mode 100644 index 00000000..a0ed194d --- /dev/null +++ b/hydra_ros/tests/hydra_ros.test @@ -0,0 +1,15 @@ + + + + + + + + + diff --git a/hydra_utils/tests/utest_main.cpp b/hydra_ros/tests/main.cpp similarity index 96% rename from hydra_utils/tests/utest_main.cpp rename to hydra_ros/tests/main.cpp index bfd73019..4e31b036 100644 --- a/hydra_utils/tests/utest_main.cpp +++ b/hydra_ros/tests/main.cpp @@ -35,9 +35,9 @@ #include #include -auto main(int argc, char **argv) -> int { +auto main(int argc, char** argv) -> int { ::testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "hydra_utest"); + ros::init(argc, argv, "test_hydra_ros"); return RUN_ALL_TESTS(); } diff --git a/hydra_ros/tests/resources/map_config.yaml b/hydra_ros/tests/resources/map_config.yaml new file mode 100644 index 00000000..b30e9ced --- /dev/null +++ b/hydra_ros/tests/resources/map_config.yaml @@ -0,0 +1,10 @@ +--- +configs: + config_a: + a: -3 + b: 5.2 + c: hello + config_b: + a: 5 + b: -1.2 + c: other diff --git a/hydra_ros/tests/resources/nested_test_config.yaml b/hydra_ros/tests/resources/nested_test_config.yaml new file mode 100644 index 00000000..823c0cd9 --- /dev/null +++ b/hydra_ros/tests/resources/nested_test_config.yaml @@ -0,0 +1,9 @@ +--- +fake_config: + foo: 10.0 + bar: 5.0 + a: -3 + b: 1 + c: 2 +msg: again +foo: 5.0 diff --git a/hydra_ros/tests/resources/test_config.yaml b/hydra_ros/tests/resources/test_config.yaml new file mode 100644 index 00000000..6ca663ef --- /dev/null +++ b/hydra_ros/tests/resources/test_config.yaml @@ -0,0 +1,17 @@ +--- +foo: 10.0 +bar: 5.0 +a: -3 +b: 1 +c: 2 +msg: world +type: GREEN +values: [4, 5, 6] +value_map: + '3': 4 + '5': 6 +vec: [7, 8, 9] +enable_map: + GREEN: true + BLUE: false +unique_values: [4, 5, 4, 4, 5, 5] diff --git a/hydra_ros/tests/test_ros_config.cpp b/hydra_ros/tests/test_ros_config.cpp new file mode 100644 index 00000000..72152a5f --- /dev/null +++ b/hydra_ros/tests/test_ros_config.cpp @@ -0,0 +1,282 @@ +/* ----------------------------------------------------------------------------- + * Copyright 2022 Massachusetts Institute of Technology. + * All Rights Reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Research was sponsored by the United States Air Force Research Laboratory and + * the United States Air Force Artificial Intelligence Accelerator and was + * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views + * and conclusions contained in this document are those of the authors and should + * not be interpreted as representing the official policies, either expressed or + * implied, of the United States Air Force or the U.S. Government. The U.S. + * Government is authorized to reproduce and distribute reprints for Government + * purposes notwithstanding any copyright notation herein. + * -------------------------------------------------------------------------- */ +#include +#include + +namespace hydra { + +enum class TestEnum { RED, GREEN, BLUE }; + +struct FakeConfig { + float foo = 5.0f; + double bar = 10.0; + int a = 1; + uint8_t b = 2; + int64_t c = -3; + std::string msg = "hello"; + std::vector values{1, 2, 3}; + std::map value_map{{"1", 2}, {"3", 4}}; + TestEnum type = TestEnum::RED; + Eigen::Matrix vec = Eigen::Matrix::Zero(); + std::map enable_map{{TestEnum::RED, true}, {TestEnum::GREEN, false}}; + std::set unique_values{1, 2, 3}; +}; + +struct FakeConfig2 { + FakeConfig fake_config; + std::string msg = "world"; +}; + +struct BarConfig { + int a = 1; + float b = 2.0f; + std::string c = "test"; +}; + +struct BarMapConfig { + std::map configs; +}; + +struct MapConverter { + MapConverter() = default; + + std::map to(const std::map& other) const; + + std::map from(const std::map& other) const; +}; + +template +void visit_config(const Visitor& v, FakeConfig& config) { + v.visit("foo", config.foo); + v.visit("bar", config.bar); + v.visit("a", config.a); + v.visit("b", config.b); + v.visit("c", config.c); + v.visit("msg", config.msg); + v.visit("values", config.values); + v.visit("value_map", config.value_map); + v.visit("type", config.type); + v.visit("vec", config.vec); + v.visit("enable_map", config.enable_map, MapConverter()); + v.visit("unique_values", config.unique_values); +} + +template +void visit_config(const Visitor& v, FakeConfig2& config) { + v.visit("fake_config", config.fake_config); + v.visit("msg", config.msg); +} + +template +void visit_config(const Visitor& v, BarConfig& config) { + v.visit("a", config.a); + v.visit("b", config.b); + v.visit("c", config.c); +} + +template +void visit_config(const Visitor& v, BarMapConfig& config) { + v.visit("configs", config.configs); +} + +struct TestLogger : config_parser::Logger { + TestLogger() = default; + + ~TestLogger() = default; + + inline void log_missing(const std::string& message) const override { + ss << message << std::endl; + } + + mutable std::stringstream ss; +}; + +} // namespace hydra + +namespace config_parser { + +template <> +struct ConfigVisitor> { + using MapType = std::map; + + template ::value, bool>::type = true> + static auto visit_config(const V& v, MapType& value) { + for (const auto& child : v.children()) { + value[child] = hydra::BarConfig(); + v.visit(child, value[child]); + } + } + + template ::value, bool>::type = true> + static auto visit_config(const V& v, MapType& value) { + v.pre_visit(); + v.post_visit(); + for (auto& kv_pair : value) { + v.visit(kv_pair.first, kv_pair.second); + } + } +}; + +} // namespace config_parser + +DECLARE_CONFIG_OSTREAM_OPERATOR(hydra, FakeConfig) +DECLARE_CONFIG_OSTREAM_OPERATOR(hydra, FakeConfig2) +DECLARE_CONFIG_OSTREAM_OPERATOR(hydra, BarConfig) +DECLARE_CONFIG_OSTREAM_OPERATOR(hydra, BarMapConfig) + +DECLARE_CONFIG_ENUM(hydra, + TestEnum, + {TestEnum::RED, "RED"}, + {TestEnum::GREEN, "GREEN"}, + {TestEnum::BLUE, "BLUE"}) + +namespace hydra { + +std::map MapConverter::to( + const std::map& other) const { + std::map to_return; + for (const auto& kv_pair : other) { + // defined by enum macro + TestEnum new_enum; + readConfigEnumFromString(kv_pair.first, new_enum); + to_return[new_enum] = kv_pair.second; + } + return to_return; +} + +std::map MapConverter::from( + const std::map& other) const { + std::map to_return; + for (const auto& kv_pair : other) { + std::stringstream ss; + ss << configEnumToString(kv_pair.first); + to_return[ss.str()] = kv_pair.second; + } + + return to_return; +} + +TEST(ConfigParser, RosName) { + config_parser::RosParserImpl parser(ros::NodeHandle("/plain_test_config")); + EXPECT_EQ(parser.name(), "/plain_test_config"); + + auto new_parser = parser.child("foo"); + EXPECT_EQ(new_parser.name(), "/plain_test_config/foo"); +} + +TEST(ConfigParsing, RosChildren) { + config_parser::RosParserImpl parser(ros::NodeHandle("/plain_test_config")); + auto children = parser.children(); + EXPECT_EQ(children.size(), 12u); + + auto value_parser = parser.child("value_map"); + auto value_children = value_parser.children(); + EXPECT_EQ(value_children.size(), 2u); + + auto enable_parser = parser.child("enable_map"); + auto enable_children = enable_parser.children(); + EXPECT_EQ(enable_children.size(), 2u); +} + +TEST(ConfigParsing, ParseSingleStructRos) { + auto config = config_parser::load_from_ros("/plain_test_config"); + + EXPECT_EQ(config.foo, 10.0f); + EXPECT_EQ(config.bar, 5.0); + EXPECT_EQ(config.a, -3); + EXPECT_EQ(static_cast(config.b), 1); + EXPECT_EQ(config.c, 2); + EXPECT_EQ(config.msg, "world"); + + std::vector expected_values{4, 5, 6}; + EXPECT_EQ(config.values, expected_values); + + std::map expected_value_map{{"3", 4}, {"5", 6}}; + EXPECT_EQ(config.value_map, expected_value_map); + + EXPECT_EQ(config.type, TestEnum::GREEN); + + Eigen::Matrix expected_vec; + expected_vec << 7, 8, 9; + EXPECT_EQ(config.vec, expected_vec); + + std::map expected_enable_map{{TestEnum::BLUE, 0}, + {TestEnum::GREEN, 1}}; + EXPECT_EQ(config.enable_map, expected_enable_map); + + std::set expected_unique_values{4, 5}; + EXPECT_EQ(config.unique_values, expected_unique_values); +} + +TEST(ConfigParsing, ParseNestedStructRos) { + auto config = config_parser::load_from_ros("/nested_test_config"); + + EXPECT_EQ(config.fake_config.foo, 10.0f); + EXPECT_EQ(config.fake_config.bar, 5.0); + EXPECT_EQ(config.fake_config.a, -3); + EXPECT_EQ(static_cast(config.fake_config.b), 1); + EXPECT_EQ(config.fake_config.c, 2); + EXPECT_EQ(config.fake_config.msg, "hello"); + + std::vector expected_values{1, 2, 3}; + EXPECT_EQ(config.fake_config.values, expected_values); + EXPECT_EQ(config.msg, "again"); + + // make sure conversion respects the default + std::map expected_enable_map{{TestEnum::RED, 1}, + {TestEnum::GREEN, 0}}; + EXPECT_EQ(config.fake_config.enable_map, expected_enable_map); +} + +TEST(ConfigParsing, ParseMapStructRos) { + auto config = config_parser::load_from_ros("/map_test_config"); + + EXPECT_EQ(config.configs.size(), 2u); + ASSERT_TRUE(config.configs.count("config_a")); + ASSERT_TRUE(config.configs.count("config_b")); + + BarConfig config_a = config.configs["config_a"]; + EXPECT_EQ(config_a.a, -3); + EXPECT_NEAR(config_a.b, 5.2f, 1.0e-6f); + EXPECT_EQ(config_a.c, "hello"); + + BarConfig config_b = config.configs["config_b"]; + EXPECT_EQ(config_b.a, 5); + EXPECT_NEAR(config_b.b, -1.2f, 1.0e-6f); + EXPECT_EQ(config_b.c, "other"); +} + +} // namespace hydra diff --git a/hydra_topology/CMakeLists.txt b/hydra_topology/CMakeLists.txt deleted file mode 100644 index 584a1a8e..00000000 --- a/hydra_topology/CMakeLists.txt +++ /dev/null @@ -1,148 +0,0 @@ -cmake_minimum_required(VERSION 3.1) -project(hydra_topology) - -add_compile_options(-Wall -Wextra) -set(CMAKE_CXX_STANDARD 17) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - -option(HYDRA_USE_COVERAGE "Build with GCOV enabled" OFF) - -find_package(spark_dsg REQUIRED) -find_package( - catkin REQUIRED - COMPONENTS dynamic_reconfigure - hydra_utils - hydra_msgs - kimera_semantics_ros - pcl_ros - roscpp - tf2_eigen - tf2_ros - voxblox - voxblox_ros -) -find_package(Boost REQUIRED COMPONENTS filesystem) -find_package(yaml-cpp REQUIRED) - -generate_dynamic_reconfigure_options(cfg/GvdVisualizer.cfg) - -configure_file(cmake/nanoflann.CMakeLists.txt.in nanoflann-download/CMakeLists.txt) -execute_process( - COMMAND "${CMAKE_COMMAND}" -G "${CMAKE_GENERATOR}" . - WORKING_DIRECTORY "${CMAKE_BINARY_DIR}/nanoflann-download" - OUTPUT_QUIET -) -execute_process( - COMMAND "${CMAKE_COMMAND}" --build . - WORKING_DIRECTORY "${CMAKE_BINARY_DIR}/nanoflann-download" - OUTPUT_QUIET -) - -set(NANOFLANN_BUILD_EXAMPLES - OFF - CACHE INTERNAL "" -) -set(NANOFLANN_BUILD_TESTS - OFF - CACHE INTERNAL "" -) -add_subdirectory(${CMAKE_BINARY_DIR}/nanoflann-src ${CMAKE_BINARY_DIR}/nanoflann-build) - -catkin_package( - CATKIN_DEPENDS - dynamic_reconfigure - hydra_utils - hydra_msgs - kimera_semantics_ros - pcl_ros - roscpp - tf2_eigen - tf2_ros - voxblox - voxblox_ros - DEPENDS spark_dsg - INCLUDE_DIRS include - LIBRARIES ${PROJECT_NAME} -) - -add_library( - ${PROJECT_NAME} - src/compression_graph_extractor.cpp - src/floodfill_graph_extractor.cpp - src/graph_extractor_interface.cpp - src/graph_extractor_types.cpp - src/graph_extractor_utilities.cpp - src/gvd_graph.cpp - src/gvd_integrator.cpp - src/gvd_parent_tracker.cpp - src/gvd_thinning.cpp - src/gvd_utilities.cpp - src/gvd_visualization_utilities.cpp - src/gvd_voxel.cpp - src/nearest_neighbor_utilities.cpp - src/ros_reconstruction.cpp - src/reconstruction_config.cpp - src/reconstruction_module.cpp - src/topology_server_visualizer.cpp - src/update_statistics.cpp - src/voxel_aware_marching_cubes.cpp - src/voxel_aware_mesh_integrator.cpp -) -target_link_libraries( - ${PROJECT_NAME} - PUBLIC ${catkin_LIBRARIES} spark_dsg::spark_dsg - PRIVATE nanoflann::nanoflann -) -target_include_directories(${PROJECT_NAME} PUBLIC include ${catkin_INCLUDE_DIRS}) -if(HYDRA_USE_COVERAGE) - target_compile_options(${PROJECT_NAME} PRIVATE --coverage) - # TODO(nathan) once we have newer cmake, target link options - target_link_libraries(${PROJECT_NAME} PRIVATE --coverage) -endif() -add_dependencies( - ${PROJECT_NAME} ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS} -) - -add_executable(${PROJECT_NAME}_node src/hydra_topology_node.cpp) -target_link_libraries(${PROJECT_NAME}_node PUBLIC ${PROJECT_NAME} Boost::boost) -add_dependencies( - ${PROJECT_NAME}_node ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS} -) - -add_executable(gvd_validator src/gvd_validator.cpp) -target_link_libraries(gvd_validator PUBLIC ${PROJECT_NAME} yaml-cpp) - -if(CATKIN_ENABLE_TESTING) - catkin_add_gtest( - utest_${PROJECT_NAME} - tests/utest_main.cpp - tests/src/test_fixtures.cpp - tests/utest_compression_graph_extractor.cpp - tests/utest_esdf.cpp - tests/utest_floodfill_graph_extractor.cpp - tests/utest_graph_extractor_utilities.cpp - tests/utest_gvd_thinning.cpp - tests/utest_gvd_utilities.cpp - tests/utest_incremental_gvd.cpp - tests/utest_incremental_integration.cpp - tests/utest_marching_cubes.cpp - tests/utest_nearest_neighbor_utilities.cpp - tests/utest_pose_graph.cpp - ) - target_include_directories( - utest_${PROJECT_NAME} PUBLIC tests/include ${catkin_INCLUDE_DIRS} - ) - target_link_libraries(utest_${PROJECT_NAME} ${PROJECT_NAME} ${catkin_LIBRARIES}) -endif() - -install( - TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} -) - -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) diff --git a/hydra_topology/README.md b/hydra_topology/README.md deleted file mode 100644 index 20daff13..00000000 --- a/hydra_topology/README.md +++ /dev/null @@ -1,3 +0,0 @@ -### Hydra Topology - -Running unit tests: `catkin build hydra_topology --catkin-make-args tests && rosrun hydra_topology utest_hydra_topology` diff --git a/hydra_topology/cmake/nanoflann.CMakeLists.txt.in b/hydra_topology/cmake/nanoflann.CMakeLists.txt.in deleted file mode 100644 index d33080a0..00000000 --- a/hydra_topology/cmake/nanoflann.CMakeLists.txt.in +++ /dev/null @@ -1,14 +0,0 @@ -cmake_minimum_required(VERSION 3.1) - -project(nanoflann_download NONE) - -include(ExternalProject) -ExternalProject_Add(nanoflann - URL https://github.com/jlblancoc/nanoflann/archive/refs/tags/v1.4.2.zip - SOURCE_DIR "${CMAKE_CURRENT_BINARY_DIR}/nanoflann-src" - BINARY_DIR "${CMAKE_CURRENT_BINARY_DIR}/nanoflann-build" - CONFIGURE_COMMAND "" - BUILD_COMMAND "" - INSTALL_COMMAND "" - TEST_COMMAND "" -) diff --git a/hydra_topology/output/.gitignore b/hydra_topology/output/.gitignore deleted file mode 100644 index 3aac6956..00000000 --- a/hydra_topology/output/.gitignore +++ /dev/null @@ -1,3 +0,0 @@ -* -.* -!.gitignore diff --git a/hydra_topology/package.xml b/hydra_topology/package.xml deleted file mode 100644 index 3d3c6cbb..00000000 --- a/hydra_topology/package.xml +++ /dev/null @@ -1,28 +0,0 @@ - - - hydra_topology - 0.0.1 - Online GVD and topological skeleton creation - - Nathan Hughes - Nathan Hughes - - BSD - - catkin - dynamic_reconfigure - hydra_msgs - hydra_utils - kimera_semantics_ros - pcl_ros - roscpp - spark_dsg - tf2_eigen - tf2_ros - voxblox - voxblox_ros - - - - - diff --git a/hydra_topology/rviz/d455.rviz b/hydra_topology/rviz/d455.rviz deleted file mode 100644 index a376b8f7..00000000 --- a/hydra_topology/rviz/d455.rviz +++ /dev/null @@ -1,210 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /Grid1 - Splitter Ratio: 0.5 - Tree Height: 186 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: RGB -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 0.5 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: -1 - Plane: XY - Plane Cell Count: 1000 - Reference Frame: - Value: true - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /hydra_topology_node/graph_viz - Name: Places - Namespaces: - topology_graph_edges: true - topology_graph_nodes: true - Queue Size: 100 - Value: true - - Class: rviz/MarkerArray - Enabled: false - Marker Topic: /hydra_topology_node/graph_label_viz - Name: Places (Labels) - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /hydra_topology_node/gvd_viz - Name: GVD - Namespaces: - gvd_visualizer: true - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /hydra_topology_node/gvd_edge_viz - Name: GVD (Edge Debugging) - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: voxblox_rviz_plugin/VoxbloxMesh - Enabled: true - Name: Mesh - Topic: /hydra_topology_node/mesh - Unreliable: false - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /hydra_topology_node/voxel_block_viz - Name: Active Window - Namespaces: - topology_server_blocks: true - Queue Size: 100 - Value: true - - Class: rviz/Image - Enabled: true - Image Topic: /cam_d455/color/image_raw - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: RGB - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: true - - Class: rviz/Image - Enabled: true - Image Topic: /cam_d455/depth_masked/image_raw - Max Value: 1 - Median window: 300 - Min Value: 0 - Name: Depth - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: true - - Class: rviz/Image - Enabled: true - Image Topic: /semantic/image_raw - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Semantics - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Default Light: true - Fixed Frame: world - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 21.729537963867188 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 0.428592324256897 - Y: 0.31402283906936646 - Z: -0.5251080989837646 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 1.1253979206085205 - Target Frame: - Value: Orbit (rviz) - Yaw: 0.7503988742828369 - Saved: ~ -Window Geometry: - Depth: - collapsed: false - Displays: - collapsed: false - Height: 1016 - Hide Left Dock: false - Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000001560000035efc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000143000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000160000000570000000000000000fb000000060052004700420100000184000000a20000001600fffffffb0000000a00440065007000740068010000022c000000bb0000001600fffffffb0000001200530065006d0061006e007400690063007301000002ed000000ac0000001600ffffff000000010000010f0000035efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b0000035e000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000024400fffffffb0000000800540069006d00650100000000000004500000000000000000000006240000035e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - RGB: - collapsed: false - Selection: - collapsed: false - Semantics: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 1920 - X: 0 - Y: 27 diff --git a/hydra_topology/rviz/sidpac.rviz b/hydra_topology/rviz/sidpac.rviz deleted file mode 100644 index ccfeced0..00000000 --- a/hydra_topology/rviz/sidpac.rviz +++ /dev/null @@ -1,247 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Grid1 - - /TF1/Status1 - Splitter Ratio: 0.5 - Tree Height: 242 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: Semantics -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 0.5 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: -1.5 - Plane: XY - Plane Cell Count: 1000 - Reference Frame: - Value: true - - Class: rviz/TF - Enabled: true - Frame Timeout: 15 - Frames: - All Enabled: true - base_link_pgo: - Value: true - camera_accel_frame: - Value: true - camera_accel_optical_frame: - Value: true - camera_gyro_frame: - Value: true - camera_imu_optical_frame: - Value: true - camera_link: - Value: true - camera_odom_frame: - Value: true - camera_pose_frame: - Value: true - left_cam: - Value: true - world: - Value: true - Marker Scale: 1 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: true - Tree: - world: - base_link_pgo: - left_cam: - {} - Update Interval: 0 - Value: true - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /hydra_topology_node/graph_viz - Name: Places - Namespaces: - topology_graph_edges: true - topology_graph_nodes: true - Queue Size: 100 - Value: true - - Class: rviz/MarkerArray - Enabled: false - Marker Topic: /hydra_topology_node/graph_label_viz - Name: Places (Labels) - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /hydra_topology_node/gvd_viz - Name: GVD - Namespaces: - gvd_visualizer: true - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /hydra_topology_node/gvd_edge_viz - Name: GVD Edges (Debug) - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/Marker - Enabled: true - Marker Topic: /hydra_topology_node/voxel_block_viz - Name: Active Window - Namespaces: - topology_server_blocks: true - Queue Size: 100 - Value: true - - Class: voxblox_rviz_plugin/VoxbloxMesh - Enabled: true - Name: Mesh - Topic: /hydra_topology_node/mesh - Unreliable: false - Value: true - - Class: rviz/Image - Enabled: true - Image Topic: /semantic/image_raw - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Semantics - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: true - - Class: rviz/Image - Enabled: true - Image Topic: /azure/depth_to_rgb/image_raw - Max Value: 1 - Median window: 300 - Min Value: 0 - Name: Depth - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: true - - Class: rviz/Image - Enabled: true - Image Topic: /azure/rgb/image_raw - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: RGB - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Default Light: true - Fixed Frame: world - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 13.451048851013184 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: -5.022566795349121 - Y: 2.528791904449463 - Z: 0.7422441840171814 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.3953983187675476 - Target Frame: - Value: Orbit (rviz) - Yaw: 1.1653977632522583 - Saved: ~ -Window Geometry: - Depth: - collapsed: false - Displays: - collapsed: false - Height: 1016 - Hide Left Dock: false - Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000001560000035efc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000017b000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001200530065006d0061006e007400690063007301000001bc0000009b0000001600fffffffb0000000a00440065007000740068010000025d0000009c0000001600fffffffb0000000600520047004201000002ff0000009a0000001600ffffff000000010000010f0000035efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b0000035e000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000024400fffffffb0000000800540069006d00650100000000000004500000000000000000000006240000035e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - RGB: - collapsed: false - Selection: - collapsed: false - Semantics: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 1920 - X: 0 - Y: 27 diff --git a/hydra_topology/rviz/uhumans2.rviz b/hydra_topology/rviz/uhumans2.rviz deleted file mode 100644 index 7791dfe8..00000000 --- a/hydra_topology/rviz/uhumans2.rviz +++ /dev/null @@ -1,255 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - Splitter Ratio: 0.5 - Tree Height: 242 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: Input Pointcloud -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 0.5 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 1000 - Reference Frame: - Value: true - - Class: rviz/TF - Enabled: true - Frame Timeout: 15 - Frames: - All Enabled: true - base_link_gt: - Value: true - front_lidar: - Value: true - left_cam: - Value: true - rear_lidar: - Value: true - right_cam: - Value: true - world: - Value: true - Marker Scale: 1 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: true - Tree: - world: - base_link_gt: - front_lidar: - {} - left_cam: - {} - rear_lidar: - {} - right_cam: - {} - Update Interval: 0 - Value: true - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /hydra_topology_node/gvd_viz - Name: GVD - Namespaces: - gvd_visualizer: true - Queue Size: 100 - Value: true - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /hydra_topology_node/graph_viz - Name: Places - Namespaces: - topology_graph_edges: true - topology_graph_nodes: true - Queue Size: 100 - Value: true - - Class: rviz/MarkerArray - Enabled: false - Marker Topic: /hydra_topology_node/graph_label_viz - Name: Places (Labels) - Namespaces: - {} - Queue Size: 100 - Value: false - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 255; 255 - Color Transformer: RGB8 - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Min Color: 0; 0; 0 - Name: Input Pointcloud - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Flat Squares - Topic: /semantic_pointcloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: voxblox_rviz_plugin/VoxbloxMesh - Enabled: true - Name: Mesh - Topic: /hydra_topology_node/mesh_viz - Unreliable: false - Value: true - - Class: rviz/Image - Enabled: true - Image Topic: /tesse/depth_cam/mono/image_raw - Max Value: 1 - Median window: 300 - Min Value: 0 - Name: Depth - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: true - - Class: rviz/Image - Enabled: true - Image Topic: /tesse/left_cam/rgb/image_raw - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: RGB - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: true - - Class: rviz/Image - Enabled: true - Image Topic: /tesse/seg_cam/rgb/image_raw - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Semantics - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Default Light: true - Fixed Frame: world - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 32.47098159790039 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: -3.6097676753997803 - Y: 23.651647567749023 - Z: -0.2755281925201416 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.7203983068466187 - Target Frame: - Value: Orbit (rviz) - Yaw: 2.838507890701294 - Saved: ~ -Window Geometry: - Depth: - collapsed: false - Displays: - collapsed: false - Height: 1016 - Hide Left Dock: false - Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000001560000035efc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000017b000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0044006500700074006801000001bc000000950000001600fffffffb0000000600520047004201000002570000009d0000001600fffffffb0000001200530065006d0061006e007400690063007301000002fa0000009f0000001600ffffff000000010000010f0000035efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b0000035e000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000024400fffffffb0000000800540069006d00650100000000000004500000000000000000000006240000035e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - RGB: - collapsed: false - Selection: - collapsed: false - Semantics: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 1920 - X: 0 - Y: 27 diff --git a/hydra_utils/README.md b/hydra_utils/README.md deleted file mode 100644 index 84088e0c..00000000 --- a/hydra_utils/README.md +++ /dev/null @@ -1,13 +0,0 @@ -## Hydra Utilities - -Contains common code used between the different hydra packages, which primarily consists of configuration parsing. - -To build and run tests (assuming you've built the package normally): - -``` -catkin build hydra_utils --catkin-make-args tests -rostest hydra_utils hydra_utils.test -``` - -Documentation: -- [Config Parsing](doc/config_parsing.md) diff --git a/hydra_utils/config/robots/a1_t265.yaml b/hydra_utils/config/robots/a1_t265.yaml deleted file mode 100644 index ea505996..00000000 --- a/hydra_utils/config/robots/a1_t265.yaml +++ /dev/null @@ -1,2 +0,0 @@ -body_R_camera: {"w": 1.0, "x": 0.0, "y": 0.0, "z": 0.0} -body_t_camera: [0.0, 0.0, 0.0] diff --git a/hydra_utils/include/hydra_utils/dsg_types.h b/hydra_utils/include/hydra_utils/dsg_types.h deleted file mode 100644 index 43cf2d60..00000000 --- a/hydra_utils/include/hydra_utils/dsg_types.h +++ /dev/null @@ -1,11 +0,0 @@ -#pragma once -#include -#include -#include -#include - -namespace hydra { - -using namespace spark_dsg; - -} // namespace hydra diff --git a/hydra_utils/launch/visual_lcd.launch b/hydra_utils/launch/visual_lcd.launch deleted file mode 100644 index ac9a70d3..00000000 --- a/hydra_utils/launch/visual_lcd.launch +++ /dev/null @@ -1,24 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - diff --git a/hydra_utils/rviz/a1_debug.rviz b/hydra_utils/rviz/a1_debug.rviz deleted file mode 100644 index 05e5b6a8..00000000 --- a/hydra_utils/rviz/a1_debug.rviz +++ /dev/null @@ -1,278 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Status1 - - /Grid1 - - /TF1 - - /TF1/Frames1 - - /Odometry1 - - /Odometry1/Shape1 - Splitter Ratio: 0.7882353067398071 - Tree Height: 362 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: Depth Image -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 0.5 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: -1 - Plane: XY - Plane Cell Count: 1000 - Reference Frame: - Value: true - - Class: rviz/Image - Enabled: true - Image Topic: /cam_d455/aligned_depth_to_color/image_raw - Max Value: 1 - Median window: 500 - Min Value: 0 - Name: Depth Image - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: true - - Class: rviz/Image - Enabled: true - Image Topic: /cam_d455/color/image_raw - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: RGB Image - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: true - - Class: rviz/Axes - Enabled: false - Length: 1 - Name: Axes - Radius: 0.10000000149011612 - Reference Frame: - Value: false - - Class: rviz/TF - Enabled: true - Frame Timeout: 15 - Frames: - All Enabled: false - cam_d455_color_optical_frame: - Value: false - cam_imu: - Value: false - cam_t265_accel_frame: - Value: false - cam_t265_accel_optical_frame: - Value: false - cam_t265_gyro_frame: - Value: false - cam_t265_gyro_optical_frame: - Value: false - cam_t265_link: - Value: false - cam_t265_odom_frame: - Value: false - cam_t265_pose_frame: - Value: true - d455_body: - Value: false - imu_cam: - Value: false - left_cam: - Value: true - world: - Value: false - Marker Scale: 1 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: true - Tree: - world: - cam_imu: - {} - cam_t265_odom_frame: - cam_t265_pose_frame: - cam_t265_link: - cam_t265_accel_frame: - cam_t265_accel_optical_frame: - {} - d455_body: - left_cam: - cam_d455_color_optical_frame: - {} - cam_t265_gyro_frame: - cam_t265_gyro_optical_frame: - {} - imu_cam: - {} - Update Interval: 0 - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 255; 255 - Color Transformer: RGB8 - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Min Color: 0; 0; 0 - Name: PointCloud2 - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Flat Squares - Topic: /semantic_pointcloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Angle Tolerance: 0.10000000149011612 - Class: rviz/Odometry - Covariance: - Orientation: - Alpha: 0.5 - Color: 255; 255; 127 - Color Style: Unique - Frame: Local - Offset: 1 - Scale: 1 - Value: true - Position: - Alpha: 0.30000001192092896 - Color: 204; 51; 204 - Scale: 1 - Value: true - Value: false - Enabled: true - Keep: 1000 - Name: Odometry - Position Tolerance: 0.10000000149011612 - Shape: - Alpha: 1 - Axes Length: 0.20000000298023224 - Axes Radius: 0.009999999776482582 - Color: 255; 25; 0 - Head Length: 0.10000000149011612 - Head Radius: 0.05000000074505806 - Shaft Length: 0.20000000298023224 - Shaft Radius: 0.009999999776482582 - Value: Axes - Topic: /cam_t265/odom/sample - Unreliable: false - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Default Light: true - Fixed Frame: world - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 11.29726505279541 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: -4.185297966003418 - Y: -0.11884849518537521 - Z: 1.305227279663086 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.14979636669158936 - Target Frame: - Value: Orbit (rviz) - Yaw: 1.3134803771972656 - Saved: ~ -Window Geometry: - Depth Image: - collapsed: false - Displays: - collapsed: false - Height: 1016 - Hide Left Dock: false - Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000001560000035efc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000001f3000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000160044006500700074006800200049006d0061006700650100000234000000980000001600fffffffb0000001200520047004200200049006d00610067006501000002d2000000c70000001600ffffff000000010000010f0000035efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b0000035e000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000024400fffffffb0000000800540069006d00650100000000000004500000000000000000000006240000035e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - RGB Image: - collapsed: false - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 1920 - X: 0 - Y: 27 diff --git a/hydra_utils/scripts/scrape_kimera_extrinsics.py b/hydra_utils/scripts/scrape_kimera_extrinsics.py deleted file mode 100755 index dff8b837..00000000 --- a/hydra_utils/scripts/scrape_kimera_extrinsics.py +++ /dev/null @@ -1,49 +0,0 @@ -#!/usr/bin/env python3 -from scipy.spatial.transform.rotation import Rotation as R -import numpy as np -import pathlib -import yaml -import sys - - -def main(): - """Scrape TF from kimera config and write to hydra format.""" - if len(sys.argv) < 3: - print("missing required arguments!", file=sys.stderr) - print( - "usage: scrape_kimera_extrinsics.py PATH_TO_KIMERA_CONIFG PATH_TO_OUTPUT", - file=sys.stderr, - ) - sys.exit(1) - - input_path = pathlib.Path(sys.argv[1]).expanduser().absolute() - output_path = pathlib.Path(sys.argv[2]).expanduser().absolute() - - with input_path.open("r") as fin: - contents = [x for x in fin] - # skip first line - if "%YAML:1.0" in contents[0]: - contents_str = "\n".join(contents[1:]) - else: - contents_str = "\n".join(contents) - config = yaml.load(contents_str, Loader=yaml.Loader) - - body_T_sensor = np.array([float(x) for x in config["T_BS"]["data"]]).reshape((4, 4)) - body_t_sensor = body_T_sensor[:3, 3] - body_R_sensor = R.from_matrix(body_T_sensor[:3, :3]).as_quat().tolist() - - to_write = { - "body_t_camera": body_t_sensor.tolist(), - "body_R_camera": { - "w": body_R_sensor[3], - "x": body_R_sensor[0], - "y": body_R_sensor[1], - "z": body_R_sensor[2], - }, - } - with output_path.open("w") as fout: - fout.write(yaml.dump(to_write)) - - -if __name__ == "__main__": - main() diff --git a/hydra_utils/tests/hydra_utils.test b/hydra_utils/tests/hydra_utils.test deleted file mode 100644 index 42025ea2..00000000 --- a/hydra_utils/tests/hydra_utils.test +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - -