From 8b0150093578324d43fb6674c38067529ca13bdb Mon Sep 17 00:00:00 2001 From: Erick Fuentes Date: Sun, 21 Jan 2024 09:57:39 -0500 Subject: [PATCH 1/4] Add Expected BRM to run_experiments --- experimental/beacon_sim/BUILD | 1 + experimental/beacon_sim/analyze_results.ipynb | 53 +++++++------------ .../beacon_sim/belief_road_map_planner.cc | 6 ++- .../beacon_sim/belief_road_map_planner.hh | 1 + .../beacon_sim/conditioned_potential.cc | 10 ++++ .../beacon_sim/experiment_config.proto | 2 +- experimental/beacon_sim/run_experiment.cc | 49 ++++++++++------- planning/belief_road_map.hh | 11 ++-- 8 files changed, 72 insertions(+), 61 deletions(-) diff --git a/experimental/beacon_sim/BUILD b/experimental/beacon_sim/BUILD index de0a70f8..cb17f9bd 100644 --- a/experimental/beacon_sim/BUILD +++ b/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", diff --git a/experimental/beacon_sim/analyze_results.ipynb b/experimental/beacon_sim/analyze_results.ipynb index b6f7597a..8363146d 100644 --- a/experimental/beacon_sim/analyze_results.ipynb +++ b/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": [] diff --git a/experimental/beacon_sim/belief_road_map_planner.cc b/experimental/beacon_sim/belief_road_map_planner.cc index 523d16f3..ad80bdfc 100644 --- a/experimental/beacon_sim/belief_road_map_planner.cc +++ b/experimental/beacon_sim/belief_road_map_planner.cc @@ -21,6 +21,7 @@ #include "common/liegroups/se2.hh" #include "common/math/combinations.hh" #include "common/math/multivariate_normal_cdf.hh" +#include "common/math/logsumexp.hh" #include "common/math/redheffer_star.hh" #include "common/time/robot_time.hh" #include "experimental/beacon_sim/correlated_beacons.hh" @@ -436,10 +437,12 @@ std::optional compute_expected_belief_road_map_plan( // Sample a number of worlds and run the BRM on it std::vector> world_samples; std::vector> plans; + std::vector log_probs; world_samples.reserve(options.num_configuration_samples); for (int i = 0; i < static_cast(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 assignment; @@ -474,6 +477,7 @@ std::optional compute_expected_belief_road_map_plan( return {{ .nodes = plans.at(min_idx), + .log_probability_mass_tracked = math::logsumexp(log_probs), }}; } diff --git a/experimental/beacon_sim/belief_road_map_planner.hh b/experimental/beacon_sim/belief_road_map_planner.hh index 551df9c9..31971a91 100644 --- a/experimental/beacon_sim/belief_road_map_planner.hh +++ b/experimental/beacon_sim/belief_road_map_planner.hh @@ -68,6 +68,7 @@ struct PathConstrainedBeliefPlanResult { struct ExpectedBeliefPlanResult { std::vector nodes; + double log_probability_mass_tracked; }; struct ExpectedBeliefRoadMapOptions { diff --git a/experimental/beacon_sim/conditioned_potential.cc b/experimental/beacon_sim/conditioned_potential.cc index 441a4b13..495df494 100644 --- a/experimental/beacon_sim/conditioned_potential.cc +++ b/experimental/beacon_sim/conditioned_potential.cc @@ -23,6 +23,16 @@ double compute_log_prob(const ConditionedPotential &pot, std::vector compute_log_marginals(const ConditionedPotential &pot, const std::vector &remaining) { + if (pot.conditioned_members.size() == pot.underlying_pot.members().size()) { + std::vector 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 all_remaining = remaining; for (const auto &[beacon_id, _] : pot.conditioned_members) { diff --git a/experimental/beacon_sim/experiment_config.proto b/experimental/beacon_sim/experiment_config.proto index 4f508818..567a49a6 100644 --- a/experimental/beacon_sim/experiment_config.proto +++ b/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 { diff --git a/experimental/beacon_sim/run_experiment.cc b/experimental/beacon_sim/run_experiment.cc index 5ad628a4..18bdff8c 100644 --- a/experimental/beacon_sim/run_experiment.cc +++ b/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 &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 &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::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, diff --git a/planning/belief_road_map.hh b/planning/belief_road_map.hh index 3a161270..8b5fd004 100644 --- a/planning/belief_road_map.hh +++ b/planning/belief_road_map.hh @@ -43,7 +43,8 @@ struct BRMSearchState { template std::vector>> successors_for_state( const BRMSearchState &state, const RoadMap &road_map, - const BeliefUpdater &belief_updater, const UncertaintySize &uncertainty_size) { + const BeliefUpdater &belief_updater, +const UncertaintySize &uncertainty_size) { std::vector>> out; for (const auto &[other_node_id, other_node_in_local] : road_map.neighbors(state.node_idx)) { const Belief new_belief = belief_updater(state.belief, state.node_idx, other_node_id); @@ -85,10 +86,10 @@ std::optional> plan( ShouldTerminateCallback should_terminate_callback = []() { return false; }) { using SearchState = detail::BRMSearchState; // Find nearest node to start and end states - const SuccessorFunc successors_func = - [&belief_updater, &road_map, &uncertainty_size](const SearchState &state) { - return detail::successors_for_state(state, road_map, belief_updater, uncertainty_size); - }; + const SuccessorFunc successors_func = [&belief_updater, + &road_map, &uncertainty_size](const SearchState &state) { + return detail::successors_for_state(state, road_map, belief_updater, uncertainty_size); + }; const GoalCheckFunc goal_check_func = [&should_terminate_callback](const Node &) { From a1bffd38dd123425bc99572c9d031acab0765c6c Mon Sep 17 00:00:00 2001 From: Erick Fuentes Date: Sun, 21 Jan 2024 09:58:02 -0500 Subject: [PATCH 2/4] add experiment configs --- ...ompare_against_baselines_complex_env.pbtxt | 110 ++++++++++++++++++ ...e_approximation_accuracy_complex_env.pbtxt | 62 ++++++++++ 2 files changed, 172 insertions(+) create mode 100644 experimental/beacon_sim/experiment_configs/compare_against_baselines_complex_env.pbtxt create mode 100644 experimental/beacon_sim/experiment_configs/evaluate_approximation_accuracy_complex_env.pbtxt diff --git a/experimental/beacon_sim/experiment_configs/compare_against_baselines_complex_env.pbtxt b/experimental/beacon_sim/experiment_configs/compare_against_baselines_complex_env.pbtxt new file mode 100644 index 00000000..a6b231aa --- /dev/null +++ b/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: {} +# } + diff --git a/experimental/beacon_sim/experiment_configs/evaluate_approximation_accuracy_complex_env.pbtxt b/experimental/beacon_sim/experiment_configs/evaluate_approximation_accuracy_complex_env.pbtxt new file mode 100644 index 00000000..3693fd65 --- /dev/null +++ b/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: {} +# } + From 51f9985b5c4afe6aec723daa700e76b8e7056108 Mon Sep 17 00:00:00 2001 From: Erick Fuentes Date: Sun, 21 Jan 2024 09:58:31 -0500 Subject: [PATCH 3/4] lint --- planning/belief_road_map.hh | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/planning/belief_road_map.hh b/planning/belief_road_map.hh index 8b5fd004..3a161270 100644 --- a/planning/belief_road_map.hh +++ b/planning/belief_road_map.hh @@ -43,8 +43,7 @@ struct BRMSearchState { template std::vector>> successors_for_state( const BRMSearchState &state, const RoadMap &road_map, - const BeliefUpdater &belief_updater, -const UncertaintySize &uncertainty_size) { + const BeliefUpdater &belief_updater, const UncertaintySize &uncertainty_size) { std::vector>> out; for (const auto &[other_node_id, other_node_in_local] : road_map.neighbors(state.node_idx)) { const Belief new_belief = belief_updater(state.belief, state.node_idx, other_node_id); @@ -86,10 +85,10 @@ std::optional> plan( ShouldTerminateCallback should_terminate_callback = []() { return false; }) { using SearchState = detail::BRMSearchState; // Find nearest node to start and end states - const SuccessorFunc successors_func = [&belief_updater, - &road_map, &uncertainty_size](const SearchState &state) { - return detail::successors_for_state(state, road_map, belief_updater, uncertainty_size); - }; + const SuccessorFunc successors_func = + [&belief_updater, &road_map, &uncertainty_size](const SearchState &state) { + return detail::successors_for_state(state, road_map, belief_updater, uncertainty_size); + }; const GoalCheckFunc goal_check_func = [&should_terminate_callback](const Node &) { From a12601dca1d77545dd85928156fffd02457152ee Mon Sep 17 00:00:00 2001 From: Erick Fuentes Date: Fri, 8 Mar 2024 09:57:10 -0500 Subject: [PATCH 4/4] lint --- experimental/beacon_sim/belief_road_map_planner.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/experimental/beacon_sim/belief_road_map_planner.cc b/experimental/beacon_sim/belief_road_map_planner.cc index ad80bdfc..f6d3eb22 100644 --- a/experimental/beacon_sim/belief_road_map_planner.cc +++ b/experimental/beacon_sim/belief_road_map_planner.cc @@ -20,8 +20,8 @@ #include "common/check.hh" #include "common/liegroups/se2.hh" #include "common/math/combinations.hh" -#include "common/math/multivariate_normal_cdf.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" #include "experimental/beacon_sim/correlated_beacons.hh"