WIP: starting to add calculation of lower eigen bounds. Based on make…

…_belief_updater

experimental/beacon_sim/BUILD

@@ -516,6 +516,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_eigen_bounds.cc

@@ -0,0 +1,70 @@
+#include "experimental/beacon_sim/make_eigen_bounds.hh"
+#include <iostream>
+
+
+namespace robot::experimental::beacon_sim {
+
+double compute_backwards_edge_belief_transform(
+    const liegroups::SE2 &local_from_robot, // end state transform
+    const Eigen::Vector2d &start_state_in_local, // 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,
+    const TransformType type) {
+    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;
+    for (Eigen::Vector2d start_in_robot = local_from_robot.inverse() * start_state_in_local;
+         start_in_robot.norm() > TOL;
+         start_in_robot = local_from_new_robot.inverse() * start_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(), start_in_robot.x());
+
+            if (std::abs(angle_to_goal_rad) > TOL) {
+                // First turn to face the goal
+                constexpr double MAX_ANGLE_STEP_RAD = DT_S * ANGULAR_VELOCITY_RADPS;
+                const double angle_step_rad = 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 towards the goal
+                constexpr double MAX_DIST_STEP_M = DT_S * VELOCITY_MPS;
+                const double dist_step_m = std::min(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;
+
+        // Compute the process update for the covariance
+        const Eigen::Matrix3d process_noise_in_robot =
+            compute_process_noise(ekf_config, DT_S, old_robot_from_new_robot.arclength());
+
+        std::cout << "produced process noise " << process_noise_in_robot << std::endl;
+
+        const TypedTransform process_transform =
+            compute_process_transform(process_noise_in_robot, old_robot_from_new_robot, type);
+
+        std::cout << "produced process transform " << process_transform << std::endl;
+
+        // Compute the measurement update for the covariance
+        const TypedTransform measurement_transform =
+            compute_measurement_transform(local_from_new_robot, ekf_config, ekf_estimate,
+                                          available_beacons, max_sensor_range_m, type);
+        std::cout << "produced measurement transform " << measurement_transform << std::endl;
+
+        // scattering_transform = (scattering_transform * process_transform).value();
+        // scattering_transform = (scattering_transform * measurement_transform).value();
+    }
+
+    return std::make_tuple(liegroups::SE2(local_from_new_robot.so2().log(), end_state_in_local),
+                           scattering_transform);
+}
+
+
+} //namespace 

experimental/beacon_sim/make_eigen_bounds.hh

@@ -0,0 +1,19 @@
+#pragma once 
+
+#include <optional>
+#include <unordered_set>
+#include <variant>
+
+#include "experimental/beacon_sim/make_belief_updater.hh"
+
+namespace robot::experimental::beacon_sim {
+
+
+double compute_backwards_edge_belief_transform(
+    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,
+    const TransformType transform_type);
+
+
+}  // namespace robot::experimental::beacon_sim

experimental/beacon_sim/make_eigen_bounds_test.cc

@@ -0,0 +1,45 @@
+#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, 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;
+    constexpr int MAX_NUM_TRANSFORMS = 10;
+    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(START_NODE_IDX));
+    const Eigen::Vector2d end_pos = road_map.points.at(END_NODE_IDX);
+
+    // Action
+    const auto edge_belief_transform = compute_edge_belief_transform(
+        local_from_robot, end_pos, ekf_slam.config(), ekf_slam.estimate(), {}, MAX_SENSOR_RANGE_M,
+        MAX_NUM_TRANSFORMS, TransformType::COVARIANCE);
+
+    // Verification
+    EXPECT_EQ(edge_belief_transform.local_from_robot.translation().x(), end_pos.x());
+    EXPECT_EQ(edge_belief_transform.local_from_robot.translation().y(), end_pos.y());
+}
+
+
+}  // namespace robot::experimental::beacon_sim