Skip to content

Commit

Permalink
Make PointRelativeMeasurement work with SE2.
Browse files Browse the repository at this point in the history
  • Loading branch information
@vkorotkine
vkorotkine committed Jan 15, 2025
1 parent 67a7813 commit eaeb96b
Show file tree
Hide file tree
Showing 2 changed files with 63 additions and 87 deletions.
123 changes: 51 additions & 72 deletions navlie/lib/models.py
View file
Original file line number Diff line number Diff line change
@@ -1,3 +1,4 @@
from navlie.lib.states import SE2State, SE3State, SE23State
from navlie.types import (
Measurement,
ProcessModel,
Expand Down Expand Up @@ -154,9 +155,7 @@ def __init__(self, Q: np.ndarray):
self._Q = Q
self.dim = int(Q.shape[0] / 2)

def evaluate(
self, x: VectorState, u: VectorInput, dt: float
) -> VectorState:
def evaluate(self, x: VectorState, u: VectorInput, dt: float) -> VectorState:
x = x.copy()
u = u.copy()
Ad = super().jacobian(x, u, dt)
Expand Down Expand Up @@ -244,9 +243,7 @@ def evaluate(
x.value = x.value @ x.group.Exp(u.value * dt)
return x

def jacobian(
self, x: MatrixLieGroupState, u: VectorInput, dt: float
) -> np.ndarray:
def jacobian(self, x: MatrixLieGroupState, u: VectorInput, dt: float) -> np.ndarray:
if x.direction == "right":
return x.group.adjoint(x.group.Exp(-u.value * dt))
elif x.direction == "left":
Expand Down Expand Up @@ -321,16 +318,12 @@ def evaluate(
x.value = x.group.Exp(-u[0] * dt) @ x.value @ x.group.Exp(u[1] * dt)
return x

def jacobian(
self, x: MatrixLieGroupState, u: VectorInput, dt: float
) -> np.ndarray:
def jacobian(self, x: MatrixLieGroupState, u: VectorInput, dt: float) -> np.ndarray:
u = u.value.reshape((2, round(u.value.size / 2)))
if x.direction == "right":
return x.group.adjoint(x.group.Exp(-u[1] * dt))
else:
raise NotImplementedError(
"TODO: left jacobian not yet implemented."
)
raise NotImplementedError("TODO: left jacobian not yet implemented.")

def covariance(
self, x: MatrixLieGroupState, u: VectorInput, dt: float
Expand All @@ -347,9 +340,7 @@ def covariance(
L2 = dt * x.group.left_jacobian(-dt * u2)
return L1 @ self._Q1 @ L1.T + L2 @ self._Q2 @ L2.T
else:
raise NotImplementedError(
"TODO: left covariance not yet implemented."
)
raise NotImplementedError("TODO: left covariance not yet implemented.")


class LinearMeasurement(MeasurementModel):
Expand Down Expand Up @@ -479,17 +470,31 @@ def jacobian(self, x: MatrixLieGroupState) -> np.ndarray:
r_zw_a = x.position.reshape((-1, 1))
C_ab = x.attitude
r_pw_a = self._landmark_position.reshape((-1, 1))
y = C_ab.T @ (r_pw_a - r_zw_a)

if x.direction == "right":
return x.jacobian_from_blocks(
attitude=-SO3.odot(y), position=-np.identity(r_zw_a.shape[0])
)

elif x.direction == "left":
return x.jacobian_from_blocks(
attitude=-C_ab.T @ SO3.odot(r_pw_a), position=-C_ab.T
)
y = C_ab.T @ (r_pw_a - r_zw_a).reshape(-1, 1)

if isinstance(x, SE2State):
P = np.array([[0, -1], [1, 0]])
if x.direction == "right":
return x.jacobian_from_blocks(
attitude=-P @ C_ab.T @ (r_pw_a - r_zw_a).reshape(-1, 1),
position=-np.eye(2),
)
if x.direction == "left":
return x.jacobian_from_blocks(
attitude=-C_ab.T @ P @ (r_pw_a).reshape(-1, 1),
position=-C_ab.T,
)

if isinstance(x, SE3State) or isinstance(x, SE23State): # SE2
if x.direction == "right":
return x.jacobian_from_blocks(
attitude=-SO3.odot(y), position=-np.identity(r_zw_a.shape[0])
)

elif x.direction == "left":
return x.jacobian_from_blocks(
attitude=-C_ab.T @ SO3.odot(r_pw_a), position=-C_ab.T
)

def covariance(self, x: MatrixLieGroupState) -> np.ndarray:
return self._R
Expand Down Expand Up @@ -541,7 +546,9 @@ def jacobian(self, x: CompositeState) -> np.ndarray:
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
# Compute Jacobian of measurement model with respect to the stated
# TODO: Implement SE2 version.

if pose.direction == "right":
pose_jacobian = pose.jacobian_from_blocks(
attitude=-SO3.odot(y), position=-np.identity(r_zw_a.shape[0])
Expand Down Expand Up @@ -624,9 +631,7 @@ def jacobian(self, x: CompositeState) -> np.ndarray:

# Compute the Jacobian of the measurement model
if pose.direction == "right":
r_pc_c = self._camera.resolve_landmark_in_cam_frame(
pose, landmark.value
)
r_pc_c = self._camera.resolve_landmark_in_cam_frame(pose, landmark.value)
dg_dr = self._compute_jac_of_projection_model(r_pc_c)
# Extract relevant states
C_bc = self._camera.T_bc.attitude
Expand Down Expand Up @@ -660,15 +665,11 @@ def jacobian(self, x: CompositeState) -> np.ndarray:
def covariance(self, x: CompositeState) -> np.ndarray:
return self._R

def _compute_jac_of_projection_model(
self, r_pc_c: np.ndarray
) -> np.ndarray:
def _compute_jac_of_projection_model(self, r_pc_c: np.ndarray) -> np.ndarray:
x, y, z = r_pc_c.ravel()
fu = self._camera.fu
fv = self._camera.fv
dg_dr = (1.0 / z) * np.array(
[[fu, 0, -fu * x / z], [0, fv, -fv * y / z]]
)
dg_dr = (1.0 / z) * np.array([[fu, 0, -fu * x / z], [0, fv, -fv * y / z]])

return dg_dr

Expand Down Expand Up @@ -805,9 +806,7 @@ class RangePoseToPose(MeasurementModel):
Compatible with ``SE2State, SE3State, SE23State, IMUState``.
"""

def __init__(
self, tag_body_position1, tag_body_position2, state_id1, state_id2, R
):
def __init__(self, tag_body_position1, tag_body_position2, state_id1, state_id2, R):
"""
Parameters
----------
Expand Down Expand Up @@ -842,9 +841,7 @@ def evaluate(self, x: CompositeState) -> np.ndarray:
C_a2 = x2.attitude
r_t1_1 = self.tag_body_position1.reshape((-1, 1))
r_t2_2 = self.tag_body_position2.reshape((-1, 1))
r_t1t2_a: np.ndarray = (C_a1 @ r_t1_1 + r_1w_a) - (
C_a2 @ r_t2_2 + r_2w_a
)
r_t1t2_a: np.ndarray = (C_a1 @ r_t1_1 + r_1w_a) - (C_a2 @ r_t2_2 + r_2w_a)
return np.array(np.linalg.norm(r_t1t2_a.flatten()))

def jacobian(self, x: CompositeState) -> np.ndarray:
Expand All @@ -856,18 +853,16 @@ def jacobian(self, x: CompositeState) -> np.ndarray:
C_a2 = x2.attitude
r_t1_1 = self.tag_body_position1.reshape((-1, 1))
r_t2_2 = self.tag_body_position2.reshape((-1, 1))
r_t1t2_a: np.ndarray = (C_a1 @ r_t1_1 + r_1w_a) - (
C_a2 @ r_t2_2 + r_2w_a
)
r_t1t2_a: np.ndarray = (C_a1 @ r_t1_1 + r_1w_a) - (C_a2 @ r_t2_2 + r_2w_a)

if C_a1.shape == (2, 2):
att_group = SO2
elif C_a1.shape == (3, 3):
att_group = SO3

rho: np.ndarray = (
r_t1t2_a / np.linalg.norm(r_t1t2_a.flatten())
).reshape((-1, 1))
rho: np.ndarray = (r_t1t2_a / np.linalg.norm(r_t1t2_a.flatten())).reshape(
(-1, 1)
)

if x1.direction == "right":
jac1 = x1.jacobian_from_blocks(
Expand All @@ -891,9 +886,7 @@ def jacobian(self, x: CompositeState) -> np.ndarray:
position=-rho.T @ np.identity(r_t2_2.size),
)

return x.jacobian_from_blocks(
{self.state_id1: jac1, self.state_id2: jac2}
)
return x.jacobian_from_blocks({self.state_id1: jac1, self.state_id2: jac2})

def covariance(self, x: CompositeState) -> np.ndarray:
return self._R
Expand Down Expand Up @@ -1061,9 +1054,7 @@ def evaluate(self, x: MatrixLieGroupState):

def jacobian(self, x: MatrixLieGroupState):
if x.direction == "right":
return x.jacobian_from_blocks(
position=x.attitude[2, :].reshape((1, -1))
)
return x.jacobian_from_blocks(position=x.attitude[2, :].reshape((1, -1)))
elif x.direction == "left":
return x.jacobian_from_blocks(
attitude=SO3.odot(x.position)[2, :].reshape((1, -1)),
Expand Down Expand Up @@ -1107,13 +1098,9 @@ def evaluate(self, x: MatrixLieGroupState):

def jacobian(self, x: MatrixLieGroupState):
if x.direction == "right":
return x.jacobian_from_blocks(
attitude=-SO3.odot(x.attitude.T @ self._g_a)
)
return x.jacobian_from_blocks(attitude=-SO3.odot(x.attitude.T @ self._g_a))
elif x.direction == "left":
return x.jacobian_from_blocks(
attitude=x.attitude.T @ -SO3.odot(self._g_a)
)
return x.jacobian_from_blocks(attitude=x.attitude.T @ -SO3.odot(self._g_a))

def covariance(self, x: MatrixLieGroupState) -> np.ndarray:
if np.isscalar(self.R):
Expand Down Expand Up @@ -1156,13 +1143,9 @@ def evaluate(self, x: MatrixLieGroupState):

def jacobian(self, x: MatrixLieGroupState):
if x.direction == "right":
return x.jacobian_from_blocks(
attitude=-SO3.odot(x.attitude.T @ self._m_a)
)
return x.jacobian_from_blocks(attitude=-SO3.odot(x.attitude.T @ self._m_a))
elif x.direction == "left":
return x.jacobian_from_blocks(
attitude=-x.attitude.T @ SO3.odot(self._m_a)
)
return x.jacobian_from_blocks(attitude=-x.attitude.T @ SO3.odot(self._m_a))

def covariance(self, x: MatrixLieGroupState) -> np.ndarray:
if np.isscalar(self.R):
Expand All @@ -1172,9 +1155,7 @@ def covariance(self, x: MatrixLieGroupState) -> np.ndarray:


class _InvariantInnovation(MeasurementModel):
def __init__(
self, y: np.ndarray, model: MeasurementModel, direction="right"
):
def __init__(self, y: np.ndarray, model: MeasurementModel, direction="right"):
self.measurement_model = model
self.y = y.ravel()
self.direction = direction
Expand Down Expand Up @@ -1225,9 +1206,7 @@ def _compute_direction(self, x: MatrixLieGroupState) -> str:
elif x.direction == "right":
direction = "left"
else:
raise ValueError(
"Invalid direction. Must be 'left', 'right' or 'auto'"
)
raise ValueError("Invalid direction. Must be 'left', 'right' or 'auto'")
return direction


Expand Down
27 changes: 12 additions & 15 deletions tests/unit/test_meas_models.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -112,9 +112,7 @@ def test_range_pose_to_pose_se23(direction):

@pytest.mark.parametrize("direction", ["right", "left"])
def test_global_position_se2(direction):
x = SE2State(
SE2.Exp([0.5, 1, 2]), stamp=0.0, state_id=2, direction=direction
)
x = SE2State(SE2.Exp([0.5, 1, 2]), stamp=0.0, state_id=2, direction=direction)
model = GlobalPosition(np.identity(3))
_jacobian_test(x, model, atol=1e-5)

Expand Down Expand Up @@ -189,9 +187,7 @@ def test_altitude_se23(direction):

@pytest.mark.parametrize("direction", ["right", "left"])
def test_gravity_so3(direction):
x = SO3State(
SO3.Exp([0.3, 0.1, 0.2]), stamp=0.0, state_id=2, direction=direction
)
x = SO3State(SO3.Exp([0.3, 0.1, 0.2]), stamp=0.0, state_id=2, direction=direction)
model = Gravitometer(np.identity(3))
_jacobian_test(x, model)

Expand Down Expand Up @@ -219,9 +215,7 @@ def test_gravity_se23(direction):

@pytest.mark.parametrize("direction", ["right", "left"])
def test_magnetometer_so3(direction):
x = SO3State(
SO3.Exp([0.3, 0.1, 0.2]), stamp=0.0, state_id=2, direction=direction
)
x = SO3State(SO3.Exp([0.3, 0.1, 0.2]), stamp=0.0, state_id=2, direction=direction)
model = Magnetometer(np.identity(3))
_jacobian_test(x, model)

Expand Down Expand Up @@ -270,9 +264,7 @@ def test_camera_projection(direction: str = "right"):


def test_invariant_magnetometer_so3():
x = SO3State(
SO3.Exp([0.3, 0.1, 0.2]), stamp=0.0, state_id=2, direction="left"
)
x = SO3State(SO3.Exp([0.3, 0.1, 0.2]), stamp=0.0, state_id=2, direction="left")

b = [1, 0, 0]
y = np.array(b)
Expand All @@ -284,9 +276,7 @@ def test_invariant_magnetometer_so3():


def test_invariant_magnetometer_se3():
x = SE3State(
SE3.Exp([0, 1, 2, 4, 5, 6]), stamp=0.0, state_id=2, direction="left"
)
x = SE3State(SE3.Exp([0, 1, 2, 4, 5, 6]), stamp=0.0, state_id=2, direction="left")

b = [1, 0, 0]
y = np.array(b)
Expand Down Expand Up @@ -315,6 +305,13 @@ def test_invariant_magnetometer_se23():
assert np.allclose(jac, jac_test, atol=1e-6)


@pytest.mark.parametrize("direction", ["right"])
def test_landmark_relative_position_se2(direction):
x = SE2State(SE2.Exp([0, 1, 2]), stamp=0.0, state_id=2, direction=direction)
model = PointRelativePosition([1, 2], np.identity(2))
_jacobian_test(x, model)


@pytest.mark.parametrize("direction", ["right", "left"])
def test_landmark_relative_position_se3(direction):
x = SE3State(
Expand Down

0 comments on commit eaeb96b

Please sign in to comment.