Pybind EkfSlam (#273)

common/liegroups/BUILD

@@ -63,9 +63,8 @@ cc_test(
 pybind_extension(
   name = "se2_python",
   srcs = ["se2_python.cc"],
-  deps = [
-    ":se2",
-  ],
+  data = [":so2_python.so"],
+  deps = [":se2"],
 )
 
 py_test(

common/liegroups/se2_python.cc

@@ -9,16 +9,21 @@ namespace py = pybind11;
 namespace robot::liegroups {
 
 PYBIND11_MODULE(se2_python, m) {
+    py::module_::import("common.liegroups.so2_python");
     py::class_<SE2>(m, "SE2")
         // Construct from rotation angle
         .def(py::init(&SE2::rot))
         // Construct from translation
         .def(py::init([](const Eigen::Vector2d &trans) { return SE2::trans(trans); }))
         .def(py::init<const double &, const Eigen::Vector2d &>())
         .def_static("exp", [](const Eigen::Vector3d &tangent) -> SE2 { return SE2::exp(tangent); })
+        .def("so2", [](const SE2 &a) -> SO2 { return a.so2(); })
+        .def("translation", py::overload_cast<>(&SE2::translation, py::const_))
+        .def("translation", py::overload_cast<>(&SE2::translation))
         .def("log", &SE2::log)
         .def("matrix", &SE2::matrix)
         .def("inverse", [](const SE2 &a) -> SE2 { return a.inverse(); })
+        .def(py::self * Eigen::Vector2d())
         .def(
             "__mul__", [](const SE2 &a, const SE2 &b) -> SE2 { return a * b; }, py::is_operator())
         .def(

common/liegroups/se2_python_test.py

@@ -39,6 +39,18 @@ def test_construct_from_translation(self):
         self.assertAlmostEqual(b_from_a_mat[0, 2], 1.0)
         self.assertAlmostEqual(b_from_a_mat[1, 2], 2.0)
 
+        trans = b_from_a.translation()
+        self.assertAlmostEqual(trans[0], 1.0)
+        self.assertAlmostEqual(trans[1], 2.0)
+
+        b_from_a_rot = b_from_a.so2()
+        b_from_a_rot_mat = b_from_a_rot.matrix()
+        self.assertAlmostEqual(b_from_a_rot_mat[0, 0], 1.0)
+        self.assertAlmostEqual(b_from_a_rot_mat[1, 1], 1.0)
+        self.assertAlmostEqual(b_from_a_rot_mat[0, 1], 0.0)
+        self.assertAlmostEqual(b_from_a_rot_mat[1, 0], 0.0)
+
+
     def test_group_operation(self):
         # Setup
         b_from_a = sep.SE2(math.pi / 3.0)
@@ -93,6 +105,18 @@ def test_inverse(self):
         total_error = np.sum(np.abs(a_from_a_mat - np.identity(3)))
         self.assertAlmostEqual(total_error, 0.0)
 
+    def test_group_action_on_point(self):
+        # Setup
+        b_from_a = sep.SE2(math.pi / 3.0, np.array([2.0, 3.0]))
+        pt_in_a = np.array([1.0, 0.0])
+
+        # Action
+        pt_in_b = b_from_a * pt_in_a
+
+        # Verification
+        self.assertAlmostEqual(pt_in_b[0], 0.5 + 2.0)
+        self.assertAlmostEqual(pt_in_b[1], math.sqrt(3) / 2.0 + 3.0)
+
 
 if __name__ == "__main__":
     unittest.main()

common/liegroups/so2_python.cc

@@ -15,6 +15,7 @@ PYBIND11_MODULE(so2_python, m) {
         .def("log", &SO2::log)
         .def("inverse", [](const SO2 &a) -> SO2 { return a.inverse(); })
         .def("matrix", &SO2::matrix)
+        .def(py::self * Eigen::Vector2d())
         .def(
             "__mul__", [](const SO2 &a, const SO2 &b) -> SO2 { return a * b; }, py::is_operator())
         .def(

common/liegroups/so2_python_test.py

@@ -3,6 +3,7 @@
 
 import common.liegroups.so2_python as sop
 import math
+import numpy as np
 
 
 class SO2PythonTest(unittest.TestCase):
@@ -83,5 +84,17 @@ def test_inverse(self):
         self.assertAlmostEqual(b_from_a_mat[1, 0], a_from_b_mat[0, 1])
         self.assertAlmostEqual(b_from_a_mat[1, 1], a_from_b_mat[1, 1])
 
+    def test_group_action_on_point(self):
+        # Setup
+        b_from_a = sop.SO2(math.pi / 3.0)
+        pt_in_a = np.array([1.0, 0.0])
+
+        # Action
+        pt_in_b = b_from_a * pt_in_a
+
+        # Verification
+        self.assertAlmostEqual(pt_in_b[0], 0.5)
+        self.assertAlmostEqual(pt_in_b[1], math.sqrt(3) / 2.0)
+
 if __name__ == "__main__":
     unittest.main()

experimental/beacon_sim/BUILD

@@ -141,6 +141,14 @@ cc_test(
   ]
 )
 
+pybind_extension(
+  name = "generate_observations_python",
+  srcs = ["generate_observations_python.cc"],
+  deps = [
+    ":generate_observations",
+  ]
+)
+
 cc_library(
   name = "ekf_slam",
   hdrs = ["ekf_slam.hh"],
@@ -154,20 +162,36 @@ cc_library(
   ]
 )
 
+cc_test(
+  name = "ekf_slam_test",
+  srcs = ["ekf_slam_test.cc"],
+  deps = [
+    ":ekf_slam",
+    "@com_google_googletest//:gtest_main",
+  ]
+)
+
 pybind_extension(
   name = "ekf_slam_python",
   srcs = ["ekf_slam_python.cc"],
+  data = [
+    "//common/time:robot_time_python.so",
+    "//common/liegroups:se2_python.so",
+    ":generate_observations_python.so",
+  ],
   deps = [
     ":ekf_slam",
   ]
 )
 
-cc_test(
-  name = "ekf_slam_test",
-  srcs = ["ekf_slam_test.cc"],
+py_test(
+  name = "ekf_slam_python_test",
+  srcs = ["ekf_slam_python_test.py"],
+  data = [
+    ":ekf_slam_python.so"
+  ], 
   deps = [
-    ":ekf_slam",
-    "@com_google_googletest//:gtest_main",
+    requirement("numpy"),
   ]
 )
 

experimental/beacon_sim/ekf_slam_python.cc

@@ -1,6 +1,8 @@
 
 #include "experimental/beacon_sim/ekf_slam.hh"
+#include "pybind11/eigen.h"
 #include "pybind11/pybind11.h"
+#include "pybind11/stl.h"
 
 namespace py = pybind11;
 using namespace pybind11::literals;
@@ -39,5 +41,27 @@ PYBIND11_MODULE(ekf_slam_python, m) {
                        &EkfSlamConfig::on_map_load_position_uncertainty_m)
         .def_readwrite("on_map_load_heading_uncertainty_rad",
                        &EkfSlamConfig::on_map_load_heading_uncertainty_rad);
+
+    py::class_<EkfSlamEstimate>(m, "EkfSlamEstimate")
+        .def_readwrite("time_of_validity", &EkfSlamEstimate::time_of_validity)
+        .def_readwrite("mean", &EkfSlamEstimate::mean)
+        .def_readwrite("cov", &EkfSlamEstimate::cov)
+        .def_readwrite("beacon_ids", &EkfSlamEstimate::beacon_ids)
+        .def("local_from_robot",
+             py::overload_cast<const liegroups::SE2 &>(&EkfSlamEstimate::local_from_robot))
+        .def("local_from_robot",
+             py::overload_cast<>(&EkfSlamEstimate::local_from_robot, py::const_))
+        .def("robot_cov", &EkfSlamEstimate::robot_cov)
+        .def("beacon_in_local", &EkfSlamEstimate::beacon_in_local)
+        .def("beacon_cov", &EkfSlamEstimate::beacon_cov);
+
+    py::class_<EkfSlam>(m, "EkfSlam")
+        .def(py::init<const EkfSlamConfig &, const time::RobotTimestamp &>())
+        .def("load_map", &EkfSlam::load_map)
+        .def("predict", &EkfSlam::predict)
+        .def("update", &EkfSlam::update)
+        .def("estimate", py::overload_cast<>(&EkfSlam::estimate, py::const_))
+        .def("estimate", py::overload_cast<>(&EkfSlam::estimate))
+        .def("config", &EkfSlam::config);
 }
 }  // namespace robot::experimental::beacon_sim

experimental/beacon_sim/ekf_slam_python_test.py

@@ -0,0 +1,60 @@
+
+import unittest
+
+from experimental.beacon_sim import ekf_slam_python as esp
+from common.time import robot_time_python as rtp
+from common.liegroups import se2_python as se2
+from experimental.beacon_sim.generate_observations_python import BeaconObservation
+
+import numpy as np
+np.set_printoptions(linewidth=200)
+
+class EkfSlamPythonTest(unittest.TestCase):
+    def test_happy_case(self):
+        # Setup
+        config = esp.EkfSlamConfig(
+            max_num_beacons=2,
+            initial_beacon_uncertainty_m=100.0,
+            along_track_process_noise_m_per_rt_meter=0.1,
+            cross_track_process_noise_m_per_rt_meter=0.01,
+            pos_process_noise_m_per_rt_s=0.01,
+            heading_process_noise_rad_per_rt_meter=0.001,
+            heading_process_noise_rad_per_rt_s=0.00001,
+            beacon_pos_process_noise_m_per_rt_s=0.01,
+            range_measurement_noise_m=0.05,
+            bearing_measurement_noise_rad=0.005,
+            on_map_load_position_uncertainty_m=0.1,
+            on_map_load_heading_uncertainty_rad=0.01,
+        )
+
+        current_time = rtp.RobotTimestamp() + rtp.as_duration(123.456)
+        dt = rtp.as_duration(0.5)
+        ekf = esp.EkfSlam(config, current_time)
+        BEACON_ID = 456
+        BEACON_IN_LOCAL = np.array([2.0, 1.0])
+
+        # Action
+        old_robot_from_new_robot = se2.SE2(np.array([0.5, 0]))
+
+        for i in range(10):
+            current_time += dt
+            ekf.predict(current_time, old_robot_from_new_robot)
+
+            # Compute the observation
+            beacon_in_robot = ekf.estimate().local_from_robot().inverse() * BEACON_IN_LOCAL
+
+            range_m = np.linalg.norm(beacon_in_robot)
+            bearing_rad = np.arctan2(beacon_in_robot[1], beacon_in_robot[0])
+
+            obs = BeaconObservation(BEACON_ID, range_m, bearing_rad)
+
+            ekf.update([obs])
+
+        # Verification
+        est_beacon_in_local = ekf.estimate().beacon_in_local(BEACON_ID)
+        self.assertAlmostEqual(est_beacon_in_local[0], BEACON_IN_LOCAL[0])
+        self.assertAlmostEqual(est_beacon_in_local[1], BEACON_IN_LOCAL[1])
+
+
+if __name__ == "__main__":
+    unittest.main()

experimental/beacon_sim/generate_observations_python.cc

@@ -0,0 +1,16 @@
+
+#include "experimental/beacon_sim/generate_observations.hh"
+#include "pybind11/pybind11.h"
+#include "pybind11/stl.h"
+
+namespace py = pybind11;
+
+namespace robot::experimental::beacon_sim {
+PYBIND11_MODULE(generate_observations_python, m) {
+    py::class_<BeaconObservation>(m, "BeaconObservation")
+        .def(py::init<std::optional<int>, std::optional<double>, std::optional<double>>())
+        .def_readwrite("maybe_id", &BeaconObservation::maybe_id)
+        .def_readwrite("maybe_range_m", &BeaconObservation::maybe_range_m)
+        .def_readwrite("maybe_bearing_rad", &BeaconObservation::maybe_bearing_rad);
+}
+}  // namespace robot::experimental::beacon_sim