├── resource └── fgsp ├── src └── fgsp │ ├── controller │ ├── __init__.py │ ├── command_post.py │ └── signal_handler.py │ ├── tools │ ├── __init__.py │ ├── path_transformer.py │ ├── cloud_saver.py │ ├── object_publisher.py │ ├── lookup_aligned_pose.py │ ├── cloud_publisher.py │ └── simulation.py │ ├── graph │ ├── __init__.py │ ├── hierarchical_graph.py │ ├── base_graph.py │ ├── wavelet_evaluator.py │ └── global_graph.py │ ├── __init__.py │ ├── common │ ├── __init__.py │ ├── font.py │ ├── lc_model.py │ ├── signal_node.py │ ├── comms.py │ ├── logger.py │ ├── signal_synchronizer.py │ ├── transform_history.py │ ├── utils.py │ ├── robot_constraints.py │ ├── visualizer.py │ ├── plotter.py │ └── config.py │ ├── classifier │ ├── __init__.py │ ├── simple_classifier.py │ ├── downstream_result.py │ ├── top_classifier.py │ ├── windowed_result.py │ └── classification_result.py │ ├── graph_monitor.py │ └── graph_client.py ├── setup.cfg ├── data └── README.md ├── deploy ├── compile.sh ├── install_packages.sh ├── install_base.sh ├── set_env.sh ├── build_docker.sh ├── Dockerfile.fgsp ├── copy_to_ctx.sh └── install_ros.sh ├── config ├── cloud_saver_config.yaml ├── object_publisher_config.yaml ├── lookup_aligned_pose.yaml ├── objects.yaml ├── cloud_publisher_config.yaml ├── simulation_config.yaml ├── path_transformer_config.yaml ├── monitor_config.yaml ├── reproject.yaml └── local_client_config.yaml ├── script ├── run_debug ├── run_graph_monitor ├── run_camel ├── run_cerberus └── run_chimera ├── .gitmodules ├── launch ├── reproject_pub_launch.py ├── cloud_saver_launch.py ├── simulation_launch.py ├── lookup_pose_launch.py ├── cloud_publisher_launch.py ├── object_publisher_launch.py ├── graph_monitor_launch.py ├── path_transformer_launch.py └── graph_client_launch.py ├── package.xml ├── LICENSE ├── setup.py ├── .gitignore └── README.md /resource/fgsp: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /src/fgsp/controller/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /src/fgsp/tools/__init__.py: -------------------------------------------------------------------------------- 1 | from .reproject_pub import ReprojectPub 2 | -------------------------------------------------------------------------------- /setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script_dir=$base/lib/fgsp 3 | [install] 4 | install_scripts=$base/lib/fgsp 5 | -------------------------------------------------------------------------------- /data/README.md: -------------------------------------------------------------------------------- 1 | # Data Folder 2 | 3 | The monitor and client will automatically write some data in this folder. 4 | -------------------------------------------------------------------------------- /src/fgsp/graph/__init__.py: -------------------------------------------------------------------------------- 1 | from .global_graph import GlobalGraph 2 | from .hierarchical_graph import HierarchicalGraph 3 | from .base_graph import BaseGraph 4 | -------------------------------------------------------------------------------- /deploy/compile.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | source /opt/ros/humble/setup.bash 3 | colcon build --packages-select maplab_msgs 4 | colcon build --packages-select fgsp 5 | -------------------------------------------------------------------------------- /src/fgsp/__init__.py: -------------------------------------------------------------------------------- 1 | from .graph_client import * 2 | from .graph_monitor import * 3 | 4 | from .classifier import * 5 | from .common import * 6 | from .controller import * 7 | from .graph import * 8 | -------------------------------------------------------------------------------- /config/cloud_saver_config.yaml: -------------------------------------------------------------------------------- 1 | cloud_saver: 2 | ros__parameters: 3 | cloud_topic: "/gt_map_cloud" 4 | export_path: "/tmp/cloud.npy" 5 | extract_intensity: False 6 | sequential: False 7 | -------------------------------------------------------------------------------- /src/fgsp/common/__init__.py: -------------------------------------------------------------------------------- 1 | from .utils import Utils 2 | from .config import MonitorConfig, ClientConfig 3 | from .plotter import Plotter 4 | from .visualizer import Visualizer 5 | from .logger import Logger 6 | -------------------------------------------------------------------------------- /config/object_publisher_config.yaml: -------------------------------------------------------------------------------- 1 | object_publisher: 2 | ros__parameters: 3 | marker_color: [0.7, 0.7, 0.0] 4 | object_file: "/home/berlukas/Documents/workspace/ros2/fgsp_ws/src/fgsp/config/objects.yaml" 5 | -------------------------------------------------------------------------------- /script/run_debug: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env zsh 2 | 3 | fgsp_dir=$(catkin locate fgsp) 4 | docker run -it --mount type=bind,source="${fgsp_dir}",target=/mnt/data fgsp.monitor_and_client "roslaunch" "fgsp" "local_client.launch" 5 | -------------------------------------------------------------------------------- /script/run_graph_monitor: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env zsh 2 | 3 | fgsp_dir=$(catkin locate fgsp) 4 | docker run -it --mount type=bind,source="${fgsp_dir}",target=/mnt/data fgsp.monitor_and_client "roslaunch" "fgsp" "graph_monitor.launch" 5 | -------------------------------------------------------------------------------- /script/run_camel: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env zsh 2 | 3 | fgsp_dir=$(catkin locate fgsp) 4 | docker run -it --mount type=bind,source="${fgsp_dir}",target=/mnt/data fgsp.monitor_and_client "roslaunch" "fgsp" "rsl_anymal_camel_client.launch" 5 | -------------------------------------------------------------------------------- /deploy/install_packages.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env zsh 2 | 3 | # Install required dependencies 4 | pip3 install pandas numpy scipy 5 | 6 | ## Install the dependencies 7 | pip3 install -e ws/src/liegroups/. 8 | pip3 install -e ws/src/pygsp/. 9 | -------------------------------------------------------------------------------- /script/run_cerberus: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env zsh 2 | 3 | fgsp_dir=$(catkin locate fgsp) 4 | docker run -it --mount type=bind,source="${fgsp_dir}",target=/mnt/data fgsp.monitor_and_client "roslaunch" "fgsp" "rsl_anymal_cerberus_client.launch" 5 | -------------------------------------------------------------------------------- /script/run_chimera: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env zsh 2 | 3 | fgsp_dir=$(catkin locate fgsp) 4 | docker run -it --mount type=bind,source="${fgsp_dir}",target=/mnt/data fgsp.monitor_and_client "roslaunch" "fgsp" "rsl_anymal_chimera_client.launch" 5 | -------------------------------------------------------------------------------- /src/fgsp/classifier/__init__.py: -------------------------------------------------------------------------------- 1 | from .classification_result import ClassificationResult 2 | from .downstream_result import DownstreamResult 3 | from .simple_classifier import SimpleClassifier 4 | from .top_classifier import TopClassifier 5 | -------------------------------------------------------------------------------- /config/lookup_aligned_pose.yaml: -------------------------------------------------------------------------------- 1 | lookup_aligned_pose: 2 | ros__parameters: 3 | input_file: "/media/berlukas/Data/data/datasets/fgsp/results/v2/darpa_artifact_img/paths/corrected.npy" 4 | align_to_file: "" 5 | timestamp_ns: 1632429922429886580 6 | -------------------------------------------------------------------------------- /config/objects.yaml: -------------------------------------------------------------------------------- 1 | objects: 2 | - type: cell_phone 3 | position: [28.716, 56.422, 0.396] 4 | - type: cell_phone 5 | position: [25.05211635, 57.37775901, -2.24574098] 6 | - type: cell_phone 7 | position: [24.1107, 57.7283, -1.7493] 8 | -------------------------------------------------------------------------------- /deploy/install_base.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env zsh 2 | 3 | # Base install 4 | apt update 5 | apt -y upgrade 6 | apt -y install software-properties-common \ 7 | python3 \ 8 | python3-pip \ 9 | python3-termcolor \ 10 | git \ 11 | curl \ 12 | iproute2 \ 13 | gnupg \ 14 | lsb-release 15 | -------------------------------------------------------------------------------- /deploy/set_env.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | . /usr/home/ws/install/setup.bash 4 | host_ip=$(ip route | awk '/default/ {print $3}') 5 | container_ip=$(hostname -I | awk '{gsub(/^ +| +$/,"")} {print $0}') 6 | export ROS_MASTER_URI=http://${host_ip}:11311 7 | export ROS_IP=${container_ip} 8 | exec "$@" 9 | -------------------------------------------------------------------------------- /src/fgsp/common/font.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | 3 | class Font: 4 | PURPLE = '\033[95m' 5 | CYAN = '\033[96m' 6 | DARKCYAN = '\033[36m' 7 | BLUE = '\033[94m' 8 | GREEN = '\033[92m' 9 | YELLOW = '\033[93m' 10 | RED = '\033[91m' 11 | BOLD = '\033[1m' 12 | UNDERLINE = '\033[4m' 13 | END = '\033[0m' 14 | CLEAR = '\033c' 15 | -------------------------------------------------------------------------------- /config/cloud_publisher_config.yaml: -------------------------------------------------------------------------------- 1 | cloud_publisher: 2 | ros__parameters: 3 | cloud_topic: "/ply_cloud" 4 | cloud_input_path: "/tmp/pub/" 5 | 6 | # path_topic: "/path" 7 | # path_input_path: "/media/berlukas/Data/data/datasets/fgsp/results/v2/darpa_artifact_img/paths/" 8 | 9 | rate: 0.1 10 | use_voxel_grid: False 11 | voxel_size: 0.15 12 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "dependencies/pygsp"] 2 | path = dependencies/pygsp 3 | url = git@github.com:epfl-lts2/pygsp.git 4 | [submodule "dependencies/liegroups"] 5 | path = dependencies/liegroups 6 | url = git@github.com:utiasSTARS/liegroups.git 7 | [submodule "dependencies/maplab_msgs"] 8 | path = dependencies/maplab_msgs 9 | url = https://github.com/ethz-asl/maplab_msgs 10 | -------------------------------------------------------------------------------- /config/simulation_config.yaml: -------------------------------------------------------------------------------- 1 | simulation: 2 | ros__parameters: 3 | server_file: "/media/berlukas/Data/data/datasets/fgsp/results/v2/seemuehle_img/paths/anymal_2.csv" 4 | # robot_file: "/home/berlukas/Documents/workspace/darpa/src/trajectory_eval/Trajectories/1_cerberus/2_compslam_post/compslam_post_tum.csv" 5 | 6 | robot_name: "anymal_2" 7 | map_frame: "map" 8 | child_frame: "base" 9 | graph_threshold_dist: 20.0 10 | 11 | odom_topic: "/odometry" 12 | monitor_topic: "/graph_monitor/sparse_graph/trajectory" 13 | 14 | out_bag_file: "/tmp/odom_and_monitor.bag" 15 | -------------------------------------------------------------------------------- /deploy/build_docker.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env zsh 2 | 3 | # Get the current directory. 4 | script_source=$(readlink -f "$0") 5 | script_dir=$(dirname "$script_source") 6 | 7 | docker_ctx=${script_dir}/docker_ctx 8 | echo "Creating build context in: ${docker_ctx}" 9 | 10 | # Create the context we use for setting up the images. 11 | mkdir -p "${docker_ctx}" 12 | "$script_dir/copy_to_ctx.sh" 13 | 14 | # Build the docker image. 15 | docker build -t "fgsp.monitor_and_client" -f "${script_dir}/Dockerfile.fgsp" "${docker_ctx}" 16 | 17 | # Clean up. 18 | echo "Finished building the images. Cleaning up." 19 | rm -rf "${docker_ctx}" 20 | -------------------------------------------------------------------------------- /config/path_transformer_config.yaml: -------------------------------------------------------------------------------- 1 | path_transformer: 2 | ros__parameters: 3 | input_path: "/media/berlukas/Data/data/datasets/fgsp/results/v2/darpa_finals/anymal_4/compslam.csv" 4 | export_path: "/tmp/transformed.npy" 5 | 6 | T: 7 | [ 8 | 0.999960, 9 | 0.004036, 10 | 0.007961, 11 | 0.018325, 12 | -0.003881, 13 | 0.999806, 14 | -0.019309, 15 | 0.021186, 16 | -0.008037, 17 | 0.019277, 18 | 0.999782, 19 | 0.101594, 20 | 0.000000, 21 | 0.000000, 22 | 0.000000, 23 | 1.000000, 24 | ] 25 | -------------------------------------------------------------------------------- /launch/reproject_pub_launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | import yaml 3 | 4 | from ament_index_python.packages import get_package_share_directory 5 | from launch import LaunchDescription 6 | from launch_ros.actions import Node 7 | 8 | 9 | def generate_launch_description(): 10 | ld = LaunchDescription() 11 | 12 | config = os.path.join( 13 | get_package_share_directory('fgsp'), 14 | 'config', 15 | 'reproject.yaml' 16 | ) 17 | 18 | pub = Node( 19 | package="fgsp", 20 | executable="reproject_pub", 21 | output={'both': 'screen'}, 22 | emulate_tty=True, 23 | parameters=[config] 24 | ) 25 | ld.add_action(pub) 26 | 27 | return ld 28 | -------------------------------------------------------------------------------- /launch/cloud_saver_launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from ament_index_python.packages import get_package_share_directory 4 | from launch import LaunchDescription 5 | from launch_ros.actions import Node 6 | 7 | 8 | def generate_launch_description(): 9 | ld = LaunchDescription() 10 | 11 | config = os.path.join( 12 | get_package_share_directory('fgsp'), 13 | 'config', 14 | 'cloud_saver_config.yaml' 15 | ) 16 | 17 | saver_node = Node( 18 | package="fgsp", 19 | executable="cloud_saver", 20 | output={'both': 'screen'}, 21 | emulate_tty=True, 22 | parameters=[config] 23 | ) 24 | ld.add_action(saver_node) 25 | 26 | return ld 27 | -------------------------------------------------------------------------------- /launch/simulation_launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from ament_index_python.packages import get_package_share_directory 4 | from launch import LaunchDescription 5 | from launch_ros.actions import Node 6 | 7 | 8 | def generate_launch_description(): 9 | ld = LaunchDescription() 10 | 11 | config = os.path.join( 12 | get_package_share_directory('fgsp'), 13 | 'config', 14 | 'simulation_config.yaml' 15 | ) 16 | 17 | simulation_node = Node( 18 | package="fgsp", 19 | executable="simulation", 20 | output={'both': 'screen'}, 21 | emulate_tty=True, 22 | parameters=[config] 23 | ) 24 | ld.add_action(simulation_node) 25 | 26 | return ld 27 | -------------------------------------------------------------------------------- /launch/lookup_pose_launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from ament_index_python.packages import get_package_share_directory 4 | from launch import LaunchDescription 5 | from launch_ros.actions import Node 6 | 7 | 8 | def generate_launch_description(): 9 | ld = LaunchDescription() 10 | 11 | config = os.path.join( 12 | get_package_share_directory('fgsp'), 13 | 'config', 14 | 'lookup_aligned_pose.yaml' 15 | ) 16 | 17 | lookup_node = Node( 18 | package="fgsp", 19 | executable="lookup_aligned_pose", 20 | output={'both': 'screen'}, 21 | emulate_tty=True, 22 | parameters=[config] 23 | ) 24 | ld.add_action(lookup_node) 25 | 26 | return ld 27 | -------------------------------------------------------------------------------- /launch/cloud_publisher_launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from ament_index_python.packages import get_package_share_directory 4 | from launch import LaunchDescription 5 | from launch_ros.actions import Node 6 | 7 | 8 | def generate_launch_description(): 9 | ld = LaunchDescription() 10 | 11 | config = os.path.join( 12 | get_package_share_directory('fgsp'), 13 | 'config', 14 | 'cloud_publisher_config.yaml' 15 | ) 16 | 17 | publisher_node = Node( 18 | package="fgsp", 19 | executable="cloud_publisher", 20 | output={'both': 'screen'}, 21 | emulate_tty=True, 22 | parameters=[config] 23 | ) 24 | ld.add_action(publisher_node) 25 | 26 | return ld 27 | -------------------------------------------------------------------------------- /launch/object_publisher_launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from ament_index_python.packages import get_package_share_directory 4 | from launch import LaunchDescription 5 | from launch_ros.actions import Node 6 | 7 | 8 | def generate_launch_description(): 9 | ld = LaunchDescription() 10 | 11 | config = os.path.join( 12 | get_package_share_directory('fgsp'), 13 | 'config', 14 | 'object_publisher_config.yaml' 15 | ) 16 | 17 | publisher_node = Node( 18 | package="fgsp", 19 | executable="object_publisher", 20 | output={'both': 'screen'}, 21 | emulate_tty=True, 22 | parameters=[config] 23 | ) 24 | ld.add_action(publisher_node) 25 | 26 | return ld 27 | -------------------------------------------------------------------------------- /launch/graph_monitor_launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from ament_index_python.packages import get_package_share_directory 4 | from launch import LaunchDescription 5 | from launch_ros.actions import Node 6 | 7 | 8 | def generate_launch_description(): 9 | ld = LaunchDescription() 10 | 11 | config = os.path.join( 12 | get_package_share_directory('fgsp'), 13 | 'config', 14 | 'monitor_config.yaml' 15 | ) 16 | 17 | monitor_node = Node( 18 | package="fgsp", 19 | executable="graph_monitor", 20 | output={'both': 'screen'}, 21 | emulate_tty=True, 22 | parameters=[config, {'use_sim_time': True}] 23 | ) 24 | ld.add_action(monitor_node) 25 | 26 | return ld 27 | -------------------------------------------------------------------------------- /launch/path_transformer_launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from ament_index_python.packages import get_package_share_directory 4 | from launch import LaunchDescription 5 | from launch_ros.actions import Node 6 | 7 | 8 | def generate_launch_description(): 9 | ld = LaunchDescription() 10 | 11 | config = os.path.join( 12 | get_package_share_directory('fgsp'), 13 | 'config', 14 | 'path_transformer_config.yaml' 15 | ) 16 | 17 | transformer_node = Node( 18 | package="fgsp", 19 | executable="path_transformer", 20 | output={'both': 'screen'}, 21 | emulate_tty=True, 22 | parameters=[config] 23 | ) 24 | ld.add_action(transformer_node) 25 | 26 | return ld 27 | -------------------------------------------------------------------------------- /launch/graph_client_launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from ament_index_python.packages import get_package_share_directory 4 | from launch import LaunchDescription 5 | from launch_ros.actions import Node 6 | 7 | 8 | def generate_launch_description(): 9 | ld = LaunchDescription() 10 | 11 | config = os.path.join( 12 | get_package_share_directory('fgsp'), 13 | 'config', 14 | 'local_client_config.yaml' 15 | ) 16 | 17 | client_node = Node( 18 | package="fgsp", 19 | executable="graph_client", 20 | output={'both': 'screen'}, 21 | emulate_tty=True, 22 | parameters=[config, {'use_sim_time': True}] 23 | ) 24 | ld.add_action(client_node) 25 | 26 | return ld 27 | -------------------------------------------------------------------------------- /deploy/Dockerfile.fgsp: -------------------------------------------------------------------------------- 1 | FROM ubuntu:22.04 2 | 3 | # Install the base packages. 4 | COPY install_base.sh ./install_base.sh 5 | RUN sh ./install_base.sh && rm ./install_base.sh 6 | 7 | # Install the ROS environment. 8 | ENV TZ=Europe/Zurich 9 | RUN ln -snf /usr/share/zoneinfo/$TZ /etc/localtime && echo $TZ > /etc/timezone 10 | COPY install_ros.sh ./install_ros.sh 11 | RUN ./install_ros.sh && rm ./install_ros.sh 12 | 13 | # Install the custom packages for which we will switch to the home directory. 14 | WORKDIR /usr/home 15 | COPY ./ws ./ws 16 | COPY install_packages.sh ./install_packages.sh 17 | RUN sh ./install_packages.sh && rm ./install_packages.sh 18 | 19 | # Build the project. 20 | WORKDIR /usr/home/ws 21 | SHELL ["/bin/bash", "-c"] 22 | COPY compile.sh ../ 23 | RUN ../compile.sh 24 | COPY set_env.sh ../ 25 | ENTRYPOINT ["../set_env.sh"] 26 | -------------------------------------------------------------------------------- /src/fgsp/common/lc_model.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | 3 | import numpy as np 4 | from scipy.spatial.transform import Rotation 5 | 6 | 7 | class LcModel(object): 8 | def __init__(self, ts_from, ts_to, T_a_b): 9 | self.ts_from = ts_from 10 | self.ts_to = ts_to 11 | self.T_a_b = self.convert_pose_msg(T_a_b) 12 | 13 | def convert_pose_msg(self, T_a_b): 14 | t_a_b = T_a_b.pose.position 15 | q_a_b = T_a_b.pose.orientation 16 | R_a_b = Rotation.from_quat([q_a_b.x, q_a_b.y, q_a_b.z, q_a_b.w]) 17 | 18 | T_a_b = np.eye(4, 4) 19 | T_a_b[0:3, 0:3] = R_a_b.as_matrix() 20 | T_a_b[0:3, 3] = np.array([t_a_b.x, t_a_b.y, t_a_b.z]) 21 | return T_a_b 22 | 23 | def get_translation(self): 24 | return self.T_a_b[0:3, 3] 25 | 26 | def get_rotation_quat(self): 27 | R = self.T_a_b[0:3, 0:3] 28 | return Rotation.from_matrix(R).as_quat() # x, y, z, w 29 | -------------------------------------------------------------------------------- /src/fgsp/common/signal_node.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | 3 | 4 | class SignalNode(object): 5 | def __init__(self): 6 | self.ts = None 7 | self.id = None 8 | self.robot_name = None 9 | self.position = None 10 | self.orientation = None 11 | self.residual = None 12 | self.degenerate = None 13 | 14 | def init(self, ts, id, robot_name, position, orientation, residual=0, degenerate=False): 15 | self.ts = ts 16 | self.id = id 17 | self.robot_name = robot_name 18 | self.position = position 19 | self.orientation = orientation 20 | self.residual = residual 21 | self.degenerate = degenerate 22 | 23 | def init_onboard(self, ts, robot_name, position, orientation, degenerate): 24 | self.ts = ts 25 | self.robot_name = robot_name 26 | self.position = position 27 | self.orientation = orientation 28 | self.degenerate = degenerate 29 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | fgsp 5 | 2.0.0 6 | Factor Graph Signal Processing 7 | Lukas Bernreiter 8 | MIT 9 | 10 | rclpy 11 | sensor_msgs_py 12 | python3-numpy 13 | sensor_msgs 14 | std_msgs 15 | nav_msgs 16 | maplab_msgs 17 | 18 | ament_copyright 19 | ament_flake8 20 | ament_pep257 21 | python3-pytest 22 | 23 | 24 | ament_python 25 | 26 | 27 | -------------------------------------------------------------------------------- /src/fgsp/classifier/simple_classifier.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | 3 | import numpy as np 4 | 5 | from src.fgsp.common.logger import Logger 6 | 7 | 8 | class SimpleClassifier(object): 9 | 10 | def classify(self, data): 11 | n_nodes = data.shape[0] 12 | labels = [] 13 | for i in range(0, n_nodes): 14 | low_mean = data[i, 0] 15 | mid_mean = data[i, 1] 16 | high_mean = data[i, 2] 17 | dists = np.array([low_mean, mid_mean, high_mean]) 18 | 19 | np.set_printoptions(suppress=True) 20 | local_labels = [] 21 | 22 | if dists[0] > 0.005: 23 | local_labels.append(3) 24 | if dists[1] > 0.002: 25 | local_labels.append(2) 26 | if dists[2] > 0.001: 27 | local_labels.append(1) 28 | 29 | Logger.LogDebug(f'WaveletEvaluator: dists are {dists}') 30 | Logger.LogDebug( 31 | f'WaveletEvaluator: Local labels are {local_labels}') 32 | labels.append(local_labels) 33 | return labels 34 | -------------------------------------------------------------------------------- /deploy/copy_to_ctx.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env zsh 2 | 3 | # Get the current directory. 4 | script_source=$(readlink -f "$0") 5 | script_dir=$(dirname "$script_source") 6 | docker_ctx=${script_dir}/docker_ctx 7 | ws=${docker_ctx}/ws/src/fgsp/ 8 | ws_src=${ws}/.. 9 | project_dir=${script_dir}/.. 10 | 11 | # Copy the project files to the context. 12 | mkdir -p "${ws}" 13 | rsync -a \ 14 | --exclude '*.pyc' \ 15 | --exclude '*.ipynb' \ 16 | --exclude '__pycache__' \ 17 | --exclude '*.git' \ 18 | --exclude '*.md' \ 19 | --exclude 'deploy' \ 20 | --exclude 'dependencies' \ 21 | --exclude 'data' \ 22 | --exclude '.ipynb_checkpoints' \ 23 | "${project_dir}" "${ws}" 24 | 25 | # Copy the dependencies to the context. 26 | rsync -a \ 27 | "${project_dir}/dependencies/" "${ws_src}" 28 | 29 | # Copy the installation script to the context. 30 | cp "${script_dir}/install_base.sh" "${docker_ctx}" 31 | cp "${script_dir}/install_ros.sh" "${docker_ctx}" 32 | cp "${script_dir}/install_packages.sh" "${docker_ctx}" 33 | cp "${script_dir}/set_env.sh" "${docker_ctx}" 34 | cp "${script_dir}/compile.sh" "${docker_ctx}" 35 | -------------------------------------------------------------------------------- /src/fgsp/classifier/downstream_result.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | 3 | from src.fgsp.classifier import ClassificationResult 4 | 5 | 6 | class DownstreamResult(ClassificationResult): 7 | def __init__(self, config, robot_name, opt_nodes, features, labels, indices): 8 | super().__init__(config, robot_name, opt_nodes, features, labels) 9 | self.indices = indices 10 | n_nodes = len(self.opt_nodes) 11 | self.labels = self.create_downstream_labels(n_nodes, labels) 12 | 13 | def create_downstream_labels(self, n_nodes, labels): 14 | # Labels in the current hierarchy 15 | n_labels = len(labels) 16 | last_label = n_labels - 1 17 | 18 | downstream_labels = [None] * n_nodes 19 | for idx in range(0, last_label): 20 | for i in range(self.indices[idx], self.indices[idx+1]): 21 | downstream_labels[i] = labels[idx] 22 | 23 | # Fix remainder 24 | for i in range(self.indices[last_label], n_nodes): 25 | downstream_labels[i] = labels[last_label] 26 | 27 | return downstream_labels 28 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2022 ETHZ ASL 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /src/fgsp/common/comms.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | 3 | import rclpy 4 | from rclpy.node import Node 5 | 6 | 7 | class Comms(object): 8 | _instance = None 9 | 10 | def __new__(self): 11 | if self._instance is None: 12 | self._instance = super(Comms, self).__new__(self) 13 | self._instance.node = None 14 | return self._instance 15 | 16 | def publish(self, msg, type, topic): 17 | publisher = self.node.create_publisher(type, topic, 10) 18 | publisher.publish(msg) 19 | 20 | def time_now(self): 21 | return self.node.get_clock().now() 22 | 23 | 24 | if __name__ == '__main__': 25 | from std_msgs.msg import String 26 | 27 | rclpy.init() 28 | 29 | msg = String() 30 | msg2 = String() 31 | msg.data = "Foo" 32 | msg2.data = "Bar" 33 | 34 | comms = Comms() 35 | comms.publish(msg, String, "/foo") 36 | 37 | comms2 = Comms() 38 | comms2.publish(msg2, String, "/foo") 39 | 40 | print(f'Singleton? {comms is comms2}') 41 | 42 | rclpy.spin_once(comms.node, timeout_sec=1) 43 | comms.node.destroy_node() 44 | rclpy.shutdown() 45 | -------------------------------------------------------------------------------- /deploy/install_ros.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | # Install ROS humble 4 | curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg 5 | echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(source /etc/os-release && echo $UBUNTU_CODENAME) main" | tee /etc/apt/sources.list.d/ros2.list > /dev/null 6 | 7 | apt -y update && apt -y install \ 8 | build-essential \ 9 | cmake \ 10 | git \ 11 | python3-colcon-common-extensions \ 12 | python3-flake8 \ 13 | python3-flake8-blind-except \ 14 | python3-flake8-builtins 15 | python3-flake8-class-newline \ 16 | python3-flake8-comprehensions \ 17 | python3-flake8-deprecated \ 18 | python3-flake8-docstrings \ 19 | python3-flake8-import-order \ 20 | python3-flake8-quotes \ 21 | python3-pip \ 22 | python3-pytest \ 23 | python3-pytest-cov \ 24 | python3-pytest-repeat \ 25 | python3-pytest-rerunfailures \ 26 | python3-rosdep \ 27 | python3-setuptools \ 28 | python3-vcstool \ 29 | wget 30 | 31 | apt -y install ros-humble-ros-base 32 | apt -y install ros-humble-sensor-msgs-py -------------------------------------------------------------------------------- /config/monitor_config.yaml: -------------------------------------------------------------------------------- 1 | graph_monitor: 2 | ros__parameters: 3 | in_graph_topic: /maplab_server/sparse_graph/graph 4 | in_traj_opt_topic: /maplab_server/sparse_graph/trajectory 5 | out_graph_topic: /graph_monitor/sparse_graph/graph 6 | out_traj_opt_topic: /graph_monitor/sparse_graph/trajectory 7 | min_node_count: 20 8 | 9 | update_rate: 0.05 10 | verification_service: /graph_monitor/verification 11 | verification_server_request: /maplab_server/verification 12 | 13 | # Reduction configuration 14 | reduce_global_graph: False 15 | reduction_method: "largest_ev" # positive_ev, negative_ev, largest_ev, every_other 16 | reduce_to_n_percent: 0.8 17 | 18 | opt_pc_topic: "/maplab_server/sparse_graph/submap" 19 | enable_graph_building: True 20 | enable_submap_constraints: True 21 | enable_submap_map_publishing: True 22 | accumulated_map_topic: "/graph_monitor/map" 23 | submap_constraint_topic: "/graph_monitor/submaps" 24 | submap_constraint_pivot_distance: 30.0 25 | submap_constraint_min_distance: 5.0 26 | submap_constraint_knn: 20 27 | submap_constraint_p_norm: 2 28 | submap_constraint_refine_icp: False 29 | submap_constraint_export_lidar_poses: False 30 | submap_min_count: 1 31 | send_separate_traj_msgs: False 32 | -------------------------------------------------------------------------------- /src/fgsp/common/logger.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | 3 | import time 4 | from .font import Font 5 | 6 | 7 | class Logger(object): 8 | Verbosity = 2 9 | 10 | @staticmethod 11 | def LogDebug(msg): 12 | if Logger.Verbosity > 4: 13 | ts_ns = time.time() * 1e6 14 | print(f'{Font.BLUE}[{ts_ns}] {msg} {Font.END}') 15 | 16 | @staticmethod 17 | def LogInfo(msg): 18 | if Logger.Verbosity > 0: 19 | ts_ns = time.time() * 1e6 20 | print(f'[{ts_ns}] {msg}') 21 | 22 | @staticmethod 23 | def LogWarn(msg): 24 | if Logger.Verbosity > 1: 25 | ts_ns = time.time() * 1e6 26 | print(f'{Font.YELLOW}[{ts_ns}] {msg} {Font.END}') 27 | 28 | @staticmethod 29 | def LogError(msg): 30 | if Logger.Verbosity > 1: 31 | ts_ns = time.time() * 1e6 32 | print(f'{Font.RED}[{ts_ns}] {msg} {Font.END}') 33 | 34 | @staticmethod 35 | def LogFatal(msg): 36 | if Logger.Verbosity > 3: 37 | ts_ns = time.time() * 1e6 38 | print(f'{Font.RED}{Font.BOLD} [{ts_ns}] {msg} {Font.END}') 39 | exit() 40 | 41 | 42 | if __name__ == '__main__': 43 | Logger.LogDebug("This is a debug message.") 44 | Logger.LogInfo("This is a info message.") 45 | Logger.LogWarn("This is a warning message.") 46 | Logger.LogError("This is a error message.") 47 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import os 3 | from setuptools import setup 4 | from glob import glob 5 | 6 | package_name = 'fgsp' 7 | 8 | setup( 9 | name=package_name, 10 | version='2.0.0', 11 | packages=[f'src/{package_name}', f'src/{package_name}.classifier', f'src/{package_name}.tools', 12 | f'src/{package_name}.graph', f'src/{package_name}.common', f'src/{package_name}.controller'], 13 | data_files=[ 14 | ('share/ament_index/resource_index/packages', 15 | ['resource/' + package_name]), 16 | ('share/' + package_name, ['package.xml']), 17 | (os.path.join('share', package_name), glob('launch/*launch.py')), 18 | (os.path.join('share', package_name, 'config'), glob('config/*.yaml')) 19 | ], 20 | install_requires=['setuptools'], 21 | zip_safe=True, 22 | maintainer='Lukas Bernreiter', 23 | maintainer_email='berlukas@ethz.ch', 24 | description='Factor Graph Signal Processing', 25 | license='MIT', 26 | tests_require=['pytest'], 27 | entry_points={ 28 | 'console_scripts': [ 29 | 'graph_monitor = src.fgsp.graph_monitor:main', 30 | 'graph_client = src.fgsp.graph_client:main', 31 | 'simulation = src.fgsp.tools.simulation:main', 32 | 'reproject_pub = src.fgsp.tools.reproject_pub:main', 33 | 'cloud_saver = src.fgsp.tools.cloud_saver:main', 34 | 'cloud_publisher = src.fgsp.tools.cloud_publisher:main', 35 | 'lookup_aligned_pose = src.fgsp.tools.lookup_aligned_pose:main', 36 | 'object_publisher = src.fgsp.tools.object_publisher:main', 37 | 'path_transformer = src.fgsp.tools.path_transformer:main', 38 | ], 39 | }, 40 | ) 41 | -------------------------------------------------------------------------------- /config/reproject.yaml: -------------------------------------------------------------------------------- 1 | reproject_viz: 2 | ros__parameters: 3 | dataroot: "/media/berlukas/Data/data/datasets/fgsp/results/v2/h_floor_vid2/" 4 | gt_traj_file: "gt.csv" 5 | est_traj_file: "compslam.csv" 6 | corr_traj_file: "poses.csv" 7 | constraints_file: "opt_labels.dat" 8 | connections_file: "opt_connections.dat" 9 | 10 | cloud_in: "/point_cloud_filter/lidar/point_cloud_filtered" 11 | skip_every_nth: 3 12 | voxel_size: 0.1 13 | update_time_s: 1.0 14 | publish_small_constraints: True 15 | publish_mid_constraints: True 16 | publish_large_constraints: True 17 | 18 | enable_gt: False 19 | enable_est: True 20 | enable_corr: True 21 | enable_graph: False 22 | 23 | graph_coords_file: "data/opt_graph_coords.npy" 24 | hierarchy_levels: 3 25 | z_offset: 10.0 26 | 27 | constraint_ts_eps_s: 0.04 28 | compute_splines: True 29 | spline_points: 10 30 | spline_low_height: 1 31 | spline_mid_height: 4 32 | spline_large_height: 7 33 | 34 | T_GT_EST: 35 | [ 36 | 0.918835, 37 | -0.394608, 38 | -0.005173, 39 | 1.077152, 40 | 0.394460, 41 | 0.918731, 42 | -0.018294, 43 | -0.268902, 44 | 0.011971, 45 | 0.014769, 46 | 0.999819, 47 | -0.428035, 48 | 0.000000, 49 | 0.000000, 50 | 0.000000, 51 | 1.000000, 52 | ] 53 | 54 | T_GT_CORR: 55 | [ 56 | 0.90748787, 57 | -0.4199247, 58 | 0.01135842, 59 | 1.06843516, 60 | 0.42005013, 61 | 0.90740832, 62 | -0.0129626, 63 | 0.31985667, 64 | -0.00486341, 65 | 0.01653451, 66 | 0.99985147, 67 | -0.48456108, 68 | 0., 69 | 0., 70 | 0., 71 | 1., 72 | ] 73 | -------------------------------------------------------------------------------- /src/fgsp/common/signal_synchronizer.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | 3 | import numpy as np 4 | 5 | from src.fgsp.common.utils import Utils 6 | from src.fgsp.common.logger import Logger 7 | 8 | 9 | class SignalSynchronizer(object): 10 | 11 | def __init__(self, config): 12 | self.config = config 13 | 14 | def synchronize(self, optimized, estimated): 15 | ts_opt = self.extract_timestamps(optimized) 16 | ts_est = self.extract_timestamps(estimated) 17 | 18 | np.set_printoptions(suppress=True, 19 | formatter={'float_kind': '{:f}'.format}) 20 | 21 | opt_size = ts_opt.shape[0] 22 | est_size = ts_est.shape[0] 23 | min_size = min(opt_size, est_size) 24 | est_idx = [] 25 | opt_idx = [] 26 | 27 | if min_size != opt_size: 28 | Logger.LogError( 29 | f'SignalSynchronizer: min size is {min_size} and opt is {opt_size}.') 30 | 31 | for i in range(0, min_size): 32 | cur_ts = ts_opt[i, 0] 33 | 34 | # TODO(lbern): Check for a max difference. 35 | ts_diff = np.absolute(ts_est - cur_ts) 36 | ts_min = np.amin(ts_diff) 37 | diff_s = Utils.ts_ns_to_seconds(ts_min) 38 | if diff_s > self.config.synchronization_max_diff_s: 39 | Logger.LogWarn( 40 | f'SignalSynchronizer: closest TS is {diff_s} seconds away.') 41 | continue 42 | 43 | cur_min_index = np.where(ts_diff == ts_min)[0] 44 | est_idx.append(cur_min_index[0]) 45 | opt_idx.append(i) 46 | 47 | est_nodes = [estimated[i] for i in est_idx] 48 | opt_nodes = [optimized[i] for i in opt_idx] 49 | 50 | return (opt_nodes, est_nodes, opt_idx, est_idx) 51 | 52 | def extract_timestamps(self, signals): 53 | n_nodes = len(signals) 54 | ts = np.zeros((n_nodes, 1)) 55 | for i in range(0, n_nodes): 56 | ts[i] = Utils.ros_time_msg_to_ns(signals[i].ts) 57 | 58 | return ts 59 | -------------------------------------------------------------------------------- /src/fgsp/classifier/top_classifier.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | 3 | import numpy as np 4 | from src.fgsp.common.logger import Logger 5 | 6 | 7 | class TopClassifier(object): 8 | 9 | def __init__(self, config): 10 | self.top_n = config.top_classifier_select_n 11 | self.threshold = config.top_classifier_min_threshold 12 | 13 | def classify(self, data): 14 | n_nodes = data.shape[0] 15 | labels = [None] * n_nodes 16 | 17 | Logger.LogDebug(f'TopClassifier: data shape is {data.shape}') 18 | Logger.LogDebug('--- DATA ---------------------------------') 19 | Logger.LogDebug(data) 20 | Logger.LogDebug('------------------------------------------') 21 | 22 | top_n = min(self.top_n, n_nodes) 23 | 24 | # Find top n entries in the data. 25 | xy_indices = np.unravel_index(np.argsort( 26 | data.ravel())[-top_n:], data.shape) 27 | Logger.LogDebug('--- TOP N ---------------------------------') 28 | for i in range(0, top_n): 29 | row_idx = xy_indices[0][i] 30 | col_idx = xy_indices[1][i] 31 | 32 | if data[row_idx, col_idx] < self.threshold: 33 | Logger.LogDebug( 34 | f'skip {data[row_idx, col_idx]} < {self.threshold}') 35 | continue 36 | else: 37 | Logger.LogDebug(data[row_idx, col_idx]) 38 | 39 | if labels[row_idx] == None: 40 | labels[row_idx] = [] 41 | labels[row_idx].append(col_idx + 1) 42 | Logger.LogDebug('------------------------------------------') 43 | 44 | Logger.LogDebug('--- LABELS ---------------------------------') 45 | Logger.LogDebug(labels) 46 | Logger.LogDebug('------------------------------------------') 47 | 48 | return labels 49 | 50 | 51 | if __name__ == '__main__': 52 | classifier = TopClassifier(3) 53 | 54 | data = np.random.random((5, 3)) 55 | print('Classifying the following data:') 56 | print(data) 57 | print('\n') 58 | 59 | labels = classifier.classify(data) 60 | 61 | print('Got the following labels:') 62 | print(labels) 63 | -------------------------------------------------------------------------------- /src/fgsp/tools/path_transformer.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | 3 | import os 4 | import numpy as np 5 | 6 | import rclpy 7 | from rclpy.node import Node 8 | from evo.tools import file_interface 9 | 10 | 11 | from src.fgsp.common.logger import Logger 12 | 13 | 14 | class PathTransformer(Node): 15 | def __init__(self): 16 | super().__init__('path_transformer') 17 | Logger.Verbosity = 5 18 | Logger.LogInfo('PathTransformer: Initializing...') 19 | 20 | input_path = self.get_param('input_path', '') 21 | export_path = self.get_param( 22 | 'export_path', '/tmp/transformed.npy') 23 | T = np.array(self.get_param( 24 | 'T', np.eye(4, 4).reshape(16).tolist())).reshape(4, 4) 25 | 26 | Logger.LogInfo(f'PathTransformer: Reading poses from {input_path}') 27 | Logger.LogInfo(f'PathTransformer: Processing poses...') 28 | transformed_path = self.transform_path(input_path, T) 29 | np.save(export_path, transformed_path) 30 | Logger.LogInfo(f'PathTransformer: Saved cloud to {export_path}') 31 | 32 | def transform_path(self, input_path, T): 33 | path = self.read_csv_file(input_path) 34 | 35 | # Transform path 36 | ones = np.ones_like(path[:, 0]) 37 | path = np.column_stack((path, ones)) 38 | path = np.column_stack((path[:, 0], np.matmul(T, path[:, 1:5].T).T)) 39 | 40 | return path[:, 0:4] 41 | 42 | def get_param(self, key, default): 43 | self.declare_parameter(key, default) 44 | return self.get_parameter(key).value 45 | 46 | def read_csv_file(self, filename): 47 | Logger.LogDebug(f'PathTransformer: Reading file {filename}.') 48 | if os.path.exists(filename): 49 | robot_traj = file_interface.read_tum_trajectory_file(filename) 50 | return np.column_stack((robot_traj.timestamps, robot_traj.positions_xyz)) 51 | else: 52 | Logger.LogError(f'PathTransformer: File does not exist!') 53 | return np.array([]) 54 | 55 | 56 | def main(args=None): 57 | rclpy.init(args=args) 58 | transformer = PathTransformer() 59 | rclpy.spin(transformer) 60 | transformer.destroy_node() 61 | rclpy.shutdown() 62 | 63 | 64 | if __name__ == '__main__': 65 | main() 66 | -------------------------------------------------------------------------------- /src/fgsp/common/transform_history.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | from enum import Enum 3 | 4 | import numpy as np 5 | from src.fgsp.common.logger import Logger 6 | 7 | 8 | class ConstraintType(Enum): 9 | SMALL = 1 10 | MID = 2 11 | LARGE = 3 12 | 13 | 14 | class TransformHistory(object): 15 | def __init__(self): 16 | self.children = [] 17 | self.transforms = [] 18 | self.largest_diff = 0.1 19 | self.types = [] 20 | 21 | def size(self): 22 | n_children = len(self.children) 23 | assert n_children == len(self.transforms) 24 | assert n_children == len(self.types) 25 | return n_children 26 | 27 | def add_record(self, child, T, constraint_type): 28 | if type(constraint_type) is not ConstraintType: 29 | Logger.LogError( 30 | 'TransformHistory: Constraint type is not of type ConstraintType.') 31 | return 32 | if self.has_child(child): 33 | self.remove_child(child) 34 | 35 | self.children.append(child) 36 | self.transforms.append(T) 37 | self.types.append(constraint_type.value) 38 | 39 | def has_child(self, child): 40 | return child in self.children 41 | 42 | def remove_child(self, child): 43 | res = [x for x, z in enumerate(self.children) if z == child] 44 | if len(res) == 0: 45 | Logger.LogWarn( 46 | 'TransformHistory: Index retrieval failed for removal.') 47 | return 48 | res = res[0] 49 | self.children.pop(res) 50 | self.transforms.pop(res) 51 | self.types.pop(res) 52 | 53 | def has_different_transform(self, child, T): 54 | if not self.has_child(child): 55 | return True 56 | 57 | res = [x for x, z in enumerate(self.children) if z == child] 58 | n_res = len(res) 59 | if n_res == 0: 60 | Logger.LogWarn( 61 | '[TransformHistory] Index retrieval failed for comparison.') 62 | return False 63 | 64 | if len(res) > 1: 65 | Logger.LogWarn( 66 | '[TransformHistory] Found multiple children for parent.') 67 | res = res[0] 68 | T_prev = self.transforms[res] 69 | T_diff = np.abs(T_prev - T) 70 | largest_diff = np.amax(T_diff) 71 | return largest_diff > self.largest_diff 72 | -------------------------------------------------------------------------------- /src/fgsp/tools/cloud_saver.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | 3 | import numpy as np 4 | 5 | import rclpy 6 | from rclpy.node import Node 7 | from sensor_msgs.msg import PointCloud2 8 | from sensor_msgs_py import point_cloud2 9 | 10 | 11 | from src.fgsp.common.logger import Logger 12 | 13 | 14 | class CloudSaver(Node): 15 | def __init__(self): 16 | super().__init__('cloud_saver') 17 | Logger.Verbosity = 5 18 | Logger.LogInfo('CloudSaver: Initializing...') 19 | 20 | self.cloud_topic = self.get_param('cloud_topic', '/point_cloud') 21 | self.export_path = self.get_param('export_path', '/tmp/cloud.npy') 22 | self.should_store_sequentially = self.get_param('sequential', False) 23 | if self.should_store_sequentially: 24 | self.n_clouds = 0 25 | self.export_path = self.export_path.replace( 26 | '.npy', '_{n_clouds}.npy') 27 | 28 | self.field_names = ['x', 'y', 'z'] 29 | if self.get_param('extract_intensity', False): 30 | self.field_names.append("intensity") 31 | 32 | self.cloud_sub = self.create_subscription( 33 | PointCloud2, self.cloud_topic, self.cloud_callback, 10) 34 | 35 | Logger.LogInfo(f'CloudSaver: Subscribed to {self.cloud_topic}') 36 | Logger.LogInfo(f'CloudSaver: Writing cloud to {self.export_path}') 37 | Logger.LogInfo('CloudSaver: Initializing done. Waiting for clouds...') 38 | 39 | def get_param(self, key, default): 40 | self.declare_parameter(key, default) 41 | return self.get_parameter(key).value 42 | 43 | def cloud_callback(self, msg): 44 | cloud = point_cloud2.read_points_numpy( 45 | msg, field_names=self.field_names, skip_nans=True, reshape_organized_cloud=False) 46 | n_dims = len(self.field_names) 47 | cloud = np.reshape(cloud, (-1, n_dims)) 48 | 49 | export_path = self.export_path 50 | if self.should_store_sequentially: 51 | export_path = self.export_path.format(n_clouds=self.n_clouds) 52 | self.n_clouds += 1 53 | np.save(export_path, cloud) 54 | Logger.LogInfo(f'CloudSaver: Saved cloud to {export_path}') 55 | 56 | 57 | def main(args=None): 58 | rclpy.init(args=args) 59 | server = CloudSaver() 60 | rclpy.spin(server) 61 | server.destroy_node() 62 | rclpy.shutdown() 63 | 64 | 65 | if __name__ == '__main__': 66 | main() 67 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | pip-wheel-metadata/ 24 | share/python-wheels/ 25 | *.egg-info/ 26 | .installed.cfg 27 | *.egg 28 | MANIFEST 29 | 30 | # PyInstaller 31 | # Usually these files are written by a python script from a template 32 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 33 | *.manifest 34 | *.spec 35 | 36 | # Installer logs 37 | pip-log.txt 38 | pip-delete-this-directory.txt 39 | 40 | # Unit test / coverage reports 41 | htmlcov/ 42 | .tox/ 43 | .nox/ 44 | .coverage 45 | .coverage.* 46 | .cache 47 | nosetests.xml 48 | coverage.xml 49 | *.cover 50 | *.py,cover 51 | .hypothesis/ 52 | .pytest_cache/ 53 | 54 | # Translations 55 | *.mo 56 | *.pot 57 | 58 | # Django stuff: 59 | *.log 60 | local_settings.py 61 | db.sqlite3 62 | db.sqlite3-journal 63 | 64 | # Flask stuff: 65 | instance/ 66 | .webassets-cache 67 | 68 | # Scrapy stuff: 69 | .scrapy 70 | 71 | # Sphinx documentation 72 | docs/_build/ 73 | 74 | # PyBuilder 75 | target/ 76 | 77 | # Jupyter Notebook 78 | .ipynb_checkpoints 79 | 80 | # IPython 81 | profile_default/ 82 | ipython_config.py 83 | 84 | # pyenv 85 | .python-version 86 | 87 | # pipenv 88 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 89 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 90 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 91 | # install all needed dependencies. 92 | #Pipfile.lock 93 | 94 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow 95 | __pypackages__/ 96 | 97 | # Celery stuff 98 | celerybeat-schedule 99 | celerybeat.pid 100 | 101 | # SageMath parsed files 102 | *.sage.py 103 | 104 | # Environments 105 | .env 106 | .venv 107 | env/ 108 | venv/ 109 | ENV/ 110 | env.bak/ 111 | venv.bak/ 112 | 113 | # Spyder project settings 114 | .spyderproject 115 | .spyproject 116 | 117 | # Rope project settings 118 | .ropeproject 119 | 120 | # mkdocs documentation 121 | /site 122 | 123 | # mypy 124 | .mypy_cache/ 125 | .dmypy.json 126 | dmypy.json 127 | 128 | # Pyre type checker 129 | .pyre/ 130 | 131 | # Tmp data 132 | *.npy 133 | *.dat 134 | 135 | # ROS2 136 | install/ 137 | log/ 138 | 139 | # VS Code 140 | .vscode/ 141 | -------------------------------------------------------------------------------- /src/fgsp/classifier/windowed_result.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | 3 | import numpy as np 4 | from src.fgsp.controller.signal_handler import SignalHandler 5 | from src.fgsp.graph.wavelet_evaluator import WaveletEvaluator 6 | from src.fgsp.classifier import ClassificationResult 7 | 8 | 9 | class WindowedResult(ClassificationResult): 10 | def __init__(self, config, robot_name, opt_nodes, est_nodes, features, labels, graph): 11 | super().__init__(config, robot_name, opt_nodes, features, labels) 12 | assert config.use_graph_hierarchies 13 | assert not config.use_downstreaming 14 | 15 | self.indices = graph.get_indices() 16 | self.graph = graph 17 | self.est_nodes = est_nodes 18 | n_nodes = len(self.opt_nodes) 19 | self.labels = self.create_windowed_labels(n_nodes, labels) 20 | 21 | def create_windowed_labels(self, n_nodes, labels): 22 | # Labels in the current hierarchy 23 | n_labels = len(labels) 24 | last_label = n_labels - 1 25 | 26 | signal = SignalHandler(self.config) 27 | x_est = signal.compute_signal(self.est_nodes) 28 | x_opt = signal.compute_signal(self.opt_nodes) 29 | 30 | downstream_labels = [None] * n_nodes 31 | downstream_labels = [lbl if lbl is not None else [] 32 | for lbl in downstream_labels] 33 | if self.n_labels == 0: 34 | return downstream_labels 35 | 36 | for idx in range(0, last_label): 37 | if len(labels[idx]) == 0: 38 | continue 39 | 40 | # Compute the windowed wavelets in the initial graph. 41 | node_range = np.arange( 42 | self.indices[idx], self.indices[idx+1], dtype=int) 43 | features = self.evaluate_node_range(node_range, x_est, x_opt) 44 | 45 | for lbl in labels[idx]: 46 | max_n = node_range[np.argmax(features[node_range, lbl-1])] 47 | downstream_labels[max_n].append(lbl) 48 | 49 | # Fix remainder 50 | node_range = np.arange(self.indices[last_label], n_nodes, dtype=int) 51 | features = self.evaluate_node_range(node_range, x_est, x_opt) 52 | for lbl in labels[last_label]: 53 | max_n = node_range[np.argmax(features[node_range, lbl-1])] 54 | downstream_labels[max_n].append(lbl) 55 | 56 | return downstream_labels 57 | 58 | def evaluate_node_range(self, node_range, x_est, x_opt): 59 | # Compute the windowed wavelets in the initial graph. 60 | eval = WaveletEvaluator() 61 | eval.compute_wavelets(self.graph.get_graph(0), node_range) 62 | 63 | # Compute the windowed features for the windowed wavelets. 64 | W_est = eval.compute_wavelet_coeffs(x_est) 65 | W_opt = eval.compute_wavelet_coeffs(x_opt) 66 | return eval.compute_features(W_opt, W_est) 67 | -------------------------------------------------------------------------------- /config/local_client_config.yaml: -------------------------------------------------------------------------------- 1 | graph_client: 2 | ros__parameters: 3 | # Subscribers. 4 | opt_graph_topic: /graph_monitor/sparse_graph/graph 5 | opt_traj_topic: /graph_monitor/sparse_graph/trajectory 6 | est_traj_topic: /trajectory 7 | est_traj_path_topic: /optimized_path 8 | opt_submap_constraint_topic: /graph_monitor/submaps 9 | 10 | # Publishers. 11 | anchor_node_topic: "/graph_client/anchor_nodes" 12 | relative_node_topic: "/graph_client/relative_nodes" 13 | intra_constraints: "/graph_client/relative_nodes" 14 | verification_service: "/graph_monitor/verification" 15 | 16 | # Subscriber and Publisher 17 | client_update_topic: "/graph_client/latest_graph" 18 | 19 | # Constraint construction: multiscale, euclidean, always, absolute 20 | client_mode: "multiscale" 21 | wavelet_scales: 6 22 | classifier: "top" # "top" or "simple" 23 | top_classifier_select_n: 10 24 | top_classifier_min_threshold: 0.07 25 | large_scale_partition_method: "id" # 'id' or 'nth' 26 | large_scale_anchor: False 27 | n_hop_mid_constraints: 5 28 | min_dist_large_constraints: 4.0 29 | min_dist_along_graph_large_constraints: 30.0 30 | max_lookup_dist_large_constraints: 75.0 31 | nn_neighbors: 5 32 | stop_method: "" # "", "dirichlet", "tv", "alv" 33 | stop_threshold: 0.1 34 | 35 | # Graph configuration. 36 | construction_method: "se3" # "se3" or "so3" or "r3" 37 | use_graph_hierarchies: False 38 | max_graph_levels: 2 # 2 means 1 level of hierarchy 39 | use_downstreaming: False 40 | graph_hierarchies_node_threshold: 1 41 | use_parallel_construction: False 42 | visualize_graph: False 43 | 44 | # Div 45 | dataroot: "/home/berlukas/Documents/workspace/ros2/fgsp_ws/src/fgsp" 46 | robot_name: "cerberus" # cerberus, euroc_X, anymal_X 47 | update_rate: 0.05 48 | enable_client_update: False 49 | enable_anchor_constraints: False 50 | enable_relative_constraints: True 51 | enable_submap_constraints: True 52 | enable_signal_recording: True 53 | enable_trajectory_recording: True 54 | signal_export_path: "/data/{src}_signal.npy" 55 | graph_coords_export_path: "/data/{src}_graph_coords.npy" 56 | graph_adj_export_path: "/data/{src}_graph_adj.npy" 57 | trajectory_export_path: "/data/{src}_trajectory.npy" 58 | trajectory_raw_export_path: "/data/{src}_raw_trajectory.npy" 59 | label_output_path: "/data/opt_labels.dat" 60 | degenerate_window: 20 61 | synchronization_max_diff_s: 0.5 62 | verbosity: 7 63 | warmup_nodes: 20 64 | max_iterations: 50 65 | 66 | T_robot_server: 67 | [ 68 | 0.03547377, 69 | 0.99931998, 70 | 0.01005935, 71 | 0.17368547, 72 | -0.9993442, 73 | 0.03539785, 74 | 0.00762637, 75 | -2.14932286, 76 | 0.00726511, 77 | -0.0103233, 78 | 0.99992032, 79 | -0.61567595, 80 | 0., 81 | 0., 82 | 0., 83 | 1., 84 | ] 85 | -------------------------------------------------------------------------------- /src/fgsp/tools/object_publisher.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | 3 | import numpy as np 4 | import os 5 | import yaml 6 | 7 | import rclpy 8 | from rclpy.node import Node 9 | from sensor_msgs.msg import PointCloud2 10 | from sensor_msgs_py import point_cloud2 11 | from evo.tools import file_interface 12 | from evo.core import sync 13 | from evo.core import metrics 14 | from evo.tools import plot 15 | import matplotlib.pyplot as plt 16 | from visualization_msgs.msg import Marker, MarkerArray 17 | from std_msgs.msg import ColorRGBA 18 | 19 | 20 | from src.fgsp.common.logger import Logger 21 | from src.fgsp.common.utils import Utils 22 | 23 | 24 | class ObjectPublisher(Node): 25 | def __init__(self): 26 | super().__init__('object_publisher') 27 | Logger.Verbosity = 5 28 | Logger.LogInfo('ObjectPublisher: Initializing...') 29 | 30 | self.color = self.get_param('marker_color', [0.8, 0.8, 0.1]) 31 | object_file = self.get_param('object_file', '') 32 | with open(object_file, 'r') as f: 33 | configuration = yaml.safe_load(f) 34 | self.artifact_pub = self.create_publisher(MarkerArray, 'artifacts', 10) 35 | 36 | self.publish_artifacts(configuration["objects"]) 37 | 38 | Logger.LogInfo( 39 | 'ObjectPublisher: Initializing done. Publishing objects...') 40 | 41 | def publish_artifacts(self, artifacts): 42 | markers = self.create_artifacts(artifacts) 43 | self.artifact_pub.publish(markers) 44 | Logger.LogInfo(f'Published {len(markers.markers)} artifacts') 45 | 46 | def create_artifacts(self, artifacts): 47 | if artifacts is None and not isinstance(artifacts, list): 48 | return 49 | ts_now = self.get_clock().now() 50 | index = 0 51 | 52 | marker_color = ColorRGBA() 53 | marker_color.r = self.color[0] 54 | marker_color.g = self.color[1] 55 | marker_color.b = self.color[2] 56 | marker_color.a = 1.0 57 | 58 | artifact_markers = MarkerArray() 59 | for art in artifacts: 60 | cube = self.create_artifact(art, ts_now, index, marker_color) 61 | artifact_markers.markers.append(cube) 62 | index += 1 63 | return artifact_markers 64 | 65 | def create_artifact(self, artifact, ts_now, index, color): 66 | (x, y, z) = artifact['position'] 67 | 68 | cube = Marker() 69 | cube.header.frame_id = 'map' 70 | cube.header.stamp = ts_now.to_msg() 71 | 72 | cube.id = index 73 | cube.action = Marker.ADD 74 | cube.pose.position.x = x 75 | cube.pose.position.y = y 76 | cube.pose.position.z = z 77 | cube.pose.orientation.w = 1.0 78 | 79 | cube.color = color 80 | cube.scale.x = cube.scale.y = cube.scale.z = 0.2 81 | 82 | cube.type = Marker.CUBE 83 | cube.frame_locked = True 84 | 85 | return cube 86 | 87 | def get_param(self, key, default): 88 | self.declare_parameter(key, default) 89 | return self.get_parameter(key).value 90 | 91 | 92 | def main(args=None): 93 | rclpy.init(args=args) 94 | pub = ObjectPublisher() 95 | rclpy.spin(pub) 96 | pub.destroy_node() 97 | rclpy.shutdown() 98 | 99 | 100 | if __name__ == '__main__': 101 | main() 102 | -------------------------------------------------------------------------------- /src/fgsp/common/utils.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | 3 | import rclpy 4 | import numpy as np 5 | from scipy.spatial.transform import Rotation 6 | from sensor_msgs_py import point_cloud2 as pc2 7 | 8 | 9 | class Utils(object): 10 | 11 | @staticmethod 12 | def ros_time_to_ns(time): 13 | return time.nanoseconds 14 | 15 | @staticmethod 16 | def ros_time_msg_to_ns(time): 17 | k_s_to_ns = 1e9 18 | return time.sec * k_s_to_ns + time.nanosec 19 | 20 | @staticmethod 21 | def ros_time_msg_to_s(time): 22 | k_s_to_ns = 1e9 23 | return Utils.ros_time_msg_to_ns(time) / k_s_to_ns 24 | 25 | @staticmethod 26 | def ts_ns_to_ros_time(ts_ns): 27 | k_ns_per_s = 1e9 28 | ros_timestamp_sec = ts_ns / k_ns_per_s 29 | ros_timestamp_nsec = ts_ns - (ros_timestamp_sec * k_ns_per_s) 30 | return rclpy.time.Time(seconds=ros_timestamp_sec, 31 | nanoseconds=ros_timestamp_nsec) 32 | 33 | @staticmethod 34 | def ts_ns_to_seconds(ts_ns): 35 | k_ns_per_s = 1e9 36 | return ts_ns / k_ns_per_s 37 | 38 | @staticmethod 39 | def convert_pointcloud2_msg_to_array(cloud_msg): 40 | points_list = [] 41 | for data in pc2.read_points(cloud_msg, skip_nans=True): 42 | points_list.append([data[0], data[1], data[2], data[3]]) 43 | return np.array(points_list) 44 | 45 | @staticmethod 46 | def convert_pose_stamped_msg_to_array(pose_msg): 47 | position = np.array( 48 | [pose_msg.pose.position.x, pose_msg.pose.position.y, pose_msg.pose.position.z]) 49 | orientation = np.array([pose_msg.pose.orientation.w, pose_msg.pose.orientation.x, 50 | pose_msg.pose.orientation.y, pose_msg.pose.orientation.z]) 51 | return position, orientation 52 | 53 | @staticmethod 54 | def convert_quat_to_rotation(quat): 55 | R = Rotation.from_quat(np.array([quat[1], quat[2], quat[3], quat[0]])) 56 | return R.as_matrix() 57 | 58 | @staticmethod 59 | def convert_pos_quat_to_transformation(pos, quat): 60 | # takes xyzw as input 61 | R = Rotation.from_quat(np.array([quat[1], quat[2], quat[3], quat[0]])) 62 | T = np.empty((4, 4)) 63 | T[0:3, 0:3] = R.as_matrix() 64 | T[0:3, 3] = pos 65 | T[3, :] = [0, 0, 0, 1] 66 | 67 | return T 68 | 69 | @staticmethod 70 | def convert_pointcloud2_msg_to_array(cloud_msg): 71 | points_list = [] 72 | for data in pc2.read_points(cloud_msg, skip_nans=True): 73 | points_list.append([data[0], data[1], data[2]]) 74 | return np.array(points_list) 75 | 76 | @staticmethod 77 | def transform_pointcloud(cloud, T): 78 | return (np.dot(cloud, T[0:3, 0:3].T) + T[0:3, 3].T) 79 | 80 | @staticmethod 81 | def downsample_pointcloud(cloud, voxel_size=0.15): 82 | # TODO: not yet implemented 83 | return cloud 84 | 85 | @staticmethod 86 | def fix_nn_output(n_neighbors, idx, nn_dists, nn_indices): 87 | self_idx = np.where(nn_indices == idx)[0][0] 88 | nn_dists = np.delete(nn_dists, [self_idx]) 89 | nn_indices = np.delete(nn_indices, [self_idx]) 90 | 91 | nn_indices = [nn_indices] if n_neighbors == 1 else nn_indices 92 | nn_dists = [nn_dists] if n_neighbors == 1 else nn_dists 93 | 94 | mask = np.isnan(nn_indices.astype(float)) 95 | mask = mask | np.isnan(nn_dists.astype(float)) 96 | mask = mask | np.isinf(nn_indices.astype(float)) 97 | mask = mask | np.isinf(nn_dists.astype(float)) 98 | mask = np.logical_not(mask) 99 | return nn_dists[mask], nn_indices[mask] 100 | -------------------------------------------------------------------------------- /src/fgsp/common/robot_constraints.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | 3 | import numpy as np 4 | from nav_msgs.msg import Path 5 | from geometry_msgs.msg import PoseStamped 6 | 7 | from src.fgsp.common.lc_model import LcModel 8 | from src.fgsp.common.utils import Utils 9 | from src.fgsp.common.logger import Logger 10 | 11 | 12 | class RobotConstraints(object): 13 | def __init__(self): 14 | self.submap_constraints = {} 15 | self.previous_timestamps = [] 16 | 17 | def add_submap_constraints(self, ts_from, ts_to, T_a_b): 18 | lc = LcModel(ts_from, ts_to, T_a_b) 19 | ts_from_ns = Utils.ros_time_msg_to_ns(ts_from) 20 | ts_to_ns = Utils.ros_time_msg_to_ns(ts_to) 21 | if ts_from_ns == ts_to_ns: 22 | print('Timestamp from and to are identical.') 23 | return 24 | 25 | if ts_from_ns not in self.submap_constraints: 26 | self.submap_constraints[ts_from_ns] = [] 27 | for i in range(len(self.submap_constraints[ts_from_ns])): 28 | if self.submap_constraints[ts_from_ns][i].ts_to == ts_to: 29 | self.submap_constraints[ts_from_ns][i] = lc 30 | return 31 | 32 | self.submap_constraints[ts_from_ns].append(lc) 33 | 34 | def construct_path_msgs(self): 35 | path_msgs = [] 36 | Logger.LogInfo( 37 | f'RobotConstraints: Constructing path message for {len(self.submap_constraints)} different submaps.') 38 | for ts_ns_from in self.submap_constraints: 39 | loop_closures = list(self.submap_constraints[ts_ns_from]) 40 | path_msg = self.construct_path_msg_for_submap( 41 | ts_ns_from, loop_closures) 42 | path_msgs.append(path_msg) 43 | return path_msgs 44 | 45 | def construct_path_msgs_using_ts(self, timestamps): 46 | path_msgs = [] 47 | Logger.LogInfo( 48 | f'RobotConstraints: Constructing path message for {len(self.submap_constraints)} different submaps.') 49 | for ts_from_ns in self.submap_constraints: 50 | if not self.should_publish_map(ts_from_ns, timestamps): 51 | continue 52 | if ts_from_ns not in self.previous_timestamps: 53 | self.previous_timestamps.append(ts_from_ns) 54 | 55 | Logger.LogWarn('RobotConstraints: Found a valid timestamp!!!') 56 | loop_closures = list(self.submap_constraints[ts_from_ns]) 57 | path_msg = self.construct_path_msg_for_submap( 58 | ts_from_ns, loop_closures) 59 | path_msgs.append(path_msg) 60 | return path_msgs 61 | 62 | def should_publish_map(self, ts_from_ns, timestamps): 63 | if ts_from_ns in self.previous_timestamps: 64 | return True 65 | 66 | ts_diff = np.absolute(timestamps - ts_from_ns) 67 | ts_min = np.amin(ts_diff) 68 | diff_s = Utils.ts_ns_to_seconds(ts_min) 69 | Logger.LogWarn(f'[RobotConstraints] min diff ts is {diff_s}') 70 | 71 | return diff_s < 3 72 | 73 | def construct_path_msg_for_submap(self, ts_ns_from, loop_closures): 74 | path_msg = Path() 75 | path_msg.header.stamp = Utils.ts_ns_to_ros_time(ts_ns_from).to_msg() 76 | 77 | for lc in loop_closures: 78 | t_a_b = lc.get_translation() 79 | q_a_b = lc.get_rotation_quat() 80 | 81 | pose_msg = PoseStamped() 82 | pose_msg.header.stamp = lc.ts_to.to_msg() 83 | pose_msg.pose.position.x = t_a_b[0] 84 | pose_msg.pose.position.y = t_a_b[1] 85 | pose_msg.pose.position.z = t_a_b[2] 86 | pose_msg.pose.orientation.x = q_a_b[0] 87 | pose_msg.pose.orientation.y = q_a_b[1] 88 | pose_msg.pose.orientation.z = q_a_b[2] 89 | pose_msg.pose.orientation.w = q_a_b[3] 90 | 91 | path_msg.poses.append(pose_msg) 92 | 93 | return path_msg 94 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Collaborative Robot Mapping using Spectral Graph Analysis 2 | 3 | This work entitled FGSP (short for Factor Graph Signal Processing) deals with the problem of creating globally consistent pose graphs in a centralized multi-robot SLAM framework. 4 | 5 | ## Overview 6 | 7 | FGSP mainly relies on two separate components for synchronization, the `graph_monitor` (server) and the `graph_client` (robots). 8 | The `graph_monitor` reads the optimized server graph published by a separate system. 9 | This is generally agnostic to the mapping server as long as the specific messages 10 | are used. 11 | The message definitions can be obtained from [maplab_msgs](https://github.com/ethz-asl/maplab_msgs) and an example server is the `maplab_server` which can be found [here](https://github.com/ethz-asl/maplab/tree/develop). 12 | 13 | The `graph_client` finds the discrepancies and publishes the required constraints to obtain consistent pose graphs on server and robot. 14 | To do so, it listens to the graph of published by the `graph_monitor` as well as the onboard state estimation. 15 | Similar to the `graph_monitor`, this is also agnostic to the used framework as long as the state estimation is published on the correct topics. 16 | 17 | ## Getting started 18 | 19 | There are mainly two options to build FGSP: 20 | * Regular `colcon` build 21 | * Deployment build with `docker` 22 | 23 | 24 | For the former, the following dependencies need to be installed on the host machine: 25 | * [liegroups](https://github.com/utiasSTARS/liegroups) 26 | * [maplab_msgs](https://github.com/ethz-asl/maplab_msgs) 27 | * [pygsp](https://github.com/epfl-lts2/pygsp) 28 | 29 | A submodule of each dependency can be found in the in the `dependencies/` directory. 30 | Other dependencies are `numpy`, `pandas`, `scipy` and need to be installed with, e.g., `pip`. 31 | 32 | ### Colcon Build 33 | Having all dependencies installed, FGSP can be built with 34 | ``` 35 | colcon build 36 | ``` 37 | 38 | The server then runs `graph_monitor.py` (see `launch/graph_monitor.launch` for an example) while the clients runs `graph_client.py` (see `launch/robots/local_client.launch` for an example). 39 | For actual robot deployments, the client configuration needs to be adapted per robot. 40 | 41 | ### Docker Build 42 | 43 | Ensure that you have `docker` installed 44 | ``` 45 | sudo apt install docker.io docker 46 | 47 | ``` 48 | 49 | Run the deployment script that builds the image and makes it available to the local Docker daemon: 50 | ``` 51 | ./deploy/build_docker.sh 52 | ``` 53 | 54 | Run files can be found in the `script/` folder. 55 | For example, for running the graph monitor on the server side: 56 | ``` 57 | ./script/run_graph_monitor 58 | ``` 59 | 60 | ## Reference 61 | 62 | Our paper is available at 63 | 64 | *Bernreiter, Lukas, Shehryar Khattak, Lionel Ott, Roland Siegwart, Marco Hutter, and Cesar Cadena. "Collaborative Robot Mapping using Spectral Graph Analysis." In The International Journal of Robotics Research 65 | , 02783649241246847.* [[Link](https://journals.sagepub.com/doi/full/10.1177/02783649241246847)] 66 | 67 | BibTex: 68 | ``` 69 | @article{bernreiter2024framework, 70 | title={A framework for collaborative multi-robot mapping using spectral graph wavelets}, 71 | author={Bernreiter, Lukas and Khattak, Shehryar and Ott, Lionel and Siegwart, Roland and Hutter, Marco and Cadena, Cesar}, 72 | journal={The International Journal of Robotics Research}, 73 | pages={02783649241246847}, 74 | year={2024}, 75 | publisher={SAGE Publications Sage UK: London, England} 76 | } 77 | ``` 78 | 79 | and 80 | 81 | *Bernreiter, Lukas, Shehryar Khattak, Lionel Ott, Roland Siegwart, Marco Hutter, and Cesar Cadena. "Collaborative Robot Mapping using Spectral Graph Analysis." In 2022 IEEE International Conference on Robotics and Automation (ICRA), IEEE, 2022.* [[Link](https://ieeexplore.ieee.org/document/9812102)] [[ArXiv](https://arxiv.org/abs/2203.00308)] 82 | 83 | 84 | BibTex: 85 | ``` 86 | @inproceedings{bernreiter2022collaborative, 87 | author={Bernreiter, Lukas and Khattak, Shehryar and Ott, Lionel and Siegwart, Roland and Hutter, Marco and Cadena, Cesar}, 88 | booktitle={2022 International Conference on Robotics and Automation (ICRA)}, 89 | title={Collaborative Robot Mapping using Spectral Graph Analysis}, 90 | year={2022}, 91 | pages={3662-3668}, 92 | doi={10.1109/ICRA46639.2022.9812102} 93 | } 94 | ``` 95 | -------------------------------------------------------------------------------- /src/fgsp/common/visualizer.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | 3 | import copy 4 | 5 | from std_msgs.msg import Header, ColorRGBA 6 | from visualization_msgs.msg import Marker, MarkerArray 7 | from geometry_msgs.msg import Point 8 | 9 | from src.fgsp.common.comms import Comms 10 | 11 | 12 | class Visualizer(object): 13 | def __init__(self, config=None): 14 | self.config = config 15 | self.line_marker = self.create_line_marker() 16 | self.sphere_graph_marker = self.create_sphere_marker() 17 | self.signal_graph_marker = self.create_sphere_marker() 18 | self.resetConstraintVisualization() 19 | 20 | self.comms = Comms() 21 | self.robot_colors = self.generate_robot_colors() 22 | self.resetConstraintVisualization() 23 | 24 | def create_line_marker(self, frame_id='map'): 25 | line_marker = Marker() 26 | line_marker.header.frame_id = frame_id 27 | line_marker.ns = "Line" 28 | line_marker.action = Marker().ADD 29 | line_marker.type = Marker().LINE_STRIP 30 | line_marker.scale.x = 0.3 31 | return line_marker 32 | 33 | def create_sphere_marker(self, frame_id='map'): 34 | sphere = Marker() 35 | sphere.header.frame_id = frame_id 36 | sphere.action = Marker.ADD 37 | sphere.pose.orientation.w = 1.0 38 | sphere.scale.x = sphere.scale.y = sphere.scale.z = 0.5 39 | sphere.color = self.get_color(0.9, 0.05, 0.05) 40 | sphere.type = Marker.SPHERE 41 | sphere.frame_locked = True 42 | return sphere 43 | 44 | def generate_robot_colors(self): 45 | robots = [] 46 | robots.append(self.get_color(0.8, 0.1, 0.1)) 47 | robots.append(self.get_color(0.4, 0.4, 0.2)) 48 | robots.append(self.get_color(0.1, 0.1, 0.8)) 49 | robots.append(self.get_color(0.2, 0.4, 0.4)) 50 | robots.append(self.get_color(0.4, 0.2, 0.4)) 51 | return robots 52 | 53 | def get_color(self, r, g, b): 54 | result = ColorRGBA() 55 | result.a = 1.0 56 | result.r = r 57 | result.g = g 58 | result.b = b 59 | return result 60 | 61 | def resetConstraintVisualization(self): 62 | self.line_marker.id = 0 63 | self.sphere_graph_marker.id = 0 64 | self.signal_graph_marker.id = 0 65 | self.spheres = MarkerArray() 66 | self.adjacency = MarkerArray() 67 | self.signals = MarkerArray() 68 | 69 | def create_sphere(self, sphere, point): 70 | sphere.header.stamp = self.time_now().to_msg() 71 | sphere.pose.position.x = float(point[0]) 72 | sphere.pose.position.y = float(point[1]) 73 | sphere.pose.position.z = float(point[2]) 74 | return sphere 75 | 76 | def add_graph_coordinate(self, point, color=[0.9, 0.05, 0.05]): 77 | self.sphere_graph_marker.id += 1 78 | sphere = copy.deepcopy(self.sphere_graph_marker) 79 | sphere = self.create_sphere(sphere, point) 80 | sphere.color = self.get_color(color[0], color[1], color[2]) 81 | self.spheres.markers.append(sphere) 82 | 83 | def add_signal_coordinate(self, point, robot_idx): 84 | self.signal_graph_marker.id += 1 85 | sphere = copy.deepcopy(self.signal_graph_marker) 86 | sphere = self.create_sphere(sphere, point) 87 | sphere.color = self.robot_colors[robot_idx] 88 | self.signals.markers.append(sphere) 89 | 90 | def add_graph_adjacency(self, point_a, point_b): 91 | points = [point_a, point_b] 92 | color = [0.9, 0.05, 0.05] 93 | line_marker = self.create_point_line_markers(points, color) 94 | 95 | self.adjacency.markers.append(line_marker) 96 | 97 | def create_point_line_markers(self, points, color=[0.9, 0.05, 0.05]): 98 | line_point_a = Point() 99 | line_point_a.x = float(points[0][0]) 100 | line_point_a.y = float(points[0][1]) 101 | line_point_a.z = float(points[0][2]) 102 | line_point_b = Point() 103 | line_point_b.x = float(points[1][0]) 104 | line_point_b.y = float(points[1][1]) 105 | line_point_b.z = float(points[1][2]) 106 | 107 | self.line_marker.id += 1 108 | line_marker = copy.deepcopy(self.line_marker) 109 | line_marker.header.stamp = self.time_now().to_msg() 110 | line_marker.color = self.get_color(color[0], color[1], color[2]) 111 | 112 | line_marker.points[:] = [] 113 | line_marker.points.append(line_point_a) 114 | line_marker.points.append(line_point_b) 115 | 116 | return line_marker 117 | 118 | def visualize_coords(self): 119 | self.comms.publish(self.spheres, MarkerArray, '/graph/coords') 120 | 121 | def visualize_adjacency(self): 122 | self.comms.publish(self.adjacency, MarkerArray, '/graph/adjacency') 123 | 124 | def visualize_signals(self, topic): 125 | self.comms.publish(self.signals, MarkerArray, topic) 126 | 127 | def time_now(self): 128 | return self.comms.node.get_clock().now() 129 | -------------------------------------------------------------------------------- /src/fgsp/graph_monitor.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | 3 | import copy 4 | import time 5 | from maplab_msgs.msg import Graph, Trajectory 6 | from multiprocessing import Lock 7 | 8 | import rclpy 9 | from rclpy.node import Node 10 | 11 | from src.fgsp.graph.global_graph import GlobalGraph 12 | from src.fgsp.controller.signal_handler import SignalHandler 13 | from src.fgsp.common.config import MonitorConfig 14 | from src.fgsp.common.plotter import Plotter 15 | from src.fgsp.common.logger import Logger 16 | 17 | 18 | class GraphMonitor(Node): 19 | def __init__(self): 20 | super().__init__('graph_monitor') 21 | self.is_initialized = False 22 | 23 | Plotter.PlotMonitorBanner() 24 | self.config = MonitorConfig(self) 25 | self.config.init_from_config() 26 | Plotter.PrintMonitorConfig(self.config) 27 | Plotter.PrintSeparator() 28 | 29 | self.mutex = Lock() 30 | self.mutex.acquire() 31 | # Publishers and subscribers. 32 | if self.config.enable_graph_building: 33 | self.graph_sub = self.create_subscription( 34 | Graph, self.config.in_graph_topic, self.graph_callback, 10) 35 | self.traj_sub = self.create_subscription( 36 | Trajectory, self.config.in_traj_opt_topic, self.traj_opt_callback, 10) 37 | self.graph_pub = self.create_publisher( 38 | Graph, self.config.out_graph_topic, 10) 39 | self.traj_pub = self.create_publisher( 40 | Trajectory, self.config.out_traj_opt_topic, 10) 41 | 42 | # Handlers and evaluators. 43 | self.graph = GlobalGraph( 44 | self.config, reduced=self.config.reduce_global_graph) 45 | self.optimized_signal = SignalHandler(self.config) 46 | 47 | # Key management to keep track of the received messages. 48 | self.optimized_keys = [] 49 | self.is_initialized = True 50 | self.latest_opt_traj_msg = None 51 | self.mutex.release() 52 | self.timer = self.create_timer(1 / self.config.rate, self.update) 53 | 54 | def graph_callback(self, msg): 55 | if self.is_initialized is False: 56 | return 57 | Logger.LogInfo('GraphMonitor: Received graph message from server.') 58 | self.mutex.acquire() 59 | 60 | # We only trigger the graph building if the msg contains new information. 61 | if self.graph.msg_contains_updates(msg) is True: 62 | self.graph.build(msg) 63 | 64 | self.mutex.release() 65 | 66 | def traj_opt_callback(self, msg): 67 | if self.is_initialized is False: 68 | return 69 | 70 | keys = self.optimized_signal.convert_signal(msg) 71 | 72 | # If the graph is reduced, we need to reduce the optimized nodes too. 73 | if self.graph.is_reduced: 74 | msg.nodes = [msg.nodes[i] for i in self.graph.reduced_ind] 75 | 76 | if self.graph.has_skipped(): 77 | msg.nodes = [element for i, element in enumerate( 78 | msg.nodes) if i not in self.graph.skip_ind] 79 | 80 | for key in keys: 81 | if self.key_in_optimized_keys(key): 82 | continue 83 | self.optimized_keys.append(key) 84 | self.latest_opt_traj_msg = msg 85 | 86 | def update(self): 87 | # Compute the global graph and signal, then publish it 88 | if self.config.enable_graph_building: 89 | self.compute_and_publish_graph() 90 | 91 | try: 92 | self.graph.publish() 93 | self.optimized_signal.publish() 94 | except Exception as e: 95 | Logger.LogError( 96 | 'GraphMonitor: Unable to publish results to client.') 97 | 98 | def compute_and_publish_graph(self): 99 | self.mutex.acquire() 100 | if self.graph.is_built is False: 101 | Logger.LogWarn('GraphMonitor: Graph is not built yet!') 102 | self.mutex.release() 103 | return 104 | Logger.LogInfo( 105 | f'GraphMonitor: Computing global graph with {self.graph.graph_size()}.') 106 | if self.graph.graph_size() < self.config.min_node_count: 107 | Logger.LogWarn( 108 | f'GraphMonitor: Not enough nodes ({self.graph.graph_size()} < {self.config.min_node_count}') 109 | self.mutex.release() 110 | return 111 | self.mutex.release() 112 | 113 | # Publish the graph to the clients. 114 | self.publish_graph_and_traj() 115 | 116 | def publish_graph_and_traj(self): 117 | graph_msg = self.graph.to_graph_msg() 118 | self.pub_graph.publish(graph_msg) 119 | Logger.LogInfo('GraphMonitor: Published global graph.') 120 | 121 | if self.config.send_separate_traj_msgs: 122 | self.send_separate_traj_msgs() 123 | elif self.latest_opt_traj_msg is not None: 124 | self.pub_traj.publish(self.latest_opt_traj_msg) 125 | Logger.LogInfo( 126 | f'GraphMonitor: Published trajectory for keys {self.optimized_keys}.') 127 | 128 | def send_separate_traj_msgs(self): 129 | for key in self.optimized_keys: 130 | traj_msg = self.optimized_signal.to_signal_msg(key) 131 | self.pub_traj.publish(traj_msg) 132 | time.sleep(0.10) 133 | Logger.LogInfo( 134 | f'GraphMonitor: Published separate trajectory for {key}.') 135 | 136 | def key_in_optimized_keys(self, key): 137 | return any(key in k for k in self.optimized_keys) 138 | 139 | 140 | def main(args=None): 141 | rclpy.init(args=args) 142 | monitor = GraphMonitor() 143 | rclpy.spin(monitor) 144 | monitor.destroy_node() 145 | rclpy.shutdown() 146 | 147 | 148 | if __name__ == '__main__': 149 | main() 150 | -------------------------------------------------------------------------------- /src/fgsp/graph/hierarchical_graph.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | 3 | import numpy as np 4 | from pygsp import graphs, reduction 5 | 6 | from src.fgsp.graph.base_graph import BaseGraph 7 | from src.fgsp.common.logger import Logger 8 | from src.fgsp.common.visualizer import Visualizer 9 | 10 | 11 | class HierarchicalGraph(BaseGraph): 12 | def __init__(self, config): 13 | BaseGraph.__init__(self, config) 14 | self.G = [None] 15 | self.adj = [None] 16 | self.coords = [None] 17 | self.indices = [None] 18 | self.idx = 0 19 | self.node_threshold = self.config.graph_hierarchies_node_threshold 20 | Logger.LogInfo( 21 | f'HierarchicalGraph: Initialized with a threshold of {self.node_threshold}.') 22 | 23 | def build(self, graph_msg): 24 | pass 25 | 26 | def build_graph(self): 27 | if len(self.adj[self.idx].tolist()) == 0: 28 | Logger.LogInfo( 29 | f'HierarchicalGraph: Path adjacency matrix is empty. Aborting graph building.') 30 | return False 31 | self.G[self.idx] = graphs.Graph(self.adj[self.idx]) 32 | 33 | n_nodes = self.G[self.idx].N 34 | 35 | if n_nodes != self.coords[self.idx].shape[0]: 36 | Logger.LogInfo( 37 | f'HierarchicalGraph: Path graph size is {n_nodes} but coords are {self.coords[self.idx].shape}') 38 | return False 39 | if n_nodes <= 1: 40 | Logger.LogError( 41 | f'HierarchicalGraph: Path graph vertex count is less than 2.') 42 | return False 43 | 44 | self.indices[self.idx] = np.arange(n_nodes) 45 | self.G[self.idx].set_coordinates(self.coords[self.idx][:, [0, 1]]) 46 | self.G[self.idx].compute_fourier_basis() 47 | self.is_built = True 48 | 49 | return True 50 | 51 | def build_from_poses(self, poses): 52 | self.idx = 0 53 | self.coords[self.idx] = poses 54 | self.adj[self.idx] = self.create_adjacency_from_poses( 55 | self.coords[self.idx]) 56 | self.build_graph() 57 | self.build_hierarchies() 58 | 59 | def build_hierarchies(self): 60 | while (self.idx+1) < self.config.max_graph_levels and self.build_hierarchy(): 61 | pass 62 | 63 | def build_hierarchy(self): 64 | if not self.is_built: 65 | return False 66 | current_n = self.G[self.idx].N 67 | if current_n <= self.node_threshold: 68 | return False 69 | self.coords.append(None) 70 | self.adj.append(None) 71 | self.G.append(None) 72 | self.indices.append(None) 73 | 74 | indices = self.reduce_every_other(self.coords[self.idx]) 75 | G_next = reduction.kron_reduction(self.G[self.idx], indices) 76 | 77 | self.idx = self.idx + 1 78 | self.indices[self.idx] = self.indices[self.idx - 1][indices] 79 | self.G[self.idx] = G_next 80 | self.adj[self.idx] = G_next.W.toarray() 81 | self.coords[self.idx] = self.coords[self.idx - 1][indices] 82 | 83 | return True 84 | 85 | def get_graph(self, idx=-1): 86 | if idx < 0: 87 | idx = self.idx 88 | return self.G[idx] 89 | 90 | def get_coords(self, idx=-1): 91 | if idx < 0: 92 | idx = self.idx 93 | return self.coords[idx] 94 | 95 | def get_indices(self, idx=-1): 96 | if idx < 0: 97 | idx = self.idx 98 | return self.indices[idx] 99 | 100 | def write_graph_to_disk(self, coords_file, adj_file): 101 | np.save(coords_file, self.coords[0]) 102 | np.save(adj_file, self.adj[0]) 103 | 104 | def publish(self): 105 | if not self.is_built: 106 | return 107 | 108 | Logger.LogInfo(f'Visualizing graph up to level: {self.idx}.') 109 | viz = Visualizer() 110 | for i in range(self.idx + 1): 111 | self.publish_graph_level( 112 | viz, self.coords[i], self.adj[i], self.G[i].N, i) 113 | viz.visualize_coords() 114 | viz.visualize_adjacency() 115 | 116 | def publish_graph_level(self, viz, coords, adj, n_nodes, level): 117 | if n_nodes > coords.shape[0] or n_nodes > adj.shape[0]: 118 | Logger.LogError( 119 | f'Size mismatch in global graph {n_nodes} vs. {coords.shape[0]} vs. {adj.shape[0]}.') 120 | return 121 | 122 | color = self.get_level_color(level) 123 | z = np.array([0, 0, 5 * level]) 124 | for i in range(0, n_nodes): 125 | pt_h_i = np.ones((4, 1), dtype=np.float32) 126 | pt_h_i[0:3, 0] = coords[i, 0:3] + z 127 | pt_i = np.dot(self.config.T_robot_server, pt_h_i) 128 | viz.add_graph_coordinate(pt_i, color) 129 | 130 | for j in range(0, n_nodes): 131 | pt_h_j = np.ones((4, 1), dtype=np.float32) 132 | pt_h_j[0:3, 0] = coords[j, 0:3] 133 | pt_j = np.dot(self.config.T_robot_server, pt_h_j) 134 | if i >= n_nodes or j >= coords.shape[0]: 135 | continue 136 | if i >= adj.shape[0] or j >= adj.shape[1]: 137 | continue 138 | if adj[i, j] <= 0.0: 139 | continue 140 | viz.add_graph_adjacency(pt_i, pt_j) 141 | Logger.LogDebug(f'HierarchicalGraph: Visualized graph level {level}.') 142 | 143 | def get_level_color(self, idx): 144 | max_idx = 6 145 | norm_idx = idx % max_idx 146 | if norm_idx == 0: 147 | return [0.95, 0.05, 0.05] 148 | elif norm_idx == 1: 149 | return [0.05, 0.95, 0.05] 150 | elif norm_idx == 2: 151 | return [0.05, 0.05, 0.95] 152 | elif norm_idx == 3: 153 | return [0.95, 0.05, 0.95] 154 | elif norm_idx == 4: 155 | return [0.95, 0.95, 0.05] 156 | elif norm_idx == 5: 157 | return [0.05, 0.95, 0.95] 158 | return [0.0, 0.0, 0.0] 159 | -------------------------------------------------------------------------------- /src/fgsp/graph/base_graph.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | 3 | import multiprocessing 4 | import numpy as np 5 | from liegroups import SE3 6 | from scipy import spatial 7 | from functools import partial 8 | from multiprocessing import Pool 9 | 10 | from src.fgsp.common.utils import Utils 11 | from src.fgsp.common.logger import Logger 12 | from src.fgsp.common.config import ClientConfig 13 | 14 | 15 | def process_poses(poses, tree, w_func, i): 16 | if len(poses) == 0: 17 | return 18 | max_pos_dist = 6.0 19 | nn_indices = tree.query_ball_point( 20 | poses[i, 0:3], r=max_pos_dist, p=2) 21 | 22 | return nn_indices, [w_func(poses[i, :], poses[nn_i, :]) for nn_i in nn_indices] 23 | 24 | 25 | def compute_distance_weights(coords_lhs, coords_rhs): 26 | sigma = 1.0 27 | normalization = 2.0*(sigma**2) 28 | dist = spatial.distance.euclidean(coords_lhs[0:3], coords_rhs[0:3]) 29 | 30 | return np.exp(-dist/normalization) 31 | 32 | 33 | def compute_so3_weights(pose_lhs, pose_rhs): 34 | R_lhs = Utils.convert_quat_to_rotation(pose_lhs[3:7]) 35 | R_rhs = Utils.convert_quat_to_rotation(pose_rhs[3:7]) 36 | rot_diff = np.matmul(R_lhs, R_rhs.transpose()) 37 | return np.trace(rot_diff) 38 | 39 | 40 | def compute_se3_weights(poses_lhs, poses_rhs): 41 | T_G_lhs = Utils.convert_pos_quat_to_transformation( 42 | poses_lhs[0:3], poses_lhs[3:7]) 43 | T_G_rhs = Utils.convert_pos_quat_to_transformation( 44 | poses_rhs[0:3], poses_rhs[3:7]) 45 | 46 | pose1 = SE3.from_matrix(T_G_lhs) 47 | pose2 = SE3.from_matrix(T_G_rhs) 48 | 49 | Xi_12 = (pose1.inv().dot(pose2)).log() 50 | W = np.eye(4, 4) 51 | W[0, 0] = 1 52 | W[1, 1] = 1 53 | W[2, 2] = 1 54 | W[3, 3] = 1 55 | inner = np.trace( 56 | np.matmul(np.matmul(SE3.wedge(Xi_12), W), SE3.wedge(Xi_12).transpose())) 57 | 58 | # Equal weighting for rotation and translation. 59 | # inner = np.matmul(Xi_12.transpose(),Xi_12) 60 | 61 | dist = np.sqrt(inner) 62 | sigma = 1.0 63 | normalization = 2.0*(sigma**2) 64 | return np.exp(-dist/normalization) 65 | 66 | 67 | class BaseGraph(object): 68 | def __init__(self, config): 69 | if config is None: 70 | config = ClientConfig() 71 | self.config = config 72 | self.is_built = False 73 | self.graph_seq = -1 74 | self.latest_graph_msg = None 75 | 76 | def build(self, graph_msg): 77 | Logger.LogFatal('Called method in BaseGraph') 78 | 79 | def build_from_poses(self, poses): 80 | Logger.LogFatal('Called method in BaseGraph') 81 | 82 | def get_graph(self): 83 | Logger.LogFatal('Called method in BaseGraph') 84 | 85 | def get_coords(self): 86 | Logger.LogFatal('Called method in BaseGraph') 87 | 88 | def write_graph_to_disk(self, coords_file, adj_file): 89 | Logger.LogFatal('Called method in BaseGraph') 90 | 91 | def publish(self): 92 | Logger.LogFatal('Called method in BaseGraph') 93 | 94 | def create_adjacency_from_poses(self, poses): 95 | n_coords = poses.shape[0] 96 | n_dims = poses.shape[1] 97 | adj = np.zeros((n_coords, n_coords)) 98 | tree = spatial.KDTree(poses[:, 0:3]) 99 | 100 | indices = np.arange(0, n_coords) 101 | 102 | if self.config.construction_method == 'se3' and n_dims >= 7: 103 | func = partial(process_poses, poses, tree, compute_se3_weights) 104 | elif self.config.construction_method == 'so3' and n_dims >= 7: 105 | func = partial(process_poses, poses, tree, compute_so3_weights) 106 | elif self.config.construction_method == 'r3': 107 | func = partial(process_poses, poses, tree, 108 | compute_distance_weights) 109 | else: 110 | Logger.LogError( 111 | f'Unknown construction method ({self.config.construction_method}) or not enough dimensions ({n_dims}) . Using position only.') 112 | func = partial(process_poses, poses, tree, 113 | compute_distance_weights) 114 | 115 | n_cores = multiprocessing.cpu_count() if self.config.use_parallel_construction else 1 116 | with Pool(n_cores) as p: 117 | for idx, (nn_indices, weights) in zip(indices, p.map(func, indices)): 118 | for nn_i, w in zip(nn_indices, weights): 119 | if nn_i != idx: 120 | adj[idx, nn_i] = w 121 | adj[nn_i, idx] = w 122 | 123 | adj = np.absolute(adj) 124 | assert np.all(adj >= 0) 125 | return adj 126 | 127 | def reduce_every_other(self, coords): 128 | n_nodes = coords.shape[0] 129 | return np.arange(0, n_nodes, 2) 130 | 131 | def reduce_largest_ev_positive(self, take_n, G): 132 | idx = np.argmax(np.abs(G.U)) 133 | _, idx_fourier = np.unravel_index(idx, G.U.shape) 134 | indices = [] 135 | for i in range(0, take_n): 136 | if (G.U[i, idx_fourier] >= 0): 137 | indices.append(i) 138 | return indices 139 | 140 | def reduce_largest_ev_negative(self, take_n, G): 141 | idx = np.argmax(np.abs(G.U)) 142 | _, idx_fourier = np.unravel_index(idx, G.U.shape) 143 | indices = [] 144 | for i in range(0, take_n): 145 | if (G.U[i, idx_fourier] < 0): 146 | indices.append(i) 147 | return indices 148 | 149 | def reduce_largest_ev(self, take_n, G): 150 | Logger.LogInfo(f'BaseGraph: Reducing to largest {take_n} EVs') 151 | indices = [] 152 | ev = np.abs(G.U) 153 | for i in range(0, take_n): 154 | idx = np.argmax(ev) 155 | idx_vertex, idx_fourier = np.unravel_index(idx, G.U.shape) 156 | if ev[idx_vertex, idx_fourier] == -1: 157 | Logger.LogWarn( 158 | f'BaseGraph: Could not reduce to requested number of nodes: {len(indices)}/{take_n}') 159 | return indices 160 | ev[idx_vertex, :] = -1 161 | indices.append(idx_vertex) 162 | return indices 163 | -------------------------------------------------------------------------------- /src/fgsp/graph/wavelet_evaluator.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | 3 | from platform import node 4 | import numpy as np 5 | from pygsp import graphs, filters, reduction 6 | from enum import Enum 7 | 8 | import pandas 9 | import scipy.spatial 10 | 11 | 12 | from src.fgsp.common.logger import Logger 13 | 14 | 15 | class SubmapState(Enum): 16 | ALL_GOOD = 1 17 | LOW_GOOD = 2 18 | HIGH_GOOD = 3 19 | NO_GOOD = 4 20 | 21 | 22 | class WaveletEvaluator(object): 23 | 24 | def __init__(self, n_scales=6): 25 | assert n_scales >= 3, 'n_scales must be at least 3.' 26 | self.n_scales = n_scales 27 | self.psi = None 28 | self.G = None 29 | self.feature_names = ['Euclidean_L', 'Euclidean_B', 'Euclidean_H', 'Correlation_L', 'Correlation_B', 30 | 'Correlation_H', 'Manhattan_L', 'Manhattan_B', 'Manhattan_H', 'Chebyshev_L', 'Chebyshev_B', 'Chebyshev_H'] 31 | self.ranges = self.compute_constraint_ranges() 32 | Logger.LogDebug( 33 | f'WaveletEvaluator: Created with ranges: {self.ranges}.') 34 | 35 | def set_scales(self, n_scales): 36 | self.n_scales = n_scales 37 | 38 | def compute_constraint_ranges(self): 39 | step_size = self.n_scales // 3 40 | ranges = np.arange(0, self.n_scales, step_size) 41 | low_scale_range = np.arange(ranges[0], ranges[1], dtype=int) 42 | mid_scale_range = np.arange(ranges[1], ranges[2], dtype=int) 43 | large_scale_range = np.arange(ranges[2], self.n_scales, dtype=int) 44 | return [low_scale_range, mid_scale_range, large_scale_range] 45 | 46 | def get_wavelets(self): 47 | return self.psi 48 | 49 | def compute_wavelets(self, G, node_range=None): 50 | Logger.LogInfo( 51 | f'WaveletEvaluator: Computing wavelets for {self.n_scales} scales.') 52 | g = filters.Meyer(G, self.n_scales) 53 | 54 | if node_range is None: 55 | node_range = np.arange(0, G.N) 56 | else: 57 | Logger.LogWarn( 58 | f'WaveletEvaluator: Computing wavelets for nodes {node_range}.') 59 | n = G.N 60 | 61 | # Evalute filter bank on the frequencies (eigenvalues). 62 | f = g.evaluate(G.e) 63 | f = np.expand_dims(f.T, 1) 64 | self.psi = np.zeros((n, n, self.n_scales)) 65 | self.G = G 66 | 67 | for i in node_range: 68 | # Create a Dirac centered at node i. 69 | x = np.zeros((n, 1)) 70 | x[i] = 1 71 | 72 | # Transform the signal to spectral domain. 73 | s = G.gft(x) 74 | 75 | # Multiply the transformed signal with filter. 76 | if s.ndim == 1: 77 | s = np.expand_dims(s, -1) 78 | s = np.expand_dims(s, 1) 79 | s = np.matmul(s, f) # [nodes, features, scales] 80 | 81 | # Transform back the features to the vertex domain. 82 | self.psi[i, :, :] = G.igft(s).squeeze() 83 | 84 | return self.psi 85 | 86 | def compute_wavelet_coeffs(self, x_signal): 87 | return self.compute_wavelet_coeffs_using_wavelet(self.psi, x_signal) 88 | 89 | def compute_wavelet_coeffs_using_wavelet(self, wavelet, x_signal): 90 | n_values = x_signal.shape[0] 91 | n_dim = x_signal.shape[1] if len(x_signal.shape) >= 2 else 1 92 | W = np.zeros((n_values, self.n_scales, n_dim)).squeeze() 93 | 94 | for i in range(0, n_values): 95 | for j in range(0, self.n_scales): 96 | W[i, j] = np.matmul(wavelet[i, :, j].transpose(), x_signal) 97 | 98 | return W if n_dim == 1 else np.mean(W, axis=2) 99 | 100 | def compute_distances_1D(self, coeffs_1, coeffs_2): 101 | distances = np.zeros((1, self.n_scales)) 102 | for j in range(0, self.n_scales): 103 | distances[0, j] = scipy.spatial.distance.euclidean( 104 | coeffs_1[j], coeffs_2[j]) 105 | 106 | return distances 107 | 108 | def compute_features(self, submap_coeffs_1, submap_coeffs_2): 109 | n_nodes = submap_coeffs_1.shape[0] 110 | all_data = pandas.DataFrame() 111 | for i in range(n_nodes): 112 | D = self.compute_distances_1D( 113 | submap_coeffs_1[i, :], submap_coeffs_2[i, :]) 114 | data = pandas.DataFrame({ 115 | # self.feature_names[0]: [np.sum(D[0, 0:2])], 116 | # self.feature_names[1]: [np.sum(D[0, 2:4])], 117 | # self.feature_names[2]: [np.sum(D[0, 4:6])], 118 | 119 | # self.feature_names[0]: [np.sum(D[0, 0:3])], 120 | # self.feature_names[1]: [np.sum(D[0, 3:6])], 121 | # self.feature_names[2]: [np.sum(D[0, 6:9])], 122 | 123 | # self.feature_names[0]: [np.sum(D[0, 0:4])], 124 | # self.feature_names[1]: [np.sum(D[0, 4:8])], 125 | # self.feature_names[2]: [np.sum(D[0, 8:12])], 126 | 127 | self.feature_names[0]: [np.sum(D[0, self.ranges[0]])], 128 | self.feature_names[1]: [np.sum(D[0, self.ranges[1]])], 129 | self.feature_names[2]: [np.sum(D[0, self.ranges[2]])], 130 | 131 | # self.feature_names[0]: [np.sum(D[0, 0])], 132 | # self.feature_names[1]: [np.sum(D[0, 1])], 133 | # self.feature_names[2]: [np.sum(D[0, 2])], 134 | }) 135 | all_data = pandas.concat([all_data, data]) 136 | return np.nan_to_num(all_data) 137 | 138 | 139 | if __name__ == '__main__': 140 | print(" --- Test Driver for the Wavelet Evaluator ----------------------") 141 | eval = WaveletEvaluator() 142 | 143 | # Create a reduced graph for quicker tests. 144 | G = graphs.Bunny() 145 | ind = np.arange(0, G.N, 10) 146 | Gs = reduction.kron_reduction(G, ind) 147 | Gs.compute_fourier_basis() 148 | 149 | # Compute wavelets. 150 | psi = eval.compute_wavelets(Gs) 151 | 152 | # Compute wavelet coefficients for two signals. 153 | x_1 = Gs.coords 154 | x_1 = np.linalg.norm(x_1, ord=2, axis=1) 155 | x_2 = Gs.coords + 10 156 | x_2 = np.linalg.norm(x_2, ord=2, axis=1) 157 | 158 | W_1 = eval.compute_wavelet_coeffs_using_wavelet(psi, x_1) 159 | W_2 = eval.compute_wavelet_coeffs_using_wavelet(psi, x_2) 160 | features = eval.compute_features(W_1, W_2) 161 | label = eval.classify_simple(features) 162 | -------------------------------------------------------------------------------- /src/fgsp/tools/lookup_aligned_pose.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | 3 | import pathlib 4 | import numpy as np 5 | import os 6 | 7 | import rclpy 8 | from rclpy.node import Node 9 | from sensor_msgs.msg import PointCloud2 10 | from sensor_msgs_py import point_cloud2 11 | from evo.tools import file_interface 12 | from evo.core import sync 13 | from evo.core import metrics 14 | from evo.tools import plot 15 | from evo.core.trajectory import PoseTrajectory3D 16 | import matplotlib.pyplot as plt 17 | 18 | 19 | from src.fgsp.common.logger import Logger 20 | 21 | 22 | class LookupAlignedPose(Node): 23 | def __init__(self): 24 | super().__init__('lookup_aligned_pose') 25 | Logger.Verbosity = 5 26 | Logger.LogInfo('LookupAlingedPose: Initializing...') 27 | 28 | input_file = self.get_param('input_file', '') 29 | align_file = self.get_param('align_to_file', '') 30 | 31 | if (input_file == '' or not os.path.exists(input_file)): 32 | Logger.LogError('LookupAlignedPose: Invalid input file') 33 | return 34 | 35 | should_align = align_file == '' or not os.path.exists(align_file) 36 | if (should_align): 37 | Logger.LogWarn( 38 | 'LookupAlignedPose: No alignement file provided or file does not exist.') 39 | 40 | lookup_ts_ns = self.get_param('timestamp_ns', 0) 41 | if (lookup_ts_ns == 0): 42 | Logger.LogError('LookupAlignedPose: Invalid timestamp') 43 | 44 | Logger.LogInfo( 45 | 'LookupAlingedPose: Initializing done. Processing data...') 46 | input_traj = self.synchronize_and_align( 47 | input_file, align_file, should_align) 48 | pose = self.lookup_aligned_pose(input_traj, lookup_ts_ns) 49 | if pose is None: 50 | Logger.LogError('LookupAlignedPose: Could not find matching pose.') 51 | else: 52 | self.print_pose(pose) 53 | 54 | def print_pose(self, pose): 55 | Logger.LogInfo(f'LookupAlignedPose: Pose:') 56 | Logger.LogInfo(f'LookupAlignedPose: xyz: {pose[1:4]}') 57 | Logger.LogInfo(f'LookupAlignedPose: wxyz: {pose[4:8]}') 58 | 59 | def lookup_aligned_pose(self, input_traj, lookup_ts_ns): 60 | k_ns_per_s = 1e9 61 | lookup_ts_s = lookup_ts_ns / k_ns_per_s 62 | idx = self.lookup_closest_ts_idx(input_traj.timestamps, lookup_ts_s) 63 | if idx < 0: 64 | return None 65 | 66 | return np.array((input_traj.timestamps[idx], *input_traj.positions_xyz[idx, :], 67 | *input_traj.orientations_quat_wxyz[idx, :])) 68 | 69 | def lookup_closest_ts_idx(self, timestamps_s, ts_s, eps=1e-2): 70 | print(f'Looking for closest timestamp to {ts_s}...') 71 | print(f'first then ts {timestamps_s[0]}') 72 | diff_timestamps = np.abs(timestamps_s - ts_s) 73 | minval = np.amin(diff_timestamps) 74 | if (minval > eps): 75 | return -1 76 | 77 | return np.where(diff_timestamps == minval)[0][0] 78 | 79 | def synchronize_and_align(self, input_file, align_file, should_align): 80 | input_traj = self.read_file(input_file) 81 | if should_align: 82 | return input_traj 83 | 84 | align_traj = self.read_file(align_file) 85 | 86 | print( 87 | f'Found {input_traj.num_poses} and {align_traj.num_poses} poses for input and align file, respectively.') 88 | 89 | align_traj, input_traj = sync.associate_trajectories( 90 | align_traj, input_traj, max_diff=0.1) 91 | 92 | alignment = input_traj.align(align_traj, correct_scale=False, 93 | correct_only_scale=False, n=1000) 94 | print('--- Alignement -----------------') 95 | print('Rotation:') 96 | print(alignment[0]) 97 | print('Translation:') 98 | print(alignment[1]) 99 | print('Scale:') 100 | print(alignment[2]) 101 | 102 | # self.evaluate(align_traj, input_traj, True) 103 | 104 | print(f'Aligned {input_traj.num_poses} synchronized poses.') 105 | return input_traj 106 | 107 | def perform_evaluation(self, data): 108 | pose_relation = metrics.PoseRelation.translation_part 109 | ape_metric_trans = metrics.APE(pose_relation) 110 | ape_metric_trans.process_data(data) 111 | ape_stats_trans = ape_metric_trans.get_all_statistics() 112 | print('--- APE ---------------------------') 113 | print(ape_stats_trans) 114 | return ape_stats_trans, ape_metric_trans 115 | 116 | def plot_trajectories(self, data, ape_metric, ape_stats): 117 | fig = plt.figure(figsize=(8, 6), dpi=160) 118 | traj_by_label = { 119 | "estimate": data[1], 120 | "reference": data[0] 121 | } 122 | plot.trajectories(fig, traj_by_label, plot.PlotMode.xy) 123 | plt.show() 124 | 125 | def evaluate(self, gt_traj, est_traj, should_plot): 126 | data = (gt_traj, est_traj) 127 | stats, metric = self.perform_evaluation(data) 128 | if should_plot: 129 | self.plot_trajectories(data, metric, stats) 130 | 131 | def read_file(self, file): 132 | ext = pathlib.Path(file).suffix 133 | traj = None 134 | if ext == '.csv': 135 | traj = self.read_csv_file(file) 136 | elif ext == '.npy': 137 | traj = self.read_npy_file(file) 138 | return traj 139 | 140 | def read_csv_file(self, filename): 141 | Logger.LogDebug(f'LookupAlignedPose: Reading file {filename}') 142 | if os.path.exists(filename): 143 | return file_interface.read_tum_trajectory_file(filename) 144 | else: 145 | Logger.LogError(f'LookupAlignedPose: File does not exist!') 146 | return np.array([]) 147 | 148 | def read_npy_file(self, filename): 149 | trajectory = np.load(filename) 150 | ts = trajectory[:, 0] 151 | xyz = trajectory[:, 1:4] 152 | wxyz = trajectory[:, 4:8] 153 | return PoseTrajectory3D(positions_xyz=xyz, orientations_quat_wxyz=wxyz, timestamps=ts) 154 | 155 | def get_param(self, key, default): 156 | self.declare_parameter(key, default) 157 | return self.get_parameter(key).value 158 | 159 | 160 | def main(args=None): 161 | rclpy.init(args=args) 162 | lookup = LookupAlignedPose() 163 | lookup.destroy_node() 164 | rclpy.shutdown() 165 | 166 | 167 | if __name__ == '__main__': 168 | main() 169 | -------------------------------------------------------------------------------- /src/fgsp/controller/command_post.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | 3 | from cProfile import label 4 | from fileinput import filename 5 | import numpy as np 6 | import time 7 | import pickle 8 | 9 | from yaml import serialize 10 | 11 | from maplab_msgs.msg import Graph, Trajectory, TrajectoryNode 12 | from geometry_msgs.msg import PoseStamped 13 | from nav_msgs.msg import Path 14 | 15 | from src.fgsp.common.logger import Logger 16 | from src.fgsp.common.comms import Comms 17 | from src.fgsp.common.utils import Utils 18 | 19 | 20 | class CommandPost(object): 21 | def __init__(self, config): 22 | self.config = config 23 | self.comms = Comms() 24 | 25 | self.degenerate_path_msg = None 26 | self.degenerate_indices = [] 27 | self.previous_relatives = {} 28 | self.small_constraint_counter = 0 29 | self.mid_constraint_counter = 0 30 | self.large_constraint_counter = 0 31 | self.anchor_constraint_counter = 0 32 | self.history = {} 33 | 34 | Logger.LogInfo("CommandPost: Initialized command post center.") 35 | 36 | def reset_msgs(self): 37 | self.degenerate_path_msg = Path() 38 | self.small_constraint_counter = 0 39 | self.mid_constraint_counter = 0 40 | self.large_constraint_counter = 0 41 | self.anchor_constraint_counter = 0 42 | 43 | def evaluate_labels_per_node(self, labels): 44 | # Should always publish for all states as we don't know 45 | # whether they reached the clients. 46 | n_nodes = labels.size() 47 | 48 | # Set the history for the current labels. 49 | labels.history = self.history 50 | for i in range(0, n_nodes): 51 | if i in self.previous_relatives.keys(): 52 | labels.labels[i] = list( 53 | set(labels.labels[i]+self.previous_relatives[i])) 54 | 55 | relative_constraint, small_relative_counter, mid_relative_counter, large_relative_counter = labels.check_and_construct_constraint_at( 56 | i) 57 | if relative_constraint is None: 58 | continue # no-op 59 | self.previous_relatives[i] = labels.labels[i] 60 | self.comms.publish(relative_constraint, Path, 61 | self.config.relative_node_topic) 62 | self.add_to_constraint_counter( 63 | small_relative_counter, mid_relative_counter, large_relative_counter) 64 | time.sleep(0.001) 65 | 66 | self.serialize_connections(self.history, labels) 67 | 68 | def add_to_constraint_counter(self, n_small_constraints, n_mid_constraints, n_large_constraints): 69 | self.small_constraint_counter = self.small_constraint_counter + n_small_constraints 70 | self.mid_constraint_counter = self.mid_constraint_counter + n_mid_constraints 71 | self.large_constraint_counter = self.large_constraint_counter + n_large_constraints 72 | 73 | def get_total_amount_of_constraints(self): 74 | return self.small_constraint_counter + self.mid_constraint_counter + self.large_constraint_counter 75 | 76 | def create_pose_msg_from_node(self, cur_opt): 77 | pose_msg = PoseStamped() 78 | pose_msg.header.stamp = cur_opt.ts 79 | pose_msg.pose.position.x = cur_opt.position[0] 80 | pose_msg.pose.position.y = cur_opt.position[1] 81 | pose_msg.pose.position.z = cur_opt.position[2] 82 | pose_msg.pose.orientation.w = cur_opt.orientation[0] 83 | pose_msg.pose.orientation.x = cur_opt.orientation[1] 84 | pose_msg.pose.orientation.y = cur_opt.orientation[2] 85 | pose_msg.pose.orientation.z = cur_opt.orientation[3] 86 | return pose_msg 87 | 88 | def send_anchors(self, all_opt_nodes, begin_send, end_send): 89 | Logger.LogError( 90 | f'CommandPost: Sending degenerate anchors for {end_send - begin_send} nodes.') 91 | indices = np.arange(begin_send, end_send, 1) 92 | self.send_anchors_based_on_indices(all_opt_nodes, indices) 93 | 94 | def send_anchors_based_on_indices(self, opt_nodes, indices): 95 | n_constraints = len(indices) 96 | Logger.LogError( 97 | f'CommandPost: Sending anchors for {n_constraints} nodes.') 98 | for i in indices: 99 | pose_msg = self.create_pose_msg_from_node(opt_nodes[i]) 100 | self.degenerate_path_msg.poses.append(pose_msg) 101 | 102 | # Update the degenerate anchor indices 103 | if not i in self.degenerate_indices: 104 | self.degenerate_indices.append(i) 105 | 106 | # Publish the anchor nodes. 107 | self.degenerate_path_msg.header.stamp = self.comms.time_now() 108 | self.comms.publish(self.degenerate_path_msg, Path, 109 | self.config.anchor_node_topic) 110 | self.anchor_constraint_counter = self.anchor_constraint_counter + n_constraints 111 | 112 | def update_degenerate_anchors(self, all_opt_nodes): 113 | if len(self.degenerate_indices) == 0: 114 | return 115 | Logger.LogError( 116 | f'CommandPost: Sending degenerate anchor update for {self.degenerate_indices}') 117 | self.send_degenerate_anchors_based_on_indices( 118 | all_opt_nodes, self.degenerate_indices) 119 | 120 | def serialize_connections(self, history, labels): 121 | edges_dict = {} 122 | labels_dict = {} 123 | for k in history.keys(): 124 | parent_node = labels.opt_nodes[k] 125 | parent_ts_ns = Utils.ros_time_msg_to_ns(parent_node.ts) 126 | n_children = history[k].size() 127 | for i in range(0, n_children): 128 | child_k = history[k].children[i] 129 | child_node = labels.opt_nodes[child_k] 130 | child_ts_ns = Utils.ros_time_msg_to_ns(child_node.ts) 131 | if parent_ts_ns not in edges_dict.keys(): 132 | edges_dict[parent_ts_ns] = [] 133 | edges_dict[parent_ts_ns].append(child_ts_ns) 134 | 135 | child_label = history[k].types[i] 136 | if parent_ts_ns not in labels_dict.keys(): 137 | labels_dict[parent_ts_ns] = [] 138 | labels_dict[parent_ts_ns].append(child_label) 139 | 140 | filename = self.config.dataroot + self.config.connections_output_path 141 | outputFile = open(filename, 'w+b') 142 | pickle.dump(edges_dict, outputFile) 143 | outputFile.close() 144 | 145 | filename = self.config.dataroot + self.config.label_output_path 146 | outputFile = open(filename, 'w+b') 147 | pickle.dump(labels_dict, outputFile) 148 | outputFile.close() 149 | -------------------------------------------------------------------------------- /src/fgsp/common/plotter.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | 3 | class font: 4 | PURPLE = '\033[95m' 5 | CYAN = '\033[96m' 6 | DARKCYAN = '\033[36m' 7 | BLUE = '\033[94m' 8 | GREEN = '\033[92m' 9 | YELLOW = '\033[93m' 10 | RED = '\033[91m' 11 | BOLD = '\033[1m' 12 | UNDERLINE = '\033[4m' 13 | END = '\033[0m' 14 | CLEAR = '\033c' 15 | 16 | 17 | class Plotter(object): 18 | 19 | @staticmethod 20 | def PlotMonitorBanner(): 21 | banner = ''' 22 | ******** **** **** 23 | **//////** /**/** **/** 24 | ** // /**//** ** /** ****** ******* 25 | /** *****/** //*** /** **////**//**///** 26 | /** *****///// /** //* /**/** /** /** /** 27 | //** ////** /** / /**/** /** /** /** 28 | //******** /** /**//****** *** /** 29 | //////// // // ////// /// // ''' 30 | 31 | print('\n{banner}\n\n\n'.format(banner=banner)) 32 | 33 | @staticmethod 34 | def PlotClientBanner(): 35 | banner = ''' 36 | ******** ****** ** ** ** 37 | **//////** **////** /**// /** 38 | ** // ** // /** ** ***** ******* ****** 39 | /** *****/** /**/** **///**//**///**///**/ 40 | /** *****///// /** /**/**/******* /** /** /** 41 | //** ////** //** ** /**/**/**//// /** /** /** 42 | //******** //****** ***/**//****** *** /** //** 43 | //////// ////// /// // ////// /// // // ''' 44 | 45 | print('\n{banner}\n\n\n'.format(banner=banner)) 46 | 47 | @staticmethod 48 | def PrintMonitorConfig(config): 49 | print('{color} --- General Configuration -------------------------------------------------- {end}'.format(color=font.BLUE, end=font.END)) 50 | print('{bold} Update rate:{end} {val}hz'.format( 51 | bold=font.BOLD, end=font.END, val=1/config.rate)) 52 | print('{bold} Reduce optimized graph using Kron:{end} {val}'.format( 53 | bold=font.BOLD, end=font.END, val=config.reduce_global_graph)) 54 | print('{bold} Minimal node count in optimized graph:{end} {val}'.format( 55 | bold=font.BOLD, end=font.END, val=config.min_node_count)) 56 | print('\n') 57 | 58 | print('{color} --- Subscriber Configuration (Input) --------------------------------------- {end}'.format( 59 | color=font.YELLOW, end=font.END)) 60 | print('{bold} Optimized graph topic:{end} {val}'.format( 61 | bold=font.BOLD, end=font.END, val=config.in_graph_topic)) 62 | print('{bold} Optimized trajectory topic:{end} {val}'.format( 63 | bold=font.BOLD, end=font.END, val=config.in_traj_opt_topic)) 64 | print('{bold} Optimized submap topic:{end} {val}'.format( 65 | bold=font.BOLD, end=font.END, val=config.opt_pc_topic)) 66 | print('{bold} Verifcation request topic:{end} {val}'.format( 67 | bold=font.BOLD, end=font.END, val=config.verification_service_topic)) 68 | print('\n') 69 | 70 | print('{color} --- Publisher Configuration (Output) --------------------------------------- {end}'.format( 71 | color=font.GREEN, end=font.END)) 72 | print('{bold} Optimized graph topic:{end} {val}'.format( 73 | bold=font.BOLD, end=font.END, val=config.out_graph_topic)) 74 | print('{bold} Optimized trajectory topic:{end} {val}'.format( 75 | bold=font.BOLD, end=font.END, val=config.out_traj_opt_topic)) 76 | print('\n') 77 | 78 | @staticmethod 79 | def PrintClientConfig(config): 80 | print('{color} --- General Configuration -------------------------------------------------- {end}'.format(color=font.BLUE, end=font.END)) 81 | print('{bold} Update rate:{end} {val}hz'.format( 82 | bold=font.BOLD, end=font.END, val=1/config.rate)) 83 | print('{bold} Dataroot:{end} {val}'.format( 84 | bold=font.BOLD, end=font.END, val=config.dataroot)) 85 | print('{bold} Robot name:{end} {val}'.format( 86 | bold=font.BOLD, end=font.END, val=config.robot_name)) 87 | print('{bold} Enable anchor constraints:{end} {val}'.format( 88 | bold=font.BOLD, end=font.END, val=config.enable_anchor_constraints)) 89 | print('{bold} Enable signal recording:{end} {val}'.format( 90 | bold=font.BOLD, end=font.END, val=config.enable_signal_recording)) 91 | print('{bold} Enable trajectory recording:{end} {val}'.format( 92 | bold=font.BOLD, end=font.END, val=config.enable_trajectory_recording)) 93 | print('{bold} Signal export path:{end} {val}'.format( 94 | bold=font.BOLD, end=font.END, val=config.signal_export_path)) 95 | print('{bold} Trajectory export path:{end} {val}'.format( 96 | bold=font.BOLD, end=font.END, val=config.trajectory_export_path)) 97 | print('\n') 98 | 99 | print('{color} --- Operating Mode --------------------------------------------------------- {end}'.format(color=font.RED, end=font.END)) 100 | if config.construction_method == 'se3': 101 | print('{bold} Using SE(3) computations {end}'.format( 102 | bold=font.BOLD, end=font.END)) 103 | elif config.construction_method == 'so3': 104 | print('{bold} Using SO(3) computations {end}'.format( 105 | bold=font.BOLD, end=font.END)) 106 | else: 107 | print('{bold} Using R^3 computations {end}'.format( 108 | bold=font.BOLD, end=font.END)) 109 | print('{bold} Classifier: {end} {val}'.format( 110 | bold=font.BOLD, end=font.END, val=config.classifier)) 111 | print('\n') 112 | 113 | print('{color} --- Subscriber Configuration (Input) --------------------------------------- {end}'.format( 114 | color=font.YELLOW, end=font.END)) 115 | print('{bold} Optimized graph topic:{end} {val}'.format( 116 | bold=font.BOLD, end=font.END, val=config.opt_graph_topic)) 117 | print('{bold} Optimized trajectory topic:{end} {val}'.format( 118 | bold=font.BOLD, end=font.END, val=config.opt_traj_topic)) 119 | print('{bold} Estimated trajectory topic:{end} {val}'.format( 120 | bold=font.BOLD, end=font.END, val=config.est_traj_topic)) 121 | print('{bold} Estimated trajectory (Path) topic:{end} {val}'.format( 122 | bold=font.BOLD, end=font.END, val=config.est_traj_path_topic)) 123 | print('{bold} Client update topic:{end} {val}'.format( 124 | bold=font.BOLD, end=font.END, val=config.client_update_topic)) 125 | print('\n') 126 | 127 | print('{color} --- Publisher Configuration (Output) --------------------------------------- {end}'.format( 128 | color=font.GREEN, end=font.END)) 129 | print('{bold} Anchor node topic:{end} {val}'.format( 130 | bold=font.BOLD, end=font.END, val=config.anchor_node_topic)) 131 | print('{bold} Relative node topic:{end} {val}'.format( 132 | bold=font.BOLD, end=font.END, val=config.relative_node_topic)) 133 | print('{bold} Intra constraints topic:{end} {val}'.format( 134 | bold=font.BOLD, end=font.END, val=config.intra_constraint_topic)) 135 | print('\n') 136 | 137 | @staticmethod 138 | def PrintSeparator(): 139 | print("{bold} ============================================================================ {end}".format( 140 | bold=font.BOLD, end=font.END)) 141 | 142 | 143 | if __name__ == '__main__': 144 | Plotter.PlotMonitorBanner() 145 | Plotter.PlotClientBanner() 146 | -------------------------------------------------------------------------------- /src/fgsp/tools/cloud_publisher.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | 3 | import os 4 | import pathlib 5 | import numpy as np 6 | 7 | import rclpy 8 | from rclpy.node import Node 9 | from sensor_msgs.msg import PointCloud2 10 | from geometry_msgs.msg import PoseStamped 11 | from sensor_msgs_py import point_cloud2 12 | from nav_msgs.msg import Path 13 | from std_msgs.msg import Header 14 | import open3d as o3d 15 | from evo.tools import file_interface 16 | from plyfile import PlyData, PlyElement 17 | 18 | from src.fgsp.common.logger import Logger 19 | 20 | 21 | class CloudPublisher(Node): 22 | def __init__(self): 23 | super().__init__('cloud_publisher') 24 | Logger.Verbosity = 5 25 | Logger.LogInfo('CloudPublisher: Initializing...') 26 | 27 | self.cloud_topic = self.get_param('cloud_topic', '/point_cloud') 28 | self.cloud_input_path = self.get_param('cloud_input_path', '') 29 | self.enable_clouds = True 30 | self.n_clouds = 0 31 | if (self.cloud_input_path == '' or not os.path.exists(self.cloud_input_path)): 32 | Logger.LogWarn( 33 | f'CloudPublisher: Invalid input path: {self.cloud_input_path}') 34 | self.enable_clouds = False 35 | 36 | self.path_topic = self.get_param('path_topic', '/path') 37 | self.path_input_path = self.get_param('path_input_path', '') 38 | self.enable_paths = True 39 | self.n_paths = 0 40 | if (self.path_input_path == '' or not os.path.exists(self.path_input_path)): 41 | Logger.LogWarn( 42 | f'CloudPublisher: Invalid input path: {self.path_input_path}') 43 | self.enable_paths = False 44 | 45 | self.use_voxel_grid = self.get_param('use_voxel_grid', False) 46 | self.voxel_size = self.get_param('voxel_size', 0.1) 47 | 48 | if self.enable_clouds: 49 | self.clouds = self.read_clouds(self.cloud_input_path) 50 | self.n_clouds = len(self.clouds) 51 | if (self.n_clouds == 0): 52 | Logger.LogError('CloudPublisher: No clouds found') 53 | 54 | self.cloud_pubs = [None] * self.n_clouds 55 | for i in range(self.n_clouds): 56 | self.cloud_pubs[i] = self.create_publisher( 57 | PointCloud2, f'{self.cloud_topic}_{i}', 10) 58 | 59 | if self.enable_paths: 60 | self.paths = self.read_paths(self.path_input_path) 61 | self.n_paths = len(self.paths) 62 | if (self.n_paths == 0): 63 | Logger.LogError('CloudPublisher: No poses found') 64 | 65 | self.path_pubs = [None] * self.n_paths 66 | for i in range(self.n_paths): 67 | self.path_pubs[i] = self.create_publisher( 68 | Path, f'{self.path_topic}_{i}', 10) 69 | 70 | Logger.LogInfo( 71 | f'CloudPublisher: Read clouds from {self.cloud_input_path}') 72 | Logger.LogInfo( 73 | f'pathPublisher: Read paths from {self.path_input_path}') 74 | Logger.LogInfo( 75 | f'CloudPublisher: Publishing to {self.cloud_topic} and {self.path_topic}') 76 | Logger.LogInfo( 77 | 'CloudPublisher: Initializing done. Publishing clouds...') 78 | 79 | rate = self.get_param('rate', 0.1) 80 | self.timer = self.create_timer( 81 | 1 / rate, self.publish) 82 | 83 | def get_param(self, key, default): 84 | self.declare_parameter(key, default) 85 | return self.get_parameter(key).value 86 | 87 | def read_ply_cloud(self, full_path): 88 | plydata = PlyData.read(full_path) 89 | vertex = plydata['vertex'] 90 | x = vertex['x'] 91 | y = vertex['y'] 92 | z = vertex['z'] 93 | i = np.zeros_like(z) 94 | if 'i' in vertex: 95 | i = vertex['i'] 96 | elif 'intensity' in vertex: 97 | i = vertex['intensity'] 98 | 99 | return np.column_stack((x, y, z, i)) 100 | 101 | def read_npy_cloud(self, full_path): 102 | return np.load(full_path) 103 | 104 | def read_clouds(self, input_path): 105 | in_clouds = os.listdir(input_path) 106 | parsed_clouds = [] 107 | if (len(in_clouds) == 0): 108 | Logger.LogError( 109 | f'CloudPublisher: No clouds found in {input_path}') 110 | return parsed_clouds 111 | for cloud_file in in_clouds: 112 | full_path = f'{input_path}/{cloud_file}' 113 | ext = pathlib.Path(full_path).suffix 114 | 115 | cloud = None 116 | if ext == '.ply': 117 | cloud = self.read_ply_cloud(full_path) 118 | elif ext == '.npy': 119 | cloud = self.read_npy_cloud(full_path) 120 | 121 | if cloud is not None: 122 | Logger.LogInfo( 123 | f'CloudPublisher: Read {cloud.shape} points from {cloud_file}') 124 | parsed_clouds.append(cloud) 125 | 126 | return parsed_clouds 127 | 128 | def parse_csv_path(self, full_path): 129 | robot_traj = file_interface.read_tum_trajectory_file(full_path) 130 | path = np.column_stack((robot_traj.timestamps, robot_traj.positions_xyz, 131 | robot_traj.orientations_quat_wxyz)) 132 | return self.parse_path(path) 133 | 134 | def parse_npy_path(self, full_path): 135 | path = np.load(full_path) 136 | return self.parse_path(path) 137 | 138 | def parse_path(self, path): 139 | path_msg = Path() 140 | for pose in path: 141 | t = pose[1:4] 142 | pose_msg = PoseStamped() 143 | pose_msg.header.stamp = self.get_clock().now().to_msg() 144 | pose_msg.pose.position.x = t[0] 145 | pose_msg.pose.position.y = t[1] 146 | pose_msg.pose.position.z = t[2] 147 | pose_msg.pose.orientation.x = 0.0 148 | pose_msg.pose.orientation.y = 0.0 149 | pose_msg.pose.orientation.z = 0.0 150 | pose_msg.pose.orientation.w = 1.0 151 | path_msg.poses.append(pose_msg) 152 | return path_msg 153 | 154 | def read_paths(self, input_path): 155 | in_paths = os.listdir(input_path) 156 | parsed_paths = [] 157 | if (len(in_paths) == 0): 158 | Logger.LogError( 159 | f'CloudPublisher: No paths found in {input_path}') 160 | for path_file in in_paths: 161 | full_path = f'{input_path}/{path_file}' 162 | ext = pathlib.Path(path_file).suffix 163 | path = None 164 | if ext == '.csv': 165 | path = self.parse_csv_path(full_path) 166 | elif ext == '.npy': 167 | path = self.parse_npy_path(full_path) 168 | 169 | if path is not None: 170 | Logger.LogInfo( 171 | f'CloudPublisher: Read {len(path.poses)} poses from {path_file}') 172 | parsed_paths.append(path) 173 | return parsed_paths 174 | 175 | def voxel_down_sample(self, cloud, voxel_size=0.1): 176 | print(f'cloud: {cloud.shape}') 177 | pcd = o3d.geometry.PointCloud(o3d.utility.Vector3dVector(cloud)) 178 | pcd = pcd.voxel_down_sample(voxel_size=voxel_size) 179 | return np.array(pcd.points) 180 | 181 | def publish(self): 182 | header = Header() 183 | header.stamp = self.get_clock().now().to_msg() 184 | header.frame_id = 'map' 185 | if self.enable_clouds: 186 | for i in range(self.n_clouds): 187 | cloud = self.clouds[i] 188 | if (self.use_voxel_grid): 189 | cloud = self.voxel_down_sample(cloud, self.voxel_size) 190 | msg = point_cloud2.create_cloud_xyz32(header, cloud[:, 0:3]) 191 | self.cloud_pubs[i].publish(msg) 192 | 193 | if self.enable_paths: 194 | for i in range(self.n_paths): 195 | path = self.paths[i] 196 | path.header = header 197 | self.path_pubs[i].publish(path) 198 | 199 | Logger.LogInfo( 200 | f'Published {self.n_clouds} clouds and {self.n_paths} paths') 201 | 202 | 203 | def main(args=None): 204 | rclpy.init(args=args) 205 | server = CloudPublisher() 206 | rclpy.spin(server) 207 | server.destroy_node() 208 | rclpy.shutdown() 209 | 210 | 211 | if __name__ == '__main__': 212 | main() 213 | -------------------------------------------------------------------------------- /src/fgsp/tools/simulation.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | 3 | from os.path import exists 4 | 5 | import rclpy 6 | import pathlib 7 | from rclpy.node import Node 8 | from evo.tools import file_interface 9 | from evo.core.trajectory import PoseTrajectory3D 10 | from maplab_msgs.msg import Graph, Trajectory, SubmapConstraint 11 | 12 | import numpy as np 13 | 14 | from rosbags.serde import serialize_cdr 15 | from rosbags.rosbag2 import Writer as Rosbag2Writer 16 | from rosbags.typesys import get_types_from_msg, register_types 17 | 18 | from src.fgsp.common.logger import Logger 19 | from src.fgsp.controller.signal_handler import SignalHandler 20 | 21 | 22 | class Simulation(Node): 23 | def __init__(self): 24 | super().__init__('simulation') 25 | Logger.Verbosity = 5 26 | Logger.LogInfo('Simulation: Initializing...') 27 | 28 | self.server_traj = self.get_traj_file('server_file') 29 | self.robot_traj = self.get_traj_file('robot_file') 30 | 31 | if self.server_traj is None and self.robot_traj is None: 32 | Logger.LogError( 33 | 'Simulation: No trajectory file has been parsed. Aborting.!') 34 | return 35 | 36 | self.odom_topic = self.get_param('odom_topic', '/odometry') 37 | self.monitor_topic = self.get_param('monitor_topic', '/monitor') 38 | self.robot_name = self.get_param('robot_name', 'robot') 39 | self.map_frame = self.get_param('map_frame', 'map') 40 | self.child_frame = self.get_param('child_frame', 'base') 41 | self.graph_threshold_dist = self.get_param('graph_threshold_dist', 0.5) 42 | self.out_bag_file = self.get_param( 43 | 'out_bag_file', '/tmp/odom_and_monitor.bag') 44 | 45 | Logger.LogInfo( 46 | f'Simulation: Writing bag file to: {self.out_bag_file}') 47 | ros2_bag_out = Rosbag2Writer(self.out_bag_file) 48 | ros2_bag_out.open() 49 | 50 | ros_time_odom = None 51 | if self.robot_traj is not None: 52 | Logger.LogInfo( 53 | f'Simulation: Robot trajectory has {self.robot_traj.num_poses} poses') 54 | Logger.LogDebug( 55 | f'Simulation: Robot trajectory: {self.robot_traj.get_infos()}') 56 | ros_time_odom = self.write_odometry(self.robot_traj, ros2_bag_out) 57 | 58 | ros_time_traj = None 59 | if self.server_traj is not None: 60 | Logger.LogInfo( 61 | f'Simulation: Server trajectory has {self.server_traj.num_poses} poses') 62 | Logger.LogDebug( 63 | f'Simulation: Server trajectory: {self.server_traj.get_infos()}') 64 | ros_time_traj = self.write_server_trajectory( 65 | self.server_traj, ros2_bag_out) 66 | 67 | self.write_delayed_msg(ros_time_odom, ros_time_traj, ros2_bag_out) 68 | 69 | ros2_bag_out.close() 70 | Logger.LogInfo('Simulation: Writing bag file done.') 71 | 72 | def get_param(self, key, default): 73 | self.declare_parameter(key, default) 74 | return self.get_parameter(key).value 75 | 76 | def get_traj_file(self, traj_key): 77 | traj_file_path = self.get_param(traj_key, '') 78 | if traj_file_path == '': 79 | return None 80 | 81 | ext = pathlib.Path(traj_file_path).suffix 82 | if ext == '.csv': 83 | Logger.LogInfo('Simulation: Reading CSV file.') 84 | return self.read_csv_file(traj_file_path) 85 | elif ext == '.npy': 86 | Logger.LogInfo('Simulation: Reading NPY file.') 87 | return self.read_npy_file(traj_file_path) 88 | Logger.LogError(f'Simulation: Unknown file extension: {ext}') 89 | return None 90 | 91 | def read_csv_file(self, filename): 92 | Logger.LogInfo(f'Simulation: Reading file {filename}.') 93 | if exists(filename): 94 | return file_interface.read_tum_trajectory_file(filename) 95 | else: 96 | Logger.LogError(f'Simulation: File does not exist!') 97 | return None 98 | 99 | def convert_to_traj(self, trajectory): 100 | k_ns_per_s = 1e9 101 | ts = trajectory[:, 0] / k_ns_per_s 102 | xyz = trajectory[:, 1:4] 103 | wxyz = trajectory[:, 4:8] 104 | return PoseTrajectory3D(positions_xyz=xyz, orientations_quat_wxyz=wxyz, timestamps=ts) 105 | 106 | def read_npy_file(self, filename): 107 | Logger.LogInfo(f'Simulation: Reading file {filename}.') 108 | if exists(filename): 109 | return self.convert_to_traj(np.load(filename)) 110 | else: 111 | Logger.LogError(f'Simulation: File does not exist!') 112 | return None 113 | 114 | def write_odometry(self, robot_traj, ros2_bag_out): 115 | 116 | from rosbags.typesys.types import ( 117 | nav_msgs__msg__Odometry as Odometry, 118 | geometry_msgs__msg__PoseWithCovariance as PoseWithCovariance, 119 | geometry_msgs__msg__TwistWithCovariance as TwistWithCovariance, 120 | geometry_msgs__msg__Twist as Twist, 121 | geometry_msgs__msg__Vector3 as Vector3, 122 | std_msgs__msg__Header as Header, 123 | geometry_msgs__msg__Pose as Pose, 124 | geometry_msgs__msg__Point as Position, 125 | geometry_msgs__msg__Quaternion as Quaternion, 126 | builtin_interfaces__msg__Time as Time) 127 | 128 | msg_type = Odometry.__msgtype__ 129 | connection = ros2_bag_out.add_connection(self.odom_topic, msg_type) 130 | 131 | Logger.LogDebug( 132 | f'Writing odometry to bag file to: {self.out_bag_file}') 133 | for stamp, xyz, quat in zip(robot_traj.timestamps, robot_traj.positions_xyz, 134 | robot_traj.orientations_quat_wxyz): 135 | sec = int(stamp // 1) 136 | nanosec = int((stamp - sec) * 1e9) 137 | ros_time = Time(sec, nanosec) 138 | header = Header(ros_time, self.map_frame) 139 | position = Position(x=xyz[0], y=xyz[1], z=xyz[2]) 140 | quaternion = Quaternion(w=quat[0], x=quat[1], y=quat[2], z=quat[3]) 141 | pose = Pose(position, quaternion) 142 | pose_with_cov = PoseWithCovariance(pose, np.array([0.0] * 36)) 143 | vec = Vector3(0.0, 0.0, 0.0) 144 | twist = Twist(vec, vec) 145 | twist_with_cov = TwistWithCovariance(twist, np.array([0.0] * 36)) 146 | odom = Odometry(header, self.child_frame, 147 | pose_with_cov, twist_with_cov) 148 | 149 | serialized_msg = serialize_cdr(odom, msg_type) 150 | ros2_bag_out.write(connection, int(stamp * 1e9), serialized_msg) 151 | Logger.LogInfo(f'Simulation: Wrote topic: {self.odom_topic}') 152 | 153 | Logger.LogInfo('Simulation: Writing odometry to bag file done.') 154 | return ros_time 155 | 156 | def write_server_trajectory(self, server_traj, ros2_bag_out): 157 | TRAJECTORY_MSG = """ 158 | std_msgs/Header header 159 | maplab_msgs/TrajectoryNode[] nodes 160 | """ 161 | TRAJECTORY_NODE_MSG = """ 162 | string robot_name 163 | int64 id 164 | geometry_msgs/PoseStamped pose 165 | float32 signal 166 | """ 167 | 168 | register_types(get_types_from_msg( 169 | TRAJECTORY_NODE_MSG, 'maplab_msgs/msg/TrajectoryNode')) 170 | register_types(get_types_from_msg( 171 | TRAJECTORY_MSG, 'maplab_msgs/msg/Trajectory')) 172 | 173 | from rosbags.typesys.types import (maplab_msgs__msg__TrajectoryNode as TrajectoryNode, 174 | maplab_msgs__msg__Trajectory as Trajectory, 175 | geometry_msgs__msg__Pose as Pose, 176 | geometry_msgs__msg__PoseStamped as PoseStamped, 177 | geometry_msgs__msg__Point as Position, 178 | geometry_msgs__msg__Quaternion as Quaternion, 179 | std_msgs__msg__Header as Header, 180 | builtin_interfaces__msg__Time as Time) 181 | 182 | msg_type = Trajectory.__msgtype__ 183 | connection = ros2_bag_out.add_connection(self.monitor_topic, msg_type) 184 | 185 | Logger.LogDebug( 186 | f'Writing server trajectory to bag file to: {self.out_bag_file}') 187 | i = 0 188 | k = 0 189 | update_every_n_poses = 10 190 | nodes = [] 191 | n_poses = len(server_traj.timestamps) 192 | last_pos = np.array([]) 193 | last_idx = n_poses - 1 194 | dist = 0.0 195 | Logger.LogDebug( 196 | f'Using a threshold of {self.graph_threshold_dist}m for distance.') 197 | for stamp, xyz, quat in zip(server_traj.timestamps, server_traj.positions_xyz, 198 | server_traj.orientations_quat_wxyz): 199 | i += 1 200 | if i > 1 and i != last_idx: 201 | dist += np.linalg.norm(xyz - last_pos) 202 | if self.graph_threshold_dist > 0.0 and dist < self.graph_threshold_dist: 203 | continue 204 | 205 | dist = 0.0 206 | last_pos = np.copy(xyz) 207 | sec = int(stamp // 1) 208 | nanosec = int((stamp - sec) * 1e9) 209 | ros_time = Time(sec, nanosec) 210 | header = Header(ros_time, self.map_frame) 211 | 212 | position = Position(x=xyz[0], y=xyz[1], z=xyz[2]) 213 | quaternion = Quaternion(w=quat[0], x=quat[1], y=quat[2], z=quat[3]) 214 | pose = Pose(position, quaternion) 215 | pose_stamped = PoseStamped(header, pose) 216 | 217 | node = TrajectoryNode(self.robot_name, k, pose_stamped, 0.0) 218 | nodes.append(node) 219 | 220 | if i == 1 or i % update_every_n_poses == 0 or i == last_idx: 221 | k = k + 1 222 | traj_msg = Trajectory(header, nodes) 223 | serialized_msg = serialize_cdr(traj_msg, msg_type) 224 | ros2_bag_out.write(connection, int( 225 | stamp * 1e9), serialized_msg) 226 | 227 | for i in range(0, 10): 228 | ros2_bag_out.write(connection, int( 229 | (stamp+i*20) * 1e9), serialized_msg) 230 | Logger.LogInfo( 231 | f'Simulation: Wrote monitor topic {self.monitor_topic} to the bag.') 232 | 233 | Logger.LogInfo( 234 | 'Simulation: Writing server trajectory to bag file done.') 235 | 236 | return ros_time 237 | 238 | def write_delayed_msg(self, ros_time_odom, ros_time_traj, ros2_bag_out): 239 | from rosbags.typesys.types import (std_msgs__msg__Header as Header, 240 | std_msgs__msg__String as String, 241 | builtin_interfaces__msg__Time as Time) 242 | if ros_time_odom is None: 243 | max_time_s = ros_time_traj.sec 244 | elif ros_time_traj is None: 245 | max_time_s = ros_time_odom.sec 246 | else: 247 | max_time_s = max(ros_time_odom.sec, ros_time_traj.sec) 248 | 249 | delay_s = 600 250 | 251 | str_msg = String(data=f'Delaying by {delay_s} seconds.') 252 | msg_type = String.__msgtype__ 253 | connection = ros2_bag_out.add_connection('delay', msg_type) 254 | serialized_msg = serialize_cdr(str_msg, msg_type) 255 | 256 | ros2_bag_out.write(connection, int( 257 | (max_time_s + delay_s) * 1e9), serialized_msg) 258 | Logger.LogInfo(f'Simulation: Wrote delayed topic to the bag.') 259 | 260 | 261 | def main(args=None): 262 | rclpy.init(args=args) 263 | server = Simulation() 264 | rclpy.spin(server) 265 | server.destroy_node() 266 | rclpy.shutdown() 267 | 268 | 269 | if __name__ == '__main__': 270 | main() 271 | -------------------------------------------------------------------------------- /src/fgsp/controller/signal_handler.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | 3 | import numpy as np 4 | from liegroups import SE3 5 | from scipy.spatial.transform import Rotation 6 | from maplab_msgs.msg import Trajectory, TrajectoryNode 7 | from geometry_msgs.msg import PoseStamped 8 | from nav_msgs.msg import Path 9 | from builtin_interfaces.msg import Time 10 | 11 | from src.fgsp.common.utils import Utils 12 | from src.fgsp.common.logger import Logger 13 | from src.fgsp.common.comms import Comms 14 | from src.fgsp.common.signal_node import SignalNode 15 | from src.fgsp.common.visualizer import Visualizer 16 | 17 | 18 | class SignalHandler(object): 19 | def __init__(self, config): 20 | self.signals = {} 21 | self.config = config 22 | self.comms = Comms() 23 | 24 | def group_robots(self, signals): 25 | n_nodes = len(signals) 26 | grouped_signals = {} 27 | for i in range(0, n_nodes): 28 | robot = signals[i].robot_name 29 | if not robot in grouped_signals.keys(): 30 | grouped_signals[robot] = [] 31 | 32 | grouped_signals[robot].append(signals[i]) 33 | return grouped_signals 34 | 35 | def publish_grouped_robots(self, grouped_signals): 36 | for key, nodes in grouped_signals.items(): 37 | path_msg = Path() 38 | for node in nodes: 39 | path_msg.poses.append(node.pose) 40 | path_msg.header.stamp = self.comms.time_now().to_msg() 41 | path_msg.header.frame_id = 'darpa' 42 | self.comms.publish( 43 | path_msg, Path, f'/graph_monitor/{key}/monitor_path') 44 | 45 | def convert_signal(self, signal_msg): 46 | grouped_signals = self.group_robots(signal_msg.nodes) 47 | self.publish_grouped_robots(grouped_signals) 48 | Logger.LogInfo( 49 | f'SignalHandler: Grouped signals are {grouped_signals.keys()}') 50 | 51 | for key, nodes in grouped_signals.items(): 52 | n_nodes = len(nodes) 53 | Logger.LogWarn( 54 | f'SignalHandler: For {key} we have {n_nodes} nodes.') 55 | if (n_nodes <= 0): 56 | continue 57 | 58 | signals = [None] * n_nodes 59 | for i in range(0, n_nodes): 60 | signals[i] = self.convert_trajectory_node(nodes[i]) 61 | self.signals[key] = signals 62 | 63 | return grouped_signals.keys() 64 | 65 | def convert_signal_from_path(self, path_msg, robot_name): 66 | n_poses = len(path_msg.poses) 67 | if (n_poses <= 0): 68 | return "" 69 | 70 | # key = path_msg.header.frame_id 71 | key = robot_name 72 | signals = [None] * n_poses 73 | for i in range(0, n_poses): 74 | signals[i] = self.convert_path_node(path_msg.poses[i], key) 75 | 76 | self.signals[key] = signals 77 | return key 78 | 79 | def convert_signal_from_poses(self, poses, robot_name): 80 | n_poses = len(poses) 81 | if (n_poses <= 0): 82 | return "" 83 | 84 | key = robot_name 85 | signals = [None] * n_poses 86 | for i in range(0, n_poses): 87 | signals[i] = self.convert_pose(poses[i, :], key) 88 | 89 | self.signals[key] = signals 90 | return key 91 | 92 | def get_all_nodes(self, key): 93 | if key in self.signals: 94 | return self.signals[key] 95 | else: 96 | return [] 97 | 98 | def get_number_of_submaps(self, key): 99 | if key in self.signals: 100 | return self.signals[key][-1].id + 1 101 | else: 102 | return 0 103 | 104 | def get_nodes_for_submap(self, key, id): 105 | nodes = self.get_all_nodes(key) 106 | filtered_nodes = [] 107 | for node in nodes: 108 | if node.id == id: 109 | filtered_nodes.append(node) 110 | return filtered_nodes 111 | 112 | def get_mask_for_submap(self, key, id): 113 | nodes = self.get_all_nodes(key) 114 | n_nodes = len(nodes) 115 | mask = [False] * n_nodes 116 | for i in range(0, n_nodes): 117 | if nodes[i].id == id: 118 | mask[i] = True 119 | return mask 120 | 121 | def get_indices_for_submap(self, key, id): 122 | nodes = self.get_all_nodes(key) 123 | n_nodes = len(nodes) 124 | indices = [] 125 | for i in range(0, n_nodes): 126 | if nodes[i].id == id: 127 | indices.append(i) 128 | return indices 129 | 130 | def convert_trajectory_node(self, node_msg): 131 | id = node_msg.id 132 | robot_name = node_msg.robot_name 133 | pose_msg = node_msg.pose 134 | ts = pose_msg.header.stamp 135 | position = np.array( 136 | [pose_msg.pose.position.x, pose_msg.pose.position.y, pose_msg.pose.position.z]) 137 | orientation = np.array([pose_msg.pose.orientation.w, pose_msg.pose.orientation.x, 138 | pose_msg.pose.orientation.y, pose_msg.pose.orientation.z]) 139 | residual = node_msg.signal 140 | 141 | signal = SignalNode() 142 | signal.init(ts, id, robot_name, position, orientation, residual) 143 | return signal 144 | 145 | def convert_path_node(self, pose_msg, robot_name): 146 | ts = pose_msg.header.stamp 147 | position = np.array( 148 | [pose_msg.pose.position.x, pose_msg.pose.position.y, pose_msg.pose.position.z]) 149 | orientation = np.array([pose_msg.pose.orientation.w, pose_msg.pose.orientation.x, 150 | pose_msg.pose.orientation.y, pose_msg.pose.orientation.z]) 151 | degenerate = pose_msg.header.frame_id.lower() == 'degenerate' 152 | 153 | signal = SignalNode() 154 | signal.init_onboard(ts, robot_name, position, orientation, degenerate) 155 | return signal 156 | 157 | def convert_pose(self, pose, robot_name): 158 | id = int(pose[0]) 159 | 160 | sec = int(pose[1] // 1) 161 | nsec = int((pose[1] - sec) * 1e9) 162 | ts = Time() 163 | ts.sec = sec 164 | ts.nanosec = nsec 165 | 166 | position = pose[2:5] 167 | if pose.shape[0] > 5: 168 | orientation = pose[5:9] 169 | else: 170 | orientation = [0, 0, 0, 1] 171 | degenerate = False 172 | 173 | signal = SignalNode() 174 | signal.init(ts, id, robot_name, position, orientation, 0.0, degenerate) 175 | return signal 176 | 177 | def compute_signal_from_key(self, key): 178 | nodes = self.signals[key] 179 | traj = self.compute_trajectory(nodes) 180 | traj_origin = traj[0, 1:4] 181 | 182 | pos_signal = (traj[:, 1:4] - traj_origin).squeeze() 183 | 184 | x = np.linalg.norm(pos_signal, ord=2, axis=1) 185 | return x 186 | 187 | def compute_r3_signal(self, nodes): 188 | traj = self.compute_trajectory(nodes) 189 | traj_origin = traj[0, 1:4] 190 | 191 | pos_signal = (traj[:, 1:4] - traj_origin).squeeze() 192 | 193 | return np.linalg.norm(pos_signal, ord=2, axis=1) 194 | 195 | def compute_so3_signal(self, nodes): 196 | traj = self.compute_trajectory(nodes) 197 | wxyz = traj[0, 4:8] 198 | traj_origin = Rotation.from_quat( 199 | [wxyz[1], wxyz[2], wxyz[3], wxyz[0]]).as_matrix() 200 | 201 | n_nodes = len(nodes) 202 | x_rot = [0] * n_nodes 203 | for i in range(0, n_nodes): 204 | wxyz = traj[i, 4:8] 205 | rot_diff = np.matmul(traj_origin, Rotation.from_quat( 206 | [wxyz[1], wxyz[2], wxyz[3], wxyz[0]]).as_matrix().transpose()) 207 | x_rot[i] = np.trace(rot_diff) 208 | return np.array(x_rot) 209 | 210 | def compute_signal(self, nodes): 211 | if self.config.construction_method == 'se3': 212 | return self.compute_se3_signal(nodes) 213 | elif self.config.construction_method == 'so3': 214 | return self.compute_so3_signal(nodes) 215 | elif self.config.construction_method == 'r3': 216 | return self.compute_r3_signal(nodes) 217 | else: 218 | Logger.LogError( 219 | f'Unknown construction method: {self.config.construction_method}. Using default SE(3).') 220 | return self.compute_r3_signal(nodes) 221 | 222 | def compute_se3_signal(self, nodes): 223 | traj = self.compute_trajectory(nodes) 224 | T_G_origin = Utils.convert_pos_quat_to_transformation( 225 | traj[0, 1:4], traj[0, 4:8]) 226 | pose_origin = SE3.from_matrix(T_G_origin) 227 | 228 | n_nodes = len(nodes) 229 | x_se3 = [0] * n_nodes 230 | for i in range(0, n_nodes): 231 | T_G = Utils.convert_pos_quat_to_transformation( 232 | traj[i, 1:4], traj[i, 4:8]) 233 | pose_cur = SE3.from_matrix(T_G) 234 | x_se3[i] = self.compute_se3_distance(pose_origin, pose_cur) 235 | return np.array(x_se3) 236 | 237 | def compute_se3_distance(self, pose_lhs, pose_rhs): 238 | Xi_12 = (pose_lhs.inv().dot(pose_rhs)).log() 239 | W = np.eye(4, 4) 240 | W[0, 0] = 10 241 | W[1, 1] = 10 242 | W[2, 2] = 0.001 243 | W[3, 3] = 0.001 244 | 245 | inner = np.trace( 246 | np.matmul(np.matmul(SE3.wedge(Xi_12), W), SE3.wedge(Xi_12).transpose())) 247 | return np.sqrt(inner) 248 | 249 | def compute_trajectory(self, nodes): 250 | n_nodes = len(nodes) 251 | trajectory = np.zeros((n_nodes, 8)) 252 | for i in range(n_nodes): 253 | trajectory[i, 0] = Utils.ros_time_msg_to_ns(nodes[i].ts) 254 | trajectory[i, 1:4] = nodes[i].position 255 | trajectory[i, 4:8] = nodes[i].orientation 256 | 257 | return trajectory 258 | 259 | def to_signal_msg(self, key): 260 | traj_msg = Trajectory() 261 | node_msg = TrajectoryNode() 262 | nodes = self.get_all_nodes(key) 263 | n_nodes = len(nodes) 264 | 265 | for i in range(n_nodes): 266 | node_msg = TrajectoryNode() 267 | node_msg.id = nodes[i].id 268 | node_msg.robot_name = nodes[i].robot_name 269 | 270 | pose_msg = PoseStamped() 271 | pose_msg.header.stamp = nodes[i].ts 272 | pose_msg.pose.position.x = nodes[i].position[0] 273 | pose_msg.pose.position.y = nodes[i].position[1] 274 | pose_msg.pose.position.z = nodes[i].position[2] 275 | pose_msg.pose.orientation.w = nodes[i].orientation[0] 276 | pose_msg.pose.orientation.x = nodes[i].orientation[1] 277 | pose_msg.pose.orientation.y = nodes[i].orientation[2] 278 | pose_msg.pose.orientation.z = nodes[i].orientation[3] 279 | 280 | node_msg.pose = pose_msg 281 | node_msg.signal = nodes[i].residual 282 | traj_msg.nodes.append(node_msg) 283 | 284 | return traj_msg 285 | 286 | def publish(self): 287 | topic_fmt = '/graph_monitor/signal/{key}_trajectory' 288 | color_idx = 0 289 | viz = Visualizer() 290 | for robot, nodes in self.signals.items(): 291 | Logger.LogWarn(f'SignalHandler: Publishing signal for {robot}') 292 | topic = topic_fmt.format(key=robot) 293 | for node in nodes: 294 | viz.add_signal_coordinate(node.position, color_idx) 295 | viz.visualize_signals(topic) 296 | viz.resetConstraintVisualization() 297 | color_idx += 1 298 | 299 | def marginalize_signal(self, signal, indices, n_nodes): 300 | n_indices = len(indices) 301 | if n_indices < 2: 302 | return signal 303 | 304 | # Marginalize all intermediate signals 305 | marginalized = np.zeros(n_indices) 306 | for idx in range(1, n_indices): 307 | for i in range(indices[idx-1], indices[idx]): 308 | marginalized[idx-1] += signal[i] 309 | # marginalized[idx-1] = max(marginalized[idx-1], signal[i]) 310 | 311 | # Marginalize remaining signals 312 | last_reduced_idx = n_indices - 1 313 | for i in range(indices[last_reduced_idx], n_nodes): 314 | marginalized[last_reduced_idx] += signal[i] 315 | # marginalized[last_reduced_idx] = max( 316 | # marginalized[last_reduced_idx], signal[i]) 317 | 318 | return marginalized 319 | 320 | 321 | if __name__ == '__main__': 322 | sh = SignalHandler() 323 | sh.to_signal_msg("foo") 324 | -------------------------------------------------------------------------------- /src/fgsp/graph/global_graph.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | import time 3 | 4 | import numpy as np 5 | from scipy import spatial 6 | from maplab_msgs.msg import Graph 7 | from geometry_msgs.msg import Point 8 | from pygsp import graphs, filters, reduction, utils 9 | from functools import partial 10 | from multiprocessing import Pool 11 | from liegroups import SE3 12 | 13 | from src.fgsp.common.logger import Logger 14 | from src.fgsp.common.utils import Utils 15 | from src.fgsp.common.visualizer import Visualizer 16 | from src.fgsp.graph.base_graph import BaseGraph 17 | from src.fgsp.common.utils import Utils 18 | from src.fgsp.common.visualizer import Visualizer 19 | 20 | 21 | class GlobalGraph(BaseGraph): 22 | def __init__(self, config, reduced=False): 23 | BaseGraph.__init__(self, config) 24 | self.adj = None 25 | self.coords = np.array([]) 26 | self.G = None 27 | self.is_reduced = reduced 28 | self.reduced_ind = [] 29 | self.skip_ind = [] 30 | self.submap_ind = [] 31 | self.graph_seq = -1 32 | self.latest_graph_msg = None 33 | Logger.LogInfo('GlobalGraph: Initialized.') 34 | 35 | def msg_contains_updates(self, graph_msg): 36 | if self.is_built is False: 37 | return True 38 | 39 | if graph_msg.header.frame_id != '': 40 | return int(graph_msg.header.frame_id) > self.graph_seq 41 | else: 42 | return graph_msg.header.seq > self.graph_seq 43 | 44 | def graph_size(self): 45 | if self.G is not None: 46 | return self.G.N 47 | else: 48 | return 0 49 | 50 | def build(self, graph_msg): 51 | start_time = time.time() 52 | self.coords = self.read_coordinates(graph_msg) 53 | Logger.LogDebug( 54 | f'GlobalGraph: Building with coords {self.coords.shape}.') 55 | self.adj = self.read_adjacency(graph_msg) 56 | Logger.LogDebug(f'GlobalGraph: Building with adj: {self.adj.shape}.') 57 | self.submap_ind = self.read_submap_indices(graph_msg) 58 | Logger.LogDebug( 59 | f'GlobalGraph: Building with ind: {len(self.submap_ind)}.') 60 | 61 | self.adj = utils.symmetrize(self.adj, method='average') 62 | if not self.build_graph(): 63 | self.G = None 64 | self.is_built = False 65 | return 66 | 67 | if graph_msg.header.frame_id != "": 68 | self.graph_seq = int(graph_msg.header.frame_id) 69 | else: 70 | self.graph_seq = graph_msg.header.seq 71 | self.is_built = True 72 | execution_time = (time.time() - start_time) 73 | Logger.LogInfo( 74 | f'GlobalGraph: Building complete ({execution_time} sec)') 75 | self.latest_graph_msg = graph_msg 76 | 77 | def build_graph(self): 78 | if len(self.adj.tolist()) == 0: 79 | Logger.LogInfo( 80 | 'GlobalGraph: Path adjacency matrix is empty. Aborting graph building.') 81 | return False 82 | self.G = graphs.Graph(self.adj) 83 | 84 | if self.G.N != self.coords.shape[0]: 85 | Logger.LogError( 86 | f'GlobalGraph: Path graph size is {self.G.N} but coords are {self.coords.shape}') 87 | return False 88 | if self.G.N <= 1: 89 | Logger.LogDebug( 90 | 'GlobalGraph: Path graph vertex count is less than 2.') 91 | return False 92 | 93 | self.G.set_coordinates(self.coords[:, [0, 1]]) 94 | self.G.compute_fourier_basis() 95 | 96 | if (self.is_reduced): 97 | self.reduce_graph() 98 | 99 | self.is_built = True 100 | return True 101 | 102 | def build_graph_from_coords_and_adj(self, coords, adj): 103 | self.coords = coords 104 | self.adj = adj 105 | self.build_graph() 106 | 107 | def build_from_path(self, path_msg): 108 | return self.build_from_pose_msgs(path_msg.poses) 109 | 110 | def build_from_pose_msgs(self, poses): 111 | n_poses = len(poses) 112 | if n_poses <= 0: 113 | Logger.LogError('GlobalGraph: Received empty path message.') 114 | return 115 | poses = self.read_coordinates_from_poses(poses) 116 | self.coords = poses[:, 0:3] 117 | Logger.LogDebug( 118 | f'GlobalGraph: Building with coords {self.coords.shape}.') 119 | self.adj = self.create_adjacency_from_poses(poses) 120 | Logger.LogDebug(f'GlobalGraph: Building with adj: {self.adj.shape}.') 121 | return self.build_graph() 122 | 123 | def build_from_poses(self, poses): 124 | n_poses = poses.shape[0] 125 | if n_poses <= 0: 126 | Logger.LogError('GlobalGraph: Received empty path message.') 127 | return 128 | self.coords = poses 129 | Logger.LogDebug( 130 | f'GlobalGraph Building with coords {self.coords.shape}.') 131 | self.adj = self.create_adjacency_from_poses(poses) 132 | Logger.LogDebug(f'GlobalGraph: Building with adj: {self.adj.shape}.') 133 | return self.build_graph() 134 | 135 | def skip_jumping_coords(self, prev_coords, next_coords): 136 | n_coords_to_check = len(prev_coords) 137 | skip_indices = [] 138 | for i in range(0, n_coords_to_check): 139 | diff = np.linalg.norm(prev_coords[i, :] - next_coords[i, :]) 140 | if diff < 0.5: 141 | continue 142 | next_coords = np.delete(next_coords, i, 0) 143 | skip_indices.append(i) 144 | 145 | return next_coords, skip_indices 146 | 147 | def has_skipped(self): 148 | return len(self.skip_ind) > 0 149 | 150 | def read_coordinates(self, graph_msg): 151 | n_coords = len(graph_msg.coords) 152 | coords = np.zeros((n_coords, 3)) 153 | for i in range(n_coords): 154 | coords[i, 0] = graph_msg.coords[i].x 155 | coords[i, 1] = graph_msg.coords[i].y 156 | coords[i, 2] = graph_msg.coords[i].z 157 | 158 | return coords 159 | 160 | def read_coordinates_from_poses(self, poses): 161 | n_coords = len(poses) 162 | coords = np.zeros((n_coords, 8)) 163 | for i in range(0, n_coords): 164 | coords[i, 0] = poses[i].pose.position.x 165 | coords[i, 1] = poses[i].pose.position.y 166 | coords[i, 2] = poses[i].pose.position.z 167 | coords[i, 3] = poses[i].pose.orientation.x 168 | coords[i, 4] = poses[i].pose.orientation.y 169 | coords[i, 5] = poses[i].pose.orientation.z 170 | coords[i, 6] = poses[i].pose.orientation.w 171 | coords[i, 7] = Utils.ros_time_msg_to_ns(poses[i].pose.header.stamp) 172 | return coords 173 | 174 | def read_adjacency(self, graph_msg): 175 | n_coords = len(graph_msg.coords) 176 | adj = np.zeros((n_coords, n_coords)) 177 | for i in range(n_coords): 178 | for j in range(n_coords): 179 | adj[i, j] = graph_msg.adjacency_matrix[j + i * n_coords] 180 | 181 | return adj 182 | 183 | def get_graph(self): 184 | return self.G 185 | 186 | def get_coords(self): 187 | return self.coords 188 | 189 | def compute_temporal_decay(self, timestmap_lhs, timestamp_rhs): 190 | ts_diff_s = Utils.ts_ns_to_seconds( 191 | np.absolute(timestmap_lhs - timestamp_rhs)) 192 | alpha = 1.5 193 | beta = 1000 194 | return alpha * np.exp(-ts_diff_s / beta) 195 | 196 | def read_submap_indices(self, graph_msg): 197 | return graph_msg.submap_indices 198 | 199 | def reduce_graph(self): 200 | if self.config.reduction_method == 'every_other': 201 | self.reduced_ind = self.reduce_every_other(self.coords) 202 | elif self.config.reduction_method == 'positive_ev': 203 | self.reduced_ind = self.reduce_largest_ev_positive( 204 | self.G.N, self.G) 205 | elif self.config.reduction_method == 'negative_ev': 206 | self.reduced_ind = self.reduce_largest_ev_negative( 207 | self.G.N, self.G) 208 | elif self.config.reduction_method == 'largest_ev': 209 | take_n = int(round(self.config.reduce_to_n_percent * self.G.N)) 210 | if take_n >= self.G.N: 211 | Logger.LogWarn( 212 | 'GlobalGraph: Requested reduction amount is equal or greater than the graph size.') 213 | print(take_n) 214 | print(self.G.N) 215 | return 216 | self.reduced_ind = self.reduce_largest_ev(take_n) 217 | else: 218 | Logger.LogError( 219 | f'GlobalGraph: Unknown graph reduction method: {self.config.reduction_method}. Aborting reduction.') 220 | return 221 | self.reduce_graph_using_indices(self.reduced_ind) 222 | 223 | def reduce_graph_using_indices(self, reduced_ind): 224 | Logger.LogInfo( 225 | f'GlobalGraph: Reducing graph using {len(reduced_ind)}/{self.G.N} indices.') 226 | self.coords = self.coords[reduced_ind] 227 | self.G = reduction.kron_reduction(self.G, reduced_ind) 228 | self.adj = self.G.W.toarray() 229 | 230 | # TODO(lbern): check why kron results in some negative weights sometimes. 231 | # self.adj[self.adj < 0] = 0 232 | # self.G = graphs.Graph(self.adj) 233 | 234 | assert np.all(self.adj >= 0) 235 | self.G.compute_fourier_basis() 236 | 237 | def to_graph_msg(self): 238 | graph_msg = Graph() 239 | graph_msg.header.seq = self.graph_seq 240 | graph_msg.header.frame_id = str(self.graph_seq) 241 | n_coords = self.G.N 242 | 243 | # Write coordinates and adjacency. 244 | graph_msg.coords = [None] * n_coords 245 | graph_msg.adjacency_matrix = [0] * (n_coords*n_coords) 246 | for i in range(0, n_coords): 247 | graph_msg.coords[i] = Point() 248 | graph_msg.coords[i].x = self.coords[i, 0] 249 | graph_msg.coords[i].y = self.coords[i, 1] 250 | graph_msg.coords[i].z = self.coords[i, 2] 251 | for j in range(0, n_coords): 252 | graph_msg.adjacency_matrix[j + i * n_coords] = self.adj[i, j] 253 | 254 | graph_msg.submap_indices = self.submap_ind 255 | graph_msg.reduced_indices = self.reduced_ind 256 | 257 | return graph_msg 258 | 259 | def write_graph_to_disk(self, coords_file, adj_file): 260 | np.save(coords_file, self.coords) 261 | np.save(adj_file, self.adj) 262 | 263 | def publish(self): 264 | if not self.is_built: 265 | return 266 | viz = Visualizer(self.config) 267 | 268 | n_coords = self.G.N 269 | if n_coords > self.coords.shape[0] or n_coords > self.adj.shape[0]: 270 | Logger.LogError( 271 | f'Size mismatch in global graph {n_coords} vs. {self.coords.shape[0]} vs. {self.adj.shape[0]}.') 272 | return 273 | 274 | # Publish the coordinates of the global graph along with the adjacency matrix 275 | for i in range(0, n_coords): 276 | pt_h_i = np.ones((4, 1), dtype=np.float32) 277 | pt_h_i[0:3, 0] = self.coords[i, 0:3] 278 | pt_i = np.dot(self.config.T_robot_server, pt_h_i) 279 | viz.add_graph_coordinate(pt_i) 280 | 281 | for j in range(0, n_coords): 282 | pt_h_j = np.ones((4, 1), dtype=np.float32) 283 | pt_h_j[0:3, 0] = self.coords[j, 0:3] 284 | pt_j = np.dot(self.config.T_robot_server, pt_h_j) 285 | if i >= n_coords or j >= self.coords.shape[0]: 286 | continue 287 | if i >= self.adj.shape[0] or j >= self.adj.shape[1]: 288 | continue 289 | if self.adj[i, j] <= 0.0: 290 | continue 291 | 292 | viz.add_graph_adjacency(pt_i, pt_j) 293 | viz.visualize_coords() 294 | viz.visualize_adjacency() 295 | 296 | Logger.LogInfo('GlobalGraph: Visualized global graph.') 297 | 298 | def compute_dirichlet_ratio(self, x1, x2): 299 | e1 = self.global_graph.G.dirichlet_energy(x1) 300 | e2 = self.global_graph.G.dirichlet_energy(x2) 301 | return e1 / e2 302 | 303 | def compute_total_variation_ratio(self, x1, x2): 304 | e1 = self.global_graph.G.total_variation_energy(x1) 305 | e2 = self.global_graph.G.total_variation_energy(x2) 306 | return e1 / e2 307 | 308 | def compute_total_variation(self, x): 309 | energy = 0 310 | p = 2 311 | largest_ev = np.amax(np.abs(self.G.U)) 312 | adj_norm = 1/largest_ev * self.adj 313 | for i in range(0, self.G.N): 314 | adj_variation = 0 315 | for j in range(0, self.G.N): 316 | adj_variation += adj_norm[i, j] * x[j] 317 | energy += np.abs(x[i] - adj_variation) ** p 318 | return 1/p * energy 319 | 320 | def compute_average_local_variation(self, x1, x2): 321 | energy = 0 322 | largest_ev = np.amax(np.abs(self.G.U)) 323 | adj_norm = 1/largest_ev * self.adj 324 | for i in range(0, self.G.N): 325 | adj_variation = 0 326 | for j in range(0, self.G.N): 327 | adj_variation += adj_norm[i, j] * np.abs(x1[j] - x2[j]) 328 | energy += np.abs((x1[i] - x2[i]) - adj_variation) 329 | return energy / self.G.N 330 | -------------------------------------------------------------------------------- /src/fgsp/common/config.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | 3 | import numpy as np 4 | from src.fgsp.common.logger import Logger 5 | 6 | 7 | class BaseConfig(object): 8 | def __init__(self, node): 9 | self.node = node 10 | 11 | def try_get_param(self, key, default=None): 12 | Logger.LogDebug('[BaseConfig] try_get_param: {key} with default {default}'.format( 13 | key=key, default=default)) 14 | self.node.declare_parameter(key, default) 15 | return self.node.get_parameter(key).value 16 | 17 | 18 | class MonitorConfig(BaseConfig): 19 | def __init__(self, node): 20 | super().__init__(node) 21 | 22 | # general config 23 | self.rate = 0.1 24 | self.enable_graph_building = True 25 | self.min_node_count = 10 26 | self.submap_min_count = 3 27 | self.send_separate_traj_msgs = True 28 | 29 | # Reduction settings. 30 | self.reduce_global_graph = False 31 | self.reduction_method = 'positive_ev' 32 | self.reduce_to_n_percent = 1.0 33 | 34 | # submap constraints 35 | self.pivot_distance = 20.0 36 | self.min_pivot_distance = 20.0 37 | self.n_nearest_neighbors = 50 38 | self.p_norm = 2 39 | self.compute_poses_in_LiDAR = False 40 | 41 | # input 42 | self.in_graph_topic = '/maplab_server/sparse_graph/graph' 43 | self.in_traj_opt_topic = '/maplab_server/sparse_graph/trajectory' 44 | self.verification_service_topic = '/grap_monitor/verification' 45 | self.opt_pc_topic = '/maplab_server/sparse_graph/submap' 46 | 47 | # output 48 | self.out_graph_topic = '/graph_monitor/sparse_graph/graph' 49 | self.out_traj_opt_topic = '/graph_monitor/sparse_graph/trajectory' 50 | self.accumulated_map_topic = '/graph_monitor/map' 51 | 52 | def init_from_config(self): 53 | # general config 54 | self.rate = self.try_get_param("update_rate", self.rate) 55 | self.enable_graph_building = self.try_get_param( 56 | "enable_graph_building", self.enable_graph_building) 57 | self.min_node_count = self.try_get_param( 58 | "min_node_count", self.min_node_count) 59 | self.send_separate_traj_msgs = self.try_get_param( 60 | "send_separate_traj_msgs", self.send_separate_traj_msgs) 61 | 62 | # Reduction settings. 63 | self.reduce_global_graph = self.try_get_param( 64 | "reduce_global_graph", self.reduce_global_graph) 65 | self.reduction_method = self.try_get_param( 66 | "reduction_method", self.reduction_method) 67 | self.reduce_to_n_percent = self.try_get_param( 68 | "reduce_to_n_percent", self.reduce_to_n_percent) 69 | 70 | # input 71 | self.in_graph_topic = self.try_get_param( 72 | "in_graph_topic", self.in_graph_topic) 73 | self.in_traj_opt_topic = self.try_get_param( 74 | "in_traj_opt_topic", self.in_traj_opt_topic) 75 | self.verification_service_topic = self.try_get_param( 76 | "verification_service", self.verification_service_topic) 77 | self.opt_pc_topic = self.try_get_param( 78 | "opt_pc_topic", self.opt_pc_topic) 79 | 80 | # output 81 | self.out_graph_topic = self.try_get_param( 82 | "out_graph_topic", self.out_graph_topic) 83 | self.out_traj_opt_topic = self.try_get_param( 84 | "out_traj_opt_topic", self.out_traj_opt_topic) 85 | self.accumulated_map_topic = self.try_get_param( 86 | "accumulated_map_topic", self.accumulated_map_topic) 87 | 88 | 89 | class ClientConfig(BaseConfig): 90 | def __init__(self, node=None): 91 | super().__init__(node) 92 | 93 | # general config 94 | self.rate = 0.5 95 | self.dataroot = '/home/berlukas/Documents/workspace/fgsp_ws/src/fgsp' 96 | self.robot_name = 'cerberus' 97 | self.enable_client_update = True 98 | self.enable_anchor_constraints = False 99 | self.enable_relative_constraints = False 100 | self.enable_signal_recording = False 101 | self.enable_trajectory_recording = False 102 | self.signal_export_path = "/data/{key}_{src}_signal.npy" 103 | self.graph_coords_export_path = "/data/{key}_{src}_graph_coords.npy" 104 | self.graph_adj_export_path = "/data/{key}_{src}_graph_adj.npy" 105 | self.trajectory_export_path = "/data/{key}_{src}_trajectory.npy" 106 | self.trajectory_raw_export_path = "/data/{key}_{src}_raw_trajectory.npy" 107 | self.label_output_path = "/data/opt_labels.dat" 108 | self.connections_output_path = "/data/opt_connections.dat" 109 | self.degenerate_window = 10 110 | self.synchronization_max_diff_s = 1.0 111 | self.verbosity = 1 112 | self.warmup_nodes = 10 113 | self.max_iterations = -1 114 | 115 | # constraint construction 116 | self.client_mode = 'multiscale' 117 | self.wavelet_scales = 6 118 | self.classifier = 'top' 119 | self.top_classifier_select_n = 10 120 | self.top_classifier_min_threshold = 0.1 121 | self.large_scale_partition_method = 'id' 122 | self.large_scale_anchor = False 123 | self.n_hop_mid_constraints = 10 124 | self.min_dist_large_constraints = 5.0 125 | self.min_dist_along_graph_large_constraints = 20.0 126 | self.max_lookup_dist_large_constraints = 50.0 127 | self.nn_neighbors = 3 128 | self.stop_method = 'none' 129 | self.stop_threshold = 0.0 130 | 131 | # Graph construction 132 | self.construction_method = 'se3' 133 | self.use_graph_hierarchies = True 134 | self.max_graph_levels = 2 135 | self.use_downstreaming = False 136 | self.graph_hierarchies_node_threshold = 100 137 | self.use_parallel_construction = True 138 | self.visualize_graph = False 139 | 140 | # input 141 | self.opt_graph_topic = "/graph_monitor/sparse_graph/graph" 142 | self.opt_traj_topic = "/graph_monitor/sparse_graph/trajectory" 143 | self.est_traj_topic = "/trajectory" 144 | self.est_traj_path_topic = "/incremental_trajectory" 145 | 146 | # output 147 | self.anchor_node_topic = "/graph_client/anchor_nodes" 148 | self.relative_node_topic = "/graph_client/relative_nodes" 149 | self.intra_constraint_topic = "/graph_client/intra_constraints" 150 | 151 | # input and output 152 | self.client_update_topic = "/graph_client/latest_graph" 153 | self.T_robot_server = np.eye(4).reshape(16).tolist() 154 | 155 | def init_from_config(self): 156 | # general config 157 | self.rate = self.try_get_param("update_rate", self.rate) 158 | self.dataroot = self.try_get_param("dataroot", self.dataroot) 159 | self.robot_name = self.try_get_param("robot_name", self.robot_name) 160 | self.enable_client_update = self.try_get_param( 161 | "enable_client_update", self.enable_client_update) 162 | self.enable_anchor_constraints = self.try_get_param( 163 | "enable_anchor_constraints", self.enable_anchor_constraints) 164 | self.enable_relative_constraints = self.try_get_param( 165 | "enable_relative_constraints", self.enable_relative_constraints) 166 | self.enable_signal_recording = self.try_get_param( 167 | "enable_signal_recording", self.enable_signal_recording) 168 | self.enable_trajectory_recording = self.try_get_param( 169 | "enable_trajectory_recording", self.enable_trajectory_recording) 170 | self.signal_export_path = self.try_get_param( 171 | "signal_export_path", self.signal_export_path) 172 | self.graph_coords_export_path = self.try_get_param( 173 | "graph_coords_export_path", self.graph_coords_export_path) 174 | self.graph_adj_export_path = self.try_get_param( 175 | "graph_adj_export_path", self.graph_adj_export_path) 176 | self.trajectory_export_path = self.try_get_param( 177 | "trajectory_export_path", self.trajectory_export_path) 178 | self.trajectory_raw_export_path = self.try_get_param( 179 | "trajectory_raw_export_path", self.trajectory_raw_export_path) 180 | self.label_output_path = self.try_get_param( 181 | "label_output_path", self.label_output_path) 182 | self.connections_output_path = self.try_get_param( 183 | "connections_output_path", self.connections_output_path) 184 | 185 | self.degenerate_window = self.try_get_param( 186 | "degenerate_window", self.degenerate_window) 187 | self.synchronization_max_diff_s = self.try_get_param( 188 | "synchronization_max_diff_s", self.synchronization_max_diff_s) 189 | self.verbosity = self.try_get_param("verbosity", self.verbosity) 190 | self.warmup_nodes = self.try_get_param( 191 | "warmup_nodes", self.warmup_nodes) 192 | self.max_iterations = self.try_get_param( 193 | "max_iterations", self.max_iterations) 194 | 195 | # constraint construction 196 | self.client_mode = self.try_get_param("client_mode", self.client_mode) 197 | self.wavelet_scales = self.try_get_param( 198 | "wavelet_scales", self.wavelet_scales) 199 | self.classifier = self.try_get_param("classifier", self.classifier) 200 | self.top_classifier_select_n = self.try_get_param( 201 | "top_classifier_select_n", self.top_classifier_select_n) 202 | self.top_classifier_min_threshold = self.try_get_param( 203 | "top_classifier_min_threshold", self.top_classifier_min_threshold) 204 | self.large_scale_partition_method = self.try_get_param( 205 | "large_scale_partition_method", self.large_scale_partition_method) 206 | self.large_scale_anchor = self.try_get_param( 207 | "large_scale_anchor", self.large_scale_anchor) 208 | self.n_hop_mid_constraints = self.try_get_param( 209 | "n_hop_mid_constraints", self.n_hop_mid_constraints) 210 | self.min_dist_large_constraints = self.try_get_param( 211 | "min_dist_large_constraints", self.min_dist_large_constraints) 212 | self.min_dist_along_graph_large_constraints = self.try_get_param( 213 | "min_dist_along_graph_large_constraints", self.min_dist_along_graph_large_constraints) 214 | self.max_lookup_dist_large_constraints = self.try_get_param( 215 | "max_lookup_dist_large_constraints", self.max_lookup_dist_large_constraints) 216 | self.nn_neighbors = self.try_get_param( 217 | "nn_neighbors", self.nn_neighbors) 218 | self.stop_method = self.try_get_param("stop_method", self.stop_method) 219 | self.stop_threshold = self.try_get_param( 220 | "stop_threshold", self.stop_threshold) 221 | 222 | # Graph construction 223 | self.construction_method = self.try_get_param( 224 | "construction_method", self.construction_method) 225 | self.use_graph_hierarchies = self.try_get_param( 226 | "use_graph_hierarchies", self.use_graph_hierarchies) 227 | self.max_graph_levels = self.try_get_param( 228 | "max_graph_levels", self.max_graph_levels) 229 | self.use_downstreaming = self.try_get_param( 230 | "use_downstreaming", self.use_downstreaming) 231 | self.graph_hierarchies_node_threshold = self.try_get_param( 232 | "graph_hierarchies_node_threshold", self.graph_hierarchies_node_threshold) 233 | self.use_parallel_construction = self.try_get_param( 234 | "use_parallel_construction", self.use_parallel_construction) 235 | self.visualize_graph = self.try_get_param( 236 | "visualize_graph", self.visualize_graph) 237 | 238 | # input 239 | self.opt_graph_topic = self.try_get_param( 240 | "opt_graph_topic", self.opt_graph_topic) 241 | self.opt_traj_topic = self.try_get_param( 242 | "opt_traj_topic", self.opt_traj_topic) 243 | self.est_traj_topic = self.try_get_param( 244 | "est_traj_topic", self.est_traj_topic) 245 | self.est_traj_path_topic = self.try_get_param( 246 | "est_traj_path_topic", self.est_traj_path_topic) 247 | 248 | # output 249 | self.anchor_node_topic = self.try_get_param( 250 | "anchor_node_topic", self.anchor_node_topic) 251 | self.relative_node_topic = self.try_get_param( 252 | "relative_node_topic", self.relative_node_topic) 253 | self.intra_constraint_topic = self.try_get_param( 254 | "intra_constraints", self.intra_constraint_topic) 255 | 256 | # input and output 257 | self.client_update_topic = self.try_get_param( 258 | "client_update_topic", self.client_update_topic) 259 | self.T_robot_server = np.array(self.try_get_param( 260 | "T_robot_server", self.T_robot_server)).reshape(4, 4) 261 | 262 | 263 | class DebugConfig(BaseConfig): 264 | def __init__(self): 265 | # Reduction settings. 266 | self.reduce_global_graph = True 267 | self.reduction_method = 'largest_ev' 268 | # self.reduction_method = 'every_other' 269 | self.reduce_to_n_percent = 0.4 270 | 271 | 272 | if __name__ == '__main__': 273 | from plotter import Plotter 274 | 275 | # cfg = MonitorConfig() 276 | # Plotter.PrintMonitorConfig(cfg) 277 | 278 | cfg = ClientConfig() 279 | Plotter.PrintClientConfig(cfg) 280 | -------------------------------------------------------------------------------- /src/fgsp/classifier/classification_result.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | 3 | from more_itertools import partition 4 | import numpy as np 5 | from nav_msgs.msg import Path 6 | from geometry_msgs.msg import PoseStamped 7 | from scipy.spatial.transform import Rotation 8 | from scipy import spatial 9 | 10 | from src.fgsp.common.utils import Utils 11 | from src.fgsp.common.logger import Logger 12 | from src.fgsp.common.transform_history import TransformHistory, ConstraintType 13 | 14 | 15 | class ClassificationResult(object): 16 | def __init__(self, config, robot_name, opt_nodes, features, labels): 17 | self.config = config 18 | self.robot_name = robot_name 19 | self.opt_nodes = opt_nodes 20 | self.n_nodes = len(opt_nodes) 21 | self.features = features 22 | self.history = None 23 | self.partitions = self.partition_nodes( 24 | self.config.large_scale_partition_method) 25 | self.ts_partitions = self.get_ts_from_nodes(self.partitions) 26 | self.labels, self.n_labels = self.check_and_fix_labels(labels) 27 | print(f'ClassificationResults: Got {self.n_labels} labels') 28 | 29 | def check_and_fix_labels(self, labels): 30 | n_nodes = len(labels) 31 | n_labels = 0 32 | for i in range(0, n_nodes): 33 | if labels[i] is None: 34 | labels[i] = [] 35 | else: 36 | n_labels += len(labels[i]) 37 | return labels, n_labels 38 | 39 | def partition_nodes(self, method='nth'): 40 | if method == 'nth': 41 | return self.take_every_nth_node() 42 | if method == 'id': 43 | return self.take_nodes_by_id() 44 | else: 45 | Logger.LogWarn(f'Unknown partiion method {method} specified') 46 | return [] 47 | 48 | def get_ts_from_nodes(self, nodes): 49 | ts = [] 50 | for node_idx in nodes: 51 | node = self.opt_nodes[node_idx] 52 | ts.append(Utils.ros_time_msg_to_ns(node.ts)) 53 | return np.array(ts) 54 | 55 | def take_every_nth_node(self): 56 | n_steps = 15 57 | prev = 0 58 | partitioned_nodes = [] 59 | partitions = np.arange(prev, self.n_nodes, n_steps) 60 | for cur in partitions: 61 | pivot = (cur + prev) // 2 62 | partitioned_nodes.append(pivot) 63 | prev = cur 64 | return np.array(partitioned_nodes) 65 | 66 | def take_nodes_by_id(self): 67 | partitioned_nodes = [] 68 | prev = 0 69 | prev_id = 0 70 | for i in range(0, self.n_nodes): 71 | cur_id = self.opt_nodes[i].id 72 | if cur_id != prev_id: 73 | pivot = (i + prev) // 2 74 | partitioned_nodes.append(pivot) 75 | prev_id = cur_id 76 | prev = i 77 | 78 | if prev_id > 0: 79 | pivot = (self.n_nodes + prev) // 2 80 | partitioned_nodes.append(pivot) 81 | 82 | return np.array(partitioned_nodes) 83 | 84 | def size(self): 85 | return len(self.opt_nodes) 86 | 87 | def check_and_construct_constraint_at(self, idx): 88 | local_labels = self.labels[idx] 89 | if local_labels is None or len(local_labels) == 0: 90 | return None, 0, 0, 0 91 | 92 | if self.history != None and idx in self.history.keys(): 93 | transform_history = self.history[idx] 94 | else: 95 | transform_history = TransformHistory() 96 | 97 | cur_opt = self.opt_nodes[idx] 98 | relative_constraint = Path() 99 | relative_constraint.header.stamp = cur_opt.ts 100 | 101 | small_relative_counter = 0 102 | mid_relative_counter = 0 103 | large_relative_counter = 0 104 | 105 | if 3 in local_labels: 106 | relative_constraint, transform_history, n_added = self.construct_small_area_constraint( 107 | idx, relative_constraint, transform_history) 108 | small_relative_counter = n_added 109 | if 2 in local_labels: 110 | relative_constraint, transform_history, n_added = self.construct_mid_area_constraint( 111 | idx, relative_constraint, transform_history) 112 | mid_relative_counter = n_added 113 | if 1 in local_labels: 114 | print(f'We have large area constraint at {idx}') 115 | relative_constraint, transform_history, n_added = self.construct_large_area_constraint( 116 | idx, relative_constraint, transform_history) 117 | large_relative_counter = n_added 118 | 119 | if len(relative_constraint.poses) == 0: 120 | return None, 0, 0, 0 121 | 122 | if self.history != None: 123 | self.history[idx] = transform_history 124 | 125 | return relative_constraint, small_relative_counter, mid_relative_counter, large_relative_counter 126 | 127 | def construct_large_area_constraint(self, idx, relative_constraint, history): 128 | cur_opt = self.opt_nodes[idx] 129 | if self.config.large_scale_partition_method == 'nth': 130 | cur_submap_idx = self.lookup_closest_submap(cur_opt) 131 | else: 132 | cur_submap_idx = cur_opt.id 133 | counter = 0 134 | 135 | submap_partitions = self.lookup_spatially_close_submaps(cur_submap_idx) 136 | for target_idx in submap_partitions: 137 | T_a_b = self.compute_relative_distance( 138 | cur_opt, self.opt_nodes[target_idx]) 139 | if history.has_different_transform(target_idx, T_a_b): 140 | pose_msg = self.create_pose_msg( 141 | self.opt_nodes[target_idx], T_a_b) 142 | relative_constraint.poses.append(pose_msg) 143 | history.add_record(target_idx, T_a_b, ConstraintType.LARGE) 144 | counter = counter + 1 145 | 146 | if self.config.large_scale_anchor and len(submap_partitions) > 0 and idx != 0: 147 | target_idx = 1 148 | T_a_b = self.compute_relative_distance( 149 | cur_opt, self.opt_nodes[target_idx]) 150 | if history.has_different_transform(target_idx, T_a_b): 151 | pose_msg = self.create_pose_msg( 152 | self.opt_nodes[target_idx], T_a_b) 153 | relative_constraint.poses.append(pose_msg) 154 | history.add_record(target_idx, T_a_b, ConstraintType.LARGE) 155 | counter = counter + 1 156 | 157 | return relative_constraint, history, counter 158 | 159 | def lookup_closest_submap(self, cur_opt): 160 | ts_diff = np.absolute(self.ts_partitions - 161 | Utils.ros_time_msg_to_ns(cur_opt.ts)) 162 | ts_min = np.amin(ts_diff) 163 | 164 | # Retrieve the index in opt_nodes. 165 | partition_idx = np.where(ts_diff == ts_min)[0][0] 166 | return self.partitions[partition_idx] 167 | 168 | def lookup_spatially_close_submaps(self, submap_idx): 169 | submap_positions = [ 170 | self.opt_nodes[i].position for i in self.partitions] 171 | if len(submap_positions) == 0: 172 | return [] 173 | dists_along_graph = self.compute_distances_along_graph( 174 | submap_positions) 175 | tree = spatial.KDTree(submap_positions) 176 | _, nn_indices = self.query_tree( 177 | submap_idx, tree, self.config.nn_neighbors, 178 | dists_along_graph, self.config.min_dist_along_graph_large_constraints, 179 | 2, self.config.max_lookup_dist_large_constraints) 180 | return self.partitions[nn_indices] 181 | 182 | def compute_distances_along_graph(self, positions): 183 | global_distances = np.linalg.norm(positions, axis=1) 184 | relative_distances = np.abs(np.diff(global_distances)) 185 | distance_along_graph = np.cumsum( 186 | np.insert(relative_distances, 0, global_distances[0])) 187 | return distance_along_graph 188 | 189 | def query_tree(self, cur_id, tree, n_neighbors, dists_along_graph, min_dist_along_graph, p_norm=2, dist=50): 190 | if cur_id >= len(self.partitions): 191 | return [], [] 192 | 193 | cur_position = self.opt_nodes[self.partitions[cur_id]].position 194 | nn_dists, nn_indices = tree.query( 195 | cur_position, 196 | p=p_norm, 197 | k=n_neighbors, 198 | distance_upper_bound=dist) 199 | 200 | # Remove self and fix output. 201 | nn_dists, nn_indices = Utils.fix_nn_output( 202 | n_neighbors, cur_id, nn_dists, nn_indices) 203 | mask_dists = nn_dists >= self.config.min_dist_large_constraints 204 | mask_dists_along_graph = dists_along_graph[nn_indices] >= min_dist_along_graph 205 | mask = np.logical_or(mask_dists, mask_dists_along_graph) 206 | return nn_dists[mask], nn_indices[mask] 207 | 208 | def construct_mid_area_constraint(self, idx, relative_constraint, history): 209 | cur_opt = self.opt_nodes[idx] 210 | counter = 0 211 | n_hop = self.config.n_hop_mid_constraints 212 | lower = idx - n_hop 213 | upper = idx + n_hop 214 | if lower >= 0: 215 | T_a_b = self.compute_relative_distance( 216 | cur_opt, self.opt_nodes[lower]) 217 | if history.has_different_transform(lower, T_a_b): 218 | pose_msg = self.create_pose_msg(self.opt_nodes[lower], T_a_b) 219 | relative_constraint.poses.append(pose_msg) 220 | history.add_record(lower, T_a_b, ConstraintType.MID) 221 | counter = counter + 1 222 | if upper < self.n_nodes: 223 | T_a_b = self.compute_relative_distance( 224 | cur_opt, self.opt_nodes[upper]) 225 | if history.has_different_transform(upper, T_a_b): 226 | pose_msg = self.create_pose_msg(self.opt_nodes[upper], T_a_b) 227 | relative_constraint.poses.append(pose_msg) 228 | history.add_record(upper, T_a_b, ConstraintType.MID) 229 | counter = counter + 1 230 | return relative_constraint, history, counter 231 | 232 | def construct_small_area_constraint(self, idx, relative_constraint, history): 233 | if self.n_nodes <= 1: 234 | return relative_constraint, history, 0 235 | cur_opt = self.opt_nodes[idx] 236 | counter = 0 237 | if idx - 1 >= 0: 238 | T_a_b = self.compute_relative_distance( 239 | cur_opt, self.opt_nodes[idx - 1]) 240 | if history.has_different_transform(idx - 1, T_a_b): 241 | pose_msg = self.create_pose_msg(self.opt_nodes[idx - 1], T_a_b) 242 | relative_constraint.poses.append(pose_msg) 243 | history.add_record(idx - 1, T_a_b, ConstraintType.SMALL) 244 | counter = counter + 1 245 | if idx - 2 >= 0: 246 | T_a_b = self.compute_relative_distance( 247 | cur_opt, self.opt_nodes[idx - 2]) 248 | if history.has_different_transform(idx - 2, T_a_b): 249 | pose_msg = self.create_pose_msg(self.opt_nodes[idx - 2], T_a_b) 250 | relative_constraint.poses.append(pose_msg) 251 | history.add_record(idx - 2, T_a_b, ConstraintType.SMALL) 252 | counter = counter + 1 253 | if idx + 1 < self.n_nodes: 254 | T_a_b = self.compute_relative_distance( 255 | cur_opt, self.opt_nodes[idx + 1]) 256 | if history.has_different_transform(idx + 1, T_a_b): 257 | pose_msg = self.create_pose_msg(self.opt_nodes[idx + 1], T_a_b) 258 | relative_constraint.poses.append(pose_msg) 259 | history.add_record(idx + 1, T_a_b, ConstraintType.SMALL) 260 | counter = counter + 1 261 | if idx + 2 < self.n_nodes: 262 | T_a_b = self.compute_relative_distance( 263 | cur_opt, self.opt_nodes[idx + 2]) 264 | if history.has_different_transform(idx + 2, T_a_b): 265 | pose_msg = self.create_pose_msg(self.opt_nodes[idx + 2], T_a_b) 266 | relative_constraint.poses.append(pose_msg) 267 | history.add_record(idx + 2, T_a_b, ConstraintType.SMALL) 268 | counter = counter + 1 269 | return relative_constraint, history, counter 270 | 271 | def create_pose_msg_from_node(self, cur_opt): 272 | pose_msg = PoseStamped() 273 | pose_msg.header.stamp = cur_opt.ts 274 | pose_msg.pose.position.x = cur_opt.position[0] 275 | pose_msg.pose.position.y = cur_opt.position[1] 276 | pose_msg.pose.position.z = cur_opt.position[2] 277 | pose_msg.pose.orientation.w = cur_opt.orientation[0] 278 | pose_msg.pose.orientation.x = cur_opt.orientation[1] 279 | pose_msg.pose.orientation.y = cur_opt.orientation[2] 280 | pose_msg.pose.orientation.z = cur_opt.orientation[3] 281 | return pose_msg 282 | 283 | def compute_relative_distance(self, opt_node_from, opt_node_to): 284 | T_G_B_a = self.create_transformation_from_node(opt_node_from) 285 | T_G_B_b = self.create_transformation_from_node(opt_node_to) 286 | T_a_b = np.matmul(np.linalg.inv(T_G_B_a), T_G_B_b) 287 | return T_a_b 288 | 289 | def create_pose_msg(self, opt_node_to, T_a_b): 290 | t_a_b, q_a_b = self.convert_transform(T_a_b) 291 | 292 | pose_msg = PoseStamped() 293 | pose_msg.header.stamp = opt_node_to.ts 294 | pose_msg.pose.position.x = t_a_b[0] 295 | pose_msg.pose.position.y = t_a_b[1] 296 | pose_msg.pose.position.z = t_a_b[2] 297 | pose_msg.pose.orientation.x = q_a_b[0] 298 | pose_msg.pose.orientation.y = q_a_b[1] 299 | pose_msg.pose.orientation.z = q_a_b[2] 300 | pose_msg.pose.orientation.w = q_a_b[3] 301 | return pose_msg 302 | 303 | def convert_transform(self, T_a_b): 304 | pos = T_a_b[0:3, 3] 305 | R = T_a_b[0:3, 0:3] 306 | return pos, Rotation.from_matrix(R).as_quat() # x, y, z, w 307 | 308 | def create_transformation_from_node(self, node): 309 | pose_msg = self.create_pose_msg_from_node(node) 310 | pos, orien = Utils.convert_pose_stamped_msg_to_array(pose_msg) 311 | return Utils.convert_pos_quat_to_transformation(pos, orien) 312 | -------------------------------------------------------------------------------- /src/fgsp/graph_client.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | import os 3 | 4 | import rclpy 5 | from rclpy.node import Node 6 | import numpy as np 7 | from nav_msgs.msg import Path 8 | from maplab_msgs.msg import Graph, Trajectory, SubmapConstraint 9 | from multiprocessing import Lock 10 | 11 | from src.fgsp.graph.wavelet_evaluator import WaveletEvaluator 12 | from src.fgsp.graph.global_graph import GlobalGraph 13 | from src.fgsp.graph.hierarchical_graph import HierarchicalGraph 14 | from src.fgsp.controller.signal_handler import SignalHandler 15 | from src.fgsp.controller.command_post import CommandPost 16 | from src.fgsp.common.signal_synchronizer import SignalSynchronizer 17 | from src.fgsp.common.config import ClientConfig 18 | from src.fgsp.common.plotter import Plotter 19 | from src.fgsp.common.utils import Utils 20 | from src.fgsp.common.comms import Comms 21 | from src.fgsp.common.logger import Logger 22 | from src.fgsp.classifier.top_classifier import TopClassifier 23 | from src.fgsp.classifier.simple_classifier import SimpleClassifier 24 | from src.fgsp.classifier.classification_result import ClassificationResult 25 | from src.fgsp.classifier.downstream_result import DownstreamResult 26 | from src.fgsp.classifier.windowed_result import WindowedResult 27 | 28 | 29 | class GraphClient(Node): 30 | def __init__(self): 31 | super().__init__('graph_client') 32 | self.comms = Comms() 33 | self.comms.node = self 34 | 35 | self.is_initialized = False 36 | self.initialize_logging = True 37 | self.is_updating = False 38 | self.n_iterations = 0 39 | self.config = ClientConfig(self) 40 | self.config.init_from_config() 41 | Plotter.PlotClientBanner() 42 | Plotter.PrintClientConfig(self.config) 43 | Plotter.PrintSeparator() 44 | Logger.Verbosity = self.config.verbosity 45 | 46 | self.mutex = Lock() 47 | self.constraint_mutex = Lock() 48 | self.mutex.acquire() 49 | 50 | # Subscriber and publisher 51 | self.graph_sub = self.create_subscription( 52 | Graph, self.config.opt_graph_topic, self.global_graph_callback, 10) 53 | self.opt_traj_sub = self.create_subscription( 54 | Trajectory, self.config.opt_traj_topic, self.traj_opt_callback, 10) 55 | self.est_traj_sub = self.create_subscription( 56 | Trajectory, self.config.est_traj_topic, self.traj_callback, 10) 57 | self.est_traj_path_sub = self.create_subscription( 58 | Path, self.config.est_traj_path_topic, self.traj_path_callback, 10) 59 | self.intra_constraint_pub = self.create_publisher( 60 | Path, self.config.intra_constraint_topic, 20) 61 | 62 | # Handlers and evaluators. 63 | if self.config.use_graph_hierarchies: 64 | self.global_graph = HierarchicalGraph(self.config) 65 | else: 66 | self.global_graph = GlobalGraph(self.config, reduced=False) 67 | 68 | self.latest_traj_msg = None 69 | self.signal = SignalHandler(self.config) 70 | self.optimized_signal = SignalHandler(self.config) 71 | self.synchronizer = SignalSynchronizer(self.config) 72 | self.eval = WaveletEvaluator(self.config.wavelet_scales) 73 | self.commander = CommandPost(self.config) 74 | 75 | if self.config.classifier == 'top': 76 | self.classifier = TopClassifier(self.config) 77 | elif self.config.classifier == 'simple': 78 | self.classifier = SimpleClassifier() 79 | else: 80 | Logger.LogError( 81 | f'Unknown classifier type: {self.config.classifier}') 82 | self.mutex.release() 83 | self.destroy_node() 84 | rclpy.shutdown() 85 | return 86 | 87 | # Key management to keep track of the received messages. 88 | self.optimized_keys = [] 89 | self.keys = [] 90 | 91 | self.mutex.release() 92 | self.is_initialized = True 93 | self.timer = self.create_timer(1 / self.config.rate, self.update) 94 | 95 | def create_data_export_folder(self): 96 | if not self.config.enable_signal_recording and not self.config.enable_trajectory_recording: 97 | return 98 | cur_ts = Utils.ros_time_to_ns(self.get_clock().now()) 99 | export_folder = self.config.dataroot + '/data/' + \ 100 | self.config.robot_name + '_%d' % np.float32(cur_ts) 101 | Logger.LogWarn( 102 | f'GraphClient: Setting up dataroot folder to {export_folder}') 103 | if not os.path.exists(export_folder): 104 | os.mkdir(export_folder) 105 | os.mkdir(export_folder + '/data') 106 | self.config.dataroot = export_folder 107 | 108 | def global_graph_callback(self, msg): 109 | Logger.LogInfo( 110 | f'GraphClient: Received graph message from monitor {msg.header.frame_id}.') 111 | if not (self.is_initialized and (self.config.enable_anchor_constraints or self.config.enable_relative_constraints)): 112 | return 113 | self.mutex.acquire() 114 | 115 | # We only trigger the graph building if the msg contains new information. 116 | if self.global_graph.msg_contains_updates(msg) and self.config.client_mode == 'multiscale': 117 | self.global_graph.build(msg) 118 | self.record_signal_for_key(np.array([0]), 'opt') 119 | self.eval.compute_wavelets(self.global_graph.get_graph()) 120 | 121 | self.mutex.release() 122 | 123 | def traj_opt_callback(self, msg): 124 | if not (self.is_initialized and (self.config.enable_anchor_constraints or self.config.enable_relative_constraints)): 125 | Logger.LogError('GraphClient: Dropped incoming opt message.') 126 | return 127 | 128 | keys = self.optimized_signal.convert_signal(msg) 129 | Logger.LogInfo( 130 | f'GraphClient: Received opt trajectory message from {keys}.') 131 | 132 | for key in keys: 133 | if self.key_in_optimized_keys(key): 134 | continue 135 | self.optimized_keys.append(key) 136 | 137 | def traj_callback(self, msg): 138 | if self.is_initialized is False: 139 | return 140 | 141 | key = self.signal.convert_signal(msg) 142 | if self.key_in_keys(key): 143 | return 144 | self.keys.append(key) 145 | Logger.LogInfo( 146 | f'GraphClient: Received est trajectory message from {key}.') 147 | 148 | def traj_path_callback(self, msg): 149 | if not (self.is_initialized and (self.config.enable_anchor_constraints or self.config.enable_relative_constraints)): 150 | return 151 | self.latest_traj_msg = msg 152 | 153 | def process_latest_robot_data(self): 154 | if self.latest_traj_msg == None: 155 | return False 156 | 157 | key = self.signal.convert_signal_from_path( 158 | self.latest_traj_msg, self.config.robot_name) 159 | if not key: 160 | Logger.LogError('GraphClient: Unable to convert msg to signal.') 161 | return False 162 | 163 | if self.key_in_keys(key): 164 | return True 165 | self.keys.append(key) 166 | return True 167 | 168 | def update(self): 169 | if (self.initialize_logging): 170 | self.create_data_export_folder() 171 | self.initialize_logging = False 172 | 173 | self.mutex.acquire() 174 | if self.is_updating: 175 | self.mutex.release() 176 | return 177 | 178 | n_opt_nodes = len(self.optimized_signal.get_all_nodes(self.config.robot_name)) if self.key_in_optimized_keys( 179 | self.config.robot_name) else 0 180 | if n_opt_nodes < self.config.warmup_nodes: 181 | self.mutex.release() 182 | Logger.LogWarn( 183 | f'GraphClient: Not enough nodes in the graph ({n_opt_nodes}/{self.config.warmup_nodes}).') 184 | return 185 | if self.config.max_iterations > 0 and self.n_iterations >= self.config.max_iterations: 186 | self.mutex.release() 187 | Logger.LogWarn( 188 | f'GraphClient: Reached max number of iterations ({self.n_iterations}/{self.config.max_iterations}).') 189 | return 190 | self.is_updating = True 191 | self.mutex.release() 192 | 193 | Logger.LogInfo('GraphClient: Updating...') 194 | self.commander.reset_msgs() 195 | # self.update_degenerate_anchors() 196 | 197 | if not self.process_latest_robot_data(): 198 | self.mutex.acquire() 199 | self.is_updating = False 200 | self.mutex.release() 201 | Logger.LogWarn('GraphClient: No new data received.') 202 | return 203 | 204 | self.compare_estimations() 205 | if self.config.visualize_graph: 206 | self.global_graph.publish() 207 | 208 | n_constraints = self.commander.get_total_amount_of_constraints() 209 | if n_constraints > 0: 210 | Logger.LogInfo( 211 | f'GraphClient: Updating completed (sent {n_constraints} constraints)') 212 | Logger.LogInfo( 213 | f'GraphClient: In detail relatives: {self.commander.small_constraint_counter} / {self.commander.mid_constraint_counter} / {self.commander.large_constraint_counter}') 214 | Logger.LogInfo( 215 | f'GraphClient: In detail anchors: {self.commander.anchor_constraint_counter}') 216 | self.n_iterations += 1 217 | self.mutex.acquire() 218 | self.is_updating = False 219 | self.mutex.release() 220 | 221 | def record_all_signals(self, x_est, x_opt): 222 | if not self.config.enable_signal_recording: 223 | return 224 | self.record_signal_for_key(x_est, 'est') 225 | self.record_signal_for_key(x_opt, 'opt') 226 | 227 | def record_raw_est_trajectory(self, traj): 228 | filename = self.config.dataroot + \ 229 | self.config.trajectory_raw_export_path.format(src='est') 230 | 231 | np.save(filename, traj) 232 | 233 | def record_synchronized_trajectories(self, traj_est, traj_opt): 234 | if not self.config.enable_trajectory_recording: 235 | return 236 | self.record_traj_for_key(traj_est, 'est') 237 | self.record_traj_for_key(traj_opt, 'opt') 238 | 239 | def record_signal_for_key(self, x, src): 240 | signal_file = self.config.dataroot + \ 241 | self.config.signal_export_path.format(src=src) 242 | np.save(signal_file, x) 243 | graph_coords_file = self.config.dataroot + \ 244 | self.config.graph_coords_export_path.format(src=src) 245 | graph_adj_file = self.config.dataroot + \ 246 | self.config.graph_adj_export_path.format(src=src) 247 | if src == 'opt': 248 | self.global_graph.write_graph_to_disk( 249 | graph_coords_file, graph_adj_file) 250 | Logger.LogWarn( 251 | f'GraphClient: for {src} we have {x.shape} and {self.global_graph.get_coords().shape}') 252 | 253 | def record_traj_for_key(self, traj, src): 254 | filename = self.config.dataroot + \ 255 | self.config.trajectory_export_path.format(src=src) 256 | np.save(filename, traj) 257 | 258 | def record_features(self, features): 259 | filename = self.config.dataroot + "/data/features.npy" 260 | np.save(filename, features) 261 | 262 | def compare_estimations(self): 263 | if not self.config.enable_relative_constraints: 264 | return 265 | Logger.LogInfo('GraphClient: Comparing estimations.') 266 | 267 | # Check whether we have an optimized version of it. 268 | if self.key_in_optimized_keys(self.config.robot_name): 269 | self.compare_stored_signals(self.config.robot_name) 270 | else: 271 | Logger.LogWarn( 272 | f'GraphClient: Found no optimized version of {self.config.robot_name} for comparison.') 273 | 274 | def publish_client_update(self): 275 | if not (self.config.enable_anchor_constraints and self.global_graph.is_built and self.config.enable_client_update): 276 | return 277 | self.mutex.acquire() 278 | graph_msg = self.global_graph.latest_graph_msg 279 | if graph_msg is not None: 280 | self.client_update_pub.publish(graph_msg) 281 | self.mutex.release() 282 | 283 | def compare_stored_signals(self, key): 284 | Logger.LogWarn(f'GraphClient: Comparing signals for {key}.') 285 | # Retrieve the estimated and optimized versions of the trajectory. 286 | all_est_nodes = self.signal.get_all_nodes(key) 287 | all_opt_nodes = self.optimized_signal.get_all_nodes(key) 288 | n_opt_nodes = len(all_opt_nodes) 289 | print( 290 | f'We have {n_opt_nodes} opt nodes and {len(all_est_nodes)} est nodes.') 291 | 292 | # Compute the features and publish the results. 293 | # This evaluates per node the scale of the difference 294 | # and creates a relative constraint accordingly. 295 | self.record_raw_est_trajectory( 296 | self.signal.compute_trajectory(all_est_nodes)) 297 | all_opt_nodes, all_est_nodes = self.reduce_and_synchronize( 298 | all_opt_nodes, all_est_nodes) 299 | if all_opt_nodes is None or all_est_nodes is None: 300 | Logger.LogError('GraphClient: Synchronization failed.') 301 | return False 302 | 303 | labels = self.compute_all_labels(key, all_opt_nodes, all_est_nodes) 304 | self.evaluate_and_publish_features(labels) 305 | 306 | # Check if we the robot identified a degeneracy in its state. 307 | # Publish an anchor node curing the affected areas. 308 | self.check_for_degeneracy(all_opt_nodes, all_est_nodes) 309 | 310 | return True 311 | 312 | def reduce_and_synchronize(self, all_opt_nodes, all_est_nodes): 313 | (all_opt_nodes, all_est_nodes, opt_idx, 314 | est_idx) = self.synchronizer.synchronize(all_opt_nodes, all_est_nodes) 315 | n_nodes = len(all_est_nodes) 316 | assert(n_nodes == len(all_opt_nodes)) 317 | assert(len(est_idx) == len(opt_idx)) 318 | if n_nodes == 0: 319 | Logger.LogWarn('GraphClient: Could not synchronize nodes.') 320 | return (None, None) 321 | 322 | # Reduce the robot graph and compute the wavelet basis functions. 323 | positions = np.array([np.array(x.position) for x in all_est_nodes]) 324 | orientations = np.array([np.array(x.orientation) 325 | for x in all_est_nodes]) 326 | timestamps = np.array( 327 | [np.array(Utils.ros_time_msg_to_ns(x.ts)) for x in all_est_nodes]) 328 | poses = np.column_stack([positions, orientations, timestamps]) 329 | 330 | # TODO(lbern): fix this temporary test 331 | # Due to the reduction we rebuild here. 332 | positions = np.array([np.array(x.position) for x in all_opt_nodes]) 333 | orientations = np.array([np.array(x.orientation) 334 | for x in all_opt_nodes]) 335 | timestamps = np.array( 336 | [np.array(Utils.ros_time_msg_to_ns(x.ts)) for x in all_opt_nodes]) 337 | global_poses = np.column_stack([positions, orientations, timestamps]) 338 | self.global_graph.build_from_poses(global_poses) 339 | 340 | if self.config.use_graph_hierarchies: 341 | self.global_graph.build_hierarchies() 342 | 343 | if self.config.client_mode == 'multiscale': 344 | self.eval.compute_wavelets(self.global_graph.get_graph()) 345 | return (all_opt_nodes, all_est_nodes) 346 | 347 | def check_for_degeneracy(self, all_opt_nodes, all_est_nodes): 348 | if not self.config.enable_anchor_constraints: 349 | return 350 | Logger.LogInfo('GraphClient: Checking for degeneracy.') 351 | n_nodes = len(all_opt_nodes) 352 | assert n_nodes == len(all_est_nodes) 353 | for i in range(0, n_nodes): 354 | if not all_est_nodes[i].degenerate: 355 | continue 356 | pivot = self.config.degenerate_window // 2 357 | begin_send = max(i - pivot, 0) 358 | end_send = min( 359 | i + (self.config.degenerate_window - pivot), n_nodes) 360 | Logger.LogInfo( 361 | f'GraphClient: Sending degenerate anchros from {begin_send} to {end_send}') 362 | self.commander.send_anchors(all_opt_nodes, begin_send, end_send) 363 | 364 | def update_degenerate_anchors(self): 365 | all_opt_nodes = self.optimized_signal.get_all_nodes( 366 | self.config.robot_name) 367 | if len(all_opt_nodes) == 0: 368 | Logger.LogError( 369 | f'[GraphClient] Robot {self.config.robot_name} does not have any optimized nodes yet.') 370 | return 371 | self.commander.update_degenerate_anchors(all_opt_nodes) 372 | 373 | def compute_all_labels(self, key, all_opt_nodes, all_est_nodes): 374 | if self.config.client_mode == 'multiscale': 375 | return self.perform_multiscale_evaluation(key, all_opt_nodes, all_est_nodes) 376 | elif self.config.client_mode == 'euclidean': 377 | return self.perform_euclidean_evaluation(key, all_opt_nodes, all_est_nodes) 378 | elif self.config.client_mode == 'always': 379 | return self.perform_relative(key, all_opt_nodes, all_est_nodes) 380 | elif self.config.client_mode == 'absolute': 381 | return self.perform_absolute(key, all_opt_nodes, all_est_nodes) 382 | else: 383 | Logger.LogError( 384 | f'GraphClient: Unknown mode specified {self.config.client_mode}') 385 | return None 386 | 387 | def perform_multiscale_evaluation(self, key, all_opt_nodes, all_est_nodes): 388 | # Compute the signal using the synchronized estimated nodes. 389 | x_est = self.signal.compute_signal(all_est_nodes) 390 | x_opt = self.optimized_signal.compute_signal(all_opt_nodes) 391 | 392 | if self.config.use_graph_hierarchies: 393 | indices = self.global_graph.get_indices() 394 | # all_est_nodes = [all_est_nodes[i] for i in robot_indices] 395 | # all_opt_nodes = [all_opt_nodes[i] for i in server_indices] 396 | n_nodes = len(all_est_nodes) 397 | x_est = self.signal.marginalize_signal(x_est, indices, n_nodes) 398 | x_opt = self.signal.marginalize_signal(x_opt, indices, n_nodes) 399 | 400 | self.record_all_signals(x_est, x_opt) 401 | self.record_synchronized_trajectories(self.signal.compute_trajectory( 402 | all_est_nodes), self.optimized_signal.compute_trajectory(all_opt_nodes)) 403 | 404 | if self.config.stop_method == 'dirichlet': 405 | dirichlet_ratio = self.global_graph.compute_dirichlet_ratio( 406 | x_est, x_opt) 407 | if dirichlet_ratio <= self.config.stop_threshold: 408 | return None 409 | 410 | if self.config.stop_method == 'tv': 411 | tv_ratio = self.global_graph.compute_total_variation_ratio( 412 | x_est, x_opt) 413 | if tv_ratio <= self.config.stop_threshold: 414 | return None 415 | 416 | if self.config.stop_method == 'alv': 417 | alv = self.global_graph.compute_average_local_variation( 418 | x_opt, x_est) 419 | if alv <= self.config.stop_threshold: 420 | Logger.LogError('---- STOPPING --------------------') 421 | return None 422 | 423 | psi = self.eval.get_wavelets() 424 | n_dim = psi.shape[0] 425 | if n_dim != x_est.shape[0] or n_dim != x_opt.shape[0]: 426 | Logger.LogWarn( 427 | f'GraphClient We have a size mismatch: {n_dim} vs. {x_est.shape[0]} vs. {x_opt.shape[0]}.') 428 | return None 429 | 430 | Logger.LogInfo('Computing features.') 431 | # Compute all the wavelet coefficients. 432 | # We will filter them later per submap. 433 | W_est = self.eval.compute_wavelet_coeffs(x_est) 434 | W_opt = self.eval.compute_wavelet_coeffs(x_opt) 435 | features = self.eval.compute_features(W_opt, W_est) 436 | self.record_features(features) 437 | 438 | labels = self.classifier.classify(features) 439 | if self.config.use_graph_hierarchies: 440 | if self.config.use_downstreaming: 441 | return DownstreamResult(self.config, key, all_opt_nodes, features, labels, self.global_graph.get_indices()) 442 | else: 443 | return WindowedResult(self.config, key, all_opt_nodes, all_est_nodes, features, labels, self.global_graph) 444 | else: 445 | return ClassificationResult(self.config, key, all_opt_nodes, features, labels) 446 | 447 | def perform_euclidean_evaluation(self, key, all_opt_nodes, all_est_nodes): 448 | est_traj = self.optimized_signal.compute_trajectory(all_opt_nodes) 449 | opt_traj = self.signal.compute_trajectory(all_est_nodes) 450 | euclidean_dist = np.linalg.norm( 451 | est_traj[:, 1:4] - opt_traj[:, 1:4], axis=1) 452 | n_nodes = est_traj.shape[0] 453 | labels = [[0]] * n_nodes 454 | for i in range(0, n_nodes): 455 | if euclidean_dist[i] > 1.0: 456 | labels[i].append(1) 457 | return ClassificationResult(self.config, key, all_opt_nodes, euclidean_dist, labels) 458 | 459 | def perform_relative(self, key, all_opt_nodes, all_est_nodes): 460 | return self.set_label_for_all_nodes(1, key, all_opt_nodes, all_est_nodes) 461 | 462 | def perform_absolute(self, key, all_opt_nodes, all_est_nodes): 463 | n_all_nodes = len(all_opt_nodes) 464 | self.commander.send_anchors(all_opt_nodes, 0, n_all_nodes) 465 | return [] 466 | # return self.set_label_for_all_nodes(5, key, all_opt_nodes, all_est_nodes) 467 | 468 | def set_label_for_all_nodes(self, label, key, all_opt_nodes, all_est_nodes): 469 | n_nodes = len(all_opt_nodes) 470 | labels = [[label]] * n_nodes 471 | return ClassificationResult(self.config, key, all_opt_nodes, None, labels) 472 | 473 | def evaluate_and_publish_features(self, labels): 474 | if labels == None or labels == [] or labels.size() == 0: 475 | Logger.LogError('[GraphClient] No labels found.') 476 | return 477 | self.commander.evaluate_labels_per_node(labels) 478 | 479 | def key_in_optimized_keys(self, key): 480 | return any(key in k for k in self.optimized_keys) 481 | 482 | def key_in_keys(self, key): 483 | return any(key in k for k in self.keys) 484 | 485 | 486 | def main(args=None): 487 | rclpy.init(args=args) 488 | client = GraphClient() 489 | rclpy.spin(client) 490 | client.destroy_node() 491 | rclpy.shutdown() 492 | 493 | 494 | if __name__ == '__main__': 495 | main() 496 | --------------------------------------------------------------------------------