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