Merge pull request #2 from kevin-robb/feat/rie-opt-project

Pose-graph SLAM using GTSAM

Author: kevin-robb

ekf_ws/src/base_pkg/CMakeLists.txt

@@ -15,7 +15,8 @@ add_message_files(
   Command.msg
   EKFState.msg
   UKFState.msg
-  PFState.msg
+  PoseGraphState.msg
+  NaiveState.msg
 )
 
 generate_messages(

ekf_ws/src/base_pkg/config/params.yaml

@@ -8,10 +8,11 @@
 
 # ----- Filter -----
 # options: ekf_slam, ukf_loc, ukf_slam, pose_graph
-filter: "ekf_slam"
+filter: "pose_graph"
 
-# ----- Iteration Period -----
-dt: 0.05
+# ----- Run duraction & characteristics -----
+dt: 0.05 # Period of timers in all nodes.
+num_iterations: 1000 # Number of iterations to run sim for when pre-generating a trajectory.
 
 # ----- Initial Vehicle Pose -----
 # (will be overridden for some maps)
@@ -26,34 +27,43 @@ constraints:
     d_max: 0.1 # max forward motion in one timestep. min is 0.
     th_max: 0.0546 # max angular motion magnitude in one timestep. min is -th_max.
   vision:
-    range_max: 4.0
-    fov_min: -3.141592653589793
-    fov_max: 3.141592653589793
+    range_max: 3.0
+    fov_min: -1.57 #-3.141592653589793
+    fov_max: 1.57 #3.141592653589793
   measurements:
     # limitations on measurements:
-    landmark_id_is_known: False # if true, filter will use the known landmark IDs.
-    min_landmark_separation: 0.1 # landmarks detected closer together than this are assumed to be the same.
+    landmark_id_is_known: true # if true, filter will use the known landmark IDs. if false, filter will perform data association.
+    min_landmark_separation: 0.1 # in data association, landmarks detected closer together than this are assumed to be the same.
 
 # ----- Noise Profiles -----
 process_noise:
   mean:
     v_d: 0.0
     v_th: 0.0
   cov:
-    V_00: 0.0004
-    V_11: 0.00007615435494667714
+    V_00: 0.01 #0.0004 # covariance on forward commands.
+    V_11: 0.001 # 0.00007615435494667714 # covariance on angular commands.
 sensing_noise:
   mean:
     w_r: 0.0
     w_b: 0.0
   cov:
-    W_00: 0.01
-    W_11: 0.00030461741978670857
+    W_00: 0.01 #0.01 # covariance on range measurements.
+    W_11: 0.01 #0.00030461741978670857 # covariance on bearing measurements.
 
-# ----- UKF -----
+# ----- UKF-Specific Parameters -----
 ukf:
   W_0: 0.2 # weight on mean sigma pt.
 
+# ----- Pose-Graph-SLAM-Specific Parameters -----
+pose_graph:
+  filter_to_compare: "naive" # Other filter to run in tandem and use for initial pose-graph estimate. Options are ekf_slam, ukf_loc, ukf_slam, naive.
+  implementation: "gtsam" # Specific PGS implementation to run. Options are gtsam, sesync, custom.
+  verbose: false # if true, spam the console with a ton of logs.
+  update_landmarks_after_adding: false # if true, allow landmark position estimates from the secondary filter to be used to update the landmark positions stored in the initial pose graph estimate.
+  solve_graph_every_iteration: true # if false, only run optimization once, after the entire graph has been constructed.
+  # TODO look into using iSAM, which is designed to work iteratively.
+
 # ----- Map -----
 map:
   bound: 10.0 # display area is +-bound in both x/y.
@@ -71,30 +81,32 @@ path_planning:
   # pure pursuit params.
   lookahead_dist_init: 0.2 # meters
   lookahead_dist_max: 2 # meters
-  astar_incl_diagonals: True # A* search diagonals or only orthogonally adjacent cells?
+  astar_incl_diagonals: true # A* search diagonals or only orthogonally adjacent cells?
 
 # ----- Precomputed Trajectory Generation -----
-# (used if precompute_trajectory option is True in roslaunch)
+# (used if precompute_trajectory option is true in roslaunch)
 trajectory_gen:
-  num_timesteps: 2000
   landmark_noise: 0.2 # noise to use for generating map for TSP solution.
   visitation_threshold: 3.0 # how close the veh must get to a lm to mark it as visited in TSP soln.
 
 # ----- Plotter -----
 plotter:
-  save_final_map: False
+  save_final_map: false # save map to file in base_pkg/plots upon node exit.
   # flags to display certain things
-  show_occ_map: True
-  show_entire_traj: False
-  show_true_traj: True
-  show_true_landmark_map: True
-  show_landmark_ellipses: True
-  show_veh_ellipse: True
+  show_occ_map: true
+  show_entire_traj: false
+  show_true_traj: true
+  show_true_landmark_map: true
+  show_landmark_ellipses: true
+  show_veh_ellipse: true
   # display params
   arrow_len: 0.1 # length of pose arrow.
   cov_std_dev: 1 # number of std dev to include in cov ellipses.
-  plot_pf_arrows: False # show individual heading for all PF particles
-  plot_ukf_arrows: True # show individual heading for all UKF sigma points
-  show_landmark_sigma_pts: True # for UKF, show sigma points for landmarks as well as vehicle.
+  plot_ukf_arrows: true # show individual heading for all UKF sigma points
+  show_landmark_sigma_pts: true # for UKF, show sigma points for landmarks as well as vehicle.
   display_region_mult: 1.0 # amount to multiply map.bound by to get the display region.
-  list_clicked_points: False # utility to display coords of all clicked points, for manually creating landmark maps.
+  list_clicked_points: false # utility to display coords of all clicked points, for manually creating landmark maps.
+  pose_graph:
+    show_normal_viz_alongside: true # if true, normal sim viz is shown on one subplot, and the pose graph is shown on a second subplot. if false, only the pose graph's plot is shown.
+    show_meas_connections: false # these are the slowest thing to plot.
+    show_cmd_connections: false # these are fast to plot but aren't even visible so it's just a waste of runtime.

ekf_ws/src/base_pkg/launch/filter_demo.launch ekf_ws/src/base_pkg/launch/filter_demo_live.launch

@@ -3,15 +3,19 @@
     <arg name="landmark_map" default="random"/> <!-- options: "random", "grid", ... -->
     <arg name="precompute_trajectory" default="true"/>
     <arg name="tight_control" default="false"/>
+    <arg name="timer_period" default="default"/> <!-- By default, uses dt from params.yaml. -->
+    <arg name="plot_result_only" default="false"/> <!-- Show a live visualization of filter progress. -->
 
     <!-- Filter node, which will use filter specified in params.yaml -->
-    <node pkg="localization_pkg" type="localization_node" name="localization_node" output="screen" />
+    <node pkg="localization_pkg" type="localization_node" name="localization_node" args="$(arg timer_period)" output="screen" />
 
     <!-- Call main sim driver script. -->
     <include file="$(find base_pkg)/launch/sim_base.launch" >
         <arg name="occ_map_img" value="$(arg occ_map_img)"/>
         <arg name="landmark_map" value="$(arg landmark_map)"/>
         <arg name="precompute_trajectory" value="$(arg precompute_trajectory)"/>
         <arg name="tight_control" value="$(arg tight_control)"/>
+        <arg name="timer_period" value="$(arg timer_period)"/>
+        <arg name="plot_result_only" value="$(arg plot_result_only)"/>
     </include>
 </launch>

ekf_ws/src/base_pkg/launch/filter_demo_results_only.launch

@@ -0,0 +1,21 @@
+<launch>
+    <arg name="occ_map_img" default="blank.jpg"/>
+    <arg name="landmark_map" default="random"/> <!-- options: "random", "grid", ... -->
+    <arg name="precompute_trajectory" default="true"/>
+    <arg name="tight_control" default="false"/>
+    <arg name="timer_period" default="0.005"/> <!-- Use a very small dt since we don't have to worry about plot update rates. -->
+    <arg name="plot_result_only" default="true"/> <!-- Don't show a live viz at all, but rather just the final results. -->
+
+    <!-- Filter node, which will use filter specified in params.yaml -->
+    <node pkg="localization_pkg" type="localization_node" name="localization_node" args="$(arg timer_period)" output="screen" />
+
+    <!-- Call main sim driver script. -->
+    <include file="$(find base_pkg)/launch/sim_base.launch" >
+        <arg name="occ_map_img" value="$(arg occ_map_img)"/>
+        <arg name="landmark_map" value="$(arg landmark_map)"/>
+        <arg name="precompute_trajectory" value="$(arg precompute_trajectory)"/>
+        <arg name="tight_control" value="$(arg tight_control)"/>
+        <arg name="timer_period" value="$(arg timer_period)"/>
+        <arg name="plot_result_only" value="$(arg plot_result_only)"/>
+    </include>
+</launch>

ekf_ws/src/base_pkg/launch/sim_base.launch

@@ -1,15 +1,17 @@
 <launch>
     <!-- Launch the basic necessities for all demos. -->
     <arg name="occ_map_img" default="blank.jpg"/>
-    <arg name="landmark_map" default="random"/>
+    <arg name="landmark_map" default="random"/> <!-- options: "random", "grid", ... -->
     <arg name="precompute_trajectory" default="false"/>
     <arg name="use_local_planner" default="false"/>
     <arg name="tight_control" default="false"/>
+    <arg name="timer_period" default="default"/> <!-- By default, uses dt from params.yaml. -->
+    <arg name="plot_result_only" default="false"/> <!-- if true, don't show a live viz at all, but rather just the final results. -->
 
     <!-- main simulator -->
-    <node pkg="base_pkg" type="sim_node.py" name="sim_node"  args="$(arg occ_map_img) $(arg landmark_map) $(arg precompute_trajectory)" output="screen" />
+    <node pkg="base_pkg" type="sim_node.py" name="sim_node"  args="$(arg occ_map_img) $(arg landmark_map) $(arg precompute_trajectory) $(arg timer_period)" output="screen" />
     <!-- plotting node -->
-    <node pkg="base_pkg" type="plotting_node.py" name="plotting_node" output="screen" />
+    <node pkg="base_pkg" type="plotting_node.py" name="plotting_node" args="$(arg timer_period) $(arg plot_result_only)" output="screen" />
     <!-- path planning/nav -->
-    <node pkg="planning_pkg" type="goal_pursuit_node.py" name="goal_pursuit_node" args="$(arg use_local_planner) $(arg precompute_trajectory) $(arg tight_control) $(arg occ_map_img)" output="screen" />
+    <node pkg="planning_pkg" type="goal_pursuit_node.py" name="goal_pursuit_node" args="$(arg use_local_planner) $(arg precompute_trajectory) $(arg tight_control) $(arg occ_map_img) $(arg timer_period)" output="screen" />
 </launch>

ekf_ws/src/base_pkg/msg/NaiveState.msg

@@ -0,0 +1,7 @@
+Header header
+# timestep.
+int32 timestep
+# vechicle pose.
+float32 x_v
+float32 y_v
+float32 yaw_v

ekf_ws/src/base_pkg/msg/PFState.msg

@@ -0,0 +1,17 @@
+Header header
+# total number of iterations that have elapsed (i.e., number of vehicle poses in full history).
+int32 timestep
+# vechicle pose components for all iterations.
+float32[] x_v
+float32[] y_v
+float32[] yaw_v
+
+# total number of detected landmarks.
+int32 M
+# landmark positions [x_1,y_1,...,x_M,y_M] (2M elements).
+float32[] landmarks
+
+# we know there is a connection between every pose and the pose at the immediate previous/next iterations.
+# however, each vehicle pose may have any number of connections to landmarks (including none).
+# list of connections takes the form [i_1,j_1,...,i_k,j_k] where i_t is an iteration number representing a vehicle pose, and j_m is a particular landmark index.
+int32[] meas_connections