Feature/backwards eigen bounds (#219)

Co-authored-by: aknh9189 <[email protected]>
Co-authored-by: Ethan Fahnestock <[email protected]>

experimental/beacon_sim/BUILD

@@ -515,6 +515,26 @@ cc_test(
   ]
 )
 
+cc_library(
+  name = "make_eigen_bounds",
+  srcs = ["make_eigen_bounds.cc"],
+  hdrs = ["make_eigen_bounds.hh"],
+  deps = [
+    "@eigen",
+    ":make_belief_updater"
+  ]
+)
+
+cc_test(
+  name = "make_eigen_bounds_test",
+  srcs = ["make_eigen_bounds_test.cc"],
+  deps = [
+    ":make_eigen_bounds",
+    ":test_helpers",
+    "@com_google_googletest//:gtest_main",
+  ]
+)
+
 cc_library(
   name = "robot_belief",
   hdrs = ["robot_belief.hh"],

experimental/beacon_sim/make_belief_updater.cc

@@ -99,8 +99,9 @@ std::optional<TypedTransform> operator*(const TypedTransform &a, const TypedTran
     }
 }
 
-Eigen::Matrix3d compute_process_noise(const EkfSlamConfig &config, const double dt_s,
-                                      const double arclength_m) {
+Eigen::DiagonalMatrix<double, 3> compute_process_noise(const EkfSlamConfig &config,
+                                                       const double dt_s,
+                                                       const double arclength_m) {
     const auto sq = [](const double x) { return x * x; };
 
     return Eigen::DiagonalMatrix<double, 3>(

experimental/beacon_sim/make_belief_updater.hh

@@ -62,6 +62,19 @@ struct EdgeTransform {
     std::vector<double> weight;
     TypedTransformVector transforms;
 };
+Eigen::DiagonalMatrix<double, 3> compute_process_noise(const EkfSlamConfig &config,
+                                                       const double dt_s, const double arclength_m);
+TypedTransform compute_process_transform(const Eigen::Matrix3d &process_noise,
+                                         const liegroups::SE2 &old_robot_from_new_robot,
+                                         const TransformType type);
+TypedTransform compute_measurement_transform(
+    const liegroups::SE2 &local_from_robot, const EkfSlamConfig &ekf_config,
+    const EkfSlamEstimate &ekf_estimate, const std::optional<std::vector<int>> &available_beacons,
+    const double max_sensor_range_m, const TransformType type);
+
+Eigen::MatrixXd build_measurement_noise(const int num_observations,
+                                        const double range_measurement_noise_m,
+                                        const double bearing_measurement_noise_rad);
 
 std::tuple<liegroups::SE2, TypedTransform> compute_edge_belief_transform(
     const liegroups::SE2 &local_from_robot, const Eigen::Vector2d &end_state_in_local,

experimental/beacon_sim/make_eigen_bounds.cc

@@ -0,0 +1,88 @@
+#include "experimental/beacon_sim/make_eigen_bounds.hh"
+
+namespace robot::experimental::beacon_sim {
+
+double step_lower_eigen_bound(  // returns a lower bound on min ev of \Omega_{t-1}
+    const StepLowerEigenBoundInputs &inputs) {
+    return 1. / (inputs.upper_eigen_value_dynamics * (1. / (inputs.lower_eigen_value_information -
+                                                            inputs.upper_eigen_value_measurement) -
+                                                      inputs.lower_eigen_value_process_noise));
+}
+
+/// Start state is the end of the edge
+/// End state is the start of the edge (as we move backwards)
+double compute_backwards_eigen_bound_transform(
+    const double lower_eigen_value_information,  // min ev of \Omega_t
+    const liegroups::SE2 &local_from_robot,      // end node state transform
+    const Eigen::Vector2d &end_state_in_local,   // start node state in local
+    const EkfSlamConfig &ekf_config, const EkfSlamEstimate &ekf_estimate,
+    const std::optional<std::vector<int>> &available_beacons, const double max_sensor_range_m) {
+    constexpr double DT_S = 0.5;
+    constexpr double VELOCITY_MPS = 2.0;
+    constexpr double ANGULAR_VELOCITY_RADPS = 2.0;
+
+    liegroups::SE2 local_from_new_robot = local_from_robot;
+    constexpr double TOL = 1e-6;
+
+    double information_lower_bound = lower_eigen_value_information;
+    for (Eigen::Vector2d start_in_robot = local_from_robot.inverse() * end_state_in_local;
+         start_in_robot.norm() > TOL;
+         start_in_robot = local_from_new_robot.inverse() * end_state_in_local) {
+        // Move towards the goal
+        const liegroups::SE2 old_robot_from_new_robot = [&]() {
+            const double angle_to_goal_rad =
+                std::atan2(start_in_robot.y(),
+                           -1.0 * start_in_robot.x());  // angle is 0 when goal is negative x axis
+
+            if (std::abs(angle_to_goal_rad) > TOL) {
+                // First turn until our back faces the goal
+                constexpr double MAX_ANGLE_STEP_RAD = DT_S * ANGULAR_VELOCITY_RADPS;
+                const double angle_step_rad =
+                    -1 * std::copysign(std::min(std::abs(angle_to_goal_rad), MAX_ANGLE_STEP_RAD),
+                                       angle_to_goal_rad);
+                return liegroups::SE2::rot(angle_step_rad);
+            } else {
+                // Then drive backwards towards the goal
+                constexpr double MAX_DIST_STEP_M = DT_S * VELOCITY_MPS;
+                const double dist_step_m =
+                    -1.0 * std::min(std::abs(start_in_robot.x()), MAX_DIST_STEP_M);
+                return liegroups::SE2::transX(dist_step_m);
+            }
+        }();
+
+        // Update the mean
+        local_from_new_robot = local_from_new_robot * old_robot_from_new_robot;
+
+        // G_t^{-1}
+        const Eigen::Matrix3d inv_dynamics_jac_wrt_state = old_robot_from_new_robot.Adj();
+        // max_eigen_value(G_t^{-1} G_t^{-T})
+        const double dynamics_upper_eigen_value_bound =
+            (inv_dynamics_jac_wrt_state * inv_dynamics_jac_wrt_state.transpose())
+                .eigenvalues()
+                .real()
+                .maxCoeff();
+        // R_t
+        const Eigen::DiagonalMatrix<double, 3> process_noise =
+            compute_process_noise(ekf_config, DT_S, old_robot_from_new_robot.arclength());
+        const double process_lower_eigen_value_bound = process_noise.diagonal().real().minCoeff();
+
+        // M_t
+        TransformType type = TransformType::COVARIANCE;
+        const TypedTransform measurement_transform =
+            compute_measurement_transform(local_from_new_robot, ekf_config, ekf_estimate,
+                                          available_beacons, max_sensor_range_m, type);
+        const Eigen::Matrix3d M_t =
+            -1 * std::get<ScatteringTransform<TransformType::COVARIANCE>>(measurement_transform)
+                     .bottomLeftCorner(3, 3);
+        const double measurement_upper_eigen_value = M_t.eigenvalues().real().maxCoeff();
+
+        information_lower_bound = step_lower_eigen_bound(
+            {.lower_eigen_value_information = information_lower_bound,
+             .upper_eigen_value_dynamics = dynamics_upper_eigen_value_bound,
+             .upper_eigen_value_measurement = measurement_upper_eigen_value,
+             .lower_eigen_value_process_noise = process_lower_eigen_value_bound});
+    }
+    return information_lower_bound;
+}
+
+}  // namespace robot::experimental::beacon_sim

experimental/beacon_sim/make_eigen_bounds.hh

@@ -0,0 +1,27 @@
+#pragma once
+
+#include <optional>
+#include <unordered_set>
+#include <variant>
+
+#include "experimental/beacon_sim/make_belief_updater.hh"
+
+namespace robot::experimental::beacon_sim {
+
+struct StepLowerEigenBoundInputs {
+    double lower_eigen_value_information;    // min ev of \Omega_t
+    double upper_eigen_value_dynamics;       // max ev of G_t^{-1} G_t^{-T}
+    double upper_eigen_value_measurement;    // max ev of M_t
+    double lower_eigen_value_process_noise;  // min ev of R_t
+};
+// https://www.desmos.com/calculator/rbt2nj14ec
+// returns a lower bound on min ev of \Omega_{t-1}
+double step_lower_eigen_bound(const StepLowerEigenBoundInputs &inputs);
+
+double compute_backwards_eigen_bound_transform(
+    const double lower_eigen_value_information,  // min ev of \Omega_t
+    const liegroups::SE2 &local_from_robot, const Eigen::Vector2d &start_state_in_local,
+    const EkfSlamConfig &ekf_config, const EkfSlamEstimate &ekf_estimate,
+    const std::optional<std::vector<int>> &available_beacons, const double max_sensor_range_m);
+
+}  // namespace robot::experimental::beacon_sim

experimental/beacon_sim/make_eigen_bounds_test.cc

@@ -0,0 +1,71 @@
+#include "experimental/beacon_sim/make_eigen_bounds.hh"
+
+#include "experimental/beacon_sim/test_helpers.hh"
+#include "gtest/gtest.h"
+
+namespace robot::experimental::beacon_sim {
+
+TEST(MakeEigenBoundsTest, step_eigen_bound_no_information) {
+    const double start_info_eigen_value = 0.5;
+    // Action
+    double newbound =
+        step_lower_eigen_bound({.lower_eigen_value_information = start_info_eigen_value,
+                                .upper_eigen_value_dynamics = 0.008,
+                                .upper_eigen_value_measurement = 0.000001,
+                                .lower_eigen_value_process_noise = 0.02});
+
+    // Verification
+    // lower bound should increase as more information is required travel the edge (as no info is
+    // gained and process noise is added)
+    ASSERT_GT(newbound, start_info_eigen_value);
+}
+TEST(MakeEigenBoundsTest, step_eigen_bound_information) {
+    const double start_info_eigen_value = 0.5;
+    // Action
+    double newbound =
+        step_lower_eigen_bound({.lower_eigen_value_information = start_info_eigen_value,
+                                .upper_eigen_value_dynamics = 0.5,
+                                .upper_eigen_value_measurement = 1.4,
+                                .lower_eigen_value_process_noise = 0.008});
+
+    // Verification
+    // lower bound should decrease as information is added along the edge that outweighs the process
+    // noise
+    ASSERT_LT(newbound, start_info_eigen_value);
+}
+
+TEST(MakeEigenBoundsTest, compute_edge_transform_no_measurements) {
+    // Setup
+    const EkfSlamConfig ekf_config{
+        .max_num_beacons = 1,
+        .initial_beacon_uncertainty_m = 100.0,
+        .along_track_process_noise_m_per_rt_meter = 0.05,
+        .cross_track_process_noise_m_per_rt_meter = 0.05,
+        .pos_process_noise_m_per_rt_s = 0.0,
+        .heading_process_noise_rad_per_rt_meter = 1e-3,
+        .heading_process_noise_rad_per_rt_s = 0.0,
+        .beacon_pos_process_noise_m_per_rt_s = 1e-6,
+        .range_measurement_noise_m = 1e-1,
+        .bearing_measurement_noise_rad = 1e-1,
+        .on_map_load_position_uncertainty_m = 2.0,
+        .on_map_load_heading_uncertainty_rad = 0.5,
+    };
+    constexpr double MAX_SENSOR_RANGE_M = 3.0;
+    const auto &[road_map, ekf_slam, _] = create_grid_environment(ekf_config);
+
+    constexpr int START_NODE_IDX = 6;
+    constexpr int END_NODE_IDX = 3;
+    const liegroups::SE2 local_from_robot = liegroups::SE2::trans(road_map.points.at(END_NODE_IDX));
+
+    const Eigen::Vector2d start_pos = road_map.points.at(START_NODE_IDX);
+    // Action
+    const double initial_info_min_eigen_value_bound = 1.0;
+    const auto edge_belief_transform = compute_backwards_eigen_bound_transform(
+        initial_info_min_eigen_value_bound, local_from_robot, start_pos, ekf_slam.config(),
+        ekf_slam.estimate(), {}, MAX_SENSOR_RANGE_M);
+
+    // Verification
+    EXPECT_LT(edge_belief_transform, initial_info_min_eigen_value_bound);
+}
+
+}  // namespace robot::experimental::beacon_sim