Move belief updater code into separate library (#213)

ewfuentes · Sep 22, 2023 · 5940d32 · 5940d32
1 parent b24c8e0
commit 5940d32
Show file tree

Hide file tree

Showing 10 changed files with 815 additions and 694 deletions.
diff --git a/experimental/beacon_sim/BUILD b/experimental/beacon_sim/BUILD
@@ -348,11 +348,11 @@ cc_library(
     ":ekf_slam",
     ":generate_observations",
     ":robot",
+    ":robot_belief",
+    ":make_belief_updater",
     "//planning:belief_road_map",
     "//planning:probabilistic_road_map",
     "//common/math:combinations",
-    "//common/math:redheffer_star",
-    "//common/geometry:nearest_point_on_segment",
   ]
 )
 
@@ -361,6 +361,7 @@ cc_test(
   srcs = ["belief_road_map_planner_test.cc"],
   deps = [
     ":belief_road_map_planner",
+    ":test_helpers",
     "@com_google_googletest//:gtest_main",
   ],
 )
@@ -486,3 +487,49 @@ cc_test(
     ":correlated_beacons_to_proto",
   ]
 )
+
+cc_library(
+  name = "make_belief_updater",
+  srcs = ["make_belief_updater.cc"],
+  hdrs = ["make_belief_updater.hh"],
+  deps = [
+    "@eigen",
+    ":ekf_slam",
+    ":correlated_beacons",
+    ":robot_belief",
+    "//common/liegroups:se2",
+    "//common/geometry:nearest_point_on_segment",
+    "//common/math:redheffer_star",
+    "//planning:belief_road_map",
+    "//planning:probabilistic_road_map",
+  ]
+)
+
+cc_test(
+  name = "make_belief_updater_test",
+  srcs = ["make_belief_updater_test.cc"],
+  deps = [
+    ":make_belief_updater",
+    ":test_helpers",
+    "@com_google_googletest//:gtest_main",
+  ]
+)
+
+cc_library(
+  name = "robot_belief",
+  hdrs = ["robot_belief.hh"],
+  deps = [
+    "//common/liegroups:se2",
+    "@eigen",
+  ]
+)
+
+cc_library(
+    name = "test_helpers",
+    hdrs = ["test_helpers.hh"],
+    srcs = ["test_helpers.cc"],
+    deps = [
+      ":ekf_slam",
+      "//planning:probabilistic_road_map",
+    ]
+)
diff --git a/experimental/beacon_sim/belief_road_map_planner.cc b/experimental/beacon_sim/belief_road_map_planner.cc
diff --git a/experimental/beacon_sim/belief_road_map_planner.hh b/experimental/beacon_sim/belief_road_map_planner.hh
@@ -6,15 +6,11 @@
 #include "common/liegroups/se2.hh"
 #include "experimental/beacon_sim/correlated_beacons.hh"
 #include "experimental/beacon_sim/ekf_slam.hh"
+#include "experimental/beacon_sim/robot_belief.hh"
 #include "planning/belief_road_map.hh"
 #include "planning/probabilistic_road_map.hh"
 
 namespace robot::experimental::beacon_sim {
-struct RobotBelief {
-    liegroups::SE2 local_from_robot;
-    Eigen::Matrix3d cov_in_robot;
-};
-
 struct BeliefRoadMapOptions {
     double max_sensor_range_m;
     int num_start_connections;
@@ -53,26 +49,4 @@ ExpectedBeliefPlanResult compute_expected_belief_road_map_plan(
     const planning::RoadMap &road_map, const EkfSlam &ekf, const BeaconPotential &beacon_potential,
     const Eigen::Vector2d &goal_state, const ExpectedBeliefRoadMapOptions &options);
 
-namespace detail {
-struct EdgeTransform {
-    static constexpr int DIM = 2 * liegroups::SE2::DoF;
-    using Matrix = Eigen::Matrix<double, DIM, DIM>;
-    liegroups::SE2 local_from_robot;
-    std::vector<double> weight;
-    std::vector<Matrix> transforms;
-};
-
-EdgeTransform compute_edge_belief_transform(const liegroups::SE2 &local_from_robot,
-                                            const Eigen::Vector2d &end_state_in_local,
-                                            const EkfSlamConfig &ekf_config,
-                                            const EkfSlamEstimate &ekf_estimate,
-                                            const BeaconPotential &beacon_potential,
-                                            const double max_sensor_range_m,
-                                            const int max_num_transforms);
-
-std::tuple<liegroups::SE2, EdgeTransform::Matrix> compute_edge_belief_transform(
-    const liegroups::SE2 &local_from_robot, const Eigen::Vector2d &end_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 detail
 }  // namespace robot::experimental::beacon_sim