From dac2f729e0261f62ed59cd7cd0c1ee63c1ef7248 Mon Sep 17 00:00:00 2001 From: Julian Nubert Date: Mon, 24 Jun 2024 21:29:38 +0200 Subject: [PATCH] latest running version of Graph-MSF for tutorial and summer school --- .../smb_msf_graph/plotting_node.py | 388 +++++++++--------- .../tutorial/config/1/core_graph_params.yaml | 60 --- ...ph_params.yaml => 1_smb_graph_params.yaml} | 6 + .../tutorial/config/2/core_graph_params.yaml | 60 --- ...ph_params.yaml => 2_smb_graph_params.yaml} | 9 + .../tutorial/config/3/core_graph_params.yaml | 60 --- ...ph_params.yaml => 3_smb_graph_params.yaml} | 10 + smb_msf_graph/tutorial/launch/plotting.launch | 3 +- .../tutorial/launch/tutorial_1.launch | 19 +- .../tutorial/launch/tutorial_2.launch | 20 +- .../tutorial/launch/tutorial_3.launch | 20 +- smb_msf_graph/tutorial/rss_plots/README.md | 2 + 12 files changed, 261 insertions(+), 396 deletions(-) delete mode 100644 smb_msf_graph/tutorial/config/1/core_graph_params.yaml rename smb_msf_graph/tutorial/config/{1/smb_graph_params.yaml => 1_smb_graph_params.yaml} (77%) delete mode 100644 smb_msf_graph/tutorial/config/2/core_graph_params.yaml rename smb_msf_graph/tutorial/config/{2/smb_graph_params.yaml => 2_smb_graph_params.yaml} (56%) delete mode 100644 smb_msf_graph/tutorial/config/3/core_graph_params.yaml rename smb_msf_graph/tutorial/config/{3/smb_graph_params.yaml => 3_smb_graph_params.yaml} (67%) create mode 100644 smb_msf_graph/tutorial/rss_plots/README.md diff --git a/smb_msf_graph/smb_msf_graph/smb_msf_graph/plotting_node.py b/smb_msf_graph/smb_msf_graph/smb_msf_graph/plotting_node.py index c315371..21105e0 100644 --- a/smb_msf_graph/smb_msf_graph/smb_msf_graph/plotting_node.py +++ b/smb_msf_graph/smb_msf_graph/smb_msf_graph/plotting_node.py @@ -1,10 +1,11 @@ #!/usr/bin/env python3 # Global -import numpy as np import matplotlib.pyplot as plt -import rospy +import numpy as np import os +import rospy +import sys import time # Custom @@ -14,198 +15,215 @@ # Name of the experiment experiment_name = "" -def plot_2d(x_coords: list, y_coords: list, x_axis_name: str, y_axis_name: str, plot_name: str): - # Check length - print("Plot: " + plot_name + ". Length of " + x_axis_name + ": " + str(len(x_coords)) + ". Length of " + y_axis_name + ": " + str(len(y_coords))) - # Only plot if there are values - if len(x_coords) > 0: - assert np.abs(len(x_coords) - len(y_coords)) <= 1 - if len(x_coords) - len(y_coords) == 1: - x_coords.pop() # remove last element - elif len(y_coords) - len(x_coords) == 1: - y_coords.pop() - # Home-dir - home_dir = os.path.expanduser('~') - # Plot - plt.rcParams['lines.markersize'] = 0.5 - plt.rcParams["figure.figsize"] = (6,6) - plt.xticks(fontsize=24) - plt.yticks(fontsize=24) - fig, ax = plt.subplots(1,1) - ax.set(xlabel=x_axis_name, ylabel=y_axis_name, title=plot_name) - ax.grid() - plt.tight_layout() - ax.plot(x_coords, y_coords, c="green", linewidth=0.7, alpha=1.0) - fig.savefig(os.path.join(home_dir, "rss_plots", experiment_name + "_" + plot_name + ".png")) + +def plot_2d(x_coords: list, y_coords: list, x_axis_name: str, y_axis_name: str, plot_name: str, log_dir: str): + # Check length + print("Length of " + x_axis_name + ": " + str(len(x_coords)) + ". Length of " + y_axis_name + ": " + str( + len(y_coords))) + # Only plot if there are values + if len(x_coords) > 0: + assert np.abs(len(x_coords) - len(y_coords)) <= 1 + if len(x_coords) - len(y_coords) == 1: + x_coords.pop() # remove last element + elif len(y_coords) - len(x_coords) == 1: + y_coords.pop() + + # Plot + plt.rcParams['lines.markersize'] = 0.5 + plt.rcParams["figure.figsize"] = (6, 6) + plt.xticks(fontsize=24) + plt.yticks(fontsize=24) + fig, ax = plt.subplots(1, 1) + ax.set(xlabel=x_axis_name, ylabel=y_axis_name, title=plot_name) + ax.grid() + plt.tight_layout() + ax.plot(x_coords, y_coords, c="green", linewidth=0.7, alpha=1.0) + fig.savefig(os.path.join(log_dir, experiment_name + "_" + plot_name + ".png")) + class MsfPlotter: - def __init__(self): - - self.state_odometry_topic_name = "/graph_msf/est_odometry_world_imu" - self.wheel_odometry_topic_name = "/control/smb_diff_drive/odom" - self.imu_bias_topic_name = "/graph_msf/accel_bias" - - # ROS - rospy.init_node('GMSF Plotter Node', anonymous=True) - - # Create dir - HOME_DIR = os.path.expanduser('~') - if not os.path.exists(os.path.join(HOME_DIR, "rss_plots")): - os.makedirs(os.path.join(HOME_DIR, "rss_plots")) - - # State Messages - ## Time - self.initial_state_time = [] - self.state_time = [] - ## Position - self.x_pos = [] - self.y_pos = [] - self.z_pos = [] - ## Velocity - self.x_vel = [] - self.y_vel = [] - # IMU Bias Messages - ## Time - self.initial_imu_bias_time = [] - self.imu_bias_time = [] - ## Acc Bias - self.x_acc_bias = [] - self.y_acc_bias = [] - self.z_acc_bias = [] - # Wheel odometry position - ## Time - self.initial_wheel_time = [] - ## Position - self.x_wheel_pos = [] - self.y_wheel_pos = [] - self.z_wheel_pos = [] - - def plot(self): - - # Plot diagrams when destructor is called - print("Saving plots...") - # Position plots ------------------------------------------- - ## x-y-plot - plot_name = "state-position-x-y" - print(plot_name) - x_axis_name = "x[m]" - y_axis_name = "y[m]" - plot_2d(x_coords=self.x_pos, y_coords=self.y_pos, x_axis_name=x_axis_name, y_axis_name=y_axis_name, plot_name=plot_name) - ## z-plot - plot_name = "state-position-z" - print(plot_name) - x_axis_name = "t[s]" - y_axis_name = "z[m]" - plot_2d(x_coords=self.state_time, y_coords=self.z_pos, x_axis_name=x_axis_name, y_axis_name=y_axis_name, plot_name=plot_name) - # Velcoity plots ------------------------------------------- - ## x-velocity - plot_name = "state-velocity-x" - print(plot_name) - x_axis_name = "t[s]" - y_axis_name = "v[m/s]" - plot_2d(x_coords=self.state_time, y_coords=self.x_vel, x_axis_name=x_axis_name, y_axis_name=y_axis_name, plot_name=plot_name) - ## y-velocity - plot_name = "state-velocity-y" - print(plot_name) - x_axis_name = "t[s]" - y_axis_name = "v[m/s]" - plot_2d(x_coords=self.state_time, y_coords=self.y_vel, x_axis_name=x_axis_name, y_axis_name=y_axis_name, plot_name=plot_name) - # Bias plots ----------------------------------------------- - if experiment_name == "1.3" or experiment_name == "2.1" or experiment_name == "2.2" or experiment_name == "3.1" or experiment_name == "3.2": - ## x-Bias - plot_name = "acc-bias-x" - print(plot_name) - x_axis_name = "t[s]" - y_axis_name = "b[m/s^2]" - plot_2d(x_coords=self.imu_bias_time, y_coords=self.x_acc_bias, x_axis_name=x_axis_name, y_axis_name=y_axis_name, plot_name=plot_name) - ## y-Bias - plot_name = "acc-bias-y" - print(plot_name) - x_axis_name = "t[s]" - y_axis_name = "b[m/s^2]" - plot_2d(x_coords=self.imu_bias_time, y_coords=self.y_acc_bias, x_axis_name=x_axis_name, y_axis_name=y_axis_name, plot_name=plot_name) - ## z-Bias - plot_name = "acc-bias-z" - print(plot_name) - x_axis_name = "t[s]" - y_axis_name = "b[m/s^2]" - plot_2d(x_coords=self.imu_bias_time, y_coords=self.z_acc_bias, x_axis_name=x_axis_name, y_axis_name=y_axis_name, plot_name=plot_name) - # Wheel odometry plots ------------------------------------- - if experiment_name == "1.2": - ## x-y-plot - plot_name = "wheel-position-x-y" - print(plot_name) - x_axis_name = "x[m]" - y_axis_name = "y[m]" - plot_2d(x_coords=self.x_wheel_pos, y_coords=self.y_wheel_pos, x_axis_name=x_axis_name, y_axis_name=y_axis_name, plot_name=plot_name) - - - def state_odometry_callback(self, state_estimate): - # Time - if not self.initial_state_time: - self.initial_state_time = state_estimate.header.stamp.to_sec() - print("Received state message.") - return - elif state_estimate.header.stamp.to_sec() - self.initial_state_time > 0.5: - self.state_time = self.state_time + [state_estimate.header.stamp.to_sec() - self.initial_state_time] - # Position - self.x_pos = self.x_pos + [state_estimate.pose.pose.position.x] - self.y_pos = self.y_pos + [state_estimate.pose.pose.position.y] - self.z_pos = self.z_pos + [state_estimate.pose.pose.position.z] - # Velocity - self.x_vel = self.x_vel + [state_estimate.twist.twist.linear.x] - self.y_vel = self.y_vel + [state_estimate.twist.twist.linear.y] - - def wheel_odometry_callback(self, wheel_odometry): - # Time - if not self.initial_wheel_time: - self.initial_wheel_time = wheel_odometry.header.stamp.to_sec() - print("Received wheel message.") - return - elif wheel_odometry.header.stamp.to_sec() - self.initial_wheel_time > 0.5: - self.x_wheel_pos = self.x_wheel_pos + [wheel_odometry.pose.pose.position.x] - self.y_wheel_pos = self.y_wheel_pos + [wheel_odometry.pose.pose.position.y] - self.z_wheel_pos = self.z_wheel_pos + [wheel_odometry.pose.pose.position.z] - - def bias_callback(self, bias_estimate): - # Time - if not self.initial_imu_bias_time: - self.initial_imu_bias_time = bias_estimate.header.stamp.to_sec() - print("Received bias message.") - return - elif bias_estimate.header.stamp.to_sec() - self.initial_imu_bias_time > 0.5: - self.imu_bias_time = self.imu_bias_time + [bias_estimate.header.stamp.to_sec() - self.initial_imu_bias_time] - # Acc. Bias - self.x_acc_bias = self.x_acc_bias + [bias_estimate.vector.x] - self.y_acc_bias = self.y_acc_bias + [bias_estimate.vector.y] - self.z_acc_bias = self.z_acc_bias + [bias_estimate.vector.z] - - def start_plotting(self): - print("======================") - print("Starting plotting...") - print("======================") - - # Odometry Subscriber - rospy.Subscriber(self.state_odometry_topic_name, Odometry, + def __init__(self): + + self.state_odometry_topic_name = "/graph_msf/est_odometry_world_imu" + self.wheel_odometry_topic_name = "/control/smb_diff_drive/odom" + self.imu_bias_topic_name = "/graph_msf/accel_bias" + + # ROS + rospy.init_node('GMSF Plotter Node', anonymous=True) + + # Create dir + HOME_DIR = os.path.expanduser('~') + if not os.path.exists(os.path.join(HOME_DIR, "rss_plots")): + os.makedirs(os.path.join(HOME_DIR, "rss_plots")) + + # State Messages + ## Time + self.initial_state_time = [] + self.state_time = [] + ## Position + self.x_pos = [] + self.y_pos = [] + self.z_pos = [] + ## Velocity + self.x_vel = [] + self.y_vel = [] + # IMU Bias Messages + ## Time + self.initial_imu_bias_time = [] + self.imu_bias_time = [] + ## Acc Bias + self.x_acc_bias = [] + self.y_acc_bias = [] + self.z_acc_bias = [] + # Wheel odometry position + ## Time + self.initial_wheel_time = [] + ## Position + self.x_wheel_pos = [] + self.y_wheel_pos = [] + self.z_wheel_pos = [] + + def plot(self, log_dir: str): + + # Plot diagrams when destructor is called + print(f"Saving plots to {log_dir}...") + # Position plots ------------------------------------------- + ## x-y-plot + plot_name = "state-position-x-y" + print(plot_name) + x_axis_name = "x[m]" + y_axis_name = "y[m]" + plot_2d(x_coords=self.x_pos, y_coords=self.y_pos, x_axis_name=x_axis_name, y_axis_name=y_axis_name, + plot_name=plot_name, log_dir=log_dir) + ## z-plot + plot_name = "state-position-z" + print(plot_name) + x_axis_name = "t[s]" + y_axis_name = "z[m]" + plot_2d(x_coords=self.state_time, y_coords=self.z_pos, x_axis_name=x_axis_name, y_axis_name=y_axis_name, + plot_name=plot_name, log_dir=log_dir) + # Velcoity plots ------------------------------------------- + ## x-velocity + plot_name = "state-velocity-x" + print(plot_name) + x_axis_name = "t[s]" + y_axis_name = "v[m/s]" + plot_2d(x_coords=self.state_time, y_coords=self.x_vel, x_axis_name=x_axis_name, y_axis_name=y_axis_name, + plot_name=plot_name, log_dir=log_dir) + ## y-velocity + plot_name = "state-velocity-y" + print(plot_name) + x_axis_name = "t[s]" + y_axis_name = "v[m/s]" + plot_2d(x_coords=self.state_time, y_coords=self.y_vel, x_axis_name=x_axis_name, y_axis_name=y_axis_name, + plot_name=plot_name, log_dir=log_dir) + # Bias plots ----------------------------------------------- + if experiment_name == "1.3" or experiment_name == "2.1" or experiment_name == "2.2" or experiment_name == "3.1" or experiment_name == "3.2": + ## x-Bias + plot_name = "acc-bias-x" + print(plot_name) + x_axis_name = "t[s]" + y_axis_name = "b[m/s^2]" + plot_2d(x_coords=self.imu_bias_time, y_coords=self.x_acc_bias, x_axis_name=x_axis_name, y_axis_name=y_axis_name, + plot_name=plot_name, log_dir=log_dir) + ## y-Bias + plot_name = "acc-bias-y" + print(plot_name) + x_axis_name = "t[s]" + y_axis_name = "b[m/s^2]" + plot_2d(x_coords=self.imu_bias_time, y_coords=self.y_acc_bias, x_axis_name=x_axis_name, y_axis_name=y_axis_name, + plot_name=plot_name, log_dir=log_dir) + ## z-Bias + plot_name = "acc-bias-z" + print(plot_name) + x_axis_name = "t[s]" + y_axis_name = "b[m/s^2]" + plot_2d(x_coords=self.imu_bias_time, y_coords=self.z_acc_bias, x_axis_name=x_axis_name, y_axis_name=y_axis_name, + plot_name=plot_name, log_dir=log_dir) + # Wheel odometry plots ------------------------------------- + if experiment_name == "1.2": + ## x-y-plot + plot_name = "wheel-position-x-y" + print(plot_name) + x_axis_name = "x[m]" + y_axis_name = "y[m]" + plot_2d(x_coords=self.x_wheel_pos, y_coords=self.y_wheel_pos, x_axis_name=x_axis_name, y_axis_name=y_axis_name, + plot_name=plot_name, log_dir=log_dir) + + def state_odometry_callback(self, state_estimate): + # Time + if not self.initial_state_time: + self.initial_state_time = state_estimate.header.stamp.to_sec() + print("Received state message.") + return + elif state_estimate.header.stamp.to_sec() - self.initial_state_time > 0.5: + self.state_time = self.state_time + [state_estimate.header.stamp.to_sec() - self.initial_state_time] + # Position + self.x_pos = self.x_pos + [state_estimate.pose.pose.position.x] + self.y_pos = self.y_pos + [state_estimate.pose.pose.position.y] + self.z_pos = self.z_pos + [state_estimate.pose.pose.position.z] + # Velocity + self.x_vel = self.x_vel + [state_estimate.twist.twist.linear.x] + self.y_vel = self.y_vel + [state_estimate.twist.twist.linear.y] + + def wheel_odometry_callback(self, wheel_odometry): + # Time + if not self.initial_wheel_time: + self.initial_wheel_time = wheel_odometry.header.stamp.to_sec() + print("Received wheel message.") + return + elif wheel_odometry.header.stamp.to_sec() - self.initial_wheel_time > 0.5: + self.x_wheel_pos = self.x_wheel_pos + [wheel_odometry.pose.pose.position.x] + self.y_wheel_pos = self.y_wheel_pos + [wheel_odometry.pose.pose.position.y] + self.z_wheel_pos = self.z_wheel_pos + [wheel_odometry.pose.pose.position.z] + + def bias_callback(self, bias_estimate): + # Time + if not self.initial_imu_bias_time: + self.initial_imu_bias_time = bias_estimate.header.stamp.to_sec() + print("Received bias message.") + return + elif bias_estimate.header.stamp.to_sec() - self.initial_imu_bias_time > 0.5: + self.imu_bias_time = self.imu_bias_time + [bias_estimate.header.stamp.to_sec() - self.initial_imu_bias_time] + # Acc. Bias + self.x_acc_bias = self.x_acc_bias + [bias_estimate.vector.x] + self.y_acc_bias = self.y_acc_bias + [bias_estimate.vector.y] + self.z_acc_bias = self.z_acc_bias + [bias_estimate.vector.z] + + def start_plotting(self, log_dir: str): + print("======================") + print("Starting plotting...") + print("======================") + + # Odometry Subscriber + rospy.Subscriber(self.state_odometry_topic_name, Odometry, self.state_odometry_callback) - # Wheel Odometry Subscriber - rospy.Subscriber(self.wheel_odometry_topic_name, Odometry, + # Wheel Odometry Subscriber + rospy.Subscriber(self.wheel_odometry_topic_name, Odometry, self.wheel_odometry_callback) - # Bias Subscriber - rospy.Subscriber(self.imu_bias_topic_name, Vector3Stamped, + # Bias Subscriber + rospy.Subscriber(self.imu_bias_topic_name, Vector3Stamped, self.bias_callback) - # Start spinning - rospy.spin() + # Start spinning + rospy.spin() + + # Plot after shutdown + self.plot(log_dir=log_dir) - # Plot after shutdown - self.plot() if __name__ == "__main__": - time.sleep(1.0) - experiment_name = input("Enter experiment name (used for file naming):") - msf_plotter = MsfPlotter() - msf_plotter.start_plotting() \ No newline at end of file + time.sleep(1.0) + if len(sys.argv) > 1: + log_dir = sys.argv[1] + print("Log directory (where plots will be saved to): " + log_dir) + else: + raise ValueError("Please provide the log directory.") + # Input + experiment_name = input("Enter experiment name (used for file naming):") + # Run Plotter + msf_plotter = MsfPlotter() + msf_plotter.start_plotting(log_dir=log_dir) diff --git a/smb_msf_graph/tutorial/config/1/core_graph_params.yaml b/smb_msf_graph/tutorial/config/1/core_graph_params.yaml deleted file mode 100644 index 95d9fe6..0000000 --- a/smb_msf_graph/tutorial/config/1/core_graph_params.yaml +++ /dev/null @@ -1,60 +0,0 @@ -# Sensor Params -sensor_params: - imuRate: 200.0 #Rate of IMU input (Hz) - useImuSignalLowPassFilter: false #If true, a low pass filter is applied to the IMU measurements - imuLowPassFilterCutoffFreq: 30.0 #Cutoff frequency of the low pass filter - imuBufferLength: 800 - imuTimeOffset: 0.0 # Offset between IMU and LiDAR Measurements: can be determined with rqt_multiplot - -# Initialization -initialization_params: - estimateGravityFromImu: false # If true, the gravity is estimated in the beginning using the IMU - gravityMagnitude: 9.80600 # If estimateGravityFromImu is false, this value is used as gravity - -# Factor Graph -graph_params: - useIsam: true # if false, then levenberg-marquardt is used, CURRENTLY NO EFFECT - smootherLag: 4.0 #Lag of fixed lag smoother[seconds], not needed for ISAM2 - maxOptimizationFrequency: 5.0 #Maximum optimization frequency [Hz], can be used to control computational load - additionalOptimizationIterations: 0 #Additional iterations of graph optimizer after update with new factors - findUnusedFactorSlots: true - enableDetailedResults: false - usingFallbackGraph: true - usingCholeskyFactorization: true # CHOLESKY faster, QR numerically more stable - usingBiasForPreIntegration: true # If true, the bias is used during pre-integration - optimizeFixedFramePosesWrtWorld: true # If true, the poses of the fixed frames are optimized w.r.t. world - -# Outlier Rejection -outlier_params: - poseMotionOutlierThreshold: 0.3 # in meters, if jumping more than this, it is considered as absent GNSS, occurs between two GNSS measurements - -# Noise Parameters -noise_params: - ## IMU - ### Position - accNoiseDensity: 2.2555e-03 # Continuous-time Noise Amplitude Spectral Density (StdDev) [m/s^2/√Hz)], default=sqrt(7.84e-06) - integrationNoiseDensity: 1.0e-04 # Continuous-time Noise Amplitude Spectral Density of integration uncertainty, default: sqrt(1.0e-07) - use2ndOrderCoriolis: false # Whether to use 2nd order coriolis effect - ### Rotation - gyrNoiseDensity: 2.356e-04 # Continuous-time Noise Amplitude Spectral Density (StdDev) [rad/s/√Hz], default=sqrt(3.4906585e-07) - omegaCoriolis: 1.07e-04 # Coriolis effect, positive on northern hemisphere, 0 at the equator, default (central europe, Switzerland): 1.07e-04 - ### Bias - accBiasRandomWalkNoiseDensity: 4.33e-03 # Continuous-time Noise Amplitude Spectral Density of Accelerometer bias random walk [m/s^3/√Hz], default: sqrt(1.0e-03) - gyrBiasRandomWalkNoiseDensity: 2.66e-04 # Continuous-time Noise Amplitude Spectral Density of Gyroscope bias random walk [rad/s^2/√Hz], default: default: sqrt(9.33e-04) - biasAccOmegaInit: 1.0e-05 # StdDev of bias at start: default: sqrt(1.0e-07) - accBiasPrior: 0.0 # Prior/starting value of accelerometer bias - gyrBiasPrior: 0.0 # Prior/starting value of gyroscope bias - -# Relinearization -relinearization_params: - positionReLinTh: 1.0e-02 # Position linearization threshold - rotationReLinTh: 1.0e-02 # Rotation linearization threshold - velocityReLinTh: 1.0e-02 # Linear Velocity linearization threshold - accBiasReLinTh: 1.0e-05 # Accelerometer bias linearization threshold - gyrBiasReLinTh: 1.0e-05 # Gyroscope bias linearization threshold - fixedFrameReLinTh: 1.0e-02 # Fixed frame linearization threshold, ONLY USED IF optimizeFixedFramePosesWrtWorld is true - relinearizeSkip: 1 # Re-linearization is skipped every n-th step, default: 10 - enableRelinearization: true # Whether to use relinearization, default: true - evaluateNonlinearError: false # Whether to evaluate the nonlinear errorr before and after the update, default: false - cacheLinearizedFactors: true # Whether to cache the linearized factors, default: true - enablePartialRelinearizationCheck: false # Whether potentially only parts of the tree needs to be relinearized, default: false diff --git a/smb_msf_graph/tutorial/config/1/smb_graph_params.yaml b/smb_msf_graph/tutorial/config/1_smb_graph_params.yaml similarity index 77% rename from smb_msf_graph/tutorial/config/1/smb_graph_params.yaml rename to smb_msf_graph/tutorial/config/1_smb_graph_params.yaml index 8c7d48c..d467705 100644 --- a/smb_msf_graph/tutorial/config/1/smb_graph_params.yaml +++ b/smb_msf_graph/tutorial/config/1_smb_graph_params.yaml @@ -1,14 +1,20 @@ # Sensor Params sensor_params: ## Config + # TODO: Change here useLioOdometry: false useWheelOdometry: false + # TODO: End of change useVioOdometry: false ## Rates lioOdometryRate: 10 wheelOdometryRate: 50 vioOdometryRate: 50 +# Alignment Parameters +alignment_params: + initialSe3AlignmentNoiseDensity: [ 1.0e-01, 1.0e-01, 1.0e-01, 1.0e-02, 1.0e-02, 1.0e-02 ] # StdDev, ORDER RPY(rad) - XYZ(meters) + # Noise Parameters noise_params: ## LiDAR diff --git a/smb_msf_graph/tutorial/config/2/core_graph_params.yaml b/smb_msf_graph/tutorial/config/2/core_graph_params.yaml deleted file mode 100644 index 8179d4a..0000000 --- a/smb_msf_graph/tutorial/config/2/core_graph_params.yaml +++ /dev/null @@ -1,60 +0,0 @@ -# Sensor Params -sensor_params: - imuRate: 200.0 #Rate of IMU input (Hz) - useImuSignalLowPassFilter: false #If true, a low pass filter is applied to the IMU measurements - imuLowPassFilterCutoffFreq: 30.0 #Cutoff frequency of the low pass filter - imuBufferLength: 800 - imuTimeOffset: 0.0 # Offset between IMU and LiDAR Measurements: can be determined with rqt_multiplot - -# Initialization -initialization_params: - estimateGravityFromImu: false # If true, the gravity is estimated in the beginning using the IMU - gravityMagnitude: 9.80600 # If estimateGravityFromImu is false, this value is used as gravity - -# Factor Graph -graph_params: - useIsam: true # if false, then levenberg-marquardt is used, CURRENTLY NO EFFECT - smootherLag: 4.0 #Lag of fixed lag smoother[seconds], not needed for ISAM2 - maxOptimizationFrequency: 5.0 #Maximum optimization frequency [Hz], can be used to control computational load - additionalOptimizationIterations: 0 #Additional iterations of graph optimizer after update with new factors - findUnusedFactorSlots: true - enableDetailedResults: false - usingFallbackGraph: true - usingCholeskyFactorization: true # CHOLESKY faster, QR numerically more stable - usingBiasForPreIntegration: true # If true, the bias is used during pre-integration - optimizeFixedFramePosesWrtWorld: true # If true, the poses of the fixed frames are optimized w.r.t. world - -# Outlier Rejection -outlier_params: - poseMotionOutlierThreshold: 0.3 # in meters, if jumping more than this, it is considered as absent GNSS, occurs between two GNSS measurements - -# Noise Parameters -noise_params: - ## IMU - ### Position - accNoiseDensity: 2.2555e-03 # Continuous-time Noise Amplitude Spectral Density (StdDev) [m/s^2/√Hz)], default=sqrt(7.84e-06) - integrationNoiseDensity: 1.0e-04 # Continuous-time Noise Amplitude Spectral Density of integration uncertainty, default: sqrt(1.0e-07) - use2ndOrderCoriolis: false # Whether to use 2nd order coriolis effect - ### Rotation - gyrNoiseDensity: 2.356e-04 # Continuous-time Noise Amplitude Spectral Density (StdDev) [rad/s/√Hz], default=sqrt(3.4906585e-07) - omegaCoriolis: 1.07e-04 # Coriolis effect, positive on northern hemisphere, 0 at the equator, default (central europe, Switzerland): 1.07e-04 - ### Bias - accBiasRandomWalkNoiseDensity: 4.33e-05 # Continuous-time Noise Amplitude Spectral Density of Accelerometer bias random walk [m/s^3/√Hz], default: sqrt(1.0e-03) - gyrBiasRandomWalkNoiseDensity: 2.66e-04 # Continuous-time Noise Amplitude Spectral Density of Gyroscope bias random walk [rad/s^2/√Hz], default: default: sqrt(9.33e-04) - biasAccOmegaInit: 1.0e-05 # StdDev of bias at start: default: sqrt(1.0e-07) - accBiasPrior: 0.0 # Prior/starting value of accelerometer bias - gyrBiasPrior: 0.0 # Prior/starting value of gyroscope bias - -# Relinearization -relinearization_params: - positionReLinTh: 1.0e-02 # Position linearization threshold - rotationReLinTh: 1.0e-02 # Rotation linearization threshold - velocityReLinTh: 1.0e-02 # Linear Velocity linearization threshold - accBiasReLinTh: 1.0e-05 # Accelerometer bias linearization threshold - gyrBiasReLinTh: 1.0e-05 # Gyroscope bias linearization threshold - fixedFrameReLinTh: 1.0e-02 # Fixed frame linearization threshold, ONLY USED IF optimizeFixedFramePosesWrtWorld is true - relinearizeSkip: 1 # Re-linearization is skipped every n-th step, default: 10 - enableRelinearization: true # Whether to use relinearization, default: true - evaluateNonlinearError: false # Whether to evaluate the nonlinear errorr before and after the update, default: false - cacheLinearizedFactors: true # Whether to cache the linearized factors, default: true - enablePartialRelinearizationCheck: false # Whether potentially only parts of the tree needs to be relinearized, default: false diff --git a/smb_msf_graph/tutorial/config/2/smb_graph_params.yaml b/smb_msf_graph/tutorial/config/2_smb_graph_params.yaml similarity index 56% rename from smb_msf_graph/tutorial/config/2/smb_graph_params.yaml rename to smb_msf_graph/tutorial/config/2_smb_graph_params.yaml index af0b979..42523d4 100644 --- a/smb_msf_graph/tutorial/config/2/smb_graph_params.yaml +++ b/smb_msf_graph/tutorial/config/2_smb_graph_params.yaml @@ -9,8 +9,17 @@ sensor_params: wheelOdometryRate: 50 vioOdometryRate: 50 +# Alignment Parameters +alignment_params: + initialSe3AlignmentNoiseDensity: [ 1.0e-01, 1.0e-01, 1.0e-01, 1.0e-02, 1.0e-02, 1.0e-02 ] # StdDev, ORDER RPY(rad) - XYZ(meters) + # Noise Parameters noise_params: + # IMU Random Walk + # TODO: Change here + accBiasRandomWalkNoiseDensity: 4.33e-05 # Continuous-time Noise Amplitude Spectral Density of Accelerometer bias random walk [m/s^3/√Hz], default: sqrt(1.0e-03) + gyrBiasRandomWalkNoiseDensity: 2.66e-06 # Continuous-time Noise Amplitude Spectral Density of Gyroscope bias random walk [rad/s^2/√Hz], default: default: sqrt(9.33e-04) + # TODO: End of change ## LiDAR lioPoseUnaryNoiseDensity: [ 0.05, 0.05, 0.05, 0.01, 0.01, 0.01 ] # StdDev, ORDER RPY(rad) - XYZ(meters) First tests: [ 1000.0, 1000.0, 1000.0, 2.0, 2.0, 2.0 ] ## Wheel diff --git a/smb_msf_graph/tutorial/config/3/core_graph_params.yaml b/smb_msf_graph/tutorial/config/3/core_graph_params.yaml deleted file mode 100644 index a44ac58..0000000 --- a/smb_msf_graph/tutorial/config/3/core_graph_params.yaml +++ /dev/null @@ -1,60 +0,0 @@ -# Sensor Params -sensor_params: - imuRate: 200.0 #Rate of IMU input (Hz) - useImuSignalLowPassFilter: false #If true, a low pass filter is applied to the IMU measurements - imuLowPassFilterCutoffFreq: 30.0 #Cutoff frequency of the low pass filter - imuBufferLength: 800 - imuTimeOffset: 0.0 # Offset between IMU and LiDAR Measurements: can be determined with rqt_multiplot - -# Initialization -initialization_params: - estimateGravityFromImu: false # If true, the gravity is estimated in the beginning using the IMU - gravityMagnitude: 9.80600 # If estimateGravityFromImu is false, this value is used as gravity - -# Factor Graph -graph_params: - useIsam: true # if false, then levenberg-marquardt is used, CURRENTLY NO EFFECT - smootherLag: 4.0 #Lag of fixed lag smoother[seconds], not needed for ISAM2 - maxOptimizationFrequency: 5.0 #Maximum optimization frequency [Hz], can be used to control computational load - additionalOptimizationIterations: 0 #Additional iterations of graph optimizer after update with new factors - findUnusedFactorSlots: true - enableDetailedResults: false - usingFallbackGraph: true - usingCholeskyFactorization: true # CHOLESKY faster, QR numerically more stable - usingBiasForPreIntegration: true # If true, the bias is used during pre-integration - optimizeFixedFramePosesWrtWorld: false # If true, the poses of the fixed frames are optimized w.r.t. world - -# Outlier Rejection -outlier_params: - poseMotionOutlierThreshold: 0.3 # in meters, if jumping more than this, it is considered as absent GNSS, occurs between two GNSS measurements - -# Noise Parameters -noise_params: - ## IMU - ### Position - accNoiseDensity: 2.2555e-03 # Continuous-time Noise Amplitude Spectral Density (StdDev) [m/s^2/√Hz)], default=sqrt(7.84e-06) - integrationNoiseDensity: 1.0e-04 # Continuous-time Noise Amplitude Spectral Density of integration uncertainty, default: sqrt(1.0e-07) - use2ndOrderCoriolis: false # Whether to use 2nd order coriolis effect - ### Rotation - gyrNoiseDensity: 2.356e-04 # Continuous-time Noise Amplitude Spectral Density (StdDev) [rad/s/√Hz], default=sqrt(3.4906585e-07) - omegaCoriolis: 1.07e-04 # Coriolis effect, positive on northern hemisphere, 0 at the equator, default (central europe, Switzerland): 1.07e-04 - ### Bias - accBiasRandomWalkNoiseDensity: 4.33e-03 # Continuous-time Noise Amplitude Spectral Density of Accelerometer bias random walk [m/s^3/√Hz], default: sqrt(1.0e-03) - gyrBiasRandomWalkNoiseDensity: 2.66e-04 # Continuous-time Noise Amplitude Spectral Density of Gyroscope bias random walk [rad/s^2/√Hz], default: default: sqrt(9.33e-04) - biasAccOmegaInit: 1.0e-05 # StdDev of bias at start: default: sqrt(1.0e-07) - accBiasPrior: 0.0 # Prior/starting value of accelerometer bias - gyrBiasPrior: 0.0 # Prior/starting value of gyroscope bias - -# Relinearization -relinearization_params: - positionReLinTh: 1.0e-02 # Position linearization threshold - rotationReLinTh: 1.0e-02 # Rotation linearization threshold - velocityReLinTh: 1.0e-02 # Linear Velocity linearization threshold - accBiasReLinTh: 1.0e-05 # Accelerometer bias linearization threshold - gyrBiasReLinTh: 1.0e-05 # Gyroscope bias linearization threshold - fixedFrameReLinTh: 1.0e-02 # Fixed frame linearization threshold, ONLY USED IF optimizeFixedFramePosesWrtWorld is true - relinearizeSkip: 1 # Re-linearization is skipped every n-th step, default: 10 - enableRelinearization: true # Whether to use relinearization, default: true - evaluateNonlinearError: false # Whether to evaluate the nonlinear errorr before and after the update, default: false - cacheLinearizedFactors: true # Whether to cache the linearized factors, default: true - enablePartialRelinearizationCheck: false # Whether potentially only parts of the tree needs to be relinearized, default: false diff --git a/smb_msf_graph/tutorial/config/3/smb_graph_params.yaml b/smb_msf_graph/tutorial/config/3_smb_graph_params.yaml similarity index 67% rename from smb_msf_graph/tutorial/config/3/smb_graph_params.yaml rename to smb_msf_graph/tutorial/config/3_smb_graph_params.yaml index af0b979..07d4294 100644 --- a/smb_msf_graph/tutorial/config/3/smb_graph_params.yaml +++ b/smb_msf_graph/tutorial/config/3_smb_graph_params.yaml @@ -1,3 +1,9 @@ +# Factor Graph +graph_params: + # TODO: Change here + optimizeFixedFramePosesWrtWorld: false # If true, the poses of the fixed frames are optimized w.r.t. world + # TODO: End of change + # Sensor Params sensor_params: ## Config @@ -9,6 +15,10 @@ sensor_params: wheelOdometryRate: 50 vioOdometryRate: 50 +# Alignment Parameters +alignment_params: + initialSe3AlignmentNoiseDensity: [ 1.0e-01, 1.0e-01, 1.0e-01, 1.0e-02, 1.0e-02, 1.0e-02 ] # StdDev, ORDER RPY(rad) - XYZ(meters) + # Noise Parameters noise_params: ## LiDAR diff --git a/smb_msf_graph/tutorial/launch/plotting.launch b/smb_msf_graph/tutorial/launch/plotting.launch index 376ffbd..ae34d9e 100644 --- a/smb_msf_graph/tutorial/launch/plotting.launch +++ b/smb_msf_graph/tutorial/launch/plotting.launch @@ -2,6 +2,7 @@ _ + @@ -9,6 +10,6 @@ - + \ No newline at end of file diff --git a/smb_msf_graph/tutorial/launch/tutorial_1.launch b/smb_msf_graph/tutorial/launch/tutorial_1.launch index 7ee7394..7397669 100644 --- a/smb_msf_graph/tutorial/launch/tutorial_1.launch +++ b/smb_msf_graph/tutorial/launch/tutorial_1.launch @@ -1,31 +1,30 @@ - + - - + - - + + - + - - + + - + - + \ No newline at end of file diff --git a/smb_msf_graph/tutorial/launch/tutorial_2.launch b/smb_msf_graph/tutorial/launch/tutorial_2.launch index 2837fdc..aa3bae0 100644 --- a/smb_msf_graph/tutorial/launch/tutorial_2.launch +++ b/smb_msf_graph/tutorial/launch/tutorial_2.launch @@ -1,31 +1,31 @@ - + - + - + - - + + - + - - + + - + - + \ No newline at end of file diff --git a/smb_msf_graph/tutorial/launch/tutorial_3.launch b/smb_msf_graph/tutorial/launch/tutorial_3.launch index 4a13518..94f0b05 100644 --- a/smb_msf_graph/tutorial/launch/tutorial_3.launch +++ b/smb_msf_graph/tutorial/launch/tutorial_3.launch @@ -1,31 +1,31 @@ - + - + - + - - + + - + - - + + - + - + \ No newline at end of file diff --git a/smb_msf_graph/tutorial/rss_plots/README.md b/smb_msf_graph/tutorial/rss_plots/README.md new file mode 100644 index 0000000..0314596 --- /dev/null +++ b/smb_msf_graph/tutorial/rss_plots/README.md @@ -0,0 +1,2 @@ +# Placeholder Directory +This directory will hold the plots from the tutorial. \ No newline at end of file