├── .clang-format ├── .docker_real_mprocs.yaml ├── .docker_virtual_mprocs.yaml ├── .github └── workflows │ ├── docs_build.yaml │ ├── foxy_build.yaml │ ├── humble_build.yaml │ ├── iron_build.yaml │ └── setup.sh ├── .gitignore ├── .gitmodules ├── .real_mprocs.yaml ├── .rosinstall_ros1 ├── .rosinstall_ros2 ├── .vscode ├── c_cpp_properties.json ├── launch.json └── settings.json ├── CHANGELOG.rst ├── LICENSE ├── README.md ├── docker ├── foxy_noetic │ └── Dockerfile └── humble │ └── Dockerfile ├── docs ├── 1.installation.md ├── 2.datasets.md ├── 3.getting_started.md ├── 4.docker.md ├── 5.ros_related.md ├── imgs │ ├── Tf-tree.png │ ├── banner.png │ ├── main_building.gif │ ├── main_building.mp4 │ ├── main_image.png │ ├── s_graphs+.gif │ ├── s_graphs_rotating.gif │ └── system_architecture.png └── index.md ├── lidar_situational_graphs ├── CMakeLists.txt ├── apps │ ├── floor_plan_node.cpp │ ├── prefiltering_node.cpp │ ├── room_segmentation_node.cpp │ ├── s_graphs_node.cpp │ └── scan_matching_odometry_node.cpp ├── cmake │ ├── FindCholmod.cmake │ └── FindG2O.cmake ├── config │ ├── prefiltering.yaml │ ├── s_graphs.yaml │ └── scan_matching.yaml ├── include │ ├── g2o │ │ ├── edge_doorway_two_rooms.hpp │ │ ├── edge_infinite_room_plane.hpp │ │ ├── edge_loop_closure.hpp │ │ ├── edge_multi_se3.hpp │ │ ├── edge_plane.hpp │ │ ├── edge_plane_identity.hpp │ │ ├── edge_plane_prior.hpp │ │ ├── edge_room.hpp │ │ ├── edge_se3_plane.hpp │ │ ├── edge_se3_point_to_plane.hpp │ │ ├── edge_se3_priorquat.hpp │ │ ├── edge_se3_priorvec.hpp │ │ ├── edge_se3_priorxy.hpp │ │ ├── edge_se3_priorxyz.hpp │ │ ├── edge_se3_two_planes.hpp │ │ ├── edge_se3_two_rooms.hpp │ │ ├── edge_wall_two_planes.hpp │ │ ├── robust_kernel_io.hpp │ │ ├── vertex_deviation.hpp │ │ ├── vertex_doorway.hpp │ │ ├── vertex_floor.hpp │ │ ├── vertex_infinite_room.hpp │ │ ├── vertex_room.hpp │ │ └── vertex_wall.hpp │ └── s_graphs │ │ ├── backend │ │ ├── floor_mapper.hpp │ │ ├── gps_mapper.hpp │ │ ├── graph_slam.hpp │ │ ├── imu_mapper.hpp │ │ ├── keyframe_mapper.hpp │ │ ├── loop_mapper.hpp │ │ ├── plane_mapper.hpp │ │ ├── room_graph_generator.hpp │ │ ├── room_mapper.hpp │ │ └── wall_mapper.hpp │ │ ├── common │ │ ├── deviations.hpp │ │ ├── door_ways.hpp │ │ ├── floors.hpp │ │ ├── graph_utils.hpp │ │ ├── infinite_rooms.hpp │ │ ├── information_matrix_calculator.hpp │ │ ├── keyframe.hpp │ │ ├── map_cloud_generator.hpp │ │ ├── nmea_sentence_parser.hpp │ │ ├── optimization_data.hpp │ │ ├── plane_utils.hpp │ │ ├── planes.hpp │ │ ├── point_types.hpp │ │ ├── registrations.hpp │ │ ├── room_utils.hpp │ │ ├── rooms.hpp │ │ ├── ros_time_hash.hpp │ │ ├── ros_utils.hpp │ │ ├── s_graphs.hpp │ │ └── walls.hpp │ │ ├── frontend │ │ ├── floor_analyzer.hpp │ │ ├── keyframe_updater.hpp │ │ ├── loop_detector.hpp │ │ ├── plane_analyzer.hpp │ │ └── room_analyzer.hpp │ │ └── visualization │ │ ├── graph_publisher.hpp │ │ └── graph_visualizer.hpp ├── launch │ ├── ros1_tf.launch │ ├── s_graphs_launch.py │ └── s_graphs_unitree_launch.py ├── lidar_situational_graphs │ ├── __init__.py │ └── map2odom_publisher.py ├── package.xml ├── rviz │ ├── s_graphs_ros1.rviz │ ├── s_graphs_ros2.rviz │ ├── s_graphs_ros2_dark.rviz │ ├── s_graphs_ros2_robot1.rviz │ ├── s_graphs_ros2_robot2.rviz │ └── s_graphs_ros2_robot3.rviz ├── src │ ├── g2o │ │ ├── edge_infinite_room_plane.cpp │ │ ├── edge_multi_se3.cpp │ │ ├── edge_plane.cpp │ │ ├── edge_plane_identity.cpp │ │ ├── edge_plane_prior.cpp │ │ ├── edge_room.cpp │ │ ├── edge_se3_plane.cpp │ │ ├── edge_se3_point_to_plane.cpp │ │ ├── edge_se3_priorquat.cpp │ │ ├── edge_se3_priorvec.cpp │ │ ├── edge_se3_priorxy.cpp │ │ ├── edge_se3_priorxyz.cpp │ │ ├── edge_se3_two_planes.cpp │ │ ├── edge_se3_two_rooms.cpp │ │ ├── edge_wall_two_planes.cpp │ │ └── robust_kernel_io.cpp │ └── s_graphs │ │ ├── backend │ │ ├── finite_room_mapper.cpp │ │ ├── floor_mapper.cpp │ │ ├── gps_mapper.cpp │ │ ├── graph_slam.cpp │ │ ├── imu_mapper.cpp │ │ ├── infinite_room_mapper.cpp │ │ ├── keyframe_mapper.cpp │ │ ├── loop_mapper.cpp │ │ ├── plane_mapper.cpp │ │ ├── room_graph_generator.cpp │ │ └── wall_mapper.cpp │ │ ├── common │ │ ├── graph_utils.cpp │ │ ├── information_matrix_calculator.cpp │ │ ├── keyframe.cpp │ │ ├── map_cloud_generator.cpp │ │ ├── plane_utils.cpp │ │ ├── registrations.cpp │ │ ├── room_utils.cpp │ │ ├── ros_utils.cpp │ │ └── s_graphs.cpp │ │ ├── frontend │ │ ├── floor_analyzer.cpp │ │ ├── plane_analyzer.cpp │ │ └── room_analyzer.cpp │ │ └── visualization │ │ ├── graph_publisher.cpp │ │ └── graph_visualizer.cpp └── test │ ├── testPlane.cpp │ ├── testRoom.cpp │ └── testRoomCentreCompute.cpp ├── mkdocs.yml ├── msg └── FloorData.msg ├── requirements.txt └── setup.sh /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | Language: Cpp 3 | BasedOnStyle: Google 4 | AccessModifierOffset: -1 5 | AlignAfterOpenBracket: Align 6 | AlignConsecutiveAssignments: false 7 | AlignConsecutiveDeclarations: false 8 | AlignOperands: true 9 | AlignTrailingComments: true 10 | AllowAllParametersOfDeclarationOnNextLine: false 11 | AllowShortBlocksOnASingleLine: false 12 | AllowShortCaseLabelsOnASingleLine: false 13 | AllowShortFunctionsOnASingleLine: All 14 | AllowShortIfStatementsOnASingleLine: true 15 | AllowShortLoopsOnASingleLine: true 16 | AlwaysBreakAfterDefinitionReturnType: None 17 | AlwaysBreakAfterReturnType: None 18 | AlwaysBreakBeforeMultilineStrings: true 19 | AlwaysBreakTemplateDeclarations: true 20 | BinPackArguments: false 21 | BinPackParameters: false 22 | BraceWrapping: 23 | AfterClass: false 24 | AfterControlStatement: false 25 | AfterEnum: false 26 | AfterFunction: false 27 | AfterNamespace: false 28 | AfterObjCDeclaration: false 29 | AfterStruct: false 30 | AfterUnion: false 31 | BeforeCatch: false 32 | BeforeElse: false 33 | IndentBraces: false 34 | BreakBeforeBinaryOperators: None 35 | BreakBeforeBraces: Attach 36 | BreakBeforeTernaryOperators: true 37 | BreakConstructorInitializersBeforeComma: false 38 | ColumnLimit: 88 39 | CommentPragmas: '^ IWYU pragma:' 40 | ConstructorInitializerAllOnOneLineOrOnePerLine: true 41 | ConstructorInitializerIndentWidth: 4 42 | ContinuationIndentWidth: 4 43 | Cpp11BracedListStyle: true 44 | DerivePointerAlignment: true 45 | DisableFormat: false 46 | ExperimentalAutoDetectBinPacking: false 47 | ForEachMacros: 48 | - foreach 49 | - Q_FOREACH 50 | - BOOST_FOREACH 51 | IncludeCategories: 52 | - Regex: '^' 53 | Priority: 2 54 | - Regex: '^<.*\.h>' 55 | Priority: 1 56 | - Regex: '^<.*' 57 | Priority: 2 58 | - Regex: '.*' 59 | Priority: 3 60 | IndentCaseLabels: true 61 | IndentWidth: 2 62 | IndentWrappedFunctionNames: false 63 | KeepEmptyLinesAtTheStartOfBlocks: false 64 | MacroBlockBegin: '' 65 | MacroBlockEnd: '' 66 | MaxEmptyLinesToKeep: 1 67 | NamespaceIndentation: None 68 | ObjCBlockIndentWidth: 2 69 | ObjCSpaceAfterProperty: false 70 | ObjCSpaceBeforeProtocolList: false 71 | PenaltyBreakBeforeFirstCallParameter: 1 72 | PenaltyBreakComment: 300 73 | PenaltyBreakFirstLessLess: 120 74 | PenaltyBreakString: 1000 75 | PenaltyExcessCharacter: 1000000 76 | PenaltyReturnTypeOnItsOwnLine: 200 77 | PointerAlignment: Left 78 | ReflowComments: true 79 | SortIncludes: true 80 | SpaceAfterCStyleCast: false 81 | SpaceBeforeAssignmentOperators: true 82 | SpaceBeforeParens: ControlStatements 83 | SpaceInEmptyParentheses: false 84 | SpacesBeforeTrailingComments: 2 85 | SpacesInAngles: false 86 | SpacesInContainerLiterals: true 87 | SpacesInCStyleCastParentheses: false 88 | SpacesInParentheses: false 89 | SpacesInSquareBrackets: false 90 | Standard: Auto 91 | TabWidth: 8 92 | UseTab: Never 93 | ... 94 | 95 | -------------------------------------------------------------------------------- /.docker_real_mprocs.yaml: -------------------------------------------------------------------------------- 1 | procs: 2 | rosbag: 3 | shell: 'bash -c "source /workspace/install/setup.bash && sleep 2 && ros2 bag play ~/Downloads/real/main_building_2_floors.db3"' 4 | autostart: true 5 | stop: 6 | send-keys: [""] 7 | s_graphs: 8 | shell: 'bash -c "source /workspace/install/setup.bash && ros2 launch lidar_situational_graphs s_graphs_launch.py lidar_topic:=/cloud/undistorted base_frame:=base_link compute_odom:=false"' 9 | autostart: true 10 | rviz: 11 | shell: 'bash -c "source /workspace/install/setup.bash && rviz2 -d /workspace/src/s_graphs/lidar_situational_graphs/rviz/s_graphs_ros2.rviz"' 12 | autostart: true 13 | -------------------------------------------------------------------------------- /.docker_virtual_mprocs.yaml: -------------------------------------------------------------------------------- 1 | procs: 2 | rosbag: 3 | shell: 'bash -c "sleep 1 && source /workspace/src/s_graphs/install/setup.bash && ros2 bag play ~/Downloads/virtual/stugalux_oetrange_f2_3r.db3"' 4 | autostart: true 5 | stop: SIGINT 6 | s_graphs: 7 | shell: 'bash -c "source /workspace/src/s_graphs/install/setup.bash && ros2 launch lidar_situational_graphs s_graphs_launch.py compute_odom:=true base_link_frame:=base_footprint"' 8 | autostart: true 9 | rviz: 10 | shell: 'bash -c "source /workspace/src/s_graphs/install/setup.bash && rviz2 -d /workspace/src/s_graphs/s_graphs/rviz/s_graphs_ros2.rviz"' 11 | autostart: true 12 | -------------------------------------------------------------------------------- /.github/workflows/docs_build.yaml: -------------------------------------------------------------------------------- 1 | name: docs build 2 | on: 3 | push: 4 | branches: 5 | - feature/multi_floor 6 | permissions: 7 | contents: write 8 | jobs: 9 | deploy: 10 | runs-on: ubuntu-latest 11 | steps: 12 | - uses: actions/checkout@v4 13 | - name: Configure Git Credentials 14 | run: | 15 | git config user.name github-actions[bot] 16 | git config user.email 41898282+github-actions[bot]@users.noreply.github.com 17 | - uses: actions/setup-python@v5 18 | with: 19 | python-version: 3.x 20 | - run: echo "cache_id=$(date --utc '+%V')" >> $GITHUB_ENV 21 | - uses: actions/cache@v4 22 | with: 23 | key: mkdocs-material-${{ env.cache_id }} 24 | path: .cache 25 | restore-keys: | 26 | mkdocs-material- 27 | - run: pip install mkdocs-material 28 | - run: mkdocs gh-deploy --force 29 | -------------------------------------------------------------------------------- /.github/workflows/foxy_build.yaml: -------------------------------------------------------------------------------- 1 | name: ROS 2 Foxy 2 | 3 | on: 4 | push: 5 | branches: 6 | - feature/multi_floor 7 | 8 | jobs: 9 | build: 10 | runs-on: ubuntu-latest 11 | container: 12 | image: ros:foxy 13 | steps: 14 | - name: Create Workspace and checkout code 15 | uses: actions/checkout@v3 16 | with: 17 | submodules: recursive 18 | 19 | - name: Build 20 | run: . /opt/ros/foxy/setup.sh && ./.github/workflows/setup.sh && colcon build -------------------------------------------------------------------------------- /.github/workflows/humble_build.yaml: -------------------------------------------------------------------------------- 1 | name: ROS 2 Humble 2 | 3 | on: 4 | push: 5 | branches: 6 | - feature/multi_floor 7 | 8 | jobs: 9 | build: 10 | runs-on: ubuntu-latest 11 | container: 12 | image: ros:humble 13 | steps: 14 | - name: Create Workspace and checkout code 15 | uses: actions/checkout@v3 16 | with: 17 | submodules: recursive 18 | 19 | - name: Build 20 | run: . /opt/ros/humble/setup.sh && ./.github/workflows/setup.sh && colcon build -------------------------------------------------------------------------------- /.github/workflows/iron_build.yaml: -------------------------------------------------------------------------------- 1 | name: ROS 2 Iron 2 | 3 | on: 4 | push: 5 | branches: 6 | - feature/multi_floor 7 | 8 | jobs: 9 | build: 10 | runs-on: ubuntu-latest 11 | container: 12 | image: ros:iron 13 | steps: 14 | - name: Create Workspace and checkout code 15 | uses: actions/checkout@v3 16 | with: 17 | submodules: recursive 18 | 19 | - name: Build 20 | run: . /opt/ros/iron/setup.sh && ./.github/workflows/setup.sh && colcon build -------------------------------------------------------------------------------- /.github/workflows/setup.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | sudo apt update 4 | DEBIAN_FRONTEND=noninteractive sudo apt install -y libceres-dev python3-pip 5 | 6 | #rosdep init 7 | rosdep update --include-eol-distros 8 | 9 | #install vcs-tools 10 | DEBIAN_FRONTEND=noninteractive sudo apt install python3-vcstool 11 | 12 | #import all repos 13 | vcs import . < .rosinstall_ros2 14 | 15 | #rosdep install 16 | DEBIAN_FRONTEND=noninteractive rosdep install --from-paths . -y --ignore-src -r 17 | 18 | # - Importing all dependencies 19 | colcon build --symlink-install -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | imgui.ini 2 | 3 | # - IDEs Folder 4 | build/ 5 | install/ 6 | log/ 7 | utils/ 8 | 9 | # - Clangd 10 | compile_commands.json 11 | .cache -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "docs/doxygen_docs/doxygen-awesome-css"] 2 | path = docs/doxygen_docs/doxygen-awesome-css 3 | url = https://github.com/jothepro/doxygen-awesome-css.git 4 | -------------------------------------------------------------------------------- /.real_mprocs.yaml: -------------------------------------------------------------------------------- 1 | procs: 2 | rosbag: 3 | shell: 'bash -c "source ~/workspaces/s_graphs/install/setup.bash && sleep 2 && ros2 bag play ~/Downloads/real/main_building_2_floors.db3"' 4 | autostart: true 5 | stop: 6 | send-keys: [""] 7 | s_graphs: 8 | shell: 'bash -c "source ~/workspaces/s_graphs/install/setup.bash && ros2 launch lidar_situational_graphs s_graphs_launch.py lidar_topic:=/cloud/undistorted base_frame:=base_link compute_odom:=false"' 9 | autostart: true 10 | rviz: 11 | shell: 'bash -c "source ~/workspaces/s_graphs/install/setup.bash && rviz2 -d ~/workspaces/s_graphs/lidar_situational_graphs/rviz/s_graphs_ros2.rviz"' 12 | autostart: true 13 | -------------------------------------------------------------------------------- /.rosinstall_ros1: -------------------------------------------------------------------------------- 1 | repositories: 2 | s_graphs_utils/mav_voxblox_planning: 3 | type: git 4 | url: https://github.com/snt-arg/mav_voxblox_planning.git 5 | version: master 6 | s_graphs_utils/catkin_boost_python_buildtool: 7 | type: git 8 | url: https://github.com/ethz-asl/catkin_boost_python_buildtool.git 9 | version: master 10 | s_graphs_utils/catkin_simple: 11 | type: git 12 | url: https://github.com/catkin/catkin_simple.git 13 | version: master 14 | s_graphs_utils/ceres_catkin: 15 | type: git 16 | url: https://github.com/ethz-asl/ceres_catkin.git 17 | version: master 18 | s_graphs_utils/eigen_catkin: 19 | type: git 20 | url: https://github.com/ethz-asl/eigen_catkin.git 21 | version: master 22 | s_graphs_utils/eigen_checks: 23 | type: git 24 | url: https://github.com/ethz-asl/eigen_checks.git 25 | version: master 26 | s_graphs_utils/gflags_catkin: 27 | type: git 28 | url: https://github.com/ethz-asl/gflags_catkin.git 29 | version: master 30 | s_graphs_utils/glog_catkin: 31 | type: git 32 | url: https://github.com/ethz-asl/glog_catkin.git 33 | version: master 34 | s_graphs_utils/mav_comm: 35 | type: git 36 | url: https://github.com/ethz-asl/mav_comm.git 37 | version: master 38 | s_graphs_utils/mav_trajectory_generation: 39 | type: git 40 | url: https://github.com/ethz-asl/mav_trajectory_generation.git 41 | version: master 42 | s_graphs_utils/minkindr: 43 | type: git 44 | url: https://github.com/ethz-asl/minkindr.git 45 | version: master 46 | s_graphs_utils/minkindr_ros: 47 | type: git 48 | url: https://github.com/ethz-asl/minkindr_ros.git 49 | version: master 50 | s_graphs_utils/nlopt: 51 | type: git 52 | url: https://github.com/ethz-asl/nlopt.git 53 | version: master 54 | s_graphs_utils/numpy_eigen: 55 | type: git 56 | url: https://github.com/ethz-asl/numpy_eigen.git 57 | version: master 58 | s_graphs_utils/protobuf_catkin: 59 | type: git 60 | url: https://github.com/ethz-asl/protobuf_catkin.git 61 | version: master 62 | s_graphs_utils/voxblox: 63 | type: git 64 | url: https://github.com/ethz-asl/voxblox.git 65 | version: master -------------------------------------------------------------------------------- /.rosinstall_ros2: -------------------------------------------------------------------------------- 1 | repositories: 2 | utils/fast_gicp: 3 | type: git 4 | url: https://github.com/koide3/fast_gicp.git 5 | version: master 6 | utils/ndt_omp: 7 | type: git 8 | url: https://github.com/koide3/ndt_omp.git 9 | version: master 10 | utils/reasoning/situational_graphs_reasoning: 11 | type: git 12 | url: https://github.com/snt-arg/situational_graphs_reasoning.git 13 | version: develop 14 | utils/reasoning/situational_graphs_datasets: 15 | type: git 16 | url: https://github.com/snt-arg/situational_graphs_datasets.git 17 | version: develop 18 | utils/reasoning/situational_graphs_wrapper: 19 | type: git 20 | url: https://github.com/snt-arg/situational_graphs_wrapper.git 21 | version: develop 22 | utils/msgs/situational_graphs_reasoning_msgs: 23 | type: git 24 | url: https://github.com/snt-arg/situational_graphs_reasoning_msgs.git 25 | version: main 26 | utils/msgs/situational_graphs_msgs: 27 | type: git 28 | url: https://github.com/snt-arg/situational_graphs_msgs.git 29 | version: main -------------------------------------------------------------------------------- /.vscode/c_cpp_properties.json: -------------------------------------------------------------------------------- 1 | { 2 | "configurations": [ 3 | { 4 | "name": "Linux", 5 | "includePath": [ 6 | "${workspaceFolder}/**", 7 | "/opt/ros/foxy/include/**", 8 | "/opt/ros/humble/include/**", 9 | "/usr/include/**", 10 | "/usr/local/include/**" 11 | ], 12 | "intelliSenseMode": "gcc-x64", 13 | "compilerPath": "/usr/bin/gcc", 14 | "cStandard": "gnu11", 15 | "cppStandard": "c++17" 16 | } 17 | ], 18 | "version": 4 19 | } -------------------------------------------------------------------------------- /.vscode/launch.json: -------------------------------------------------------------------------------- 1 | { 2 | "version": "0.2.0", 3 | "configurations": [ 4 | { 5 | "name": "C++ Debugger", 6 | "request": "launch", 7 | "type": "cppdbg", 8 | "miDebuggerServerAddress": "localhost:3000", 9 | "cwd": "/", 10 | "program": "${workspaceFolder}/install/lidar_situational_graphs/lib/lidar_situational_graphs/s_graphs_node" 11 | } 12 | ] 13 | } -------------------------------------------------------------------------------- /.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "editor.formatOnSave": true, 3 | "clang-format.executable": "/usr/bin/clang-format-11", 4 | "python.autoComplete.extraPaths": [ 5 | "${workspaceFolder}/install/s_graphs/lib/python3.8/site-packages", 6 | "${workspaceFolder}/install/graph_manager_msgs/lib/python3.8/site-packages", 7 | "/opt/ros/foxy/lib/python3.8/site-packages", 8 | "/opt/ros/humble/lib/python3.8/site-packages" 9 | ], 10 | "files.associations": { 11 | "cctype": "cpp", 12 | "clocale": "cpp", 13 | "cmath": "cpp", 14 | "csignal": "cpp", 15 | "cstdarg": "cpp", 16 | "cstddef": "cpp", 17 | "cstdio": "cpp", 18 | "cstdlib": "cpp", 19 | "cstring": "cpp", 20 | "ctime": "cpp", 21 | "cwchar": "cpp", 22 | "cwctype": "cpp", 23 | "any": "cpp", 24 | "array": "cpp", 25 | "atomic": "cpp", 26 | "hash_map": "cpp", 27 | "hash_set": "cpp", 28 | "strstream": "cpp", 29 | "*.tcc": "cpp", 30 | "bitset": "cpp", 31 | "chrono": "cpp", 32 | "codecvt": "cpp", 33 | "complex": "cpp", 34 | "condition_variable": "cpp", 35 | "cstdint": "cpp", 36 | "deque": "cpp", 37 | "forward_list": "cpp", 38 | "list": "cpp", 39 | "unordered_map": "cpp", 40 | "unordered_set": "cpp", 41 | "vector": "cpp", 42 | "exception": "cpp", 43 | "algorithm": "cpp", 44 | "functional": "cpp", 45 | "iterator": "cpp", 46 | "map": "cpp", 47 | "memory": "cpp", 48 | "memory_resource": "cpp", 49 | "numeric": "cpp", 50 | "optional": "cpp", 51 | "random": "cpp", 52 | "ratio": "cpp", 53 | "regex": "cpp", 54 | "set": "cpp", 55 | "string": "cpp", 56 | "string_view": "cpp", 57 | "system_error": "cpp", 58 | "tuple": "cpp", 59 | "type_traits": "cpp", 60 | "utility": "cpp", 61 | "slist": "cpp", 62 | "fstream": "cpp", 63 | "future": "cpp", 64 | "initializer_list": "cpp", 65 | "iomanip": "cpp", 66 | "iosfwd": "cpp", 67 | "iostream": "cpp", 68 | "istream": "cpp", 69 | "limits": "cpp", 70 | "mutex": "cpp", 71 | "new": "cpp", 72 | "ostream": "cpp", 73 | "shared_mutex": "cpp", 74 | "sstream": "cpp", 75 | "stdexcept": "cpp", 76 | "streambuf": "cpp", 77 | "thread": "cpp", 78 | "cfenv": "cpp", 79 | "cinttypes": "cpp", 80 | "typeindex": "cpp", 81 | "typeinfo": "cpp", 82 | "valarray": "cpp", 83 | "variant": "cpp", 84 | "bit": "cpp", 85 | "*.ipp": "cpp", 86 | "core": "cpp", 87 | "numericaldiff": "cpp", 88 | "eigen": "cpp", 89 | "*.inc": "cpp", 90 | "nonlinearoptimization": "cpp", 91 | "compare": "cpp", 92 | "concepts": "cpp", 93 | "numbers": "cpp", 94 | "semaphore": "cpp", 95 | "stop_token": "cpp" 96 | }, 97 | "C_Cpp.errorSquiggles": "enabled", 98 | "python.formatting.provider": "black", 99 | "python.analysis.extraPaths": [ 100 | "${workspaceFolder}/install/s_graphs/lib/python3.8/site-packages", 101 | "${workspaceFolder}/install/graph_manager_msgs/lib/python3.8/site-packages", 102 | "/opt/ros/foxy/lib/python3.8/site-packages", 103 | "/opt/ros/foxy/lib/python3.8/site-packages" 104 | ], 105 | "ros.distro": "humble", 106 | "cmake.configureOnOpen": false, 107 | "scm.showHistoryGraph": false 108 | } -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 |
2 |

