From 1f05e0918ad24cb5e450a9c11d8569b2c67e7019 Mon Sep 17 00:00:00 2001 From: rlagneau Date: Wed, 29 May 2024 13:57:39 +0200 Subject: [PATCH] [PYTHON][EXAMPLE] Vectorized computation of mean and residual --- .../python/examples/ukf-nonlinear-complex-example.py | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/modules/python/examples/ukf-nonlinear-complex-example.py b/modules/python/examples/ukf-nonlinear-complex-example.py index bdb5464719..2a81d71809 100644 --- a/modules/python/examples/ukf-nonlinear-complex-example.py +++ b/modules/python/examples/ukf-nonlinear-complex-example.py @@ -118,8 +118,8 @@ def measurement_mean(measurements: List[ColVector], wm: List[float]) -> ColVecto sumCos[j] += np.cos(measurements[i][(2*j)+1]) * wm[i] sumSin[j] += np.sin(measurements[i][(2*j)+1]) * wm[i] - for j in range(nbLandmarks): - mean[(2*j)+1] = np.arctan2(sumSin[j], sumCos[j]) + orientations = np.arctan2(sumSin, sumCos) + mean[1::2] = orientations[0::1] return ColVector(mean) def measurement_residual(meas: ColVector, toSubstract: ColVector) -> ColVector: @@ -132,12 +132,8 @@ def measurement_residual(meas: ColVector, toSubstract: ColVector) -> ColVector: @return ColVector \b meas - \b toSubstract . """ - nbMeasures = meas.size() - nbPoints = int(nbMeasures / 2) - res = np.zeros(nbMeasures) - for i in range(nbPoints): - res[2*i] = meas[2*i] - toSubstract[2*i] - res[2*i+1] = normalize_angle(meas[2*i + 1] - toSubstract[2*i + 1]) + res = meas.numpy() - toSubstract.numpy() + res[1::2] = [normalize_angle(angle) for angle in res[1::2]] return ColVector(res) def state_add_vectors(a: ColVector, b: ColVector) -> ColVector: