Add single step update func and tests. Got robot driving backwards on…

… edges. tests run but full edge propogation stil wip

experimental/beacon_sim/make_belief_updater.hh

@@ -62,6 +62,18 @@ struct EdgeTransform {
     std::vector<double> weight;
     TypedTransformVector transforms;
 };
+Eigen::Matrix3d 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

@@ -4,9 +4,21 @@
 
 namespace robot::experimental::beacon_sim {
 
+
+double step_lower_eigen_bound( // returns a lower bound on min ev of \Omega_{t-1}
+    const double lower_eigen_value_information, // min ev of \Omega_t
+    const double upper_eigen_value_dynamics, // max ev of G_t^{-1} G_t^{-T}
+    const double upper_eigen_value_measurement, // max ev of M_t
+    const double lower_eigen_value_process_noise // min ev of R_t
+) {
+    return 1. / (upper_eigen_value_dynamics * (1. / (lower_eigen_value_information - upper_eigen_value_measurement) - 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_edge_belief_transform(
-    const liegroups::SE2 &local_from_robot, // end state transform
-    const Eigen::Vector2d &start_state_in_local, // start state in local
+    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,
@@ -17,23 +29,26 @@ double compute_backwards_edge_belief_transform(
 
     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;
+    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() * start_state_in_local) {
+         start_in_robot = local_from_new_robot.inverse() * end_state_in_local) {
         // Move towards the goal
+        std::cout << "Starting loop in walk along edge" << std::endl;
+        std::cout << "  start in robot is " << std::endl << start_in_robot << std::endl;
         const liegroups::SE2 old_robot_from_new_robot = [&]() {
-            const double angle_to_goal_rad = std::atan2(start_in_robot.y(), start_in_robot.x());
+            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
+            std::cout << "  angle to goal is " << angle_to_goal_rad << std::endl;
 
             if (std::abs(angle_to_goal_rad) > TOL) {
-                // First turn to face the goal
+                // First turn until our back faces the goal
                 constexpr double MAX_ANGLE_STEP_RAD = DT_S * ANGULAR_VELOCITY_RADPS;
-                const double angle_step_rad = std::copysign(
+                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 towards the goal
+                // Then drive backwards 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);
+                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);
             }
         }();
@@ -45,25 +60,22 @@ double compute_backwards_edge_belief_transform(
         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;
+        std::cout << "produced process noise " << std::endl << 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;
-
+        std::cout << "produced process transform " << std::endl << std::get<ScatteringTransform<TransformType::INFORMATION>>(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;
+        std::cout << "produced measurement transform " << std::endl << std::get<ScatteringTransform<TransformType::INFORMATION>>(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);
+    return 0.0;
 }
 
 

experimental/beacon_sim/make_eigen_bounds.hh

@@ -9,6 +9,14 @@
 namespace robot::experimental::beacon_sim {
 
 
+// https://www.desmos.com/calculator/rbt2nj14ec
+double step_lower_eigen_bound( // returns a lower bound on min ev of \Omega_{t-1}
+    const double lower_eigen_value_information, // min ev of \Omega_t
+    const double upper_eigen_value_dynamics, // max ev of G_t^{-1} G_t^{-T}
+    const double upper_eigen_value_measurement, // max ev of M_t
+    const double lower_eigen_value_process_noise // min ev of R_t
+);
+
 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,

experimental/beacon_sim/make_eigen_bounds_test.cc

@@ -5,6 +5,35 @@
 
 namespace robot::experimental::beacon_sim {
 
+TEST(MakeEigenBoundsTest, step_eigen_bound_no_information) {
+    // Setup 
+    constexpr double start_info_eigen_value = 1.0;
+    constexpr double ub_dynamics_eigen_value = 1.0;
+    constexpr double ub_measurement_eigen_value = 0.0;
+    constexpr double lower_process_noise_eigen_value = 1.0;
+
+    // Action
+    double newbound = step_lower_eigen_bound(start_info_eigen_value, ub_dynamics_eigen_value, ub_measurement_eigen_value, lower_process_noise_eigen_value);
+
+    // 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) {
+    // Setup 
+    constexpr double start_info_eigen_value = 1.0;
+    constexpr double ub_dynamics_eigen_value = 1.0;
+    constexpr double ub_measurement_eigen_value = 1.0;
+    constexpr double lower_process_noise_eigen_value = 1.0;
+
+    // Action
+    double newbound = step_lower_eigen_bound(start_info_eigen_value, ub_dynamics_eigen_value, ub_measurement_eigen_value, lower_process_noise_eigen_value);
+
+    // 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{
@@ -22,23 +51,24 @@ TEST(MakeEigenBoundsTest, compute_edge_transform_no_measurements) {
         .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);
+        liegroups::SE2::trans(road_map.points.at(END_NODE_IDX));
+
+    std::cout << "End node position is " << local_from_robot.translation() << std::endl;
+    const Eigen::Vector2d start_pos = road_map.points.at(START_NODE_IDX);
+    std::cout << "Start node position is " << start_pos << std::endl;
 
     // 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);
+    const auto edge_belief_transform = compute_backwards_edge_belief_transform(
+        local_from_robot, start_pos, ekf_slam.config(), ekf_slam.estimate(), {}, MAX_SENSOR_RANGE_M,
+        TransformType::INFORMATION);
 
     // 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());
+    EXPECT_EQ(edge_belief_transform, 0.0);
 }