From d3c2e159cd16b3810750761c2e12cdca0cf07713 Mon Sep 17 00:00:00 2001 From: Mitchell Cohen Date: Tue, 12 Dec 2023 18:13:38 -0500 Subject: [PATCH 1/2] Add simple toy SLAM problem to examples --- examples/ex_inertial_nav.py | 216 +++++------------------------------- examples/ex_slam.py | 161 +++++++++++++++++++++++++++ navlie/lib/datasets.py | 171 +++++++++++++++++++++++++++- navlie/lib/models.py | 77 +++++++++++++ navlie/utils.py | 7 +- tests/test_examples.py | 6 + 6 files changed, 447 insertions(+), 191 deletions(-) create mode 100644 examples/ex_slam.py diff --git a/examples/ex_inertial_nav.py b/examples/ex_inertial_nav.py index daf4ac74..96f58ba7 100644 --- a/examples/ex_inertial_nav.py +++ b/examples/ex_inertial_nav.py @@ -2,202 +2,46 @@ A slightly more complicated example of a robot localizing itself from relative position measurements to known landmarks. """ -from typing import List import numpy as np -from pymlg import SE23 -from navlie.lib import ( - IMU, - IMUState, - IMUKinematics, - InvariantMeasurement, - PointRelativePosition, -) import navlie as nav - -# ############################################################################## -# Problem Setup - -t_start = 0 -t_end = 15 -imu_freq = 100 - -# IMU noise parameters -sigma_gyro_ct = 0.01 -sigma_accel_ct = 0.01 -sigma_gyro_bias_ct = 0.0001 -sigma_accel_bias_ct = 0.0001 -init_gyro_bias = np.array([0.02, 0.03, -0.04]).reshape((-1, 1)) -init_accel_bias = np.array([0.01, 0.02, 0.05]).reshape((-1, 1)) - -# Landmark parameters -cylinder_radius = 7 -n_level = 1 -n_landmarks_per_level = 3 -max_height = 2.5 - -# Landmark sensor parameters -sigma_landmark_sensor = 0.1 # [m] -landmark_sensor_freq = 10 - -# Initialization parameters -sigma_phi_init = 0.1 -sigma_v_init = 0.1 -sigma_r_init = 0.1 -sigma_bg_init = 0.01 -sigma_ba_init = 0.01 -nav_state_init = SE23.from_components( - np.identity(3), - np.array([3, 0, 0]).reshape((-1, 1)), - np.array([0, 3, 0]).reshape((-1, 1)), -) - -################################################################################ -################################################################################ - -# Continuous-time Power Spectral Density -Q_c = np.eye(12) -Q_c[0:3, 0:3] *= sigma_gyro_ct**2 -Q_c[3:6, 3:6] *= sigma_accel_ct**2 -Q_c[6:9, 6:9] *= sigma_gyro_bias_ct**2 -Q_c[9:12, 9:12] *= sigma_accel_bias_ct**2 -dt = 1 / imu_freq - -Q_noise = Q_c / dt - - -def generate_landmark_positions( - cylinder_radius: float, - max_height: float, - n_levels: int, - n_landmarks_per_level: int, -) -> List[np.ndarray]: - """Generates landmarks arranged in a cylinder. - - Parameters - ---------- - cylinder_radius : float - Radius of the cylinder that the landmarks are arranged in. - max_height : float - Top of cylinder. - n_levels : int - Number of discrete levels to place landmarks at vertically. - n_landmarks_per_level : int - Number of landmarks per level - - Returns - ------- - List[np.ndarray] - List of landmarks. - """ - - z = np.linspace(0, max_height, n_levels) - - angles = np.linspace(0, 2 * np.pi, n_landmarks_per_level + 1) - angles = angles[0:-1] - x = cylinder_radius * np.cos(angles) - y = cylinder_radius * np.sin(angles) - - # Generate landmarks - landmarks = [] - for level_idx in range(n_levels): - for landmark_idx in range(n_landmarks_per_level): - cur_landmark = np.array( - [x[landmark_idx], y[landmark_idx], z[level_idx]] - ).reshape((3, -1)) - landmarks.append(cur_landmark) - - return landmarks - - -def input_profile(stamp: float, x: IMUState) -> np.ndarray: - """Generates an IMU measurement for a circular trajectory, - where the robot only rotates about the z-axis and the acceleration - points towards the center of the circle. - """ - - # Add biases to true angular velocity and acceleration - bias_gyro = x.bias_gyro.reshape((-1, 1)) - bias_accel = x.bias_accel.reshape((-1, 1)) - - C_ab = x.attitude - g_a = np.array([0, 0, -9.80665]).reshape((-1, 1)) - omega = np.array([0, 0, -1.0]).reshape((-1, 1)) + bias_gyro - accel = np.array([0, -3.0, 0]).reshape((-1, 1)) + bias_accel - C_ab.T @ g_a - - # Generate a random input to drive the bias random walk - Q_bias = Q_noise[6:, 6:] - bias_noise = nav.randvec(Q_bias) - - u = IMU(omega, accel, stamp, bias_noise[0:3], bias_noise[3:6]) - return u - - -landmarks = generate_landmark_positions( - cylinder_radius, max_height, n_level, n_landmarks_per_level -) - -process_model = IMUKinematics(Q_c / dt) -meas_cov = np.identity(3) * sigma_landmark_sensor**2 -meas_model_list = [PointRelativePosition(pos, meas_cov) for pos in landmarks] - -# Create data generator -data_gen = nav.DataGenerator( - process_model, - input_func=input_profile, - input_covariance=Q_noise, - input_freq=imu_freq, - meas_model_list=meas_model_list, - meas_freq_list=landmark_sensor_freq, -) - -# Initial state and covariance -x0 = IMUState( - nav_state_init, - init_gyro_bias, - init_accel_bias, - stamp=t_start, - state_id=0, - direction="left", -) - -P0 = np.eye(15) -P0[0:3, 0:3] *= sigma_phi_init**2 -P0[3:6, 3:6] *= sigma_v_init**2 -P0[6:9, 6:9] *= sigma_r_init**2 -P0[9:12, 9:12] *= sigma_bg_init**2 -P0[12:15, 12:15] *= sigma_ba_init**2 - -# Generate all data -states_true, input_list, meas_list = data_gen.generate( - x0, t_start, t_end, noise=True -) - -# **************** Conversion to Invariant Measurements ! ********************* -invariants = [InvariantMeasurement(meas, "right") for meas in meas_list] -# ***************************************************************************** - -# Zero-out the random walk values -for u in input_list: - u.bias_gyro_walk = np.array([0, 0, 0]) - u.bias_accel_walk = np.array([0, 0, 0]) - - -ekf = nav.ExtendedKalmanFilter(process_model) - -estimate_list = nav.run_filter(ekf, x0, P0, input_list, invariants) - -# Postprocess the results and plot -results = nav.GaussianResultList.from_estimates(estimate_list, states_true) +from navlie.lib.datasets import SimulatedInertialLandmarkDataset + +def main(): + np.set_printoptions(precision=3, suppress=True, linewidth=200) + np.random.seed(0) + # Load simulated dataset with default parameters + dataset = SimulatedInertialLandmarkDataset(t_start=0, t_end=20) + gt_states = dataset.get_ground_truth() + input_data = dataset.get_input_data() + meas_data = dataset.get_measurement_data() + + # Filter initialization + P0 = np.eye(15) + P0[0:3, 0:3] *= 0.1**2 + P0[3:6, 3:6] *= 0.1**2 + P0[6:9, 6:9] *= 0.1**2 + P0[9:12, 9:12] *= 0.01**2 + P0[12:15, 12:15] *= 0.01**2 + x0 = gt_states[0].plus(nav.randvec(P0)) + + # ########################################################################### + # Run filter + ekf = nav.ExtendedKalmanFilter(dataset.process_model) + estimate_list = nav.run_filter(ekf, x0, P0, input_data, meas_data) + + # Postprocess the results and plot + results = nav.GaussianResultList.from_estimates(estimate_list, gt_states) + return results, dataset if __name__ == "__main__": + results, dataset = main() import matplotlib.pyplot as plt import seaborn as sns fig = plt.figure() ax = plt.axes(projection="3d") - landmarks = np.array(landmarks) + landmarks = np.array(dataset.get_groundtruth_landmarks()) ax.scatter(landmarks[:, 0], landmarks[:, 1], landmarks[:, 2]) - states_list = [x.state for x in estimate_list] nav.plot_poses( results.state, ax, line_color="tab:blue", step=500, label="Estimate" ) diff --git a/examples/ex_slam.py b/examples/ex_slam.py new file mode 100644 index 00000000..2510404e --- /dev/null +++ b/examples/ex_slam.py @@ -0,0 +1,161 @@ +"""A toy SLAM example where we are interested in estimating robot poses and +3D landmark positions from IMU measurements and relative position measurements +to the landmarks. + +Here, the purpose is simply to show how the default EKF provided in navlie +can be used in SLAM-type problem settings. The structure of the SLAM problem +is not exploited by doing this. For a more efficient EKF implementation for +SLAM, see the document: + Simulataneous localization and mapping with the extended Kalman filter by Joan + Solà (2014). +""" + +import typing +import numpy as np +import navlie as nav +from navlie.lib.imu import IMUState +from navlie.lib.datasets import SimulatedInertialLandmarkDataset +from navlie.lib.states import VectorState, CompositeState +from navlie.lib.models import PointRelativePositionSLAM, CompositeInput, CompositeProcessModel +from scipy.linalg import block_diag + +class LandmarkProcessModel(nav.ProcessModel): + def evaluate(self, x: VectorState, t: float, u: np.ndarray): + return x.copy() + + def jacobian(self, x: VectorState, t: float, u: np.ndarray): + return np.eye(3) + + def covariance(self, x: VectorState, t: float, u: np.ndarray): + return np.zeros((3, 3)) + + +def main(): + np.set_printoptions(precision=5, suppress=True, linewidth=1000) + np.random.seed(0) + # Load simulated dataset with default parameters + dataset = SimulatedInertialLandmarkDataset(t_start=0, t_end=10.0, R=0.1) + gt_states = dataset.get_ground_truth() + input_data = dataset.get_input_data() + meas_data = dataset.get_measurement_data() + landmarks = dataset.get_groundtruth_landmarks() + + # Filter initialization - set small covariance on yaw and position + # as these are unobservable + P0_imu = np.eye(15) + P0_imu[0:2, 0:2] *= 0.1**2 + P0_imu[2, 2] *= 1e-15 + P0_imu[3:6, 3:6] *= 0.1**2 + P0_imu[6:9, 6:9] *= 1e-15 + P0_imu[9:12, 9:12] *= 0.01**2 + P0_imu[12:15, 12:15] *= 0.01**2 + init_imu_state = gt_states[0].plus(nav.randvec(P0_imu)) + + # Set the state ID to be "r" for robot state and "l" for landmark state + robot_state_id = "r" + landmark_state_id = "l" + init_imu_state.state_id = robot_state_id + + landmark_state_ids: typing.List[str] = [] + + # Create a SLAM state that includes both the landmark states and the robot + # state + state_list = [init_imu_state] + P0_landmark = 0.1**2 + P0_landmark_block = np.identity(3 * len(landmarks)) * P0_landmark + for i, pos in enumerate(landmarks): + # Perturb the initial landmark position + perturbed_pos = pos + nav.randvec(P0_landmark * np.eye(3)) + cur_landmark_state_id = landmark_state_id + str(i) + state_list.append(VectorState(perturbed_pos, 0.0, cur_landmark_state_id)) + landmark_state_ids.append(cur_landmark_state_id) + + init_state = CompositeState(state_list, stamp=init_imu_state.stamp) + init_cov = block_diag(P0_imu, P0_landmark_block) + + # Convert all measurments to SLAM measurements, where the landmark position + # is a state to be estimated + for meas in meas_data: + current_landmark_id = landmark_state_id + str(meas.model._landmark_id) + meas.model = PointRelativePositionSLAM( + robot_state_id, current_landmark_id, meas.model._R + ) + + # Create a composite process model that includes the robot process model and + # the (constant) landmark process model for each landmark + landmark_process_model = LandmarkProcessModel() + process_model_list = [dataset.process_model] + for i in range(len(landmarks)): + process_model_list.append(landmark_process_model) + process_model = CompositeProcessModel(process_model_list) + + composite_inputs = [] + for u in input_data: + # Create a composite input for each of the landmarks + input_list = [u] + for i in range(len(landmarks)): + input_list.append(None) + composite_inputs.append(CompositeInput(input_list)) + + # ########################################################################### + # Create and run filter + ekf = nav.ExtendedKalmanFilter(process_model) + estimate_list = nav.run_filter(ekf, init_state, init_cov, composite_inputs, meas_data) + + # Extract the IMU state estimates from the estimate list + imu_state_list: typing.List[IMUState] = [] + imu_cov_list: typing.List[np.ndarray] = [] + for estimate in estimate_list: + imu_state = estimate.state.get_state_by_id(robot_state_id) + imu_state.stamp = estimate.state.stamp + imu_state_list.append(imu_state) + imu_state_slice = estimate.state.get_slice_by_id(robot_state_id) + imu_cov_list.append(estimate.covariance[imu_state_slice, imu_state_slice]) + + imu_estimates_list: typing.List[nav.StateWithCovariance] = [] + for state, cov in zip(imu_state_list, imu_cov_list): + imu_estimates_list.append(nav.StateWithCovariance(state, cov)) + + # Extract the final estimated landmark positions + final_estimate = estimate_list[-1] + landmark_est_list: typing.List[np.ndarray] = [] + for landmark_id in landmark_state_ids: + landmark_state = final_estimate.state.get_state_by_id(landmark_id) + landmark_est_list.append(landmark_state.value) + + # Postprocess the results and plot + imu_results = nav.GaussianResultList.from_estimates(imu_estimates_list, gt_states) + return imu_results, landmark_est_list, dataset + + +if __name__ == "__main__": + imu_results, landmark_est_list, dataset = main() + import matplotlib.pyplot as plt + import seaborn as sns + + fig = plt.figure() + ax = plt.axes(projection="3d") + landmarks = np.array(dataset.get_groundtruth_landmarks()) + est_landmarks = np.array(landmark_est_list) + ax.scatter(est_landmarks[:, 0], est_landmarks[:, 1], est_landmarks[:, 2], marker="x", color="tab:blue") + ax.scatter(landmarks[:, 0], landmarks[:, 1], landmarks[:, 2], color="tab:red") + nav.plot_poses(imu_results.state, ax, line_color="tab:blue", step=500, label="Estimate") + nav.plot_poses( + imu_results.state_true, + ax, + line_color="tab:red", + step=500, + label="Groundtruth", + ) + ax.legend() + + sns.set_theme() + fig, axs = nav.plot_error(imu_results) + axs[0, 0].set_title("Attitude") + axs[0, 1].set_title("Velocity") + axs[0, 2].set_title("Position") + axs[0, 3].set_title("Gyro bias") + axs[0, 4].set_title("Accel bias") + axs[-1, 2] + + plt.show() \ No newline at end of file diff --git a/navlie/lib/datasets.py b/navlie/lib/datasets.py index fd53c6b1..fd725bab 100644 --- a/navlie/lib/datasets.py +++ b/navlie/lib/datasets.py @@ -11,6 +11,7 @@ RangePoseToAnchor, BodyFrameVelocity, GlobalPosition, + PointRelativePosition, ) import numpy as np from pymlg import SE3, SE23 @@ -143,9 +144,7 @@ def __init__( Q_c = np.eye(12) Q_c[0:3, 0:3] *= 0.01**2 # Gyro continuous-time covariance Q_c[3:6, 3:6] *= 0.01**2 # Accel continuous-time covariance - Q_c[6:9, 6:9] *= ( - 0.0001**2 - ) # Gyro random-walk continuous-time covariance + Q_c[6:9, 6:9] *= 0.0001**2 # Gyro random-walk continuous-time covariance Q_c[9:12, 9:12] *= ( 0.0001**2 ) # Accel random-walk continuous-time covariance @@ -215,3 +214,169 @@ def get_input_data(self) -> List[IMU]: def get_measurement_data(self) -> List[nav.Measurement]: return self.meas_data + + +class SimulatedInertialLandmarkDataset(nav.Dataset): + def __init__( + self, + x0: IMUState = None, + Q: np.ndarray = None, + R: np.ndarray = 0.01, + input_freq: int = 200, + meas_freq: int = 10, + t_start: int = 0, + t_end: int = 50, + noise_active: bool = True, + cylinder_radius: float = 5, + max_height: float = 5, + n_level: int = 5, + n_landmarks_per_level: int = 5, + ): + if x0 is None: + init_nav_state = SE23.from_components( + np.identity(3), + np.array([0, 3, 3]).reshape((-1, 1)), + np.array([3, 0, 0]).reshape((-1, 1)), + ) + init_gyro_bias = np.array([0.02, 0.03, -0.04]).reshape((-1, 1)) + init_accel_bias = np.array([0.01, 0.02, 0.05]).reshape((-1, 1)) + x0 = IMUState( + init_nav_state, + init_gyro_bias, + init_accel_bias, + stamp=t_start, + state_id=0, + direction="left", + ) + + if Q is None: + imu_freq = 200 + Q_c = np.eye(12) + Q_c[0:3, 0:3] *= 0.01**2 # Gyro continuous-time covariance + Q_c[3:6, 3:6] *= 0.01**2 # Accel continuous-time covariance + Q_c[6:9, 6:9] *= 0.0001**2 # Gyro random-walk continuous-time covariance + Q_c[9:12, 9:12] *= ( + 0.0001**2 + ) # Accel random-walk continuous-time covariance + dt = 1 / imu_freq + Q = Q_c / dt + + if isinstance(R, float): + R = R * np.identity(3) + + def input_profile(stamp: float, x: IMUState) -> np.ndarray: + """Generates an IMU measurement for a circular trajectory, + where the robot only rotates about the z-axis and the acceleration + points towards the center of the circle. + """ + + # Add biases to true angular velocity and acceleration + bias_gyro = x.bias_gyro.reshape((-1, 1)) + bias_accel = x.bias_accel.reshape((-1, 1)) + + C_ab = x.attitude + g_a = np.array([0, 0, -9.80665]).reshape((-1, 1)) + omega = np.array([0.1, 0, 0.5]).reshape((-1, 1)) + bias_gyro + a_a = np.array( + [-3 * np.cos(stamp), -3 * np.sin(stamp), -9 * np.sin(3 * stamp)] + ).reshape((-1, 1)) + accel = C_ab.T @ a_a + bias_accel - C_ab.T @ g_a + + # Generate a random input to drive the bias random walk + Q_bias = Q[6:, 6:] + bias_noise = nav.randvec(Q_bias) + + u = IMU(omega, accel, stamp, bias_noise[0:3], bias_noise[3:6]) + return u + + # Generate the landmark positions + landmarks = generate_landmark_positions( + cylinder_radius, + max_height, + n_level, + n_landmarks_per_level, + ) + + # Create process and measurement models and generate data + process_model = IMUKinematics(Q) + meas_model_list = [PointRelativePosition(pos, R, i) for i, pos in enumerate(landmarks)] + data_gen = DataGenerator( + process_model, + input_func=input_profile, + input_covariance=Q, + input_freq=input_freq, + meas_model_list=meas_model_list, + meas_freq_list=meas_freq, + ) + # Generate all data + gt_data, input_data, meas_data = data_gen.generate( + x0, t_start, t_end, noise=noise_active + ) + + # Zero-out the random walk values (thus creating "noise") + if noise_active: + for u in input_data: + u.bias_gyro_walk = np.array([0, 0, 0]) + u.bias_accel_walk = np.array([0, 0, 0]) + # Save all data and process model + self.gt_data = gt_data + self.input_data = input_data + self.meas_data = meas_data + self.process_model = process_model + self.landmarks = landmarks + + def get_ground_truth(self) -> List[IMUState]: + return self.gt_data + + def get_input_data(self) -> List[IMU]: + return self.input_data + + def get_measurement_data(self) -> List[nav.Measurement]: + return self.meas_data + + def get_groundtruth_landmarks(self) -> List[np.ndarray]: + return self.landmarks + + +def generate_landmark_positions( + cylinder_radius: float, + max_height: float, + n_levels: int, + n_landmarks_per_level: int, +) -> List[np.ndarray]: + """Generates landmarks arranged in a cylinder. + + Parameters + ---------- + cylinder_radius : float + Radius of the cylinder that the landmarks are arranged in. + max_height : float + Top of cylinder. + n_levels : int + Number of discrete levels to place landmarks at vertically. + n_landmarks_per_level : int + Number of landmarks per level + + Returns + ------- + List[np.ndarray] + List of landmarks. + """ + + z = np.linspace(0, max_height, n_levels) + + angles = np.linspace(0, 2 * np.pi, n_landmarks_per_level + 1) + angles = angles[0:-1] + x = cylinder_radius * np.cos(angles) + y = cylinder_radius * np.sin(angles) + + # Generate landmarks + landmarks = [] + for level_idx in range(n_levels): + for landmark_idx in range(n_landmarks_per_level): + cur_landmark = np.array( + [x[landmark_idx], y[landmark_idx], z[level_idx]] + ).reshape((3, -1)) + landmarks.append(cur_landmark) + + return landmarks diff --git a/navlie/lib/models.py b/navlie/lib/models.py index 049f3cde..9f09dbc7 100644 --- a/navlie/lib/models.py +++ b/navlie/lib/models.py @@ -677,6 +677,7 @@ def __init__( self, landmark_position: np.ndarray, R: np.ndarray, + landmark_id: Any = None, ): """ Parameters @@ -685,9 +686,13 @@ def __init__( Position of landmark in body frame. R : np.ndarray Measurement covariance. + landmark_id: Any, optional + A unique identifier for the landmark that this measurement + is of. """ self._landmark_position = np.array(landmark_position).ravel() self._R = R + self._landmark_id = landmark_id def evaluate(self, x: MatrixLieGroupState) -> np.ndarray: r_zw_a = x.position.reshape((-1, 1)) @@ -714,6 +719,78 @@ def jacobian(self, x: MatrixLieGroupState) -> np.ndarray: def covariance(self, x: MatrixLieGroupState) -> np.ndarray: return self._R +class PointRelativePositionSLAM(MeasurementModel): + """ + Measurement model describing the position of an unknown landmark relative + to the robot, resolved in the body frame. That is, the state is the pose of + the robot and the inertial lndmark position, and the measurement is relative + landmark measurements resolved in the robot frame. + + .. math:: + \mathbf{y} = \mathbf{C}_{ab}^T (\mathbf{r}_\ell - \mathbf{r}) + + where :math:`\mathbf{C}` is the attitude of the robot, :math:`\mathbf{r}_\ell` + is the position of the landmark, and :math:`\mathbf{r}` is the position of + the robot. + + This class is comptabile with ``SE2State, SE3State, SE23State, IMUState``. + """ + def __init__( + self, + pose_state_id: Any, + landmark_state_id: Any, + R: np.ndarray, + ): + self._pose_state_id = pose_state_id + self._landmark_state_id = landmark_state_id + self._R = R + + def evaluate(self, x: CompositeState) -> np.ndarray: + """Evaluates the measurement model for the landmark state.""" + + # The pose is always assumed to be the first element + # TODO: is there a better way to do this? The + # Measurement class already hold on to the IDs of these two + # states + pose: MatrixLieGroupState = x.get_state_by_id(self._pose_state_id) + landmark: VectorState = x.get_state_by_id(self._landmark_state_id) + + r_zw_a = pose.position.reshape((-1, 1)) + C_ab = pose.attitude + r_pw_a = landmark.value.reshape((-1, 1)) + return C_ab.T @ (r_pw_a - r_zw_a) + + def jacobian(self, x: CompositeState) -> np.ndarray: + pose: MatrixLieGroupState = x.get_state_by_id(self._pose_state_id) + landmark: VectorState = x.get_state_by_id(self._landmark_state_id) + + r_zw_a = pose.position.reshape((-1, 1)) + C_ab = pose.attitude + r_pw_a = landmark.value.reshape((-1, 1)) + y = C_ab.T @ (r_pw_a - r_zw_a) + + # Compute Jacobian of measurement model with respect to the state + if pose.direction == "right": + pose_jacobian = pose.jacobian_from_blocks( + attitude=-SO3.odot(y), position=-np.identity(r_zw_a.shape[0]) + ) + elif pose.direction == "left": + pose_jacobian = pose.jacobian_from_blocks( + attitude=-C_ab.T @ SO3.odot(r_pw_a), position=-C_ab.T + ) + + # Compute jacobian of measurement model with respect to the landmark + landmark_jacobian = pose.attitude.T + + # Build full Jacobian + state_ids = [state.state_id for state in x.value] + jac_dict = {} + jac_dict[state_ids[0]] = pose_jacobian + jac_dict[state_ids[1]] = landmark_jacobian + return x.jacobian_from_blocks(jac_dict) + + def covariance(self, x: CompositeState) -> np.ndarray: + return self._R class InvariantPointRelativePosition(MeasurementModel): def __init__(self, y: np.ndarray, model: PointRelativePosition): diff --git a/navlie/utils.py b/navlie/utils.py index 4e8b1308..ef14e676 100644 --- a/navlie/utils.py +++ b/navlie/utils.py @@ -888,10 +888,13 @@ def plot_poses( if isinstance(poses, StateWithCovariance): poses = [poses.state] - + + if isinstance(poses, np.ndarray): + poses = poses.tolist() + if not isinstance(poses, list): poses = [poses] - + if ax is None: fig = plt.figure() ax = plt.axes(projection="3d") diff --git a/tests/test_examples.py b/tests/test_examples.py index 282e32c4..58de1f5e 100644 --- a/tests/test_examples.py +++ b/tests/test_examples.py @@ -20,6 +20,7 @@ examples/ex_ukf_se2.py examples/ex_ukf_vector.py examples/ex_varying_noise.py +examples/ex_slam.py """ @@ -107,3 +108,8 @@ def test_ex_varying_noise(): from ex_varying_noise import main main() + +def test_ex_slam(): + from ex_slam import main + + main() \ No newline at end of file From 774ea0bd96ffc14a7d0a1490d35d5cdb590a3405 Mon Sep 17 00:00:00 2001 From: Mitchell Cohen Date: Thu, 14 Dec 2023 13:34:10 -0500 Subject: [PATCH 2/2] Add tests for relative position SLAM measurement model --- tests/unit/test_meas_models.py | 30 ++++++++++++++++++++++++++++++ 1 file changed, 30 insertions(+) diff --git a/tests/unit/test_meas_models.py b/tests/unit/test_meas_models.py index c53f9e99..b4450b54 100644 --- a/tests/unit/test_meas_models.py +++ b/tests/unit/test_meas_models.py @@ -5,6 +5,7 @@ SE3State, SE23State, CompositeState, + VectorState, ) from navlie.lib.imu import IMUState from navlie.lib.models import ( @@ -13,6 +14,7 @@ InvariantMeasurement, Magnetometer, PointRelativePosition, + PointRelativePositionSLAM, RangePoseToAnchor, RangePoseToPose, Gravitometer, @@ -301,6 +303,34 @@ def test_landmark_relative_position_se3(direction): _jacobian_test(x, model) +@pytest.mark.parametrize("direction", ["right", "left"]) +def test_landmark_relative_position_slam_se3(direction): + x = SE3State( + SE3.Exp([0, 1, 2, 4, 5, 6]), + stamp=0.0, + state_id="p", + direction=direction, + ) + landmark = VectorState([1, 2, 3], 0.0, "l") + slam_state = CompositeState([x, landmark], stamp=0.0) + model = PointRelativePositionSLAM("p", "l", np.identity(3)) + _jacobian_test(slam_state, model) + + +@pytest.mark.parametrize("direction", ["right", "left"]) +def test_landmark_relative_position_slam_se23(direction: str): + x = SE23State( + SE23.Exp([0, 1, 2, 4, 5, 6, 7, 8, 9]), + stamp=0.0, + state_id="p", + direction=direction, + ) + landmark = VectorState([1, 2, 3], 0.0, "l") + slam_state = CompositeState([x, landmark], stamp=0.0) + model = PointRelativePositionSLAM("p", "l", np.identity(3)) + _jacobian_test(slam_state, model) + + @pytest.mark.parametrize("direction", ["right", "left"]) def test_landmark_relative_position_se23(direction): x = SE23State(