Add expected brm to run experiments (#276)

experimental/beacon_sim/BUILD

@@ -393,6 +393,7 @@ cc_library(
     "//common:check",
     "//common/math:combinations",
     "//common/math:multivariate_normal_cdf",
+    "//common/math:logsumexp",
     "//common/time:robot_time",
     "//planning:belief_road_map",
     "//planning:probabilistic_road_map",

experimental/beacon_sim/analyze_results.ipynb

@@ -12,7 +12,9 @@
     "from experimental.beacon_sim.experiment_results_pb2 import ExperimentResult\n",
     "import pandas as pd\n",
     "from collections import defaultdict\n",
-    "import matplotlib.pyplot as plt"
+    "import matplotlib.pyplot as plt\n",
+    "\n",
+    "import numpy as np"
    ]
   },
   {
@@ -22,6 +24,7 @@
    "metadata": {},
    "outputs": [],
    "source": [
+    "# file_name = '/tmp/compare_against_baseline_complex_env.pb'\n",
     "file_name = '/tmp/test_timing.pb'\n",
     "with open(file_name, 'rb') as file_in:\n",
     "    data = ExperimentResult()\n",
@@ -94,16 +97,27 @@
    "source": [
     "\n",
     "\n",
+    "plt.figure(figsize=(12,8))\n",
+    "bins = np.linspace(0, 3, 1000)\n",
+    "for i, g in df.groupby('planner_id'):\n",
+    "    # if 'Landmark' in planners[i]:\n",
+    "        plt.hist(g['expected_det'], density=True, histtype='step', label=planners[i], bins=bins, cumulative=True)\n",
+    "    # if i > 1:\n",
+    "    #     break\n",
+    "plt.xlabel('Expected Determinant')\n",
+    "plt.ylabel('Fraction')\n",
+    "plt.legend()\n",
+    "    # print(i, g)\n",
+    "\n",
     "axes = df.plot.hist(by='planner_id', column='expected_det', bins=50, figsize=(13,10))\n",
     "for i, ax in enumerate(axes):\n",
-    "    ax.set_yscale('log')\n",
+    "    # ax.set_yscale('log')\n",
     "    ax.set_title(planners[i])\n",
     "axes[-1].set_xlabel('Expected Determinant')\n",
     "plt.suptitle('Expected Determinant')\n",
     "plt.tight_layout()\n",
     "\n",
     "\n",
-    "\n",
     "axes = df.plot.hist(by='planner_id', column='elapsed_time', bins=50, figsize=(13,10))\n",
     "for i, ax in enumerate(axes):\n",
     "    ax.set_yscale('log')\n",
@@ -124,38 +138,7 @@
   {
    "cell_type": "code",
    "execution_count": null,
-   "id": "0265e63f-ea38-409b-936f-4b7768043b0f",
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "import numpy as np\n",
-    "np.exp(-1)"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "id": "310eca29-d7d6-490e-bffc-54b81a005aa7",
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "axes[0].set_yscale('log')"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "id": "87b2d11d-e0f1-498d-af44-4136ef25a334",
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "data"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "id": "7fb329b0-f6df-4225-b285-93d7a37e6bd7",
+   "id": "01231abd-3872-4819-9f0c-a711d78618fd",
    "metadata": {},
    "outputs": [],
    "source": []

experimental/beacon_sim/belief_road_map_planner.cc

@@ -20,6 +20,7 @@
 #include "common/check.hh"
 #include "common/liegroups/se2.hh"
 #include "common/math/combinations.hh"
+#include "common/math/logsumexp.hh"
 #include "common/math/multivariate_normal_cdf.hh"
 #include "common/math/redheffer_star.hh"
 #include "common/time/robot_time.hh"
@@ -436,10 +437,12 @@ std::optional<ExpectedBeliefPlanResult> compute_expected_belief_road_map_plan(
     // Sample a number of worlds and run the BRM on it
     std::vector<std::vector<int>> world_samples;
     std::vector<std::vector<int>> plans;
+    std::vector<double> log_probs;
     world_samples.reserve(options.num_configuration_samples);
     for (int i = 0; i < static_cast<int>(options.num_configuration_samples); i++) {
         world_samples.emplace_back(beacon_potential.sample(make_in_out(gen)));
-        const auto &sample = world_samples.at(i);
+        const auto &sample = world_samples.back();
+        log_probs.push_back(beacon_potential.log_prob(sample));
 
         // Create a potential with only this assignment
         std::unordered_map<int, bool> assignment;
@@ -474,6 +477,7 @@ std::optional<ExpectedBeliefPlanResult> compute_expected_belief_road_map_plan(
 
     return {{
         .nodes = plans.at(min_idx),
+        .log_probability_mass_tracked = math::logsumexp(log_probs),
     }};
 }
 

experimental/beacon_sim/belief_road_map_planner.hh

@@ -68,6 +68,7 @@ struct PathConstrainedBeliefPlanResult {
 
 struct ExpectedBeliefPlanResult {
     std::vector<int> nodes;
+    double log_probability_mass_tracked;
 };
 
 struct ExpectedBeliefRoadMapOptions {

experimental/beacon_sim/conditioned_potential.cc

@@ -23,6 +23,16 @@ double compute_log_prob(const ConditionedPotential &pot,
 
 std::vector<LogMarginal> compute_log_marginals(const ConditionedPotential &pot,
                                                const std::vector<int> &remaining) {
+    if (pot.conditioned_members.size() == pot.underlying_pot.members().size()) {
+        std::vector<int> present_beacons;
+        for (const auto &[beacon_id, is_present] : pot.conditioned_members) {
+            if (is_present) {
+                present_beacons.push_back(beacon_id);
+            }
+        }
+        return {{.present_beacons = std::move(present_beacons), .log_marginal = 0.0}};
+    }
+
     // Check that the remaining members are consistent with the conditioned members
     std::vector<int> all_remaining = remaining;
     for (const auto &[beacon_id, _] : pot.conditioned_members) {

experimental/beacon_sim/experiment_config.proto

@@ -29,7 +29,7 @@ message LandmarkBRMPlanner {
 }
 
 message ExpectedBRMPlanner {
-  int32 num_environment_samples = 1;
+  int32 num_configuration_samples = 1;
 }
 
 message OptimisticBRMPlanner {

experimental/beacon_sim/experiment_configs/compare_against_baselines_complex_env.pbtxt

@@ -0,0 +1,110 @@
+name: "evaluate_approximation_quality_complex_env"
+
+map_config_path: "data/env_3/world_map_config.pb"
+ekf_state_path: "data/env_3/mapped_landmarks.pb"
+road_map_path: "data/env_3/road_map.pb"
+evaluation_base_seed: 987654321
+num_eval_trials: 1000
+start_goal_seed: 12345
+start_goal_connection_radius_m: 20.0
+num_trials: 200
+# plan_timeout_s: 30.0
+max_sensor_range_m: 5.0
+
+# planner_configs: {
+#   name: "Optimistic BRM"
+#   optimistic_brm_config: {}
+# }
+# 
+# 
+planner_configs: {
+  name: "Landmark BRM 1 component"
+  landmark_brm_config: {
+    max_num_components: 1
+  }
+}
+
+# planner_configs: {
+#   name: "Expected BRM 1 sample"
+#   expected_brm_config: {
+#     num_configuration_samples: 1
+#   }
+# }
+# 
+# planner_configs: {
+#   name: "Landmark BRM 10 component"
+#   landmark_brm_config: {
+#     max_num_components: 10
+#   }
+# }
+# 
+# planner_configs: {
+#   name: "Expected BRM 10 sample"
+#   expected_brm_config: {
+#     num_configuration_samples: 10
+#   }
+# }
+# 
+# planner_configs: {
+#   name: "Landmark BRM 100 component"
+#   landmark_brm_config: {
+#     max_num_components: 100
+#   }
+# }
+# 
+# planner_configs: {
+#   name: "Expected BRM 100 sample"
+#   expected_brm_config: {
+#     num_configuration_samples: 100
+#   }
+# }
+# 
+# planner_configs: {
+#   name: "Landmark BRM 1000 component"
+#   landmark_brm_config: {
+#     max_num_components: 1000
+#   }
+# }
+# 
+# planner_configs: {
+#   name: "Expected BRM 1000 sample"
+#   expected_brm_config: {
+#     num_configuration_samples: 1000
+#   }
+# }
+
+# planner_configs: {
+#   name: "Landmark BRM 10000 component"
+#   landmark_brm_config: {
+#     max_num_components: 10000
+#   }
+# }
+# 
+# planner_configs: {
+#   name: "Expected BRM 10000 sample"
+#   expected_brm_config: {
+#     num_configuration_samples: 10000
+#   }
+# }
+
+# planner_configs: {
+#   name: "Landmark BRM 100000 component"
+#   landmark_brm_config: {
+#     max_num_components: 10000
+#   }
+# }
+# 
+# planner_configs: {
+#   name: "Expected BRM 100000 sample"
+#   expected_brm_config: {
+#     num_configuration_samples: 10000
+#   }
+# }
+
+
+# Running the full distribution takes too much RAM
+# planner_configs: {
+#   name: "Full Distribution"
+#   landmark_brm_config: {}
+# }
+

experimental/beacon_sim/experiment_configs/evaluate_approximation_accuracy_complex_env.pbtxt

@@ -0,0 +1,62 @@
+name: "evaluate_approximation_quality_complex_env"
+
+map_config_path: "data/env_3/world_map_config.pb"
+ekf_state_path: "data/env_3/mapped_landmarks.pb"
+road_map_path: "data/env_3/road_map.pb"
+evaluation_base_seed: 987654321
+num_eval_trials: 1000
+start_goal_seed: 12345
+start_goal_connection_radius_m: 20.0
+num_trials: 200
+plan_timeout_s: 30.0
+max_sensor_range_m: 5.0
+
+
+planner_configs: {
+  name: "1 components"
+  landmark_brm_config: {
+    max_num_components: 1
+  }
+}
+
+planner_configs: {
+  name: "10 components"
+  landmark_brm_config: {
+    max_num_components: 10
+  }
+}
+
+planner_configs: {
+  name: "100 components"
+  landmark_brm_config: {
+    max_num_components: 100
+  }
+}
+
+planner_configs: {
+  name: "1000 components"
+  landmark_brm_config: {
+    max_num_components: 1000
+  }
+}
+
+planner_configs: {
+  name: "10000 components"
+  landmark_brm_config: {
+    max_num_components: 10000
+  }
+}
+
+planner_configs: {
+  name: "100000 components"
+  landmark_brm_config: {
+    max_num_components: 100000
+  }
+}
+
+# Running the full distribution takes too much RAM
+# planner_configs: {
+#   name: "Full Distribution"
+#   landmark_brm_config: {}
+# }
+

experimental/beacon_sim/run_experiment.cc

@@ -160,25 +160,36 @@ PlannerResult run_planner(const planning::RoadMap &road_map, const EkfSlam &ekf,
                                      : std::nullopt};
 }
 
-PlannerResult run_planner(
-    [[maybe_unused]] const planning::RoadMap &road_map, [[maybe_unused]] const EkfSlam &ekf,
-    [[maybe_unused]] const BeaconPotential &beacon_potential,
-    [[maybe_unused]] const proto::ExpectedBRMPlanner &config,
-    [[maybe_unused]] const double max_sensor_range_m,
-    [[maybe_unused]] const std::optional<time::RobotTimestamp::duration> &timeout) {
-    // const auto plan = compute_landmark_belief_road_map_plan(
-    //     road_map, ekf, beacon_potential,
-    //     {.max_sensor_range_m = max_sensor_range_m,
-    //      .sampled_belief_options =
-    //          config.has_max_num_components()
-    //              ? std::make_optional(LandmarkBeliefRoadMapOptions::SampledBeliefOptions{
-    //                    .max_num_components = config.max_num_components(), .seed = 12345})
-    //              : std::nullopt,
-    //      .timeout = timeout
-    //              });
-    // CHECK(plan.has_value());
-    // return plan->nodes;
-    return {};
+PlannerResult run_planner(const planning::RoadMap &road_map, [[maybe_unused]] const EkfSlam &ekf,
+                          const BeaconPotential &beacon_potential,
+                          const proto::ExpectedBRMPlanner &config, const double max_sensor_range_m,
+                          const std::optional<time::RobotTimestamp::duration> &timeout) {
+    const ExpectedBeliefRoadMapOptions options = {
+        .num_configuration_samples = config.num_configuration_samples(),
+        .seed = 12345,
+        .brm_options =
+            {
+                .max_sensor_range_m = max_sensor_range_m,
+                .uncertainty_tolerance = std::nullopt,
+                .max_num_edge_transforms = std::numeric_limits<int>::max(),
+                .timeout = timeout,
+            },
+    };
+
+    const time::RobotTimestamp start_time = time::current_robot_time();
+    const auto maybe_plan =
+        compute_expected_belief_road_map_plan(road_map, ekf, beacon_potential, options);
+    const time::RobotTimestamp::duration elapsed_time = time::current_robot_time() - start_time;
+
+    return {
+        .elapsed_time = elapsed_time,
+        .plan = maybe_plan.has_value()
+                    ? std::make_optional(PlannerResult::Plan{
+                          .nodes = maybe_plan->nodes,
+                          .log_prob_mass_tracked = maybe_plan->log_probability_mass_tracked,
+                      })
+                    : std::nullopt,
+    };
 }
 
 PlannerResult run_planner(const planning::RoadMap &road_map, const EkfSlam &ekf,