Test examples

README.rst

@@ -33,7 +33,7 @@ Clone this repo, change to its directory, and execute
 
     pip install -e .
 
-This command should automatically install all dependencies, including our package `pylie` (found at https://github.com/decargroup/pylie) for back-end Lie group mathematical operations.
+This command should automatically install all dependencies, including our package `pymlg` (found at https://github.com/decargroup/pymlg) for back-end Lie group mathematical operations.
 
 Examples
 ^^^^^^^^
@@ -105,7 +105,7 @@ As another more complicated example, a state object belonging to the SE(3) Lie g
 .. code-block:: python
 
     from navlie.types import State 
-    from pylie import SE3 
+    from pymlg import SE3 
     import numpy as np 
 
     class SE3State(State):

examples/ex_batch_se3.py

@@ -3,88 +3,82 @@
 process and measurement models.
 """
 
-from navlie.batch.estimator import BatchEstimator
-from navlie.lib.states import SE3State
-from navlie.datagen import DataGenerator
-from navlie.utils import GaussianResult, GaussianResultList, randvec
-from navlie.lib.models import (
+from navlie.lib import (
     BodyFrameVelocity,
     RangePoseToAnchor,
+    SE3State
 )
-from navlie.utils import plot_error
+import navlie as nav
 import numpy as np
-from typing import List
-from pylie import SE3
 import matplotlib.pyplot as plt
 
-# #############################################################################
-# Create the batch estimator with desired settings
-estimator = BatchEstimator(solver_type="GN", max_iters=20)
+def main():
+    # ##########################################################################
+    # Create the batch estimator with desired settings
+    estimator = nav.BatchEstimator(solver_type="GN", max_iters=20)
 
-# ##############################################################################
-# Problem Setup
-t_end = 4
-x0 = SE3State(SE3.Exp([0, 0, 0, 0, 0, 0]), stamp=0.0)
-P0 = 0.1**2 * np.identity(6)
-R = 0.1**2
-Q = np.diag([0.01**2, 0.01**2, 0.01**2, 0.1, 0.1, 0.1])
-range_models = [
-    RangePoseToAnchor([1, 0, 0], [0.17, 0.17, 0], R),
-    RangePoseToAnchor([1, 0, 0], [-0.17, 0.17, 0], R),
-    RangePoseToAnchor([-1, 0, 0], [0.17, 0.17, 0], R),
-    RangePoseToAnchor([-1, 0, 0], [-0.17, 0.17, 0], R),
-    RangePoseToAnchor([0, 2, 0], [0.17, 0.17, 0], R),
-    RangePoseToAnchor([0, 2, 0], [-0.17, 0.17, 0], R),
-    RangePoseToAnchor([0, 2, 2], [0.17, 0.17, 0], R),
-    RangePoseToAnchor([0, 2, 2], [-0.17, 0.17, 0], R),
-]
+    # ##########################################################################
+    # Problem Setup
+    t_end = 4
+    x0 = SE3State([0, 0, 0, 0, 0, 0], stamp=0.0)
+    P0 = 0.1**2 * np.identity(6)
+    R = 0.1**2
+    Q = np.diag([0.01**2, 0.01**2, 0.01**2, 0.1, 0.1, 0.1])
+    range_models = [
+        RangePoseToAnchor([1, 0, 0], [0.17, 0.17, 0], R),
+        RangePoseToAnchor([1, 0, 0], [-0.17, 0.17, 0], R),
+        RangePoseToAnchor([-1, 0, 0], [0.17, 0.17, 0], R),
+        RangePoseToAnchor([-1, 0, 0], [-0.17, 0.17, 0], R),
+        RangePoseToAnchor([0, 2, 0], [0.17, 0.17, 0], R),
+        RangePoseToAnchor([0, 2, 0], [-0.17, 0.17, 0], R),
+        RangePoseToAnchor([0, 2, 2], [0.17, 0.17, 0], R),
+        RangePoseToAnchor([0, 2, 2], [-0.17, 0.17, 0], R),
+    ]
 
-range_freqs = 20
-process_model = BodyFrameVelocity(Q)
-input_profile = lambda t, x: np.array(
-    [np.sin(0.1 * t), np.cos(0.1 * t), np.sin(0.1 * t), 1, 0, 0]
-)
+    range_freqs = 20
+    process_model = BodyFrameVelocity(Q)
+    input_profile = lambda t, x: np.array(
+        [np.sin(0.1 * t), np.cos(0.1 * t), np.sin(0.1 * t), 1, 0, 0]
+    )
 
-input_covariance = Q
-input_freq = 100
-noise_active = True
+    input_freq = 100
+    noise_active = True
 
 
-# Generate data with no noise
-dg = DataGenerator(
-    process_model,
-    input_profile,
-    Q,
-    input_freq,
-    range_models,
-    range_freqs,
-)
+    # Generate data with no noise
+    dg = nav.DataGenerator(
+        process_model,
+        input_profile,
+        Q,
+        input_freq,
+        range_models,
+        range_freqs,
+    )
 
-state_true, input_list, meas_list = dg.generate(x0, 0, t_end, noise_active)
+    state_true, input_list, meas_list = dg.generate(x0, 0, t_end, noise_active)
 
-# Run batch
-estimate_list, opt_results = estimator.solve(
-    x0,
-    P0,
-    input_list,
-    meas_list,
-    process_model,
-    return_opt_results=True,
-)
+    # Run batch
+    estimate_list, opt_results = estimator.solve(
+        x0,
+        P0,
+        input_list,
+        meas_list,
+        process_model,
+        return_opt_results=True,
+    )
 
-print(opt_results["summary"])
+    print(opt_results["summary"])
 
 
-results = GaussianResultList(
-    [
-        GaussianResult(estimate_list[i], state_true[i])
-        for i in range(len(estimate_list))
-    ]
-)
+    results = nav.GaussianResultList.from_estimates(estimate_list, state_true)
+    return results
+
 
-fig, ax = plot_error(results)
-ax[-1][0].set_xlabel("Time (s)")
-ax[-1][1].set_xlabel("Time (s)")
-ax[0][0].set_title("Orientation Error")
-ax[0][1].set_title("Position Error")
-plt.show()
+if __name__ == "__main__":
+    results = main()
+    fig, ax = nav.plot_error(results)
+    ax[-1][0].set_xlabel("Time (s)")
+    ax[-1][1].set_xlabel("Time (s)")
+    ax[0][0].set_title("Orientation Error")
+    ax[0][1].set_title("Position Error")
+    plt.show()

examples/ex_batch_vector.py

@@ -6,91 +6,84 @@
 models.
 """
 
-from navlie.batch.estimator import BatchEstimator
-from navlie.lib.states import VectorState
-from navlie.datagen import DataGenerator
-from navlie.utils import (
-    GaussianResult,
-    GaussianResultList,
-    randvec,
-    plot_error,
-    associate_stamps,
-)
-from navlie.lib.models import SingleIntegrator, RangePointToAnchor
+import navlie as nav
+from navlie.lib import SingleIntegrator, RangePointToAnchor, VectorState
 import numpy as np
 from typing import List
 import matplotlib.pyplot as plt
 
-# #############################################################################
-# Create the batch estimator with desired settings
-estimator = BatchEstimator(solver_type="GN", max_iters=20)
+def main():
+    # #############################################################################
+    # Create the batch estimator with desired settings
+    estimator = nav.BatchEstimator(solver_type="GN", max_iters=5)
 
-# ##############################################################################
-# Problem Setup
+    # ##############################################################################
+    # Problem Setup
 
-x0 = VectorState(np.array([1, 0]), stamp=0)
-P0 = np.diag([1, 1])
-R = 0.1**2
-Q = 0.1 * np.identity(2)
-range_models = [
-    RangePointToAnchor([0, 4], R),
-    RangePointToAnchor([-2, 0], R),
-    RangePointToAnchor([2, 0], R),
-]
-range_freqs = [50, 50, 50]
-process_model = SingleIntegrator(Q)
-input_profile = lambda t, x: np.array([np.sin(t), np.cos(t)])
-input_covariance = Q
-input_freq = 180
-noise_active = True
+    x0 = VectorState(np.array([1, 0]), stamp=0)
+    P0 = np.diag([1, 1])
+    R = 0.1**2
+    Q = 0.1 * np.identity(2)
+    range_models = [
+        RangePointToAnchor([0, 4], R),
+        RangePointToAnchor([-2, 0], R),
+        RangePointToAnchor([2, 0], R),
+    ]
+    range_freqs = [50, 50, 50]
+    process_model = SingleIntegrator(Q)
+    input_profile = lambda t, x: np.array([np.sin(t), np.cos(t)])
+    input_covariance = Q
+    input_freq = 180
+    noise_active = True
 
-# ##############################################################################
-# Data Generation
+    # ##############################################################################
+    # Data Generation
 
-dg = DataGenerator(
-    process_model,
-    input_profile,
-    input_covariance,
-    input_freq,
-    range_models,
-    range_freqs,
-)
+    dg = nav.DataGenerator(
+        process_model,
+        input_profile,
+        input_covariance,
+        input_freq,
+        range_models,
+        range_freqs,
+    )
 
-gt_data, input_data, meas_data = dg.generate(x0, 0, 10, noise=noise_active)
+    gt_data, input_data, meas_data = dg.generate(x0, 0, 10, noise=noise_active)
 
-# ##############################################################################
-# Run batch
-if noise_active:
-    x0 = x0.plus(randvec(P0))
+    # ##############################################################################
+    # Run batch
+    if noise_active:
+        x0 = x0.plus(nav.randvec(P0))
 
-estimate_list, opt_results = estimator.solve(
-    x0, P0, input_data, meas_data, process_model, return_opt_results=True
-)
+    estimate_list, opt_results = estimator.solve(
+        x0, P0, input_data, meas_data, process_model, return_opt_results=True
+    )
 
-print(opt_results["summary"])
-# # The estimate list returns the estimates at each of the
-# # interoceptive and measurement timestamps.
-# Find matching timestamps
-estimate_stamps = [float(x.state.stamp) for x in estimate_list]
-gt_stamps = [x.stamp for x in gt_data]
+    print(opt_results["summary"])
+    # # The estimate list returns the estimates at each of the
+    # # interoceptive and measurement timestamps.
+    # Find matching timestamps
+    estimate_stamps = [float(x.state.stamp) for x in estimate_list]
+    gt_stamps = [x.stamp for x in gt_data]
 
-matches = associate_stamps(estimate_stamps, gt_stamps)
+    matches = nav.associate_stamps(estimate_stamps, gt_stamps)
 
-est_list = []
-gt_list = []
-for match in matches:
-    gt_list.append(gt_data[match[1]])
-    est_list.append(estimate_list[match[0]])
+    est_list = []
+    gt_list = []
+    for match in matches:
+        gt_list.append(gt_data[match[1]])
+        est_list.append(estimate_list[match[0]])
 
-# Postprocess the results and plot
-results = GaussianResultList(
-    [GaussianResult(est_list[i], gt_list[i]) for i in range(len(est_list))]
-)
+    # Postprocess the results and plot
+    results = nav.GaussianResultList.from_estimates(est_list, gt_list)
+    return results
 
-fig, ax = plot_error(results)
-ax[0].set_title("Position")
-ax[1].set_title("Velocity")
-ax[0].set_xlabel("Time (s)")
-ax[1].set_xlabel("Time (s)")
-plt.tight_layout()
-plt.show()
+if __name__ == "__main__":
+    results = main()
+    fig, ax = nav.plot_error(results)
+    ax[0].set_title("Position")
+    ax[1].set_title("Velocity")
+    ax[0].set_xlabel("Time (s)")
+    ax[1].set_xlabel("Time (s)")
+    plt.tight_layout()
+    plt.show()