LiDAR S-Graphs

3 | 4 | 5 | 6 |
7 | 8 | **LiDAR Situational Graphs (S-Graphs)** is a ROS2 package for generating in real-time four-layered hierarchical factor graphs for single or multi-floor scenes. It reepresents a scene graph using 3D LiDAR which includes **_Keyframes_** registring the robot poses, **_Walls_** which map wall planes, **_Rooms Layer_** constraining the wall planes using 4 wall-room or 2 wall-room factors, **_Floors_** constraining the rooms within a given floor level. It also supports several graph constraints, such as GPS, IMU acceleration (gravity vector), IMU orientation (magnetic sensor). We have tested this package mostly with Ouster OS-1 and Velodyne (VLP16) sensors in structured indoor environments. 9 | 10 |

11 | 12 | Video Thumbnail 13 | 14 |

15 | 16 | ## 📖 Published Papers 17 | 18 | 19 |
20 | S-Graphs 2.0 -- A Hierarchical-Semantic Optimization and Loop Closure for SLAM 21 | 22 | 23 | @misc{bavle2025sgraphs20hierarchicalsemantic, 24 | title={S-Graphs 2.0 -- A Hierarchical-Semantic Optimization and Loop Closure for SLAM}, 25 | author={Hriday Bavle and Jose Luis Sanchez-Lopez and Muhammad Shaheer and Javier Civera and Holger Voos}, 26 | year={2025}, 27 | eprint={2502.18044}, 28 | archivePrefix={arXiv}, 29 | primaryClass={cs.RO}, 30 | url={https://arxiv.org/abs/2502.18044}, 31 | } 32 | 33 |
34 | 35 |
36 | S-Graphs+: Real-time Localization and Mapping leveraging Hierarchical Representations 37 | 38 | 39 | @ARTICLE{10168233, 40 | author={Bavle, Hriday and Sanchez-Lopez, Jose Luis and Shaheer, Muhammad and Civera, Javier and Voos, Holger}, 41 | journal={IEEE Robotics and Automation Letters}, 42 | title={S-Graphs+: Real-Time Localization and Mapping Leveraging Hierarchical Representations}, 43 | year={2023}, 44 | volume={8}, 45 | number={8}, 46 | pages={4927-4934}, 47 | doi={10.1109/LRA.2023.3290512}} 48 | 49 |
50 | 51 |
52 | Situational Graphs for Robot Navigation in Structured Indoor Environments 53 | 54 | @ARTICLE{9826367, 55 | author={Bavle, Hriday and Sanchez-Lopez, Jose Luis and Shaheer, Muhammad and Civera, Javier and Voos, Holger}, 56 | journal={IEEE Robotics and Automation Letters}, 57 | title={Situational Graphs for Robot Navigation in Structured Indoor Environments}, 58 | year={2022}, 59 | volume={7}, 60 | number={4}, 61 | pages={9107-9114}, 62 | doi={10.1109/LRA.2022.3189785}} 63 | 64 |
65 | 66 | 67 | ## Documentation 68 | 69 | For installation refer to the documentation available [here](https://snt-arg.github.io/lidar_situational_graphs) 70 | -------------------------------------------------------------------------------- /docker/foxy_noetic/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ros:foxy 2 | 3 | # Install necessary and useful packages 4 | RUN apt update && \ 5 | apt install --no-install-recommends -y \ 6 | ssh \ 7 | vim \ 8 | python3-pip \ 9 | python3-vcstool \ 10 | python3-catkin-tools \ 11 | wget \ 12 | git 13 | 14 | # Add the requirements.txt file and install the libraries 15 | # ADD requirements.txt requirements.txt 16 | # RUN pip3 install -r requirements.txt 17 | 18 | # Install ros2 packages 19 | RUN apt install --no-install-recommends -y \ 20 | ros-foxy-joint-state-publisher-gui \ 21 | ros-foxy-tf-transformations \ 22 | ros-foxy-xacro \ 23 | ros-foxy-depth-image-proc \ 24 | ros-foxy-velodyne \ 25 | ros-foxy-teleop-twist-keyboard 26 | 27 | # Creating working spaces directory and clone s_graphs inside it 28 | RUN mkdir -p /root/workspaces 29 | WORKDIR /root/workspaces 30 | RUN git clone https://github.com/snt-arg/lidar_situational_graphs.git -b feature/ros2 s_graphs 31 | 32 | # Fetching all packages specified in the gitsubmodules and running build everything with colcon build. 33 | WORKDIR /root/workspaces/s_graphs 34 | RUN vcs import . < .rosinstall_ros2 35 | WORKDIR /root/workspaces/s_graphs 36 | RUN sudo apt install python3-rosdep 37 | RUN rosdep install --from-paths . -y --ignore-src -r 38 | SHELL ["/bin/bash", "-c"] 39 | RUN source /opt/ros/foxy/setup.bash && \ 40 | colcon build --symlink-install 41 | 42 | # Install ROS1 43 | RUN sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' 44 | RUN apt install --no-install-recommends -y curl 45 | RUN curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo apt-key add - 46 | RUN apt update 47 | RUN apt install --no-install-recommends -y \ 48 | ros-noetic-desktop \ 49 | python3-rosdep \ 50 | python3-rosinstall \ 51 | python3-rosinstall-generator \ 52 | python3-wstool \ 53 | build-essential 54 | RUN rosdep update --include-eol-distros 55 | 56 | # Setup ROS1 workspace for ROS1 related dependencies of s_graphs 57 | RUN mkdir -p /root/workspaces/s_graphs_ros1_ws/src 58 | WORKDIR /root/workspaces/s_graphs_ros1_ws/src 59 | RUN git clone https://github.com/snt-arg/lidar_situational_graphs.git -b feature/ros2 s_graphs 60 | WORKDIR /root/workspaces/s_graphs_ros1_ws/src/s_graphs 61 | RUN vcs import --recursive ../ < .rosinstall_ros1 62 | WORKDIR /root/workspaces/s_graphs_ros1_ws 63 | RUN rosdep install --from-paths src --ignore-src -y -r 64 | RUN apt install --no-install-recommends -y \ 65 | libtool \ 66 | ros-noetic-pcl-ros \ 67 | ros-foxy-ros1-bridge 68 | RUN source /opt/ros/noetic/setup.bash && \ 69 | catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release && \ 70 | catkin build 71 | 72 | # Install mprocs 73 | WORKDIR /root 74 | RUN wget https://github.com/pvolok/mprocs/releases/download/v0.6.4/mprocs-0.6.4-linux64.tar.gz 75 | RUN tar -xf mprocs* && \ 76 | rm mprocs*.tar.gz && \ 77 | mv mprocs /usr/local/bin 78 | 79 | # alias for mprocs 80 | RUN echo "alias mprocs_real='mprocs -c /root/workspaces/s_graphs/.real_mprocs.yaml'" >> /root/.bashrc 81 | RUN echo "alias mprocs_virtual='mprocs -c /root/workspaces/s_graphs/.virtual_mprocs.yaml'" >> /root/.bashrc -------------------------------------------------------------------------------- /docker/humble/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM osrf/ros:humble-desktop-full AS base 2 | 3 | ARG COLCON_JOBS=4 4 | 5 | ENV DEBIAN_FRONTEND=noninteractive 6 | ENV MAKEFLAGS="-j $COLCON_JOBS" 7 | 8 | # Install necessary and useful packages 9 | RUN apt update && \ 10 | apt install --no-install-recommends -y \ 11 | vim python3-pip python3-vcstool wget git \ 12 | ros-$ROS_DISTRO-rmw-cyclonedds-cpp 13 | 14 | FROM base AS optional 15 | 16 | # Install mprocs 17 | WORKDIR /opt 18 | 19 | # Install Cargo 20 | RUN curl --proto '=https' --tlsv1.2 -sSf https://sh.rustup.rs | bash -s -- -y 21 | 22 | # Install Mprocs 23 | RUN /root/.cargo/bin/cargo install mprocs 24 | 25 | FROM base AS sgraphs_prerequisites 26 | 27 | RUN pip3 install scikit-learn torch==2.0.1 torchvision==0.15.2 torchaudio==2.0.2 28 | 29 | # Clone lidar_situational_graphs 30 | COPY . /workspace/src/s_graphs 31 | # RUN git clone https://github.com/snt-arg/lidar_situational_graphs s_graphs 32 | WORKDIR /workspace/src/s_graphs 33 | RUN vcs import . < .rosinstall_ros2 34 | 35 | FROM sgraphs_prerequisites AS sgraphs_build 36 | 37 | # Build lidar_situational_graphs 38 | WORKDIR /workspace 39 | RUN rosdep update && \ 40 | rosdep install --from-paths . -y --ignore-src -r 41 | RUN . /opt/ros/humble/setup.sh && colcon build 42 | 43 | 44 | # Copy mprocs bin 45 | COPY --from=optional /root/.cargo/bin/mprocs /usr/bin 46 | 47 | # alias for mprocs 48 | RUN echo "alias mprocs_real='mprocs -c /workspace/src/s_graphs/.docker_real_mprocs.yaml'" >> ~/.bashrc 49 | RUN echo "alias mprocs_virtual='mprocs -c /workspace/src/s_graphs/.docker_virtual_mprocs.yaml'" >> ~/.bashrc 50 | 51 | # Build Entrypoint 52 | RUN echo "#!/bin/bash" >> /entrypoint.sh \ 53 | && echo "source /opt/ros/humble/setup.bash" >> /entrypoint.sh \ 54 | && echo "source /workspace/install/setup.bash" >> /entrypoint.sh \ 55 | && echo 'exec "$@"' >> /entrypoint.sh \ 56 | && chmod a+x /entrypoint.sh 57 | 58 | WORKDIR /workspace 59 | 60 | ENTRYPOINT ["/entrypoint.sh"] 61 | CMD ["bash"] 62 | -------------------------------------------------------------------------------- /docs/1.installation.md: -------------------------------------------------------------------------------- 1 | # ⚙️ Installation 2 | 3 | !!! info 4 | 5 | S-Graphs has only been tested on Ubuntu 22.04, ROS2 Foxy, Humble Distros. 6 | 7 | ## 📦 From Source 8 | 9 | !!! warning "Important" 10 | 11 | Before proceeding, make sure you have `rosdep` installed. You can install it using `sudo apt-get install python3-rosdep` 12 | In addition, ssh keys are needed to be configured on you GitHub account. If you haven't 13 | yet configured ssh keys, follow this [tutorial](https://docs.github.com/en/authentication/connecting-to-github-with-ssh/generating-a-new-ssh-key-and-adding-it-to-the-ssh-agent) 14 | 15 | 1. Update Rosdep: 16 | 17 | ```bash 18 | rosdep init 19 | rosdep update --include-eol-distros 20 | ``` 21 | 22 | 2. Create a ROS2 workspace for S-Graphs 23 | 24 | ```bash 25 | mkdir -p $HOME/workspaces && cd $HOME/workspaces 26 | ``` 27 | 28 | 3. Clone the S-Graphs repository into the created workspace 29 | 30 | ```bash 31 | git clone git@github.com:snt-arg/lidar_situational_graphs.git -b feature/multi_floor s_graphs 32 | ``` 33 | 34 | !!! info 35 | 36 | If you have Nvidia GPU please install CUDA from this [link](https://developer.nvidia.com/cuda-11-8-0-download-archive). This code has only been tested with CUDA 11.8. 37 | If you dont have CUDA S-Graphs will use CPU only. 38 | 39 | 4. Install required dependencies. 40 | 41 | !!! info 42 | 43 | Lidar SGraphs can can take some time to build depending on the platform and may consume a lot of memory. 44 | If your computer does not have a lot of RAM is it advised to limit the number of threads make uses with 45 | `export MAKEFLAGS="-j `, where `` should be replaced with the max of number of CPUs. Change $ROS_DISTRO to your ros2 version. 46 | 47 | ```bash 48 | cd s_graphs && source /opt/ros/$ROS_DISTRO/setup.sh && ./setup.sh 49 | ``` 50 | 51 | !!! info 52 | 53 | If you want to compile with debug traces (from backward_cpp) run: 54 | ```bash 55 | colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo -DUSE_RGB_CLOUD=ON 56 | ``` 57 | 58 | #### Optional ROS1 Install (Old Version of Room Segmentation) 59 | 60 |
61 | 62 | !!! info 63 | 64 | This is an optional older version of room segmentation algorithm which requires ROS1 noetic. There is no hard dependecy on this package so you can easily ignore this step. 65 | 66 | ##### Download ROS Bridge 67 | 68 | ```bash 69 | source /opt/ros/foxy/setup.bash && sudo apt install ros-foxy-ros1-bridge 70 | ``` 71 | 72 | ##### Installation on ROS1 73 | 74 | !!! warning 75 | 76 | Before following the instructions from below, ensure that you are in a fresh 77 | terminal, **without ROS2 sourced**. 78 | 79 | 1. Create a ROS1 workspace for S-Graphs 80 | 81 | ```bash 82 | mkdir -p $HOME/workspaces/s_graphs_ros1_ws/src && cd $HOME/workspaces/s_graphs_ros1_ws/src && source /opt/ros/noetic/setup.bash 83 | ``` 84 | 85 | 2. Clone the S-Graphs repository into the created workspace 86 | 87 | ```bash 88 | git clone git@github.com:snt-arg/lidar_situational_graphs.git -b feature/multi_floor s_graphs 89 | ``` 90 | 91 | 3. Install required dependencies using `vcstool` 92 | 93 | ```bash 94 | cd s_graphs && vcs import --recursive ../ < .rosinstall_ros1 95 | ``` 96 | 97 | 4. Install required ROS packages 98 | 99 | ```bash 100 | cd ../../ && rosdep install --from-paths src --ignore-src -y -r 101 | ``` 102 | 103 | 5. Install `pcl_ros` 104 | 105 | ```sh 106 | sudo apt install ros-noetic-pcl-ros 107 | ``` 108 | 109 | 6. Build workspace 110 | 111 | !!! warning 112 | 113 | Make sure s_graphs_ros1_ws is built in Release otherwise the room extraction won't work properly. 114 | 115 | ```bash 116 | catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release && catkin build 117 | ``` 118 | 119 |
120 | 121 | ## 🧪 Unit Tests 122 | 123 | Some unit tests are available. In case you want to add additional tests, run the following command: 124 | 125 | ```bash 126 | colcon test --packages-select s_graphs --event-handler=console_direct+ 127 | ``` 128 | -------------------------------------------------------------------------------- /docs/2.datasets.md: -------------------------------------------------------------------------------- 1 | # 🚀 Example on Datasets 2 | 3 | !!! warning 4 | 5 | For execution of the experiments we use [mprocs](https://github.com/pvolok/mprocs), which makes the process of launching different processes easier. 6 | 7 | ## Real Dataset 8 | 9 | !!! info "Dataset Download" 10 | 11 | Download real dataset using this [link](https://uniluxembourg-my.sharepoint.com/:u:/g/personal/joseluis_sanchezlopez_uni_lu/Ea35rS7rZXxDqxFMITM-xL4BWbz4a2OBtNb9kyeurJbNMg?e=auGZXW) and store it in the folder `~/Downloads/real`, the below mprocs script will not work otherwise. 12 | 13 | ```bash 14 | cd $HOME/workspaces/s_graphs && mprocs --config .real_mprocs.yaml 15 | ``` -------------------------------------------------------------------------------- /docs/3.getting_started.md: -------------------------------------------------------------------------------- 1 | # 🛠️ Run S-Graphs On Your Data 2 | 3 | 1. Define the transformation between your sensors (LIDAR, IMU, GPS) and base_link of your system using static_transform_publisher (see [line](https://github.com/snt-arg/s_graphs/blob/c0489660552cb3a2fc8ac0bef17998ee5fb6e15a/launch/s_graphs_launch.py#L118), s_graphs_launch.py). All the sensor data will be transformed into the common `base_link` frame, and then fed to the SLAM algorithm. Note: `base_link` frame in virtual dataset is set to `base_footprint` and in real dataset is set to `body`. You can set the `frames`, `topics` for your dataset easily during the launch execution as follows: 4 | 5 | ```bash 6 | ros2 launch lidar_situational_graphs s_graphs_launch.py compute_odom:=true lidar_topic:=/rs_lidar/points 7 | ``` 8 | 9 | 3. If you have an odometry source convert it to base ENU frame, then set the arg `compute_odom` to `false` in `s_graphs_ros2_launch.py` and then remap odom topic in **s_graphs_node** like 10 | 11 | ```bash 12 | ros2 launch lidar_situational_graphs s_graphs_launch.py compute_odom:=false lidar_topic:=/rs_lidar/points odom_topic:=/odom 13 | ``` 14 | 15 | !!! info 16 | 17 | If you want to visualize the tfs correctly from your odom source, you MUST provide a tf from the `odom` to `base_link` frame. 18 | -------------------------------------------------------------------------------- /docs/4.docker.md: -------------------------------------------------------------------------------- 1 | # 🐳 Docker 2 | 3 | We offer a docker image available on [DockerHub](https://hub.docker.com/repository/docker/sntarg/s_graphs/general) to easily pull and use. Otherwise, one can 4 | just build it locally. 5 | 6 | ## Running S-Graphs 7 | 8 | ### Pre-requisite 9 | 10 | Before running, you need some dataset to run on S-Graphs. We provide a real dataset 11 | collected by us. You can download it [here](https://uniluxembourg-my.sharepoint.com/:u:/g/personal/joseluis_sanchezlopez_uni_lu/Ea35rS7rZXxDqxFMITM-xL4BWbz4a2OBtNb9kyeurJbNMg?e=auGZXW). 12 | 13 | !!! danger "Important" 14 | 15 | Place the downloaded file under `~/Downloads/real` 16 | 17 | Once you have downloaded, you can now run the container as follows: 18 | 19 | !!! danger "Important" 20 | 21 | Before anything, in order to have rviz working, run the following command: 22 | `xhost +local:docker` 23 | 24 | ```bash 25 | docker run -it --rm \ 26 | -e "$DISPLAY" -v "/tmp/.X11-unix:/tmp/.X11-unix:rw" \ 27 | -v ~/Downloads/real:/root/Downloads/real \ 28 | sntarg/s_graphs:latest bash 29 | ``` 30 | 31 | Then, once inside the container, run `mprocs_real` 32 | 33 | ## Build Docker Image manually 34 | 35 | 1. Create a ROS2 workspace for S-Graphs 36 | 37 | 2. Clone lidar_situational_graphs repository 38 | 39 | ```sh 40 | git clone git@github.com:snt-arg/lidar_situational_graphs.git s_graphs && cd s_graphs 41 | ``` 42 | 43 | 3. Build image 44 | 45 | !!! info 46 | 47 | You can use the build argument `COLCON_JOBS` in order to specify the amount of CPUs cmake will use. 48 | By default it is set to 4. lidar_situational_graphs is memory hungry when building, so choose wisely. 49 | 50 | ```sh 51 | docker build -t sntarg/s_graphs -f docker/humble/Dockerfile \ 52 | --build-arg COLCON_JOBS=4 . 53 | ``` 54 | -------------------------------------------------------------------------------- /docs/imgs/Tf-tree.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/snt-arg/lidar_situational_graphs/301146234f6d83bbedfe50dffa1474cef863da5b/docs/imgs/Tf-tree.png -------------------------------------------------------------------------------- /docs/imgs/banner.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/snt-arg/lidar_situational_graphs/301146234f6d83bbedfe50dffa1474cef863da5b/docs/imgs/banner.png -------------------------------------------------------------------------------- /docs/imgs/main_building.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/snt-arg/lidar_situational_graphs/301146234f6d83bbedfe50dffa1474cef863da5b/docs/imgs/main_building.gif -------------------------------------------------------------------------------- /docs/imgs/main_building.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/snt-arg/lidar_situational_graphs/301146234f6d83bbedfe50dffa1474cef863da5b/docs/imgs/main_building.mp4 -------------------------------------------------------------------------------- /docs/imgs/main_image.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/snt-arg/lidar_situational_graphs/301146234f6d83bbedfe50dffa1474cef863da5b/docs/imgs/main_image.png -------------------------------------------------------------------------------- /docs/imgs/s_graphs+.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/snt-arg/lidar_situational_graphs/301146234f6d83bbedfe50dffa1474cef863da5b/docs/imgs/s_graphs+.gif -------------------------------------------------------------------------------- /docs/imgs/s_graphs_rotating.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/snt-arg/lidar_situational_graphs/301146234f6d83bbedfe50dffa1474cef863da5b/docs/imgs/s_graphs_rotating.gif -------------------------------------------------------------------------------- /docs/imgs/system_architecture.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/snt-arg/lidar_situational_graphs/301146234f6d83bbedfe50dffa1474cef863da5b/docs/imgs/system_architecture.png -------------------------------------------------------------------------------- /docs/index.md: -------------------------------------------------------------------------------- 1 | # LiDAR S-Graphs {align=center} 2 | 3 |
4 | 5 | 6 | 7 |
8 | --- 9 | 10 | **LiDAR Situational Graphs (S-Graphs)** is a ROS2 package for generating in real-time four-layered hierarchical factor graphs for single or multi-floor scenes. It reepresents a scene graph using 3D LiDAR which includes **_Keyframes_** registring the robot poses, **_Walls_** which map wall planes, **_Rooms Layer_** constraining the wall planes using 4 wall-room or 2 wall-room factors, **_Floors_** constraining the rooms within a given floor level. It also supports several graph constraints, such as GPS, IMU acceleration (gravity vector), IMU orientation (magnetic sensor). We have tested this package mostly with Ouster OS-1 and Velodyne (VLP16) sensors in structured indoor environments. This work is a fork of [hdl_graph_slam](https://github.com/koide3/hdl_graph_slam) which as previously in ROS1. 11 | 12 |
13 | 17 |
18 | 19 | **Additional video:** [Dataset Comparison](https://youtu.be/vOH8X7XnQms?si=QtxYDyQhrSazDjqB) 20 | 21 | 22 | 23 | ## 📖 Published Papers 24 | 25 | 26 |
27 | S-Graphs 2.0 -- A Hierarchical-Semantic Optimization and Loop Closure for SLAM 28 | 29 | 30 | @misc{bavle2025sgraphs20hierarchicalsemantic, 31 | title={S-Graphs 2.0 -- A Hierarchical-Semantic Optimization and Loop Closure for SLAM}, 32 | author={Hriday Bavle and Jose Luis Sanchez-Lopez and Muhammad Shaheer and Javier Civera and Holger Voos}, 33 | year={2025}, 34 | eprint={2502.18044}, 35 | archivePrefix={arXiv}, 36 | primaryClass={cs.RO}, 37 | url={https://arxiv.org/abs/2502.18044}, 38 | } 39 | 40 |
41 | 42 |
43 | S-Graphs+: Real-time Localization and Mapping leveraging Hierarchical Representations 44 | 45 | 46 | @ARTICLE{10168233, 47 | author={Bavle, Hriday and Sanchez-Lopez, Jose Luis and Shaheer, Muhammad and Civera, Javier and Voos, Holger}, 48 | journal={IEEE Robotics and Automation Letters}, 49 | title={S-Graphs+: Real-Time Localization and Mapping Leveraging Hierarchical Representations}, 50 | year={2023}, 51 | volume={8}, 52 | number={8}, 53 | pages={4927-4934}, 54 | doi={10.1109/LRA.2023.3290512}} 55 | 56 |
57 |
58 | Situational Graphs for Robot Navigation in Structured Indoor Environments 59 | 60 | @ARTICLE{9826367, 61 | author={Bavle, Hriday and Sanchez-Lopez, Jose Luis and Shaheer, Muhammad and Civera, Javier and Voos, Holger}, 62 | journal={IEEE Robotics and Automation Letters}, 63 | title={Situational Graphs for Robot Navigation in Structured Indoor Environments}, 64 | year={2022}, 65 | volume={7}, 66 | number={4}, 67 | pages={9107-9114}, 68 | doi={10.1109/LRA.2022.3189785}} 69 | 70 |
71 | -------------------------------------------------------------------------------- /lidar_situational_graphs/apps/s_graphs_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2023, University of Luxembourg 3 | All rights reserved. 4 | 5 | Redistributions and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its 16 | contributors may be used to endorse or promote products derived from 17 | this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | */ 29 | 30 | #include "s_graphs/common/s_graphs.hpp" 31 | 32 | int main(int argc, char* argv[]) { 33 | rclcpp::init(argc, argv); 34 | rclcpp::executors::MultiThreadedExecutor multi_executor; 35 | std::shared_ptr s_graphs_node = 36 | std::make_shared(); 37 | 38 | s_graphs_node->declare_parameter("enable_optimization_timer", true); 39 | s_graphs_node->declare_parameter("enable_keyframe_timer", true); 40 | s_graphs_node->declare_parameter("enable_map_publish_timer", true); 41 | 42 | s_graphs_node->start_timers(s_graphs_node->get_parameter("enable_optimization_timer") 43 | .get_parameter_value() 44 | .get(), 45 | s_graphs_node->get_parameter("enable_keyframe_timer") 46 | .get_parameter_value() 47 | .get(), 48 | s_graphs_node->get_parameter("enable_map_publish_timer") 49 | .get_parameter_value() 50 | .get()); 51 | 52 | multi_executor.add_node(s_graphs_node); 53 | multi_executor.spin(); 54 | rclcpp::shutdown(); 55 | return 0; 56 | } 57 | -------------------------------------------------------------------------------- /lidar_situational_graphs/cmake/FindCholmod.cmake: -------------------------------------------------------------------------------- 1 | # Cholmod lib usually requires linking to a blas and lapack library. 2 | # It is up to the user of this module to find a BLAS and link to it. 3 | 4 | if (CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES) 5 | set(CHOLMOD_FIND_QUIETLY TRUE) 6 | endif (CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES) 7 | 8 | find_path(CHOLMOD_INCLUDE_DIR 9 | NAMES 10 | cholmod.h 11 | PATHS 12 | $ENV{CHOLMODDIR} 13 | ${INCLUDE_INSTALL_DIR} 14 | PATH_SUFFIXES 15 | suitesparse 16 | ufsparse 17 | ) 18 | 19 | find_library(CHOLMOD_LIBRARY cholmod PATHS $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 20 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARY}) 21 | 22 | if(CHOLMOD_LIBRARIES) 23 | 24 | get_filename_component(CHOLMOD_LIBDIR ${CHOLMOD_LIBRARIES} PATH) 25 | 26 | find_library(AMD_LIBRARY amd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 27 | if (AMD_LIBRARY) 28 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${AMD_LIBRARY}) 29 | else () 30 | set(CHOLMOD_LIBRARIES FALSE) 31 | endif () 32 | 33 | endif(CHOLMOD_LIBRARIES) 34 | 35 | if(CHOLMOD_LIBRARIES) 36 | 37 | find_library(COLAMD_LIBRARY colamd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 38 | if (COLAMD_LIBRARY) 39 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${COLAMD_LIBRARY}) 40 | else () 41 | set(CHOLMOD_LIBRARIES FALSE) 42 | endif () 43 | 44 | endif(CHOLMOD_LIBRARIES) 45 | 46 | if(CHOLMOD_LIBRARIES) 47 | 48 | find_library(CAMD_LIBRARY camd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 49 | if (CAMD_LIBRARY) 50 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CAMD_LIBRARY}) 51 | else () 52 | set(CHOLMOD_LIBRARIES FALSE) 53 | endif () 54 | 55 | endif(CHOLMOD_LIBRARIES) 56 | 57 | if(CHOLMOD_LIBRARIES) 58 | 59 | find_library(CCOLAMD_LIBRARY ccolamd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 60 | if (CCOLAMD_LIBRARY) 61 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CCOLAMD_LIBRARY}) 62 | else () 63 | set(CHOLMOD_LIBRARIES FALSE) 64 | endif () 65 | 66 | endif(CHOLMOD_LIBRARIES) 67 | 68 | if(CHOLMOD_LIBRARIES) 69 | 70 | find_library(CHOLMOD_METIS_LIBRARY metis PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 71 | if (CHOLMOD_METIS_LIBRARY) 72 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CHOLMOD_METIS_LIBRARY}) 73 | endif () 74 | 75 | endif(CHOLMOD_LIBRARIES) 76 | 77 | if(CHOLMOD_LIBRARIES) 78 | find_library(CHOLMOD_SUITESPARSECONFIG_LIBRARY NAMES suitesparseconfig 79 | PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 80 | if (CHOLMOD_SUITESPARSECONFIG_LIBRARY) 81 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CHOLMOD_SUITESPARSECONFIG_LIBRARY}) 82 | endif () 83 | endif(CHOLMOD_LIBRARIES) 84 | 85 | include(FindPackageHandleStandardArgs) 86 | find_package_handle_standard_args(CHOLMOD DEFAULT_MSG 87 | CHOLMOD_INCLUDE_DIR CHOLMOD_LIBRARIES) 88 | 89 | mark_as_advanced(CHOLMOD_LIBRARIES) -------------------------------------------------------------------------------- /lidar_situational_graphs/cmake/FindG2O.cmake: -------------------------------------------------------------------------------- 1 | # Find the header files 2 | 3 | FIND_PATH(G2O_INCLUDE_DIR g2o/core/base_vertex.h 4 | ${G2O_ROOT}/include 5 | $ENV{G2O_ROOT}/include 6 | $ENV{G2O_ROOT} 7 | /usr/local/include 8 | /usr/include 9 | /opt/local/include 10 | /sw/local/include 11 | /sw/include 12 | /opt/ros/$ENV{ROS_DISTRO}/include 13 | NO_DEFAULT_PATH 14 | ) 15 | 16 | # Macro to unify finding both the debug and release versions of the 17 | # libraries; this is adapted from the OpenSceneGraph FIND_LIBRARY 18 | # macro. 19 | 20 | MACRO(FIND_G2O_LIBRARY MYLIBRARY MYLIBRARYNAME) 21 | 22 | FIND_LIBRARY("${MYLIBRARY}_DEBUG" 23 | NAMES "g2o_${MYLIBRARYNAME}_d" 24 | PATHS 25 | ${G2O_ROOT}/lib/Debug 26 | ${G2O_ROOT}/lib 27 | $ENV{G2O_ROOT}/lib/Debug 28 | $ENV{G2O_ROOT}/lib 29 | /opt/ros/$ENV{ROS_DISTRO}/lib 30 | NO_DEFAULT_PATH 31 | ) 32 | 33 | FIND_LIBRARY("${MYLIBRARY}_DEBUG" 34 | NAMES "g2o_${MYLIBRARYNAME}_d" 35 | PATHS 36 | ~/Library/Frameworks 37 | /Library/Frameworks 38 | /usr/local/lib 39 | /usr/local/lib64 40 | /usr/lib 41 | /usr/lib64 42 | /opt/local/lib 43 | /sw/local/lib 44 | /sw/lib 45 | ) 46 | 47 | FIND_LIBRARY(${MYLIBRARY} 48 | NAMES "g2o_${MYLIBRARYNAME}" 49 | PATHS 50 | ${G2O_ROOT}/lib/Release 51 | ${G2O_ROOT}/lib 52 | $ENV{G2O_ROOT}/lib/Release 53 | $ENV{G2O_ROOT}/lib 54 | /opt/ros/$ENV{ROS_DISTRO}/lib 55 | NO_DEFAULT_PATH 56 | ) 57 | 58 | FIND_LIBRARY(${MYLIBRARY} 59 | NAMES "g2o_${MYLIBRARYNAME}" 60 | PATHS 61 | ~/Library/Frameworks 62 | /Library/Frameworks 63 | /usr/local/lib 64 | /usr/local/lib64 65 | /usr/lib 66 | /usr/lib64 67 | /opt/local/lib 68 | /sw/local/lib 69 | /sw/lib 70 | ) 71 | 72 | IF(NOT ${MYLIBRARY}_DEBUG) 73 | IF(MYLIBRARY) 74 | SET(${MYLIBRARY}_DEBUG ${MYLIBRARY}) 75 | ENDIF(MYLIBRARY) 76 | ENDIF( NOT ${MYLIBRARY}_DEBUG) 77 | 78 | ENDMACRO(FIND_G2O_LIBRARY LIBRARY LIBRARYNAME) 79 | 80 | # Find the core elements 81 | FIND_G2O_LIBRARY(G2O_STUFF_LIBRARY stuff) 82 | FIND_G2O_LIBRARY(G2O_CORE_LIBRARY core) 83 | 84 | # Find the CLI library 85 | FIND_G2O_LIBRARY(G2O_CLI_LIBRARY cli) 86 | 87 | # Find the pluggable solvers 88 | FIND_G2O_LIBRARY(G2O_SOLVER_CHOLMOD solver_cholmod) 89 | FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE solver_csparse) 90 | FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE_EXTENSION csparse_extension) 91 | FIND_G2O_LIBRARY(G2O_SOLVER_DENSE solver_dense) 92 | FIND_G2O_LIBRARY(G2O_SOLVER_PCG solver_pcg) 93 | FIND_G2O_LIBRARY(G2O_SOLVER_SLAM2D_LINEAR solver_slam2d_linear) 94 | FIND_G2O_LIBRARY(G2O_SOLVER_STRUCTURE_ONLY solver_structure_only) 95 | FIND_G2O_LIBRARY(G2O_SOLVER_EIGEN solver_eigen) 96 | 97 | # Find the predefined types 98 | FIND_G2O_LIBRARY(G2O_TYPES_DATA types_data) 99 | FIND_G2O_LIBRARY(G2O_TYPES_ICP types_icp) 100 | FIND_G2O_LIBRARY(G2O_TYPES_SBA types_sba) 101 | FIND_G2O_LIBRARY(G2O_TYPES_SCLAM2D types_sclam2d) 102 | FIND_G2O_LIBRARY(G2O_TYPES_SIM3 types_sim3) 103 | FIND_G2O_LIBRARY(G2O_TYPES_SLAM2D types_slam2d) 104 | FIND_G2O_LIBRARY(G2O_TYPES_SLAM3D types_slam3d) 105 | FIND_G2O_LIBRARY(G2O_TYPES_SLAM3D_ADDONS types_slam3d_addons) 106 | 107 | # G2O solvers declared found if we found at least one solver 108 | SET(G2O_SOLVERS_FOUND "NO") 109 | IF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN) 110 | SET(G2O_SOLVERS_FOUND "YES") 111 | ENDIF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN) 112 | 113 | # G2O itself declared found if we found the core libraries and at least one solver 114 | SET(G2O_FOUND "NO") 115 | IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND) 116 | SET(G2O_FOUND "YES") 117 | ENDIF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND) 118 | -------------------------------------------------------------------------------- /lidar_situational_graphs/config/prefiltering.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | use_sim_time: True 4 | base_link_frame: "body" 5 | use_distance_filter: true 6 | distance_near_thresh: 0.5 7 | distance_far_thresh: 100.0 8 | downsample_method: "VOXELGRID" # NONE, VOXELGRID, or APPROX_VOXELGRID 9 | downsample_resolution: 0.1 10 | outlier_removal_method: "RADIUS" # NONE, RADIUS, or STATISTICAL 11 | statistical_mean_k: 30 12 | statistical_stddev: 1.2 13 | radius_min_neighbors: 0.5 14 | radius_radius: 2.0 15 | -------------------------------------------------------------------------------- /lidar_situational_graphs/config/s_graphs.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | use_sim_time: True 4 | map_frame_id: "map" 5 | odom_frame_id: "odom" 6 | plane_extraction_frame_id: body 7 | plane_visualization_frame_id: body_elevated 8 | use_map2map_transform: false # True if loading a previous posegraph for multi-session 9 | 10 | # Optimization params 11 | g2o_solver_type: "lm_var_cholmod" # gn_var, gn_fix6_3, gn_var_cholmod, lm_var, lm_fix6_3, lm_var_cholmod 12 | # g2o_solver_type: "gn_var_cholmod" # gn_var, gn_fix6_3, gn_var_cholmod, lm_var, lm_fix6_3, lm_var_cholmod 13 | g2o_solver_num_iterations: 512 14 | 15 | # Constraint switches 16 | enable_gps: false 17 | enable_imu_acceleration: false 18 | enable_imu_orientation: false 19 | 20 | # Keyframe registration 21 | max_keyframes_per_update: 1000 22 | keyframe_delta_trans: 2.0 #this param is set from the s_graphs launch file 23 | keyframe_delta_angle: 2.0 #this param is set from the s_graphs launch file 24 | 25 | # Fix first node for optimization stability 26 | fix_first_node: true 27 | fix_first_node_stddev: 1 1 1 1 1 1 28 | 29 | fix_first_node_adaptive: true 30 | 31 | # Loop closure 32 | distance_thresh: 1.0 33 | accum_distance_thresh: 3.0 34 | min_edge_interval: 5.0 35 | fitness_score_thresh: 0.5 36 | 37 | # Scan matching 38 | registration_method: "FAST_GICP" 39 | reg_num_threads: 8 40 | reg_transformation_epsilon: 0.01 41 | reg_maximum_iterations: 64 42 | reg_max_correspondence_distance: 2.5 43 | reg_max_optimizer_iterations: 20 44 | reg_use_reciprocal_correspondences: false 45 | reg_correspondence_randomness: 20 46 | reg_resolution: 1.0 47 | reg_nn_search_method: "DIRECT7" 48 | 49 | # Edge parameters 50 | 51 | ## GPS 52 | gps_edge_robust_kernel: "NONE" 53 | gps_edge_robust_kernel_size: 1.0 54 | gps_edge_stddev_xy: 20.0 55 | gps_edge_stddev_z: 5.0 56 | 57 | ## IMU orientation 58 | imu_orientation_edge_robust_kernel: "NONE" 59 | imu_orientation_edge_stddev: 1.0 60 | 61 | ## IMU acceleration 62 | imu_acceleration_edge_robust_kernel: "NONE" 63 | imu_acceleration_edge_stddev: 1.0 64 | 65 | # Ground plane 66 | floor_edge_robust_kernel: "NONE" 67 | floor_edge_stddev: 10.0 68 | 69 | # Scan matching 70 | odometry_edge_robust_kernel: "Huber" # NONE, Cauchy, DCS, Fair, GemanMcClure, Huber, PseudoHuber, Saturated, Tukey, Welsch 71 | odometry_edge_robust_kernel_size: 1.0 72 | loop_closure_edge_robust_kernel: "Huber" 73 | loop_closure_edge_robust_kernel_size: 1.0 74 | use_const_inf_matrix: false 75 | const_stddev_x: 0.5 76 | const_stddev_q: 0.1 77 | var_gain_a: 20.0 78 | min_stddev_x: 0.1 79 | max_stddev_x: 5.0 80 | min_stddev_q: 0.2 81 | max_stddev_q: 0.8 82 | 83 | # Plane detection 84 | extract_planar_surfaces: true 85 | min_seg_points: 100 86 | use_euclidean_filter: true 87 | min_horizontal_inliers: 100 88 | min_vertical_inliers: 100 89 | cluster_min_size: 50 90 | cluster_clean_tolerance: 0.2 91 | cluster_seperation_tolerance: 2.0 92 | plane_ransac_itr: 500 93 | plane_ransac_acc: 0.01 94 | 95 | # Mapping 96 | stand_still_time: 3.0 97 | stand_still_delta: 0.1 98 | record_kf_when_still: false 99 | odom_pc_sync_queue: 1000 100 | graph_update_interval: 3.0 101 | keyframe_timer_update_interval: 1.0 102 | map_cloud_update_interval: 3.0 103 | marker_duration: 10 104 | map_cloud_resolution: 0.05 105 | map_cloud_pub_resolution: 0.0 106 | fast_mapping: false #if set to true increase the decay time of map_points and wall_points in rviz 107 | save_dense_map: false 108 | viz_dense_map: false #true: might make the rviz visualization slow (#this param is set from the s_graphs launch file) 109 | floor_level_viz_height: 5.0 110 | room_viz_height: 6.0 111 | floor_node_viz_height: 8.0 112 | viz_all_floor_cloud: true 113 | 114 | keyframe_window_size: 1 115 | plane_information: 0.1 116 | room_information: 0.1 117 | corridor_information: 0.1 118 | plane_dist_threshold: 0.35 119 | plane_points_dist: 2.0 120 | constant_covariance: true 121 | min_plane_points: 100 122 | min_plane_points_opti: 100 123 | dupl_plane_matching_information: 0.1 124 | optimization_window_size: 5 125 | optimization_type: "FLOOR_GLOBAL" 126 | 127 | # Visualization 128 | line_color_r: 0.0 129 | line_color_g: 0.0 130 | line_color_b: 0.0 131 | 132 | floor_cube_color_r: 1.0 133 | floor_cube_color_g: 0.0 134 | floor_cube_color_b: 1.0 135 | 136 | line_marker_size: 0.04 137 | kf_marker_size: 0.3 138 | room_marker_size: 0.5 139 | floor_marker_size: 0.6 140 | 141 | use_floor_color_for_map: true #only work if code is built with rgb flag on 142 | 143 | -------------------------------------------------------------------------------- /lidar_situational_graphs/config/scan_matching.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | use_sim_time: True 4 | publish_tf: true 5 | odom_frame_id: "odom" 6 | robot_odom_frame_id: "odom" 7 | keyframe_delta_trans: 0.25 8 | keyframe_delta_angle: 2.0 9 | keyframe_delta_time: 10000.0 10 | transform_thresholding: false 11 | max_acceptable_trans: 1.0 12 | max_acceptable_angle: 1.0 13 | downsample_method: "NONE" 14 | downsample_resolution: 0.1 15 | # ICP, GICP, NDT, GICP_OMP, NDT_OMP, FAST_GICP (recommended), or FAST_VGICP 16 | registration_method: "FAST_GICP" 17 | reg_num_threads: 8 18 | reg_transformation_epsilon: 0.01 19 | reg_maximum_iterations: 64 20 | reg_max_correspondence_distance: 2.0 21 | reg_max_optimizer_iterations: 20 22 | reg_use_reciprocal_correspondences: false 23 | reg_correspondence_randomness: 20 24 | reg_resolution: 1.0 25 | reg_nn_search_method: "DIRECT7" 26 | -------------------------------------------------------------------------------- /lidar_situational_graphs/include/g2o/edge_loop_closure.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2023, University of Luxembourg 3 | All rights reserved. 4 | 5 | Redistributions and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its 16 | contributors may be used to endorse or promote products derived from 17 | this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | */ 29 | 30 | // g2o - General Graph Optimization 31 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 32 | // All rights reserved. 33 | // 34 | // Redistribution and use in source and binary forms, with or without 35 | // modification, are permitted provided that the following conditions are 36 | // met: 37 | // 38 | // * Redistributions of source code must retain the above copyright notice, 39 | // this list of conditions and the following disclaimer. 40 | // * Redistributions in binary form must reproduce the above copyright 41 | // notice, this list of conditions and the following disclaimer in the 42 | // documentation and/or other materials provided with the distribution. 43 | // 44 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 45 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 46 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 47 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 48 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 49 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 50 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 51 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 52 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 53 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 54 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 55 | 56 | #ifndef EDGE_LOOP_CLOSURE_HPP 57 | #define EDGE_LOOP_CLOSURE_HPP 58 | 59 | #include 60 | 61 | #include 62 | 63 | namespace g2o { 64 | 65 | class EdgeLoopClosure : public EdgeSE3 { 66 | public: 67 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 68 | EdgeLoopClosure() : EdgeSE3() {} 69 | }; 70 | 71 | } // namespace g2o 72 | 73 | #endif // EDGE_PLANE_PARALLEL_HPP 74 | -------------------------------------------------------------------------------- /lidar_situational_graphs/include/g2o/edge_multi_se3.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2023, University of Luxembourg 3 | All rights reserved. 4 | 5 | Redistributions and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its 16 | contributors may be used to endorse or promote products derived from 17 | this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | */ 29 | 30 | // g2o - General Graph Optimization 31 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 32 | // All rights reserved. 33 | // 34 | // Redistribution and use in source and binary forms, with or without 35 | // modification, are permitted provided that the following conditions are 36 | // met: 37 | // 38 | // * Redistributions of source code must retain the above copyright notice, 39 | // this list of conditions and the following disclaimer. 40 | // * Redistributions in binary form must reproduce the above copyright 41 | // notice, this list of conditions and the following disclaimer in the 42 | // documentation and/or other materials provided with the distribution. 43 | // 44 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 45 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 46 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 47 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 48 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 49 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 50 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 51 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 52 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 53 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 54 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 55 | 56 | #ifndef MULTI_SE3_HPP 57 | #define MULTI_SE3_HPP 58 | 59 | #include 60 | #include 61 | #include 62 | #include 63 | #include 64 | 65 | #include 66 | 67 | #include "g2o/vertex_room.hpp" 68 | 69 | namespace g2o { 70 | class EdgeMultiSE3 : public g2o::BaseMultiEdge<6, Isometry3> { 71 | public: 72 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 73 | EdgeMultiSE3() : g2o::BaseMultiEdge<6, Isometry3>() { resize(3); } 74 | 75 | void computeError() override; 76 | void setMeasurement(const g2o::Isometry3& m) override { _measurement = m; } 77 | virtual bool read(std::istream& is) override; 78 | virtual bool write(std::ostream& os) const override; 79 | }; 80 | } // namespace g2o 81 | #endif // MULTI_SE3_HPP -------------------------------------------------------------------------------- /lidar_situational_graphs/include/g2o/edge_plane_identity.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2023, University of Luxembourg 3 | All rights reserved. 4 | 5 | Redistributions and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its 16 | contributors may be used to endorse or promote products derived from 17 | this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | */ 29 | 30 | // g2o - General Graph Optimization 31 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 32 | // All rights reserved. 33 | // 34 | // Redistribution and use in source and binary forms, with or without 35 | // modification, are permitted provided that the following conditions are 36 | // met: 37 | // 38 | // * Redistributions of source code must retain the above copyright notice, 39 | // this list of conditions and the following disclaimer. 40 | // * Redistributions in binary form must reproduce the above copyright 41 | // notice, this list of conditions and the following disclaimer in the 42 | // documentation and/or other materials provided with the distribution. 43 | // 44 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 45 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 46 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 47 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 48 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 49 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 50 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 51 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 52 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 53 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 54 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 55 | 56 | #ifndef EDGE_PLANE_IDENTITY_HPP 57 | #define EDGE_PLANE_IDENTITY_HPP 58 | 59 | #include 60 | #include 61 | 62 | #include 63 | 64 | namespace g2o { 65 | 66 | /** 67 | * @brief A modified version of g2o::EdgePlane. This class takes care of flipped plane 68 | * normals. 69 | * 70 | */ 71 | class EdgePlaneIdentity 72 | : public BaseBinaryEdge<4, Eigen::Vector4d, VertexPlane, VertexPlane> { 73 | public: 74 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 75 | EdgePlaneIdentity() : BaseBinaryEdge<4, Eigen::Vector4d, VertexPlane, VertexPlane>() { 76 | _information.setIdentity(); 77 | _error.setZero(); 78 | } 79 | void computeError() override; 80 | virtual bool read(std::istream& is) override; 81 | 82 | virtual bool write(std::ostream& os) const override; 83 | 84 | virtual void setMeasurement(const Eigen::Vector4d& m) override { _measurement = m; } 85 | 86 | virtual int measurementDimension() const override { return 4; } 87 | }; 88 | 89 | } // namespace g2o 90 | 91 | #endif // EDGE_PLANE_PARALLEL_HPP 92 | -------------------------------------------------------------------------------- /lidar_situational_graphs/include/g2o/edge_plane_prior.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2023, University of Luxembourg 3 | All rights reserved. 4 | 5 | Redistributions and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its 16 | contributors may be used to endorse or promote products derived from 17 | this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | */ 29 | 30 | // g2o - General Graph Optimization 31 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 32 | // All rights reserved. 33 | // 34 | // Redistribution and use in source and binary forms, with or without 35 | // modification, are permitted provided that the following conditions are 36 | // met: 37 | // 38 | // * Redistributions of source code must retain the above copyright notice, 39 | // this list of conditions and the following disclaimer. 40 | // * Redistributions in binary form must reproduce the above copyright 41 | // notice, this list of conditions and the following disclaimer in the 42 | // documentation and/or other materials provided with the distribution. 43 | // 44 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 45 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 46 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 47 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 48 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 49 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 50 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 51 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 52 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 53 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 54 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 55 | 56 | #ifndef EDGE_PLANE_PRIOR_HPP 57 | #define EDGE_PLANE_PRIOR_HPP 58 | 59 | #include 60 | #include 61 | 62 | #include 63 | 64 | namespace g2o { 65 | class EdgePlanePriorNormal 66 | : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexPlane> { 67 | public: 68 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 69 | EdgePlanePriorNormal() : g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexPlane>() {} 70 | 71 | void computeError() override; 72 | 73 | void setMeasurement(const Eigen::Vector3d& m) override { _measurement = m; } 74 | 75 | virtual bool read(std::istream& is) override; 76 | virtual bool write(std::ostream& os) const override; 77 | }; 78 | 79 | class EdgePlanePriorDistance : public g2o::BaseUnaryEdge<1, double, g2o::VertexPlane> { 80 | public: 81 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 82 | EdgePlanePriorDistance() : g2o::BaseUnaryEdge<1, double, g2o::VertexPlane>() {} 83 | 84 | void computeError() override; 85 | 86 | void setMeasurement(const double& m) override { _measurement = m; } 87 | 88 | virtual bool read(std::istream& is) override; 89 | virtual bool write(std::ostream& os) const override; 90 | }; 91 | } // namespace g2o 92 | 93 | #endif // EDGE_SE3_PRIORXY_HPP 94 | -------------------------------------------------------------------------------- /lidar_situational_graphs/include/g2o/edge_se3_plane.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2023, University of Luxembourg 3 | All rights reserved. 4 | 5 | Redistributions and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its 16 | contributors may be used to endorse or promote products derived from 17 | this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | */ 29 | 30 | // g2o - General Graph Optimization 31 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 32 | // All rights reserved. 33 | // 34 | // Redistribution and use in source and binary forms, with or without 35 | // modification, are permitted provided that the following conditions are 36 | // met: 37 | // 38 | // * Redistributions of source code must retain the above copyright notice, 39 | // this list of conditions and the following disclaimer. 40 | // * Redistributions in binary form must reproduce the above copyright 41 | // notice, this list of conditions and the following disclaimer in the 42 | // documentation and/or other materials provided with the distribution. 43 | // 44 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 45 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 46 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 47 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 48 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 49 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 50 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 51 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 52 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 53 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 54 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 55 | 56 | #ifndef KKL_G2O_EDGE_SE3_PLANE_HPP 57 | #define KKL_G2O_EDGE_SE3_PLANE_HPP 58 | 59 | #include 60 | #include 61 | #include 62 | 63 | namespace g2o { 64 | class EdgeSE3Plane 65 | : public g2o::BaseBinaryEdge<3, g2o::Plane3D, g2o::VertexSE3, g2o::VertexPlane> { 66 | public: 67 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 68 | EdgeSE3Plane() 69 | : BaseBinaryEdge<3, g2o::Plane3D, g2o::VertexSE3, g2o::VertexPlane>() {} 70 | 71 | void computeError() override; 72 | 73 | void setMeasurement(const g2o::Plane3D& m) override { _measurement = m; } 74 | 75 | virtual bool read(std::istream& is) override; 76 | virtual bool write(std::ostream& os) const override; 77 | }; 78 | } // namespace g2o 79 | 80 | #endif 81 | -------------------------------------------------------------------------------- /lidar_situational_graphs/include/g2o/edge_se3_point_to_plane.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2023, University of Luxembourg 3 | All rights reserved. 4 | 5 | Redistributions and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its 16 | contributors may be used to endorse or promote products derived from 17 | this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | */ 29 | 30 | // g2o - General Graph Optimization 31 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 32 | // All rights reserved. 33 | // 34 | // Redistribution and use in source and binary forms, with or without 35 | // modification, are permitted provided that the following conditions are 36 | // met: 37 | // 38 | // * Redistributions of source code must retain the above copyright notice, 39 | // this list of conditions and the following disclaimer. 40 | // * Redistributions in binary form must reproduce the above copyright 41 | // notice, this list of conditions and the following disclaimer in the 42 | // documentation and/or other materials provided with the distribution. 43 | // 44 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 45 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 46 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 47 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 48 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 49 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 50 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 51 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 52 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 53 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 54 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 55 | 56 | #ifndef KKL_G2O_EDGE_SE3_POINT_TO_PLANE_HPP 57 | #define KKL_G2O_EDGE_SE3_POINT_TO_PLANE_HPP 58 | 59 | #include 60 | #include 61 | #include 62 | 63 | namespace g2o { 64 | class EdgeSE3PointToPlane 65 | : public g2o::BaseBinaryEdge<1, Eigen::Matrix4d, g2o::VertexSE3, g2o::VertexPlane> { 66 | public: 67 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 68 | EdgeSE3PointToPlane() 69 | : BaseBinaryEdge<1, Eigen::Matrix4d, g2o::VertexSE3, g2o::VertexPlane>() {} 70 | 71 | void computeError() override; 72 | 73 | void setMeasurement(const Eigen::Matrix4d& m) override { _measurement = m; } 74 | 75 | virtual bool read(std::istream& is) override; 76 | virtual bool write(std::ostream& os) const override; 77 | }; 78 | } // namespace g2o 79 | 80 | #endif 81 | -------------------------------------------------------------------------------- /lidar_situational_graphs/include/g2o/edge_se3_priorquat.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2023, University of Luxembourg 3 | All rights reserved. 4 | 5 | Redistributions and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its 16 | contributors may be used to endorse or promote products derived from 17 | this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | */ 29 | 30 | // g2o - General Graph Optimization 31 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 32 | // All rights reserved. 33 | // 34 | // Redistribution and use in source and binary forms, with or without 35 | // modification, are permitted provided that the following conditions are 36 | // met: 37 | // 38 | // * Redistributions of source code must retain the above copyright notice, 39 | // this list of conditions and the following disclaimer. 40 | // * Redistributions in binary form must reproduce the above copyright 41 | // notice, this list of conditions and the following disclaimer in the 42 | // documentation and/or other materials provided with the distribution. 43 | // 44 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 45 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 46 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 47 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 48 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 49 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 50 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 51 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 52 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 53 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 54 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 55 | 56 | #ifndef KKL_G2O_EDGE_SE3_PRIORQUAT_HPP 57 | #define KKL_G2O_EDGE_SE3_PRIORQUAT_HPP 58 | 59 | #include 60 | #include 61 | 62 | namespace g2o { 63 | class EdgeSE3PriorQuat 64 | : public g2o::BaseUnaryEdge<3, Eigen::Quaterniond, g2o::VertexSE3> { 65 | public: 66 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 67 | EdgeSE3PriorQuat() : g2o::BaseUnaryEdge<3, Eigen::Quaterniond, g2o::VertexSE3>() {} 68 | 69 | void computeError() override; 70 | 71 | void setMeasurement(const Eigen::Quaterniond& m) override; 72 | 73 | virtual bool read(std::istream& is) override; 74 | virtual bool write(std::ostream& os) const override; 75 | }; 76 | } // namespace g2o 77 | 78 | #endif 79 | -------------------------------------------------------------------------------- /lidar_situational_graphs/include/g2o/edge_se3_priorvec.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2023, University of Luxembourg 3 | All rights reserved. 4 | 5 | Redistributions and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its 16 | contributors may be used to endorse or promote products derived from 17 | this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | */ 29 | 30 | // g2o - General Graph Optimization 31 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 32 | // All rights reserved. 33 | // 34 | // Redistribution and use in source and binary forms, with or without 35 | // modification, are permitted provided that the following conditions are 36 | // met: 37 | // 38 | // * Redistributions of source code must retain the above copyright notice, 39 | // this list of conditions and the following disclaimer. 40 | // * Redistributions in binary form must reproduce the above copyright 41 | // notice, this list of conditions and the following disclaimer in the 42 | // documentation and/or other materials provided with the distribution. 43 | // 44 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 45 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 46 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 47 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 48 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 49 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 50 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 51 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 52 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 53 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 54 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 55 | 56 | #ifndef KKL_G2O_EDGE_SE3_PRIORVEC_HPP 57 | #define KKL_G2O_EDGE_SE3_PRIORVEC_HPP 58 | 59 | #include 60 | #include 61 | 62 | namespace g2o { 63 | class EdgeSE3PriorVec 64 | : public g2o::BaseUnaryEdge<3, Eigen::Matrix, g2o::VertexSE3> { 65 | public: 66 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 67 | EdgeSE3PriorVec() 68 | : g2o::BaseUnaryEdge<3, Eigen::Matrix, g2o::VertexSE3>() {} 69 | 70 | void computeError() override; 71 | 72 | void setMeasurement(const Eigen::Matrix& m) override; 73 | virtual bool read(std::istream& is) override; 74 | virtual bool write(std::ostream& os) const override; 75 | }; 76 | } // namespace g2o 77 | 78 | #endif 79 | -------------------------------------------------------------------------------- /lidar_situational_graphs/include/g2o/edge_se3_priorxy.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2023, University of Luxembourg 3 | All rights reserved. 4 | 5 | Redistributions and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its 16 | contributors may be used to endorse or promote products derived from 17 | this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | */ 29 | 30 | // g2o - General Graph Optimization 31 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 32 | // All rights reserved. 33 | // 34 | // Redistribution and use in source and binary forms, with or without 35 | // modification, are permitted provided that the following conditions are 36 | // met: 37 | // 38 | // * Redistributions of source code must retain the above copyright notice, 39 | // this list of conditions and the following disclaimer. 40 | // * Redistributions in binary form must reproduce the above copyright 41 | // notice, this list of conditions and the following disclaimer in the 42 | // documentation and/or other materials provided with the distribution. 43 | // 44 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 45 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 46 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 47 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 48 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 49 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 50 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 51 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 52 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 53 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 54 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 55 | 56 | #ifndef EDGE_SE3_PRIORXY_HPP 57 | #define EDGE_SE3_PRIORXY_HPP 58 | 59 | #include 60 | #include 61 | 62 | namespace g2o { 63 | class EdgeSE3PriorXY : public g2o::BaseUnaryEdge<2, Eigen::Vector2d, g2o::VertexSE3> { 64 | public: 65 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 66 | EdgeSE3PriorXY() : g2o::BaseUnaryEdge<2, Eigen::Vector2d, g2o::VertexSE3>() {} 67 | 68 | void computeError() override; 69 | 70 | void setMeasurement(const Eigen::Vector2d& m) override { _measurement = m; } 71 | 72 | virtual bool read(std::istream& is) override; 73 | virtual bool write(std::ostream& os) const override; 74 | }; 75 | } // namespace g2o 76 | 77 | #endif // EDGE_SE3_PRIORXY_HPP 78 | -------------------------------------------------------------------------------- /lidar_situational_graphs/include/g2o/edge_se3_priorxyz.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2023, University of Luxembourg 3 | All rights reserved. 4 | 5 | Redistributions and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its 16 | contributors may be used to endorse or promote products derived from 17 | this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | */ 29 | 30 | // g2o - General Graph Optimization 31 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 32 | // All rights reserved. 33 | // 34 | // Redistribution and use in source and binary forms, with or without 35 | // modification, are permitted provided that the following conditions are 36 | // met: 37 | // 38 | // * Redistributions of source code must retain the above copyright notice, 39 | // this list of conditions and the following disclaimer. 40 | // * Redistributions in binary form must reproduce the above copyright 41 | // notice, this list of conditions and the following disclaimer in the 42 | // documentation and/or other materials provided with the distribution. 43 | // 44 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 45 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 46 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 47 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 48 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 49 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 50 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 51 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 52 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 53 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 54 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 55 | 56 | #ifndef KKL_G2O_EDGE_SE3_PRIORXYZ_HPP 57 | #define KKL_G2O_EDGE_SE3_PRIORXYZ_HPP 58 | 59 | #include 60 | #include 61 | 62 | namespace g2o { 63 | class EdgeSE3PriorXYZ : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexSE3> { 64 | public: 65 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 66 | EdgeSE3PriorXYZ() : g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexSE3>() {} 67 | 68 | void computeError() override; 69 | 70 | void setMeasurement(const Eigen::Vector3d& m) override { _measurement = m; } 71 | 72 | virtual bool read(std::istream& is) override; 73 | virtual bool write(std::ostream& os) const override; 74 | }; 75 | } // namespace g2o 76 | 77 | #endif 78 | -------------------------------------------------------------------------------- /lidar_situational_graphs/include/g2o/edge_se3_two_planes.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2023, University of Luxembourg 3 | All rights reserved. 4 | 5 | Redistributions and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its 16 | contributors may be used to endorse or promote products derived from 17 | this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | */ 29 | 30 | // g2o - General Graph Optimization 31 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 32 | // All rights reserved. 33 | // 34 | // Redistribution and use in source and binary forms, with or without 35 | // modification, are permitted provided that the following conditions are 36 | // met: 37 | // 38 | // * Redistributions of source code must retain the above copyright notice, 39 | // this list of conditions and the following disclaimer. 40 | // * Redistributions in binary form must reproduce the above copyright 41 | // notice, this list of conditions and the following disclaimer in the 42 | // documentation and/or other materials provided with the distribution. 43 | // 44 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 45 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 46 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 47 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 48 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 49 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 50 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 51 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 52 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 53 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 54 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 55 | 56 | #ifndef KKL_G2O_EDGE_SE3_TWO_PLANES_HPP 57 | #define KKL_G2O_EDGE_SE3_TWO_PLANES_HPP 58 | #include 59 | #include 60 | #include 61 | #include 62 | 63 | #include 64 | #include 65 | 66 | namespace g2o { 67 | class EdgeSE3PlanePlane : public g2o::BaseMultiEdge<3, Eigen::Vector3d> { 68 | public: 69 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 70 | EdgeSE3PlanePlane() : BaseMultiEdge<3, Eigen::Vector3d>() { resize(3); } 71 | 72 | void computeError() override; 73 | 74 | void setMeasurement(const Eigen::Vector3d& m) override { _measurement = m; } 75 | 76 | virtual bool read(std::istream& is) override; 77 | virtual bool write(std::ostream& os) const override; 78 | }; 79 | } // namespace g2o 80 | 81 | #endif 82 | -------------------------------------------------------------------------------- /lidar_situational_graphs/include/g2o/edge_se3_two_rooms.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2023, University of Luxembourg 3 | All rights reserved. 4 | 5 | Redistributions and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its 16 | contributors may be used to endorse or promote products derived from 17 | this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | */ 29 | 30 | // g2o - General Graph Optimization 31 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 32 | // All rights reserved. 33 | // 34 | // Redistribution and use in source and binary forms, with or without 35 | // modification, are permitted provided that the following conditions are 36 | // met: 37 | // 38 | // * Redistributions of source code must retain the above copyright notice, 39 | // this list of conditions and the following disclaimer. 40 | // * Redistributions in binary form must reproduce the above copyright 41 | // notice, this list of conditions and the following disclaimer in the 42 | // documentation and/or other materials provided with the distribution. 43 | // 44 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 45 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 46 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 47 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 48 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 49 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 50 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 51 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 52 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 53 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 54 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 55 | 56 | #ifndef EDGE_SE3_TWO_ROOMS_H 57 | #define EDGE_SE3_TWO_ROOMS_H 58 | 59 | #include 60 | #include 61 | 62 | #include 63 | #include 64 | 65 | namespace g2o { 66 | 67 | class EdgeSE3RoomRoom : public g2o::BaseMultiEdge<6, g2o::Isometry3> { 68 | public: 69 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 70 | EdgeSE3RoomRoom() : BaseMultiEdge<6, g2o::Isometry3>() { resize(3); }; 71 | 72 | void computeError() override; 73 | void setMeasurement(const g2o::Isometry3 &m) override { _measurement = m; } 74 | virtual bool read(std::istream &is) override; 75 | virtual bool write(std::ostream &os) const override; 76 | }; 77 | 78 | } // namespace g2o 79 | 80 | #endif // EDGE_SE3_TWO_ROOMS_H 81 | -------------------------------------------------------------------------------- /lidar_situational_graphs/include/g2o/edge_wall_two_planes.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2023, University of Luxembourg 3 | All rights reserved. 4 | 5 | Redistributions and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its 16 | contributors may be used to endorse or promote products derived from 17 | this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | */ 29 | 30 | // g2o - General Graph Optimization 31 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 32 | // All rights reserved. 33 | // 34 | // Redistribution and use in source and binary forms, with or without 35 | // modification, are permitted provided that the following conditions are 36 | // met: 37 | // 38 | // * Redistributions of source code must retain the above copyright notice, 39 | // this list of conditions and the following disclaimer. 40 | // * Redistributions in binary form must reproduce the above copyright 41 | // notice, this list of conditions and the following disclaimer in the 42 | // documentation and/or other materials provided with the distribution. 43 | // 44 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 45 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 46 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 47 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 48 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 49 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 50 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 51 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 52 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 53 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 54 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 55 | 56 | #ifndef EDGE_WALL_2_PLANES_HPP 57 | #define EDGE_WALL_2_PLANES_HPP 58 | 59 | #include 60 | #include 61 | #include 62 | #include 63 | 64 | #include 65 | 66 | #include "g2o/vertex_wall.hpp" 67 | namespace g2o { 68 | 69 | /* Define Wall edge with wall surfaces here*/ 70 | 71 | class EdgeWall2Planes : public BaseMultiEdge<3, Eigen::Vector3d> { 72 | public: 73 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 74 | EdgeWall2Planes(Eigen::Vector3d wall_point) : BaseMultiEdge<3, Eigen::Vector3d>() { 75 | resize(3); 76 | _wall_point = wall_point; 77 | } 78 | 79 | void computeError() override; 80 | 81 | virtual bool read(std::istream& is) override; 82 | 83 | virtual bool write(std::ostream& os) const override; 84 | 85 | Eigen::Vector3d get_wall_point() { return _wall_point; } 86 | 87 | private: 88 | void correct_plane_direction(Eigen::Vector4d& plane); 89 | Eigen::Vector3d _wall_point; 90 | }; 91 | 92 | } // namespace g2o 93 | #endif // EDGE_WALL_2_PLANES_HPP 94 | -------------------------------------------------------------------------------- /lidar_situational_graphs/include/g2o/robust_kernel_io.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2023, University of Luxembourg 3 | All rights reserved. 4 | 5 | Redistributions and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its 16 | contributors may be used to endorse or promote products derived from 17 | this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | */ 29 | 30 | #ifndef ROBUST_KERNEL_IO_HPP 31 | #define ROBUST_KERNEL_IO_HPP 32 | 33 | #include 34 | 35 | #include 36 | 37 | namespace g2o { 38 | 39 | std::string kernel_type(g2o::RobustKernel* kernel); 40 | 41 | bool save_robust_kernels(const std::string& filename, 42 | g2o::SparseOptimizer* graph); 43 | 44 | bool load_robust_kernels(const std::string& filename, 45 | g2o::SparseOptimizer* graph); 46 | 47 | } // namespace g2o 48 | 49 | #endif // ROBUST_KERNEL_IO_HPP 50 | -------------------------------------------------------------------------------- /lidar_situational_graphs/include/g2o/vertex_floor.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2023, University of Luxembourg 3 | All rights reserved. 4 | 5 | Redistributions and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its 16 | contributors may be used to endorse or promote products derived from 17 | this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | */ 29 | 30 | // g2o - General Graph Optimization 31 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 32 | // All rights reserved. 33 | // 34 | // Redistribution and use in source and binary forms, with or without 35 | // modification, are permitted provided that the following conditions are 36 | // met: 37 | // 38 | // * Redistributions of source code must retain the above copyright notice, 39 | // this list of conditions and the following disclaimer. 40 | // * Redistributions in binary form must reproduce the above copyright 41 | // notice, this list of conditions and the following disclaimer in the 42 | // documentation and/or other materials provided with the distribution. 43 | // 44 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 45 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 46 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 47 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 48 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 49 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 50 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 51 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 52 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 53 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 54 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 55 | 56 | #ifndef G2O_VERTEX_FLOOR 57 | #define G2O_VERTEX_FLOOR 58 | 59 | #include 60 | 61 | #include 62 | 63 | #include "vertex_room.hpp" 64 | 65 | namespace g2o { 66 | 67 | class G2O_TYPES_SLAM3D_API VertexFloor : public VertexRoom { 68 | public: 69 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 70 | VertexFloor() { 71 | _numOplusCalls = 0; 72 | setToOriginImpl(); 73 | updateCache(); 74 | } 75 | }; 76 | 77 | } // namespace g2o 78 | 79 | #endif 80 | -------------------------------------------------------------------------------- /lidar_situational_graphs/include/g2o/vertex_infinite_room.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2023, University of Luxembourg 3 | All rights reserved. 4 | 5 | Redistributions and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its 16 | contributors may be used to endorse or promote products derived from 17 | this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | */ 29 | 30 | // g2o - General Graph Optimization 31 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 32 | // All rights reserved. 33 | // 34 | // Redistribution and use in source and binary forms, with or without 35 | // modification, are permitted provided that the following conditions are 36 | // met: 37 | // 38 | // * Redistributions of source code must retain the above copyright notice, 39 | // this list of conditions and the following disclaimer. 40 | // * Redistributions in binary form must reproduce the above copyright 41 | // notice, this list of conditions and the following disclaimer in the 42 | // documentation and/or other materials provided with the distribution. 43 | // 44 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 45 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 46 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 47 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 48 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 49 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 50 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 51 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 52 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 53 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 54 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 55 | 56 | #ifndef G2O_VERTEX_INFINITE_ROOM_H 57 | #define G2O_VERTEX_INFINITE_ROOM_H 58 | 59 | // #include "g2o/config.h" 60 | #include 61 | 62 | #include 63 | 64 | #include "g2o/core/base_vertex.h" 65 | #include "g2o/core/hyper_graph_action.h" 66 | 67 | namespace g2o { 68 | 69 | class G2O_TYPES_SLAM3D_API VertexInfiniteRoom : public BaseVertex<1, double> { 70 | public: 71 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 72 | VertexInfiniteRoom() {} 73 | 74 | virtual void setToOriginImpl() { _estimate = 0; } 75 | 76 | virtual bool setEstimateDataImpl(const number_t* est) { 77 | _estimate = est[0]; 78 | 79 | return true; 80 | } 81 | 82 | virtual bool getEstimateData(number_t* est) const { 83 | est[0] = _estimate; 84 | return true; 85 | } 86 | 87 | virtual int estimateDimension() const { return 1; } 88 | 89 | virtual bool setMinimalEstimateDataImpl(const number_t* est) { 90 | setEstimateData(est); 91 | return true; 92 | } 93 | 94 | virtual bool getMinimalEstimateData(number_t* est) const { 95 | getEstimateData(est); 96 | return true; 97 | } 98 | 99 | virtual int minimalEstimateDimension() const { return 1; } 100 | 101 | virtual void oplusImpl(const number_t* update) { _estimate += update[0]; } 102 | 103 | virtual bool read(std::istream& is) { 104 | is >> _estimate; 105 | return true; 106 | } 107 | 108 | virtual bool write(std::ostream& os) const { 109 | os << _estimate; 110 | return true; 111 | } 112 | }; 113 | 114 | } // namespace g2o 115 | 116 | #endif 117 | -------------------------------------------------------------------------------- /lidar_situational_graphs/include/g2o/vertex_wall.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2023, University of Luxembourg 3 | All rights reserved. 4 | 5 | Redistributions and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its 16 | contributors may be used to endorse or promote products derived from 17 | this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | */ 29 | 30 | // g2o - General Graph Optimization 31 | // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 32 | // All rights reserved. 33 | // 34 | // Redistribution and use in source and binary forms, with or without 35 | // modification, are permitted provided that the following conditions are 36 | // met: 37 | // 38 | // * Redistributions of source code must retain the above copyright notice, 39 | // this list of conditions and the following disclaimer. 40 | // * Redistributions in binary form must reproduce the above copyright 41 | // notice, this list of conditions and the following disclaimer in the 42 | // documentation and/or other materials provided with the distribution. 43 | // 44 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 45 | // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 46 | // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 47 | // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 48 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 49 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 50 | // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 51 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 52 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 53 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 54 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 55 | 56 | #ifndef G2O_VERTEX_WALL_XYZ_H 57 | #define G2O_VERTEX_WALL_XYZ_H 58 | 59 | //#include "g2o/config.h" 60 | #include 61 | 62 | #include 63 | 64 | #include "g2o/core/base_vertex.h" 65 | #include "g2o/core/hyper_graph_action.h" 66 | 67 | namespace g2o { 68 | 69 | class G2O_TYPES_SLAM3D_API VertexWall : public BaseVertex<3, Vector3> { 70 | public: 71 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 72 | VertexWall() {} 73 | 74 | virtual void setToOriginImpl() { _estimate.fill(0.); } 75 | 76 | virtual bool setEstimateDataImpl(const number_t* est) { 77 | Eigen::Map _est(est); 78 | _estimate = _est; 79 | 80 | return true; 81 | } 82 | 83 | virtual bool getEstimateData(number_t* est) const { 84 | Eigen::Map _est(est); 85 | _est = _estimate; 86 | return true; 87 | } 88 | 89 | virtual int estimateDimension() const { return 3; } 90 | 91 | virtual bool setMinimalEstimateDataImpl(const number_t* est) { 92 | _estimate = Eigen::Map(est); 93 | return true; 94 | } 95 | 96 | virtual bool getMinimalEstimateData(number_t* est) const { 97 | Eigen::Map v(est); 98 | v = _estimate; 99 | return true; 100 | } 101 | 102 | virtual int minimalEstimateDimension() const { return 3; } 103 | 104 | virtual void oplusImpl(const number_t* update) { 105 | for (int i = 0; i < 3; i++) _estimate[i] += update[i]; 106 | } 107 | 108 | virtual bool read(std::istream& is) { return internal::readVector(is, _estimate); } 109 | virtual bool write(std::ostream& os) const { 110 | return internal::writeVector(os, estimate()); 111 | } 112 | }; 113 | 114 | } // namespace g2o 115 | 116 | #endif 117 | -------------------------------------------------------------------------------- /lidar_situational_graphs/include/s_graphs/backend/gps_mapper.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2023, University of Luxembourg 3 | All rights reserved. 4 | 5 | Redistributions and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its 16 | contributors may be used to endorse or promote products derived from 17 | this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | */ 29 | 30 | #ifndef GPS_MAPPER_HPP 31 | #define GPS_MAPPER_HPP 32 | 33 | #include 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | 40 | #include "geodesy/utm.h" 41 | #include "geodesy/wgs84.h" 42 | #include "geographic_msgs/msg/geo_point_stamped.hpp" 43 | #include "rclcpp/rclcpp.hpp" 44 | 45 | namespace s_graphs { 46 | 47 | /** 48 | * @brief 49 | */ 50 | class GPSMapper { 51 | public: 52 | /** 53 | * @brief Constructor for class GPSMapper 54 | * 55 | * @param private_nh 56 | */ 57 | GPSMapper(const rclcpp::Node::SharedPtr node, std::mutex& graph_mutex); 58 | ~GPSMapper(); 59 | 60 | public: 61 | bool map_gps_data( 62 | std::shared_ptr& covisibility_graph, 63 | std::deque& gps_queue, 64 | const std::map& keyframes); 65 | 66 | private: 67 | std::mutex& shared_graph_mutex; 68 | 69 | boost::optional zero_utm; 70 | double gps_edge_stddev_xy; 71 | double gps_edge_stddev_z; 72 | }; 73 | } // namespace s_graphs 74 | 75 | #endif // GPS_MAPPER_HPP 76 | -------------------------------------------------------------------------------- /lidar_situational_graphs/include/s_graphs/backend/imu_mapper.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2023, University of Luxembourg 3 | All rights reserved. 4 | 5 | Redistributions and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its 16 | contributors may be used to endorse or promote products derived from 17 | this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | */ 29 | 30 | #ifndef IMU_MAPPER_HPP 31 | #define IMU_MAPPER_HPP 32 | 33 | #include 34 | #include 35 | 36 | #include "g2o/edge_se3_priorquat.hpp" 37 | #include "g2o/edge_se3_priorvec.hpp" 38 | #include "geometry_msgs/msg/quaternion_stamped.hpp" 39 | #include "geometry_msgs/msg/vector3.hpp" 40 | #include "geometry_msgs/msg/vector3_stamped.hpp" 41 | #include "rclcpp/rclcpp.hpp" 42 | #include "sensor_msgs/msg/imu.hpp" 43 | #include "tf2_geometry_msgs/tf2_geometry_msgs.h" 44 | #include "tf2_ros/buffer.h" 45 | #include "tf2_ros/buffer_interface.h" 46 | 47 | namespace s_graphs { 48 | 49 | /** 50 | * @brief 51 | */ 52 | class IMUMapper { 53 | public: 54 | /** 55 | * @brief Constructor for class IMUMapper 56 | * 57 | * @param private_nh 58 | */ 59 | IMUMapper(const rclcpp::Node::SharedPtr node, std::mutex& mutex); 60 | ~IMUMapper(); 61 | 62 | public: 63 | bool map_imu_data(std::shared_ptr& graph_slam, 64 | const std::unique_ptr& tf_buffer, 65 | std::deque& imu_queue, 66 | const std::map& keyframes, 67 | const std::string base_frame_id); 68 | 69 | private: 70 | std::mutex& shared_graph_mutex; 71 | 72 | bool enable_imu_orientation; 73 | double imu_orientation_edge_stddev; 74 | bool enable_imu_acceleration; 75 | double imu_acceleration_edge_stddev; 76 | }; 77 | } // namespace s_graphs 78 | 79 | #endif // IMU_MAPPER_HPP 80 | -------------------------------------------------------------------------------- /lidar_situational_graphs/include/s_graphs/backend/loop_mapper.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2023, University of Luxembourg 3 | All rights reserved. 4 | 5 | Redistributions and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its 16 | contributors may be used to endorse or promote products derived from 17 | this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | */ 29 | 30 | #ifndef LOOP_MAPPER_HPP 31 | #define LOOP_MAPPER_HPP 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | namespace s_graphs { 39 | 40 | class LoopMapper { 41 | public: 42 | LoopMapper(const rclcpp::Node::SharedPtr node, std::mutex& graph_mutex); 43 | ~LoopMapper(); 44 | 45 | public: 46 | void add_loops(const std::shared_ptr& covisibility_graph, 47 | const std::vector& loops); 48 | 49 | private: 50 | void set_data(g2o::VertexSE3* keyframe_node); 51 | bool get_floor_data(g2o::VertexSE3* keyframe_node); 52 | 53 | private: 54 | std::mutex& shared_graph_mutex; 55 | 56 | std::unique_ptr inf_calclator; 57 | }; 58 | } // namespace s_graphs 59 | 60 | #endif #LOOP_MAPPER_HPP -------------------------------------------------------------------------------- /lidar_situational_graphs/include/s_graphs/backend/room_graph_generator.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2023, University of Luxembourg 3 | All rights reserved. 4 | 5 | Redistributions and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its 16 | contributors may be used to endorse or promote products derived from 17 | this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | */ 29 | 30 | #ifndef LOCAL_GRAPH_GENERATOR_HPP 31 | #define LOCAL_GRAPH_GENERATOR_HPP 32 | 33 | #include 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | 43 | namespace s_graphs { 44 | 45 | class RoomGraphGenerator { 46 | public: 47 | RoomGraphGenerator(rclcpp::Node::SharedPtr node, std::mutex& graph_mutex); 48 | ~RoomGraphGenerator(); 49 | 50 | public: 51 | /** 52 | * @brief Get the current room object 53 | * 54 | * @param x_vert_planes 55 | * @param y_vert_planes 56 | * @param keyframe 57 | * @param rooms_vec 58 | * @return * Rooms 59 | */ 60 | Rooms get_current_room(const std::unordered_map& x_vert_planes, 61 | const std::unordered_map& y_vert_planes, 62 | const KeyFrame::Ptr keyframe, 63 | const std::unordered_map& rooms_vec); 64 | 65 | /** 66 | * @brief Get the keyframes inside room object 67 | * 68 | * @param current_room 69 | * @param x_vert_planes 70 | * @param y_vert_planes 71 | * @param keyframes 72 | * @return * std::map 73 | */ 74 | std::map get_keyframes_inside_room( 75 | const Rooms& current_room, 76 | const std::unordered_map& x_vert_planes, 77 | const std::unordered_map& y_vert_planes, 78 | const std::map& keyframes); 79 | 80 | /** 81 | * @brief Get the room planes object 82 | * 83 | * @param current_room 84 | * @param x_vert_planes 85 | * @param y_vert_planes 86 | * @return * std::vector 87 | */ 88 | std::vector get_room_planes( 89 | const Rooms& current_room, 90 | const std::unordered_map& x_vert_planes, 91 | const std::unordered_map& y_vert_planes); 92 | 93 | /** 94 | * @brief 95 | * 96 | * @param keyframe_mapper 97 | * @param covisibility_graph 98 | * @param filtered_keyframes 99 | * @param odom2map 100 | * @param current_room 101 | * @return * void 102 | */ 103 | void generate_local_graph(std::unique_ptr& keyframe_mapper, 104 | std::shared_ptr covisibility_graph, 105 | std::map filtered_keyframes, 106 | const Eigen::Isometry3d& odom2map, 107 | Rooms& current_room); 108 | 109 | /** 110 | * @brief update the room graph keyframes after global optimization 111 | * 112 | * @param room 113 | * @param covisibility_graph 114 | */ 115 | void update_room_graph(const Rooms room, 116 | const std::shared_ptr& covisibility_graph); 117 | 118 | private: 119 | std::mutex& shared_graph_mutex; 120 | }; 121 | } // namespace s_graphs 122 | 123 | #endif // LOCAL_GRAPH_GENERATOR_HPP 124 | -------------------------------------------------------------------------------- /lidar_situational_graphs/include/s_graphs/backend/wall_mapper.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2023, University of Luxembourg 3 | All rights reserved. 4 | 5 | Redistributions and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its 16 | contributors may be used to endorse or promote products derived from 17 | this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | */ 29 | 30 | #ifndef WALL_MAPPER_HPP 31 | #define WALL_MAPPER_HPP 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | 51 | #include "geometry_msgs/msg/point.hpp" 52 | #include "pcl_ros/transforms.hpp" 53 | #include "rclcpp/rclcpp.hpp" 54 | #include "sensor_msgs/msg/point_cloud2.hpp" 55 | #include "situational_graphs_msgs/msg/plane_data.hpp" 56 | #include "situational_graphs_msgs/msg/planes_data.hpp" 57 | #include "situational_graphs_msgs/msg/room_data.hpp" 58 | #include "situational_graphs_msgs/msg/rooms_data.hpp" 59 | #include "visualization_msgs/msg/marker_array.hpp" 60 | 61 | namespace s_graphs { 62 | 63 | class WallMapper { 64 | public: 65 | /** 66 | * @brief Contructor of class WallMapper 67 | * 68 | * @param private_nh 69 | */ 70 | WallMapper(const rclcpp::Node::SharedPtr node, std::mutex& graph_mutex); 71 | ~WallMapper(); 72 | 73 | public: 74 | /** 75 | * @brief Eigen::Vector3d wall_pose, 76 | * 77 | * @param covisibility_graph 78 | * @param wall_pose 79 | * @param x_planes_msg 80 | * @param y_planes_msg 81 | * @param x_vert_planes 82 | * @param y_vert_planes 83 | * @param walls_vec 84 | */ 85 | void factor_wall( 86 | const std::shared_ptr covisibility_graph, 87 | const Eigen::Vector3d& wall_pose, 88 | const Eigen::Vector3d& wall_point, 89 | const std::vector x_planes_msg, 90 | const std::vector y_planes_msg, 91 | std::unordered_map& x_vert_planes, 92 | std::unordered_map& y_vert_planes, 93 | std::unordered_map& walls_vec); 94 | 95 | /** 96 | * @brief 97 | * 98 | * @param covisibility_graph 99 | * @param wall_pose 100 | * @param plane1 101 | * @param plane2 102 | * @return 103 | */ 104 | int add_wall_node_and_edge(const std::shared_ptr covisibility_graph, 105 | const Eigen::Vector3d& wall_pose, 106 | const Eigen::Vector3d& wall_point, 107 | VerticalPlanes& plane1, 108 | VerticalPlanes& plane2); 109 | 110 | /** 111 | * @brief 112 | * 113 | * @param covisibility_graph 114 | * @param wall 115 | * @param planes 116 | */ 117 | void add_saved_walls(const std::shared_ptr covisibility_graph, 118 | const std::unordered_map& planes, 119 | const Walls wall); 120 | 121 | private: 122 | bool use_point_to_plane; 123 | 124 | private: 125 | rclcpp::Node::SharedPtr node_obj; 126 | std::mutex& shared_graph_mutex; 127 | }; 128 | 129 | } // namespace s_graphs 130 | 131 | #endif // WALL_MAPPER_HPP 132 | -------------------------------------------------------------------------------- /lidar_situational_graphs/include/s_graphs/common/deviations.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2023, University of Luxembourg 3 | All rights reserved. 4 | 5 | Redistributions and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its 16 | contributors may be used to endorse or promote products derived from 17 | this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | */ 29 | 30 | #ifndef DEVIATIONS_HPP 31 | #define DEVIATIONS_HPP 32 | 33 | #include 34 | 35 | #include 36 | #include 37 | 38 | namespace s_graphs { 39 | 40 | struct WallDeviations { 41 | g2o::VertexDeviation* deviation_node = nullptr; 42 | g2o::VertexPlane* a_graph_plane_node = nullptr; 43 | g2o::VertexPlane* s_graph_plane_node = nullptr; 44 | }; 45 | 46 | struct RoomDeviations { 47 | g2o::VertexDeviation* deviation_node = nullptr; 48 | g2o::VertexRoom* a_graph_room_node = nullptr; 49 | g2o::VertexRoom* s_graph_room_node = nullptr; 50 | }; 51 | 52 | } // namespace s_graphs 53 | #endif // DEVIATIONS_HPP -------------------------------------------------------------------------------- /lidar_situational_graphs/include/s_graphs/common/door_ways.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2023, University of Luxembourg 3 | All rights reserved. 4 | 5 | Redistributions and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its 16 | contributors may be used to endorse or promote products derived from 17 | this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | */ 29 | 30 | #ifndef DOOR_WAYS_HPP 31 | #define DOOR_WAYS_HPP 32 | 33 | #include 34 | #include 35 | #include 36 | 37 | #include 38 | #include 39 | 40 | namespace g2o { 41 | class VertexDoorWay; 42 | class HyperGraph; 43 | class SparseOptimizer; 44 | } // namespace g2o 45 | 46 | namespace s_graphs { 47 | 48 | /** 49 | * @brief 50 | * 51 | * @param id, Door's I.D for graph slam 52 | * @param prior_id, Door's I.D from prior knowledge 53 | * @param room1_id, I.D of 1st room 54 | * @param room2_id, I.D of 2nd room 55 | * @param door_pos_w, Door's position in WOrld frame 56 | * @param door_pos_r1, Door's position in 1st room's frame 57 | * @param door_pos_r1, Door's position in 2nd room's frame 58 | */ 59 | struct DoorWays { 60 | public: 61 | int id; 62 | int prior_id; 63 | Eigen::Vector3d door_pos_w, door_pos_r1, door_pose_r2; 64 | int room1_id, room2_id; 65 | g2o::VertexDoorWay* node; // node instance 66 | }; 67 | 68 | } // namespace s_graphs 69 | #endif // DOOR_WAYS_HPP 70 | -------------------------------------------------------------------------------- /lidar_situational_graphs/include/s_graphs/common/infinite_rooms.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2023, University of Luxembourg 3 | All rights reserved. 4 | 5 | Redistributions and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its 16 | contributors may be used to endorse or promote products derived from 17 | this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | */ 29 | 30 | #ifndef INFINITE_ROOMS_HPP 31 | #define INFINITE_ROOMS_HPP 32 | 33 | #include 34 | #include 35 | #include 36 | 37 | #include 38 | #include 39 | #include 40 | 41 | #include "visualization_msgs/msg/marker_array.hpp" 42 | 43 | namespace g2o { 44 | class VertexSE3; 45 | class HyperGraph; 46 | class SparseOptimizer; 47 | } // namespace g2o 48 | 49 | namespace s_graphs { 50 | 51 | /** 52 | * @brief Struct that holds information about an infinite room (infinite_room). 53 | * 54 | * @var id: Unique Id of the infinite room. 55 | * @var connected_id 56 | * @var connected_neighbour_ids 57 | * @var plane1, plane2: Planes that form the inifite room 58 | * @var plane1_id, plane2_id: Planes unique ids 59 | * @var plane1_node, plane2_node 60 | * @var cluster_center_node 61 | * @var node 62 | * @var cluster_array 63 | */ 64 | class InfiniteRooms { 65 | public: 66 | InfiniteRooms() {} 67 | 68 | InfiniteRooms(const InfiniteRooms& old_room, const bool deep_copy = false) { 69 | *this = old_room; 70 | 71 | if (deep_copy) { 72 | node = new g2o::VertexRoom(); 73 | node->setEstimate(old_room.node->estimate()); 74 | 75 | cluster_center_node = new g2o::VertexRoom(); 76 | cluster_center_node->setEstimate(old_room.cluster_center_node->estimate()); 77 | 78 | plane1_node = new g2o::VertexPlane(); 79 | plane1_node->setEstimate(old_room.plane1_node->estimate()); 80 | plane2_node = new g2o::VertexPlane(); 81 | plane2_node->setEstimate(old_room.plane2_node->estimate()); 82 | } 83 | } 84 | 85 | InfiniteRooms& operator=(const InfiniteRooms& old_room) { 86 | id = old_room.id; 87 | plane1 = old_room.plane1; 88 | plane2 = old_room.plane2; 89 | plane1_id = old_room.plane1_id; 90 | plane2_id = old_room.plane2_id; 91 | floor_level = old_room.floor_level; 92 | sub_infinite_room = old_room.sub_infinite_room; 93 | cluster_array = old_room.cluster_array; 94 | 95 | plane1_node = old_room.plane1_node; 96 | plane2_node = old_room.plane2_node; 97 | cluster_center_node = old_room.cluster_center_node; 98 | node = old_room.node; 99 | local_graph = old_room.local_graph; 100 | 101 | return *this; 102 | } 103 | 104 | public: 105 | int id; 106 | g2o::Plane3D plane1; 107 | g2o::Plane3D plane2; 108 | int plane1_id; 109 | int plane2_id; 110 | int floor_level; 111 | bool sub_infinite_room; 112 | visualization_msgs::msg::MarkerArray cluster_array; 113 | 114 | g2o::VertexPlane* plane1_node = nullptr; 115 | g2o::VertexPlane* plane2_node = nullptr; 116 | g2o::VertexRoom* cluster_center_node; 117 | g2o::VertexRoom* node = nullptr; // node instance in covisibility graph 118 | std::shared_ptr local_graph; 119 | }; 120 | 121 | } // namespace s_graphs 122 | #endif // INFINITE_ROOMS_HPP 123 | -------------------------------------------------------------------------------- /lidar_situational_graphs/include/s_graphs/common/information_matrix_calculator.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2023, University of Luxembourg 3 | All rights reserved. 4 | 5 | Redistributions and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its 16 | contributors may be used to endorse or promote products derived from 17 | this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | */ 29 | 30 | #ifndef INFORMATION_MATRIX_CALCULATOR_HPP 31 | #define INFORMATION_MATRIX_CALCULATOR_HPP 32 | 33 | #include 34 | #include 35 | 36 | #include 37 | 38 | #include "rclcpp/rclcpp.hpp" 39 | 40 | namespace s_graphs { 41 | 42 | /** 43 | * @brief 44 | */ 45 | class InformationMatrixCalculator { 46 | public: 47 | InformationMatrixCalculator() {} 48 | InformationMatrixCalculator(const rclcpp::Node::SharedPtr node); 49 | ~InformationMatrixCalculator(); 50 | 51 | template 52 | 53 | /** 54 | * @brief 55 | * 56 | * @param params 57 | */ 58 | void load(ParamServer& params) { 59 | use_const_inf_matrix = params.template param("use_const_inf_matrix", false); 60 | const_stddev_x = params.template param("const_stddev_x", 0.5); 61 | const_stddev_q = params.template param("const_stddev_q", 0.1); 62 | 63 | var_gain_a = params.template param("var_gain_a", 20.0); 64 | min_stddev_x = params.template param("min_stddev_x", 0.1); 65 | max_stddev_x = params.template param("max_stddev_x", 5.0); 66 | min_stddev_q = params.template param("min_stddev_q", 0.05); 67 | max_stddev_q = params.template param("max_stddev_q", 0.2); 68 | fitness_score_thresh = params.template param("fitness_score_thresh", 2.5); 69 | } 70 | 71 | /** 72 | * @brief 73 | * 74 | * @param cloud1 75 | * @param cloud2 76 | * @param relpose 77 | * @param max_range 78 | * @return 79 | */ 80 | static double calc_fitness_score( 81 | const pcl::PointCloud::ConstPtr& cloud1, 82 | const pcl::PointCloud::ConstPtr& cloud2, 83 | const Eigen::Isometry3d& relpose, 84 | double max_range = std::numeric_limits::max()); 85 | 86 | /** 87 | * @brief 88 | * 89 | * @param cloud1 90 | * @param cloud2 91 | * @param relpose 92 | * @return Information matrix 93 | */ 94 | Eigen::MatrixXd calc_information_matrix( 95 | const pcl::PointCloud::ConstPtr& cloud1, 96 | const pcl::PointCloud::ConstPtr& cloud2, 97 | const Eigen::Isometry3d& relpose) const; 98 | 99 | private: 100 | /** 101 | * @brief 102 | * 103 | * @param a 104 | * @param max_x 105 | * @param min_y 106 | * @param max_y 107 | * @param x 108 | * @return 109 | */ 110 | double weight(double a, double max_x, double min_y, double max_y, double x) const { 111 | double y = (1.0 - std::exp(-a * x)) / (1.0 - std::exp(-a * max_x)); 112 | return min_y + (max_y - min_y) * y; 113 | } 114 | 115 | private: 116 | bool use_const_inf_matrix; 117 | double const_stddev_x; 118 | double const_stddev_q; 119 | 120 | double var_gain_a; 121 | double min_stddev_x; 122 | double max_stddev_x; 123 | double min_stddev_q; 124 | double max_stddev_q; 125 | double fitness_score_thresh; 126 | }; 127 | 128 | } // namespace s_graphs 129 | 130 | #endif // INFORMATION_MATRIX_CALCULATOR_HPP 131 | -------------------------------------------------------------------------------- /lidar_situational_graphs/include/s_graphs/common/optimization_data.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2023, University of Luxembourg 3 | All rights reserved. 4 | 5 | Redistributions and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its 16 | contributors may be used to endorse or promote products derived from 17 | this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | */ 29 | 30 | #ifndef OPTIMIZATION_DATA_H 31 | #define OPTIMIZATION_DATA_H 32 | 33 | #include 34 | #include 35 | #include 36 | 37 | #include "g2o/core/optimizable_graph.h" 38 | 39 | namespace s_graphs { 40 | class OptimizationData : public g2o::HyperGraph::Data { 41 | public: 42 | OptimizationData() {} 43 | ~OptimizationData() {} 44 | 45 | OptimizationData& operator=(OptimizationData prev_data) { 46 | bool marginalized_value; 47 | prev_data.get_marginalized_info(marginalized_value); 48 | set_marginalized_info(marginalized_value); 49 | 50 | bool get_node_value; 51 | prev_data.get_stair_node_info(get_node_value); 52 | set_stair_node_info(get_node_value); 53 | return *this; 54 | } 55 | 56 | bool read(std::istream& is) { 57 | std::cout << "read not implemented" << std::endl; 58 | return false; 59 | } 60 | 61 | bool write(std::ostream& os) const { 62 | std::cout << "write not implemented" << std::endl; 63 | return false; 64 | } 65 | 66 | void set_edge_info(bool value) { artificial_edge = value; } 67 | 68 | void get_edge_info(bool& value) { value = artificial_edge; } 69 | 70 | void get_rep_node_info(bool& value) { value = rep_node; } 71 | 72 | void set_rep_node_info(bool value) { 73 | rep_node = value; 74 | if (rep_node) marginalized = false; 75 | } 76 | 77 | void get_stair_node_info(bool& value) { value = stair_node; } 78 | 79 | void set_stair_node_info(bool value) { stair_node = value; } 80 | 81 | void get_loop_closure_info(bool& value) { value = loop_closure; } 82 | 83 | void set_loop_closure_info(bool value) { 84 | loop_closure = value; 85 | if (loop_closure) marginalized = false; 86 | } 87 | 88 | void get_marginalized_info(bool& value) { value = marginalized; } 89 | 90 | void set_marginalized_info(bool value) { 91 | if (loop_closure) { 92 | marginalized = false; 93 | } else { 94 | marginalized = value; 95 | } 96 | } 97 | 98 | void set_anchor_node_info(bool value) { anchor_node = value; } 99 | 100 | void get_anchor_node_info(bool& value) { value = anchor_node; } 101 | 102 | private: 103 | bool marginalized = false; 104 | bool loop_closure = false; 105 | bool anchor_node = false; 106 | bool rep_node = false; 107 | bool artificial_edge = false; 108 | bool stair_node = false; 109 | }; 110 | } // namespace s_graphs 111 | #endif // OPTIMIZATION_DATA_HPP 112 | -------------------------------------------------------------------------------- /lidar_situational_graphs/include/s_graphs/common/point_types.hpp: -------------------------------------------------------------------------------- 1 | #ifndef POINT_TYPES_H 2 | #define POINT_TYPES_H 3 | 4 | #include 5 | 6 | #ifdef USE_RGB_CLOUD 7 | using PointT = pcl::PointXYZRGB; 8 | #else 9 | using PointT = pcl::PointXYZI; 10 | #endif 11 | 12 | using PointNormal = pcl::PointXYZRGBNormal; 13 | 14 | #endif // POINT_TYPES_H -------------------------------------------------------------------------------- /lidar_situational_graphs/include/s_graphs/common/registrations.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2023, University of Luxembourg 3 | All rights reserved. 4 | 5 | Redistributions and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its 16 | contributors may be used to endorse or promote products derived from 17 | this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | */ 29 | 30 | #ifndef REGISTRATIONS_HPP 31 | #define REGISTRATIONS_HPP 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | #include 40 | #include 41 | #include 42 | #include 43 | 44 | #ifdef USE_VGICP_CUDA 45 | #include 46 | #endif 47 | #include 48 | 49 | namespace s_graphs { 50 | 51 | struct registration_params { 52 | public: 53 | std::string registration_method; 54 | long int reg_num_threads; 55 | double reg_transformation_epsilon; 56 | long int reg_maximum_iterations; 57 | double reg_max_correspondence_distance; 58 | long int reg_correspondence_randomness; 59 | double reg_resolution; 60 | bool reg_use_reciprocal_correspondences; 61 | long int reg_max_optimizer_iterations; 62 | std::string reg_nn_search_method; 63 | }; 64 | 65 | /** 66 | * @brief Select a scan matching algorithm according to rosparams. 67 | * 68 | * @param registration_params 69 | * @return selected scan matching 70 | */ 71 | pcl::Registration::Ptr select_registration_method( 72 | registration_params params); 73 | 74 | } // namespace s_graphs 75 | 76 | #endif // REGISTRATIONS_HPP 77 | -------------------------------------------------------------------------------- /lidar_situational_graphs/include/s_graphs/common/ros_time_hash.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2023, University of Luxembourg 3 | All rights reserved. 4 | 5 | Redistributions and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its 16 | contributors may be used to endorse or promote products derived from 17 | this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | */ 29 | 30 | 31 | 32 | #ifndef ROS_TIME_HASH_HPP 33 | #define ROS_TIME_HASH_HPP 34 | 35 | #include 36 | #include 37 | #include 38 | 39 | /** 40 | * @brief Hash calculation for rclcpp::Time 41 | */ 42 | class RosTimeHash { 43 | public: 44 | size_t operator()(const rclcpp::Time& val) const { 45 | size_t seed = 0; 46 | boost::hash_combine(seed, val.seconds()); 47 | boost::hash_combine(seed, val.nanoseconds()); 48 | return seed; 49 | } 50 | }; 51 | 52 | #endif // ROS_TIME_HASHER_HPP 53 | -------------------------------------------------------------------------------- /lidar_situational_graphs/include/s_graphs/common/ros_utils.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2023, University of Luxembourg 3 | All rights reserved. 4 | 5 | Redistributions and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its 16 | contributors may be used to endorse or promote products derived from 17 | this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | */ 29 | 30 | #ifndef ROS_UTILS_HPP 31 | #define ROS_UTILS_HPP 32 | 33 | #include 34 | #include 35 | #include 36 | 37 | #include 38 | 39 | #include "geometry_msgs/msg/pose.hpp" 40 | #include "geometry_msgs/msg/pose_stamped.hpp" 41 | #include "geometry_msgs/msg/transform_stamped.hpp" 42 | #include "nav_msgs/msg/odometry.hpp" 43 | #include "rclcpp/rclcpp.hpp" 44 | #include "s_graphs/common/keyframe.hpp" 45 | #include "situational_graphs_reasoning_msgs/msg/keyframe.hpp" 46 | 47 | namespace s_graphs { 48 | 49 | /** 50 | * @brief convert Eigen::Matrix to geometry_msgs::TransformStamped 51 | * 52 | * @param stamp timestamp 53 | * @param pose Eigen::Matrix to be converted 54 | * @param frame_id tf frame_id 55 | * @param child_frame_id tf child frame_id 56 | * @return converted TransformStamped 57 | */ 58 | geometry_msgs::msg::TransformStamped matrix2transform( 59 | const rclcpp::Time& stamp, 60 | const Eigen::Matrix4f& pose, 61 | const std::string& frame_id, 62 | const std::string& child_frame_id); 63 | 64 | /** 65 | * @brief 66 | * 67 | * @param 68 | * @return 69 | */ 70 | geometry_msgs::msg::PoseStamped matrix2PoseStamped(const rclcpp::Time& stamp, 71 | const Eigen::Matrix4f& pose, 72 | const std::string& frame_id); 73 | /** 74 | * @brief 75 | * 76 | * @param 77 | * @return 78 | */ 79 | Eigen::Isometry3d pose2isometry(const geometry_msgs::msg::Pose& pose); 80 | 81 | /** 82 | * @brief 83 | * 84 | * @param 85 | * @return 86 | */ 87 | Eigen::Isometry3d tf2isometry(const geometry_msgs::msg::TransformStamped& trans); 88 | 89 | /** 90 | * @brief 91 | * 92 | * @param 93 | * @return 94 | */ 95 | geometry_msgs::msg::Pose isometry2pose(const Eigen::Isometry3d& mat); 96 | 97 | /** 98 | * @brief 99 | * 100 | * @param 101 | * @return 102 | */ 103 | Eigen::Isometry3d odom2isometry(const nav_msgs::msg::Odometry::SharedPtr& odom_msg); 104 | 105 | /** 106 | * @brief 107 | * 108 | * @param 109 | * @return 110 | */ 111 | KeyFrame ROS2Keyframe(const situational_graphs_reasoning_msgs::msg::Keyframe& msg); 112 | 113 | /** 114 | * @brief 115 | * 116 | * @param 117 | * @return 118 | */ 119 | situational_graphs_reasoning_msgs::msg::Keyframe Keyframe2ROS(const KeyFrame& keyframe); 120 | 121 | /** 122 | * @brief 123 | * 124 | */ 125 | Eigen::Matrix4f transformStamped2EigenMatrix( 126 | const geometry_msgs::msg::TransformStamped& transform_stamped); 127 | 128 | /** 129 | * @brief 130 | * 131 | */ 132 | geometry_msgs::msg::TransformStamped eigenMatrixToTransformStamped( 133 | const Eigen::Matrix4f& matrix, 134 | const std::string& frame_id, 135 | const std::string& child_frame_id); 136 | 137 | } // namespace s_graphs 138 | #endif // ROS_UTILS_HPP 139 | -------------------------------------------------------------------------------- /lidar_situational_graphs/include/s_graphs/frontend/floor_analyzer.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2023, University of Luxembourg 3 | All rights reserved. 4 | 5 | Redistributions and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its 16 | contributors may be used to endorse or promote products derived from 17 | this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' 20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | */ 29 | 30 | #ifndef FLOOR_ANALYZER_HPP 31 | #define FLOOR_ANALYZER_HPP 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include 53 | #include 54 | 55 | #include "pcl_ros/transforms.hpp" 56 | #include "rclcpp/rclcpp.hpp" 57 | #include "sensor_msgs/msg/point_cloud2.hpp" 58 | #include "situational_graphs_msgs/msg/room_data.hpp" 59 | #include "situational_graphs_msgs/msg/rooms_data.hpp" 60 | #include "tf2_ros/transform_listener.h" 61 | 62 | namespace s_graphs { 63 | 64 | /** 65 | * @brief Class that provides tools for different analysis over 66 | * pointclouds to extract the floor surfaces. 67 | */ 68 | class FloorAnalyzer { 69 | public: 70 | /** 71 | * @brief Constructor for FloorAnalyzer. 72 | * 73 | */ 74 | FloorAnalyzer(); 75 | ~FloorAnalyzer(); 76 | 77 | public: 78 | /** 79 | * @brief Splits current data into multiples floors nodes if available. 80 | * 81 | * @param current_x_vert_planes 82 | * @param current_y_vert_planes 83 | * @param floor_plane_candidates_vec 84 | */ 85 | void perform_floor_segmentation( 86 | const std::vector& current_x_vert_planes, 87 | const std::vector& current_y_vert_planes, 88 | std::vector& floor_plane_candidates_vec); 89 | }; 90 | } // namespace s_graphs 91 | 92 | #endif // FLOOR_ANALYZER_HPP 93 | -------------------------------------------------------------------------------- /lidar_situational_graphs/launch/ros1_tf.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /lidar_situational_graphs/lidar_situational_graphs/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/snt-arg/lidar_situational_graphs/301146234f6d83bbedfe50dffa1474cef863da5b/lidar_situational_graphs/lidar_situational_graphs/__init__.py -------------------------------------------------------------------------------- /lidar_situational_graphs/lidar_situational_graphs/map2odom_publisher.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # SPDX-License-Identifier: BSD-2-Clause 3 | import rclpy 4 | from rclpy.node import Node 5 | from rclpy.qos import qos_profile_sensor_data 6 | from geometry_msgs.msg import TransformStamped 7 | from tf2_ros import TransformBroadcaster 8 | 9 | 10 | class Map2OdomPublisher(Node): 11 | 12 | def __init__(self): 13 | super().__init__('map2odom_publisher_node') 14 | 15 | self.tf_broadcaster = TransformBroadcaster(self) 16 | self.subcription = self.create_subscription( 17 | TransformStamped, 18 | '/s_graphs/odom2map', 19 | self.callback, 20 | qos_profile_sensor_data) 21 | 22 | self.declare_parameter('odom_frame_id', 'odom') 23 | self.declare_parameter('map_frame_id', 'map') 24 | 25 | self.odom_frame_id = self.get_parameter( 26 | 'odom_frame_id').get_parameter_value().string_value 27 | self.map_frame_id = self.get_parameter( 28 | 'map_frame_id').get_parameter_value().string_value 29 | 30 | self.timer = self.create_timer(0.1, self.timer_callback) 31 | 32 | def callback(self, odom_msg): 33 | self.odom_msg = odom_msg 34 | 35 | def timer_callback(self): 36 | transform = TransformStamped() 37 | transform.header.stamp = self.get_clock().now().to_msg() 38 | transform.header.frame_id = self.map_frame_id 39 | transform.child_frame_id = self.odom_frame_id 40 | 41 | if not hasattr(self, 'odom_msg'): 42 | transform.transform.translation.x = 0.0 43 | transform.transform.translation.y = 0.0 44 | transform.transform.translation.z = 0.0 45 | transform.transform.rotation.x = 0.0 46 | transform.transform.rotation.y = 0.0 47 | transform.transform.rotation.z = 0.0 48 | transform.transform.rotation.w = 1.0 49 | 50 | self.tf_broadcaster.sendTransform(transform) 51 | return 52 | 53 | transform.transform = self.odom_msg.transform 54 | self.tf_broadcaster.sendTransform(transform) 55 | 56 | 57 | def main(args=None): 58 | rclpy.init(args=args) 59 | node = Map2OdomPublisher() 60 | rclpy.spin(node) 61 | 62 | node.destroy_node() 63 | rclpy.shutdown() 64 | 65 | 66 | if __name__ == '__main__': 67 | main() 68 | -------------------------------------------------------------------------------- /lidar_situational_graphs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | lidar_situational_graphs 4 | 0.0.1 5 | The lidar situational graphs package for generating 3D optimizable scene graphs 6 | 7 | hriday 8 | 9 | GPLv3 10 | 11 | ament_cmake 12 | ament_cmake_python 13 | 14 | ndt_omp 15 | fast_gicp 16 | pcl_ros 17 | rclcpp 18 | rclpy 19 | geodesy 20 | nmea_msgs 21 | sensor_msgs 22 | geometry_msgs 23 | nav_msgs 24 | visualization_msgs 25 | libg2o 26 | tf2 27 | tf2_ros 28 | tf2_eigen 29 | tf2_sensor_msgs 30 | geographic_msgs 31 | message_filters 32 | backward_ros 33 | rviz_visual_tools 34 | libceres-dev 35 | situational_graphs_msgs 36 | situational_graphs_reasoning 37 | situational_graphs_reasoning_msgs 38 | 39 | ament_cmake_gtest 40 | 41 | ament_cmake_gtest 42 | ament_lint_auto 43 | 44 | 45 | ament_cmake 46 | 47 | 48 | -------------------------------------------------------------------------------- /lidar_situational_graphs/src/g2o/edge_plane.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | #include 6 | 7 | namespace g2o { 8 | 9 | void EdgePlaneParallel::computeError() { 10 | const VertexPlane* v1 = static_cast(_vertices[0]); 11 | const VertexPlane* v2 = static_cast(_vertices[1]); 12 | 13 | double num = v1->estimate().normal().dot(v2->estimate().normal()); 14 | double den = v1->estimate().normal().norm() * (v2->estimate().normal().norm()); 15 | 16 | _error[0] = acos(fabs(num) / den); 17 | } 18 | bool EdgePlaneParallel::read(std::istream& is) { 19 | Eigen::Vector3d v; 20 | for (int i = 0; i < 3; ++i) { 21 | is >> v[i]; 22 | } 23 | 24 | setMeasurement(v); 25 | for (int i = 0; i < information().rows(); ++i) { 26 | for (int j = i; j < information().cols(); ++j) { 27 | is >> information()(i, j); 28 | if (i != j) { 29 | information()(j, i) = information()(i, j); 30 | } 31 | } 32 | } 33 | 34 | return true; 35 | } 36 | 37 | bool EdgePlaneParallel::write(std::ostream& os) const { 38 | for (int i = 0; i < 3; ++i) { 39 | os << _measurement[i] << " "; 40 | } 41 | 42 | for (int i = 0; i < information().rows(); ++i) { 43 | for (int j = i; j < information().cols(); ++j) { 44 | os << " " << information()(i, j); 45 | }; 46 | } 47 | return os.good(); 48 | } 49 | 50 | void EdgePlanePerpendicular::computeError() { 51 | const VertexPlane* v1 = static_cast(_vertices[0]); 52 | const VertexPlane* v2 = static_cast(_vertices[1]); 53 | 54 | Eigen::Vector3d normal1 = v1->estimate().normal().normalized(); 55 | Eigen::Vector3d normal2 = v2->estimate().normal().normalized(); 56 | 57 | _error[0] = normal1.dot(normal2); 58 | } 59 | 60 | bool EdgePlanePerpendicular::read(std::istream& is) { 61 | Eigen::Vector3d v; 62 | for (int i = 0; i < 3; ++i) { 63 | is >> v[i]; 64 | } 65 | 66 | setMeasurement(v); 67 | for (int i = 0; i < information().rows(); ++i) { 68 | for (int j = i; j < information().cols(); ++j) { 69 | is >> information()(i, j); 70 | if (i != j) { 71 | information()(j, i) = information()(i, j); 72 | } 73 | } 74 | } 75 | 76 | return true; 77 | } 78 | 79 | bool EdgePlanePerpendicular::write(std::ostream& os) const { 80 | for (int i = 0; i < 3; ++i) { 81 | os << _measurement[i] << " "; 82 | } 83 | 84 | for (int i = 0; i < information().rows(); ++i) { 85 | for (int j = i; j < information().cols(); ++j) { 86 | os << " " << information()(i, j); 87 | }; 88 | } 89 | return os.good(); 90 | } 91 | 92 | void Edge2Planes::computeError() { 93 | const VertexPlane* v1 = static_cast(_vertices[0]); 94 | const VertexPlane* v2 = static_cast(_vertices[1]); 95 | g2o::Plane3D plane1 = v1->estimate(); 96 | g2o::Plane3D plane2 = v2->estimate(); 97 | 98 | _error = plane1.ominus(plane2); 99 | } 100 | 101 | bool Edge2Planes::read(std::istream& is) { 102 | Eigen::Vector3d v; 103 | for (int i = 0; i < 3; ++i) { 104 | is >> v[i]; 105 | } 106 | 107 | setMeasurement(v); 108 | for (int i = 0; i < information().rows(); ++i) { 109 | for (int j = i; j < information().cols(); ++j) { 110 | is >> information()(i, j); 111 | if (i != j) { 112 | information()(j, i) = information()(i, j); 113 | } 114 | } 115 | } 116 | 117 | return true; 118 | } 119 | 120 | bool Edge2Planes::write(std::ostream& os) const { 121 | for (int i = 0; i < 3; ++i) { 122 | os << _measurement[i] << " "; 123 | } 124 | 125 | for (int i = 0; i < information().rows(); ++i) { 126 | for (int j = i; j < information().cols(); ++j) { 127 | os << " " << information()(i, j); 128 | }; 129 | } 130 | return os.good(); 131 | } 132 | 133 | } // namespace g2o 134 | -------------------------------------------------------------------------------- /lidar_situational_graphs/src/g2o/edge_plane_identity.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | #include 6 | 7 | namespace g2o { 8 | 9 | /** 10 | * @brief A modified version of g2o::EdgePlane. This class takes care of flipped plane 11 | * normals. 12 | * 13 | */ 14 | void EdgePlaneIdentity::computeError() { 15 | const VertexPlane* v1 = static_cast(_vertices[0]); 16 | const VertexPlane* v2 = static_cast(_vertices[1]); 17 | 18 | Eigen::Vector4d p1 = v1->estimate().toVector(); 19 | Eigen::Vector4d p2 = v2->estimate().toVector(); 20 | 21 | if (p1.dot(p2) < 0.0) { 22 | p2 = -p2; 23 | } 24 | 25 | _error = (p2 - p1) - _measurement; 26 | } 27 | bool EdgePlaneIdentity::read(std::istream& is) { 28 | Eigen::Vector4d v; 29 | for (int i = 0; i < 4; ++i) { 30 | is >> v[i]; 31 | } 32 | 33 | setMeasurement(v); 34 | for (int i = 0; i < information().rows(); ++i) { 35 | for (int j = i; j < information().cols(); ++j) { 36 | is >> information()(i, j); 37 | if (i != j) { 38 | information()(j, i) = information()(i, j); 39 | } 40 | } 41 | } 42 | 43 | return true; 44 | } 45 | 46 | bool EdgePlaneIdentity::write(std::ostream& os) const { 47 | for (int i = 0; i < 4; ++i) { 48 | os << _measurement[i] << " "; 49 | } 50 | 51 | for (int i = 0; i < information().rows(); ++i) { 52 | for (int j = i; j < information().cols(); ++j) { 53 | os << " " << information()(i, j); 54 | }; 55 | } 56 | return os.good(); 57 | } 58 | 59 | } // namespace g2o 60 | -------------------------------------------------------------------------------- /lidar_situational_graphs/src/g2o/edge_plane_prior.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | #include 6 | 7 | namespace g2o { 8 | void EdgePlanePriorNormal::computeError() { 9 | const g2o::VertexPlane* v1 = static_cast(_vertices[0]); 10 | Eigen::Vector3d normal = v1->estimate().normal(); 11 | 12 | if (normal.dot(_measurement) < 0.0) { 13 | normal = -normal; 14 | } 15 | 16 | _error = normal - _measurement; 17 | } 18 | 19 | bool EdgePlanePriorNormal::read(std::istream& is) { 20 | Eigen::Vector3d v; 21 | is >> v(0) >> v(1) >> v(2); 22 | setMeasurement(v); 23 | for (int i = 0; i < information().rows(); ++i) 24 | for (int j = i; j < information().cols(); ++j) { 25 | is >> information()(i, j); 26 | if (i != j) information()(j, i) = information()(i, j); 27 | } 28 | return true; 29 | } 30 | bool EdgePlanePriorNormal::write(std::ostream& os) const { 31 | Eigen::Vector3d v = _measurement; 32 | os << v(0) << " " << v(1) << " " << v(2) << " "; 33 | for (int i = 0; i < information().rows(); ++i) 34 | for (int j = i; j < information().cols(); ++j) os << " " << information()(i, j); 35 | return os.good(); 36 | } 37 | 38 | void EdgePlanePriorDistance::computeError() { 39 | const g2o::VertexPlane* v1 = static_cast(_vertices[0]); 40 | _error[0] = _measurement - v1->estimate().distance(); 41 | } 42 | 43 | bool EdgePlanePriorDistance::read(std::istream& is) { 44 | is >> _measurement; 45 | for (int i = 0; i < information().rows(); ++i) 46 | for (int j = i; j < information().cols(); ++j) { 47 | is >> information()(i, j); 48 | if (i != j) information()(j, i) = information()(i, j); 49 | } 50 | return true; 51 | } 52 | bool EdgePlanePriorDistance::write(std::ostream& os) const { 53 | os << _measurement; 54 | for (int i = 0; i < information().rows(); ++i) 55 | for (int j = i; j < information().cols(); ++j) os << " " << information()(i, j); 56 | return os.good(); 57 | } 58 | } // namespace g2o 59 | -------------------------------------------------------------------------------- /lidar_situational_graphs/src/g2o/edge_se3_plane.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include 6 | 7 | namespace g2o { 8 | 9 | void EdgeSE3Plane::computeError() { 10 | const g2o::VertexSE3* v1 = static_cast(_vertices[0]); 11 | const g2o::VertexPlane* v2 = static_cast(_vertices[1]); 12 | 13 | Eigen::Isometry3d w2n = v1->estimate().inverse(); 14 | Plane3D local_plane = w2n * v2->estimate(); 15 | _error = local_plane.ominus(_measurement); 16 | } 17 | 18 | bool EdgeSE3Plane::read(std::istream& is) { 19 | Eigen::Vector4d v; 20 | is >> v(0) >> v(1) >> v(2) >> v(3); 21 | setMeasurement(Plane3D(v)); 22 | for (int i = 0; i < information().rows(); ++i) 23 | for (int j = i; j < information().cols(); ++j) { 24 | is >> information()(i, j); 25 | if (i != j) information()(j, i) = information()(i, j); 26 | } 27 | return true; 28 | } 29 | bool EdgeSE3Plane::write(std::ostream& os) const { 30 | Eigen::Vector4d v = _measurement.toVector(); 31 | os << v(0) << " " << v(1) << " " << v(2) << " " << v(3) << " "; 32 | for (int i = 0; i < information().rows(); ++i) 33 | for (int j = i; j < information().cols(); ++j) os << " " << information()(i, j); 34 | return os.good(); 35 | } 36 | } // namespace g2o 37 | -------------------------------------------------------------------------------- /lidar_situational_graphs/src/g2o/edge_se3_point_to_plane.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include 6 | 7 | namespace g2o { 8 | 9 | void EdgeSE3PointToPlane::computeError() { 10 | const g2o::VertexSE3* v1 = static_cast(_vertices[0]); 11 | const g2o::VertexPlane* v2 = static_cast(_vertices[1]); 12 | 13 | Eigen::Matrix4d Ti = v1->estimate().matrix(); 14 | Eigen::Vector4d Pj = v2->estimate().toVector(); 15 | Eigen::Matrix4d Gij = _measurement; 16 | _error = Pj.transpose() * Ti * Gij * Ti.transpose() * Pj / 2; 17 | } 18 | 19 | bool EdgeSE3PointToPlane::read(std::istream& is) { 20 | Eigen::Matrix4d v; 21 | for (int i = 0; i < measurement().rows(); ++i) 22 | for (int j = 0; j < measurement().cols(); ++j) { 23 | is >> v(i, j); 24 | } 25 | setMeasurement(Eigen::Matrix4d(v)); 26 | 27 | for (int i = 0; i < information().rows(); ++i) 28 | for (int j = i; j < information().cols(); ++j) { 29 | is >> information()(i, j); 30 | if (i != j) information()(j, i) = information()(i, j); 31 | } 32 | return true; 33 | } 34 | bool EdgeSE3PointToPlane::write(std::ostream& os) const { 35 | Eigen::Matrix4d v = _measurement; 36 | 37 | for (int i = 0; i < measurement().rows(); ++i) 38 | for (int j = 0; j < measurement().cols(); ++j) { 39 | os << " " << v(i, j); 40 | } 41 | 42 | for (int i = 0; i < information().rows(); ++i) 43 | for (int j = i; j < information().cols(); ++j) os << " " << information()(i, j); 44 | return os.good(); 45 | } 46 | } // namespace g2o 47 | -------------------------------------------------------------------------------- /lidar_situational_graphs/src/g2o/edge_se3_priorquat.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | 6 | namespace g2o { 7 | 8 | void EdgeSE3PriorQuat::computeError() { 9 | const g2o::VertexSE3* v1 = static_cast(_vertices[0]); 10 | 11 | Eigen::Quaterniond estimate = Eigen::Quaterniond(v1->estimate().linear()); 12 | 13 | if (_measurement.coeffs().dot(estimate.coeffs()) < 0.0) { 14 | estimate.coeffs() = -estimate.coeffs(); 15 | } 16 | _error = estimate.vec() - _measurement.vec(); 17 | } 18 | 19 | void EdgeSE3PriorQuat::setMeasurement(const Eigen::Quaterniond& m) { 20 | _measurement = m; 21 | if (m.w() < 0.0) { 22 | _measurement.coeffs() = -m.coeffs(); 23 | } 24 | } 25 | 26 | bool EdgeSE3PriorQuat::read(std::istream& is) { 27 | Eigen::Quaterniond q; 28 | is >> q.w() >> q.x() >> q.y() >> q.z(); 29 | setMeasurement(q); 30 | for (int i = 0; i < information().rows(); ++i) 31 | for (int j = i; j < information().cols(); ++j) { 32 | is >> information()(i, j); 33 | if (i != j) information()(j, i) = information()(i, j); 34 | } 35 | return true; 36 | } 37 | bool EdgeSE3PriorQuat::write(std::ostream& os) const { 38 | Eigen::Quaterniond q = _measurement; 39 | os << q.w() << " " << q.x() << " " << q.y() << " " << q.z(); 40 | for (int i = 0; i < information().rows(); ++i) 41 | for (int j = i; j < information().cols(); ++j) os << " " << information()(i, j); 42 | return os.good(); 43 | } 44 | } // namespace g2o 45 | -------------------------------------------------------------------------------- /lidar_situational_graphs/src/g2o/edge_se3_priorvec.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | 6 | namespace g2o { 7 | 8 | void EdgeSE3PriorVec::computeError() { 9 | const g2o::VertexSE3* v1 = static_cast(_vertices[0]); 10 | 11 | Eigen::Vector3d direction = _measurement.head<3>(); 12 | Eigen::Vector3d measurement = _measurement.tail<3>(); 13 | 14 | Eigen::Vector3d estimate = (v1->estimate().linear().inverse() * direction); 15 | 16 | _error = estimate - measurement; 17 | } 18 | 19 | void EdgeSE3PriorVec::setMeasurement(const Eigen::Matrix& m) { 20 | _measurement.head<3>() = m.head<3>().normalized(); 21 | _measurement.tail<3>() = m.tail<3>().normalized(); 22 | } 23 | 24 | bool EdgeSE3PriorVec::read(std::istream& is) { 25 | Eigen::Matrix v; 26 | is >> v[0] >> v[1] >> v[2] >> v[3] >> v[4] >> v[5]; 27 | setMeasurement(v); 28 | for (int i = 0; i < information().rows(); ++i) 29 | for (int j = i; j < information().cols(); ++j) { 30 | is >> information()(i, j); 31 | if (i != j) information()(j, i) = information()(i, j); 32 | } 33 | return true; 34 | } 35 | bool EdgeSE3PriorVec::write(std::ostream& os) const { 36 | Eigen::Matrix v = _measurement; 37 | os << v[0] << " " << v[1] << " " << v[2] << " " << v[3] << " " << v[4] << " " << v[5]; 38 | for (int i = 0; i < information().rows(); ++i) 39 | for (int j = i; j < information().cols(); ++j) os << " " << information()(i, j); 40 | return os.good(); 41 | } 42 | } // namespace g2o 43 | -------------------------------------------------------------------------------- /lidar_situational_graphs/src/g2o/edge_se3_priorxy.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | 6 | namespace g2o { 7 | void EdgeSE3PriorXY::computeError() { 8 | const g2o::VertexSE3* v1 = static_cast(_vertices[0]); 9 | 10 | Eigen::Vector2d estimate = v1->estimate().translation().head<2>(); 11 | _error = estimate - _measurement; 12 | } 13 | 14 | bool EdgeSE3PriorXY::read(std::istream& is) { 15 | Eigen::Vector2d v; 16 | is >> v(0) >> v(1); 17 | setMeasurement(v); 18 | for (int i = 0; i < information().rows(); ++i) 19 | for (int j = i; j < information().cols(); ++j) { 20 | is >> information()(i, j); 21 | if (i != j) information()(j, i) = information()(i, j); 22 | } 23 | return true; 24 | } 25 | bool EdgeSE3PriorXY::write(std::ostream& os) const { 26 | Eigen::Vector2d v = _measurement; 27 | os << v(0) << " " << v(1) << " "; 28 | for (int i = 0; i < information().rows(); ++i) 29 | for (int j = i; j < information().cols(); ++j) os << " " << information()(i, j); 30 | return os.good(); 31 | } 32 | } // namespace g2o 33 | -------------------------------------------------------------------------------- /lidar_situational_graphs/src/g2o/edge_se3_priorxyz.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | 6 | namespace g2o { 7 | 8 | void EdgeSE3PriorXYZ::computeError() { 9 | const g2o::VertexSE3* v1 = static_cast(_vertices[0]); 10 | 11 | Eigen::Vector3d estimate = v1->estimate().translation(); 12 | _error = estimate - _measurement; 13 | } 14 | 15 | bool EdgeSE3PriorXYZ::read(std::istream& is) { 16 | Eigen::Vector3d v; 17 | is >> v(0) >> v(1) >> v(2); 18 | setMeasurement(v); 19 | for (int i = 0; i < information().rows(); ++i) 20 | for (int j = i; j < information().cols(); ++j) { 21 | is >> information()(i, j); 22 | if (i != j) information()(j, i) = information()(i, j); 23 | } 24 | return true; 25 | } 26 | bool EdgeSE3PriorXYZ::write(std::ostream& os) const { 27 | Eigen::Vector3d v = _measurement; 28 | os << v(0) << " " << v(1) << " " << v(2) << " "; 29 | for (int i = 0; i < information().rows(); ++i) 30 | for (int j = i; j < information().cols(); ++j) os << " " << information()(i, j); 31 | return os.good(); 32 | } 33 | } // namespace g2o 34 | -------------------------------------------------------------------------------- /lidar_situational_graphs/src/g2o/edge_se3_two_planes.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | namespace g2o { 9 | 10 | void EdgeSE3PlanePlane::computeError() { 11 | const g2o::VertexDeviation* v1 = 12 | static_cast(_vertices[0]); 13 | const g2o::VertexPlane* v2 = static_cast(_vertices[1]); 14 | const g2o::VertexPlane* v3 = static_cast(_vertices[2]); 15 | 16 | g2o::Plane3D a_graph_plane = v2->estimate(); 17 | g2o::Plane3D s_graph_plane = v3->estimate(); 18 | 19 | Eigen::Isometry3d deviation = v1->estimate(); 20 | Eigen::Isometry3d dev_inverse = v1->estimate().inverse(); 21 | g2o::Plane3D modified_s_graph_plane = dev_inverse * v3->estimate(); 22 | _error = modified_s_graph_plane.ominus(v2->estimate()); 23 | // if (v2->id() == 18 || v2->id() == 12) { 24 | // std::cout << "a_graph plane fixed : " << v2->fixed() << std::endl; 25 | // std::cout << "s_graph plane fixed : " << v3->fixed() << std::endl; 26 | // std::cout << "error : " << std::endl; 27 | // std::cout << _error << std::endl; 28 | // std::cout << "deviation : " << std::endl; 29 | // std::cout << v1->estimate().matrix() << std::endl; 30 | // std::cout << "a_graph_plane : " << std::endl; 31 | // std::cout << v2->estimate().toVector() << std::endl; 32 | // std::cout << "s_graph_plane : " << std::endl; 33 | // std::cout << v3->estimate().toVector() << std::endl; 34 | // } 35 | } 36 | bool EdgeSE3PlanePlane::read(std::istream& is) { 37 | Eigen::Vector3d v; 38 | is >> v(0) >> v(1) >> v(2); 39 | setMeasurement(v); 40 | for (int i = 0; i < information().rows(); ++i) 41 | for (int j = i; j < information().cols(); ++j) { 42 | is >> information()(i, j); 43 | if (i != j) information()(j, i) = information()(i, j); 44 | } 45 | return true; 46 | } 47 | bool EdgeSE3PlanePlane::write(std::ostream& os) const { 48 | Eigen::Vector3d v = _measurement; 49 | os << v(0) << " " << v(1) << " " << v(2) << " "; 50 | for (int i = 0; i < information().rows(); ++i) 51 | for (int j = i; j < information().cols(); ++j) os << " " << information()(i, j); 52 | return os.good(); 53 | } 54 | 55 | } // namespace g2o 56 | -------------------------------------------------------------------------------- /lidar_situational_graphs/src/g2o/edge_se3_two_rooms.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | namespace g2o { 5 | 6 | void EdgeSE3RoomRoom::computeError() { 7 | // std::cout << "defining edge !" << std::endl; 8 | const g2o::VertexDeviation *v1 = 9 | static_cast(_vertices[0]); 10 | const g2o::VertexRoom *v2 = static_cast(_vertices[1]); 11 | const g2o::VertexRoom *v3 = static_cast(_vertices[2]); 12 | 13 | Eigen::Isometry3d error_matrix = 14 | v1->estimate() * v2->estimate().inverse() * v3->estimate(); 15 | 16 | // Set the error vector to the translation and rotation parts of the error matrix 17 | _error.head(3) = error_matrix.translation(); 18 | Eigen::Quaterniond q_error(error_matrix.rotation()); 19 | _error.tail(3) = 20 | Eigen::Vector3d(q_error.x(), q_error.y(), q_error.z()) * 2.0 * q_error.w(); 21 | // if (v2->id() == 56) { 22 | // std::cout << "error : " << std::endl; 23 | // std::cout << _error << std::endl; 24 | // std::cout << "Deviation : " << std::endl; 25 | // std::cout << v1->estimate().matrix() << std::endl; 26 | // std::cout << "A-Graph room : " << std::endl; 27 | // std::cout << v2->estimate().matrix() << std::endl; 28 | // std::cout << "S-Graph room : " << std::endl; 29 | // std::cout << v3->estimate().matrix() << std::endl; 30 | // } 31 | } 32 | 33 | bool EdgeSE3RoomRoom::read(std::istream &is) { 34 | // Read the measurement from the input stream 35 | Eigen::Quaterniond q; 36 | Eigen::Vector3d t; 37 | for (int i = 0; i < 4; ++i) { 38 | is >> q.coeffs()[i]; 39 | } 40 | for (int i = 0; i < 3; ++i) { 41 | is >> t[i]; 42 | } 43 | 44 | // Set the measurement using the quaternion and translation 45 | _measurement.matrix() = Eigen::Matrix4d::Identity(); 46 | _measurement.matrix().topLeftCorner(3, 3) = q.normalized().toRotationMatrix(); 47 | _measurement.matrix().topRightCorner(3, 1) = t; 48 | 49 | // Read the information matrix (if needed) 50 | for (int i = 0; i < information().rows() && is.good(); ++i) { 51 | for (int j = i; j < information().cols() && is.good(); ++j) { 52 | is >> information()(i, j); 53 | if (i != j) { 54 | information()(j, i) = information()(i, j); 55 | } 56 | } 57 | } 58 | 59 | return true; 60 | } 61 | 62 | bool EdgeSE3RoomRoom::write(std::ostream &os) const { 63 | // Write the measurement (quaternion and translation) 64 | Eigen::Quaterniond q(_measurement.rotation()); 65 | Eigen::Vector3d t(_measurement.translation()); 66 | 67 | os << q.coeffs().transpose() << " " << t.transpose(); 68 | 69 | // Write the information matrix (if needed) 70 | for (int i = 0; i < information().rows(); ++i) { 71 | for (int j = i; j < information().cols(); ++j) { 72 | os << " " << information()(i, j); 73 | } 74 | } 75 | return os.good(); 76 | } 77 | 78 | } // namespace g2o 79 | -------------------------------------------------------------------------------- /lidar_situational_graphs/src/g2o/edge_wall_two_planes.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include "g2o/vertex_wall.hpp" 10 | namespace g2o { 11 | 12 | /* Define Wall edge with wall surfaces here*/ 13 | 14 | void EdgeWall2Planes::computeError() { 15 | const VertexWall* v1 = static_cast(_vertices[0]); 16 | const VertexPlane* v2 = static_cast(_vertices[1]); 17 | const VertexPlane* v3 = static_cast(_vertices[2]); 18 | 19 | Eigen::Vector3d wall_center = v1->estimate(); 20 | Eigen::Vector4d plane1 = v2->estimate().coeffs(); 21 | Eigen::Vector4d plane2 = v3->estimate().coeffs(); 22 | 23 | correct_plane_direction(plane1); 24 | correct_plane_direction(plane2); 25 | 26 | Eigen::Vector3d estimated_wall_center; 27 | if (fabs(plane1(3)) > fabs(plane2(3))) { 28 | estimated_wall_center = 29 | (0.5 * (fabs(plane1(3)) * plane1.head(3) - fabs(plane2(3)) * plane2.head(3))) + 30 | fabs(plane2(3)) * plane2.head(3); 31 | } else { 32 | estimated_wall_center = 33 | (0.5 * (fabs(plane2(3)) * plane2.head(3) - fabs(plane1(3)) * plane1.head(3))) + 34 | fabs(plane1(3)) * plane1.head(3); 35 | } 36 | 37 | Eigen::Vector3d estimated_wall_center_normalized = 38 | estimated_wall_center.head(2) / estimated_wall_center.norm(); 39 | Eigen::Vector3d final_wall_center = 40 | estimated_wall_center.head(2) + 41 | (_wall_point - (_wall_point.dot(estimated_wall_center_normalized)) * 42 | estimated_wall_center_normalized); 43 | 44 | _error = wall_center - final_wall_center; 45 | } 46 | 47 | bool EdgeWall2Planes::read(std::istream& is) { 48 | Eigen::Vector3d v; 49 | is >> v(0) >> v(1) >> v(2); 50 | 51 | setMeasurement(v); 52 | for (int i = 0; i < information().rows(); ++i) { 53 | for (int j = i; j < information().cols(); ++j) { 54 | is >> information()(i, j); 55 | if (i != j) { 56 | information()(j, i) = information()(i, j); 57 | } 58 | } 59 | } 60 | 61 | return true; 62 | } 63 | 64 | bool EdgeWall2Planes::write(std::ostream& os) const { 65 | Eigen::Vector3d v = _measurement; 66 | os << v(0) << " " << v(1) << " " << v(2) << " "; 67 | 68 | for (int i = 0; i < information().rows(); ++i) { 69 | for (int j = i; j < information().cols(); ++j) { 70 | os << " " << information()(i, j); 71 | }; 72 | } 73 | return os.good(); 74 | } 75 | 76 | void EdgeWall2Planes::correct_plane_direction(Eigen::Vector4d& plane) { 77 | if (plane(3) > 0) { 78 | plane(0) = -1 * plane(0); 79 | plane(1) = -1 * plane(1); 80 | plane(2) = -1 * plane(2); 81 | plane(3) = -1 * plane(3); 82 | } else { 83 | plane = plane; 84 | } 85 | } 86 | 87 | } // namespace g2o 88 | -------------------------------------------------------------------------------- /lidar_situational_graphs/src/g2o/robust_kernel_io.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | namespace g2o { 11 | 12 | std::string kernel_type(g2o::RobustKernel* kernel) { 13 | if (dynamic_cast(kernel)) { 14 | return "Huber"; 15 | } 16 | if (dynamic_cast(kernel)) { 17 | return "Cauchy"; 18 | } 19 | if (dynamic_cast(kernel)) { 20 | return "DCS"; 21 | } 22 | if (dynamic_cast(kernel)) { 23 | return "Fair"; 24 | } 25 | if (dynamic_cast(kernel)) { 26 | return "GemanMcClure"; 27 | } 28 | if (dynamic_cast(kernel)) { 29 | return "PseudoHuber"; 30 | } 31 | if (dynamic_cast(kernel)) { 32 | return "Saturated"; 33 | } 34 | if (dynamic_cast(kernel)) { 35 | return "Tukey"; 36 | } 37 | if (dynamic_cast(kernel)) { 38 | return "Welsch"; 39 | } 40 | return ""; 41 | } 42 | 43 | bool save_robust_kernels(const std::string& filename, g2o::SparseOptimizer* graph) { 44 | std::ofstream ofs(filename); 45 | if (!ofs) { 46 | std::cerr << "failed to open output stream!!" << std::endl; 47 | return false; 48 | } 49 | 50 | for (const auto& edge_ : graph->edges()) { 51 | g2o::OptimizableGraph::Edge* edge = 52 | static_cast(edge_); 53 | g2o::RobustKernel* kernel = edge->robustKernel(); 54 | if (!kernel) { 55 | continue; 56 | } 57 | 58 | std::string type = kernel_type(kernel); 59 | if (type.empty()) { 60 | std::cerr << "unknown kernel type!!" << std::endl; 61 | continue; 62 | } 63 | 64 | ofs << edge->vertices().size() << " "; 65 | for (size_t i = 0; i < edge->vertices().size(); i++) { 66 | ofs << edge->vertices()[i]->id() << " "; 67 | } 68 | ofs << type << " " << kernel->delta() << std::endl; 69 | } 70 | 71 | return true; 72 | } 73 | 74 | class KernelData { 75 | public: 76 | KernelData(const std::string& line) { 77 | std::stringstream sst(line); 78 | size_t num_vertices; 79 | sst >> num_vertices; 80 | 81 | vertex_indices.resize(num_vertices); 82 | for (size_t i = 0; i < num_vertices; i++) { 83 | sst >> vertex_indices[i]; 84 | } 85 | 86 | sst >> type >> delta; 87 | } 88 | 89 | bool match(g2o::OptimizableGraph::Edge* edge) const { 90 | if (edge->vertices().size() != vertex_indices.size()) { 91 | return false; 92 | } 93 | 94 | for (size_t i = 0; i < edge->vertices().size(); i++) { 95 | if (edge->vertices()[i]->id() != vertex_indices[i]) { 96 | return false; 97 | } 98 | } 99 | 100 | return true; 101 | } 102 | 103 | g2o::RobustKernel* create() const { 104 | auto factory = g2o::RobustKernelFactory::instance(); 105 | g2o::RobustKernel* kernel = factory->construct(type); 106 | kernel->setDelta(delta); 107 | 108 | return kernel; 109 | } 110 | 111 | public: 112 | std::vector vertex_indices; 113 | std::string type; 114 | double delta; 115 | }; 116 | 117 | bool load_robust_kernels(const std::string& filename, g2o::SparseOptimizer* graph) { 118 | std::ifstream ifs(filename); 119 | if (!ifs) { 120 | std::cerr << "warning: failed to open input stream!!" << std::endl; 121 | return true; 122 | } 123 | 124 | std::vector kernels; 125 | 126 | while (!ifs.eof()) { 127 | std::string line; 128 | std::getline(ifs, line); 129 | if (line.empty()) { 130 | continue; 131 | } 132 | 133 | kernels.push_back(KernelData(line)); 134 | } 135 | std::cout << "kernels: " << kernels.size() << std::endl; 136 | 137 | for (auto& edge_ : graph->edges()) { 138 | g2o::OptimizableGraph::Edge* edge = 139 | static_cast(edge_); 140 | 141 | for (auto itr = kernels.begin(); itr != kernels.end(); itr++) { 142 | if (itr->match(edge)) { 143 | edge->setRobustKernel(itr->create()); 144 | kernels.erase(itr); 145 | break; 146 | } 147 | } 148 | } 149 | 150 | if (kernels.size() != 0) { 151 | std::cerr << "warning: there is non-associated kernels!!" << std::endl; 152 | } 153 | return true; 154 | } 155 | 156 | } // namespace g2o 157 | -------------------------------------------------------------------------------- /lidar_situational_graphs/src/s_graphs/backend/gps_mapper.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace s_graphs { 4 | 5 | GPSMapper::GPSMapper(const rclcpp::Node::SharedPtr node, std::mutex& graph_mutex) 6 | : shared_graph_mutex(graph_mutex) { 7 | gps_edge_stddev_xy = 8 | node->get_parameter("gps_edge_stddev_xy").get_parameter_value().get(); 9 | gps_edge_stddev_z = 10 | node->get_parameter("gps_edge_stddev_z").get_parameter_value().get(); 11 | } 12 | 13 | GPSMapper::~GPSMapper() {} 14 | 15 | bool GPSMapper::map_gps_data( 16 | std::shared_ptr& covisibility_graph, 17 | std::deque& gps_queue, 18 | const std::map& keyframes) { 19 | bool updated = false; 20 | auto gps_cursor = gps_queue.begin(); 21 | 22 | for (auto& keyframe : keyframes) { 23 | if (keyframe.second->stamp > gps_queue.back()->header.stamp) { 24 | break; 25 | } 26 | 27 | if (keyframe.second->stamp < (*gps_cursor)->header.stamp || 28 | keyframe.second->utm_coord) { 29 | continue; 30 | } 31 | 32 | // find the gps data which is closest to the keyframe 33 | auto closest_gps = gps_cursor; 34 | for (auto gps = gps_cursor; gps != gps_queue.end(); gps++) { 35 | auto dt = (rclcpp::Time((*closest_gps)->header.stamp) - keyframe.second->stamp) 36 | .seconds(); 37 | auto dt2 = 38 | (rclcpp::Time((*gps)->header.stamp) - keyframe.second->stamp).seconds(); 39 | if (std::abs(dt) < std::abs(dt2)) { 40 | break; 41 | } 42 | 43 | closest_gps = gps; 44 | } 45 | 46 | // if the time residual between the gps and keyframe is too large, skip it 47 | gps_cursor = closest_gps; 48 | if (0.2 < 49 | std::abs((rclcpp::Time((*closest_gps)->header.stamp) - keyframe.second->stamp) 50 | .seconds())) { 51 | continue; 52 | } 53 | 54 | // convert (latitude, longitude, altitude) -> (easting, northing, altitude) in UTM 55 | // coordinate 56 | geodesy::UTMPoint utm; 57 | geodesy::fromMsg((*closest_gps)->position, utm); 58 | Eigen::Vector3d xyz(utm.easting, utm.northing, utm.altitude); 59 | 60 | // the first gps data position will be the origin of the map 61 | if (!zero_utm) { 62 | zero_utm = xyz; 63 | } 64 | xyz -= (*zero_utm); 65 | 66 | keyframe.second->utm_coord = xyz; 67 | 68 | g2o::OptimizableGraph::Edge* edge; 69 | if (std::isnan(xyz.z())) { 70 | Eigen::Matrix2d information_matrix = 71 | Eigen::Matrix2d::Identity() / gps_edge_stddev_xy; 72 | shared_graph_mutex.lock(); 73 | edge = covisibility_graph->add_se3_prior_xy_edge( 74 | keyframe.second->node, xyz.head<2>(), information_matrix); 75 | shared_graph_mutex.unlock(); 76 | } else { 77 | Eigen::Matrix3d information_matrix = Eigen::Matrix3d::Identity(); 78 | information_matrix.block<2, 2>(0, 0) /= gps_edge_stddev_xy; 79 | information_matrix(2, 2) /= gps_edge_stddev_z; 80 | shared_graph_mutex.lock(); 81 | edge = covisibility_graph->add_se3_prior_xyz_edge( 82 | keyframe.second->node, xyz, information_matrix); 83 | shared_graph_mutex.unlock(); 84 | } 85 | 86 | shared_graph_mutex.lock(); 87 | covisibility_graph->add_robust_kernel(edge, "Huber", 1.0); 88 | shared_graph_mutex.unlock(); 89 | 90 | updated = true; 91 | 92 | auto remove_loc = std::upper_bound( 93 | gps_queue.begin(), 94 | gps_queue.end(), 95 | keyframes.rbegin()->second->stamp, 96 | [=](const rclcpp::Time& stamp, 97 | const geographic_msgs::msg::GeoPointStamped::SharedPtr& geopoint) { 98 | return stamp < geopoint->header.stamp; 99 | }); 100 | gps_queue.erase(gps_queue.begin(), remove_loc); 101 | return updated; 102 | } 103 | return updated; 104 | } 105 | 106 | } // namespace s_graphs 107 | -------------------------------------------------------------------------------- /lidar_situational_graphs/src/s_graphs/backend/loop_mapper.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace s_graphs { 4 | 5 | LoopMapper::LoopMapper(const rclcpp::Node::SharedPtr node, std::mutex& graph_mutex) 6 | : shared_graph_mutex(graph_mutex) { 7 | inf_calclator.reset(new InformationMatrixCalculator(node)); 8 | } 9 | 10 | LoopMapper::~LoopMapper() {} 11 | 12 | void LoopMapper::add_loops(const std::shared_ptr& covisibility_graph, 13 | const std::vector& loops) { 14 | for (const auto& loop : loops) { 15 | Eigen::Isometry3d relpose(loop->relative_pose.cast()); 16 | Eigen::MatrixXd information_matrix = inf_calclator->calc_information_matrix( 17 | loop->key1->cloud, loop->key2->cloud, relpose); 18 | shared_graph_mutex.lock(); 19 | std::cout << "loop found between keyframes " << loop->key1->node->id() << " and " 20 | << loop->key2->node->id() << std::endl; 21 | 22 | set_data(loop->key1->node); 23 | set_data(loop->key2->node); 24 | 25 | bool kf1_stair = get_floor_data(loop->key1->node); 26 | bool kf2_stair = get_floor_data(loop->key2->node); 27 | 28 | if (!kf1_stair && !kf2_stair) { 29 | g2o::EdgeLoopClosure* edge = covisibility_graph->add_loop_closure_edge( 30 | loop->key1->node, loop->key2->node, relpose, information_matrix); 31 | covisibility_graph->add_robust_kernel(edge, "Huber", 1.0); 32 | } else { 33 | std::cout << "not adding this loop as node is on floor " << std::endl; 34 | } 35 | shared_graph_mutex.unlock(); 36 | } 37 | } 38 | 39 | void LoopMapper::set_data(g2o::VertexSE3* keyframe_node) { 40 | auto current_key_data = dynamic_cast(keyframe_node->userData()); 41 | if (current_key_data) { 42 | current_key_data->set_loop_closure_info(true); 43 | } else { 44 | OptimizationData* data = new OptimizationData(); 45 | data->set_loop_closure_info(true); 46 | keyframe_node->setUserData(data); 47 | } 48 | } 49 | 50 | bool LoopMapper::get_floor_data(g2o::VertexSE3* keyframe_node) { 51 | bool on_stairs = false; 52 | auto current_key_data = dynamic_cast(keyframe_node->userData()); 53 | if (current_key_data) { 54 | current_key_data->get_stair_node_info(on_stairs); 55 | } 56 | return on_stairs; 57 | } 58 | 59 | } // namespace s_graphs -------------------------------------------------------------------------------- /lidar_situational_graphs/src/s_graphs/common/information_matrix_calculator.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | 6 | namespace s_graphs { 7 | 8 | InformationMatrixCalculator::InformationMatrixCalculator( 9 | const rclcpp::Node::SharedPtr node) { 10 | use_const_inf_matrix = 11 | node->get_parameter("use_const_inf_matrix").get_parameter_value().get(); 12 | const_stddev_x = 13 | node->get_parameter("const_stddev_x").get_parameter_value().get(); 14 | const_stddev_q = 15 | node->get_parameter("const_stddev_q").get_parameter_value().get(); 16 | 17 | var_gain_a = node->get_parameter("var_gain_a").get_parameter_value().get(); 18 | min_stddev_x = 19 | node->get_parameter("min_stddev_x").get_parameter_value().get(); 20 | max_stddev_x = 21 | node->get_parameter("max_stddev_x").get_parameter_value().get(); 22 | min_stddev_q = 23 | node->get_parameter("min_stddev_q").get_parameter_value().get(); 24 | max_stddev_q = 25 | node->get_parameter("max_stddev_q").get_parameter_value().get(); 26 | fitness_score_thresh = 27 | node->get_parameter("fitness_score_thresh").get_parameter_value().get(); 28 | } 29 | 30 | InformationMatrixCalculator::~InformationMatrixCalculator() {} 31 | 32 | Eigen::MatrixXd InformationMatrixCalculator::calc_information_matrix( 33 | const pcl::PointCloud::ConstPtr& cloud1, 34 | const pcl::PointCloud::ConstPtr& cloud2, 35 | const Eigen::Isometry3d& relpose) const { 36 | if (use_const_inf_matrix || cloud1->points.empty() || cloud2->points.empty()) { 37 | Eigen::MatrixXd inf = Eigen::MatrixXd::Identity(6, 6); 38 | inf.topLeftCorner(3, 3).array() /= const_stddev_x; 39 | inf.bottomRightCorner(3, 3).array() /= const_stddev_q; 40 | return inf; 41 | } 42 | 43 | double fitness_score = calc_fitness_score(cloud1, cloud2, relpose); 44 | 45 | double min_var_x = std::pow(min_stddev_x, 2); 46 | double max_var_x = std::pow(max_stddev_x, 2); 47 | double min_var_q = std::pow(min_stddev_q, 2); 48 | double max_var_q = std::pow(max_stddev_q, 2); 49 | 50 | float w_x = 51 | weight(var_gain_a, fitness_score_thresh, min_var_x, max_var_x, fitness_score); 52 | float w_q = 53 | weight(var_gain_a, fitness_score_thresh, min_var_q, max_var_q, fitness_score); 54 | 55 | Eigen::MatrixXd inf = Eigen::MatrixXd::Identity(6, 6); 56 | inf.topLeftCorner(3, 3).array() /= w_x; 57 | inf.bottomRightCorner(3, 3).array() /= w_q; 58 | return inf; 59 | } 60 | 61 | double InformationMatrixCalculator::calc_fitness_score( 62 | const pcl::PointCloud::ConstPtr& cloud1, 63 | const pcl::PointCloud::ConstPtr& cloud2, 64 | const Eigen::Isometry3d& relpose, 65 | double max_range) { 66 | pcl::search::KdTree::Ptr tree_(new pcl::search::KdTree()); 67 | tree_->setInputCloud(cloud1); 68 | 69 | double fitness_score = 0.0; 70 | 71 | // Transform the input dataset using the final transformation 72 | pcl::PointCloud input_transformed; 73 | pcl::transformPointCloud(*cloud2, input_transformed, relpose.cast()); 74 | 75 | std::vector nn_indices(1); 76 | std::vector nn_dists(1); 77 | 78 | // For each point in the source dataset 79 | int nr = 0; 80 | for (size_t i = 0; i < input_transformed.points.size(); ++i) { 81 | // Find its nearest neighbor in the target 82 | tree_->nearestKSearch(input_transformed.points[i], 1, nn_indices, nn_dists); 83 | 84 | // Deal with occlusions (incomplete targets) 85 | if (nn_dists[0] <= max_range) { 86 | // Add to the fitness score 87 | fitness_score += nn_dists[0]; 88 | nr++; 89 | } 90 | } 91 | 92 | if (nr > 0) 93 | return (fitness_score / nr); 94 | else 95 | return (std::numeric_limits::max()); 96 | } 97 | 98 | } // namespace s_graphs 99 | -------------------------------------------------------------------------------- /lidar_situational_graphs/src/s_graphs/frontend/floor_analyzer.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace s_graphs { 4 | 5 | FloorAnalyzer::FloorAnalyzer() {} 6 | 7 | FloorAnalyzer::~FloorAnalyzer() {} 8 | 9 | void FloorAnalyzer::perform_floor_segmentation( 10 | const std::vector& current_x_vert_planes, 11 | const std::vector& current_y_vert_planes, 12 | std::vector& floor_plane_candidates_vec) { 13 | // analyze the largest x plane pair 14 | situational_graphs_msgs::msg::PlaneData floor_x_plane1, floor_x_plane2; 15 | situational_graphs_msgs::msg::PlaneData floor_y_plane1, floor_y_plane2; 16 | floor_x_plane1.nx = -1; 17 | floor_x_plane2.nx = -1; 18 | floor_y_plane1.nx = -1; 19 | floor_y_plane2.nx = -1; 20 | 21 | float max_xplane_width = 0; 22 | for (auto x_plane1 : current_x_vert_planes) { 23 | if (x_plane1.nx < 0) { 24 | continue; 25 | } 26 | for (auto x_plane2 : current_x_vert_planes) { 27 | if (x_plane2.nx > 0) { 28 | continue; 29 | } 30 | 31 | float x_plane_width = PlaneUtils::width_between_planes(x_plane1, x_plane2); 32 | bool planes_placed_correctly = false; 33 | if (!x_plane1.plane_points.empty() && !x_plane2.plane_points.empty()) { 34 | planes_placed_correctly = PlaneUtils::compute_point_difference( 35 | x_plane1.plane_points.back().x, x_plane2.plane_points.back().x); 36 | } else 37 | continue; 38 | 39 | float dot_product = PlaneUtils::plane_dot_product(x_plane1, x_plane2); 40 | if (abs(dot_product) < 0.9) continue; 41 | 42 | if (x_plane_width > max_xplane_width && planes_placed_correctly) { 43 | max_xplane_width = x_plane_width; 44 | floor_x_plane1 = x_plane1; 45 | floor_x_plane2 = x_plane2; 46 | } 47 | } 48 | } 49 | 50 | float max_yplane_width = 0; 51 | for (auto y_plane1 : current_y_vert_planes) { 52 | if (y_plane1.ny < 0) { 53 | continue; 54 | } 55 | for (auto y_plane2 : current_y_vert_planes) { 56 | if (y_plane2.ny > 0) { 57 | continue; 58 | } 59 | 60 | float y_plane_width = PlaneUtils::width_between_planes(y_plane1, y_plane2); 61 | bool planes_placed_correctly = false; 62 | if (!y_plane1.plane_points.empty() && !y_plane2.plane_points.empty()) { 63 | planes_placed_correctly = PlaneUtils::compute_point_difference( 64 | y_plane1.plane_points.back().y, y_plane2.plane_points.back().y); 65 | } else 66 | continue; 67 | 68 | float dot_product = PlaneUtils::plane_dot_product(y_plane1, y_plane2); 69 | if (abs(dot_product) < 0.9) continue; 70 | 71 | if (y_plane_width > max_yplane_width && planes_placed_correctly) { 72 | max_yplane_width = y_plane_width; 73 | floor_y_plane1 = y_plane1; 74 | floor_y_plane2 = y_plane2; 75 | } 76 | } 77 | } 78 | 79 | // std::cout << "xplane1: " << floor_x_plane1.nx << ", " << floor_x_plane1.ny << ", " 80 | // << floor_x_plane1.nz << ", " << floor_x_plane1.d << std::endl; std::cout << 81 | // "xplane2: " << floor_x_plane2.nx << ", " << floor_x_plane2.ny << ", " << 82 | // floor_x_plane2.nz << ", " << floor_x_plane2.d << std::endl; std::cout << "yplane1: 83 | // " << floor_y_plane1.nx << ", " << floor_y_plane1.ny << ", " << floor_y_plane1.nz << 84 | // ", " << floor_y_plane1.d << std::endl; std::cout << "yplane2: " << 85 | // floor_y_plane2.nx << ", " << floor_y_plane2.ny << ", " << floor_y_plane2.nz << ", " 86 | // << floor_y_plane2.d << std::endl; 87 | 88 | if (floor_x_plane1.nx != -1) floor_plane_candidates_vec.push_back(floor_x_plane1); 89 | if (floor_x_plane2.nx != -1) floor_plane_candidates_vec.push_back(floor_x_plane2); 90 | if (floor_y_plane1.nx != -1) floor_plane_candidates_vec.push_back(floor_y_plane1); 91 | if (floor_y_plane2.nx != -1) floor_plane_candidates_vec.push_back(floor_y_plane2); 92 | } 93 | 94 | } // namespace s_graphs 95 | -------------------------------------------------------------------------------- /mkdocs.yml: -------------------------------------------------------------------------------- 1 | site_name: Lidar Situational Graphs 2 | site_url: https://snt-arg.github.io/lidar_situational_graphs 3 | repo_url: https://github.com/snt-arg/lidar_situational_graphs 4 | repo_name: snt-arg/lidar_situational_graphs 5 | theme: 6 | features: 7 | - content.code.copy 8 | - search.suggest 9 | - search.highlight 10 | icon: 11 | repo: fontawesome/brands/github 12 | name: material 13 | palette: 14 | # Dark Mode 15 | - scheme: slate 16 | primary: black 17 | accent: teal 18 | toggle: 19 | icon: material/weather-sunny 20 | name: Switch to light mode 21 | 22 | # Light Mode 23 | - scheme: default 24 | primary: black 25 | accent: teal 26 | toggle: 27 | icon: material/weather-night 28 | name: Switch to dark mode 29 | markdown_extensions: 30 | - admonition 31 | - abbr 32 | - attr_list 33 | - md_in_html 34 | - pymdownx.inlinehilite 35 | - pymdownx.snippets 36 | - pymdownx.details 37 | - pymdownx.superfences 38 | - pymdownx.blocks.caption 39 | - pymdownx.highlight: 40 | anchor_linenums: true 41 | line_spans: __span 42 | pygments_lang_class: true 43 | -------------------------------------------------------------------------------- /msg/FloorData.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | int32 id 4 | geometry_msgs/Pose floor_center 5 | int32[] keyframe_ids -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | cuda-python==12.2.0 2 | networkx==2.8.7 3 | numpy==1.23.4 4 | matplotlib>=3.3.1 5 | tensorboard==2.14.0 6 | colorama==0.4.6 7 | shapely==2.0.4 8 | markupsafe==2.1.5 9 | seaborn==0.13.2 10 | numexpr==2.8.4 11 | bottleneck==1.3.6 -------------------------------------------------------------------------------- /setup.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # - Installing cpp dependencies 4 | sudo apt update 5 | sudo apt install -y python3-vcstool 6 | sudo apt install -y libceres-dev 7 | sudo apt install -y python3-pip 8 | 9 | #install python dependencies 10 | pip3 install torch==2.0.1+cu118 -f https://download.pytorch.org/whl/torch_stable.html 11 | pip3 install torch-geometric==2.3.1 12 | pip3 install torch-sparse==0.6.17+pt20cu118 -f https://data.pyg.org/whl/torch-2.0.1+cu118.html 13 | pip3 install pyg-lib -f https://data.pyg.org/whl/torch-2.0.1+cu118.html 14 | pip install protobuf==3.20.* 15 | 16 | #rosdep init 17 | rosdep update --include-eol-distros 18 | 19 | #import all repos 20 | vcs import . < .rosinstall_ros2 21 | 22 | #install python requirements 23 | pip3 install -r requirements.txt 24 | 25 | #rosdep install 26 | rosdep install --from-paths . -y --ignore-src -r 27 | 28 | # - Importing all dependencies 29 | colcon build --symlink-install --cmake-args -DUSE_RGB_CLOUD=ON 30 | 31 | # - Sourcing the workspace 32 | source install/setup.bash --------------------------------------------------------------------------------